#include "kinematics.hpp"
#include <chrono>
#include <iomanip>
#include <iostream>
#include <vector>
using namespace hexapod;
using Clock = std::chrono::steady_clock;
volatile double sink = 0;
int main() {
  Model m;
  constexpr int samples=20000, warmup=1000;
  for(int mode=0;mode<3;++mode) {
    Six seed{};
    std::vector<double> times; times.reserve(samples);
    int failures=0,maxIterations=0; double iterationSum=0;
    for(int n=-warmup;n<samples;++n) {
      const double t=(n+warmup)*.001;
      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)};
      const auto begin=Clock::now();
      const auto command=m.inverse(q); // One IK plus one FK per timed call.
      auto measured=command;
      if(mode==2) measured[0]=1000; // Impossible: exercise nonconvergent path.
      const auto result=forward(m,measured,mode==0 ? seed : Six{});
      const auto end=Clock::now();
      sink=command[0]+result.pose[0]+result.residual;
      if(mode==0 && result.converged) seed=result.pose;
      if(n>=0) {
        times.push_back(std::chrono::duration<double,std::micro>(end-begin).count());
        failures+=!result.converged;
        maxIterations=std::max(maxIterations,result.iterations);
        iterationSum+=result.iterations;
      }
    }
    std::sort(times.begin(),times.end());
    double sum=0; for(double t:times) sum+=t;
    std::cout<<std::fixed<<std::setprecision(3)
      <<(mode==0 ? "tracking" : mode==1 ? "home seed" : "nonconvergent")
      <<": mean_us="<<sum/samples<<" median_us="<<times[samples/2]
      <<" p99_us="<<times[samples*99/100]<<" max_us="<<times.back()
      <<" mean_iterations="<<iterationSum/samples<<" max_iterations="<<maxIterations
      <<" failures="<<failures<<"/"<<samples<<"\n";
  }
}