#include <ros/ros.h>
#include "ros_utils.hpp"
#include "kinematics.hpp"
#include "estimators.hpp"
#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>
#include <Eigen/Dense>
#include <unistd.h>
#include <math.h>

#include <gtsam/config.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/Vector.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Key.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/inference/LabeledSymbol.h>

#include <gtsam_node/ArticulatedAngles.h>
#include <gtsam_node/CanData.h>
#include "std_msgs/String.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/Twist.h"
#include "ackermann_msgs/AckermannDrive.h"

#include <visualization_msgs/Marker.h>
#include <cmath>

using namespace std;
using namespace gtsam;

class CircleFactor: public NoiseModelFactor1<Vector5> {

  Vector3 circle_;

 public:

  typedef boost::shared_ptr<CircleFactor> shared_ptr;

  CircleFactor(Key j, Vector3 circle, const SharedNoiseModel& model):
    NoiseModelFactor1<Vector5>(model, j), circle_(circle) {}

  virtual ~CircleFactor() {}

  Vector evaluateError(const Vector5& po,
                       boost::optional<Matrix&> H = boost::none) const override {

    /*
    double err = 1/(pow(po[0]-circle_[0],2)+pow(po[1]-circle_[1],2)-pow(circle_[2],2));

    double ctrval = 0.05;
    double jX = (ctrval*(2*po[0] - 2*circle_[0]))/pow(pow(po[0]-circle_[0],2)+pow(po[1]-circle_[1],2)-pow(circle_[2],2),2);
    double jY = (ctrval*(2*po[1] - 2*circle_[1]))/pow(pow(po[0]-circle_[0],2)+pow(po[1]-circle_[1],2)-pow(circle_[2],2),2);

    if (H) (*H) = (Matrix(1, 5) << jX, jY, 0.0, 0.0, 0.0).finished();
    return (Vector(1) << err*ctrval).finished();
    /**/
    //
    double ctrval = 0.05;
    double err = (1/(pow(2,(ctrval*((pow(po[0]-circle_[0],2)+pow(po[1]-circle_[1],2)-pow(circle_[2],2))/pow(circle_[2],2))))))-1;

    double jX = (-0.6931471*ctrval*(po[0]-circle_[0])*pow(2,(1-(ctrval*((pow(po[0]-circle_[0],2)+pow(po[1]-circle_[1],2)-pow(circle_[2],2))/pow(circle_[2],2)) ))))/pow(circle_[2],2);
    double jY = (-0.6931471*ctrval*(po[1]-circle_[1])*pow(2,(1-(ctrval*((pow(po[0]-circle_[0],2)+pow(po[1]-circle_[1],2)-pow(circle_[2],2))/pow(circle_[2],2)) ))))/pow(circle_[2],2);

    if (H) (*H) = (Matrix(1, 5) << jX, jY, 0.0, 0.0, 0.0).finished();
    //ROS_INFO_STREAM("errY: " << err << "");
    //ROS_INFO_STREAM("errYJ: " << jY << "");
    return (Vector(1) << err).finished();
    /**/
  }

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new CircleFactor(*this))); }

};  // CircleFactor

class PolyPointFactor: public NoiseModelFactor2<Vector8,Vector5> {

  private:
    using This = PolyPointFactor;
    using Base = gtsam::NoiseModelFactor2<Vector8, Vector5>;

    Vector8 mpoly_;
    Vector5 mpoint_;
    gtsam::Key j_;
  public:

    typedef boost::shared_ptr<PolyPointFactor> shared_ptr;

    PolyPointFactor(Key i, Key j, Vector8 mpoly, Vector5 mpoint, const SharedNoiseModel& model)
        : Base(model, i, j), mpoly_(mpoly), mpoint_(mpoint),j_(j) {}

    virtual ~PolyPointFactor()
    {}

    double getPoly(const double& x, const Vector8& p8) const
    {
        double y;
        y = (p8[0]*pow(x,7))+(p8[1]*pow(x,6))+(p8[2]*pow(x,5))+(p8[3]*pow(x,4))+(p8[4]*pow(x,3))+(p8[5]*pow(x,2))+(p8[6]*x)+p8[7];

        return y; // order p0 x^7 -> p7 x^1
    }

    double diffPoly(const double& x, const Vector8& p8) const
    {
        double y;
        y = (7*p8[0]*pow(x,6))+(6*p8[1]*pow(x,5))+(5*p8[2]*pow(x,4))+(4*p8[3]*pow(x,3))+(3*p8[4]*pow(x,2))+(2*p8[5]*x)+p8[6];

        return y; // order p0 x^7 -> p7 x^1
    }

    double diff2Poly(const double& x, const Vector8& p8) const
    {
        double y;
        y = (42*p8[0]*pow(x,5))+(30*p8[1]*pow(x,4))+(20*p8[2]*pow(x,3))+(12*p8[3]*pow(x,2))+(6*p8[4]*x)+(2*p8[5]);

        return y; // order p0 x^7 -> p7 x^1
    }

     Vector evaluateError(const Vector8& p8, const Vector5& p5,
        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override
    {
        //PrintKey(j_);
        gtsam::Vector err(13);
        err = Vector::Zero(13);
        err[9] =  p5[1] - getPoly(p5[0],p8);
        //ROS_INFO_STREAM("errY: " << err[9] << "");

        double errYx = diffPoly(p5[0],p8);
        double errY2x = diff2Poly(p5[0],p8);
        double curve = abs(errY2x)/pow((pow(errYx,2)+1),1.5);
        double ctrVal = 1;
        double curveMax = 0.1;
        //err[11] =  1/(pow(2,ctrVal*(curveMax - curve)));
        err[9] = err[9] + 1/(pow(2,ctrVal*(curveMax - curve)));
        ROS_INFO_STREAM("Curve: " << curve << "");
        //ROS_INFO_STREAM("errCurve: " << err[11] << "");

        //curve = (abs(42*p0*x^5+30*p1*x^4+20*p2*x^3+12*p3*x^2+6*p4*x+2*p5)/(1+(7*p0*x^6+6*p1*x^5+5p2*x^4+4*p3*x^3+3*p4*x^2+2*p5*x+p6)^2)^(3/2))

        //errCurve = 1/2^(a*(m-(((abs(42*p0*x^5+30*p1*x^4+20*p2*x^3+12*p3*x^2+6*p4*x+2*p5)/(1+(7*p0*x^6+6*p1*x^5+5p2*x^4+4*p3*x^3+3*p4*x^2+2*p5*x+p6)^2)^(3/2))))

/**/
        gtsam::Vector crvJp(7);
        crvJp = Vector::Zero(7);
        double lnCtr = -0.6931471*ctrVal;
        double deno = (pow(2,ctrVal*(curveMax - curve)));
        double eqp1 = (errYx* abs(errY2x))/(pow( (pow(errYx,2)+1),2.5));
        double eqp2 = pow( (pow(errYx,2)+1) ,1.5) *abs(errY2x);

        crvJp[0] = (lnCtr*(( (21* pow(p5[0],6)* eqp1)  - ((42*pow(p5[0],5) *  errY2x)  / eqp2))))   / deno;

        crvJp[1] = (lnCtr*(( (18* pow(p5[0],5)* eqp1)  - ((30*pow(p5[0],4) *  errY2x)  / eqp2))))   / deno;

        crvJp[2] = (lnCtr*(( (15* pow(p5[0],4)* eqp1)  - ((20*pow(p5[0],3) *  errY2x)  / eqp2))))   / deno;

        crvJp[3] = (lnCtr*(( (12* pow(p5[0],3)* eqp1)  - ((12*pow(p5[0],2) *  errY2x)  / eqp2))))   / deno;

        crvJp[4] = (lnCtr*(( (9*  pow(p5[0],2)* eqp1)  - ((6*      p5[0]   *  errY2x)  / eqp2))))   / deno;

        crvJp[5] = (lnCtr*(( (6*      p5[0]*    eqp1)  - ((2*                 errY2x)  / eqp2))))   / deno;

        crvJp[6] = (lnCtr*   3*     errYx* abs(errY2x)) / (pow( (pow(errYx,2)+1),2.5)*deno);
        /*
        if (isnan(crvJp[0])) {crvJp[0] = 0.0;}
        if (isnan(crvJp[1])) {crvJp[1] = 0.0;}
        if (isnan(crvJp[2])) {crvJp[2] = 0.0;}
        if (isnan(crvJp[3])) {crvJp[3] = 0.0;}
        if (isnan(crvJp[4])) {crvJp[4] = 0.0;}
        if (isnan(crvJp[5])) {crvJp[5] = 0.0;}
        if (isnan(crvJp[6])) {crvJp[6] = 0.0;}

        /*
        ROS_INFO_STREAM("crvJp6: " << crvJp[6] << "");
        ROS_INFO_STREAM("crvJp5: " << crvJp[5] << "");
        ROS_INFO_STREAM("crvJp4: " << crvJp[4] << "");
        ROS_INFO_STREAM("crvJp3: " << crvJp[3] << "");
        ROS_INFO_STREAM("crvJp2: " << crvJp[2] << "");
        ROS_INFO_STREAM("crvJp1: " << crvJp[1] << "");
        ROS_INFO_STREAM("crvJp0: " << crvJp[0] << "");
        */

        if (H1) (*H1) = (Matrix(13, 8) <<   0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            -pow(p5[0],7)+crvJp[0], -pow(p5[0],6)+crvJp[1], -pow(p5[0],5)+crvJp[2], -pow(p5[0],4)+crvJp[3], -pow(p5[0],3)+crvJp[4], -pow(p5[0],2)+crvJp[5], -p5[0]+crvJp[6], -1.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished();
          //crvJp0, crvJp1, crvJp2, crvJp3, crvJp4, crvJp5, crvJp6, 0.0,
          //crvJp[0], crvJp[1], crvJp[2], crvJp[3], crvJp[4], crvJp[5], crvJp[6], 0.0,
        //(-1)*errYx
        //double kdot = (0.6931471*ctrVal)/(pow(2,ctrVal*( curveMax - curve)));
        //-pow(p5[0],7), -pow(p5[0],6), -pow(p5[0],5), -pow(p5[0],4), -pow(p5[0],3), -pow(p5[0],2), -p5[0], -1.0,
        if (H2) (*H2) = (Matrix(13, 5) <<   0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 1.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 1.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 1.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 1.0).finished();
        return err;
    }

    gtsam::NonlinearFactor::shared_ptr clone() const override {
      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new PolyPointFactor(*this))); }

};  // PolyPointFactor

class ppFactor: public NoiseModelFactor2<Vector5,Vector5> {

  private:
    using This = ppFactor;
    using Base = gtsam::NoiseModelFactor2<Vector5, Vector5>;
  public:
    typedef boost::shared_ptr<ppFactor> shared_ptr;
    ppFactor(Key i, Key j, Vector5 pointA, Vector5 pointB, const SharedNoiseModel& model)
        : Base(model, i, j) {}

    virtual ~ppFactor()
    {}
     Vector evaluateError(const Vector5& pA, const Vector5& pB,
        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override
    {

        gtsam::Vector err(10);
        err = Vector::Zero(10);
        double dis = 0.1;
        err[0] =  pA[0] - pB[0] + dis ;
        err[5] =  pB[0] - pA[0] - dis ;

        if (H1) (*H1) = (Matrix(10, 5) <<   1.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            -1.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0).finished();

        if (H2) (*H2) = (Matrix(10, 5) <<   -1.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            1.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0,
                                            0.0, 0.0, 0.0, 0.0, 0.0).finished();
        return err;
    }

    gtsam::NonlinearFactor::shared_ptr clone() const override {
      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new ppFactor(*this))); }

};  // point<->point Factor

double getPolyY(const double& x, const Vector8& p8)  //only for illustration purposes
{
    double y;
    y = (p8[0]*pow(x,7))+(p8[1]*pow(x,6))+(p8[2]*pow(x,5))+(p8[3]*pow(x,4))+(p8[4]*pow(x,3))+(p8[5]*pow(x,2))+(p8[6]*x)+ p8[7];
    return y; // order x^7 -> x^0
}

