#pragma once
#include "ros/ros.h"
#include <ros/console.h>
#include <boost/array.hpp>
#include <geometry_msgs/PointStamped.h>
#include <gnss_msgs/BaselinePosition.h>
#include <gnss_msgs/BaselineVelocity.h>
#include <gnss_msgs/RtkAvailable.h>
#include <geometry_msgs/PointStamped.h>
#include <piksi_rtk_msgs/BaselineNed.h>
#include <piksi_rtk_msgs/VelNedCov.h>
#include <libsbp_ros_msgs/MsgBaselineNed.h>
#include <libsbp_ros_msgs/MsgVelNedCov.h>
#include <libsbp_ros_msgs/MsgPosLlh.h>
#include <libsbp_ros_msgs/MsgGpsTime.h>
#include <eigen3/Eigen/Dense>
#include <math.h>
#include <std_msgs/Bool.h>

#include <diagnostic_msgs/DiagnosticArray.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>

#include "libsbp_ros_msgs/ros_conversion.h"

class SwiftNavRover{
public:
    SwiftNavRover(ros::NodeHandle* node_handle, double la, double lo, double al);

    // WGS 84 derived geometric constants
    const double kSemimajorAxis = 6378137.0;
    const double kSemiminorAxis = 6356752.3142;
    const double kFirstEccentricitySquared = 6.69437999014 * 0.001;
    const double kSecondEccentricitySquared = 6.73949674228 * 0.001;
    const double kFlattening = 1/298.257223563;

    typedef Eigen::Matrix<double,3,3> Matrix3x3d;
    Matrix3x3d ecef_to_ned_matrix;

    ros::NodeHandle nh;

    /**
     * Initialize the publishers and subscribers for the ROS node
     * @param
     * @return
     */
    void init();

    // Publisher functions
    /**
     * Publish the baseline position (NED), with mesage type gnss_msgs/BaselinePosition
     * @param fields of the gnss_msgs/BaselinePosition message type
     * @return void
     */
    void publishBaselinePosition(ros::Time& t, double n, double e, double d, int tow, boost::array<double, 9>& covariance, int n_sats, int fixed_mode);

    /**
     * Publish the baseline velocity, with mesage type gnss_msgs/BaselineVelocity
     * @param fields of the gnss_msgs/BaselineVelocity message type
     * @return
     */
    void publishBaselineVelocity(ros::Time& t, int tow,double n,double e,double d,boost::array<double, 9>& covariance,int n_sats,int vel_mode,int ins_mode);

    /**
     * Publish the availability of RTK positioning, as a boolean
     * @param ROS timestamp
     * @return
     */
    void publishRTKAvailable(ros::Time& t);

    /**
     * Publish the diagnostic information of the GNSS driver
     * @param ROS timer
     * @return
     */
    void publishStatus(const ros::TimerEvent& event);

    // Callback functions
    /**
     * Callback function when message of type libsbp_ros_msgs/MsgVelNedCov is published
     * @param pointer to the message
     * @return
     */
    void baselineVelocityCallback(const libsbp_ros_msgs::MsgVelNedCov::ConstPtr & msg);

    /**
     * Callback function when message of type libsbp_ros_msgs/MsgPosLlh is published
     * @param pointer to the message
     * @return
     */
    void posLLHCallback(const libsbp_ros_msgs::MsgPosLlh::ConstPtr & msg);

    /**
     * Callback function when message of type libsbp_ros_msgs/MsgBaselineNed is published
     * @param pointer to the message
     * @return
     */
    void baselinePositionCallback(const libsbp_ros_msgs::MsgBaselineNed::ConstPtr & msg);

    /**
     * Callback function when message of type libsbp_ros_msgs/MsgGpsTime is published
     * @param pointer to the message
     * @return
     */
    void gpsTimeCallback(const libsbp_ros_msgs::MsgGpsTime::ConstPtr & msg);

    // Co-ordinate conversions
    /**
     * Convert geodetic frame position (LLH) to NED coordinates
     * @param LLH position, and pointer to the NED position variables
     * @return
     */
    void geodetic2ned(double latitude, double longitude, double altitude, double* north, double* east, double* down);

    /**
     * Convert geodetic frame position (LLH) to ECEF coordinates
     * @param LLH position, and pointer to the ECEF position variables
     * @return
     */
    void geodetic2ecef(double latitude, double longitude, double altitude, double *x, double *y, double *z);

    /**
     * Convert ECEF coordinates to NED coordinates
     * @param ECEF position, and pointer to the NED position variables
     * @return
     */
    void ecef2ned(double x_t, double y_t, double z_t, double* north, double* east, double* down);

    // Setters
    void setHomeECEF(double* x, double* y, double* z)
    {
        home_ecef_x_ = *x;
        home_ecef_y_ = *y;
        home_ecef_z_ = *z;
    }

    // Utility functions
    inline double deg2rad(double degrees)
    {
        return (degrees / 180.0) * M_PI;
    }

    inline Matrix3x3d nRe(const double lat_radians, const double lon_radians)
    {
        const double sLat = sin(lat_radians);
        const double sLon = sin(lon_radians);
        const double cLat = cos(lat_radians);
        const double cLon = cos(lon_radians);
        Matrix3x3d ret;
        ret(0, 0) = -sLat * cLon;
        ret(0, 1) = -sLat * sLon;
        ret(0, 2) = cLat;
        ret(1, 0) = -sLon;
        ret(1, 1) = cLon;
        ret(1, 2) = 0.0;
        ret(2, 0) = cLat * cLon;
        ret(2, 1) = cLat * sLon;
        ret(2, 2) = sLat;
        return ret;
    }

    private:
    // Timer to publish status messages at 1Hz
    ros::Timer status_timer_;

    double home_lat_, home_lon_, home_alt_;
    double home_ecef_x_, home_ecef_y_, home_ecef_z_;
    double home_lat_rad_, home_lon_rad_;

    // Declare subscribers
    ros::Subscriber vel_sub_;
    ros::Subscriber pos_sub_;
    ros::Subscriber pos_llh_sub_;
    ros::Subscriber gps_time_sub_;

    // Declare publishers
    ros::Publisher ned_point_fix_;
    ros::Publisher ned_baseline_position_fix_;
    ros::Publisher ned_vel_cov_fix_;
    ros::Publisher rtk_mode_available_;
    ros::Publisher diagnostic_publisher_;

    // Flag to check if satellites are visible - if this drops to 0,
    // RTK blackout has occured. If false, it means that RTK blackout
    // has occured
    bool satellite_check_ = false;

    // Variable to store the operation mode of the rover - float RTK,
    // fix-RTK, SBAS, etc
    int positioning_mode_;

    // /gps_time converted to UNIX time stamp
    ros::Time convertedTime_;

    int week_;
    int residual_;
    int leap_seconds_ = 18;
};


#include <iostream>
#include <ros/ros.h>
#include <Eigen/Core>
#include <vector>
//#include <Matrix.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/cloud_viewer.h>
//#include <pcl/common/centroid.h>
#include <pcl/registration/icp.h>
#include <pcl/point_cloud.h>
#include "pcl/common/transformation_from_correspondences.h"
//#include <boost/thread/thread.hpp>
#include <pcl/filters/project_inliers.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <vtkCellArray.h>
#include <iostream>
#include <ros/ros.h>
#include <Eigen/Core>
#include <vector>
//#include <Matrix.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/centroid.h>

#include <boost/thread/thread.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_traits.h>
#include <pcl/PointIndices.h>
#include <pcl/cloud_iterator.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/console/print.h>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <flann/flann.h>
//#include <flann/io/hdf5.h>
#include <boost/filesystem.hpp>

ros::Publisher pub;
pcl::PointCloud<pcl::PointXYZ>::Ptr cluster (new pcl::PointCloud<pcl::PointXYZ>);

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "obstacle_projection");
  ros::NodeHandle nh;

  pub = nh.advertise<pcl::PCLPointCloud2> ("cloud_demean", 1);
    // Load object and scene
  pcl::console::print_highlight ("Loading point clouds...\n");

  pcl::PCDReader reader;
  reader.read ("cloud_cluster_2.pcd", *cluster);

 Eigen::Vector4f centroid;
 pcl::compute3DCentroid (*cluster, centroid);
 cout << centroid[0] << " " <<  centroid[1] << " " <<   centroid[2] << " " <<   centroid[3] << " \n";
 /*pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_demean (new pcl::PointCloud<pcl::PointXYZ>);
 pcl::demeanPointCloud<pcl::PointXYZ> (cloud_xyz, centroid, *cloud_xyz_demean);

 pcl::console::print_highlight ("centroid\n", centroid);
 std::stringstream ss;
 ss << "cloud_centroid_" << j << ".pcd";
 writer.write<pcl::PointXYZ> (ss.str (), *cloud_xyz_demean, false);
 pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
 viewer.showCloud (cloud_xyz_demean);
 pcl::PCLPointCloud2 outcloud_tmp_visual;
 pcl::toPCLPointCloud2 (cloud_xyz_demean, outcloud_tmp_visual);
 pub.publish (outcloud_tmp_visual); */

  //ros::spin ();
  return (0);
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file   export_slam
 * @brief  wraps slam classes
 * @author Ellon Paiva Mendes (LAAS-CNRS)
 * @author Frank Dellaert
 **/

#include <boost/python.hpp>

#define NO_IMPORT_ARRAY
#include <numpy_eigen/NumpyEigenConverter.hpp>

#include <gtsam/slam/RotateFactor.h>

using namespace boost::python;
using namespace gtsam;
using namespace std;

void export_slam() {
  class_<RotateDirectionsFactor, bases<NonlinearFactor>>(
      "RotateDirectionsFactor",
      init<Key, const Unit3&, const Unit3&, noiseModel::Base::shared_ptr>())
      .def("Initialize", &RotateDirectionsFactor::Initialize)
      .staticmethod("Initialize");
}


#include <kv/allsol-simple.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <boost/timer.hpp>
#ifdef TEST_DD
#include <kv/dd.hpp>
#include <kv/rdd.hpp>
#endif

using namespace std;

namespace ub = boost::numeric::ublas;

struct Func {
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		ub::vector<T> y(2);

		y(0) = x(0) * x(0) + x(1) * x(1) - 1.;
		y(1) = x(0) - x(1);

		return y;
	}
};

struct Yamamura {
	template <class T> T g(const T& x){
		return 2.5 * x*x*x - 10.5 * x*x + 11.8 * x;
	}
	template <class T> ub::vector<T> operator() (const ub::vector<T>& x){
		int n = x.size();
		ub::vector<T> y(n);
		T s;
		int i;

		s = 0.;
		for (i=0; i<n; i++) s += x(i);

		for (i=0; i<n; i++) {
			y(i) = g(x(i)) + s - (i+1.);
		}

		return y;
	}
};

#ifdef TEST_DD
typedef kv::interval<kv::dd> itv;
#else
typedef kv::interval<double> itv;
#endif

int main()
{
#ifdef TEST_DD
	std::cout.precision(33);
#else
	std::cout.precision(17);
#endif

	int i;
	boost::timer t;
	ub::vector< itv > I;

	Func f1;

	I.resize(2);
	for (i=0; i<I.size(); i++) I(i) = itv(-10., 10.);

	t.restart();
	kv::allsol_simple(f1, I, 1);
	cout << t.elapsed() << " sec\n";

	Yamamura f2;

	I.resize(5);
	for (i=0; i<I.size(); i++) I(i) = itv(-10., 10.);

	t.restart();
	kv::allsol_simple(f2, I, 1);
	cout << t.elapsed() << " sec\n";
}


