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

  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 <niftkFileHelper.h>
#include <mitkTestingMacros.h>
#include <mitkLogMacros.h>
#include <mitkOpenCVMaths.h>
#include <boost/math/special_functions/fpclassify.hpp>
#include <cmath>

/**
 * \file Tests for some of the functions in openCVMaths.
 */

void ArithmaticTests()
{
  cv::Point2d point1 = cv::Point2d ( 1.0 , 1.0 );
  cv::Point2d point2 = cv::Point2d ( 0.5 , 0.3 );
  cv::Point2d point3 = cv::Point2d ( 0.7 , 1.4 );
  cv::Point2d point4 = cv::Point2d ( 1.5 , 2.3 );

  double tolerance = 1e-6;
  MITK_TEST_CONDITION ( point1 == point1 , "Testing point2d equality operator" );
  MITK_TEST_CONDITION (mitk::NearlyEqual (point1, point1, tolerance) , "Testing Nearly Equal " );
  MITK_TEST_CONDITION (mitk::NearlyEqual (( point1 + point2 ), cv::Point2d ( 1.5 , 1.3 ), tolerance), "Testing addition operator");
  MITK_TEST_CONDITION (mitk::NearlyEqual ((point4 - point3) ,cv::Point2d(0.8, 0.9), tolerance), "Testing subtraction operator");

  cv::Point3d point3d_1 = cv::Point3d ( 1.0, 1.0, 1.0 );
  cv::Point3d point3d_2 = cv::Point3d ( 4.0, 3.0, -1.0 );
  cv::Matx44d mat44d_1;
  cv::Mat mat ( 4,4,CV_64FC1 );
  mat44d_1(0,0) = 1.0; mat44d_1(0,1) = 0.0; mat44d_1(0,2) = 0.0; mat44d_1(0,3) = 10.0;
  mat44d_1(1,0) = 0.0; mat44d_1(1,1) = 1.0; mat44d_1(1,2) = 0.0; mat44d_1(1,3) = 16.0;
  mat44d_1(2,0) = 0.0; mat44d_1(2,1) = 0.0; mat44d_1(2,2) = 1.0; mat44d_1(2,3) = 21.0;
  mat44d_1(3,0) = 0.0; mat44d_1(3,1) = 0.0; mat44d_1(3,2) = 0.0; mat44d_1(3,3) = 1.0;

  mat.at<double>(0,0) = 1.0; mat.at<double>(0,1) = 0.0; mat.at<double>(0,2) = 0.0; mat.at<double>(0,3) = 12.0;
  mat.at<double>(1,0) = 0.0; mat.at<double>(1,1) = 1.0; mat.at<double>(1,2) = 0.0; mat.at<double>(1,3) = 14.0;
  mat.at<double>(2,0) = 0.0; mat.at<double>(2,1) = 0.0; mat.at<double>(2,2) = 1.0; mat.at<double>(2,3) = 9.0;
  mat.at<double>(3,0) = 0.0; mat.at<double>(3,1) = 0.0; mat.at<double>(3,2) = 0.0; mat.at<double>(3,3) = 1.0;

  using namespace mitk;
  cv::Point3d product = mat * point3d_1;
  MITK_TEST_CONDITION (mitk::NearlyEqual(product, cv::Point3d(13.0, 15.0, 10.0), tolerance), "Checking cv::Mat * cv::Point3d operator");
  product = mat44d_1 * point3d_1;
  MITK_TEST_CONDITION (mitk::NearlyEqual(product, cv::Point3d(11.0, 17.0, 22.0), tolerance), "Checking cv::Matx44d * cv::Point3d operator");
  std::pair < cv::Point3d, cv::Point3d > pairProduct = mitk::TransformPointPair ( mat44d_1,
      std::pair < cv::Point3d, cv::Point3d > ( point3d_1, point3d_2 ));
  MITK_TEST_CONDITION (mitk::NearlyEqual(pairProduct.first, cv::Point3d(11.0, 17.0, 22.0), tolerance), "Checking transform point pair first");
  MITK_TEST_CONDITION (mitk::NearlyEqual(pairProduct.second, cv::Point3d(14.0, 19.0, 20.0), tolerance), "Checking transform point pair second");

}

void RMSTest()
{
  //make some measured points
  std::vector < mitk::ProjectedPointPairsWithTimingError > measured;
  std::vector < mitk::ProjectedPointPairsWithTimingError > actual;

  mitk::ProjectedPointPairsWithTimingError measured_0;
  measured_0.m_Points.push_back(
      mitk::ProjectedPointPair (cv::Point2d ( -0.1, 0.7 ),cv::Point2d (1.1 , 0.9)));
  measured.push_back(measured_0);

  mitk::ProjectedPointPairsWithTimingError actual_0;
  actual_0.m_Points.push_back(
      mitk::ProjectedPointPair (cv::Point2d ( -0.1, 0.7 ),cv::Point2d (1.1 , 0.9)));
  actual.push_back(actual_0);

  int index = -1;
  cv::Point2d outlierSD = cv::Point2d(2.0, 2.0);
  long long allowableTimingError = 30e6;
  bool duplicateLines = true;
  std::pair <double,double> rmsError = mitk::RMSError ( measured , actual ,
      index, outlierSD, allowableTimingError , duplicateLines );

  double tolerance = 1e-4;
  MITK_TEST_CONDITION ( mitk::NearlyEqual ( cv::Point2d( rmsError.first, rmsError.second ), cv::Point2d ( 0.0, 0.0), tolerance) , "Testing RMSError returns 0.0 when no error" );

  mitk::ProjectedPointPairsWithTimingError actual_1;
  actual_1.m_Points.push_back(
      mitk::ProjectedPointPair (cv::Point2d ( -0.2, 0.5 ),cv::Point2d (1.2 , 0.3)));

  measured.push_back(measured_0);
  actual.push_back(actual_1);

  duplicateLines=false;
  rmsError = mitk::RMSError ( measured , actual ,
      index, outlierSD, allowableTimingError , duplicateLines );

  MITK_TEST_CONDITION ( mitk::NearlyEqual ( cv::Point2d( rmsError.first, rmsError.second ) , cv::Point2d ( 0.15811, 0.43012), tolerance ) , "Testing RMSError returns right value for a real error" );

  duplicateLines=true;
  rmsError = mitk::RMSError ( measured , actual ,
      index, outlierSD, allowableTimingError , duplicateLines );

  MITK_TEST_CONDITION ( mitk::NearlyEqual ( cv::Point2d (rmsError.first, rmsError.second) , cv::Point2d ( 0.0, 0.0), tolerance) , "Testing duplicate lines parameter has the desired effect" );

  mitk::ProjectedPointPairsWithTimingError measured_1;
  measured_1.m_Points.push_back(
      mitk::ProjectedPointPair (cv::Point2d ( -0.1, 0.7 ),cv::Point2d (1.1 , 0.9)));
  measured_1.m_TimingError = 30e7;
  measured.push_back(measured_1);
  actual.push_back(actual_0);

  duplicateLines=false;
  rmsError = mitk::RMSError ( measured , actual ,
      index, outlierSD, allowableTimingError , duplicateLines );

  MITK_TEST_CONDITION ( mitk::NearlyEqual ( cv::Point2d (rmsError.first, rmsError.second ) ,cv::Point2d ( 0.15811, 0.43012), tolerance) , "Testing RMSError rejects high timing error points" );

  allowableTimingError = 31e7;

  outlierSD = cv::Point2d(std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity());
  rmsError = mitk::RMSError ( measured , actual ,
      index, outlierSD, allowableTimingError , duplicateLines );

  MITK_TEST_CONDITION ( mitk::NearlyEqual ( cv::Point2d(rmsError.first, rmsError.second) ,cv::Point2d ( 0.12910, 0.35119), tolerance) , "Testing RMSError accepts when allowable timing error increased" );

  outlierSD = cv::Point2d(2.0, 2.0);
  rmsError = mitk::RMSError ( measured , actual ,
      index, outlierSD, allowableTimingError , duplicateLines );

  MITK_TEST_CONDITION ( mitk::NearlyEqual ( cv::Point2d(rmsError.first, rmsError.second) , cv::Point2d( 0.0, 0.0), tolerance) , "Testing RMSError culls outliers" );

}

void FindIntersectTest()
{
  cv::Vec4i line1;
  cv::Vec4i line2;
  cv::Point2d intersect;

  for ( unsigned int i = 0 ; i < 4 ; i ++ )
  {
    line1[i] = 0;
    line2[i] = 0;
  }
  intersect = mitk::FindIntersect (line1, line2);

  MITK_TEST_CONDITION ((boost::math::isnan(intersect.x) && boost::math::isnan(intersect.y)), "IntersectTest: all zeros : " << intersect );
  //parallel vertical lines
  line1[0] = 0 ; line1[1] = 0 ; line1[2] = 0 ; line1[3] = 1;
  line2[0] = 1 ; line2[1] = 0 ; line2[2] = 1 ; line2[3] = 1;
  intersect = mitk::FindIntersect (line1, line2);
  MITK_TEST_CONDITION ((boost::math::isnan(intersect.x) && boost::math::isnan(intersect.y)), "IntersectTest: parallel vertical lines" << intersect );
  //parallel horizontal lines
  line1[0] = 0 ; line1[1] = 0 ; line1[2] = 1 ; line1[3] = 0;
  line2[0] = 0 ; line2[1] = 1 ; line2[2] = 1 ; line2[3] = 1;
  intersect = mitk::FindIntersect (line1, line2);
  MITK_TEST_CONDITION ((boost::math::isnan(intersect.x) && boost::math::isnan(intersect.y)), "IntersectTest: parallel horizontal lines" << intersect );
  //parallel oblique lines
  line1[0] = 0 ; line1[1] = 0 ; line1[2] = 1 ; line1[3] = 1;
  line2[0] = 1 ; line2[1] = 0 ; line2[2] = 2 ; line2[3] = 1;
  intersect = mitk::FindIntersect (line1, line2);
  MITK_TEST_CONDITION ((boost::math::isnan(intersect.x) && boost::math::isnan(intersect.y)), "IntersectTest: parallel oblique lines" << intersect );
  //simple intersection
  line1[0] = 0 ; line1[1] = 0 ; line1[2] = 1 ; line1[3] = 1;
  line2[0] = 1 ; line2[1] = 0 ; line2[2] = 0 ; line2[3] = 1;
  intersect = mitk::FindIntersect (line1, line2);
  MITK_TEST_CONDITION (intersect == cv::Point2d(0.5,0.5), "IntersectTest: 45 degrees intersect at 0.5" << intersect );
  //line1 vertical
  line1[0] = 0 ; line1[1] = 0 ; line1[2] = 0 ; line1[3] = 1;
  line2[0] = 1 ; line2[1] = 0 ; line2[2] = 0 ; line2[3] = 1;
  intersect = mitk::FindIntersect (line1, line2);
  MITK_TEST_CONDITION (intersect == cv::Point2d(0.0,1.0), "IntersectTest: line 1 vertical " << intersect );
  //line 2 vertical
  line1[0] = 0 ; line1[1] = 0 ; line1[2] = 1 ; line1[3] = 1;
  line2[0] = 1 ; line2[1] = 0 ; line2[2] = 1 ; line2[3] = 1;
  intersect = mitk::FindIntersect (line1, line2);
  MITK_TEST_CONDITION (intersect == cv::Point2d(1.0,1.0), "IntersectTest: line 2 vertical " << intersect );

}

