/*
 * Software License Agreement (Apache License)
 *
 * Copyright (c) 2014, Southwest Research Institute
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <gtest/gtest.h>
#include <yaml-cpp/yaml.h>
#include <fstream>
#include <iostream>

#include <Eigen/Geometry>
#include <Eigen/Core>
#include <industrial_extrinsic_cal/ceres_costs_utils.hpp>

using namespace industrial_extrinsic_cal;

Point3d xformPoint(Point3d &original_point, double &ax, double &ay, double &az, double &x, double&y, double &z);

class Observation
{
public:
  Observation()
  {
    point_id = 0;
    image_loc_x = 0.0;
    image_loc_y = 0.0;
  };
  ~Observation(){};
  int point_id;
  double image_loc_x;
  double image_loc_y;
};

Observation projectPoint(CameraParameters C, Point3d P)
{
  double p[3];
  double pt[3];
  pt[0] = P.x;
  pt[1] = P.y;
  pt[2] = P.z;

  /* transform point into camera frame */
  /* note, camera transform takes points from camera frame into world frame */
  ceres::AngleAxisRotatePoint(C.angle_axis, pt, p);

  p[0] += C.position[0];
  p[1] += C.position[1];
  p[2] += C.position[2];

  double xp = p[0] / p[2];
  double yp = p[1] / p[2];

  double r2 = xp * xp + yp * yp;
  double r4 = r2 * r2;
  double r6 = r2 * r4;

  double xp2 = xp * xp; /* temporary variables square of others */
  double yp2 = yp * yp;

  /* apply the distortion coefficients to refine pixel location */
  double xpp = xp + C.distortion_k1 * r2 * xp
    + C.distortion_k2 * r4 * xp
    + C.distortion_k3 * r6 * xp
    + C.distortion_p2 * (r2 + 2 * xp2)
    + C.distortion_p1 * xp * yp * 2.0;
  double ypp = yp + C.distortion_k1 * r2 * yp
    + C.distortion_k2 * r4 * yp
    + C.distortion_k3 * r6 * yp
    + C.distortion_p1 * (r2 + 2 * yp2)
    + C.distortion_p2 * xp * yp * 2.0;

  /* perform projection using focal length and camera center into image plane */
  Observation O;
  O.point_id = 0;
  O.image_loc_x = C.focal_length_x * xpp + C.center_x;
  O.image_loc_y = C.focal_length_y * ypp + C.center_y;

  return (O);
}

// GLOBAL VARIABLES FOR TESTING
std::vector<Point3d> created_points;
double aa[3]; // angle axis known/set
double p[3]; // point rotated known/set
std::vector<Point3d> transformed_points;
std::vector<Observation> observations;
CameraParameters C;

TEST(IndustrialExtrinsicCalCeresSuite, rotationProduct)
{
  double R1[9];
  double R2[9];
  double R3[9];
  double R1p[9];
  double R2p[9];

  // create 2 poses, one the inverse of the other
  // multiply them and see if we get identity
  Pose6d P1, P2;
  P1.setAngleAxis(.1, .2, 3.0);
  P2 = P1.getInverse();
  ceres::AngleAxisToRotationMatrix(P1.pb_aa,R1);
  ceres::AngleAxisToRotationMatrix(P2.pb_aa,R2);
  rotationProduct(R1,  R2,  R3);
  // NOTE, this should work regardless of row/column major snaffos
  for(int i=0; i<3; i++){
    for(int j=0; j<3; j++){
      if(i==j){
	ASSERT_NEAR( 1.0, R3[i+3*j], .001);
      }
      else{
	ASSERT_NEAR( 0.0, R3[i+3*j], .001);
      }
    }
  }
}
TEST(IndustrialExtrinsicCalCeresSuite, extractCameraIntrinsics)
{
  C.angle_axis[0]=0.0;
  C.angle_axis[1]=0.0;
  C.angle_axis[2]=0.0;
  C.position[0]=0.0;
  C.position[1]=0.0;
  C.position[2]=0.0;
  C.focal_length_x=525;
  C.focal_length_y=525;
  C.center_x=320;
  C.center_y=240;
  C.distortion_k1=0.01;
  C.distortion_k2=0.02;
  C.distortion_k3=0.03;
  C.distortion_p1=0.01;
  C.distortion_p2=0.01;

  double intrinsics[9];
  intrinsics[0] = C.focal_length_x;
  intrinsics[1] = C.focal_length_y;
  intrinsics[2] = C.center_x;
  intrinsics[3] = C.center_y;
  intrinsics[4] = C.distortion_k1;
  intrinsics[5] = C.distortion_k2;
  intrinsics[6] = C.distortion_k3;
  intrinsics[7] = C.distortion_p1;
  intrinsics[8] = C.distortion_p2;

  double fx, fy, cx, cy, k1, k2, k3, p1, p2;
  extractCameraIntrinsics(C.pb_intrinsics, fx, fy, cx, cy, k1, k2, k3, p1, p2);

  ASSERT_NEAR(C.focal_length_x, fx, .00001);
  ASSERT_NEAR(C.focal_length_y, fy, .00001);
  ASSERT_NEAR(C.center_x, cx, .00001);
  ASSERT_NEAR(C.center_y, cy, .00001);
  ASSERT_NEAR(C.distortion_k1, k1, .00001);
  ASSERT_NEAR(C.distortion_k2, k2, .00001);
  ASSERT_NEAR(C.distortion_k3, k3, .00001);
  ASSERT_NEAR(C.distortion_p1, p1, .00001);
  ASSERT_NEAR(C.distortion_p2, p2, .00001);
}
TEST(IndustrialExtrinsicCalCeresSuite, rotationInverse)
{
  double R1[9], R2[9];
  double angle_axis[3];
  angle_axis[0] = .1;
  angle_axis[1] = -3.0;
  angle_axis[2] = .5;
  ceres::AngleAxisToRotationMatrix(angle_axis, R1);
  rotationInverse(R1, R2);
  for(int i=0; i<3; i++){
    for(int j=0; j<3; j++){
      int index1 = i*3+j;
      int index2 = i+j*3;
      ASSERT_NEAR(R1[index1],R2[index2], .00001);
    }
  }
}
TEST(IndustrialExtrinsicCalCeresSuite, transformPoint)
{
  double point[3], tx[3];
  tx[0] = 15.0;
  tx[1] = 30.0;
  tx[2] = 23;
  point[0] = 10.0;
  point[1] = -35;
  point[2] = -100;
  double angle_axis[3];
  angle_axis[0] = .1;
  angle_axis[1] = -3.0;
  angle_axis[2] = .5;
  double t_point[3];
  transformPoint(angle_axis, tx, point, t_point);

  Pose6d pose;
  pose.setAngleAxis(angle_axis[0], angle_axis[1], angle_axis[2]);
  pose.setOrigin(tx[0],tx[1],tx[2]);

  // invert transformation and apply
  Pose6d posei = pose.getInverse();
  angle_axis[0] = posei.ax;
  angle_axis[1] = posei.ay;
  angle_axis[2] = posei.az;
  tx[0] = posei.x;
  tx[1] = posei.y;
  tx[2] = posei.z;
  double tt_point[3];
  transformPoint(angle_axis, tx, t_point, tt_point);
  ASSERT_NEAR(tt_point[0], point[0], .00001);
  ASSERT_NEAR(tt_point[1], point[1], .00001);
  ASSERT_NEAR(tt_point[2], point[2], .00001);
}

TEST(IndustrialExtrinsicCalCeresSuite, poseTransformPoint)
{
  Pose6d pose;
  Pose6d posei;
  double point[3];
  point[0] = 10.0;
  point[1] = -35;
  point[2] = -100;
  pose.setAngleAxis(1.2, 2.2, 3.3);
  pose.setOrigin(1.2, 2.2, 3.3);
  posei  = pose.getInverse();

  double t_point[3];
  double tt_point[3];
  poseTransformPoint(pose, point, t_point);
  poseTransformPoint(posei, t_point, tt_point);
  ASSERT_NEAR(tt_point[0], point[0], .00001);
  ASSERT_NEAR(tt_point[1], point[1], .00001);
  ASSERT_NEAR(tt_point[2], point[2], .00001);

}

TEST(IndustrialExtrinsicCalCeresSuite, transformPoint3d)
{
  double  tx[3];
  tx[0] = 15.0;
  tx[1] = 30.0;
  tx[2] = 23;
  Point3d point;
  point.x = 10.0;
  point.y = -35;
  point.z = -100;
  double angle_axis[3];
  angle_axis[0] = .1;
  angle_axis[1] = -3.0;
  angle_axis[2] = .5;
  double t_point[3];
  transformPoint3d(angle_axis, tx, point, t_point);
  Point3d t_point3d;
  t_point3d.x = t_point[0];
  t_point3d.y = t_point[1];
  t_point3d.z = t_point[2];

  Pose6d pose;
  pose.setAngleAxis(angle_axis[0], angle_axis[1], angle_axis[2]);
  pose.setOrigin(tx[0],tx[1],tx[2]);

  // invert transformation and apply
  Pose6d posei = pose.getInverse();
  angle_axis[0] = posei.ax;
  angle_axis[1] = posei.ay;
  angle_axis[2] = posei.az;
  tx[0] = posei.x;
  tx[1] = posei.y;
  tx[2] = posei.z;

  double tt_point[3];
  transformPoint3d(angle_axis, tx, t_point3d, tt_point);
  ASSERT_NEAR(tt_point[0], point.x, .00001);
  ASSERT_NEAR(tt_point[1], point.y, .00001);
  ASSERT_NEAR(tt_point[2], point.z, .00001);

}
TEST(IndustrialExtrinsicCalCeresSuite, poseRotationMatrix)
{
  Pose6d pose;
  double ax = .5; // must use small values to avoid alternate solutions
  double ay = .23;
  double az = .45;
  pose.setAngleAxis(ax, ay, az);
  pose.setOrigin(11.5, 21.5, 31.5);
  double R[9];
  poseRotationMatrix(pose, R);
  double aa[3];
  ceres::RotationMatrixToAngleAxis(R, aa);
  ASSERT_NEAR(aa[0], ax, .00001);
  ASSERT_NEAR(aa[1], ay, .00001);
  ASSERT_NEAR(aa[2], az, .00001);
}

TEST(IndustrialExtrinsicCalCeresSuite, cameraPntResidualDist)
{
}

TEST(IndustrialExtrinsicCalCeresSuite, cameraCircResidualDist)
{
}
TEST(IndustrialExtrinsicCalCeresSuite, cameraCircResidual)
{
}
// used for intrinsic cal
// Camera may move to several locations (multiple extrinsics)
// target has no transform, origin is origin of target, single static target
// points are known, no calibraition of the target itself
TEST(IndustrialExtrinsicCalCeresSuite, CircleCameraReprjErrorWithDistortion)
{
}
// used for extrinsic cal of camera on robot with target on ground
// finds transform from link to camera, and transform from robot to target
// should have multiple images from different robot locations
TEST(IndustrialExtrinsicCalCeresSuite, LinkCameraCircleTargetReprjError)
{
}