/*
 *  Copyright 2015 Aldebaran. All rights reserved.
 *
 */
#include <almath/scenegraph/meshfactory.h>
#include <almath/scenegraph/mesh.h>
#include <stdexcept>
#include <cmath>
#include <stddef.h>
#include <boost/math/constants/constants.hpp>

namespace AL {

void addBoxMesh(float xHE, float yHE, float zHE, Mesh &data) {
  /*****************
         4____5   vert
        /|  /|    coords
      3/_|_/_|6   ^ z
       | / | /    |
       |/__|/     |
       2   1      +---> y
  ******************/

  // vertex positions
  size_t v0 = data.position(xHE, yHE, zHE);  // 0 index is v0
  data.position(xHE, yHE, -zHE);             // 1
  data.position(xHE, -yHE, -zHE);            // 2
  data.position(xHE, -yHE, zHE);             // 3
  data.position(-xHE, -yHE, zHE);            // 4
  data.position(-xHE, yHE, zHE);             // 5
  data.position(-xHE, yHE, -zHE);            // 6
  data.position(-xHE, -yHE, -zHE);           // 7

  // begin inputing vertices
  data.begin(Mesh::QUADS);

  // front face
  data.normal(1.f, 0.f, 0.f);
  data.vertex(v0);
  data.vertex(v0 + 3);
  data.vertex(v0 + 2);
  data.vertex(v0 + 1);

  // back face
  data.normal(-1.f, 0.f, 0.f);
  data.vertex(v0 + 4);
  data.vertex(v0 + 5);
  data.vertex(v0 + 6);
  data.vertex(v0 + 7);

  // top face
  data.normal(0.f, 0.f, 1.f);
  data.vertex(v0 + 5);
  data.vertex(v0 + 4);
  data.vertex(v0 + 3);
  data.vertex(v0);

  // bottom face
  data.normal(0.f, 0.f, -1.f);
  data.vertex(v0 + 7);
  data.vertex(v0 + 6);
  data.vertex(v0 + 1);
  data.vertex(v0 + 2);

  // right face
  data.normal(0.f, 1.f, 0.f);
  data.vertex(v0 + 5);
  data.vertex(v0);
  data.vertex(v0 + 1);
  data.vertex(v0 + 6);

  // left face
  data.normal(0.f, -1.f, 0.f);
  data.vertex(v0 + 3);
  data.vertex(v0 + 4);
  data.vertex(v0 + 7);
  data.vertex(v0 + 2);

  data.end();
}

Mesh createBoxMesh(float xHalfExtent, float yHalfExtent, float zHalfExtent) {
  Mesh mesh;
  addBoxMesh(xHalfExtent, yHalfExtent, zHalfExtent, mesh);
  return mesh;
}

// small helper to hold the index of the postion and the normal of a vertex
class Vert {
 public:
  explicit Vert(size_t position = Mesh::NO_INDEX,
                size_t normal = Mesh::NO_INDEX)
      : p(position), n(normal) {}
  size_t p;
  size_t n;
  Vert operator+(int idx) { return Vert(p + idx, n + idx); }
};

static void xAddVert(const Vert &vert, Mesh &data) {
  data.normal(vert.n);
  data.vertex(vert.p);
}

static Vert xAddMeridian(float pHalfExtentZ, float pRadius, int pNbVertices,
                         float x, float y,
                         float lambda,  // azimuthal angle
                         Mesh &data) {
  Vert vert(data.positionsNb(), data.normalsNb());
  const int nphi = 2 * (pNbVertices + 1);
  const float dphi = 3.14f / nphi;
  float z = pHalfExtentZ;
  for (int iphi = 1; iphi < nphi; ++iphi) {
    float phi = iphi * dphi;  // polar angle
    float rxy = pRadius * std::sin(phi);
    data.normal(std::sin(phi) * std::cos(lambda),
                std::sin(phi) * std::sin(lambda), std::cos(phi));
    data.position(x + rxy * std::cos(lambda), y + rxy * std::sin(lambda),
                  z + pRadius * std::cos(phi));
    if ((iphi % (nphi / 2) == 0) && pHalfExtentZ != 0) {
      z = -pHalfExtentZ;
      data.normal(std::sin(phi) * std::cos(lambda),
                  std::sin(phi) * std::sin(lambda), std::cos(phi));
      data.position(x + rxy * std::cos(lambda), y + rxy * std::sin(lambda),
                    z + pRadius * std::cos(phi));
    }
  }
  return vert;
}

class Sector {
 public:
  Vert north;
  Vert south;
  float x, y;
  int ratio;
  std::vector<Vert> meridians;
};

int xSectorNbAdditionalVertices(float pHalfExtentZ, float pRadius,
                                int pNbArcVertices) {
  assert(pRadius != 0.f);
  return 2 * pNbArcVertices + ((pHalfExtentZ == 0) ? 1 : 2);
}

Sector xAddSector(float pHalfExtentZ, float pRadius, int pNbArcVertices,
                  float x, float y, float pAzimuthStart, float pAzimuthStep,
                  int pAzimuthNbSteps, size_t pNorthNormal, size_t pSouthNormal,
                  Mesh &data) {
  Sector q;
  int n = xSectorNbAdditionalVertices(pHalfExtentZ, pRadius, pNbArcVertices);
  float z = pHalfExtentZ + pRadius;
  q.north = Vert(data.position(x, y, z), pNorthNormal);
  q.south = Vert(data.position(x, y, -z), pSouthNormal);

  Vert pos, ppos;
  for (int j = 0; j < pAzimuthNbSteps; ++j) {
    float lambda = pAzimuthStart + j * pAzimuthStep;
    pos =
        xAddMeridian(pHalfExtentZ, pRadius, pNbArcVertices, x, y, lambda, data);
    assert(n == static_cast<int>(data.positionsNb() - pos.p));
    assert(n == static_cast<int>(data.normalsNb() - pos.n));
    if (ppos.p != Mesh::NO_INDEX) {
      data.begin(Mesh::TRIANGLES);
      xAddVert(q.north, data);
      xAddVert(ppos, data);
      xAddVert(pos, data);
      data.end();
      data.begin(Mesh::QUADS);
      for (int i = 0; i < n - 1; ++i) {
        xAddVert(pos + i, data);
        xAddVert(ppos + i, data);
        xAddVert(ppos + (1 + i), data);
        xAddVert(pos + (1 + i), data);
      }
      data.end();
      data.begin(Mesh::TRIANGLES);
      xAddVert(pos + (n - 1), data);
      xAddVert(ppos + (n - 1), data);
      xAddVert(q.south, data);
      data.end();
    }
    q.meridians.push_back(pos);
    ppos = pos;
  }
  return q;
}

static const float pi = boost::math::constants::pi<float>();

void addRoundedBoxMesh(float pHalfExtentX, float pHalfExtentY,
                       float pHalfExtentZ, float pRadius, int pNbArcVertices,
                       Mesh &data) {
  assert(pHalfExtentX >= 0.f);
  assert(pHalfExtentY >= 0.f);
  assert(pHalfExtentZ >= 0.f);

  if (pRadius == 0.f) {
    addBoxMesh(pHalfExtentX, pHalfExtentY, pHalfExtentZ, data);
    return;
  }
  assert(pRadius > 0.f);
  int n = xSectorNbAdditionalVertices(pHalfExtentZ, pRadius, pNbArcVertices);
  // add normals for the top and and bottom faces
  size_t northNormal = data.normal(0.f, 0.f, 1.f);
  size_t southNormal = data.normal(0.f, 0.f, -1.f);

  int nbSteps = 4 * (pNbArcVertices + 1);
  float step = 2 * pi / nbSteps;
  std::vector<Sector> sectors;
  // first quadrant
  if (pHalfExtentX == 0 && pHalfExtentY == 0) {
    sectors.push_back(xAddSector(pHalfExtentZ, pRadius, pNbArcVertices,
                                 pHalfExtentX, pHalfExtentY, 0.f, step, nbSteps,
                                 northNormal, southNormal, data));
  } else if (pHalfExtentX == 0) {
    sectors.push_back(xAddSector(
        pHalfExtentZ, pRadius, pNbArcVertices, pHalfExtentX, pHalfExtentY, 0.f,
        step, nbSteps / 2 + 1, northNormal, southNormal, data));
    sectors.push_back(xAddSector(
        pHalfExtentZ, pRadius, pNbArcVertices, pHalfExtentX, -pHalfExtentY, pi,
        step, nbSteps / 2 + 1, northNormal, southNormal, data));
  } else if (pHalfExtentY == 0) {
    sectors.push_back(xAddSector(
        pHalfExtentZ, pRadius, pNbArcVertices, pHalfExtentX, pHalfExtentY,
        -pi / 2, step, nbSteps / 2 + 1, northNormal, southNormal, data));
    sectors.push_back(xAddSector(
        pHalfExtentZ, pRadius, pNbArcVertices, -pHalfExtentX, pHalfExtentY,
        pi / 2, step, nbSteps / 2 + 1, northNormal, southNormal, data));
  } else {
    sectors.push_back(xAddSector(
        pHalfExtentZ, pRadius, pNbArcVertices, pHalfExtentX, pHalfExtentY, 0.f,
        step, nbSteps / 4 + 1, northNormal, southNormal, data));
    sectors.push_back(xAddSector(
        pHalfExtentZ, pRadius, pNbArcVertices, -pHalfExtentX, pHalfExtentY,
        pi / 2, step, nbSteps / 4 + 1, northNormal, southNormal, data));
    sectors.push_back(xAddSector(
        pHalfExtentZ, pRadius, pNbArcVertices, -pHalfExtentX, -pHalfExtentY, pi,
        step, nbSteps / 4 + 1, northNormal, southNormal, data));
    sectors.push_back(xAddSector(
        pHalfExtentZ, pRadius, pNbArcVertices, pHalfExtentX, -pHalfExtentY,
        3 * pi / 2, step, nbSteps / 4 + 1, northNormal, southNormal, data));
  }

  assert(sectors.size() > 0u);
  for (size_t i = 0; i < sectors.size(); ++i) {
    size_t ii = (i + 1) % (sectors.size());
    Vert prev = sectors[i].meridians.back();
    Vert next = sectors[ii].meridians.front();
    data.begin(Mesh::QUADS);
    xAddVert(sectors[ii].north, data);
    xAddVert(sectors[i].north, data);
    xAddVert(prev, data);
    xAddVert(next, data);
    for (int j = 0; j < n - 1; ++j) {
      xAddVert(next + j, data);
      xAddVert(prev + j, data);
      xAddVert(prev + (1 + j), data);
      xAddVert(next + (1 + j), data);
    }
    xAddVert(next + (n - 1), data);
    xAddVert(prev + (n - 1), data);
    xAddVert(sectors[i].south, data);
    xAddVert(sectors[ii].south, data);
    data.end();
  }
  if (sectors.size() >= 3) {
    data.begin(Mesh::POLYGON);
    for (size_t i = 0; i < sectors.size(); ++i) {
      xAddVert(sectors[i].north, data);
    }
    data.end();
    data.begin(Mesh::POLYGON);
    for (size_t i = sectors.size(); i > 0; --i) {
      xAddVert(sectors[i - 1u].south, data);
    }
    data.end();
  }
}

Mesh createRoundedBoxMesh(float pHalfExtentX, float pHalfExtentY,
                          float pHalfExtentZ, float pRadius, int pNbVertices) {
  Mesh mesh;
  addRoundedBoxMesh(pHalfExtentX, pHalfExtentY, pHalfExtentZ, pRadius,
                    pNbVertices, mesh);
  return mesh;
}
}