void FindIntersectsTest()
{
  cv::Vec4i line1;
  cv::Vec4i line2;
  cv::Vec4i line3;
  cv::Vec4i line4;

  //line1 is degenerate
  line1[0] = 0 ; line1[1] = 0 ; line1[2] = 0 ; line1[3] = 0;
  //line2 y = 2x + 10
  line2[0] = 0 ; line2[1] = 10 ; line2[2] = 10 ; line2[3] = 30;
  //line3 y = -x ;
  line3[0] = -5; line3[1] = 5 ; line3[2] = 5; line3[3] = -5;
  //line4 y = x;
  line4[0] = 0; line4[1] = 0 ; line4[2] = 15; line4[3] = 15;

  std::vector <cv::Vec4i> lines;
  lines.push_back(line1);
  lines.push_back(line2);
  lines.push_back(line3);
  lines.push_back(line4);

  bool rejectPointsNotOnBothLines = false;
  bool rejectNonPerpendicularLines = false;
  double perpendicularityTolerance = 20.0;

  std::vector <cv::Point2d> intersects;
  intersects = mitk::FindIntersects ( lines, rejectPointsNotOnBothLines, rejectNonPerpendicularLines, perpendicularityTolerance );
  // there should be 3 intersections, (-10/3, 10/3 ) , (-10,-10), and ( 0,0)
  MITK_TEST_CONDITION ( intersects.size() == 3 , "Testing size of intesects vector with no conditions " << intersects.size());
  MITK_TEST_CONDITION ( mitk::NearlyEqual ( intersects[0], cv::Point2d (-3.333333, 3.333333), 1e-6) , "Testing first intersect with no conditions " << intersects[0]);
  MITK_TEST_CONDITION ( mitk::NearlyEqual ( intersects[1], cv::Point2d (-10.0, -10.0),1e-6) , "Testing second intersect with no conditions " << intersects[1]);
  MITK_TEST_CONDITION ( mitk::NearlyEqual ( intersects[2], cv::Point2d (0.0, 0.0), 1e-6) , "Testing third intersect with no conditions " << intersects[2]);
  // add a perpendicularity constraint should remove line 2 line 4
  rejectNonPerpendicularLines = true;
  perpendicularityTolerance = 20.0;
  intersects = mitk::FindIntersects ( lines, rejectPointsNotOnBothLines, rejectNonPerpendicularLines, perpendicularityTolerance );
  MITK_TEST_CONDITION ( intersects.size() == 2 , "Testing size of intesects vector with 20 degree perpendicularity" << intersects.size());
  MITK_TEST_CONDITION ( mitk::NearlyEqual ( intersects[0], cv::Point2d (-3.333333, 3.333333), 1e-6) , "Testing first intersect with no conditions " << intersects[0]);
  MITK_TEST_CONDITION ( mitk::NearlyEqual ( intersects[1], cv::Point2d (0.0, 0.0), 1e-6) , "Testing second intersect with no conditions " << intersects[1]);
  // add an on both lines constraint should leave only line3 line4
  rejectPointsNotOnBothLines = true;
  intersects = mitk::FindIntersects ( lines, rejectPointsNotOnBothLines, rejectNonPerpendicularLines, perpendicularityTolerance );
  MITK_TEST_CONDITION ( intersects.size() == 1 , "Testing size of intesects vector with 20 degree perpendicularity and on interval requirement" << intersects.size());
  MITK_TEST_CONDITION ( mitk::NearlyEqual ( intersects[0], cv::Point2d (0.0, 0.0), 1e-6) , "Testing first intersect with all conditions " << intersects[0]);

}
void AngleBetweenLinesTest()
{
  double tolerance = 1e-6;
  cv::Vec4i line1;
  cv::Vec4i line2;

  for ( unsigned int i = 0 ; i < 4 ; i++ )
  {
    line1[i] = 0;
    line2[i] = 0;
  }

  MITK_TEST_CONDITION ( boost::math::isnan(mitk::AngleBetweenLines ( line1, line2)) , "Testing angle between 2 zero vectors " << mitk::AngleBetweenLines ( line1, line2) );

  line1[1] = 1;
  line2[3] = 1;

  MITK_TEST_CONDITION ( ( fabs (mitk::AngleBetweenLines ( line1, line2) - 0.0) < tolerance  ) , "Testing angle between 2 parallel vectors " << mitk::AngleBetweenLines ( line1, line2) );

  line2[1] = 1;
  line2[0] = 1;

  MITK_TEST_CONDITION ( ( fabs (mitk::AngleBetweenLines ( line1, line2) - CV_PI/2.0 ) < tolerance ) , "Testing angle between 2 perpendicular vectors " << mitk::AngleBetweenLines ( line1, line2) );

  line2[1]=2;

  MITK_TEST_CONDITION ( ( fabs (mitk::AngleBetweenLines ( line1, line2) - CV_PI/4.0) < tolerance ) , "Testing angle between 2 vectors at 45 degrees " << mitk::AngleBetweenLines ( line1, line2) );

  for ( unsigned int i = 0 ; i < 4 ; i++ )
  {
    line1[i] = 0;
    line2[i] = 0;
  }

  line1[0] = 1;
  line2[2] = -1;

  MITK_TEST_CONDITION ( ( fabs (mitk::AngleBetweenLines ( line1, line2) - 0.0 ) < tolerance ) , "Testing angle between 2 opposite parallel vectors " << mitk::AngleBetweenLines ( line1, line2) );

}

void CheckIfLinesArePerpendicularTest ( )
{

  double angleTolerance = 10.0;
  cv::Vec4i line1;
  cv::Vec4i line2;

  for ( unsigned int i = 0 ; i < 4 ; i++ )
  {
    line1[i] = 0;
    line2[i] = 0;
  }

  MITK_TEST_CONDITION ( (! mitk::CheckIfLinesArePerpendicular(line1,line2, angleTolerance ) ), "Checking that 2 zero vectors are not perpendicular" );

  line1[1] = 1;
  line2[3] = 1;

  MITK_TEST_CONDITION ( (! mitk::CheckIfLinesArePerpendicular(line1,line2, angleTolerance) ), "Checking that 2 parallel vectors are not perpendicular" );

  angleTolerance = 90.0;

  MITK_TEST_CONDITION ( ( mitk::CheckIfLinesArePerpendicular(line1,line2, angleTolerance) ), "Checking that 2 parallel vectors are perpendicular within 90 degrees tolerance" );

  line1[1] = 0;
  line2[3] = 0;

  MITK_TEST_CONDITION ( (! mitk::CheckIfLinesArePerpendicular(line1,line2, angleTolerance ) ), "Checking that 2 zero vectors are not perpendicular even when tolerance = 90 degrees" );

  angleTolerance = 0.0;

  line1[0] = 1000;
  line2[1] = -1000;

  MITK_TEST_CONDITION ( ( mitk::CheckIfLinesArePerpendicular(line1,line2, angleTolerance ) ), "Checking that 2 perpendicular vectors are exactly  perpendicular" );

  angleTolerance = 10.0;
  line1[1]=-167;

  MITK_TEST_CONDITION ( ( mitk::CheckIfLinesArePerpendicular(line1,line2, angleTolerance ) ), "Checking that 2 lines at 80.5 degrees are perpendicular within 10 degrees" );

  angleTolerance = 9.0;

  MITK_TEST_CONDITION ( ( ! mitk::CheckIfLinesArePerpendicular(line1,line2, angleTolerance ) ), "Checking that 2 lines at 80.5 degrees are not perpendicular within 9 degrees" );

  angleTolerance = 10.0;
  line1[1]=167;

  MITK_TEST_CONDITION ( ( mitk::CheckIfLinesArePerpendicular(line1,line2, angleTolerance ) ), "Checking that 2 lines at 99.5 degrees are perpendicular within 10 degrees" );

  angleTolerance = 9.0;

  MITK_TEST_CONDITION ( ( ! mitk::CheckIfLinesArePerpendicular(line1,line2, angleTolerance ) ), "Checking that 2 lines at 99.5 degrees are not perpendicular within 9 degrees" );

}

void PointInIntervalTest ( )
{

  cv::Vec4i line;

  //when x2 and y2 are greater than x1 and x1

  line[0] = -10;
  line[1] = 5;
  line[2] = 5;
  line[3] = 7;

  MITK_TEST_CONDITION ( ( ! mitk::PointInInterval(cv::Point2d(-10.1,6),line)), "Point just outside x bound" );
  MITK_TEST_CONDITION ( ( mitk::PointInInterval(cv::Point2d(-10.0,5),line)), "Point on x bound" );
  MITK_TEST_CONDITION ( ( mitk::PointInInterval(cv::Point2d(-9.9,5.5),line)), "Point just inside x bound" );
  MITK_TEST_CONDITION ( ( ! mitk::PointInInterval(cv::Point2d(-10.0,0),line)), "Point outside y bound" );
  MITK_TEST_CONDITION ( ( ! mitk::PointInInterval(cv::Point2d(20.0,0),line)), "Point outside both bound" );

  //when x1 and y1 are greater than x2 and x2

  line[2] = -10;
  line[3] = 5;
  line[0] = 5;
  line[1] = 7;

  MITK_TEST_CONDITION ( ( ! mitk::PointInInterval(cv::Point2d(-10.1,6),line)), "Point just outside x bound" );
  MITK_TEST_CONDITION ( ( mitk::PointInInterval(cv::Point2d(-10.0,5),line)), "Point on x bound" );
  MITK_TEST_CONDITION ( ( mitk::PointInInterval(cv::Point2d(-9.9,5.5),line)), "Point just inside x bound" );
  MITK_TEST_CONDITION ( ( ! mitk::PointInInterval(cv::Point2d(-10.0,0),line)), "Point outside y bound" );
  MITK_TEST_CONDITION ( ( ! mitk::PointInInterval(cv::Point2d(20.0,0),line)), "Point outside both bound" );
  MITK_TEST_CONDITION ( ( ! mitk::PointInInterval(cv::Point2d(std::numeric_limits<double>::quiet_NaN(),0),line)), "Point is NaN" );

  line[2] = 0;
  line[3] = 0;
  line[0] = 0;
  line[1] = 0;

  MITK_TEST_CONDITION ( ( ! mitk::PointInInterval(cv::Point2d(-10.1,6),line)), "Zero interval" );
  MITK_TEST_CONDITION ( ( mitk::PointInInterval(cv::Point2d(0.0,0.0),line)), "Zero interval and point" );

}

void DistanceToLineTest ()
{
  cv::Point3d u ( 0.0,0.0,0.0);
  cv::Point3d v ( 1.0,1.0,1.0);
  cv::Point3d x ( 0.0,1.0,0.5);

  double dtl = mitk::DistanceToLine (std::pair<cv::Point3d, cv::Point3d>(u,v),x);
  MITK_TEST_CONDITION( fabs(dtl - 0.70711) < 1e-5 ,"Checking distance to line " << dtl);

  cv::Point3d u1 ( 10.0, 10.0 , 10.0);
  cv::Point3d v1 ( 11.0, 11.0, 11.0);

  double dtl1 = mitk::DistanceToLine (std::pair<cv::Point3d, cv::Point3d>(u1,v1),x);
  MITK_TEST_CONDITION( fabs(dtl1 - 0.70711) < 1e-5 ,"Checking distance to line again " << dtl1);
}

void DistanceToLineSegmentTest ()
{
  cv::Point3d u ( 0.0,0.0,0.0);
  cv::Point3d v ( 1.0,1.0,1.0);
  cv::Point3d x ( 0.0,1.0,0.5);

  double dtl = mitk::DistanceToLineSegment (std::pair<cv::Point3d, cv::Point3d>(u,v),x);
  MITK_TEST_CONDITION( fabs(dtl - 0.70711) < 1e-5 ,"Checking distance to line segment with projection in segment: " << dtl);
  dtl = mitk::DistanceToLineSegment (std::pair<cv::Point3d, cv::Point3d>(v,u),x);
  MITK_TEST_CONDITION( fabs(dtl - 0.70711) < 1e-5 ,"Checking distance to line segment with projection in segment, flipped ends: " << dtl);

  x = cv::Point3d ( 2.0, 2.0, 2.0 );
  dtl = mitk::DistanceToLineSegment (std::pair<cv::Point3d, cv::Point3d>(u,v),x);
  MITK_TEST_CONDITION( fabs(dtl - sqrt(3.0)) < 1e-5 ,"Checking distance to line segment with projection on line but off segment: " << dtl);
  dtl = mitk::DistanceToLineSegment (std::pair<cv::Point3d, cv::Point3d>(v,u),x);
  MITK_TEST_CONDITION( fabs(dtl - sqrt(3.0)) < 1e-5 ,"Checking distance to line segment with projection on line but off segment, flipped ends: " << dtl);

  v = cv::Point3d ( -1.0, -1.0, -1.0 );
  x = cv::Point3d ( 0.0,1.0,0.5);

  dtl = mitk::DistanceToLineSegment (std::pair<cv::Point3d, cv::Point3d>(u,v),x);
  MITK_TEST_CONDITION( fabs(dtl - sqrt(1.25)) < 1e-5 ,"Checking distance to line segment with projection off line and off segment: " << dtl);
  dtl = mitk::DistanceToLineSegment (std::pair<cv::Point3d, cv::Point3d>(v,u),x);
  MITK_TEST_CONDITION( fabs(dtl - sqrt(1.25)) < 1e-5 ,"Checking distance to line segment with projection off line and off segment, flipped ends: " << dtl);

  u = cv::Point3d ( -2.0, -2.0, -2.0 );
  x = cv::Point3d ( 0.0,1.0,-0.5);

  dtl = mitk::DistanceToLineSegment (std::pair<cv::Point3d, cv::Point3d>(u,v),x);
  MITK_TEST_CONDITION( fabs(dtl - sqrt(5.25)) < 1e-5 ,"Checking distance to line segment with projection off line and off segment: " << dtl);
  dtl = mitk::DistanceToLineSegment (std::pair<cv::Point3d, cv::Point3d>(v,u),x);
  MITK_TEST_CONDITION( fabs(dtl - sqrt(5.25)) < 1e-5 ,"Checking distance to line segment with projection off line and off segment, flipped ends: " << dtl);

  cv::Point3d u1 ( 10.0, 10.0 , 10.0);
  cv::Point3d v1 ( 11.0, 11.0, 11.0);
  x = cv::Point3d ( 0.0,1.0,0.5);

  double dtl1 = mitk::DistanceToLineSegment (std::pair<cv::Point3d, cv::Point3d>(u1,v1),x);
  MITK_TEST_CONDITION( fabs(dtl1 - 16.4696690) < 1e-5 ,"Checking point off line and off segment " << dtl1);
}

