#include "LHCOpticsApproximator.h"
#include <vector>
#include <iostream>
#include "TROOT.h"
#include "TMath.h"
#include <boost/shared_ptr.hpp>
#include "CurrentMemoryUsage.h"

ClassImp(LHCOpticsApproximator);
ClassImp(LHCApertureApproximator);

void LHCOpticsApproximator::Init()
{
  out_polynomials.clear();
  apertures_.clear();
  out_polynomials.push_back(&x_parametrisation);
  out_polynomials.push_back(&theta_x_parametrisation);
  out_polynomials.push_back(&y_parametrisation);
  out_polynomials.push_back(&theta_y_parametrisation);

  coord_names.clear();
  coord_names.push_back("x");
  coord_names.push_back("theta_x");
  coord_names.push_back("y");
  coord_names.push_back("theta_y");
  coord_names.push_back("ksi");

  s_begin_ = 0.0;
  s_end_ = 0.0;
  trained_ = false;
}

LHCOpticsApproximator::LHCOpticsApproximator(std::string name, std::string title, TMultiDimFet::EMDFPolyType polynom_type, std::string beam_direction, double nominal_beam_energy)
: x_parametrisation(5, polynom_type, "k"),
  theta_x_parametrisation(5, polynom_type, "k"),
  y_parametrisation(5, polynom_type, "k"),
  theta_y_parametrisation(5, polynom_type, "k")
{
  this->SetName(name.c_str());
  this->SetTitle(title.c_str());
  Init();

  if(beam_direction == "lhcb1")
    beam = lhcb1;
  else if(beam_direction == "lhcb2")
    beam = lhcb2;
  else
    beam = lhcb1;

  nominal_beam_energy_ = nominal_beam_energy;
  nominal_beam_momentum_ = TMath::Sqrt(nominal_beam_energy_*nominal_beam_energy_ - 0.938272029*0.938272029);
}

LHCOpticsApproximator::LHCOpticsApproximator(std::string name, std::string title, TMultiDimFet::EMDFPolyType polynom_type, std::string beam_direction, double nominal_beam_energy,
  TMultiDimFet *given_x_parametrisation, TMultiDimFet *given_theta_x_parametrisation, TMultiDimFet *given_y_parametrisation, TMultiDimFet *given_theta_y_parametrisation)
{
  this->x_parametrisation = *given_x_parametrisation;
  this->theta_x_parametrisation = *given_theta_x_parametrisation;
  this->y_parametrisation = *given_y_parametrisation;
  this->theta_y_parametrisation = *given_theta_y_parametrisation;
  this->SetName(name.c_str());
  this->SetTitle(title.c_str());
  Init();

  if(beam_direction == "lhcb1")
    beam = lhcb1;
  else if(beam_direction == "lhcb2")
    beam = lhcb2;
  else
    beam = lhcb1;

  nominal_beam_energy_ = nominal_beam_energy;
  nominal_beam_momentum_ = TMath::Sqrt(nominal_beam_energy_*nominal_beam_energy_ - 0.938272029*0.938272029);
  trained_ = true;
}

LHCOpticsApproximator::LHCOpticsApproximator()
{
  Init();
  beam = lhcb1;
  nominal_beam_energy_ = 7000;
  nominal_beam_momentum_ = TMath::Sqrt(nominal_beam_energy_*nominal_beam_energy_ - 0.938272029*0.938272029);
}

bool LHCOpticsApproximator::Transport(double *in, double *out, bool check_apertures)
{
  if(in==NULL || out==NULL || !trained_)
    return false;

  bool res = CheckInputRange(in);

  out[0] = x_parametrisation.Eval(in);
  out[1] = theta_x_parametrisation.Eval(in);
  out[2] = y_parametrisation.Eval(in);
  out[3] = theta_y_parametrisation.Eval(in);
  out[4] = in[4];

  if(check_apertures)
  {
    for(int i=0; i<apertures_.size(); i++)
    {
      res = res && apertures_[i].CheckAperture(in);
    }
  }
  return res;
}

bool LHCOpticsApproximator::Transport_m_GeV(double in_pos[3], double in_momentum[3], double out_pos[3], double out_momentum[3],
        bool check_apertures, double z2_z1_dist)
{
  double in[5];
  double out[5];
  double part_mom = 0.0;
  for(int i=0; i<3; i++)
    part_mom += in_momentum[i]*in_momentum[i];

  part_mom = TMath::Sqrt(part_mom);

  in[0] = in_pos[0];
  in[1] = in_momentum[0]/nominal_beam_momentum_;
  in[2] = in_pos[1];
  in[3] = in_momentum[1]/nominal_beam_momentum_;
  in[4] = (part_mom-nominal_beam_momentum_)/nominal_beam_momentum_;

  bool res = Transport(in, out, check_apertures);

  out_pos[0] = out[0];
  out_pos[1] = out[2];
  out_pos[2] = in_pos[2] + z2_z1_dist;

  out_momentum[0] = out[1]*nominal_beam_momentum_;
  out_momentum[1] = out[3]*nominal_beam_momentum_;
  double part_out_total_mom = (out[4]+1)*nominal_beam_momentum_;
  out_momentum[2] = TMath::Sqrt(part_out_total_mom*part_out_total_mom - out_momentum[0]*out_momentum[0] - out_momentum[1]*out_momentum[1]);
  out_momentum[2] = TMath::Sign(out_momentum[2], in_momentum[2]);

  return res;
}

bool LHCOpticsApproximator::Transport(const MadKinematicDescriptor *in, MadKinematicDescriptor *out, bool check_apertures)
{
  if(in==NULL || out==NULL || !trained_)
    return false;

  Double_t input[5];
  Double_t output[5];
  input[0] = in->x;
  input[1] = in->theta_x;
  input[2] = in->y;
  input[3] = in->theta_y;
  input[4] = in->ksi;

  bool res = Transport(input, output, check_apertures);

  out->x = output[0];
  out->theta_x = output[1];
  out->y = output[2];
  out->theta_y = output[3];
  out->ksi = output[4];

  return res;
}

LHCOpticsApproximator::LHCOpticsApproximator(const LHCOpticsApproximator &org) : TNamed(org), x_parametrisation(org.x_parametrisation), theta_x_parametrisation(org.theta_x_parametrisation), y_parametrisation(org.y_parametrisation), theta_y_parametrisation(org.theta_y_parametrisation)
{
//  std::cout<<"LHCOpticsApproximator::LHCOpticsApproximator(const LHCOpticsApproximator &org) entered"<<std::endl;
    void Init();
    s_begin_ = org.s_begin_;
    s_end_ = org.s_end_;
    trained_ = org.trained_;
    apertures_ = org.apertures_;
    beam = org.beam;
    nominal_beam_energy_ = org.nominal_beam_energy_;
    nominal_beam_momentum_ = org.nominal_beam_momentum_;
//  std::cout<<"LHCOpticsApproximator::LHCOpticsApproximator(const LHCOpticsApproximator &org) left"<<std::endl;
}

double LHCOpticsApproximator::GetBegin(){
  return s_begin_;
}

double LHCOpticsApproximator::GetEnd(){
  return s_end_;
}

LHCOpticsApproximator & LHCOpticsApproximator::operator=(const LHCOpticsApproximator &org)
{
  if(this!=&org)
  {
    void Init();
    TNamed::operator=(org);
    s_begin_ = org.s_begin_;
    s_end_ = org.s_end_;
    trained_ = org.trained_;

    x_parametrisation = org.x_parametrisation;
    theta_x_parametrisation = org.theta_x_parametrisation;
    y_parametrisation = org.y_parametrisation;
    theta_y_parametrisation = org.theta_y_parametrisation;
    apertures_ = org.apertures_;
    beam = org.beam;
    nominal_beam_energy_ = org.nominal_beam_energy_;
    nominal_beam_momentum_ = org.nominal_beam_momentum_;
  }
}

void LHCOpticsApproximator::Train(TTree *inp_tree, std::string data_prefix, polynomials_selection mode, int max_degree_x, int max_degree_tx, int max_degree_y, int max_degree_ty, bool common_terms, double *prec)
{
  if(inp_tree==NULL)
    return;

  PrintCurrentMemoryUsage("Train, begin");

  InitializeApproximators(mode, max_degree_x, max_degree_tx, max_degree_y, max_degree_ty, common_terms);
  std::cout<<this->GetName()<<" is being trained..."<<std::endl;

  //in-variables
  //x_in, theta_x_in, y_in, theta_y_in, ksi_in, s_in
  double in_var[6];

  //out-variables
  //x_out, theta_x_out, y_out, theta_y_out, ksi_out, s_out, valid_out;
  double out_var[7];

  //in- out-lables
  std::string x_in_lab = "x_in";
  std::string theta_x_in_lab = "theta_x_in";
  std::string y_in_lab = "y_in";
  std::string theta_y_in_lab = "theta_y_in";
  std::string ksi_in_lab = "ksi_in";
  std::string s_in_lab = "s_in";

  std::string x_out_lab = data_prefix + "_x_out";
  std::string theta_x_out_lab = data_prefix + "_theta_x_out";
  std::string y_out_lab = data_prefix + "_y_out";
  std::string theta_y_out_lab = data_prefix + "_theta_y_out";
  std::string ksi_out_lab = data_prefix + "_ksi_out";
  std::string s_out_lab = data_prefix + "_s_out";
  std::string valid_out_lab = data_prefix + "_valid_out";

  //disable not needed branches to speed up the readin
  inp_tree->SetBranchStatus("*",0);  //disable all branches
  inp_tree->SetBranchStatus(x_in_lab.c_str(),1);
  inp_tree->SetBranchStatus(theta_x_in_lab.c_str(),1);
  inp_tree->SetBranchStatus(y_in_lab.c_str(),1);
  inp_tree->SetBranchStatus(theta_y_in_lab.c_str(),1);
  inp_tree->SetBranchStatus(ksi_in_lab.c_str(),1);
  inp_tree->SetBranchStatus(x_out_lab.c_str(),1);
  inp_tree->SetBranchStatus(theta_x_out_lab.c_str(),1);
  inp_tree->SetBranchStatus(y_out_lab.c_str(),1);
  inp_tree->SetBranchStatus(theta_y_out_lab.c_str(),1);
  inp_tree->SetBranchStatus(s_out_lab.c_str(),1);
  inp_tree->SetBranchStatus(ksi_out_lab.c_str(),1);
  inp_tree->SetBranchStatus(valid_out_lab.c_str(),1);

  //set input data adresses
  inp_tree->SetBranchAddress(x_in_lab.c_str(), &(in_var[0]) );
  inp_tree->SetBranchAddress(theta_x_in_lab.c_str(), &(in_var[1]) );
  inp_tree->SetBranchAddress(y_in_lab.c_str(), &(in_var[2]) );
  inp_tree->SetBranchAddress(theta_y_in_lab.c_str(), &(in_var[3]) );
  inp_tree->SetBranchAddress(ksi_in_lab.c_str(), &(in_var[4]) );
  inp_tree->SetBranchAddress(s_in_lab.c_str(), &(in_var[5]) );

  //set output data adresses
  inp_tree->SetBranchAddress(x_out_lab.c_str(), &(out_var[0]) );
  inp_tree->SetBranchAddress(theta_x_out_lab.c_str(), &(out_var[1]) );
  inp_tree->SetBranchAddress(y_out_lab.c_str(), &(out_var[2]) );
  inp_tree->SetBranchAddress(theta_y_out_lab.c_str(), &(out_var[3]) );
  inp_tree->SetBranchAddress(ksi_out_lab.c_str(), &(out_var[4]) );
  inp_tree->SetBranchAddress(s_out_lab.c_str(), &(out_var[5]) );
  inp_tree->SetBranchAddress(valid_out_lab.c_str(), &(out_var[6]) );

  Long64_t entries = inp_tree->GetEntries();
  if(entries>0)
  {
    inp_tree->SetBranchStatus(s_in_lab.c_str(),1);
    inp_tree->SetBranchStatus(s_out_lab.c_str(),1);
    inp_tree->GetEntry(0);
    s_begin_ = in_var[5];
    s_end_ = out_var[5];
    inp_tree->SetBranchStatus(s_in_lab.c_str(),0);
    inp_tree->SetBranchStatus(s_out_lab.c_str(),0);
  }

  PrintCurrentMemoryUsage("Train, before setting data");
  //set input and output variables for fitting
  for(Long64_t i=0; i<entries; ++i)
  {
    inp_tree->GetEntry(i);
    if(out_var[6] != 0)  //if out data valid
    {
      x_parametrisation.AddRow(in_var, out_var[0], 0);
      theta_x_parametrisation.AddRow(in_var, out_var[1], 0);
      y_parametrisation.AddRow(in_var, out_var[2], 0);
      theta_y_parametrisation.AddRow(in_var, out_var[3], 0);
    }
  }

  std::cout<<"Optical functions parametrizations from "<<s_begin_<<" to "<<s_end_<<std::endl;
  PrintInputRange();
  PrintCurrentMemoryUsage("Train, before fitting");
  for(int i=0; i<4; i++)
  {
    double best_precision=0.0;
    if(prec)
      best_precision = prec[i];
    out_polynomials[i]->FindParameterization(best_precision);
    std::cout<<"Out variable "<<coord_names[i]<<" polynomial"<<std::endl;
    out_polynomials[i]->PrintPolynomialsSpecial("M");
    std::cout<<std::endl;
  }

  trained_ = true;
  PrintCurrentMemoryUsage("Train, function end");
}