/**

Copyright (c) 2016, Hutmacher Robin, Kleinert Daniel, Meißner Pascal
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include <ivt_bridge/ivt_calibration.h>
#include <boost/make_shared.hpp>

#include <iostream>

namespace ivt_bridge {

cv::Mat_<double> matx34ToMat_(const cv::Matx<double, 3,4> &matx) {
	cv::Mat_<double>M(3,4);
	for (int i = 0; i < M.rows; ++i) {
		for (int j = 0; j < M.cols; ++j) {
			M(i,j) = matx(i,j);
		}
	}
	return M;
}

cv::Mat_<double> matx33ToMat_(const cv::Matx<double, 3,3> &matx) {
	cv::Mat_<double>M(3,3);
	for (int i = 0; i < M.rows; ++i) {
		for (int j = 0; j < M.cols; ++j) {
			M(i,j) = matx(i,j);
		}
	}
	return M;
}

cv::Mat_<double> matx35ToMat_(const cv::Matx<double, 3,5> &matx) {
	cv::Mat_<double>M(3,5);
	for (int i = 0; i < M.rows; ++i) {
		for (int j = 0; j < M.cols; ++j) {
			M(i,j) = matx(i,j);
		}
	}
	return M;
}

boost::shared_ptr<CCalibration> calibrationImpl(const image_geometry::PinholeCameraModel& cam_model, bool forRectifiedImages){
	boost::shared_ptr<CCalibration> calibration = boost::make_shared<CCalibration>();

	Mat3d ivtR; Vec3d ivtt;
	// Set with + height
	calibration->SetCameraParameters(0, 0, 0, 0, 0, 0, 0, 0, ivtR, ivtt, cam_model.rawRoi().width, cam_model.rawRoi().height);

	cv::Mat_<double> projectionMatrix = matx34ToMat_(cam_model.projectionMatrix());
	cv::Mat_<double> intrinsicMatrix = matx33ToMat_(cam_model.intrinsicMatrix());
	cv::Mat_<double> R = matx33ToMat_(cam_model.rotationMatrix());
	cv::Mat_<double> t;
	const cv::Mat_<double> distortion;

	if (forRectifiedImages) {
		cv::decomposeProjectionMatrix(projectionMatrix, intrinsicMatrix, R, t);
		// IVT calibrates in mm while ROS does it in m. (-1) determined empirical.
		t = t * -1000;
		calibration->SetDistortion(0, 0, 0, 0);
	} else {
		//intrinsicMatrix = cam_model.intrinsicMatrix();
		// Rec = P[0,3]^(-1) * H * K (src:Learning OpenCV Book) => H = P[0,3] * Rec * K^(-1)
		const cv::Mat_<double> H = projectionMatrix.colRange(0,3) * R * intrinsicMatrix.inv();
		// P^(*) = H * P = H * K * [R|t] (src:  Image processing, Analysis, and Machine vision) => [R|t] = K^(-1) * H^(-1) * P^(*)
		const cv::Mat_<double> Rt = intrinsicMatrix.inv() * H.inv() * projectionMatrix;
		R = Rt.colRange(0,3);
		// IVT calibrates in mm while ROS does it in m.
		t = Rt.col(3) * 1000;
		cv::Mat_<double> distortion = cam_model.distortionCoeffs();
		calibration->SetDistortion(distortion(0,0), distortion(0,1), distortion(0,2), distortion(0,3));
	}

	calibration->SetIntrinsicBase(intrinsicMatrix(0,2), intrinsicMatrix(1,2), intrinsicMatrix(0,0), intrinsicMatrix(1,1));

	Math3d::SetMat(ivtR,
			R(0,0), R(0,1), R(0,2),
			R(1,0), R(1,1), R(1,2),
			R(2,0), R(2,1), R(2,2));
	calibration->SetRotation(ivtR);

	Math3d::SetVec(ivtt, t(0,0), t(1,0), t(2,0));
	calibration->SetTranslation(ivtt);

	return calibration;

}

bool IvtCalibration::fromCameraInfo(const sensor_msgs::CameraInfo& msg){
	return cam_model.fromCameraInfo(msg);
}

bool IvtCalibration::fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg){
	return cam_model.fromCameraInfo(msg);
}

boost::shared_ptr<CCalibration> IvtCalibration::getCalibration(bool forRectifiedImages) const{
	return calibrationImpl(cam_model, forRectifiedImages);
}

bool IvtStereoCalibration::fromCameraInfo(const sensor_msgs::CameraInfo& left, const sensor_msgs::CameraInfo& right){
	return cam_model.fromCameraInfo(left, right);
}

bool IvtStereoCalibration::fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& left, const sensor_msgs::CameraInfoConstPtr& right){
	return cam_model.fromCameraInfo(left, right);
}

boost::shared_ptr<CStereoCalibration> IvtStereoCalibration::getStereoCalibration(bool forRectifiedImages) const{
	boost::shared_ptr<CStereoCalibration> stereoCalibration = boost::make_shared<CStereoCalibration>();
	stereoCalibration->SetSingleCalibrations(*calibrationImpl(cam_model.left(), forRectifiedImages), *calibrationImpl(cam_model.right(), forRectifiedImages), true);

	Mat3d Hl, Hr;
	cv::Mat_<double> projectionMatrixLeft = matx34ToMat_(cam_model.left().projectionMatrix());
	cv::Mat_<double> projectionMatrixRight = matx34ToMat_(cam_model.right().projectionMatrix());
	cv::Mat_<double> intrinsicMatrixLeft = matx33ToMat_(cam_model.left().intrinsicMatrix());
	cv::Mat_<double> intrinsicMatrixRight = matx33ToMat_(cam_model.right().intrinsicMatrix());
	cv::Mat_<double> RLeft = matx33ToMat_(cam_model.left().rotationMatrix());
	cv::Mat_<double> RRight = matx33ToMat_(cam_model.right().rotationMatrix());

	if (forRectifiedImages) {
		// use identity
		Math3d::SetMat(Hl, 1,0,0,0,1,0,0,0,1);
		Math3d::SetMat(Hr, 1,0,0,0,1,0,0,0,1);
	} else {
		// ivt saves H inverted
		// Rec = P[0,3]^(-1) * H * K (src:Learning OpenCV Book) => H = P[0,3] * Rec * K^(-1)
		const cv::Mat_<double> rectificationMatrixLeft = (projectionMatrixLeft.colRange(0,3) * RLeft * intrinsicMatrixLeft.inv()).inv();
		Math3d::SetMat(Hl,
				rectificationMatrixLeft(0,0), rectificationMatrixLeft(0,1), rectificationMatrixLeft(0,2),
				rectificationMatrixLeft(1,0), rectificationMatrixLeft(1,1), rectificationMatrixLeft(1,2),
				rectificationMatrixLeft(2,0), rectificationMatrixLeft(2,1), rectificationMatrixLeft(2,2));

		const cv::Mat_<double> rectificationMatrixRight = (projectionMatrixRight.colRange(0,3) * RRight * intrinsicMatrixRight.inv()).inv();
		Math3d::SetMat(Hr,
				rectificationMatrixRight(0,0), rectificationMatrixRight(0,1), rectificationMatrixRight(0,2),
				rectificationMatrixRight(1,0), rectificationMatrixRight(1,1), rectificationMatrixRight(1,2),
				rectificationMatrixRight(2,0), rectificationMatrixRight(2,1), rectificationMatrixRight(2,2));
	}

	stereoCalibration->rectificationHomographyLeft = Hl;
	stereoCalibration->rectificationHomographyRight = Hr;

	return stereoCalibration;
}

} //namespace ivt_bridge


#include <boost/test/unit_test.hpp>
#include <vector>
#include <numeric>
#include <limits>
#include <list>
#include <set>
#include <map>
#include <stack>
#include <math.h>
#include <stdio.h>

using namespace std;

struct StudentScore
{
	int ID;
	int Score;
	int Date;
};
typedef list<int> Scores;

class ScoreBoard
{
public:
	typedef map<int, Scores> Board;

	struct InDescendingOrder
	{
		bool operator()( int x, int y )
		{
			return x > y;
		}
	};

	void Process( StudentScore* scores, int scoreCount )
	{
		// make a map with id and scores as list
		Board board;
		for( int i=0; i<scoreCount; ++i )
		{
			const StudentScore& s( scores[i] );
			board[ s.ID ].push_back( s.Score );
		}

		// iterate each item in the map and sort and take first 5 and make average
		Board::iterator B;
		for( B = board.begin(); B!= board.end(); ++B )
		{
			Scores& scores(B->second);
			scores.sort( InDescendingOrder() );

			int count = 0;
			int sum = 0;
			for( Scores::iterator S=scores.begin(); S!=scores.end() && count<5; ++S, ++count)
			{
				sum += *S;
			}

			double average = (double)sum / count;
		}
	}
};

BOOST_AUTO_TEST_CASE( TestQuiz3 )
{
	StudentScore scores[] = {
		{ 0, 89, 112233 },
		{ 0, 90, 112233 },
		{ 0, 91, 112233 },
		{ 0, 89, 112233 },
		{ 0, 99, 112233 },
		{ 0, 89, 112233 } };

	ScoreBoard board;
	board.Process( scores, 6 );

}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestCount
#include <boost/test/unit_test.hpp>

#include <string>

#include <boost/compute/command_queue.hpp>
#include <boost/compute/function.hpp>
#include <boost/compute/lambda.hpp>
#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/count.hpp>
#include <boost/compute/algorithm/count_if.hpp>
#include <boost/compute/algorithm/iota.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/constant_iterator.hpp>

#include "context_setup.hpp"

namespace bc = boost::compute;
namespace compute = boost::compute;

BOOST_AUTO_TEST_CASE(count_int)
{
    int data[] = { 1, 2, 1, 2, 3 };
    bc::vector<int> vector(data, data + 5);
    BOOST_CHECK_EQUAL(bc::count(vector.begin(), vector.end(), 1), size_t(2));
    BOOST_CHECK_EQUAL(bc::count(vector.begin(), vector.end(), 2), size_t(2));
    BOOST_CHECK_EQUAL(bc::count(vector.begin(), vector.end(), 3), size_t(1));
    BOOST_CHECK_EQUAL(bc::count(vector.begin() + 1, vector.end(), 1), size_t(1));
    BOOST_CHECK_EQUAL(bc::count(vector.begin() + 1, vector.end() - 1, 3), size_t(0));
    BOOST_CHECK_EQUAL(bc::count(vector.begin() + 1, vector.end() - 1, 2), size_t(2));
}

BOOST_AUTO_TEST_CASE(count_constant_int_range)
{
    BOOST_CHECK_EQUAL(
        bc::count(bc::make_constant_iterator(18, 0),
                  bc::make_constant_iterator(18, 5),
                  18,
                  queue),
        size_t(5)
    );

    BOOST_CHECK_EQUAL(
        bc::count(bc::make_constant_iterator(19, 0),
                  bc::make_constant_iterator(19, 5),
                  18,
                  queue),
        size_t(0)
    );
}

BOOST_AUTO_TEST_CASE(count_if_greater_than_two)
{
    float data[] = { 1.0f, 2.5f, -1.0f, 3.0f, 5.0f, -8.0f };
    bc::vector<float> vector(data, data + 5);

    BOOST_CHECK_EQUAL(
        bc::count_if(vector.begin(), vector.end(), bc::_1 > 2.0f),
        size_t(3)
    );
}

BOOST_AUTO_TEST_CASE(count_int4)
{
    int data[] = { 1, 2, 3, 4,
                   4, 5, 6, 7,
                   7, 8, 9, 1,
                   1, 2, 3, 4,
                   4, 5, 6, 7,
                   0, 3, 2, 2 };
    bc::vector<bc::int4_> vector(reinterpret_cast<bc::int4_ *>(data),
                                 reinterpret_cast<bc::int4_ *>(data) + 6);
    BOOST_CHECK_EQUAL(vector.size(), size_t(6));

    BOOST_CHECK_EQUAL(
        bc::count(vector.begin(), vector.end(), bc::int4_(1, 2, 3, 4)),
        size_t(2)
    );
    BOOST_CHECK_EQUAL(
        bc::count(vector.begin(), vector.end(), bc::int4_(4, 5, 6, 7)),
        size_t(2)
    );
    BOOST_CHECK_EQUAL(
        bc::count(vector.begin(), vector.end(), bc::int4_(7, 8, 9, 1)),
        size_t(1)
    );
    BOOST_CHECK_EQUAL(
        bc::count(vector.begin(), vector.end(), bc::int4_(0, 3, 2, 2)),
        size_t(1)
    );
    BOOST_CHECK_EQUAL(
        bc::count(vector.begin(), vector.end(), bc::int4_(3, 4, 4, 5)),
        size_t(0)
    );
    BOOST_CHECK_EQUAL(
        bc::count(vector.begin(), vector.end(), bc::int4_(1, 2, 3, 0)),
        size_t(0)
    );
    BOOST_CHECK_EQUAL(
        bc::count(vector.begin(), vector.end(), bc::int4_(1, 9, 8, 7)),
        size_t(0)
    );
}

BOOST_AUTO_TEST_CASE(count_newlines)
{
    std::string string = "abcdefg\nhijklmn\nopqrs\ntuv\nwxyz\n";
    compute::vector<char> data(string.size());
    compute::copy(string.begin(), string.end(), data.begin());

    BOOST_CHECK_EQUAL(
        compute::count(data.begin(), data.end(), '\n'),
        size_t(5)
    );
}

BOOST_AUTO_TEST_CASE(count_uchar)
{
    using boost::compute::uchar_;

    unsigned char data[] = { 0x00, 0x10, 0x2F, 0x10, 0x01, 0x00, 0x01, 0x00 };
    compute::vector<uchar_> vector(8);
    compute::copy(data, data + 8, vector.begin());

    BOOST_CHECK_EQUAL(
        compute::count(vector.begin(), vector.end(), 0x00),
        size_t(3)
    );
    BOOST_CHECK_EQUAL(
        compute::count(vector.begin(), vector.end(), 0x10),
        size_t(2)
    );
    BOOST_CHECK_EQUAL(
        compute::count(vector.begin(), vector.end(), 0x2F),
        size_t(1)
    );
    BOOST_CHECK_EQUAL(
        compute::count(vector.begin(), vector.end(), 0x01),
        size_t(2)
    );
    BOOST_CHECK_EQUAL(
        compute::count(vector.begin(), vector.end(), 0xFF),
        size_t(0)
    );
}

BOOST_AUTO_TEST_CASE(count_vector_component)
{
    int data[] = {
        1, 2,
        3, 4,
        5, 6,
        7, 8
    };

    using boost::compute::int2_;

    compute::vector<int2_> vector(4);
    compute::copy(
        reinterpret_cast<int2_ *>(data),
        reinterpret_cast<int2_ *>(data) + 4,
        vector.begin()
    );

    using boost::compute::lambda::_1;
    using boost::compute::lambda::get;

    BOOST_CHECK_EQUAL(
        compute::count_if(vector.begin(), vector.end(), get<0>(_1) < 4),
        size_t(2)
    );
    BOOST_CHECK_EQUAL(
        compute::count_if(vector.begin(), vector.end(), get<1>(_1) > 3),
        size_t(3)
    );
}

BOOST_AUTO_TEST_CASE(count_if_odd)
{
    compute::vector<int> vec(2048, context);
    compute::iota(vec.begin(), vec.end(), 0, queue);

    BOOST_COMPUTE_FUNCTION(bool, is_odd, (int x),
    {
        return x & 1;
    });

    BOOST_CHECK_EQUAL(
        compute::count_if(vec.begin(), vec.end(), is_odd, queue), vec.size() / 2
    );
}

BOOST_AUTO_TEST_CASE(count_if_with_reduce)
{
    compute::vector<int> vec(2048, context);
    compute::iota(vec.begin(), vec.end(), 0, queue);

    using boost::compute::lambda::_1;

    BOOST_CHECK_EQUAL(
        compute::detail::count_if_with_reduce(
            vec.begin(), vec.end(), _1 > 1024, queue
        ),
        size_t(1023)
    );
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_BITWISE_FUNCTIONS_SCALAR_TWOPOWER_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_SCALAR_TWOPOWER_HPP_INCLUDED
#include <boost/simd/bitwise/functions/twopower.hpp>
#include <boost/simd/include/constants/digits.hpp>
#include <boost/dispatch/meta/adapted_traits.hpp>
#include <boost/simd/include/functions/scalar/is_ltz.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( twopower_, tag::cpu_, (A0)
                            , (scalar_< integer_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return (is_ltz(a0))?Zero<A0>():(One<A0>()<<a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( twopower_, tag::cpu_, (A0)
                            , (scalar_< unsigned_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1) { return One<A0>()<<a0; }
  };
} } }

#endif


#include "ccn.h"

#include <stdio.h>

#include <openssl/bn.h>
#include <openssl/bio.h>
#include <string>
#include <cstring>
#include <utility>
#include <iterator>
#include <typeinfo>
#include <openssl/crypto.h>
#include <boost/algorithm/string.hpp>

#include "utils.h"

#include <openssl/evp.h>

CCN::CCN(unsigned int keysize)
{
    this->keysize = keysize;
	this->numberG = BN_new();
    BIGNUM * number1 = BN_new();
    BN_one(number1);
    BN_add(this->numberG, number1, number1);
    BN_clear_free(number1);
};

CCN::~CCN(void)
{
    if(this->numberX_2)
        BN_clear_free(this->numberX_2);
    if(this->primeQ)
        BN_clear_free(this->primeQ);
    BN_clear_free(this->numberG);
};

void CCN::setPrimeQ(const BIGNUM * Q)
{
    this->primeQ = BN_dup(Q);
};

void CCN::setServerKey(const BIGNUM * serverKey)
{
    this->numberX_2 = BN_dup(serverKey);
};

void CCN::setNumberG(const BIGNUM * my_g)
{
    this->numberG = BN_dup(my_g);
};

BIGNUM * CCN::CCNTD(const std::pair<BIGNUM *, BIGNUM *> T)
{
    BIGNUM * aux = BN_new();
    BN_CTX * ctx = BN_CTX_new();
    BN_mod_exp(aux, T.first, this->numberX_2, this->primeQ, ctx);
    BN_CTX_free(ctx);

    BIGNUM * res_aux = BN_new();
    ctx = BN_CTX_new();
    BN_mul(res_aux, aux, T.second, ctx);
    BN_CTX_free(ctx);

    BIGNUM * res = BN_new();
    ctx = BN_CTX_new();
    BN_mod(res, res_aux, this->primeQ, ctx);
    BN_CTX_free(ctx);

    BN_clear_free(res_aux);
    BN_clear_free(aux);

    return res;
}

char * CCN::FullCCNTD(const std::vector<std::pair<BIGNUM *, BIGNUM *> > * content_names)
{
    //printf("FULLCCNTD\n");
    //std::vector<BIGNUM *> * res = new std::vector<BIGNUM *>(content_names->size());

    char * new_content_name;
    unsigned int offset = 0;
    new_content_name = (char *) calloc(sizeof(char), content_names->size() * ((SECURITY_KEYSIZE/4) + 1 + 1) ); // TODO: 1 for the /, 1 for the \0

    std::vector<std::pair<BIGNUM *, BIGNUM *> >::const_iterator it;

    /* Initialize name */
    memcpy((void *) (new_content_name + offset), (void *) "/", 1 + 1);
    offset +=1;

    for (it = content_names->begin() ; it != content_names->end(); it++)
    {

        std::pair<BIGNUM *, BIGNUM *> pair_name = *it;

        //printDEBUG(*pair_name.first);
        //printDEBUG(*pair_name.second);
        BIGNUM * encrypted_bignum = this->CCNTD(pair_name);
        //printDEBUG(*encrypted_bignum);
        //res->push_back(encrypted_bignum);

        char * aux_component = BN_bn2hex(encrypted_bignum);
        memcpy((void *) (new_content_name + offset), (void *) aux_component, (SECURITY_KEYSIZE/4));
        offset +=(SECURITY_KEYSIZE/4);
        memcpy((void *) (new_content_name + offset), (void *) "/", 1 + 1);
        offset +=1;

        BN_clear_free(encrypted_bignum);
        OPENSSL_free(aux_component);
    }

    return new_content_name;
}