TEST(IndustrialExtrinsicCalCeresSuite, create_points)
{
  Point3d x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11, x12, x13, x14, x15, x16, x17, x18, x19, x20;
  x1.pb[0]=1.0;x1.pb[1]=1.0;x1.pb[2]=1;
  created_points.push_back(x1);
  x2.pb[0]=0;x2.pb[1]=0;x2.pb[2]=0.01;
  created_points.push_back(x2);
  x3.pb[0]=0.65;x3.pb[1]=0.94;x3.pb[2]=0.01;
  created_points.push_back(x3);
  x4.pb[0]=0.2;x4.pb[1]=0.3;x4.pb[2]=0.01;
  created_points.push_back(x4);
  x5.pb[0]=0.372;x5.pb[1]=0.4;x5.pb[2]=0.4;
  created_points.push_back(x5);
  x6.pb[0]=0.3762;x6.pb[1]=0.8;x6.pb[2]=0.3;
  created_points.push_back(x6);
  x7.pb[0]=0.4;x7.pb[1]=0.389;x7.pb[2]=0.4;
  created_points.push_back(x7);
  x8.pb[0]=0.431;x8.pb[1]=0.7;x8.pb[2]=0.7;
  created_points.push_back(x8);
  x9.pb[0]=0.535;x9.pb[1]=0.9;x9.pb[2]=0.01;
  created_points.push_back(x9);
  x10.pb[0]=0.596;x10.pb[1]=1;x10.pb[2]=1.0;
  created_points.push_back(x10);
  x11.pb[0]=0.24;x11.pb[1]=1.0;x11.pb[2]=0.01;
  created_points.push_back(x11);
  x12.pb[0]=0.673;x12.pb[1]=1.7;x12.pb[2]=0.8;
  created_points.push_back(x12);
  x13.pb[0]=0.552;x13.pb[1]=1.15;x13.pb[2]=0.01;
  created_points.push_back(x13);
  x14.pb[0]=0.56;x14.pb[1]=0.81;x14.pb[2]=0.01;
  created_points.push_back(x14);
  x15.pb[0]=.70;x15.pb[1]=1.10;x15.pb[2]=0.01;
  created_points.push_back(x15);
  x16.pb[0]=0.3762;x16.pb[1]=0.02435;x16.pb[2]=0.3;
  created_points.push_back(x16);
  x17.pb[0]=0.0234;x17.pb[1]=0.389;x17.pb[2]=0.132;
  created_points.push_back(x17);
  x18.pb[0]=0.431;x18.pb[1]=0.245;x18.pb[2]=0.0235;
  created_points.push_back(x18);
  x19.pb[0]=0.535;x19.pb[1]=0.673;x19.pb[2]=0.01;
  created_points.push_back(x19);
  x20.pb[0]=0.76;x20.pb[1]=0.453;x20.pb[2]=1.0;
  created_points.push_back(x20);
  std::cout<<"Original Point 1: "<<x1.pb[0]<<" "<<x1.pb[1]<<" "<<x1.pb[2]<<std::endl;

  //create known transform
  aa[0] = 2.7;
  aa[1] = 0.3;
  aa[2] = 0.1;
  p[0]=0.2;
  p[1]=0.4;
  p[2]=0.5;

  // initialize the camera parameters
  C.angle_axis[0]=0.0;
  C.angle_axis[1]=0.0;
  C.angle_axis[2]=0.0;
  C.position[0]=0.0;
  C.position[1]=0.0;
  C.position[2]=0.0;
  C.focal_length_x=525;
  C.focal_length_y=525;
  C.center_x=320;
  C.center_y=240;
  C.distortion_k1=0.01;
  C.distortion_k2=0.02;
  C.distortion_k3=0.03;
  C.distortion_p1=0.01;
  C.distortion_p2=0.01;

  // transform points, and then project into image plane
  Point3d t_point;
  Observation o_point;
  for (int i=0; i<created_points.size();i++)
    {
      t_point = xformPoint(created_points.at(i), aa[0], aa[1], aa[2], p[0], p[1], p[2]);
      o_point = projectPoint(C,t_point);
      transformed_points.push_back(t_point);
      observations.push_back(o_point);
    }
}

TEST(IndustrialExtrinsicCalCeresSuite, points_costfunction)
{
  //create known transform to move created points to transformed points
  aa[0] = 2.7;
  aa[1] = 0.3;
  aa[2] = 0.1;
  p[0]=0.2;
  p[1]=0.4;
  p[2]=0.5;

  // initialize the camera parameters
  C.angle_axis[0]=0.0;
  C.angle_axis[1]=0.0;
  C.angle_axis[2]=0.0;
  C.position[0]=0.0;
  C.position[1]=0.0;
  C.position[2]=0.0;
  C.focal_length_x=525;
  C.focal_length_y=525;
  C.center_x=320;
  C.center_y=240;
  C.distortion_k1=0.01;
  C.distortion_k2=0.02;
  C.distortion_k3=0.03;
  C.distortion_p1=0.01;
  C.distortion_p2=0.01;

  // transform points, and then project into image plane
  Point3d t_point;
  Observation o_point;
  for (int i=0; i<created_points.size();i++)
    {
      t_point = xformPoint(created_points.at(i), aa[0], aa[1], aa[2], p[0], p[1], p[2]);
      o_point = projectPoint(C,t_point);
      transformed_points.push_back(t_point);
      observations.push_back(o_point);
    }

  double extrinsics[6];
  extrinsics[0] = C.angle_axis[0];
  extrinsics[1] = C.angle_axis[1];
  extrinsics[2] = C.angle_axis[2];
  extrinsics[3] = C.position[0];
  extrinsics[4] = C.position[1];
  extrinsics[5] = C.position[2];

  double intrinsics[9];
  intrinsics[0] = C.focal_length_x;
  intrinsics[1] = C.focal_length_y;
  intrinsics[2] = C.center_x;
  intrinsics[3] = C.center_y;
  intrinsics[4] = C.distortion_k1;
  intrinsics[5] = C.distortion_k2;
  intrinsics[6] = C.distortion_k3;
  intrinsics[7] = C.distortion_p1;
  intrinsics[8] = C.distortion_p2;
  ceres::Problem problem;
  // when points, extrinsics, and intrinsics are not perturbed, there should be very little re-projection error
  for (int j = 0; j < transformed_points.size(); ++j)
    {
      double ox = observations[j].image_loc_x;
      double oy = observations[j].image_loc_y;
      ceres::CostFunction* cost_function = CameraReprjErrorWithDistortion::Create(ox, oy);
      problem.AddResidualBlock(cost_function, NULL, extrinsics, intrinsics, transformed_points[j].pb);
      double residual[2];
      CameraReprjErrorWithDistortion CFC(ox, oy);
      CFC(extrinsics, intrinsics, transformed_points[j].pb, residual);
      // no reprojection error should be observed
      ASSERT_NEAR(0.0, residual[0], .1);
      ASSERT_NEAR(0.0, residual[1], .1);
    }

  // optimization should therefore not do anything either
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = false;
  options.max_num_iterations = 1000;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  // no changes due to optimization
  ASSERT_NEAR(C.angle_axis[0], extrinsics[0], .1);
  ASSERT_NEAR(C.angle_axis[1], extrinsics[1], .1);
  ASSERT_NEAR(C.angle_axis[2], extrinsics[2], .1);
  ASSERT_NEAR(C.position[0], extrinsics[3], .1);
  ASSERT_NEAR(C.position[1], extrinsics[4], .1);
  ASSERT_NEAR(C.position[2], extrinsics[5], .1);

  ASSERT_NEAR(C.focal_length_x, intrinsics[0], .1);
  ASSERT_NEAR(C.focal_length_y, intrinsics[1], .1);
  ASSERT_NEAR(C.center_x, intrinsics[2], .1);
  ASSERT_NEAR(C.center_y, intrinsics[3], .1);
  ASSERT_NEAR(C.distortion_k1, intrinsics[4], .1);
  ASSERT_NEAR(C.distortion_k2, intrinsics[5], .1);
  ASSERT_NEAR(C.distortion_k3, intrinsics[6], .1);
  ASSERT_NEAR(C.distortion_p1, intrinsics[7], .1);
  ASSERT_NEAR(C.distortion_p2, intrinsics[8], .1);

}

TEST(IndustrialExtrinsicCalCeresSuite, circle_cost)
{
  //create known transform to move created points to transformed points
  aa[0] = 2.7;
  aa[1] = 0.3;
  aa[2] = 0.1;
  p[0]=0.2;
  p[1]=0.4;
  p[2]=0.5;

  // initialize the camera parameters
  C.angle_axis[0]=0.0;
  C.angle_axis[1]=0.0;
  C.angle_axis[2]=0.0;
  C.position[0]=0.0;
  C.position[1]=0.0;
  C.position[2]=0.0;
  C.focal_length_x=525;
  C.focal_length_y=525;
  C.center_x=320;
  C.center_y=240;
  C.distortion_k1=0.01;
  C.distortion_k2=0.02;
  C.distortion_k3=0.03;
  C.distortion_p1=0.01;
  C.distortion_p2=0.01;

  // transform points, and then project into image plane
  Point3d t_point;
  Observation o_point;
  for (int i=0; i<created_points.size();i++)
    {
      t_point = xformPoint(created_points.at(i), aa[0], aa[1], aa[2], p[0], p[1], p[2]);
      o_point = projectPoint(C,t_point);
      transformed_points.push_back(t_point);
      observations.push_back(o_point);
    }

  double extrinsics[6];
  extrinsics[0] = C.angle_axis[0];
  extrinsics[1] = C.angle_axis[1];
  extrinsics[2] = C.angle_axis[2];
  extrinsics[3] = C.position[0];
  extrinsics[4] = C.position[1];
  extrinsics[5] = C.position[2];

  double intrinsics[9];
  intrinsics[0] = C.focal_length_x;
  intrinsics[1] = C.focal_length_y;
  intrinsics[2] = C.center_x;
  intrinsics[3] = C.center_y;
  intrinsics[4] = C.distortion_k1;
  intrinsics[5] = C.distortion_k2;
  intrinsics[6] = C.distortion_k3;
  intrinsics[7] = C.distortion_p1;
  intrinsics[8] = C.distortion_p2;
  ceres::Problem problem;
  // when points, extrinsics, and intrinsics are not perturbed, there should be very little re-projection error
  for (int j = 0; j < transformed_points.size(); ++j)
    {
      double ox = observations[j].image_loc_x;
      double oy = observations[j].image_loc_y;
      ceres::CostFunction* cost_function = CameraReprjErrorWithDistortion::Create(ox, oy);
      problem.AddResidualBlock(cost_function, NULL, extrinsics, intrinsics, transformed_points[j].pb);
      double residual[2];
      CameraReprjErrorWithDistortion CFC(ox, oy);
      CFC(extrinsics, intrinsics, transformed_points[j].pb, residual);
      // no reprojection error should be observed
      ASSERT_NEAR(0.0, residual[0], .1);
      ASSERT_NEAR(0.0, residual[1], .1);
    }

  // optimization should therefore not do anything either
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = false;
  options.max_num_iterations = 1000;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  // no changes due to optimization
  ASSERT_NEAR(C.angle_axis[0], extrinsics[0], .1);
  ASSERT_NEAR(C.angle_axis[1], extrinsics[1], .1);
  ASSERT_NEAR(C.angle_axis[2], extrinsics[2], .1);
  ASSERT_NEAR(C.position[0], extrinsics[3], .1);
  ASSERT_NEAR(C.position[1], extrinsics[4], .1);
  ASSERT_NEAR(C.position[2], extrinsics[5], .1);

  ASSERT_NEAR(C.focal_length_x, intrinsics[0], .1);
  ASSERT_NEAR(C.focal_length_y, intrinsics[1], .1);
  ASSERT_NEAR(C.center_x, intrinsics[2], .1);
  ASSERT_NEAR(C.center_y, intrinsics[3], .1);
  ASSERT_NEAR(C.distortion_k1, intrinsics[4], .1);
  ASSERT_NEAR(C.distortion_k2, intrinsics[5], .1);
  ASSERT_NEAR(C.distortion_k3, intrinsics[6], .1);
  ASSERT_NEAR(C.distortion_p1, intrinsics[7], .1);
  ASSERT_NEAR(C.distortion_p2, intrinsics[8], .1);

}

Point3d xformPoint(Point3d &original_point, double &ax, double &ay, double &az, double &x, double&y, double &z)
{
  //std::cout<<"ange axis inputs ax, ay, az: "<<ax<<", "<<ay<<", "<<az<<std::endl;
  Eigen::Matrix3f m_ceres;
  Eigen::Matrix3f m_eigen;
  m_eigen = Eigen::AngleAxisf(ax, Eigen::Vector3f::UnitX())
    * Eigen::AngleAxisf(ay, Eigen::Vector3f::UnitY())
    * Eigen::AngleAxisf(az, Eigen::Vector3f::UnitZ());
  //std::cout<<"m_eigen : "<<std::endl<< m_eigen <<std::endl;
  double aa[3]; // angle axis
  double p[3]; // point rotated
  aa[0] = ax;
  aa[1] = ay;
  aa[2] = az;
  Eigen::Vector3f orig_point(original_point.pb[0], original_point.pb[1], original_point.pb[2]);
  //std::cout<<"within transformPoint, original point Vector3f x, y, z: "<<orig_point.x()<<", "<<orig_point.y()<<", "<<orig_point.z()<<std::endl;

  double R[9];
  ceres::AngleAxisToRotationMatrix(aa, R);
  m_ceres << R[0], R[1],R[2],
    R[3], R[4], R[5],
    R[6], R[7], R[8];
  //std::cout<<"m_ceres : "<<std::endl<< m_ceres <<std::endl;
  Eigen::Vector3f rot_point=m_ceres*orig_point;
  //std::cout<<"within transformPoint, rotated point Vector3f x, y, z: "<<rot_point.x()<<", "<<rot_point.y()<<", "<<rot_point.z()<<std::endl;
  double xp1 = rot_point.x() + x; // point rotated and translated
  double yp1 = rot_point.y() + y;
  double zp1 = rot_point.z() + z;
  //std::cout<<"within transformPoint, original point double x, y, z: "<<xp1<<", "<<yp1<<", "<<zp1<<std::endl;
  Point3d t_point;
  t_point.pb[0]=xp1;t_point.pb[1]=yp1;t_point.pb[2]=zp1;
  //std::cout<<"within transformPoint, t_point x, y, z: "<<t_point.pb[0]<<", "<<t_point.pb[1]<<", "<<t_point.pb[2]<<std::endl;
  return t_point;
}

