#include <sys/time.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>

#include "ros/ros.h"
#include "bosch_arm.h"
#include <kdl/frames.hpp>
#include "motor_controller2.h"
#include <bosch_arm_control/ArmState.h>
#include <bosch_arm_control/PointCmd.h>
#include <bosch_arm_control/Diagnostic.h>
#include "cc2.h"
#include <Eigen/Core>
#include<Eigen/Array>
#include <Eigen/LU>
#include <Eigen/QR>

/** This is an upgrade for motor_controller, LTISys is seperated as a standalone
 *  library, calibration tools are removed. It tunes 4 motors all
 *  together.
**/

using namespace std;
void TrajectoryController::singlePtCmdCallBack(const bosch_arm_control::PointCmd& msg)
{
//   MoveAbsAction* a=new MoveAbsAction(msg.coordinates,this,msg.duration);
//   act_que.push_back(a);
}

TrajectoryController::TrajectoryController(BoschArm *ptr)
{
  rob=ptr;
  diagnostic_pub =  n.advertise<bosch_arm_control::Diagnostic> ("/diagnostics",100);
  sigle_point_cmd_sub = n.subscribe("/joint_point_cmd",3, &TrajectoryController::singlePtCmdCallBack, this);
  joint_state_pub =  n.advertise<bosch_arm_control::ArmState> ("/joint_states",100);
  js.position.resize(4);
  js.velocity.resize(4);
  js.effort.resize(4);
  Kqp[0]=20;
  Kqp[1]=20;
  Kqp[2]=10;
  Kqp[3]=20;

  Kqv[0]=0.01;
  Kqv[1]=0.01;
  Kqv[2]=0.01;
  Kqv[3]=0.01;

  joint_limit_low[0]=-1.7;
  joint_limit_high[0]=2.1;
  joint_limit_low[1]=-0.6;
  joint_limit_high[1]=0.55;
  joint_limit_low[2]=-2.5;
  joint_limit_high[2]=2.3;
  joint_limit_low[3]=-2.3;
  joint_limit_high[3]=2.4;

  qtmax[0]=constants::t_max/constants::r1;
  qtmax[1]=constants::t_max/constants::r2;
  qtmax[2]=constants::t_max/constants::r2;
  qtmax[3]=constants::t_max/constants::r2;

  Kmp[0]=0.8;
  Kmp[1]=0.8;
  Kmp[2]=0.8;
  Kmp[3]=0.8;

  Kmv[0]=0.006;
  Kmv[1]=0.006;
  Kmv[2]=0.006;
  Kmv[3]=0.006;

  mtmax[0]=constants::t_max;
  mtmax[1]=constants::t_max;
  mtmax[2]=constants::t_max;
  mtmax[3]=constants::t_max;

  qvtime[0]=3000;
  qvtime[1]=1500;
  qvtime[2]=800;
  qvtime[3]=800;

  mvtime[0]=3000;
  mvtime[1]=1500;
  mvtime[2]=800;
  mvtime[3]=800;

  qvstep[0]=0.01;
  qvstep[1]=0.01;
  qvstep[2]=0.01;
  qvstep[3]=0.01;

  mvstep[0]=0.01;
  mvstep[1]=0.01;
  mvstep[2]=0.01;
  mvstep[3]=0.01;

  chirp_step=1;

  do_qvstep=false;
  do_mvstep=false;
  do_chirp=false;
  do_msin=false;
  do_qsin=false;
  close_loop=false;

  qtvstep=0;
  mtvstep=0;
  max_torque=0.05;
  clear_ref();

  for(int i=0;i<4;i++)
  {
    filter[i]=new LTISys(1);
    flt_delay[i]=new LTISys(1);
    fpara[i]=new double[6];
    fpara[i][0]=0;
    fpara[i][1]=0.25;
    fpara[i][2]=0;
    fpara[i][3]=0.25;
    fpara[i][4]=0;
    fpara[i][5]=0.3;
    fpara[i][6]=3;
    updateFilter(i);
    mvtime[i]=50;
  }

  fpara[0][0]=25;
  fpara[0][1]=0.25;
  fpara[0][2]=62;
  fpara[0][3]=0.25;
  fpara[0][4]=50;
  fpara[0][5]=0.3;
  fpara[0][6]=2.3;
  Kmp[0]=6;
  updateFilter(0);

  fpara[1][0]=16;
  fpara[1][1]=0.25;
  fpara[1][2]=34;
  fpara[1][3]=0.25;
  fpara[1][4]=40;
  fpara[1][5]=0.3;
  fpara[1][6]=2.3;
  Kmp[1]=6;
  updateFilter(1);

  fpara[2][0]=11;
  fpara[2][1]=0.2;
  fpara[2][2]=23;
  fpara[2][3]=0.25;
  fpara[2][4]=30;
  fpara[2][5]=0.3;
  fpara[2][6]=2.3;
  Kmp[2]=6;
  updateFilter(2);

  fpara[3][0]=25;
  fpara[3][1]=0.2;
  fpara[3][2]=55;
  fpara[3][3]=0.2;
  fpara[3][4]=50;
  fpara[3][5]=0.2;
  fpara[3][6]=2.3;
  Kmp[3]=6;

  updateFilter(3);

}

void TrajectoryController::updateFilter(int mid)
{
  double ts=1.0/constants::cycle_hz;
  //lead-lag compensation: boost up phase margin at cutoff frequency
  LTISys lead(1);
  LTISys lag(1);
  //LTISys leadlag(1);
  if(fpara[mid][4]>0)
  {
  double wc=fpara[mid][4]*2*M_PI;

  double alpha=1.0/fpara[mid][5];
  double wleadl=wc/alpha;
  double wleadh=wc*alpha;

  double wlagh=wc/10;
  double wlagl=wlagh/alpha;

  double sleadb[]={1,1/wleadl};
  double sleada[]={1,1/wleadh};
  double slagb[]={1,1/wlagh};
  double slaga[]={1,1/wlagl};
  double glag= sqrt((wc/wlagh*wc/wlagh+1)/(wc/wlagl*wc/wlagl+1));
  double glead=sqrt((wc/wleadl*wc/wleadl+1)/(wc/wleadh*wc/wleadh+1));
  lead=LTISys(sleadb,1,sleada,1,1);
  lag=LTISys(slagb,1,slaga,1,1/glag/glead);
  //leadlag=lead*lag;
  }
  //resonance compensation: supress leakage of high frequency noise
  LTISys reso(1);
  LTISys reso1(1);
  LTISys reso2(1);
  if(fpara[mid][0]>0&&fpara[mid][2]>0)
  {
  double w1=fpara[mid][0]*2*M_PI;
  double zeta1=fpara[mid][1];
  double w2=fpara[mid][2]*2*M_PI;
  double zeta2=fpara[mid][3];
  double sa1[]={1,2*zeta1/w1,1/w1/w1};
  double sb1[]={1,2*zeta2/w2,1/w2/w2};
  double ss[]={1};
  reso=LTISys(sb1,2,sa1,2);
  //reso1=LTISys(sb1,2,ss,0);
  //reso2=LTISys(ss,0,sa1,2);
  }
  LTISys overall(reso*lag);
  //LTISys overall(reso2*lag);

  LTISys *tmp=filter[mid];
  filter[mid]=new LTISys(overall.tustin(ts));
  if(tmp->order_b>0)
    filter[mid]->initialize(tmp->x[0]);
  delete tmp;

  //delay compensation for input.
  LTISys delay(1);
  if(fpara[mid][6]>0)
  {
  double td=fpara[mid][6]*1e-3;
  double sdelayb[]={1,td};
  double sdelaya[]={1,td/10};
  delay=LTISys(sdelayb,1,sdelaya,1);
  }
  tmp=flt_delay[mid];
  LTISys overall2(delay*lead);
  flt_delay[mid]=new LTISys(overall2.tustin(ts));
  if(tmp->order_b>0)
    flt_delay[mid]->initialize(tmp->x[0]);
//  cout<<*flt_delay;
  delete tmp;

}

