#include <gtest/gtest.h>

#include "helpers.hh"
#include "../CameraModel/cameramodel.hh"
#include "../Spatialiser/spatialiser.hh"
#include "../util/Maybe.hh"
#include "../util/memory.hh"

#include <memory>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;
using namespace bold;
using namespace Eigen;

auto imageWidth = 11;
auto imageHeight = 11;

Spatialiser createTestSpatialiser(double rangeVertical = 90, double rangeHorizontal = 90)
{
  shared_ptr<CameraModel> cameraModel = allocate_aligned_shared<CameraModel>(imageWidth, imageHeight, rangeVertical, rangeHorizontal);
  return Spatialiser(cameraModel);
}

TEST (SpatialiserTests, findGroundPointForPixelLookingStraightDown)
{
  Spatialiser spatialiser = createTestSpatialiser();

  // Look straight down at the ground
  Affine3d agentCameraTr1 = Translation3d(0,0,1) * AngleAxisd(-M_PI/2, Vector3d::UnitX());
  Affine3d agentCameraTr2 = Translation3d(0,0,1.5) * AngleAxisd(-M_PI/2, Vector3d::UnitX());

  // Columns should give camera frame in agent frame:
  // Rotated around x, so unchanged
  // y now points down
  // z points foreward
  EXPECT_TRUE ( VectorsEqual(agentCameraTr1.matrix().col(0).head<3>(), Vector3d(1, 0,  0)) );
  EXPECT_TRUE ( VectorsEqual(agentCameraTr1.matrix().col(1).head<3>(), Vector3d(0, 0, -1)) );
  EXPECT_TRUE ( VectorsEqual(agentCameraTr1.matrix().col(2).head<3>(), Vector3d(0, 1,  0)) );

  Maybe<Vector3d> groundPoint;

  groundPoint = spatialiser.findGroundPointForPixel(Vector2d(5.5, 5.5), agentCameraTr1);
  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(0,0,0), *groundPoint) );

  groundPoint = spatialiser.findGroundPointForPixel(Vector2d(5.5, 5.5), agentCameraTr2);
  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(0,0,0), *groundPoint) );

  groundPoint =
    spatialiser.findGroundPointForPixel(Vector2d(5.5, 11), agentCameraTr1);
  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(0,1.0,0), *groundPoint) );

  groundPoint =
    spatialiser.findGroundPointForPixel(Vector2d(5.5, 11), agentCameraTr2);
  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(0,1.5,0), *groundPoint) );

  groundPoint =
    spatialiser.findGroundPointForPixel(Vector2d(5.5, 0), agentCameraTr1);
  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(0,-1.0,0), *groundPoint) );

  groundPoint =
    spatialiser.findGroundPointForPixel(Vector2d(5.5, 0), agentCameraTr2);
  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(0,-1.5,0), *groundPoint) );

  groundPoint =
    spatialiser.findGroundPointForPixel(Vector2d(11, 5.5), agentCameraTr1);
  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(-1.0,0,0), *groundPoint) );

  groundPoint =
    spatialiser.findGroundPointForPixel(Vector2d(11, 5.5), agentCameraTr2);
  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(-1.5,0,0), *groundPoint) );

  groundPoint =
    spatialiser.findGroundPointForPixel(Vector2d(0, 5.5), agentCameraTr1);
  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(1.0,0,0), *groundPoint) );

  groundPoint =
    spatialiser.findGroundPointForPixel(Vector2d(0, 5.5), agentCameraTr2);
  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(1.5,0,0), *groundPoint) );
}

TEST (SpatialiserTests, findRaisedGroundPointForPixel)
{
  Spatialiser spatialiser = createTestSpatialiser();

  // Look straight down at the ground
  Affine3d agentCameraTr = Translation3d(0,0,2) * AngleAxisd(-M_PI/2, Vector3d::UnitX());

  Maybe<Vector3d> groundPoint;

  groundPoint = spatialiser.findGroundPointForPixel(Vector2d(5.5, 5.5), agentCameraTr, 1.0);
  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(0,0,0), *groundPoint) );

  groundPoint =
    spatialiser.findGroundPointForPixel(Vector2d(5.5, 11), agentCameraTr, 1.0);
  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(0,1.0,0), *groundPoint) );

  groundPoint =
    spatialiser.findGroundPointForPixel(Vector2d(5.5, 0), agentCameraTr, 1.0);
  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(0,-1.0,0), *groundPoint) );

  groundPoint =
    spatialiser.findGroundPointForPixel(Vector2d(11, 5.5), agentCameraTr, 1.0);
  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(-1.0,0,0), *groundPoint) );

  groundPoint =
    spatialiser.findGroundPointForPixel(Vector2d(0, 5.5), agentCameraTr, 1.0);
  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(1.0,0,0), *groundPoint) );

}

TEST (SpatialiserTests, findGroundPointForPixelScanUp)
{
  // When scanning up in the image when looking straight ahead, y should increase
  Spatialiser spatialiser = createTestSpatialiser();
  auto agentCameraTr = Affine3d{Translation3d{0, 0, 1}};

  Maybe<Vector3d> groundPoint;

  groundPoint = spatialiser.findGroundPointForPixel(Vector2d(5.5, 0), agentCameraTr, 0);
  ASSERT_TRUE ( groundPoint.hasValue() );
  double previousY = groundPoint->y();
  for (unsigned y = 1; y <=5; ++y)
  {
    groundPoint = spatialiser.findGroundPointForPixel(Vector2d(5.5, y), agentCameraTr, 0);
    ASSERT_TRUE ( groundPoint.hasValue() );
    ASSERT_TRUE ( groundPoint->y() > previousY ) << "For pixel: " << y << "; y: " << groundPoint->y() << "; prev: " << previousY;
    previousY = groundPoint->y();
  }
}

TEST (SpatialiserTests, findGroundPointForPixelFromCorner)
{
  // NOTE different vertical range -- camera is wider than it is tall
  Spatialiser spatialiser = createTestSpatialiser(/*rangeVertical*/60, /*rangeHorizontal*/90);

  //  ^y _ y==x
  //  |  /|
  //  | /
  //  |/
  //  *---> x

  // From one unit of elevation, look down the y==x axis (rotated around Z by -45 degrees),
  // parallel to the Z plane.
  Affine3d agentCameraTr = Translation3d(0,0,1) * AngleAxisd(-M_PI/4, Vector3d::UnitZ());

  ASSERT_TRUE ( VectorsEqual(Vector3d(sqrt(.5), sqrt(.5), 1), agentCameraTr * Vector3d(0,1,0)) );

  for (int y = 0; y < 4; y++)
  {
    Maybe<Vector3d> groundPoint = spatialiser.findGroundPointForPixel(Vector2d(5.5,y), agentCameraTr);

    ASSERT_TRUE ( groundPoint.hasValue() ) << "No luck with y=" << y;
    EXPECT_NEAR( (*groundPoint).x(), (*groundPoint).y(), 0.001 );
  }

  for (int y = 0; y < 4; y++)
  {
    Maybe<Vector3d> groundPoint = spatialiser.findGroundPointForPixel(Vector2d(0,y), agentCameraTr);

    ASSERT_TRUE ( groundPoint.hasValue() ) << "No luck with y=" << y;
    EXPECT_NEAR( (*groundPoint).y(), 0, 0.001 );
  }

  for (int y = 0; y < 4; y++)
  {
    Maybe<Vector3d> groundPoint = spatialiser.findGroundPointForPixel(Vector2d(imageWidth,y), agentCameraTr);

    ASSERT_TRUE ( groundPoint.hasValue() ) << "No luck with y=" << y;
    EXPECT_NEAR( (*groundPoint).x(), 0, 0.001 );
  }
}

TEST (SpatialiserTests, findGroundPointForPixelLooking45DegreesDown)
{
  Spatialiser spatialiser = createTestSpatialiser();

  // Look 45 deg down at the ground
  Affine3d agentCameraTr = Translation3d(0,0,1) * AngleAxisd(-M_PI/4, Vector3d::UnitX());

  EXPECT_EQ ( 1, agentCameraTr.translation().z() );

  Maybe<Vector3d> groundPoint =
    spatialiser.findGroundPointForPixel(Vector2d(5.5,5.5), agentCameraTr);

  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(0,1,0), *groundPoint) );

  groundPoint =
    spatialiser.findGroundPointForPixel(Vector2d(5.5,0), agentCameraTr);

  ASSERT_TRUE ( groundPoint.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector3d(0,0,0), *groundPoint) );
}

TEST (SpatialiserTests, findGroundPointForPixelEmptyIfSkybound)
{
  Spatialiser spatialiser = createTestSpatialiser();

  // Look straight up at the sky
  Affine3d agentCameraTr = Translation3d(0,0,1) * AngleAxisd(M_PI/2, Vector3d::UnitX());

  Maybe<Vector3d> groundPoint = spatialiser.findGroundPointForPixel(Vector2d(5.5,5.5), agentCameraTr);

  EXPECT_EMPTY ( groundPoint );
}

TEST (SpatialiserTests, findPixelForAgentPointLooking45DegreesDown)
{
  Spatialiser spatialiser = createTestSpatialiser();

  // Look 45 deg down at the ground from one unit above the origin
  Affine3d agentCameraTr = Translation3d(0,0,1) * AngleAxisd(-M_PI/4, Vector3d::UnitX());
  Affine3d cameraAgentTr = agentCameraTr.inverse();

  // Find the pixel for the point one metre along the y-axis from the origin.
  // This should be exactly in the middle of the camera, and therefore the image.
  Maybe<Vector2d> pixel = spatialiser.findPixelForAgentPoint(Vector3d(0,1,0), cameraAgentTr);

  ASSERT_TRUE ( pixel.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector2d(5.5, 5.5), *pixel) );

  pixel = spatialiser.findPixelForAgentPoint(Vector3d(0, 0, 0), cameraAgentTr);

  ASSERT_TRUE ( pixel.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector2d(5.5, 0), *pixel) );
}

TEST (SpatialiserTests, findPixelForAgentPointLookingStraightDown)
{
  // NOTE different vertical range -- camera is wider than it is tall
  Spatialiser spatialiser = createTestSpatialiser(/*rangeVertical*/60, /*rangeHorizontal*/90);

  // Look straight down down at the ground from one unit above the origin
  Affine3d agentCameraTr = Translation3d(0,0,1) * AngleAxisd(-M_PI/2, Vector3d::UnitX());
  Affine3d cameraAgentTr = agentCameraTr.inverse();

  Maybe<Vector2d> pixel;

  // Find the pixel for the origin.
  // This should be on the camera's y-axis, and therefore the middle of the image.
  pixel = spatialiser.findPixelForAgentPoint(Vector3d(0,0,0), cameraAgentTr);
  ASSERT_TRUE ( pixel.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector2d(5.5, 5.5), *pixel) );

  // NOTE that the image is flipped in both x and y axes
  //
  // At the right edge, pixel x = 0
  //        left edge,  pixel x = 11

  double extremeX = tan(spatialiser.getCameraModel()->rangeHorizontalRads() / 2.0);

  ASSERT_NEAR( 1, extremeX, 0.0001 );

  // Find the pixel at the RIGHT of the camera's view
  pixel = spatialiser.findPixelForAgentPoint(Vector3d(extremeX,0,0), cameraAgentTr);
  ASSERT_TRUE ( pixel.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector2d(0, imageHeight/2.0), *pixel) );

  // Find the pixel at the LEFT of the camera's view
  pixel = spatialiser.findPixelForAgentPoint(Vector3d(-extremeX,0,0), cameraAgentTr);
  ASSERT_TRUE ( pixel.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector2d(imageWidth, imageHeight/2.0), *pixel) );

  // Find the pixel halfway from the origin to the RIGHT of the camera's view
  pixel = spatialiser.findPixelForAgentPoint(Vector3d(extremeX/2.0,0,0), cameraAgentTr);
  ASSERT_TRUE ( pixel.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector2d(5.5/2.0, imageHeight/2.0), *pixel) );

  double extremeY = tan(spatialiser.getCameraModel()->rangeVerticalRads() / 2.0);

  ASSERT_NEAR( 0.577350269, extremeY, 0.0001 );

  // Find the pixel at the TOP of the camera's view
  pixel = spatialiser.findPixelForAgentPoint(Vector3d(0,-extremeY,0), cameraAgentTr);
  ASSERT_TRUE ( pixel.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector2d(5.5, 0), *pixel) );

  // Find the pixel at the BOTTOM of the camera's view
  pixel = spatialiser.findPixelForAgentPoint(Vector3d(0,extremeY,0), cameraAgentTr);
  ASSERT_TRUE ( pixel.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector2d(5.5, imageHeight), *pixel) );

  // Test some locations which are outside the viewing frustum, but still mappable to the image plane
  pixel = spatialiser.findPixelForAgentPoint(Vector3d(extremeX*2,0,0), cameraAgentTr);
  ASSERT_TRUE ( pixel.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector2d(5.5 - imageWidth, 5.5), *pixel) );

  pixel = spatialiser.findPixelForAgentPoint(Vector3d(0,extremeY*2,0), cameraAgentTr);
  ASSERT_TRUE ( pixel.hasValue() );
  EXPECT_TRUE ( VectorsEqual(Vector2d(5.5,5.5 + imageHeight), *pixel) );

  // Test some locations which are undefined in the camera's field of view
  EXPECT_EMPTY ( spatialiser.findPixelForAgentPoint(Vector3d(0,0,1), cameraAgentTr) );
  EXPECT_EMPTY ( spatialiser.findPixelForAgentPoint(Vector3d(0,extremeY,1), cameraAgentTr) );
  EXPECT_EMPTY ( spatialiser.findPixelForAgentPoint(Vector3d(extremeX,0,1), cameraAgentTr) );
  EXPECT_EMPTY ( spatialiser.findPixelForAgentPoint(Vector3d(0,extremeY,1.001), cameraAgentTr) );
  EXPECT_EMPTY ( spatialiser.findPixelForAgentPoint(Vector3d(extremeX,0,1.001), cameraAgentTr) );
  EXPECT_EMPTY ( spatialiser.findPixelForAgentPoint(Vector3d(0,0,2), cameraAgentTr) );
}