// Run all the tests that were declared with TEST()
int main(int argc, char **argv)
{
  //ros::init(argc, argv, "test");
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();

}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/eye.hpp
 *
 * \brief The \c eye operation.
 *
 * The \c eye operation creates an identity matrix.
 * This operation takes inspiration from the MATLAB's \e eye function
 * and the Mathematica's \e IdentityMatrix function.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2021, Marco Guazzone
 *
 * 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_NUMERIC_UBLAS_EYE_HPP
#define BOOST_NUMERIC_UBLAS_EYE_HPP

//#include <boost/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <cstddef>
#include <memory>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

#if __cplusplus > 199711L
// C++0x allows default template parameters for functions

/**
 * \brief Create an unmutable identity square matrix.
 *
 * \tparam ValueT The type of object stored in the matrix (like double, float, complex, etc...).
 *  By default, an integral type is used.
 * \tparam AllocT An allocator for storing the zeros and one elements.
 *  By default, a standard allocator is used.
 *
 * \param n The number of rows and columns of the resulting identity matrix.
 * \return An unmutable n-by-n identity matrix.
 */
template<typename ValueT = int, // The same default value used by the `identity_matrix` type.
         typename AllocT = std::allocator<ValueT> > // The same default allocator used by the `identity_matrix` type.
BOOST_UBLAS_INLINE
identity_matrix<ValueT,AllocT> eye(std::size_t n)
{
    return identity_matrix<ValueT,AllocT>(n);
}

/**
 * \brief Create an unmutable identity rectangular matrix.
 *
 * \tparam ValueT The type of object stored in the matrix (like double, float, complex, etc...)
 *  By default, an integral type is used.
 * \tparam AllocT An allocator for storing the zeros and one elements.
 *  By default, a standard allocator is used.
 *
 * \param nr The number of rows of the resulting identity matrix.
 * \param nc The number of columns of the resulting identity matrix.
 * \return An unmutable nr-by-nc identity matrix.
 */
template<typename ValueT = int, // The same default value used by the `identity_matrix` type.
         typename AllocT = std::allocator<ValueT> > // The same default allocator used by the `identity_matrix` type.
BOOST_UBLAS_INLINE
identity_matrix<ValueT,AllocT> eye(std::size_t nr, std::size_t nc)
{
    return identity_matrix<ValueT,AllocT>(nr, nc);
}

#else // __cplusplus

// C++98 does not allow default template parameters for functions

/**
 * \brief Create an unmutable identity square matrix.
 *
 * \tparam ValueT The type of object stored in the matrix (like double, float, complex, etc...).
 * \tparam AllocT An allocator for storing the zeros and one elements.
 *
 * \param n The number of rows and columns of the resulting identity matrix.
 * \return An unmutable n-by-n identity matrix.
 */
template<typename ValueT,
         typename AllocT>
BOOST_UBLAS_INLINE
identity_matrix<ValueT,AllocT> eye(std::size_t n)
{
    return identity_matrix<ValueT,AllocT>(n);
}

/**
 * \brief Create an unmutable identity square matrix with a default allocator.
 *
 * \tparam ValueT The type of object stored in the matrix (like double, float, complex, etc...).
 *
 * \param n The number of rows and columns of the resulting identity matrix.
 * \return An unmutable n-by-n identity matrix.
 */
template<typename ValueT>
BOOST_UBLAS_INLINE
identity_matrix<ValueT> eye(std::size_t n)
{
    return identity_matrix<ValueT>(n);
}

/**
 * \brief Create an unmutable identity rectangular matrix.
 *
 * \tparam ValueT The type of object stored in the matrix (like double, float, complex, etc...)
 *  By default, an integral type is used.
 * \tparam AllocT An allocator for storing the zeros and one elements.
 *  By default, a standard allocator is used.
 *
 * \param nr The number of rows of the resulting identity matrix.
 * \param nc The number of columns of the resulting identity matrix.
 * \return An unmutable nr-by-nc identity matrix.
 */
template<typename ValueT,
         typename AllocT>
BOOST_UBLAS_INLINE
identity_matrix<ValueT,AllocT> eye(std::size_t nr, std::size_t nc)
{
    return identity_matrix<ValueT,AllocT>(nr, nc);
}

/**
 * \brief Create an unmutable identity rectangular matrix with a default allocator.
 *
 * \tparam ValueT The type of object stored in the matrix (like double, float, complex, etc...)
 *  By default, an integral type is used.
 *
 * \param nr The number of rows of the resulting identity matrix.
 * \param nc The number of columns of the resulting identity matrix.
 * \return An unmutable nr-by-nc identity matrix.
 */
template<typename ValueT>
BOOST_UBLAS_INLINE
identity_matrix<ValueT> eye(std::size_t nr, std::size_t nc)
{
    return identity_matrix<ValueT>(nr, nc);
}

#endif // __cplusplus

/**
 * \brief Create an unmutable identity square matrix with default value type and allocator.
 *
 * \param n The number of rows and columns of the resulting identity matrix.
 * \return An unmutable n-by-n identity matrix.
 */
BOOST_UBLAS_INLINE
identity_matrix<> eye(std::size_t n)
{
    return identity_matrix<>(n);
}

/**
 * \brief Create an unmutable identity rectangular matrix with default value type and allocator.
 *
 * \param nr The number of rows of the resulting identity matrix.
 * \param nc The number of columns of the resulting identity matrix.
 * \return An unmutable nr-by-nc identity matrix.
 */
BOOST_UBLAS_INLINE
identity_matrix<> eye(std::size_t nr, std::size_t nc)
{
    return identity_matrix<>(nr, nc);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_EYE_HPP


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Use, modification and distribution is subject to 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)
//
// Example: Custom coordinate system example

#include <iostream>

#include <boost/geometry/geometry.hpp>

// 1: declare a coordinate system. For example for Mars
//    Like for the Earth, we let the use choose between degrees or radians
//    (Unfortunately, in real life Mars has two coordinate systems:
//     http://planetarynames.wr.usgs.gov/Page/MARS/system)
template<typename DegreeOrRadian>
struct martian
{
    typedef DegreeOrRadian units;
};

// 2: give it also a family
struct martian_tag;

// 3: register to which coordinate system family it belongs to
//    this must be done in namespace boost::geometry::traits
namespace boost { namespace geometry { namespace traits
{

template <typename DegreeOrRadian>
struct cs_tag<martian<DegreeOrRadian> >
{
    typedef martian_tag type;
};

}}} // namespaces

// NOTE: if the next steps would not be here,
// compiling a distance function call with martian coordinates
// would result in a MPL assertion

// 4: so register a distance strategy as its default strategy
namespace boost { namespace geometry { namespace strategy { namespace distance { namespace services
{

template <typename Point1, typename Point2>
struct default_strategy<point_tag, point_tag, Point1, Point2, martian_tag, martian_tag>
{
    typedef haversine<double> type;
};

}}}}} // namespaces

// 5: not worked out. To implement a specific distance strategy for Mars,
//    e.g. with the Mars radius given by default,
//    you will have to implement (/register) several other metafunctions:
//      tag, return_type, similar_type, comparable_type,
//    and structs:
//      get_similar, get_comparable, result_from_distance
//   See e.g. .../boost/geometry/extensions/gis/geographic/strategies/andoyer.hpp

int main()
{
    typedef boost::geometry::model::point
        <
            double, 2, martian<boost::geometry::degree>
        > mars_point;

    // Declare two points
    // (Source: http://nssdc.gsfc.nasa.gov/planetary/mars_mileage_guide.html)
    // (Other sources: Wiki and Google give slightly different coordinates, resulting
    //  in other distance, 20 km off)
    mars_point viking1(-48.23, 22.54); // Viking 1 landing site in Chryse Planitia
    mars_point pathfinder(-33.55, 19.33); // Pathfinder landing site in Ares Vallis

    double d = boost::geometry::distance(viking1, pathfinder); // Distance in radians on unit-sphere

    // Using the Mars mean radius
    // (Source: http://nssdc.gsfc.nasa.gov/planetary/factsheet/marsfact.html)
    std::cout << "Distance between Viking1 and Pathfinder landing sites: "
        << d * 3389.5 << " km" << std::endl;

    // We would get 832.616 here, same order as the 835 (rounded on 5 km) listed
    // on the mentioned site

#ifdef OPTIONALLY_ELLIPSOIDAL
    // Optionally the distance can be calculated more accurate by an Ellipsoidal approach,
    // giving 834.444 km
    d = boost::geometry::distance(viking1, pathfinder,
        boost::geometry::strategy::distance::andoyer<mars_point>
            (boost::geometry::srs::spheroid<double>(3396.2, 3376.2)));
    std::cout << "Ellipsoidal distance: " << d << " km" << std::endl;
#endif

    return 0;
}


#ifndef CANNON_GEOM_TRAJECTORY
#define CANNON_GEOM_TRAJECTORY

/*!
 * \file cannon/geom/trajectory.hpp
 * \brief File containing Trajectory class definition.
 */

#include <vector>

#include <Eigen/Dense>

#include <cannon/utils/class_forward.hpp>

using namespace Eigen;

namespace cannon {

  namespace math {
    CANNON_CLASS_FORWARD(MultiSpline);
  }

  using namespace cannon::math;

  namespace geom {

    /*!
     * \brief Class representing a time-parameterized trajectory.
     */
    class Trajectory {
      public:

        /*!
         * \brief Default constructor.
         */
        Trajectory();

        /*!
         * \brief Constructor taking vector of states and times.
         */
        Trajectory(const std::vector<VectorXd> &states,
                   const std::vector<double> times);

        /*!
         * \brief Get the state of this trajectory at the input time. This
         * trajectory is treated as a piecewise-linear function over time.
         *
         * \param t The time to evaluate the trajectory at.
         *
         * \returns The linearly interpolated state of this trajectory at the
         * input time.
         */
        VectorXd operator()(double t) const;

        /*!
         * \brief Add a new state at the end of this trajectory, at the input
         * time.
         *
         * \param state The state to add.
         * \param time The time for the new state. Should be greater than any
         * time in the trajectory so far.
         */
        void push_back(const Ref<const VectorXd>& state, double time);

        /*!
         * \brief Get the length (total time) of this trajectory.
         *
         * \returns Length of trajectory.
         */
        double length() const;

        /*!
         * \brief Get the size (number of states) in this trajectory.
         *
         * \returns Trajectory size.
         */
        size_t size() const;

        /*!
         * \brief Get a spline that interpolates this trajectory.
         *
         * \returns A spline which smoothly interpolates this trajectory.
         */
        MultiSpline interp() const;

        /*!
         * \brief Save this trajectory to a file.
         *
         * \param path The path to save this trajectory to.
         */
        void save(const std::string& path);

        /*!
         * \brief Load a trajectory from a file. Overwrites any existing points
         * in this trajectory.
         *
         * \param path The path to load a trajectory from.
         */
        void load(const std::string& path);

      private:

        /*!
         * \brief Find the closest idx such that times_[idx] <= t
         *
         * \param t Input value to locate
         *
         * \returns Index of closest time
         */
        unsigned int find_closest_t_(double t) const;

        std::vector<VectorXd> states_; //!< States in trajectory
        std::vector<double> times_; //!< Times for states in trajectory

    };

    /*!
     * \brief Class representing a time-parameterized trajectory with controls.
     */
    class ControlledTrajectory {
      public:

        /*!
         * \brief Default constructor.
         */
        ControlledTrajectory();

        /*!
         * \brief Constructor taking vector of states, vector of controls, and times.
         */
        ControlledTrajectory(const std::vector<VectorXd> &states,
                             const std::vector<VectorXd> &controls,
                             const std::vector<double> times);

        /*!
         * \brief Get the trajectory of states from this controlled trajectory.
         *
         * \returns A trajectory of the states making up this controlled trajectory.
         */
        Trajectory state_traj() const;

        /*!
         * \brief Get the state and control of this controlled trajectory at the
         * input time. This trajectory is treated as a piecewise-linear function
         * over time.
         *
         * \param t The time to evaluate the trajectory at.
         *
         * \returns A pair containing the linearly interpolated state and
         * control of this trajectory at the input time.
         */
        std::pair<VectorXd, VectorXd> operator()(double t) const;