void LHCOpticsApproximator::InitializeApproximators(polynomials_selection mode, int max_degree_x, int max_degree_tx, int max_degree_y, int max_degree_ty, bool common_terms)
{
  SetDefaultAproximatorSettings(x_parametrisation, X, max_degree_x);
  SetDefaultAproximatorSettings(theta_x_parametrisation, THETA_X, max_degree_tx);
  SetDefaultAproximatorSettings(y_parametrisation, Y, max_degree_y);
  SetDefaultAproximatorSettings(theta_y_parametrisation, THETA_Y, max_degree_ty);

  if(mode == PREDEFINED)
  {
    SetTermsManually(x_parametrisation, X, max_degree_x, common_terms);
    SetTermsManually(theta_x_parametrisation, THETA_X, max_degree_tx, common_terms);
    SetTermsManually(y_parametrisation, Y, max_degree_y, common_terms);
    SetTermsManually(theta_y_parametrisation, THETA_Y, max_degree_ty, common_terms);
  }
}

void LHCOpticsApproximator::SetDefaultAproximatorSettings(TMultiDimFet &approximator, variable_type var_type, int max_degree)
{
  if(max_degree<1 || max_degree>20)
    max_degree = 10;

  if(var_type == X || var_type == THETA_X)
  {
    Int_t mPowers[] = { 2, 4, 2, 4, max_degree };
    approximator.SetMaxPowers (mPowers);
    approximator.SetMaxFunctions (3000);
    approximator.SetMaxStudy (3000);
    approximator.SetMaxTerms (3000);
    approximator.SetPowerLimit (1.6);
//    approximator.SetMinAngle (2e-4);
//    approximator.SetMaxAngle (10);
    approximator.SetMinRelativeError (1e-13);
  }

  if(var_type == Y || var_type == THETA_Y)
  {
    Int_t mPowers[] = { 2, 4, 2, 4, max_degree };
    approximator.SetMaxPowers (mPowers);
    approximator.SetMaxFunctions (3000);
    approximator.SetMaxStudy (3000);
    approximator.SetMaxTerms (3000);
    approximator.SetPowerLimit (1.6);
//    approximator.SetMinAngle (2e-4);
//    approximator.SetMaxAngle (10);
    approximator.SetMinRelativeError (1e-13);
  }
}

void LHCOpticsApproximator::SetTermsManually(TMultiDimFet &approximator, variable_type variable, int max_degree, bool common_terms)
{
  if(max_degree<1 || max_degree>20)
    max_degree = 10;

  //put terms of shape:
  //1,0,0,0,t    0,1,0,0,t    0,2,0,0,t    0,3,0,0,t    0,0,0,0,t
  //t: 0,1,...,max_degree
//  int total_terms = 5*(max_degree+1);
//  int table_size = total_terms*5;
//  Int_t powers[table_size];

  std::vector<Int_t> term_literals;
  term_literals.reserve(5000);

  if(variable == X || variable == THETA_X)
  {
    //1,0,0,0,t
    for(int i=0; i<=max_degree; ++i)
    {
      term_literals.push_back(1);
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(i);
    }
    //0,1,0,0,t
    for(int i=0; i<=max_degree; ++i)
    {
      term_literals.push_back(0);
      term_literals.push_back(1);
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(i);
    }
    //0,2,0,0,t
    for(int i=0; i<=max_degree; ++i)
    {
      term_literals.push_back(0);
      term_literals.push_back(2);
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(i);
    }
    //0,3,0,0,t
    for(int i=0; i<=max_degree; ++i)
    {
      term_literals.push_back(0);
      term_literals.push_back(3);
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(i);
    }
    //0,0,0,0,t
    for(int i=0; i<=max_degree; ++i)
    {
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(i);
    }
  }

  if(variable == Y || variable == THETA_Y)
  {
    //0,0,1,0,t
    for(int i=0; i<=max_degree; ++i)
    {
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(1);
      term_literals.push_back(0);
      term_literals.push_back(i);
    }
    //0,0,0,1,t
    for(int i=0; i<=max_degree; ++i)
    {
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(1);
      term_literals.push_back(i);
    }
    //0,0,0,2,t
    for(int i=0; i<=max_degree; ++i)
    {
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(2);
      term_literals.push_back(i);
    }
    //0,0,0,3,t
    for(int i=0; i<=max_degree; ++i)
    {
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(3);
      term_literals.push_back(i);
    }
    //0,0,0,0,t
    for(int i=0; i<=max_degree; ++i)
    {
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(0);
      term_literals.push_back(i);
    }
  }

  //push common terms
  if(common_terms)
  {
    term_literals.push_back(1), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(0);
    term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(0);
    term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(0);
    term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(0);
    term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(0);
    term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(1), term_literals.push_back(0);

    term_literals.push_back(1), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(1);
    term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(1);
    term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(1);
    term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(1);
    term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(1);
    term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(1), term_literals.push_back(1);

    term_literals.push_back(1), term_literals.push_back(2), term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(0);
    term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(2), term_literals.push_back(0), term_literals.push_back(0);
    term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(2), term_literals.push_back(0);
    term_literals.push_back(2), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(0);
    term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(2), term_literals.push_back(0), term_literals.push_back(0);
    term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(2), term_literals.push_back(0);
    term_literals.push_back(2), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(0);
    term_literals.push_back(0), term_literals.push_back(2), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(0);
    term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(2), term_literals.push_back(0);
    term_literals.push_back(2), term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(0);
    term_literals.push_back(0), term_literals.push_back(2), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(0);
    term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(2), term_literals.push_back(1), term_literals.push_back(0);

    term_literals.push_back(1), term_literals.push_back(2), term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(1);
    term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(2), term_literals.push_back(0), term_literals.push_back(1);
    term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(2), term_literals.push_back(1);
    term_literals.push_back(2), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(1);
    term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(2), term_literals.push_back(0), term_literals.push_back(1);
    term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(2), term_literals.push_back(1);
    term_literals.push_back(2), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(1);
    term_literals.push_back(0), term_literals.push_back(2), term_literals.push_back(1), term_literals.push_back(0), term_literals.push_back(1);
    term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(2), term_literals.push_back(1);
    term_literals.push_back(2), term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(1);
    term_literals.push_back(0), term_literals.push_back(2), term_literals.push_back(0), term_literals.push_back(1), term_literals.push_back(1);
    term_literals.push_back(0), term_literals.push_back(0), term_literals.push_back(2), term_literals.push_back(1), term_literals.push_back(1);
  }

  Int_t powers[term_literals.size()];
  for(int i=0; i<term_literals.size(); ++i)
  {
    powers[i] = term_literals[i];
  }
  approximator.SetPowers(powers, term_literals.size()/5);
}

void LHCOpticsApproximator::Test(TTree *inp_tree, TFile *f_out, std::string data_prefix, std::string base_out_dir)
{
  PrintCurrentMemoryUsage("Test, begin");
  if(inp_tree==NULL || f_out==NULL)
    return;

  std::cout<<this->GetName()<<" is being tested..."<<std::endl;
  //in-variables
  //x_in, theta_x_in, y_in, theta_y_in, ksi_in, s_in
  double in_var[6];

  //out-variables
  //x_out, theta_x_out, y_out, theta_y_out, ksi_out, s_out, valid_out;
  double out_var[7];

  //in- out-lables
  std::string x_in_lab = "x_in";
  std::string theta_x_in_lab = "theta_x_in";
  std::string y_in_lab = "y_in";
  std::string theta_y_in_lab = "theta_y_in";
  std::string ksi_in_lab = "ksi_in";
  std::string s_in_lab = "s_in";

  std::string x_out_lab = data_prefix + "_x_out";
  std::string theta_x_out_lab = data_prefix + "_theta_x_out";
  std::string y_out_lab = data_prefix + "_y_out";
  std::string theta_y_out_lab = data_prefix + "_theta_y_out";
  std::string ksi_out_lab = data_prefix + "_ksi_out";
  std::string s_out_lab = data_prefix + "_s_out";
  std::string valid_out_lab = data_prefix + "_valid_out";

  //disable not needed branches to speed up the readin
  inp_tree->SetBranchStatus("*",0);  //disable all branches
  inp_tree->SetBranchStatus(x_in_lab.c_str(),1);
  inp_tree->SetBranchStatus(theta_x_in_lab.c_str(),1);
  inp_tree->SetBranchStatus(y_in_lab.c_str(),1);
  inp_tree->SetBranchStatus(theta_y_in_lab.c_str(),1);
  inp_tree->SetBranchStatus(ksi_in_lab.c_str(),1);
  inp_tree->SetBranchStatus(x_out_lab.c_str(),1);
  inp_tree->SetBranchStatus(theta_x_out_lab.c_str(),1);
  inp_tree->SetBranchStatus(y_out_lab.c_str(),1);
  inp_tree->SetBranchStatus(theta_y_out_lab.c_str(),1);
  inp_tree->SetBranchStatus(ksi_out_lab.c_str(),1);
  inp_tree->SetBranchStatus(s_out_lab.c_str(),1);
  inp_tree->SetBranchStatus(valid_out_lab.c_str(),1);

  //set input data adresses
  inp_tree->SetBranchAddress(x_in_lab.c_str(), &(in_var[0]) );
  inp_tree->SetBranchAddress(theta_x_in_lab.c_str(), &(in_var[1]) );
  inp_tree->SetBranchAddress(y_in_lab.c_str(), &(in_var[2]) );
  inp_tree->SetBranchAddress(theta_y_in_lab.c_str(), &(in_var[3]) );
  inp_tree->SetBranchAddress(ksi_in_lab.c_str(), &(in_var[4]) );
  inp_tree->SetBranchAddress(s_in_lab.c_str(), &(in_var[5]) );

  //set output data adresses
  inp_tree->SetBranchAddress(x_out_lab.c_str(), &(out_var[0]) );
  inp_tree->SetBranchAddress(theta_x_out_lab.c_str(), &(out_var[1]) );
  inp_tree->SetBranchAddress(y_out_lab.c_str(), &(out_var[2]) );
  inp_tree->SetBranchAddress(theta_y_out_lab.c_str(), &(out_var[3]) );
  inp_tree->SetBranchAddress(ksi_out_lab.c_str(), &(out_var[4]) );
  inp_tree->SetBranchAddress(s_out_lab.c_str(), &(out_var[5]) );
  inp_tree->SetBranchAddress(valid_out_lab.c_str(), &(out_var[6]) );

  PrintCurrentMemoryUsage("Test, before allocating the histograms");
  //test histogramms
  TH1D *err_hists[4];
  TH2D *err_inp_cor_hists[4][5];
  TH2D *err_out_cor_hists[4][5];

  AllocateErrorHists(err_hists);
  AllocateErrorInputCorHists(err_inp_cor_hists);
  AllocateErrorOutputCorHists(err_out_cor_hists);

  Long64_t entries = inp_tree->GetEntries();
  //set input and output variables for fitting
  for(Long64_t i=0; i<entries; ++i)
  {
    double errors[4];
    inp_tree->GetEntry(i);

    errors[0] = out_var[0] - x_parametrisation.Eval(in_var);
    errors[1] = out_var[1] - theta_x_parametrisation.Eval(in_var);
    errors[2] = out_var[2] - y_parametrisation.Eval(in_var);
    errors[3] = out_var[3] - theta_y_parametrisation.Eval(in_var);

    FillErrorHistograms(errors, err_hists);
    FillErrorDataCorHistograms(errors, in_var, err_inp_cor_hists);
    FillErrorDataCorHistograms(errors, out_var, err_out_cor_hists);
  }

  PrintCurrentMemoryUsage("Test, before writing the histograms");
  WriteHistograms(err_hists, err_inp_cor_hists, err_out_cor_hists, f_out, base_out_dir);
  std::cout<<"Histograms have been written."<<std::endl;

  DeleteErrorHists(err_hists);
  DeleteErrorCorHistograms(err_inp_cor_hists);
  DeleteErrorCorHistograms(err_out_cor_hists);
  PrintCurrentMemoryUsage("Test, end of function");
}