TEST (SpatialiserTests, findHorizonForColumnSquareCam)
{
  Spatialiser spatialiser = createTestSpatialiser();

  // Look straight ahead
  Affine3d cameraAgentTr(Matrix4d::Identity());

  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(0, cameraAgentTr) );
  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(5, cameraAgentTr) );
  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(10, cameraAgentTr) );

  // Vertical displacement should not matter
  cameraAgentTr = Translation3d(0, 0, -1.0);

  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(0, cameraAgentTr) );
  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(5, cameraAgentTr) );
  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(10, cameraAgentTr) );

  // Look 45 degrees down
  cameraAgentTr =  AngleAxisd(-M_PI/4.0, Vector3d::UnitX()).inverse();

  EXPECT_EQ ( 10, spatialiser.findHorizonForColumn(0, cameraAgentTr) );
  EXPECT_EQ ( 10, spatialiser.findHorizonForColumn(5, cameraAgentTr) );
  EXPECT_EQ ( 10, spatialiser.findHorizonForColumn(10, cameraAgentTr) );

  // Tilted 45 degrees
  cameraAgentTr =  AngleAxisd(-M_PI/4.0, Vector3d::UnitY()).inverse();
  EXPECT_EQ ( 0, spatialiser.findHorizonForColumn(0, cameraAgentTr) );
  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(5, cameraAgentTr) );
  EXPECT_EQ ( 10, spatialiser.findHorizonForColumn(10, cameraAgentTr) );

  // Rotation around z-axis should not matter
  cameraAgentTr = AngleAxisd(M_PI/2.0, Vector3d::UnitZ()).inverse();
  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(0, cameraAgentTr) );
  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(5, cameraAgentTr) );
  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(10, cameraAgentTr) );

  // Rotation around z-axis while leaning forward
  cameraAgentTr = (AngleAxisd(-M_PI/4.0, Vector3d::UnitX()) * AngleAxisd(M_PI/2.0, Vector3d::UnitZ())).inverse();
  EXPECT_EQ ( 10, spatialiser.findHorizonForColumn(0, cameraAgentTr) );
  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(5, cameraAgentTr) );
  EXPECT_EQ ( 0, spatialiser.findHorizonForColumn(10, cameraAgentTr) );
}

TEST (SpatialiserTests, findHorizonForColumnWideCam)
{
  auto imageWidth = 11;
  auto imageHeight = 11;
  auto rangeVertical = 45;
  auto rangeHorizontal = 60;

  shared_ptr<CameraModel> cameraModel = allocate_aligned_shared<CameraModel>(imageWidth, imageHeight, rangeVertical, rangeHorizontal);

  Spatialiser spatialiser(cameraModel);

  // Look straight ahead
  Affine3d cameraAgentTr(Matrix4d::Identity());

  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(0, cameraAgentTr) );
  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(5, cameraAgentTr) );
  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(10, cameraAgentTr) );

  // Vertical displacement should not matter
  cameraAgentTr = Translation3d(0, 0, -1.0);

  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(0, cameraAgentTr) );
  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(5, cameraAgentTr) );
  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(10, cameraAgentTr) );

  // Look 22.5 degrees down
  cameraAgentTr =  AngleAxisd(-M_PI/8.0, Vector3d::UnitX()).inverse();

  EXPECT_EQ ( 10, spatialiser.findHorizonForColumn(0, cameraAgentTr) );
  EXPECT_EQ ( 10, spatialiser.findHorizonForColumn(5, cameraAgentTr) );
  EXPECT_EQ ( 10, spatialiser.findHorizonForColumn(10, cameraAgentTr) );

  // Tilt to have horizon go through corners
  // atan(2/3)
  cameraAgentTr =  AngleAxisd(-0.622, Vector3d::UnitY()).inverse();
  EXPECT_EQ ( 0, spatialiser.findHorizonForColumn(0, cameraAgentTr) );
  EXPECT_EQ ( 5, spatialiser.findHorizonForColumn(5, cameraAgentTr) );
  EXPECT_EQ ( 10, spatialiser.findHorizonForColumn(10, cameraAgentTr) );
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/kinematics/skeleton/skeleton_types.h>
#include <OpenTissue/kinematics/inverse/inverse.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

typedef OpenTissue::math::default_math_types		  			                 math_types;
typedef math_types::value_traits                                         value_traits;
typedef math_types::real_type                                            real_type;
typedef OpenTissue::skeleton::DefaultBoneTraits<math_types>              base_bone_traits;
typedef OpenTissue::kinematics::inverse::BoneTraits<base_bone_traits>    bone_traits;
typedef OpenTissue::skeleton::Types<math_types, bone_traits>             skeleton_types;
typedef skeleton_types::skeleton_type                                    skeleton_type;
typedef skeleton_types::bone_type                                        bone_type;
typedef ublas::vector<real_type>                                         vector_type;
typedef OpenTissue::kinematics::inverse::Chain< skeleton_type >          chain_type;

BOOST_AUTO_TEST_SUITE(opentissue_kinematics_inverse_compute_weighted_diff);

BOOST_AUTO_TEST_CASE(test_cases)
{
  using std::fabs;

  real_type const too_tiny = boost::numeric_cast<real_type>(10e-15);

  // Create some skeleton to test with
  skeleton_type skeleton;
  bone_type * b[4];
  b[0] = skeleton.create_bone();
  b[1] = skeleton.create_bone(b[0]);
  b[2] = skeleton.create_bone(b[1]);
  b[3] = skeleton.create_bone(b[0]);

  // Now we try to set up some testing data
  //
  // Skeleton hierarchy looks like this
  //
  //            |  hinge
  //            b0
  //    slider /  \   ball
  //          b1   b3
  //    ball  |
  //          b2
  //
  b[0]->type() = bone_traits::hinge_type;
  b[1]->type() = bone_traits::slider_type;
  b[2]->type() = bone_traits::ball_type;
  b[3]->type() = bone_traits::ball_type;

  b[0]->u() = math_types::vector3_type( value_traits::zero(), value_traits::zero(), value_traits::one() );
  b[0]->relative().T().clear();
  OpenTissue::kinematics::inverse::ACCESSOR::unsynch_set_theta( *(b[0]),  0u, value_traits::pi() );
  b[0]->box_limits().min_limit(0) = -value_traits::pi();
  b[0]->box_limits().max_limit(0) =  value_traits::pi();

  b[1]->u() = math_types::vector3_type( value_traits::one(), value_traits::zero(), value_traits::zero() );
  b[1]->relative().Q().identity();
  OpenTissue::kinematics::inverse::ACCESSOR::unsynch_set_theta( *(b[1]),  0u, value_traits::one() );
  b[1]->box_limits().min_limit(0) = -value_traits::two();
  b[1]->box_limits().max_limit(0) =  value_traits::two();

  b[2]->relative().T().clear();
  for(size_t j=0u;j<3u;++j)
  {
    OpenTissue::kinematics::inverse::ACCESSOR::unsynch_set_theta( *(b[2]),  j, value_traits::pi_half() );
    b[2]->box_limits().min_limit(j) = -value_traits::pi_quarter();
    b[2]->box_limits().max_limit(j) =  value_traits::pi_quarter();
  }
  b[2]->relative().T().clear();
  for(size_t j=0u;j<3u;++j)
  {
    OpenTissue::kinematics::inverse::ACCESSOR::unsynch_set_theta( *(b[3]),  j, value_traits::zero() );
    b[3]->box_limits().min_limit(j) = -value_traits::pi_half();
    b[3]->box_limits().max_limit(j) =  value_traits::pi_half();
  }

  // we assume that get/set joint parameters work and use these for initializing data.
  vector_type theta;
  OpenTissue::kinematics::inverse::get_joint_parameters( skeleton, theta );
  OpenTissue::kinematics::inverse::set_joint_parameters( skeleton, theta );

  // Create some chains
  std::vector<chain_type> chains;
  chains.resize( 2 );
  chains[0].init( b[0], b[2] );
  chains[1].init( b[0], b[3] );

  // Set some positional goal placements
  chains[0].p_global() =  math_types::vector3_type( - value_traits::two(), value_traits::zero(), value_traits::zero() );
  chains[1].p_global() =  math_types::vector3_type(   value_traits::one(), value_traits::zero(), value_traits::zero() );
  chains[0].p_local().clear();
  chains[1].p_local().clear();
  chains[0].only_position() = true;
  chains[1].only_position() = true;

  // Compute the difference vector
  vector_type delta;
  OpenTissue::kinematics::inverse::compute_weighted_difference( chains.begin(), chains.end(), delta );

  // Test if we received the expected values
  BOOST_CHECK( delta.size() == 6u );
  BOOST_CHECK( fabs(-value_traits::one() - delta(0)) < too_tiny );
  BOOST_CHECK( fabs(delta(1)) < too_tiny );
  BOOST_CHECK( fabs(delta(2)) < too_tiny );
  BOOST_CHECK( fabs(value_traits::one()-delta(3)) < too_tiny );
  BOOST_CHECK( fabs(delta(4)) < too_tiny );
  BOOST_CHECK( fabs(delta(5)) < too_tiny );

  //--- Next we will test a rotation part of goal placement ------
  chains[1].only_position() = false;
  chains[1].x_global() =  math_types::vector3_type(   value_traits::zero(), value_traits::zero(), value_traits::one() );
  chains[1].y_global() =  math_types::vector3_type(   value_traits::zero(), value_traits::one(), value_traits::zero() );
  chains[1].x_local()  =  math_types::vector3_type(   value_traits::one(),  value_traits::zero(), value_traits::zero() );
  chains[1].y_local()  =  math_types::vector3_type(   value_traits::zero(), value_traits::one(), value_traits::zero()  );

  // Set up some controlled relative transformation
  OpenTissue::kinematics::inverse::ACCESSOR::unsynch_set_theta( *(b[3]),  0u, value_traits::zero() );
  OpenTissue::kinematics::inverse::ACCESSOR::unsynch_set_theta( *(b[3]),  1u, value_traits::pi_half() );
  OpenTissue::kinematics::inverse::ACCESSOR::unsynch_set_theta( *(b[3]),  2u, value_traits::pi() );

  // We use get/set- joint paramters to make sure everything is initialized
  OpenTissue::kinematics::inverse::get_joint_parameters( skeleton, theta );
  OpenTissue::kinematics::inverse::set_joint_parameters( skeleton, theta );

  // Compute difference vector
  OpenTissue::kinematics::inverse::compute_weighted_difference( chains.begin(), chains.end(), delta );

  // Test if we have the expected values
  BOOST_CHECK( delta.size() == 12u );
  BOOST_CHECK( fabs(-value_traits::one() - delta(0)) < too_tiny );
  BOOST_CHECK( fabs(delta(1)) < too_tiny );
  BOOST_CHECK( fabs(delta(2)) < too_tiny );
  BOOST_CHECK( fabs(value_traits::one()-delta(3)) < too_tiny );
  BOOST_CHECK( fabs(delta(4)) < too_tiny );
  BOOST_CHECK( fabs(delta(5)) < too_tiny );
  BOOST_CHECK( fabs(delta(6)) < too_tiny );
  BOOST_CHECK( fabs(delta(7)) < too_tiny );
  BOOST_CHECK( fabs(delta(8)) < too_tiny );
  BOOST_CHECK( fabs(delta(9)) < too_tiny );
  BOOST_CHECK( fabs(delta(10)) < too_tiny );
  BOOST_CHECK( fabs(delta(11)) < too_tiny );

}

BOOST_AUTO_TEST_SUITE_END();


#include <iostream>
#include <fstream>
#include <cmath>
#include "CftData.h"
#include "common.h"
#include <boost/math/special_functions/gamma.hpp>

std::ostream& operator<< (std::ostream& out, const CftConfig& config)
{
    out << "{D=" << config.D << ", OperatorNumbers=" << config.OperatorNumbers << '}';
    return out;
}

CftData::CftData(CftConfig config)
{
    this->D = config.D;
    this->opsNumbers = config.OperatorNumbers;
    GenerateRandomData();
}