        /*!
         * \brief Add a new state and control at the end of this trajectory, at
         * the input time.
         *
         * \param state The state to add.
         * \param control The control to add.
         * \param time The time for the new state. Should be greater than any
         * time in the trajectory so far.
         */
        void push_back(const Ref<const VectorXd> &state,
                       const Ref<const VectorXd> &control, double time);

        /*!
         * \brief Get the length (total time) of this trajectory.
         *
         * \returns Length of trajectory.
         */
        double length() const;

        /*!
         * \brief Get the size (number of states) of this trajectory.
         *
         * \returns Trajectory size.
         */
        size_t size() const;

        /*!
         * \brief Save this trajectory to a file.
         *
         * \param path The path to save this trajectory to.
         */
        void save(const std::string& path);

        /*!
         * \brief Load a trajectory from a file. Overwrites any existing points
         * in this trajectory.
         *
         * \param path The path to load a trajectory from.
         */
        void load(const std::string& path);

      private:

        /*!
         * \brief Find the closest idx such that times_[idx] <= t
         *
         * \param t Input value to locate
         *
         * \returns Index of closest time
         */
        unsigned int find_closest_t_(double t) const;

        std::vector<VectorXd> states_; //!< States in trajectory
        std::vector<VectorXd> controls_; //!< Controls in trajectory
        std::vector<double> times_; //!< Times for states in trajectory

    };

  }
}

#endif /* ifndef CANNON_GEOM_TRAJECTORY */


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2016-2017, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_TEST_DISTANCE_GEO_COMMON_HPP
#define BOOST_GEOMETRY_TEST_DISTANCE_GEO_COMMON_HPP

#include <iostream>
#include <string>

#include <boost/mpl/assert.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/is_same.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
#include <boost/geometry/geometries/multi_linestring.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

#include <boost/geometry/io/wkt/write.hpp>
#include <boost/geometry/io/dsv/write.hpp>

#include <boost/geometry/algorithms/num_interior_rings.hpp>
#include <boost/geometry/algorithms/distance.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#include <from_wkt.hpp>
#include <string_from_type.hpp>

#include "distance_brute_force.hpp"

namespace bg = ::boost::geometry;

//===================================================================
//tag dispatching for swaping arguments in segments

template <typename Tag> struct dispatch
{
    template <typename T>
    static inline T swap(T const& t)
    {
        return t;
    }
};

// Specialization for segments
template <> struct dispatch<boost::geometry::segment_tag>
{
    template <typename Segment>
    static inline Segment swap(Segment const& s)
    {
        Segment s_swaped;

        bg::set<0, 0>(s_swaped, bg::get<1, 0>(s));
        bg::set<0, 1>(s_swaped, bg::get<1, 1>(s));
        bg::set<1, 0>(s_swaped, bg::get<0, 0>(s));
        bg::set<1, 1>(s_swaped, bg::get<0, 1>(s));

        return s_swaped;
    }
};

//========================================================================

template <typename T>
struct check_equal
{
    template <typename Value, typename = void>
    struct equal_to
    {
        static inline void apply(Value const& x, Value const& y)
        {
            BOOST_CHECK(x == y);
        }
    };

    template <typename Dummy>
    struct equal_to<double, Dummy>
    {
        static inline void apply(double x, double y)
        {
            BOOST_CHECK_CLOSE(x, y, 0.001);
        }
    };

    template <typename Geometry1, typename Geometry2>
    static inline void apply(std::string const& /*case_id*/,
                             std::string const& /*subcase_id*/,
                             Geometry1 const& /*geometry1*/,
                             Geometry2 const& /*geometry2*/,
                             T const& detected,
                             T const& expected)
    {
        equal_to<T>::apply(expected, detected);
    }
};

//========================================================================

template
<
    typename Geometry1, typename Geometry2,
    int id1 = bg::geometry_id<Geometry1>::value,
    int id2 = bg::geometry_id<Geometry2>::value
>
struct test_distance_of_geometries
    : public test_distance_of_geometries<Geometry1, Geometry2, 0, 0>
{};

template <typename Geometry1, typename Geometry2>
struct test_distance_of_geometries<Geometry1, Geometry2, 0, 0>
{
    template <typename DistanceType, typename Strategy>
    static inline
    void apply(std::string const& case_id,
               std::string const& wkt1,
               std::string const& wkt2,
               DistanceType const& expected_distance,
               Strategy const& strategy,
               bool test_reversed = true,
               bool swap_geometry_args = false)
    {
        Geometry1 geometry1 = from_wkt<Geometry1>(wkt1);
        Geometry2 geometry2 = from_wkt<Geometry2>(wkt2);

        apply(case_id, geometry1, geometry2,
              expected_distance,
              strategy, test_reversed, swap_geometry_args);
    }

    template
    <
        typename DistanceType,
        typename Strategy
    >
    static inline
    void apply(std::string const& case_id,
               Geometry1 const& geometry1,
               Geometry2 const& geometry2,
               DistanceType const& expected_distance,
               Strategy const& strategy,
               bool test_reversed = true,
               bool swap_geometry_args = false)
    {
#ifdef BOOST_GEOMETRY_TEST_DEBUG
        std::cout << "case ID: " << case_id << "; "
                  << "G1: " << bg::wkt(geometry1)
                  << " - "
                  << "G2: " << bg::wkt(geometry2)
                  << std::endl;
#endif
        namespace services = bg::strategy::distance::services;

        using bg::unit_test::distance_brute_force;

        typedef typename bg::default_distance_result
            <
                Geometry1, Geometry2
            >::type default_distance_result;

        typedef typename services::return_type
            <
                Strategy, Geometry1, Geometry2
            >::type distance_result_from_strategy;

        static const bool same_regular = boost::is_same
            <
                default_distance_result,
                distance_result_from_strategy
            >::type::value;

        BOOST_CHECK(same_regular);

        // check distance with passed strategy
        distance_result_from_strategy dist =
            bg::distance(geometry1, geometry2, strategy);

        check_equal
            <
                distance_result_from_strategy
            >::apply(case_id, "a", geometry1, geometry2,
                     dist, expected_distance);

        // check against the comparable distance computed in a
        // brute-force manner
        default_distance_result dist_brute_force
            = distance_brute_force(geometry1, geometry2, strategy);

        check_equal
            <
                default_distance_result
            >::apply(case_id, "b", geometry1, geometry2,
                     dist_brute_force, expected_distance);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
        std::cout << string_from_type<typename bg::coordinate_type<Geometry1>::type>::name()
                  << string_from_type<typename bg::coordinate_type<Geometry2>::type>::name()
                  << " -> "
                  << string_from_type<default_distance_result>::name()
                  << std::endl;
        std::cout << "expected distance = "
                  << expected_distance << " ; "
                  << std::endl;
        std::cout << "distance = "
                  << dist << " ; "
                  << std::endl;

        if ( !test_reversed )
        {
            std::cout << std::endl;
        }
#endif

        if ( test_reversed )
        {
            // check distance with given strategy
            dist = bg::distance(geometry2, geometry1, strategy);

            check_equal
                <
                    default_distance_result
                >::apply(case_id, "ra", geometry2, geometry1,
                         dist, expected_distance);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
            std::cout << "distance[reversed args] = "
                      << dist
                      << std::endl;
#endif
        }

        if (swap_geometry_args)
        {
            Geometry1 g1 = dispatch
                <
                    typename boost::geometry::tag<Geometry1>::type
                >::swap(geometry1);

            Geometry2 g2 = dispatch
                <
                    typename boost::geometry::tag<Geometry2>::type
                >::swap(geometry2);

            // check distance with given strategy
            dist = bg::distance(g1, g2, strategy);

            check_equal
                <
                    default_distance_result
                >::apply(case_id, "swap", g1, g2,
                         dist, expected_distance);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
            std::cout << "distance[swap geometry args] = "
                      << dist
                      << std::endl;
            std::cout << std::endl;
#endif
         }
#ifdef BOOST_GEOMETRY_TEST_DEBUG
            std::cout << std::endl;
#endif

    }
};

//========================================================================

template <typename Geometry1, typename Geometry2, typename Strategy>
void test_empty_input(Geometry1 const& geometry1,
                      Geometry2 const& geometry2,
                      Strategy const& strategy)
{
    try
    {
        bg::distance(geometry1, geometry2);
    }
    catch(bg::empty_input_exception const& )
    {
        return;
    }
    BOOST_CHECK_MESSAGE(false,
                        "A empty_input_exception should have been thrown");

    try
    {
        bg::distance(geometry2, geometry1);
    }
    catch(bg::empty_input_exception const& )
    {
        return;
    }
    BOOST_CHECK_MESSAGE(false,
                        "A empty_input_exception should have been thrown");

    try
    {
        bg::distance(geometry1, geometry2, strategy);
    }
    catch(bg::empty_input_exception const& )
    {
        return;
    }
    BOOST_CHECK_MESSAGE(false,
                        "A empty_input_exception should have been thrown");

    try
    {
        bg::distance(geometry2, geometry1, strategy);
    }
    catch(bg::empty_input_exception const& )
    {
        return;
    }
    BOOST_CHECK_MESSAGE(false,
                        "A empty_input_exception should have been thrown");
}

#endif // BOOST_GEOMETRY_TEST_DISTANCE_GEO_COMMON_HPP


/*
 * Copyright Andrey Semashev 2020
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * https://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file bit_ops.hpp
 *
 * This header includes all algorithms for bit operations.
 */

#ifndef BOOST_BIT_OPS_HPP_INCLUDED_
#define BOOST_BIT_OPS_HPP_INCLUDED_

#include <boost/bit_ops/pow2.hpp>
#include <boost/bit_ops/counting.hpp>
#include <boost/bit_ops/rotating.hpp>

#endif // BOOST_BIT_OPS_HPP_INCLUDED_


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Hidekazu Ikeno
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

///
/// \file prony_like_method_common.hpp
///
#ifndef MXPFIT_PRONY_LIKE_METHOD_COMMON_HPP
#define MXPFIT_PRONY_LIKE_METHOD_COMMON_HPP

#include <cassert>
#include <type_traits>

#include <Eigen/Core>
#include <Eigen/SVD>

#include <mxpfit/exponential_sum.hpp>
#include <mxpfit/matrix_free_gemv.hpp>
#include <mxpfit/vandermonde_least_squares.hpp>

namespace mxpfit
{

namespace detail
{

/// \internal
/// Select roots of Prony polynomials on the unit disk
template <typename T>
struct prony_roots_on_unit_disk
{
    using Scalar        = T;
    using RealScalar    = typename Eigen::NumTraits<Scalar>::Real;
    using ComplexScalar = std::complex<RealScalar>;
    using Index         = Eigen::Index;

    using Vector        = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using RealVector    = Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;
    using ComplexVector = Eigen::Matrix<ComplexScalar, Eigen::Dynamic, 1>;

    enum
    {
        IsComplex = Eigen::NumTraits<Scalar>::IsComplex,
    };

    /// \param z  array of roots to be filtered
    /// \param tolerance  small real number to acceptable tolerance for the
    //                    mangnitude of roots.
    static ComplexVector compute(const Eigen::Ref<const ComplexVector>& z,
                                 RealScalar tolerance)
    {
        using Eigen::numext::abs;
        using Eigen::numext::imag;
        static const auto eps     = Eigen::NumTraits<RealScalar>::epsilon();
        constexpr const auto zero = RealScalar();
        constexpr const auto one  = RealScalar(1);

        // Count the number of roots insize unit disk
        Index count = 0;
        for (Index i = 0; i < z.size(); ++i)
        {
            const auto abs_zi = abs(z(i));
            if (abs_zi <= one + tolerance)
            {
                if (IsComplex)
                {
                    ++count;
                }
                else
                {
                    // Discard negative real z(i).
                    const auto xi = real(z(i));
                    const auto yi = imag(z(i));
                    if (!(abs(yi) < eps && xi < zero))
                    {
                        ++count;
                    }
                }
            }
        }

        ComplexVector ret(count);
        Index n = 0;
        for (Index i = 0; i < z.size(); ++i)
        {
            const auto abs_zi = abs(z(i));
            if (abs_zi <= one + tolerance)
            {
                if (IsComplex)
                {
                    ret(n) = z(i);
                    ++n;
                }
                else
                {
                    // Discard negative real z(i).
                    const auto xi = real(z(i));
                    const auto yi = imag(z(i));
                    if (!(abs(yi) < eps && xi < zero))
                    {
                        ret(n) = z(i);
                        ++n;
                    }
                }
            }
        }

        return ret;
    }
};

/// \internal
///
/// Solve overdetermined linear system
///
/// \f[
///    V\boldsymbol{x} = \boldsymbol{b}
/// \f]
///
/// to compute weights of the exponential sum approximation, where \f$V\f$ is a
/// column Vandermonde matrix constructed from roots of a Prony polynomial.
///
/// \param[in] prony_roots  roots of a Prony polynomial
/// \param[in] rhs   a sequence of function/data values on uniform grid
/// \param[out] dst  the solution of overdetermined Vandermonde system
/// \param[in] eps      a prescribed accuracy required for the least-squares
/// \param[in] max_iter maximum number of iterations for CGLS linear solver
///
template <typename VecNodes, typename VecRHS, typename VecDest, typename RealT>
void solve_overdetermined_vandermonde(
    const Eigen::DenseBase<VecNodes>& prony_roots,
    const Eigen::MatrixBase<VecRHS>& rhs, Eigen::DenseBase<VecDest>& dst,
    RealT eps, Eigen::Index max_iter)
{
    EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecNodes);
    EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecRHS);
    EIGEN_STATIC_ASSERT_VECTOR_ONLY(VecDest);