// void TrajectoryController::updateFilter(int mid)
// {
//   double ts=1.0/constants::cycle_hz;
//   //lead-lag compensation: boost up phase margin at cutoff frequency
//   LTISys leadlag(1);
//   if(fpara[mid][4]>0)
//   {
//   double wc=fpara[mid][4]*2*M_PI;
//
//   double alpha=1.0/fpara[mid][5];
//   double wleadl=wc/alpha;
//   double wleadh=wc*alpha;
//
//   double wlagh=wc/10;
//   double wlagl=wlagh/alpha;
//
//   double sleadb[]={1,1/wleadl};
//   double sleada[]={1,1/wleadh};
//   double slagb[]={1,1/wlagh};
//   double slaga[]={1,1/wlagl};
//   double glag= sqrt((wc/wlagh*wc/wlagh+1)/(wc/wlagl*wc/wlagl+1));
//   double glead=sqrt((wc/wleadl*wc/wleadl+1)/(wc/wleadh*wc/wleadh+1));
//   LTISys lead(sleadb,1,sleada,1,1/glead);
//   LTISys lag(slagb,1,slaga,1,1/glag);
//   leadlag=lead*lag;
//   }
//   //resonance compensation: supress leakage of high frequency noise
//   LTISys reso(1);
//   if(fpara[mid][0]>0&&fpara[mid][2]>0)
//   {
//   double w1=fpara[mid][0]*2*M_PI;
//   double zeta1=fpara[mid][1];
//   double w2=fpara[mid][2]*2*M_PI;
//   double zeta2=fpara[mid][3];
//   double sa1[]={1,2*zeta1/w1,1/w1/w1};
//   double sb1[]={1,2*zeta2/w2,1/w2/w2};
//   reso=LTISys(sb1,2,sa1,2);
//   }
//   LTISys overall(reso*leadlag);
//
//   LTISys *tmp=filter[mid];
//   filter[mid]=new LTISys(overall.tustin(ts));
//   if(tmp->order_b>0)
//     filter[mid]->initialize(tmp->x[0]);
//   delete tmp;
//
//   //delay compensation for input.
//   LTISys delay(1);
//   if(fpara[mid][6]>0)
//   {
//   double td=fpara[mid][6]*1e-3;
//   double sdelayb[]={1,td};
//   double sdelaya[]={1,td/10};
//   delay=LTISys(sdelayb,1,sdelaya,1);
//   }
//   tmp=flt_delay[mid];
//   flt_delay[mid]=new LTISys(delay.tustin(ts));
//   if(tmp->order_b>0)
//     flt_delay[mid]->initialize(tmp->x[0]);
// //  cout<<*flt_delay;
//   delete tmp;
//
// }

void TrajectoryController::set_position_ref()
{
  for (int i=0;i<4;i++)
  {
    mzero[i]=m[i];
    qzero[i]=q[i];

  }
}
void TrajectoryController::set_gc()
{
  for (int i=0;i<4;i++)
  {
    tzero[i]=mt[i];
  }
}
void TrajectoryController::clear_ref()
{
  for (int i=0;i<4;i++)
  {
    mzero[i]=0;
    qzero[i]=0;
    tzero[i]=0;
  }
}

//calculate the frames given the joint values.
void TrajectoryController::updateKinematics()
{
  frm_cur[0]=frm_home[0];
  frm0[0]=frm_home[0];
  for (int i=1;i<=4;i++)
  {
    frm_cur[i].p=frm_home[i].p;
    frm_cur[i].M=frm_home[i].M*Rotation::RotZ(q[i-1]);
    frm0[i]=frm0[i-1]*frm_cur[i];
  }
  frm_cur[5]=frm_home[5];
  frm0[5]=frm0[4]*frm_cur[5];
  frmn[4]=frm_cur[5];
  Vector z(0,0,1);

  //jacobian[3](joint 4) = z4*p54
  for (int i=3;i>=0;i--)
  {
    frmn[i]=frm_cur[i+1]*frmn[i+1];
    jacobian[i]=z*frmn[i+1].p;
    jacobian[i]=frm0[i+1].M*jacobian[i];
  }
}

void TrajectoryController::start()
{
  rob->initialize();
  //L0,L3 are not available until rob is initialized.
  frm_home[0]=Frame::Identity();
  Rotation r(0,-1,0,
             0,0,-1,
             1,0,0);
  frm_home[1].M=r;
  frm_home[1].p=Vector(0,0,rob->L0);
  frm_home[2].M=r;
  frm_home[3].M=r;
  frm_home[4].M=r;
  frm_home[4].p=Vector(-rob->L5,0,rob->L3);
  frm_home[5].p=Vector(rob->L4,0,0);

  rob->motor2JointPosition(rob->q,q);

  for (int i=0;i<4;i++)
  {
    qv[i]=0;
    dqv[i]=0;
    dq[i]=0;
    qt[i]=0;
    qd[i]=q[i];
    qvd[i]=qv[i];

    m[i]=rob->q[i];
    mv[i]=0;
    md[i]=m[i];
    mvd[i]=mv[i];
    dm[i]=0;
    dmv[i]=0;
    mt[i]=0;
  }
  for (int i=0;i<4;i++)
    gc[i]=Vector(constants::gravc[i*2],constants::gravc[i*2+1],0);

  state=SERVO;
  //ctr_mode=GRAVITY;
  ctr_mode=GC_M2JT;
  //ctr_mode=PDWITHGC;

}

// void TrajectoryController::vstep(int joint, double step)
// {
//   vector<double> des(4,0);
//   for (int i=0;i<4;i++)
//     des[i]=qd[i];
//   des[joint]+=step;
//   cur_act=new MoveAbsAction(des,this);
//   cur_act->initialize();
//
// }

void TrajectoryController::qstep(int joint, double step)
{

//   for (int i=0;i<4;i++)
//     qd[i]=q[i];
  qd[joint]+=step;

}

void TrajectoryController::mstep(int joint, double step)
{

//   for (int i=0;i<4;i++)
//     md[i]=m[i];
  md[joint]+=step;

}

void TrajectoryController::update()
{
  rob->update();
  for (int i=0;i<4;i++)
  {
    m[i]=rob->q[i];
    mv[i]=rob->v[i];
  }
  clock_gettime(CLOCK_REALTIME, &ts);
  //save the last joint position
//   for (int i=0;i<4;i++)
//     ql[i]=q[i];
  rob->motor2JointPosition(rob->q,q);
  rob->motor2JointVelocity(rob->v,qv);

  if (do_qvstep)
      do_qVStep();
    else if (do_mvstep)
      do_mVStep();
    else if (do_qsin)
      do_qSin();
//     else if (do_msin)
//       do_mSin();
    else if (do_chirp)
      //do_motorSine();
      md_Km_flt();
      //do_ChirpSine();
      //do_stretch();

    if (ctr_mode==QD_KQ_M2JT)
      qd_Kq_m2jT();
    else if (ctr_mode==QD_KQ_GC_M2JT)
      qd_Kq_gc_m2jT();
    else if (ctr_mode==QD_J2M_KM)
      qd_j2m_Km();
    else if (ctr_mode==GC_M2JT)
      gc_m2jT();
    else if (ctr_mode==MD_KM)
      md_VarKm();
    else if (ctr_mode==NOCONTROL)
    {
      for (int i=0;i<4;i++)
        rob->torque[i]=tzero[i];
    }
    else if (ctr_mode==SYSID)
    {
      for (int i=0;i<4;i++)
        rob->torque[i]=mt[i];
    }

    logging();

}

void TrajectoryController::do_stretch()
{
  for (int i=0;i<4;i++)
    mt[i]=tzero[i];

  if (mtvstep<mvtime[jid])
  {
    mt[jid]-=mtvstep*0.15/5000-0.02;
    mtvstep++;
    if (mtvstep%100==0)
      cout<<mt[jid]<<endl;
  }
}

void TrajectoryController::do_ChirpSine()
{

  double t=double(mtvstep)/double(constants::cycle_hz);
  double freq=chirp_step;
  for (int i=0;i<4;i++)
  {
    mt[i]=tzero[i];
    md[i]=mzero[i];
  }
  mt[jid]+=mvstep[jid]*sin(2*M_PI*freq*t);
  mtvstep++;
//     if(mtvstep%1000==0)
//       cout<<freq<<"Hz"<<endl;

}

