//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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
//==============================================================================
#include <nt2/trigonometric/include/functions/rem_2pi.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/twopi.hpp>
#include <nt2/include/constants/zero.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>

NT2_TEST_CASE_TPL ( rem_2pi_real__1_0,  NT2_SIMD_REAL_TYPES)
{
  using nt2::rem_2pi;
  using nt2::tag::rem_2pi_;
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                vT;

  // specific values tests
  {
    vT res = rem_2pi(nt2::Zero<vT>());
    NT2_TEST_ULP_EQUAL( res, nt2::Zero<vT>(), 1.5);
    res = rem_2pi(nt2::Pi<vT>());
    NT2_TEST_ULP_EQUAL( res, nt2::Pi<vT>(), 1.5);
    res = rem_2pi(nt2::Twopi<vT>());
    NT2_TEST_ULP_EQUAL( res, nt2::Zero<vT>(), 1.5);
    res = rem_2pi(nt2::Pio_2<vT>());
    NT2_TEST_ULP_EQUAL( res, nt2::Pio_2<vT>(), 1.5);
  }
} // end of test for floating_

NT2_TEST_CASE_TPL ( rem_2pi_targeted,  NT2_SIMD_REAL_TYPES)
{

  using nt2::rem_2pi;
  using nt2::tag::rem_2pi_;
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                vT;

  vT x = nt2::Twopi<vT>(), xr;
  xr = rem_2pi(x, nt2::meta::as_<nt2::big_>());
  NT2_TEST_ULP_EQUAL( xr, nt2::Zero<vT>(), 1.5);

  xr = rem_2pi(x, nt2::meta::as_<nt2::medium_>());
  NT2_TEST_ULP_EQUAL( xr, nt2::Zero<vT>(), 1.5);

  xr = rem_2pi(x, nt2::meta::as_<nt2::small_>());
  NT2_TEST_ULP_EQUAL( xr, nt2::Zero<vT>(), 1.5);

  xr = rem_2pi(x, nt2::meta::as_<nt2::very_small_>());
  NT2_TEST_ULP_EQUAL( xr, nt2::Zero<vT>(), 1.5);

}


/*=============================================================================

  NifTK: A software platform for medical image computing.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#if defined(_MSC_VER)
#pragma warning ( disable : 4786 )
#endif

#include <mitkTestingMacros.h>
#include <mitkLogMacros.h>
#include <mitkCameraCalibrationFacade.h>
#include <mitkHandeyeCalibrate.h>
#include <mitkOpenCVMaths.h>
#include <mitkOpenCVFileIOUtils.h>

#include <boost/lexical_cast.hpp>
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <niftkFileHelper.h>
#include <fstream>

/**
 * Test for stereo calibration and handeye.
 * Start with the calibration grid, located somewhere in 3D space
 * Load in some camera calibration parameters, and some tracking matrices,
 * Project the grid points onto the camera, then use
 * mitk::CalibrateStereoCameraParameters to reconstruct the calibration.
 * Optionally then perform and handeye calibration.
 * Optionally add some tracking and or projection errors
 */