    assert(prony_roots.size() == dst.size());

    using Scalar          = typename VecNodes::Scalar; // usually complex type
    using VandermondeGEMV = MatrixFreeGEMV<VandermondeMatrix<Scalar>>;

    // --- Solve with CGLS method.
    VandermondeMatrix<Scalar> matV(rhs.size(), prony_roots);
    VandermondeGEMV opV(matV);
    VandermondeLeastSquaresSolver<Scalar> solver(opV);
    solver.setTolerance(eps);
    solver.setMaxIterations(max_iter);

    //
    // We need cast the scalar type of RHS vector as the VecRHS::Scalar might be
    // different from VecNodes::Scalar. We should be consider the following
    // case:
    //
    // VecNodes::Scalar -> complex type
    // VecRHS::Scalar   -> real type
    //
    dst = solver.solve(rhs.template cast<Scalar>());

    if (solver.info() == Eigen::NoConvergence)
    {
        //
        // CGLS did not converge.
        // Fall back to least-squares with dense QR factorization.
        //
        auto denseV = matV.toDenseMatrix();
        dst = denseV.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV)
                  .solve(rhs.template cast<Scalar>());
    }

    return;
}

/// \internal
///
/// ### gen_prony_like_method_result
///
/// Generate and manipulate a result (an exponential sum) of Prony-like method.
///

template <typename T>
struct gen_prony_like_method_result;

template <typename T>
struct gen_prony_like_method_result
{
    using Real       = T;
    using Complex    = std::complex<Real>;
    using ResultType = ExponentialSum<Complex, Complex>;

    template <typename ArrayZ, typename ArrayW>
    static ResultType
    create(const Eigen::ArrayBase<ArrayZ>& z, // exp(-exponents)
           const Eigen::ArrayBase<ArrayW>& w, // weights
           Real x0, Real delta)
    {
        using Index = Eigen::Index;

        using Eigen::numext::abs;
        using Eigen::numext::conj;
        using Eigen::numext::exp;
        using Eigen::numext::imag;
        using Eigen::numext::log;
        using Eigen::numext::real;

        static const auto eps     = Eigen::NumTraits<Real>::epsilon();
        static const auto pi      = 4 * Eigen::numext::atan(Real(1));
        constexpr const auto zero = Real();
        constexpr const auto half = Real(0.5);

        //-------------------------------------------------------------------------
        // The exponents are obtained as a_i = -log(z_i), where {z_i} are the
        // roots of the Prony polynomial.
        //
        // Some z_i might be real and negative: in this case, the corresponding
        // parameter a_i becomes a complex, i.e, a_i = -ln|z_i|+i \pi.
        // However, its complex conjugate a_i^* is not included in the final
        // exponential sum approximation which makes the approximated function
        // non-real. Thus, we disregards those terms.
        //-------------------------------------------------------------------------

        // Count negative, real-valued Prony roots
        Index count = 0;
        for (Index i = 0; i < z.size(); ++i)
        {
            const auto xi = real(z(i));
            const auto yi = imag(z(i));
            if (xi < zero && abs(yi) < eps)
            {
                ++count;
            }
        }

        if (count)
        {
            // Found negative real roots.
            ResultType ret(z.size() + count);

            Index n = 0;
            for (Index i = 0; i < z.size(); ++i)
            {
                const auto xi = real(z(i));
                const auto yi = imag(z(i));
                if (xi < zero && abs(yi) < eps)
                {
                    // Log(z) = -log(xi) + i pi
                    const auto an       = Complex(-log(-xi), -pi) / delta;
                    ret.exponent(n)     = an;
                    ret.exponent(n + 1) = conj(an);

                    const auto wn     = half * w(i) * exp(-x0 * an);
                    ret.weight(n)     = wn;
                    ret.weight(n + 1) = conj(wn);

                    n += 2;
                }
                else
                {
                    const auto an   = -log(z(i)) / delta;
                    ret.exponent(n) = an;
                    ret.weight(n)   = w(i) * exp(-x0 * an);

                    ++n;
                }
            }

            return ret;
        }
        else
        {
            ResultType ret(z.size());
            ret.exponents() = -z.log() / delta;
            if (x0 == Real())
            {
                ret.weights() = w; // Don't forget to copy weights
            }
            else
            {
                ret.weights() = w * (-x0 * ret.exponents()).exp();
            }
            return ret;
        }
    }
};

template <typename T>
struct gen_prony_like_method_result<std::complex<T>>
{
    using Real       = T;
    using Complex    = std::complex<Real>;
    using ResultType = ExponentialSum<Complex, Complex>;

    template <typename ArrayZ, typename ArrayW>
    static ResultType
    create(const Eigen::ArrayBase<ArrayZ>& z, // exp(-exponents)
           const Eigen::ArrayBase<ArrayW>& w, // weights
           Real x0, Real delta)
    {
        ResultType ret(z.size());
        ret.exponents() = -z.log() / delta;
        if (x0 == Real())
        {
            ret.weights() = w; // Don't forget to copy weights
        }
        else
        {
            ret.weights() = w * (-x0 * ret.exponents()).exp();
        }

        return ret;
    }
};

} // namespace detail
} // namespace mxpfit

#endif /* MXPFIT_PRONY_LIKE_METHOD_COMMON_HPP */


/*
//@HEADER
// ************************************************************************
//
// qr_eigen_dense_out_of_place_impl.hpp
//                     		  Pressio
//                             Copyright 2019
//    National Technology & Engineering Solutions of Sandia, LLC (NTESS)
//
// Under the terms of Contract DE-NA0003525 with NTESS, the
// U.S. Government retains certain rights in this software.
//
// Pressio is licensed under BSD-3-Clause terms of use:
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
// IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Questions? Contact Francesco Rizzi (fnrizzi@sandia.gov)
//
// ************************************************************************
//@HEADER
*/

#ifndef QR_IMPL_EIGEN_QR_EIGEN_DENSE_OUT_OF_PLACE_IMPL_HPP_
#define QR_IMPL_EIGEN_QR_EIGEN_DENSE_OUT_OF_PLACE_IMPL_HPP_

#include <Eigen/QR>

namespace pressio{ namespace qr{ namespace impl{

template< typename MatrixType, typename R_t>
class QRHouseholderDenseEigenMatrix<
  MatrixType, R_t,
  ::pressio::mpl::enable_if_t<
    ::pressio::is_dense_matrix_eigen<MatrixType>::value
    >
  >
{
public:
  using sc_t	       = typename ::pressio::Traits<MatrixType>::scalar_type;
  using Q_type = Eigen::Matrix<sc_t, Eigen::Dynamic, Eigen::Dynamic>;
  using factorizer_t = Eigen::HouseholderQR<MatrixType>;

private:
  mutable std::shared_ptr<Q_type> Qmat_	     = {};
  mutable std::shared_ptr<factorizer_t> fct_ = {};

public:
  QRHouseholderDenseEigenMatrix() = default;
  ~QRHouseholderDenseEigenMatrix() = default;

  void computeThinOutOfPlace(const MatrixType & A)
  {
    const auto rows = A.rows();
    const auto cols = A.cols();
    fct_ = std::make_shared<factorizer_t>(A);

    if (!Qmat_ or (Qmat_->rows()!=rows and Qmat_->cols()!=cols)){
      Qmat_ = std::make_shared<Q_type>(rows,cols);
      Qmat_->setZero();
    }

    *Qmat_ = fct_->householderQ() * Q_type::Identity(rows,cols);
  }

  template < typename VectorInType, typename VectorOutType>
  void applyQTranspose(const VectorInType & vecIn, VectorOutType & vecOut) const
  {
    constexpr auto beta  = ::pressio::utils::Constants<sc_t>::zero();
    constexpr auto alpha = ::pressio::utils::Constants<sc_t>::one();
    ::pressio::ops::product(::pressio::transpose(), alpha, *this->Qmat_, vecIn, beta, vecOut);
  }

  template < typename VectorInType, typename VectorOutType>
  void applyRTranspose(const VectorInType & vecIn, VectorOutType & y) const
  {
    // y = R^T vecIn
    auto vecSize = ::pressio::ops::extent(y, 0);
    auto & Rm = fct_->matrixQR().block(0,0,vecSize,vecSize).template triangularView<Eigen::Upper>();
    y = Rm.transpose() * vecIn;
  }

  template <typename VectorType>
  void doLinSolve(const VectorType & rhs, VectorType & y)const
  {
    auto vecSize = ::pressio::ops::extent(y, 0);
    auto & Rm = fct_->matrixQR().block(0,0,vecSize,vecSize).
      template triangularView<Eigen::Upper>();
    y = Rm.solve(rhs);
  }

  const Q_type & QFactor() const {
    return *this->Qmat_;
  }
};

}}} // end namespace pressio::qr::impl
#endif  // QR_IMPL_EIGEN_QR_EIGEN_DENSE_OUT_OF_PLACE_IMPL_HPP_


#include <boost/hana.hpp>
#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <complex>
#include "../include/AlgorithmFactory.h"
#include <iostream>
#include <memory>

#include "../include/basic/SineCosine.h"
#include "../include/basic/Ceil.h"
#include "../include/basic/Floor.h"
#include "../include/basic/Round.h"

namespace constants = boost::math::constants;
namespace hana = boost::hana;
namespace jb = jeanbaptiste;
namespace jbo = jeanbaptiste::options;
namespace jw = jeanbaptiste::windowing;

int main()
{
    std::array<std::complex<double>, 16> complexData =
    {
        std::complex<double>(-1.0f, 0.0f),
		std::complex<double>(-1.0f, 0.0f),
		std::complex<double>(-1.0f, 0.0f),
		std::complex<double>(-1.0f, 0.0f),
		std::complex<double>( 1.0f, 0.0f),
		std::complex<double>( 1.0f, 0.0f),
		std::complex<double>( 1.0f, 0.0f),
		std::complex<double>( 1.0f, 0.0f),
		std::complex<double>(-1.0f, 0.0f),
		std::complex<double>(-1.0f, 0.0f),
		std::complex<double>(-1.0f, 0.0f),
		std::complex<double>(-1.0f, 0.0f),
		std::complex<double>( 1.0f, 0.0f),
		std::complex<double>( 1.0f, 0.0f),
		std::complex<double>( 1.0f, 0.0f),
		std::complex<double>( 1.0f, 0.0f)
    };

    jb::AlgorithmFactory<1, 6, jbo::Radix_2, jbo::Decimation_In_Frequency, jbo::Direction_Forward,
       jbo::Window_None, jbo::Normalization_Square_Root, std::complex<double>> algorithmFactory;

	constexpr auto stage = 4;
    auto algorithm = algorithmFactory.getAlgorithm(stage);
    algorithm->operator()(&complexData[0]);

	std::cout << "Selected algorithm - stage: " << stage << ", samples: " << algorithm->numberOfSamples() << ", frequencies: " << algorithm->numberOfFrequencies() << "\n\n";

	std::cout << "Applying FFT:\n";
    for (auto value : complexData)
       std::cout << std::setw(10) << std::setprecision(5) << value.real() << "\t" << value.imag() << "I\n";

	std::cout << "\nApplying iFFT:\n";
	jb::AlgorithmFactory<1, 6, jbo::Radix_2, jbo::Decimation_In_Frequency, jbo::Direction_Backward,
       jbo::Window_None, jbo::Normalization_Square_Root, std::complex<double>> iAlgorithmFactory;

	auto iAlgorithm = iAlgorithmFactory.getAlgorithm(stage);
    iAlgorithm->operator()(&complexData[0]);

    for (auto value : complexData)
       std::cout << std::setw(10) << std::setprecision(5) << value.real() << "\t" << value.imag() << "I\n";

    std::cin.get();

     return 0;
}