void CftData::GenerateRandomData()
{
    // clear the data.
    ops.clear();
    opeCoefs.clear();

    // add Identity operator.
    ops.push_back(PrimaryInfo(0, 0, 0));

    int id = 0;
    for (uint i = 0; i < this->opsNumbers.size(); i++) {
        float_type lastDim = .0;
        float_type gap;

        // the unitary bound.
        if (i == 0) {
            gap = this->D / 2.0 - 1;
        } else {
            gap = i + this->D - 2.0;
        }

        for (int j = 0; j < this->opsNumbers[i]; j++) {
            double dim;
            id++;
            // for stress tensor
            if (i == 2 && j == 0) {
                dim = (double)this->D;
            } else {
                // randomly set scaling dimsion for primaries except for that
                // scaling dimesions increase with id.
                dim = lastDim + gap + random(0, .2);
            }

            gap = .8;

            ops.push_back(PrimaryInfo(id, dim, i));
            lastDim = dim;
        }
    }

    // randomly generate OPE coefficients for three scalars operators and two scalars and one spinning operators..
    for (int i = 1; i <= this->opsNumbers[0]; i++) {
        for (int j = i; j <= this->opsNumbers[0]; j++) {
            for (uint k = j; k <= this->ops.size(); k++) {
                SetOpeCoefficient(this->ops[i].Id, this->ops[j].Id, this->ops[k].Id, random(-5.0, 5.0));
            }
        }
    }

    SetFixedCftData();
}

void CftData::SetFixedCftData()
{
    maxPrimaryId = Sum(opsNumbers);
    this->StressTensorId = opsNumbers[0] + opsNumbers[1] + 1;

    // normalize OPE coefficients of two identitical scalars to identity operator or stress tensor.
    // for identity operator the OPE coefficients are 1.
    // for stress tensor, it is -d * Dim/(d-1)  * 1/S_d, where S_d is is the volumn of the unit sphere S^(d-1).
    float_type Pi = acos(-1.0);
    for (int i = 0; i <= opsNumbers[0]; i++) {
        SetOpeCoefficient(i, i, 0, 1.0);
        if (i > 0) {
            double coef = -D * GetPrimaryDim(i) / (D - 1);
            double sd = 2 * pow(Pi, D/2.0) / boost::math::tgamma<float_type>(D/2.0);
            SetOpeCoefficient(i, i, StressTensorId, coef / sd);
        }

        for (int j = i + 1; j <= opsNumbers[0]; j++) {
            SetOpeCoefficient(i, j, 0, 0.0);
            SetOpeCoefficient(i, j, StressTensorId, 0.0);
        }
    }
}

PrimaryInfo CftData::GetPrimaryInfo(int id) const
{
    return this->ops[id];
}

void CftData::SetPrimaryDim(int id, double dim)
{
    this->ops[id].Dim = dim;
}

int CftData::PrimaryNumber(int spin) const
{
    return this->opsNumbers[spin];
}

float_type CftData::GetOpeCoefficient(int id1, int id2, int id3)
{
    return this->opeCoefs[OpeCoefficientKey(id1, id2, id3)];
}

float_type CftData::GetOpeCoefficient(OpeCoefficientKey& key)
{
    return this->opeCoefs[key];
}

void CftData::SetOpeCoefficient(int id1, int id2, int id3, float_type coef)
{
    this->opeCoefs[OpeCoefficientKey(id1, id2, id3)] = coef;
}

void CftData::SetOpeCoefficient(OpeCoefficientKey& key, float_type coef)
{
    this->opeCoefs[key] = coef;
}

void CftData::Output(vector<int> ops)
{
    std::cout << "Dimensions of operators" << std::endl;
    for (uint i = 0; i < ops.size(); i++) {
        std::cout << ops[i] << ":\t" << GetPrimaryDim(ops[i]) << std::endl;
    }

    std::cout << std::endl << "OPE coefficients: " << std::endl;

    int count = 0;

    for (uint i = 0; i < ops.size(); i++) {
        for (uint j = i; j < ops.size(); j++) {
            for (uint k = j; k < ops.size(); k++) {
                std::cout << '(' << ops[i] << ',' << ops[j] << ',' << ops[k] << "): ";
                std::cout << GetOpeCoefficient(ops[i], ops[j], ops[k]) << "\t";
                count++;
                if (count % 5 == 0) std::cout << endl;
            }
        }
    }
    std::cout << std::endl;
}

void CftData::Save(string file)
{
    ofstream out(file);
    out << this->D << ' ' << this->opsNumbers.size() << std::endl;

    out << "OperatorNubmers:" << std::endl;
    for (uint i = 0; i < this->opsNumbers.size(); i++) {
        if (i > 0) out << ' ';
        out << opsNumbers[i];
    }
    out << std::endl;

    out << "ScalingDimensions:" << std::endl;
    for (uint i = 1; i < ops.size(); i++) {
        if (i > 1) out << ' ';
        out << setprecision(15) << ops[i].Dim;
    }
    out << std::endl;

    out << "OpeCoefficients:" << std::endl;
    for (int i = 1; i <= this->opsNumbers[0]; i++) {
        for (int j = i; j <= this->opsNumbers[0]; j++) {
            for (uint k = j; k < this->ops.size(); k++) {
                if (k == StressTensorId) continue;
                out << i << ' ' << j << ' ' << k << ' ' << setprecision(15) << GetOpeCoefficient(i, j, k) << std::endl;
            }
        }
    }

    out.close();
}

void CftData::LoadFromFile(string file)
{
    ifstream in(file);
    int spinNumber;
    string buf;
    in >> this->D >> spinNumber;
    this->opsNumbers.resize(spinNumber, 0);

    in >> buf;
    for (int i = 0; i < spinNumber; i++) {
        in >> opsNumbers[i];
    }

    in >> buf;

    int id = 0;
    float_type dim;
    ops.clear();
    ops.push_back(PrimaryInfo(id, 0.0, 0));
    for (int spin = 0; spin < spinNumber; spin++) {
        for (int i = 0; i < opsNumbers[spin]; i++) {
            in >> dim;
            ops.push_back(PrimaryInfo(++id, dim, spin));
        }
    }

    in >> buf;

    int op1, op2, op3;
    float_type coef;
    while(!in.eof()) {
        in >> op1 >> op2 >> op3 >> coef;
        if (!in.good()) break;
        SetOpeCoefficient(op1, op2, op3, coef);
    }

    in.close();

    SetFixedCftData();
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_REALPOW_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REALPOW_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-exponential
    Function object implementing realpow capabilities

    Returns power function,  but
    asserts if the result is to be complex.

    @par Semantic:

    For every parameters of floating type T:

    @code
    T r = realpow(x,y);
    @endcode

    is similar to:

    @code
    T r = pow(x, y);
    @endcode

  **/
  const boost::dispatch::functor<tag::realpow_> realpow = {};
} }
#endif

#include <boost/simd/function/scalar/realpow.hpp>
#include <boost/simd/function/simd/realpow.hpp>

#endif


/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 *     its contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Feb 3, 2015
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

/**
 * @file implementation/EquidistantDistortion.hpp
 * @brief Header implementation file for the EquidistantDistortion class.
 * @author Stefan Leutenegger
 */

#include <Eigen/LU>
#include <iostream>