// void TrajectoryController::md_Km_flt()
// {
//   double t=double(mtvstep)/double(constants::cycle_hz);
//   double freq=chirp_step;
//   for (int i=0;i<4;i++)
//   {
//     mt[i]=tzero[i];
//     md[i]=mzero[i];
//     dmv[i]=-mv[i];
//   }
//
//   md[jid]+=mvstep[jid]*sin(2*M_PI*freq*t);
//
//   for(int i=0;i<4;i++)
//   {
//     int jid=i;
//   if(mtvstep==0)
//     flt_delay[jid]->initialize(m[jid]);
//   if(close_loop)
//   {
//     qd[jid]=flt_delay[jid]->filter(m[jid]);
//     //double tmp=dm[i];
//     dm[jid]=md[jid]-qd[jid];
//     //dmv[i]=0-mv[i];
//     dq[jid]=filter[jid]->filter(dm[jid]);
//
//     double dis=fabs(dq[jid]);
//     double f0=mvtime[jid]*1e-3;
//     double d0=f0/Kmp[jid];
//     double d1=300*d0;
//     double f1=2*f0;
//     double c=3.0;
//     if(dis<d0)
//       mt[jid]+=Kmp[jid]*dq[jid];
//     else if(dis<d1&&dq[jid]>0)
//       mt[jid]+=f0+(f1-f0)*(exp(-c*d0)-exp(-c*dis))/(exp(-c*d0)-exp(-c*d1));
//     else if(dis<d1&&dq[jid]<0)
//       mt[jid]-=f0+(f1-f0)*(exp(-c*d0)-exp(-c*dis))/(exp(-c*d0)-exp(-c*d1));
//     else if(dq[jid]>0)
//       mt[jid]+=f1;
//     else
//       mt[jid]-=f1;
//
//     //cout<<m[jid]<<','<<qd[jid]<<','<<dm[jid]<<','<<dq[jid]<<','<<mt[jid]<<endl;
//   }
//   else
//   {
//
//     dm[jid]=md[jid]-mzero[jid];
//     dq[jid]=filter[jid]->filter(dm[jid]);
//     qd[jid]=flt_delay[jid]->filter(m[jid]);
//     //dq[jid]=dm[jid];
//     mt[jid]+=Kmp[jid]*dq[jid];
//   }
//   }
//   //mt[jid]+=mvstep[jid]*sin(2*M_PI*freq*t);
//   mtvstep++;
// }

void TrajectoryController::md_Km_flt()
{
  double t=double(mtvstep)/double(constants::cycle_hz);
  double freq=chirp_step;
  for (int i=0;i<4;i++)
  {
    mt[i]=tzero[i];
    md[i]=mzero[i];
    dmv[i]=-mv[i];
  }

  md[jid]+=mvstep[jid]*sin(2*M_PI*freq*t);

  for(int i=0;i<4;i++)
  {
    int jid=i;
  if(mtvstep==0)
    flt_delay[jid]->initialize(m[jid]);
  if(close_loop)
  {
    qd[jid]=flt_delay[jid]->filter(m[jid]);
    //double tmp=dm[i];
    dm[jid]=md[jid]-qd[jid];
    //dmv[i]=0-mv[i];
    dq[jid]=filter[jid]->filter(dm[jid]);

    double r=Kmp[jid]*dq[jid]/(mvtime[jid]*1e-3);
    if (fabs(r)<=1)
      mt[jid]+=Kmp[jid]*dq[jid];
    else if(fabs(r)<=8)
    {
      double mt1=Kmp[jid]*dq[jid];
      double mt2=Kmp[jid]/8*dq[jid]+Kmv[jid]*dmv[jid];
      double ratio= 1/(1+10*exp(fabs(r)-1)*exp(fabs(r)-1));
      mt[jid]+=mt1*ratio+mt2*(1-ratio);
    }
    else if (r>8)
    {
      double mt1=Kmp[jid]/8*dq[jid]+Kmv[jid]*dmv[jid];
      double mt2=max_torque+Kmv[jid]/(1+log(fabs(r/8)))*dmv[jid];
      double ratio= 1/(1+10*exp(fabs(r/8)-1)*exp(fabs(r/8)-1));
      mt[jid]+=mt1*ratio+mt2*(1-ratio);
      //mt[jid]=max_torque+Kmv[jid]/(1+log(fabs(r)))*dmv[jid];
    }
    else
    {
      double mt1=Kmp[jid]/8*dq[jid]+Kmv[jid]*dmv[jid];
      double mt2=-max_torque+Kmv[jid]/(1+log(fabs(r/8)))*dmv[jid];
      double ratio= 1/(1+10*exp(fabs(r/8)-1)*exp(fabs(r/8)-1));
      mt[jid]+=mt1*ratio+mt2*(1-ratio);
      //mt[jid]=-max_torque+Kmv[jid]/(1+log(fabs(r)))*dmv[jid];
    }
    //mt[jid]+=Kmp[jid]*dq[jid];
    //cout<<m[jid]<<','<<qd[jid]<<','<<dm[jid]<<','<<dq[jid]<<','<<mt[jid]<<endl;
  }
  else
  {

    dm[jid]=md[jid]-mzero[jid];
    dq[jid]=filter[jid]->filter(dm[jid]);
    qd[jid]=flt_delay[jid]->filter(m[jid]);
    //dq[jid]=dm[jid];
    mt[jid]+=Kmp[jid]*dq[jid];
  }
  }
  //mt[jid]+=mvstep[jid]*sin(2*M_PI*freq*t);
  mtvstep++;
}

void TrajectoryController::do_motorSine()
{
  double t=double(mtvstep)/double(constants::cycle_hz);
  double freq=chirp_step;
  for (int i=0;i<4;i++)
  {
    mt[i]=tzero[i];
    md[i]=mzero[i];
    dmv[i]=-mv[i];
  }

  md[jid]+=mvstep[jid]*sin(2*M_PI*freq*t);

  if(mtvstep==0)
    flt_delay[jid]->initialize(m[jid]);
  if(close_loop)
  {
    qd[jid]=flt_delay[jid]->filter(m[jid]);
    //double tmp=dm[i];
    dm[jid]=md[jid]-qd[jid];
    //dmv[i]=0-mv[i];
    dq[jid]=filter[jid]->filter(dm[jid]);

    double r=Kmp[jid]*dq[jid]/max_torque;
    if (fabs(r)<=1)
      mt[jid]+=Kmp[jid]*dq[jid];
    else if (r>1)
      //mt[jid]=max_torque+Kmv[jid]/(1+log(fabs(r)))*dmv[jid];
      mt[jid]+=Kmp[jid]*dq[jid]/fabs(r);
    else
      mt[jid]+=Kmp[jid]*dq[jid]/fabs(r);
      //mt[jid]=-max_torque+Kmv[jid]/(1+log(fabs(r)))*dmv[jid];

    //mt[jid]+=Kmp[jid]*dq[jid];
    //cout<<m[jid]<<','<<qd[jid]<<','<<dm[jid]<<','<<dq[jid]<<','<<mt[jid]<<endl;
  }
  else
  {

    dm[jid]=md[jid]-mzero[jid];
    dq[jid]=filter[jid]->filter(dm[jid]);
    qd[jid]=flt_delay[jid]->filter(m[jid]);
    //dq[jid]=dm[jid];
    mt[jid]+=Kmp[jid]*dq[jid];
  }

  //mt[jid]+=mvstep[jid]*sin(2*M_PI*freq*t);
  mtvstep++;
}

void TrajectoryController::do_qSin()
{

  if (qtvstep==2*qvtime[jid])
    qtvstep=0;
  double tmp=qzero[jid]+qvstep[jid]*sin(double(qtvstep)/double(qvtime[jid])*M_PI);
  qvd[jid]=tmp-qd[jid];
  qd[jid]=tmp;
  qtvstep++;

}

void TrajectoryController::do_Chirp()
{

//   updateKinematics();
//   Vector g(0,0,-1);
//   //double tq[4];
//   for (int i=0;i<4;i++)
//   {
//     Vector Zi=frm0[i+1].M.UnitZ();
//     qt[i]=0;
//     for (int j=i;j<4;j++)
//     {
//       Rotation R0j=frm0[j+1].M.Inverse();
//       Vector tmp=R0j*(g*Zi);
//       qt[i]-=dot(gc[j],tmp);
//     }
//   }
//
//   for (int i=0;i<4;i++)
//   {
//     mt[i]=0;
//     for (int j=0;j<4;j++)
//     {
//       mt[i]+=rob->m2j[4*j+i]*qt[j];
//
//     }
//
//   }
//   if (mtvstep==0)
//     for (int j=0;j<4;j++)
//       for (int i=0;i<filter_order;i++)
//       {
//         tx[j][i]=tzero[j];
//         ty[j][i]=tzero[j];
//       }
  double t=double(mtvstep)/double(constants::cycle_hz);
  double freq=exp(chirp_step*t*5);
  for (int i=0;i<4;i++)
    mt[i]=tzero[i];
  if (mtvstep<mvtime[jid])
  {
    mt[jid]+=mvstep[jid]*sin(2*M_PI*freq*t);
    mtvstep++;
    if (mtvstep%1000==0)
      cout<<freq<<"Hz"<<endl;
  }
//   for(int i=0;i<4;i++)
//     qt[i]=mt[i];
//   double tmp[4];
//   for(int i=2;i<=2;i++)
//   {
//     tmp[i]=b[0]*mt[i];
//     for(int j=0;j<filter_order;j++)
//       tmp[i]+=b[j+1]*tx[i][j];
//     for(int j=0;j<filter_order;j++)
//       tmp[i]-=a[j+1]*ty[i][j];
//     for(int j=filter_order-1;j>0;j--)
//       tx[i][j]=tx[i][j-1];
//     tx[i][0]=mt[i];
//     for(int j=filter_order-1;j>0;j--)
//       ty[i][j]=ty[i][j-1];
//     ty[i][0]=tmp[i];
//     //q[i]=tmp[i];
//     mt[i]=ty[i][0];
//   }

//   if(mtvstep>mvtime[jid])
//     do_chirp=false;
}