void LHCOpticsApproximator::AllocateErrorHists(TH1D *err_hists[4])
{
  std::vector<std::string> error_labels;
  error_labels.push_back("x error");
  error_labels.push_back("theta_x error");
  error_labels.push_back("y error");
  error_labels.push_back("theta_y error");

  for(int i=0; i<4; ++i)
  {
    err_hists[i] = new TH1D(error_labels[i].c_str(), error_labels[i].c_str(), 100, -0.0000000001, 0.0000000001);
    err_hists[i]->SetXTitle(error_labels[i].c_str());
    err_hists[i]->SetYTitle("counts");
    err_hists[i]->SetDirectory(0);
    err_hists[i]->SetCanExtend(TH1::kAllAxes);
  }
}

void LHCOpticsApproximator::TestAperture(TTree *inp_tree, TTree *out_tree)  //x, theta_x, y, theta_y, ksi, mad_accepted, parametriz_accepted
{
  if(inp_tree==NULL || out_tree==NULL)
    return;

  Long64_t entries = inp_tree->GetEntries();
  double entry[7];
  double parametrization_out[5];

  inp_tree->SetBranchAddress("x", &(entry[0]) );
  inp_tree->SetBranchAddress("theta_x", &(entry[1]) );
  inp_tree->SetBranchAddress("y", &(entry[2]) );
  inp_tree->SetBranchAddress("theta_y", &(entry[3]) );
  inp_tree->SetBranchAddress("ksi", &(entry[4]) );
  inp_tree->SetBranchAddress("mad_accept", &(entry[5]) );
  inp_tree->SetBranchAddress("par_accept", &(entry[6]) );

  out_tree->SetBranchAddress("x", &(entry[0]) );
  out_tree->SetBranchAddress("theta_x", &(entry[1]) );
  out_tree->SetBranchAddress("y", &(entry[2]) );
  out_tree->SetBranchAddress("theta_y", &(entry[3]) );
  out_tree->SetBranchAddress("ksi", &(entry[4]) );
  out_tree->SetBranchAddress("mad_accept", &(entry[5]) );
  out_tree->SetBranchAddress("par_accept", &(entry[6]) );

//  int ind=0;
  for(Long64_t i=0; i<entries; i++)
  {
    inp_tree->GetEntry(i);
//    for(int j=0; j<7; j++)
//      std::cout<<entry[j]<<" ";
//    std::cout<<std::endl;

    bool res = Transport(entry, parametrization_out, true);
//    for(int j=0; j<5; j++)
//      std::cout<<parametrization_out[j]<<" ";
//    std::cout<<"TestAperture "<<res<<std::endl;

    if( res )
      entry[6] = 1.0;
    else
      entry[6] = 0.0;

    out_tree->Fill();
//    if(ind++>300)
//      exit(0);
  }
}

void LHCOpticsApproximator::AllocateErrorInputCorHists(TH2D *err_inp_cor_hists[4][5])
{
  std::vector<std::string> error_labels;
  std::vector<std::string> data_labels;

  error_labels.push_back("x error");
  error_labels.push_back("theta_x error");
  error_labels.push_back("y error");
  error_labels.push_back("theta_y error");

  data_labels.push_back("x input");
  data_labels.push_back("theta_x input");
  data_labels.push_back("y input");
  data_labels.push_back("theta_y input");
  data_labels.push_back("ksi input");

  for(int eri=0; eri<4; ++eri)
  {
    for(int dati=0; dati<5; ++dati)
    {
      std::string name = error_labels[eri] + " vs. " + data_labels[dati];
      std::string title = name;
      err_inp_cor_hists[eri][dati] = new TH2D(name.c_str(), title.c_str(), 100,-0.0000000001,0.0000000001,100,-0.0000000001,0.0000000001);
      err_inp_cor_hists[eri][dati]->SetXTitle(error_labels[eri].c_str());
      err_inp_cor_hists[eri][dati]->SetYTitle(data_labels[dati].c_str());
      err_inp_cor_hists[eri][dati]->SetDirectory(0);
      err_inp_cor_hists[eri][dati]->SetCanExtend(TH2::kAllAxes);
    }
  }
}

void LHCOpticsApproximator::AllocateErrorOutputCorHists(TH2D *err_out_cor_hists[4][5])
{
  std::vector<std::string> error_labels;
  std::vector<std::string> data_labels;

  error_labels.push_back("x error");
  error_labels.push_back("theta_x error");
  error_labels.push_back("y error");
  error_labels.push_back("theta_y error");

  data_labels.push_back("x output");
  data_labels.push_back("theta_x output");
  data_labels.push_back("y output");
  data_labels.push_back("theta_y output");
  data_labels.push_back("ksi output");

  for(int eri=0; eri<4; ++eri)
  {
    for(int dati=0; dati<5; ++dati)
    {
      std::string name = error_labels[eri] + " vs. " + data_labels[dati];
      std::string title = name;
      err_out_cor_hists[eri][dati] = new TH2D(name.c_str(), title.c_str(), 100,-0.0000000001,0.0000000001,100,-0.0000000001,0.0000000001);
      err_out_cor_hists[eri][dati]->SetXTitle(error_labels[eri].c_str());
      err_out_cor_hists[eri][dati]->SetYTitle(data_labels[dati].c_str());
      err_out_cor_hists[eri][dati]->SetDirectory(0);
      err_out_cor_hists[eri][dati]->SetCanExtend(TH2::kAllAxes);
    }
  }
}

void LHCOpticsApproximator::FillErrorHistograms(double errors[4], TH1D *err_hists[4])
{
  for(int i=0; i<4; ++i)
  {
    err_hists[i]->Fill(errors[i]);
  }
}

void LHCOpticsApproximator::FillErrorDataCorHistograms(double errors[4], double var[5], TH2D *err_cor_hists[4][5])
{
  for(int eri=0; eri<4; ++eri)
  {
    for(int dati=0; dati<5; ++dati)
    {
      err_cor_hists[eri][dati]->Fill(errors[eri], var[dati]);
    }
  }
}

void LHCOpticsApproximator::DeleteErrorHists(TH1D *err_hists[4])
{
  for(int i=0; i<4; ++i)
  {
    delete err_hists[i];
  }
}

void LHCOpticsApproximator::DeleteErrorCorHistograms(TH2D *err_cor_hists[4][5])
{
  for(int eri=0; eri<4; ++eri)
  {
    for(int dati=0; dati<5; ++dati)
    {
      delete err_cor_hists[eri][dati];
    }
  }
}

void LHCOpticsApproximator::WriteHistograms(TH1D *err_hists[4], TH2D *err_inp_cor_hists[4][5], TH2D *err_out_cor_hists[4][5], TFile *f_out, std::string base_out_dir)
{
  if(f_out==NULL)
    return;

  f_out->cd();
  if(!gDirectory->cd(base_out_dir.c_str()))
    gDirectory->mkdir(base_out_dir.c_str());

  gDirectory->cd(base_out_dir.c_str());
  gDirectory->mkdir(this->GetName());
  gDirectory->cd(this->GetName());
  gDirectory->mkdir("x");
  gDirectory->mkdir("theta_x");
  gDirectory->mkdir("y");
  gDirectory->mkdir("theta_y");

  gDirectory->cd("x");
  err_hists[0]->Write("", TObject::kWriteDelete);
  for(int i=0; i<5; i++)
  {
    err_inp_cor_hists[0][i]->Write("", TObject::kWriteDelete);
    err_out_cor_hists[0][i]->Write("", TObject::kWriteDelete);
  }

  gDirectory->cd("..");
  gDirectory->cd("theta_x");
  err_hists[1]->Write("", TObject::kWriteDelete);
  for(int i=0; i<5; i++)
  {
    err_inp_cor_hists[1][i]->Write("", TObject::kWriteDelete);
    err_out_cor_hists[1][i]->Write("", TObject::kWriteDelete);
  }

  gDirectory->cd("..");
  gDirectory->cd("y");
  err_hists[2]->Write("", TObject::kWriteDelete);
  for(int i=0; i<5; i++)
  {
    err_inp_cor_hists[2][i]->Write("", TObject::kWriteDelete);
    err_out_cor_hists[2][i]->Write("", TObject::kWriteDelete);
  }

  gDirectory->cd("..");
  gDirectory->cd("theta_y");
  err_hists[3]->Write("", TObject::kWriteDelete);
  for(int i=0; i<5; i++)
  {
    err_inp_cor_hists[3][i]->Write("", TObject::kWriteDelete);
    err_out_cor_hists[3][i]->Write("", TObject::kWriteDelete);
  }
  gDirectory->cd("..");
  gDirectory->cd("..");
}

void LHCOpticsApproximator::PrintInputRange()
{
  const TVectorD* min_var = x_parametrisation.GetMinVariables();
  const TVectorD* max_var = x_parametrisation.GetMaxVariables();

  std::cout<<"Covered input parameters range:"<<std::endl;
  for(int i=0; i<5; i++)
  {
    std::cout<<(*min_var)(i)<<" < "<<coord_names[i]<<" < "<<(*max_var)(i)<<std::endl;
  }
  std::cout<<std::endl;
}

bool LHCOpticsApproximator::CheckInputRange(double *in)  //x, thx, y, thy, ksi
{
  const TVectorD* min_var = x_parametrisation.GetMinVariables();
  const TVectorD* max_var = x_parametrisation.GetMaxVariables();
  bool res = true;

  for(int i=0; i<5; i++)
  {
    res = res && in[i]>=(*min_var)(i) && in[i]<=(*max_var)(i);
//    std::cout<<"test ranges: "<<in[i]<<" "<<(*min_var)(i)<<" "<<in[i]<<" "<<(*max_var)(i)<<std::endl;
  }
//  std::cout<<"result dupa:"<<res<<std::endl;
  return res;
}

void LHCOpticsApproximator::AddRectEllipseAperture(const LHCOpticsApproximator &in, double rect_x, double rect_y, double r_el_x, double r_el_y)
{
  apertures_.push_back(LHCApertureApproximator(in, rect_x, rect_y, r_el_x, r_el_y, LHCApertureApproximator::RECTELLIPSE));
}

//////////////////////////////////////////////////////////////////

LHCApertureApproximator::LHCApertureApproximator()
{
  rect_x_ = rect_y_ = r_el_x_ = r_el_y_ = 0.0;
  ap_type_ = NO_APERTURE;
}

LHCApertureApproximator::LHCApertureApproximator(const LHCOpticsApproximator &in, double rect_x, double rect_y, double r_el_x, double r_el_y,
        aperture_type type) : LHCOpticsApproximator(in)
{
  rect_x_ = rect_x;
  rect_y_ = rect_y;
  r_el_x_ = r_el_x;
  r_el_y_ = r_el_y;
  ap_type_ = type;
}