/// \brief okvis Main namespace of this package.
namespace vio {
/// \brief cameras Namespace for camera-related functionality.
namespace cameras {
    int FTR_UNDIST_DL_MAX_ITERATIONS              = 10;
    float FTR_UNDIST_DL_RADIUS_INITIAL              = 1.0f;      // 1.0^2
    float FTR_UNDIST_DL_RADIUS_MIN                  = 1.0e-10f;  // 0.00001^2
    float FTR_UNDIST_DL_RADIUS_MAX                  = 1.0e4f;    // 100.0^2
    float FTR_UNDIST_DL_RADIUS_FACTOR_INCREASE      = 9.0f;      // 3.0^2
    float FTR_UNDIST_DL_RADIUS_FACTOR_DECREASE      = 0.25f;     // 0.5^2
    float FTR_UNDIST_DL_GAIN_RATIO_MIN              = 0.25f;
    float FTR_UNDIST_DL_GAIN_RATIO_MAX              = 0.75f;
// The default constructor with all zero ki
EquidistantDistortion::EquidistantDistortion()
        : k1_(0.0),
          k2_(0.0),
          k3_(0.0),
          k4_(0.0)
{
    parameters_.setZero();
}

// Constructor initialising ki
EquidistantDistortion::EquidistantDistortion(float k1, float k2, float k3,
                                             float k4)
{
    parameters_[0] = k1;
    parameters_[1] = k2;
    parameters_[2] = k3;
    parameters_[3] = k4;
    k1_ = k1;
    k2_ = k2;
    k3_ = k3;
    k4_ = k4;
}

bool EquidistantDistortion::setParameters(const Eigen::VectorXd & parameters)
{
    if (parameters.cols() != NumDistortionIntrinsics) {
        return false;
    }
    parameters_ = parameters.cast<float>();;
    k1_ = parameters[0];
    k2_ = parameters[1];
    k3_ = parameters[2];
    k4_ = parameters[3];
    return true;
}

bool EquidistantDistortion::distort(const Eigen::Vector2d & pointUndistorted,
                                    Eigen::Vector2d * pointDistorted) const
{
    // distortion only:
    const float u0 = pointUndistorted[0];
    const float u1 = pointUndistorted[1];
    const float r = sqrt(u0 * u0 + u1 * u1);
    const float theta = atan(r);
    const float theta2 = theta * theta;
    const float theta4 = theta2 * theta2;
    const float theta6 = theta4 * theta2;
    const float theta8 = theta4 * theta4;
    const float thetad = theta
                          * (1 + k1_ * theta2 + k2_ * theta4 + k3_ * theta6 + k4_ * theta8);

    const float scaling = (r > 1e-8) ? thetad / r : 1.0;
    (*pointDistorted)[0] = scaling * u0;
    (*pointDistorted)[1] = scaling * u1;
    return true;
}

bool EquidistantDistortion::distort(const Eigen::Vector2f & pointUndistorted,
                                    Eigen::Vector2f * pointDistorted) const
{
    // distortion only:
    const float u0 = pointUndistorted[0];
    const float u1 = pointUndistorted[1];
    const float r = sqrt(u0 * u0 + u1 * u1);
    const float theta = atan(r);
    const float theta2 = theta * theta;
    const float theta4 = theta2 * theta2;
    const float theta6 = theta4 * theta2;
    const float theta8 = theta4 * theta4;
    const float thetad = theta
                         * (1 + k1_ * theta2 + k2_ * theta4 + k3_ * theta6 + k4_ * theta8);

    const float scaling = (r > 1e-8) ? thetad / r : 1.0;
    (*pointDistorted)[0] = scaling * u0;
    (*pointDistorted)[1] = scaling * u1;
    return true;
}

bool EquidistantDistortion::distort(const Eigen::Vector2d & pointUndistorted,
                                    Eigen::Vector2d * pointDistorted,
                                    Eigen::Matrix2d * pointJacobian,
                                    Eigen::Matrix2Xd * parameterJacobian) const
{
    // distortion first:
    const float u0 = pointUndistorted[0];
    const float u1 = pointUndistorted[1];
    const float r = sqrt(u0 * u0 + u1 * u1);
    const float theta = atan(r);
    const float theta2 = theta * theta;
    const float theta4 = theta2 * theta2;
    const float theta6 = theta4 * theta2;
    const float theta8 = theta4 * theta4;
    const float thetad = theta
                          * (1 + k1_ * theta2 + k2_ * theta4 + k3_ * theta6 + k4_ * theta8);

    const float scaling = (r > 1e-8) ? thetad / r : 1.0;
    (*pointDistorted)[0] = scaling * u0;
    (*pointDistorted)[1] = scaling * u1;

    Eigen::Matrix2d & J = *pointJacobian;
    if (r > 1e-8) {
        // mostly matlab generated...
        float t2;
        float t3;
        float t4;
        float t6;
        float t7;
        float t8;
        float t9;
        float t11;
        float t17;
        float t18;
        float t19;
        float t20;
        float t25;

        t2 = u0 * u0;
        t3 = u1 * u1;
        t4 = t2 + t3;
        t6 = atan(sqrt(t4));
        t7 = t6 * t6;
        t8 = 1.0 / sqrt(t4);
        t9 = t7 * t7;
        t11 = 1.0 / ((t2 + t3) + 1.0);
        t17 = (((k1_ * t7 + k2_ * t9) + k3_ * t7 * t9) + k4_ * (t9 * t9)) + 1.0;
        t18 = 1.0 / t4;
        t19 = 1.0 / sqrt(t4 * t4 * t4);
        t20 = t6 * t8 * t17;
        t25 = ((k2_ * t6 * t7 * t8 * t11 * u1 * 4.0
                + k3_ * t6 * t8 * t9 * t11 * u1 * 6.0)
               + k4_ * t6 * t7 * t8 * t9 * t11 * u1 * 8.0)
              + k1_ * t6 * t8 * t11 * u1 * 2.0;
        t4 = ((k2_ * t6 * t7 * t8 * t11 * u0 * 4.0
               + k3_ * t6 * t8 * t9 * t11 * u0 * 6.0)
              + k4_ * t6 * t7 * t8 * t9 * t11 * u0 * 8.0)
             + k1_ * t6 * t8 * t11 * u0 * 2.0;
        t7 = t11 * t17 * t18 * u0 * u1;
        J(0, 1) = (t7 + t6 * t8 * t25 * u0) - t6 * t17 * t19 * u0 * u1;
        J(1, 1) = ((t20 - t3 * t6 * t17 * t19) + t3 * t11 * t17 * t18)
                  + t6 * t8 * t25 * u1;
        J(0, 0) = ((t20 - t2 * t6 * t17 * t19) + t2 * t11 * t17 * t18)
                  + t6 * t8 * t4 * u0;
        J(1, 0) = (t7 + t6 * t8 * t4 * u1) - t6 * t17 * t19 * u0 * u1;

        if (parameterJacobian) {
            Eigen::Matrix2Xd & Ji = *parameterJacobian;
            Ji.resize(2,NumDistortionIntrinsics);
            // mostly matlab generated...
            float t6;
            float t2;
            float t3;
            float t8;
            float t10;

            t6 = u0 * u0 + u1 * u1;
            t2 = atan(sqrt(t6));
            t3 = t2 * t2;
            t8 = t3 * t3;
            t6 = 1.0 / sqrt(t6);
            t10 = t8 * t8;
            Ji(0, 0) = t2 * t3 * t6 * u0;
            Ji(1, 0) = t2 * t3 * t6 * u1;
            Ji(0, 1) = t2 * t8 * t6 * u0;
            Ji(1, 1) = t2 * t8 * t6 * u1;
            Ji(0, 2) = t2 * t3 * t8 * t6 * u0;
            Ji(1, 2) = t2 * t3 * t8 * t6 * u1;
            Ji(0, 3) = t2 * t6 * t10 * u0;
            Ji(1, 3) = t2 * t6 * t10 * u1;

        }
    } else {
        // handle limit case for [u0,u1]->0
        if (parameterJacobian) {
            parameterJacobian->resize(2,NumDistortionIntrinsics);
            parameterJacobian->setZero();
        }
        J.setIdentity();
    }

    return true;
}

bool EquidistantDistortion::distort(const Eigen::Vector2f & pointUndistorted,
                                    Eigen::Vector2f* pointDistorted,
                                    Eigen::Matrix2f * pointJacobian,
                                    Eigen::Matrix2Xf * parameterJacobian) const
{

    ////针对k1,k2,k2,k3的等距投影模型, r^2 = x^2 + y^2
    ////残差2*1 归一化坐标的2维： min F(x,y) = 0.5*r(x,y)^2 约束： x^2+y^2 <=
    ///      r.x = x*(theta*(1 + k1*theta^2 + k2*theta^4 + k3 *theta^6 + k4*theta^8)/r) - x0
    ////     r.x = y*(theta*(1 + k1*theta^2 + k2*theta^4 + k3 *theta^6 + k4*theta^8)/r) - y0
    // distortion first:
    const float u0 = pointUndistorted[0];//x
    const float u1 = pointUndistorted[1];//y
    const float r = sqrt(u0 * u0 + u1 * u1);//r = sqrt(x^2 + y^2)
    const float theta = atan(r);
    const float theta2 = theta * theta;
    const float theta4 = theta2 * theta2;
    const float theta6 = theta4 * theta2;
    const float theta8 = theta4 * theta4;
    const float thetad = theta
                         * (1 + k1_ * theta2 + k2_ * theta4 + k3_ * theta6 + k4_ * theta8);

    const float scaling = (r > 1e-8) ? thetad / r : 1.0;
    (*pointDistorted)[0] = scaling * u0;
    (*pointDistorted)[1] = scaling * u1;

    Eigen::Matrix2f & J = *pointJacobian;
    if (r > 1e-8) {
        // mostly matlab generated...
        float t2;//x^2
        float t3;//y^2
        float t4;//r^2 = x^2+y^2
        float t6;//theta
        float t7;//theta^2
        float t8;// 1/r
        float t9;//theta^4
        float t11;// 1 / ((x^2+y^2) + 1.0);
        float t17;// (((k1_ * theta^2 + k2_ * theta^4) + k3_ * t7 * t9) + k4_ * (t9 * t9)) + 1.0
        float t18;
        float t19;
        float t20;
        float t25;

        t2 = u0 * u0;
        t3 = u1 * u1;
        t4 = t2 + t3;
        t6 = atan(sqrt(t4));
        t7 = t6 * t6;
        t8 = 1.0 / sqrt(t4);
        t9 = t7 * t7;
        t11 = 1.0 / ((t2 + t3) + 1.0);
        t17 = (((k1_ * t7 + k2_ * t9) + k3_ * t7 * t9) + k4_ * (t9 * t9)) + 1.0;
        t18 = 1.0 / t4;
        t19 = 1.0 / sqrt(t4 * t4 * t4);
        t20 = t6 * t8 * t17;
        t25 = ((k2_ * t6 * t7 * t8 * t11 * u1 * 4.0
                + k3_ * t6 * t8 * t9 * t11 * u1 * 6.0)
               + k4_ * t6 * t7 * t8 * t9 * t11 * u1 * 8.0)
              + k1_ * t6 * t8 * t11 * u1 * 2.0;
        t4 = ((k2_ * t6 * t7 * t8 * t11 * u0 * 4.0
               + k3_ * t6 * t8 * t9 * t11 * u0 * 6.0)
              + k4_ * t6 * t7 * t8 * t9 * t11 * u0 * 8.0)
             + k1_ * t6 * t8 * t11 * u0 * 2.0;
        t7 = t11 * t17 * t18 * u0 * u1;
        J(0, 1) = (t7 + t6 * t8 * t25 * u0) - t6 * t17 * t19 * u0 * u1;
        J(1, 1) = ((t20 - t3 * t6 * t17 * t19) + t3 * t11 * t17 * t18)
                  + t6 * t8 * t25 * u1;
        J(0, 0) = ((t20 - t2 * t6 * t17 * t19) + t2 * t11 * t17 * t18)
                  + t6 * t8 * t4 * u0;
        J(1, 0) = (t7 + t6 * t8 * t4 * u1) - t6 * t17 * t19 * u0 * u1;

        if (parameterJacobian) {
            Eigen::Matrix2Xf & Ji = *parameterJacobian;
            Ji.resize(2,NumDistortionIntrinsics);
            // mostly matlab generated...
            float t6;
            float t2;
            float t3;
            float t8;
            float t10;

            t6 = u0 * u0 + u1 * u1;
            t2 = atan(sqrt(t6));
            t3 = t2 * t2;
            t8 = t3 * t3;
            t6 = 1.0 / sqrt(t6);
            t10 = t8 * t8;
            Ji(0, 0) = t2 * t3 * t6 * u0;
            Ji(1, 0) = t2 * t3 * t6 * u1;
            Ji(0, 1) = t2 * t8 * t6 * u0;
            Ji(1, 1) = t2 * t8 * t6 * u1;
            Ji(0, 2) = t2 * t3 * t8 * t6 * u0;
            Ji(1, 2) = t2 * t3 * t8 * t6 * u1;
            Ji(0, 3) = t2 * t6 * t10 * u0;
            Ji(1, 3) = t2 * t6 * t10 * u1;

        }
    } else {
        // handle limit case for [u0,u1]->0
        if (parameterJacobian) {
            parameterJacobian->resize(2,NumDistortionIntrinsics);
            parameterJacobian->setZero();
        }
        J.setIdentity();
    }

    return true;
}

bool EquidistantDistortion::distortWithExternalParameters(
        const Eigen::Vector2d & pointUndistorted,
        const Eigen::VectorXd & parameters, Eigen::Vector2d * pointDistorted,
        Eigen::Matrix2d * pointJacobian, Eigen::Matrix2Xd * parameterJacobian) const
{
    // decompose parameters

    const float k1 = parameters[0];
    const float k2 = parameters[1];
    const float k3 = parameters[2];
    const float k4 = parameters[3];
    // distortion first:
    const float u0 = pointUndistorted[0];
    const float u1 = pointUndistorted[1];
    const float r = sqrt(u0 * u0 + u1 * u1);
    const float theta = atan(r);
    const float theta2 = theta * theta;
    const float theta4 = theta2 * theta2;
    const float theta6 = theta4 * theta2;
    const float theta8 = theta4 * theta4;
    const float thetad = theta
                          * (1 + k1 * theta2 + k2 * theta4 + k3 * theta6 + k4 * theta8);

    const float scaling = (r > 1e-8) ? thetad / r : 1.0;
    (*pointDistorted)[0] = scaling * u0;
    (*pointDistorted)[1] = scaling * u1;

    Eigen::Matrix2d & J = *pointJacobian;
    if (r > 1e-8) {
        // mostly matlab generated...
        float t2;
        float t3;
        float t4;
        float t6;
        float t7;
        float t8;
        float t9;
        float t11;
        float t17;
        float t18;
        float t19;
        float t20;
        float t25;

        t2 = u0 * u0;
        t3 = u1 * u1;
        t4 = t2 + t3;
        t6 = atan(sqrt(t4));
        t7 = t6 * t6;
        t8 = 1.0 / sqrt(t4);
        t9 = t7 * t7;
        t11 = 1.0 / ((t2 + t3) + 1.0);
        t17 = (((k1 * t7 + k2 * t9) + k3 * t7 * t9) + k4 * (t9 * t9)) + 1.0;
        t18 = 1.0 / t4;
        t19 = 1.0 / sqrt(t4 * t4 * t4);
        t20 = t6 * t8 * t17;
        t25 = ((k2 * t6 * t7 * t8 * t11 * u1 * 4.0
                + k3 * t6 * t8 * t9 * t11 * u1 * 6.0)
               + k4 * t6 * t7 * t8 * t9 * t11 * u1 * 8.0)
              + k1 * t6 * t8 * t11 * u1 * 2.0;
        t4 = ((k2 * t6 * t7 * t8 * t11 * u0 * 4.0
               + k3 * t6 * t8 * t9 * t11 * u0 * 6.0)
              + k4 * t6 * t7 * t8 * t9 * t11 * u0 * 8.0)
             + k1 * t6 * t8 * t11 * u0 * 2.0;
        t7 = t11 * t17 * t18 * u0 * u1;
        J(0, 0) = (t7 + t6 * t8 * t25 * u0) - t6 * t17 * t19 * u0 * u1;
        J(1, 0) = ((t20 - t3 * t6 * t17 * t19) + t3 * t11 * t17 * t18)
                  + t6 * t8 * t25 * u1;
        J(0, 1) = ((t20 - t2 * t6 * t17 * t19) + t2 * t11 * t17 * t18)
                  + t6 * t8 * t4 * u0;
        J(1, 1) = (t7 + t6 * t8 * t4 * u1) - t6 * t17 * t19 * u0 * u1;
        if (parameterJacobian) {
            Eigen::Matrix2Xd & Ji = *parameterJacobian;
            Ji.resize(2,NumDistortionIntrinsics);
            // mostly matlab generated...
            float t6;
            float t2;
            float t3;
            float t8;
            float t10;

            t6 = u0 * u0 + u1 * u1;
            t2 = atan(sqrt(t6));
            t3 = t2 * t2;
            t8 = t3 * t3;
            t6 = 1.0 / sqrt(t6);
            t10 = t8 * t8;
            Ji(0, 0) = t2 * t3 * t6 * u0;
            Ji(1, 0) = t2 * t3 * t6 * u1;
            Ji(0, 1) = t2 * t8 * t6 * u0;
            Ji(1, 1) = t2 * t8 * t6 * u1;
            Ji(0, 2) = t2 * t3 * t8 * t6 * u0;
            Ji(1, 2) = t2 * t3 * t8 * t6 * u1;
            Ji(0, 3) = t2 * t6 * t10 * u0;
            Ji(1, 3) = t2 * t6 * t10 * u1;

        }
    } else {
        // handle limit case for [u0,u1]->0
        if (parameterJacobian) {
            parameterJacobian->resize(2,NumDistortionIntrinsics);
            parameterJacobian->setZero();
        }
        J.setIdentity();
    }

    return true;
}
bool EquidistantDistortion::undistort(const Eigen::Vector2d & pointDistorted,
                                      Eigen::Vector2d * pointUndistorted) const
{
    // this is expensive: we solve with Gauss-Newton...
    Eigen::Vector2d x_bar = pointDistorted;  // initialise at distorted point
    const int n = 5;  // just 5 iterations max.
    Eigen::Matrix2d E;  // error Jacobian

    bool success = false;
    for (int i = 0; i < n; i++) {

        Eigen::Vector2d x_tmp;

        distort(x_bar, &x_tmp, &E);

        Eigen::Vector2d e(pointDistorted - x_tmp);//Hx = b=>
        Eigen::Vector2d du = (E.transpose() * E).inverse() * E.transpose() * e;

        x_bar += du;

        const double chi2 = e.dot(e);
        if (chi2 < 1e-2) {
            success = true;
        }
        //std::cout<<"chi2"<<chi2<<std::endl;
        if (chi2 < 1e-15) {
            success = true;
            break;
        }

    }
    *pointUndistorted = x_bar;

    return success;
}

bool EquidistantDistortion::undistort(const Eigen::Vector2f & pointDistorted,
                                      Eigen::Vector2f * pointUndistorted) const
{

    float dx2GN/*G-N解出的步长的欧式距离*/, dx2GD/*pU点的欧式距离*/, delta2/*信赖域半径*/, beta;
    Eigen::Vector2f dxGN/*G-N解出的增量*/, dxGD/*pU点*/;
    bool update, converge;
    float dr, j, dx2;

    delta2 = 1.0;
    // this is expensive: we solve with Gauss-Newton...
    Eigen::Vector2f x_bar = pointDistorted;  // initialise at distorted point
    const int n = 30;  // just 5 iterations max.
    const int UNDIST_DL_MAX_ITERATIONS = 4;
    Eigen::Matrix2f E;  // error Jacobian

    bool success = false;
    for (int i = 0; i < n; i++)
    {

        Eigen::Vector2f x_tmp;

        distort(x_bar, &x_tmp, &E);

        Eigen::Vector2f e(x_tmp -pointDistorted);
        Eigen::Vector2f dx = -(E.transpose() * E).inverse() * E.transpose() * e;
        Eigen::Vector2f b = -E.transpose() * e;
        Eigen::Matrix2f A = -E.transpose() * E;

        x_bar += dx;

//
//        dx2 = dx.squaredNorm();//x的欧式距离
//        //by wya
//       dxGN = dx;//G-N法求出的增量
//        dx2GN = dx2;//G-N法求出的增量的欧式距离
//        dx2GD = 0.0f;//dogleg
//        const float F = e.squaredNorm();//dogleg迭代开始前残差的欧式距离
//        const Eigen::Vector2f xnBkp = x_bar;//待优化变量
//        update = true;
//        converge = false;//狗腿迭代次数
//        for (int iIterDL = 0; iIterDL < UNDIST_DL_MAX_ITERATIONS; ++iIterDL)
//        {
//            if (dx2GN > delta2 && dx2GD == 0.0f) {//如果G-N增量在信赖域外且dx2GD还没有初始化
//                const float bl = sqrtf(b.squaredNorm());//模长
//                const Eigen::Vector2f g = b * (1.0f / bl);//梯度方向
//                const Eigen::Vector2f Ag = A * g;
//                const float xl = bl / g.dot(Ag);//计算pU点的步长
//                dxGD = g * -xl;//负梯度*步长,pU点
//                dx2GD = xl * xl;//pU点的半径
//
//            }
//            //三种情况,1：GN极值点在域内直接变成无约束条件 2:GN和pU点都在域外,那么就在给pU点的步长一个比例因子(域半径/自己的步长^2（因为用的是最小2乘）),让它刚好落在域半径上
//            //3:GN在域外,pU点在域内,那么增量就是GN极值点和pU点的连线与信赖域的交点
//            if (dx2GN <= delta2) {//如果G-N的极值在信赖域内,那么就是一个无约束问题,就直接用GN法求出的增量就可以
//                dx = dxGN;
//                dx2 = dx2GN;
//                beta = 1.0f;
//            } else if (dx2GD >= delta2) {//如果G-N和pU点求的最优点都在信赖域外
//                if (delta2 == 0.0f) {//信赖域为0,直接用pU点求得最优值
//                    dx = dxGD;
//                    dx2 = dx2GD;
//                } else {
//                    dx =  dxGD * sqrtf(delta2 / dx2GD);//乘比例因子
//                    dx2 = delta2;
//                }
//                beta = 0.0f;
//            } else {//GN在域外,pU点在域内,那么增量就是GN极值点和pU点的连线与信赖域的交点
//                const Eigen::Vector2f v = dxGN - dxGD;//方向
//                const float d = dxGD.dot(v), v2 = v.squaredNorm();
//                //beta = float((-d + sqrt(double(d) * d + (delta2 - dx2GD) * double(v2))) / v2);
//                beta = (-d + sqrtf(d * d + (delta2 - dx2GD) * v2)) / v2;//算得是在域外那段连线的长度
//                dx = dxGD;
//                dx += v * beta;
//                dx2 = delta2;
//            }
//            x_bar += dx;//加上这一次的增量
//            Eigen::Vector2f x_d_temp;
//            distort(x_bar, &x_d_temp);
//            const float dFa = F - (x_d_temp - pointDistorted).squaredNorm();//实际下降的
//            const float dFp = F - (e + ( E.transpose()) * dx).squaredNorm();//理论下降值,直接用J*dx近似下降值了
//            const float rho = dFa > 0.0f && dFp > 0.0f ? dFa / dFp : -1.0f;//求实际/理论的比值,理论不可能为负,实际为负的时候拒绝这次更新
//            //信赖域： Numerical Optimization 第二版 p69
//
//            //rho < 0.25 如果大于0说明近似的不好,需要减小信赖域,减小近似的范围。如果<0就说明是错误的近似,那么就拒绝这次的增量
//            if (rho < FTR_UNDIST_DL_GAIN_RATIO_MIN) {
//                delta2 *= FTR_UNDIST_DL_RADIUS_FACTOR_DECREASE;
//                if (delta2 < FTR_UNDIST_DL_RADIUS_MIN) {
//                    delta2 = FTR_UNDIST_DL_RADIUS_MIN;
//                }
//                x_bar = xnBkp;//取消这次增量
//                update = false;//不更新
//                converge = false;
//                continue;
//            } else if (rho > FTR_UNDIST_DL_GAIN_RATIO_MAX) //rho > 0.75,可以扩大信赖域半径
//            {
//                delta2 = std::max(delta2, FTR_UNDIST_DL_RADIUS_FACTOR_INCREASE * dx2);
//                if (delta2 > FTR_UNDIST_DL_RADIUS_MAX) {//信赖域半径最大值
//                    delta2 = FTR_UNDIST_DL_RADIUS_MAX;
//                }
//            }
//            update = true;//
//
//            converge = dx2 < 1e-12;//增量小于阈值,认为收敛
//            break;
//        }
//        if (!update || converge) {
//            break;
//        }
//
//        const double chi2 = e.dot(e);
//        if (chi2 < 1e-5) {
//            success = true;
//        }
        const double chi2 = e.dot(e);

        if (chi2 < 1e-8) {
            success = true;
            break;
        }

    }
    *pointUndistorted = x_bar;
    if(converge)
        success = true;

    return success;
}

bool EquidistantDistortion::undistort(const Eigen::Vector2d & pointDistorted,
                                      Eigen::Vector2d * pointUndistorted,
                                      Eigen::Matrix2d * pointJacobian) const
{
    // this is expensive: we solve with Gauss-Newton...
    Eigen::Vector2d x_bar = pointDistorted;  // initialise at distorted point
    const int n = 5;  // just 5 iterations max.
    Eigen::Matrix2d E;  // error Jacobian

    bool success = false;
    for (int i = 0; i < n; i++) {

        Eigen::Vector2d x_tmp;

        distort(x_bar, &x_tmp, &E);

        Eigen::Vector2d e(pointDistorted - x_tmp);
        Eigen::Vector2d dx = (E.transpose() * E).inverse() * E.transpose() * e;

        x_bar += dx;

        const double chi2 = e.dot(e);
        if (chi2 < 1e-2) {
            success = true;
        }
        if (chi2 < 1e-15) {
            success = true;
            break;
        }

    }
    *pointUndistorted = x_bar;

    // the Jacobian of the inverse map is simply the inverse Jacobian.
    *pointJacobian = E.inverse();

    return success;
}

bool EquidistantDistortion::undistort(const Eigen::Vector2f & pointDistorted,
                                      Eigen::Vector2f * pointUndistorted,
                                      Eigen::Matrix2f * pointJacobian) const
{
    // this is expensive: we solve with Gauss-Newton...
    Eigen::Vector2f x_bar = pointDistorted;  // initialise at distorted point
    const int n = 5;  // just 5 iterations max.
    Eigen::Matrix2f E;  // error Jacobian

    bool success = false;
    for (int i = 0; i < n; i++) {

        Eigen::Vector2f x_tmp;

        distort(x_bar, &x_tmp, &E);

        Eigen::Vector2f e(pointDistorted - x_tmp);
        Eigen::Vector2f dx = (E.transpose() * E).inverse() * E.transpose() * e;

        x_bar += dx;

        const double chi2 = e.dot(e);
        if (chi2 < 1e-2) {
            success = true;
        }
        if (chi2 < 1e-15) {
            success = true;
            break;
        }

    }
    *pointUndistorted = x_bar;

    // the Jacobian of the inverse map is simply the inverse Jacobian.
    *pointJacobian = E.inverse();

    return success;
}

}  // namespace cameras
}  // namespace okvis