char * CCN::FullCCNTD(const char * user_encrypted_trapdoors) // This function receives the bignums encoded into a CCN name
{
    /* split in keywords */
    std::vector<std::string> keywords;
    boost::split(keywords, user_encrypted_trapdoors, boost::is_any_of("/"));

    std::vector<std::pair<BIGNUM *, BIGNUM *> > * vec = new std::vector<std::pair<BIGNUM *, BIGNUM *> >();

    // This hack is done because the first component is ndnx:
    std::vector<std::string>::iterator it = keywords.begin();
    std::advance(it, 1);

    for(; it < keywords.end(); it+=2)
    {
        std::string t1 = *it;
        if (t1.compare("") == 0)
        {
            std::advance(it, 1);
            if (it == keywords.end())
                break;
            t1 = *it;
        }

		if (t1.compare("") == 0 or t1.size() < SECURITY_KEYSIZE/4/2) // horrible hack to handle when the are options in the name
            continue;

        //printf("retrieved t1: %s\n", t1.c_str());
        std::string t2 = *boost::next(it);
        //printf("retrieved t2: %s\n", t2.c_str());

        if (t1.compare("") == 0 or t2.compare("") == 0 or t1.size() < SECURITY_KEYSIZE/4/2 or t2.size() < SECURITY_KEYSIZE/4/2) // horrible hack
            continue;

        BIGNUM *p1 = BN_new();
        BIGNUM *p2 = BN_new();

        const char * c1 = t1.c_str();
        const char * c2 = t2.c_str();
        //printf("THe number are: %s, %s\n", c1, c2);
        BN_hex2bn(&p1, c1);
        BN_hex2bn(&p2, c2);

        //printDEBUG(*p1);
        //printDEBUG(*p2);

        std::pair<BIGNUM *, BIGNUM *> utrapdoor = std::make_pair(p1, p2);

        vec->push_back(utrapdoor);
    }

    char * res = this->FullCCNTD(vec);
    // free vec

    for(std::vector<std::pair<BIGNUM *, BIGNUM *> >::iterator it2 = vec->begin(); it2 != vec->end(); it2++)
    {
        std::pair<BIGNUM *, BIGNUM *> p = *it2;

        BN_clear_free(p.first);
        BN_clear_free(p.second);

    }
    delete vec;

    return res;
}