bool LHCApertureApproximator::CheckAperture(double *in)  //x, thx. y, thy, ksi
{
  double out[5];
  bool result = Transport(in, out);
//  std::cout<<"in apreture: "<<out[0]<<" "<<out[1]<<" "<<out[2]<<" "<<out[3]<<" "<<out[4]<<std::endl;
//  std::cout<<rect_x_<<", "<<rect_y_<<", "<<r_el_x_<<", "<<r_el_y_<<", "<<ap_type_<<std::endl;

//  std::cout<<"LHCApertureApproximator::CheckAperture(double *in)  //x, thx. y, thy, ksi"<<std::endl;
  if(ap_type_==RECTELLIPSE)
  {
//    std::cout<<"before cond. checked: "<<result<<std::endl;
    result = result && out[0]<rect_x_ && out[0]>-rect_x_ && out[2]<rect_y_ && out[2]>-rect_y_ &&
        ( out[0]*out[0]/(r_el_x_*r_el_x_) + out[2]*out[2]/(r_el_y_*r_el_y_) < 1 );
//    std::cout<<"LHCApertureApproximator::CheckAperture(double *in)  //x, thx. y, thy, ksi"<<std::endl;
//    std::cout<<"after cond. checked: "<<result<<std::endl;
  }
  return result;
}

/*
bool LHCApertureApproximator::CheckAperture(MadKinematicDescriptor *in)  //x, thx. y, thy, ksi
{
  MadKinematicDescriptor out;
  bool result = Transport(in, &out);
  if(ap_type_==RECTELLIPSE)
  {

    result = result && out.x<rect_x_ && out.x>-rect_x_ && out.y<rect_y_ && out.y>-rect_y_ &&
        ( out.x*out.x/(r_el_x_*r_el_x_) + out.y*out.y/(r_el_y_*r_el_y_) < 1 );
  }
  return result;
}
*/

void LHCOpticsApproximator::PrintOpticalFunctions()
{
  std::cout<<std::endl<<"Linear terms of optical functions:"<<std::endl;
  for(int i=0; i<4; i++)
  {
    PrintCoordinateOpticalFunctions(*out_polynomials[i], coord_names[i], coord_names);
  }
}

void LHCOpticsApproximator::PrintCoordinateOpticalFunctions(TMultiDimFet &parametrization, const std::string &coord_name, const std::vector<std::string> &input_vars)
{
  double in[5];
  double out;
  double d_out_d_in[5];
  double d_par = 1e-5;
  double bias = 0;

  for(int j=0; j<5; j++)
      in[j]=0.0;

  const TVectorD* min_var = x_parametrisation.GetMinVariables();
  const TVectorD* max_var = x_parametrisation.GetMaxVariables();

  bias = parametrization.Eval(in);

  for(int i=0; i<5; i++)
  {
    for(int j=0; j<5; j++)
      in[j]=0.0;

    d_par = -((*max_var)[i]-(*min_var)[i])/10.0;
    in[i] = d_par;
    d_out_d_in[i] = parametrization.Eval(in);
    in[i] = 0.0;
    d_out_d_in[i] = d_out_d_in[i] - parametrization.Eval(in);
    d_out_d_in[i] = d_out_d_in[i]/d_par;
  }
  std::cout<<coord_name<<" = "<<bias;
  for(int i=0; i<5; i++)
  {
    std::cout<<" + "<<d_out_d_in[i]<<"*"<<input_vars[i];
  }
  std::cout<<std::endl;
}

//real angles in the matrix, MADX convention used only for input
void LHCOpticsApproximator::GetLineariasedTransportMatrixX(
    double mad_init_x, double mad_init_thx, double mad_init_y, double mad_init_thy,
    double mad_init_xi, TMatrixD &transp_matrix, double d_mad_x, double d_mad_thx)
{
  double MADX_momentum_correction_factor = 1.0 + mad_init_xi;
  transp_matrix.ResizeTo(2,2);
  double in[5];
  in[0] = mad_init_x;
  in[1] = mad_init_thx;
  in[2] = mad_init_y;
  in[3] = mad_init_thy;
  in[4] = mad_init_xi;

  double out[5];

  Transport(in, out);
  double x1 = out[0];
  double thx1 = out[1];

  in[0] = mad_init_x + d_mad_x;
  Transport(in, out);
  double x2_dx = out[0];
  double thx2_dx = out[1];

  in[0] = mad_init_x;
  in[1] = mad_init_thx + d_mad_thx;  //?
  Transport(in, out);
  double x2_dthx = out[0];
  double thx2_dthx = out[1];

//  | dx/dx,   dx/dthx    |
//  | dthx/dx, dtchx/dthx |

  transp_matrix(0,0) = (x2_dx-x1)/d_mad_x;
  transp_matrix(1,0) = (thx2_dx-thx1)/(d_mad_x*MADX_momentum_correction_factor);
  transp_matrix(0,1) = MADX_momentum_correction_factor*(x2_dthx-x1)/d_mad_thx;
  transp_matrix(1,1) = (thx2_dthx-thx1)/d_mad_thx;
}

//real angles in the matrix, MADX convention used only for input
void LHCOpticsApproximator::GetLineariasedTransportMatrixY(
    double mad_init_x, double mad_init_thx, double mad_init_y, double mad_init_thy,
    double mad_init_xi, TMatrixD &transp_matrix, double d_mad_y, double d_mad_thy)
{
  double MADX_momentum_correction_factor = 1.0 + mad_init_xi;
  transp_matrix.ResizeTo(2,2);
  double in[5];
  in[0] = mad_init_x;
  in[1] = mad_init_thx;
  in[2] = mad_init_y;
  in[3] = mad_init_thy;
  in[4] = mad_init_xi;

  double out[5];

  Transport(in, out);
  double y1 = out[2];
  double thy1 = out[3];

  in[2] = mad_init_y + d_mad_y;
  Transport(in, out);
  double y2_dy = out[2];
  double thy2_dy = out[3];

  in[2] = mad_init_y;
  in[3] = mad_init_thy + d_mad_thy;  //?
  Transport(in, out);
  double y2_dthy = out[2];
  double thy2_dthy = out[3];

//  | dy/dy,   dy/dthy    |
//  | dthy/dy, dtchy/dthy |

  transp_matrix(0,0) = (y2_dy-y1)/d_mad_y;
  transp_matrix(1,0) = (thy2_dy-thy1)/(d_mad_y*MADX_momentum_correction_factor);
  transp_matrix(0,1) = MADX_momentum_correction_factor*(y2_dthy-y1)/d_mad_thy;
  transp_matrix(1,1) = (thy2_dthy-thy1)/d_mad_thy;
}

//MADX convention used only for input
double LHCOpticsApproximator::GetDx(
    double mad_init_x, double mad_init_thx, double mad_init_y, double mad_init_thy,
    double mad_init_xi, double d_mad_xi)
{
  double in[5];
  in[0] = mad_init_x;
  in[1] = mad_init_thx;
  in[2] = mad_init_y;
  in[3] = mad_init_thy;
  in[4] = mad_init_xi;

  double out[5];

  Transport(in, out);
  double x1 = out[0];

  in[4] = mad_init_xi + d_mad_xi;
  Transport(in, out);
  double x2_dxi = out[0];
  double dispersion = (x2_dxi-x1)/d_mad_xi;

  return dispersion;
}

//MADX convention used only for input
//angular dispersion
double LHCOpticsApproximator::GetDxds(
    double mad_init_x, double mad_init_thx, double mad_init_y, double mad_init_thy,
    double mad_init_xi, double d_mad_xi)
{
  double MADX_momentum_correction_factor = 1.0 + mad_init_xi;
  double in[5];
  in[0] = mad_init_x;
  in[1] = mad_init_thx;
  in[2] = mad_init_y;
  in[3] = mad_init_thy;
  in[4] = mad_init_xi;

  double out[5];

  Transport(in, out);
  double thx1 = out[1]/MADX_momentum_correction_factor;

  in[4] = mad_init_xi + d_mad_xi;
  Transport(in, out);
  double thx2_dxi = out[1]/MADX_momentum_correction_factor;
  double dispersion = (thx2_dxi-thx1)/d_mad_xi;

  return dispersion;
}


// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <Eigen/Core>
#include <franka/model.h>
#include <research_interface/robot/service_types.h>

#include <sstream>

#include "model_library.h"
#include "network.h"

using namespace std::string_literals;

namespace franka {

auto operator++(Frame& frame, int /* dummy */) noexcept -> Frame {
  Frame original = frame;
  frame = static_cast<Frame>(static_cast<std::underlying_type_t<Frame>>(frame) + 1);
  return original;
}

Model::Model(Network& network) : library_{new ModelLibrary(network)} {}

// Has to be declared here, as the ModelLibrary type is incomplete in the header
Model::~Model() noexcept = default;
Model::Model(Model&&) noexcept = default;
auto Model::operator=(Model&&) noexcept -> Model& = default;

auto Model::pose(Frame frame, const franka::RobotState& robot_state) const -> std::array<double, 16> {
  return pose(frame, robot_state.q, robot_state.F_T_EE, robot_state.EE_T_K);
}

std::array<double, 16> Model::pose(
    Frame frame,
    const std::array<double, 7>& q,
    const std::array<double, 16>& F_T_EE,
    const std::array<double, 16>& EE_T_K)
    const {
  std::array<double, 16> output;
  switch (frame) {
    case Frame::kJoint1:
      library_->joint1(q.data(), output.data());
      break;
    case Frame::kJoint2:
      library_->joint2(q.data(), output.data());
      break;
    case Frame::kJoint3:
      library_->joint3(q.data(), output.data());
      break;
    case Frame::kJoint4:
      library_->joint4(q.data(), output.data());
      break;
    case Frame::kJoint5:
      library_->joint5(q.data(), output.data());
      break;
    case Frame::kJoint6:
      library_->joint6(q.data(), output.data());
      break;
    case Frame::kJoint7:
      library_->joint7(q.data(), output.data());
      break;
    case Frame::kFlange:
      library_->flange(q.data(), output.data());
      break;
    case Frame::kEndEffector:
      library_->ee(q.data(), F_T_EE.data(), output.data());
      break;
    case Frame::kStiffness:
      library_->ee(
          q.data(),
          Eigen::Matrix4d(Eigen::Matrix4d(F_T_EE.data()) * Eigen::Matrix4d(EE_T_K.data())).data(),
          output.data());
      break;
    default:
      throw std::invalid_argument("Invalid frame given.");
  }

  return output;
}

auto Model::bodyJacobian(Frame frame, const franka::RobotState& robot_state) const -> std::array<double, 42> {
  return bodyJacobian(frame, robot_state.q, robot_state.F_T_EE, robot_state.EE_T_K);
}

std::array<double, 42> Model::bodyJacobian(
    Frame frame,
    const std::array<double, 7>& q,
    const std::array<double, 16>& F_T_EE,
    const std::array<double, 16>& EE_T_K)
    const {
  std::array<double, 42> output;
  switch (frame) {
    case Frame::kJoint1:
      library_->body_jacobian_joint1(output.data());
      break;
    case Frame::kJoint2:
      library_->body_jacobian_joint2(q.data(), output.data());
      break;
    case Frame::kJoint3:
      library_->body_jacobian_joint3(q.data(), output.data());
      break;
    case Frame::kJoint4:
      library_->body_jacobian_joint4(q.data(), output.data());
      break;
    case Frame::kJoint5:
      library_->body_jacobian_joint5(q.data(), output.data());
      break;
    case Frame::kJoint6:
      library_->body_jacobian_joint6(q.data(), output.data());
      break;
    case Frame::kJoint7:
      library_->body_jacobian_joint7(q.data(), output.data());
      break;
    case Frame::kFlange:
      library_->body_jacobian_flange(q.data(), output.data());
      break;
    case Frame::kEndEffector:
      library_->body_jacobian_ee(q.data(), F_T_EE.data(), output.data());
      break;
    case Frame::kStiffness:
      library_->body_jacobian_ee(
          q.data(),
          Eigen::Matrix4d(Eigen::Matrix4d(F_T_EE.data()) * Eigen::Matrix4d(EE_T_K.data())).data(),
          output.data());
      break;
    default:
      throw std::invalid_argument("Invalid frame given.");
  }

  return output;
}

auto Model::zeroJacobian(Frame frame, const franka::RobotState& robot_state) const -> std::array<double, 42> {
  return zeroJacobian(frame, robot_state.q, robot_state.F_T_EE, robot_state.EE_T_K);
}

std::array<double, 42> Model::zeroJacobian(
    Frame frame,
    const std::array<double, 7>& q,
    const std::array<double, 16>& F_T_EE,
    const std::array<double, 16>& EE_T_K)
    const {
  std::array<double, 42> output;
  switch (frame) {
    case Frame::kJoint1:
      library_->zero_jacobian_joint1(output.data());
      break;
    case Frame::kJoint2:
      library_->zero_jacobian_joint2(q.data(), output.data());
      break;
    case Frame::kJoint3:
      library_->zero_jacobian_joint3(q.data(), output.data());
      break;
    case Frame::kJoint4:
      library_->zero_jacobian_joint4(q.data(), output.data());
      break;
    case Frame::kJoint5:
      library_->zero_jacobian_joint5(q.data(), output.data());
      break;
    case Frame::kJoint6:
      library_->zero_jacobian_joint6(q.data(), output.data());
      break;
    case Frame::kJoint7:
      library_->zero_jacobian_joint7(q.data(), output.data());
      break;
    case Frame::kFlange:
      library_->zero_jacobian_flange(q.data(), output.data());
      break;
    case Frame::kEndEffector:
      library_->zero_jacobian_ee(q.data(), F_T_EE.data(), output.data());
      break;
    case Frame::kStiffness:
      library_->zero_jacobian_ee(
          q.data(),
          Eigen::Matrix4d(Eigen::Matrix4d(F_T_EE.data()) * Eigen::Matrix4d(EE_T_K.data())).data(),
          output.data());
      break;
    default:
      throw std::invalid_argument("Invalid frame given.");
  }

  return output;
}

auto franka::Model::mass(const franka::RobotState& robot_state) const noexcept -> std::array<double, 49> {
  return mass(robot_state.q, robot_state.I_total, robot_state.m_total, robot_state.F_x_Ctotal);
}

std::array<double, 49> franka::Model::mass(
    const std::array<double, 7>& q,
    const std::array<double, 9>& I_total,
    double m_total,
    const std::array<double, 3>& F_x_Ctotal)
    const noexcept {
  std::array<double, 49> output;
  library_->mass(q.data(), I_total.data(), m_total, F_x_Ctotal.data(), output.data());

  return output;
}

auto franka::Model::coriolis(const franka::RobotState& robot_state) const
    noexcept -> std::array<double, 7> {
  return coriolis(robot_state.q, robot_state.dq, robot_state.I_total, robot_state.m_total,
                  robot_state.F_x_Ctotal);
}

std::array<double, 7> franka::Model::coriolis(
    const std::array<double, 7>& q,
    const std::array<double, 7>& dq,
    const std::array<double, 9>& I_total,
    double m_total,
    const std::array<double, 3>& F_x_Ctotal)
    const noexcept {
  std::array<double, 7> output;
  library_->coriolis(q.data(), dq.data(), I_total.data(), m_total, F_x_Ctotal.data(),
                     output.data());

  return output;
}

auto franka::Model::gravity(const franka::RobotState& robot_state,
                                             const std::array<double, 3>& gravity_earth) const
    noexcept -> std::array<double, 7> {
  return gravity(robot_state.q, robot_state.m_total, robot_state.F_x_Ctotal, gravity_earth);
};

std::array<double, 7> franka::Model::gravity(
    const std::array<double, 7>& q,
    double m_total,
    const std::array<double, 3>& F_x_Ctotal,
    const std::array<double, 3>& gravity_earth) const noexcept {
  std::array<double, 7> output;
  library_->gravity(q.data(), gravity_earth.data(), m_total, F_x_Ctotal.data(), output.data());

  return output;
}

}  // namespace franka


