/**
 * Orderings for a binary problem
 */

#ifndef BP_ORDERINGS_HPP_
#define BP_ORDERINGS_HPP_

#include <vector>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cuthill_mckee_ordering.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/bandwidth.hpp>
#include "../../core/order.hpp"
#include "../../util/options.hpp"
#include "bp_instance.hpp"

#define DEFAULT_BP_ORDERING 4

using namespace std;

/** Return an ordering for a binary problem given an id */
Ordering* get_ordering_by_id_bp(int id, BPInstance* inst, Options& options);

/**
 * Ordering that runs the Cuthill-McKee heuristic to minimize bandwidth on constraints with
 * pairs of variables; ignores all other constraints.
 */
struct CuthillMcKeePairOrdering : Ordering {
	BPInstance* inst;
	vector<int> v_in_layer;   // vertex at each layer

	CuthillMcKeePairOrdering(BPInstance* _inst) : inst(_inst)
	{
		construct_ordering();
	}

	int select_next_var(int layer)
	{
		assert(layer >= 0 && layer < inst->nvars);
		return v_in_layer[layer];
	}

private:

	void construct_ordering();
};

#endif // BP_ORDERINGS_HPP_


/* =========================================================================
   Copyright (c) 2010-2016, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example blas3.cpp
*
*  In this tutorial it is shown how BLAS level 3 functionality in ViennaCL can be used.
*
*  We begin with defining preprocessor constants and including the necessary headers.
**/

//disable debug mechanisms to have a fair comparison with ublas:
#ifndef NDEBUG
 #define NDEBUG
#endif

// System headers
#include <iostream>

// ublas headers
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

// Must be set if you want to use ViennaCL algorithms on ublas objects
#define VIENNACL_WITH_UBLAS 1

// ViennaCL headers
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/tools/random.hpp"
#include "viennacl/tools/timer.hpp"

#define BLAS3_MATRIX_SIZE   400

using namespace boost::numeric;

/**
*  Later in this tutorial we will iterate over all available OpenCL devices.
*  To ensure that this tutorial also works if no OpenCL backend is activated, we need this dummy-struct.
**/
#ifndef VIENNACL_WITH_OPENCL
  struct dummy
  {
    std::size_t size() const { return 1; }
  };
#endif

/**
* We don't need additional auxiliary routines, so let us start straight away with main():
*/
int main()
{
  typedef float     ScalarType;

  viennacl::tools::timer timer;
  double exec_time;

  viennacl::tools::uniform_random_numbers<ScalarType> randomNumber;

  /**
  * Set up some ublas objects and initialize with data:
  **/
  ublas::matrix<ScalarType> ublas_A(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);
  ublas::matrix<ScalarType, ublas::column_major> ublas_B(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);
  ublas::matrix<ScalarType> ublas_C(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);
  ublas::matrix<ScalarType> ublas_C1(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);

  for (unsigned int i = 0; i < ublas_A.size1(); ++i)
    for (unsigned int j = 0; j < ublas_A.size2(); ++j)
      ublas_A(i,j) = ScalarType(10) * randomNumber();

  for (unsigned int i = 0; i < ublas_B.size1(); ++i)
    for (unsigned int j = 0; j < ublas_B.size2(); ++j)
      ublas_B(i,j) = ScalarType(10) * randomNumber();

  /**
  * Set up some ViennaCL objects. Data initialization will happen later.
  **/
  //viennacl::ocl::set_context_device_type(0, viennacl::ocl::gpu_tag());  //uncomment this is you wish to use GPUs only
  viennacl::matrix<ScalarType> vcl_A(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);
  viennacl::matrix<ScalarType, viennacl::column_major> vcl_B(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);
  viennacl::matrix<ScalarType> vcl_C(BLAS3_MATRIX_SIZE, BLAS3_MATRIX_SIZE);

  /**
  *
Matrix-matrix Products

  *
  * First compute the reference product using uBLAS:
  **/
  std::cout << "--- Computing matrix-matrix product using ublas ---" << std::endl;
  timer.start();
  ublas_C = ublas::prod(ublas_A, ublas_B);
  exec_time = timer.get();
  std::cout << " - Execution time: " << exec_time << std::endl;

  /**
  * Now iterate over all OpenCL devices in the context and compute the matrix-matrix product.
  * If the OpenCL backend is disabled, we use the dummy-struct defined above.
  **/
  std::cout << std::endl << "--- Computing matrix-matrix product on each available compute device using ViennaCL ---" << std::endl;
#ifdef VIENNACL_WITH_OPENCL
  std::vector<viennacl::ocl::device> devices = viennacl::ocl::current_context().devices();
#else
  dummy devices;
#endif

  for (std::size_t device_id=0; device_id<devices.size(); ++device_id)
  {
#ifdef VIENNACL_WITH_OPENCL
    viennacl::ocl::current_context().switch_device(devices[device_id]);
    std::cout << " - Device Name: " << viennacl::ocl::current_device().name() << std::endl;
#endif

    /**
    * Copy the data from the uBLAS objects, compute one matrix-matrix-product as a 'warm up', then take timings:
    **/
    viennacl::copy(ublas_A, vcl_A);
    viennacl::copy(ublas_B, vcl_B);
    vcl_C = viennacl::linalg::prod(vcl_A, vcl_B);
    viennacl::backend::finish();
    timer.start();
    vcl_C = viennacl::linalg::prod(vcl_A, vcl_B);
    viennacl::backend::finish();
    exec_time = timer.get();
    std::cout << " - Execution time on device (no setup time included): " << exec_time << std::endl;

    /**
    * Verify the result
    **/
    viennacl::copy(vcl_C, ublas_C1);

    std::cout << " - Checking result... ";
    bool check_ok = true;
    for (std::size_t i = 0; i < ublas_C.size1(); ++i)
    {
      for (std::size_t j = 0; j < ublas_C.size2(); ++j)
      {
        if ( std::fabs(ublas_C1(i,j) - ublas_C(i,j)) / ublas_C(i,j) > 1e-4 )
        {
          check_ok = false;
          break;
        }
      }
      if (!check_ok)
        break;
    }
    if (check_ok)
      std::cout << "[OK]" << std::endl << std::endl;
    else
      std::cout << "[FAILED]" << std::endl << std::endl;

  }

  /**
  *  That's it. A more extensive benchmark for dense BLAS routines is also available.
  **/
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;
  return EXIT_SUCCESS;
}


// Boost.Function library examples

// Copyright (C) 2001 Doug Gregor (gregod@cs.rpi.edu)
//
// Permission to copy, use, sell and distribute this software is granted
// provided this copyright notice appears in all copies.
// Permission to modify the code and to distribute modified code is granted
// provided this copyright notice appears in all copies, and a notice
// that the code was modified is included with the copyright notice.
//
// This software is provided "as is" without express or implied warranty,
// and with no claim as to its suitability for any purpose.

// For more information, see http://www.boost.org

#include <iostream>
#include <boost/function.hpp>

struct int_div {
  float operator()(int x, int y) const { return ((float)x)/y; };
};

int
main()
{
  boost::function<float, int, int> f;
  f = int_div();

  std::cout << f(5, 3) << std::endl; // 1.66667

  return 0;
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
Matrix<int, 3, 4, ColMajor> Acolmajor;
Acolmajor << 8, 2, 2, 9,
             9, 1, 4, 4,
	     3, 5, 4, 5;
cout << "The matrix A:" << endl;
cout << Acolmajor << endl << endl;

cout << "In memory (column-major):" << endl;
for (int i = 0; i < Acolmajor.size(); i++)
  cout << *(Acolmajor.data() + i) << "  ";
cout << endl << endl;

Matrix<int, 3, 4, RowMajor> Arowmajor = Acolmajor;
cout << "In memory (row-major):" << endl;
for (int i = 0; i < Arowmajor.size(); i++)
  cout << *(Arowmajor.data() + i) << "  ";
cout << endl;

}
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
#include "intersect_ray.h"
#include "aabb.h"
#include "aabb_binary.h"
#include <Eigen/Dense>
#include <geogram/mesh/mesh_geometry.h>
#include <limits>
#include <numeric>
#undef IGL_STATIC_LIBRARY
#include <igl/ray_mesh_intersect.h>
////////////////////////////////////////////////////////////////////////////////

bool triangle_intersect_ray(const Eigen::Vector3d &a,
                            const Eigen::Vector3d &b,
                            const Eigen::Vector3d &c,
                            const Eigen::Vector3d &ray_origin,
                            const Eigen::Vector3d &ray_direction,
                            Eigen::Vector3d &hit_position,
                            Eigen::Vector3d &hit_normal,
                            double &hit_param)
{
    igl::Hit hit;
    Eigen::Matrix<double, 3, 3> V;
    V.row(0) = a.transpose();
    V.row(1) = b.transpose();
    V.row(2) = c.transpose();
    Eigen::RowVector3i F(0, 1, 2);
    auto ret = igl::ray_mesh_intersect(ray_origin, ray_direction, V, F, hit);
    if (ret) {
        hit_param = hit.t;
        hit_position = ray_origin + hit_param * ray_direction;
        hit_normal = (b - a).cross(c - a).normalized();
    }
    return ret;
}

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

bool box_intersect_ray(const Eigen::AlignedBox3d &box,
                       const Eigen::Vector3d &ray_origin,
                       const Eigen::Vector3d &ray_direction)
{
    double tmin = 0;
    double tmax = std::numeric_limits<double>::infinity();
    for (int dim = 0; dim < 3; ++dim) {
        if (std::abs(ray_direction(dim)) < 1e-5) {
            if (ray_origin[dim] < box.min()[dim] || ray_origin[dim] > box.max()[dim]) {
                return false;
            }
        }
        else {
            double t1 = (box.min()[dim] - ray_origin[dim]) / ray_direction[dim];
            double t2 = (box.max()[dim] - ray_origin[dim]) / ray_direction[dim];
            tmin = std::max(std::min(t1, t2), tmin);
            tmax = std::min(std::max(t1, t2), tmax);
        }
    }
    return (tmin <= tmax);
}

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

template <>
bool aabb_intersect_ray<AABBTree>(const Eigen::MatrixXd &V,
                                  const Eigen::MatrixXi &F,
                                  const AABBTree &tree,
                                  const Eigen::Vector3d &ray_origin,
                                  const Eigen::Vector3d &ray_direction,
                                  Eigen::Vector3d &hit_position,
                                  Eigen::Vector3d &hit_normal,
                                  double &hit_param)
{
    return tree.shoot_ray(V, F, ray_origin, ray_direction, hit_position, hit_normal, hit_param);
}

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

template <>
bool aabb_intersect_ray<AABBTreeBinary>(const Eigen::MatrixXd &V,
                                        const Eigen::MatrixXi &F,
                                        const AABBTreeBinary &tree,
                                        const Eigen::Vector3d &ray_origin,
                                        const Eigen::Vector3d &ray_direction,
                                        Eigen::Vector3d &hit_position,
                                        Eigen::Vector3d &hit_normal,
                                        double &hit_param)
{
    return tree.shoot_ray(V, F, ray_origin, ray_direction, hit_position, hit_normal, hit_param);
}

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

template <>
bool aabb_intersect_ray<igl::AABB<Eigen::MatrixXd, 3>>(const Eigen::MatrixXd &V,
                                                       const Eigen::MatrixXi &F,
                                                       const igl::AABB<Eigen::MatrixXd, 3> &tree,
                                                       const Eigen::Vector3d &ray_origin,
                                                       const Eigen::Vector3d &ray_direction,
                                                       Eigen::Vector3d &hit_position,
                                                       Eigen::Vector3d &hit_normal,
                                                       double &hit_param)
{
    igl::Hit hit;
    bool ret = tree.intersect_ray(V, F, ray_origin.transpose(), ray_direction.transpose(), hit);
    if (ret) {
        hit_param = hit.t;
        hit_position = ray_origin + hit.t * ray_direction;
        Eigen::Vector3d a = V.row(F.row(hit.id)[0]).transpose();
        Eigen::Vector3d b = V.row(F.row(hit.id)[1]).transpose();
        Eigen::Vector3d c = V.row(F.row(hit.id)[2]).transpose();
        hit_normal = (b - a).cross(c - a).normalized();
    }
    return ret;
}

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