void DotAndCrossProductTest ()
{
  cv::Point3d x ( 2.0,3.0,5.0);
  cv::Point3d y ( 7.0,11.0,13.0);
  cv::Point3d cp (-16.0,9.0,1.0);
  double dp =  112.0;

  MITK_TEST_CONDITION( mitk::NearlyEqual (mitk::CrossProduct(x,y),cp,1e-6),"Checking cross product " << mitk::CrossProduct(x,y));
  MITK_TEST_CONDITION( ( mitk::DotProduct(x,y) - dp < 1e-6),"Checking dot product " << mitk::DotProduct(x,y));
}

void NormTest ()
{
  cv::Point3d x ( 2.0,3.0,5.0);

  MITK_TEST_CONDITION ( ( mitk::Norm (x) - 6.1644)  < 1e-4, "Checking point vector norm " << mitk::Norm(x) );
}

void DistanceBetweenLinesTest ()
{
  //a line through the origin an (1,1,0)
  cv::Point3d P0 (0,0,0);
  cv::Point3d u (0.70711,0.70711,0);
  //a line through (1,0,1) and (0,1,1)
  cv::Point3d Q0 (1,0,1);
  cv::Point3d v (-0.70711,0.70711,0);
  cv::Point3d midPoint;
  double distance = mitk::DistanceBetweenLines(P0, u, Q0 , v , midPoint);

  MITK_TEST_CONDITION ( ( distance - 1.0 )  < 1e-6, "Checking distance between two lines " << distance );
  MITK_TEST_CONDITION( mitk::NearlyEqual (midPoint,cv::Point3d(0.5,0.5,0.5),1e-6),"Checking midpoint " << midPoint);

  distance = mitk::DistanceBetweenLines(P0, u, Q0 , v , midPoint);
  MITK_TEST_CONDITION ( ( distance - 1.0 )  < 1e-6, "Checking distance between two lines " << distance );
  MITK_TEST_CONDITION( mitk::NearlyEqual (midPoint,cv::Point3d(0.5,0.5,0.5),1e-6),"Checking midpoint " << midPoint);

  //parallel  lines
  //a line through the origin and (1,1,0)
  P0 = cv::Point3d (0,0,0);
  u = cv::Point3d (0.70711,0.70711,0);
  //a line through (1,0,0) and (2,1,0)
  Q0 = cv::Point3d(1,0,0);
  v = cv::Point3d (0.70711,0.70711,0);
  distance = mitk::DistanceBetweenLines(P0, u, Q0 , v , midPoint);

  MITK_TEST_CONDITION ( ( fabs (distance - sqrt ( 0.5 * 0.5 + 0.5 * 0.5 ) ))  < 1e-6, "Checking distance between two parallel lines " << distance );
  MITK_TEST_CONDITION( ( ! mitk::IsNotNaNorInf (midPoint)), "Checking midpoint for parallel lines " << midPoint);

  v = cv::Point3d (-0.70711,-0.70711,0);
  distance = mitk::DistanceBetweenLines(P0, u, Q0 , v , midPoint);

  MITK_TEST_CONDITION ( ( fabs (distance - sqrt ( 0.5 * 0.5 + 0.5 * 0.5 )) )  < 1e-6, "Checking distance between two parallel lines 2 " << distance );
  MITK_TEST_CONDITION( ( ! mitk::IsNotNaNorInf (midPoint)), "Checking midpoint for parallel lines 2 " << midPoint);

  Q0 = cv::Point3d(10,9,0);
  v = cv::Point3d (0.70711,0.70711,0);

  distance = mitk::DistanceBetweenLines(P0, u, Q0 , v , midPoint);

  MITK_TEST_CONDITION ( ( fabs (distance - sqrt ( 0.5 * 0.5 + 0.5 * 0.5 )) )  < 1e-6, "Checking distance between two parallel lines 3 " << distance );
  MITK_TEST_CONDITION( ( ! mitk::IsNotNaNorInf (midPoint)), "Checking midpoint for parallel lines 3 " << midPoint);

}

void DistanceBetweenLineAndSegmentTest ()
{
  //a line through the origin and (1,1,0)
  cv::Point3d P0 (0,0,0);
  cv::Point3d u (0.70711,0.70711,0);
  //a line through (1,0,1) and (0,1,1)
  cv::Point3d x0 (1,0,1);
  cv::Point3d x1 (0,1,1);
  cv::Point3d closestPointOnSecondLine;

  double distance = mitk::DistanceBetweenLineAndSegment(P0, u, x0, x1 , closestPointOnSecondLine);
  MITK_TEST_CONDITION ( ( distance - 1.0 )  < 1e-6, "Checking distance between line and segment " << distance );
  MITK_TEST_CONDITION( mitk::NearlyEqual (closestPointOnSecondLine,cv::Point3d(0.5,0.5,1.0),1e-6),"Checking closest point on second line " << closestPointOnSecondLine);

  //parallel  lines
  //a line through the origin and (1,1,0)
  P0 = cv::Point3d (0,0,0);
  u = cv::Point3d (0.70711,0.70711,0);
  //a line through (1,0,0) and (2,1,0)
  x0 = cv::Point3d(1,0,0);
  x1 = cv::Point3d (2,1,0);
  distance = mitk::DistanceBetweenLineAndSegment(P0, u, x0, x1 , closestPointOnSecondLine);

  MITK_TEST_CONDITION ( ( fabs (distance - sqrt ( 0.5 * 0.5 + 0.5 * 0.5 ) ))  < 1e-6, "Checking distance between line and parallel segment " << distance );
  MITK_TEST_CONDITION( ( ! mitk::IsNotNaNorInf (closestPointOnSecondLine)), "Checking closest point on parallel segment " << closestPointOnSecondLine);

  x1 = cv::Point3d (0,-1,0);
  distance = mitk::DistanceBetweenLineAndSegment(P0, u, x0, x1, closestPointOnSecondLine);

  MITK_TEST_CONDITION ( ( fabs (distance - sqrt ( 0.5 * 0.5 + 0.5 * 0.5 )) )  < 1e-6, "Checking distance between line and parallel segment " << distance );
  MITK_TEST_CONDITION( ( ! mitk::IsNotNaNorInf (closestPointOnSecondLine)), "Checking closest point on  parallel segment 2 " << closestPointOnSecondLine);

  x0 = cv::Point3d(5,4,1);
  x1 = cv::Point3d (4,5,1);

  distance = mitk::DistanceBetweenLineAndSegment(P0, u, x0, x1, closestPointOnSecondLine);

  MITK_TEST_CONDITION ( ( fabs (distance - sqrt ( 1.0 )) )  < 1e-6, "Checking distance between line and non parallel line 2 " << distance );
  MITK_TEST_CONDITION( mitk::NearlyEqual (closestPointOnSecondLine,cv::Point3d(4.5,4.5,1.0),1e-6),"Checking closest point on second line " << closestPointOnSecondLine);

  x0 = cv::Point3d(7,4,1);
  x1 = cv::Point3d (6,5,1);

  distance = mitk::DistanceBetweenLineAndSegment(P0, u, x0, x1, closestPointOnSecondLine);

  MITK_TEST_CONDITION ( ( fabs (distance - sqrt ( 1.0 * 1.0 + 0.5 * 0.5 + 0.5 * 0.5 )) )  < 1e-6, "Checking distance between line and non parallel line 3 " << distance );
  MITK_TEST_CONDITION( mitk::NearlyEqual (closestPointOnSecondLine,cv::Point3d(6.0,5.0,1.0),1e-6),"Checking closest point on second line " << closestPointOnSecondLine);

}

void TwoPointsToPLambdaTest ()
{
  //a line through the origin an (1,1,1)
  cv::Point3d P0 (0,0,0);
  cv::Point3d P1 (1,1,1);
  //a line through (1,1,1) and (-3,1,-3)
  cv::Point3d Q0 (1,1,1);
  cv::Point3d Q1 (-3,1,-3);

  cv::Point3d x1;
  cv::Point3d u1;
  cv::Point3d x2;
  cv::Point3d u2;

  std::pair <cv::Point3d, cv::Point3d>  line1 = mitk::TwoPointsToPLambda (std::pair <cv::Point3d, cv::Point3d> ( P0,P1));
  std::pair <cv::Point3d, cv::Point3d>  line2 = mitk::TwoPointsToPLambda (std::pair <cv::Point3d, cv::Point3d> ( Q0,Q1));

  MITK_TEST_CONDITION( mitk::NearlyEqual (line1.first,cv::Point3d(0,0,0),1e-6),"Checking line 1 point " << line1.first);
  MITK_TEST_CONDITION( mitk::NearlyEqual (line1.second,cv::Point3d(-1/sqrt(3.0),-1/sqrt(3.0),-1/sqrt(3.0)),1e-6),"Checking line 1 vector " << line1.second);
  MITK_TEST_CONDITION( mitk::NearlyEqual (line2.first,cv::Point3d(1,1,1),1e-6),"Checking line 2 point " << line2.first);
  MITK_TEST_CONDITION( mitk::NearlyEqual (line2.second,cv::Point3d(4/sqrt(32.0),0,4/sqrt(32.0)),1e-6),"Checking line 2 vector " << line2.second);
}

void RemoveOutliersTest ()
{
  std::vector < cv::Point3d > points;
  unsigned int pointsGone;

  pointsGone= mitk::RemoveOutliers ( points ,
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity(),
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity(),
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity());
  MITK_TEST_CONDITION ( pointsGone == 0 , " Remove points test with empty vector " << pointsGone );

  points.push_back ( cv::Point3d ( std::numeric_limits<double>::infinity () , 0 , 0 ) );
  pointsGone= mitk::RemoveOutliers ( points ,
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity(),
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity(),
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity());
  MITK_TEST_CONDITION ( pointsGone == 0 , " Remove points test with infinite point and infinite limit " << pointsGone );
  pointsGone= mitk::RemoveOutliers ( points ,
      -(std::numeric_limits<double>::infinity()), 10,
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity(),
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity());
  MITK_TEST_CONDITION ( pointsGone == 1 , " Remove points test with infinite point and finite limit " << pointsGone );
  points.push_back ( cv::Point3d ( 0,  std::numeric_limits<double>::quiet_NaN () , 0 ) );
  pointsGone= mitk::RemoveOutliers ( points ,
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity(),
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity(),
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity());
  MITK_TEST_CONDITION ( pointsGone == 0 , " Remove points test with NaN point and infinite limit " << pointsGone );
  pointsGone= mitk::RemoveOutliers ( points ,
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity() ,
      -(std::numeric_limits<double>::infinity()), 10,
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity());
  MITK_TEST_CONDITION ( pointsGone == 0 , " Remove points test with NaN point and finite limit " << pointsGone );
  points.push_back ( cv::Point3d ( 0,  -20.0, 50.0 ) );
  pointsGone= mitk::RemoveOutliers ( points ,
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity() ,
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::quiet_NaN (),
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity());
  MITK_TEST_CONDITION ( pointsGone == 0 , " Remove points test with NaN point and NaN limit " << pointsGone );
  pointsGone= mitk::RemoveOutliers ( points ,
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity() ,
      -(std::numeric_limits<double>::infinity()), 0.0,
      -(std::numeric_limits<double>::infinity()), 40.0);
  MITK_TEST_CONDITION ( pointsGone == 1 , " Remove points test with numeric point and numeric limit " << pointsGone );
  points.push_back ( cv::Point3d ( 0,  -20.0, 50.0 ) );
  pointsGone= mitk::RemoveOutliers ( points ,
      -(std::numeric_limits<double>::infinity()), std::numeric_limits<double>::infinity() ,
      -10.0, 0.0,
      -(std::numeric_limits<double>::infinity()), 100.0);
  MITK_TEST_CONDITION ( pointsGone == 1 , " Remove points test with numeric point and numeric limit " << pointsGone );
}