#ifndef INCLUDED_scheme_nest_MultiNest_HH
#define INCLUDED_scheme_nest_MultiNest_HH

#include "scheme/nest/NEST.hh"

#include <math.h>
#include <boost/assert.hpp>

namespace scheme { namespace nest {

	///@brief Base class for NEST
	///@tparam Index index type
	///@detail Base class for NEST, virtual NEST interface
	template<
		class Index=uint64_t,
		class BigIndex=uint64_t
	>
	struct MultiNest : public NestBase<Index> {

		typedef NestBase<Index> Nest;
		typedef shared_ptr<Nest> Nestp;
		typedef std::vector<Nestp> Nests;
		typedef std::vector<Index> Indices;

		Index dim_, tot_ncells_;
		Nests nests_;
		Indices ncells_, ncells_pref_sum_;
		Index max_valid_resl_;

		MultiNest(){ init(); }
		MultiNest(Nests const & nests){ init(nests); }

		Index max_valid_resl() const { return max_valid_resl_; }

		void add_nest( Nestp nestp ){
			nests_.push_back(nestp);
			init();
		}

		void init( Nests const & nests ){ nests_ = nests; init(); }

		void init(){
			ncells_.resize(nests_.size());
			ncells_pref_sum_.resize(nests_.size());
			dim_ = 0;
			tot_ncells_ = 1;
            if(nests_.size()==0) return;
			for(size_t i = 0; i < nests_.size(); ++i){
				Nestp nest = nests_[i];
				dim_ += nest->virtual_dim();
				ncells_[i] = nest->virtual_num_cells();
				tot_ncells_ *= ncells_[i];
			}
			ncells_pref_sum_[0] = 1;
			for(size_t i = 1; i < nests_.size(); ++i){
				ncells_pref_sum_[i] = ncells_pref_sum_[i-1] * ncells_[i-1];
			}

			if( dim_ == 0 ){
				max_valid_resl_ = std::numeric_limits<Index>::max();
				return;
			}
			// std::cout << tot_ncells_ << " " << dim_ << std::endl;
			Index const n_hier_bits = sizeof(BigIndex)*8 - std::ceil( log2(tot_ncells_) );
			max_valid_resl_ = n_hier_bits / dim_;
			// std::cout << "MultiNest max resl: " << max_valid_resl_ << std::endl;
		}

		template<class Anys>
		bool get_states(BigIndex const & index, Index resl, Anys & anys) const {
			BOOST_VERIFY( resl <= max_valid_resl_ );
			BOOST_VERIFY(anys.size()==nests_.size());
			BOOST_VERIFY( index < size(resl) );
			BigIndex big_cell_index = index >> dim_*resl;
			BigIndex const one(1);
			BigIndex hier_index = index & ((one<<(dim_*resl))-one);
			// std::cout << "cell_index: " << big_cell_index << std::endl;
			// std::cout << "hier_index: " << hier_index << std::endl;
			Indices hier_indices;
			expand_index( hier_index, hier_indices );
			size_t iindex = 0;
			for(size_t i = 0; i < nests_.size(); ++i){
				Index cell_index = big_cell_index / ncells_pref_sum_[i] % ncells_[i];
				// std::cout << "MultiNest  nest " << resl << ", nest " << i << ", ci " << cell_index << ", ii " << iindex << std::endl;
				if( ! nests_[i]->virtual_get_state( hier_indices, cell_index, iindex, resl, anys[i] ) ) return false;
			}
            return true;
		}

		void expand_index( BigIndex const & index, Indices & out ) const {
			out.resize(dim_);
			for(Index i = 0; i < dim_; ++i){
				out[i] = util::undilate( dim_, index>>i );
			}
		 }

		///@brief need virtual destructor
		virtual ~MultiNest(){}

		///@brief virtual virtual function to set the state of this nest
		///@returns false if invalid index
		virtual bool
		virtual_get_state(
			Index index,
			Index resl,
			boost::any & result
		){
			BOOST_VERIFY( resl <= max_valid_resl_ );
			std::cout << "sheffler: not implemented yet" << std::endl;
			std::exit(-1);
		}

		///@brief virtual virtual function to set the state of this nest
		///@detail will consume DIM indices from hindices vector, starting at iindex, then will increment iindex
		///        for use in composite data structures containing NestBases
		///@returns false if invalid index
		virtual	bool
		virtual_get_state(
			std::vector<Index> const & indices,
			Index cell_index,
			size_t & iindex,
			Index resl,
			boost::any & result
		){
			BOOST_VERIFY( resl <= max_valid_resl_ );
			std::cout << "sheffler: not implemented yet" << std::endl;
			std::exit(-1);
		}

		///@brief get the total size of this NEST at resl
		///@return number of possible states at depth resl
		virtual Index
		virtual_size(Index resl) const {
			BOOST_VERIFY( resl <= max_valid_resl_ );
			Index s = (Index)size(resl);
			BOOST_VERIFY( s <= BigIndex( std::numeric_limits<Index>::max() ) );
			return s;
		}

		///@brief get the number of cells in this nest
		virtual Index
		virtual_num_cells() const {
			return tot_ncells_;
		}

		///@brief get the dimension of this nest
		///@return dimension of Nest
		virtual size_t
		virtual_dim() const { return dim(); }

		size_t dim() const { return dim_; }
		BigIndex size( Index resl ) const {
			return BigIndex((unsigned long)tot_ncells_) * ((BigIndex(1))<<(resl*dim_));
		}

		///@brief virtual function returning index of value (sent as boost::any)
		virtual Index virtual_get_index( boost::any const & val, Index resl ) const {
			BOOST_VERIFY( resl <= max_valid_resl_ );
			// std::cout << "attempt to cast to vector<any> cosnt *" << std::endl;
			std::vector<boost::any> const & anys = *boost::any_cast< std::vector<boost::any> * >(val);
			// std::cout << "   cast success" << std::endl;
			BOOST_VERIFY( nests_.size() == anys.size() );
			std::vector<Index> indices, cell_indices;
			for(int i = 0; i < nests_.size(); ++i){
				if( nests_[i]->virtual_dim()==0 && nests_[i]->virtual_size(resl)==1 ){
                    // if 0-dim and only one choice, take it... avoids problems with DiscreteChoiceNest
                    cell_indices.push_back(0);
				} else {
					Index cell_index_tmp;
					bool status = nests_[i]->virtual_get_indices( anys[i], resl, cell_index_tmp, indices );
					BOOST_VERIFY( status );
					cell_indices.push_back(cell_index_tmp);
				}
			}
			BOOST_VERIFY( cell_indices.size() == nests_.size() );
			BOOST_VERIFY( indices.size() == dim_ );

			// std::cout << "MultiNest virtual_get_index INDICES:";
			// BOOST_FOREACH( Index i, indices ) std::cout << " " << i;
			// std::cout << std::endl;
			// std::cout << "MultiNest virtual_get_index CELL_INDICES:";
			// BOOST_FOREACH( Index i, cell_indices ) std::cout << " " << i;
			// std::cout << std::endl;

			BigIndex index = 0, cell_index = 0;
			for(int i = 0; i <       dim_         ; ++i)      index |= util::dilate( dim_, indices[i] ) << i;
			for(int i = 0; i < cell_indices.size(); ++i) cell_index += cell_indices[i] * ncells_pref_sum_[i];
			return index | cell_index << ( dim_*resl );
		}

		virtual bool
		virtual_get_indices(
			boost::any const & val,
			Index resl,
			Index & cell_index_out,
			std::vector<Index> & indices_out
		 ) const {
			BOOST_VERIFY( resl <= max_valid_resl_ );
			BOOST_VERIFY(false);
		}

