#include "../src/logic.cpp"
#include <iostream>
#include <stdexcept>
#include <array>
std::array<double,13> setpoint{},encoder{};
int badAxis=0,writeFailure=0;
int32_t error(int32_t axis) { return axis==badAxis; }
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 value,const char* message) { if(!value) throw std::runtime_error(message); }
int main() {
  ecmcCppLogicHostServices host{};
  host.get_axis_error=error; host.set_axis_ext_set_pos=putSet; host.set_axis_ext_act_pos=putEnc;
  ecmcCpp::setHostServices(&host);
  HexapodLogic logic;
  logic.run(); check(logic.fault==0,"home cycle");
  logic.pose_set={5,-4,8,2,-3,4}; logic.run(); check(logic.fault==0,"IK cycle");
  for(int i=0;i<6;++i) logic.leg_trajectory[i]=setpoint[i+7];
  logic.run(); check(logic.fault==0,"leg trajectory cycle");
  for(int i=0;i<6;++i) logic.leg_actual[i]=encoder[i+7];
  logic.run(); check(logic.fault==0,"FK cycle");
  for(int i=0;i<6;++i) check(std::abs(encoder[i+1]-logic.pose_set[i])<1e-5,"coordinate encoder FK");
  HexapodLogic direct;
  direct.leg_trajectory={1,2,3,4,5,6};
  direct.run(); check(direct.fault==0,"direct leg move cycle");
  for(int i=0;i<6;++i) {
    check(encoder[i+7]==direct.leg_trajectory[i],"leg encoder follows selected trajectory");
    check(setpoint[i+7]==0,"IK remains separate from direct leg trajectory");
    direct.leg_actual[i]=encoder[i+7];
  }
  direct.run(); check(direct.fault==0,"direct leg FK cycle");
  check(std::abs(direct.pose_actual[2])>0.01,"direct leg motion updates coordinate feedback");
  auto held=setpoint; logic.pose_set[2]=1000; logic.run();
  check(logic.fault==1 && setpoint==held,"stroke fails before writes");
  logic.pose_set={}; logic.run(); check(logic.fault==1 && setpoint==held,"fault latched");
  HexapodLogic fkFail; fkFail.leg_actual[0]=-1000; fkFail.run(); check(fkFail.fault==2,"FK error");
  HexapodLogic axisFail; badAxis=8; axisFail.run(); check(axisFail.fault==4,"axis error"); badAxis=0;
  HexapodLogic hostFail; writeFailure=1; hostFail.run(); check(hostFail.fault==3,"host error");
  std::cout<<"PASS: 12-axis adapter, encoder feedback, latched stroke/FK/axis/host faults\n";
}