bool CompareOpenCVMatrices2(cv::Mat mat1, cv::Mat mat2 , double tolerance)
{
  assert ( mat1.size() == mat2.size() );
  assert ( mat1.type() == mat2.type() );
  cv::Mat absDiff = cv::Mat(mat1.size(),mat1.type());
  cv::Mat abs = cv::Mat(mat1.size(),mat1.type());
  cv::absdiff(mat1,mat2,absDiff);
  abs = cv::abs(mat1);
  cv::Scalar Sum1 = cv::sum (abs);
  cv::Scalar Sum = cv::sum (absDiff);
  MITK_INFO << std::endl << "Absolute difference Sum = " << Sum[0] << " : Normalised sum = " << Sum[0]/Sum1[0];
  if ( Sum[0]/Sum1[0] < tolerance )
  {
    return true;
  }
  else
  {
    return false;
  }
}
bool CompareOpenCVMatrices(CvMat* mat1, cv::Mat mat2, double tolerance)
{
  return CompareOpenCVMatrices2(cv::Mat(mat1), mat2,tolerance);
}
int mitkIdealStereoCalibrationTest ( int argc, char * argv[] )
{

  std::string calibrationDirectory = "";
  std::string gridToWorldTransform = "";
  std::string trackingMatrixDirectory = "";
  double pixelNoise = 0.0;
  double trackingNoise = 0.0;
  bool quantize = false;
  bool discardFramesWithNonVisiblePoints = true;
  double screenWidth = 1980;
  double screenHeight = 540;
  double tolerance = 0.05;

  cv::Size imageSize;
  imageSize.height = screenHeight;
  imageSize.width = screenWidth;
  //the calibration grid pattern;
  int xcorners = 14;
  int ycorners = 10;
  double squareSize = 3; // in mm
  int maxTrackingMatrices = -1;

  cv::Mat gridToWorld = cvCreateMat (4,4,CV_64FC1);

  bool ok;
  while ( argc > 1 )
  {
    ok = false;
    if (( ok == false ) && (strcmp(argv[1],"--calibration") == 0 ))
    {
      argc--;
      argv++;
      calibrationDirectory = argv[1];
      MITK_INFO << "Loading calibration from " << calibrationDirectory;
      argc--;
      argv++;
      ok=true;
    }
    if (( ok == false ) && (strcmp(argv[1],"--tolerance") == 0 ))
    {
      argc--;
      argv++;
      tolerance = atof(argv[1]);
      MITK_INFO << "Setting tolerance to  " << tolerance;
      argc--;
      argv++;
      ok=true;
    }
    if (( ok == false ) && (strcmp(argv[1],"--tracking") == 0 ))
    {
      argc--;
      argv++;
      trackingMatrixDirectory = argv[1];
      MITK_INFO << "Loading tracking from " << trackingMatrixDirectory;
      argc--;
      argv++;
      ok=true;
    }
    if (( ok == false ) && (strcmp(argv[1],"--gridToWorldTransform")==0))
    {
      argc--;
      argv++;
      std::ifstream fin(argv[1]);
      for ( int row = 0; row < 4; row ++ )
      {
        for ( int col = 0; col < 4; col ++ )
        {
          fin >> gridToWorld.at<double>(row,col);
        }
      }
      fin.close();
      argc--;
      argv++;
      ok=true;
    }

    if (( ok == false ) && (strcmp(argv[1],"--maxTrackingMatricesToUse") == 0 ))
    {
      argc--;
      argv++;
      maxTrackingMatrices = atof(argv[1]);
      argc--;
      argv++;
      ok =true;
    }
    if (( ok == false ) && (strcmp(argv[1],"--pixelNoise") == 0 ))
    {
      argc--;
      argv++;
      pixelNoise = atof(argv[1]);
      argc--;
      argv++;
      ok =true;
    }
    if (( ok == false ) && (strcmp(argv[1],"--quantize") == 0 ))
    {
      argc--;
      argv++;
      quantize=true;
      ok=true;
    }
    if ( ok == false )
    {
      MITK_ERROR << "Failed to parse arguments";
      return EXIT_FAILURE;
    }
  }

  MITK_TEST_BEGIN("mitkIdealStereoCalibrationTest");

  //get the tracking matrices
  std::vector<cv::Mat> MarkerToWorld = mitk::LoadMatricesFromDirectory(trackingMatrixDirectory);

  //get the calibration data
  cv::Mat leftCameraPositionToFocalPointUnitVector = cv::Mat(1,3,CV_64FC1);
  cv::Mat leftCameraIntrinsic = cv::Mat(3,3,CV_64FC1);
  cv::Mat leftCameraDistortion = cv::Mat(1,4,CV_64FC1);
  cv::Mat rightCameraIntrinsic = cv::Mat(3,3,CV_64FC1);
  cv::Mat rightCameraDistortion = cv::Mat(1,4,CV_64FC1);
  cv::Mat rightToLeftRotationMatrix = cv::Mat(3,3,CV_64FC1);
  cv::Mat rightToLeftTranslationVector = cv::Mat(3,1,CV_64FC1);
  cv::Mat leftCameraToTracker = cv::Mat(4,4,CV_64FC1);

  mitk::LoadStereoCameraParametersFromDirectory (calibrationDirectory,
     &leftCameraIntrinsic,&leftCameraDistortion,&rightCameraIntrinsic,
     &rightCameraDistortion,&rightToLeftRotationMatrix,
     &rightToLeftTranslationVector,&leftCameraToTracker);

  leftCameraPositionToFocalPointUnitVector.at<double>(0,0) = 0.0;
  leftCameraPositionToFocalPointUnitVector.at<double>(0,1) = 0.0;
  leftCameraPositionToFocalPointUnitVector.at<double>(0,2) = 1.0;

  std::vector<cv::Mat> allLeftImagePoints;
  std::vector<cv::Mat> allLeftObjectPoints;
  std::vector<cv::Mat> allRightImagePoints;
  std::vector<cv::Mat> allRightObjectPoints;

  cv::Mat worldPoints = cv::Mat (xcorners*ycorners,3,CV_64FC1);
  cv::Mat gridPoints = cv::Mat (xcorners*ycorners,3,CV_64FC1);
  cv::Mat leftCameraNormals = cv::Mat (xcorners*ycorners,3,CV_64FC1);

  for ( int row = 0 ; row < ycorners ; row ++ )
  {
    for ( int col = 0 ; col < xcorners ; col ++ )
    {
      cv::Point3d gridCorner = cv::Point3d ( row * squareSize, col * squareSize, 0 );
      cv::Point3d worldCorner;
      {
        using namespace mitk;
        worldCorner = gridToWorld * gridCorner;
      }
      worldPoints.at<double>(row * xcorners + col,0) = worldCorner.x;
      worldPoints.at<double>(row * xcorners + col,1) = worldCorner.y;
      worldPoints.at<double>(row * xcorners + col,2) = worldCorner.z;
      gridPoints.at<double>(row * xcorners + col,0) = gridCorner.x;
      gridPoints.at<double>(row * xcorners + col,1) = gridCorner.y;
      gridPoints.at<double>(row * xcorners + col,2) = gridCorner.z;
      leftCameraNormals.at<double>(row*xcorners + col,0) = 0.0;
      leftCameraNormals.at<double>(row*xcorners + col,1) = 0.0;
      leftCameraNormals.at<double>(row*xcorners + col,2) = -1.0;
    }
  }
  const int corner00 = 0;
  const int corner01 = 0 * xcorners + ycorners-1;
  const int corner10 = (ycorners-1)*xcorners;
  const int corner11 = (ycorners-1)*xcorners + (xcorners-1);

  MITK_INFO << "World Corners 1: (" << worldPoints.at<double>(corner00,0) << "," <<
    worldPoints.at<double>(corner00,1) << "," <<  worldPoints.at<double>(corner00,2) << ")(" <<
    worldPoints.at<double>(corner01,0) << "," <<
    worldPoints.at<double>(corner01,1) << "," <<
    worldPoints.at<double>(corner01,2) << ")";
  MITK_INFO << "World Corners 2: (" <<
    worldPoints.at<double>(corner10,0) << "," <<
    worldPoints.at<double>(corner10,1) << "," <<
    worldPoints.at<double>(corner10,2) << ")(" <<
    worldPoints.at<double>(corner11,0) << "," <<
    worldPoints.at<double>(corner11,1) << "," <<
    worldPoints.at<double>(corner11,2) << ")";
  std::string trackerDirectory = "testTrackerMatrices";
  niftk::CreateDirAndParents(trackerDirectory);

  int views = MarkerToWorld.size();
  int stepSize = 1;

  if ( maxTrackingMatrices != -1 )
  {
    if ( maxTrackingMatrices < MarkerToWorld.size() )
    {
      stepSize = MarkerToWorld.size() / maxTrackingMatrices;
    }
    else
    {
      stepSize = 1;
    }
  }

  for ( unsigned int frame = 0 ; frame < views ; frame += stepSize )
  {

    //get world points into camera lens coordinates
    cv::Mat leftCameraPoints = cv::Mat (xcorners*ycorners,3,CV_64FC1);
    cv::Mat worldToCamera = (MarkerToWorld[frame] * leftCameraToTracker).inv();

    for ( int j = 0 ; j < xcorners*ycorners ; j ++ )
    {
      cv::Point3d worldCorner ;
      cv::Point3d leftCameraCorner;
      worldCorner.x = worldPoints.at<double>(j,0);
      worldCorner.y = worldPoints.at<double>(j,1);
      worldCorner.z = worldPoints.at<double>(j,2);

      {
        using namespace mitk;
        leftCameraCorner = worldToCamera * worldCorner;
      }
      leftCameraPoints.at<double>(j,0) = leftCameraCorner.x;
      leftCameraPoints.at<double>(j,1) = leftCameraCorner.y;
      leftCameraPoints.at<double>(j,2) = leftCameraCorner.z;
    }
    MITK_INFO << frame << " Left Cam Corners 1: (" << leftCameraPoints.at<double>(corner00,0) << "," <<
      leftCameraPoints.at<double>(corner00,1) << "," <<  leftCameraPoints.at<double>(corner00,2) << ")(" <<
      leftCameraPoints.at<double>(corner01,0) << "," <<
      leftCameraPoints.at<double>(corner01,1) << "," <<
      leftCameraPoints.at<double>(corner01,2) << ")";
    MITK_INFO << frame << " Left Cam Corners 2: (" <<
      leftCameraPoints.at<double>(corner10,0) << "," <<
      leftCameraPoints.at<double>(corner10,1) << "," <<
      leftCameraPoints.at<double>(corner10,2) << ")(" <<
      leftCameraPoints.at<double>(corner11,0) << "," <<
      leftCameraPoints.at<double>(corner11,1) << "," <<
      leftCameraPoints.at<double>(corner11,2) << ")";

    //project onto screen
    CvMat* outputLeftCameraWorldPointsIn3D = NULL;
    CvMat* outputLeftCameraWorldNormalsIn3D = NULL ;
    CvMat* output2DPointsLeft = NULL ;
    CvMat* output2DPointsRight = NULL;
//

    //project points
    std::vector<int> Points = mitk::ProjectVisible3DWorldPointsToStereo2D
    ( leftCameraPoints,leftCameraNormals,
      leftCameraPositionToFocalPointUnitVector,
      leftCameraIntrinsic,leftCameraDistortion,
      rightCameraIntrinsic,rightCameraDistortion,
      rightToLeftRotationMatrix,rightToLeftTranslationVector,
      outputLeftCameraWorldPointsIn3D,
      outputLeftCameraWorldNormalsIn3D,
      output2DPointsLeft,
      output2DPointsRight);

    boost::mt19937 rng;
    boost::normal_distribution<> nd(0.0,pixelNoise);
    boost::variate_generator<boost::mt19937& , boost::normal_distribution<> > var_nor (rng,nd);
    for ( int j = 0 ; j < xcorners*ycorners ; j ++ )
    {
      CV_MAT_ELEM (*output2DPointsLeft ,double,j,0) += var_nor();
      CV_MAT_ELEM (*output2DPointsLeft ,double,j,1) += var_nor();
      CV_MAT_ELEM (*output2DPointsRight ,double,j,0) += var_nor();
      CV_MAT_ELEM (*output2DPointsRight ,double,j,1) += var_nor();
    }

    if ( quantize )
    {
      for ( int j = 0 ; j < xcorners*ycorners ; j ++ )
      {
        CV_MAT_ELEM (*output2DPointsLeft ,double,j,0) =
            floor ( CV_MAT_ELEM (*output2DPointsLeft ,double,j,0) + 0.5 );
        CV_MAT_ELEM (*output2DPointsLeft ,double,j,1) =
            floor ( CV_MAT_ELEM (*output2DPointsLeft ,double,j,1) + 0.5 );
        CV_MAT_ELEM (*output2DPointsRight ,double,j,0) =
            floor ( CV_MAT_ELEM (*output2DPointsRight ,double,j,0) + 0.5 );
        CV_MAT_ELEM (*output2DPointsRight ,double,j,1) =
            floor ( CV_MAT_ELEM (*output2DPointsRight ,double,j,1) + 0.5 );
      }
    }

    //check if all points are on screen
    bool allOnScreen = true;
    for ( int j = 0 ; j < xcorners*ycorners ; j++ )
    {
      if (
          CV_MAT_ELEM (*output2DPointsLeft ,double,j,0) < 0 ||
          CV_MAT_ELEM (*output2DPointsLeft ,double,j,0) > screenWidth
          || CV_MAT_ELEM (*output2DPointsLeft ,double,j,1) < 0 ||
          CV_MAT_ELEM (*output2DPointsLeft ,double,j,1) > screenHeight ||
          CV_MAT_ELEM (*output2DPointsRight ,double,j,0) < 0 ||
          CV_MAT_ELEM (*output2DPointsRight ,double,j,0) > screenWidth
          || CV_MAT_ELEM (*output2DPointsRight ,double,j,1) < 0 ||
          CV_MAT_ELEM (*output2DPointsRight ,double,j,1) > screenHeight
          )
      {
        allOnScreen = false;
        j=xcorners*ycorners;
      }
    }

    if ( allOnScreen )
    {
      //add to our point sets
      allLeftImagePoints.push_back(output2DPointsLeft);
      allLeftObjectPoints.push_back(gridPoints);
      allRightImagePoints.push_back(output2DPointsRight);
      allRightObjectPoints.push_back(gridPoints);

      //and copy the matrix, (with added tracking noise?) to a subdirectory
      std::string trackerFilename = trackerDirectory + "/" + boost::lexical_cast<std::string>(allLeftImagePoints.size()) + ".txt";
      std::ofstream fs_tracker;
      fs_tracker.open(trackerFilename.c_str(), std::ios::out);
      for ( int row = 0 ; row < 4 ; row ++ )
      {
        for ( int col = 0 ; col < 4 ; col ++ )
        {
          fs_tracker << MarkerToWorld[frame].at<double>(row,col) << " " ;
        }
        fs_tracker << std::endl;
      }
      fs_tracker.close();

    }

  }

  MITK_INFO << "There are " << allLeftImagePoints.size() << " good frames";
  cv::Mat leftImagePoints (xcorners * ycorners * allLeftImagePoints.size(),2,CV_64FC1);
  cv::Mat leftObjectPoints (xcorners * ycorners * allLeftImagePoints.size(),3,CV_64FC1);
  cv::Mat rightImagePoints (xcorners * ycorners * allLeftImagePoints.size(),2,CV_64FC1);
  cv::Mat rightObjectPoints (xcorners * ycorners * allLeftImagePoints.size(),3,CV_64FC1);

  cv::Mat leftPointCounts (allLeftImagePoints.size(),1,CV_32SC1);
  cv::Mat rightPointCounts (allLeftImagePoints.size(),1,CV_32SC1);

  for ( unsigned int i = 0 ; i < allLeftImagePoints.size() ; i++ )
  {
    MITK_INFO << "Filling "  << i;
    for ( unsigned int j = 0 ; j < xcorners*ycorners ; j ++ )
    {
      leftImagePoints.at<double>(i* xcorners * ycorners + j,0) =
        allLeftImagePoints[i].at<double>(j,0);
      leftImagePoints.at<double>(i* xcorners * ycorners + j,1) =
        allLeftImagePoints[i].at<double>(j,1);
      leftObjectPoints.at<double>(i* xcorners * ycorners + j,0) =
        allLeftObjectPoints[i].at<double>(j,0);
      leftObjectPoints.at<double>(i* xcorners * ycorners + j,1) =
        allLeftObjectPoints[i].at<double>(j,1);
      leftObjectPoints.at<double>(i* xcorners * ycorners + j,2) =
        allLeftObjectPoints[i].at<double>(j,2);
      rightImagePoints.at<double>(i* xcorners * ycorners + j,0) =
        allRightImagePoints[i].at<double>(j,0);
      rightImagePoints.at<double>(i* xcorners * ycorners + j,1) =
        allRightImagePoints[i].at<double>(j,1);
      rightObjectPoints.at<double>(i* xcorners * ycorners + j,0) =
        allRightObjectPoints[i].at<double>(j,0);
      rightObjectPoints.at<double>(i* xcorners * ycorners + j,1) =
        allRightObjectPoints[i].at<double>(j,1);
      rightObjectPoints.at<double>(i* xcorners * ycorners + j,2) =
        allRightObjectPoints[i].at<double>(j,2);
    }
    leftPointCounts.at<int>(i,0) = xcorners * ycorners;
    rightPointCounts.at<int>(i,0) = xcorners * ycorners;
  }

  MITK_INFO << "Starting intrinisic calibration";
  CvMat* outputIntrinsicMatrixLeft = cvCreateMat(3,3,CV_64FC1);
  CvMat* outputDistortionCoefficientsLeft = cvCreateMat(1,4,CV_64FC1);
  CvMat* outputRotationVectorsLeft = cvCreateMat(allLeftImagePoints.size(),3,CV_64FC1);
  CvMat* outputTranslationVectorsLeft= cvCreateMat(allLeftImagePoints.size(),3,CV_64FC1);
  CvMat* outputIntrinsicMatrixRight= cvCreateMat(3,3,CV_64FC1);
  CvMat* outputDistortionCoefficientsRight= cvCreateMat(1,4,CV_64FC1);
  CvMat* outputRotationVectorsRight= cvCreateMat(allLeftImagePoints.size(),3,CV_64FC1);
  CvMat* outputTranslationVectorsRight= cvCreateMat(allLeftImagePoints.size(),3,CV_64FC1);
  CvMat* outputRightToLeftRotation = cvCreateMat(3,3,CV_64FC1);
  CvMat* outputRightToLeftTranslation = cvCreateMat(3,1,CV_64FC1);
  CvMat* outputEssentialMatrix = cvCreateMat(3,3,CV_64FC1);
  CvMat* outputFundamentalMatrix= cvCreateMat(3,3,CV_64FC1);

  mitk::CalibrateStereoCameraParameters(
    leftObjectPoints,
    leftImagePoints,
    leftPointCounts,
    imageSize,
    rightObjectPoints,
    rightImagePoints,
    rightPointCounts,
    *outputIntrinsicMatrixLeft,
    *outputDistortionCoefficientsLeft,
    *outputRotationVectorsLeft,
    *outputTranslationVectorsLeft,
    *outputIntrinsicMatrixRight,
    *outputDistortionCoefficientsRight,
    *outputRotationVectorsRight,
    *outputTranslationVectorsRight,
    *outputRightToLeftRotation,
    *outputRightToLeftTranslation,
    *outputEssentialMatrix,
    *outputFundamentalMatrix);

  //write it out
  std::string leftIntrinsic = "test.calib.left.intrinsic.txt";
  std::string rightIntrinsic = "test.calib.right.intrinsic.txt";
  std::string rightToLeft = "test.calib.r2l.txt";
  std::string extrinsic = "test.leftextrinsics.txt";

  std::ofstream fs_leftIntrinsic;
  std::ofstream fs_rightIntrinsic;
  std::ofstream fs_r2l;
  std::ofstream fs_ext;

  fs_leftIntrinsic.open(leftIntrinsic.c_str(), std::ios::out);
  fs_rightIntrinsic.open(rightIntrinsic.c_str(), std::ios::out);
  fs_r2l.open(rightToLeft.c_str(), std::ios::out);
  fs_ext.open(extrinsic.c_str(), std::ios::out);

  for ( int row = 0 ; row < 3 ; row ++ )
  {
    for ( int col = 0 ; col < 3 ; col ++ )
    {
      fs_leftIntrinsic << CV_MAT_ELEM (*outputIntrinsicMatrixLeft, double, row,col) << " ";
      fs_rightIntrinsic << CV_MAT_ELEM (*outputIntrinsicMatrixRight, double, row,col) << " ";
      fs_r2l << CV_MAT_ELEM (*outputRightToLeftRotation, double , row,col) << " ";
    }
    fs_leftIntrinsic << std::endl;
    fs_rightIntrinsic << std::endl;
    fs_r2l << std::endl;
  }
  for ( int i = 0 ; i < 4 ; i ++ )
  {
    fs_leftIntrinsic << CV_MAT_ELEM (*outputDistortionCoefficientsLeft, double , 0, i ) << " ";
    fs_rightIntrinsic << CV_MAT_ELEM (*outputDistortionCoefficientsRight, double , 0, i ) << " ";
  }
  for ( int i = 0 ; i < 3 ; i ++ )
  {
    fs_r2l << CV_MAT_ELEM (*outputRightToLeftTranslation, double , i, 0 ) << " ";
  }

  fs_leftIntrinsic.close();
  fs_rightIntrinsic.close();
  fs_r2l.close();
  for ( unsigned int view = 0 ; view < allLeftImagePoints.size() ; view ++ )
  {
    for ( int i = 0 ; i < 3 ; i ++ )
    {
      fs_ext << CV_MAT_ELEM ( *outputRotationVectorsLeft , double  , view, i) << " ";
    }
    for ( int i = 0 ; i < 3 ; i ++ )
    {
      fs_ext << CV_MAT_ELEM ( *outputTranslationVectorsLeft , double  , view, i) << " ";
    }
    fs_ext << std::endl;
  }
  fs_ext.close();

  //now do the handeye
  mitk::HandeyeCalibrate::Pointer handeyecalibrator = mitk::HandeyeCalibrate::New();
  handeyecalibrator->Calibrate(trackerDirectory,extrinsic);

  //Next need to do a bunch of tests, could start with just checking that the
  //inputs approximately equal the outputs, but then could also do some proper
  //reprojection tests.

  MITK_TEST_CONDITION ( CompareOpenCVMatrices(outputIntrinsicMatrixLeft, leftCameraIntrinsic, tolerance), "Testing left intrinsic Matrix");
  MITK_TEST_CONDITION ( CompareOpenCVMatrices(outputDistortionCoefficientsLeft, leftCameraDistortion, tolerance), "Testing left distortion Matrix");
  MITK_TEST_CONDITION ( CompareOpenCVMatrices(outputIntrinsicMatrixRight, rightCameraIntrinsic, tolerance), "Testing right intrinsic Matrix");
  MITK_TEST_CONDITION ( CompareOpenCVMatrices(outputDistortionCoefficientsRight, rightCameraDistortion, tolerance), "Testing right distortion Matrix");
  MITK_TEST_CONDITION ( CompareOpenCVMatrices(outputDistortionCoefficientsRight, rightCameraDistortion, tolerance), "Testing right distortion Matrix");
  MITK_TEST_CONDITION ( CompareOpenCVMatrices(outputRightToLeftRotation, rightToLeftRotationMatrix, tolerance), "Testing right to left rotation Matrix");
  MITK_TEST_CONDITION ( CompareOpenCVMatrices(outputRightToLeftTranslation, rightToLeftTranslationVector, tolerance), "Testing right to left translation vector");
  MITK_TEST_CONDITION ( CompareOpenCVMatrices2(handeyecalibrator->GetCameraToMarker(), leftCameraToTracker, tolerance), "Testing handeye");
  MITK_TEST_CONDITION ( CompareOpenCVMatrices2(handeyecalibrator->GetGridToWorld(), gridToWorld, tolerance), "Testing grid to world");

  MITK_TEST_END();
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_ASSERT(
        zip_with(_ * _, list(1, 2, 3, 4), list(5, 6, 7, 8, "ignored"))
        ==
        list(5, 12, 21, 32)
    );
    //! [main]
}