		size_t size() const { return nests_.size(); }

	};

// uint64_t const BOGUS_INDEX = 0xFFFFFFFFFFFFFFFFul;

// using std::cout;
// using std::endl;
// using ObjexxFCL::format::F;

// static const uint64_t ONE(1);
// static const uint64_t TWO(2);
// static const uint64_t THREE(3);

// void CompositeNest::set_position( NestIndex const & index, uint64_t resl ) {
// 	uint64_t bindex0 = (index >> (dim_*resl)).touint64();
// 	NestIndex hindex0 = index & (NestIndex(1<< (dim_*resl))-1);
// 	vector<uint64_t> hindex;
// 	expand_index(hindex0,dim_,hindex);
// 	// cout << index << " B" << bindex0 << " H" << hindex0;
// 	// BOOST_FOREACH(uint64_t i,hindex) cout << " " << i;
// 	// cout << endl;
// 	uint64_t nb0 = 1;
// 	vector<uint64_t>::iterator ih = hindex.begin();
// 	vector<uint64_t> atomicindex;
// 	for(unsigned ibase = 0; ibase < nbase_.size(); ++ibase){
// 		uint64_t const & nb = nbase_[ibase];
// 		uint64_t const thisbindex = (( bindex0 / nb0 ) % nb);
// 		// cout << "basei " << (( bindex0 / nb0 ) % nb) << endl;
// 		if(is_dependent_[ibase]){
// 			; // pass
// 		} else if(is0dim_[ibase]){
// 			atomicindex.push_back(thisbindex);
// 		} else {
// 			atomicindex.push_back( *ih + (thisbindex << resl) );
// 			++ih;
// 		}
// 		nb0 *= nb;
// 	}
// 	// cout << " resulting indices from " << index << ": ";
// 	// BOOST_FOREACH(uint64_t i,atomicindex) cout << " " << i;
// 	// cout << endl;
// 	int tmp=0;
// 	BOOST_FOREACH(NestOP gop,nests_) gop->set_position(atomicindex,tmp,resl);

// 	runtime_assert(tmp==(int)atomicindex.size());
// 	BOOST_FOREACH(Size2 ds,dependent_map_){
// 		DependentNest * dNest = dynamic_cast<DependentNest*>(nests_[ds.first ]());
// 		runtime_assert( dNest != 0 );
// 		dNest->modify_xform();
// 	}
// 	// vector<NestOP>::const_iterator ig = nests_.begin();
// 	// BOOST_FOREACH(Xform x,out) cout << x.t << " " << (*(ig++))->name() <<  endl;
//  }

}}

#endif


#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <thread>

#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/MarkerArray.h>

#include <tf/transform_datatypes.h>

#include <igvc_msgs/trajectory.h>
#include <parameter_assertions/assertions.h>

#include "motion_profiler.h"
#include "trajectory_planner.h"

TrajectoryPlanner::TrajectoryPlanner()
{
  ros::NodeHandle nh;
  ros::NodeHandle pNh("~");

  double axle_length;
  SmoothControlOptions smooth_control_options{};
  PathGenerationOptions path_generation_options{};
  TargetSelectionOptions target_selection_options{};
  CurvatureBlendingOptions curvature_blending_options{};

  WheelConstraint wheel_constraints{};
  RobotConstraint robot_constraints{};
  MotionProfilerOptions motion_profiler_options{};
  double target_velocity;

  assertions::param(pNh, "axle_length", axle_length, 0.52);
  assertions::param(pNh, "path_timeout", path_timeout_, 3.0);
  assertions::param(pNh, "node/loop_hz", loop_hz_, 20.0);

  assertions::param(pNh, "smooth_control/k1", smooth_control_options.k1, 1.0);
  assertions::param(pNh, "smooth_control/k2", smooth_control_options.k2, 3.0);

  assertions::param(pNh, "path_generation/simulation_frequency", path_generation_options.simulation_frequency, 100.0);
  assertions::param(pNh, "path_generation/simulation_horizon", path_generation_options.simulation_horizon, 5.0);
  assertions::param(pNh, "path_generation/velocity", path_generation_options.simulation_velocity, 1.0);

  assertions::param(pNh, "target_selection/lookahead_dist", target_selection_options.lookahead_dist, 2.0);
  assertions::param(pNh, "target_selection/reached_distance", target_selection_options.target_reached_distance, 0.05);
  assertions::param(pNh, "target_selection/move_threshold", target_selection_options.target_move_threshold, 0.05);

  assertions::param(pNh, "curvature_blending/blending_distance", curvature_blending_options.blending_distance, 1.0);

  assertions::param(pNh, "wheel_constraints/velocity", wheel_constraints.velocity, 3.0);
  assertions::param(pNh, "wheel_constraints/acceleration", wheel_constraints.acceleration, 5.0);
  assertions::param(pNh, "robot_constraints/velocity", robot_constraints.velocity, 2.0);
  assertions::param(pNh, "robot_constraints/acceleration", robot_constraints.acceleration, 5.0);

  assertions::param(pNh, "motion_profiler/target_velocity", target_velocity, 1.0);
  assertions::param(pNh, "motion_profiler/beta", motion_profiler_options.beta, 2.0);
  assertions::param(pNh, "motion_profiler/lambda", motion_profiler_options.lambda, 1.0);

  assertions::getParam(pNh, "end_distance_threshold", end_distance_threshold);

  double linear_acceleration_curvature_threshold;
  assertions::param(pNh, "linear_velocity_curvature_threshold", linear_acceleration_curvature_threshold, 1e-2);

  if (path_generation_options.simulation_frequency <= 0)
  {
    ROS_WARN_STREAM("Simulation frequency (currently " << path_generation_options.simulation_frequency
                                                       << ") should be greater than 0. Setting to 1 for now.");
    path_generation_options.simulation_frequency = 1;
  }
  controller_ = std::make_unique<SmoothControl>(smooth_control_options, path_generation_options,
                                                target_selection_options, curvature_blending_options, axle_length);
  motion_profiler_ =
      std::make_unique<MotionProfiler>(axle_length, wheel_constraints, robot_constraints, motion_profiler_options,
                                       target_velocity, linear_acceleration_curvature_threshold);

  // load global parameters
  assertions::param(pNh, "stop_dist", stop_dist_, 0.9);

  ros::Subscriber path_sub = nh.subscribe("/path", 1, &TrajectoryPlanner::pathCallback, this);
  ros::Subscriber encoder_sub = nh.subscribe("/encoders", 1, &TrajectoryPlanner::encoderCallback, this);
  ros::Subscriber pose_sub = nh.subscribe("/odometry/filtered", 1, &TrajectoryPlanner::positionCallback, this);
  ros::Subscriber waypoint_sub = nh.subscribe("/waypoint", 1, &TrajectoryPlanner::waypointCallback, this);

  cmd_pub_ = nh.advertise<igvc_msgs::velocity_pair>("/motors", 1);
  target_pub_ = nh.advertise<geometry_msgs::PointStamped>("/target_point", 1);
  trajectory_pub_ = nh.advertise<igvc_msgs::trajectory>("/trajectory", 1);
  smoothed_pub_ = nh.advertise<nav_msgs::Path>("/smoothed", 1);
  debug_pub_ = nh.advertise<nav_msgs::Path>("/trajectory_planner/debug", 1);

  std::thread trajectory_plan_thread(&TrajectoryPlanner::updateTrajectory, this);

  ros::spin();
  ROS_INFO("Shutting down...");
  trajectory_plan_thread.join();
}

void TrajectoryPlanner::pathCallback(const nav_msgs::PathConstPtr& msg)
{
  ROS_DEBUG_STREAM("Follower got path. Size: " << msg->poses.size());
  nav_msgs::PathConstPtr path = getPatchedPath(msg);
  smoothed_pub_.publish(path);
  std::lock_guard<std::mutex> path_mutex(path_mutex_);
  path_ = path;
  last_path_time_ = ros::Time::now();
}

void TrajectoryPlanner::waypointCallback(const geometry_msgs::PointStampedConstPtr& msg)
{
  waypoint_ = msg;
}

/**
Constructs a new trajectory to follow using the current path msg and publishes
the first velocity command from this trajectory.
*/
void TrajectoryPlanner::positionCallback(const nav_msgs::OdometryConstPtr& msg)
{
  state_.setState(msg);
  last_time_ = msg->header.stamp;
}

nav_msgs::PathConstPtr TrajectoryPlanner::getPatchedPath(const nav_msgs::PathConstPtr& msg) const
{
  // Get points between each point to smooth it out
  nav_msgs::PathPtr smoothed_path = boost::make_shared<nav_msgs::Path>();
  smoothed_path->poses.emplace_back(msg->poses.front());
  for (size_t i = 0; i < msg->poses.size() - 1; i++)
  {
    double mid_x = (msg->poses[i].pose.position.x + msg->poses[i + 1].pose.position.x) / 2;
    double mid_y = (msg->poses[i].pose.position.y + msg->poses[i + 1].pose.position.y) / 2;
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = mid_x;
    pose.pose.position.y = mid_y;
    smoothed_path->poses.emplace_back(pose);
  }
  smoothed_path->poses.emplace_back(msg->poses.back());

  // Patch heading
  for (size_t i = 0; i < smoothed_path->poses.size(); i++)
  {
    double delta_x = smoothed_path->poses[i + 1].pose.position.x - smoothed_path->poses[i].pose.position.x;
    double delta_y = smoothed_path->poses[i + 1].pose.position.y - smoothed_path->poses[i].pose.position.y;
    double heading = atan2(delta_y, delta_x);
    smoothed_path->poses[i].pose.orientation = tf::createQuaternionMsgFromYaw(heading);
  }
  smoothed_path->poses.back().pose.orientation = smoothed_path->poses[msg->poses.size() - 2].pose.orientation;
  smoothed_path->header = msg->header;
  return smoothed_path;
}

void TrajectoryPlanner::encoderCallback(const igvc_msgs::velocity_pairConstPtr& msg)
{
  std::lock_guard<std::mutex> state_lock(state_mutex_);
  state_.setVelocity(msg);
  last_time_ = msg->header.stamp;
}

void TrajectoryPlanner::updateTrajectory()
{
  ros::Rate rate(loop_hz_);
  while (ros::ok())
  {
    std::optional<igvc_msgs::trajectoryPtr> trajectory = getSmoothPath();
    if ((ros::Time::now() - last_path_time_).toSec() > path_timeout_)
    {
      ROS_INFO_STREAM_THROTTLE(1, "TIMEOUT ON TRAJ PLANNER " << (ros::Time::now() - last_path_time_).toSec());
      igvc_msgs::trajectoryPtr empty_traj = boost::make_shared<igvc_msgs::trajectory>();
      empty_traj->header.stamp = ros::Time::now();
      empty_traj->header.frame_id = "/odom";
      publishTrajectory(empty_traj);
    }

    if (!trajectory || trajectory.value().get() == nullptr || trajectory.value()->trajectory.empty())
    {
      publishStop();
      continue;
    }

    publishDebug(*trajectory);
    motion_profiler_->profileTrajectory(*trajectory, state_);
    publishTrajectory(*trajectory);
    rate.sleep();
  }
}

void TrajectoryPlanner::publishStop()
{
  igvc_msgs::trajectory trajectory;
  igvc_msgs::trajectory_point trajectory_point;
  trajectory_point.velocity = 0.0;
  trajectory_point.curvature = 0.0;
  trajectory.trajectory.emplace_back(trajectory_point);

  trajectory_pub_.publish(trajectory);
}

std::optional<igvc_msgs::trajectoryPtr> TrajectoryPlanner::getSmoothPath()
{
  if (waypoint_.get() == nullptr)
  {
    ROS_INFO_THROTTLE(1, "Waypoint empty.");
    return std::nullopt;
  }
  if (path_.get() == nullptr || path_->poses.empty() || path_->poses.size() < 2)
  {
    ROS_INFO_THROTTLE(1, "Path empty.");
    return std::nullopt;
  }

  double goal_dist = state_.distTo(waypoint_->point.x, waypoint_->point.y);

  ROS_DEBUG_STREAM_THROTTLE(1, "Distance to waypoint: " << goal_dist << "(m.)");

  igvc_msgs::trajectoryPtr trajectory = boost::make_shared<igvc_msgs::trajectory>();

  std::lock_guard<std::mutex> state_lock(state_mutex_);
  std::lock_guard<std::mutex> path_lock(path_mutex_);
  controller_->getPath(path_, trajectory, state_, end_distance_threshold);
  return trajectory;
}

void TrajectoryPlanner::publishTrajectory(const igvc_msgs::trajectoryConstPtr& trajectory)
{
  trajectory_pub_.publish(trajectory);
}

void TrajectoryPlanner::publishDebug(const igvc_msgs::trajectoryConstPtr& trajectory)
{
  igvc_msgs::trajectory_point point = trajectory->trajectory.front();
  nav_msgs::Path path;
  for (const auto& point : trajectory->trajectory)
  {
    geometry_msgs::PoseStamped pose;
    pose.pose = point.pose;
    pose.header.stamp = point.header.stamp;
    pose.header.frame_id = "/odom";

    path.poses.emplace_back(pose);
  }

  path.header.stamp = trajectory->header.stamp;
  path.header.frame_id = "/odom";
  debug_pub_.publish(path);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "trajectory_planner");
  TrajectoryPlanner path_follower;
  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::model::include.hpp                                  //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_SURVIVAL_MODEL_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_MODEL_INCLUDE_HPP_ER_2009