int main(int argc, char **argv)
{

    using namespace gtsam_node;
    using namespace ast;
    using namespace ast::ros;
    ::ros::init(argc, argv, "gtsam_node");
    NodeHandle nh;

    /*--------------------------------------------| Get Params |---------------------------------------------*/
    CarParams carParams;
    MheParams mheParams;
    ParamsIn(carParams, mheParams, nh); //TODO: change mhe to graph (just old name )
    /*-------------------------------------------------------------------------------------------------------*/

    /*-----------------------------------------------| Rviz |-------------------------------------------------*/
    ::ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("GTSAM_points", 10);
    /*--------------------------------------------------------------------------------------------------------*/

    ROS_INFO_STREAM("Estimator loop rate: "<< mheParams.loopRate <<"");

    sleep(2);
    ::ros::Rate loop_rate(mheParams.loopRate);
    while(::ros::ok())
    {
        auto now = ::ros::Time::now();
        /*---------------------------------| Rviz initialization |--------------------------------------------*/
        visualization_msgs::Marker points, pointsInit, PointOpt, line_strip, circleObst;
        circleObst.header.frame_id = PointOpt.header.frame_id = pointsInit.header.frame_id = points.header.frame_id = line_strip.header.frame_id = "/gtsam_frame";
        circleObst.header.stamp = points.header.stamp = line_strip.header.stamp = PointOpt.header.stamp = pointsInit.header.stamp = now;
        circleObst.ns = pointsInit.ns = PointOpt.ns = points.ns = line_strip.ns = "points_and_lines";
        circleObst.action = pointsInit.action = PointOpt.action = points.action = line_strip.action = visualization_msgs::Marker::ADD;
        circleObst.pose.orientation.w = pointsInit.pose.orientation.w = PointOpt.pose.orientation.w = points.pose.orientation.w = line_strip.pose.orientation.w = 1.0;

        points.id = 0; line_strip.id = 1; pointsInit.id = 2; PointOpt.id = 3; circleObst.id = 4;
        points.type = visualization_msgs::Marker::POINTS;
        line_strip.type = visualization_msgs::Marker::LINE_STRIP;
        pointsInit.type = visualization_msgs::Marker::POINTS;
        PointOpt.type = visualization_msgs::Marker::POINTS;
        circleObst.type = visualization_msgs::Marker::LINE_STRIP;
        // POINTS markers use x and y scale for width/height respectively
        points.scale.x = 0.2; points.scale.y = 0.2;  points.scale.z = 0.0;
        pointsInit.scale.x = 0.05; pointsInit.scale.y = 0.05; pointsInit.scale.z = 0.0;
        PointOpt.scale.x = 0.05; PointOpt.scale.y = 0.05; PointOpt.scale.z = 0.0;
        points.color.g = 1.0; points.color.a = 1.0;
        line_strip.color.b = 1.0; line_strip.color.a = 1.0;
        line_strip.scale.x = 0.02; line_strip.scale.y = 0.02; line_strip.scale.z = 0.0;
        circleObst.scale.x = 0.01;circleObst.scale.y = 0.01;circleObst.scale.z = 0.0; circleObst.color.a = 1.0;
        circleObst.color.b = 1.0; circleObst.color.g = 1.0;
        pointsInit.color.r = 1.0; pointsInit.color.a = 1.0;
        PointOpt.color.r = 1.0; PointOpt.color.b = 1.0; PointOpt.color.a = 1.0;
        /*--------------------------------------------------------------------------------------------------------*/

        NonlinearFactorGraph graph;
        /*---------------------------------------| polynomal node |-----------------------------------------------*/
        gtsam::Vector polyPriorSigmas(8);
        polyPriorSigmas << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ;
        auto priorPolyNoise = gtsam::noiseModel::Diagonal::Sigmas(polyPriorSigmas);
        gtsam::Vector polyParams(8);
        polyParams << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ; // order x^7 -> x^0
        //polyParams << 0.001, -0.036, 0.294, -0.808, -0.505, 4.54, -2.492, 1.500; // order x^7 -> x^0
        //graph.addPrior(Symbol('l', 0), polyParams, priorPolyNoise);
        /*--------------------------------------------------------------------------------------------------------*/

        /*--------------------------------| polynomal<->point factor |--------------------------------------------*/
        gtsam::Vector polyPointSigmas(13);
        polyPointSigmas << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ;
        auto polyPointNoise = gtsam::noiseModel::Diagonal::Sigmas(polyPointSigmas);

        gtsam::Vector po1(5), po2(5), poj(5) ;
        po1 << -3.0, 3.0, 0.0, 0.5, 0.0;
        poj << 0.0, 0.0, 0.0, 0.5, 0.0;
        po2 << 3.0, -3.0, 0.0, 0.5, 0.0;
        gtsam::Vector poPriorSigmas(5);
        poPriorSigmas << 0.0, 0.0, 0.0, 0.0, 0.0;
        auto priorPoNoise = gtsam::noiseModel::Diagonal::Sigmas(poPriorSigmas);
        graph.addPrior(Symbol('p', 0), po1, priorPoNoise);   //add prior on first point
        graph.addPrior(Symbol('p', 59), po2, priorPoNoise);   //add prior on first point
        geometry_msgs::Point pois;
        pois.x = po1[0];
        pois.y = po1[1];
        points.points.push_back(pois);
        pois.x = po2[0];
        pois.y = po2[1];
        points.points.push_back(pois);

        std::vector<gtsam::Vector5> poses;
        for(size_t j = 0; j < 60; ++j) // create vector of poses
        {
          poses.push_back(poj);
        }

        for(size_t j = 0; j < poses.size(); ++j) //add polynoaml<->point factors to the graph
        {
          graph.push_back(boost::make_shared<PolyPointFactor>(Symbol('l', 0), Symbol('p', j), polyParams, poses[j], polyPointNoise));
          ROS_INFO_STREAM("key: "<< j <<"");
        }
        /*--------------------------------------------------------------------------------------------------------*/

        /*-----------------------------------| point<->point factor |---------------------------------------------*/
        gtsam::Vector ppSigmas(10);
        ppSigmas << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
        auto ppNoise = gtsam::noiseModel::Diagonal::Sigmas(ppSigmas);

        for (size_t j = 0; j < (poses.size()-1); ++j)   //add distance factor in x direction between each neighbouring point
        {
          graph.push_back(boost::make_shared<ppFactor>(Symbol('p', j), Symbol('p', j+1), poses[j], poses[j+1], ppNoise));
        }
        /*--------------------------------------------------------------------------------------------------------*/

        /*--------------------------------------| obstacle  factor |----------------------------------------------*/
        auto circleNoise = noiseModel::Diagonal::Sigmas(Vector1(0.0));  // 10cm std on x,y
        gtsam::Vector circle0(3),circle1(3); //x, y, r
        circle0 <<  0.5, 2.6, 0.5;
        circle1 << 0.2, 0.0, 0.6;

        for(size_t j = 0; j < poses.size(); ++j)
        {
          graph.push_back(boost::make_shared<CircleFactor>(Symbol('p', j), circle0, circleNoise));
          graph.push_back(boost::make_shared<CircleFactor>(Symbol('p', j), circle1, circleNoise));
        }
        /*--------------------------------------------------------------------------------------------------------*/
        ROS_INFO_STREAM("/////////////////////////////////////////////////////////");
        //graph.print("\nFactor Graph:\n");  // cout the graph

        /*----------------------------| graph optimization initialization |----------------------------------------*/
        Values initialEstimate;

        gtsam::Vector po0init(8);
        //po0init << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
        po0init << 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0;
        initialEstimate.insert(Symbol('l', 0), po0init);

        geometry_msgs::Point poisInit;
        gtsam::Vector po1init(5);
        po1init << -3.1, 3.1, 0.0, 0.5, 0.0;
        poisInit.x = po1init[0];
        poisInit.y = po1init[1];
        pointsInit.points.push_back(poisInit);

        for (size_t j = 0; j < poses.size(); ++j)   //points.size()
        {
          initialEstimate.insert(Symbol('p', j), po1init);
        }
        //initialEstimate.print("\nInitial Estimate:\n");  // print
        /*--------------------------------------------------------------------------------------------------------*/

        /*-------------------------------------| optimize the graph  |---------------------------------------------*/
        LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
        Values result = optimizer.optimize();
        //result.print("Final Result:\n");
        geometry_msgs::Point poisOpt;

        Vector8 polyUpdate = result.at<Vector8>(Symbol('l', 0));
        for (size_t j = 0; j < poses.size(); ++j)   //points.size()
        {
          Vector5 x_update = result.at<Vector5>(Symbol('p', j));
          poisOpt.x = x_update[0];
          poisOpt.y = x_update[1];
          PointOpt.points.push_back(poisOpt);
        }

        Marginals marginals(graph, result);
        /*
        cout << "polynomial covariance:\n" << marginals.marginalCovariance(Symbol('l', 0)) << endl;
        cout << "p1 covariance:\n" << marginals.marginalCovariance(Symbol('p', 0)) << endl;
        cout << "p2 covariance:\n" << marginals.marginalCovariance(Symbol('p', 1)) << endl;
        cout << "p3 covariance:\n" << marginals.marginalCovariance(Symbol('p', 2)) << endl;
        cout << "p4 covariance:\n" << marginals.marginalCovariance(Symbol('p', 3)) << endl;
        cout << "p5 covariance:\n" << marginals.marginalCovariance(Symbol('p', 4)) << endl;
        /**/
        /*--------------------------------------------------------------------------------------------------------*/

        /*------------------------------------| graph result to rviz  |-------------------------------------------*/

        auto circle1bst = circleObst;
        circle1bst.id = 5;
        for (double i = 0; i < 100; ++i)
        {
        geometry_msgs::Point p;
        p.x = ((double)i/10) - 5;
        p.y = getPolyY(p.x, polyUpdate);
        p.z = 0;
        line_strip.points.push_back(p);
        }
        for (double i = 0; i < (2 * M_PI); i = i + (M_PI/50))
        {
        geometry_msgs::Point p;
        p.x = circle0[2] * cos(i) + circle0[0];
        p.y = circle0[2] * sin(i) + circle0[1];
        p.z = 0;
        circleObst.points.push_back(p);
        }

        for (double i = 0; i < (2 * M_PI); i = i + (M_PI/50))
        {
        geometry_msgs::Point p;
        p.x = circle1[2] * cos(i) + circle1[0];
        p.y = circle1[2] * sin(i) + circle1[1];
        p.z = 0;
        circle1bst.points.push_back(p);
        }

        marker_pub.publish(circleObst);
        marker_pub.publish(circle1bst);
        marker_pub.publish(line_strip);
        marker_pub.publish(points);
        //marker_pub.publish(pointsInit);
        marker_pub.publish(PointOpt);

        /*--------------------------------------------------------------------------------------------------------*/
        auto loopTime =  ::ros::Time::now() - now;
        ROS_INFO_STREAM("loop time" << loopTime.toSec());

        ::ros::spinOnce();
        loop_rate.sleep();

    }

    return 0;
}

/*
gtsam::Vector crvJp(7);
        crvJp = Vector::Zero(7);
        crvJp[0] = (-0.6931471*ctrVal*(((21* pow(p5[0],6)* errYx*  sqrt( pow(errYx,2)  +1))/ abs(errY2x))  - ((42*pow(p5[0],5) *  pow( (pow(errYx,2)+1) ,1.5))  / (errY2x*abs(errY2x))  )))  / (pow(2,ctrVal*(p5[3] - curveMax)));
        //if (isnan(crvJp[0])) {crvJp[0] = 0.0;}
        crvJp[1] = (-0.6931471*ctrVal*(((18* pow(p5[0],5)* errYx*  sqrt( pow(errYx,2)  +1))/ abs(errY2x))  - ((30*pow(p5[0],4) *  pow( (pow(errYx,2)+1) ,1.5))  / (errY2x*abs(errY2x))  )))  / (pow(2,ctrVal*(p5[3] - curveMax)));
        //if (isnan(crvJp[1])) {crvJp[1] = 0.0;}
        crvJp[2] = (-0.6931471*ctrVal*(((15* pow(p5[0],4)* errYx*  sqrt( pow(errYx,2)  +1))/ abs(errY2x))  - ((20*pow(p5[0],3) *  pow( (pow(errYx,2)+1) ,1.5))  / (errY2x*abs(errY2x))  )))  / (pow(2,ctrVal*(p5[3] - curveMax)));
        //if (isnan(crvJp[2])) {crvJp[2] = 0.0;}
        crvJp[3] = (-0.6931471*ctrVal*(((12* pow(p5[0],3)* errYx*  sqrt( pow(errYx,2)  +1))/ abs(errY2x))  - ((12*pow(p5[0],2) *  pow( (pow(errYx,2)+1) ,1.5))  / (errY2x*abs(errY2x))  )))  / (pow(2,ctrVal*(p5[3] - curveMax)));
        //if (isnan(crvJp[3])) {crvJp[3] = 0.0;}
        crvJp[4] = (-0.6931471*ctrVal*(((9*  pow(p5[0],2)* errYx*  sqrt( pow(errYx,2)  +1))/ abs(errY2x))  - ((6*      p5[0]   *  pow( (pow(errYx,2)+1) ,1.5))  / (errY2x*abs(errY2x))  )))  / (pow(2,ctrVal*(p5[3] - curveMax)));
        //if (isnan(crvJp[4])) {crvJp[4] = 0.0;}
        crvJp[5] = (-0.6931471*ctrVal*(((6*      p5[0]*    errYx*  sqrt( pow(errYx,2)  +1))/ abs(errY2x))  - ((2*                 pow( (pow(errYx,2)+1) ,1.5))  / (errY2x*abs(errY2x))  )))  / (pow(2,ctrVal*(p5[3] - curveMax)));
        //if (isnan(crvJp[5])) {crvJp[5] = 0.0;}
        crvJp[6] = (-0.6931471*ctrVal*   3*                errYx*  sqrt( pow(errYx,2)  +1))                                                                                /(abs(errY2x)*pow(2,ctrVal*(p5[3] - curveMax)));
        //if (isnan(crvJp[6])) {crvJp[6] = 0.0;}
        */