bool MatchTDs(const BIGNUM * a, const BIGNUM * b)
{
    return BN_cmp(a, b) == 0;
}

std::pair<BIGNUM *, BIGNUM *> CCN::ContentProviderReEnc(const std::pair<BIGNUM *, BIGNUM *> encrypted_payload)
{
	if(this->primeQ == NULL || this->numberG == NULL)
    {
        printf("Values not initialized\n");
        exit(-1);
    }

	BIGNUM * aux = BN_new();
	BN_CTX * ctx = BN_CTX_new();
	BN_mod_exp(aux, encrypted_payload.first, this->numberX_2, this->primeQ, ctx);
	BN_CTX_free(ctx);

	//Create duplicate of first pair
	BIGNUM * t_1 = BN_dup(encrypted_payload.first);

	// Multiply by second pair
	BIGNUM * t_2 = BN_new();
	ctx = BN_CTX_new();
	BN_mod_mul(t_2,  aux, encrypted_payload.second, this->primeQ, ctx);
	BN_CTX_free(ctx);

	BN_clear_free(aux);

	return std::make_pair(t_1, t_2);
}

std::pair<BIGNUM *, BIGNUM *> CCN::CCNPreDec(const std::pair<BIGNUM *, BIGNUM *> encrypted_payload)
{
	//TODO: add this value to global

	if(this->primeQ == NULL || this->numberG == NULL || this->numberX_2 == NULL)
    {
        printf("Values not initialized\n");
        exit(-1);
    }

	BIGNUM * t_1 = BN_dup(encrypted_payload.first);

	BIGNUM * aux = BN_new();
	BN_CTX * ctx = BN_CTX_new();
	BN_mod_exp(aux, encrypted_payload.first, this->numberX_2, this->primeQ, ctx);
	BN_CTX_free(ctx);

	ctx = BN_CTX_new();
	BIGNUM * aux2 = BN_new();
	aux2 = BN_mod_inverse(aux2, aux, this->primeQ, ctx);
	BN_CTX_free(ctx);

	if (aux2 == NULL)
	{
		printf("CCNPreDec:The calculus of the inverse has failed\n");
		exit(-1);
	}

	BIGNUM * t_2 = BN_new();
	ctx = BN_CTX_new();
	BN_mod_mul(t_2,  aux2, encrypted_payload.second, this->primeQ, ctx);
	BN_CTX_free(ctx);

	BN_clear_free(aux);
	BN_clear_free(aux2);

	return std::make_pair(t_1, t_2);
}

/****** WRAPPER FOR USE IN A LIBRARY ************/
// Wrapper for C
extern "C" char * ReEncryptUserTD(const char * user_trapdoor, const BIGNUM * primeQ,  const BIGNUM * serverKey)
{
    CCN *router = new CCN(512); //TODO: CHANGE
    //printf("The router was created\n");

    router->setPrimeQ(primeQ);
    router->setServerKey(serverKey);
    //printf("The prime and server key also: %s\n", user_trapdoor);

    char * res = router->FullCCNTD(user_trapdoor);

    //printf("After the function: %s\n", res);

    delete router;

    return res;
}

extern "C" char * libprotector_SimpleReEncryptUserTD(const char * user_trapdoor)
{
    //printf("SimpleReEncryptUserTD\n");
    BIGNUM * primeQ = BN_new();
    BIGNUM * serverKey = BN_new();

    char * res_getServerK = retrieveKeyFromServer("serverK:0");
    char * res_getPrimeQ = retrieveKeyFromServer("primeQ");

    BN_hex2bn(&primeQ, res_getPrimeQ);
    BN_hex2bn(&serverKey, res_getServerK);

    CCN *router = new CCN(512); // TODO: change
    //printf("The router was created\n");

    router->setPrimeQ(primeQ);
    router->setServerKey(serverKey);
    ///printf("The prime and server key also: %s\n", user_trapdoor);

    //printf("Execute FullCCNTD: %s\n", user_trapdoor);
    char * res = router->FullCCNTD(user_trapdoor);

    //printf("After the function: %s\n", res);

    delete router;

	int length = strlen(res);
    //int length = strnlen(res, 1 + ((SECURITY_KEYSIZE/4 + 1)* vec->size() * 2) + 1 + 1);
    char * res_final = (char *) calloc(sizeof(char), 5 + length +1);
    std::strcpy(res_final, "ccnx:");
    std::strcat(res_final, res);

    free(res);

    free(res_getServerK);
    free(res_getPrimeQ);

    BN_clear_free(primeQ);
    BN_clear_free(serverKey);

    return res_final;
}

extern "C" char * libprotector_ReEncryptUserContentNoNetwork(const char * user_encr_content, const unsigned int user_encr_len, const BIGNUM * serverKey, const BIGNUM * router_primeQ)
{
    CCN *router = new CCN(512); //TODO: CHANGE

    router->setPrimeQ(router_primeQ);
    router->setServerKey(serverKey);

	/* Convert char into BIGNUM */
	std::vector<std::string> keywords;
    boost::split(keywords, user_encr_content, boost::is_any_of("|"));

	BIGNUM *b1 = BN_new();
	BIGNUM *b2 = BN_new();

    //printf("Parts to be re-encrypted: %d\n", keywords.size());

	const char * c1 = keywords[0].c_str();
	const char * c2 = keywords[1].c_str();
	BN_hex2bn(&b1, c1);
	BN_hex2bn(&b2, c2);

	std::pair<BIGNUM *, BIGNUM *> content_trapdoor = std::make_pair(b1, b2);

	std::pair<BIGNUM *, BIGNUM *> p2 = router->ContentProviderReEnc(content_trapdoor);

	char * first_part_encr = BN_bn2hex(p2.first);
	char * second_part_encr = BN_bn2hex(p2.second);

	BN_clear_free(p2.first);
	BN_clear_free(p2.second);

	/* pack both in a string: append both parts with a pipe */
    char *reencrypted_content = (char*) calloc(sizeof(char), (SECURITY_KEYSIZE/4 + 1) * 2 + 1 + 1);
    strcpy(reencrypted_content, first_part_encr);
	strcat(reencrypted_content, "|");
    strcat(reencrypted_content, second_part_encr);

	/*Free all the generated value */
	BN_clear_free(b1);
	BN_clear_free(b2);

	delete router;

	OPENSSL_free(first_part_encr);
	OPENSSL_free(second_part_encr);

	return reencrypted_content;
}

extern "C" char * libprotector_ReEncryptUserContentWithKeys(const char * user_encr_content, const unsigned int user_encr_len, const char * res_getServerKey, const char * res_router_getPrimeQ)
{
    BIGNUM * router_primeQ = BN_new();
    BIGNUM * serverKey = BN_new();

    BN_hex2bn(&router_primeQ, res_router_getPrimeQ);
    BN_hex2bn(&serverKey, res_getServerKey);

    char * res = libprotector_ReEncryptUserContentNoNetwork(user_encr_content, user_encr_len, serverKey, router_primeQ);

	BN_clear_free(serverKey);
	BN_clear_free(router_primeQ);

	return res;
}

extern "C" char * libprotector_ReEncryptUserContent(const char * user_encr_content, const unsigned int user_encr_len)
{
	char * res_getServerK = retrieveKeyFromServer("serverK:0");
    char * res_router_getPrimeQ = retrieveKeyFromServer("primeQ");

    char * res = libprotector_ReEncryptUserContentWithKeys(user_encr_content, user_encr_len, res_getServerK, res_router_getPrimeQ);

	free(res_router_getPrimeQ);
	free(res_getServerK);

    return res;

    /**
    CCN *router = new CCN(512); //TODO: CHANGE

    router->setPrimeQ(router_primeQ);
    router->setServerKey(serverKey);

	// Convert char into BIGNUM
	std::vector<std::string> keywords;
    boost::split(keywords, user_encr_content, boost::is_any_of("|"));

	BIGNUM *b1 = BN_new();
	BIGNUM *b2 = BN_new();

    //printf("Parts to be re-encrypted: %d\n", keywords.size());

	const char * c1 = keywords[0].c_str();
	const char * c2 = keywords[1].c_str();
	BN_hex2bn(&b1, c1);
	BN_hex2bn(&b2, c2);

	std::pair<BIGNUM *, BIGNUM *> content_trapdoor = std::make_pair(b1, b2);

	std::pair<BIGNUM *, BIGNUM *> p2 = router->ContentProviderReEnc(content_trapdoor);

	char * first_part_encr = BN_bn2hex(p2.first);
	char * second_part_encr = BN_bn2hex(p2.second);

	// pack both in a string: append both parts with a pipe
    char *reencrypted_content = (char*) calloc(sizeof(char), (SECURITY_KEYSIZE/4 + 1) * 2 + 1 + 1);
    strcpy(reencrypted_content, first_part_encr);
	strcat(reencrypted_content, "|");
    strcat(reencrypted_content, second_part_encr);

	//Free all the generated value
	BN_clear_free(b1);
	BN_clear_free(b2);

	delete router;

	OPENSSL_free(first_part_encr);
	OPENSSL_free(second_part_encr);

	return reencrypted_content;**/
}

extern "C" char * libprotector_ReEncryptAndSplitUserContent(const char * user_encr_content, const unsigned int user_encr_len)
{
	BIGNUM * router_primeQ = BN_new();
    BIGNUM * serverKey = BN_new();

    char * res_getServerK = retrieveKeyFromServer("serverK:0");
    char * res_router_getPrimeQ = retrieveKeyFromServer("primeQ");

    BN_hex2bn(&router_primeQ, res_router_getPrimeQ);
    BN_hex2bn(&serverKey, res_getServerK);

    CCN *router = new CCN(512); //TODO: CHANGE

    router->setPrimeQ(router_primeQ);
    router->setServerKey(serverKey);

	/* Convert char into BIGNUM */
	std::vector<std::string> keywords;
    boost::split(keywords, user_encr_content, boost::is_any_of("|"));

    char *reencrypted_content = (char*) calloc(sizeof(char), (SECURITY_KEYSIZE/4 + 1) * keywords.size() + 1 + 1);

    for(unsigned int i=0; i<keywords.size(); i+=2)
    {
	    BIGNUM *b1 = BN_new();
	    BIGNUM *b2 = BN_new();

        //printf("Parts to be re-encrypted (libprotector_ReEncryptAndSplitUserContent): %u\n", keywords.size());

	    const char * c1 = keywords[i].c_str();
	    const char * c2 = keywords[i+1].c_str();
	    BN_hex2bn(&b1, c1);
	    BN_hex2bn(&b2, c2);

	    std::pair<BIGNUM *, BIGNUM *> content_trapdoor = std::make_pair(b1, b2);

	    std::pair<BIGNUM *, BIGNUM *> p2 = router->ContentProviderReEnc(content_trapdoor);

	    char * first_part_encr = BN_bn2hex(p2.first);
	    char * second_part_encr = BN_bn2hex(p2.second);

	    /* pack both in a string: append both parts with a pipe */

        strcat(reencrypted_content, first_part_encr);
	    strcat(reencrypted_content, "|");
        strcat(reencrypted_content, second_part_encr);
        // Dont add the character if its the last char
        if(i + 2 != keywords.size())
            strcat(reencrypted_content, "|");

	    /*Free all the generated value */
	    BN_clear_free(b1);
	    BN_clear_free(b2);

        OPENSSL_free(first_part_encr);
    	OPENSSL_free(second_part_encr);
    }

    printf("OUTPUT: %s\n", reencrypted_content);

	delete router;

	return reencrypted_content;
}