// g++-4.4 bench_gemm.cpp -I .. -O2 -DNDEBUG -lrt -fopenmp && OMP_NUM_THREADS=2  ./a.out
// icpc bench_gemm.cpp -I .. -O3 -DNDEBUG -lrt -openmp  && OMP_NUM_THREADS=2  ./a.out

// Compilation options:
//
// -DSCALAR=std::complex<double>
// -DSCALARA=double or -DSCALARB=double
// -DHAVE_BLAS
// -DDECOUPLED
//

#include <iostream>
#include <Eigen/Core>
#include <bench/BenchTimer.h>

using namespace std;
using namespace Eigen;

#ifndef SCALAR
// #define SCALAR std::complex<float>
#define SCALAR float
#endif

#ifndef SCALARA
#define SCALARA SCALAR
#endif

#ifndef SCALARB
#define SCALARB SCALAR
#endif

typedef SCALAR Scalar;
typedef NumTraits<Scalar>::Real RealScalar;
typedef Matrix<SCALARA,Dynamic,Dynamic> A;
typedef Matrix<SCALARB,Dynamic,Dynamic> B;
typedef Matrix<Scalar,Dynamic,Dynamic> C;
typedef Matrix<RealScalar,Dynamic,Dynamic> M;

#ifdef HAVE_BLAS

extern "C" {
  #include <Eigen/src/misc/blas.h>
}

static float fone = 1;
static float fzero = 0;
static double done = 1;
static double szero = 0;
static std::complex<float> cfone = 1;
static std::complex<float> cfzero = 0;
static std::complex<double> cdone = 1;
static std::complex<double> cdzero = 0;
static char notrans = 'N';
static char trans = 'T';
static char nonunit = 'N';
static char lower = 'L';
static char right = 'R';
static int intone = 1;

void blas_gemm(const MatrixXf& a, const MatrixXf& b, MatrixXf& c)
{
  int M = c.rows(); int N = c.cols(); int K = a.cols();
  int lda = a.rows(); int ldb = b.rows(); int ldc = c.rows();

  sgemm_(&notrans,&notrans,&M,&N,&K,&fone,
         const_cast<float*>(a.data()),&lda,
         const_cast<float*>(b.data()),&ldb,&fone,
         c.data(),&ldc);
}

EIGEN_DONT_INLINE void blas_gemm(const MatrixXd& a, const MatrixXd& b, MatrixXd& c)
{
  int M = c.rows(); int N = c.cols(); int K = a.cols();
  int lda = a.rows(); int ldb = b.rows(); int ldc = c.rows();

  dgemm_(&notrans,&notrans,&M,&N,&K,&done,
         const_cast<double*>(a.data()),&lda,
         const_cast<double*>(b.data()),&ldb,&done,
         c.data(),&ldc);
}

void blas_gemm(const MatrixXcf& a, const MatrixXcf& b, MatrixXcf& c)
{
  int M = c.rows(); int N = c.cols(); int K = a.cols();
  int lda = a.rows(); int ldb = b.rows(); int ldc = c.rows();

  cgemm_(&notrans,&notrans,&M,&N,&K,(float*)&cfone,
         const_cast<float*>((const float*)a.data()),&lda,
         const_cast<float*>((const float*)b.data()),&ldb,(float*)&cfone,
         (float*)c.data(),&ldc);
}

void blas_gemm(const MatrixXcd& a, const MatrixXcd& b, MatrixXcd& c)
{
  int M = c.rows(); int N = c.cols(); int K = a.cols();
  int lda = a.rows(); int ldb = b.rows(); int ldc = c.rows();

  zgemm_(&notrans,&notrans,&M,&N,&K,(double*)&cdone,
         const_cast<double*>((const double*)a.data()),&lda,
         const_cast<double*>((const double*)b.data()),&ldb,(double*)&cdone,
         (double*)c.data(),&ldc);
}

#endif

void matlab_cplx_cplx(const M& ar, const M& ai, const M& br, const M& bi, M& cr, M& ci)
{
  cr.noalias() += ar * br;
  cr.noalias() -= ai * bi;
  ci.noalias() += ar * bi;
  ci.noalias() += ai * br;
  // [cr ci] += [ar ai] * br + [-ai ar] * bi
}

void matlab_real_cplx(const M& a, const M& br, const M& bi, M& cr, M& ci)
{
  cr.noalias() += a * br;
  ci.noalias() += a * bi;
}

void matlab_cplx_real(const M& ar, const M& ai, const M& b, M& cr, M& ci)
{
  cr.noalias() += ar * b;
  ci.noalias() += ai * b;
}

template<typename A, typename B, typename C>
EIGEN_DONT_INLINE void gemm(const A& a, const B& b, C& c)
{
 c.noalias() += a * b;
}

int main(int argc, char ** argv)
{
  std::ptrdiff_t l1 = internal::queryL1CacheSize();
  std::ptrdiff_t l2 = internal::queryTopLevelCacheSize();
  std::cout << "L1 cache size     = " << (l1>0 ? l1/1024 : -1) << " KB\n";
  std::cout << "L2/L3 cache size  = " << (l2>0 ? l2/1024 : -1) << " KB\n";
  typedef internal::gebp_traits<Scalar,Scalar> Traits;
  std::cout << "Register blocking = " << Traits::mr << " x " << Traits::nr << "\n";

  int rep = 1;    // number of repetitions per try
  int tries = 2;  // number of tries, we keep the best

  int s = 2048;
  int m = s;
  int n = s;
  int p = s;
  int cache_size1=-1, cache_size2=l2, cache_size3 = 0;

  bool need_help = false;
  for (int i=1; i<argc;)
  {
    if(argv[i][0]=='-')
    {
      if(argv[i][1]=='s')
      {
        ++i;
        s = atoi(argv[i++]);
        m = n = p = s;
        if(argv[i][0]!='-')
        {
          n = atoi(argv[i++]);
          p = atoi(argv[i++]);
        }
      }
      else if(argv[i][1]=='c')
      {
        ++i;
        cache_size1 = atoi(argv[i++]);
        if(argv[i][0]!='-')
        {
          cache_size2 = atoi(argv[i++]);
          if(argv[i][0]!='-')
            cache_size3 = atoi(argv[i++]);
        }
      }
      else if(argv[i][1]=='t')
      {
        ++i;
        tries = atoi(argv[i++]);
      }
      else if(argv[i][1]=='p')
      {
        ++i;
        rep = atoi(argv[i++]);
      }
    }
    else
    {
      need_help = true;
      break;
    }
  }

  if(need_help)
  {
    std::cout << argv[0] << " -s <matrix sizes> -c <cache sizes> -t <nb tries> -p <nb repeats>\n";
    std::cout << "   <matrix sizes> : size\n";
    std::cout << "   <matrix sizes> : rows columns depth\n";
    return 1;
  }

#if EIGEN_VERSION_AT_LEAST(3,2,90)
  if(cache_size1>0)
    setCpuCacheSizes(cache_size1,cache_size2,cache_size3);
#endif

  A a(m,p); a.setRandom();
  B b(p,n); b.setRandom();
  C c(m,n); c.setOnes();
  C rc = c;

  std::cout << "Matrix sizes = " << m << "x" << p << " * " << p << "x" << n << "\n";
  std::ptrdiff_t mc(m), nc(n), kc(p);
  internal::computeProductBlockingSizes<Scalar,Scalar>(kc, mc, nc);
  std::cout << "blocking size (mc x kc) = " << mc << " x " << kc << "\n";

  C r = c;

  // check the parallel product is correct
  #if defined EIGEN_HAS_OPENMP
  Eigen::initParallel();
  int procs = omp_get_max_threads();
  if(procs>1)
  {
    #ifdef HAVE_BLAS
    blas_gemm(a,b,r);
    #else
    omp_set_num_threads(1);
    r.noalias() += a * b;
    omp_set_num_threads(procs);
    #endif
    c.noalias() += a * b;
    if(!r.isApprox(c)) std::cerr << "Warning, your parallel product is crap!\n\n";
  }
  #elif defined HAVE_BLAS
    blas_gemm(a,b,r);
    c.noalias() += a * b;
    if(!r.isApprox(c)) {
      std::cout << (r  - c).norm() << "\n";
      std::cerr << "Warning, your product is crap!\n\n";
    }
  #else
    if(1.*m*n*p<2000.*2000*2000)
    {
      gemm(a,b,c);
      r.noalias() += a.cast<Scalar>() .lazyProduct( b.cast<Scalar>() );
      if(!r.isApprox(c)) {
        std::cout << (r  - c).norm() << "\n";
        std::cerr << "Warning, your product is crap!\n\n";
      }
    }
  #endif

  #ifdef HAVE_BLAS
  BenchTimer tblas;
  c = rc;
  BENCH(tblas, tries, rep, blas_gemm(a,b,c));
  std::cout << "blas  cpu         " << tblas.best(CPU_TIMER)/rep  << "s  \t" << (double(m)*n*p*rep*2/tblas.best(CPU_TIMER))*1e-9  <<  " GFLOPS \t(" << tblas.total(CPU_TIMER)  << "s)\n";
  std::cout << "blas  real        " << tblas.best(REAL_TIMER)/rep << "s  \t" << (double(m)*n*p*rep*2/tblas.best(REAL_TIMER))*1e-9 <<  " GFLOPS \t(" << tblas.total(REAL_TIMER) << "s)\n";
  #endif

  BenchTimer tmt;
  c = rc;
  BENCH(tmt, tries, rep, gemm(a,b,c));
  std::cout << "eigen cpu         " << tmt.best(CPU_TIMER)/rep  << "s  \t" << (double(m)*n*p*rep*2/tmt.best(CPU_TIMER))*1e-9  <<  " GFLOPS \t(" << tmt.total(CPU_TIMER)  << "s)\n";
  std::cout << "eigen real        " << tmt.best(REAL_TIMER)/rep << "s  \t" << (double(m)*n*p*rep*2/tmt.best(REAL_TIMER))*1e-9 <<  " GFLOPS \t(" << tmt.total(REAL_TIMER) << "s)\n";

  #ifdef EIGEN_HAS_OPENMP
  if(procs>1)
  {
    BenchTimer tmono;
    omp_set_num_threads(1);
    Eigen::setNbThreads(1);
    c = rc;
    BENCH(tmono, tries, rep, gemm(a,b,c));
    std::cout << "eigen mono cpu    " << tmono.best(CPU_TIMER)/rep  << "s  \t" << (double(m)*n*p*rep*2/tmono.best(CPU_TIMER))*1e-9  <<  " GFLOPS \t(" << tmono.total(CPU_TIMER)  << "s)\n";
    std::cout << "eigen mono real   " << tmono.best(REAL_TIMER)/rep << "s  \t" << (double(m)*n*p*rep*2/tmono.best(REAL_TIMER))*1e-9 <<  " GFLOPS \t(" << tmono.total(REAL_TIMER) << "s)\n";
    std::cout << "mt speed up x" << tmono.best(CPU_TIMER) / tmt.best(REAL_TIMER)  << " => " << (100.0*tmono.best(CPU_TIMER) / tmt.best(REAL_TIMER))/procs << "%\n";
  }
  #endif

  if(1.*m*n*p<30*30*30)
  {
      BenchTimer tmt;
      c = rc;
      BENCH(tmt, tries, rep, c.noalias()+=a.lazyProduct(b));
      std::cout << "lazy cpu         " << tmt.best(CPU_TIMER)/rep  << "s  \t" << (double(m)*n*p*rep*2/tmt.best(CPU_TIMER))*1e-9  <<  " GFLOPS \t(" << tmt.total(CPU_TIMER)  << "s)\n";
      std::cout << "lazy real        " << tmt.best(REAL_TIMER)/rep << "s  \t" << (double(m)*n*p*rep*2/tmt.best(REAL_TIMER))*1e-9 <<  " GFLOPS \t(" << tmt.total(REAL_TIMER) << "s)\n";
  }

  #ifdef DECOUPLED
  if((NumTraits<A::Scalar>::IsComplex) && (NumTraits<B::Scalar>::IsComplex))
  {
    M ar(m,p); ar.setRandom();
    M ai(m,p); ai.setRandom();
    M br(p,n); br.setRandom();
    M bi(p,n); bi.setRandom();
    M cr(m,n); cr.setRandom();
    M ci(m,n); ci.setRandom();

    BenchTimer t;
    BENCH(t, tries, rep, matlab_cplx_cplx(ar,ai,br,bi,cr,ci));
    std::cout << "\"matlab\" cpu    " << t.best(CPU_TIMER)/rep  << "s  \t" << (double(m)*n*p*rep*2/t.best(CPU_TIMER))*1e-9  <<  " GFLOPS \t(" << t.total(CPU_TIMER)  << "s)\n";
    std::cout << "\"matlab\" real   " << t.best(REAL_TIMER)/rep << "s  \t" << (double(m)*n*p*rep*2/t.best(REAL_TIMER))*1e-9 <<  " GFLOPS \t(" << t.total(REAL_TIMER) << "s)\n";
  }
  if((!NumTraits<A::Scalar>::IsComplex) && (NumTraits<B::Scalar>::IsComplex))
  {
    M a(m,p);  a.setRandom();
    M br(p,n); br.setRandom();
    M bi(p,n); bi.setRandom();
    M cr(m,n); cr.setRandom();
    M ci(m,n); ci.setRandom();

    BenchTimer t;
    BENCH(t, tries, rep, matlab_real_cplx(a,br,bi,cr,ci));
    std::cout << "\"matlab\" cpu    " << t.best(CPU_TIMER)/rep  << "s  \t" << (double(m)*n*p*rep*2/t.best(CPU_TIMER))*1e-9  <<  " GFLOPS \t(" << t.total(CPU_TIMER)  << "s)\n";
    std::cout << "\"matlab\" real   " << t.best(REAL_TIMER)/rep << "s  \t" << (double(m)*n*p*rep*2/t.best(REAL_TIMER))*1e-9 <<  " GFLOPS \t(" << t.total(REAL_TIMER) << "s)\n";
  }
  if((NumTraits<A::Scalar>::IsComplex) && (!NumTraits<B::Scalar>::IsComplex))
  {
    M ar(m,p); ar.setRandom();
    M ai(m,p); ai.setRandom();
    M b(p,n);  b.setRandom();
    M cr(m,n); cr.setRandom();
    M ci(m,n); ci.setRandom();

    BenchTimer t;
    BENCH(t, tries, rep, matlab_cplx_real(ar,ai,b,cr,ci));
    std::cout << "\"matlab\" cpu    " << t.best(CPU_TIMER)/rep  << "s  \t" << (double(m)*n*p*rep*2/t.best(CPU_TIMER))*1e-9  <<  " GFLOPS \t(" << t.total(CPU_TIMER)  << "s)\n";
    std::cout << "\"matlab\" real   " << t.best(REAL_TIMER)/rep << "s  \t" << (double(m)*n*p*rep*2/t.best(REAL_TIMER))*1e-9 <<  " GFLOPS \t(" << t.total(REAL_TIMER) << "s)\n";
  }
  #endif

  return 0;
}