#include <iostream>
#define ARMA_DONT_USE_WRAPPER
#include <armadillo>
#include <cmath>

using namespace std;
using namespace arma;

// g++ mat2.cpp -o mat2 -std=c++11 -O2 -larmadillo -framework Accelerate

int
main(int argc, char** argv)
  {
      float pi=M_PI;
      float eu=std::exp(1.0);
      float raiz= sqrt(2);
      float raiz1= pow(pi,0.3333);
      float coseno = cos(pi/4);
      float cdiag = pow(7,0.3333);

      mat A(10,10, fill::zeros); //A(fila, columna)
      cout << "A.n_rows: " << A.n_rows << endl;
      cout << "A.n_cols: " << A.n_cols << endl;
      A.rows(0,4) += pi;
      A.rows(5,9) += eu;
      A.save("A1.txt", raw_ascii);

      mat B(10,10, fill::zeros);
      cout << "B.n_rows: " << B.n_rows << endl;
      cout << "B.n_cols: " << B.n_cols << endl;
      B.col(0) += raiz;
      B.col(2) += raiz;
      B.col(4) += raiz;
      B.col(8) += raiz;
      B.col(1) += raiz1;
      B.col(6) += raiz1;
      B.col(7) += raiz1;
      B.col(3) += coseno;
      B.col(5) += coseno;
      B.col(9) += coseno;
      B.save("B1.txt", raw_ascii);

      mat C = repelem(A, 1, 1);
      C.diag() += cdiag;
      C.save("C1.txt", raw_ascii);

      mat D(10,10, fill::randu);
      mat E(10,10, fill::zeros );

      E += pow((A.t()+B),-1 ) + (pow(A,-1)*pow(B,0.3333)*pow(D,0.2)) - (pow(D,-1)*B.t()*(A.t()*pow(C,-1))) + D*(cos(C)*sin(A)+pow(sin(B),2)) ;
      E.save("E1.txt", raw_ascii);

      mat F(10,10, fill::zeros);

      F += exp(C)*D + exp(A)*sin(A) + (sqrt(pow(pi, 3))/exp(pi))*cos(D) - log10(sqrt(pi/8))*pow(cos(B),4) + pow(sin(D.t()*pow(C,3)*A),-1)+ E*(cos(D)+sin(D));
      F.save("F1.txt", raw_ascii);

      cout << "C.n_rows: " << C.n_rows << endl;
      cout << "C.n_cols: " << C.n_cols << endl;
      cout << "D.n_rows: " << D.n_rows << endl;
      cout << "D.n_cols: " << D.n_cols << endl;
      cout << "E.n_rows: " << E.n_rows << endl;
      cout << "E.n_cols: " << E.n_cols << endl;
      cout << "F.n_rows: " << F.n_rows << endl;
      cout << "F.n_cols: " << F.n_cols << endl;

      return 0;

  }


#pragma once

#include <list>
#include <vector>
#include <NTL/ZZ.h>
#include <math/mpz_class.hh>
#include <math.h>
#include <algorithm>    // std::min
#include <float.h>
#include <limits.h>
using namespace std;
struct encnum
{
	int  exponent;
	mpz_class mantissa;
	//double plaintext;
};
class Paillier {
 public:
    Paillier(const std::vector<mpz_class> &pk, gmp_randstate_t state);
    std::vector<mpz_class> pubkey() const { return { n, g }; }
    encnum encrypt_f(double plaintext);
    std::vector  <std::vector<encnum>> encryptMatrix(const std::vector < std::vector<double> > &p);
    encnum encode( double plaintext) const;

    encnum sub_f(const encnum &a, const encnum &b) const;
    encnum add_f(const encnum &a, const encnum &b) const;
    std::vector  <std::vector<encnum>> addMatrix(const std::vector < std::vector<encnum> > &c0, const std::vector < std::vector<encnum> > &c1) ;
    std::vector  <std::vector<encnum>> multConstMatrixbyMatrix(const std::vector < std::vector<double> > &c, const std::vector < std::vector<encnum> > &mat) ;
    std::vector  <std::vector<encnum>> multMatrixbyConstMatrix(const std::vector < std::vector<encnum> > &mat , const std::vector < std::vector<double> > &c) ;

    encnum decrease_exponent_too(const encnum &y,const int new_exp) const;
    encnum constMult_f(const double &m, const encnum &c) const;
	vector < vector<encnum> > multMatrixbyConst( double alpha,const vector < vector<encnum> > &mat) ;

    mpz_class encrypt(const mpz_class &plaintext);
    mpz_class add(const mpz_class &c0, const mpz_class &c1) const;
    mpz_class sub(const mpz_class &c0, const mpz_class &c1) const;
    mpz_class constMult(const mpz_class &m, const mpz_class &c) const;
    mpz_class constMult(long m, const mpz_class &c) const;
    mpz_class constMult(const mpz_class &c, long m) const { return constMult(m,c); };
    mpz_class scalarize(const mpz_class &c);
    void refresh(mpz_class &c);
    mpz_class random_encryption();

    mpz_class dot_product(const std::vector<mpz_class> &c, const std::vector<mpz_class> &v);
    mpz_class dot_product(const std::vector<mpz_class> &c, const std::vector<long> &v);
    void rand_gen(size_t niter = 100, size_t nmax = 1000);
    double logbase(double base, double x) const;

    void printMatrix(std::vector  <std::vector<double>> &Mat);

    const double precision = 0.00000001 ; //0.00000001 0.001
    bool acc_per = false;
    int max_exponent = 0;
	//int exponent;
	//int int_rep;
    int BASE = 10;
    double LOG2_BASE = log2(BASE);
    double FLOAT_MANTISSA_BITS = LDBL_MANT_DIG;
    encnum zero_enc;
   // double max_int = n.get_d()/3 -1;
    //double nsquare = n.get_d() * n.get_d() ;
 protected:
    /* Public key */
    const mpz_class n, g;

    /* Randomness state */
    gmp_randstate_t _randstate;

    /* Cached values */
    const uint nbits;
    const mpz_class n2;
    bool good_generator;

    /* Pre-computed randomness */
    std::list<mpz_class> rqueue;
};

class Paillier_priv : public Paillier {
 public:
    Paillier_priv(const std::vector<mpz_class> &sk, gmp_randstate_t state);
    std::vector<mpz_class> privkey() const { return { p, q, g, a }; }
    void find_crt_factors();

    // if a !=0, and if you are encrypting using the private key, use this function
    // 75% speedup
    mpz_class encrypt(const mpz_class &plaintext);

    // no speedup compared to the fast_encrypt
    mpz_class fast_encrypt_precompute(const mpz_class &plaintext);

    mpz_class decrypt(const mpz_class &ciphertext) const;
    static std::vector<mpz_class> keygen(gmp_randstate_t state, uint nbits = 1024, uint abits = 256);
    double decrypt_f(encnum x) const;
    std::vector< std::vector<double> > decryptMatrix(const std::vector < std::vector<encnum> > &c);

 protected:
    /* Private key, including g from public part; n=pq */
    const mpz_class p, q;
    const mpz_class a;      /* non-zero for fast mode */

    /* Cached values */
    const bool fast;
    const mpz_class p2, q2;
    mpz_class e_p2, e_q2;
    const mpz_class two_p, two_q;
    const mpz_class pinv, qinv;
    const mpz_class hp, hq;
};

class Paillier_priv_fast : public Paillier_priv {
public:
    Paillier_priv_fast(const std::vector<mpz_class> &sk, gmp_randstate_t state);
    void precompute_powers();
    mpz_class compute_g_star_power(const mpz_class &x);
    static std::vector<mpz_class> keygen(gmp_randstate_t state, uint nbits = 1024);

    mpz_class encrypt(const mpz_class &plaintext);
private:
    const mpz_class g_star_;
    const mpz_class phi_n;
    const mpz_class phi_n2;
    const uint phi_n2_bits;
    std::vector<mpz_class> g_star_powers_p_;
    std::vector<mpz_class> g_star_powers_q_;
};


/*
 * ex1x.cpp
 *
 *  Created on: 25.04.2017
 *
 *  Author: Daniel Rehfeldt
 *
 *  Compile: g++ ex1x.cpp -o ex1x -O3 -std=c++14 -Wall
 */

#include <vector>
#include <cmath>
#include <limits>
#include <iostream>
#include <fstream>
#include <cassert>
#include <boost/spirit/include/qi.hpp>

int main(int argc, char* argv[])
{
    // check number of parameters
    if( argc != 2 )
    {
       // tell the user how to run the program
       std::cerr << "Usage: " << argv[0] << " filename" << std::endl;
       exit(EXIT_FAILURE);
    }

    /* start of definitions for geometric mean */

    std::vector<double> valvec[2];
    double geomean[2] {0.0, 0.0};
    int geomeancount[2] {0, 0};

    // get number of multiplications in geo mean computation guaranteed to be without underflow; unfortunately, constexpr is not supported here by c++14
    const int nmults = std::log2(std::numeric_limits<double>::min()) / std::log2(0.5);

    assert(nmults > 0);
    assert(std::ilogb(std::numeric_limits<double>::max()) + 1 <= std::numeric_limits<int>::max() / nmults); // guard against overflow of exponent

    // compute logarithm of product of values entries in range [start, end)
    auto subsetlog = [](auto start, auto end, std::vector<double> & values)
    {
       // split values into mantissa and exponent
       int exponentsum = 0;
       double mantissaprod = 1.0;
       for( auto j = start; j < end; j++ )
       {
            int ex;
            mantissaprod *= std::frexp(values[j], &ex);
            exponentsum += ex;
       }
       return std::log2(mantissaprod) + exponentsum;
    };

    /* end of definitions for geometric mean */

    unsigned int nlines = 0;

    std::ifstream file (argv[1], std::ifstream::in);
    std::string strline;

    // read line by line
    while( getline(file, strline) )
    {
       nlines++;

       using namespace boost::spirit;
       using qi::int_;
       using qi::double_;
       using qi::phrase_parse;
       using ascii::space;

       double value;
       int location;

       // iterator which will be changed by parse method
       auto it = strline.begin();

       // parse line as int;int;double and ignore whitespaces
       bool success = phrase_parse(it, strline.end(),
             int_ >> ';' >> int_[([&location](int i){ location = i; })] >> ';' >> double_[([&value](double i){ value = i; })]
                      , space);

       if( success && it == strline.end() )
       {
          if( location < 1 || location > 2 || value <= 0 || std::isnan(value) || value <= 0 )
             continue;

          // finally, store the value
          valvec[--location].push_back(value);

          // update geo mean?
          assert(valvec[location].size() <= std::numeric_limits<int>::max());
          assert(int(valvec[location].size()) - geomeancount[location] <= nmults);
          if( int(valvec[location].size()) - geomeancount[location] == nmults )
          {
             geomean[location] += subsetlog(geomeancount[location], int(valvec[location].size()), valvec[location]);
             geomeancount[location] = int(valvec[location].size());
          }
       }
    }

    file.close();

    // update and print geo mean
    for( int i = 0; i < 2; i++ )
    {
      geomean[i] += subsetlog(geomeancount[i], int(valvec[i].size()), valvec[i]);
      geomean[i] /= double(valvec[i].size());
      geomean[i] = std::pow(2.0, geomean[i]);

      std::cout << "Valid values Loc" << i + 1 <<  " " << valvec[i].size() << " with geo mean: " << geomean[i] << std::endl;
    }
    std::cout << "File: " << argv[1] << " with " << nlines << " lines " << std::endl;

    return EXIT_SUCCESS;
}