void TrajectoryController::do_qVStep()
{
  if (qtvstep==2*qvtime[jid])
    qtvstep=0;
  if (qtvstep<qvtime[jid]/2)
  {
    qd[jid]+=qvstep[jid]*2/qvtime[jid];
    qvd[jid]=qvstep[jid]*2/qvtime[jid];
  }
  else if (qtvstep<qvtime[jid]/2+qvtime[jid])
  {
    qd[jid]-=qvstep[jid]*2/qvtime[jid];
    qvd[jid]=-qvstep[jid]*2/qvtime[jid];
  }
  else
  {
    qd[jid]+=qvstep[jid]*2/qvtime[jid];
    qvd[jid]=qvstep[jid]*2/qvtime[jid];
  }
  qtvstep++;
}

void TrajectoryController::do_mVStep()
{
  if (mtvstep==2*mvtime[jid])
    mtvstep=0;
  if (mtvstep<mvtime[jid]/2)
  {
    md[jid]+=mvstep[jid]*2/mvtime[jid];
    mvd[jid]=mvstep[jid]*2/mvtime[jid];
  }
  else if (mtvstep<mvtime[jid]/2+mvtime[jid])
  {
    md[jid]-=mvstep[jid]*2/mvtime[jid];
    mvd[jid]=-mvstep[jid]*2/mvtime[jid];
  }
  else
  {
    md[jid]+=mvstep[jid]*2/mvtime[jid];
    mvd[jid]=mvstep[jid]*2/mvtime[jid];
  }
  mtvstep++;
}

void TrajectoryController::gc_m2jT()
{

  updateKinematics();
  //T=sum(gcj) dot (g*Zi
  Vector g(0,0,-1);
  //double tq[4];
  for (int i=0;i<4;i++)
  {
    Vector Zi=frm0[i+1].M.UnitZ();
    qt[i]=0;
    for (int j=i;j<4;j++)
    {
      Rotation R0j=frm0[j+1].M.Inverse();
      Vector tmp=R0j*(g*Zi);
      qt[i]-=dot(gc[j],tmp);
    }
  }

  for (int i=0;i<4;i++)
  {
    mt[i]=0;
    for (int j=0;j<4;j++)
    {
      mt[i]+=rob->m2j[4*j+i]*qt[j];

    }

  }
  for (int i=0;i<4;i++)
    rob->torque[i]=mt[i];
  //logging();

}

void TrajectoryController::qd_Kq_gc_m2jT()
{
  updateKinematics();
  for (int i=0;i<4;i++)
  {

    dq[i]=qd[i]-q[i];
    dqv[i]=qvd[i]-qv[i];
    qt[i]=Kqp[i]*dq[i]+Kqv[i]*dqv[i];
  }

  //T=sum(gcj) dot (g*Zi
  Vector g(0,0,-1);
  //double tq[4];
  for (int i=0;i<4;i++)
  {
    Vector Zi=frm0[i+1].M.UnitZ();
    for (int j=i;j<4;j++)
    {
      Rotation R0j=frm0[j+1].M.Inverse();
      Vector tmp=R0j*(g*Zi);
      qt[i]-=dot(gc[j],tmp);
    }
  }
  for (int i=0;i<4;i++)
  {
    mt[i]=0;
    for (int j=0;j<4;j++)
    {
      mt[i]+=rob->m2j[4*j+i]*qt[j];

    }

  }
  for (int i=0;i<4;i++)
    rob->torque[i]=mt[i];
  //logging();
}

void TrajectoryController::md_VarKm()
{
  for (int i=0;i<4;i++)
  {
    dm[i]=md[i]-m[i];
    dmv[i]=mvd[i]-mv[i];
    //dv[i]=-v[i];
  }

  for (int i=0;i<4;i++)
  {
    double r=Kmp[i]*dm[i]/max_torque;
    if (fabs(r)<=1)
      mt[i]=Kmp[i]*dm[i]+Kmv[i]*dmv[i];
    else if (r>1)
      mt[i]=max_torque+Kmv[i]/(1+log(fabs(r)))*dmv[i];
    else
      mt[i]=-max_torque+Kmv[i]/(1+log(fabs(r)))*dmv[i];
  }
  for (int i=0;i<4;i++)
    rob->torque[i]=mt[i];
}

void TrajectoryController::md_Km_VarKm_Comp()
{

  for (int i=0;i<4;i++)
  {
    dm[i]=md[i]-m[i];
    dmv[i]=mvd[i]-mv[i];
    //dv[i]=-v[i];
  }

  for (int i=0;i<4;i++)
  {
    if (fabs(Kmp[i]*dm[i])<=max_torque)
      mt[i]=Kmp[i]*dm[i]+Kmv[i]*dmv[i];
    else if (Kmp[i]*dm[i]>max_torque)
      mt[i]=max_torque+Kmv[i]*dmv[i];
    else
      mt[i]=-max_torque+Kmv[i]*dmv[i];
  }

  for (int i=2;i<=2;i++)
  {
    if (fabs(Kmp[i]*dm[i])<=max_torque)
      mt[i]=Kmp[i]*dm[i];
    else if (Kmp[i]*dm[i]>max_torque)
      mt[i]=max_torque;
    else
      mt[i]=-max_torque;
  }

//   double tmp[4];
//   for (int i=2;i<=2;i++)
//   {
//     tmp[i]=b[0]*mt[i];
//     for (int j=0;j<filter_order;j++)
//       tmp[i]+=b[j+1]*tx[i][j];
//     for (int j=0;j<filter_order;j++)
//       tmp[i]-=a[j+1]*ty[i][j];
//     for (int j=filter_order-1;j>0;j--)
//       tx[i][j]=tx[i][j-1];
//     tx[i][0]=mt[i];
//     for (int j=filter_order-1;j>0;j--)
//       ty[i][j]=ty[i][j-1];
//     ty[i][0]=tmp[i];
//     //q[i]=tmp[i];
//     mt[i]=ty[i][0];
//   }

  for (int i=0;i<4;i++)
    rob->torque[i]=mt[i];

}

void TrajectoryController::md_Km()
{
  for (int i=0;i<4;i++)
  {
    dm[i]=md[i]-m[i];
    dmv[i]=mvd[i]-mv[i];
    //dv[i]=-v[i];
  }

  for (int i=0;i<4;i++)
  {
    mt[i]=Kmp[i]*dm[i]+Kmv[i]*dmv[i];
  }
  for (int i=0;i<4;i++)
    rob->torque[i]=mt[i];
  //logging();
}

void TrajectoryController::qd_j2m_Km()
{
  for (int i=0;i<4;i++)
  {
    dq[i]=qd[i]-q[i];
    dqv[i]=qvd[i]-qv[i];
    //dv[i]=-v[i];
  }

  //transform errors into motor space

  for (int i=0;i<4;i++)
  {
    dm[i]=0;
    dmv[i]=0;
    for (int j=0;j<4;j++)
    {
      dm[i]+=dq[j]*rob->j2m[4*i+j];
      dmv[i]+=dqv[j]*rob->j2m[4*i+j];
    }
  }

  for (int i=0;i<4;i++)
  {
    mt[i]=Kmp[i]*dm[i]+Kmv[i]*dmv[i];
  }
  for (int i=0;i<4;i++)
    rob->torque[i]=mt[i];
  //logging();
}

void TrajectoryController::qd_Kq_m2jT()
{
  for (int i=0;i<4;i++)
  {

    dq[i]=qd[i]-q[i];
    qt[i]=Kqp[i]*dq[i]-Kqv[i]*(qv[i]-qvd[i]);
  }
  for (int i=0;i<4;i++)
  {
    mt[i]=0;
    for (int j=0;j<4;j++)
    {
      mt[i]+=rob->m2j[4*j+i]*qt[j];

    }

  }
  for (int i=0;i<4;i++)
    rob->torque[i]=mt[i];
  //logging();
}