void FindNearestPointTest ()
{
  mitk::PickedObject p ( "left", 0, 0 , cv::Scalar ( 255, 255 , 255 ));
  std::vector < mitk::PickedObject> classifierPoints;
  mitk::PickedObject c1( "left", 0, 0 , cv::Scalar ( 255, 255, 255 ));
  c1.m_Id = 0;
  c1.m_Points.push_back ( cv::Point3d ( 1.0, 0.0, 0.0 ));
  classifierPoints.push_back ( c1 );
  mitk::PickedObject c2( "left", 0, 0 , cv::Scalar ( 255, 255, 255 ));
  c2.m_Id = 1;
  c2.m_Points.push_back ( cv::Point3d ( 1.0, 1.0, 0.0 ));
  classifierPoints.push_back ( c2 );
  mitk::PickedObject c3( "left", 0, 0 , cv::Scalar ( 255, 255, 255 ));
  c3.m_Id = 2;
  c3.m_Points.push_back ( cv::Point3d ( 0.0, 1.0, 0.0 ));
  classifierPoints.push_back ( c3 );
  mitk::PickedObject c4( "left", 0, 0, cv::Scalar ( 255, 255 , 255 ) );
  c4.m_Id = 3;
  c4.m_Points.push_back ( cv::Point3d ( 0.0, 0.0, 0.0 ));
  classifierPoints.push_back ( c4 );
  mitk::PickedObject c5( "left", 0, 0 , cv::Scalar (255, 255, 255));
  c5.m_Id = 0;
  c5.m_IsLine = true;
  c5.m_Points.push_back ( cv::Point3d ( 0.0, 0.0, 0.0 ));
  c5.m_Points.push_back ( cv::Point3d ( 1.0, 1.0, 0.0 ));
  classifierPoints.push_back ( c5 );

  p.m_Points.push_back ( cv::Point3d ( 0.3, 0.6, 0.0) );
  p.m_Id = -1;

  mitk::PickedObject matched;
  double minRatio = 0 ;
  matched = mitk::FindNearestPickedObject ( p , classifierPoints, &minRatio );

  MITK_TEST_CONDITION ( matched.m_Id == 2 , "Testing matched to point 2, got , " << matched.m_Id );
  MITK_TEST_CONDITION ( ( fabs ( minRatio - 0.6708/0.5000)) < 1e-3  , "Testing min ratio is 1.342 , got  " << minRatio) ;

  p.m_Id = 3;
  matched = mitk::FindNearestPickedObject ( p , classifierPoints, &minRatio );
  MITK_TEST_CONDITION ( matched.m_Id == 3 , "Testing matched to point 3 when point id set, got , " << matched.m_Id );
  MITK_TEST_CONDITION ( boost::math::isinf(minRatio), "Testing min ratio is infinite , got  " << minRatio) ;

  p.m_IsLine = true;
  matched = mitk::FindNearestPickedObject ( p , classifierPoints, &minRatio );
  MITK_TEST_CONDITION ( matched.m_Id == -1 , "Testing matched to nothing (-1) when point id beyond end, got , " << matched.m_Id );
  MITK_TEST_CONDITION ( boost::math::isnan(minRatio), "Testing min ratio is NaN , got  " << minRatio) ;

  p.m_Id = -1;
  matched = mitk::FindNearestPickedObject ( p , classifierPoints, &minRatio );
  MITK_TEST_CONDITION ( matched.m_Id == 0 , "Testing matched to line 0 when point id not set, got , " << matched.m_Id );
  MITK_TEST_CONDITION ( boost::math::isinf(minRatio), "Testing min ratio is inf , got  " << minRatio) ;

  classifierPoints.clear();
  matched = mitk::FindNearestPickedObject ( p , classifierPoints, &minRatio );
  MITK_TEST_CONDITION ( matched.m_Id == -1 , "Testing matched to nothing (-1) when classifier points in empty " << matched.m_Id );
  MITK_TEST_CONDITION ( boost::math::isnan(minRatio), "Testing min ratio is NaN , got  " << minRatio) ;

}

void DistanceBetweenTwoPointsTest()
{
  cv::Point3d p1 ( -1.1, 20.2, 3.0);
  cv::Point3d p2 ( 1.1, 10.4, 3.6);
  cv::Point3d p3 ( 1.1, 10.4, std::numeric_limits<double>::quiet_NaN());
  cv::Point3d p4 ( 1.1, 10.4, std::numeric_limits<double>::infinity());

  cv::Point3d delta;
  double distance;
  distance = mitk::DistanceBetweenTwoPoints ( p1, p2, &delta );
  MITK_TEST_CONDITION ( fabs ( distance - 10.062 ) < 1e-3 , "Testing distance between points = 10.062, got : " << distance );
  MITK_TEST_CONDITION ( mitk::NearlyEqual ( delta, cv::Point3d ( 2.2, -9.8, 0.6 ), 1e-6), "Testing delta, got : " << delta );
  distance = mitk::DistanceBetweenTwoPoints ( p2, p1, &delta );
  MITK_TEST_CONDITION ( fabs ( distance - 10.062 ) < 1e-3 , "Testing distance between points = 10.062, got : " << distance );
  MITK_TEST_CONDITION ( mitk::NearlyEqual ( delta, cv::Point3d ( -2.2, 9.8, -0.6 ), 1e-6) ,"Testing delta, got : " << delta );
  distance = mitk::DistanceBetweenTwoPoints ( p1, p3, &delta );
  MITK_TEST_CONDITION ( boost::math::isnan( distance ), "Testing distance between points is not a number, got : " << distance );
  MITK_TEST_CONDITION ( ! mitk::NearlyEqual ( delta, cv::Point3d (  2.2, -9.8, std::numeric_limits<double>::quiet_NaN()  ), 1e-6), "Testing delta, got : " << delta );
  distance = mitk::DistanceBetweenTwoPoints ( p2, p4, &delta );
  MITK_TEST_CONDITION ( boost::math::isinf( distance ), "Testing distance between points is infinite, got : " << distance );
  MITK_TEST_CONDITION ( ! mitk::NearlyEqual ( delta, cv::Point3d ( 0.0, 0.0, std::numeric_limits<double>::infinity() ), 1e-6), "Testing delta, got : " << delta );
  distance = mitk::DistanceBetweenTwoPoints ( p4, p3, &delta );
  MITK_TEST_CONDITION ( boost::math::isnan( distance ), "Testing distance between points is not a number, got : " << distance );
  MITK_TEST_CONDITION ( ! mitk::NearlyEqual ( delta, cv::Point3d ( 0.0, 0.0, std::numeric_limits<double>::quiet_NaN() ), 1e-6), "Testing delta, got : " << delta );
}

void DistanceBetweenTwoSplinesTest()
{
  cv::Point3d l1 ( -1.0, -1.0, -1.0);
  cv::Point3d l2 ( 0.0, 0.0, 0.0);
  cv::Point3d l3 ( 2.0, 2.0, 1.0);
  cv::Point3d l4 ( 2.0, 2.0, std::numeric_limits<double>::infinity());
  cv::Point3d l5 ( 2.0, 2.0, std::numeric_limits<double>::quiet_NaN());

  cv::Point3d p1 ( 0.0, -0.5, -0.5);
  cv::Point3d p2 ( 1.0, 1.0, 1.0);

  std::vector < cv::Point3d > spline1;
  std::vector < cv::Point3d > spline2;

  spline1.push_back(l1);
  spline1.push_back(l2);
  spline1.push_back(l3);

  spline2.push_back(p1);

  double distance;
  cv::Point3d delta;
  distance = mitk::DistanceBetweenTwoSplines ( spline2, spline1, 1, &delta );
  MITK_TEST_CONDITION ( fabs ( distance - 0.408 ) < 1e-3 , "Testing distance between splines = 0.408, got : " << distance );
  MITK_TEST_CONDITION ( mitk::NearlyEqual ( delta, cv::Point3d ( -0.333, 0.167, 0.167 ), 1e-3), "Testing delta, got : " << delta );
  spline2.push_back(p2);
  distance = mitk::DistanceBetweenTwoSplines ( spline2, spline1, 1, &delta );
  MITK_TEST_CONDITION ( fabs ( distance -  0.440 ) < 1e-3 , "Testing distance between splines = 0.440, got : " << distance );
  MITK_TEST_CONDITION ( mitk::NearlyEqual ( delta, cv::Point3d ( -0.111, 0.139, -0.139 ), 1e-3), "Testing delta, got : " << delta );
  distance = mitk::DistanceBetweenTwoSplines ( spline1, spline2, 1, &delta);
  MITK_TEST_CONDITION ( fabs ( distance -  0.980 ) < 1e-3 , "Testing distance0.139n splines = 0.980, got : " << distance );
  MITK_TEST_CONDITION ( mitk::NearlyEqual ( delta, cv::Point3d ( 0.091, 0.136, -0.197 ), 1e-3), "Testing delta, got : " << delta );

  spline1.push_back(l4);
  distance = mitk::DistanceBetweenTwoSplines ( spline2, spline1, 1, &delta );
  MITK_TEST_CONDITION ( fabs ( distance -  0.440 ) < 1e-3 , "Testing distance between splines = 0.440, got : " << distance );
  MITK_TEST_CONDITION ( mitk::NearlyEqual ( delta, cv::Point3d ( -0.111, 0.139, -0.139 ), 1e-3), "Testing delta, got : " << delta );
  distance = mitk::DistanceBetweenTwoSplines ( spline1, spline2, 1, &delta );
  MITK_TEST_CONDITION ( boost::math::isinf( distance ), "Testing distance between splines is infinite, got : " << distance );
  MITK_TEST_CONDITION ( !mitk::IsNotNaNorInf ( delta ), "Testing delta is inf, got : " << delta );

  spline1.push_back(l5);
  distance = mitk::DistanceBetweenTwoSplines ( spline2, spline1, 1, &delta );
  MITK_TEST_CONDITION ( boost::math::isnan( distance ), "Testing distance between splines is not a number, got : " << distance );
  MITK_TEST_CONDITION ( !mitk::IsNotNaNorInf ( delta ), "Testing delta is inf, got : " << delta );
  distance = mitk::DistanceBetweenTwoSplines ( spline1, spline2, 1, &delta );
  MITK_TEST_CONDITION ( boost::math::isnan( distance ), "Testing distance between splines is not a number, got : " << distance );
  MITK_TEST_CONDITION ( !mitk::IsNotNaNorInf ( delta ), "Testing delta is inf, got : " << delta );

}

int mitkOpenCVMathTests(int argc, char * argv[])
{
  // always start with this!
  MITK_TEST_BEGIN("mitkOpenCVMathTests");

  //MITK_TEST_CONDITION ( ArithmaticTests() , "Testing basic arithmetic");
  //MITK_TEST_CONDITION ( RMSTest(), "Testing RMSError" );
  ArithmaticTests();
  RMSTest();
  FindIntersectTest();
  FindIntersectsTest();
  AngleBetweenLinesTest();
  CheckIfLinesArePerpendicularTest();
  PointInIntervalTest();
  DistanceToLineTest();
  DistanceToLineSegmentTest();
  DotAndCrossProductTest();
  NormTest();
  DistanceBetweenLinesTest();
  DistanceBetweenLineAndSegmentTest();
  TwoPointsToPLambdaTest ();
  RemoveOutliersTest();
  FindNearestPointTest();
  DistanceBetweenTwoPointsTest();
  DistanceBetweenTwoSplinesTest();

  MITK_TEST_END();
}


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

#include <nt2/toolbox/euler/functions/gammaln.hpp>
#include <boost/simd/sdk/math.hpp>
#include <nt2/include/functions/scalar/is_eqz.hpp>
#include <nt2/include/functions/scalar/is_invalid.hpp>
#include <boost/math/special_functions/gamma.hpp>

#include <nt2/include/constants/real.hpp>
#include <nt2/include/constants/digits.hpp>
#include <nt2/sdk/error/policies.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type is double_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::gammaln_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {

    typedef typename meta::strip<A0>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      if ((a0 == Inf<A0>()) || is_eqz(a0) ) return Inf<A0>();
      if ((a0 < Zero<A0>()) || is_invalid(a0)) return Nan<A0>();
    #ifdef BOOST_SIMD_HAS_LGAMMA
      return ::lgamma(a0);
    #else
      return boost::math::lgamma(a0, nt2_policy());
    #endif
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type is single_
/////////////////////////////////////////////////////////////////////////////
#ifdef BOOST_SIMD_HAS_LGAMMAF
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::gammaln_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                            )
  {

    typedef typename meta::strip<A0>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      if ((a0 == Inf<A0>()) || is_eqz(a0) ) return Inf<A0>();
      if ((a0 < Zero<A0>()) || is_invalid(a0)) return Nan<A0>();
      return ::lgammaf(a0);
    }
  };
} }

#endif

/////////////////////////////////////////////////////////////////////////////
// Implementation when type is integer_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::gammaln_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      return nt2::gammaln(result_type(a0));
    }
  };
} }

#endif