// NOTE: this is a subset sum problem
#include <algorithm>
#include <fstream>
#include <iostream>
#include <iterator>
#include <numeric>
#include <string>
#include <vector>

#include <boost/range/adaptor/reversed.hpp>
#include <boost/range/irange.hpp>

// this recursive function will go through all possible combinations of weights, starting with the last weight in the list,
// which is indicated by parameter 'last', until the sum (or what's left of it) is reached
template<typename Set, typename Sets, typename W, typename T, typename N>
auto find_subset(Set&& subset, Sets&& subsets, const W& weights, const T& tbl_has_sum, N row, N sum_left) -> void {

	const auto only_one_element_left = (row == 0);

	// first base case: only one weight left (first of the weights), sum is non-zero
	// and the final weight to be added to reach 'sum_left' is the the first weight
	if(only_one_element_left && (sum_left > 0) && tbl_has_sum[row][sum_left]) {

		subset.push_back(weights[row]);

		subsets.push_back(subset);

	} else if(only_one_element_left && (sum_left == 0)) {

		// second base case: only one weight left, there's no more sum left
		// to add to, so the current subset can be added to the collection
		subsets.push_back(subset);

	} else {

		const auto prev_row = (row - 1);

		// first recursive case: the matrix tells us whether or not sum_left
		// can be formed without the current weight but with all previous weights
		if(tbl_has_sum[prev_row][sum_left]) {

			// there are potentially two successive calls to find_subset, which means the passed subset will be modified
			// both times - but on the same recursion level we need the subset to be the same, so the first call will get
			// a copy, and when the recursion for that copy is finished, which at that point will be modified, we can continue
			// to the second call with the original that we started the recursion with
			auto new_subset = subset;

			find_subset(new_subset, subsets, weights, tbl_has_sum, prev_row, sum_left);
		}

		const auto weight = weights[row];

		const auto new_sum_left = (sum_left - weight);

		// second recursive case: to make access to the matrix safe, we need to check first,
		// if the current weight can be added without going over the sum we need - if the first
		// check is successful, the matrix will tell us, whether or not the previous row can
		// create the sum 'sum_left - weight' - if the answer is 'yes', we can add the current
		// weight to the current subset and make another recursive call
		if((sum_left >= weight) && tbl_has_sum[prev_row][new_sum_left]) {

			subset.push_back(weight);

			find_subset(subset, subsets, weights, tbl_has_sum, prev_row, new_sum_left);
		}
	}
}

// this function is based on: https://stackoverflow.com/a/45427013/699211
template<typename T, typename N>
auto find_subsets(const T& weights, N sum) {

	auto subsets = std::vector<T>{};

	// in dynamic programming, the first row of the lookup matrix
	// represents zero elements (empty subsets), so normally such
	// a matrix would have (n + 1) rows, but since we won't do anything
	// with that particular row, we might just as well stick to n rows
	const auto rows = weights.size();

	using VectorBool = std::vector<std::uint8_t>;
	using Matrix = std::vector<VectorBool>;

	// todo: rename lookup

	// this is a truth table - matrix[i][s] returns true or false to indicate the answer to the question:
	// "using the first i items in the array can we find a subset sum to s?"
	auto tbl_has_sum = Matrix(rows);

	const auto columns = (sum + 1);

	for(auto& row : tbl_has_sum) {
		row = VectorBool(columns);
		// first column represents subset sum = 0, which is true for all sets
		row.front() = true;
	}

	// first element of the weights container
	const auto first = weights.front();

	// if the first element of weights is <= sum, then the set containing that weight will have it as subset sum,
	// allowing to check if it's possible to combine that weight with any of the subset sums of the previous row
	// to see if we can create the subset sum we are looking for
	if (first <= sum) {
		tbl_has_sum.front()[first] = true;
	}

	for(const auto row : boost::irange({1}, rows)) {

		for(const auto col : boost::irange({}, columns)) {

			auto& cell = tbl_has_sum[row][col];

			const auto weight = weights[row];
			const auto& prev_row = tbl_has_sum[row-1];
			const auto cell_prev_row = prev_row[col];

			// ensure we don't accidentally go out of bounds
			if(weight <= col) {
				// this checks if the subset sum 'col' can be formed by
				// a) the previous set of weights (cell_prev_row) or
				// b) if subset sum 'col-weight' is formable by the previous set
				// this is the implementation of the following logic:
				// set X with subset-sum k has subset-sum k1 if subset X1 has subset-sum (k - k1)
				cell = (cell_prev_row || prev_row[col-weight]);
			} else {
				cell = cell_prev_row;
			}
		}
	}

	// start recursion
	find_subset(T{}, subsets, weights, tbl_has_sum, (rows - 1), sum);

	return subsets;
}

template<typename T>
auto find_min_sets(const T& subsets) {

	using value_type = decltype(subsets.size());
	auto min_size = std::numeric_limits<value_type>::max();

	auto min_sets = T{};

	for(const auto& subset : subsets) {

		const auto subset_size = subset.size();

		// we don't need any sets that have size > min_size
		// so if a smaller set is found, the min_sets collection
		// can immediately be replaced
        if(subset_size < min_size) {
        	min_size = subset_size;
        	min_sets = {subset};
        } else if(subset_size == min_size) {
        	min_sets.push_back(subset);
        }
	}

	return min_sets;
}

// =======================================
// SOME TEMPLATE NONSENSE FOR DEDUCING THE
// INNERMOST TYPE OF A NESTED CONTAINER
// =======================================
template<typename T, typename = std::void_t<>>
struct innermost_value_type {
    using type = T;
};

template<typename T>
struct innermost_value_type<T, std::void_t<typename T::value_type>> {
    using type = typename innermost_value_type<typename T::value_type>::type;
};

template<typename T>
using innermost_value_type_t = typename innermost_value_type<T>::type;
// =======================================
// END OF TEMPLATE NONSENSE
// =======================================

template<typename T>
auto find_min_quantum_entanglement(const T& min_sets) {

	using ValueType = innermost_value_type_t<T>;

	auto min_value = std::numeric_limits<ValueType>::max();

	for(const auto& min_set : min_sets) {

		const auto begin = min_set.begin();
		const auto end   = min_set.end();
		const auto init  = ValueType{1};
		const auto binop = std::multiplies{};

		const auto new_value = std::accumulate(begin, end, init, binop);

		min_value = std::min(min_value, new_value);
	}

	return min_value;
}

int main() {

	const auto filename = std::string{"weights.txt"};
	auto file = std::fstream{filename};

	if(file.is_open()) {

		auto weights = std::vector<std::uint64_t>{};

		using NumericType = innermost_value_type_t<decltype(weights)>;

		using stream_iterator = std::istream_iterator<NumericType>;

		const auto begin = stream_iterator{file};
		const auto end   = stream_iterator{};
		const auto init  = NumericType{};
		const auto binop = [&weights] (auto acc, auto weight) {
			weights.push_back(weight);
			return (acc + weight);
		};

		const auto total_weight = std::accumulate(begin, end, init, binop);

		const auto n_groups = 3;

		const auto sum_per_group = (total_weight / n_groups);

		std::sort(weights.begin(), weights.end());

		const auto subsets = find_subsets(weights, sum_per_group);

		const auto min_sets = find_min_sets(subsets);

		std::cout << find_min_quantum_entanglement(min_sets) << std::endl;

	} else {
		std::cerr << "Error! Could not open \"" << filename << "\"!" << std::endl;
	}

	return 0;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include <Eigen/Core>
#include <cassert>
#include <cmath>
#include <map>

namespace fluid {
namespace algorithm {

class NNActivations
{

public:
  enum class Activation { kLinear, kSigmoid, kReLU, kTanh };

  using ArrayXXd = Eigen::ArrayXXd;
  using ActivationsMap =
      std::map<Activation, std::function<void(Eigen::Ref<Eigen::ArrayXXd>,
                                              Eigen::Ref<Eigen::ArrayXXd>)>>;

  static ActivationsMap& activation()
  {
    static ActivationsMap _funcs = {
        {Activation::kLinear,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = in;
         }},
        {Activation::kSigmoid,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = 1 / (1 + (-in).exp());
         }},
        {Activation::kReLU,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = in.max(0);
         }},
        {Activation::kTanh,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = (in.exp() - (-in).exp()) / (in.exp() + (-in).exp());
         }},
    };
    return _funcs;
  }

  // derivative from output of activation
  static ActivationsMap& derivative()
  {
    static ActivationsMap _funcs = {
        {Activation::kLinear,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = ArrayXXd::Ones(in.rows(), in.cols());
         }},
        {Activation::kSigmoid,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = in * (1 - in);
         }},
        {Activation::kReLU,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = (in > 0).cast<double>();
         }},
        {Activation::kTanh,
         [](Eigen::Ref<Eigen::ArrayXXd> in, Eigen::Ref<Eigen::ArrayXXd> out) {
           out = 1 - in.square();
         }}};
    return _funcs;
  }
};
} // namespace algorithm
} // namespace fluid


#include "QSLIM.h"

#include <wmtk/TriMesh.h>
#include <wmtk/utils/VectorUtils.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <wmtk/ExecutionScheduler.hpp>
#include <wmtk/utils/TupleUtils.hpp>

using namespace wmtk;
using namespace qslim;

// called in collapse_edge_after. update the quadric for v
// adding the A, b , c for two vertices of the old edge and store at the vert_attr of new vertex
void QSLIM::update_quadrics(const Tuple& new_v)
{
    vertex_attrs[new_v.vid(*this)].Q = {
        cache.local().Q1.A + cache.local().Q2.A,
        cache.local().Q1.b + cache.local().Q2.b,
        cache.local().Q1.c + cache.local().Q2.c};
}

// want to store the A, b, c for each face as face attribute
Quadrics QSLIM::compute_quadric_for_face(const TriMesh::Tuple& f_tuple)
{
    Quadrics Q;
    Eigen::Vector3d n = face_attrs[f_tuple.fid(*this)].n;
    Q.A = n * n.transpose();
    double d = -n.dot(vertex_attrs[oriented_tri_vertices(f_tuple)[0].vid(*this)].pos);
    Q.b = d * n;
    Q.c = d * d;
    return Q;
}
double QSLIM::compute_cost_for_e(const TriMesh::Tuple& v_tuple)
{
    // first get Q
    Quadrics Q1 = vertex_attrs[v_tuple.vid(*this)].Q;
    Quadrics Q2 = vertex_attrs[v_tuple.switch_vertex(*this).vid(*this)].Q;
    Quadrics Q = {Q1.A + Q2.A, Q1.b + Q2.b, Q1.c + Q2.c};
    double cost = 0.0;
    Eigen::Vector3d vbar(0.0, 0.0, 0.0);
    Eigen::Vector3d v1 = vertex_attrs[v_tuple.vid(*this)].pos;
    Eigen::Vector3d v2 = vertex_attrs[v_tuple.switch_vertex(*this).vid(*this)].pos;

    // test if A is invertible
    // not invertible vbar is smallest of two vertices
    // if A is invertible, compute vbar using A.inv@b
    if (Q.A.determinant() < 1e-10 && Q.A.determinant() > -1e-10) {
        // not invertible
        if ((v1.transpose() * Q1.A * v1 + 2 * Q1.b.dot(v1) + Q1.c) <
            (v2.transpose() * Q2.A * v2 + 2 * Q2.b.dot(v2) + Q2.c))
            vbar = v1;
        else
            vbar = v2;

    } else {
        vbar = -Q.A.inverse() * Q.b;
    }
    cost = (vbar.dot(Q.A * vbar) + 2 * Q.b.dot(vbar) + Q.c);
    // the cost has to greater than 0
    if (cost < 0 && cost > -1e-15)
        cost = 0; // floating point error
    else if (cost < 0)
        wmtk::logger().info(
            "cost is {} smaller than zero \n Q.A is \n{} \nQ.b \n{}\n Q.c \n {} \nvbar is\n{} "
            " ",
            cost,
            Q.A,
            Q.b,
            Q.c,
            vbar);
    // wmtk::logger().info("cost is smaller than zero");
    edge_attrs[v_tuple.eid(*this)].vbar = vbar;
    return cost;
}