template <>
bool aabb_intersect_ray<igl::embree::EmbreeIntersector>(const Eigen::MatrixXd &V,
                                                        const Eigen::MatrixXi &F,
                                                        const igl::embree::EmbreeIntersector &tree,
                                                        const Eigen::Vector3d &ray_origin,
                                                        const Eigen::Vector3d &ray_direction,
                                                        Eigen::Vector3d &hit_position,
                                                        Eigen::Vector3d &hit_normal,
                                                        double &hit_param)
{
    igl::Hit hit;
    bool ret = tree.intersectRay(ray_origin.transpose().cast<float>(),
                                 ray_direction.transpose().cast<float>(), hit);
    if (ret) {
        hit_param = hit.t;
        hit_position = ray_origin + hit.t * ray_direction;
        Eigen::Vector3d a = V.row(F.row(hit.id)[0]).transpose();
        Eigen::Vector3d b = V.row(F.row(hit.id)[1]).transpose();
        Eigen::Vector3d c = V.row(F.row(hit.id)[2]).transpose();
        hit_normal = (b - a).cross(c - a).normalized();
    }
    return ret;
}

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

template <>
bool aabb_intersect_ray<GEO::MeshFacetsAABB>(const Eigen::MatrixXd &V,
                                             const Eigen::MatrixXi &F,
                                             const GEO::MeshFacetsAABB &tree,
                                             const Eigen::Vector3d &ray_origin,
                                             const Eigen::Vector3d &ray_direction,
                                             Eigen::Vector3d &hit_position,
                                             Eigen::Vector3d &hit_normal,
                                             double &hit_param)
{
    // Multiply by big constant because AABB has a segment isect routine
    // (not ray isect routine), so it would ignore intersections further
    // away than (R.origin + R.direction), and we want them!
    GEO::vec3 origin(ray_origin.data());
    GEO::vec3 direction(ray_direction.data());
    GEO::vec3 p2 = origin + 10000.0 * direction;
    double t;
    GEO::index_t f;
    if (tree.segment_nearest_intersection(origin, p2, t, f)) {
        t *= 10000.0;
        if (t > 0) {
            hit_param = t;
            hit_position = ray_origin + t * ray_direction;
            GEO::vec3 normal = GEO::normalize(GEO::Geom::mesh_facet_normal(*tree.mesh(), f));
            hit_normal << normal[0], normal[1], normal[2];
            return true;
        }
    }
    return false;
}


// Standard headers
#include "rclcpp/rclcpp.hpp"
#include <cstdlib>
#include <cmath>
#include <random>
#include <vector>
#include <algorithm>
#include <iostream>
#include <thread>
#include <functional>
#include <limits>
#include <signal.h>
#include "cmath"
#include <iterator>
#include <boost/range/combine.hpp>
#include <unordered_set>
#include <Eigen/Dense>

#include <cinematography_msgs/msg/drone_state.hpp>
#include <cinematography_msgs/msg/multi_do_farray.hpp>
#include <cinematography_msgs/msg/multi_dof.hpp>
#include <cinematography_msgs/msg/artistic_spec.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/msg/pose_array.hpp>
#include "tsdf_package_msgs/msg/tsdf.hpp"
#include "tsdf_package_msgs/msg/voxel.hpp"
#include "optimize_drone_path.cuh"

#include <Eigen/Dense>

#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"

#define DEG_TO_RAD(d)   d*M_PI/180
#define RAD_TO_DEG(r)   r*180/M_PI

using namespace std;
using std::placeholders::_1;

class MotionPlanner : public rclcpp::Node {
private:
    rclcpp::Publisher<cinematography_msgs::msg::MultiDOFarray>::SharedPtr drone_traj_pub;
    rclcpp::Publisher<cinematography_msgs::msg::MultiDOFarray>::SharedPtr ideal_traj_pub;
    rclcpp::Subscription<cinematography_msgs::msg::MultiDOFarray>::SharedPtr actor_traj_sub;
    rclcpp::Subscription<tsdf_package_msgs::msg::Tsdf>::SharedPtr tsdf_sub;

    // // TODO: change the bound to something meaningful
    // double x__low_bound__global = -200, x__high_bound__global = 200;
    // double y__low_bound__global = -200 , y__high_bound__global = 200;
    // double z__low_bound__global = 0, z__high_bound__global = 40;
    // double sampling_interval__global = 0.5;
    // double v_max__global = 3, a_max__global = 5;
    // float g_planning_budget = 4;
    // std::string motion_planning_core_str;

    // double drone_height__global = 0.6;
    // double drone_radius__global = 2;

    // Define default artistic constraints
    double viewport_heading = M_PI / 3;
    double viewport_pitch = M_PI / 4;
    double viewport_distance = 3;

    msr::airlib::MultirotorRpcLibClient* airsim_client;
    std::string airsim_hostname;
    std::string vehicle_name = "drone_1";

    tf2_ros::Buffer* tf_buffer;
    tf2_ros::TransformListener* tf_listener;

    std::string world_frame;
    std::string drone_frame;

    double truncation_distance = 4;
    double voxel_size = .5;
    bool received_first_msg = false;

    std::vector<Voxel> voxels_set[NUM_BUCKETS];
    int voxels_set_size;

    std::chrono::time_point<std::chrono::high_resolution_clock> global_start;
    int global_iterations = 0;
    double average = 0;
    bool first_time = true;

    int MAX_ITERATIONS;

    // Calculate an ideal drone trajectory using a given actor trajectory (both in NED and radians)
    cinematography_msgs::msg::MultiDOFarray calc_ideal_drone_traj(const cinematography_msgs::msg::MultiDOFarray &  actor_traj);

    void update_traj_position_derivatives(cinematography_msgs::msg::MultiDOFarray& drone_traj);

    // Moves the starting point to the drone's current position, leave the final point in place, and all the intermediate points are stretched in between
    void move_traj_start(cinematography_msgs::msg::MultiDOFarray& drone_traj, cinematography_msgs::msg::MultiDOF& drone_pose);

    void face_actor(cinematography_msgs::msg::MultiDOFarray& drone_traj, const cinematography_msgs::msg::MultiDOFarray& actor_traj);

    size_t get_bucket(const Eigen::Matrix<double, 3, 1> & position);

    double floor_fun(const double & x, const double & scale);

    /*
    * Given world point return center of volume given by volume_size
    */
    Eigen::Matrix<double, 3, 1> get_volume_center_from_point(Eigen::Matrix<double, 3, 1> point, double volume_size);

    /*
    * Check if two Eigen::Matrix<double, 3, 1> are equal
    */
    bool check_floating_point_vectors_equal(Eigen::Matrix<double, 3, 1> A, Eigen::Matrix<double, 3, 1> B, double epsilon);

    /*
    * Get voxels between point start and point end
    * For more information on voxel traversal algo: http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.42.3443&rep=rep1&type=pdf
    */
    std::vector<Eigen::Matrix<double, 3, 1>> get_voxels(const cinematography_msgs::msg::MultiDOF point_start, const cinematography_msgs::msg::MultiDOF point_end, const double & volume_size);

    /*
    * Cost of sdf value
    */
    inline double get_cost(const double & sdf){
        if(fabs(sdf) >= truncation_distance){
            return 0;
        }
        else if(sdf > 0){
            return pow((sdf - truncation_distance), 2) / (2* truncation_distance);
        }else{
            return sdf * -1 + .5 * truncation_distance;
        }

    }

    inline double get_voxel_cost(const Eigen::Matrix<double, 3, 1> & voxel_pos){

        size_t bucket = get_bucket(voxel_pos);

        for(Voxel v : voxels_set[bucket]){
            if(check_floating_point_vectors_equal(voxel_pos, v.position, voxel_size)){
                return get_cost(v.sdf);
            }
        }

        return 0; //voxel does not exist so it is in free space(or inside an object) and return 0 cost
    }

    /*
    * Compute cost gradient for a voxel specified by voxel_pos. Check cost values of voxel at voxel_pos and voxels around
    */
    Eigen::Matrix<double, 3, 1> get_voxel_cost_gradient(const Eigen::Matrix<double, 3, 1> & voxel_pos);

    double get_segment_cost(const cinematography_msgs::msg::MultiDOF point_start, const cinematography_msgs::msg::MultiDOF point_end, bool gradient=false);

    Eigen::Matrix<double, 3, 1> get_segment_cost_gradient(const cinematography_msgs::msg::MultiDOF point_start,
                    const cinematography_msgs::msg::MultiDOF point_end, bool gradient=false);

    //======================VVV==Cost functions==VVV====================================

    double traj_smoothness(const cinematography_msgs::msg::MultiDOFarray& drone_traj, int delta_t);

    double shot_quality(const cinematography_msgs::msg::MultiDOFarray& drone_traj, cinematography_msgs::msg::MultiDOFarray& ideal_traj);

    double obstacle_avoidance(const cinematography_msgs::msg::MultiDOFarray& drone_traj);

    double occlusion_avoidance(const cinematography_msgs::msg::MultiDOFarray& drone_traj, cinematography_msgs::msg::MultiDOFarray& actor_traj);

    double traj_cost_function(const cinematography_msgs::msg::MultiDOFarray& drone_traj, cinematography_msgs::msg::MultiDOFarray& actor_traj, cinematography_msgs::msg::MultiDOFarray& ideal_traj, double t);

    //======================^^^==Cost functions==^^^====================================

    //======================VVV==Gradient functions==VVV====================================

    // TODO: Implement gradient equivalents of all the above, and hessian approximations (A_smooth + delta_1 * A_shot)
    Eigen::Matrix<double, Eigen::Dynamic, 3> traj_smoothness_gradient(const cinematography_msgs::msg::MultiDOFarray& drone_traj, double delta_t, const Eigen::MatrixXd  & K, const Eigen::MatrixXd  & K0, const Eigen::MatrixXd  & K1, const Eigen::MatrixXd  & K2, const Eigen::MatrixXd  & A);

    Eigen::Matrix<double, Eigen::Dynamic, 3> shot_quality_gradient(const cinematography_msgs::msg::MultiDOFarray& drone_traj, cinematography_msgs::msg::MultiDOFarray& ideal_traj, const Eigen::MatrixXd & A);

    Eigen::Matrix<double, Eigen::Dynamic, 3>  obstacle_avoidance_gradient(const cinematography_msgs::msg::MultiDOFarray& drone_traj);

    Eigen::Matrix<double, Eigen::Dynamic, 3>  occlusion_avoidance_gradient(const cinematography_msgs::msg::MultiDOFarray& drone_traj, const cinematography_msgs::msg::MultiDOFarray& actor_traj);

    //======================^^^==Gradient functions==^^^====================================

    void optimize_trajectory(cinematography_msgs::msg::MultiDOFarray& drone_traj, const cinematography_msgs::msg::MultiDOFarray& actor_traj);

    //bool get_trajectory_fun(airsim_ros_pkgs::get_trajectory::Request &req, airsim_ros_pkgs::get_trajectory::Response &res)
    // Get actor's predicted trajectory (in NED and radians)
    void get_actor_trajectory(const cinematography_msgs::msg::MultiDOFarray::SharedPtr actor_traj);

    void tsdf_callback(tsdf_package_msgs::msg::Tsdf::SharedPtr tsdf);

public:
    MotionPlanner();

    ~MotionPlanner() {
        delete airsim_client;
    }
};


/**
 * Beck Pang, 20180926, use the four features to build a basic perspective controller, works.
 * Reference: Chaumette, François, and Seth Hutchinson. "Visual servo control. I. Basic approaches."
 */

#include <iostream>
#include <ros/ros.h>
#include <cmath>
#include <Eigen/Dense>
#include <geometry_msgs/Twist.h>
#include "rm_cv/vertice.h"
#include "VisualServoController.h"
#include "camera_model/camera_models/CameraFactory.h"

using namespace std;
using namespace Eigen;

ros::Publisher cmd_pub, debug_pub_typeI, debug_pub_typeII;
string cv_topic, publisher_topic, debug_typeI_topic, debug_typeII_topic;

// Camera
std::string cfg_file_name = "/home/nvidia/ws/src/6_controller/gimbal_controller/cfg/camera_tracking_camera_calib.yaml";
camera_model::CameraPtr m_camera;

// Controller
int n = 4;
int m = 2;
double Kp = 1.0;

double target_Z = 1;
double pixel_x_max = 640;
double pixel_y_max = 512;
double pixel_dx = 68;
double pixel_dy = 25;

MatrixXd target_image_frame(n, m);

VisualServoController ctl;

void
publish_message(const double wz, const ros::Publisher& pub)
{
    geometry_msgs::Twist vel_msg;
    vel_msg.angular.z = wz;
    pub.publish(vel_msg);
}