#include <boost/statistics/survival/model/meta/wrap.hpp>
#include <boost/statistics/survival/model/functional/prepare_weights.hpp>

#endif


#ifndef AIKIDO_PLANNER_PARABOLIC_PARABOLICSMOOTHER_HPP_
#define AIKIDO_PLANNER_PARABOLIC_PARABOLICSMOOTHER_HPP_

#include <Eigen/Dense>

#include "aikido/planner/TrajectoryPostProcessor.hpp"
#include "aikido/trajectory/Interpolated.hpp"
#include "aikido/trajectory/Spline.hpp"

namespace aikido {
namespace planner {
namespace parabolic {

constexpr double DEFAULT_TIMELIMIT = 3.0;
constexpr double DEFAULT_BLEND_RADIUS = 0.5;
constexpr int DEFAULT_BLEND_ITERATIONS = 4;

constexpr double DEFAULT_CHECK_RESOLUTION = 5e-3;
constexpr double DEFAULT_TOLERANCE = 1e-3;

/// Shortcut waypoints in a trajectory using parabolic splines.
///
/// This function smooths `_inputTrajectory' by iteratively sampling
/// two waypoints in a trajectory and trying to find a shortcut using
/// the following algorithm:
///
/// while _timelimit is not out:
/// - Randomly sample two times uniformly at random.
/// - Find two points according two times in _inputTrajectory.
/// - Attempt to connect the two points with a time-optimal parabolic
/// spline.
///   The feasibility of the spline is evaluated using
/// \c _feasibilityCheck .
///
/// \param _inputTrajectory input piecewise Geodesic trajectory
/// \param _feasibilityCheck Check whether a position is feasible
/// \param _maxVelocity maximum velocity for each dimension
/// \param _maxAcceleration maximum acceleration for each dimension
/// \param _rng A random generator for sampling time in shortcut.
/// \param _timelimit The maximum time to allow for doing shortcut
/// \param _checkResolution the resolution in discretizing a segment in
/// checking the feasibility of the segment
/// \param _tolerance this tolerance is used in a piecewise linear
/// discretization that deviates no more than \c _tolerance
/// from the parabolic ramp along any axis, and then checks for
/// configuration and segment feasibility along that piecewise linear path.
/// \return smoothed trajectory that satisfies acceleration constraints
std::unique_ptr<trajectory::Spline> doShortcut(
    const trajectory::Spline& _inputTrajectory,
    aikido::constraint::TestablePtr _feasibilityCheck,
    const Eigen::VectorXd& _maxVelocity,
    const Eigen::VectorXd& _maxAcceleration,
    aikido::common::RNG& _rng,
    double _timelimit = DEFAULT_TIMELIMIT,
    double _checkResolution = DEFAULT_CHECK_RESOLUTION,
    double _tolerance = DEFAULT_TOLERANCE);

/// Blend around waypoints in a trajectory using parabolic splines.
///
/// This function smooths `_inputTrajectory` by blending around
/// waypoints that have zero velocity using the following algorithm:
///
/// while _inputTrajectory has a waypoint with zero velocity:
/// - Construct the time-optimal parabolic spline from the state of the
///  trajectory at time `t - _blendRadius` to the state of the trajectory
///  at time `t + _blendRadius`.
/// - Check the validity of the segment against _feasabilityCheck.
/// - If the segment is valid, replace the portion of the trajectory between
///  times `t - _blendRadius` and `t + _blendRadius` with the segment.
///
/// If blending with _blendRadius fails to remove all waypoints with zero
/// velocity, the process described above repeats with half the initial
/// _blendRadius. Halving occurs _blendIteration times, with the final
/// iteration using a blend radius of
/// _blendRadius / std::pow(2, _blendIterations - 1).
/// \param _inputTrajectory input piecewise Geodesic trajectory
/// \param _feasibilityCheck Check whether a position is feasible
/// \param _maxVelocity maximum velocity for each dimension
/// \param _maxAcceleration maximum acceleration for each dimension
/// \param _blendRadius the radius used in doing blend
/// \param _blendIterations the maximum iteration number in doing blend
/// \param _checkResolution the resolution in discretizing a segment in
/// checking the feasibility of the segment
/// \param _tolerance this tolerance is used in a piecewise linear
/// discretization that deviates no more than \c _tolerance
/// from the parabolic ramp along any axis, and then checks for
/// configuration and segment feasibility along that piecewise linear path.
/// \return smoothed trajectory that satisfies acceleration constraints
std::unique_ptr<trajectory::Spline> doBlend(
    const trajectory::Spline& _inputTrajectory,
    aikido::constraint::TestablePtr _feasibilityCheck,
    const Eigen::VectorXd& _maxVelocity,
    const Eigen::VectorXd& _maxAcceleration,
    double _blendRadius = DEFAULT_BLEND_RADIUS,
    int _blendIterations = DEFAULT_BLEND_ITERATIONS,
    double _checkResolution = DEFAULT_CHECK_RESOLUTION,
    double _tolerance = DEFAULT_TOLERANCE);

/// Shortcut and blends waypoints in a trajectory using parabolic splines.
///
/// This function smooths `_inputTrajectory' by firstly applying shortcut
/// to _inputTrajectory and then using blend to remove segments that have
/// zero velocities.
/// Calling this function is more efficienct than calling those two
/// functions in sequence because it avoids duplicated effort.
/// \param _inputTrajectory input piecewise Geodesic trajectory
/// \param _feasibilityCheck Check whether a position is feasible
/// \param _maxVelocity maximum velocity for each dimension
/// \param _maxAcceleration maximum acceleration for each dimension
/// \param _rng A random generator for sampling time in shortcut.
/// \param _timelimit The maximum time to allow for doing shortcut
/// (unit in second)
/// \param _blendRadius the radius used in doing blend
/// \param _blendIterations the maximum iteration number in doing blend
/// \param _checkResolution the resolution in discretizing a segment in
/// checking the feasibility of the segment
/// \param _tolerance this tolerance is used in a piecewise linear
/// discretization that deviates no more than \c _tolerance
/// from the parabolic ramp along any axis, and then checks for
/// configuration and segment feasibility along that piecewise linear path.
/// \return smoothed trajectory that satisfies acceleration constraints
std::unique_ptr<trajectory::Spline> doShortcutAndBlend(
    const trajectory::Spline& _inputTrajectory,
    aikido::constraint::TestablePtr _feasibilityCheck,
    const Eigen::VectorXd& _maxVelocity,
    const Eigen::VectorXd& _maxAcceleration,
    aikido::common::RNG& _rng,
    double _timelimit = DEFAULT_TIMELIMIT,
    double _blendRadius = DEFAULT_BLEND_RADIUS,
    int _blendIterations = DEFAULT_BLEND_ITERATIONS,
    double _checkResolution = DEFAULT_CHECK_RESOLUTION,
    double _tolerance = DEFAULT_TOLERANCE);

/// Class for performing parabolic smoothing on trajectories
class ParabolicSmoother : public aikido::planner::TrajectoryPostProcessor
{
public:
  /// Hauser postprocessor parameters.
  struct Params
  {
    /// \param _enableShortcut Whether shortcutting is used in smoothing.
    /// \param _enableBlend Whether blending is used in smoothing.
    /// \param _shortcutTimelimit Timelimit for shortcutting. It is ineffective
    /// when _enableShortcut is false.
    /// \param _blendRadius Blend radius for blending. It is ineffective
    /// when _enableBlend is false.
    /// \param _blendIterations Blend iterations for blending. It is
    /// ineffective when _enableBlend is false.
    /// \param _feasibilityCheckResolution The resolution in discretizing
    /// a segment in checking the feasibility of the segment.
    /// \param _feasibilityApproxTolerance This tolerance is used in a
    /// piecewise linear discretization that deviates no more than
    /// \c _feasibilityApproxTolerance from the parabolic ramp along any
    /// axis, and then checks for configuration and segment feasibility along
    /// that piecewise linear path.
    Params(
        bool _enableShortcut = true,
        bool _enableBlend = true,
        double _shortcutTimelimit = DEFAULT_TIMELIMIT,
        double _blendRadius = DEFAULT_BLEND_RADIUS,
        int _blendIterations = DEFAULT_BLEND_ITERATIONS,
        double _feasibilityCheckResolution = DEFAULT_CHECK_RESOLUTION,
        double _feasibilityApproxTolerance = DEFAULT_TOLERANCE)
      : mEnableShortcut(_enableShortcut)
      , mEnableBlend(_enableBlend)
      , mShortcutTimelimit(_shortcutTimelimit)
      , mBlendRadius(_blendRadius)
      , mBlendIterations(_blendIterations)
      , mFeasibilityCheckResolution(_feasibilityCheckResolution)
      , mFeasibilityApproxTolerance(_feasibilityApproxTolerance)
    {
      // Do nothing.
    }

    bool mEnableShortcut;
    bool mEnableBlend;
    double mShortcutTimelimit;
    double mBlendRadius;
    int mBlendIterations;
    double mFeasibilityCheckResolution;
    double mFeasibilityApproxTolerance;
  };

  /// \param _velocityLimits Maximum velocity for each dimension.
  /// \param _accelerationLimits Maximum acceleration for each dimension.
  /// \param _enableShortcut Whether shortcutting is used in smoothing.
  /// \param _enableBlend Whether blending is used in smoothing.
  /// \param _shortcutTimelimit Timelimit for shortcutting. It is ineffective
  /// when _enableShortcut is false.
  /// \param _blendRadius Blend radius for blending. It is ineffective
  /// when _enableBlend is false.
  /// \param _blendIterations Blend iterations for blending. It is
  /// ineffective when _enableBlend is false.
  /// \param _feasibilityCheckResolution The resolution in discretizing
  /// a segment in checking the feasibility of the segment.
  /// \param _feasibilityApproxTolerance This tolerance is used in a
  /// piecewise linear discretization that deviates no more than
  /// \c _feasibilityApproxTolerance from the parabolic ramp along any
  /// axis, and then checks for configuration and segment feasibility along that
  /// piecewise linear path.
  ParabolicSmoother(
      const Eigen::VectorXd& _velocityLimits,
      const Eigen::VectorXd& _accelerationLimits,
      bool _enableShortcut = true,
      bool _enableBlend = true,
      double _shortcutTimelimit = DEFAULT_TIMELIMIT,
      double _blendRadius = DEFAULT_BLEND_RADIUS,
      int _blendIterations = DEFAULT_BLEND_ITERATIONS,
      double _feasibilityCheckResolution = DEFAULT_CHECK_RESOLUTION,
      double _feasibilityApproxTolerance = DEFAULT_TOLERANCE);

  /// \param _velocityLimits Maximum velocity for each dimension.
  /// \param _accelerationLimits Maximum acceleration for each dimension.
  /// \param _params Postprocessor parameters.
  ParabolicSmoother(
      const Eigen::VectorXd& _velocityLimits,
      const Eigen::VectorXd& _accelerationLimits,
      const Params& _params);

  /// Performs parabolic smoothing on an input trajectory.
  /// \param _inputTraj The untimed trajectory for the arm to process.
  /// \param _rng Random number generator.
  /// \param _collisionTestable Collision constraint that must be satisfied
  ///        after prcoessing.
  std::unique_ptr<aikido::trajectory::Spline> postprocess(
      const aikido::trajectory::Interpolated& _inputTraj,
      const aikido::common::RNG& _rng,
      const aikido::constraint::TestablePtr& _collisionTestable) override;

  /// Performs parabolic smoothing on an input *spline* trajectory.
  /// \param _inputTraj The untimed trajectory for the arm to process.
  /// \param _rng Random number generator.
  /// \param _collisionTestable Collision constraint that must be satisfied
  ///        after prcoessing.
  std::unique_ptr<aikido::trajectory::Spline> postprocess(
      const aikido::trajectory::Spline& _inputTraj,
      const aikido::common::RNG& _rng,
      const aikido::constraint::TestablePtr& _collisionTestable) override;

private:
  /// Common logic to do shortcutting and/or blending on the input trajectory
  /// as dictated by mEnableShortcut and mEnableBlend.
  std::unique_ptr<aikido::trajectory::Spline> handleShortcutOrBlend(
      const aikido::trajectory::Spline& _inputTraj,
      const aikido::common::RNG& _rng,
      const aikido::constraint::TestablePtr& _collisionTestable);