extern "C" char * libprotector_DecryptContentWithKeys(const char * reencrypted_content, const char * res_getServerK, const char * res_router_getPrimeQ)
{
    BIGNUM * router_primeQ = BN_new();
    BIGNUM * serverKey = BN_new();

    BN_hex2bn(&router_primeQ, res_router_getPrimeQ);
    BN_hex2bn(&serverKey, res_getServerK);

    CCN *router = new CCN(512); // TODO: CHANGE

    router->setPrimeQ(router_primeQ);
    router->setServerKey(serverKey);

	/* Convert char into BIGNUM */
	std::vector<std::string> keywords;
    boost::split(keywords, reencrypted_content, boost::is_any_of("|"));

	BIGNUM *b1 = BN_new();
	BIGNUM *b2 = BN_new();

	const char * c1 = keywords[0].c_str();
	const char * c2 = keywords[1].c_str();
	BN_hex2bn(&b1, c1);
	BN_hex2bn(&b2, c2);

	std::pair<BIGNUM *, BIGNUM *> content_trapdoor = std::make_pair(b1, b2);

	std::pair<BIGNUM *, BIGNUM *> p3 = router->CCNPreDec(content_trapdoor);

    char * first_part_encr = BN_bn2hex(p3.first);
	char * second_part_encr = BN_bn2hex(p3.second);

	/* pack both in a string: append both parts with a pipe */
    char *decrypted_content = (char*) calloc(sizeof(char), (SECURITY_KEYSIZE/4 + 1) * 2 + 1 + 1);
    strcpy(decrypted_content, first_part_encr);
	strcat(decrypted_content, "|");
    strcat(decrypted_content, second_part_encr);

	/*Free all the generated value */
	BN_clear_free(p3.first);
	BN_clear_free(p3.second);
	BN_clear_free(b1);
    BN_clear_free(b2);

	delete router;

	OPENSSL_free(first_part_encr);
	OPENSSL_free(second_part_encr);

	BN_clear_free(serverKey);
	BN_clear_free(router_primeQ);

	return decrypted_content;
}

extern "C" char * libprotector_DecryptContent(const char * reencrypted_content)
{
    char * res_getServerK = retrieveKeyFromServer("serverK:0");
    char * res_router_getPrimeQ = retrieveKeyFromServer("primeQ");

	char * decrypted_content = libprotector_DecryptContentWithKeys(reencrypted_content, res_getServerK, res_router_getPrimeQ);

	free(res_router_getPrimeQ);
	free(res_getServerK);

	return decrypted_content;
}

extern "C" char * libprotector_DecryptAndSplitContent(const char * reencrypted_content)
{
	BIGNUM * router_primeQ = BN_new();
    BIGNUM * serverKey = BN_new();

    char * res_getServerK = retrieveKeyFromServer("serverK:0");
    char * res_router_getPrimeQ = retrieveKeyFromServer("primeQ");

    BN_hex2bn(&router_primeQ, res_router_getPrimeQ);
    BN_hex2bn(&serverKey, res_getServerK);

    CCN *router = new CCN(512); // TODO: CHANGE

    router->setPrimeQ(router_primeQ);
    router->setServerKey(serverKey);

	/* Convert char into BIGNUM */
	std::vector<std::string> keywords;
    boost::split(keywords, reencrypted_content, boost::is_any_of("|"));

    //printf("Parts to be re-encrypted (libprotector_DecryptAndSplitContent): %u\n", keywords.size());

    char *decrypted_content = (char*) calloc(sizeof(char), (SECURITY_KEYSIZE/4 + 1) * keywords.size() + 1 + 1);
    decrypted_content[0] = '\0';

    for(unsigned int i = 0; i< keywords.size(); i+=2)
    {
	    BIGNUM *b1 = BN_new();
	    BIGNUM *b2 = BN_new();

	    const char * c1 = keywords[i].c_str();
	    const char * c2 = keywords[i+1].c_str();
	    BN_hex2bn(&b1, c1);
	    BN_hex2bn(&b2, c2);

	    std::pair<BIGNUM *, BIGNUM *> content_trapdoor = std::make_pair(b1, b2);

	    std::pair<BIGNUM *, BIGNUM *> p3 = router->CCNPreDec(content_trapdoor);

	    char * first_part_encr = BN_bn2hex(p3.first);
	    char * second_part_encr = BN_bn2hex(p3.second);

	    /* pack both in a string: append both parts with a pipe */
        strcat(decrypted_content, first_part_encr);
	    strcat(decrypted_content, "|");
        strcat(decrypted_content, second_part_encr);
        // Dont add the character if its the last char
        if(i + 2 != keywords.size())
            strcat(decrypted_content, "|");

	    /*Free all the generated value */
	    BN_clear_free(b1);
	    BN_clear_free(b2);

	    OPENSSL_free(first_part_encr);
	    OPENSSL_free(second_part_encr);
    }

    delete router;

    printf("OUTPUT: %s\n", decrypted_content);

	return decrypted_content;
}

/* libprotector C interface */
extern "C" CCN * libprotector_CCN_new(unsigned int keysize)
{
    return new CCN(keysize);
}

extern "C" void libprotector_CCN_setServerKey(CCN * c, const char * proxy_key)
{
    BIGNUM * aux = BN_new();
    BN_hex2bn(&aux, proxy_key);
    reinterpret_cast<CCN *>(c)->setServerKey(aux);
    BN_clear_free(aux);
};

extern "C" void libprotector_CCN_setNumberG(CCN * c, const char * number_g)
{
    BIGNUM * aux = BN_new();
    BN_hex2bn(&aux, number_g);
    reinterpret_cast<CCN *>(c)->setNumberG(aux);
    BN_clear_free(aux);
};

extern "C" void libprotector_CCN_setPrimeQ(CCN * c, const char * prime_q)
{
    BIGNUM * aux = BN_new();
    BN_hex2bn(&aux, prime_q);
    reinterpret_cast<CCN *>(c)->setPrimeQ(aux);
    BN_clear_free(aux);
};

/*extern "C" void libprotector_CCN_setSalt(CCN * c, const char * salt_key)
{
    BIGNUM * aux = BN_new();
    BN_hex2bn(&aux, salt_key);
    reinterpret_cast<CCN *>(c)->setSalt(aux);
    BN_clear_free(aux);
};*/

extern "C" void libprotector_CCN_CCNTD(CCN * c, const char * component1, const char * component2, char *& p1)
{
    unsigned int keysize = reinterpret_cast<CCN *>(c)->keysize;;

    BIGNUM * aux = BN_new();
    BN_hex2bn(&aux, component1);

    BIGNUM * aux2 = BN_new();
    BN_hex2bn(&aux2, component2);

    std::pair<BIGNUM *, BIGNUM *> my_pair = std::make_pair(aux, aux2);

    BIGNUM * p = reinterpret_cast<CCN *>(c)->CCNTD(my_pair);

    p1 = (char *) calloc(sizeof(char), ((keysize/4)) );
    char * aux_component = BN_bn2hex(p);
    memcpy((void *) (p1), (void *) aux_component, (keysize/4));

    free(aux_component);
}

extern "C" void libprotector_CCN_CCNContentTD(CCN * c, const char * component1, const char * component2, char *& p1, char *& p2)
{
    unsigned int keysize = reinterpret_cast<CCN *>(c)->keysize;

    BIGNUM * aux = BN_new();
    BN_hex2bn(&aux, component1);

    BIGNUM * aux2 = BN_new();
    BN_hex2bn(&aux2, component2);

    std::pair<BIGNUM *, BIGNUM *> encrypted_payload = std::make_pair(aux, aux2);

    std::pair<BIGNUM *, BIGNUM *> p = reinterpret_cast<CCN *>(c)->ContentProviderReEnc(encrypted_payload);

    p1 = (char *) calloc(sizeof(char), ((keysize/4)) );
    char * aux_component = BN_bn2hex(p.first);
    memcpy((void *) (p1), (void *) aux_component, (keysize/4));

    p2 = (char *) calloc(sizeof(char), ((keysize/4)) );
    char * aux_component2 = BN_bn2hex(p.second);
    memcpy((void *) (p2), (void *) aux_component2, (keysize/4));

    free(aux_component);
    free(aux_component2);
}

extern "C" void libprotector_CCN_CCNContentPreDec(CCN * c, const char * component1, const char * component2, char *& p1, char *& p2)
{
    unsigned int keysize = reinterpret_cast<CCN *>(c)->keysize;

    BIGNUM * aux = BN_new();
    BN_hex2bn(&aux, component1);

    BIGNUM * aux2 = BN_new();
    BN_hex2bn(&aux2, component2);

    std::pair<BIGNUM *, BIGNUM *> encrypted_payload = std::make_pair(aux, aux2);

    std::pair<BIGNUM *, BIGNUM *> p = reinterpret_cast<CCN *>(c)->CCNPreDec(encrypted_payload);

    p1 = (char *) calloc(sizeof(char), ((keysize/4)) );
    char * aux_component = BN_bn2hex(p.first);
    memcpy((void *) (p1), (void *) aux_component, (keysize/4));

    p2 = (char *) calloc(sizeof(char), ((keysize/4)) );
    char * aux_component2 = BN_bn2hex(p.second);
    memcpy((void *) (p2), (void *) aux_component2, (keysize/4));

    free(aux_component);
    free(aux_component2);
}


/* boost random/detail/polynomial.hpp header file
 *
 * Copyright Steven Watanabe 2014
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 */

#ifndef BOOST_RANDOM_DETAIL_POLYNOMIAL_HPP
#define BOOST_RANDOM_DETAIL_POLYNOMIAL_HPP

#include <cstddef>
#include <limits>
#include <vector>
#include <algorithm>
#include <boost/assert.hpp>
#include <boost/cstdint.hpp>

namespace boost {
namespace random {
namespace detail {

class polynomial_ops {
public:
    typedef unsigned long digit_t;

    static void add(std::size_t size, const digit_t * lhs,
                       const digit_t * rhs, digit_t * output)
    {
        for(std::size_t i = 0; i < size; ++i) {
            output[i] = lhs[i] ^ rhs[i];
        }
    }