void TrajectoryController::logging()
{
  std::string log;
  std::stringstream out;
  for (int i=0;i<4;i++)
    out<<m[i]-mzero[i]<<",";
  for (int i=0;i<4;i++)
    out<<md[i]-mzero[i]<<",";
  for (int i=0;i<4;i++)
    out<<mv[i]<<",";
  for (int i=0;i<4;i++)
    out<<mt[i]<<",";
  for (int i=0;i<4;i++)
    out<<dm[i]<<",";
  for (int i=0;i<4;i++)
    out<<q[i]-qzero[i]<<",";
  for (int i=0;i<4;i++)
    out<<qd[i]-mzero[i]<<",";
  for (int i=0;i<3;i++)
    out<<qv[i]<<",";
  out<<rob->dt<<',';
  for (int i=0;i<4;i++)
    out<<qt[i]<<",";
  for (int i=0;i<4;i++)
    out<<dq[i]<<",";

  out << ts.tv_sec << ','<< ts.tv_nsec;
  log = out.str();
  diag.data=log;
  diag.header.stamp.sec=ts.tv_sec;
  diag.header.stamp.nsec=ts.tv_nsec;
  diagnostic_pub.publish(diag);
  //vector<double> cur_pos_act(q,q+4);
  js.header.stamp.sec=ts.tv_sec;
  js.header.stamp.nsec=ts.tv_nsec;
  //js.position=cur_pos_act;

  for (int i=0;i<4;i++)
  {
    js.position[i]=q[i];
    js.velocity[i]=qv[i];
    js.effort[i]=qt[i];
  }
  joint_state_pub.publish(js);
}

// void TrajectoryController::logging()
// {
//   std::string log;
//   std::stringstream out;
//   for (int i=0;i<4;i++)
//     out<<q[i]<<",";
//   for (int i=0;i<4;i++)
//     out<<qd[i]<<",";
//   for (int i=0;i<4;i++)
//     out<<v[i]<<",";
//   for (int i=0;i<4;i++)
//     out<<f[i]<<",";
//   for (int i=0;i<4;i++)
//     out<<dq[i]<<",";
//   for (int i=0;i<4;i++)
//     out<<rob->q[i]<<',';
//   for (int i=0;i<4;i++)
//     out<<rob->v[i]<<',';
//   for (int i=0;i<4;i++)
//     out<<rob->torque[i]<<',';
//   out << ts.tv_sec << ','<< ts.tv_nsec;
//   log = out.str();
//   diag.data=log;
//   diag.header.stamp.sec=ts.tv_sec;
//   diag.header.stamp.nsec=ts.tv_nsec;
//   diagnostic_pub.publish(diag);
//   //vector<double> cur_pos_act(q,q+4);
//   js.header.stamp.sec=ts.tv_sec;
//   js.header.stamp.nsec=ts.tv_nsec;
//   //js.position=cur_pos_act;
//
//   for (int i=0;i<4;i++)
//   {
//     js.position[i]=q[i];
//     js.velocity[i]=v[i];
//     js.effort[i]=f[i];
//   }
//   joint_state_pub.publish(js);
// }
void TrajectoryController::stop()
{
  rob->close();
}


// Copyright (c) 2021 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <pybind11/eigen.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>

#include <Eigen/Core>
#include <atomic>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>

#include "pyinterp/detail/math.hpp"
#include "pyinterp/detail/thread.hpp"
#include "pyinterp/eigen.hpp"
#include "pyinterp/grid.hpp"