  /// Set to the value of \c _feasibilityCheckResolution.
  double mFeasibilityCheckResolution;

  /// Set to the value of \c _feasibilityApproxTolerance.
  double mFeasibilityApproxTolerance;

  /// Set to the value of \c _velocityLimits.
  const Eigen::VectorXd mVelocityLimits;

  /// Set to the value of \c _accelerationLimits.
  const Eigen::VectorXd mAccelerationLimits;

  /// Set to the value of \c _enableShortcut.
  bool mEnableShortcut;

  /// Set to the value of \c _enableBlend.
  bool mEnableBlend;

  /// Set to the value of \c _shortcutTimelimit.
  double mShortcutTimelimit;

  /// Set to the value of \c _blendRadius.
  double mBlendRadius;

  /// Set to the value of \c _blendIterations.
  int mBlendIterations;
};

} // namespace parabolic
} // namespace planner
} // namespace aikido

#endif // ifndef AIKIDO_PLANNER_PARABOLIC_PARABOLICSMOOTHER_HPP_


#include <ros/ros.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2/buffer_core.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/TransformStamped.h>

#include <boost/date_time/posix_time/posix_time.hpp>

#include <fstream>

geometry_msgs::TransformStamped
get_transform(tf2_ros::Buffer& buffer,
              const std::string& gps_origin_frame,
              const std::string& gps_base_frame,
              const std::string& map_frame,
              const std::string& odom_frame,
              const std::string& base_frame) {

    auto odom = buffer.lookupTransform(odom_frame,
                                       base_frame,
                                       ros::Time{0},
                                       ros::Duration{5});
    auto transform = buffer.lookupTransform(gps_origin_frame,
                                            gps_base_frame,
                                            ros::Time{0},
                                            ros::Duration{5});
    tf2::Transform gps_tf, odom_tf;
    tf2::convert(transform.transform, gps_tf);
    tf2::convert(odom.transform, odom_tf);
    // "Subtract" odom_frame (i.e. however much travelled in odom)
    transform.transform = tf2::toMsg(gps_tf * odom_tf.inverse());
    transform.child_frame_id = map_frame;
    return transform;
}

int
main(int argc, char* argv[]) {
    ros::init(argc, argv, "map_origin_broadcaster");

    ros::NodeHandle nh_p{"~"};

    std::string gps_origin_frame;
    std::string gps_base_frame;
    std::string map_frame;
    std::string odom_frame;
    std::string base_frame;
    bool save_on_exit;

    nh_p.param("gps_origin_frame", gps_origin_frame, std::string{"gps_origin"});
    nh_p.param("gps_base_frame", gps_base_frame, std::string{"gps_base_link"});
    nh_p.param("map_frame", map_frame, std::string{"map"});
    nh_p.param("odom_frame", odom_frame, std::string{"odom"});
    nh_p.param("base_frame", base_frame, std::string{"base_link"});
    nh_p.param("save_on_exit", save_on_exit, true);

    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener tf_l{buffer};
    tf2_ros::StaticTransformBroadcaster tf_sb;

    // Make sure that the odom frame actually exists
    while (ros::ok()) {
        bool odom_ready = buffer.canTransform(odom_frame,
                                              base_frame,
                                              ros::Time{0},
                                              ros::Duration{5});
        if (odom_ready) {
            break;
        }

        ROS_WARN_STREAM("Waiting for odometry");
    }

    while (ros::ok()) {
        // Transformation between GPS origin and GPS antenna
        bool gnss_ready = buffer.canTransform(gps_origin_frame,
                                              gps_base_frame,
                                              ros::Time{0},
                                              ros::Duration{5});
        if (gnss_ready) {
            ROS_INFO_STREAM("Okay, got GNSS location of " << map_frame);
            auto transform = get_transform(buffer,
                                           gps_origin_frame,
                                           gps_base_frame,
                                           map_frame,
                                           odom_frame,
                                           base_frame);
            tf_sb.sendTransform(transform);
            if (save_on_exit) {
                // In the form: 2017-10-27T10:31:05
                std::string datetime =
                    boost::posix_time::to_iso_extended_string(transform.header.stamp.toBoost());
                std::ofstream ofs;
                ofs.open(datetime + ".csv");
                // Write header
                ofs << "stamp" << ","
                    << "gps_origin_frame" << ","
                    << "map_frame" << ","
                    << "translation.x" << ","
                    << "translation.y" << ","
                    << "translation.z" << ","
                    << "rotation.x" << ","
                    << "rotation.y" << ","
                    << "rotation.z" << ","
                    << "rotation.w" << std::endl;
                // Write data
                ofs << datetime << ","
                    << transform.header.frame_id << ","
                    << transform.child_frame_id << ","
                    << transform.transform.translation.x << ","
                    << transform.transform.translation.y << ","
                    << transform.transform.translation.z << ","
                    << transform.transform.rotation.x << ","
                    << transform.transform.rotation.y << ","
                    << transform.transform.rotation.z << ","
                    << transform.transform.rotation.w << std::endl;
                ofs.close();
            }
            break;
        }

        ROS_WARN_STREAM("Waiting for GNSS data");
    }

	ros::spin();

    return 0;
}


#include "gtest/gtest.h"

#include "../a2o/utils/geometry/Angle.h"
#include "../a2o/utils/geometry/Pose2D.h"
#include "../a2o/worldmodel/odometry/SteeringAngleOdometry.h"

#include <boost/shared_ptr.hpp>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <cmath>

using namespace A2O;
using namespace std;
using namespace Eigen;

namespace {

class SteeringAngleOdometryTest: public ::testing::Test {
protected:
  SteeringAngleOdometryTest() {};
  virtual ~SteeringAngleOdometryTest() {};

  virtual void SetUp() {
  };
  virtual void TearDown() {
  };
};

// TODO: Refine Test
// TEST_F(SteeringAngleOdometryTest, update) {
//   SteeringAngleOdometry testee;
// //   double wheelTick = 0;
// //   double steeringAngle = 0;
// //   long time = 0;
// //   testee.update(wheelTick, steeringAngle, time);
//   Pose2D resultPose;
//   // const double& WheelTicks, const double& steeringAngle, const long& time
//   resultPose = testee.update(0, 0, 0);
// //   std::cout << "result: angle: " << resultPose.getAngle().deg() << " x: " << resultPose.x() << " y: " << resultPose.y() << std::endl;
//   ASSERT_NEAR(resultPose.x(), 0, 0.000001);
//   ASSERT_NEAR(resultPose.y(), 0, 0.000001);
//   ASSERT_NEAR(resultPose.getAngle().deg(), 0, 0.000001);
//
//
//   resultPose = testee.update(4,0, 10);
// //  std::cout << "result: angle: " << resultPose.getAngle().deg() << " x: " << resultPose.x() << " y: " << resultPose.y() << std::endl;
//   ASSERT_NEAR(resultPose.x(), 0.136659, 0.000001);
//   ASSERT_NEAR(resultPose.y(), 0, 0.000001);
//   ASSERT_NEAR(resultPose.getAngle().deg(), 0, 0.000001);
//
// resultPose = testee.update(10, 0, 20);
// //   std::cout << "result2: angle: " << resultPose.getAngle().deg() << " x: " << resultPose.x() << " y: " << resultPose.y() << std::endl;
//   ASSERT_NEAR(resultPose.x(), 0.341648, 0.000001);
//   ASSERT_NEAR(resultPose.y(), 0, 0.000001);
//   ASSERT_NEAR(resultPose.getAngle().deg(), 0, 0.001);
//
//
//   resultPose = testee.update(20, -20, 30);
// //   std::cout << "result: rechts angle: " << resultPose.getAngle().deg() << " x: " << resultPose.x() << " y: " << resultPose.y() << std::endl;
//   ASSERT_NEAR(resultPose.x(), 0.670768, 0.000001);
//   ASSERT_NEAR(resultPose.y(), -0.122284, 0.000001);
//   ASSERT_NEAR(resultPose.getAngle().deg(), -13.2643, 0.001);
//
//   resultPose = testee.update(30, 20, 40);
// //   std::cout << "result: links angle: " << resultPose.getAngle().deg() << " x: " << resultPose.x() << " y: " << resultPose.y() << std::endl;
//   ASSERT_NEAR(resultPose.x(), 1.01916, 0.00001);
//   ASSERT_NEAR(resultPose.y(), -0.0787768, 0.00001);
//   ASSERT_NEAR(resultPose.getAngle().deg(), 0, 0.001);
//
//
//   resultPose = testee.update(40, 20, 40);
// //   std::cout << "result: links  angle: " << resultPose.getAngle().deg() << " x: " << resultPose.x() << " y: " << resultPose.y() << std::endl;
//   ASSERT_NEAR(resultPose.x(), 1.34828, 0.00001);
//   ASSERT_NEAR(resultPose.y(), 0.0435069, 0.00001);
//   ASSERT_NEAR(resultPose.getAngle().deg(), 13.2643, 0.001);
//
//   resultPose = testee.update(50, -20, 40);
// //   std::cout << "result: rechts angle: " << resultPose.getAngle().deg() << " x: " << resultPose.x() << " y: " << resultPose.y() << std::endl;
//   ASSERT_NEAR(resultPose.x(), 1.69668, 0.00001);
//   ASSERT_NEAR(resultPose.y(), 0, 0.00001);
//   ASSERT_NEAR(resultPose.getAngle().deg(), 0, 0.001);
//
// }

// TEST_F(SteeringAngleOdometryTest, update2) {
//
//   const double diameter = 0.105;
//   const double frontAxleLength = 0.24;
//   const double rearAxleLength = 0.26;
//   const Eigen::Vector3d frontAxle (0.18,0,0);
//   const Eigen::Vector3d rearAxle (-0.18, 0 ,0);
//
//   SteeringAngleOdometry* testee = new SteeringAngleOdometry(diameter, frontAxle, frontAxleLength, rearAxle, rearAxleLength);
//   Pose2D resultPose;
//   Angle angle = Angle::deg(178.634);
//   Pose2D pose(0, 0, angle);
//   testee->setCurrentPose(pose);
//     resultPose = testee->update(0, 0, 0);
// //   std::cout << "result: angle: " << resultPose.getAngle().deg() << " x: " << resultPose.x() << " y: " << resultPose.y() << std::endl;
//
//   resultPose = testee->update(1, 23.3721, 10);
// //   std::cout << "3, 25 result: angle: " << resultPose.getAngle().deg() << " x: " << resultPose.x() << " y: " << resultPose.y() << std::endl;
//   ASSERT_NEAR(resultPose.x(), -0.0350332, 0.000001);
//   ASSERT_NEAR(resultPose.y(), -0.0238322, 0.000001);
//   ASSERT_NEAR(resultPose.getAngle().deg(), -174.664, 0.01);
// }

TEST_F(SteeringAngleOdometryTest, update3) {

  const double diameter = 0.105;
  const double frontAxleLength = 0.24;
  const Eigen::Vector3d frontAxle (0.18,0,0);

  SteeringAngleOdometry* testee = new SteeringAngleOdometry(diameter, frontAxle, frontAxleLength, frontAxleLength);
  Pose2D resultPose;
  Angle angle = Angle::deg(0);
  Pose2D pose(0.0, 0.0, angle);
  testee->setCurrentPose(pose);
    resultPose = testee->update(0, 0);
  std::cout << "result: angle: " << resultPose.getAngle().deg() << " x: " << resultPose.x() << " y: " << resultPose.y() << std::endl;

  resultPose = testee->update(1, 23.0233);
  std::cout << "3, 25 result: angle: " << resultPose.getAngle().deg() << " x: " << resultPose.x() << " y: " << resultPose.y() << std::endl;
//   ASSERT_NEAR(resultPose.x(), -0.0350332, 0.000001);
//   ASSERT_NEAR(resultPose.y(), -0.0238322, 0.000001);
//   ASSERT_NEAR(resultPose.getAngle().deg(), -174.664, 0.01);
}
}