    static void add_shifted_inplace(std::size_t size, const digit_t * lhs,
                                    digit_t * output, std::size_t shift)
    {
        if(shift == 0) {
            add(size, lhs, output, output);
            return;
        }
        std::size_t bits = std::numeric_limits<digit_t>::digits;
        digit_t prev = 0;
        for(std::size_t i = 0; i < size; ++i) {
            digit_t tmp = lhs[i];
            output[i] ^= (tmp << shift) | (prev >> (bits-shift));
            prev = tmp;
        }
        output[size] ^= (prev >> (bits-shift));
    }

    static void multiply_simple(std::size_t size, const digit_t * lhs,
                                   const digit_t * rhs, digit_t * output)
    {
        std::size_t bits = std::numeric_limits<digit_t>::digits;
        for(std::size_t i = 0; i < 2*size; ++i) {
            output[i] = 0;
        }
        for(std::size_t i = 0; i < size; ++i) {
            for(std::size_t j = 0; j < bits; ++j) {
                if((lhs[i] & (digit_t(1) << j)) != 0) {
                    add_shifted_inplace(size, rhs, output + i, j);
                }
            }
        }
    }

    // memory requirements: (size - cutoff) * 4 + next_smaller
    static void multiply_karatsuba(std::size_t size,
                               const digit_t * lhs, const digit_t * rhs,
                               digit_t * output)
    {
        if(size < 64) {
            multiply_simple(size, lhs, rhs, output);
            return;
        }
        // split in half
        std::size_t cutoff = size/2;
        multiply_karatsuba(cutoff, lhs, rhs, output);
        multiply_karatsuba(size - cutoff, lhs + cutoff, rhs + cutoff,
                              output + cutoff*2);
        std::vector<digit_t> local1(size - cutoff);
        std::vector<digit_t> local2(size - cutoff);
        // combine the digits for the inner multiply
        add(cutoff, lhs, lhs + cutoff, &local1[0]);
        if(size & 1) local1[cutoff] = lhs[size - 1];
        add(cutoff, rhs + cutoff, rhs, &local2[0]);
        if(size & 1) local2[cutoff] = rhs[size - 1];
        std::vector<digit_t> local3((size - cutoff) * 2);
        multiply_karatsuba(size - cutoff, &local1[0], &local2[0], &local3[0]);
        add(cutoff * 2, output, &local3[0], &local3[0]);
        add((size - cutoff) * 2, output + cutoff*2, &local3[0], &local3[0]);
        // Finally, add the inner result
        add((size - cutoff) * 2, output + cutoff, &local3[0], output + cutoff);
    }

    static void multiply_add_karatsuba(std::size_t size,
                                       const digit_t * lhs, const digit_t * rhs,
                                       digit_t * output)
    {
        std::vector<digit_t> buf(size * 2);
        multiply_karatsuba(size, lhs, rhs, &buf[0]);
        add(size * 2, &buf[0], output, output);
    }

    static void multiply(const digit_t * lhs, std::size_t lhs_size,
                         const digit_t * rhs, std::size_t rhs_size,
                         digit_t * output)
    {
        std::fill_n(output, lhs_size + rhs_size, digit_t(0));
        multiply_add(lhs, lhs_size, rhs, rhs_size, output);
    }

    static void multiply_add(const digit_t * lhs, std::size_t lhs_size,
                             const digit_t * rhs, std::size_t rhs_size,
                             digit_t * output)
    {
        // split into pieces that can be passed to
        // karatsuba multiply.
        while(lhs_size != 0) {
            if(lhs_size < rhs_size) {
                std::swap(lhs, rhs);
                std::swap(lhs_size, rhs_size);
            }

            multiply_add_karatsuba(rhs_size, lhs, rhs, output);

            lhs += rhs_size;
            lhs_size -= rhs_size;
            output += rhs_size;
        }
    }

    static void copy_bits(const digit_t * x, std::size_t low, std::size_t high,
                   digit_t * out)
    {
        const std::size_t bits = std::numeric_limits<digit_t>::digits;
        std::size_t offset = low/bits;
        x += offset;
        low -= offset*bits;
        high -= offset*bits;
        std::size_t n = (high-low)/bits;
        if(low == 0) {
            for(std::size_t i = 0; i < n; ++i) {
                out[i] = x[i];
            }
        } else {
            for(std::size_t i = 0; i < n; ++i) {
                out[i] = (x[i] >> low) | (x[i+1] << (bits-low));
            }
        }
        if((high-low)%bits) {
            digit_t low_mask = (digit_t(1) << ((high-low)%bits)) - 1;
            digit_t result = (x[n] >> low);
            if(low != 0 && (n+1)*bits < high) {
                result |= (x[n+1] << (bits-low));
            }
            out[n] = (result & low_mask);
        }
    }

    static void shift_left(digit_t * val, std::size_t size, std::size_t shift)
    {
        const std::size_t bits = std::numeric_limits<digit_t>::digits;
        BOOST_ASSERT(shift > 0);
        BOOST_ASSERT(shift < bits);
        digit_t prev = 0;
        for(std::size_t i = 0; i < size; ++i) {
            digit_t tmp = val[i];
            val[i] = (prev >> (bits - shift)) | (val[i] << shift);
            prev = tmp;
        }
    }

    static digit_t sqr(digit_t val) {
        const std::size_t bits = std::numeric_limits<digit_t>::digits;
        digit_t mask = (digit_t(1) << bits/2) - 1;
        for(std::size_t i = bits; i > 1; i /= 2) {
            val = ((val & ~mask) << i/2) | (val & mask);
            mask = mask & (mask >> i/4);
            mask = mask | (mask << i/2);
        }
        return val;
    }

    static void sqr(digit_t * val, std::size_t size)
    {
        const std::size_t bits = std::numeric_limits<digit_t>::digits;
        digit_t mask = (digit_t(1) << bits/2) - 1;
        for(std::size_t i = 0; i < size; ++i) {
            digit_t x = val[size - i - 1];
            val[(size - i - 1) * 2] = sqr(x & mask);
            val[(size - i - 1) * 2 + 1] = sqr(x >> bits/2);
        }
    }

    // optimized for the case when the modulus has few bits set.
    struct sparse_mod {
        sparse_mod(const digit_t * divisor, std::size_t divisor_bits)
        {
            const std::size_t bits = std::numeric_limits<digit_t>::digits;
            _remainder_bits = divisor_bits - 1;
            for(std::size_t i = 0; i < divisor_bits; ++i) {
                if(divisor[i/bits] & (digit_t(1) << i%bits)) {
                    _bit_indices.push_back(i);
                }
            }
            BOOST_ASSERT(_bit_indices.back() == divisor_bits - 1);
            _bit_indices.pop_back();
            if(_bit_indices.empty()) {
                _block_bits = divisor_bits;
                _lower_bits = 0;
            } else {
                _block_bits = divisor_bits - _bit_indices.back() - 1;
                _lower_bits = _bit_indices.back() + 1;
            }

            _partial_quotient.resize((_block_bits + bits - 1)/bits);
        }
        void operator()(digit_t * dividend, std::size_t dividend_bits)
        {
            const std::size_t bits = std::numeric_limits<digit_t>::digits;
            while(dividend_bits > _remainder_bits) {
                std::size_t block_start = (std::max)(dividend_bits - _block_bits, _remainder_bits);
                std::size_t block_size = (dividend_bits - block_start + bits - 1) / bits;
                copy_bits(dividend, block_start, dividend_bits, &_partial_quotient[0]);
                for(std::size_t i = 0; i < _bit_indices.size(); ++i) {
                    std::size_t pos = _bit_indices[i] + block_start - _remainder_bits;
                    add_shifted_inplace(block_size, &_partial_quotient[0], dividend + pos/bits, pos%bits);
                }
                add_shifted_inplace(block_size, &_partial_quotient[0], dividend + block_start/bits, block_start%bits);
                dividend_bits = block_start;
            }
        }
        std::vector<digit_t> _partial_quotient;
        std::size_t _remainder_bits;
        std::size_t _block_bits;
        std::size_t _lower_bits;
        std::vector<std::size_t> _bit_indices;
    };

    // base should have the same number of bits as mod
    // base, and mod should both be able to hold a power
    // of 2 >= mod_bits.  out needs to be twice as large.
    static void mod_pow_x(boost::uintmax_t exponent, const digit_t * mod, std::size_t mod_bits, digit_t * out)
    {
        const std::size_t bits = std::numeric_limits<digit_t>::digits;
        const std::size_t n = (mod_bits + bits - 1) / bits;
        const std::size_t highbit = mod_bits - 1;
        if(exponent == 0) {
            out[0] = 1;
            std::fill_n(out + 1, n - 1, digit_t(0));
            return;
        }
        boost::uintmax_t i = std::numeric_limits<boost::uintmax_t>::digits - 1;
        while(((boost::uintmax_t(1) << i) & exponent) == 0) {
            --i;
        }
        out[0] = 2;
        std::fill_n(out + 1, n - 1, digit_t(0));
        sparse_mod m(mod, mod_bits);
        while(i--) {
            sqr(out, n);
            m(out, 2 * mod_bits - 1);
            if((boost::uintmax_t(1) << i) & exponent) {
                shift_left(out, n, 1);
                if(out[highbit / bits] & (digit_t(1) << highbit%bits))
                    add(n, out, mod, out);
            }
        }
    }
};

class polynomial
{
    typedef polynomial_ops::digit_t digit_t;
public:
    polynomial() : _size(0) {}
    class reference {
    public:
        reference(digit_t &value, int idx)
            : _value(value), _idx(idx) {}
        operator bool() const { return (_value & (digit_t(1) << _idx)) != 0; }
        reference& operator=(bool b)
        {
            if(b) {
                _value |= (digit_t(1) << _idx);
            } else {
                _value &= ~(digit_t(1) << _idx);
            }
            return *this;
        }
        reference &operator^=(bool b)
        {
            _value ^= (digit_t(b) << _idx);
            return *this;
        }

        reference &operator=(const reference &other)
        {
            return *this = static_cast<bool>(other);
        }
	reference(const reference&) = default;
    private:
        digit_t &_value;
        int _idx;
    };
    reference operator[](std::size_t i)
    {
        static const std::size_t bits = std::numeric_limits<digit_t>::digits;
        ensure_bit(i);
        return reference(_storage[i/bits], i%bits);
    }
    bool operator[](std::size_t i) const
    {
        static const std::size_t bits = std::numeric_limits<digit_t>::digits;
        if(i < size())
            return (_storage[i/bits] & (digit_t(1) << (i%bits))) != 0;
        else
            return false;
    }
    std::size_t size() const
    {
        return _size;
    }
    void resize(std::size_t n)
    {
        static const std::size_t bits = std::numeric_limits<digit_t>::digits;
        _storage.resize((n + bits - 1)/bits);
        // clear the high order bits in case we're shrinking.
        if(n%bits) {
            _storage.back() &= ((digit_t(1) << (n%bits)) - 1);
        }
        _size = n;
    }
    friend polynomial operator*(const polynomial &lhs, const polynomial &rhs);
    friend polynomial mod_pow_x(boost::uintmax_t exponent, polynomial mod);
private:
    std::vector<polynomial_ops::digit_t> _storage;
    std::size_t _size;
    void ensure_bit(std::size_t i)
    {
        if(i >= size()) {
            resize(i + 1);
        }
    }
    void normalize()
    {
        while(size() && (*this)[size() - 1] == 0)
            resize(size() - 1);
    }
};

inline polynomial operator*(const polynomial &lhs, const polynomial &rhs)
{
    polynomial result;
    result._storage.resize(lhs._storage.size() + rhs._storage.size());
    polynomial_ops::multiply(&lhs._storage[0], lhs._storage.size(),
                             &rhs._storage[0], rhs._storage.size(),
                             &result._storage[0]);
    result._size = lhs._size + rhs._size;
    return result;
}

inline polynomial mod_pow_x(boost::uintmax_t exponent, polynomial mod)
{
    polynomial result;
    mod.normalize();
    std::size_t mod_size = mod.size();
    result._storage.resize(mod._storage.size() * 2);
    result._size = mod.size() * 2;
    polynomial_ops::mod_pow_x(exponent, &mod._storage[0], mod_size, &result._storage[0]);
    result.resize(mod.size() - 1);
    return result;
}

}
}
}