#include <catch2/catch_all.hpp>
#include <catch2/catch_test_macros.hpp>

#include <geogram/mesh/mesh.h>
#include <geogram/mesh/mesh_io.h>
#include <geogram/mesh/mesh_repair.h>
#include <geogram/mesh/mesh_remesh.h>
#include <geogram/mesh/mesh_tetrahedralize.h>

#include <boost/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>

#include "MeshGraph.h"
#include "GeogramVoronoi.h"

#include <geogram/basic/command_line.h>
#include <geogram/basic/command_line_args.h>
#include <geogram/basic/common.h>

#include <iostream>

using namespace GEO;
namespace bgi = boost::geometry::index;

TEST_CASE("Test slice query", "[Query]")
{
    std::string path("./test_artifact/3DBenchy.stl");

    initialize();
    CmdLine::import_arg_group("standard");
    CmdLine::import_arg_group("pre");
    CmdLine::import_arg_group("remesh");
    CmdLine::import_arg_group("algo");
    CmdLine::import_arg_group("post");
    CmdLine::import_arg_group("opt");
    CmdLine::import_arg_group("co3ne");
    CmdLine::import_arg_group("tet");
    CmdLine::import_arg_group("poly");
    Mesh m_in;
    Mesh m_smooth;
    REQUIRE(mesh_load(path, m_in));
    mesh_repair(m_in);

    MeshIOFlags attr;
    attr.set_attributes(MeshAttributesFlags::MESH_ALL_ATTRIBUTES);

    index_t nb_points = 10000;
    remesh_smooth(m_in, m_smooth, nb_points);

    REQUIRE(mesh_tetrahedralize(m_smooth));
    mesh_save(m_smooth, "./full_mesh_tet.obj", attr);
    m_smooth.cells.compute_borders();

    auto m_hex = std::make_shared<Mesh>();
    OasisLib::polyhedral_mesher(m_smooth, *m_hex, 1000, \
                "tets_voronoi_boundary", 0.001, 5, 30, true, 0.0);

    mesh_save(*m_hex, "./full_mesh.obj", attr);
    SECTION("test rtree construction")
    {
        auto t = OasisLib::MeshHeightSlicer(m_hex);
        Mesh single_layer;

        REQUIRE(t.nb_nodes > 0);
        t.get_layer(single_layer, 20);

        REQUIRE(single_layer.facets.nb() > 0);
        mesh_repair(single_layer);
        mesh_save(single_layer, "./mesh_slice.obj", attr);
    }
}


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file qle/termstructures/blackvolsurfacewithatm.hpp
    \brief Wrapper class for a BlackVolTermStructure that easily exposes ATM vols.
    \ingroup termstructures
*/

#ifndef quantext_blackvolsurfacewithatm_hpp
#define quantext_blackvolsurfacewithatm_hpp

#include <boost/shared_ptr.hpp>
#include <ql/quote.hpp>
#include <ql/termstructures/volatility/equityfx/blackvoltermstructure.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>

namespace QuantExt {
using namespace QuantLib;

//! Wrapper class for a BlackVolTermStructure that easily exposes ATM vols.
/*! This class implements BlackVolatilityTermStructure and takes a surface (well, any BlackVolTermStructure) as an
    input. If asked for a volatility with strike=Null<Real>() or 0 it will calculate the forward value and use this as
    the strike, this makes it easy to access ATMF values.

    The forward value is calculated using the input spot and yield curves, so can be used for both FX and Equity vols.

    For FX markets, one should set the spot to be the FX spot rate, yield1 to be the base discount curve and yield2 to
    be the reference discount curve (e.g. EURUSD, yield1 = EUR).

    For Equity markets, one should set the spot to be the equity price, yield1 to be the discount curve and yield2 to
    be the dividend curve.
 */
//!\ingroup termstructures

class BlackVolatilityWithATM : public BlackVolatilityTermStructure {
public:
    //! Constructor. This is a floating term structure (settlement days is zero)
    BlackVolatilityWithATM(const boost::shared_ptr<BlackVolTermStructure>& surface, const Handle<Quote>& spot,
                           const Handle<YieldTermStructure>& yield1, const Handle<YieldTermStructure>& yield2);

    //! \name TermStructure interface
    //@{
    DayCounter dayCounter() const { return surface_->dayCounter(); }
    Date maxDate() const { return surface_->maxDate(); }
    Time maxTime() const { return surface_->maxTime(); }
    const Date& referenceDate() const { return surface_->referenceDate(); }
    Calendar calendar() const { return surface_->calendar(); }
    Natural settlementDays() const { return surface_->settlementDays(); }
    //! \name VolatilityTermStructure interface
    //@{
    Rate minStrike() const { return surface_->minStrike(); }
    Rate maxStrike() const { return surface_->maxStrike(); }
    //@}

protected:
    // Here we check if strike is Null<Real>() or 0 and calculate ATMF if so.
    Volatility blackVolImpl(Time t, Real strike) const;

private:
    boost::shared_ptr<BlackVolTermStructure> surface_;
    Handle<Quote> spot_;
    Handle<YieldTermStructure> yield1_, yield2_;
};

} // namespace QuantExt

#endif


//
// Created by Xinyu Zhang on 3/26/21.
//
// gcc -I/Users/xinyuzhang/Desktop/Spring2021/c++pattern/project/cosan /Users/xinyuzhang/Desktop/Spring2021/c++pattern/project/cosan/test/preprocessor/StandardScalerTest.cpp

#include <iostream>
// #include <Eigen/Dense>
// #include <cosan/io/utils.h>
#include <cosan/preprocessing/standardScaler.h>
#include <vector>
//using namespace Eigen;
//using namespace std;

int main() {

    EigenMatrix X{3,3};
    X << 2, -1, 2,
         2, 0, 0,
         1, 1, -1;
    Cosan::StandardScaler ss1{};
    ss1.fit(X);
    std::cout << ss1.GetMean() << '\n' << std::endl;
    std::cout << ss1.GetStd() << '\n' << std::endl;
    std::cout << ss1.transform(X)<< '\n' << std::endl;
    std::cout << ss1.InvTransform(ss1.transform(X))<< '\n' << std::endl;

    Cosan::StandardScaler ss2{};
    std::cout << ss2.standardize(X) << std::endl;

    return 0;
//
//    m.transposeInPlace();
//    cout<<m<<endl;
//    m.resize(1,4);
//    cout<<n<<endl;
}


#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <gtest/gtest.h>
#include <rct_optimizations/pnp.h>
#include <rct_optimizations_tests/observation_creator.h>
#include <rct_optimizations_tests/utilities.h>

using namespace rct_optimizations;

static const unsigned TARGET_ROWS = 10;
static const unsigned TARGET_COLS = 14;
static const double SPACING = 0.025;
static const double CORRELATION_COEFFICIENT_THRESHOLD = 0.8;
static const std::size_t N_RANDOM_SAMPLES = 30;

void checkCorrelation(const Eigen::MatrixXd& cov)
{
  for (Eigen::Index row = 0; row < cov.rows(); ++row)
  {
    for (Eigen::Index col = 0; col < cov.cols(); ++col)
    {
      // Since the covariance matrix is symmetric, just check the values in the top triangle
      if(row < col)
        EXPECT_LT(std::abs(cov(row, col)), CORRELATION_COEFFICIENT_THRESHOLD);
    }
  }
}

void printMatrix(const Eigen::MatrixXd& mat, const std::string& title)
{
  Eigen::IOFormat fmt(4, 0, " | ", "\n", "|", "|");
  std::cout << title << ":\n" << mat.format(fmt) << std::endl;
}

class PnP2DTest : public ::testing::Test
{
  public:
  PnP2DTest()
    : camera(test::makeKinectCamera())
    , target(TARGET_ROWS, TARGET_COLS, SPACING)
    , target_to_camera(Eigen::Isometry3d::Identity())
  {
    double x = static_cast<double>(TARGET_ROWS - 1) * SPACING / 2.0;
    double y = static_cast<double>(TARGET_COLS - 1) * SPACING / 2.0;
    target_to_camera.translate(Eigen::Vector3d(x, y, 0.4));
    target_to_camera.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()));
  }

  test::Camera camera;
  test::Target target;
  Eigen::Isometry3d target_to_camera;
};

TEST_F(PnP2DTest, PerfectInitialConditions)
{
  PnPProblem problem;
  problem.camera_to_target_guess = target_to_camera.inverse();
  problem.intr = camera.intr;
  EXPECT_NO_THROW(problem.correspondences =
                    test::getCorrespondences(target_to_camera, Eigen::Isometry3d::Identity(), camera, target, true));

  PnPResult result = optimize(problem);
  EXPECT_TRUE(result.converged);
  EXPECT_TRUE(result.camera_to_target.isApprox(target_to_camera.inverse()));
  EXPECT_LT(result.initial_cost_per_obs, 1.0e-15);
  EXPECT_LT(result.final_cost_per_obs, 1.0e-15);

  checkCorrelation(result.covariance.correlation_matrix);
  printMatrix(result.covariance.correlation_matrix, "Correlation");

  std::cout << result.covariance.toString() << std::endl;
}

TEST_F(PnP2DTest, PerturbedInitialCondition)
{
  PnPProblem problem;
  problem.intr = camera.intr;
  problem.correspondences = test::getCorrespondences(target_to_camera,
                                                     Eigen::Isometry3d::Identity(),
                                                     camera,
                                                     target,
                                                     true);

  namespace ba = boost::accumulators;
  ba::accumulator_set<double, ba::features<ba::stats<ba::tag::mean, ba::tag::variance>>> residual_acc;
  ba::accumulator_set<double, ba::features<ba::stats<ba::tag::mean, ba::tag::variance>>> pos_acc;
  ba::accumulator_set<double, ba::features<ba::stats<ba::tag::mean, ba::tag::variance>>> ori_acc;

  for (std::size_t i = 0; i < N_RANDOM_SAMPLES; ++i)
  {
    problem.camera_to_target_guess = test::perturbPose(target_to_camera.inverse(), 0.05, 0.05);

    PnPResult result = optimize(problem);

    EXPECT_TRUE(result.converged);
    checkCorrelation(result.covariance.correlation_matrix);

    // Calculate the difference between the transforms (ideally, an identity matrix)
    Eigen::Isometry3d diff = result.camera_to_target * target_to_camera;

    // Accumulate the positional, rotational, and residual errors
    pos_acc(diff.translation().norm());
    ori_acc(Eigen::Quaterniond::Identity().angularDistance(Eigen::Quaterniond(diff.linear())));
    residual_acc(result.final_cost_per_obs);
  }

  // Expect 99% of the outputs (i.e. 3 standard deviations) to be within the corresponding threshold
  EXPECT_LT(ba::mean(pos_acc) + 3 * std::sqrt(ba::variance(pos_acc)), 1.0e-7);
  EXPECT_LT(ba::mean(ori_acc) + 3 * std::sqrt(ba::variance(ori_acc)), 1.0e-6);
  EXPECT_LT(ba::mean(residual_acc) + 3 * std::sqrt(ba::variance(residual_acc)), 1.0e-10);

}

TEST_F(PnP2DTest, BadIntrinsicParameters)
{
  PnPProblem problem;
  problem.camera_to_target_guess = target_to_camera.inverse();
  problem.correspondences = test::getCorrespondences(target_to_camera,
                                                     Eigen::Isometry3d::Identity(),
                                                     camera,
                                                     target,
                                                     true);

  // Tweak the camera intrinsics such that we are optimizing with different values (+/- 1%)
  // than were used to generate the observations
  problem.intr = camera.intr;
  problem.intr.fx() *= 1.01;
  problem.intr.fy() *= 0.99;
  problem.intr.cx() *= 1.01;
  problem.intr.cy() *= 0.99;

  PnPResult result = optimize(problem);

  // The optimization should still converge by moving the camera further away from the target,
  // but the residual error and error in the resulting transform should be much higher
  EXPECT_TRUE(result.converged);
  EXPECT_FALSE(result.camera_to_target.isApprox(target_to_camera.inverse(), 1.0e-3));
  EXPECT_GT(result.final_cost_per_obs, 1.0e-3);

  checkCorrelation(result.covariance.correlation_matrix);
  printMatrix(result.covariance.correlation_matrix, "Correlation");
  std::cout << result.covariance.toString() << std::endl;
}

