#include "kinematics.hpp"
#include <iostream>
#include <random>
#include <stdexcept>
using namespace hexapod;
void check(bool value,const char* message) { if(!value) throw std::runtime_error(message); }
int main() {
  Model m; Six home{}; auto lengths=m.inverse(home);
  check(m.withinStroke(lengths),"home stroke");
  for(int i=0;i<6;++i) {
    double delta=40*rad;
    double expected=std::sqrt(250*250+200*200+120*120-2*200*120*std::cos(delta));
    check(std::abs(lengths[i]-expected)<1e-10,"analytic home length");
  }
  // Attachment order around the platform must match the base order; a
  // reversal within a pair reproduces the original crossed-leg regression.
  double winding=0;
  for(int i=0;i<6;++i) {
    const auto& a=m.top[i]; const auto& b=m.top[(i+1)%6];
    double turn=std::atan2(a[0]*b[1]-a[1]*b[0],a[0]*b[0]+a[1]*b[1]);
    check(turn>0,"platform attachment order must not reverse");
    winding+=turn;
    check(std::abs(lengths[i]-lengths[0])<1e-10,"symmetric home lengths");
  }
  check(std::abs(winding-360*rad)<1e-10,"platform winding");
  auto p=rotate({1,0,0},{0,0,0,0,0,90}); check(std::abs(p[0])<1e-12 && std::abs(p[1]-1)<1e-12,"rotation convention");
  std::mt19937 rng(1234); std::uniform_real_distribution<double> u(-1,1);
  double worst=0; int maxIterations=0;
  for(int n=0;n<1000;++n) {
    Six q{}; for(int j=0;j<6;++j) q[j]=u(rng)*(j<3?20:8);
    auto l=m.inverse(q); auto f=forward(m,l,home);
    check(f.converged,"roundtrip convergence");
    for(int j=0;j<6;++j) { worst=std::max(worst,std::abs(f.pose[j]-q[j])); check(std::abs(f.pose[j]-q[j])<1e-5,"roundtrip pose"); }
    maxIterations=std::max(maxIterations,f.iterations);
  }
  Six invalid=home; invalid[0]=std::numeric_limits<double>::quiet_NaN();
  check(!forward(m,invalid,home).converged,"NaN rejection");
  check(!forward(m,lengths,invalid).converged,"NaN seed rejection");
  auto impossible=lengths; impossible[0]=-1; check(!forward(m,impossible,home).converged,"negative length rejection");
  check(!forward(m,m.inverse({10,5,2,3,1,1}),home,0).converged,"iteration limit");
  check(!m.withinStroke(m.inverse({0,0,500,0,0,0})),"stroke rejection");
  Geometry bad; bad.platformRadius=0; check(!forward(Model(bad),lengths,home).converged,"invalid geometry");
  // Follow a smooth trajectory using the previous solution as seed.
  Six seed{};
  for(int n=0;n<1000;++n) {
    double t=n*.01; Six q{10*std::sin(t),8*std::cos(t),5*std::sin(t),3*std::sin(t),2*std::cos(t),4*std::sin(t)};
    auto f=forward(m,m.inverse(q),seed); check(f.converged,"tracking"); seed=f.pose;
  }
  std::cout<<"PASS: 1000 random IK/FK roundtrips, trajectory and invalid inputs. Worst pose error "<<worst<<", max iterations "<<maxIterations<<"\n";
}