namespace pyinterp {
namespace detail {

/// Calculate the zonal average in x direction
///
/// @param grid The grid to be processed.
/// @param mask Matrix describing the undefined pixels of the grid providedNaN
/// NumberReplaces all missing (_FillValue) values in a grid with values derived
/// from solving Poisson's equation via relaxation. of threads used for the
/// calculation
///
/// @param grid
template <typename Type>
void set_zonal_average(pybind11::EigenDRef<Matrix<Type>>& grid,
                       Matrix<bool>& mask, const size_t num_threads) {
  // Captures the detected exceptions in the calculation function
  // (only the last exception captured is kept)
  auto except = std::exception_ptr(nullptr);

  detail::dispatch(
      [&](size_t y_start, size_t y_end) {
        try {
          // Calculation of longitude band means.
          for (size_t iy = y_start; iy < y_end; ++iy) {
            auto acc = boost::accumulators::accumulator_set<
                Type,
                boost::accumulators::stats<boost::accumulators::tag::count,
                                           boost::accumulators::tag::mean>>();
            for (int64_t ix = 0; ix < grid.rows(); ++ix) {
              if (!mask(ix, iy)) {
                acc(grid(ix, iy));
              }
            }

            // The masked value is replaced by the average of the longitude band
            // if it is defined; otherwise it is replaced by zero.
            auto first_guess = boost::accumulators::count(acc)
                                   ? boost::accumulators::mean(acc)
                                   : Type(0);
            for (int64_t ix = 0; ix < grid.rows(); ++ix) {
              if (mask(ix, iy)) {
                grid(ix, iy) = first_guess;
              }
            }
          }
        } catch (...) {
          except = std::current_exception();
        }
      },
      grid.cols(), num_threads);

  if (except != nullptr) {
    std::rethrow_exception(except);
  }
}

///  Replaces all undefined values (NaN) in a grid using the Gauss-Seidel
///  method by relaxation.
///
/// @param grid The grid to be processed
/// @param is_circle True if the X axis of the grid defines a circle.
/// @param relaxation Relaxation constant
/// @return maximum residual value
template <typename Type>
auto gauss_seidel(pybind11::EigenDRef<pyinterp::Matrix<Type>>& grid,
                  Matrix<bool>& mask, const bool is_circle,
                  const Type relaxation, const size_t num_threads) -> Type {
  // Maximum residual values for each thread.
  std::vector<Type> max_residuals(num_threads);

  // Shape of the grid
  auto x_size = grid.rows();
  auto y_size = grid.cols();

  // Captures the detected exceptions in the calculation function
  // (only the last exception captured is kept)
  auto except = std::exception_ptr(nullptr);

  // Gets the index of the pixel (ix, iy) in the matrix.
  auto coordinates = [](const int64_t ix, const int64_t iy,
                        const int64_t size) -> int64_t {
    return iy * size + ix;
  };

  // Thread worker responsible for processing several strips along the y-axis
  // of the grid.
  //
  // @param y_start First index y of the band to be processed.
  // @param y_end Last index y, excluded, of the band to be processed.
  // @param max_residual Maximum residual of this strip.
  // @param pipe_out Last index to be processed on in this band.
  // @param pipe_in Last index processed in the previous band.
  auto worker = [&](int64_t y_start, int64_t y_end, Type* max_residual,
                    std::atomic<int64_t>* pipe_out,
                    std::atomic<int64_t>* pipe_in) -> void {
    // Modifies the value of a masked pixel.
    auto cell_fill = [&grid, &relaxation, &max_residual](
                         int64_t ix0, int64_t ix, int64_t ix1, int64_t iy0,
                         int64_t iy, int64_t iy1) {
      auto residual = (Type(0.25) * (grid(ix0, iy) + grid(ix1, iy) +
                                     grid(ix, iy0) + grid(ix, iy1)) -
                       grid(ix, iy)) *
                      relaxation;
      grid(ix, iy) += residual;
      *max_residual = std::max(*max_residual, std::fabs(residual));
    };

    // Initialization of the maximum value of the residuals of the treated
    // strips.
    *max_residual = Type(0);

    try {
      for (auto ix = 0; ix < x_size; ++ix) {
        //
        auto ix0 = ix == 0 ? (is_circle ? x_size - 1 : 1) : ix - 1;
        auto ix1 = ix == x_size - 1 ? (is_circle ? 0 : x_size - 2) : ix + 1;

        // If necessary, check that the last index required for this block has
        // been processed in the previous band.
        if (pipe_in) {
          auto next_coordinates = coordinates(ix, y_start, y_size);
          while (*pipe_in < next_coordinates) {
            std::this_thread::sleep_for(std::chrono::nanoseconds(5));
          }
        }

        for (auto iy = y_start; iy < y_end; ++iy) {
          auto iy0 = iy == 0 ? 1 : iy - 1;
          auto iy1 = iy == y_size - 1 ? y_size - 2 : iy + 1;
          if (mask(ix, iy)) {
            cell_fill(ix0, ix, ix1, iy0, iy, iy1);
          }
        }

        // If necessary, the other thread responsible for processing the next
        // band is notified.
        if (pipe_out) {
          *pipe_out = coordinates(ix, y_end, y_size);
        }
      }
    } catch (...) {
      except = std::current_exception();
    }
  };

  if (num_threads == 1) {
    worker(0, y_size, &max_residuals[0], nullptr, nullptr);
  } else {
    assert(num_threads >= 2);
    std::vector<std::atomic<int64_t>> pipeline(num_threads);
    std::vector<std::thread> threads;

    int64_t start = 0;
    int64_t shift = y_size / num_threads;

    for (auto& item : pipeline) {
      item = std::numeric_limits<int>::min();
    }

    for (size_t index = 0; index < num_threads - 1; ++index) {
      threads.emplace_back(std::thread(
          worker, start, start + shift, &max_residuals[index], &pipeline[index],
          index == 0 ? nullptr : &pipeline[index - 1]));
      start += shift;
    }
    threads.emplace_back(std::thread(worker, start, y_size,
                                     &max_residuals[num_threads - 1], nullptr,
                                     &pipeline[num_threads - 2]));
    for (auto&& item : threads) {
      item.join();
    }
  }
  if (except != nullptr) {
    std::rethrow_exception(except);
  }
  return *std::max_element(max_residuals.begin(), max_residuals.end());
}

}  // namespace detail

namespace fill {

/// Type of first guess grid.
enum FirstGuess {
  kZero,          //!< Use 0.0 as an initial guess
  kZonalAverage,  //!< Use zonal average in x direction
};

/// Replaces all undefined values (NaN) in a grid using the Gauss-Seidel
/// method by relaxation.
///
/// @param grid The grid to be processed
/// @param is_circle True if the X axis of the grid defines a circle.
/// @param max_iterations Maximum number of iterations to be used by relaxation.
/// @param epsilon Tolerance for ending relaxation before the maximum number of
/// iterations limit.
/// @param relaxation Relaxation constant
/// @param num_threads The number of threads to use for the computation. If 0
/// all CPUs are used. If 1 is given, no parallel computing code is used at all,
/// which is useful for debugging.
/// @return A tuple containing the number of iterations performed and the
/// maximum residual value.
template <typename Type>
auto gauss_seidel(pybind11::EigenDRef<Matrix<Type>>& grid,
                  const FirstGuess first_guess, const bool is_circle,
                  const size_t max_iterations, const Type epsilon,
                  const Type relaxation, size_t num_threads)
    -> std::tuple<size_t, Type> {
  /// If the grid doesn't have an undefined value, this routine has nothing more
  /// to do.
  if (!grid.hasNaN()) {
    return std::make_tuple(0, Type(0));
  }

  /// Calculation of the maximum number of threads if the user chooses.
  if (num_threads == 0) {
    num_threads = std::thread::hardware_concurrency();
  }

  /// Calculation of the position of the undefined values on the grid.
  auto mask = Matrix<bool>(grid.array().isNaN());

  /// Calculation of the first guess with the chosen method
  switch (first_guess) {
    case FirstGuess::kZero:
      grid = (mask.array()).select(0, grid);
      break;
    case FirstGuess::kZonalAverage:
      detail::set_zonal_average(grid, mask, num_threads);
      break;
    default:
      throw std::invalid_argument("Invalid guess type: " +
                                  std::to_string(first_guess));
  }

  // Initialization of the function results.
  size_t iteration = 0;
  Type max_residual = 0;

  for (size_t it = 0; it < max_iterations; ++it) {
    ++iteration;
    max_residual = detail::gauss_seidel<Type>(grid, mask, is_circle, relaxation,
                                              num_threads);
    if (max_residual < epsilon) {
      break;
    }
  }
  return std::make_tuple(iteration, max_residual);
}

// Get the indexes that frame a given index.
inline auto frame_index(const int64_t index, const int64_t size,
                        const bool is_angle, std::vector<int64_t>& frame)
    -> void {
  // Index in the center of the window
  auto center = static_cast<int64_t>(frame.size() / 2);

  for (int64_t ix = 0; ix < static_cast<int64_t>(frame.size()); ++ix) {
    auto idx = index - center + ix;

    // Normalizing longitude?
    if (is_angle) {
      idx = detail::math::remainder(idx, size);
    } else {
      // Otherwise, the symmetrical indexes are used if the indexes are outside
      // the domain definition.
      if (idx < 0 || idx >= size) {
        auto where = detail::math::remainder(idx, (size - 1) * 2);
        if (where >= size) {
          idx = size - 2 - detail::math::remainder(where, size);
        } else {
          idx = detail::math::remainder(where, size);
        }
      }
    }
    frame[ix] = idx;
  }
}

/// Checking the size of the filter window.
inline auto check_windows_size(const std::string& name1, const uint32_t size)
    -> void {
  if (size < 1) {
    throw std::invalid_argument(name1 + " must be >= 1");
  }
}

/// Checking the size of the filter window.
template <typename... Args>
inline auto check_windows_size(const std::string& name1, uint32_t size,
                               Args... args) -> void {
  check_windows_size(name1, size);
  check_windows_size(args...);
}

/// Type of values processed by the Loess filter.
enum ValueType {
  kUndefined,  //!< Undefined values (fill undefined values)
  kDefined,    //!< Defined values (smooth values)
  kAll         //!< Smooth and fill values
};

/// Fills undefined values using a locally weighted regression function or
/// LOESS. The weight function used for LOESS is the tri-cube weight
/// function, w(x)=(1-|d|^{3})^{3}
///
/// @param grid Grid Function on a uniform 2-dimensional grid to be filled.
/// @param nx Number of points of the half-window to be taken into account
/// along the longitude axis.
/// @param nx Number of points of the half-window to be taken into account
/// along the latitude axis.
/// @param value_type Type of values processed by the filter
/// @param num_threads The number of threads to use for the computation. If
/// 0 all CPUs are used. If 1 is given, no parallel computing code is used
/// at all, which is useful for debugging.
/// @return The grid will have all the NaN filled with extrapolated values.
template <typename Type>
auto loess(const Grid2D<Type>& grid, const uint32_t nx, const uint32_t ny,
           const ValueType value_type, const size_t num_threads)
    -> pybind11::array_t<Type> {
  check_windows_size("nx", nx, "ny", ny);
  auto result = pybind11::array_t<Type>(
      pybind11::array::ShapeContainer{grid.x()->size(), grid.y()->size()});
  auto _result = result.template mutable_unchecked<2>();

  // Captures the detected exceptions in the calculation function
  // (only the last exception captured is kept)
  auto except = std::exception_ptr(nullptr);

  auto worker = [&](const size_t start, const size_t end) {
    try {
      // Access to the shared pointer outside the loop to avoid data races
      const auto& x_axis = *grid.x();
      const auto& y_axis = *grid.y();
      auto x_frame = std::vector<int64_t>(nx * 2 + 1);
      auto y_frame = std::vector<int64_t>(ny * 2 + 1);

      for (size_t ix = start; ix < end; ++ix) {
        auto x = x_axis(ix);

        // We retrieve the indexes framing the current value.
        frame_index(ix, x_axis.size(), x_axis.is_angle(), x_frame);

        // Read the first value of the calculated window.
        const auto x0 = x_axis(x_frame[0]);

        // The current value is normalized to the first value in the
        // window.
        if (x_axis.is_angle()) {
          x = detail::math::normalize_angle(x, x0, 360.0);
        }

        for (int64_t iy = 0; iy < y_axis.size(); ++iy) {
          auto z = grid.value(ix, iy);

          // If the current value is masked.
          const auto undefined = std::isnan(z);
          if (value_type == kAll || (value_type == kDefined && !undefined) ||
              (value_type == kUndefined && undefined)) {
            auto y = y_axis(iy);

            // We retrieve the indexes framing the current value.
            frame_index(iy, y_axis.size(), false, y_frame);

            // Initialization of values to calculate the extrapolated
            // value.
            auto value = Type(0);
            auto weight = Type(0);

            // For all the coordinates of the frame.
            for (auto wx : x_frame) {
              auto xi = x_axis(wx);

              // We normalize the window's coordinates to its first value.
              if (x_axis.is_angle()) {
                xi = detail::math::normalize_angle(xi, x0, 360.0);
              }

              for (auto wy : y_frame) {
                auto zi = grid.value(wx, wy);

                // If the value is not masked, its weight is calculated from
                // the tri-cube weight function
                if (!std::isnan(zi)) {
                  const auto power = 3.0;
                  auto d =
                      std::sqrt(detail::math::sqr(((xi - x)) / nx) +
                                detail::math::sqr(((y_axis(wy) - y)) / ny));
                  auto wi = d <= 1 ? std::pow((1.0 - std::pow(d, power)), power)
                                   : 0.0;
                  value += static_cast<Type>(wi * zi);
                  weight += static_cast<Type>(wi);
                }
              }
            }

            // Finally, we calculate the extrapolated value if possible,
            // otherwise we will recopy the masked original value.
            if (weight != 0) {
              z = value / weight;
            }
          }
          _result(ix, iy) = z;
        }
      }
    } catch (...) {
      except = std::current_exception();
    }
  };

  {
    pybind11::gil_scoped_release release;
    detail::dispatch(worker, grid.x()->size(), num_threads);
  }
  return result;
}

template <typename Type, typename AxisType>
auto loess(const Grid3D<Type, AxisType>& grid, const uint32_t nx,
           const uint32_t ny, const ValueType value_type,
           const size_t num_threads) -> pybind11::array_t<Type> {
  check_windows_size("nx", nx, "ny", ny);
  auto result = pybind11::array_t<Type>(pybind11::array::ShapeContainer{
      grid.x()->size(), grid.y()->size(), grid.z()->size()});
  auto _result = result.template mutable_unchecked<3>();

  // Captures the detected exceptions in the calculation function
  // (only the last exception captured is kept)
  auto except = std::exception_ptr(nullptr);

  auto worker = [&](const size_t start, const size_t end) {
    try {
      // Access to the shared pointer outside the loop to avoid data races
      const auto& x_axis = *grid.x();
      const auto& y_axis = *grid.y();
      auto x_frame = std::vector<int64_t>(nx * 2 + 1);
      auto y_frame = std::vector<int64_t>(ny * 2 + 1);

      for (size_t iz = start; iz < end; ++iz) {
        for (int64_t ix = 0; ix < x_axis.size(); ++ix) {
          auto x = x_axis(ix);

          // We retrieve the indexes framing the current value.
          frame_index(ix, x_axis.size(), x_axis.is_angle(), x_frame);

          // Read the first value of the calculated window.
          const auto x0 = x_axis(x_frame[0]);

          // The current value is normalized to the first value in the
          // window.
          if (x_axis.is_angle()) {
            x = detail::math::normalize_angle(x, x0, 360.0);
          }

          for (int64_t iy = 0; iy < y_axis.size(); ++iy) {
            auto z = grid.value(ix, iy, iz);

            // If the current value is masked.
            const auto undefined = std::isnan(z);
            if (value_type == kAll || (value_type == kDefined && !undefined) ||
                (value_type == kUndefined && undefined)) {
              auto y = y_axis(iy);

              // We retrieve the indexes framing the current value.
              frame_index(iy, y_axis.size(), false, y_frame);

              // Initialization of values to calculate the extrapolated
              // value.
              auto value = Type(0);
              auto weight = Type(0);

              // For all the coordinates of the frame.
              for (auto wx : x_frame) {
                auto xi = x_axis(wx);

                // We normalize the window's coordinates to its first value.
                if (x_axis.is_angle()) {
                  xi = detail::math::normalize_angle(xi, x0, 360.0);
                }

                for (auto wy : y_frame) {
                  auto zi = grid.value(wx, wy, iz);

                  // If the value is not masked, its weight is calculated
                  // from the tri-cube weight function
                  if (!std::isnan(zi)) {
                    const auto power = 3.0;
                    auto d =
                        std::sqrt(detail::math::sqr(((xi - x)) / nx) +
                                  detail::math::sqr(((y_axis(wy) - y)) / ny));
                    auto wi = d <= 1
                                  ? std::pow((1.0 - std::pow(d, power)), power)
                                  : 0.0;
                    value += static_cast<Type>(wi * zi);
                    weight += static_cast<Type>(wi);
                  }
                }
              }

              // Finally, we calculate the extrapolated value if possible,
              // otherwise we will recopy the masked original value.
              if (weight != 0) {
                z = value / weight;
              }
            }
            _result(ix, iy, iz) = z;
          }
        }
      }
    } catch (...) {
      except = std::current_exception();
    }
  };

  {
    pybind11::gil_scoped_release release;
    detail::dispatch(worker, grid.z()->size(), num_threads);
  }
  return result;
}

}  // namespace fill
}  // namespace pyinterp