class PnP3DTest : public ::testing::Test
{
  public:
  PnP3DTest()
    : target(TARGET_ROWS, TARGET_COLS, SPACING)
    , target_to_camera(Eigen::Isometry3d::Identity())
  {
    double x = static_cast<double>(TARGET_ROWS - 1) * SPACING / 2.0;
    double y = static_cast<double>(TARGET_COLS - 1) * SPACING / 2.0;
    target_to_camera.translate(Eigen::Vector3d(x, y, 0.4));
    target_to_camera.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()));
  }

  test::Target target;
  Eigen::Isometry3d target_to_camera;
};

TEST_F(PnP3DTest, PerfectInitialConditions)
{
  PnPProblem3D problem;
  problem.camera_to_target_guess = target_to_camera.inverse();
  EXPECT_NO_THROW(problem.correspondences =
                    test::getCorrespondences(target_to_camera, Eigen::Isometry3d::Identity(), target));

  PnPResult result = optimize(problem);
  EXPECT_TRUE(result.converged);
  EXPECT_TRUE(result.camera_to_target.isApprox(target_to_camera.inverse()));
  EXPECT_LT(result.initial_cost_per_obs, 1.0e-15);
  EXPECT_LT(result.final_cost_per_obs, 1.0e-15);

  checkCorrelation(result.covariance.correlation_matrix);

  // BUG: tests pass but nothing is printed
//  printMatrix(result.covariance.correlation_matrix, "Correlation");
//  std::cout << result.covariance.toString() << std::endl;
}

TEST_F(PnP3DTest, PerturbedInitialCondition)
{
  PnPProblem3D problem;
  problem.correspondences = test::getCorrespondences(target_to_camera,
                                                     Eigen::Isometry3d::Identity(),
                                                     target);

  namespace ba = boost::accumulators;
  ba::accumulator_set<double, ba::features<ba::stats<ba::tag::mean, ba::tag::variance>>> residual_acc;
  ba::accumulator_set<double, ba::features<ba::stats<ba::tag::mean, ba::tag::variance>>> pos_acc;
  ba::accumulator_set<double, ba::features<ba::stats<ba::tag::mean, ba::tag::variance>>> ori_acc;

  for (std::size_t i = 0; i < N_RANDOM_SAMPLES; ++i)
  {
    problem.camera_to_target_guess = test::perturbPose(target_to_camera.inverse(), 0.05, 0.05);

    PnPResult result = optimize(problem);
    EXPECT_TRUE(result.converged);
    checkCorrelation(result.covariance.correlation_matrix);

    // Calculate the difference between the transforms (ideally, an identity matrix)
    Eigen::Isometry3d diff = result.camera_to_target * target_to_camera;

    // Accumulate the positional, rotational, and residual errors
    pos_acc(diff.translation().norm());
    ori_acc(Eigen::Quaterniond::Identity().angularDistance(Eigen::Quaterniond(diff.linear())));
    residual_acc(result.final_cost_per_obs);
  }

  // Expect 99% of the outputs (i.e. 3 standard deviations) to be within the corresponding threshold
  EXPECT_LT(ba::mean(pos_acc) + 3 * std::sqrt(ba::variance(pos_acc)), 1.0e-7);
  EXPECT_LT(ba::mean(ori_acc) + 3 * std::sqrt(ba::variance(ori_acc)), 1.0e-6);
  EXPECT_LT(ba::mean(residual_acc) + 3 * std::sqrt(ba::variance(residual_acc)), 1.0e-10);
}

int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


/**
 * @file tests/main_tests/local_coordinate_coding_test.cpp
 * @author Bhavya Bahl
 *
 * Test mlpackMain() of local_coordinate_coding_main.cpp.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <string>

#define BINDING_TYPE BINDING_TYPE_TEST
static const std::string testName = "LocalCoordinateCoding";

#include <mlpack/core.hpp>
#include <mlpack/core/util/mlpack_main.hpp>
#include "test_helper.hpp"
#include <mlpack/methods/local_coordinate_coding/local_coordinate_coding_main.cpp>

#include <boost/test/unit_test.hpp>
#include "../test_tools.hpp"

using namespace mlpack;

struct LCCTestFixture
{
 public:
  LCCTestFixture()
  {
    // Cache in the options for this program.
    CLI::RestoreSettings(testName);
  }

  ~LCCTestFixture()
  {
    // Clear the settings.
    bindings::tests::CleanMemory();
    CLI::ClearSettings();
  }
};

BOOST_FIXTURE_TEST_SUITE(LCCMainTest, LCCTestFixture);

/**
 * Ensure that the dimensions of encoded test points
 * and output dictionary are correct.
 */
BOOST_AUTO_TEST_CASE(LCCDimensionsTest)
{
  arma::mat x;
  x.load("mnist_first250_training_4s_and_9s.arm");
  int rows = x.n_rows, cols = x.n_cols;
  arma::mat t = x;
  int atoms = 10;

  SetInputParam("training", std::move(x));
  SetInputParam("test", std::move(t));
  SetInputParam("atoms", atoms);
  SetInputParam("max_iterations", (int) 2);

  mlpackMain();

  // Check that the output has correct dimensions.
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("codes").n_rows, atoms);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("codes").n_cols, cols);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("dictionary").n_rows, rows);
  BOOST_REQUIRE_EQUAL(CLI::GetParam<arma::mat>("dictionary").n_cols, atoms);
}

/**
 * Ensure that trained model can be reused.
 */
BOOST_AUTO_TEST_CASE(LCCOutputModelTest)
{
  arma::mat x;
  x.load("mnist_first250_training_4s_and_9s.arm");
  arma::mat t = x;

  SetInputParam("training", std::move(x));
  SetInputParam("test", t);
  SetInputParam("atoms", (int) 10);
  SetInputParam("max_iterations", (int) 2);

  mlpackMain();

  // Get the encoded output and dictionary after training.
  arma::mat initCodes = std::move(CLI::GetParam<arma::mat>("codes"));
  arma::mat initDict = std::move(CLI::GetParam<arma::mat>("dictionary"));
  LocalCoordinateCoding* outputModel =
      std::move(CLI::GetParam<LocalCoordinateCoding*>("output_model"));

  CLI::Parameters()["training"].wasPassed = false;

  SetInputParam("input_model", std::move(outputModel));
  SetInputParam("test", std::move(t));

  mlpackMain();

  // Compare the output after reusing the trained model
  // to the original matrices.
  CheckMatrices(initCodes, CLI::GetParam<arma::mat>("codes"));
  CheckMatrices(initDict, CLI::GetParam<arma::mat>("dictionary"));
}

/**
 * Ensure that the number of rows in initial dictionary is same as
 * the dimension of the points.
 */
