#include "../src/logic.cpp"
#include <iostream>
#include <stdexcept>
std::array<double,13> setpoint{},encoder{};
int badAxis=0,writeFailure=0;
int32_t error(int32_t axis) { return axis==badAxis; }
int32_t errorId(int32_t axis) { return axis==badAxis?85017:0; }
int32_t putSet(int32_t axis,double value) { setpoint.at(axis)=value; return writeFailure; }
int32_t putEnc(int32_t axis,double value) { encoder.at(axis)=value; return writeFailure; }
void check(bool ok,const char* msg) { if(!ok) throw std::runtime_error(msg); }
int main() {
  ecmcCppLogicHostServices host{};
  host.get_axis_error=error; host.get_axis_error_id=errorId; host.set_axis_ext_set_pos=putSet; host.set_axis_ext_act_pos=putEnc;
  ecmcCpp::setHostServices(&host);
  ArmLogic logic; logic.run(); check(!logic.fault,"zero axes represent home");
  // Verify real binding descriptors rather than relying on the mock to resolve names.
  check(logic.ecmc.count()==18,"18 input bindings");
  check(std::string(logic.ecmc.data()[0].itemName)=="ax1.setpos","coordinate asyn path");
  check(std::string(logic.ecmc.data()[1].itemName)=="ax7.actpos","joint asyn path");
  logic.pose_set={5,-4,8,2,-3,4}; logic.run(); check(!logic.fault,"IK cycle");
  for(int i=0;i<6;++i) logic.joint_trajectory[i]=setpoint[i+7];
  logic.run();
  for(int i=0;i<6;++i) logic.joint_actual[i]=encoder[i+7];
  logic.run(); check(!logic.fault,"FK cycle");
  for(int i=0;i<6;++i) check(std::abs(encoder[i+1]-logic.pose_set[i])<1e-4,"coordinate FK feedback");
  ArmLogic direct; direct.joint_trajectory={1,2,3,4,5,6}; direct.run();
  for(int i=0;i<6;++i) { check(encoder[i+7]==i+1,"direct joint ideal plant"); direct.joint_actual[i]=encoder[i+7]; }
  direct.run(); check(!direct.fault&&std::abs(direct.pose_actual[0])>.01,"direct joint FK");
  const auto held=setpoint; logic.pose_set[0]=10000; logic.run(); check(logic.fault==2&&setpoint==held,"IK failure holds outputs");
  logic.pose_set={}; logic.run(); check(logic.fault==2,"latched fault");
  // An IK fault must stop new commands without freezing the simulation plant.
  logic.joint_trajectory[0]+=0.5;
  logic.run();
  check(logic.fault==2&&setpoint==held,"fault inhibits new IK commands");
  check(encoder[7]==logic.joint_trajectory[0],"plant runs after IK fault");
  ArmLogic limits; limits.joint_trajectory[0]=200; limits.run(); check(limits.fault==1,"joint limit");
  ArmLogic axis; badAxis=1; axis.joint_trajectory[0]=2; axis.run();
  check(axis.fault==4&&axis.fault_axis==1&&axis.fault_axis_error==85017,"axis stall diagnostics");
  check(encoder[7]==2,"plant runs on axis fault cycle");
  const auto stopped=setpoint;
  axis.joint_trajectory[0]=3; axis.joint_actual[0]=2; axis.run();
  check(encoder[7]==3&&std::abs(axis.pose_actual[1])>.01,"plant and FK run while fault latched");
  check(setpoint==stopped,"axis fault inhibits new IK commands"); badAxis=0;
  ArmLogic write; writeFailure=1; write.run(); check(write.fault==3,"host failure");
  std::cout<<"PASS: input bindings, IK/FK pipeline, direct joint plant, latched faults\n";
}