bool QSLIM::invariants(const std::vector<Tuple>& new_tris)
{
    if (m_has_envelope) {
        for (auto& t : new_tris) {
            std::array<Eigen::Vector3d, 3> tris;
            auto vs = t.oriented_tri_vertices(*this);
            for (auto j = 0; j < 3; j++) tris[j] = vertex_attrs[vs[j].vid(*this)].pos;
            if (m_envelope.is_outside(tris)) return false;
        }
    }
    return true;
}

bool QSLIM::write_triangle_mesh(std::string path)
{
    Eigen::MatrixXd V = Eigen::MatrixXd::Zero(vert_capacity(), 3);
    for (auto& t : get_vertices()) {
        auto i = t.vid(*this);
        V.row(i) = vertex_attrs[i].pos;
    }

    Eigen::MatrixXi F = Eigen::MatrixXi::Constant(tri_capacity(), 3, -1);
    for (auto& t : get_faces()) {
        auto i = t.fid(*this);
        auto vs = oriented_tri_vertices(t);
        for (int j = 0; j < 3; j++) {
            F(i, j) = vs[j].vid(*this);
        }
    }

    return igl::write_triangle_mesh(path, V, F);
}

bool QSLIM::collapse_edge_before(const Tuple& t)
{
    if (!ConcurrentTriMesh::collapse_edge_before(t)) return false;
    if (vertex_attrs[t.vid(*this)].freeze || vertex_attrs[t.switch_vertex(*this).vid(*this)].freeze)
        return false;
    cache.local().v1p = vertex_attrs[t.vid(*this)].pos;
    cache.local().v2p = vertex_attrs[t.switch_vertex(*this).vid(*this)].pos;
    cache.local().Q1 = vertex_attrs[t.vid(*this)].Q;
    cache.local().Q2 = vertex_attrs[t.switch_vertex(*this).vid(*this)].Q;
    cache.local().vbar = edge_attrs[t.eid(*this)].vbar;
    return true;
}

bool QSLIM::collapse_edge_after(const TriMesh::Tuple& t)
{
    auto vid = t.vid(*this);
    vertex_attrs[vid].pos = cache.local().vbar;
    // update the quadrics
    update_quadrics(t);
    return true;
}

std::vector<TriMesh::Tuple> QSLIM::new_edges_after(const std::vector<TriMesh::Tuple>& tris) const
{
    std::vector<TriMesh::Tuple> new_edges;
    std::vector<TriMesh::Tuple> one_ring_verts;
    for (auto t : tris) {
        auto incident_verts = t.oriented_tri_vertices(*this);
        for (auto j = 0; j < 3; j++) one_ring_verts.push_back(incident_verts[j]);
    }
    for (auto v : one_ring_verts) {
        auto incident_edges = get_one_ring_edges_for_vertex(v);
        for (auto e : incident_edges) new_edges.push_back(e);
    }
    wmtk::unique_edge_tuples(*this, new_edges);
    return new_edges;
}

bool QSLIM::collapse_qslim(int target_vert_number)
{
    auto collect_all_ops = std::vector<std::pair<std::string, Tuple>>();
    int starting_num = get_vertices().size();
    for (auto& loc : get_edges()) collect_all_ops.emplace_back("edge_collapse", loc);

    auto renew = [](auto& m, auto op, auto& tris) {
        auto edges = m.new_edges_after(tris);
        auto optup = std::vector<std::pair<std::string, Tuple>>();
        for (auto& e : edges) optup.emplace_back("edge_collapse", e);
        return optup;
    };
    auto measure_priority = [this](auto& m, auto op, const Tuple& new_e) {
        return -compute_cost_for_e(new_e);
    };
    auto setup_and_execute = [&](auto executor) {
        executor.num_threads = NUM_THREADS;
        executor.renew_neighbor_tuples = renew;
        executor.priority = measure_priority;
        executor.stopping_criterion_checking_frequency = std::numeric_limits<int>::max();
        executor.stopping_criterion_checking_frequency = target_vert_number > 0
                                                             ? starting_num - target_vert_number - 1
                                                             : std::numeric_limits<int>::max();
        executor.stopping_criterion = [](auto& m) { return true; };
        executor.is_weight_up_to_date = [&collect_all_ops, this](auto& m, auto& ele) {
            auto& [val, op, e] = ele;
            //if (val > 0) return false; // priority is negated.
            double pri = -compute_cost_for_e(e);
            if (((val - pri) * (val - pri)) > 1e-8) {
                wmtk::logger().info("the priority is different val is {}, pri is {}", val, pri);
                return false;
            }
            return true;
        };
        executor(*this, collect_all_ops);
    };

    if (NUM_THREADS > 0) {
        auto executor = wmtk::ExecutePass<QSLIM, ExecutionPolicy::kPartition>();
        executor.lock_vertices = [](auto& m, const auto& e, int task_id) {
            return m.try_set_edge_mutex_two_ring(e, task_id);
        };
        setup_and_execute(executor);
    } else {
        auto executor = wmtk::ExecutePass<QSLIM, ExecutionPolicy::kSeq>();
        setup_and_execute(executor);
    }
    return true;
}


// MLpackTest.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include <iostream>
#include <armadillo>
#include <mlpack/core.hpp>

#include "mlpack/methods/random_forest/random_forest.hpp"
#include "mlpack/methods/decision_tree/random_dimension_select.hpp"
#include "mlpack/methods/adaboost/adaboost.hpp"

#include <mlpack/core/hpt/hpt.hpp>
#include <mlpack/core/cv/k_fold_cv.hpp>
#include <mlpack/core/cv/metrics/accuracy.hpp>
#include <mlpack/core/cv/metrics/precision.hpp>
#include <mlpack/core/cv/metrics/recall.hpp>
#include <mlpack/core/cv/metrics/F1.hpp>

using namespace mlpack;
using namespace mlpack::tree;
using namespace mlpack::cv;
using namespace mlpack::hpt;

int main()
{
	std::string msg = "";
	arma::mat dataset;
	bool loaded = mlpack::data::Load("data.csv", dataset);
	if (!loaded) {
		std::cout << "Load data file failed\n";
	}
	else {
		/*msg += "Load data file successfully\n";
		msg += "Get DATA " + std::to_string(dataset.n_rows) + " X " + std::to_string(dataset.n_cols) + "\n";
		arma::Row<size_t> labels;
		labels = arma::conv_to<arma::Row<size_t>>::from(dataset.row(dataset.n_rows - 1));
		dataset.shed_row(dataset.n_rows - 1);*/

		//model selection
		/*RandomForest<GiniGain, RandomDimensionSelect> rf;
		rf = RandomForest<GiniGain, RandomDimensionSelect>(dataset, labels, 3, 30);*/

		/*const size_t k = 15;

		KFoldCV<RandomForest<GiniGain, RandomDimensionSelect>, Accuracy> cv_rf_clf(k,dataset, labels, 3, true);
		double cvAcc_1 = cv_rf_clf.Evaluate(40, 1, 1e-6, 9);
		auto rf_1 = cv_rf_clf.Model();
		arma::Row<size_t> predictions_1;
		rf_1.Classify(dataset, predictions_1);
		double cvPrecision_1 = Precision<Binary>::Evaluate(rf_1, dataset, labels);
		double cvRecall_1 = Recall<Binary>::Evaluate(rf_1, dataset, labels);
		double cvF1_1 = F1<Binary>::Evaluate(rf_1, dataset, labels);

		const size_t correct_1 = arma::accu(predictions_1 == labels);
		msg += "\nRandom Forest:";
		msg += "\nTraining Accuracy: " + std::to_string((double(correct_1) / double(labels.n_elem)));
		msg += "\nKFoldCV Accuracy: " + std::to_string(cvAcc_1);
		msg += "\nPrecision: " + std::to_string(cvPrecision_1);
		msg += "\nRecall: " + std::to_string(cvRecall_1);
		msg += "\nF1: " + std::to_string(cvF1_1);
		std::cout << msg;*/

		/*HyperParameterTuner<RandomForest<GiniGain, RandomDimensionSelect>, Accuracy, KFoldCV> hpt_rf(k, dataset, labels, 3, true);
		arma::vec numTreesSet = { 5, 7, 10};
		arma::vec minimumLeafSizeSet = { 1,2,3 };
		arma::vec minimumGainSplitSet = { 1e-5, 1e-3, 1e-2};
		arma::vec maximumDepthSet = { 3,4,5 };
		double a,b,c,d;
		std::tie(a, b, c, d) = hpt_rf.Optimize(numTreesSet, minimumLeafSizeSet, minimumGainSplitSet, maximumDepthSet);
		std::cout << a << b << c << d;*/

		//
		///*adaboost::AdaBoost<DecisionStump<>> adaclf;
		//adaclf = adaboost::AdaBoost<DecisionStump<>>(dataset, labels, 3, wl_1);*/
		/*DecisionStump<> wl_1;
		msg = "\nAdaboost-DecisionStump:";
		KFoldCV<adaboost::AdaBoost<DecisionStump<>>, Accuracy> cv_ada_clf(k, dataset, labels, 3, true);
		double cvAcc_2 = cv_ada_clf.Evaluate(wl_1, 25, 1e-2);
		auto rf_2 = cv_ada_clf.Model();
		arma::Row<size_t> predictions_2;
		rf_2.Classify(dataset, predictions_2);
		double cvPrecision_2 = Precision<Binary>::Evaluate(rf_2, dataset, labels);
		double cvRecall_2 = Recall<Binary>::Evaluate(rf_2, dataset, labels);
		double cvF1_2 = F1<Binary>::Evaluate(rf_2, dataset, labels);

		const size_t correct_2 = arma::accu(predictions_2 == labels);
		msg += "\nTraining Accuracy: " + std::to_string((double(correct_2) / double(labels.n_elem)));
		msg += "\nKFoldCV Accuracy: " + std::to_string(cvAcc_2);
		msg += "\nPrecision: " + std::to_string(cvPrecision_2);
		msg += "\nRecall: " + std::to_string(cvRecall_2);
		msg += "\nF1: " + std::to_string(cvF1_2);
		std::cout << msg;*/

		/*DecisionStump<> wl_1;
		HyperParameterTuner<adaboost::AdaBoost<DecisionStump<>>, Accuracy, KFoldCV> hpt_ada_ds(k, dataset, labels, 3, true);
		arma::vec iterationsSet{5, 10, 15, 20, 30, 40, 50};
		arma::vec toleranceSet{1, 0.1, 0.01, 0.001, 0.0001};
		double bestIterations, bestTolerance;
		std::tie(bestIterations, bestTolerance) = hpt_ada_ds.Optimize(Fixed(wl_1), iterationsSet, toleranceSet);
		std::cout << "bestIterations: " << bestIterations << "\n" << "bestTolerance: " << bestTolerance;*/

		/*const size_t correct = arma::accu(predictions == labels);
		msg += "\nTraining Accuracy: " + std::to_string((double(correct_2) / double(labels.n_elem)));
		msg += "\nKFoldCV Accuracy: " + std::to_string(cvAcc_2);
		msg += "\nPrecision: " + std::to_string(cvPrecision_2);
		msg += "\nRecall: " + std::to_string(cvRecall_2);
		msg += "\nF1: " + std::to_string(cvF1_2);
		std::cout << msg;*/

		///*init mlpack adaboost type
		//adaboost::AdaBoost<Perceptron<>> adaclf;
		//adaclf = adaboost::AdaBoost<>(dataset, labels, 3, wl_1);*/
		/*perceptron::Perceptron<> wl_2;
		msg = "\nAdaboost-Perceptron:";
		KFoldCV<adaboost::AdaBoost<>, Accuracy> cv_ada_P_clf(k, dataset, labels, 3, true);
		double cvAcc_3 = cv_ada_P_clf.Evaluate(wl_2);
		auto rf_3 = cv_ada_P_clf.Model();
		arma::Row<size_t> predictions_3;
		rf_3.Classify(dataset, predictions_3);
		double cvPrecision_3 = Precision<Binary>::Evaluate(rf_3, dataset, labels);
		double cvRecall_3 = Recall<Binary>::Evaluate(rf_3, dataset, labels);
		double cvF1_3 = F1<Binary>::Evaluate(rf_3, dataset, labels);

		const size_t correct_3 = arma::accu(predictions_3 == labels);
		msg += "\nTraining Accuracy: " + std::to_string((double(correct_3) / double(labels.n_elem)));
		msg += "\nKFoldCV Accuracy: " + std::to_string(cvAcc_3);
		msg += "\nPrecision: " + std::to_string(cvPrecision_3);
		msg += "\nRecall: " + std::to_string(cvRecall_3);
		msg += "\nF1: " + std::to_string(cvF1_3);
		std::cout << msg;*/

		///*DecisionTree<> dtclf;
		//dtclf = DecisionTree<>(dataset, labels, 3);*/
		/*msg = "\nDecision Tree:";
		AllDimensionSelect tempSelect = AllDimensionSelect();
		KFoldCV<DecisionTree<>, Accuracy> cv_dt_clf(k, dataset, labels, 3, true);
		double cvAcc_4 = cv_dt_clf.Evaluate(9, 1e-5, 3, tempSelect);
		auto rf_4 = cv_dt_clf.Model();
		arma::Row<size_t> predictions_4;
		rf_4.Classify(dataset, predictions_4);
		double cvPrecision_4 = Precision<Binary>::Evaluate(rf_4, dataset, labels);
		double cvRecall_4 = Recall<Binary>::Evaluate(rf_4, dataset, labels);
		double cvF1_4 = F1<Binary>::Evaluate(rf_4, dataset, labels);

		const size_t correct_4 = arma::accu(predictions_4 == labels);
		msg += "\nTraining Accuracy: " + std::to_string((double(correct_4) / double(labels.n_elem)));
		msg += "\nKFoldCV Accuracy: " + std::to_string(cvAcc_4);
		msg += "\nPrecision: " + std::to_string(cvPrecision_4);
		msg += "\nRecall: " + std::to_string(cvRecall_4);
		msg += "\nF1: " + std::to_string(cvF1_4);
		std::cout << msg;*/

		/*HyperParameterTuner<DecisionTree<>, Accuracy, KFoldCV> hpt_dt(k, dataset, labels, 3, true);
		arma::vec minLeafSet{3, 6, 9};
		arma::vec minSpitSet{0.0001, 0.00001, 0.000001, 0.0000001};
		arma::vec maxDepthSet{2, 3, 4, 5};
		AllDimensionSelect tempSelect = AllDimensionSelect();
		double BestminLeafSet, BestminSpitSet, BestmaxDepthSet;
		std::tie(BestminLeafSet, BestminSpitSet, BestmaxDepthSet) = hpt_dt.Optimize(minLeafSet, minSpitSet, maxDepthSet, Fixed(tempSelect));
		std::cout << BestminLeafSet << "," << BestminSpitSet <<","<< BestmaxDepthSet;
		//BestminLeafSet << 9 << BestminSpitSet << 0.0001 << BestmaxDepthSet << 5;
		*/

		/*const size_t correct = arma::accu(predictions == labels);
		msg += "\nTraining Accuracy: " + std::to_string((double(correct_2) / double(labels.n_elem)));
		msg += "\nKFoldCV Accuracy: " + std::to_string(cvAcc_2);
		msg += "\nPrecision: " + std::to_string(cvPrecision_2);
		msg += "\nRecall: " + std::to_string(cvRecall_2);
		msg += "\nF1: " + std::to_string(cvF1_2);
		std::cout << msg;

		/*adaboost::AdaBoost<DecisionTree<>> adaclf;
		dtclf = DecisionTree<>(dataset, labels, 3);*/
		/*DecisionTree<> wl_3;
		msg = "\nAdaboost-DecisionTree:";
		KFoldCV<adaboost::AdaBoost<DecisionTree<>>, Accuracy> cv_ada_dt_clf(k, dataset, labels, 3, true);
		double cvAcc_5 = cv_ada_dt_clf.Evaluate(wl_3);
		auto rf_5 = cv_ada_dt_clf.Model();
		arma::Row<size_t> predictions_5;
		rf_5.Classify(dataset, predictions_5);
		double cvPrecision_5 = Precision<Binary>::Evaluate(rf_5, dataset, labels);
		double cvRecall_5 = Recall<Binary>::Evaluate(rf_5, dataset, labels);
		double cvF1_5 = F1<Binary>::Evaluate(rf_5, dataset, labels);

		const size_t correct_5 = arma::accu(predictions_5 == labels);
		msg += "\nTraining Accuracy: " + std::to_string((double(correct_5) / double(labels.n_elem)));
		msg += "\nKFoldCV Accuracy: " + std::to_string(cvAcc_5);
		msg += "\nPrecision: " + std::to_string(cvPrecision_5);
		msg += "\nRecall: " + std::to_string(cvRecall_5);
		msg += "\nF1: " + std::to_string(cvF1_5);
		std::cout << msg;*/

		//mlpack::data::Save<adaboost::AdaBoost<DecisionStump<>>>("AdaModel.xml", "AdaModel", rf_2);
		//mlpack::data::Save<RandomForest<GiniGain, RandomDimensionSelect>>("RFModel.xml", "RFModel", rf_1);

		RandomForest<GiniGain, RandomDimensionSelect> clf;
		mlpack::data::Load("RFModel.xml", "RFModel", clf);
		msg = "Reload it : ";
		/*double cvPrecision2 = Precision<Binary>::Evaluate(clf, dataset, labels);
		msg += "\nPrecision: " + std::to_string(cvPrecision2);
		double cvRecall2 = Recall<Binary>::Evaluate(clf, dataset, labels);
		msg += "\nRecall: " + std::to_string(cvRecall2);
		double cvF12 = F1<Binary>::Evaluate(clf, dataset, labels);
		msg += "\nF1: " + std::to_string(cvF12);*/
		std::cout << msg;
		arma::mat sample("94.0458,0.281167,1.02107,0.00181479,0.122677,0.00073466,-18.2187,12.0354,-8.20159,347,162,316,"
			"265,405,151,380,140,316,129,265,136,425,254,566,312,390,349,201,266,3,311,168,348");
		arma::mat probabilities;
		arma::Row<size_t> predictions;
		clf.Classify(sample, predictions, probabilities);
		//arma::u64 result = predictions.at(0);
		//std::cout << "\nClassification result: " << predictions << "\n" << "Probabilities: " <<arma::mean(probabilities.t());
		arma::Mat<double> result = arma::mean(probabilities.t());
		std::cout << typeid(result[0]).name() << std::endl;
		std::cout << result.at(1) << std::endl;
		std::cout << result.at(2) << std::endl;
	}
}