BOOST_AUTO_TEST_CASE(LCCInitDictTrainTest)
{
  arma::mat x = {{1, 1, 1, 1}, {2, 2, 2, 2}, {3, 3, 3, 3}, {4, 4, 4, 4}};
  arma::mat initDict = {{1, 1}, {2, 2}, {3, 3}};

  SetInputParam("training", std::move(x));
  SetInputParam("initial_dictionary", std::move(initDict));
  SetInputParam("atoms", (int) 2);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Ensure that the number of columns in initial dictionary is same as
 * the number of atoms.
 */
BOOST_AUTO_TEST_CASE(LCCInitDictAtomTest)
{
  arma::mat x = {{1, 1, 1, 1}, {2, 2, 2, 2}, {3, 3, 3, 3}, {4, 4, 4, 4}};
  arma::mat initDict = {{1, 1}, {2, 2}, {3, 3}, {4, 4}};

  SetInputParam("training", std::move(x));
  SetInputParam("initial_dictionary", std::move(initDict));
  SetInputParam("atoms", (int) 3);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Ensure that training data and test data points
 * have same dimensionality.
 */
BOOST_AUTO_TEST_CASE(LCCTrainAndTestDataDimTest)
{
  arma::mat x;
  x.load("mnist_first250_training_4s_and_9s.arm");
  arma::mat t = x;

  t.shed_rows(1, 2);

  // Input data.
  SetInputParam("training", x);
  SetInputParam("atoms", (int) 10);
  SetInputParam("max_iterations", (int) 2);
  SetInputParam("test", std::move(t));

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Ensure that only one out of training and input_model are specified.
 */
BOOST_AUTO_TEST_CASE(LCCTrainAndInputModelTest)
{
  arma::mat x;
  x.load("mnist_first250_training_4s_and_9s.arm");

  SetInputParam("training", x);
  SetInputParam("atoms", (int) 10);
  SetInputParam("max_iterations", (int) 2);

  mlpackMain();

  LocalCoordinateCoding* outputModel =
      std::move(CLI::GetParam<LocalCoordinateCoding*>("output_model"));

  // No need to input training data again.
  SetInputParam("input_model", std::move(outputModel));

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Ensure that dimensionality of the trained model matches
 * the dimensionality of the test points.
 */
BOOST_AUTO_TEST_CASE(LCCTrainedModelDimTest)
{
  arma::mat x;
  x.load("mnist_first250_training_4s_and_9s.arm");
  arma:: mat t = x;
  t.shed_rows(1, 2);

  SetInputParam("training", x);
  SetInputParam("atoms", (int) 10);
  SetInputParam("max_iterations", (int) 2);

  mlpackMain();

  LocalCoordinateCoding* outputModel =
      std::move(CLI::GetParam<LocalCoordinateCoding*>("output_model"));

  SetInputParam("input_model", std::move(outputModel));
  SetInputParam("test", std::move(t));

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/*
* Ensure that the number of atoms is positive and
* less than the number of training points.
*/
BOOST_AUTO_TEST_CASE(LCCAtomsBoundTest)
{
  arma::mat x = {{1, 1, 1, 1}, {2, 2, 2, 2}, {3, 3, 3, 3}, {4, 4, 4, 4}};
  SetInputParam("training", std::move(x));
  SetInputParam("atoms", (int) 5);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);

  SetInputParam("atoms", (int) -1);
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/*
* Ensure that the program throws error for negative regularization parameter.
*/
BOOST_AUTO_TEST_CASE(LCCNegativeLambdaTest)
{
  arma::mat x = {{1, 1, 1, 1}, {2, 2, 2, 2}, {3, 3, 3, 3}, {4, 4, 4, 4}};
  SetInputParam("training", std::move(x));
  SetInputParam("atoms", (int) 2);
  SetInputParam("lambda", -1.0);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/*
* Ensure that the program throws error for negative tolerance.
*/
BOOST_AUTO_TEST_CASE(LCCNegativeToleranceTest)
{
  arma::mat x = {{1, 1, 1, 1}, {2, 2, 2, 2}, {3, 3, 3, 3}, {4, 4, 4, 4}};
  SetInputParam("training", std::move(x));
  SetInputParam("atoms", (int) 2);
  SetInputParam("tolerance", -1.0);

  Log::Fatal.ignoreInput = true;
  BOOST_REQUIRE_THROW(mlpackMain(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/*
* Ensure that the normalize parameter works.
*/
BOOST_AUTO_TEST_CASE(LCCNormalizationTest)
{
  // Minimum required difference between the encodings of the test data.
  double delta = 1.0;

  arma::mat x = {{1, 2, 3, 4}, {2, 2, 3, 1}, {3, 2, 3, 0}, {1, 1, 4, 4}};
  arma::mat t = x;
  arma::mat initDict = {{1, 2}, {2, 3}, {3, 4}, {4, 5}};

  SetInputParam("training", x);
  SetInputParam("atoms", (int) 2);
  SetInputParam("initial_dictionary", initDict);
  SetInputParam("max_iterations", 2);
  SetInputParam("test", t);

  mlpackMain();

  arma::mat codes = std::move(CLI::GetParam<arma::mat>("codes"));

  bindings::tests::CleanMemory();

  SetInputParam("training", std::move(x));
  SetInputParam("atoms", (int) 2);
  SetInputParam("initial_dictionary", std::move(initDict));
  SetInputParam("max_iterations", (int) 2);
  SetInputParam("test", std::move(t));
  SetInputParam("normalize", (bool) 1);

  mlpackMain();

  double normDiff =
      arma::norm(CLI::GetParam<arma::mat>("codes") - codes, "fro");

  BOOST_REQUIRE_GT(normDiff, delta);
}

/*
* Ensure that changing max iterations changes the output.
*/
BOOST_AUTO_TEST_CASE(LCCMaxIterTest)
{
  // Minimum required difference between the encodings of the test data.
  double delta = 1.0;

  arma::mat x = {{1, 2, 3, 4}, {2, 2, 3, 1}, {3, 2, 3, 0}, {1, 1, 4, 4}};
  arma::mat t = x;
  arma::mat initDict = {{1, 2}, {2, 3}, {3, 4}, {4, 5}};

  SetInputParam("training", x);
  SetInputParam("atoms", (int) 2);
  SetInputParam("initial_dictionary", initDict);
  SetInputParam("max_iterations", 2);
  SetInputParam("test", t);

  mlpackMain();
  arma::mat codes = std::move(CLI::GetParam<arma::mat>("codes"));

  bindings::tests::CleanMemory();

  SetInputParam("training", std::move(x));
  SetInputParam("atoms", (int) 2);
  SetInputParam("initial_dictionary", std::move(initDict));
  SetInputParam("max_iterations", (int) 4);
  SetInputParam("test", std::move(t));

  mlpackMain();

  double normDiff =
      arma::norm(CLI::GetParam<arma::mat>("codes") - codes, "fro");

  BOOST_REQUIRE_GT(normDiff, delta);
}

/*
* Ensure that changing tolerance changes the output.
*/
BOOST_AUTO_TEST_CASE(LCCToleranceTest)
{
  // Minimum required difference between the encodings of the test data.
  double delta = 0.05;

  arma::mat x = {{1, 2, 3, 4}, {2, 2, 3, 1}, {3, 2, 3, 0}, {1, 1, 4, 4}};
  arma::mat t = x;
  arma::mat initDict = {{1, 2}, {2, 3}, {3, 4}, {4, 5}};

  SetInputParam("training", x);
  SetInputParam("atoms", (int) 2);
  SetInputParam("initial_dictionary", initDict);
  SetInputParam("test", t);
  SetInputParam("tolerance", (double) 0.01);

  mlpackMain();
  arma::mat codes = std::move(CLI::GetParam<arma::mat>("codes"));

  bindings::tests::CleanMemory();

  SetInputParam("training", std::move(x));
  SetInputParam("atoms", (int) 2);
  SetInputParam("initial_dictionary", std::move(initDict));
  SetInputParam("test", std::move(t));
  SetInputParam("tolerance", (double) 100.0);

  mlpackMain();

  double normDiff =
      arma::norm(CLI::GetParam<arma::mat>("codes") - codes, "fro");

  BOOST_REQUIRE_GT(normDiff, delta);
}

/*
* Ensure that changing regularization parameter changes the output.
*/
BOOST_AUTO_TEST_CASE(LCCLambdaTest)
{
  // Minimum required difference between the encodings of the test data.
  double delta = 1.0;

  arma::mat x = {{1, 2, 3, 4}, {2, 2, 3, 1}, {3, 2, 3, 0}, {1, 1, 4, 4}};
  arma::mat t = x;
  arma::mat initDict = {{1, 2}, {2, 3}, {3, 4}, {4, 5}};

  SetInputParam("training", x);
  SetInputParam("atoms", (int) 2);
  SetInputParam("initial_dictionary", initDict);
  SetInputParam("test", t);
  SetInputParam("lambda", (double) 0.0);

  mlpackMain();
  arma::mat codes = std::move(CLI::GetParam<arma::mat>("codes"));

  bindings::tests::CleanMemory();

  SetInputParam("training", std::move(x));
  SetInputParam("atoms", (int) 2);
  SetInputParam("initial_dictionary", std::move(initDict));
  SetInputParam("test", std::move(t));
  SetInputParam("lambda", (double) 1.0);

  mlpackMain();

  double normDiff =
      arma::norm(CLI::GetParam<arma::mat>("codes") - codes, "fro");

  BOOST_REQUIRE_GT(normDiff, delta);
}

BOOST_AUTO_TEST_SUITE_END();


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "structures.h"
#include "transformations.h"
#include "relative_pose_tait_bryan_wc_jacobian.h"

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -10.0;
float translate_x, translate_y = 0.0;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

bool is_loop_closure_added = false;

std::vector<Eigen::Affine3d> m_poses;
std::vector<Eigen::Affine3d> m_poses_desired;
std::vector<std::pair<int, int>> odo_edges;

void calculate_cov(std::vector<Eigen::Affine3d> m_poses,
				   std::vector<Eigen::Affine3d> m_poses_desired,
				   std::vector<std::pair<int, int>> odo_edges,
				   Eigen::MatrixXd & cov_b);

void draw_ellipse(const Eigen::Matrix3d& covar, Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd  = 3)
{
    Eigen::LLT<Eigen::Matrix<double,3,3> > cholSolver(covar);
    Eigen::Matrix3d transform = cholSolver.matrixL();

    const double pi = 3.141592;
    const double di = 0.02;
    const double dj = 0.04;
    const double du = di*2*pi;
    const double dv = dj*pi;
    glColor3f(color.x(), color.y(),color.z());

    for (double i = 0; i < 1.0; i+=di)  //horizonal
    {
        for (double j = 0; j < 1.0; j+=dj)  //vertical
        {
            double u = i*2*pi;      //0     to  2pi
            double v = (j-0.5)*pi;  //-pi/2 to pi/2

            const Eigen::Vector3d pp0( cos(v)* cos(u),cos(v) * sin(u),sin(v));
            const Eigen::Vector3d pp1(cos(v) * cos(u + du) ,cos(v) * sin(u + du) ,sin(v));
            const Eigen::Vector3d pp2(cos(v + dv)* cos(u + du) ,cos(v + dv)* sin(u + du) ,sin(v + dv));
            const Eigen::Vector3d pp3( cos(v + dv)* cos(u),cos(v + dv)* sin(u),sin(v + dv));
            Eigen::Vector3d tp0 = transform * (nstd*pp0) + mean;
            Eigen::Vector3d tp1 = transform * (nstd*pp1) + mean;
            Eigen::Vector3d tp2 = transform * (nstd*pp2) + mean;
            Eigen::Vector3d tp3 = transform * (nstd*pp3) + mean;

            glBegin(GL_LINE_LOOP);
            glVertex3dv(tp0.data());
            glVertex3dv(tp1.data());
            glVertex3dv(tp2.data());
            glVertex3dv(tp3.data());
            glEnd();
        }
    }
}

void draw_ellipse2D(const Eigen::Matrix3d& covar, Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd  = 3)
{
    Eigen::LLT<Eigen::Matrix<double,3,3> > cholSolver(covar);
    Eigen::Matrix3d transform = cholSolver.matrixL();

    const double pi = 3.141592;
    const double di = 0.02;
    const double dj = 0.04;
    const double du = di*2*pi;
    const double dv = dj*pi;
    glColor3f(color.x(), color.y(),color.z());

    for (double i = 0; i < 1.0; i+=di) { //horizonal
		double u = i*2*pi;      //0     to  2pi
		const Eigen::Vector3d pp0( cos(u), sin (u),0);
		const Eigen::Vector3d pp1( cos(u+du), sin(u+du),0);
		Eigen::Vector3d tp0 = transform * (nstd*pp0) + mean;
		Eigen::Vector3d tp1 = transform * (nstd*pp1) + mean;
		glBegin(GL_LINE_LOOP);
		glVertex3dv(tp0.data());
		glVertex3dv(tp1.data());
		glEnd();
	}
}

int main(int argc, char *argv[]){
	TaitBryanPose p0;
	p0.px = 0;
	p0.py = 0;
	p0.pz = 0;
	p0.om = 0;
	p0.fi = 0;
	p0.ka = 0;
	Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(p0);
	m_poses.push_back(m);

	TaitBryanPose p_rel_forward_x;
	p_rel_forward_x.px = 1.0;
	p_rel_forward_x.py = 0.0;
	p_rel_forward_x.pz = 0.0;
	p_rel_forward_x.om = 0.0;
	p_rel_forward_x.fi = 0.0;
	p_rel_forward_x.ka = 0.0;

	TaitBryanPose p_rel_rotate;
	p_rel_rotate.px = 0.0;
	p_rel_rotate.py = 0.0;
	p_rel_rotate.pz = 0.0;
	p_rel_rotate.om = 0.0;
	p_rel_rotate.fi = 0.0;
	p_rel_rotate.ka = 90 * M_PI/180.0;

	Eigen::Affine3d m_rel;

	for(int c = 0; c < 4; c++){
		for(size_t i = 0 ; i < 3; i++){
			m_rel = affine_matrix_from_pose_tait_bryan(p_rel_forward_x);
			m = m * m_rel;
			m_poses.push_back(m);
		}
		m_rel = affine_matrix_from_pose_tait_bryan(p_rel_rotate);
		m = m * m_rel;
	}
	m_poses.pop_back();
	m_poses_desired = m_poses;
	m_poses.clear();
	p0.px = 0;
	p0.py = 0;
	p0.pz = 0;
	p0.om = 0;
	p0.fi = 0;
	p0.ka = 30 * M_PI/180;
	m = affine_matrix_from_pose_tait_bryan(p0);
	m_poses.push_back(m);

	p_rel_forward_x.px = 1.1;
	p_rel_forward_x.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
	p_rel_forward_x.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_forward_x.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_forward_x.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_forward_x.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;

	p_rel_rotate.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_rotate.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_rotate.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_rotate.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_rotate.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_rotate.ka = 80 * M_PI/180.0;

	for(int c = 0; c < 4; c++){
		for(size_t i = 0 ; i < 3; i++){
			m_rel = affine_matrix_from_pose_tait_bryan(p_rel_forward_x);
			m = m * m_rel;
			m_poses.push_back(m);
			p_rel_forward_x.px += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
			p_rel_forward_x.py += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
			p_rel_forward_x.pz += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			p_rel_forward_x.om += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			p_rel_forward_x.fi += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			p_rel_forward_x.ka += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
		}
		m_rel = affine_matrix_from_pose_tait_bryan(p_rel_rotate);
		m = m * m_rel;
		p_rel_rotate.px += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
		p_rel_rotate.py += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
		p_rel_rotate.pz += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
		p_rel_rotate.om += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
		p_rel_rotate.fi += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
		p_rel_rotate.ka += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	}
	m_poses.pop_back();
	for(size_t i = 1; i < m_poses.size(); i++){
		odo_edges.emplace_back(i-1,i);
	}
	if (false == initGL(&argc, argv)) {
		return 4;
	}
	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();
	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("relative_pose_covariances");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);
	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	/*glBegin(GL_LINES);
	glColor3f(1.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(1.0f, 0.0f, 0.0f);

	glColor3f(0.0f, 1.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 1.0f, 0.0f);

	glColor3f(0.0f, 0.0f, 1.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 1.0f);
	glEnd();*/

	glColor3f(1,0,0);
	glBegin(GL_LINES);
	for(size_t i = 0; i < odo_edges.size(); i++){
		glVertex3f(m_poses[odo_edges[i].first](0,3), m_poses[odo_edges[i].first](1,3), m_poses[odo_edges[i].first](2,3) );
		glVertex3f(m_poses[odo_edges[i].second](0,3), m_poses[odo_edges[i].second](1,3), m_poses[odo_edges[i].second](2,3) );
	}
	glEnd();

	Eigen::MatrixXd cov_b(m_poses.size() * 2, m_poses.size() * 2);
	cov_b = Eigen::MatrixXd::Zero(m_poses.size() * 2, m_poses.size() * 2);
	calculate_cov( m_poses,	m_poses_desired, odo_edges, cov_b);

	for(size_t i = 0; i < m_poses.size(); i++){
		Eigen::Vector3d mean(m_poses[i](0,3), m_poses[i](1,3), m_poses[i](2,3));
		Eigen::Matrix3d cov;
		int r = i * 6;
		int c = i * 6;
		cov(0,0) = cov_b(r+0,c+0);
		cov(0,1) = cov_b(r+0,c+1);
		cov(0,2) = cov_b(r+0,c+2);
		cov(1,0) = cov_b(r+1,c+0);
		cov(1,1) = cov_b(r+1,c+1);
		cov(1,2) = cov_b(r+1,c+2);
		cov(2,0) = cov_b(r+2,c+0);
		cov(2,1) = cov_b(r+2,c+1);
		cov(2,2) = cov_b(r+2,c+2);
		draw_ellipse2D(cov, mean, Eigen::Vector3f(1,0,0),1);
		draw_ellipse2D(cov, mean, Eigen::Vector3f(0,1,0),2);
		draw_ellipse2D(cov, mean, Eigen::Vector3f(0,0,1),3);
	}
	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'e':{
			odo_edges.emplace_back(1, m_poses.size()-2);
			is_loop_closure_added = true;
			break;
		}
		case 'n':{
			for(size_t i = 0 ; i < m_poses.size(); i++){
				TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_poses[i]);
				pose.px += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.py += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.pz += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.001;
				pose.om += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.0001;
				pose.fi += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.0001;
				pose.ka += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.0001;
				m_poses[i] = affine_matrix_from_pose_tait_bryan(pose);
			}
			break;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			std::vector<TaitBryanPose> poses;
			std::vector<TaitBryanPose> poses_desired;

			for(size_t i = 0 ; i < m_poses.size(); i++){
				poses.push_back(pose_tait_bryan_from_affine_matrix(m_poses[i]));
			}
			for(size_t i = 0 ; i < m_poses_desired.size(); i++){
				poses_desired.push_back(pose_tait_bryan_from_affine_matrix(m_poses_desired[i]));
			}

			for(size_t i = 0 ; i < odo_edges.size(); i++){
				Eigen::Matrix<double, 6, 1> relative_pose_measurement_odo;
				relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo,
						poses_desired[odo_edges[i].first].px,
						poses_desired[odo_edges[i].first].py,
						poses_desired[odo_edges[i].first].pz,
						poses_desired[odo_edges[i].first].om,
						poses_desired[odo_edges[i].first].fi,
						poses_desired[odo_edges[i].first].ka,
						poses_desired[odo_edges[i].second].px,
						poses_desired[odo_edges[i].second].py,
						poses_desired[odo_edges[i].second].pz,
						poses_desired[odo_edges[i].second].om,
						poses_desired[odo_edges[i].second].fi,
						poses_desired[odo_edges[i].second].ka);

				Eigen::Matrix<double, 6, 1> delta;
				relative_pose_obs_eq_tait_bryan_wc_case1(
						delta,
						poses[odo_edges[i].first].px,
						poses[odo_edges[i].first].py,
						poses[odo_edges[i].first].pz,
						poses[odo_edges[i].first].om,
						poses[odo_edges[i].first].fi,
						poses[odo_edges[i].first].ka,
						poses[odo_edges[i].second].px,
						poses[odo_edges[i].second].py,
						poses[odo_edges[i].second].pz,
						poses[odo_edges[i].second].om,
						poses[odo_edges[i].second].fi,
						poses[odo_edges[i].second].ka,
						relative_pose_measurement_odo(0,0),
						relative_pose_measurement_odo(1,0),
						relative_pose_measurement_odo(2,0),
						relative_pose_measurement_odo(3,0),
						relative_pose_measurement_odo(4,0),
						relative_pose_measurement_odo(5,0));

				Eigen::Matrix<double, 6, 12, Eigen::RowMajor> jacobian;
				relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian,
						poses[odo_edges[i].first].px,
						poses[odo_edges[i].first].py,
						poses[odo_edges[i].first].pz,
						poses[odo_edges[i].first].om,
						poses[odo_edges[i].first].fi,
						poses[odo_edges[i].first].ka,
						poses[odo_edges[i].second].px,
						poses[odo_edges[i].second].py,
						poses[odo_edges[i].second].pz,
						poses[odo_edges[i].second].om,
						poses[odo_edges[i].second].fi,
						poses[odo_edges[i].second].ka);

				int ir = tripletListB.size();

				int ic_1 = odo_edges[i].first * 6;
				int ic_2 = odo_edges[i].second * 6;

				for(size_t row = 0 ; row < 6; row ++){
					tripletListA.emplace_back(ir + row, ic_1    , -jacobian(row,0));
					tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row,1));
					tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row,2));
					tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row,3));
					tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row,4));
					tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row,5));

					tripletListA.emplace_back(ir + row, ic_2    , -jacobian(row,6));
					tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row,7));
					tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row,8));
					tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row,9));
					tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row,10));
					tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row,11));
				}

				tripletListB.emplace_back(ir,     0, delta(0,0));
				tripletListB.emplace_back(ir + 1, 0, delta(1,0));
				tripletListB.emplace_back(ir + 2, 0, delta(2,0));

				float angle_diff = delta(3,0);
				if(fabs(angle_diff) > M_PI){
					angle_diff -= 2.0*M_PI;
				}
				if(fabs(angle_diff)< -M_PI){
					angle_diff += 2.0*M_PI;
				}
				tripletListB.emplace_back(ir + 3, 0, angle_diff);

				angle_diff = delta(4,0);
				if(fabs(angle_diff) > M_PI){
					angle_diff -= 2.0*M_PI;
				}
				if(fabs(angle_diff)< -M_PI){
					angle_diff += 2.0*M_PI;
				}
				tripletListB.emplace_back(ir + 4, 0, angle_diff);

				angle_diff = delta(5,0);
				if(fabs(angle_diff) > M_PI){
					angle_diff -= 2.0*M_PI;
				}
				if(fabs(angle_diff)< -M_PI){
					angle_diff += 2.0*M_PI;
				}
				tripletListB.emplace_back(ir + 5, 0, angle_diff);

				tripletListP.emplace_back(ir ,    ir,     1);
				tripletListP.emplace_back(ir + 1, ir + 1, 1);
				tripletListP.emplace_back(ir + 2, ir + 2, 1);
				tripletListP.emplace_back(ir + 3, ir + 3, 1);
				tripletListP.emplace_back(ir + 4, ir + 4, 1);
				tripletListP.emplace_back(ir + 5, ir + 5, 1);
			}

			int ir = tripletListB.size();
			tripletListA.emplace_back(ir     , 0, 1);
			tripletListA.emplace_back(ir + 1 , 1, 1);
			tripletListA.emplace_back(ir + 2 , 2, 1);
			tripletListA.emplace_back(ir + 3 , 3, 1);
			tripletListA.emplace_back(ir + 4 , 4, 1);
			tripletListA.emplace_back(ir + 5 , 5, 1);

			tripletListP.emplace_back(ir     , ir,     1);
			tripletListP.emplace_back(ir + 1 , ir + 1, 1);
			tripletListP.emplace_back(ir + 2 , ir + 2, 1);
			tripletListP.emplace_back(ir + 3 , ir + 3, 1);
			tripletListP.emplace_back(ir + 4 , ir + 4, 1);
			tripletListP.emplace_back(ir + 5 , ir + 5, 1);

			tripletListB.emplace_back(ir     , 0, 0);
			tripletListB.emplace_back(ir + 1 , 0, 0);
			tripletListB.emplace_back(ir + 2 , 0, 0);
			tripletListB.emplace_back(ir + 3 , 0, 0);
			tripletListB.emplace_back(ir + 4 , 0, 0);
			tripletListB.emplace_back(ir + 5 , 0, 0);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), m_poses.size() * 6);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(m_poses.size() * 6 , m_poses.size() * 6);
			Eigen::SparseMatrix<double> AtPB(m_poses.size() * 6 , 1);

			{
				Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
				AtPA = (AtP) * matA;
				AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}
			std::cout << "h_x.size(): " << h_x.size() << std::endl;
			std::cout << "AtPA=AtPB SOLVED" << std::endl;

			for(size_t i = 0 ; i < h_x.size(); i++){
				std::cout << h_x[i] << std::endl;
			}

			if(h_x.size() == 6 * m_poses.size()){
				int counter = 0;

				for(size_t i = 0; i < m_poses.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_poses[i]);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.om += h_x[counter++];
					pose.fi += h_x[counter++];
					pose.ka += h_x[counter++];
					m_poses[i] = affine_matrix_from_pose_tait_bryan(pose);
				}
				std::cout << "optimizing with tait bryan finished" << std::endl;
			}else{
				std::cout << "optimizing with tait bryan FAILED" << std::endl;
			}
			break;
		}
	}
	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}
	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "n: add noise to poses" << std::endl;
	std::cout << "t: optimize (Tait-Bryan)" << std::endl;
	std::cout << "e: add edge (loop closure)" << std::endl;
}