/* test_negative_binomial_distribution.cpp
 *
 * Copyright Steven Watanabe 2010
 * 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)
 *
 * $Id$
 *
 */

#include <boost/random/negative_binomial_distribution.hpp>
#include <limits>

#define BOOST_RANDOM_DISTRIBUTION boost::random::negative_binomial_distribution<>
#define BOOST_RANDOM_ARG1 k
#define BOOST_RANDOM_ARG2 p
#define BOOST_RANDOM_ARG1_DEFAULT 1
#define BOOST_RANDOM_ARG2_DEFAULT 0.5
#define BOOST_RANDOM_ARG1_VALUE 10
#define BOOST_RANDOM_ARG2_VALUE 0.25

#define BOOST_RANDOM_DIST0_MIN 0
#define BOOST_RANDOM_DIST0_MAX (std::numeric_limits<int>::max)()
#define BOOST_RANDOM_DIST1_MIN 0
#define BOOST_RANDOM_DIST1_MAX (std::numeric_limits<int>::max)()
#define BOOST_RANDOM_DIST2_MIN 0
#define BOOST_RANDOM_DIST2_MAX (std::numeric_limits<int>::max)()

#define BOOST_RANDOM_TEST1_PARAMS
#define BOOST_RANDOM_TEST1_MIN 0
#define BOOST_RANDOM_TEST1_MAX 10

#define BOOST_RANDOM_TEST2_PARAMS (100, 0.5)
#define BOOST_RANDOM_TEST2_MIN 50

#include "test_distribution.ipp"


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_FAST_HYPOT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_FAST_HYPOT_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/fast_hypot.hpp>
#include <boost/simd/include/functions/simd/sqrt.hpp>
#include <boost/simd/include/functions/simd/sqr.hpp>
#include <boost/simd/include/functions/simd/fma.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( fast_hypot_, tag::cpu_
                                    , (A0)
                                    , (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return boost::simd::sqrt(boost::simd::fma(a0, a0, sqr(a1)));
    }
  };

} } }

#endif


#include "../../includes/SpecialFunc.h"
#include "../../includes/utils.h"
#include "../../includes/BaseClass.h"
#include <iostream>

#include <stdio.h>
#include <tgmath.h>
#include <math.h>
#include <complex.h>
#include <boost/math/quadrature/trapezoidal.hpp>

#include <pybind11/pybind11.h>
#include <pybind11/complex.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>

namespace py = pybind11;

typedef std::vector<double> Vec;
typedef std::complex<double> complex128;
typedef std::vector<complex128> iVec;
typedef py::array_t<double> ndarray;
typedef py::array_t<complex128> Cndarray;
#define J complex128(0.0,1.0)

#define EPS 1e-10
#define Correction 1.//2.*exp(J*1.0472351410849077)

using boost::math::quadrature::trapezoidal;
using boost::math::constants::two_pi;
using boost::math::constants::pi;

struct argument
  {
  int n, m;
  double r, rhon, angle, s, R0, w0, xi, k;
  complex128 Q, beta;
  Vec Offset;
} args, args0 ;

template <typename func_type>
complex128 simpson_rule(func_type f,
                        double a,
                        double b,
                        int n // Number of intervals
                        )
{
    double h = (b - a) / n;

    // Internal sample points, there should be n - 1 of them
    complex128 sum_odds = 0.0;
    for (int i = 1; i < n; i += 2)
    {
        sum_odds += f(a + i * h);
    }
    complex128 sum_evens = 0.0;
    for (int i = 2; i < n; i += 2)
    {
        sum_evens += f(a + i * h);
    }

    return (f(a) + f(b) + 2. * sum_evens + 4. * sum_odds) * h / 3.;
}

iVec
Q_(double r,
  Vec    theta,
  double w0,
  Vec    Offset,
  double k)
{
   iVec result;
   for (auto const& angle: theta)
   {
     result.push_back(1./( 2. * ( r * cos(angle) - Offset[2]/k )/(k * w0*w0 ) - J ) ) ;
   }
   return result;
}

complex128
Q(double r,
  double theta,
  double w0,
  Vec    Offset,
  double k)
{
  return 1./( 2. * ( r * cos(theta) - Offset[2]/k)/(k * w0*w0 ) - J )  ;
}

complex128
ImSimpson(int m, complex128 beta)
{
 auto func = [=](double angle){return exp( beta * cos(angle)  - J *  (double)m * angle) ;};

 complex128 integral = simpson_rule(func,  0.0, two_pi<double>(), 1000);

 return 1./( two_pi<double>()) * integral;
}

complex128
ImTrapz(int m, complex128 beta)
{
 auto func = [=](double angle)->complex128{return exp( beta * cos(angle)  - J *  (double)m * angle) ;};

 complex128 integral = trapezoidal(func, 0.0, two_pi<double>(), EPS);

 return 1./( two_pi<double>()) * integral;
}

complex128
ImHat(double     m,
      complex128 beta,
      double     xi)
{
  return ImTrapz(m, beta) * exp(-beta - J * m * xi ) ;
}

complex128
I_0(argument arg)
{

  complex128 term0 = pow(-J, arg.n) * arg.rhon*arg.rhon,
             term1 = (2. * (double)arg.n + 1.) * _Psi(0,arg.n, arg.rhon),
             term2 = exp(-J * arg.Offset[2]);

  return term0 / term1 * term2;
}

complex128 I_2(argument arg){ return (2. * arg.Q * arg.s*arg.s * arg.rhon * cos(arg.angle) - 1. ) * sin(arg.angle); }

complex128 I_3(argument arg){ return 4. * arg.Q * arg.s*arg.s * arg.Offset[0] * cos(arg.angle); }

complex128 I_4(argument arg){ return 4. * arg.Q * arg.s*arg.s * arg.Offset[1] * cos(arg.angle); }

complex128
I_1(argument arg)
{
  complex128 term0 = J * arg.Q * arg.s*arg.s * pow(arg.R0 - arg.rhon * sin(arg.angle), 2. ),
             term2 = J * arg.rhon * cos(arg.angle),
             term3 = NPnm(arg.n, abs(arg.m), cos(arg.angle)) * sin(arg.angle) ;

  return arg.Q * exp(term0 + term2 ) * term3;
}

complex128
Bnm_integrand(double angle, argument args0)
{
  if (abs(args0.m) >args0.n){return 0;}

  args0.Q = Q(args0.r, angle, args0.w0, args0.Offset, args0.k);

  args0.angle = angle;

  complex128 beta = -2. * J * args0.Q * args0.s*args0.s * args0.R0 * args0.rhon * sin(angle);

  complex128 term0 =  ImHat(args0.m+1, beta, args0.xi) - ImHat(args0.m-1, beta, args0.xi);

  term0 *= I_2(args0);

  term0 -= (I_4(args0) * ImHat(args0.m, beta, args0.xi));

  term0 *= I_1(args0);

  return term0;

}

complex128
Anm_integrand(double angle, argument args0)
{
  if (abs(args0.m) >args0.n){return 0;}

  args0.Q = Q(args0.r, angle, args0.w0, args0.Offset, args0.k);

  args0.angle = angle;

  complex128 beta = -2. * J * args0.Q * args0.s*args0.s * args0.R0 * args0.rhon * sin(angle);

  complex128 term0 =  ImHat(args0.m+1, beta, args0.xi) + ImHat(args0.m-1, beta, args0.xi);

  term0 *= I_2(args0);

  term0 -= (I_3(args0) * ImHat(args0.m, beta, args0.xi));

  term0 *= I_1(args0);

  return term0;

}

complex128
Anm(int    n,
    int    m,
    double k,
    double w0,
    Vec    Offset,
    double Tolerance)
{
  argument args0;
           args0.n      = n;
           args0.m      = m;
           args0.rhon   = (n + 0.5);
           args0.Offset = Offset;
           args0.k      = k;
           args0.R0     = sqrt(Offset[0]*Offset[0] + Offset[1]*Offset[1]);
           args0.xi     = acos(Offset[0]/args0.R0);
           args0.s      = 1./(k*w0);
           args0.w0     = w0;
           args0.r      = args.rhon/k;

  auto func_ = [=](double angle)->complex128 {return Anm_integrand(angle, args0);};

  return -trapezoidal(func_, 0.0, pi<double>(),Tolerance) * I_0(args0) * Correction;

}

complex128
Bnm(int n,
    int m,
    double k,
    double w0,
    Vec Offset,
    double Tolerance)
{
  argument args0;
           args0.n      = n;
           args0.m      = m;
           args0.rhon   = (n + 0.5);
           args0.Offset = Offset;
           args0.k      = k;
           args0.R0     = sqrt(Offset[0]*Offset[0] + Offset[1]*Offset[1]);
           args0.xi     = acos(Offset[0]/args0.R0);
           args0.s      = 1./(k*w0);
           args0.w0     = w0;
           args0.r      = args.rhon/k;

  auto func_ = [=](double angle)->complex128 {return Bnm_integrand(angle, args0);};

  return -trapezoidal(func_, 0.0, pi<double>(),Tolerance) * I_0(args0) * Correction;

}

std::tuple<ndarray,Cndarray>
PyAnm_integrand(int    n,
                int    m,
                int    sampling,
                double k,
                double w0,
                Vec    Offset,
                double Tolerance)
{

 argument args0;
          args0.n      = n;
          args0.m      = m;
          args0.rhon   = (n + 0.5);
          args0.Offset = Offset;
          args0.k      = k;
          args0.R0     = sqrt(Offset[0]*Offset[0] + Offset[1]*Offset[1]);
          args0.xi     = acos(Offset[0]/args0.R0);
          args0.s      = 1./(k*w0);
          args0.w0     = w0;
          args0.r      = args.rhon/k;

  Cndarray _Anm = Cndarray(sampling);
  auto _Anm_data = _Anm.mutable_data();

  Cndarray Angle = Cndarray(sampling);
  auto Angle_data = Angle.mutable_data();

  Vec X = Linspace( 0.0, pi<double>(),sampling);

  for (auto i=0; i<sampling;i++)
  {
    _Anm_data[i] = Anm_integrand(X[i], args);
    Angle_data[i] = X[i];
  }
  return std::make_tuple(Angle, _Anm);
}