/*
@copyright Louis Dionne 2015
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/assert.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <type_traits>
#include <vector>
using namespace boost::hana;

int main() {

{

//! [and_]
BOOST_HANA_CONSTANT_CHECK(and_(true_, true_, true_, true_));
BOOST_HANA_CONSTEXPR_CHECK(!and_(true_, false, true_, true_));
//! [and_]

}{

//! [or_]
BOOST_HANA_CONSTANT_CHECK(or_(false_, false_, true_));
BOOST_HANA_CONSTANT_CHECK(!or_(false_, false_, false_));
//! [or_]

}{

//! [if_]
BOOST_HANA_CONSTEXPR_CHECK(if_(true, 1, 2) == 1);
BOOST_HANA_CONSTEXPR_CHECK(if_(false, 1, 2) == 2);

BOOST_HANA_CONSTEXPR_CHECK(
    if_(true_,
        make<Tuple>('t', 'r', 'u', 'e'),
        make<Tuple>('f', 'a', 'l', 's', 'e')
    )
    ==
    make<Tuple>('t', 'r', 'u', 'e')
);
//! [if_]

}{

//! [not_]
BOOST_HANA_CONSTANT_CHECK(not_(true_) == false_);
BOOST_HANA_CONSTEXPR_CHECK(not_(false) == true);
//! [not_]

}{

//! [heterogeneous_eval_if]
BOOST_HANA_CONSTEXPR_LAMBDA auto safe_make_unsigned = [](auto t) {
    return eval_if(trait<std::is_integral>(t),
        [=](auto id) { return id(template_<std::make_unsigned_t>)(t); },
        always(t)
    );
};

BOOST_HANA_CONSTANT_CHECK(safe_make_unsigned(type<void>) == type<void>);
BOOST_HANA_CONSTANT_CHECK(safe_make_unsigned(type<int>) == type<unsigned int>);
//! [heterogeneous_eval_if]
(void)safe_make_unsigned;

//! [homogeneous_eval_if]
BOOST_HANA_CONSTEXPR_LAMBDA auto safe_divide = [](auto x, auto y) {
    return eval_if(y == 0,
        [=](auto) { return 0; },
        [=](auto id) { return id(x) / y; }
    );
};

BOOST_HANA_CONSTEXPR_CHECK(safe_divide(6, 3) == 2);
BOOST_HANA_CONSTEXPR_CHECK(safe_divide(6, 0) == 0);
//! [homogeneous_eval_if]

}{

//! [homogeneous_while]
std::vector<int> ints;
int final_state = while_(_ < 10, 0, [&](int i) {
    ints.push_back(i);
    return i + 1;
});

// The state is known only at runtime
BOOST_HANA_RUNTIME_CHECK(final_state == 10);

BOOST_HANA_RUNTIME_CHECK(ints == std::vector<int>{0, 1, 2, 3, 4, 5, 6, 7, 8, 9});
//! [homogeneous_while]

}{

//! [heterogeneous_while]
using namespace literals;
std::vector<int> ints;
auto final_state = while_(_ < 10_c, 0_c, [&](auto i) {
    ints.push_back(i);
    return i + 1_c;
});

// The state is known at compile-time
BOOST_HANA_CONSTANT_CHECK(final_state == 10_c);

BOOST_HANA_RUNTIME_CHECK(ints == std::vector<int>{0, 1, 2, 3, 4, 5, 6, 7, 8, 9});
//! [heterogeneous_while]

}{

//! [homogeneous_until]
std::vector<int> ints;
int final_state = until(_ == 10, 0, [&](int i) {
    ints.push_back(i);
    return i + 1;
});

// The state is known only at runtime
BOOST_HANA_RUNTIME_CHECK(final_state == 10);

BOOST_HANA_RUNTIME_CHECK(ints == std::vector<int>{0, 1, 2, 3, 4, 5, 6, 7, 8, 9});
//! [homogeneous_until]

}{

//! [heterogeneous_until]
using namespace literals;
std::vector<int> ints;
auto final_state = until(_ == 10_c, 0_c, [&](auto i) {
    ints.push_back(i);
    return i + 1_c;
});

// The state is known at compile-time
BOOST_HANA_CONSTANT_CHECK(final_state == 10_c);

BOOST_HANA_RUNTIME_CHECK(ints == std::vector<int>{0, 1, 2, 3, 4, 5, 6, 7, 8, 9});
//! [heterogeneous_until]

}

}


// Vector production via a one-loop box diagram
//
// Author:       Daniel Winney (2021)
// Affiliation:  Joint Physics Analysis Center (JPAC)
// Email:        dwinney@iu.edu
// ---------------------------------------------------------------------------

#ifndef _BOX_AMP_
#define _BOX_AMP_

#include "constants.hpp"
#include "amplitudes/amplitude.hpp"
#include "amplitudes/reaction_kinematics.hpp"
#include "box/box_discontinuity.hpp"

#include <boost/math/quadrature/gauss_kronrod.hpp>

#include "Math/GSLIntegrator.h"
#include "Math/GaussLegendreIntegrator.h"
#include "Math/IntegrationTypes.h"
#include "Math/Functor.h"

namespace jpacPhoto
{
    class box_amplitude : public amplitude
    {
        public:
        // Constructor.
        // Need the parent reaction kinematics and pre-set sub-amplitudes
        box_amplitude(reaction_kinematics * xkinem, box_discontinuity * disc, std::string id = "Box Amplitude")
        : amplitude(xkinem, id), _disc(disc)
        {};

        box_amplitude(reaction_kinematics * xkinem, amplitude * left, amplitude * right, std::string id = "Box Amplitude")
        : amplitude(xkinem, id)
        {
            _disc = new box_discontinuity(left, right);
            _needDelete = true;
        };

        // Destructor
        ~box_amplitude()
        {
            if (_needDelete) delete _disc;
        };

        // Setter for max cutoff in dispersion relation
        inline void set_cutoff(double s_cut)
        {
            _s_cut = s_cut;
        };

        // only vector available
        inline std::vector<std::array<int,2>> allowedJP()
        {
            return { {1, -1} };
        };

        // Evaluate the helicity amplitude by dispersing
        std::complex<double> helicity_amplitude(std::array<int, 4> helicities, double s, double t);

        // Override the jpacPhoto::amplitude::integrated_xsection
        double integrated_xsection(double s);

        private:

        // Discontinutity given in terms of the two tree amplitudes
        box_discontinuity * _disc;
        bool _needDelete = false;

        // Integration momentum cutoff. Defaults to 2 GeV (an arbitrary but sensible value)
        double _s_cut = 2.;
    };
};

#endif


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

#include <boost/simd/arithmetic/functions/fast_sqrt.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/fast_rsqrt.hpp>
#include <boost/simd/include/functions/simd/sqrt.hpp>
#include <boost/simd/include/functions/simd/if_else_zero.hpp>
#include <boost/dispatch/attributes.hpp>

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

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return if_else_zero(a0, a0 * fast_rsqrt(a0));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( fast_sqrt_
                                    , tag::cpu_
                                    , (A0)
                                    , (generic_<integer_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return boost::simd::sqrt(a0);
    }
  };
} } }

#endif


#include <complex>
#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <H5Cpp.h>

#include "eigen3-hdf5-sparse.hpp"

#include <gtest/gtest.h>

TEST(SparseMatrix, Double) {
    Eigen::SparseMatrix<double> mat(3, 3), mat2;
    mat.insert(0, 1) = 2.7;
    mat.insert(2, 0) = 82;
    {
        H5::H5File file("test_SparseMatrix_Double.h5", H5F_ACC_TRUNC);
        EigenHDF5::save_sparse(file, "mat", mat);
    }
    {
        H5::H5File file("test_SparseMatrix_Double.h5", H5F_ACC_RDONLY);
        EigenHDF5::load_sparse(file, "mat", mat2);
    }
#ifdef LOGGING
    std::cout << mat2 << std::endl;
#endif
    ASSERT_EQ(Eigen::MatrixXd(mat), Eigen::MatrixXd(mat2));
}

TEST(SparseMatrix, Complex) {
    Eigen::SparseMatrix<std::complex<double> > mat(4, 4), mat2;
    mat.insert(0, 1) = std::complex<double>(2, 4.5);
    mat.insert(1, 2) = std::complex<double>(82, 1);
    {
        H5::H5File file("test_SparseMatrix_Complex.h5", H5F_ACC_TRUNC);
        EigenHDF5::save_sparse(file, "mat", mat);
    }
    {
        H5::H5File file("test_SparseMatrix_Complex.h5", H5F_ACC_RDONLY);
        EigenHDF5::load_sparse(file, "mat", mat2);
    }
#ifdef LOGGING
    std::cout << mat2 << std::endl;
#endif
    ASSERT_EQ(Eigen::MatrixXcd(mat), Eigen::MatrixXcd(mat2));
}


#include <boost/math/special_functions/laguerre.hpp>


#include "util/coordinate.hpp"
#include "util/coordinate_calculation.hpp"
#include "util/trigonometry_table.hpp"
#include "util/web_mercator.hpp"

#include <boost/assert.hpp>

#include <cmath>

#include <limits>
#include <utility>

namespace osrm
{
namespace util
{

namespace coordinate_calculation
{

// Does not project the coordinates!
std::uint64_t squaredEuclideanDistance(const Coordinate lhs, const Coordinate rhs)
{
    const std::uint64_t dx = static_cast<std::int32_t>(lhs.lon - rhs.lon);
    const std::uint64_t dy = static_cast<std::int32_t>(lhs.lat - rhs.lat);

    return dx * dx + dy * dy;
}

double haversineDistance(const Coordinate coordinate_1, const Coordinate coordinate_2)
{
    auto lon1 = static_cast<int>(coordinate_1.lon);
    auto lat1 = static_cast<int>(coordinate_1.lat);
    auto lon2 = static_cast<int>(coordinate_2.lon);
    auto lat2 = static_cast<int>(coordinate_2.lat);
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    const double lt1 = lat1 / COORDINATE_PRECISION;
    const double ln1 = lon1 / COORDINATE_PRECISION;
    const double lt2 = lat2 / COORDINATE_PRECISION;
    const double ln2 = lon2 / COORDINATE_PRECISION;

    const double dlat1 = lt1 * detail::DEGREE_TO_RAD;
    const double dlong1 = ln1 * detail::DEGREE_TO_RAD;
    const double dlat2 = lt2 * detail::DEGREE_TO_RAD;
    const double dlong2 = ln2 * detail::DEGREE_TO_RAD;

    const double dlong = dlong1 - dlong2;
    const double dlat = dlat1 - dlat2;

    const double aharv = std::pow(std::sin(dlat / 2.0), 2.0) +
                         std::cos(dlat1) * std::cos(dlat2) * std::pow(std::sin(dlong / 2.), 2);
    const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
    return detail::EARTH_RADIUS * charv;
}

double greatCircleDistance(const Coordinate coordinate_1, const Coordinate coordinate_2)
{
    auto lon1 = static_cast<int>(coordinate_1.lon);
    auto lat1 = static_cast<int>(coordinate_1.lat);
    auto lon2 = static_cast<int>(coordinate_2.lon);
    auto lat2 = static_cast<int>(coordinate_2.lat);
    BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
    BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());

    const double float_lat1 = (lat1 / COORDINATE_PRECISION) * detail::DEGREE_TO_RAD;
    const double float_lon1 = (lon1 / COORDINATE_PRECISION) * detail::DEGREE_TO_RAD;
    const double float_lat2 = (lat2 / COORDINATE_PRECISION) * detail::DEGREE_TO_RAD;
    const double float_lon2 = (lon2 / COORDINATE_PRECISION) * detail::DEGREE_TO_RAD;

    const double x_value = (float_lon2 - float_lon1) * std::cos((float_lat1 + float_lat2) / 2.0);
    const double y_value = float_lat2 - float_lat1;
    return std::hypot(x_value, y_value) * detail::EARTH_RADIUS;
}

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location,
                             Coordinate &nearest_location,
                             double &ratio)
{
    using namespace coordinate_calculation;

    BOOST_ASSERT(query_location.IsValid());

    FloatCoordinate projected_nearest;
    std::tie(ratio, projected_nearest) =
        projectPointOnSegment(web_mercator::fromWGS84(segment_source),
                              web_mercator::fromWGS84(segment_target),
                              web_mercator::fromWGS84(query_location));
    nearest_location = web_mercator::toWGS84(projected_nearest);

    const double approximate_distance = greatCircleDistance(query_location, nearest_location);
    BOOST_ASSERT(0.0 <= approximate_distance);
    return approximate_distance;
}

double perpendicularDistance(const Coordinate source_coordinate,
                             const Coordinate target_coordinate,
                             const Coordinate query_location)
{
    double ratio;
    Coordinate nearest_location;

    return perpendicularDistance(
        source_coordinate, target_coordinate, query_location, nearest_location, ratio);
}

Coordinate centroid(const Coordinate lhs, const Coordinate rhs)
{
    Coordinate centroid;
    // The coordinates of the midpoints are given by:
    // x = (x1 + x2) /2 and y = (y1 + y2) /2.
    centroid.lon = (lhs.lon + rhs.lon) / FixedLongitude{2};
    centroid.lat = (lhs.lat + rhs.lat) / FixedLatitude{2};
    return centroid;
}

double degToRad(const double degree)
{
    using namespace boost::math::constants;
    return degree * (pi<double>() / 180.0);
}

double radToDeg(const double radian)
{
    using namespace boost::math::constants;
    return radian * (180.0 * (1. / pi<double>()));
}

double bearing(const Coordinate first_coordinate, const Coordinate second_coordinate)
{
    const double lon_diff =
        static_cast<double>(toFloating(second_coordinate.lon - first_coordinate.lon));
    const double lon_delta = degToRad(lon_diff);
    const double lat1 = degToRad(static_cast<double>(toFloating(first_coordinate.lat)));
    const double lat2 = degToRad(static_cast<double>(toFloating(second_coordinate.lat)));
    const double y = std::sin(lon_delta) * std::cos(lat2);
    const double x =
        std::cos(lat1) * std::sin(lat2) - std::sin(lat1) * std::cos(lat2) * std::cos(lon_delta);
    double result = radToDeg(std::atan2(y, x));
    while (result < 0.0)
    {
        result += 360.0;
    }

    while (result >= 360.0)
    {
        result -= 360.0;
    }
    return result;
}

double computeAngle(const Coordinate first, const Coordinate second, const Coordinate third)
{
    using namespace boost::math::constants;
    using namespace coordinate_calculation;

    if (first == second || second == third)
        return 180;

    BOOST_ASSERT(first.IsValid());
    BOOST_ASSERT(second.IsValid());
    BOOST_ASSERT(third.IsValid());

    const double v1x = static_cast<double>(toFloating(first.lon - second.lon));
    const double v1y =
        web_mercator::latToY(toFloating(first.lat)) - web_mercator::latToY(toFloating(second.lat));
    const double v2x = static_cast<double>(toFloating(third.lon - second.lon));
    const double v2y =
        web_mercator::latToY(toFloating(third.lat)) - web_mercator::latToY(toFloating(second.lat));

    double angle = (atan2_lookup(v2y, v2x) - atan2_lookup(v1y, v1x)) * 180. / pi<double>();

    while (angle < 0.)
    {
        angle += 360.;
    }

    BOOST_ASSERT(angle >= 0);
    return angle;
}

boost::optional<Coordinate>
circleCenter(const Coordinate C1, const Coordinate C2, const Coordinate C3)
{
    // free after http://paulbourke.net/geometry/circlesphere/
    // require three distinct points
    if (C1 == C2 || C2 == C3 || C1 == C3)
    {
        return boost::none;
    }

    // define line through c1, c2 and c2,c3
    const double C2C1_lat = static_cast<double>(toFloating(C2.lat - C1.lat)); // yDelta_a
    const double C2C1_lon = static_cast<double>(toFloating(C2.lon - C1.lon)); // xDelta_a
    const double C3C2_lat = static_cast<double>(toFloating(C3.lat - C2.lat)); // yDelta_b
    const double C3C2_lon = static_cast<double>(toFloating(C3.lon - C2.lon)); // xDelta_b

    // check for collinear points in X-Direction / Y-Direction
    if ((std::abs(C2C1_lon) < std::numeric_limits<double>::epsilon() &&
         std::abs(C3C2_lon) < std::numeric_limits<double>::epsilon()) ||
        (std::abs(C2C1_lat) < std::numeric_limits<double>::epsilon() &&
         std::abs(C3C2_lat) < std::numeric_limits<double>::epsilon()))
    {
        return boost::none;
    }
    else if (std::abs(C2C1_lon) < std::numeric_limits<double>::epsilon())
    {
        // vertical line C2C1
        // due to c1.lon == c2.lon && c1.lon != c3.lon we can rearrange this way
        BOOST_ASSERT(std::abs(static_cast<double>(toFloating(C3.lon - C1.lon))) >=
                         std::numeric_limits<double>::epsilon() &&
                     std::abs(static_cast<double>(toFloating(C2.lon - C3.lon))) >=
                         std::numeric_limits<double>::epsilon());
        return circleCenter(C1, C3, C2);
    }
    else if (std::abs(C3C2_lon) < std::numeric_limits<double>::epsilon())
    {
        // vertical line C3C2
        // due to c2.lon == c3.lon && c1.lon != c3.lon we can rearrange this way
        // after rearrangement both deltas will be zero
        BOOST_ASSERT(std::abs(static_cast<double>(toFloating(C1.lon - C2.lon))) >=
                         std::numeric_limits<double>::epsilon() &&
                     std::abs(static_cast<double>(toFloating(C3.lon - C1.lon))) >=
                         std::numeric_limits<double>::epsilon());
        return circleCenter(C2, C1, C3);
    }
    else
    {
        const double C2C1_slope = C2C1_lat / C2C1_lon;
        const double C3C2_slope = C3C2_lat / C3C2_lon;

        if (std::abs(C2C1_slope) < std::numeric_limits<double>::epsilon())
        {
            // Three non-collinear points with C2,C1 on same latitude.
            // Due to the x-values correct, we can swap C3 and C1 to obtain the correct slope value
            return circleCenter(C3, C2, C1);
        }
        // valid slope values for both lines, calculate the center as intersection of the lines

        // can this ever happen?
        if (std::abs(C2C1_slope - C3C2_slope) < std::numeric_limits<double>::epsilon())
            return boost::none;

        const double C1_y = static_cast<double>(toFloating(C1.lat));
        const double C1_x = static_cast<double>(toFloating(C1.lon));
        const double C2_y = static_cast<double>(toFloating(C2.lat));
        const double C2_x = static_cast<double>(toFloating(C2.lon));
        const double C3_y = static_cast<double>(toFloating(C3.lat));
        const double C3_x = static_cast<double>(toFloating(C3.lon));

        const double lon = (C2C1_slope * C3C2_slope * (C1_y - C3_y) + C3C2_slope * (C1_x + C2_x) -
                            C2C1_slope * (C2_x + C3_x)) /
                           (2 * (C3C2_slope - C2C1_slope));
        const double lat = (0.5 * (C1_x + C2_x) - lon) / C2C1_slope + 0.5 * (C1_y + C2_y);
        if (lon < -180.0 || lon > 180.0 || lat < -90.0 || lat > 90.0)
            return boost::none;
        else
            return Coordinate(FloatLongitude{lon}, FloatLatitude{lat});
    }
}

double circleRadius(const Coordinate C1, const Coordinate C2, const Coordinate C3)
{
    // a circle by three points requires thee distinct points
    auto center = circleCenter(C1, C2, C3);
    if (center)
        return haversineDistance(C1, *center);
    else
        return std::numeric_limits<double>::infinity();
}

Coordinate interpolateLinear(double factor, const Coordinate from, const Coordinate to)
{
    BOOST_ASSERT(0 <= factor && factor <= 1.0);

    const auto from_lon = static_cast<std::int32_t>(from.lon);
    const auto from_lat = static_cast<std::int32_t>(from.lat);
    const auto to_lon = static_cast<std::int32_t>(to.lon);
    const auto to_lat = static_cast<std::int32_t>(to.lat);

    FixedLongitude interpolated_lon{
        static_cast<std::int32_t>(from_lon + factor * (to_lon - from_lon))};
    FixedLatitude interpolated_lat{
        static_cast<std::int32_t>(from_lat + factor * (to_lat - from_lat))};

    return {std::move(interpolated_lon), std::move(interpolated_lat)};
}

// compute the signed area of a triangle
double signedArea(const Coordinate first_coordinate,
                  const Coordinate second_coordinate,
                  const Coordinate third_coordinate)
{
    const auto lat_1 = static_cast<double>(toFloating(first_coordinate.lat));
    const auto lon_1 = static_cast<double>(toFloating(first_coordinate.lon));
    const auto lat_2 = static_cast<double>(toFloating(second_coordinate.lat));
    const auto lon_2 = static_cast<double>(toFloating(second_coordinate.lon));
    const auto lat_3 = static_cast<double>(toFloating(third_coordinate.lat));
    const auto lon_3 = static_cast<double>(toFloating(third_coordinate.lon));
    return 0.5 * (-lon_2 * lat_1 + lon_3 * lat_1 + lon_1 * lat_2 - lon_3 * lat_2 - lon_1 * lat_3 +
                  lon_2 * lat_3);
}

// check if a set of three coordinates is given in CCW order
bool isCCW(const Coordinate first_coordinate,
           const Coordinate second_coordinate,
           const Coordinate third_coordinate)
{
    return signedArea(first_coordinate, second_coordinate, third_coordinate) > 0;
}

std::pair<util::Coordinate, util::Coordinate>
leastSquareRegression(const std::vector<util::Coordinate> &coordinates)
{
    BOOST_ASSERT(coordinates.size() >= 2);
    double sum_lon = 0, sum_lat = 0, sum_lon_lat = 0, sum_lon_lon = 0;
    double min_lon = static_cast<double>(toFloating(coordinates.front().lon));
    double max_lon = static_cast<double>(toFloating(coordinates.front().lon));
    for (const auto coord : coordinates)
    {
        min_lon = std::min(min_lon, static_cast<double>(toFloating(coord.lon)));
        max_lon = std::max(max_lon, static_cast<double>(toFloating(coord.lon)));
        sum_lon += static_cast<double>(toFloating(coord.lon));
        sum_lon_lon +=
            static_cast<double>(toFloating(coord.lon)) * static_cast<double>(toFloating(coord.lon));
        sum_lat += static_cast<double>(toFloating(coord.lat));
        sum_lon_lat +=
            static_cast<double>(toFloating(coord.lon)) * static_cast<double>(toFloating(coord.lat));
    }

    const auto dividend = coordinates.size() * sum_lon_lat - sum_lon * sum_lat;
    const auto divisor = coordinates.size() * sum_lon_lon - sum_lon * sum_lon;
    if (std::abs(divisor) < std::numeric_limits<double>::epsilon())
        return std::make_pair(coordinates.front(), coordinates.back());

    // slope of the regression line
    const auto slope = dividend / divisor;
    const auto intercept = (sum_lat - slope * sum_lon) / coordinates.size();

    const auto GetLatAtLon = [intercept,
                              slope](const util::FloatLongitude longitude) -> util::FloatLatitude {
        return {intercept + slope * static_cast<double>((longitude))};
    };

    const util::Coordinate regression_first = {
        toFixed(util::FloatLongitude{min_lon - 1}),
        toFixed(util::FloatLatitude(GetLatAtLon(util::FloatLongitude{min_lon - 1})))};
    const util::Coordinate regression_end = {
        toFixed(util::FloatLongitude{max_lon + 1}),
        toFixed(util::FloatLatitude(GetLatAtLon(util::FloatLongitude{max_lon + 1})))};

    return {regression_first, regression_end};
}

} // ns coordinate_calculation
} // ns util
} // ns osrm


#define EIGEN_USE_BLAS
// #define NDEBUG

#include <Eigen/Dense>

#include "ray.h"
#include "model.h"
#include "radiance.h"
#include <cmath>
#include <cstdlib>
#include <cstdio>
#include <string>
#include <fstream>
#include <iostream>
#include <chrono>
using namespace std;
using namespace Eigen;
using namespace chrono;
using vec = Eigen::Vector3d;

inline double clamp(double x) { return x < 0 ? 0 : x > 1 ? 1 : x; }

inline int toInt(double x) { return int(pow(clamp(x), 1 / 2.2) * 255 + .5); }

inline unsigned char toChar(double x) {//Gamma correction
  return static_cast<unsigned char>(toInt(x));
}

void save_ppm(const std::string &name, const std::vector<vec> &img, const int w, const int h) {
  FILE *f = fopen(name.c_str(), "w");
  fprintf(f, "P6\n%i %i 255\n", w, h);
  for (int i = 0; i < w * h; ++i) {
    for(size_t dim = 0; dim < 3; ++dim){
      fputc(toChar(img[i](dim)), f);
    }
  }
  fclose(f);
}

class Setting {
public:
  int spp, w, h;
  double fovy, aspect;
  Ray cam;
  vec up;

  Setting(const string &file) {
    ifstream fin(file);
    double fovy_angle = 0;
    fin >> spp >> w >> h >> fovy_angle;

    aspect = w * 1.0 / h;
    fovy = tan(M_PI * fovy_angle / 2 / 180.0);

    fin >> cam.origin_(0) >> cam.origin_(1) >> cam.origin_(2);
    vec lookat;
    fin >> lookat(0) >> lookat(1) >> lookat(2);
    cam.dire_ = (lookat - cam.origin_);
    cam.dire_ /= cam.dire_.norm();

    fin >> up(0) >> up(1) >> up(2);
    up /= up.norm();

    double scale;
    fin >> scale;
    cam.dire_ *= scale;

    // up = up - up.dot(cam.dire_) * cam.dire_;

  }
};

int main(int argc, char *argv[]) {
  Eigen::initParallel();
  if (argc < 2) return printf("usage: %s input.obj cam.txt\n", argv[0]);
  const Scene scene(argv[1]);
  const Setting st(argv[2]);
  const int samps = st.spp / 4;// using 4 subpiexel

  cout << "[INFO]>>>>>>>>>>>>>>>>>>>forest<<<<<<<<<<<<<<<<<<" << endl;
  size_t height = 7;
  size_t num_model = scene.models.size();
  cout << "num model is "<< num_model << endl;
  vector<unique_ptr<KD_tree_tris>> KD_forest(num_model);
  #pragma omp parallel for
  for(size_t m_id = 0; m_id < num_model; ++m_id){
    KD_forest[m_id] = unique_ptr<KD_tree_tris>(new KD_tree_tris(scene.models[m_id]->tris_, height, 0, nullptr, true));
  }
  for(const auto& mtl : scene.materials)
    cout << mtl.dissolve <<endl;

  cout << "[INFO]>>>>>>>>>>>>>>>>>>>forest<<<<<<<<<<<<<<<<<<" << endl;

  // camera coordinate
  vec cx = st.cam.dire_.cross(st.up), cy = cx.cross(st.cam.dire_), r;
  cx *=  st.aspect * st.fovy * (1 / cx.norm() );
  cy *=  st.fovy * (1 / cy.norm() );
  cout << "cx is "<< endl << cx << endl << "cy is "<< endl << cy << endl;
  vector<vec> c(st.w * st.h);// color buffer
  auto start = system_clock::now();
#pragma omp parallel for schedule(dynamic, 1) private(r)
  for (int y = 0; y <  st.h; y++) {
    // cout << "y is " << y << endl;
    fprintf(stderr, "\rRendering (%d spp) %5.2f%%", st.spp, 100. * y / (st.h - 1));
    unsigned short Xi[3] = {0, 0, y * y * y};
    for (int x = 0; x < st.w; x++) {
      // cout << "x is " << x << endl;
      for (int sy = 0, i = (st.h - y - 1) * st.w + x; sy < 2; sy++) { // 2x2 subpixel
        for (int sx = 0; sx < 2; sx++, r = vec::Zero()) {
          for (int s = 1; s <= samps; s++) {
            double r1 = 2 * erand48(Xi), dx = r1 < 1 ? sqrt(r1) - 1 : 1 - sqrt(2 - r1);
            double r2 = 2 * erand48(Xi), dy = r2 < 1 ? sqrt(r2) - 1 : 1 - sqrt(2 - r2);
            vec d = cx * (((sx + .5 + dx) / 2 + x) / st.w - .5) +
                    cy * (((sy + .5 + dy) / 2 + y) / st.h - .5) + st.cam.dire_;
            d /= d.norm();
            r = r + radiance(Ray(st.cam.origin_, d), 0, Xi, scene, KD_forest) * (1. / samps);
          }
          // TODO: check whether we need lock here
          vec delt_c;
          delt_c << clamp(r(0)), clamp(r(1)), clamp(r(2));
          c[i] = c[i] + delt_c * .25;
        }
      }
    }
  }
  auto end = system_clock::now();
  auto duration = duration_cast<microseconds>(end - start);
  cout <<  "花费了"
       << double(duration.count()) * microseconds::period::num / microseconds::period::den
       << "秒" << endl;

  fprintf(stderr, "\n");
  save_ppm("image.ppm", c, st.w, st.h);
}


#include <stdlib.h>
#include <assert.h>
#include <time.h>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <vector>
#include <set>
#include <boost/numeric/ublas/matrix.hpp>

#include "../Point.h"
#include "../ANN.h"
#include "../RMSUtils.h"
#include "../IOUtil.h"

using namespace std;

vector<Point> validDirs;
vector<double> validResults;

bool validate_coreset(const vector<Point> &fatP, vector<size_t> &idxs, double epsilon) {
    assert(idxs.size() <= fatP.size());

    if (fatP.empty())
        return true;

    if (idxs.empty())
        return false;

    assert(idxs[0] >= 0 && idxs[idxs.size() - 1] < fatP.size());

    for (size_t i = 0; i < validDirs.size(); i++) {
        double cor_max = fatP[idxs[0]].dotP(validDirs[i]);
        for (size_t j = 1; j < idxs.size(); j++) {
            double corval = fatP[idxs[j]].dotP(validDirs[i]);
            if (corval > cor_max)
                cor_max = corval;
        }

        double ptval = validResults[i];
        if (cor_max < (1 - epsilon) * ptval)
            return false;
    }
    return true;
}

void loss_distribution(const vector<Point> &coreset, const vector<Point> &queries,
                       const vector<double> &results, vector<double> &regret_dist) {
    int n = min(queries.size(), results.size());
    regret_dist.reserve(1000);

    vector<double> regret_ratios(n);
    for (int i = 0; i < n; i++) {
        assert(results[i] > 0);
        double dot_max = 0;
        for (int j = 0; j < coreset.size(); ++j) {
            double dotp = coreset[j].dotP(queries[i]);
            if (dotp > dot_max)
                dot_max = dotp;
        }

        double regret_ratio_i = (results[i] - dot_max) / results[i];
        regret_ratios[i] = regret_ratio_i;
    }

    sort (regret_ratios.begin(), regret_ratios.end());

    for (int i = 1; i < 1000; ++i) {
        regret_dist[i - 1] = regret_ratios[n * i / 1000];
    }
    regret_dist[999] = regret_ratios[n - 1];
}

bool validate_eps_kernel(const vector<Point> &fatP, vector<size_t> &idxs, double epsilon) {
    assert(idxs.size() <= fatP.size());

    if (fatP.empty())
        return true;

    if (idxs.empty())
        return false;

    assert(idxs[0] >= 0 && idxs[idxs.size() - 1] < fatP.size());

    size_t m = validDirs.size() / 2;
    for (size_t i = 0; i < m; i++) {
        double cor_max = fatP[idxs[0]].dotP(validDirs[2 * i]);
        double cor_min = fatP[idxs[0]].dotP(validDirs[2 * i + 1]);
        for (size_t j = 1; j < idxs.size(); j++) {
            double cor_val1 = fatP[idxs[j]].dotP(validDirs[2 * i]);
            double cor_val2 = fatP[idxs[j]].dotP(validDirs[2 * i + 1]);
            if (cor_val1 > cor_max)
                cor_max = cor_val1;
            if (cor_val2 > cor_min)
                cor_min = cor_val2;
        }

        if ((cor_max + cor_min) < (1 - epsilon) * (validResults[2 * i] + validResults[2 * i + 1]))
            return false;
    }
    return true;
}

void coreset_by_sample(ANN *ann_ds, size_t dim, double outer_rad, double delta, size_t deltam, vector<size_t> &idxs) {
    vector<Point> randomP;
    RMSUtils::get_random_sphere_points(outer_rad, dim, deltam, randomP, false);

    vector<size_t> new_idxs;
    ann_ds->getANNs(randomP, delta, new_idxs);

    //figure out all the distinct indices
    set<size_t> unique_idxs;
    for (size_t i = 0; i < idxs.size(); i++)
        unique_idxs.insert(idxs[i]);
    for (size_t i = 0; i < new_idxs.size(); i++)
        unique_idxs.insert(new_idxs[i]);

    idxs.clear();

    set<size_t>::iterator it;
    for (it = unique_idxs.begin(); it != unique_idxs.end(); ++it)
        idxs.push_back((*it));
}

vector<size_t> get_coreset(const vector<Point> &fatP, const int r, const double epsilon, bool &isValid) {
    size_t dim = fatP[0].get_dimension();

    double outer_rad;
    outer_rad = 1 + sqrt(dim);
    double delta = epsilon / (2 * outer_rad);

    vector<size_t> idxs;
    ANN *ann_ds = new ANN();
    ann_ds->insertPts(fatP);

    size_t m = r;
    bool b = false;
    while (!b) {
        coreset_by_sample(ann_ds, dim, outer_rad, delta, m, idxs);
        b = validate_coreset(fatP, idxs, epsilon);
        m += 5;

        if (b && idxs.size() <= r) {
            isValid = true;
            break;
        }

        if (idxs.size() > r) {
            isValid = false;
            break;
        }
    }

    delete ann_ds;
    return idxs;
}

int main(int argc, char **argv) {
    if (argc < 7) {
        cerr << "coreset: Usage " << argv[0] << "<r> <dim> <data_path> <query_path> <valid_path> <output_path>\n";
        exit(1);
    }

    int r = atoi(argv[1]);
    size_t dim = atoi(argv[2]);

    vector<Point> fatP;

    IOUtil::read_input_points(argv[3], dim, fatP);
    IOUtil::read_validate_dirs(argv[4], dim, validDirs);
    IOUtil::read_validate_results(argv[5], validResults);

    ofstream result_file;
    result_file.open(argv[6], ofstream::out | ofstream::app);

    cout << "ann " << argv[3] << " " << fatP.size() << " " << dim << endl;

    result_file << "dataset=" << argv[3] << " r=" << r << "\n" << flush;

    double eps = 0.2;

    vector<size_t> idxs;
    bool isValid;
    while (true) {
        cout << eps << endl;
        idxs = get_coreset(fatP, r, eps, isValid);
        if (isValid) {
            break;
        } else {
            eps += 0.01;
        }
    }

    vector<Point> coreset;
    vector<double> regretDist;
    coreset.reserve(idxs.size());
    for (int i = 0; i < idxs.size(); ++i)
        coreset.push_back(fatP[idxs[i]]);
    loss_distribution(coreset, validDirs, validResults, regretDist);

    cout << regretDist[999] << endl;

    for (int i = 0; i < 1000; ++i)
        result_file << regretDist[i] << "\n";
    result_file.flush();
    result_file.close();
}


#include <iostream>
#include <condition_variable>
#include <mutex>
#include <thread>
#include <string>
#include <cmath>
#include <stdlib.h>
#include <stdio.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <cstdio>
#include <ctime>
#include <Eigen/Core>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

#include <podi_navigation_helpers/couplingModel.h>
#include <podi_navigation_helpers/robot.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>

#include "ros/ros.h"
#include "std_msgs/Int16.h"
#include "std_msgs/Empty.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/TwistStamped.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PolygonStamped.h"
#include "sensor_msgs/Joy.h"

#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>

namespace podi_navigation_helpers {

class CouplingModelNode
{
public:
  CouplingModelNode();
  ~CouplingModelNode();
private:
  ros::NodeHandle private_nh;
	std::thread updateCouplingLoopThread;
  double loop_freq;
	// Coupling Model
	podi_navigation_helpers::CouplingModel* couplingModel_;
	tf::TransformListener tf_;
	ros::Publisher humanPosePub, humanRegionPub, humanPoseInOdomFramePub;
  // Frame names
  std::string baseFrame, odomFrame, mapFrame;
	// Synchronization constants to let the user reset the human position
	ros::Subscriber resetStateSub;
	bool isOldHumanPosSet;
	std::mutex isOldHumanPosSetMutex;
	void updateCouplingLoop();
	void resetStateCB(const std_msgs::Empty &msg);
  double degreeToRad(int degree) const {
    return degree*M_PI/180.0;
  };
  double radToDegree(double rad) const {
    return rad*180.0/M_PI;
  };
};

CouplingModelNode::CouplingModelNode() {
  ros::NodeHandle private_nh("~");

	// Create the Coupling Model
  double handleLength, regionOffset, regionSize, regionAngle, alpha, beta, distanceEqualityThreshold, orientationEqualityThreshold;
  int multiplicativeFactor = 1, intervalsWhenDrawingCurves;
  private_nh.param("handleLengthForCouplingModel", handleLength, double(0.171477));
  private_nh.param("regionOffset", regionOffset, double(0.506822));
  private_nh.param("regionSize", regionSize, double(0.241369));
  private_nh.param("regionAngle", regionAngle, double(39.8526)); // 1.001948 rads
  private_nh.param("alpha", alpha, double(0.676452));
  private_nh.param("beta", beta, double(0.854187));
  private_nh.param("distanceEqualityThreshold", distanceEqualityThreshold, double(0.1));
  private_nh.param("orientationEqualityThreshold", orientationEqualityThreshold, double(1.0));
  private_nh.param("intervals_when_drawing_curves", intervalsWhenDrawingCurves, 10);
  couplingModel_ = new podi_navigation_helpers::CouplingModel(
    handleLength, regionOffset, regionSize, regionAngle, alpha, beta, distanceEqualityThreshold, orientationEqualityThreshold,
    intervalsWhenDrawingCurves, multiplicativeFactor);

  // Create the tf listener and odom helper, which will allow us to get robot pose
	tf::TransformListener tf_(ros::Duration(10));
  private_nh.param("update_human_pose_freq", loop_freq, 5.0); // Hz

  private_nh.param("baseFrame", baseFrame, std::string("/base_link"));
  private_nh.param("odomFrame", odomFrame, std::string("/odom"));
  private_nh.param("mapFrame", mapFrame, std::string("/map"));

  // Publishers for human position, handle position, and human region
	humanPosePub = private_nh.advertise<geometry_msgs::PoseStamped>("human_position", 1);
  humanPoseInOdomFramePub = private_nh.advertise<geometry_msgs::PoseStamped>("human_position_in_odom_frame", 1);
	humanRegionPub = private_nh.advertise<geometry_msgs::PolygonStamped>("human_region", 1);

  // Variables to allow the human pos to be reset to a default
	isOldHumanPosSet = false;
	resetStateSub = private_nh.subscribe("resetState", 1000, &CouplingModelNode::resetStateCB, this);

  // Start the thread that will keep updating the coupling model
	updateCouplingLoopThread = std::thread(&CouplingModelNode::updateCouplingLoop, this);
}

CouplingModelNode::~CouplingModelNode() {
  updateCouplingLoopThread.join();
  if (couplingModel_)
    delete couplingModel_;
}

void CouplingModelNode::updateCouplingLoop() {
  double oldRobotX, oldRobotY, oldRobotTheta;
  double oldHumanX, oldHumanY, oldHumanTheta;
  double newRobotX, newRobotY, newRobotTheta;
  double newHumanX, newHumanY, newHumanTheta;
  // ros::Rate doesn't work across threads, that's why I have to create it in
  // this function and not the initialization function
  std::tuple<double, double, double> humanPos;
  ros::Rate loop_rate(loop_freq);
	while (!ros::isShuttingDown()) {
		// transfrom the 0 vector in \base_link to \map
		tf::Stamped<tf::Pose> base_pose, robot_pose;
    base_pose.frame_id_ = baseFrame;
    base_pose.stamp_ = ros::Time::now();
    base_pose.setOrigin(tf::Vector3(0, 0, 0));
		base_pose.setRotation(tf::Quaternion(0, 0, 0, 1));

		try {
			tf_.waitForTransform(odomFrame, base_pose.frame_id_, base_pose.stamp_, ros::Duration(1.0));
			tf_.transformPose(odomFrame, base_pose, robot_pose);
    } catch (tf::TransformException ex) {
      ROS_WARN("CouplingModelNode::updateCouplingLoop %s", ex.what());
      loop_rate.sleep();
			continue;
    }

		// transform that robot location to x, y, theta
		newRobotX = robot_pose.getOrigin().getX();
		newRobotY = robot_pose.getOrigin().getY(); // mm
		newRobotTheta = radToDegree(tf::getYaw(robot_pose.getRotation()));

		// get the human position, depending on if this is the first iteration or not
    std::unique_lock<std::mutex> isOldHumanPosSetLock(isOldHumanPosSetMutex);
		if (!isOldHumanPosSet) {
      isOldHumanPosSet = true;
			isOldHumanPosSetLock.unlock();
			humanPos = couplingModel_->getInitialHumanPose(newRobotX, newRobotY, newRobotTheta);
		} else {
      isOldHumanPosSetLock.unlock();
			// get the handle and transform that to a human position
			humanPos = couplingModel_->getHumanPose(oldRobotX, oldRobotY, oldRobotTheta, oldHumanX, oldHumanY, oldHumanTheta, newRobotX, newRobotY, newRobotTheta);
		}
    newHumanX = std::get<0>(humanPos);
    newHumanY = std::get<1>(humanPos);
    newHumanTheta = std::get<2>(humanPos);

		// convert the human pose to a msg and publish it
		geometry_msgs::PoseStamped human_pose_msg;
		human_pose_msg.header.frame_id = odomFrame;
    human_pose_msg.header.stamp = robot_pose.stamp_;
    human_pose_msg.pose.position.x = newHumanX;
    human_pose_msg.pose.position.y = newHumanY;
    human_pose_msg.pose.position.z = 0.0;
    human_pose_msg.pose.orientation = tf::createQuaternionMsgFromYaw(degreeToRad(newHumanTheta));
		humanPoseInOdomFramePub.publish(human_pose_msg);

    // also publish the human pose in the map frame
    tf::Stamped<tf::Pose> humanPoseInOdomFrame, humanPoseInMapFrame;
    tf::poseStampedMsgToTF(human_pose_msg, humanPoseInOdomFrame);
    try {
			tf_.waitForTransform(mapFrame, humanPoseInOdomFrame.frame_id_, humanPoseInOdomFrame.stamp_, ros::Duration(1.0));
			tf_.transformPose(mapFrame, humanPoseInOdomFrame, humanPoseInMapFrame);
    } catch (tf::TransformException ex) {
      ROS_WARN("CouplingModelNode::updateCouplingLoop %s", ex.what());
      loop_rate.sleep();
			continue;
    }
    human_pose_msg.header.frame_id = mapFrame;
    human_pose_msg.header.stamp = humanPoseInMapFrame.stamp_;
    human_pose_msg.pose.position.x = humanPoseInMapFrame.getOrigin().getX();
    human_pose_msg.pose.position.y = humanPoseInMapFrame.getOrigin().getY();
    human_pose_msg.pose.position.z = 0.0;
    human_pose_msg.pose.orientation = tf::createQuaternionMsgFromYaw(tf::getYaw(humanPoseInMapFrame.getRotation()));
		humanPosePub.publish(human_pose_msg);

		// get and publish the region
    geometry_msgs::PolygonStamped region = couplingModel_->drawRegion(newRobotX, newRobotY, newRobotTheta, odomFrame);
    humanRegionPub.publish(region);

    oldRobotX = newRobotX;
    oldRobotY = newRobotY;
    oldRobotTheta = newRobotTheta;

		oldHumanX = newHumanX;
    oldHumanY = newHumanY;
    oldHumanTheta = newHumanTheta;

		loop_rate.sleep();
	}
}

void CouplingModelNode::resetStateCB(const std_msgs::Empty &msg) {
	std::unique_lock<std::mutex> isOldHumanPosSetLock(isOldHumanPosSetMutex);
	isOldHumanPosSet = false;
	isOldHumanPosSetLock.unlock();
}

} // end namespace

int main(int argc, char **argv) {
	ros::init(argc, argv, "coupling_model_node");
  podi_navigation_helpers::CouplingModelNode cmn;
	ros::spin();

	return 0;
}


#ifndef ASLAM_BACKEND_EUCLIDEAN_EXPRESSION_NODE_HPP
#define ASLAM_BACKEND_EUCLIDEAN_EXPRESSION_NODE_HPP

#include <aslam/backend/JacobianContainer.hpp>
#include "RotationExpressionNode.hpp"
#include "ScalarExpressionNode.hpp"
#include "TransformationExpressionNode.hpp"
#include "MatrixExpressionNode.hpp"
#include <boost/shared_ptr.hpp>
#include <Eigen/Core>
#include <sm/kinematics/RotationalKinematics.hpp>
#include <aslam/backend/VectorExpressionNode.hpp>

namespace aslam {
  namespace backend {
    template <int D> class VectorExpression;
    class HomogeneousExpressionNode;
    /**
     * \class EuclideanExpressionNode
     * \brief The superclass of all classes representing euclidean points.
     */
    typedef VectorExpressionNode<3>  EuclideanExpressionNode;

    /**
     * \class EuclideanExpressionNodeMultiply
     *
     * \brief A class representing the multiplication of two euclidean matrices.
     *
     */
    class EuclideanExpressionNodeMultiply : public EuclideanExpressionNode
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      EuclideanExpressionNodeMultiply(boost::shared_ptr<RotationExpressionNode> lhs,
                                      boost::shared_ptr<EuclideanExpressionNode> rhs);
      ~EuclideanExpressionNodeMultiply() override;

      void accept(ExpressionNodeVisitor& visitor) override;
    private:
      Eigen::Vector3d evaluateImplementation() const override;
      void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
      void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

      boost::shared_ptr<RotationExpressionNode> _lhs;
      mutable Eigen::Matrix3d _C_lhs;
      boost::shared_ptr<EuclideanExpressionNode> _rhs;
      mutable Eigen::Vector3d _p_rhs;
    };

    // ## New Class for Multiplication with a MatrixExpression
    /**
      * \class EuclideanExpressionNodeMatrixMultiply
      *
      * \brief A class representing the multiplication of a Matrix with a euclidean Vector.
      *
      */
     class EuclideanExpressionNodeMatrixMultiply : public EuclideanExpressionNode
     {
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW

       EuclideanExpressionNodeMatrixMultiply(boost::shared_ptr<MatrixExpressionNode> lhs, boost::shared_ptr<EuclideanExpressionNode> rhs);
       ~EuclideanExpressionNodeMatrixMultiply() override;

     private:
       Eigen::Vector3d evaluateImplementation() const override;
       void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
       void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

       boost::shared_ptr<MatrixExpressionNode> _lhs;
       mutable Eigen::Matrix3d _A_lhs;
       boost::shared_ptr<EuclideanExpressionNode> _rhs;
       mutable Eigen::Vector3d _p_rhs;

     };

    /**
      * \class EuclideanExpressionNodeCrossEuclidean
      *
      * \brief A class representing the cross product of two euclidean expressions.
      *
      */
     class EuclideanExpressionNodeCrossEuclidean : public EuclideanExpressionNode
     {
     public:
       EuclideanExpressionNodeCrossEuclidean(boost::shared_ptr<EuclideanExpressionNode> lhs,
           boost::shared_ptr<EuclideanExpressionNode> rhs);
       ~EuclideanExpressionNodeCrossEuclidean() override;

       void accept(ExpressionNodeVisitor& visitor) override;
     private:
       Eigen::Vector3d evaluateImplementation() const override;
       void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
       void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

       boost::shared_ptr<EuclideanExpressionNode> _lhs;
       boost::shared_ptr<EuclideanExpressionNode> _rhs;
     };

     /**
       * \class EuclideanExpressionNodeAddEuclidean
       *
       * \brief A class representing the addition of two euclidean expressions.
       *
       */
      class EuclideanExpressionNodeAddEuclidean : public EuclideanExpressionNode
      {
      public:
        EuclideanExpressionNodeAddEuclidean(boost::shared_ptr<EuclideanExpressionNode> lhs,
            boost::shared_ptr<EuclideanExpressionNode> rhs);
        ~EuclideanExpressionNodeAddEuclidean() override;

        void accept(ExpressionNodeVisitor& visitor) override;
      private:
        Eigen::Vector3d evaluateImplementation() const override;
        void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
        void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

        boost::shared_ptr<EuclideanExpressionNode> _lhs;
        boost::shared_ptr<EuclideanExpressionNode> _rhs;
      };

      /**
      * \class EuclideanExpressionNodeSubtractEuclidean
      *
      * \brief A class representing the subtraction of two Euclidean expressions.
      *
      */
     class EuclideanExpressionNodeSubtractEuclidean : public EuclideanExpressionNode
     {
     public:
       EuclideanExpressionNodeSubtractEuclidean(boost::shared_ptr<EuclideanExpressionNode> lhs,
           boost::shared_ptr<EuclideanExpressionNode> rhs);
       ~EuclideanExpressionNodeSubtractEuclidean() override;

     private:
       Eigen::Vector3d evaluateImplementation() const override;
       void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
       void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

       boost::shared_ptr<EuclideanExpressionNode> _lhs;
       boost::shared_ptr<EuclideanExpressionNode> _rhs;
     };

    /**
      * \class EuclideanExpressionNodeSubtractVector
      *
      * \brief A class representing the subtraction of a vector from an Euclidean expression.
      *
      */
     class EuclideanExpressionNodeSubtractVector : public EuclideanExpressionNode
     {
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW

       EuclideanExpressionNodeSubtractVector(boost::shared_ptr<EuclideanExpressionNode> lhs,
              const Eigen::Vector3d & rhs);
       ~EuclideanExpressionNodeSubtractVector() override;

     private:
       Eigen::Vector3d evaluateImplementation() const override;
       void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
       void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

       boost::shared_ptr<EuclideanExpressionNode> _lhs;
       Eigen::Vector3d _rhs;
     };

     /**
       * \class EuclideanExpressionNodeSubtractVector
       *
       * \brief A class representing the negated Euclidean expression.
       *
       */
      class EuclideanExpressionNodeNegated : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeNegated(boost::shared_ptr<EuclideanExpressionNode> operand);
        ~EuclideanExpressionNodeNegated() override;

      private:
        Eigen::Vector3d evaluateImplementation() const override;
        void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
        void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

        boost::shared_ptr<EuclideanExpressionNode> _operand;
      };

     /**
       * \class EuclideanExpressionNodeScalarMultiply
       *
       * \brief A class representing the multiplication of a ScalarExpression with an Euclidean expression.
       *
       */
      class EuclideanExpressionNodeScalarMultiply : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeScalarMultiply(boost::shared_ptr<EuclideanExpressionNode> p, boost::shared_ptr<ScalarExpressionNode> s);
        ~EuclideanExpressionNodeScalarMultiply() override;

      private:
        Eigen::Vector3d evaluateImplementation() const override;
        void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
        void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

        boost::shared_ptr<EuclideanExpressionNode> _p;
        boost::shared_ptr<ScalarExpressionNode> _s;
      };

      class EuclideanExpressionNodeTranslation : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeTranslation(boost::shared_ptr<TransformationExpressionNode> operand);
        ~EuclideanExpressionNodeTranslation() override;

      private:
        Eigen::Vector3d evaluateImplementation() const override;
        void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
        void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

        boost::shared_ptr<TransformationExpressionNode> _operand;
      };

  class EuclideanExpressionNodeRotationParameters : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeRotationParameters(boost::shared_ptr<RotationExpressionNode> operand, sm::kinematics::RotationalKinematics::Ptr rk);
        ~EuclideanExpressionNodeRotationParameters() override;

      private:
        Eigen::Vector3d evaluateImplementation() const override;
        void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
        void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

        boost::shared_ptr<RotationExpressionNode> _operand;
        sm::kinematics::RotationalKinematics::Ptr _rk;
      };

  class EuclideanExpressionNodeFromHomogeneous : public EuclideanExpressionNode
      {
      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        EuclideanExpressionNodeFromHomogeneous(boost::shared_ptr<HomogeneousExpressionNode> root);
        ~EuclideanExpressionNodeFromHomogeneous() override;

      private:
        Eigen::Vector3d evaluateImplementation() const override;
        void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
        void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

        boost::shared_ptr<HomogeneousExpressionNode> _root;
      };

    /**
      * \class EuclideanExpressionNodeElementwiseMultiplyEuclidean
      *
      * \brief A class representing the elementwise product of two euclidean expressions.
      *
      */
     class EuclideanExpressionNodeElementwiseMultiplyEuclidean : public EuclideanExpressionNode
     {
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW

       EuclideanExpressionNodeElementwiseMultiplyEuclidean(boost::shared_ptr<EuclideanExpressionNode> lhs,
           boost::shared_ptr<EuclideanExpressionNode> rhs);
       ~EuclideanExpressionNodeElementwiseMultiplyEuclidean() override;

     private:
       Eigen::Vector3d evaluateImplementation() const override;
       void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
       void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

       boost::shared_ptr<EuclideanExpressionNode> _lhs;
       boost::shared_ptr<EuclideanExpressionNode> _rhs;
     };

  } // namespace backend
} // namespace aslam

#endif /* ASLAM_BACKEND_EUCLIDEAN_EXPRESSION_NODE_HPP */