void calculate_cov(std::vector<Eigen::Affine3d> m_poses,
				   std::vector<Eigen::Affine3d> m_poses_desired,
				   std::vector<std::pair<int, int>> odo_edges,
				   Eigen::MatrixXd & cov_b)
{
	Eigen::MatrixXd d2sum_dbeta2(m_poses.size() * 6, m_poses.size() * 6);
	d2sum_dbeta2 = Eigen::MatrixXd::Zero(m_poses.size() * 6, m_poses.size() * 6);

	std::vector<TaitBryanPose> poses;
	std::vector<TaitBryanPose> poses_desired;

	for(size_t i = 0 ; i < m_poses.size(); i++){
		poses.push_back(pose_tait_bryan_from_affine_matrix(m_poses[i]));
	}
	for(size_t i = 0 ; i < m_poses_desired.size(); i++){
		poses_desired.push_back(pose_tait_bryan_from_affine_matrix(m_poses_desired[i]));
	}

	for (size_t i = 0; i < odo_edges.size(); i++ ){
		Eigen::Matrix<double, 6, 1> relative_pose_measurement_odo;
		relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo,
			poses_desired[odo_edges[i].first].px,
			poses_desired[odo_edges[i].first].py,
			poses_desired[odo_edges[i].first].pz,
			poses_desired[odo_edges[i].first].om,
			poses_desired[odo_edges[i].first].fi,
			poses_desired[odo_edges[i].first].ka,
			poses_desired[odo_edges[i].second].px,
			poses_desired[odo_edges[i].second].py,
			poses_desired[odo_edges[i].second].pz,
			poses_desired[odo_edges[i].second].om,
			poses_desired[odo_edges[i].second].fi,
			poses_desired[odo_edges[i].second].ka);

		Eigen::Matrix<double, 12, 12, Eigen::RowMajor> d2sum_dbeta2i;
		relative_pose_obs_eq_tait_bryan_wc_case1_d2sum_dbeta2(
				d2sum_dbeta2i,
				poses[odo_edges[i].first].px,
				poses[odo_edges[i].first].py,
				poses[odo_edges[i].first].pz,
				poses[odo_edges[i].first].om,
				poses[odo_edges[i].first].fi,
				poses[odo_edges[i].first].ka,
				poses[odo_edges[i].second].px,
				poses[odo_edges[i].second].py,
				poses[odo_edges[i].second].pz,
				poses[odo_edges[i].second].om,
				poses[odo_edges[i].second].fi,
				poses[odo_edges[i].second].ka,
				relative_pose_measurement_odo(0,0),
				relative_pose_measurement_odo(1,0),
				relative_pose_measurement_odo(2,0),
				relative_pose_measurement_odo(3,0),
				relative_pose_measurement_odo(4,0),
				relative_pose_measurement_odo(5,0));

		int raw = odo_edges[i].first * 6;
		int cal = odo_edges[i].first * 6;
		for(size_t r = 0; r < 6; r++){
			for(size_t c = 0; c < 6; c++){
				d2sum_dbeta2(raw + r, cal + c) =
						d2sum_dbeta2(raw + r, cal + c) + d2sum_dbeta2i(r,c);
			}
		}
		raw = odo_edges[i].second * 6;
		cal = odo_edges[i].second * 6;
		for(size_t r = 0; r < 6; r++){
			for(size_t c = 0; c < 6; c++){
				d2sum_dbeta2(raw + r, cal + c) =
						d2sum_dbeta2(raw + r, cal + c) + d2sum_dbeta2i(r+6,c+6);
			}
		}

		raw = odo_edges[i].first * 6;
		cal = odo_edges[i].second * 6;
		for(size_t r = 0; r < 6; r++){
			for(size_t c = 0; c < 6; c++){
				d2sum_dbeta2(raw + r, cal + c) =
						d2sum_dbeta2(raw + r, cal + c) + d2sum_dbeta2i(r,c+6);
			}
		}
		raw = odo_edges[i].second * 6;
		cal = odo_edges[i].first * 6;
		for(size_t r = 0; r < 6; r++){
			for(size_t c = 0; c < 6; c++){
				d2sum_dbeta2(raw + r, cal + c) =
						d2sum_dbeta2(raw + r, cal + c) + d2sum_dbeta2i(r + 6,c);
			}
		}
	}

	for (size_t i = 0; i < 6; i++ ){
		d2sum_dbeta2(i,i) += 1000000;
	}

	Eigen::MatrixXd d2sum_dbetadx(m_poses.size() * 12, 6 * odo_edges.size());
	d2sum_dbetadx = Eigen::MatrixXd::Zero(m_poses.size() * 12, 6 * odo_edges.size());

	for (int i = 0; i < odo_edges.size() ; i++)
	{
		Eigen::Matrix<double, 6, 1> relative_pose_measurement_odo;
		relative_pose_tait_bryan_wc_case1(
			relative_pose_measurement_odo,
			poses_desired[odo_edges[i].first].px,
			poses_desired[odo_edges[i].first].py,
			poses_desired[odo_edges[i].first].pz,
			poses_desired[odo_edges[i].first].om,
			poses_desired[odo_edges[i].first].fi,
			poses_desired[odo_edges[i].first].ka,
			poses_desired[odo_edges[i].second].px,
			poses_desired[odo_edges[i].second].py,
			poses_desired[odo_edges[i].second].pz,
			poses_desired[odo_edges[i].second].om,
			poses_desired[odo_edges[i].second].fi,
			poses_desired[odo_edges[i].second].ka);
		Eigen::Matrix<double, 12, 6, Eigen::RowMajor> d2sum_dbetadx_temp;
		relative_pose_obs_eq_tait_bryan_wc_case1_d2sum_dbetadx(
			d2sum_dbetadx_temp,
			poses[odo_edges[i].first].px,
			poses[odo_edges[i].first].py,
			poses[odo_edges[i].first].pz,
			poses[odo_edges[i].first].om,
			poses[odo_edges[i].first].fi,
			poses[odo_edges[i].first].ka,
			poses[odo_edges[i].second].px,
			poses[odo_edges[i].second].py,
			poses[odo_edges[i].second].pz,
			poses[odo_edges[i].second].om,
			poses[odo_edges[i].second].fi,
			poses[odo_edges[i].second].ka,
			relative_pose_measurement_odo(0,0),
			relative_pose_measurement_odo(1,0),
			relative_pose_measurement_odo(2,0),
			relative_pose_measurement_odo(3,0),
			relative_pose_measurement_odo(4,0),
			relative_pose_measurement_odo(5,0));

		int raw = odo_edges[i].first * 6;
		int cal = i * 6;
		for(size_t r = 0; r < 6; r++){
			for(size_t c = 0; c < 6; c++){
				d2sum_dbetadx(raw + r, cal + c) = d2sum_dbetadx_temp(r,c);
			}
		}
		raw = odo_edges[i].second * 6;
		for(size_t r = 0; r < 6; r++){
			for(size_t c = 0; c < 6; c++){
				d2sum_dbetadx(raw + r, cal + c) = d2sum_dbetadx_temp(r + 6,c);
			}
		}
	}

	Eigen::MatrixXd cov_x(6 * odo_edges.size(), 6 * odo_edges.size());
	cov_x = Eigen::MatrixXd::Zero(6 * odo_edges.size(), 6 * odo_edges.size());

	for(int i = 0 ; i < 6 * odo_edges.size(); i+=6){
		cov_x(i,i)     = 0.005 * 0.005;
		cov_x(i+1,i+1) = 0.05 * 0.05;
		cov_x(i+2,i+2) = 0.000000001 * 0.000000001;
		cov_x(i+3,i+3) = 0.000000001 * 0.000000001;
		cov_x(i+4,i+4) = 0.000000001 * 0.000000001;
		cov_x(i+5,i+5) = 0.01 * 0.01;
	}
	if(is_loop_closure_added){
		int i = 6 * (odo_edges.size()-1);
		cov_x(i,i)     = 0.01*0.01;
		cov_x(i+1,i+1) = 0.01*0.01;
		cov_x(i+2,i+2) = 0.01*0.01;
		cov_x(i+3,i+3) = 0.01 * 0.01;
		cov_x(i+4,i+4) = 0.01 * 0.01;
		cov_x(i+5,i+5) = 0.01 * 0.01;
	}

	cov_b = d2sum_dbeta2.inverse() * d2sum_dbetadx * cov_x * d2sum_dbetadx.transpose() * d2sum_dbeta2.inverse();
}