std::tuple<ndarray,Cndarray>
PyBnm_integrand(int    n,
                int    m,
                int    sampling,
                double k,
                double w0,
                Vec    Offset,
                double Tolerance)
{

  argument args0;
           args0.n      = n;
           args0.m      = m;
           args0.rhon   = (n + 0.5);
           args0.Offset = Offset;
           args0.k      = k;
           args0.R0     = sqrt(Offset[0]*Offset[0] + Offset[1]*Offset[1]);
           args0.xi     = acos(Offset[0]/args0.R0);
           args0.s      = 1./(k*w0);
           args0.w0     = w0;
           args0.r      = args.rhon/k;

   Cndarray _Bnm = Cndarray(sampling);
   auto _Bnm_data = _Bnm.mutable_data();

   Cndarray Angle = Cndarray(sampling);
   auto Angle_data = Angle.mutable_data();

   Vec X = Linspace( 0.0, pi<double>(),sampling);

   for (auto i=0; i<sampling;i++)
   {
     _Bnm_data[i] = Bnm_integrand(X[i], args);
     Angle_data[i] = X[i];
   }
   return std::make_tuple(Angle, _Bnm);
 }

PYBIND11_MODULE(GaussianBeam, module) {
    module.doc() = "Generalized Lorenz-Mie Theory (GLMT) c++ binding module for light scattering from a spherical scatterer";

    module.def("Anm",
               &Anm,
               py::arg("n"),
               py::arg("m"),
               py::arg("k"),
               py::arg("w0"),
               py::arg("Offset"),
               py::arg("Tolerance") = 1e-8,
               "Compute Anm");

     module.def("Anm_integrand",
                &PyAnm_integrand,
                py::arg("n"),
                py::arg("m"),
                py::arg("sampling"),
                py::arg("k"),
                py::arg("w0"),
                py::arg("Offset"),
                py::arg("Tolerance") = 1e-8,
                "Compute Anm integrand as a function of theta");

    module.def("Bnm",
               &Bnm,
               py::arg("n"),
               py::arg("m"),
               py::arg("k"),
               py::arg("w0"),
               py::arg("Offset"),
               py::arg("Tolerance") = 1e-8,
               "Compute Bnm");

     module.def("Bnm_integrand",
                &PyBnm_integrand,
                py::arg("n"),
                py::arg("m"),
                py::arg("sampling"),
                py::arg("k"),
                py::arg("w0"),
                py::arg("Offset"),
                py::arg("Tolerance") = 1e-8,
                "Compute Bnm integrand as a function of theta");
}

//-


#pragma once

#include <boost/array.hpp>
#include <mhe_estimator/CanData.h>
#include <mhe_estimator/ArticulatedAngles.h>
#include "ackermann_msgs/AckermannDrive.h"
#include <tf/tf.h>

namespace mhe_estimator
{
    using namespace ast;

    typedef double Real;
    typedef boost::array<Real, 6> Vec6;
    typedef boost::array<Real, 5> Vec5;
    typedef boost::array<Real, 4> Vec4;
    typedef boost::array<Real, 3> Vec3;
    typedef boost::array<Real, 2> Vec2;

    struct CarParams
    {
        bool moveGuidancePoint, respectSteeringLimits;
        Real L, L1, Lh1, L2, Lh2;
        Real steeringLimit, trailer1Limit, trailer2Limit, LinearVelLimit, SteeringVelLimit;
        Real upperTh, lowerTh, upperX, lowerX, upperY, lowerY;
        Real TrailerNumber;
    };

    struct MheParams
    {
        bool mheActive, WeightedActive, covarianceFromTopicStamp, perceptionGPS;
        Real loopRate;
        Real noiseVariancePos, noiseVarianceTh, noiseVariancesteering, noiseVarianceTrailer1;
        Real noiseVarianceTrailer2, noiseVarianceLinearVel, noiseVarianceSteeringVel;
        Real WeightPos, WeightTh, WeightSteering, WeightTrailer1, WeightTrailer2;

    };

    inline int sign(Real val)
    {
      return (0 < val) - (val < 0);
    }

    inline Real sat(Real x, Real lim)
    {
      if(x > lim)
        return lim;
      else if(x < -lim)
        return - lim;
      else
        return x;
    }

    inline double continuousAngle(Real angle, Real lastAngle)
    {
        auto dAngle = fmod(angle, 2*M_PI) - fmod(lastAngle, 2*M_PI);

        if (dAngle > M_PI)
            return lastAngle + dAngle - 2.0 * M_PI;
        else if (dAngle < -M_PI)
            return lastAngle + dAngle + 2.0 * M_PI;
        else
            return lastAngle + dAngle;
    }
    //Kinematics
    inline Vec3 RDCarKinematicsGPFront(const CarParams& params,const  Vec3& q,const ackermann_msgs::AckermannDrive& u)
    {
        Vec3 dq;
        Real vF = u.speed / cos(u.steering_angle);

        dq[0] = u.speed*(1/params.L)*tan(u.steering_angle);
        dq[1] = vF*cos(u.steering_angle + q[0]);
        dq[2] = vF*sin(u.steering_angle + q[0]);
        return dq;
    }

    inline Vec3 RDCarKinematicsGPRear(const CarParams& params,const Vec3& q,const ackermann_msgs::AckermannDrive& u)
    {
        Vec3 dq;
        dq[0] = u.speed*(1/params.L)*tan(u.steering_angle);
        dq[1] = u.speed*cos(q[0]);
        dq[2] = u.speed*sin(q[0]);
        return dq;
    }

    inline Vec4 OneTrailerKinematicsGPFront(const CarParams& params,const Vec4& q,const ackermann_msgs::AckermannDrive& u, bool brakeOnSingularity = true)
    {

        Vec4 dq;
        Real k1 = (1/params.L1)*tan(q[0] - atan((params.Lh1/params.L)*tan(u.steering_angle)));
        dq[0] = u.speed * (sin(q[0])/params.Lh1 - (1 + (params.L1/params.Lh1)*cos(q[0]))*k1);
        dq[1] = u.speed * ( -(params.L1/params.Lh1)*cos(q[0])*k1 + sin(q[0])/params.Lh1 );
        dq[2] = u.speed * cos(q[1]) * ( params.L1*sin(q[0])*k1 + cos(q[0]) );
        dq[3] = u.speed * sin(q[1]) * ( params.L1*sin(q[0])*k1 + cos(q[0]) );
        return dq;
    }

    inline Vec4 OneTrailerKinematicsGPRear(const CarParams& params,const Vec4& q,const ackermann_msgs::AckermannDrive& u, bool brakeOnSingularity = true)
    {
        Vec4 dq;
        Real k1 = (1/params.L1)*tan(q[0] - atan((params.Lh1/params.L)*tan(u.steering_angle)));

        dq[0] = u.speed * (sin(q[0])/params.Lh1 - (1 + (params.L1/params.Lh1)*cos(q[0]))*k1);
        dq[1] = u.speed*k1;
        dq[2] = u.speed*cos(q[1]);
        dq[3] = u.speed*sin(q[1]);
        return dq;
    }

    template <class T>
    void singleParamIn(T& ParamVar,std::string& paramName,ast::ros::NodeHandle& nh)
    {
        if (nh.hasParam(paramName))
        {
        nh.getParam(paramName,ParamVar);
        ROS_INFO_STREAM(""<<paramName<<" = "<<ParamVar<<"");
        }else
        {
        ROS_WARN_STREAM("parameter: " << paramName << " could not be found");
        }
    }

    void ParamsIn(mhe_estimator::CarParams& carParams,mhe_estimator::MheParams& mheParams,ast::ros::NodeHandle& nh)
    {
        std::string paramName;

        paramName = "/mhe_estimator/mheParam/mheActive";
        singleParamIn(mheParams.mheActive,paramName,nh);

        paramName = "/mhe_estimator/mheParam/WeightedActive";
        singleParamIn(mheParams.WeightedActive,paramName,nh);

        paramName = "/mhe_estimator/mheParam/loopRate";
        singleParamIn(mheParams.loopRate,paramName,nh);

        paramName = "/mhe_estimator/mheParam/perceptionGPS";
        singleParamIn(mheParams.perceptionGPS,paramName,nh);

        paramName = "/mhe_estimator/mheParam/covarianceFromTopicStamp";
        singleParamIn(mheParams.covarianceFromTopicStamp,paramName,nh);

        paramName = "/mhe_estimator/mheParam/noiseVariancePos";
        singleParamIn(mheParams.noiseVariancePos,paramName,nh);

        paramName = "/mhe_estimator/mheParam/noiseVarianceTh";
        singleParamIn(mheParams.noiseVarianceTh,paramName,nh);

        paramName = "/mhe_estimator/mheParam/noiseVariancesteering";
        singleParamIn(mheParams.noiseVariancesteering,paramName,nh);

        paramName = "/mhe_estimator/mheParam/noiseVarianceTrailer1";
        singleParamIn(mheParams.noiseVarianceTrailer1,paramName,nh);

        paramName = "/mhe_estimator/mheParam/noiseVarianceTrailer2";
        singleParamIn(mheParams.noiseVarianceTrailer2,paramName,nh);

        paramName = "/mhe_estimator/mheParam/noiseVarianceLinearVel";
        singleParamIn(mheParams.noiseVarianceLinearVel,paramName,nh);

        paramName = "/mhe_estimator/mheParam/noiseVarianceSteeringVel";
        singleParamIn(mheParams.noiseVarianceSteeringVel,paramName,nh);

        paramName = "/mhe_estimator/mheParam/WeightPos";
        singleParamIn(mheParams.WeightPos,paramName,nh);

        paramName = "/mhe_estimator/mheParam/WeightTh";
        singleParamIn(mheParams.WeightTh,paramName,nh);

        paramName = "/mhe_estimator/mheParam/WeightSteering";
        singleParamIn(mheParams.WeightSteering,paramName,nh);

        paramName = "/mhe_estimator/mheParam/WeightTrailer1";
        singleParamIn(mheParams.WeightTrailer1,paramName,nh);

        paramName = "/mhe_estimator/mheParam/WeightTrailer2";
        singleParamIn(mheParams.WeightTrailer2,paramName,nh);

        paramName = "/mhe_estimator/carParam/WagonNumbers";
        singleParamIn(carParams.TrailerNumber,paramName,nh);

        paramName = "/mhe_estimator/carParam/L";
        singleParamIn(carParams.L,paramName,nh);

        paramName = "/mhe_estimator/carParam/L1";
        singleParamIn(carParams.L1,paramName,nh);

        paramName = "/mhe_estimator/carParam/Lh1";
        singleParamIn(carParams.Lh1,paramName,nh);

        paramName = "/mhe_estimator/carParam/L2";
        singleParamIn(carParams.L2,paramName,nh);

        paramName = "/mhe_estimator/carParam/Lh2";
        singleParamIn(carParams.Lh2,paramName,nh);

        paramName = "/mhe_estimator/carParam/steeringLimit";
        singleParamIn(carParams.steeringLimit,paramName,nh);

        paramName = "/mhe_estimator/carParam/trailer1Limit";
        singleParamIn(carParams.trailer1Limit,paramName,nh);

        paramName = "/mhe_estimator/carParam/trailer2Limit";
        singleParamIn(carParams.trailer2Limit,paramName,nh);

        paramName = "/mhe_estimator/carParam/LinearVelLimit";
        singleParamIn(carParams.LinearVelLimit,paramName,nh);

        paramName = "/mhe_estimator/carParam/SteeringVelLimit";
        singleParamIn(carParams.SteeringVelLimit,paramName,nh);

        paramName = "/mhe_estimator/carParam/moveGuidancePoint";
        singleParamIn(carParams.moveGuidancePoint,paramName,nh);

        paramName = "/mhe_estimator/carParam/respectSteeringLimits";
        singleParamIn(carParams.respectSteeringLimits,paramName,nh);

        paramName = "/mhe_estimator/carParam/upperTh";
        singleParamIn(carParams.upperTh,paramName,nh);

        paramName = "/mhe_estimator/carParam/lowerTh";
        singleParamIn(carParams.lowerTh,paramName,nh);

        paramName = "/mhe_estimator/carParam/upperX";
        singleParamIn(carParams.upperX,paramName,nh);

        paramName = "/mhe_estimator/carParam/lowerX";
        singleParamIn(carParams.lowerX,paramName,nh);

        paramName = "/mhe_estimator/carParam/upperY";
        singleParamIn(carParams.upperY,paramName,nh);

        paramName = "/mhe_estimator/carParam/lowerY";
        singleParamIn(carParams.lowerY,paramName,nh);

        ROS_INFO_STREAM("End of receiving parameters");

    }

}