//
// Created by daeyun on 5/10/17.
//

#include "meshdist.h"

#include <Eigen/Dense>
#include <type_traits>
#include <chrono>

#include "lib/common.h"
#include "lib/random_utils.h"
#include "lib/benchmark.h"

namespace scene3d {
namespace meshdist_cgal {

typedef CGAL::Simple_cartesian<float> K;
typedef std::list<K::Triangle_3>::iterator Iterator;
typedef CGAL::AABB_triangle_primitive<K, Iterator> Primitive;
typedef CGAL::AABB_traits<K, Primitive> AABB_triangle_traits;
typedef CGAL::AABB_tree<AABB_triangle_traits> Tree;

void Triangle::ApplyRt(const Mat34 &M) {
  auto R = M.leftCols<3>();
  auto t = M.col(3);
  a = (R * a + t).eval();
  b = (R * b + t).eval();
  c = (R * c + t).eval();
  OnTransformation();
}

void Triangle::ApplyR(const Mat33 &M) {
  a = (M * a).eval();
  b = (M * b).eval();
  c = (M * c).eval();
  OnTransformation();
}

void Triangle::Translate(const Vec3 &dxyz) {
  a += dxyz;
  b += dxyz;
  c += dxyz;
  OnTransformation();
}

void Triangle::Print() const {
  std::cout << a.transpose() << ",      "
            << b.transpose() << ",      "
            << c.transpose() << std::endl;
}

Vec3 Triangle::SamplePoint() const {
  Vec2 v12{scene3d::Random::Rand(), scene3d::Random::Rand()};
  if (v12.sum() > 1) {
    v12 = 1 - v12.array();
  }
  return a + (v12(0) * ab_) + (v12(1) * ac_);
}

double Triangle::Area() const {
  return ab_.cross(ac_).norm() * 0.5f;
}

void Triangle::OnTransformation() {
  assert(a.allFinite());
  assert(b.allFinite());
  assert(c.allFinite());

  ab_ = b - a;
  ac_ = c - a;
}

void SamplePointsOnTriangles(const std::vector<Triangle> &triangles, float density, Points3d *points, std::vector<size_t> *triangle_indices) {
  std::vector<double> areas;
  areas.reserve(triangles.size());
  for (auto &&triangle : triangles) {
    areas.push_back(triangle.Area());
  }

  double surface_area = std::accumulate(areas.begin(), areas.end(), static_cast<double>(0));
  Expects(surface_area > 0.0);

  int num_samples = static_cast<int>(surface_area * density);
  if (num_samples <= 0) {
    // Surface area is non-zero
    // the density parameter is not big enough. This should be a rare case. For now, print warning and just make sure to sample at least one point.
    num_samples = 1;
    LOGGER->warn("density is too small. Surface area is {}", surface_area);
  }
  Expects(num_samples > 0);

  std::discrete_distribution<> distribution(std::begin(areas), std::end(areas));

  Eigen::Matrix<int, Dynamic, 1> counts;

  Eigen::Matrix<int, Dynamic, Dynamic> local_counts;

#pragma omp parallel if (USE_OMP && num_samples > 1e6)
  {
    const int num_threads = omp_get_num_threads();
    const int i_thread = omp_get_thread_num();

#pragma omp single
    {
      local_counts.resize(triangles.size(), num_threads);
      local_counts.fill(0);
    }

#pragma omp for
    for (int i = 0; i < num_samples; ++i) {
      ++local_counts(distribution(scene3d::Random::Engine()), i_thread);
    }
  }

  counts = local_counts.rowwise().sum();

  points->resize(3, num_samples);
  if (triangle_indices) {
    triangle_indices->resize(static_cast<size_t>(num_samples));
  }
  int k = 0;
  for (int i = 0; i < triangles.size(); ++i) {
    for (int j = 0; j < counts(i); ++j) {
      points->col(k) = triangles[i].SamplePoint();
      if (triangle_indices) {
        triangle_indices->push_back((size_t) i);
      }
      ++k;
    }
  }
  Expects(k == num_samples);

  Eigen::PermutationMatrix<Dynamic, Dynamic> perm(points->cols());
  perm.setIdentity();
  std::shuffle(perm.indices().data(), perm.indices().data() + perm.indices().size(), scene3d::Random::Engine());
  *points *= perm; // permute columns
}

// TODO(daeyun): divide by mean distance

float MeshToMeshDistanceOneDirection(const std::vector<Triangle> &from,
                                     const std::vector<Triangle> &to,
                                     float sampling_density,
                                     std::vector<float> *distances) {

  Points3d points;

  SamplePointsOnTriangles(from, sampling_density, &points);

  std::list<K::Triangle_3> triangle_list;
  for (const auto &triangle : to) {
    triangle_list.emplace_back(K::Point_3{triangle.a[0], triangle.a[1], triangle.a[2]},
                               K::Point_3{triangle.b[0], triangle.b[1], triangle.b[2]},
                               K::Point_3{triangle.c[0], triangle.c[1], triangle.c[2]});
  }
  std::vector<K::Point_3> point_list;
  for (int i = 0; i < points.cols(); ++i) {
    point_list.emplace_back(points(0, i), points(1, i), points(2, i));
  }

  int num_triangles = static_cast<int>(to.size());
  int num_points = static_cast<int>(points.cols());

  LOGGER->debug("Computing minimum distances from {} points to {} triangles.", num_points, num_triangles);

  auto start = scene3d::TimeSinceEpoch<std::milli>();
  Tree tree(triangle_list.begin(), triangle_list.end());
  tree.build();
  tree.accelerate_distance_queries();
  LOGGER->debug("Time elapsed for building tree (CGAL): {}", scene3d::TimeSinceEpoch<std::milli>() - start);

  float distance_sum = 0;

#pragma omp parallel for if(USE_OMP) reduction(+:distance_sum) schedule(static)
  for (int i = 0; i < point_list.size(); ++i) {
    float dist = tree.squared_distance(point_list[i]);
    distance_sum += dist;

// TODO: needs refactoring
    if (distances) {
#pragma omp critical
      distances->push_back(dist);
    }
  }

  LOGGER->debug("distance: {}", distance_sum);
  float rms = static_cast<float>(std::sqrt(distance_sum / static_cast<double>(point_list.size())));
  LOGGER->debug("RMS: {}", rms);
  auto elapsed = scene3d::TimeSinceEpoch<std::milli>() - start;
  LOGGER->debug("Time elapsed (CGAL): {} ms", elapsed);

  return rms;
}

float PointsToMeshDistanceOneDirection(const std::vector<std::array<float, 3>> &from,
                                       const std::vector<Triangle> &to) {
  std::list<K::Triangle_3> triangle_list;
  for (const auto &triangle : to) {
    triangle_list.emplace_back(K::Point_3{triangle.a[0], triangle.a[1], triangle.a[2]},
                               K::Point_3{triangle.b[0], triangle.b[1], triangle.b[2]},
                               K::Point_3{triangle.c[0], triangle.c[1], triangle.c[2]});
  }
  std::vector<K::Point_3> point_list;
  for (int i = 0; i < from.size(); ++i) {
    point_list.emplace_back(from[i][0], from[i][1], from[i][2]);
  }

  int num_triangles = static_cast<int>(to.size());
  int num_points = static_cast<int>(from.size());

  LOGGER->debug("Computing minimum distances from {} points to {} triangles.", num_points, num_triangles);

  auto start = scene3d::TimeSinceEpoch<std::milli>();
  Tree tree(triangle_list.begin(), triangle_list.end());
  tree.build();
  tree.accelerate_distance_queries();
  LOGGER->debug("Time elapsed for building tree (CGAL): {}", scene3d::TimeSinceEpoch<std::milli>() - start);

  float distance_sum = 0;

#pragma omp parallel for if(USE_OMP) reduction(+:distance_sum) schedule(static)
  for (int i = 0; i < point_list.size(); ++i) {
    float dist = tree.squared_distance(point_list[i]);
    distance_sum += dist;
  }

  LOGGER->debug("distance: {}", distance_sum);
  float rms = static_cast<float>(std::sqrt(distance_sum / static_cast<double>(point_list.size())));

  LOGGER->debug("RMS: {}", rms);
  auto elapsed = scene3d::TimeSinceEpoch<std::milli>() - start;
  LOGGER->debug("Time elapsed (CGAL): {} ms", elapsed);

  return rms;
}

float MeshToPointsDistanceOneDirection(const std::vector<Triangle> &from,
                                       const std::vector<std::array<float, 3>> &target_points,
                                       float sampling_density) {
  std::list<K::Triangle_3> triangle_list;
  for (const auto &triangle : from) {
    triangle_list.emplace_back(K::Point_3{triangle.a[0], triangle.a[1], triangle.a[2]},
                               K::Point_3{triangle.b[0], triangle.b[1], triangle.b[2]},
                               K::Point_3{triangle.c[0], triangle.c[1], triangle.c[2]});
  }

  Points3d points_on_mesh;
  SamplePointsOnTriangles(from, sampling_density, &points_on_mesh);

  int num_source_points = static_cast<int>(points_on_mesh.cols());
  int num_target_points = static_cast<int>(target_points.size());

  LOGGER->debug("Computing minimum distances from {} points on triangles to {} points.", num_source_points, num_target_points);

  auto start = scene3d::TimeSinceEpoch<std::milli>();

  float distance_sum = 0;

#pragma omp parallel for if(USE_OMP) reduction(+:distance_sum) schedule(static)
  for (int i = 0; i < num_source_points; ++i) {
    const auto source_point = points_on_mesh.col(i);
    double min_dist = kInfinity;
    // TODO(daeyun): this is brute force
    for (int j = 0; j < num_target_points; ++j) {
      double dx = (source_point(0) - target_points[j][0]);
      double dy = (source_point(1) - target_points[j][1]);
      double dz = (source_point(2) - target_points[j][2]);
      double dist = std::sqrt(dx * dx + dy * dy + dz * dz);
      if (dist < min_dist) {
        min_dist = dist;
      }
    }
    distance_sum += min_dist;
  }

  LOGGER->debug("distance: {}", distance_sum);
  float rms = static_cast<float>(std::sqrt(distance_sum / static_cast<double>(num_source_points)));

  LOGGER->debug("RMS: {}", rms);
  auto elapsed = scene3d::TimeSinceEpoch<std::milli>() - start;
  LOGGER->debug("Time elapsed (CGAL): {} ms", elapsed);

  return rms;
}

float MeshToMeshDistance(const std::vector<Triangle> &a, const std::vector<Triangle> &b) {
  auto start = scene3d::TimeSinceEpoch<std::milli>();

  constexpr int kSamplingDensity = 300;

  float d1 = meshdist_cgal::MeshToMeshDistanceOneDirection(a, b, kSamplingDensity);
  float d2 = meshdist_cgal::MeshToMeshDistanceOneDirection(b, a, kSamplingDensity);

  auto elapsed = scene3d::TimeSinceEpoch<std::milli>() - start;
  LOGGER->debug("Time elapsed (MeshToMeshDistance): {} ms", elapsed);
  LOGGER->debug("{}, {}", d1, d2);
  return static_cast<float>((d1 + d2) * 0.5);
}

void TrianglesFromTriMesh(const TriMesh &mesh, std::vector<Triangle> *out) {
  for (const auto &face: mesh.faces) {
    const auto &v1 = mesh.vertices[face[0]];
    const auto &v2 = mesh.vertices[face[1]];
    const auto &v3 = mesh.vertices[face[2]];
    out->push_back(Triangle(Vec3{v1[0], v1[1], v1[2]}, Vec3{v2[0], v2[1], v2[2]}, Vec3{v3[0], v3[1], v3[2]}));
  }
}

}
}