/*
 *                              utilities.cpp
 *
 * Author: Nic H.
 * Date: 2015-Nov-23
 *
 * implementations for the utilities
 */

#include <algorithm>
#include <deque>

#include <boost/config.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/topological_sort.hpp>

#include "utilities.h"

using namespace std;
using namespace cflr;

dependency_info::dep_res_t dependency_info::find_dependencies(const dep_list_t& deps){
    typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::directedS > dgraph_t;
    // find the size of the dependency graph
    unsigned size = 0;
    for(const auto& d : deps) size = max(size, max(d.first+1, d.second+1));
    // init the strong components graph
    dgraph_t sccG(size);
    for(const auto& d : deps) add_edge(d.first, d.second, sccG);
    // get an integer mapping for the vertex index to a strong component for that index
    vector<int> comps(size);
    int numSCC = strong_components(sccG, make_iterator_property_map(comps.begin(), get(boost::vertex_index, sccG), comps[0]));
    // init the topological dependency graph
    dgraph_t topG(numSCC);
    for(const auto& d : deps){
        int fc = comps[d.first];
        int sc = comps[d.second];
        if(fc != sc) add_edge(fc, sc, topG);
    }
    // sort the graph into a topological order
    vector<int> topol;
    topological_sort(topG, back_inserter(topol), vertex_index_map(boost::identity_property_map()));
    dep_res_t ret;
    for(auto top : topol){
        vector<unsigned> ord;
        size_t n = 0;
        for(auto comp : comps){
            if(comp == top) ord.push_back(n);
            n++;
        }
        ret.push_back(ord);
    }
    return ret;
}


//    Copyright (C) Dimitrios Michail 2019 - 2021.
// 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)

#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN
#include "doctest.h"

#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map/property_map.hpp>

#include <parmcb/detail/fvs.hpp>

TEST_CASE("Feedback Vertex Set")
{

    typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property,
            boost::property<boost::edge_weight_t, double> > Graph;

    Graph graph;

    auto v0 = boost::add_vertex(graph);
    auto v1 = boost::add_vertex(graph);
    auto v2 = boost::add_vertex(graph);
    auto v3 = boost::add_vertex(graph);
    auto v4 = boost::add_vertex(graph);
    auto v5 = boost::add_vertex(graph);
    auto v6 = boost::add_vertex(graph);
    auto v7 = boost::add_vertex(graph);
    auto v8 = boost::add_vertex(graph);
    auto v9 = boost::add_vertex(graph);
    auto v10 = boost::add_vertex(graph);
    auto v11 = boost::add_vertex(graph);
    auto v12 = boost::add_vertex(graph);
    auto v13 = boost::add_vertex(graph);
    auto v14 = boost::add_vertex(graph);
    boost::add_vertex(graph);
    boost::add_vertex(graph);

    boost::add_edge(v0, v3, graph);
    boost::add_edge(v0, v2, graph);
    boost::add_edge(v1, v2, graph);
    boost::add_edge(v1, v4, graph);
    boost::add_edge(v3, v5, graph);
    boost::add_edge(v4, v5, graph);
    boost::add_edge(v4, v6, graph);
    boost::add_edge(v5, v12, graph);
    boost::add_edge(v6, v12, graph);
    boost::add_edge(v7, v8, graph);
    boost::add_edge(v8, v9, graph);
    boost::add_edge(v8, v11, graph);
    boost::add_edge(v9, v10, graph);
    boost::add_edge(v10, v11, graph);
    boost::add_edge(v9, v13, graph);
    boost::add_edge(v14, v13, graph);
    boost::add_edge(v4, v8, graph);

    CHECK(boost::num_vertices(graph) == 17);
    CHECK(boost::num_edges(graph) == 17);

    //
    //   0 -- 3 -- 5 -- 12
    //   |         |    |
    //   2 -- 1 -- 4 -- 6
    //             |
    //        7 -- 8  -- 9 -- 13 -- 14
    //             |     |
    //  15   16    11 -- 10
    //

    typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
    std::set<Vertex> result;
    parmcb::greedy_fvs(graph, std::inserter(result, result.end()));

    CHECK(result.size() == 2);
    CHECK(result.count(4)>0);
    CHECK(result.count(9)>0);

}


#ifndef INCLUDED_numeric_rand_xform_HH
#define INCLUDED_numeric_rand_xform_HH

#include "scheme/numeric/util.hh"
#include <Eigen/Geometry>
#include <random>

namespace scheme { namespace numeric {

template<class T>
void
rand_xform(
	std::mt19937 & rng,
	Eigen::Transform<T,3,Eigen::Affine> & x,
	T cart_bound = 512.0
){
	std::uniform_real_distribution<> runif;
	std::normal_distribution<> rnorm;
	Eigen::Quaterniond qrand( rnorm(rng), rnorm(rng), rnorm(rng), rnorm(rng) );
	qrand.normalize();
	Eigen::Matrix3d m = qrand.matrix();

	x.data()[3] = 0; x.data()[7] = 0; x.data()[11] = 0; x.data()[15] = 1;
	for(int i = 0; i <  3; ++i) x.data()[i] = m.data()[i-0];
	for(int i = 4; i <  7; ++i) x.data()[i] = m.data()[i-1];
	for(int i = 8; i < 11; ++i) x.data()[i] = m.data()[i-2];
	x.data()[12] = runif(rng) * cart_bound - cart_bound/2.0;
	x.data()[13] = runif(rng) * cart_bound - cart_bound/2.0;
	x.data()[14] = runif(rng) * cart_bound - cart_bound/2.0;
}

template<class T>
void
rand_xform(
	std::mt19937 & rng,
	Eigen::Transform<T,3,Eigen::AffineCompact> & x,
	T cart_bound = 512.0
){
	std::uniform_real_distribution<> runif;
	std::normal_distribution<> rnorm;
	Eigen::Quaterniond qrand( rnorm(rng), rnorm(rng), rnorm(rng), rnorm(rng) );
	qrand.normalize();
	Eigen::Matrix3d m = qrand.matrix();
	for(int i = 0; i < 9; ++i) x.data()[i] = m.data()[i];

	x.data()[ 9] = runif(rng) * cart_bound - cart_bound/2.0;
	x.data()[10] = runif(rng) * cart_bound - cart_bound/2.0;
	x.data()[11] = runif(rng) * cart_bound - cart_bound/2.0;
}

template<class X>
X rand_xform(
	std::mt19937 & rng,
	scalar<X> cart_bound = 512.0
){
	X x;
	rand_xform(rng,x,cart_bound);
	return x;
}

template<class T>
void
rand_xform_cartnormal(
	std::mt19937 & rng,
	Eigen::Transform<T,3,Eigen::AffineCompact> & x,
	T const & cart_sd
){
	std::uniform_real_distribution<> runif;
	std::normal_distribution<> rnorm;
	Eigen::Quaterniond qrand( rnorm(rng), rnorm(rng), rnorm(rng), rnorm(rng) );
	qrand.normalize();
	Eigen::Matrix3d m = qrand.matrix();
	for(int i = 0; i < 9; ++i) x.data()[i] = m.data()[i];
	x.data()[ 9] = rnorm(rng) * cart_sd;
	x.data()[10] = rnorm(rng) * cart_sd;
	x.data()[11] = rnorm(rng) * cart_sd;
}

template<class T>
void
rand_xform_quat(
	std::mt19937 & rng,
	Eigen::Transform<T,3,Eigen::AffineCompact> & x,
	double cart_bound, double quat_bound
){
	std::uniform_real_distribution<> runif;
	std::normal_distribution<> rnorm;

	assert( quat_bound < sqrt(3.0)/2.0 );

	{ // ori part
		Eigen::Quaterniond qrand( 0.0, rnorm(rng), rnorm(rng), rnorm(rng) );
		double scale = 1.0 - runif(rng)*runif(rng);
		double len = qrand.norm();
		qrand.x() *= scale * quat_bound / len;
		qrand.y() *= scale * quat_bound / len;
		qrand.z() *= scale * quat_bound / len;
		qrand.w() = sqrt( 1.0 - qrand.squaredNorm() );
		assert( fabs( qrand.norm() - 1.0) < 0.00001 );

		qrand.normalize();
		Eigen::Matrix3d m = qrand.matrix();
		for(int i = 0; i < 9; ++i) x.data()[i] = m.data()[i];
	}
	{ // cart part
		x.data()[ 9] = rnorm(rng);
		x.data()[10] = rnorm(rng);
		x.data()[11] = rnorm(rng);
		double scale = 1.0 - runif(rng)*runif(rng);
		double len = sqrt( x.data()[9]*x.data()[9] + x.data()[10]*x.data()[10] + x.data()[11]*x.data()[11] );
		x.data()[ 9] *= scale * cart_bound / len;
		x.data()[10] *= scale * cart_bound / len;
		x.data()[11] *= scale * cart_bound / len;
	}
}

template<class T>
void
rand_xform_sphere(
	std::mt19937 & rng,
	Eigen::Transform<T,3,Eigen::AffineCompact> & x,
	T const cart_radius,
	T const ang_radius
){
	std::normal_distribution<> rnorm;
	std::uniform_real_distribution<> runif;

	float ang = (1.0 - runif(rng)*runif(rng)) * ang_radius;
	Eigen::Matrix<float,1,3> axis( rnorm(rng), rnorm(rng), rnorm(rng) );
	axis.normalize();
	Eigen::AngleAxis<T> aa( ang, axis );
	x = Eigen::Transform<T,3,Eigen::AffineCompact> ( aa );

	Eigen::AngleAxis<T> aa2( x.rotation() );

	// std::cout << ang << " " << ang_radius << " " << aa2.angle() << std::endl;

	Eigen::Matrix<float,1,3> delta( 9e9, 9e9, 9e9 );
	while( delta.squaredNorm() > cart_radius*cart_radius ){
		delta = 2.0 * cart_radius * Eigen::Matrix<float,1,3>( runif(rng)-0.5, runif(rng)-0.5, runif(rng)-0.5 );
	}

	x.translation() = delta;

}

}}

#endif


#include "Icontroller.hpp"
#include <iostream>
#include <Eigen/Eigen>
#include "include/Sensor.h"
#include "include/SensorArray.h"
#include "math.h"
#include "lqr.cpp"
#include <algorithm>
#include <chrono>
#include <vector>
#include <memory>
#include <string>
//#include "m5op.h"
#include <stdio.h>      /* printf, scanf, puts, NULL */
#include <stdlib.h>     /* srand, rand */
#include <time.h>

int main()
	{
	std::vector<int> controlData;
	unsigned long int auxCount=0;

	simulator_msgs::SensorArray arraymsg;
	std::vector<double> out;
	std::vector<double> xref;
	std::vector<double> error;
	std::vector<double> x;
	int k=0;

		teste* control = new teste();

		control->config();

		while(k<100){

		out=control->execute(arraymsg);
		std::cout << " Out:";
		for (auto i = out.begin(); i != out.end(); ++i)
    			std::cout << *i << ' ';
		k++;
		std::cout << " \n";
		}

		return 0;
	}


#include "ros/ros.h"
#include <lwr_controllers/PoseRPY.h>
#include <kdl/tree.hpp>
#include <Eigen/Dense>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>