// No need to reject noise
void
visual_servo_cb(const rm_cv::vertice::ConstPtr cv_ptr)
{
    // convert the pixel value to image coordinate value
    MatrixXd input_pixel(n, m);
    Vector3d input_pixel_output[n];
    MatrixXd input_image_frame(n, m);
	int i;
    for ( i = 0; i < n; ++i) {
        input_pixel(i, 0) = cv_ptr->vertex[i].x;
        input_pixel(i, 1) = cv_ptr->vertex[i].y;
        m_camera->liftSphere(input_pixel.row(i), input_pixel_output[i] );
        input_image_frame.row(i) << input_pixel_output[i](0), input_pixel_output[i](1);
    }
    std::cout << "input in image frame " << std::endl << input_image_frame << std::endl;

    // use the image frame coordinate value to control
    ctl.setKp(Kp);
    VectorXd ctl_val = ctl.control(input_image_frame);

    // publish the message in camera y axis
    publish_message(ctl_val(1), cmd_pub);

    /*
    // For comparision only
    VectorXd ctl_val_typeI = ctl.control_type_current(input_image_frame);
    VectorXd ctl_val_typeII = ctl.control_type_target(input_image_frame);
    publish_message(ctl_val_typeI(1), debug_pub_typeI);
    publish_message(ctl_val_typeII(1), debug_pub_typeII);
    */
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "four_point_visual_servo");
    ros::NodeHandle nh("~");

    nh.param("Kp", Kp, 1.0);
    nh.param("cv_topic", cv_topic, string("/detected_vertice"));
    nh.param("publisher_topic", publisher_topic, string("/cmd_vel"));
    nh.param("debug_typeI_topic", debug_typeI_topic, string("/cmd_vel_type_current"));
    nh.param("debug_typeII_topic", debug_typeII_topic, string("/cmd_vel_type_target"));

    ros::Subscriber sub = nh.subscribe(cv_topic, 10, visual_servo_cb);
    cmd_pub = nh.advertise<geometry_msgs::Twist>(publisher_topic, 10);
	debug_pub_typeI = nh.advertise<geometry_msgs::Twist>(debug_typeI_topic, 10);
	debug_pub_typeII = nh.advertise<geometry_msgs::Twist>(debug_typeII_topic, 10);

    // create a camera model
    m_camera = camera_model::CameraFactory::instance()->generateCameraFromYamlFile(cfg_file_name);

	// setup the target coordinate
	MatrixXd target_pixel(n, m);
	MatrixXd target_image_frame(n, m);

    double pixel_x_down= (pixel_x_max - pixel_dx) * 0.5;
    double pixel_x_top_= (pixel_x_max + pixel_dx) * 0.5;
    double pixel_y_down= (pixel_y_max - pixel_dy) * 0.5;
    double pixel_y_top_= (pixel_y_max + pixel_dy) * 0.5;

	target_pixel <<
            pixel_x_down, pixel_y_down,
		    pixel_x_down, pixel_y_top_,
		    pixel_x_top_, pixel_y_down,
		    pixel_x_top_, pixel_y_top_; // 1000 mm

	Vector3d target_pixel_output[n];

	for	(int i=0; i < n; ++i) {
	    m_camera->liftSphere(target_pixel.row(i), target_pixel_output[i] );
        target_image_frame.row(i) << target_pixel_output[i](0), target_pixel_output[i](1);
	}
	std::cout << "target in image frame " << std::endl << target_image_frame << std::endl;

	ctl.setTarget(target_image_frame);

    ros::spin();
}


#include <benchmark/benchmark.h>
#include <random>
#include <complex>
#include <boost/math/fft/bsl_backend.hpp>
#include <boost/math/fft/fftw_backend.hpp>
#include <boost/math/fft/gsl_backend.hpp>
#include <boost/math/constants/constants.hpp>

#include "fft_test_helpers.hpp"

std::default_random_engine gen;
std::uniform_real_distribution<double> distribution;

typedef std::complex<double> cd;
std::vector<cd> random_vec(size_t N)
{
    std::vector<cd> V(N);
    for (auto& x : V)
      x = distribution(gen);
    return V;
}

void bench_bsl_dit(benchmark::State& state)
{
    auto A = random_vec(state.range(0));
    for (auto _ : state)
    {
        using boost::math::fft::dft_forward;
        dft_forward<boost::math::fft::test_dft_power2_dit>(A.data(),A.data()+A.size(),A.data());
    }
    state.SetComplexityN(state.range(0));
}
void bench_bsl_dif(benchmark::State& state)
{
    auto A = random_vec(state.range(0));
    for (auto _ : state)
    {
        using boost::math::fft::dft_forward;
        dft_forward<boost::math::fft::test_dft_power2_dif>(A.data(),A.data()+A.size(),A.data());
    }
    state.SetComplexityN(state.range(0));
}
void bench_gsl(benchmark::State& state)
{
    auto A = random_vec(state.range(0));
    for (auto _ : state)
    {
        using boost::math::fft::dft_forward;
        dft_forward<boost::math::fft::gsl_dft>(A.data(),A.data()+A.size(),A.data());
    }
    state.SetComplexityN(state.range(0));
}
void bench_fftw(benchmark::State& state)
{
    auto A = random_vec(state.range(0));
    for (auto _ : state)
    {
        using boost::math::fft::dft_forward;
        dft_forward<boost::math::fft::fftw_dft>(A.data(),A.data()+A.size(),A.data());
    }
    state.SetComplexityN(state.range(0));
}

BENCHMARK(bench_bsl_dit)
    ->RangeMultiplier(8)
    ->Range(1 << 5, 1 << 20)
    ->Complexity(benchmark::oNLogN);

BENCHMARK(bench_bsl_dif)
    ->RangeMultiplier(8)
    ->Range(1 << 5, 1 << 20)
    ->Complexity(benchmark::oNLogN);

BENCHMARK(bench_gsl)
    ->RangeMultiplier(8)
    ->Range(1 << 5, 1 << 20)
    ->Complexity(benchmark::oNLogN);

BENCHMARK(bench_fftw)
    ->RangeMultiplier(8)
    ->Range(1 << 5, 1 << 20)
    ->Complexity(benchmark::oNLogN);

BENCHMARK_MAIN();


// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @file           coordinate.cpp
*   @brief          飞行器用到的各种坐标系变换。
*   @details        现有：地球坐标系，导航坐标系，机体坐标系。
                    地球坐标系采用WGS-84坐标系各项参数。
					地球空间直角坐标系原点为参考椭球的中心，X轴和Y轴位于赤道平面，X轴通过零子午线，Z轴与椭球极轴一致。
					导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地下。
					机体坐标系原点位于质心点，X轴指向机头前方，Y轴指向右侧，Z轴指向下方。
					所有坐标系均为右手系。
*   @author         LiDaiwei
*   @date           20191203
*   @version        1.0.0.1
*   @par Copyright
*                   LiDaiwei
*   @par History
*                   1.0.0.1: LiDaiwei, 20191203, 首次创建
*