#include "client.hpp"

#include "simulation.hpp"
#include "stats.hpp"
#include "traffic.hpp"
#include "utils.hpp"

#include <boost/property_map/property_map.hpp>

#include <map>
#include <utility>

using namespace std;

client::client(double mht, double mnu, traffic &tra):
  m_htd(1 / mht), m_nud(mnu - 1),
  conn(m_mdl), st(stats::get()), tra(tra)
{
  // Try to setup the connection.
  if (set_up())
    {
      // Register the client with the traffic.
      tra.insert(this);
      // Holding time.
      double ht = m_htd(m_rne);
      // Tear down time.
      tdt = now() + ht;
      schedule(tdt);
    }
  else
    // We didn't manage to establish the connection, and so the client
    // should be deleted.
    tra.delete_me_later(this);
}

void client::operator()(double t)
{
  destroy();
}

bool client::set_up()
{
  // The new demand.
  demand d;
  // The demand end nodes.
  d.first = random_node_pair(m_mdl, m_rne);
  // The number of units the signal requires.  It's Poisson + 1.
  d.second = m_nud(m_rne) + 1;

  // Set up the connection.
  bool status = conn.establish(d);

  // Report whether the connection was established or not.
  st.established(status);

  // If established, report the connection.
  if (status)
    st.established_conn(conn);

  return status;
}

const connection &
client::get_connection() const
{
  return conn;
}

void
client::destroy()
{
  assert(conn.is_established());
  conn.tear_down();
  tra.erase(this);
  tra.delete_me_later(this);
}