#ifndef ASTAR_HPP_
#define ASTAR_HPP_

/*
   A star path planning implement by c++
*/

#include <iostream> // std::cerr, std::endl
#include <algorithm> // std::find
#include <ctime> // clock
#include <list> // std::list
#include <Eigen/Dense> // Eigen::MatrixXd

extern bool ASTAR_VERBOSE;
extern bool ASTAR_SHOWTIME;

namespace AStar{
 class Node{
  private:
    double x_, y_;
    double f_, g_, h_; // Cost function
    Node* parent_; // Parent address
  public:
    Node():x_(0.), y_(0.), parent_(NULL), f_(0), g_(0), h_(0){};
    Node(double x, double y, Node* parent):
       x_(x), y_(y), parent_(parent), f_(0), g_(0), h_(0){};
    bool operator==(const Node& target){
      if(x_==target.get_x() && y_==target.get_y()) return true;
      else return false;
    };
    // Get functions
    Node* get_parent(void) {return parent_;}
    double get_f(void) {return f_;}
    double get_g(void) {return g_;}
    double get_h(void) {return h_;}
    double get_x(void) const {return x_;}
    double get_y(void) const {return y_;}
    // Set functions
    void set_f(double f) {f_ = f;}
    void set_g(double g) {g_ = g;}
    void set_h(double h) {h_ = h;}
  };

 typedef std::list<Node> NODE_LIST;