*/

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @name           头文件。
*   @{
*/

#include "coordinate.h"
#include <algorithm>
//#include <Eigen/Dense>

#include<iostream>
using namespace std;
using namespace Eigen;
/** @}  */

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          计算重力加速度。
*   @details        计算重力加速度。
*   @param[out]     gravity         精确重力
*   @param[in]      latitude        纬度(角度)
*   @retval         0               正常
*   @retval         1               错误
*/
int accuraty_gravity (
	double* gravity,
	const double latitude)
{
	double L=0;
	deg2rad(&L,latitude);
	*gravity=978.03267714*(1+0.00193185138639*sin(L)*sin(L))/
		sqrt(1-0.00669437999013*sin(L)*sin(L));
	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          计算地球主曲率半径。
*   @details        参考椭球子午圈上各点曲率半径RM和卯酉圈（它所在的平面与子午面垂直）上各点的曲率半径RN称为主曲率半径。
*   @param[out]     RM              子午圈曲率半径
*   @param[out]     RN              卯酉圈曲率半径
*   @param[in]      latitude        纬度(角度)
*   @retval         0               正常
*   @retval         1               错误
*/
int earth_curvature_radius (
	double* RM,
	double* RN,
	const double latitude)
{
	double L=0;
	deg2rad(&L,latitude);
	*RM=earth_radius*(1-2*earth_ellipticity+3*earth_ellipticity*sin(L)*sin(L));
	*RN=earth_radius*(1+earth_ellipticity*sin(L)*sin(L));

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          地球空间直角坐标系转换到经纬度高度坐标系。
*   @details        地球空间直角坐标系原点为参考椭球的中心，X轴和Y轴位于赤道平面，X轴通过零子午线，Z轴与椭球极轴一致。
*   @param[out]     longitude       经度（角度）
*   @param[out]     latitude        纬度（角度）
*   @param[out]     height          高度
*   @param[in]      x               地球空间直角坐标系X坐标
*   @param[in]      y               地球空间直角坐标系Y坐标
*   @param[in]      z               地球空间直角坐标系Z坐标
*   @retval         0               正常
*   @retval         1               错误
*/
int xyz_to_llh(
	double* longitude,
	double* latitude,
	double* height,
	const double x,
	const double y,
	const double z)
{
	double lon=0,lat=0;
	lon=atan2(y,x);
	//lon = lon < 0 ? (M_PI + lon) : lon;
	rad2deg(longitude,lon);
	//迭代法求纬度和高度
	double HpRN_ip1=0,RN_ip1=0;
	double lat_i=0,lat_ip1=0;
	lat_i=atan(z/(pow((1-earth_ellipticity),2)*sqrt(x*x+y*y)));
	int num = 0;
	do{
		HpRN_ip1=x/(cos(lat_i)*cos(lon));
		RN_ip1=earth_radius/sqrt(pow(cos(lat_i),2)+(1-pow(earth_e1,2))*pow(sin(lat_i),2));
		lat_ip1=atan(HpRN_ip1*z/((HpRN_ip1-RN_ip1*pow(earth_e1,2))*sqrt(x*x+y*y)));
		num++;
	}while (abs( lat_ip1-lat_i)>1e-6 && num<1e3);

	lat=lat_ip1;
	rad2deg(latitude,lat);
	*height=HpRN_ip1-RN_ip1;
	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          经纬度高度坐标系转换到地球空间直角坐标系。
*   @details        地球空间直角坐标系原点为参考椭球的中心，X轴和Y轴位于赤道平面，X轴通过零子午线，Z轴与椭球极轴一致。
*   @param[out]      x               地球空间直角坐标系X坐标
*   @param[out]      y               地球空间直角坐标系Y坐标
*   @param[out]      z               地球空间直角坐标系Z坐标
*   @param[in]       longitude       经度（角度）
*   @param[in]       latitude        纬度（角度）
*   @param[in]       height          高度
*   @retval          0               正常
*   @retval          1               错误
*/
int llh_to_xyz(
	double* x,
	double* y,
	double* z,
	const double longitude,
	const double latitude,
	const double height)
{
	double lon=0,lat=0,RN=0,RM;
	deg2rad(&lon,longitude);
	deg2rad(&lat,latitude);
	earth_curvature_radius(&RM,&RN,latitude);

	*x=(height+RN)*cos(lat)*cos(lon);
	*y=(height+RN)*cos(lat)*sin(lon);
	*z=(RN*pow((1-earth_ellipticity),2)+height)*sin(lat);
	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          地球经纬度坐标系转换到导航坐标系的旋转矩阵。
*   @details        导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地下。
*   @param[out]     R_en            旋转矩阵:3x3,正交
*   @param[in]      longitude0      导航坐标系参考点经度(角度)
*   @param[in]      latitude0       导航坐标系参考点纬度(角度)
*   @retval         0               正常
*   @retval         1               错误
*/
int rotation_earth_to_navigation(
	Matrix3d* R_en,
	const double longitude0,
	const double latitude0)
{
	double lon = 0, L = 0;
	deg2rad(&lon, longitude0); deg2rad(&L, latitude0);
	//先转到天东北系
	Matrix3d R_en2;
	R_en2(0, 0) = cos(L) * cos(lon);
	R_en2(0, 1) = cos(L) * sin(lon);
	R_en2(0, 2) = sin(L);
	R_en2(1, 0) = -sin(lon);
	R_en2(1, 1) = cos(lon);
	R_en2(1, 2) = 0;
	R_en2(2, 0) = -sin(L) * cos(lon);
	R_en2(2, 1) = -sin(L) * sin(lon);
	R_en2(2, 2) = cos(L);
	//再从天东北转到北东地
	Matrix3d R_n2n;
	R_n2n << 0, 0, 1,
		0, 1, 0,
		-1, 0, 0;
	*R_en = R_n2n * R_en2;
	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          导航坐标系转换到地球经纬度坐标系的旋转矩阵。
*   @details        导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地下。
*   @param[out]     R_ne            旋转矩阵:3x3,正交
*   @param[in]      longitude0      导航坐标系参考点经度(角度)
*   @param[in]      latitude0       导航坐标系参考点纬度(角度)
*   @retval         0               正常
*   @retval         1               错误
*/
int rotation_navigation_to_earth(
	Matrix3d* R_ne,
	const double longitude0,
	const double latitude0)
{
	Matrix3d R_en;
	rotation_earth_to_navigation(&R_en, longitude0, latitude0);
	*R_ne = R_en.transpose();
	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          地球经纬度坐标系转换到导航坐标系。
*   @details        导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地面。
*   @param[out]     north            导航坐标系内东向坐标
*   @param[out]     east           导航坐标系内北向坐标
*   @param[out]     downward          导航坐标系内天向坐标
*   @param[in]      longitude       机体经度（角度）
*   @param[in]      latitude        机体纬度（角度）
*   @param[in]      height          机体高度
*   @param[in]      longitude0      导航坐标系参考点经度（角度）
*   @param[in]      latitude0       导航坐标系参考点纬度（角度）
*   @param[in]      height0         导航坐标系参考点高度
*   @retval         0               正常
*   @retval         1               错误
*/
int earth_to_navigation(
	double* north,
	double* east,
	double* downward,
	const double longitude,
	const double latitude,
	const double height,
	const double longitude0,
	const double latitude0,
	const double height0)
{

	Matrix3d R_en;
	rotation_earth_to_navigation(&R_en, longitude0, latitude0);
	Vector3d Pe, Pn, t;
	llh_to_xyz(&Pe(0), &Pe(1), &Pe(2), longitude, latitude, height);
	llh_to_xyz(&t(0), &t(1), &t(2), longitude0, latitude0, height0);

	Pn = R_en * (Pe - t);

	*north = Pn(0);
	*east = Pn(1);
	*downward = Pn(2);
	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          导航坐标系转换到地球经纬度坐标系。
*   @details        导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地面。
*   @param[out]     longitude       机体经度（角度）
*   @param[out]     latitude        机体纬度（角度）
*   @param[out]     height          机体高度
*   @param[in]      north            机体导航坐标系内东向坐标
*   @param[in]      east           机体导航坐标系内北向坐标
*   @param[in]      downward          机体导航坐标系内天向坐标
*   @param[in]      longitude0      导航坐标系参考点经度（角度）
*   @param[in]      latitude0       导航坐标系参考点纬度（角度）
*   @param[in]      height0         导航坐标系参考点高度
*   @retval         0               正常
*   @retval         1               错误
*/
int navigation_to_earth(
	double* longitude,
	double* latitude,
	double* height,
	const double north,
	const double east,
	const double downward,
	const double longitude0,
	const double latitude0,
	const double height0)
{
	Matrix3d R_ne;
	rotation_navigation_to_earth(&R_ne, longitude0, latitude0);
	Vector3d Pe, Pn, t;
	Pn << north, east, downward;//在导航系内的坐标
	llh_to_xyz(&t(0), &t(1), &t(2), longitude0, latitude0, height0);
	Pe = R_ne * Pn + t;
	xyz_to_llh(longitude, latitude, height, Pe(0), Pe(1), Pe(2));

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          由欧拉角得到导航坐标系转换到机体坐标系的旋转矩阵。
*   @details        导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地下。
                    机体坐标系原点位于质心点，X轴指向机头，Y轴指向右侧，Z轴指向下方。
*   @param[out]     R_nb            旋转矩阵:3x3,正交
*   @param[in]      roll            机体导航坐标系内横滚角（角度）
*   @param[in]      pitch           机体导航坐标系内俯仰角（角度）
*   @param[in]      yaw             机体导航坐标系内航向角（角度）
*   @retval         0               正常
*   @retval         1               错误
*/
int rotation_navigation_to_body(
	Matrix3d* R_nb,
	const double roll,
	const double pitch,
	const double yaw)
{
	////////先将东-北-天的导航坐标系转到北-东-地的方向
	//////Matrix3d R_nn2;
	//////R_nn2 << 0, 1, 0,
	//////		1, 0, 0,
	//////		0, 0, -1;

	//从北-东-地的导航坐标系以Z（航向）-Y（俯仰）-X（滚转）的顺序旋转到载体系的旋转矩阵：
	double r, p, y;
	deg2rad(&r, roll); deg2rad(&p, pitch); deg2rad(&y, yaw);
	double c1 = cos(y), s1 = sin(y);
	double c2 = cos(p), s2 = sin(p);
	double c3 = cos(r), s3 = sin(r);
	//可查Z1Y2X3顺规旋转矩阵如下：
	Matrix3d R_n2b;
	R_n2b(0, 0) = c1 * c2;
	R_n2b(0, 1) = c1 * s2 * s3 - c3 * s1;
	R_n2b(0, 2) = s1 * s3 + c1 * c3 * s2;
	R_n2b(1, 0) = c2 * s1;
	R_n2b(1, 1) = c1 * c3 + s1 * s2 * s3;
	R_n2b(1, 2) = c3 * s1 * s2 - c1 * s3;
	R_n2b(2, 0) = -s2;
	R_n2b(2, 1) = c2 * s3;
	R_n2b(2, 2) = c2 * c3;

	*R_nb = R_n2b.transpose();// *R_nn2;

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          机体坐标系转换到导航坐标系的旋转矩阵。
*   @details        导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地下。
					机体坐标系原点位于质心点，X轴指向机头，Y轴指向右侧，Z轴指向下方。
*   @param[out]     R_bn            旋转矩阵:3x3,正交
*   @param[in]      roll            机体导航坐标系内横滚角（角度）
*   @param[in]      pitch           机体导航坐标系内俯仰角（角度）
*   @param[in]      yaw             机体导航坐标系内航向角（角度）
*   @retval         0               正常
*   @retval         1               错误
*/
int rotation_body_to_navigation(
	Matrix3d* R_bn,
	const double roll,
	const double pitch,
	const double yaw)
{
	Matrix3d R_nb;
	rotation_navigation_to_body(&R_nb, roll, pitch, yaw);
	*R_bn = R_nb.transpose();

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          由Rnb旋转矩阵得到欧拉角。
*   @details        Rnb是导航坐标系转换到机体坐标系
                    导航坐标系原点位于参考点，X轴指向北边，Y轴指向东边，Z轴指向地下。
					机体坐标系原点位于质心点，X轴指向机头，Y轴指向右侧，Z轴指向下方。
*   @param[out]     roll            机体导航坐标系内横滚角（角度）
*   @param[out]     pitch           机体导航坐标系内俯仰角（角度）
*   @param[out]     yaw             机体导航坐标系内航向角（角度）
*   @param[in]      R_nb            旋转矩阵:3x3,正交
*   @retval         0               正常
*   @retval         1               错误
*/
int rotation_nb_to_euler(
	double* roll,
	double* pitch,
	double* yaw,
	const Matrix3d R_nb)
{
	double r, p, y;
	//////Matrix3d R_nn2_inv;
	//////R_nn2_inv << 0, 1, 0,
	//////		1, 0, 0,
	//////		0, 0, -1;
	Matrix3d R_n2b = R_nb.transpose();// *R_nn2_inv;
	r = atan2(R_n2b(2,1), R_n2b(2,2));
	p = atan(-R_n2b(2, 0) / sqrt(1 - R_n2b(2, 0) * R_n2b(2, 0)));
	y = atan2(R_n2b(1, 0), R_n2b(0, 0));

	rad2deg(roll, r);
	rad2deg(pitch, p);
	rad2deg(yaw, y);

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          由方向余弦旋转矩阵求四元数
*   @details        由方向余弦旋转矩阵求四元数
*   @param[out]     q               四元数:4维向量
*   @param[in]      R               方向余弦旋转矩阵:3x3
*   @retval         0               正常
*   @retval         1               错误
*/
int rotation_to_quaternion(
	Vector4d* q,
	const Matrix3d R)
{
	Vector4d __q;
	__q(0) = 0.5 * sqrt(1 + R(0, 0) + R(1, 1) + R(2, 2));
	if (fabs(__q(0)) < 1e-4) {
		if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
			double t = sqrt(1 + R(0, 0) - R(1, 1) - R(2, 2));
			__q(0) = (R(2, 1) - R(1, 2)) / t;
			__q(1) = t / 4;
			__q(2) = (R(0, 2) + R(2, 0)) / t;
			__q(3) = (R(0, 1) + R(1, 0)) / t;
		}
		else if (R(1, 1) > R(0, 0) && R(1, 1) > R(2, 2)) {
			double t = sqrt(1 - R(0, 0) + R(1, 1) - R(2, 2));
			__q(0) = (R(0, 2) - R(2, 0)) / t;
			__q(1) = (R(0, 1) + R(1, 0)) / t;
			__q(2) =  t / 4;
			__q(3) = (R(2, 1) + R(1, 2)) / t;
		}
		else {
			double t = sqrt(1 - R(0, 0) - R(1, 1) + R(2, 2));
			__q(0) = (R(1, 0) - R(0, 1)) / t;
			__q(1) = (R(0, 2) + R(2, 0)) / t;
			__q(2) = (R(1, 2) - R(2, 1)) / t;
			__q(3) =  t / 4;
		}
	}
	else {
		__q(1) = (R(2, 1) - R(1, 2)) / (4 * __q(0));
		__q(2) = (R(0, 2) - R(2, 0)) / (4 * __q(0));
		__q(3) = (R(1, 0) - R(0, 1)) / (4 * __q(0));
	}
	quaterntion_normalized(q, __q);
	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          由四元数求方向余弦旋转矩阵
*   @details        由四元数求方向余弦旋转矩阵
*   @param[out]     R               方向余弦旋转矩阵:3x3
*   @param[in]      q               四元数:4维向量
*   @retval         0               正常
*   @retval         1               错误
*/
int quaternion_to_rotation(
	Matrix3d* R,
	const Vector4d & q)//不加'&'会报错：具有 __declspec(align('16')) 的形参将不被对齐
{
	(*R)(0, 0) = q(0) * q(0) + q(1) * q(1) - q(2) * q(2) - q(3) * q(3);
	(*R)(0, 1) = 2 * (q(1) * q(2) - q(0) * q(3));
	(*R)(0, 2) = 2 * (q(1) * q(3) + q(0) * q(2));
	(*R)(1, 0) = 2 * (q(1) * q(2) + q(0) * q(3));
	(*R)(1, 1) = q(0) * q(0) - q(1) * q(1) + q(2) * q(2) - q(3) * q(3);
	(*R)(1, 2) = 2 * (q(2) * q(3) - q(0) * q(1));
	(*R)(2, 0) = 2 * (q(1) * q(3) - q(0) * q(2));
	(*R)(2, 1) = 2 * (q(2) * q(3) + q(0) * q(1));
	(*R)(2, 2) = q(0) * q(0) - q(1) * q(1) - q(2) * q(2) + q(3) * q(3);

	//Matrix3d R_nn2_inv;
	//R_nn2_inv << 0, 1, 0,
	//	1, 0, 0,
	//	0, 0, -1;
	//(*R) = (*R) * R_nn2_inv;

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          由欧拉角求从载体系到导航系的转动四元数
*   @details        由欧拉角求从载体系到导航系的转动四元数
*   @param[out]     q               从载体系到导航系的转动四元数:4维向量
*   @param[in]      roll            机体导航坐标系内横滚角（角度）
*   @param[in]      pitch           机体导航坐标系内俯仰角（角度）
*   @param[in]      yaw             机体导航坐标系内航向角（角度）
*   @retval         0               正常
*   @retval         1               错误
*/
int euler_to_quaternion_bn(
	Vector4d* q,
	const double roll,
	const double pitch,
	const double yaw)
{
	Matrix3d Rnb;
	rotation_navigation_to_body(&Rnb, roll, pitch, yaw);
	//cout << R << endl;
	//cin.get();
	Matrix3d Rbn;
	Rbn = Rnb.transpose();
	rotation_to_quaternion(q, Rbn);
	if (fabs(roll) < 1e-4 && fabs(pitch) < 1e-4 && fabs(yaw) < 1e-4) {
		(*q) << 0, 0, 0, 0;
		(*q)(0) = 1;
	}

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          由从载体系到导航系的转动四元数求欧拉角
*   @details        由从载体系到导航系的转动四元数求欧拉角
*   @param[out]     roll            机体导航坐标系内横滚角（角度）
*   @param[out]     pitch           机体导航坐标系内俯仰角（角度）
*   @param[out]     yaw             机体导航坐标系内航向角（角度）
*   @param[in]      q               四元数:4维向量
*   @retval         0               正常
*   @retval         1               错误
*/
int quaternion_bn_to_euler(
	double* roll,
	double* pitch,
	double* yaw,
	const Vector4d& q)//不加'&'会报错：具有 __declspec(align('16')) 的形参将不被对齐
{
	Matrix3d Rbn;
	quaternion_to_rotation(&Rbn, q);

	rotation_nb_to_euler(roll, pitch, yaw, Rbn.transpose());

	return 0;
}

// --------------------------------------------------------------------------------------------------------------------------------
/**
*   @brief          四元数归一化
*   @details        四元数归一化
*   @param[out]     qout            归一化后的四元数
*   @param[in]      qin             四元数:4维向量
*   @retval         0               正常
*   @retval         1               错误
*/

int quaterntion_normalized(
	Vector4d* qout,
	const Vector4d& qin)
{

	double q0, q1, q2, q3;

	q0 = qin(0) / qin.norm();
	q1 = qin(1) / qin.norm();
	q2 = qin(2) / qin.norm();
	q3 = qin(3) / qin.norm();
	(*qout)<< q0, q1, q2, q3;

	return 0;
}


#include <vector>
#include <string>
#include <cstdarg>
#include <cstring>
#include <cstddef>
#include <iterator>

#include <boost/lexical_cast.hpp>

#include <pybind11/pybind11.h>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_conformer_2.h>
#include <CGAL/Delaunay_mesher_2.h>
#include <CGAL/Delaunay_mesh_face_base_2.h>
#include <CGAL/Delaunay_mesh_vertex_base_2.h>
#include <CGAL/Delaunay_mesh_size_criteria_2.h>
#include <CGAL/lloyd_optimize_mesh_2.h>

namespace py = pybind11;

using K = CGAL::Exact_predicates_inexact_constructions_kernel;
using Vb = CGAL::Delaunay_mesh_vertex_base_2<K>;
using Fb = CGAL::Delaunay_mesh_face_base_2<K>;
using Tds = CGAL::Triangulation_data_structure_2<Vb, Fb>;
using CDT = CGAL::Constrained_Delaunay_triangulation_2<K, Tds>;
using Criteria = CGAL::Delaunay_mesh_size_criteria_2<CDT>;
using Mesher = CGAL::Delaunay_mesher_2<CDT, Criteria>;

using Point = CDT::Point;
using Vertex_handle = CDT::Vertex_handle;

template <typename T>
class TypedInputIterator
{
public:
    using iterator_category = std::input_iterator_tag;
    using difference_type = std::ptrdiff_t;
    using value_type = T;
    using pointer = T*;
    using reference = T&;

    explicit TypedInputIterator(py::iterator& py_iter) :
        py_iter_(py_iter)
    {
    }

    explicit TypedInputIterator(py::iterator&& py_iter) :
        py_iter_(py_iter)
    {
    }

    value_type operator*()
    {
        return (*py_iter_).template cast<value_type>();
    }

    TypedInputIterator operator++(int)
    {
        auto copy = *this;
        ++py_iter_;
        return copy;
    }

    TypedInputIterator& operator++()
    {
        ++py_iter_;
        return *this;
    }

    bool operator!=(TypedInputIterator &rhs)
    {
        return py_iter_ != rhs.py_iter_;
    }

    bool operator==(TypedInputIterator &rhs)
    {
        return py_iter_ == rhs.py_iter_;
    }

private:
    py::iterator py_iter_;
};

PYBIND11_MODULE(cgal_mesher, m)
{
    py::class_<Point>(m, "Point")
            .def(py::init<int, int>(),  py::arg("x"), py::arg("y"))
            .def(py::init<double, double>(), py::arg("x"), py::arg("y"))
            .def_property_readonly("x", &Point::x)
            .def_property_readonly("y", &Point::y)
            .def("__repr__",
            [](const Point &p) {
                std::string r("Point(");
                r += boost::lexical_cast<std::string>(p.x());
                r += ", ";
                r += boost::lexical_cast<std::string>(p.y());
                r += ")";
                return r;
            })
            ;

    py::class_<Vertex_handle>(m, "VertexHandle");

    py::class_<CDT>(m, "ConstrainedDelaunayTriangulation")
            .def(py::init())
            .def("insert", [](CDT & cdt, const Point & p) { return cdt.insert(p); })
            .def("insert_constraint",
                 [](CDT & cdt, Vertex_handle a, Vertex_handle b)
                 {
                     cdt.insert_constraint(a, b);
                 })
            .def("remove", &CDT::remove)
            .def("number_of_vertices", &CDT::number_of_vertices)
            .def("number_of_faces", &CDT::number_of_faces)
            .def("vertices", [](CDT & cdt) -> py::iterator
                 {
                     return py::make_iterator(cdt.vertices_begin(), cdt.vertices_end());
                 })
            ;

    m.def("make_conforming_delaunay", &CGAL::make_conforming_Delaunay_2<CDT>,
          py::arg("cdt")
    );

    m.def("make_conforming_gabriel", &CGAL::make_conforming_Gabriel_2<CDT>,
        py::arg("cdt")
    );

    py::class_<Criteria>(m, "Criteria")
            .def(py::init<double, double>(),
                 py::arg("aspect_bound") = 0.125,
                 py::arg("size_bound") = 0.0)
            .def_property("size_bound", &Criteria::size_bound, &Criteria::set_size_bound)
            .def_property("aspect_bound",
                          [](const Criteria & c) { c.bound(); },
                          [](Criteria & c, double bound) { c.set_bound(bound); })
            ;

    py::class_<Mesher>(m, "Mesher")
            .def(py::init<CDT&>())
            .def("refine_mesh", &Mesher::refine_mesh)
            .def_property(
                "criteria",
                &Mesher::get_criteria,
                [](Mesher& mesher, const Criteria & criteria)
                {
                    mesher.set_criteria(criteria);
                }
            )
            .def_property_readonly("seeds", [](Mesher& mesher) {
                return py::make_iterator(mesher.seeds_begin(), mesher.seeds_end());
            })
            .def("seeds_from", [](Mesher & mesher, py::iterable iterable, bool mark)
            {
                py::iterator iterator = py::iter(iterable);
                TypedInputIterator<Point> points_begin(iterator);
                TypedInputIterator<Point> points_end(py::iterator::sentinel());
                mesher.set_seeds(points_begin, points_end, mark);
            })
            .def("clear_seeds", &Mesher::clear_seeds)
            ;

    m.def("lloyd_optimize", [](
        CDT& cdt,
        int max_iteration_number,
        double time_limit,
        double convergence,
        double freeze_bound)
        {
            CGAL::lloyd_optimize_mesh_2(cdt,
                CGAL::parameters::max_iteration_number = max_iteration_number,
                CGAL::parameters::time_limit = time_limit,
                CGAL::parameters::convergence = convergence,
                CGAL::parameters::freeze_bound = freeze_bound
            );
        },
          py::arg("cdt"),
          py::arg("max_iteration_number") = 0,
          py::arg("time_limit") = 0.0,
          py::arg("convergence") = 0.001,
          py::arg("freeze_bound") = 0.001
    );
}


/*! This file makes the AlgebraicVector class (and thus DA objects) directly interface-able with the Boost ODEINT numerical integration library.
 * It works by defining custom operations to supplement the operator overloads in the AlgebraicVector class natively.
 * The following are defined:
      - Infinity norm (maximum coefficient in the entire AlgebraicVector)
      - Resizeable toggle (assumed not - requires the .resize() method in the RHS of the ODE)
      - The abs() method for the AlgebraicVector class, via modified header files in this repository.
  !!
    The vector_space_algebra argument must be used when defining your stepper. Eg:
            ```typedef runge_kutta_dopri5< AlgebraicVector<DACE::DA>, double, AlgebraicVector<DACE::DA>,
                                           double, vector_space_algebra > stepper;```
  !!
 */
 #ifndef __DACELIB_BOOST_COMPAT_H__
 #define __DACELIB_BOOST_COMPAT_H__

 #include <boost/operators.hpp> /* Only one explicitly needed here */
 #include <dace/AlgebraicVector.hpp>

 namespace boost
 {
     namespace numeric
     {
         namespace odeint
         {
             /* Define the infinity norm for the AlgebraicVector.*/
             template<>
             struct vector_space_norm_inf<DACE::AlgebraicVector<DACE::DA>>
             {
                 typedef double result_type; // DACE only returns abs(DA) as double, no point templating
                 double operator()(const DACE::AlgebraicVector<DACE::DA>& p) const
                 {
                     double maxCoeff = 0.0;
                     for (size_t i = 0; i < p.size(); ++i)
                     {
                         T thisNorm = abs(p[i]); // C
                         maxCoeff = thisNorm > maxCoeff ? thisNorm : maxCoeff;
                     }
                     return maxCoeff;
                 }
             };

             /* Tell BOOST that AlgebraicVector-s resizes. */
             template <>
             struct is_resizeable<DACE::AlgebraicVector<DACE::DA>>
             {
                 typedef boost::true_type type;
                 const static bool value = type::value;
             };
         }
    }
}

#endif


/** @file lane_detection_vrep.hpp
 *  @brief The header file for lane detection when using IMACS framework with VREP simulator
 */
#ifndef LANEDETECTION_LANE_DETECTION_H_
#define LANEDETECTION_LANE_DETECTION_H_

#include <iostream>
#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>
#include <cmath>
#include "Halide.h"
#include "polyfit.hpp"
#include "config_vrep.hpp"
#include "paths.hpp"

/// @brief Class for lane Detection in VREP. Inherits the base class pathsIMACS to load the paths to image.
class laneDetection : pathsIMACS {
private:

    /**  @brief C++ implementation of defining BEV transformation points
           Vertices for the BEV transformation are hard-coded in this function.
         @return    BEV transformation's source and destination vertices
      */
    std::vector<std::vector<cv::Point2f>> get_bev_points();

    /**  @brief C++ implementation of Bird's Eye View (BEV) Transformation
           Gets the BEV transformation points, calculates the homography matrix and performs the BEV transformation
         @param[in] 	src            Source image matrix
         @param[in,out] dst            Destination image matrix
      */
    void bev_transform(cv::Mat& src, cv::Mat& dst);

    /**  @brief C++ implementation of (Reverse) Bird's Eye View (BEV) Transformation
           Gets the BEV transformation points, calculates the reverse homography matrix and performs the BEV transformation
         @param[in,out] 	src             Source image matrix
         @param[in] dst         Destination 	image matrix
      */
    void bev_rev_transform(cv::Mat& src, cv::Mat& dst);

    /**  @brief C++ implementation of Sliding window lane tracking
           Find the lane in the image
         @param[in] 	src            Source image matrix
	 @return lane points in the image
      */
    std::vector<std::vector<cv::Point>> sliding_window_lane_tracking(cv::Mat& src);

    /**  @brief C++ implementation of lateral deviation calculation
           From the given lane points, polyfits the lanes and then finds the deviation of the car from the centre of the lane at the look-ahead distance
         @param[in] 	left_lane_inds            left lane (points)
         @param[in] 	right_lane_inds           right lane (points)
	 @return lateral deviation of the vehicle with the current heading at the look-ahead distance
      */
    std::vector<long double> calculate_lateral_deviation(std::vector<cv::Point> left_lane_inds,
                                            std::vector<cv::Point> right_lane_inds);

    /**  @brief C++ implementation to overlay the detected lanes onto the original image

         @param[in] 	lanes            	detected lanes
         @param[in] 	src           		source image
         @param[in,out]	img_roi           	Image with only the RoI
         @param[in] 	img_warped           	RoI image after BEV transform
         @param[in,out]	img_detected_lanes      Image with the detected lanes
         @param[in,out]	draw_lines           	Image with lanes drawn
         @param[in,out]	diff_src_rebev          Image with diff_src reverse BEV
         @param[in,out]	img_rev_warped          Original image with lines overlayed
      */
    void lane_identification(std::vector<std::vector<cv::Point>> lanes, cv::Mat& src, cv::Mat& img_roi,
                            cv::Mat& img_warped, cv::Mat& img_detected_lanes,
                            cv::Mat& draw_lines, cv::Mat& diff_src_rebev, cv::Mat& img_rev_warped);
public:
    /// constructor
    laneDetection();
    /// destructor
    ~laneDetection();

    /**  @brief C++ implementation of lane detection pipeline
           Scales the image, does BEV transformation, Gray-scale conversion, White masking, Image thresholding, Sliding window lane tracking and lateral deviation calculation. Images are also stored in lane_out_img_dir, if RE_DRAW_IMAGE is defined.
         @param[in] src            Source image Matrix
         @return    the calculated lateral deviation yL
      */
    long double lane_detection_pipeline(cv::Mat src);
};

#endif


/* Copyright (c) 2016, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#include <iostream>
#include <sstream>
#include <sys/time.h>
//#include <random> // can only use with C++11
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include "cudaPcl/pinhole.h"

#include <boost/program_options.hpp>
#include <boost/random.hpp>

namespace po = boost::program_options;
using std::cout;
using std::endl;

#include <jsCore/timer.hpp>

float ToDeg(float rad) {
  return rad*180./M_PI;
}
float ToRad(float deg) {
  return deg/180.*M_PI;
}
double ToDeg(double rad) {
  return rad*180./M_PI;
}
double ToRad(double deg) {
  return deg/180.*M_PI;
}

Eigen::Vector3d ComputePcMean(pcl::PointCloud<pcl::PointXYZRGB>&
    pc) {
  // take 3 values (x,y,z of normal) with an offset of 4 values (x,y,z
  // and one float which is undefined) and the step is 12 (4 for xyz, 4
  // for normal xyz and 4 for curvature and rgb).
  Eigen::MatrixXf xyz = pc.getMatrixXfMap(3, 8, 0); // this works for PointXYZRGB?
  Eigen::Vector3d mean =  Eigen::Vector3d::Zero();

  for (size_t i=0; i<xyz.cols(); ++i) {
    mean += xyz.col(i).cast<double>();
  }
  return  mean / xyz.cols();
}

Eigen::Matrix3d ComputePcCov(pcl::PointCloud<pcl::PointXYZRGB>&
    pc) {

  // take 3 values (x,y,z of normal) with an offset of 4 values (x,y,z
  // and one float which is undefined) and the step is 12 (4 for xyz, 4
  // for normal xyz and 4 for curvature and rgb).
  Eigen::MatrixXf xyz = pc.getMatrixXfMap(3, 8, 0); // this works for PointXYZRGB?
  Eigen::Vector3d mean = ComputePcMean(pc);
  Eigen::Matrix3d S = Eigen::Matrix3d::Zero();
  for (size_t i=0; i<xyz.cols(); ++i) {
    S += (xyz.col(i).cast<double>()-mean)*(xyz.col(i).cast<double>()-mean).transpose();
  }
  return  S / (xyz.cols()-1);
}

void SampleTransformation(float angle, float translation,
    Eigen::Matrix3f& R, Eigen::Vector3f& t) {
  // Using boost here because C11 and CUDA seem to have troubles.
  timeval tNow;
  gettimeofday(&tNow, NULL);
  boost::mt19937 gen(tNow.tv_usec);
  boost::normal_distribution<> N(0,1);
  // Sample axis of rotation:
  Eigen::Vector3f axis(N(gen), N(gen), N(gen));
  axis /= axis.norm();
  // Construct rotation:
  Eigen::AngleAxisf aa(ToRad(angle), axis);
  Eigen::Quaternionf q(aa);
  R = q.matrix();
  // Sample translation on sphere with radius translation:
  t = Eigen::Vector3f(N(gen), N(gen), N(gen));
  t *= translation / t.norm();

//  std::cout << "sampled random transformation:\n"
//    << R << std::endl << t.transpose() << std::endl;
}

bool RenderPointCloudNoisy(const pcl::PointCloud<pcl::PointXYZRGBNormal>& pcIn,
    const cudaPcl::Pinhole& c, pcl::PointCloud<pcl::PointXYZRGBNormal>&
    pcOut, uint32_t nUpsample) {

  // Transform both points by T as well as surface normals by R
  // manually since the standard transformPointCloud does not seem to
  // touch the Surface normals.
  Eigen::MatrixXf d = 1e10*Eigen::MatrixXf::Ones(c.GetH(), c.GetW());
  Eigen::MatrixXi id = Eigen::MatrixXi::Zero(c.GetH(), c.GetW());
  uint32_t hits = 0;
  for (uint32_t i=0; i<pcIn.size(); ++i) {
    Eigen::Map<const Eigen::Vector3f> pA_W(&(pcIn.at(i).x));
    Eigen::Vector3f p_W = pA_W;
    Eigen::Vector3f p_C;
    Eigen::Vector2i pI;
    if (c.IsInImage(p_W, &p_C, &pI)) {
      if (p_C(2) > 0. && d(pI(1),pI(0)) >  p_C(2)) {
        d(pI(1),pI(0)) = p_C(2);
        id(pI(1),pI(0)) = i;
      }
      ++hits;
    }
  }
//  std::cout << " # hits: " << hits
//    << " for total number of pixels in output camera: " << c.GetSize()
//    << " percentage: " << (100.*hits/float(c.GetSize())) << std::endl;
  timeval tNow;
  gettimeofday(&tNow, NULL);
  boost::mt19937 gen(tNow.tv_usec);
  Eigen::Vector3f d_C;
  d_C << 0.,0.,1.;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  uint32_t n_sampled = 0;
  for (uint32_t i=0; i<c.GetW(); ++i)
    for (uint32_t j=0; j<c.GetH(); ++j)
      if (d(j,i) < 100.) {
//        Eigen::Vector3f n_C = c.GetR_C_W() * Eigen::Map<const
//          Eigen::Vector3f>(pcIn.at(id(j,i)).normal);
        Eigen::Vector3f n_C =  Eigen::Map<const
          Eigen::Vector3f>(pcIn.at(id(j,i)).normal);
        Eigen::Vector3f p_C = c.UnprojectToCameraCosy(i,j,d(j,i));
        double dot = n_C.transpose()*d_C;
        double theta = acos(std::min(std::max(dot, -1.),1.));
        if (ToDeg(theta) < 80.) {
          //http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=6375037
          double sig_L = (0.8+0.035*theta/(M_PI*0.5-theta))* p_C(2)/c.GetF();
          double sig_z =0.0012 + 0.0019*(p_C(2)-0.4)*(p_C(2)-0.4);
          if (ToDeg(theta) > 60.) {
            sig_z += 0.0001/sqrt(p_C(2))*theta*theta/((M_PI*0.5-theta)
                *(M_PI*0.5-theta));
          }
 //        std::cout << sig_L << " " << sig_z << std::endl;
          boost::normal_distribution<> N_L(0,sig_L);
          boost::normal_distribution<> N_z(0,sig_z);
          for (uint32_t k=0; k< nUpsample; ++k) {
            pcl::PointXYZRGB pB;
            pB.rgb = pcIn.at(id(j,i)).rgb;
            Eigen::Map<Eigen::Vector3f> p_Cout(&(pB.x));
            p_Cout =  p_C;
//            if (i%10==0) std::cout << p_Cout.transpose() << " ";
            p_Cout(0) += N_L(gen);
            p_Cout(1) += N_L(gen);
            p_Cout(2) += N_z(gen);
//            if (p_Cout.norm() > 10.)
              //          if (i%10==0)
//              std::cout << p_Cout.transpose() << " " << sig_z << " " << sig_L
//                << " " << ToDeg(theta) << std::endl;
            cloud->push_back(pB);
            ++n_sampled;
          }
        } else {
          pcl::PointXYZRGB pB;
          pB.rgb = pcIn.at(id(j,i)).rgb;
          Eigen::Map<Eigen::Vector3f>(&(pB.x)) = p_C;
          cloud->push_back(pB);
        }
      }
  if (cloud->size() < pcIn.size()/10) return false;
  std::cout << " output pointcloud size is: " << pcOut.size()
    << " percentage of input cloud: "
    << (100.*pcOut.size()/float(pcIn.size()))
    << " sampled at total of " << 100.*n_sampled/float(pcOut.size()) << "%."<< std::endl;

  pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
  sor.setInputCloud (cloud);
  sor.setMeanK (50);
  sor.setStddevMulThresh (1.0);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZRGB>);
  sor.filter (*cloud_f);

//  Eigen::Matrix3d S = ComputePcCov(*cloud_f);
//  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig(S);
//  std::cout << eig.eigenvalues().array().sqrt().matrix().transpose() << std::endl;
//  if (!(eig.eigenvalues().array().sqrt() > 0.10).all())
//    return false;

  // Extract surface normals
  pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
  ne.setInputCloud (cloud_f);
  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
  ne.setSearchMethod (tree);
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
  ne.setKSearch(100);
  ne.compute (*cloud_normals);

  pcOut.clear();
  for (uint32_t i=0; i<cloud_f->size(); ++i) {
    pcl:: PointXYZRGBNormal p;
    Eigen::Map<Eigen::Vector3f>(p.normal) =
    Eigen::Map<Eigen::Vector3f>(cloud_normals->at(i).normal);
    Eigen::Map<Eigen::Vector3f>(&(p.x)) = Eigen::Map<Eigen::Vector3f>(&(cloud_f->at(i).x));
    p.rgb = cloud_f->at(i).rgb;
    pcOut.push_back(p);
  }
  return true;
}

bool RenderPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal>& pcIn,
    const cudaPcl::Pinhole& c, pcl::PointCloud<pcl::PointXYZRGBNormal>&
    pcOut) {
  // Transform both points by T as well as surface normals by R
  // manually since the standard transformPointCloud does not seem to
  // touch the Surface normals.
  Eigen::MatrixXf d = 1e10*Eigen::MatrixXf::Ones(c.GetH(), c.GetW());
  Eigen::MatrixXi id = Eigen::MatrixXi::Zero(c.GetH(), c.GetW());
  uint32_t hits = 0;
  for (uint32_t i=0; i<pcIn.size(); ++i) {
    Eigen::Map<const Eigen::Vector3f> pA_W(&(pcIn.at(i).x));
    Eigen::Vector3f p_W = pA_W;
    Eigen::Vector3f p_C;
    Eigen::Vector2i pI;
    if (c.IsInImage(p_W, &p_C, &pI)) {
      if (d(pI(1),pI(0)) >  p_C(2)) {
        d(pI(1),pI(0)) = p_C(2);
        id(pI(1),pI(0)) = i;
      }
      ++hits;
    }
  }
//  std::cout << " # hits: " << hits
//    << " for total number of pixels in output camera: " << c.GetSize()
//    << " percentage: " << (100.*hits/float(c.GetSize())) << std::endl;

  pcOut.clear();
  for (uint32_t i=0; i<c.GetW(); ++i)
    for (uint32_t j=0; j<c.GetH(); ++j)
      if (d(j,i) < 100.) {
        pcl::PointXYZRGBNormal pB;
        Eigen::Map<Eigen::Vector3f> p_C(&(pB.x));
        p_C = c.UnprojectToCameraCosy(i,j,d(j,i));
        Eigen::Map<Eigen::Vector3f>(pB.normal) =
          c.GetR_C_W() * Eigen::Map<const
          Eigen::Vector3f>(pcIn.at(id(j,i)).normal);
        pB.rgb = pcIn.at(id(j,i)).rgb;
        pcOut.push_back(pB);
      }
//  std::cout << " output pointcloud size is: " << pcOut.size()
//    << " percentage of input cloud: "
//    << (100.*pcOut.size()/float(pcIn.size())) << std::endl;
  return pcOut.size() > (pcIn.size()/10);
}

uint32_t VisiblePointsOfPcInCam(const
    pcl::PointCloud<pcl::PointXYZRGBNormal>& pc,
    const Eigen::Matrix3f& R_PC_W, const Eigen::Vector3f& t_PC_W, const
    cudaPcl::Pinhole& c) {
  uint32_t hits =0;
  for (uint32_t i=0; i<pc.size(); ++i) {
    Eigen::Vector3f p_PC = Eigen::Map<const Eigen::Vector3f> (&(pc.at(i).x));
    Eigen::Vector3f p_W = R_PC_W.transpose() * (p_PC - t_PC_W);
    if (c.IsInImage(p_W, NULL, NULL)) {
      ++hits;
    }
  }
  return hits;
}

int main (int argc, char** argv)
{
  // Declare the supported options.
  po::options_description desc("Render a point cloud from a different pose.");
  desc.add_options()
    ("help,h", "produce help message")
    ("input,i", po::value<string>(),"path to input point cloud")
    ("output,o", po::value<string>(),"path to output transformed point cloud")
    ("angle,a", po::value<double>(),"magnitude of rotation (deg)")
    ("translation,t", po::value<double>(),"magnitude of translation (m)")
    ("min,m", po::value<double>(),"minimum overlap to be accepted (%)")
    ("local,l", "simulate local alignment problem i.e. do not transform the pointclouds by a random transformation")
    ;
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << desc << "\n";
    return 1;
  }

  double angle = 10.; // In degree
  double translation = 1.0;
  double min_overlap = 50.;
  string inputPath = "./file.ply";
  string outputPath = "./out";
  if(vm.count("input")) inputPath = vm["input"].as<string>();
  if(vm.count("output")) outputPath = vm["output"].as<string>();
  if(vm.count("angle")) angle = vm["angle"].as<double>();
  if(vm.count("min")) min_overlap = vm["min"].as<double>();
  if(vm.count("translation")) translation = vm["translation"].as<double>();

  std::stringstream ssOutPathA;
  std::stringstream ssOutPathB;
  std::stringstream ssTransformationAFile;
  std::stringstream ssTransformationBFile;
  std::stringstream ssTransformationTotalFile;
  ssOutPathA << outputPath << "_A_angle_" << angle << "_translation_" <<
    translation << ".ply";
  ssOutPathB << outputPath << "_B_angle_" << angle << "_translation_" <<
    translation << ".ply";
  ssTransformationAFile << outputPath << "_angle_" << angle <<
    "_translation_" << translation << "_Transformation_A_W" << ".csv";
  ssTransformationBFile << outputPath << "_angle_" << angle <<
    "_translation_" << translation << "_Transformation_B_W" << ".csv";
  ssTransformationTotalFile << outputPath << "_angle_" << angle <<
    "_translation_" << translation << "_TrueTransformation" << ".csv";

  std::string outputPathA = ssOutPathA.str();
  std::string outputPathB = ssOutPathB.str();
  std::string transformationAOutputPath= ssTransformationAFile.str();
  std::string transformationBOutputPath= ssTransformationBFile.str();
  std::string transformationTotalOutputPath = ssTransformationTotalFile.str();

  // Load point cloud.
  pcl::PointCloud<pcl::PointXYZRGBNormal> pcIn, pcOutA, pcOutB;
  pcl::PLYReader reader;
  int err = reader.read(inputPath, pcIn);
  if (err) {
    std::cout << "error reading " << inputPath << std::endl;
    return err;
  } else {
    std::cout << "loaded pc from " << inputPath << ": " << pcIn.width << "x"
      << pcIn.height << std::endl;
  }

  std::cout<< " input pointcloud from "<<inputPath<<std::endl;
  std::cout<< "  angular magnitude "<< angle <<std::endl;
  std::cout<< "  translational magnitude "<< translation <<std::endl;

  uint32_t w = 320;
  uint32_t h = 280;
  float f = 540.;
  Eigen::Matrix3f R_A_W, R_B_W;
  Eigen::Vector3f t_A_W, t_B_W;
  bool succA = false;
  bool succB = false;
  double overlap = 0.;
  uint32_t attemptsOuter = 0;
  uint32_t attempts = 0;
  do {
    overlap = 0.;
    attempts = 0;
    do {
      // sample pc A
      R_A_W = Eigen::Matrix3f::Identity();
      t_A_W = Eigen::Vector3f::Zero();
//      SampleTransformation(angle, translation, R_A_W, t_A_W);
      cudaPcl::Pinhole camA(R_A_W, t_A_W, f, w, h);
      if (!RenderPointCloud(pcIn, camA, pcOutA))
        continue;
      // sample pc B
      SampleTransformation(angle, translation, R_B_W, t_B_W);
      cudaPcl::Pinhole camB(R_B_W, t_B_W, f, w, h);
      if (!RenderPointCloud(pcIn, camB, pcOutB))
        continue;

      uint32_t hitsAinB = VisiblePointsOfPcInCam(pcOutA, R_A_W, t_A_W, camB);
      uint32_t hitsBinA = VisiblePointsOfPcInCam(pcOutB, R_B_W, t_B_W, camA);

      overlap = std::min(100*hitsAinB/float(pcOutA.size()),
          100*hitsBinA/float(pcOutB.size()));

      ++ attempts;
      if (attempts%20 == 0) {
        std::cout << ".";
        std::cout.flush();
      }
    } while(overlap < min_overlap && attempts < 1000);
    std::cout << std::endl;
    std::cout << "overlap: " << overlap
      << "% attempt: " << attempts<< std::endl;
    if (attempts >= 1000) return 1;
    // Now sample point clouds from those views.
    cudaPcl::Pinhole camA(R_A_W, t_A_W, f, w, h);
    succA = RenderPointCloudNoisy(pcIn, camA, pcOutA, 2);
    cudaPcl::Pinhole camB(R_B_W, t_B_W, f, w, h);
    succB = RenderPointCloudNoisy(pcIn, camB, pcOutB, 2);
  } while ((!succA || !succB) && attemptsOuter < 100);

  {
    Eigen::Quaternionf q(R_A_W);
    Eigen::Vector3f t = t_A_W;
    std::ofstream out(transformationAOutputPath.c_str());
    out << "q_w q_x q_y q_z t_x t_y t_z size" << std::endl;
    out << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " "
      << t(0) << " " << t(1) << " " << t(2) << " "
      << " " << pcOutA.size();
    out.close();
  }
  {
    Eigen::Quaternionf q(R_B_W);
    Eigen::Vector3f t = t_B_W;
    std::ofstream out(transformationBOutputPath.c_str());
    out << "q_w q_x q_y q_z t_x t_y t_z size" << std::endl;
    out << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " "
      << t(0) << " " << t(1) << " " << t(2) << " "
      << " " << pcOutB.size();
    out.close();
  }

  // If we want to simulate a global alignment problem sample another
  // intermediate random transformation and apply it to pointcloud B as
  // well as the global true transformation.
  if(!vm.count("local")) {
    // Now sample another transformation of the same parameters to
    // transform pcB
    Eigen::Matrix3f R_B_B;
    Eigen::Vector3f t_B_B;
    SampleTransformation(angle, translation, R_B_B, t_B_B);
    // Transform pc B
    for (uint32_t i=0; i<pcOutB.size(); ++i) {
      Eigen::Map<Eigen::Vector3f> p(&(pcOutB.at(i).x));
      p = R_B_B * p + t_B_B;
      Eigen::Map<Eigen::Vector3f> n(pcOutB.at(i).normal);
      n = R_B_B*n;
    }
    // chain the new transformation into the global transformation.
    R_B_W = R_B_B*R_B_W;
    t_B_W = R_B_B*t_B_W + t_B_B;
  }
  // Write the Point clouds to files.
  pcl::PLYWriter writer;
  writer.write(outputPathA, pcOutA, false, false);
  writer.write(outputPathB, pcOutB, false, false);

  // Compute the final relative transformation between the PCs in the
  // as they are saved to file. This is the transformation that any
  // alignment algorithm needs to find.
  Eigen::Matrix3f R_B_A = R_B_W * R_A_W.transpose();
  Eigen::Vector3f t_B_A = - R_B_W * R_A_W.transpose() * t_A_W + t_B_W;
  Eigen::Quaternionf q(R_B_A);
  Eigen::Vector3f t = t_B_A;

  std::cout << "magnitude of rotation: " << ToDeg(acos(q.w())*2.)
    << " magnitude of translation: " << t_B_A.norm() << std::endl;

  std::ofstream out(transformationTotalOutputPath.c_str());
  out << "q_w q_x q_y q_z t_x t_y t_z overlap sizeA sizeB" << std::endl;
  out << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " "
    << t(0) << " " << t(1) << " " << t(2) << " "
    << overlap << " " << pcOutA.size() << " " << pcOutB.size();
  out.close();

  std::cout<< " output to "<<outputPathA<<std::endl << " and to " << outputPathB << std::endl;
  std::cout<< " sampled total transformation to " << transformationTotalOutputPath << std::endl;
  std::cout<< " sampled transformation of rendering T_A_W to "
    << transformationAOutputPath << std::endl;
  std::cout<< " sampled transformation of rendering T_B_W to "
    << transformationBOutputPath << std::endl;
  return 0;
}


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

// This program will make use of the ATOM solver to construct a transfer trajectory
// between two points in space. Only a single transfer segment is simulated here, that is, no multitargeting.
// There is just one departure and one arrival object considered in the simulation. The object TLEs are
// taken from a catalog file. The user can specify which object will be departure and which will be arrival
// along with the time of flight.

#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <fstream>
#include <exception>
#include <cstdlib>
#include <iterator>

#include <libsgp4/Globals.h>
#include <libsgp4/SGP4.h>
#include <libsgp4/Tle.h>

#include <Atom/atom.hpp>
#include <Atom/convertCartesianStateToTwoLineElements.hpp>

#include <Astro/orbitalElementConversions.hpp>

#include <SML/sml.hpp>
#include <SML/constants.hpp>
#include <SML/basicFunctions.hpp>
#include <SML/linearAlgebra.hpp>

#include <boost/array.hpp>

#include </usr/local/abhi/pykep/src/lambert_problem.cpp>
#include </usr/local/abhi/pykep/src/lambert_problem.h>
#include </usr/local/abhi/pykep/src/keplerian_toolbox.h>

typedef double Real;
typedef std::vector < Real > Vector6;
typedef std::vector < Real > Vector3;
typedef std::vector < Real > Vector2;
typedef std::vector < std::vector < Real > > Vector2D;
typedef boost::array < Real, 3 > array3;

//! Remove newline characters from string.
void removeNewline( std::string& string )
{
    string.erase( std::remove( string.begin( ), string.end( ), '\r' ), string.end( ) );
    string.erase( std::remove( string.begin( ), string.end( ), '\n' ), string.end( ) );
}

//! Convert SGP4 ECI object to state vector.
Vector6 getStateVector( const Eci state )
{
    Vector6 result( 6 );
    result[ 0 ] = state.Position( ).x;
    result[ 1 ] = state.Position( ).y;
    result[ 2 ] = state.Position( ).z;
    result[ 3 ] = state.Velocity( ).x;
    result[ 4 ] = state.Velocity( ).y;
    result[ 5 ] = state.Velocity( ).z;
    return result;
}

Vector6 getStateVectorInMetre( const Eci state )
{
    Vector6 result( 6 );
    result[ 0 ] = state.Position( ).x * 1000.0;
    result[ 1 ] = state.Position( ).y * 1000.0;
    result[ 2 ] = state.Position( ).z * 1000.0;
    result[ 3 ] = state.Velocity( ).x * 1000.0;
    result[ 4 ] = state.Velocity( ).y * 1000.0;
    result[ 5 ] = state.Velocity( ).z * 1000.0;
    return result;
}

Vector6 standardKeplerian( const Vector6 elements )
{
    Vector6 result( 6 );
    result[ 0 ] = elements[ 0 ] / 1000.0;
    result[ 1 ] = elements[ 1 ];
    result[ 2 ] = sml::convertRadiansToDegrees( elements[ 2 ] );
    result[ 3 ] = sml::convertRadiansToDegrees( elements[ 3 ] );
    result[ 4 ] = sml::convertRadiansToDegrees( elements[ 4 ] );
    result[ 5 ] = sml::convertRadiansToDegrees( elements[ 5 ] );
    return result;
}

int main( void )
{
    // conversion from km to m
    // const double km2m = 1000;
    const double Rearth = kXKMPER; // earth radius in km
    // earth radius and diameter
    // const double EarthRadius = kXKMPER * km2m; // unit m
    // const double EarthDiam = 2 * EarthRadius;

    // grav. parameter 'mu' of earth
    const double muEarth = kMU*( pow( 10, 9 ) ); // unit m^3/s^2

    // vectors to store arrival and departure velocities for the transfer trajectory
    Vector3 DepartureVelocity( 3 );
    Vector3 ArrivalVelocity( 3 );

    // read the TLE file. Line based parsing, using string streams
    std::string line;

    // std::ifstream tlefile( "../../src/napa_prograde_catalog.txt" );
    // const bool is_retro = false;

    std::ifstream tlefile( "../../src/napa_retrograde_catalog.txt" );
    const bool is_retro = true;

    if( !tlefile.is_open() )
        perror("error while opening file");

    std::vector < Tle > tleObjects; // vector of TLE objects

    while( !tlefile.eof( ) )
    {
        std::vector < std::string > tleStrings;

        std::getline( tlefile, line ); // read line from the catalog file
        removeNewline( line ); // remove new line characters such as '/n'
        tleStrings.push_back( line );
        // std::cout << line << std::endl;

        std::getline( tlefile, line );
        removeNewline( line );
        tleStrings.push_back( line );
        // std::cout << line << std::endl;

        std::getline( tlefile, line );
        removeNewline( line );
        tleStrings.push_back( line );
        // std::cout << line << std::endl << std::endl;

        tleObjects.push_back( Tle( tleStrings[ 0 ], tleStrings[ 1 ], tleStrings[ 2 ] ) );
    }
    tlefile.close( );

    const int DebrisObjects = tleObjects.size( );
    std::cout << "Total debris objects = " << DebrisObjects << std::endl;

    // some variables for the "catch" segment
    int failCount = 0;
    int catchDepartureID;
    int catchArrivalID;

    std::cout.precision( 15 );

    //*********************** Inputs *********************************************************************************************************//
    Tle departureObject = tleObjects[ 2 ];
    Tle arrivalObject = tleObjects[ 3 ];
    DateTime departureEpoch;
    // departureEpoch.Initialise( 2016, 1, 11, 4, 51, 21, 455936 );
    departureEpoch.Initialise( 2016, 1, 11, 4, 50, 21, 0 );
    const double TOF = 15130.0; // time of flight
    //***************************************************************************************************************************************//

    SGP4 sgp4Departure( departureObject );
    const Eci tleDepartureState = sgp4Departure.FindPosition( departureEpoch );
    const Vector6 departureState = getStateVector( tleDepartureState );

    array3 departurePosition;
    array3 departureVelocity;
    for( int j = 0; j < 3; j++ )
    {
        departurePosition[ j ] = departureState[ j ];
        departureVelocity[ j ] = departureState[ j + 3 ];
    }

    const int departureObjectId = static_cast< int >( departureObject.NoradNumber( ) );
    catchDepartureID = departureObjectId; // for the catch segment of the program

    SGP4 sgp4Arrival( arrivalObject );
    const int arrivalObjectId = static_cast< int >( arrivalObject.NoradNumber( ) );
    catchArrivalID = arrivalObjectId;

    const DateTime arrivalEpoch = departureEpoch.AddSeconds( TOF );
    const Eci tleArrivalState = sgp4Arrival.FindPosition( arrivalEpoch );
    // const Eci tleArrivalState = sgp4Arrival.FindPosition( 0.0 );
    const Vector6 arrivalState = getStateVector( tleArrivalState );

    array3 arrivalPosition;
    array3 arrivalVelocity;
    for( int j = 0; j < 3; j++ )
    {
        arrivalPosition[ j ] = arrivalState[ j ];
        arrivalVelocity[ j ] = arrivalState[ j + 3 ];
    }

    kep_toolbox::lambert_problem targeter( departurePosition, arrivalPosition, TOF, kMU, is_retro, 5 );
    const int numberOfSolutions = targeter.get_v1( ).size( );
    std::vector< array3 > departureDeltaVs( numberOfSolutions ); // delta-V components at the departure point
    std::vector< array3 > arrivalDeltaVs( numberOfSolutions );
    std::vector< Real > transferDeltaVs( numberOfSolutions ); // magnitude of the total delta-V of one transfer between two points

    for ( int j = 0; j < numberOfSolutions; j++ )
    {
        array3 transferDepartureVelocity = targeter.get_v1( )[ j ]; // velocity of the s/c at the departure point in the transfer orbit
        array3 transferArrivalVelocity = targeter.get_v2( )[ j ];

        departureDeltaVs[ j ] = sml::add( transferDepartureVelocity, sml::multiply( departureVelocity, -1.0 ) );
        arrivalDeltaVs[ j ] = sml::add( transferArrivalVelocity, sml::multiply( arrivalVelocity, -1.0 ) );

        transferDeltaVs[ j ] = sml::norm< Real >( departureDeltaVs[ j ] ) + sml::norm< Real >( arrivalDeltaVs[ j ] );
    }

    const std::vector< Real >::iterator minDeltaVIterator = std::min_element( transferDeltaVs.begin( ), transferDeltaVs.end( ) );
    const int minimumDeltaVIndex = std::distance( transferDeltaVs.begin( ), minDeltaVIterator );

    Real lambertDepartureBurn = sml::norm< Real >( departureDeltaVs[ minimumDeltaVIndex ] );
    Real lambertArrivalBurn = sml::norm< Real >( arrivalDeltaVs[ minimumDeltaVIndex ] );
    Real lambertDV = transferDeltaVs[ minimumDeltaVIndex ];

    array3 minIndexDepartureVelocity = targeter.get_v1( )[ minimumDeltaVIndex ]; // best guess for velocity in transfer orbit at the departure point

    Vector6 lambertDepState( 6 );
    for ( int i = 0; i < 3; i++ )
    {
        lambertDepState[ i ] = departurePosition [ i ] * 1000.0;
        lambertDepState[ i + 3 ] = minIndexDepartureVelocity[ i ] * 1000.0;
    }
    const Real tolerance = 10.0 * std::numeric_limits< Real >::epsilon( );
    Vector6 LambertKep( 6 );
    LambertKep = astro::convertCartesianToKeplerianElements( lambertDepState, muEarth, tolerance );
    Vector6 LambertKepStd = standardKeplerian( LambertKep );
    std::cout << std::endl << "LambertKep = " << LambertKepStd << std::endl << std::endl;

    Vector3 departureVelocityGuess( 3 );
    Vector3 atomDeparturePosition( 3 );
    Vector3 atomArrivalPosition( 3 );

    for( int j = 0; j < 3; j++ )
    {
        departureVelocityGuess[ j ] = minIndexDepartureVelocity[ j ];
        atomDeparturePosition[ j ] = departurePosition[ j ];
        atomArrivalPosition[ j ] = arrivalPosition[ j ];
    }

    array3 atomDepartureVelocity;
    array3 atomArrivalVelocity;

    std::string SolverStatusSummary;
    int numberOfIterations;
    const int maxIterations = 100;
    const Tle referenceTle = Tle( );

    try
    {
        Vector3 outputDepartureVelocity( 3 );
        Vector3 outputArrivalVelocity( 3 );
        atom::executeAtomSolver< Real, Vector3 >( atomDeparturePosition,
                                                  departureEpoch,
                                                  atomArrivalPosition,
                                                  TOF,
                                                  departureVelocityGuess,
                                                  outputDepartureVelocity,
                                                  outputArrivalVelocity,
                                                  SolverStatusSummary,
                                                  numberOfIterations,
                                                  referenceTle,
                                                  kMU,
                                                  kXKMPER,
                                                  1.0e-10,
                                                  1.0e-5,
                                                  maxIterations );

        Vector6 atomDepState( 6 );
        for ( int i = 0; i < 3; i++ )
        {
            atomDepState[ i ] = departurePosition[ i ] * 1000.0;
            atomDepState[ i + 3 ] = outputDepartureVelocity[ i ] * 1000.0;
        }
        Vector6 atomDepKep = astro::convertCartesianToKeplerianElements( atomDepState, muEarth, tolerance );
        Vector6 atomDepKepStd( 6 );
        atomDepKepStd = standardKeplerian( atomDepKep );
        std::cout << std::endl << "atom dep kep = " << atomDepKepStd << std::endl << std::endl;

        for( int k = 0; k < 3; k++)
        {
            atomDepartureVelocity[ k ] = outputDepartureVelocity[ k ];
            atomArrivalVelocity[ k ] = outputArrivalVelocity[ k ];
        }

        array3 atomDepartureDeltaV;
        array3 atomArrivalDeltaV;
        Real AtomDeltaV;

        atomDepartureDeltaV = sml::add( atomDepartureVelocity, sml::multiply( departureVelocity, -1.0 ) );
        atomArrivalDeltaV = sml::add( atomArrivalVelocity, sml::multiply( arrivalVelocity, -1.0 ) );

        double AtomDepartureBurn = sml::norm< Real >( atomDepartureDeltaV );
        double AtomArrivalBurn = sml::norm< Real >( atomArrivalDeltaV );
        AtomDeltaV = sml::norm< Real >( atomDepartureDeltaV ) + sml::norm< Real >( atomArrivalDeltaV );
        std::cout << "Atom DeltaV = " << AtomDeltaV << std::endl;
    }

    catch( const std::exception& err )
    {
        ++failCount;
        std::cout << "Exception Caught = " << err.what( ) << std::endl;
        std::cout << "For departure ID = " << catchDepartureID << " " << "For Arrival ID = " << catchArrivalID << std::endl << std::endl;
        // std::cout << "Fail count = " << failCount << std::endl;
    }

   return EXIT_SUCCESS;
}


/*
    Copyright 2012 Ulrik Mikaelsson <ulrik.mikaelsson@gmail.com>

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

#include "treestore.hpp"

#include <boost/assert.hpp>

uint32_t calc_leaves(uint32_t treesize, int layers)
{
	if (treesize == 0) return 0;
	if (layers == 0) return 1;
	uint32_t leftside = 1 << layers;
	if (leftside <= treesize)
		return (1<<(layers-1)) + calc_leaves(treesize-leftside, layers-1);
	else
		return calc_leaves(treesize-1, layers-1);
}

uint32_t calc_leaves(uint32_t treesize) {
	BOOST_ASSERT(treesize >= 1);
	uint32_t layers = (int)log2f(treesize);
	return calc_leaves(treesize, layers);
}

std::ostream& operator<<(std::ostream& str, const NodeIdx& idx)
{
	return str << "NodeIdx("<<idx.nodeIdx<<','<<idx.layerSize<<')';
}


#define BOOST_TEST_MAIN TestExponentialHistogram
#include <boost/test/unit_test.hpp>
#include <stdexcept>
#include <sam/ExponentialHistogram.hpp>

using namespace sam;

BOOST_AUTO_TEST_CASE( eh_test_numlevels )
{

  BOOST_CHECK_THROW(ExponentialHistogram<size_t>(0,2), std::out_of_range);
  ExponentialHistogram<size_t> eh1(1, 2);
  BOOST_CHECK_EQUAL(eh1.getNumLevels(), 1);
  ExponentialHistogram<size_t> eh2(2, 2);
  BOOST_CHECK_EQUAL(eh2.getNumLevels(), 1);
  ExponentialHistogram<size_t> eh3(3, 2);
  BOOST_CHECK_EQUAL(eh3.getNumLevels(), 1);
  ExponentialHistogram<size_t> eh4(4, 2);
  BOOST_CHECK_EQUAL(eh4.getNumLevels(), 2);
  ExponentialHistogram<size_t> eh5(5, 2);
  BOOST_CHECK_EQUAL(eh5.getNumLevels(), 2);
  ExponentialHistogram<size_t> eh9(9, 2);
  BOOST_CHECK_EQUAL(eh9.getNumLevels(), 2);
  ExponentialHistogram<size_t> eh10(10, 2);
  BOOST_CHECK_EQUAL(eh10.getNumLevels(), 3);
  ExponentialHistogram<size_t> eh21(21, 2);
  BOOST_CHECK_EQUAL(eh21.getNumLevels(), 3);
  ExponentialHistogram<size_t> eh22(22, 2);
  BOOST_CHECK_EQUAL(eh22.getNumLevels(), 4);
  BOOST_CHECK_THROW(ExponentialHistogram<size_t>(
                      ExponentialHistogram<size_t>::MAX_SIZE, 2),
                      std::out_of_range);
}

BOOST_AUTO_TEST_CASE( eh_test_add )
{
  ExponentialHistogram<size_t> eh(21, 2);
  BOOST_CHECK_EQUAL(eh.getNumSlots(), 22);
  BOOST_CHECK_EQUAL(eh.getTotal(), 0);
  for(int i = 0; i < 22; i++) {
    eh.add(1);
    BOOST_CHECK_EQUAL(eh.getTotal(), i + 1);
  }
  eh.add(1);
  BOOST_CHECK_EQUAL(eh.getTotal(), 15);
}

BOOST_AUTO_TEST_CASE( eh_test_long_add )
{
  ExponentialHistogram<size_t> eh(12285, 2);
  for(int i =0; i < 1000000000; i++) {
    eh.add(1);
  }
  BOOST_CHECK_CLOSE(static_cast<double>(eh.getTotal()),
                    static_cast<double>(12285),
                    static_cast<double>(pow(2,eh.getNumLevels()-1)));

}