/***************************************************************************
/* Javier Juan Albarracin - jajuaal1@ibime.upv.es                         */
/* Universidad Politecnica de Valencia, Spain                             */
/*                                                                        */
/* Copyright (C) 2018 Javier Juan Albarracin                              */
/*                                                                        */
/***************************************************************************
* Classic image feature extraction algorithms                              *
***************************************************************************/

#define cimg_display 0
#include <CImg.h>
#include <CImgMATLAB.hpp>
#include <EigenMATLAB.hpp>
#include <ScalarMATLAB.hpp>
#include <STDMATLAB.hpp>
#include <vector>
#include <cmath>
#include <omp.h>

#define INF 1e+300

using namespace cimg_library;

class ImageFeatureExtraction
{
public:
	template<typename T>
	static CImg<double> FirstOrderCentralMoments(const CImg<T> &image, const CImg<bool> &mask, const std::vector<int> &radius);
	template<typename T>
	static CImg<double> LocalMedianMAD(const CImg<T> &image, const CImg<bool> &mask, const std::vector<int> &radius);
	template<typename T>
	static CImg<double> LocalEnergy(const CImg<T> &image, const CImg<bool> &mask, const std::vector<int> &radius);
	template<typename T>
	static CImg<double> LocalEntropy(const CImg<T> &image, const CImg<bool> &mask, const std::vector<int> &radius, const int nbins);
}

template<typename T>
CImg<double> ImageFeatureExtraction::FirstOrderCentralMoments(const CImg<T> &image, const CImg<bool> &mask, const std::vector<int> &radius)
{
	CImg<double> F(image.width(), image.height(), image.depth(), 7);

    #pragma omp parallel for
	for (int x = 0; x < image.width(); ++x)
	{
		for (int y = 0; y < image.height(); ++y)
		{
			for (int z = 0; z < image.depth(); ++z)
			{
				for (int f = 0; f < 7; ++f)
					F(x, y, z, f) = 0;

				if (!mask(x, y, z))
					continue;

				// Local Mean
				int n = 0;
				double mu = 0;
				for (int xx = -radius[0]; xx <= radius[0]; ++xx)
				{
					const int xr = x + xx;
					if (xr < 0 || xr >= image.width())
						continue;

					for (int yy = -radius[1]; yy <= radius[1]; ++yy)
					{
						const int yr = y + yy;
						if (yr < 0 || yr >= image.height())
							continue;
						for (int zz = -radius[2]; zz <= radius[2]; ++zz)
						{
							const int zr = z + zz;
							if (zr < 0 || zr >= image.depth())
								continue;

							if (!mask(xr, yr, zr))
								continue;

							mu += image(xr, yr, zr);
							++n;
						}
					}
				}
				mu = n == 0 ? image(x, y, z) : mu / (double) n;

				// Variance, Skewness, Kurtosis, Energy0, Energy1
				n = 0;
				double var = 0;
				double skw = 0;
				double krt = 0;
				double energy0 = 0;
				double energy1 = 0;
				double max = -INF;
				double min = INF;
				for (int xx = -radius[0]; xx <= radius[0]; ++xx)
				{
					const int xr = x + xx;
					if (xr < 0 || xr >= image.width())
						continue;

					for (int yy = -radius[1]; yy <= radius[1]; ++yy)
					{
						const int yr = y + yy;
						if (yr < 0 || yr >= image.height())
							continue;

						for (int zz = -radius[2]; zz <= radius[2]; ++zz)
						{
							const int zr = z + zz;
							if (zr < 0 || zr >= image.depth())
								continue;

							if (!mask(xr, yr, zr))
								continue;

							const double value = image(xr, yr, zr);
							const double diff = value - mu;

							var += (diff * diff);
                            skw += (diff * diff * diff);
                            krt += (diff * diff * diff * diff);
							energy0 += (value * value);
							energy1 += value;
							max = value > max ? value : max;
							min = value < min ? value : min;
							++n;
						}
					}
				}
				F(x, y, z, 0) = mu;
				F(x, y, z, 1) = n <= 1 ? 0 : var / (double) (n - 1);
				F(x, y, z, 2) = n <= 0 || var == 0 ? 0 : (skw / (double) n) / pow(var / (double) n, 1.5);
				F(x, y, z, 3) = n <= 0 || var == 0 ? 0 : ((krt / (double) n) / pow(var / (double) n, 2)) - 3;
				F(x, y, z, 4) = n == 0 ? 0 : energy0 / (double) n;
				F(x, y, z, 5) = n == 0 ? 0 : (energy1 * energy1) / (double) (n * n);
				F(x, y, z, 6) = max - min;
			}
		}
	}
	return F;
}

template<typename T>
CImg<double> ImageFeatureExtraction::LocalEntropy(const CImg<T> &image, const CImg<bool> &mask, const std::vector<int> &radius, const int nbins)
{
	CImg<double> F(image.width(), image.height(), image.depth());
	const double ratio = 256 / (double) nbins;

	#pragma omp parallel for
	for (int x = 0; x < image.width(); ++x)
	{
		for (int y = 0; y < image.height(); ++y)
		{
			for (int z = 0; z < image.depth(); ++z)
			{
				F(x, y, z) = 0;

				if (!mask(x, y, z))
					continue;

				int n = 0;
				std::vector<int> bins(nbins, 0);
				for (int xx = -radius[0]; xx <= radius[0]; ++xx)
				{
					const int xr = x + xx;
					if (xr < 0 || xr >= image.width())
						continue;

					for (int yy = -radius[1]; yy <= radius[1]; ++yy)
					{
						const int yr = y + yy;
						if (yr < 0 || yr >= image.height())
							continue;

						for (int zz = -radius[2]; zz <= radius[2]; ++zz)
						{
							const int zr = z + zz;
							if (zr < 0 || zr >= image.depth())
								continue;

							if (!mask(xr, yr, zr))
								continue;

							bins[std::floor((double) image(xr, yr, zr) / ratio)]++;
							n++;
						}
					}
				}

				if (n == 0)
					continue;

				double H = 0;
				for (int i = 0; i < nbins; i++)
				{
					const double p = bins[i] / (double) n;
					if (p > 0)
						H += p * log(p);
				}
				F(x, y, z) = -H;
			}
		}
	}
	return F;
}

