#include "kinematics.hpp"
#include <iostream>
#include <random>
#include <stdexcept>
void check(bool ok,const char* msg) { if(!ok) throw std::runtime_error(msg); }
int main() {
  arm::Model m;
  arm::Six zero{}; const auto home=m.pose(m.home);
  for(double x:home) check(std::abs(x)<1e-10,"home pose offsets");
  // Independent straight-arm and base-yaw geometry checks.
  auto t=m.forward(zero); check(std::abs(t.p[0]-630)<1e-10 && t.p[2]==300,"straight arm dimensions");
  zero[0]=90; t=m.forward(zero); check(std::abs(t.p[0])<1e-9 && std::abs(t.p[1]-630)<1e-9,"base rotation");
  auto half=arm::logRotation(arm::rotation(0,arm::pi)); check(std::abs(std::abs(half[0])-arm::pi)<1e-8,"180 degree orientation error");
  std::mt19937 rng(42); std::uniform_real_distribution<double> jitter(-8,8);
  for(int sample=0;sample<500;++sample) {
    auto q=m.home; for(double& v:q) v+=jitter(rng);
    const auto pose=m.pose(q); auto r=m.inverse(pose,m.home);
    check(r.converged && m.withinLimits(r.joints),"near-home IK round trip");
    check(r.residual<1e-5,"round trip residual");
  }
  auto seed=m.home;
  for(int i=0;i<1000;++i) {
    auto q=m.home; for(int k=0;k<6;++k) q[k]+=10*std::sin(i*.005+k*.4);
    auto r=m.inverse(m.pose(q),seed); check(r.converged,"continuous IK tracking"); seed=r.joints;
  }
  check(!m.inverse({10000,0,0,0,0,0},m.home).converged,"unreachable target");
  auto bad=m.home; bad[0]=1000; check(!m.inverse({},bad).converged,"invalid seed");
  arm::Geometry invalid; invalid.tool=-1; check(!arm::Model(invalid).inverse({},m.home).converged,"invalid geometry");
  auto nan=arm::Six{}; nan[0]=NAN; check(!m.inverse(nan,m.home).converged,"nonfinite pose");
  // Straight wrist is singular but holding its current pose must remain valid.
  arm::Six singular{}; check(m.inverse(m.pose(singular),singular).converged,"singular pose hold");
  std::cout<<"PASS: geometry, orientation, 500 IK round trips, 1000 tracking samples, invalid and unreachable inputs\n";
}