#endif // BOOST_RANDOM_DETAIL_POLYNOMIAL_HPP


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2013 Nikhar Agrawal
//  Copyright 2013 Christopher Kormanyos
//  Copyright 2013 John Maddock
//  Copyright 2013 Paul Bristow
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef _BOOST_BERNOULLI_B2N_2013_05_30_HPP_
#define _BOOST_BERNOULLI_B2N_2013_05_30_HPP_

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/detail/unchecked_bernoulli.hpp>
#include <boost/math/special_functions/detail/bernoulli_details.hpp>

namespace boost { namespace math {

namespace detail {

template <class T, class OutputIterator, class Policy, int N>
OutputIterator bernoulli_number_imp(OutputIterator out, std::size_t start, std::size_t n, const Policy& pol, const mpl::int_<N>& tag)
{
   for(std::size_t i = start; (i <= max_bernoulli_b2n<T>::value) && (i < start + n); ++i)
   {
      *out = unchecked_bernoulli_imp<T>(i, tag);
      ++out;
   }

   for(std::size_t i = (std::max)(static_cast<std::size_t>(max_bernoulli_b2n<T>::value + 1), start); i < start + n; ++i)
   {
      // We must overflow:
      *out = (i & 1 ? 1 : -1) * policies::raise_overflow_error<T>("boost::math::bernoulli_b2n<%1%>(n)", 0, T(i), pol);
      ++out;
   }
   return out;
}

template <class T, class OutputIterator, class Policy>
OutputIterator bernoulli_number_imp(OutputIterator out, std::size_t start, std::size_t n, const Policy& pol, const mpl::int_<0>& tag)
{
   for(std::size_t i = start; (i <= max_bernoulli_b2n<T>::value) && (i < start + n); ++i)
   {
      *out = unchecked_bernoulli_imp<T>(i, tag);
      ++out;
   }
   //
   // Short circuit return so we don't grab the mutex below unless we have to:
   //
   if(start + n <= max_bernoulli_b2n<T>::value)
      return out;

   return get_bernoulli_numbers_cache<T, Policy>().copy_bernoulli_numbers(out, start, n, pol);
}

} // namespace detail

template <class T, class Policy>
inline T bernoulli_b2n(const int i, const Policy &pol)
{
   typedef mpl::int_<detail::bernoulli_imp_variant<T>::value> tag_type;
   if(i < 0)
      return policies::raise_domain_error<T>("boost::math::bernoulli_b2n<%1%>", "Index should be >= 0 but got %1%", T(i), pol);

   T result = static_cast<T>(0); // The = 0 is just to silence compiler warnings :-(
   boost::math::detail::bernoulli_number_imp<T>(&result, static_cast<std::size_t>(i), 1u, pol, tag_type());
   return result;
}

template <class T>
inline T bernoulli_b2n(const int i)
{
   return boost::math::bernoulli_b2n<T>(i, policies::policy<>());
}

template <class T, class OutputIterator, class Policy>
inline OutputIterator bernoulli_b2n(const int start_index,
                                    const unsigned number_of_bernoullis_b2n,
                                    OutputIterator out_it,
                                    const Policy& pol)
{
   typedef mpl::int_<detail::bernoulli_imp_variant<T>::value> tag_type;
   if(start_index < 0)
   {
      *out_it = policies::raise_domain_error<T>("boost::math::bernoulli_b2n<%1%>", "Index should be >= 0 but got %1%", T(start_index), pol);
      return ++out_it;
   }

   return boost::math::detail::bernoulli_number_imp<T>(out_it, start_index, number_of_bernoullis_b2n, pol, tag_type());
}

template <class T, class OutputIterator>
inline OutputIterator bernoulli_b2n(const int start_index,
                                    const unsigned number_of_bernoullis_b2n,
                                    OutputIterator out_it)
{
   return boost::math::bernoulli_b2n<T, OutputIterator>(start_index, number_of_bernoullis_b2n, out_it, policies::policy<>());
}

template <class T, class Policy>
inline T tangent_t2n(const int i, const Policy &pol)
{
   if(i < 0)
      return policies::raise_domain_error<T>("boost::math::tangent_t2n<%1%>", "Index should be >= 0 but got %1%", T(i), pol);

   T result;
   boost::math::detail::get_bernoulli_numbers_cache<T, Policy>().copy_tangent_numbers(&result, i, 1, pol);
   return result;
}

template <class T>
inline T tangent_t2n(const int i)
{
   return boost::math::tangent_t2n<T>(i, policies::policy<>());
}

template <class T, class OutputIterator, class Policy>
inline OutputIterator tangent_t2n(const int start_index,
                                    const unsigned number_of_tangent_t2n,
                                    OutputIterator out_it,
                                    const Policy& pol)
{
   if(start_index < 0)
   {
      *out_it = policies::raise_domain_error<T>("boost::math::tangent_t2n<%1%>", "Index should be >= 0 but got %1%", T(start_index), pol);
      return ++out_it;
   }

   return boost::math::detail::get_bernoulli_numbers_cache<T, Policy>().copy_tangent_numbers(out_it, start_index, number_of_tangent_t2n, pol);
}

template <class T, class OutputIterator>
inline OutputIterator tangent_t2n(const int start_index,
                                    const unsigned number_of_tangent_t2n,
                                    OutputIterator out_it)
{
   return boost::math::tangent_t2n<T, OutputIterator>(start_index, number_of_tangent_t2n, out_it, policies::policy<>());
}

} } // namespace boost::math

#endif // _BOOST_BERNOULLI_B2N_2013_05_30_HPP_


#pragma once

#include <vector>

#include <Eigen/Core>

#include <ipc/collision_constraint.hpp>

namespace ipc {

/// @brief Construct a set of point-plane distance constraints used to compute
/// the barrier potential.
///
/// @note The given pv_constraints will be cleared.
///
/// @param[in]  V  Vertex positions as rows of a matrix.
/// @param[in]  plane_origins  Plane origins as rows of a matrix.
/// @param[in]  plane_normals  Plane normals as rows of a matrix.
/// @param[in]  dhat  The activation distance of the barrier.
/// @param[out] pv_constraints  The constructed set of constraints.
/// @param[in]  dmin  Minimum distance.
/// @param[in] can_collide
///     A function that takes a vertex ID (row numbers in V) and a plane ID (row
///     number in plane_origins) then returns true if the vertex can collide
///     with the plane. By default all points can collide with all planes.
void construct_point_plane_constraint_set(
    const Eigen::MatrixXd& V,
    const Eigen::MatrixXd& plane_origins,
    const Eigen::MatrixXd& plane_normals,
    const double dhat,
    std::vector<PlaneVertexConstraint>& pv_constraints,
    const double dmin = 0,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

///////////////////////////////////////////////////////////////////////////////
// Collision detection

/// @brief Determine if the step is collision free.
///
/// @note Assumes the trajectory is linear.
/// @note V can either be the surface vertices or the entire mesh vertices.
///
/// @param[in] V0 Vertex positions at start as rows of a matrix.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] plane_origins  Plane origins as rows of a matrix.
/// @param[in] plane_normals  Plane normals as rows of a matrix.
/// @param[in] can_collide
///     A function that takes a vertex ID (row numbers in V) and a plane ID (row
///     number in plane_origins) then returns true if the vertex can collide
///     with the plane. By default all points can collide with all planes.
/// @returns True if <b>any</b> collisions occur.
bool is_step_point_plane_collision_free(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXd& plane_origins,
    const Eigen::MatrixXd& plane_normals,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

/// @brief Computes a maximal step size that is collision free.
///
/// @note Assumes V0 is intersection free.
/// @note Assumes the trajectory is linear.
/// @note A value of 1.0 if a full step and 0.0 is no step.
///
/// @param[in] V0 Vertex positions at start as rows of a matrix.
/// @param[in] V1 Vertex positions at end as rows of a matrix.
/// @param[in] plane_origins  Plane origins as rows of a matrix.
/// @param[in] plane_normals  Plane normals as rows of a matrix.
/// @param[in] can_collide
///     A function that takes a vertex ID (row numbers in V) and a plane ID (row
///     number in plane_origins) then returns true if the vertex can collide
///     with the plane. By default all points can collide with all planes.
/// @returns A step-size \f$\in [0, 1]\f$ that is collision free.
bool compute_point_plane_collision_free_stepsize(
    const Eigen::MatrixXd& V0,
    const Eigen::MatrixXd& V1,
    const Eigen::MatrixXd& plane_origins,
    const Eigen::MatrixXd& plane_normals,
    const std::function<bool(size_t, size_t)>& can_collide =
        [](size_t, size_t) { return true; });

} // namespace ipc


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2011 Gael Guennebaud <g.gael@free.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "sparse_solver.h"
#include <Eigen/IterativeLinearSolvers>

template<typename T> void test_lscg_T()
{
  LeastSquaresConjugateGradient<SparseMatrix<T> > lscg_colmajor_diag;
  LeastSquaresConjugateGradient<SparseMatrix<T>, IdentityPreconditioner> lscg_colmajor_I;
  LeastSquaresConjugateGradient<SparseMatrix<T,RowMajor> > lscg_rowmajor_diag;
  LeastSquaresConjugateGradient<SparseMatrix<T,RowMajor>, IdentityPreconditioner> lscg_rowmajor_I;

  CALL_SUBTEST( check_sparse_square_solving(lscg_colmajor_diag)  );
  CALL_SUBTEST( check_sparse_square_solving(lscg_colmajor_I)     );

  CALL_SUBTEST( check_sparse_leastsquare_solving(lscg_colmajor_diag)  );
  CALL_SUBTEST( check_sparse_leastsquare_solving(lscg_colmajor_I)     );

  CALL_SUBTEST( check_sparse_square_solving(lscg_rowmajor_diag)  );
  CALL_SUBTEST( check_sparse_square_solving(lscg_rowmajor_I)     );

  CALL_SUBTEST( check_sparse_leastsquare_solving(lscg_rowmajor_diag)  );
  CALL_SUBTEST( check_sparse_leastsquare_solving(lscg_rowmajor_I)     );
}

EIGEN_DECLARE_TEST(lscg)
{
  CALL_SUBTEST_1(test_lscg_T<double>());
  CALL_SUBTEST_2(test_lscg_T<std::complex<double> >());
}