template<typename T>
CImg<double> ImageFeatureExtraction::LocalMedianMAD(const CImg<T> &image, const CImg<bool> &mask, const std::vector<int> &radius)
{
	const int N = (2 * radius[0] + 1) * (2 * radius[1] + 1) * (2 * radius[2] + 1);
	const int mid = (int) std::floor((double) N / 2.0);

	CImg<double> F(image.width(), image.height(), image.depth(), 2);

	#pragma omp parallel for
	for (int x = 0; x < image.width(); ++x)
	{
		for (int y = 0; y < image.height(); ++y)
		{
			for (int z = 0; z < image.depth(); ++z)
			{
				F(x, y, z, 0) = 0;
				F(x, y, z, 1) = 0;

				if (!mask(x, y, z))
					continue;

				std::vector<double> patch;
				for (int xx = -radius[0]; xx <= radius[0]; ++xx)
				{
					const int xr = x + xx;
					if (xr < 0 || xr >= image.width())
					{
						patch.push_back(image(x, y, z));
						continue;
					}

					for (int yy = -radius[1]; yy <= radius[1]; ++yy)
					{
						const int yr = y + yy;
						if (yr < 0 || yr >= image.height())
						{
							patch.push_back(image(x, y, z));
							continue;
						}

						for (int zz = -radius[2]; zz <= radius[2]; ++zz)
						{
							const int zr = z + zz;
							if (zr < 0 || zr >= image.depth())
							{
								patch.push_back(image(x, y, z));
								continue;
							}

							patch.push_back(image(xr, yr, zr));
						}
					}
				}
				// Sort data and get median value
				std::sort(patch.begin(), patch.end());
				double median = patch[mid];
				F(x, y, z, 0) = median;

				// Compute MAD
				std::vector<double> residues;
				for (int l = 0; l < N; l++)
					residues.push_back(std::abs(patch[l] - median));

				// Sort data and get median value
				std::sort(residues.begin(), residues.end());
				F(x, y, z, 1) = residues[mid];
			}
		}
	}
	return F;
}


/******************************************************************************
Created By : Zhang Zhimin
Created On : 2012/11/22
Purpose    :
********************************************************************************/

#pragma once

#include <skynet/config.hpp>
#include <skynet/utility/math.hpp>
#include <skynet/utility/serialization.hpp>
#include <skynet/utility/eigen_lib.hpp>

#include <Eigen/Core>
#include <Eigen/LU>

namespace skynet{namespace statistics{

	template <typename T>
	class gauss_function;

	//template <typename T>
	template <>
	class gauss_function<float> : public unary_function<float, float>{
		static_assert(std::is_floating_point<float>::value, "the normal function template T should be float point type.");
	public:
		typedef float			value_type;
		typedef value_type		point_type;

		gauss_function() {}

		gauss_function(value_type mean, value_type sigma): m_mean(mean), m_sigma(sigma) {
			init();
		}

		void mean(value_type mean)			{ m_mean = mean; init(); }
		value_type mean() const				{ return m_mean; }
		void sigma(value_type sigma)		{ m_sigma = sigma; init(); }
		value_type sigma() const			{ return m_sigma; }
		void covariance(value_type cov)		{ m_sigma = sqrt(cov); init(); }
		value_type covariance() const		{ return sqr(m_sigma); }

		value_type operator()(const value_type & x) const{
			return	exp(-sqr(x-m_mean)/m_2sqr_sigma) / m_sigma_sqrt_2pi;
		}

		template <typename Archive>
		void serialize(Archive &ar, unsigned int){
			ar & boost::serialization::make_nvp("mean", m_mean);
			ar & boost::serialization::make_nvp("sigma", m_sigma);

			init();
		}

	protected:
		void init() {
			m_sigma_sqrt_2pi = m_sigma * sqrt(2 * PI);
			m_2sqr_sigma = 2 * sqr(m_sigma);
		}

	private:
		value_type		m_mean;
		value_type		m_sigma;

		value_type		m_sigma_sqrt_2pi;
		value_type		m_2sqr_sigma;
	};

	template <>
	class gauss_function<double> : public unary_function<double, double>{
		static_assert(std::is_floating_point<double>::value, "the normal function template T should be double point type.");
	public:
		typedef double			value_type;
		typedef value_type		point_type;

		gauss_function(value_type mean, value_type sigma): m_mean(mean), m_sigma(sigma) {
			init();
		}

		void mean(value_type mean)			{ m_mean = mean; init(); }
		value_type mean() const				{ return m_mean; }
		void sigma(value_type sigma)		{ m_sigma = sigma; init(); }
		value_type sigma() const			{ return m_sigma; }
		value_type covariance() const		{ return sqr(m_sigma); }

		value_type operator()(const value_type & x) const{
			return	exp(-sqr(x-m_mean)/m_2sqr_sigma) / m_sigma_sqrt_2pi;
		}

	protected:
		void init() {
			m_sigma_sqrt_2pi = m_sigma * sqrt(2 * PI);
			m_2sqr_sigma = 2 * sqr(m_sigma);
		}

	private:
		value_type		m_mean;
		value_type		m_sigma;

		value_type		m_sigma_sqrt_2pi;
		value_type		m_2sqr_sigma;
	};

	template <>
	class gauss_function<Eigen::VectorXd> : public std::unary_function<Eigen::VectorXd, double>{
	public:
		typedef Eigen::VectorXd			value_type;
		typedef Eigen::MatrixXd			matrix_type;

		gauss_function(){}

		gauss_function(const gauss_function & g): m_mean(g.m_mean), m_covariance(g.m_covariance), m_inv_cov(g.m_inv_cov), m_scale(g.m_scale){}

		void attach(matrix_type  feature_matrix){
			m_mean.resize(feature_matrix.rows());
			m_mean.setZero();
			for (int i = 0; i < feature_matrix.cols(); ++i){
				m_mean += feature_matrix.col(i);
			}

			m_mean /= feature_matrix.cols();

			for (int i = 0; i < feature_matrix.cols(); ++i){
				feature_matrix.col(i) -= m_mean;
			}

			m_covariance = feature_matrix * feature_matrix.transpose();
			m_covariance /= feature_matrix.cols();
			m_inv_cov = m_covariance.inverse();

			m_scale = sqrt(m_covariance.determinant()) * sqrt(std::pow((2*PI), m_mean.rows()));
		}

		result_type	 operator()(const argument_type &x) const{
			auto x_sub_mean = x - m_mean;
			auto dis = -0.5 * (x_sub_mean.transpose() * m_inv_cov * x_sub_mean);
			double dis_d = dis.value();
			return std::exp(dis_d)/m_scale;
		}

		void mean(const value_type &mean)		{ m_mean = mean; }
		value_type mean() const					{ return m_mean; }

		template <typename Archive>
		void serialize(Archive &ar, const unsigned int &){
			 ar & boost::serialization::make_nvp("mean_feature", m_mean);
			 ar & boost::serialization::make_nvp("covariance", m_covariance);
			 ar & boost::serialization::make_nvp("inv_covariance", m_inv_cov);
			 ar & boost::serialization::make_nvp("scale", m_scale);
		}

	private:
		value_type						m_mean;

		matrix_type						m_covariance;
		matrix_type						m_inv_cov;
		double							m_scale;

	};

}}


/**
 * @file OrientationUtils.hpp
 * @author Brahayam Ponton (brahayam.ponton@tuebingen.mpg.de)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-10-08
 */

#pragma once

#include <Eigen/Geometry>

namespace momentumopt {

  /** computes the error between two quaternions by rotating the desired orientation
   *  back by the current orientation, and taking the logarithm of the result.
   */
  Eigen::Vector3d orientationError(const Eigen::Quaternion<double>& desired_orientation,
                                   const Eigen::Quaternion<double>& current_orientation);

  /** computation of the required angular velocity to move from a current orientation
   *  to a desired orientation in a given time interval. Angular velocity can be returned
   *  in body or world coordinates.
   */
  Eigen::Vector3d requiredAngularVelocity(const Eigen::Quaternion<double>& desired_orientation,
                                          const Eigen::Quaternion<double>& current_orientation,
                                          const double& time_step, bool in_body_coordinates=false);

  /** integration of a quaternion given an angular velocity given either in
   *  body or world coordinates and a time interval for integration
   */
  Eigen::Quaternion<double> integrateAngularVelocity(const Eigen::Quaternion<double>& current_orientation,
                                                     const Eigen::Vector3d& angular_velocity,
                                                     const double& time_step, bool in_body_coordinates=false);

}


// Copyright (C) 2017 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (sweeney.chris.m@gmail.com)

#include "theia/sfm/pose/relative_pose_from_two_points_with_known_rotation.h"

#include <Eigen/Core>
#include <Eigen/LU>
#include <glog/logging.h>
#include <vector>

namespace theia {

// We use the epipolar constraint to solve for the unknown relative position:
//
//   p1^t * [t]_x * p2 = 0
//
// By stacking this constraint for both correspondences, we can constraint the 2
// degrees of freedom in our unknown relative position and solve for the
// solution easily.
bool RelativePoseFromTwoPointsWithKnownRotation(
    const Eigen::Vector2d rotated_features1[2],
    const Eigen::Vector2d rotated_features2[2],
    Eigen::Vector3d* relative_position2) {
  CHECK_NOTNULL(relative_position2);

  // The epipolar constraint can be rewritten in terms of the rotated features p
  // an d q as:
  //
  // p2 * q1 * t3 - p2 * t1 - q1 * t2 + q2 * t1 - p1 * q2 * t3 + p1 * t2 = 0
  //
  // Note that p3 and q3 are both 1.0, so they do not appear in this
  // equation. These constraints are stacked together to form a linear system
  // whose null space reveals the relative position.
  Eigen::Matrix<double, 2, 3> epipolar_constraint;
  for (int i = 0; i < 2; i++) {
    epipolar_constraint(i, 0) =
        -rotated_features1[i].y() + rotated_features2[i].y();
    epipolar_constraint(i, 1) =
        -rotated_features2[i].x() + rotated_features1[i].x();
    epipolar_constraint(i, 2) =
        rotated_features1[i].y() * rotated_features2[i].x() -
        rotated_features1[i].x() * rotated_features2[i].y();
  }

  // Compute the null space of this matrix.
  Eigen::FullPivLU<Eigen::Matrix<double, 2, 3> > linear_solver(
      epipolar_constraint);
  // If the null space is greater than 1 dimension then the linear system is
  // ill-conditioned or otherwise degenerate and the relative position cannot be
  // solved for.
  if (linear_solver.dimensionOfKernel() != 1) {
    return false;
  }
  // Extract the relative position as the null space kernel.
  *relative_position2 = linear_solver.kernel().normalized();
  return true;
}

}  // namespace theia


/**
 * @file tests/bayesian_linear_regression_test.cpp
 * @author Clement Mercier
 *
 * Test for BayesianLinearRegression.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */

#include <mlpack/core/data/load.hpp>
#include <mlpack/methods/bayesian_linear_regression/bayesian_linear_regression.hpp>
#include <mlpack/methods/linear_regression/linear_regression.hpp>

#include <boost/test/unit_test.hpp>

using namespace mlpack::regression;
using namespace mlpack::data;

BOOST_AUTO_TEST_SUITE(BayesianLinearRegressionTest);

void GenerateProblem(arma::mat& matX,
                     arma::rowvec& y,
                     size_t nPoints,
                     size_t nDims,
                     float sigma = 0.0)
{
  matX = arma::randn(nDims, nPoints);
  arma::colvec omega = arma::randn(nDims);
  // Compute y and add noise.
  y = omega.t() * matX + arma::randn(nPoints).t() * sigma;
}

// Ensure that predictions are close enough to the target
// for a free noise dataset.
BOOST_AUTO_TEST_CASE(BayesianLinearRegressionRegressionTest)
{
  arma::mat matX;
  arma::rowvec y, predictions;

  GenerateProblem(matX, y, 200, 10);

  // Instanciate and train the estimator.
  BayesianLinearRegression estimator(true);
  estimator.Train(matX, y);
  estimator.Predict(matX, predictions);

  // Check the predictions are close enough to the targets in a free noise case.
  for (size_t i = 0; i < y.size(); i++)
    BOOST_REQUIRE_CLOSE(predictions[i], y[i], 1e-6);

  // Check that the estimated variance is zero.
  BOOST_REQUIRE_SMALL(estimator.Variance(), 1e-6);
}