#define PI 3.141592653

ros::Subscriber sub_terminal;

ros::Publisher pub_right;
ros::Publisher pub_left;

Eigen::Matrix<double,3,1> p_global_r, p_global_l, p_right, p_left;

tf::StampedTransform transform_right;
tf::StampedTransform transform_left;

double L = 0.1;

Eigen::Matrix<double,3,3> rot_right, rot_left;
Eigen::Matrix<double,3,3> rot_des_r, rot_des_l;
Eigen::Matrix<double,3,3> rot_fin_r, rot_fin_l;
Eigen::Matrix<double,3,1> pos_right, pos_left;

Eigen::Matrix<double,3,3> quat_rot(double x, double y, double z, double w) {
	Eigen::Matrix<double,3,3> temp;
	temp << 1-2*pow(y,2)-2*pow(z,2),             2*x*y-2*w*z,             2*x*z+2*w*y,
			            2*x*y+2*w*z, 1-2*pow(x,2)-2*pow(z,2),             2*z*y-2*w*x,
			            2*x*z-2*w*y,             2*z*y+2*w*x, 1-2*pow(x,2)-2*pow(y,2);
	return temp;
}

int sign(double n) {
	if (n > 0)
		return 1;
	else if (n < 0)
		return -1;
	else
		return 0;
}

Eigen::Matrix<double,4,1> rot_quat(Eigen::Matrix<double,3,3> r) {
	Eigen::Matrix<double,4,1> temp;
	temp << 0.5*sign(r(2,1)-r(1,2))*sqrt(r(0,0)-r(1,1)-r(2,2)+1),
			0.5*sign(r(0,2)-r(2,0))*sqrt(r(1,1)-r(2,2)-r(0,0)+1),
			0.5*sign(r(1,0)-r(0,1))*sqrt(r(2,2)-r(0,0)-r(1,1)+1),
			0.5*sqrt(r(0,0)+r(1,1)+r(2,2)+1);
	return temp;
}

void gposeCallback(const lwr_controllers::PoseRPY::ConstPtr& msg) {
	KDL::Rotation rot_msg = KDL::Rotation::EulerZYX(msg->orientation.yaw, msg->orientation.pitch, msg->orientation.roll);
	double t1,t2,t3,t4;
	rot_msg.GetQuaternion(t1,t2,t3,t4);
	Eigen::Matrix<double,3,3> rot_msg_mat;
	rot_msg_mat = quat_rot(t1,t2,t3,t4);

	Eigen::Matrix<double,3,1> vec_r, vec_l;
	vec_r << 0, L/2, 0;
	vec_l << 0, -L/2, 0;
	p_global_r = rot_msg_mat*vec_r;
	p_global_l = rot_msg_mat*vec_l;
	p_global_r << msg->position.x + p_global_r(0), msg->position.y + p_global_r(1), msg->position.z + p_global_r(2);
	p_global_l << msg->position.x + p_global_l(0), msg->position.y + p_global_l(1), msg->position.z + p_global_l(2);

	p_right = rot_right*p_global_r + pos_right;
	p_left = rot_left*p_global_l + pos_left;

	KDL::Rotation rot_glob = KDL::Rotation::EulerZYX(msg->orientation.yaw,msg->orientation.pitch,msg->orientation.roll);
	KDL::Rotation rot_des_r_kdl = rot_glob;
	rot_des_r_kdl.DoRotX(PI/2);
	KDL::Rotation rot_des_l_kdl = rot_glob;
	rot_des_l_kdl.DoRotX(-PI/2);

	Eigen::Matrix<double,3,3> rot_des_r, rot_des_l;

	rot_des_r_kdl.GetQuaternion(t1,t2,t3,t4);
	rot_des_r = quat_rot(t1,t2,t3,t4);
	rot_des_l_kdl.GetQuaternion(t1,t2,t3,t4);
	rot_des_l = quat_rot(t1,t2,t3,t4);

	rot_fin_r = rot_right*rot_des_r;
	rot_fin_l = rot_left*rot_des_l;

	Eigen::Matrix<double,4,1> r, l;
	r = rot_quat(rot_fin_r);
	l = rot_quat(rot_fin_l);
	KDL::Rotation q_r = KDL::Rotation::Quaternion(r(0),r(1),r(2),r(3));
	KDL::Rotation q_l = KDL::Rotation::Quaternion(l(0),l(1),l(2),l(3));

	lwr_controllers::PoseRPY msg_right;
	lwr_controllers::PoseRPY msg_left;

	q_r.GetEulerZYX(msg_right.orientation.yaw, msg_right.orientation.pitch, msg_right.orientation.roll);
	q_l.GetEulerZYX(msg_left.orientation.yaw, msg_left.orientation.pitch, msg_left.orientation.roll);

	msg_left.id = 0;
	msg_left.position.x = p_left(0);
	msg_left.position.y = p_left(1);
	msg_left.position.z = p_left(2);

	msg_right.id = 0;
	msg_right.position.x = p_right(0);
	msg_right.position.y = p_right(1);
	msg_right.position.z = p_right(2);

	pub_right.publish(msg_right);
	pub_left.publish(msg_left);
}

int main(int argc, char **argv) {
	// Initialize the node
	ros::init(argc, argv, "command_vito");
	ros::NodeHandle node;

// 	rot_des_l <<   0, -1, 0,
// 				   0,  0, 1,
// 				  -1,  0, 0;
// 	rot_des_r <<  1, 0,  0,
// 				  0, 0, -1,
// 				  0, 1,  0;

	tf::TransformListener listener_right;
	tf::TransformListener listener_left;
	transform_right.setIdentity();
	transform_left.setIdentity();
	sleep(5.0);
	while (transform_right.getOrigin().getX() == 0 && transform_left.getOrigin().getX() == 0) {
		try{
			listener_right.lookupTransform("world", "right_arm_base_link", ros::Time(0), transform_right);
			listener_left.lookupTransform("world", "left_arm_base_link", ros::Time(0), transform_left);
		} catch (tf::TransformException &ex) {
			ROS_ERROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}
	}

	rot_right = quat_rot(transform_right.inverse().getRotation().getX(),
						 transform_right.inverse().getRotation().getY(),
						 transform_right.inverse().getRotation().getZ(),
						 transform_right.inverse().getRotation().getW());
	rot_left = quat_rot(transform_left.inverse().getRotation().getX(),
						transform_left.inverse().getRotation().getY(),
						transform_left.inverse().getRotation().getZ(),
						transform_left.inverse().getRotation().getW());
	pos_right << transform_right.inverse().getOrigin().getX(),
				 transform_right.inverse().getOrigin().getY(),
				 transform_right.inverse().getOrigin().getZ();
	pos_left << transform_left.inverse().getOrigin().getX(),
				transform_left.inverse().getOrigin().getY(),
				transform_left.inverse().getOrigin().getZ();

	sub_terminal = node.subscribe("/global_pose", 30, &gposeCallback);

    pub_right = node.advertise<lwr_controllers::PoseRPY>("/right_arm/joint_impedance_controller/command", 30);
	pub_left = node.advertise<lwr_controllers::PoseRPY>("/left_arm/joint_impedance_controller/command", 30);

	sleep(0.1);
	// Loop at a specified frequency, publishing movement commands until we shut down
	ros::Rate rate(30);

	while (ros::ok()) {
		ros::spinOnce();
		rate.sleep();
	}
}


///////////////////////////////////////////////////////////////////////////////
// math::shortcut::shortcut.hpp                                              //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_MATH_CONSTANT_SHORTCUT_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_MATH_CONSTANT_SHORTCUT_HPP_ER_2009
#include <limits>
#include <iostream>
#include <boost/mpl/bool.hpp>
#include <boost/numeric/conversion/converter.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/type_traits.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace math{
namespace constant{

        template<typename T>
        class shortcut{
         public:
            typedef boost::numeric::converter<T,unsigned> conv_t;
            typedef boost::numeric::bounds<T> bounds_;

            static const T zero;
            static const T eps;
            static const T one;
            static const T two;
            static const T lmin; //exp(lmin_)>eps
            static const T lmax; //exp(lmax_)<inf
            static const T inf;
            static const T highest;
            static const T low;
            static const T has_inf;
            static const T inf_else_highest;
            static const T quiet_nan;
            static const T sig_nan;

         private:
            static T inf_else_highest_impl(boost::mpl::bool_<true>)
            {
                return inf;
            }
            static T inf_else_highest_impl(boost::mpl::bool_<false>)
            {
                return highest;
            }

        };

        template<typename T>
        const T shortcut<T>::has_inf = std::numeric_limits<T>::has_infinity;

        template<typename T>
        const T shortcut<T>::zero = conv_t::convert(0);

        template<typename T>
        const T shortcut<T>::eps = boost::math::tools::epsilon<T>();

        template<typename T>
        const T shortcut<T>::one = conv_t::convert(1);

        template<typename T>
        const T shortcut<T>::two = conv_t::convert(2);

        template<typename T>
        const T shortcut<T>::highest = shortcut<T>::bounds_::highest();

        template<typename T>
        const T shortcut<T>::low = shortcut<T>::bounds_::lowest();

        template<typename T>
        const T shortcut<T>::lmin = boost::math::tools::log_min_value<T>();

        template<typename T>
        const T shortcut<T>::lmax = boost::math::tools::log_max_value<T>();

        template<typename T>
        const T shortcut<T>::inf = std::numeric_limits<T>::infinity();

        template<typename T>
        const T shortcut<T>::quiet_nan = std::numeric_limits<T>::quiet_NaN();

        template<typename T>
        const T shortcut<T>::sig_nan = std::numeric_limits<T>::signaling_NaN();

        template<typename T>
        const T shortcut<T>::inf_else_highest
            = (shortcut<T>::has_inf) ? shortcut<T>::inf : shortcut<T>::highest;

}// constant
}// math
}// detail
}// statistics
}// boost

#endif


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/concept/value.hpp>
#include <eve/constant/valmin.hpp>
#include <eve/constant/valmax.hpp>
#include <eve/function/gamma_p_inv.hpp>
#include <type_traits>
#include <cmath>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/platform.hpp>
#include <boost/math/special_functions/gamma.hpp>

//==================================================================================================
// Types tests
//==================================================================================================
EVE_TEST_TYPES( "Check return types of gamma_p_inv"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;

  TTS_EXPR_IS( eve::gamma_p_inv(T(), T())  , T);
  TTS_EXPR_IS( eve::gamma_p_inv(T(), v_t()), T);
  TTS_EXPR_IS( eve::gamma_p_inv(v_t(), T()), T);
  TTS_EXPR_IS( eve::gamma_p_inv(v_t(), v_t()), v_t);
};

//==================================================================================================
// gamma_p_inv  tests
//==================================================================================================
EVE_TEST( "Check behavior of gamma_p_inv on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 1.0)
                             , eve::test::randoms(0.0, 100.0))
        )
<typename T>(T const& a0, T const& a1 )
{
  using v_t = eve::element_type_t<T>;
  using eve::gamma_p_inv;
  TTS_ULP_EQUAL( eve::gamma_p_inv(a0, a1),  map([&](auto e, auto f) -> v_t{ return boost::math::gamma_p_inv(f, e); }, a0, a1), 33);

  auto bggpi =  [](auto e, auto f){return boost::math::gamma_p_inv(f, e); };
  if constexpr( eve::platform::supports_invalids )
  {
    TTS_IEEE_EQUAL(gamma_p_inv(eve::nan(eve::as<T>()), T(1))  , eve::nan(eve::as<T>()) );
    TTS_IEEE_EQUAL(gamma_p_inv(eve::inf(eve::as<T>()), T(1))  , eve::nan(eve::as<T>()) );
  }
  TTS_ULP_EQUAL(gamma_p_inv(T(0.1), T(0.75))   , T(bggpi(v_t(0.1), v_t(0.75)))     , 20);
  TTS_ULP_EQUAL(gamma_p_inv(T(0.75), T(0.75))  , T(bggpi(v_t(0.75), v_t(0.75)))    , 20);
  TTS_ULP_EQUAL(gamma_p_inv(T(0.1), T(0.1))    , T(bggpi(v_t(0.1), v_t(0.1)))      , 20);
  TTS_ULP_EQUAL(gamma_p_inv(T(0.25), T(0.75))  , T(bggpi(v_t(0.25), v_t(0.75)))    , 20);
  TTS_ULP_EQUAL(gamma_p_inv(T(0.25), T(0.25))  , T(bggpi(v_t(0.25), v_t(0.25)))    , 20);
  TTS_ULP_EQUAL(gamma_p_inv(T(0.75), T(0.25))  , T(bggpi(v_t(0.75), v_t(0.25)))    , 20);
  TTS_ULP_EQUAL(gamma_p_inv(T(0.25), T(2.0))   , T(bggpi(v_t(0.25), v_t(2.0)))     , 20);
  TTS_ULP_EQUAL(gamma_p_inv(T(0.25), T(2.25))  , T(bggpi(v_t(0.25), v_t(2.25)))    , 20);
  TTS_ULP_EQUAL(gamma_p_inv(T(0.75), T(2.25))  , T(bggpi(v_t(0.75), v_t(2.25)))    , 20);

};