 class AStar{
   private:
     NODE_LIST planned_list; // Planned list to return
     NODE_LIST open, closed; // Open and closed list
     Node start_, end_; // Start and end node
     Eigen::MatrixXd map_; // Map, in Eigen::MatrixXd
     double height, width; // Dimension of map
     const static int FACTOR=3; // Multiplier to consider as failed planning
     /* Check if given start and goal nodes are valid
        @output
          True if valid, false otherwise
     */
     bool checkIfValid(void){
       // Check bound
       bool bound_check = true, node_check = true;
       if(start_.get_x()<0 or start_.get_x()>=height or start_.get_y()<0 or start_.get_y()>=width) bound_check = false;
       if(end_.get_x()<0 or end_.get_x()>=height or end_.get_y()<0 or end_.get_y()>=width) bound_check = false;
       if(!bound_check) {
         if(ASTAR_VERBOSE)
           std::cerr << "\033[1;31mInvalid input, ignoring...\033[0m" << std::endl;
         return false;
       }
       // Check nodes
       if(map_(start_.get_x(), start_.get_y()) > 50.) node_check = false;
       if(map_(end_.get_x(), end_.get_y()) > 50.) node_check = false;
       if(!node_check) {
         if(ASTAR_VERBOSE) std::cerr << "\033[1;31mStart node or end node at obstacle, ignoring...\033[0m" << std::endl;
         return false;
       }
       return true;
     }
     void clear_list(void){
       open.clear(); closed.clear(); planned_list.clear();
     }
   public:
     /*
        Empty constructor
     */
     AStar() {}
     /* Constructor:
        @param
          Node start: start node, in form of matrix indice
          Node end: goal node, in form of matrix indice
          Eigen::MatrixXd map: map to plan
     */
     AStar(Node start, Node end, Eigen::MatrixXd map): \
           start_(start), end_(end), map_(map){
       height = map.rows(); width = map.cols();
       // Check if valid input
       if(!checkIfValid()) return;
       open.push_back(start_); // Add start node to open list
     }
     /*
        Initial planner
        @param
          Node start
          Node end
          Eigen::MatrixXd
     */
     void initial(Node start, Node end, Eigen::MatrixXd map){
       start_ = start;
       end_ = end;
       map_ = map;
       /*int map_size = int(21/2)*2;  //set maze size
       for(int x=-map_size; x<map_size; x++){
         map =
       }*/

       // Set the maze around the start point to 0
       for(int x=-1; x<=1; ++x){
        for(int y=-1; y<=1; ++y){
          map_(start.get_x()+x, start.get_y()+y) = 0;
        }
       }
       height = map.rows(); width = map.cols();
       if(!checkIfValid()) {
         if(ASTAR_VERBOSE)
           std::cerr << "\033[1;31mInvalid input, ignoring...\033[0m" << std::endl;
         return;}
       open.push_back(start_); // Add start node to open list
     }
     /* Planing function
        @param
     /* Planing function
        @param
          NODE_LIST&: planing result placeholder
        @output
          true if successfully find path from start to end, false otherwise
     */
     bool plan(NODE_LIST &res){
       clock_t start_t = clock();
       int count = 0;
       while(open.size()!=0){
         count += 1;
         if(count>=height*FACTOR) {if(ASTAR_VERBOSE) printf("[AStar] Too much iterations, ignore...\n");
           clear_list(); return false;}
         Node* current_node = new Node ();
         // Get the current node
         *current_node = open.front();
         NODE_LIST::iterator current_idx = open.begin();
         for(NODE_LIST::iterator lit=open.begin(); lit!=open.end(); ++lit){
           if(lit->get_f() < current_node->get_f()){
             *current_node = *lit;
             current_idx = lit;
           } // End if
         } // End for
         Node* candidatePtr;
         if(count==1){
           candidatePtr = new Node(current_node->get_x(), current_node->get_y(), NULL);
           candidatePtr->set_g(current_node->get_g());
         }
         else{
           candidatePtr = new Node(current_node->get_x(), current_node->get_y(), candidatePtr);
           candidatePtr->set_g(current_node->get_g());
         }
         delete current_node;
         // Pop candidate off open list and add to closed list
         open.erase(current_idx);
         closed.push_back(*candidatePtr);
         // Check if found goal
         if(*candidatePtr==end_){
           Node* currentPtr = candidatePtr;
           while(currentPtr!=NULL){
             planned_list.push_back(*currentPtr);
             // Added to make g-value stepwise
             if(currentPtr->get_parent()!=NULL){
               if(currentPtr->get_g() == currentPtr->get_parent()->get_g()+1){
                 currentPtr = currentPtr->get_parent();
               }
               else{
                 double last_g = currentPtr->get_g(); // Save g-value to variable first
                 while(last_g!=currentPtr->get_g()+1) {
                   currentPtr = currentPtr->get_parent();
                 }
               } // End else
             } // End if
             else break;
           } // End while
           planned_list.reverse(); res = planned_list;
           clear_list();
           if(ASTAR_SHOWTIME) printf("[AStar] Cost time: %f seconds\n", double((clock_t()-start_t)/CLOCKS_PER_SEC));
           return true;
         } // End if
         // --------------------------------------------- Planning ---------------------------------------------
         // Generate children
         NODE_LIST children;
         for(int i=-1; i<2; ++i){
           for(int j=-1; j<2; ++j){
             if(i==0 and j==0) continue;
             double new_x = candidatePtr->get_x()+i,
                    new_y = candidatePtr->get_y()+j;
             // Make sure within map
             if(new_x<0 or new_x>=width or new_y<0 or new_y>=height) continue;
             // Make sure walkable
             if(map_(new_x,new_y) >= 50.) continue;
             // Create new node
             Node* new_nodePtr = new Node(new_x, new_y, candidatePtr);
             children.push_back(*new_nodePtr);
             delete new_nodePtr;
           } // End for
         } // End for
         // Loop through children
         NODE_LIST::iterator nit;
         for(nit=children.begin(); nit!=children.end(); ++nit){
           // Check if already in close list
           NODE_LIST::iterator findIter;
           // Create F, G and H values
           nit->set_g(candidatePtr->get_g()+1);
           double dx = nit->get_x() - end_.get_x(),
                  dy = nit->get_y() - end_.get_y();
           nit->set_h(dx*dx+dy*dy);
           //std::cout<< "g:" << nit->get_g() << ", h:" << nit->get_h() << ", map:" << map_(nit->get_x(), nit->get_y()) << std::endl;
           nit->set_f(nit->get_g() + nit->get_h() + 1.8*map_(nit->get_x(), nit->get_y()));
           // Last term is added to make path tends to away from potential obstacle
           // Check if already in the open list
           findIter = std::find(open.begin(), open.end(), *nit);
           if(findIter!=open.end()){
             if(findIter->get_g() > nit->get_g()) continue;
           }
           // And child to open list
           open.push_back(*nit);
         } // End for
       } // End while
     } // End plan
  }; // End class
}; // End namespace

#endif


// AUTORIGHTS
// -------------------------------------------------------
// Copyright (C) 2011-2012 Ross Girshick
//
// This file is part of the voc-releaseX code
// (http://people.cs.uchicago.edu/~rbg/latent/)
// and is available under the terms of an MIT-like license
// provided in COPYING. Please retain this notice and
// COPYING if you use this file (or a portion of it) in
// your project.
// -------------------------------------------------------

/* This code is used for computing filter responses.  It computes the
 * response of a set of filters with a feature map.
 *
 * Multithreaded SSE accelerated version.
 */

#include "mex.h"
#include <pthread.h>
#include <xmmintrin.h>
#include <boost/preprocessor/repeat.hpp>
// #include "../external/boost/preprocessor/repeat.hpp"

// N.B. If you change the number of features you will need to unroll
// the unrolled loop in process() more.
const static int NUM_FEATURES = 4*META_NUM_FEATURES;

struct thread_data {
  float  *A;
  float  *B;
  double *C;
  mxArray *mxC;
  const mwSize *A_dims;
  const mwSize *B_dims;
  mwSize C_dims[2];
};

// Convolve A (feature map) and B (filter)
void *process(void *thread_arg) {
  thread_data *args    = (thread_data *)thread_arg;
  float *A             = args->A;
  float *B             = args->B;
  double *C            = args->C;
  const mwSize *A_dims = args->A_dims;
  const mwSize *B_dims = args->B_dims;
  const mwSize *C_dims = args->C_dims;

  __m128 a, b, c;
  double *dst = C;
  // Loop over output positions (y, x)
  for (int x = 0; x < C_dims[1]; x++) {
    for (int y = 0; y < C_dims[0]; y++) {
      __m128 accum = _mm_setzero_ps();
      const float *A_src = A + y*NUM_FEATURES + x*A_dims[0]*NUM_FEATURES;
      const float *B_src = B;
      // Loop over filter cells (yp, xp)
      for (int xp = 0; xp < B_dims[1]; xp++) {
        const float *A_off = A_src;
        const float *B_off = B_src;
        for (int yp = 0; yp < B_dims[0]; yp++) {
          // Loop body: dot product of two 4-vectors of floats
          #define DOT4(z, step, ignore)           \
            a     = _mm_load_ps(A_off + 4*step);  \
            b     = _mm_load_ps(B_off + 4*step);  \
            c     = _mm_mul_ps(a, b);             \
            accum = _mm_add_ps(accum, c);         \

          // Unrolled loop over feature vector dimensions
          BOOST_PP_REPEAT(META_NUM_FEATURES, DOT4, ignore)

          // N.B. Unroll me more/less if you change NUM_FEATURES
          A_off += NUM_FEATURES;
          B_off += NUM_FEATURES;
        }
        A_src += A_dims[0]*NUM_FEATURES;
        B_src += B_dims[0]*NUM_FEATURES;
      }
      float buf[4] __attribute__ ((aligned (64)));
      _mm_store_ps(buf, accum);
      _mm_empty();
      *(dst++) = buf[0]+buf[1]+buf[2]+buf[3];
    }
  }
  pthread_exit(NULL);
}

float *prepare(float *in, const mwSize *dims) {
  float *F = (float *)_mm_malloc(dims[0]*dims[1]*NUM_FEATURES*sizeof(float), 64);

  float *p = F;
  for (int x = 0; x < dims[1]; x++) {
    for (int y = 0; y < dims[0]; y++) {
      for (int f = 0; f < dims[2]; f++)
        *(p++) = in[y + f*dims[0]*dims[1] + x*dims[0]];
      for (int f = dims[2]; f < NUM_FEATURES; f++)
        *(p++) = 0;
    }
  }
  return F;
}

// matlab entry point
// C = fconv(A, B, start, end);
// A        Nx x Ny x 32 dimensional HOG feature map (class: single)
// B        cell array of filters (class: single)
// start    starting index in B
// end      ending index in B (filters in B{start:end} will be used)
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  if (nrhs < 4)
    mexErrMsgTxt("Wrong number of inputs");
  if (nlhs != 1)
    mexErrMsgTxt("Wrong number of outputs");

  // get A (HOG feature map)
  const mxArray *mxA = prhs[0];
  { // error checking
    if (mxGetNumberOfDimensions(mxA) != 3)
      mexErrMsgTxt("First argument (feature map) must be a 3D array");
    if (mxGetClassID(mxA) != mxSINGLE_CLASS)
      mexErrMsgTxt("First argument (feature map) must be single precision");
    if (mxGetDimensions(mxA)[2] > NUM_FEATURES)
      mexErrMsgTxt("First argument (feature map) feature dimension is too large");
  }
  const mwSize *A_dims = mxGetDimensions(mxA);
  float *A             = prepare((float *)mxGetPr(mxA), A_dims);

  // get B (cell array of filters)
  const mxArray *cellB = prhs[1];
  const mwSize num_bs  = mxGetNumberOfElements(cellB);

  // start and end indices in B
  const int start = (int)mxGetScalar(prhs[2]) - 1;
  const int end   = (int)mxGetScalar(prhs[3]) - 1;
  const int len   = end-start+1;
  if (start < 0 || end >= num_bs || start > end)
    mexErrMsgTxt("Inputs start and end exceed boundaries");

  // Start one thread per filter
  thread_data *td = (thread_data *)mxCalloc(len, sizeof(thread_data));
  pthread_t *ts = (pthread_t *)mxCalloc(len, sizeof(pthread_t));
  for (int i = 0; i < len; i++) {
    const mxArray *mxB = mxGetCell(cellB, i+start);
    td[i].A_dims       = A_dims;
    td[i].A            = A;
    td[i].B_dims       = mxGetDimensions(mxB);
    td[i].B            = prepare((float *)mxGetPr(mxB), td[i].B_dims);
    { // error checking
      if (mxGetNumberOfDimensions(mxB) != 3)
        mexErrMsgTxt("Filter must be a 3D array");
      if (mxGetClassID(mxB) != mxSINGLE_CLASS)
        mexErrMsgTxt("Filter must be single precision");
      if (td[i].A_dims[2] != td[i].B_dims[2])
        mexErrMsgTxt("Filter feature dimension doesn't match feature map");
    }

    // Compute output size and allocate array
    int height = td[i].A_dims[0] - td[i].B_dims[0] + 1;
    int width  = td[i].A_dims[1] - td[i].B_dims[1] + 1;
    if (height < 1 || width < 1)
      mexErrMsgTxt("Filter is too large for feature map");
    td[i].C_dims[0] = height;
    td[i].C_dims[1] = width;
    td[i].mxC       = mxCreateNumericArray(2, td[i].C_dims, mxDOUBLE_CLASS, mxREAL);
    td[i].C         = (double *)mxGetPr(td[i].mxC);

    if (pthread_create(&ts[i], NULL, process, (void *)&td[i]))
      mexErrMsgTxt("Error creating thread");
  }

  // Wait for the treads to finish, set return values, and free memory
  void *status;
  plhs[0] = mxCreateCellMatrix(1, len);
  for (int i = 0; i < len; i++) {
    pthread_join(ts[i], &status);
    mxSetCell(plhs[0], i, td[i].mxC);
    _mm_free(td[i].B);
  }
  mxFree(td);
  mxFree(ts);
  _mm_free(A);
}


#ifndef GPS_CONSTELLATION_HH
#define GPS_CONSTELLATION_HH

/********************************* TRICK HEADER *******************************
PURPOSE:
      (Describe the GPS Receiver model)
LIBRARY DEPENDENCY:
      ((../src/GPS_constellation.cpp))
PROGRAMMERS:
      (((Lai Jun Xu) () () () ))
*******************************************************************************/

#include <armadillo>
#include <functional>

#include "aux.hh"

#include "GPS.hh"

#include "Time_management.hh"
#include "cad_utility.hh"
#include "global_constants.hh"

#include "time_utility.hh"
struct range_t {
  time_util::GPS_TIME g;
  double range;
  double rate;
  double d;
  arma::vec2 azel;
  double iono_delay;
  arma::vec3 pos;
  arma::vec3 vel;
  arma::vec2 clk;
};

struct ephem_t {
  int vflg; /*!< Valid Flag */
  time_util::UTC_TIME t;
  time_util::GPS_TIME toc; /*!< Time of Clock */
  time_util::GPS_TIME toe; /*!< Time of Ephemeris */
  int iodc;                /*!< Issue of Data, Clock */
  int iode;                /*!< Isuse of Data, Ephemeris */
  double deltan;           /*!< Delta-N (radians/sec) */
  double cuc;              /*!< Cuc (radians) */
  double cus;              /*!< Cus (radians) */
  double cic;              /*!< Correction to inclination cos (radians) */
  double cis;              /*!< Correction to inclination sin (radians) */
  double crc;              /*!< Correction to radius cos (meters) */
  double crs;              /*!< Correction to radius sin (meters) */
  double ecc;              /*!< e Eccentricity */
  double sqrta;            /*!< sqrt(A) (sqrt(m)) */
  double m0;               /*!< Mean anamoly (radians) */
  double omg0;             /*!< Longitude of the ascending node (radians) */
  double inc0;             /*!< Inclination (radians) */
  double aop;
  double omgdot; /*!< Omega dot (radians/s) */
  double idot;   /*!< IDOT (radians/s) */
  double af0;    /*!< Clock offset (seconds) */
  double af1;    /*!< rate (sec/sec) */
  double af2;    /*!< acceleration (sec/sec^2) */
  double tgd;    /*!< Group delay L2 bias */
  int svhlth;
  int codeL2;
  // Working variables follow
  double n;       /*!< Mean motion (Average angular velocity) */
  double sq1e2;   /*!< sqrt(1-e^2) */
  double A;       /*!< Semi-major axis */
  double omgkdot; /*!< OmegaDot-OmegaEdot */
};