// Verify centerData and scaleData equal false do not affect the solution.
BOOST_AUTO_TEST_CASE(TestCenter0ScaleData0)
{
  arma::mat matX;
  arma::rowvec y;
  size_t nDims = 30, nPoints = 100;

  GenerateProblem(matX, y, nPoints, nDims, 0.5);

  BayesianLinearRegression estimator(false, false);

  estimator.Train(matX, y);

  // Check dataOffset is empty.
  BOOST_REQUIRE(estimator.DataOffset().n_elem == 0);

  // To be neutral responseOffset must be 0.
  BOOST_REQUIRE(estimator.ResponsesOffset() == 0);

  // Check dataScale is empty.
  BOOST_REQUIRE(estimator.DataScale().n_elem == 0);
}

// Verify that centering and normalization are correct.
BOOST_AUTO_TEST_CASE(TestCenterDataTrueScaleDataTrue)
{
  arma::mat matX;
  arma::rowvec y;
  size_t nDims = 5, nPoints = 100;
  GenerateProblem(matX, y, nPoints, nDims, 0.5);

  BayesianLinearRegression estimator(true, true);
  estimator.Train(matX, y);

  arma::colvec xMean = arma::mean(matX, 1);
  arma::colvec xStd = arma::stddev(matX, 0, 1);
  double yMean = arma::mean(y);

  BOOST_REQUIRE_SMALL((double) abs(sum(estimator.DataOffset() - xMean)), 1e-6);
  BOOST_REQUIRE_SMALL((double) abs(sum(estimator.DataScale() - xStd)), 1e-6);
  BOOST_REQUIRE_CLOSE(estimator.ResponsesOffset(), yMean, 1e-6);
}

// Make sure a model with center ans scale option set is different than a model
// without it set.
BOOST_AUTO_TEST_CASE(OptionsMakeModelDifferent)
{
  arma::mat matX;
  arma::rowvec y;
  size_t nDims = 10, nPoints = 100;
  GenerateProblem(matX, y, nPoints, nDims, 0.5);

  BayesianLinearRegression blr(false, false), blrC(true, false),
      blrCS(true, true);

  blr.Train(matX, y);
  blrC.Train(matX, y);
  blrCS.Train(matX, y);

  for (size_t i = 0; i < nDims; ++i)
    BOOST_REQUIRE((blr.Omega()(i) != blrC.Omega()(i)) &&
                  (blr.Omega()(i) != blrCS.Omega()(i)) &&
                  (blrC.Omega()(i) != blrCS.Omega()(i)));
}

// Check that Train() does not fail with two colinear vectors.
BOOST_AUTO_TEST_CASE(SingularMatix)
{
  arma::mat matX;
  arma::rowvec y;

  GenerateProblem(matX, y, 200, 10);
  // Now the first and the second rows are indentical.
  matX.row(1) = matX.row(0);

  BayesianLinearRegression estimator;
  estimator.Train(matX, y);
}

// Check that std are well computed/coherent. At least higher than the
// estimated predictive variance.
BOOST_AUTO_TEST_CASE(PredictiveUncertainties)
{
  arma::mat matX;
  arma::rowvec y;

  GenerateProblem(matX, y, 100, 10, 1);

  BayesianLinearRegression estimator(true, true);
  estimator.Train(matX, y);

  arma::rowvec responses, std;
  estimator.Predict(matX, responses, std);
  const double estStd = sqrt(estimator.Variance());

  for (size_t i = 0; i < matX.n_cols; i++)
    BOOST_REQUIRE_GT(std[i], estStd);

  // Check that the estimated variance is close to 1.
  BOOST_REQUIRE_CLOSE(estStd, 1, 30);
}

// Check the solution is equal to the classical ridge.
BOOST_AUTO_TEST_CASE(EqualtoRidge)
{
  arma::mat matX;
  arma::rowvec y, blrPred, ridgePred;

  size_t trial = 0;
  for ( ; trial < 3; ++trial)
  {
    GenerateProblem(matX, y, 100, 10, 1);

    BayesianLinearRegression blr(false, false);
    blr.Train(matX, y);

    LinearRegression ridge(matX, y, blr.Alpha() / blr.Beta(), false);

    blr.Predict(matX, blrPred);
    ridge.Predict(matX, ridgePred);

    // If the predictions seem far off, just try again.
    if (arma::norm(blrPred - ridgePred) > 1e-5)
      continue;

    // Check the predictions are close enough between ridge and our blr.
    for (size_t i = 0; i < y.size(); ++i)
      BOOST_REQUIRE_CLOSE(blrPred[i], ridgePred[i], 1);

    // Exit once a test case has completed.
    break;
  }

  BOOST_REQUIRE_LT(trial, 3);
}

BOOST_AUTO_TEST_SUITE_END();


#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/seq.hpp>
#include <cstddef>
#include "libs/numeric/ublasx/test/utils.hpp"

namespace ublas = ::boost::numeric::ublas;
namespace ublasx = ::boost::numeric::ublasx;

static const std::size_t tol = 1.0e-5;

BOOST_UBLASX_TEST_DEF( test_range )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Range");

	typedef double value_type;
	typedef ublas::vector<value_type> vector_type;

	const std::size_t n(3);

	vector_type res;
	vector_type expect_res;

	res = ublasx::seq(5, 5+n);
	expect_res = vector_type(n);
	expect_res(0) = 5;
	expect_res(1) = 6;
	expect_res(2) = 7;

	BOOST_UBLASX_DEBUG_TRACE( "res = " << res );
	BOOST_UBLASX_DEBUG_TRACE( "expect res = " << expect_res );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect_res, n, tol );
}

BOOST_UBLASX_TEST_DEF( test_slice_incr )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Slice - Increasing");

	typedef double value_type;
	typedef ublas::vector<value_type> vector_type;

	const std::size_t n(9);

	vector_type res;
	vector_type expect_res;

	res = ublasx::seq(4, 2, n);
	expect_res = vector_type(n);
	std::size_t v(4);
	for (std::size_t i = 0; i < n; ++i)
	{
		expect_res(i) = v;
		v += 2;
	}

	BOOST_UBLASX_DEBUG_TRACE( "res = " << res );
	BOOST_UBLASX_DEBUG_TRACE( "expect res = " << expect_res );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect_res, n, tol );
}

BOOST_UBLASX_TEST_DEF( test_slice_decr )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: Slice - Decreasing");

	typedef double value_type;
	typedef ublas::vector<value_type> vector_type;

	const std::size_t n(9);

	vector_type res;
	vector_type expect_res;

	res = ublasx::seq(4, -2, n);
	expect_res = vector_type(n);
	short v(4);
	for (std::size_t i = 0; i < n; ++i)
	{
		expect_res(i) = v;
		v -= 2;
	}

	BOOST_UBLASX_DEBUG_TRACE( "res = " << res );
	BOOST_UBLASX_DEBUG_TRACE( "expect res = " << expect_res );
	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect_res, n, tol );
}

int main()
{
	BOOST_UBLASX_DEBUG_TRACE("Test Suite: 'seq' operation");

	BOOST_UBLASX_TEST_BEGIN();

	BOOST_UBLASX_TEST_DO( test_range );
	BOOST_UBLASX_TEST_DO( test_slice_incr );
	BOOST_UBLASX_TEST_DO( test_slice_decr );

	BOOST_UBLASX_TEST_END();
}


#define BOOST_TEST_MODULE test_utils

#include <boost/test/unit_test.hpp>
#include <Utils/utils.h>

BOOST_AUTO_TEST_SUITE(utils_boost)

    BOOST_AUTO_TEST_CASE(hello_world) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double f = 0.1;
        auto sum = add(f, 10);

        auto product = f * 10;

        BOOST_TEST_MESSAGE(" - sum: " << sum);
        BOOST_TEST_MESSAGE(" - product: " << product);
        BOOST_TEST_MESSAGE(" - diff " << sum - product);
        BOOST_TEST(sum == product, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(hello_world2) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double f = 0.1;
        auto sum = add(f, 10);

        auto product = f * 10;

        BOOST_TEST_MESSAGE(" - sum: " << sum);
        BOOST_TEST_MESSAGE(" - product: " << product);
        BOOST_TEST_MESSAGE(" - diff " << sum - product);
        BOOST_TEST(sum == product, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(annual_cap1) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 100;
        double annual_rate = 10.0 / 100;
        int number_of_years = 2;
        double theoretical_value = 121; // (100*(1.1)^2)

        auto calculated_value = annual_capitalization(amount, annual_rate, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_capitalization: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(period_cap1) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 100;
        double annual_rate = 10.0 / 100;
        int periods_per_year = 2;
        int number_of_years = 1;
        double theoretical_value = 110.25; // (100*(1.0.5)^2)

        auto calculated_value = period_capitalization(amount, annual_rate, periods_per_year, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_capitalization: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(continuous_cap1) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double amount = 100;
        double annual_rate = 10.0 / 100;
        int number_of_years = 2;
        double theoretical_value = 122.140275816; // (100*(1.1)^2) rounded to second

        auto calculated_value = continuous_capitalization(amount, annual_rate, number_of_years);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_capitalization: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-8));
    }

    BOOST_AUTO_TEST_CASE(fwd_rate1) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double zero_coupon_total = 0.04;
        int years_total = 2;

        double zero_coupon_partial = 0.03;
        int years_partial = 1;

        double theoretical_value = 0.05; // (100*(1.1)^2) rounded to second

        auto calculated_value = forward_rate(zero_coupon_total, years_total, zero_coupon_partial, years_partial);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_fwd_rate: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(annual_to_cont1) {
            BOOST_TEST_MESSAGE("using tolerances within checks.");

            double annual_rate = 0.12;

            double theoretical_value = 0.1133286853; // ln(1.12)

            auto calculated_value = annual_to_continuous_rate(1, annual_rate);

            BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
            BOOST_TEST_MESSAGE(" - known_fwd_rate: " << theoretical_value);
            BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
            BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-10));
    }

    BOOST_AUTO_TEST_CASE(cont_to_annual) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        double continuous_rate = 0.11332868531; //(e^0.1)-1

        double theoretical_value = 0.12; // (e^0.1)-1

        auto calculated_value = continuous_to_annual_rate(1, continuous_rate);

        BOOST_TEST_MESSAGE(" - calculated_value: " << calculated_value);
        BOOST_TEST_MESSAGE(" - known_fwd_rate: " << theoretical_value);
        BOOST_TEST_MESSAGE(" - diff " << calculated_value - theoretical_value);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-9));
    }

    /*
    BOOST_AUTO_TEST_CASE(actual_count) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

			  const double notional = 100;
				const std::vector<double> CashFlows = {2.99, 2.98, 2.97, 101.60};
				const int sPeriods = 2;
			  const double tPPeriods = 2.0;
				const int mPeriod = 1;

				//Bono_TIR bond = Bono_TIR(notional, CashFlows, sPeriods, tPPeriods, mPeriod);

				//Bono_TIR bond = Bono_TIR();

				double expected_values = 	6.76;
				//double calculated_values = bond(notional, CashFlows, sPeriods, tPPeriods, mPeriod);

				double calculated_values = bond.cbrt(6.76);

        BOOST_TEST_MESSAGE(" - Calculated Value: " << calculated_values);
        BOOST_TEST_MESSAGE(" - Expected Value: " << expected_values);
        BOOST_TEST_MESSAGE(" - Diff " << calculated_values - expected_values);
        BOOST_TEST(expected_values == calculated_values, boost::test_tools::tolerance(1e-15));
    }*/

BOOST_AUTO_TEST_SUITE_END()


#pragma once

// equations based on: https://mste.illinois.edu/regression/directory.html

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/covariance.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>

// alternative iterator implementation
template <typename _InputIter0, typename _InputIter1>
decltype(auto) lineFitting(_InputIter0 begin0, _InputIter0 end0,
                           _InputIter1 begin1) {
    using _ResType = decltype((*begin0) * (*begin1));
    namespace ba = boost::accumulators;

    ba::accumulator_set<
        _ResType, ba::stats<ba::tag::variance,
                            ba::tag::covariance<_ResType, ba::tag::covariate1>>>
        stat_x;

    ba::accumulator_set<_ResType, ba::stats<ba::tag::mean>> stat_y;

    std::for_each(begin0, end0, [&begin1, &stat_x, &stat_y](const _ResType& x) {
        stat_y(*begin1);
        stat_x(x, ba::covariate1 = *begin1++);
    });
    const auto slope = ba::covariance(stat_x) / ba::variance(stat_x);
    return std::make_pair(slope, ba::mean(stat_y) - slope * ba::mean(stat_x));
}