struct ionoutc_t {
  int enable;
  int vflg;
  double alpha0, alpha1, alpha2, alpha3;
  double beta0, beta1, beta2, beta3;
  double A0, A1;
  int dtls, tot, wnt;
  int dtlsf, dn, wnlsf;
};

struct channel_t {
  int prn;                     /*< PRN Number */
  int ca[CA_SEQ_LEN];          /*< C/A Sequence */
  double f_carr;               /*< Carrier frequency */
  double f_code;               /*< Code frequency */
  unsigned int carr_phase;     /*< Carrier phase */
  int carr_phasestep;          /*< Carrier phasestep */
  double code_phase;           /*< Code phase */
  time_util::GPS_TIME g0;      /*!< GPS time at start */
  uint32_t sbf[5][N_DWRD_SBF]; /*!< current subframe */
  uint32_t dwrd[N_DWRD];       /*!< Data words of sub-frame */
  int iword;                   /*!< initial word */
  int ibit;                    /*!< initial bit */
  int icode;                   /*!< initial code */
  int dataBit;                 /*!< current data bit */
  int codeCA;                  /*!< current C/A code */
  arma::vec2 azel;
  range_t rho0;
};

class GPS_constellation {
  TRICK_INTERFACE(GPS_constellation);
  friend class GPS_FSW;

 public:
  GPS_constellation();
  GPS_constellation(const GPS_constellation &other);
  GPS_constellation &operator=(const GPS_constellation &other);

  void readfile(const char *fname);
  void initialize();
  void compute();
  void show();
  channel_t *get_channel();
  transmit_channel *get_transmit_data();
  unsigned int get_gps_update() { return gps_update; }
  void clear_gps_flag() { gps_update = 0; }
  int write_to_(const char *bus_name);
  std::function<arma::vec3()> grab_SBEE;
  std::function<arma::mat33()> grab_TEI;
  std::function<double()> grab_phibdx;
  std::function<double()> grab_thtbdx;
  std::function<double()> grab_psibdx;
  std::function<arma::mat33()> grab_TBI;

 private:
  int readRinexNavAll(ephem_t eph[][MAX_SAT], ionoutc_t *ionoutc,
                      const char *fname);
  int allocateChannel(channel_t *chan, ephem_t *eph, ionoutc_t ionoutc,
                      time_util::GPS_TIME grx, arma::vec3 XYZ, double elvMask);
  int checkSatVisibility(ephem_t eph, time_util::GPS_TIME g, arma::vec3 xyz,
                         double elvMask, arma::vec2 &azel);
  void computeRange(range_t *rho, ephem_t eph, ionoutc_t *ionoutc,
                    time_util::GPS_TIME g, arma::vec3 XYZ);
  int replaceExpDesignator(char *str, int len);
  void satpos(ephem_t eph, time_util::GPS_TIME g, arma::vec3 &pos,
              arma::vec3 &vel, arma::vec2 &clk);
  void xyz2llh(const arma::vec3 xyz, arma::vec3 &llh);
  arma::vec3 llh2xyz(const arma::vec3 llh);
  void ned2azel(arma::vec2 &azel, const arma::vec3 neu);
  double ionosphericDelay(const ionoutc_t *ionoutc, time_util::GPS_TIME g,
                          arma::vec3 llh, arma::vec2 azel);
  double subGpsTime(time_util::GPS_TIME g1, time_util::GPS_TIME g0);
  arma::mat33 ltcmat(const arma::vec3 llh);
  arma::mat33 enu2ecef(arma::vec3 llh);
  arma::mat33 enu2atenna(double phi, double tht, double psi);
  void GDOP(channel_t *chan, arma::vec3 xyz);
  void packet();

  time_management *time;

  int neph; /* *io  (--)    number of ephemeris */
  int ieph; /* *io  (--)    which ephemeris been select */

  int nsat; /*  *io  (--)                               */
  channel_t chan[MAX_CHAN];
  transmit_channel trans_chan[MAX_CHAN];
  ionoutc_t ionoutc;
  ephem_t eph[EPHEM_ARRAY_SIZE][MAX_SAT];

  int allocatedSat[MAX_SAT]; /* *o (--)                       */

  double gdop;             /* *io  (--)  Geometric Dilution of Precsision */
  uint32_t gps_update; /* *o (--)       GPS update? > 0 updated */
                           // arma::vec azel;
                           // double _azel[2];

  // arma::vec clk;
  // double _clk[2];

  // arma::vec llh;
  // double _llh[3];

  // arma::vec neu;
  // double _neu[3];

  // arma::vec pos;
  // double _pos[3];

  // arma::vec vel;
  // double _vel[3];

  // arma::vec los;
  // double _los[3];
};

#endif


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#ifdef _MSC_VER
#define _SCL_SECURE_NO_WARNINGS
#endif

#include <nil/crypto3/multiprecision/tommath.hpp>

#define NO_MIXED_OPS
#define BOOST_MP_NOT_TESTING_NUMBER

#include "test_arithmetic.hpp"

#include <boost/rational.hpp>

template<class T>
struct is_boost_rational<boost::rational<T>> : public boost::mpl::true_ { };

namespace boost {
    namespace multiprecision {

        template<>
        struct number_category<rational<tom_int>> : public boost::mpl::int_<number_kind_rational> { };

    }    // namespace multiprecision
}    // namespace boost

int main() {
    test<boost::rational<nil::crypto3::multiprecision::tom_int>>();
    return boost::report_errors();
}


#ifdef CANNON_BUILD_GRAPHICS

#include <glad/glad.h>
#include <Eigen/Dense>

#include <cannon/graphics/window.hpp>
#include <cannon/graphics/vertex_buffer.hpp>
#include <cannon/graphics/vertex_array_object.hpp>
#include <cannon/graphics/vertex_shader.hpp>
#include <cannon/graphics/fragment_shader.hpp>
#include <cannon/graphics/shader_program.hpp>

using namespace Eigen;
using namespace cannon::graphics;

int main() {
  Window w;

  glPointSize(15.0);
  glLineWidth(5.0);

  MatrixX2f points(10, 2);
  points = MatrixX2f::Random(10, 2);

  auto vao1 = std::make_shared<VertexArrayObject>();
  VertexBuffer buf1(vao1);
  buf1.buffer(points);

  MatrixX2f lines(4, 2);
  lines << -1.0, 0.0,
           1.0, 0.0,
           0.0, -1.0,
           0.0, 1.0;

  auto vao2 = std::make_shared<VertexArrayObject>();
  VertexBuffer buf2(vao2);
  buf2.buffer(lines);

  const char* v_src = BASIC_VERTEX_SHADER_2D.c_str();
  const char* f_src = BASIC_FRAGMENT_SHADER.c_str();

  VertexShader v(&v_src);
  FragmentShader f(&f_src);

  ShaderProgram program;
  program.attach_shader(v);
  program.attach_shader(f);
  program.link();
  program.activate();

  w.set_clear_color(Vector4f(1.0, 1.0, 1.0, 1.0));

  w.render_loop([&] {
      program.activate();
      program.set_uniform("uColor", Vector4f(0.0f, 0.0f, 0.0f, 1.0f));

      buf1.bind();
      glDrawArrays(GL_POINTS, 0, 10);

      buf2.bind();
      glDrawArrays(GL_LINES, 0, 4);
      });
}
#else
int main() {}
#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2008 Andreas Gaida
 Copyright (C) 2008 Ralph Schreyer
 Copyright (C) 2008 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file ninepointlinearop.hpp
    \brief nine point linear operator
*/

#ifndef quantlib_nine_point_linear_op_hpp
#define quantlib_nine_point_linear_op_hpp

#include <ql/math/matrixutilities/sparsematrix.hpp>
#include <ql/methods/finitedifferences/operators/fdmlinearop.hpp>
#if !defined(QL_USE_STD_UNIQUE_PTR)
#include <boost/shared_array.hpp>
#endif
#include <memory>

namespace QuantLib {

    class FdmMesher;

    class NinePointLinearOp : public FdmLinearOp {
      public:
        NinePointLinearOp(Size d0, Size d1,
                const ext::shared_ptr<FdmMesher>& mesher);
        NinePointLinearOp(const NinePointLinearOp& m);
        NinePointLinearOp(NinePointLinearOp&& m) QL_NOEXCEPT;
        #ifdef QL_USE_DISPOSABLE
        NinePointLinearOp(const Disposable<NinePointLinearOp>& m);
        #endif
        NinePointLinearOp& operator=(const NinePointLinearOp& m);
        NinePointLinearOp& operator=(NinePointLinearOp&& m) QL_NOEXCEPT;
        #ifdef QL_USE_DISPOSABLE
        NinePointLinearOp& operator=(const Disposable<NinePointLinearOp>& m);
        #endif

        Disposable<Array> apply(const Array& r) const override;
        Disposable<NinePointLinearOp> mult(const Array& u) const;

        void swap(NinePointLinearOp& m);

        Disposable<SparseMatrix> toMatrix() const override;

      protected:
        NinePointLinearOp() = default;

        Size d0_, d1_;
        #if !defined(QL_USE_STD_UNIQUE_PTR)
        boost::shared_array<Size> i00_, i10_, i20_;
        boost::shared_array<Size> i01_, i21_;
        boost::shared_array<Size> i02_, i12_, i22_;
        boost::shared_array<Real> a00_, a10_, a20_;
        boost::shared_array<Real> a01_, a11_, a21_;
        boost::shared_array<Real> a02_, a12_, a22_;
        #else
        std::unique_ptr<Size[]> i00_, i10_, i20_;
        std::unique_ptr<Size[]> i01_, i21_;
        std::unique_ptr<Size[]> i02_, i12_, i22_;
        std::unique_ptr<Real[]> a00_, a10_, a20_;
        std::unique_ptr<Real[]> a01_, a11_, a21_;
        std::unique_ptr<Real[]> a02_, a12_, a22_;
        #endif

        ext::shared_ptr<FdmMesher> mesher_;
    };

    inline NinePointLinearOp::NinePointLinearOp(NinePointLinearOp&& m) QL_NOEXCEPT {
        swap(m);
    }

    inline NinePointLinearOp& NinePointLinearOp::operator=(const NinePointLinearOp& m) {
        NinePointLinearOp temp(m);
        swap(temp);
        return *this;
    }

    inline NinePointLinearOp& NinePointLinearOp::operator=(NinePointLinearOp&& m) QL_NOEXCEPT {
        swap(m);
        return *this;
    }

}

#endif


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_LINE_INTERPOLATE_CARTESIAN_HPP
#define BOOST_GEOMETRY_STRATEGIES_LINE_INTERPOLATE_CARTESIAN_HPP

#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/strategies/cartesian/line_interpolate.hpp>

#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/distance/detail.hpp>
#include <boost/geometry/strategies/line_interpolate/services.hpp>

#include <boost/geometry/util/type_traits.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace line_interpolate
{

template <typename CalculationType = void>
struct cartesian
    : public strategies::detail::cartesian_base
{
    template <typename Geometry1, typename Geometry2>
    static auto distance(Geometry1 const&, Geometry2 const&,
                         distance::detail::enable_if_pp_t<Geometry1, Geometry2> * = nullptr)
    {
        return strategy::distance::pythagoras<CalculationType>();
    }

    template <typename Geometry>
    static auto line_interpolate(Geometry const&)
    {
        return strategy::line_interpolate::cartesian<CalculationType>();
    }
};

namespace services
{

template <typename Geometry>
struct default_strategy<Geometry, cartesian_tag>
{
    using type = strategies::line_interpolate::cartesian<>;
};

template <typename CT, typename DS>
struct strategy_converter<strategy::line_interpolate::cartesian<CT, DS> >
{
    static auto get(strategy::line_interpolate::cartesian<CT, DS> const&)
    {
        return strategies::line_interpolate::cartesian<CT>();
    }
};

} // namespace services

}} // namespace strategies::line_interpolate

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_LINE_INTERPOLATE_CARTESIAN_HPP


#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/physics.hh>

/* A simple drag plugin. Usage:
 *
 *    <gazebo>
 *      <plugin name="OrcaDragPlugin" filename="libOrcaDragPlugin.so">
 *        <link name="base_link">
 *          <center_of_mass>0 0 -0.2</center_of_mass>
 *          <tether_attach>-0.5, -0.4, 0</tether_attach>
 *          <linear_drag>10 20 30</linear_drag>
 *          <angular_drag>5 10 15</angular_drag>
 *          <tether_drag>4</tether_drag>
 *          <surface>10</surface>
 *        </link>
 *      </plugin>
 *    </gazebo>
 *
 *    <center_of_mass> Drag force is applied to the center of mass.
 *    <tether_attach> Relative position of tether attachment.
 *    <linear_drag> Linear drag constants. See default calculation.
 *    <angular_drag> Angular drag constants. See defaut calculation.
 *    <tether_drag> Tether drag constant. See default calculation.
 *    <surface> How far above z=0 the surface of the water is; used to calculate depth.
 *
 * Limitations:
 *    Tether drag is modeled only in x
 */

namespace gazebo {

/* drag = 0.5 * density * area * velocity^2 * coefficient
 *
 * The drag coefficient for a box is 1.0, so we'll use 0.9 for the ROV.
 * The drag coefficient for an unfaired tether is 1.2.
 *
 * The ROV constants below capture all but velocity:
 * constant = 0.5 * density * area * coefficient
 *
 * The tether constant below captures all but depth and velocity:
 * constant = 0.5 * density * width * coefficient
 */

constexpr double FLUID_DENSITY = 1029;    // Fluid density of seawater
constexpr double ROV_DIM_X = 0.457;       // Length
constexpr double ROV_DIM_Y = 0.338;       // Width
constexpr double ROV_DIM_Z = 0.254;       // Height
constexpr double ROV_AREA_X = ROV_DIM_Y * ROV_DIM_Z;  // Fore, aft area
constexpr double ROV_AREA_Y = ROV_DIM_X * ROV_DIM_Z;  // Top, bottom area
constexpr double ROV_AREA_Z = ROV_DIM_X * ROV_DIM_Y;  // Port, starboard area
constexpr double ROV_DRAG_COEFFICIENT_X = 0.8;
constexpr double ROV_DRAG_COEFFICIENT_Y = 0.95;
constexpr double ROV_DRAG_COEFFICIENT_Z = 0.95;
constexpr double ROV_LINEAR_DRAG_X = 0.5 * FLUID_DENSITY * ROV_AREA_X * ROV_DRAG_COEFFICIENT_X;
constexpr double ROV_LINEAR_DRAG_Y = 0.5 * FLUID_DENSITY * ROV_AREA_Y * ROV_DRAG_COEFFICIENT_Y;
constexpr double ROV_LINEAR_DRAG_Z = 0.5 * FLUID_DENSITY * ROV_AREA_Z * ROV_DRAG_COEFFICIENT_Z;
constexpr double ANGULAR_DRAG_X = ROV_LINEAR_DRAG_X / 2; // A hack
constexpr double ANGULAR_DRAG_Y = ROV_LINEAR_DRAG_Y / 2;
constexpr double ANGULAR_DRAG_Z = ROV_LINEAR_DRAG_Z / 2; // TODO these are wrong, see revised cals in orca_mission.cpp
constexpr double TETHER_DIAM = 0.008;
constexpr double TETHER_DRAG_COEFFICIENT = 1.1;
constexpr double TETHER_DRAG = 0.5 * FLUID_DENSITY * TETHER_DIAM * TETHER_DRAG_COEFFICIENT;

class OrcaDragPlugin : public ModelPlugin
{
private:

  physics::LinkPtr base_link_;

  // Drag force will be applied to the center_of_mass_ (body frame)
  ignition::math::Vector3d center_of_mass_ {0, 0, 0};

  // Tether drag will be applied to the tether attachment point (body frame)
  ignition::math::Vector3d tether_attach_ {0, 0, 0};

  // Drag constants (body frame)
  ignition::math::Vector3d linear_drag_ {ROV_LINEAR_DRAG_X, ROV_LINEAR_DRAG_Y, ROV_LINEAR_DRAG_Z};
  ignition::math::Vector3d angular_drag_ {ANGULAR_DRAG_X, ANGULAR_DRAG_Y, ANGULAR_DRAG_Z};
  double tether_drag_ {TETHER_DRAG};

  // Distance to surface
  double surface_ {10};

  event::ConnectionPtr update_connection_;

public:

  // Called once when the plugin is loaded.
  void Load(physics::ModelPtr model, sdf::ElementPtr sdf)
  {
    std::string link_name {"base_link"};

    std::cout << std::endl;
    std::cout << "ORCA DRAG PLUGIN PARAMETERS" << std::endl;
    std::cout << "-----------------------------------------" << std::endl;
    std::cout << "Default link name: " << link_name << std::endl;
    std::cout << "Default center of mass: " << center_of_mass_ << std::endl;
    std::cout << "Default tether attachment point: " << tether_attach_ << std::endl;
    std::cout << "Default linear drag: " << linear_drag_ << std::endl;
    std::cout << "Default angular drag: " << angular_drag_ << std::endl;
    std::cout << "Default tether drag: " << tether_drag_ << std::endl;
    std::cout << "Default surface: " << surface_ << std::endl;

    GZ_ASSERT(model != nullptr, "Model is null");
    GZ_ASSERT(sdf != nullptr, "SDF is null");

    if (sdf->HasElement("link"))
    {
      sdf::ElementPtr linkElem = sdf->GetElement("link"); // Only one link is supported

      if (linkElem->HasAttribute("name"))
      {
        linkElem->GetAttribute("name")->Get(link_name);
        std::cout << "Link name: " << link_name << std::endl;
      }

      if (linkElem->HasElement("center_of_mass"))
      {
        center_of_mass_ = linkElem->GetElement("center_of_mass")->Get<ignition::math::Vector3d>();
        std::cout << "Center of mass: " << center_of_mass_ << std::endl;
      }

      if (linkElem->HasElement("tether_attach"))
      {
        tether_attach_ = linkElem->GetElement("tether_attach")->Get<ignition::math::Vector3d>();
        std::cout << "Tether attachment point: " << tether_attach_ << std::endl;
      }

      if (linkElem->HasElement("linear_drag"))
      {
        linear_drag_ = linkElem->GetElement("linear_drag")->Get<ignition::math::Vector3d>();
        std::cout << "Linear drag: " << linear_drag_ << std::endl;
      }

      if (linkElem->HasElement("angular_drag"))
      {
        angular_drag_ = linkElem->GetElement("angular_drag")->Get<ignition::math::Vector3d>();
        std::cout << "Angular drag: " << angular_drag_ << std::endl;
      }

      if (linkElem->HasElement("tether_drag")) // TODO should be child of gazebo element, not link element
      {
        tether_drag_ = linkElem->GetElement("tether_drag")->Get<double>();
        std::cout << "Tether drag: " << tether_drag_ << std::endl;
      }

      if (linkElem->HasElement("surface")) // TODO should be child of gazebo element, not link element
      {
        surface_ = linkElem->GetElement("surface")->Get<double>();
        std::cout << "Surface: " << surface_ << std::endl;
      }
    }

    base_link_ = model->GetLink(link_name);
    GZ_ASSERT(base_link_ != nullptr, "Missing link");

    // Listen for the update event. This event is broadcast every simulation iteration.
    update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&OrcaDragPlugin::OnUpdate, this, _1));

    std::cout << "-----------------------------------------" << std::endl;
    std::cout << std::endl;
  }

  // Called by the world update start event, up to 1000 times per second.
  void OnUpdate(const common::UpdateInfo& /*info*/)
  {
    ignition::math::Vector3d linear_velocity = base_link_->RelativeLinearVel();
    ignition::math::Vector3d angular_velocity = base_link_->RelativeAngularVel();

    ignition::math::Vector3d drag_force;
    drag_force.X() = linear_velocity.X() * fabs(linear_velocity.X()) * -linear_drag_.X();
    drag_force.Y() = linear_velocity.Y() * fabs(linear_velocity.Y()) * -linear_drag_.Y();
    drag_force.Z() = linear_velocity.Z() * fabs(linear_velocity.Z()) * -linear_drag_.Z();
    base_link_->AddLinkForce(drag_force, center_of_mass_);

    ignition::math::Vector3d drag_torque;
    drag_torque.X() = angular_velocity.X() * fabs(angular_velocity.X()) * -angular_drag_.X();
    drag_torque.Y() = angular_velocity.Y() * fabs(angular_velocity.Y()) * -angular_drag_.Y();
    drag_torque.Z() = angular_velocity.Z() * fabs(angular_velocity.Z()) * -angular_drag_.Z();
    base_link_->AddRelativeTorque(drag_torque); // ODE adds torque at the center of mass

    // Tether drag only accounts for motion in x (forward/reverse)
    ignition::math::Vector3d tether_force;
    double depth = surface_ - base_link_->WorldPose().Pos().Z();
    tether_force.X() = linear_velocity.X() * fabs(linear_velocity.X()) * depth * -tether_drag_;
    tether_force.Y() = 0;
    tether_force.Z() = 0;
    base_link_->AddLinkForce(tether_force, tether_attach_);
  }
};

GZ_REGISTER_MODEL_PLUGIN(OrcaDragPlugin)

}


/*
 *
 * Copyright (c) Toon Knapen, Karl Meerbergen & Kresimir Fresl 2003
 *
 * Permission to copy, modify, use and distribute this software
 * for any non-commercial or commercial purpose is granted provided
 * that this license appear on all copies of the software source code.
 *
 * Authors assume no responsibility whatsoever for its use and makes
 * no guarantees about its quality, correctness or reliability.
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_GEQRF_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_GEQRF_HPP

#include <complex>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>
// #include <boost/numeric/bindings/traits/std_vector.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // QR factorization of a general m x n matrix  A = Q * R
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * geqrf() computes the QR factorization of a rectangular matrix
     * A = Q *  R,  where Q is a M x min(M,N) matrix with orthogonal
     * and normalized column (i.e. herm(Q) $ Q = I) and R is a
     * min(M,N) x N upper triangular matrix.
     *
     * On return of geqrf, the elements on and above the diagonal of
     * A contain the min(M,N) x N upper trapezoidal matrix R (R is
     * upper triangular if  m  >=  n);  the elements below the diagonal,
     * with the array TAU, represent the orthogonal matrix Q as a product
     * of min(M,N) elementary reflectors.
     */

    namespace detail {

      inline
      void geqrf (int const m, int const n,
                 float* a, int const lda,
                 float* tau, float* work, int const lwork, int& info)
      {
        LAPACK_SGEQRF (&m, &n, a, &lda, tau, work, &lwork, &info);
      }

      inline
      void geqrf (int const m, int const n,
                 double* a, int const lda,
                 double* tau, double* work, int const lwork, int& info)
      {
        LAPACK_DGEQRF (&m, &n, a, &lda, tau, work, &lwork, &info);
      }

      inline
      void geqrf (int const m, int const n,
                  std::complex<float>* a, int const lda,
                  std::complex<float>* tau, std::complex<float>* work,
		  int const lwork, int& info)
      {
        LAPACK_CGEQRF (&m, &n,
                      reinterpret_cast<fcomplex_t*> (a), &lda,
                      reinterpret_cast<fcomplex_t*> (tau),
                      reinterpret_cast<fcomplex_t*> (work), &lwork, &info );
      }

      inline
      void geqrf (int const m, int const n,
                  std::complex<double>* a, int const lda,
                  std::complex<double>* tau, std::complex<double>* work,
		  int const lwork, int& info)
      {
        LAPACK_ZGEQRF (&m, &n,
                      reinterpret_cast<dcomplex_t*> (a), &lda,
                      reinterpret_cast<dcomplex_t*> (tau),
                      reinterpret_cast<dcomplex_t*> (work), &lwork, &info );
      }

    }

    template <typename A, typename Tau, typename Work>
    inline
    int geqrf (A& a, Tau& tau, Work& work) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<A>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      assert (std::min(m,n) <= traits::vector_size (tau));
      assert (n <= traits::vector_size (work));

      int info;
      detail::geqrf (m, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (tau),
                     traits::vector_storage (work),
                     traits::vector_size (work),
                     info);
      return info;
    }

    // Computation of the QR factorization.
    // Workspace is allocated dynamically so that the optimization of
    // blas 3 calls is optimal.
    template <typename A, typename Tau>
    inline
    int geqrf (A& a, Tau& tau, optimal_workspace ) {
       typedef typename A::value_type value_type ;
       const int n = traits::matrix_size2 (a);
       traits::detail::array<value_type> work(std::max(1, n*32));
       return geqrf( a, tau, work );
    }

    // Computation of the QR factorization.
    // Workspace is allocated dynamically to its minimum size.
    // Blas 3 calls are not optimal.
    template <typename A, typename Tau>
    inline
    int geqrf (A& a, Tau& tau, minimal_workspace ) {
       typedef typename A::value_type value_type ;
       const int n = traits::matrix_size2 (a);
       traits::detail::array<value_type> work(std::max(1, n));
       return geqrf( a, tau, work );
    }

    // Computation of the QR factorization.
    // Workspace is taken from the array in workspace.
    // The calling sequence is
    // geqrf( a, tau, workspace( work ) ) where work is an array with the same value_type
    // as a.
    template <typename A, typename Tau, typename Work>
    inline
    int geqrf (A& a, Tau& tau, detail::workspace1<Work> workspace ) {
       typedef typename A::value_type value_type ;
       const int n = traits::matrix_size2 (a);
       traits::detail::array<value_type> work(std::max(1, n));
       return geqrf( a, tau, workspace.w_ );
    }

    // Function without workarray as argument
    template <typename A, typename Tau>
    inline
    int geqrf (A& a, Tau& tau) {
       return geqrf( a, tau, optimal_workspace() );
    }

  }

}}}

#endif


#include <boost/graph/fruchterman_reingold.hpp>