// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_POINT_IN_POLY_CROSSINGS_MULTIPLY_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_POINT_IN_POLY_CROSSINGS_MULTIPLY_HPP

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace within
{

/*!
\brief Within detection using cross counting,
\ingroup strategies
\tparam Point \tparam_point
\tparam PointOfSegment \tparam_segment_point
\tparam CalculationType \tparam_calculation
\see http://tog.acm.org/resources/GraphicsGems/gemsiv/ptpoly_haines/ptinpoly.c
\note Does NOT work correctly for point ON border
\qbk{
[heading See also]
[link geometry.reference.algorithms.within.within_3_with_strategy within (with strategy)]
}
 */

template
<
    typename Point,
    typename PointOfSegment = Point,
    typename CalculationType = void
>
class crossings_multiply
{
    typedef typename select_calculation_type
        <
            Point,
            PointOfSegment,
            CalculationType
        >::type calculation_type;

    class flags
    {
        bool inside_flag;
        bool first;
        bool yflag0;

    public :

        friend class crossings_multiply;

        inline flags()
            : inside_flag(false)
            , first(true)
            , yflag0(false)
        {}
    };

public :

    typedef Point point_type;
    typedef PointOfSegment segment_point_type;
    typedef flags state_type;

    static inline bool apply(Point const& point,
            PointOfSegment const& seg1, PointOfSegment const& seg2,
            flags& state)
    {
        calculation_type const tx = get<0>(point);
        calculation_type const ty = get<1>(point);
        calculation_type const x0 = get<0>(seg1);
        calculation_type const y0 = get<1>(seg1);
        calculation_type const x1 = get<0>(seg2);
        calculation_type const y1 = get<1>(seg2);

        if (state.first)
        {
            state.first = false;
            state.yflag0 = y0 >= ty;
        }

        bool yflag1 = y1 >= ty;
        if (state.yflag0 != yflag1)
        {
            if ( ((y1-ty) * (x0-x1) >= (x1-tx) * (y0-y1)) == yflag1 )
            {
                state.inside_flag = ! state.inside_flag;
            }
        }
        state.yflag0 = yflag1;
        return true;
    }

    static inline int result(flags const& state)
    {
        return state.inside_flag ? 1 : -1;
    }
};

}} // namespace strategy::within

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_POINT_IN_POLY_CROSSINGS_MULTIPLY_HPP


/* NMAAnalyzer.cpp
 * -*- coding: utf-8 -*-
 *
 */

#include <boost/test/unit_test.hpp>

// system includes =============================================================

#include <memory>
#include <vector>

// local includes ==============================================================

#include "../src/utils/CSVReader.hpp"
#include "../src/TrajectoryAnalyzer.hpp"
#include "../src/ProteinFile.hpp"
#include "../src/Protein.hpp"

#include "utils/test.hpp"
#include "utils/log.hpp"

BOOST_AUTO_TEST_SUITE(nma_analyzer_test_suite)

BOOST_AUTO_TEST_CASE(kk_matrix_global){
    TEST_MESSAGE("write");

    auto pdb = std::make_shared<PDB>("test/resources/integration/mb_ca.pdb");
    double temperature = 120.0;

    Protein protein(pdb);
    std::vector<std::shared_ptr<ProteinSegment>> protein_segments{
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 1, 151, COMPLETE_PROTEIN))};

    CSVReader csv_reader;
    Eigen::MatrixXd nma_covariance = csv_reader.read_matrix(
        "test/resources/integration/covariance.csv",
        protein.residue_count() * 3);

    TrajectoryAnalyzer analyzer;
    analyzer.analyze(nma_covariance, protein_segments, temperature);

    Eigen::MatrixXd force_constant = protein_segments[0]->force_constant();
    compare_force_constant_matrix(
                force_constant,
                "test/resources/forceconst_nma/force_constant_complete_protein.csv",
                10e-5,
                false,
                false);
}

BOOST_AUTO_TEST_CASE(kk_matrix_segments){
    TEST_MESSAGE("write");

    auto pdb = std::make_shared<PDB>("test/resources/integration/mb_ca.pdb");
    double temperature = 120.0;

    Protein protein(pdb);

    std::vector<std::shared_ptr<ProteinSegment>> protein_segments{
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 1, 20, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 18, 37, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 35, 54, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 52, 71, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 69, 88, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 86, 105, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 103, 122, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 120, 139, COMPLETE_PROTEIN)),
        std::shared_ptr<ProteinSegment>(new ProteinSegment(protein.get_residues(), 132, 151, COMPLETE_PROTEIN))
    };

    CSVReader csv_reader;
    Eigen::MatrixXd nma_covariance = csv_reader.read_matrix(
        "test/resources/integration/covariance.csv",
        protein.residue_count() * 3);

    TrajectoryAnalyzer analyzer;
    analyzer.analyze(nma_covariance, protein_segments, temperature);

    for (size_t i=0; i<9; ++i)
    {
        Eigen::MatrixXd force_constants = protein_segments.at(i)->force_constant();
        std::stringstream ss;
        ss << "test/resources/forceconst_nma/force_constant_segment0" << i+1 << ".csv";
        compare_force_constant_matrix(force_constants, ss.str(), 10e-5, false, false);
    }
}

BOOST_AUTO_TEST_SUITE_END()

// vim: tabstop=8 expandtab shiftwidth=4 softtabstop=4 fenc=utf-8


/*
 * This file is a part of the TChecker project.
 *
 * See files AUTHORS and LICENSE for copyright details.
 *
 */

#if BOOST_VERSION <= 106600
#include <boost/functional/hash.hpp>
#else
#include <boost/container_hash/hash.hpp>
#endif

#include "tchecker/refzg/state.hh"

namespace tchecker {

namespace refzg {

state_t::state_t(tchecker::intrusive_shared_ptr_t<tchecker::shared_vloc_t> const & vloc,
                 tchecker::intrusive_shared_ptr_t<tchecker::shared_intval_t> const & intval,
                 tchecker::intrusive_shared_ptr_t<tchecker::refzg::shared_zone_t> const & zone)
    : tchecker::ta::state_t(vloc, intval), _zone(zone)
{
  assert(_zone.ptr() != nullptr);
}

state_t::state_t(tchecker::ta::state_t const & s, tchecker::intrusive_shared_ptr_t<tchecker::shared_vloc_t> const & vloc,
                 tchecker::intrusive_shared_ptr_t<tchecker::shared_intval_t> const & intval,
                 tchecker::intrusive_shared_ptr_t<tchecker::refzg::shared_zone_t> const & zone)
    : tchecker::ta::state_t(s, vloc, intval), _zone(zone)
{
  assert(_zone.ptr() != nullptr);
}

bool operator==(tchecker::refzg::state_t const & s1, tchecker::refzg::state_t const & s2)
{
  return tchecker::ta::operator==(s1, s2) && (s1.zone() == s2.zone());
}

bool operator!=(tchecker::refzg::state_t const & s1, tchecker::refzg::state_t const & s2) { return !(s1 == s2); }

std::size_t hash_value(tchecker::refzg::state_t const & s)
{
  std::size_t h = tchecker::ta::hash_value(s);
  boost::hash_combine(h, s.zone());
  return h;
}

bool operator<=(tchecker::refzg::state_t const & s1, tchecker::refzg::state_t const & s2)
{
  return tchecker::ta::operator==(s1, s2) && (s1.zone() <= s2.zone());
}

bool sync_alu_le(tchecker::refzg::state_t const & s1, tchecker::refzg::state_t const & s2,
                 tchecker::clockbounds::map_t const & l, tchecker::clockbounds::map_t const & u)
{
  return tchecker::ta::operator==(s1, s2) && s1.zone().sync_alu_le(s2.zone(), l, u);
}

int lexical_cmp(tchecker::refzg::state_t const & s1, tchecker::refzg::state_t const & s2)
{
  int ta_cmp = tchecker::ta::lexical_cmp(s1, s2);
  if (ta_cmp != 0)
    return ta_cmp;
  return s1.zone().lexical_cmp(s2.zone());
}

} // end of namespace refzg

} // end of namespace tchecker


#include <iostream>

#include <CameraMarkerTransform.hpp>

#include <ros/ros.h>

#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <cv_bridge/cv_bridge.h>

#include <Eigen/Geometry>

#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/expressions.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>

#include <vector>

using namespace dls::perception;
using namespace cv;
using namespace cv::aruco;
using namespace gtsam;

typedef typename message_filters::Subscriber<sensor_msgs::Image> ImgSubscriber;
typedef typename std::shared_ptr<ImgSubscriber> ImgSubscriberPtr;

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> MyPolicy;
typedef typename message_filters::Synchronizer<MyPolicy> Synchronizer;
typedef typename std::shared_ptr<Synchronizer> SynchronizerPtr;

typedef Expression<Pose3> Pose3_;

class MultiCameraCalibNode {
public:
    typedef Eigen::Affine3d Transform;
public:
    MultiCameraCalibNode(ros::NodeHandle& nh) :
        nh_(nh),
        x1_asus(Symbol('x', 0)),
        x2_multisense(Symbol('x', 1))
    {

    tf::Matrix3x3 temp_tf_matrix;
    temp_tf_matrix.setRPY(0,60*M_PI/180,0);
    tf::Vector3 temp_tf_vector(0.65,0,0.1);
    tf::Transform temp_tf_transform(temp_tf_matrix,temp_tf_vector);
    tf::transformTFToEigen(temp_tf_transform,init_asus);

    tf::Matrix3x3 temp_tf_matrix2;
    temp_tf_matrix2.setRPY(0,30*M_PI/180,0);
    tf::Vector3 temp_tf_vector2(0.75,0,0.1);
    tf::Transform temp_tf_transform2(temp_tf_matrix2,temp_tf_vector2);
    tf::transformTFToEigen(temp_tf_transform2,init_multisense);

    tf::Matrix3x3 temp_tf_matrix3;
    temp_tf_matrix3.setRPY(0,30*M_PI/180,0);
    tf::Vector3 temp_tf_vector3(0.85,0,-0.1);
    tf::Transform temp_tf_transform3(temp_tf_matrix3,temp_tf_vector3);
    tf::transformTFToEigen(temp_tf_transform3,init_pose);

    asus_image_sub.reset(new ImgSubscriber(nh, asus_image_topic, 1));
    multisense_image_sub.reset(new ImgSubscriber(nh, multisense_image_topic, 1));
    sync.reset(new Synchronizer(MyPolicy(10),*asus_image_sub,*multisense_image_sub));
    sync->registerCallback(boost::bind(&MultiCameraCalibNode::camerasCallback,this,_1,_2));
    priorModel = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(1e-6), Vector3::Constant(1e-4)).finished());
    measurementModel = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(1e-6), Vector3::Constant(1e-4)).finished());
    }

    void run(){
        bool tf_ok_asus = false;
        bool tf_ok_multisense = false;
        bool tf_ok_extender=false;

        if(verbose){
            ROS_INFO("Preparing to run the calibration with parameters:");
            ROS_INFO_STREAM("multisense_image_topic: " << multisense_image_topic);
            ROS_INFO_STREAM("multisense_camera_info_topic: " << multisense_camera_info_topic);
            ROS_INFO_STREAM("multisense_camera_frame_name: " << multisense_camera_frame_name);
            ROS_INFO_STREAM("multisense_sensor_frame_name: " << multisense_sensor_frame_name);
            ROS_INFO_STREAM("asus_image_topic: " << asus_image_topic);
            ROS_INFO_STREAM("asus_camera_info_topic: " << asus_camera_info_topic);
            ROS_INFO_STREAM("asus_camera_frame_name: " << asus_camera_frame_name);
            ROS_INFO_STREAM("asus_sensor_frame_name: " << asus_sensor_frame_name);
            ROS_INFO_STREAM("base_frame_name: " << base_frame_name);
            ROS_INFO_STREAM("marker_frame_name" << marker_frame_name);
            ROS_INFO_STREAM("knee_frame_name" << knee_frame_name);
        }

       // cameras_sub_ = nh_.subscribe(multisense_image_topic,10, &MultiCameraCalibNode::multisenseInfoCallback, this);
        multisense_camera_info_sub_ = nh_.subscribe(multisense_camera_info_topic, 10, &MultiCameraCalibNode::multisenseInfoCallback, this);
        asus_camera_info_sub_ = nh_.subscribe(asus_camera_info_topic,10,&MultiCameraCalibNode::asusInfoCallback,this);

        // Don't leave until you get the camera to sensor mount transform!
        while(nh_.ok() && !tf_ok_multisense && !tf_ok_asus && !tf_ok_extender){
                   try{
                       ros::Time now = ros::Time::now();
                       tf_listener_.waitForTransform(asus_sensor_frame_name, asus_camera_frame_name,
                                                     now, ros::Duration(3.0));
                       tf_listener_.lookupTransform(asus_sensor_frame_name, asus_camera_frame_name,
                                                    now, tf_transform_);

                       tf::transformTFToEigen(tf_transform_, sTc_asus);

                       if(verbose){
                           ROS_INFO("Asus Camera to sensor captured:");
                           ROS_INFO_STREAM(sTc_asus.matrix().format(fmt_));
                       }
                       tf_ok_asus = true;

                   }
                   catch (tf::TransformException ex){
                       ROS_ERROR("%s",ex.what());
                       ros::Duration(1.0).sleep();
                   }

            try{
                ros::Time now = ros::Time::now();
                tf_listener_.waitForTransform(multisense_sensor_frame_name, multisense_camera_frame_name,
                                              now, ros::Duration(3.0));
                tf_listener_.lookupTransform(multisense_sensor_frame_name, multisense_camera_frame_name,
                                             now, tf_transform_);

                tf::transformTFToEigen(tf_transform_, sTc_multisense);

                if(verbose){
                    ROS_INFO("Camera to sensor captured:");
                    ROS_INFO_STREAM(sTc_multisense.matrix().format(fmt_));
                }
                tf_ok_multisense = true;

            }
            catch (tf::TransformException ex){
                ROS_ERROR("%s",ex.what());
                ros::Duration(1.0).sleep();
            }

                   try{
                       ros::Time now = ros::Time::now();
                       tf_listener_.waitForTransform(marker_frame_name, knee_frame_name,
                                                     now, ros::Duration(3.0));
                       tf_listener_.lookupTransform(marker_frame_name, knee_frame_name,
                                                    now, tf_transform_);

                       tf::transformTFToEigen(tf_transform_, mTk_);

                       if(verbose){
                           ROS_INFO("Knee to Marker captured:");
                           ROS_INFO_STREAM(mTk_.matrix().format(fmt_));
                       }
                       tf_ok_extender = true;

                   }
                   catch (tf::TransformException ex){
                       ROS_ERROR("%s",ex.what());
                       ros::Duration(1.0).sleep();
                   }
               }

        // don't leave until I say so!
        while(nh_.ok() && !stop){
            ros::spinOnce();
        }

                std::cout << " Time to process data and factor graph " << std::endl;

                graph.print("\nFactor Graph:\n");

                Values initials;
                initials.insert(Symbol('x', 0), Pose3(Rot3(init_asus.rotation()),init_asus.translation()));
                initials.insert(Symbol('x', 1), Pose3(Rot3(init_multisense.rotation()),init_multisense.translation()));

                for (int i=2;i<=points_number;i++)
                             {
                                 std::cout << "init: " << i << std::endl;
                                 initials.insert(Symbol('x',i),Pose3(Rot3(init_pose.rotation()),init_pose.translation()));
                             }

                GaussNewtonParams parameters;
                parameters.setVerbosity("ERROR");

                // optimize!
                 GaussNewtonOptimizer optimizer(graph, initials, parameters);
                 Values results = optimizer.optimize();

               // print final values
                results.print("Final Result:\n");

//                // Calculate marginal covariances for all poses
//                Marginals marginals(graph, results);

//                // print marginal covariances
//                std::cout << "x1 covariance:\n" << marginals.marginalCovariance(Symbol('x', 1)) << std::endl;
//                std::cout << "x2 covariance:\n" << marginals.marginalCovariance(Symbol('x', 2)) << std::endl;

    }

    void camerasCallback(const sensor_msgs::ImageConstPtr& image_asus,
                         const sensor_msgs::ImageConstPtr& image_multisense)
    {
        std::cout << "msg received" << std::endl;

        cv_bridge::CvImageConstPtr asus_temp_image = cv_bridge::toCvShare(image_asus, "mono8");
        Mat asus_frame = asus_temp_image->image;

        cv_bridge::CvImageConstPtr multisense_temp_image = cv_bridge::toCvShare(image_multisense, "mono8");
        Mat multisense_frame = multisense_temp_image->image;
            if(debug){
                std::cout << "ASUS image: " << asus_frame.rows
                          << " rows and " << asus_frame.cols << " cols" << std::endl;
                std::cout << "Multisense image: " << multisense_frame.rows
                          << " rows and " << multisense_frame.cols << " cols" << std::endl;
            }
            std::cout << "\nAsus looks for the image ... " <<std::endl;
            if(!asus_camera_marker_transf_.getCameraMarkerTransform(asus_frame,cTm_asus))
            {
                return;
            }
            std::cout << "\nMultisense looks for the image ... " << std::endl;
            if(!multisense_camera_marker_transf_.getCameraMarkerTransform(multisense_frame,cTm_multisense))
            {
                return;
            }

            ros::Time now_asus = image_asus->header.stamp;
            ros::Time now_multisese = image_multisense->header.stamp;
            ros::Time now  = ros::Time(0);
            if(1)
            {
                std::cout << "\nasus_stamp - multisense_stamp = " << now_asus-now_multisese << std::endl;
                std::cout << " asus_stamp " << now_asus << " multisese_stamp " << now_multisese << " now " << now << std::endl;
            }

            try{
                       tf_listener_.waitForTransform(base_frame_name, knee_frame_name,
                                                    now_asus, ros::Duration(3.0));
                       tf_listener_.lookupTransform(base_frame_name, knee_frame_name,
                                                   now_asus, tf_transform_);

                       tf::transformTFToEigen(tf_transform_, bTk_);

                   }
                   catch (tf::TransformException ex)
                   {
                       ROS_ERROR("%s",ex.what());
                       ros::Duration(1.0).sleep();
                       std::cout << " DATA SKIPPED " << std::endl;
                       return;
                   }

    factor_dot_asus = bTk_ * mTk_.inverse() * cTm_asus.inverse()*sTc_asus.inverse();
    factor_dot_multisense = bTk_ * mTk_.inverse() * cTm_multisense.inverse()*sTc_multisense.inverse();
    Transform asusTmulti=sTc_asus*cTm_asus*cTm_multisense.inverse()*sTc_multisense.inverse();

    Transform pose_knee = bTk_ * mTk_.inverse();
    Transform sTm_asus = sTc_asus * cTm_asus;
    Transform sTm_multisense = sTc_multisense * cTm_multisense;

    if (verbose)
           {
           std::cout << " \n<------------------DOT FACTORS --------------->" << std::endl;
           std::cout << " sTc_asus " << std::endl;
           std::cout << sTc_asus.matrix().format(fmt_) << std::endl << std::endl;
           std::cout << " sTc_multisense " << std::endl;
           std::cout << sTc_multisense.matrix().format(fmt_) << std::endl << std::endl;
           std::cout << " cTm_asus " << std::endl;
           std::cout << cTm_asus.matrix().format(fmt_) << std::endl << std::endl;
           std::cout << " cTm_multisense " << std::endl;
           std::cout << cTm_multisense.matrix().format(fmt_) << std::endl << std::endl;
           std::cout << " mTk_ " << std::endl;
           std::cout << mTk_.matrix().format(fmt_) << std::endl << std::endl;
           std::cout << " bTk_ " << std::endl;
           std::cout << bTk_.matrix().format(fmt_) << std::endl << std::endl;
           std::cout << " factor_dot_asus " << std::endl;
           std::cout << factor_dot_asus.matrix().format(fmt_) << std::endl << std::endl;
           std::cout << " factor_dot_multisense " << std::endl;
           std::cout << factor_dot_multisense.matrix().format(fmt_) << std::endl << std::endl;
           std::cout << " relative transform asus - multisense " << std::endl;
           std::cout << asusTmulti.matrix().format(fmt_) << std::endl << std::endl;
           }

    if (count <= points_number)
       {
   Pose3_ foot_pose(Symbol('x', count));

   graph.addExpressionFactor(foot_pose,
                                Pose3(Rot3(pose_knee.rotation()),
                                      pose_knee.translation()),
                                priorModel);

   graph.addExpressionFactor(between(foot_pose,x1_asus),
                         Pose3(Rot3( sTm_asus.inverse().rotation()),
                               sTm_asus.inverse().translation()),
                               measurementModel);

   graph.addExpressionFactor(between(foot_pose,x2_multisense),
                         Pose3(Rot3( sTm_multisense.inverse().rotation()),
                               sTm_multisense.inverse().translation()),
                               measurementModel);

     graph.addExpressionFactor(between(x1_asus,x2_multisense),
                            Pose3(Rot3(asusTmulti.rotation()),asusTmulti.translation()),
                            measurementModel);

    }

//    graph.addExpressionFactor(x1_asus,
//                              Pose3(Rot3(factor_dot_asus.rotation()),factor_dot_asus.translation()),
//                              priorModel);
//    graph.addExpressionFactor(x2_multisense,
//                              Pose3(Rot3(factor_dot_multisense.rotation()),factor_dot_multisense.translation()),
//                              priorModel);

//    graph.addExpressionFactor(between(x1_asus,x2_multisense),
//                          Pose3(Rot3(asusTmulti.rotation()),asusTmulti.translation()),
//                          measurementModel);

         if (count >= points_number)
         {
             std::cout << " Accumulated point exceeded, stop = true " << std::endl;
             stop=true;
         }

         count++;

         if (1)
         {
             std::cout << "Number of factors accumulated " << count << std::endl;
         }

    }

    void multisenseInfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info_msg){
        if(verbose){
            std::cout << "Received camera info: " << std::endl;
            std::cout << "-- distortion model: " << camera_info_msg->distortion_model << std::endl;
            std::cout << "-- coefficients: (k1, k2, t1, t2, k3): ";
        }
        for(int i = 0; i < 5; i++){
            std::cout << camera_info_msg->D[i]  << " ";
        }
        std::cout << std::endl;

        std::cout << camera_info_msg->D.size() << std::endl;

        if(!camera_info_msg->distortion_model.compare("plumb_bob") == 0){
            std::cout << "Sorry, only plumb_bob distortion model is supported" << std::endl;
            return;
        }

        Matx33d cameraMatrix;
        for(int i = 0; i < 9; i++) {
            cameraMatrix(i / 3, i % 3) = camera_info_msg->K[i];
        }

        cv::Vec<double,5> distCoeff;
        for(int i = 0; i < 5; i++) {
            distCoeff(i) = (double)camera_info_msg->D[i];
        }

        if(verbose){
            std::cout << "-- Receiving Multisense parameters" << std::endl;
            std::cout << "   Camera Matrix: " << std::endl << cameraMatrix << std::endl;
            std::cout << std::endl;
            std::cout << "   Distortion coefficients: " << std::endl << distCoeff;
            std::cout << std::endl;
        }
        multisense_camera_marker_transf_.setCameraParameters(cameraMatrix, distCoeff);

        // we need the parameters only once.
        multisense_camera_info_sub_.shutdown();
    }

    void asusInfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info_msg){
        if(verbose){
            std::cout << "Received camera info: " << std::endl;
            std::cout << "-- distortion model: " << camera_info_msg->distortion_model << std::endl;
            std::cout << "-- coefficients: (k1, k2, t1, t2, k3): ";
        }
        for(int i = 0; i < 5; i++){
            std::cout << camera_info_msg->D[i]  << " ";
        }
        std::cout << std::endl;

        std::cout << camera_info_msg->D.size() << std::endl;

        if(!camera_info_msg->distortion_model.compare("plumb_bob") == 0){
            std::cout << "Sorry, only plumb_bob distortion model is supported" << std::endl;
            return;
        }

        Matx33d cameraMatrix;
        for(int i = 0; i < 9; i++) {
            cameraMatrix(i / 3, i % 3) = camera_info_msg->K[i];
        }

        cv::Vec<double,5> distCoeff;
        for(int i = 0; i < 5; i++) {
            distCoeff(i) = (double)camera_info_msg->D[i];
        }

        if(verbose){
            std::cout << "-- Receiving Asus parameters" << std::endl;
            std::cout << "   Camera Matrix: " << std::endl << cameraMatrix << std::endl;
            std::cout << std::endl;
            std::cout << "   Distortion coefficients: " << std::endl << distCoeff;
            std::cout << std::endl;
        }
        asus_camera_marker_transf_.setCameraParameters(cameraMatrix, distCoeff);

        // we need the parameters only once.
        asus_camera_info_sub_.shutdown();
    }

private:
    ros::NodeHandle nh_;
    CameraMarkerTransform multisense_camera_marker_transf_;
    CameraMarkerTransform asus_camera_marker_transf_;

    ImgSubscriberPtr asus_image_sub;
    ImgSubscriberPtr multisense_image_sub;

    ros::Subscriber cameras_sub_;
    ros::Subscriber multisense_camera_info_sub_;
    ros::Subscriber asus_camera_info_sub_;

    SynchronizerPtr sync;

    tf::TransformListener tf_listener_;
    tf::StampedTransform tf_transform_;

    Transform cTm_asus = Transform::Identity();
    Transform cTm_multisense = Transform::Identity();
    Transform bTk_ = Transform::Identity(); // from base frame to knee frame
    Transform mTk_ = Transform::Identity(); // from knee frame to marker frame
    Transform sTc_asus = Transform::Identity();
    Transform sTc_multisense = Transform::Identity();

    Transform factor_dot_asus=Transform::Identity();
    Transform factor_dot_multisense=Transform::Identity();

    Transform pose_asus=Transform::Identity();
    Transform pose_multisense=Transform::Identity();

    Transform init_asus=Transform::Identity();
    Transform init_multisense=Transform::Identity();
    Transform init_pose = Transform::Identity();

    Eigen::IOFormat fmt_ = Eigen::IOFormat(Eigen::FullPrecision, 0, "\t ", "\n", "[", "]");

    bool stop = false;

    int count=2;

    ExpressionFactorGraph graph;
    noiseModel::Diagonal::shared_ptr measurementModel;
    noiseModel::Diagonal::shared_ptr priorModel;

    Pose3_ x2_multisense,x1_asus;

public:
    std::string multisense_image_topic = "/multisense/left/image_mono";
    std::string multisense_camera_info_topic = "/multisense/left/image_mono/camera_info";
    std::string asus_image_topic="/asus/rgb/image_raw";
    std::string asus_camera_info_topic = "/asus/rgb/camera_info";

    std::string base_frame_name = "/base_link";
    std::string multisense_camera_frame_name = "/multisense/left_camera_optical_frame";
    std::string multisense_sensor_frame_name = "/head";
    std::string asus_camera_frame_name = "/asus_rgb_optical_frame";
    std::string asus_sensor_frame_name = "/asus_mount_link";
    std::string marker_frame_name = "/ext_calibration_plate_head";
    std::string knee_frame_name = "/lf_lowerleg";

    int points_number = 100;

    bool debug = false;
    bool verbose = false;
};

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

    std::string multisense_image_topic = "/multisense/left/image_mono";
    std::string multisense_camera_info_topic = "/multisense/left/image_mono/camera_info";
    std::string asus_image_topic="/asus/rgb/image_raw";
    std::string asus_camera_info_topic = "/asus/rgb/camera_info";

    std::string multisense_camera_frame_name = "/multisense/left_camera_optical_frame";
    std::string multisense_sensor_frame_name = "/head";
    std::string asus_camera_frame_name = "/asus_rgb_optical_frame";
    std::string asus_sensor_frame_name = "/asus_mount_link";
    std::string knee_frame_name = "/lf_lowerleg";

    bool verbose = true;
    int points_number=100;

    // getting parameters from outside (e.g., launch file)
    nh.getParam("multisense_image_topic", multisense_image_topic);
    nh.getParam("multisense_camera_info_topic", multisense_camera_info_topic);
    nh.getParam("asus_image_topic", asus_image_topic);
    nh.getParam("asus_camera_info_topic", asus_camera_info_topic);

    nh.getParam("multisense_camera_frame_name", multisense_camera_frame_name);
    nh.getParam("multisense_sensor_frame_name",multisense_sensor_frame_name);
    nh.getParam("asus_camera_frame_name", asus_camera_frame_name);
    nh.getParam("asus_sensor_frame_name",asus_sensor_frame_name);

    nh.getParam("verbose", verbose);
    nh.getParam("points_number",points_number);

    calib_node.multisense_image_topic = multisense_image_topic;
    calib_node.multisense_camera_info_topic = multisense_camera_info_topic;
    calib_node.multisense_camera_frame_name = multisense_camera_frame_name;
    calib_node.multisense_sensor_frame_name = multisense_sensor_frame_name;

    calib_node.asus_image_topic = asus_image_topic;
    calib_node.asus_camera_info_topic = asus_camera_info_topic;
    calib_node.asus_camera_frame_name = asus_camera_frame_name;
    calib_node.asus_sensor_frame_name = asus_sensor_frame_name;

    calib_node.points_number = points_number;

    calib_node.verbose = verbose;

    calib_node.run();
    return 0;
}


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

////////////////////////////////////////////////////////////////////////////////
// Meta-function that creates a standard real type from a size in bytes.
// As all make_xxx, also provides an optional lambda to apply to the result.
// See: http://nt2.metascale.org/sdk/meta/traits/make_floating.html
////////////////////////////////////////////////////////////////////////////////
#include <boost/mpl/apply.hpp>
#include <boost/dispatch/meta/na.hpp>

namespace boost { namespace dispatch { namespace meta
{
  template<std::size_t Size, class Transform = na_>
  struct  make_floating
        : boost::mpl::apply<Transform,float> {};

  template<> struct  make_floating<sizeof(double) , na_ > { typedef double  type; };
  template<> struct  make_floating<sizeof(float)  , na_ > { typedef float   type; };

  template<class Transform>
  struct  make_floating<sizeof(double),Transform>
        : boost::mpl::apply<Transform,double> {};

  template<class Transform>
  struct  make_floating<sizeof(float),Transform>
        : boost::mpl::apply<Transform,float> {};
} } }

#endif


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/algorithm/string.hpp>
#include <ored/utilities/correlationmatrix.hpp>
#include <ored/utilities/parsers.hpp>
#include <ql/quotes/derivedquote.hpp>
#include <ql/quotes/simplequote.hpp>

using namespace QuantLib;
using namespace std;

namespace CT = QuantExt::CrossAssetModelTypes;

namespace {

string invertFx(const string& ccyPair) {
    QL_REQUIRE(ccyPair.size() == 6, "invertFx: Expected currency pair to be 6 characters but got: " << ccyPair);
    return ccyPair.substr(3, 3) + ccyPair.substr(0, 3);
}

}

namespace ore {
namespace data {

bool operator<(const CorrelationFactor& lhs, const CorrelationFactor& rhs) {
    return tie(lhs.type, lhs.name, lhs.index) < tie(rhs.type, rhs.name, rhs.index);
}

bool operator==(const CorrelationFactor& lhs, const CorrelationFactor& rhs) {
    return lhs.type == rhs.type && lhs.name == rhs.name && lhs.index == rhs.index;
}

bool operator!=(const CorrelationFactor& lhs, const CorrelationFactor& rhs) {
    return !(lhs == rhs);
}

ostream& operator<<(ostream& out, const CorrelationFactor& f) {
    return out << "{" << f.type << "," << f.name << "," << f.index << "}";
}

CorrelationFactor parseCorrelationFactor(const string& name) {

    Size pos = name.find(':');
    QL_REQUIRE(pos != string::npos, "Expected the factor to be of the form 'type:name'");

    CT::AssetType factorType = parseCamAssetType(name.substr(0, pos));
    string factorName = name.substr(pos + 1);

    return { factorType, factorName, 0 };
}

void CorrelationMatrixBuilder::reset() {
    corrs_.clear();
}

void CorrelationMatrixBuilder::addCorrelation(const string& factor1, const string& factor2, Real correlation) {
    CorrelationFactor f_1 = parseCorrelationFactor(factor1);
    CorrelationFactor f_2 = parseCorrelationFactor(factor2);
    addCorrelation(f_1, f_2, Handle<Quote>(boost::make_shared<SimpleQuote>(correlation)));
}

void CorrelationMatrixBuilder::addCorrelation(const string& factor1, const string& factor2,
    const Handle<Quote>& correlation) {
    CorrelationFactor f_1 = parseCorrelationFactor(factor1);
    CorrelationFactor f_2 = parseCorrelationFactor(factor2);
    addCorrelation(f_1, f_2, correlation);
}

void CorrelationMatrixBuilder::addCorrelation(const CorrelationFactor& f_1,
    const CorrelationFactor& f_2, Real correlation) {
    addCorrelation(f_1, f_2, Handle<Quote>(boost::make_shared<SimpleQuote>(correlation)));
}

void CorrelationMatrixBuilder::addCorrelation(const CorrelationFactor& f_1, const CorrelationFactor& f_2,
    const Handle<Quote>& correlation) {

    // Check the factors
    checkFactor(f_1);
    checkFactor(f_2);

    // Store the correlation.
    CorrelationKey ck = createKey(f_1, f_2);
    QL_REQUIRE(corrs_.find(ck) == corrs_.end(), "Correlation for key [" <<
        ck.first << "," << ck.second << "] already set");
    QL_REQUIRE(correlation->value() >= -1.0 && correlation->value() <= 1.0, "Correlation value, " <<
        correlation->value() << ", for key [" << ck.first << "," << ck.second << "] should be in [-1.0,1.0]");
    corrs_[ck] = correlation;
    DLOG("Added correlation: (" << f_1 << "," << f_2 << ") = " << correlation->value() << ".");
}

Disposable<Matrix> CorrelationMatrixBuilder::correlationMatrix(const vector<string>& ccys) {
    ProcessInfo pi = createProcessInfo(ccys);
    return correlationMatrix(pi);
}

Disposable<Matrix> CorrelationMatrixBuilder::correlationMatrix(const vector<string>& ccys,
    const vector<string>& infIndices) {
    ProcessInfo pi = createProcessInfo(ccys, infIndices);
    return correlationMatrix(pi);
}

Disposable<Matrix> CorrelationMatrixBuilder::correlationMatrix(const vector<string>& ccys,
    const vector<string>& infIndices, const vector<string>& names) {
    ProcessInfo pi = createProcessInfo(ccys, infIndices, names);
    return correlationMatrix(pi);
}

Disposable<Matrix> CorrelationMatrixBuilder::correlationMatrix(const vector<string>& ccys,
    const vector<string>& infIndices, const vector<string>& names, const vector<string>& equities) {
    ProcessInfo pi = createProcessInfo(ccys, infIndices, names, equities);
    return correlationMatrix(pi);
}

Disposable<Matrix> CorrelationMatrixBuilder::correlationMatrix(const ProcessInfo& processInfo) {

    // Check that all IR processes are currency codes and that we have at least one currency
    auto outerIt = processInfo.find(CT::IR);
    QL_REQUIRE(outerIt != processInfo.end() && !outerIt->second.empty(),
        "Need at least one currency to build correlation matrix.");

    for (const auto& p : outerIt->second) {
        QL_REQUIRE(p.first.size() == 3, "Invalid ccy code " << p.first);
    }

    // Get the dimension of the matrix to create and create a list of factors.
    Size dim = 0;
    vector<CorrelationFactor> factors;
    for (const auto& kv : processInfo) {
        for (const auto& p : kv.second) {

            // Don't allow multiple factors for FX for now. Need to check later the FX inversion in the lookup below
            // if we want to extend the builder to multiple factors for each FX process.
            if (kv.first == CT::FX) {
                QL_REQUIRE(p.second == 1, "CorrelationMatrixBuilder does not support multiple factors for FX. " <<
                    p.first << " is set up with " << p.second << " factors.");
            }

            dim += p.second;
            for (Size i = 0; i < p.second; ++i) {
                factors.push_back({ kv.first, p.first, i });
            }
        }
    }

    // Start with the identity matrix
    Matrix corr(dim, dim, 0.0);
    for (Size i = 0; i < dim; i++)
        corr[i][i] = 1.0;

    // Populate all of the off-diagonal elements
    for (Size i = 0; i < dim; ++i) {
        for (Size j = 0; j < i; ++j) {
            corr[i][j] = corr[j][i] = getCorrelation(factors[i], factors[j])->value();
        }
    }

    return corr;
}

CorrelationMatrixBuilder::ProcessInfo CorrelationMatrixBuilder::createProcessInfo(
    const vector<string>& ccys,
    const std::vector<std::string>& inflationIndices,
    const std::vector<std::string>& creditNames,
    const std::vector<std::string>& equityNames) {

    // Check the currencies.
    QL_REQUIRE(!ccys.empty(), "At least one currency required to build correlation matrix");
    for (const string& ccy : ccys) {
        QL_REQUIRE(ccy.size() == 3, "Invalid currency code " << ccy);
    }

    // Hold the resulting process information.
    // Supporting a legacy method, assumed that there is 1 factor per process.
    ProcessInfo result;

    // Add process information for each currency.
    for (const string& ccy : ccys) {
        result[CT::IR].emplace_back(ccy, 1);
    }

    // Add process information for each FX pair.
    for (Size i = 1; i < ccys.size(); ++i) {
        string ccyPair = ccys[i] + ccys[0];
        result[CT::FX].emplace_back(ccyPair, 1);
    }

    // Add process information for inflation indices.
    for (const string& inflationIndex : inflationIndices) {
        result[CT::INF].emplace_back(inflationIndex, 1);
    }

    // Add process information for credit names.
    for (const string& creditName : creditNames) {
        result[CT::CR].emplace_back(creditName, 1);
    }

    // Add process information for equity names.
    for (const string& equityName : equityNames) {
        result[CT::EQ].emplace_back(equityName, 1);
    }

    return result;
}

void CorrelationMatrixBuilder::checkFactor(const CorrelationFactor& f) const {
    switch (f.type) {
    case CT::IR:
    case CT::AUX:
        QL_REQUIRE(f.name.size() == 3, "Expected IR factor name to be 3 character currency code but got: " << f.name);
        break;
    case CT::FX:
        QL_REQUIRE(f.name.size() == 6, "Expected FX factor name to be 6 character currency pair but got: " << f.name);
        break;
    case CT::INF:
    case CT::CR:
    case CT::EQ:
        QL_REQUIRE(!f.name.empty(), "Expected non-empty factor name for factor type " << f.type);
        break;
    default:
        QL_FAIL("Did not recognise factor type " << static_cast<int>(f.type) << ".");
    }
}

CorrelationKey CorrelationMatrixBuilder::createKey(const CorrelationFactor& f_1,
    const CorrelationFactor& f_2) const {

    QL_REQUIRE(f_1 != f_2, "Correlation factors must be unique: " << f_1 << ".");

    if (f_1 < f_2)
        return make_pair(f_1, f_2);
    else
        return make_pair(f_2, f_1);
}

Handle<Quote> CorrelationMatrixBuilder::lookup(const string& f1, const string& f2) {
    CorrelationFactor f_1 = parseCorrelationFactor(f1);
    CorrelationFactor f_2 = parseCorrelationFactor(f2);
    return getCorrelation(f_1, f_2);
}

//! Get the correlation between the factor \p f_1 and \p f_2.
Handle<Quote> CorrelationMatrixBuilder::getCorrelation(const CorrelationFactor& f_1,
    const CorrelationFactor& f_2) const {

    // Create key with the correct order for lookup
    CorrelationKey ck = createKey(f_1, f_2);

    // If we have the correlation via direct lookup, return it.
    if (corrs_.find(ck) != corrs_.end())
        return corrs_.at(ck);

    // If one or both of the factors are FX, we may still be able to generate a correlation by using the inverse of
    // a provided FX quote. Note that we have FX restricted to 1 factor in this class above.

    typedef DerivedQuote<negate<Real>> InvQuote;

    // If factor 1 is FX
    if (f_1.type == CT::FX) {
        CorrelationFactor if_1{ CT::FX, invertFx(f_1.name), f_1.index };
        ck = createKey(if_1, f_2);
        auto it = corrs_.find(ck);
        if (it != corrs_.end())
            return Handle<Quote>(boost::make_shared<InvQuote>(it->second, negate<Real>()));
    }

    // If factor 2 is FX
    if (f_2.type == CT::FX) {
        CorrelationFactor if_2{ CT::FX, invertFx(f_2.name), f_2.index };
        ck = createKey(f_1, if_2);
        auto it = corrs_.find(ck);
        if (it != corrs_.end())
            return Handle<Quote>(boost::make_shared<InvQuote>(it->second, negate<Real>()));
    }

    // If factor 1 and factor 2 are both FX
    if (f_1.type == CT::FX && f_2.type == CT::FX) {
        CorrelationFactor if_1{ CT::FX, invertFx(f_1.name), f_1.index };
        CorrelationFactor if_2{ CT::FX, invertFx(f_2.name), f_2.index };
        ck = createKey(if_1, if_2);
        auto it = corrs_.find(ck);
        if (it != corrs_.end())
            return it->second;
    }

    // If we still haven't found anything, return a correlation of 0.
    return Handle<Quote>(boost::make_shared<SimpleQuote>(0.0));
}

const map<CorrelationKey, Handle<Quote>>& CorrelationMatrixBuilder::correlations() {
    return corrs_;
}

} // namespace data
} // namespace ore


#include <boost/icl/closed_interval.hpp>


/*******************************************************************************
 * Copyright (c) 2014, 2016  IBM Corporation and others
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *******************************************************************************/

#ifndef GaussianProcessLight_hpp
#define GaussianProcessLight_hpp

#include <iostream>
#include <limits>
#include <Eigen/Dense>

#include "KernelFunction.hpp"
#include "GaussianProcess.hpp"
#include "SerializeUtils.hpp"

namespace loc{

    class GaussianProcessLight : public GaussianProcess{

    private:
        // variables to be serialized
        std::vector<GaussianProcess> LGPs_;     //Local Gaussian Processes
        std::vector<Eigen::VectorXd> centers_;  //center for each LGP

        double sigmaN_ = 1.0;
        GaussianKernel gaussianKernel_;

    public:
        static const int N_FEATURES = 4;
        constexpr static const double MIN_DENOMINATOR = std::numeric_limits<double>::min() * 1e+16;

        GaussianProcessLight() = default;

        enum ClusteringType{
            GREEDY,
            KMEANS
        };
        ClusteringType clType = KMEANS;
        bool usesOverlap = true;
        int mLocalsMixed_ = 3;

        // A function for serealization
        template<class Archive>
        void serialize(Archive& ar){
            ar(CEREAL_NVP(LGPs_));
            ar(CEREAL_NVP(centers_));
            ar(CEREAL_NVP(sigmaN_));
            ar(CEREAL_NVP(gaussianKernel_));

            std::vector<std::string> names;
            try{
                ar(CEREAL_NVP(mLocalsMixed_));
            }catch(cereal::Exception& e){
                names.push_back("mLocalsMixed_");
            }

            if(0<names.size()){
                std::stringstream ss;
                ss << "[notice] parameters not found: ";
                for(const auto& name: names){
                    ss << name << ",";
                }
                std::cerr << ss.str() << std::endl;
            }
        }

        GaussianProcessLight& sigmaN(double sigmaN){
            sigmaN_ = sigmaN;
            return *this;
        }

        double sigmaN() const{
            return sigmaN_;
        }

        //Calculate maximum cluster size based on max complexity of the function predict()
        static const size_t MAX_CLUSTER_SIZE(const size_t MAX_COMPLEXITY,
                                             const size_t MAX_N_OVERLAP,
                                             const size_t N_LOCALS_MIXED) {
            const size_t k = MAX_N_OVERLAP;     //max # cluster overlaps for a sample
            const size_t M = N_LOCALS_MIXED;    //# local models mixed in prediction
            const size_t MAX_AMP = (std::min(k, M) + 1) * std::max(k, M);
            const size_t MAX_CLUSTER_SIZE = MAX_COMPLEXITY / MAX_AMP;

            const size_t MIN_FEASIBLE_CLUSTER_SIZE = 50;
            if (MAX_CLUSTER_SIZE < MIN_FEASIBLE_CLUSTER_SIZE) {
                std::cout << "WARNING: MAX_CLUSTER_SIZE=" << MAX_CLUSTER_SIZE << " is less than MIN_FEASIBLE_CLUSTER_SIZE=" << MIN_FEASIBLE_CLUSTER_SIZE << std::endl;
            }
            return MAX_CLUSTER_SIZE;
        }

        GaussianProcessLight& fit(const Eigen::MatrixXd& X, const Eigen::MatrixXd& Y)
        {
            //Clustering samples
            const size_t TARGET_N_CLUSTER = 1 + (X.rows() / MAX_CLUSTER_SIZE(800, 3, 3));
            CentroidBasedClusteringResult cr;
            if(clType==GREEDY){
                cr = clusteringGreedyWithMagicThreshold(X, Y, 1e-8);
            }else if(clType==KMEANS){
                cr = kMeansClustering(X, Y, TARGET_N_CLUSTER);
            }

            cr.printSummary();

            if(usesOverlap){
            //Improve the clusters
            const double OVERLAP_SCALE = 0.001;
            improveWithOverlap(cr, OVERLAP_SCALE, X, Y);
            cr.printSummary();
            }

            std::cout << "clustered into " << cr.nCluster() << " local models" << std::endl;

            centers_ = cr.centers;

            //Get local models by cluster
            for (auto k=0; k < cr.nCluster(); k++) {
                GaussianProcess gp;
                gp.sigmaN(sigmaN_);
                gp.gaussianKernel(gaussianKernel_);

                gp.fit(cr.XC[k], cr.YC[k]);

                LGPs_.push_back(gp);
            }

            return *this;
        }

        double predict(double x[], int index) const
        {
            std::vector<int> indices(1, index);
            std::vector<double> ypreds = predict(x, indices);
            return ypreds.at(0);
        }

        //TODO change return type: Eigen::VectorXd would be better
        std::vector<double> predict(double x[], const std::vector<int>& indices) const
        {
            const size_t M = mLocalsMixed_;
            const size_t n = centers_.size();

            std::vector<double> weights(n);
            for (size_t i=0; i < n; ++i) {
                const double* c = centers_.at(i).data();
                weights[i] = gaussianKernel_.computeKernel(x, c);
            }

            //indices of k-nearest (=top-k weight) neigbors
            std::vector<size_t> neighbors = top_k(weights, std::min(M, n));

            Eigen::VectorXd sum_wy = Eigen::VectorXd::Zero(indices.size());
            double sum_w = 0.0;
            for (auto m : neighbors) {
                double w = weights.at(m);
                std::vector<double> tmp = LGPs_.at(m).predict(x, indices);
                Eigen::VectorXd y = Eigen::Map<Eigen::VectorXd>(tmp.data(), indices.size());
                sum_wy += w * y;
                sum_w  += w;
            }

            Eigen::VectorXd y_hat = sum_wy;
            if (sum_w > MIN_DENOMINATOR) {
                y_hat /= sum_w;
            } else {
                std::vector<double> top1ypred = LGPs_.at(neighbors.at(0)).predict(x, indices);
                y_hat = Eigen::Map<Eigen::VectorXd>(top1ypred.data(), indices.size());
//                std::cout << "WARN: sum_w~=0 in predict() with " << n << " LGPs"
//                          << " >> predicted only with the nearest local model."<< std::endl;
            }
            std::vector<double> ypreds(y_hat.data(), y_hat.data() + indices.size());
            return ypreds;
        }

        /**
         * Estimate parameters as preparation
         */
        void fitCV(const Eigen::MatrixXd& X, const Eigen::MatrixXd& Y, const Eigen::MatrixXd& Actives)
        {
            GaussianProcess gp;
            gp.sigmaN(sigmaN_);
            gp.gaussianKernel(gaussianKernel_);

            // estimate parameters using GaussianProcess::fitCV
            gp.fitCV(X, Y, Actives);

            // set estimated parameters to this
            sigmaN_ = gp.sigmaN();
            gaussianKernel_ = gp.gaussianKernel();

            this->fit(X, Y);
        }

//        Eigen::VectorXd predictVarianceF(double x[]) const;
//        Eigen::VectorXd predictVarianceF(const Eigen::VectorXd& kstar) const{;
//        double computeLogLikelihood(double x[], const Eigen::VectorXd& y) const;

    private:
        class CentroidBasedClusteringResult{
        public:
            std::vector<Eigen::MatrixXd> XC;
            std::vector<Eigen::MatrixXd> YC;
            std::vector<Eigen::VectorXd> centers = {};
            size_t nCluster() const { return centers.size(); }
            void printSummary() const;
            void printAll() const;
        };

        /**
         * k-means++ clustering
         */
        CentroidBasedClusteringResult kMeansClustering(const Eigen::MatrixXd& X,
                                                       const Eigen::MatrixXd& Y,
                                                       const size_t TARGET_N_CLUSTER) const;

        /**
         * Greedy clustering
         */
        CentroidBasedClusteringResult clusteringGreedyWithMagicThreshold(const Eigen::MatrixXd& X,
                                                                         const Eigen::MatrixXd& Y,
                                                                         const double MAGIC_W_THRESHOLD) const;

        void improveWithOverlap(CentroidBasedClusteringResult& cr,
                                const double OVERLAP_SCALE,
                                const Eigen::MatrixXd& X,
                                const Eigen::MatrixXd& Y) const;

    public:
        // TODO move to an appropriate util class
        static std::vector<size_t> top_k(const std::vector<double>& values, const size_t k)
        {
            assert(k <= values.size());
            size_t n = values.size();
            std::vector<size_t> labels(n);
            for (auto i=0; i < n; ++i) { labels[i] = i; }

            auto comp = [&values](size_t i, size_t j) { return values[i] > values[j]; };

            if (n == k) {
                std::sort(labels.begin(), labels.end(), comp);
                return labels;
            }

            assert(n > k);  //need +1 elements to make heap
            auto first = labels.begin(), last = labels.begin() + k, end = labels.end();
            std::make_heap(first, last + 1, comp);
            std::pop_heap(first, last + 1, comp);
            for (auto it = last + 1; it != end; it++) {
                if (values[*it] <= values[*first]) {
                    continue;
                } else {
                    *last = *it;
                    std::pop_heap(first, last + 1, comp);
                }
            }
            std::sort_heap(first, last, comp);
            return std::vector<size_t>{first, last};
        }

//    private:
//        CentroidBasedClusteringResult aggregativeClustering(const Eigen::MatrixXd& X,
//                                                            const Eigen::MatrixXd& Y,
//                                                            const size_t TARGET_N_CLUSTER) const
//        class CentroidBasedCluster{
//        public:
//            CentroidBasedCluster(size_t i, const Eigen::MatrixXd& X) {
//                labels.push_back(i);
//                center = X.row(i);
//            };
//            std::vector<size_t> labels;
//            Eigen::VectorXd center;
//            void merge(const CentroidBasedCluster& that) {
//                std::copy(that.labels.begin(),that.labels.end(),std::back_inserter(this->labels));
//                std::sort(this->labels.begin(), this->labels.end());
//                double nis = this->labels.size();
//                double nat = that.labels.size();
//                this->center = (nis * this->center + nat * that.center) / (nis + nat);
//            }
//        };

    };
}

#endif /* GaussianProcessLight_hpp */


/*
 Copyright (C) 2019 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <ql/instruments/makecapfloor.hpp>
#include <ql/pricingengines/capfloor/bacheliercapfloorengine.hpp>
#include <ql/pricingengines/capfloor/blackcapfloorengine.hpp>
#include <ql/quotes/derivedquote.hpp>
#include <qle/termstructures/capfloorhelper.hpp>

using namespace QuantLib;
using std::ostream;

namespace {
void no_deletion(OptionletVolatilityStructure*) {}
} // namespace

namespace QuantExt {

// The argument to RelativeDateBootstrapHelper below is not simply the `quote`. Instead we create a DerivedQuote that,
// each time it is asked for its value, it returns a premium by calling CapFloorHelper::npv with the `quote` value. In
// this way, the `BootstrapHelper<T>::quoteError()` is always based on the cap floor premium and we do not have imply
// the volatility. This leads to all kinds of issues when deciding on max and min values in the iterative bootstrap
// where the quote volatility type input is of one type and the optionlet structure that we are trying to build is of
// another different type.
CapFloorHelper::CapFloorHelper(Type type, const Period& tenor, Rate strike, const Handle<Quote>& quote,
                               const boost::shared_ptr<IborIndex>& iborIndex,
                               const Handle<YieldTermStructure>& discountingCurve, bool moving,
                               const QuantLib::Date& effectiveDate, QuoteType quoteType,
                               QuantLib::VolatilityType quoteVolatilityType, QuantLib::Real quoteDisplacement,
                               bool endOfMonth, bool firstCapletExcluded)
    : RelativeDateBootstrapHelper<OptionletVolatilityStructure>(
          Handle<Quote>(boost::make_shared<DerivedQuote<boost::function<Real(Real)> > >(
              quote, boost::bind(&CapFloorHelper::npv, this, _1)))),
      type_(type), tenor_(tenor), strike_(strike), iborIndex_(iborIndex), discountHandle_(discountingCurve),
      moving_(moving), effectiveDate_(effectiveDate), quoteType_(quoteType), quoteVolatilityType_(quoteVolatilityType),
      quoteDisplacement_(quoteDisplacement), endOfMonth_(endOfMonth), firstCapletExcluded_(firstCapletExcluded),
      rawQuote_(quote), initialised_(false) {

    if (quoteType_ == Premium) {
        QL_REQUIRE(type_ != Automatic, "Cannot have CapFloorHelper type 'Automatic' with quote type of Premium");
    }

    QL_REQUIRE(!(moving_ && effectiveDate_ != Date()),
               "A fixed effective date does not make sense for a moving helper");

    registerWith(iborIndex_);
    registerWith(discountHandle_);

    initializeDates();
    initialised_ = true;
}

void CapFloorHelper::initializeDates() {

    if (!initialised_ || moving_) {
        CapFloor::Type capFloorType = CapFloor::Cap;
        if (type_ == CapFloorHelper::Floor) {
            capFloorType = CapFloor::Floor;
        }

        // Initialise the instrument and a copy
        // The strike can be Null<Real>() to indicate an ATM cap floor helper
        Rate dummyStrike = strike_ == Null<Real>() ? 0.01 : strike_;
        capFloor_ = MakeCapFloor(capFloorType, tenor_, iborIndex_, dummyStrike, 0 * Days)
                        .withEndOfMonth(endOfMonth_)
                        .withEffectiveDate(effectiveDate_, firstCapletExcluded_);
        capFloorCopy_ = MakeCapFloor(capFloorType, tenor_, iborIndex_, dummyStrike, 0 * Days)
                            .withEndOfMonth(endOfMonth_)
                            .withEffectiveDate(effectiveDate_, firstCapletExcluded_);

        // Maturity date is just the maturity date of the cap floor
        maturityDate_ = capFloor_->maturityDate();

        // We need the leg underlying the cap floor to determine the remaining date members
        const Leg& leg = capFloor_->floatingLeg();

        // Earliest date is the first optionlet fixing date
        boost::shared_ptr<CashFlow> cf = leg.front();
        boost::shared_ptr<FloatingRateCoupon> frc = boost::dynamic_pointer_cast<FloatingRateCoupon>(cf);
        QL_REQUIRE(frc, "Expected the first cashflow on the cap floor instrument to be a FloatingRateCoupon");
        earliestDate_ = frc->fixingDate();

        // Remaining dates are each equal to the fixing date on the final optionlet
        cf = leg.back();
        frc = boost::dynamic_pointer_cast<FloatingRateCoupon>(cf);
        QL_REQUIRE(frc, "Expected the final cashflow on the cap floor instrument to be a FloatingRateCoupon");
        pillarDate_ = latestDate_ = latestRelevantDate_ = frc->fixingDate();
    }
}

void CapFloorHelper::setTermStructure(OptionletVolatilityStructure* ovts) {

    if (strike_ == Null<Real>()) {
        // If the strike is Null<Real>(), we want an ATM helper
        Rate atm = capFloor_->atmRate(**discountHandle_);
        capFloor_ = MakeCapFloor(capFloor_->type(), tenor_, iborIndex_, atm, 0 * Days)
                        .withEndOfMonth(endOfMonth_)
                        .withEffectiveDate(effectiveDate_, firstCapletExcluded_);
        capFloorCopy_ = MakeCapFloor(capFloor_->type(), tenor_, iborIndex_, atm, 0 * Days)
                            .withEndOfMonth(endOfMonth_)
                            .withEffectiveDate(effectiveDate_, firstCapletExcluded_);

    } else if (type_ == CapFloorHelper::Automatic && quoteType_ != Premium) {
        // If the helper is set to automatically choose the underlying instrument type, do it now based on the ATM rate
        Rate atm = capFloor_->atmRate(**discountHandle_);
        CapFloor::Type capFloorType = atm > strike_ ? CapFloor::Floor : CapFloor::Cap;
        if (capFloor_->type() != capFloorType) {
            capFloor_ = MakeCapFloor(capFloorType, tenor_, iborIndex_, strike_, 0 * Days)
                            .withEndOfMonth(endOfMonth_)
                            .withEffectiveDate(effectiveDate_, firstCapletExcluded_);
            capFloorCopy_ = MakeCapFloor(capFloorType, tenor_, iborIndex_, strike_, 0 * Days)
                                .withEndOfMonth(endOfMonth_)
                                .withEffectiveDate(effectiveDate_, firstCapletExcluded_);
        }
    }

    // Set this helper's optionlet volatility structure
    boost::shared_ptr<OptionletVolatilityStructure> temp(ovts, no_deletion);
    ovtsHandle_.linkTo(temp, false);

    // Set the term structure pointer member variable in the base class
    RelativeDateBootstrapHelper<OptionletVolatilityStructure>::setTermStructure(ovts);

    // Set this helper's pricing engine depending on the type of the optionlet volatilities
    if (ovts->volatilityType() == ShiftedLognormal) {
        capFloor_->setPricingEngine(boost::make_shared<BlackCapFloorEngine>(discountHandle_, ovtsHandle_));
    } else {
        capFloor_->setPricingEngine(boost::make_shared<BachelierCapFloorEngine>(discountHandle_, ovtsHandle_));
    }

    // If the quote type is not a premium, we will need to use capFloorCopy_ to return the premium from the volatility
    // quote
    if (quoteType_ != Premium) {
        if (quoteVolatilityType_ == ShiftedLognormal) {
            capFloorCopy_->setPricingEngine(boost::make_shared<BlackCapFloorEngine>(
                discountHandle_, rawQuote_, ovtsHandle_->dayCounter(), quoteDisplacement_));
        } else {
            capFloorCopy_->setPricingEngine(
                boost::make_shared<BachelierCapFloorEngine>(discountHandle_, rawQuote_, ovtsHandle_->dayCounter()));
        }
    }
}

Real CapFloorHelper::impliedQuote() const {
    QL_REQUIRE(termStructure_ != 0, "CapFloorHelper's optionlet volatility term structure has not been set");
    capFloor_->recalculate();
    return capFloor_->NPV();
}

void CapFloorHelper::accept(AcyclicVisitor& v) {
    if (Visitor<CapFloorHelper>* v1 = dynamic_cast<Visitor<CapFloorHelper>*>(&v))
        v1->visit(*this);
    else
        RelativeDateBootstrapHelper<OptionletVolatilityStructure>::accept(v);
}

Real CapFloorHelper::npv(Real quoteValue) {
    if (quoteType_ == Premium) {
        return quoteValue;
    } else {
        // If the quote value is a volatility, return the premium
        return capFloorCopy_->NPV();
    }
}

ostream& operator<<(ostream& out, CapFloorHelper::Type type) {
    switch (type) {
    case CapFloorHelper::Cap:
        return out << "Cap";
    case CapFloorHelper::Floor:
        return out << "Floor";
    case CapFloorHelper::Automatic:
        return out << "Automatic";
    default:
        QL_FAIL("Unknown CapFloorHelper::Type (" << Integer(type) << ")");
    }
}

ostream& operator<<(ostream& out, CapFloorHelper::QuoteType type) {
    switch (type) {
    case CapFloorHelper::Volatility:
        return out << "Volatility";
    case CapFloorHelper::Premium:
        return out << "Premium";
    default:
        QL_FAIL("Unknown CapFloorHelper::QuoteType (" << Integer(type) << ")");
    }
}

} // namespace QuantExt


//| Copyright Inria May 2015
//| 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 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#ifndef LIMBO_OPT_RPROP_HPP
#define LIMBO_OPT_RPROP_HPP

#include <algorithm>

#include <Eigen/Core>

#include <limbo/opt/optimizer.hpp>
#include <limbo/tools/macros.hpp>
#include <limbo/tools/math.hpp>

namespace limbo {
    namespace defaults {
        struct opt_rprop {
            /// @ingroup opt_defaults
            /// number of max iterations
            BO_PARAM(int, iterations, 300);

            /// gradient norm epsilon for stopping
            BO_PARAM(double, eps_stop, 0.0);
        };
    }
    namespace opt {
        /// @ingroup opt
        /// Gradient-based optimization (rprop)
        /// - partly inspired by libgp: https://github.com/mblum/libgp
        /// - reference :
        /// Blum, M., & Riedmiller, M. (2013). Optimization of Gaussian
        /// Process Hyperparameters using Rprop. In European Symposium
        /// on Artificial Neural Networks, Computational Intelligence
        /// and Machine Learning.
        ///
        /// Parameters:
        /// - int iterations
        /// - double eps_stop
        template <typename Params>
        struct Rprop {
            template <typename F>
            Eigen::VectorXd operator()(const F& f, const Eigen::VectorXd& init, bool bounded) const
            {
                assert(Params::opt_rprop::eps_stop() >= 0.);

                size_t param_dim = init.size();
                double delta0 = 0.1;
                double deltamin = 1e-6;
                double deltamax = 50;
                double etaminus = 0.5;
                double etaplus = 1.2;
                double eps_stop = Params::opt_rprop::eps_stop();

                Eigen::VectorXd delta = Eigen::VectorXd::Ones(param_dim) * delta0;
                Eigen::VectorXd grad_old = Eigen::VectorXd::Zero(param_dim);
                Eigen::VectorXd params = init;

                if (bounded) {
                    for (int j = 0; j < params.size(); j++) {
                        if (params(j) < 0)
                            params(j) = 0;
                        if (params(j) > 1)
                            params(j) = 1;
                    }
                }

                Eigen::VectorXd best_params = params;
                double best = log(0);

                for (int i = 0; i < Params::opt_rprop::iterations(); ++i) {
                    auto perf = opt::eval_grad(f, params);
                    double lik = opt::fun(perf);
                    if (lik > best) {
                        best = lik;
                        best_params = params;
                    }
                    Eigen::VectorXd grad = -opt::grad(perf);
                    grad_old = grad_old.cwiseProduct(grad);

                    for (int j = 0; j < grad_old.size(); ++j) {
                        if (grad_old(j) > 0) {
                            delta(j) = std::min(delta(j) * etaplus, deltamax);
                        }
                        else if (grad_old(j) < 0) {
                            delta(j) = std::max(delta(j) * etaminus, deltamin);
                            grad(j) = 0;
                        }
                        params(j) += -tools::signum(grad(j)) * delta(j);

                        if (bounded && params(j) < 0)
                            params(j) = 0;
                        if (bounded && params(j) > 1)
                            params(j) = 1;
                    }

                    grad_old = grad;
                    if (grad_old.norm() < eps_stop)
                        break;
                }

                return best_params;
            }
        };
    }
}

#endif


//          Copyright Keld Helsgaun 2000, Oliver Kowalke 2014.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <cstdlib>
#include <cstddef>
#include <iostream>
#include <vector>

#include <boost/coroutine/all.hpp>

typedef boost::coroutines::symmetric_coroutine< void >  coro_t;

std::vector< int > merge( std::vector< int > const& a, std::vector< int > const& b)
{
    std::vector< int > c;
    std::size_t idx_a = 0, idx_b = 0;
    coro_t::call_type * other_a = 0, * other_b = 0;

    coro_t::call_type coro_a(
        [&]( coro_t::yield_type & yield) {
            while ( idx_a < a.size() )
            {
                if ( b[idx_b] < a[idx_a])
                    yield( * other_b);
                c.push_back(a[idx_a++]);
            }
            while ( c.size() < a.size() + b.size())
                c.push_back( b[idx_b]);
        });

    coro_t::call_type coro_b(
        [&]( coro_t::yield_type & yield) {
            while ( idx_b < b.size() )
            {
                if ( a[idx_a] < b[idx_b])
                    yield( * other_a);
                c.push_back(b[idx_b++]);
            }
            while ( c.size() < ( a.size() + b.size() ) )
                c.push_back( a[idx_a]);
        });

    other_a = & coro_a;
    other_b = & coro_b;

    coro_a();

    return c;
}

void print( std::string const& name, std::vector< int > const& v)
{
    std::cout << name << " : ";
    for ( auto itm : v)
    { std::cout << itm << " "; }
    std::cout << "\n";
}

int main( int argc, char * argv[])
{
    std::vector< int > a = { 1, 5, 6, 10 };
    print( "a", a);

    std::vector< int > b = { 2, 4, 7, 8, 9, 13 };
    print( "b", b);

    std::vector< int > c = merge( a, b);
    print( "c", c);

    std::cout << "Done" << std::endl;

    return EXIT_SUCCESS;
}


/**
 * Copyright 2014 Lucas Nunes de Lima
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 **/

#include "base64.hpp"

#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/transform_width.hpp>
#include <boost/archive/iterators/insert_linebreaks.hpp>
#include <boost/archive/iterators/remove_whitespace.hpp>
#include <iostream>
#include <string>

namespace controller
{
    namespace base64
    {
        using namespace boost::archive::iterators;

        typedef transform_width< binary_from_base64<remove_whitespace<std::string::const_iterator> >, 8, 6 > it_binary_t;
        typedef insert_linebreaks<base64_from_binary<transform_width<std::string::const_iterator,6,8> >, 72 > it_base64_t;

        std::string encode(const std::string& msg)
        {
            unsigned int write_padd_chars = (3 - msg.length() % 3) % 3;

            std::string result(it_base64_t(msg.begin()), it_base64_t(msg.end()));
            result.append(write_padd_chars, '=');

            return result;
        }

        std::string decode(const std::string& encoded)
        {
            unsigned int padd_chars = count(encoded.begin(), encoded.end(), '=');
            std::string temp = encoded;

            std::replace(temp.begin(), temp.end(), '=', 'A');
            std::string result(it_binary_t(temp.begin()), it_binary_t(temp.end()));
            result.erase(result.end() - padd_chars, result.end());

            return result;
        }

        std::string encodeAccount(const std::string& login, const std::string& pass)
        {
            std::string msg = login;
            msg += ':';
            msg += pass;

            return encode(msg);
        }
    }
}


#ifndef INPUT_PREPROCESSOR_HH
#define INPUT_PREPROCESSOR_HH

#include "lwtnn/lightweight_network_config.hh"
#include "lwtnn/Exceptions.hh"

#include <Eigen/Dense>
#include <map>
#include <vector>

namespace lwt {

  using Eigen::VectorXd;
  using Eigen::MatrixXd;

  // use a normal map externally, since these are more common in user
  // code.  TODO: is it worth changing to unordered_map?
  typedef std::map<std::string, double> ValueMap;
  typedef std::vector<std::pair<std::string, double> > ValueVector;
  typedef std::map<std::string, std::vector<double> > VectorMap;

  // ______________________________________________________________________
  // input preprocessor (handles normalization and packing into Eigen)

  class InputPreprocessor
  {
  public:
    InputPreprocessor(const std::vector<Input>& inputs);
    VectorXd operator()(const ValueMap&) const;
  private:
    // input transformations
    VectorXd m_offsets;
    VectorXd m_scales;
    std::vector<std::string> m_names;
  };

  class InputVectorPreprocessor
  {
  public:
    InputVectorPreprocessor(const std::vector<Input>& inputs);
    MatrixXd operator()(const VectorMap&) const;
  private:
    // input transformations
    VectorXd m_offsets;
    VectorXd m_scales;
    std::vector<std::string> m_names;
  };
}

#endif


// Copyright (C) 2015 Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/python.h>
#include <boost/python/args.hpp>
#include <dlib/geometry.h>
#include "indexing.h"

using namespace dlib;
using namespace std;
using namespace boost::python;

// ----------------------------------------------------------------------------------------

long left(const rectangle& r) { return r.left(); }
long top(const rectangle& r) { return r.top(); }
long right(const rectangle& r) { return r.right(); }
long bottom(const rectangle& r) { return r.bottom(); }
long width(const rectangle& r) { return r.width(); }
long height(const rectangle& r) { return r.height(); }
unsigned long area(const rectangle& r) { return r.area(); }

double dleft(const drectangle& r) { return r.left(); }
double dtop(const drectangle& r) { return r.top(); }
double dright(const drectangle& r) { return r.right(); }
double dbottom(const drectangle& r) { return r.bottom(); }
double dwidth(const drectangle& r) { return r.width(); }
double dheight(const drectangle& r) { return r.height(); }
double darea(const drectangle& r) { return r.area(); }

template <typename rect_type>
bool is_empty(const rect_type& r) { return r.is_empty(); }

template <typename rect_type>
point center(const rect_type& r) { return center(r); }

template <typename rect_type>
point dcenter(const rect_type& r) { return dcenter(r); }

template <typename rect_type>
bool contains(const rect_type& r, const point& p) { return r.contains(p); }

template <typename rect_type>
bool contains_xy(const rect_type& r, const long x, const long y) { return r.contains(point(x, y)); }

template <typename rect_type>
bool contains_rec(const rect_type& r, const rect_type& r2) { return r.contains(r2); }

template <typename rect_type>
rect_type intersect(const rect_type& r, const rect_type& r2) { return r.intersect(r2); }

template <typename rect_type>
string print_rectangle_str(const rect_type& r)
{
    std::ostringstream sout;
    sout << r;
    return sout.str();
}

template <typename rect_type>
string print_rectangle_repr(const rect_type& r)
{
    std::ostringstream sout;
    sout << "rectangle(" << r.left() << "," << r.top() << "," << r.right() << "," << r.bottom() << ")";
    return sout.str();
}

// ----------------------------------------------------------------------------------------

void bind_rectangles()
{
    using boost::python::arg;
    {
    typedef rectangle type;
    class_<type>("rectangle", "This object represents a rectangular area of an image.")
        .def(init<long,long,long,long>( (arg("left"),arg("top"),arg("right"),arg("bottom")) ))
        .def("area",   &::area)
        .def("left",   &::left)
        .def("top",    &::top)
        .def("right",  &::right)
        .def("bottom", &::bottom)
        .def("width",  &::width)
        .def("height", &::height)
        .def("is_empty", &::is_empty<type>)
        .def("center", &::center<type>)
        .def("dcenter", &::dcenter<type>)
        .def("contains", &::contains<type>, arg("point"))
        .def("contains", &::contains_xy<type>, (arg("x"), arg("y")))
        .def("contains", &::contains_rec<type>, (arg("rectangle")))
        .def("intersect", &::intersect<type>, (arg("rectangle")))
        .def("__str__", &::print_rectangle_str<type>)
        .def("__repr__", &::print_rectangle_repr<type>)
        .def(self == self)
        .def(self != self)
        .def_pickle(serialize_pickle<type>());
    }
    {
    typedef drectangle type;
    class_<type>("drectangle", "This object represents a rectangular area of an image with floating point coordinates.")
        .def(init<double,double,double,double>( (arg("left"),arg("top"),arg("right"),arg("bottom")) ))
        .def("area",   &::darea)
        .def("left",   &::dleft)
        .def("top",    &::dtop)
        .def("right",  &::dright)
        .def("bottom", &::dbottom)
        .def("width",  &::dwidth)
        .def("height", &::dheight)
        .def("is_empty", &::is_empty<type>)
        .def("center", &::center<type>)
        .def("dcenter", &::dcenter<type>)
        .def("contains", &::contains<type>, arg("point"))
        .def("contains", &::contains_xy<type>, (arg("x"), arg("y")))
        .def("contains", &::contains_rec<type>, (arg("rectangle")))
        .def("intersect", &::intersect<type>, (arg("rectangle")))
        .def("__str__", &::print_rectangle_str<type>)
        .def("__repr__", &::print_rectangle_repr<type>)
        .def(self == self)
        .def(self != self)
        .def_pickle(serialize_pickle<type>());
    }
    {
    typedef std::vector<rectangle> type;
    class_<type>("rectangles", "An array of rectangle objects.")
        .def(vector_indexing_suite<type>())
        .def("clear", &type::clear)
        .def("resize", resize<type>)
        .def_pickle(serialize_pickle<type>());
    }
}

// ----------------------------------------------------------------------------------------


//==================================================================================================
/*
  Copyright 2012 - 2015 NumScale SAS

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

#include <boost/simd/pack.hpp>
#include <boost/simd/io.hpp>
#include <boost/simd/functions/splat.hpp>
#include <boost/simd/functions/plus.hpp>
#include <boost/simd/functions/multiplies.hpp>
#include <iostream>

//! [scalar-dot]
template<typename Value> Value dot(Value* first1, Value* last1, Value* first2)
{
  Value v{};

  while(first1 != last1)
  {
    v += *first1 *  *first2;
    first1++;
    first2++;
  }

  return v;
}
//! [scalar-dot]

int main()
{
}


#include <neopluto/database.hpp>
#include <neopluto/naive_bayes.hpp>

#include <sqlite3.h>
#include <sstream>
#include <string>
#include <unordered_map>

#include <boost/locale.hpp>

#ifndef NDEBUG
#include <iostream>
#endif

namespace npl
{

auto database::update_tag_model() -> void
{
  assert(db);

  std::unordered_map<std::string, std::size_t> words;
  std::unordered_map<std::size_t, std::int64_t> label_to_tag;
  std::unordered_map<std::int64_t, std::size_t> tag_to_label;

  std::vector<std::pair<std::size_t, std::vector<std::string>>> dataset;

  std::size_t current_word = 0;
  std::size_t current_label = 0;

  boost::locale::generator gen;
  std::locale locale = gen("");

#ifndef NDEBUG
  std::clog << "Using locale: " << locale.name() << std::endl;
#endif

  // Get all words.
  constexpr char query[] = "SELECT description, tag FROM Entry WHERE tag IS NOT NULL;";

  exec_query(query, [&](sqlite3_stmt* stmt) {
    const auto description = boost::locale::to_upper(
      reinterpret_cast<const char*>(sqlite3_column_text(stmt, 0)), locale);

    const auto tag = sqlite3_column_int(stmt, 1);

    if (const auto[it, inserted] = tag_to_label.try_emplace(tag, current_label);
        inserted) {
#ifndef NDEBUG
      std::clog << "detected new tag: " << it->second << ": " << it->first << std::endl;
#endif
      label_to_tag.emplace(current_label, tag);
      ++current_label;
    }

#ifndef NDEBUG
    std::clog << "Entry description: " << description << std::endl;
#endif

    namespace bb = boost::locale::boundary;
    bb::ssegment_index map(bb::word, description.begin(), description.end(), locale);
    map.rule(bb::word_letter);

    dataset.push_back({tag_to_label.at(tag), {map.begin(), map.end()}});

    for (const auto& word : dataset.back().second) {
      if (const auto[it, inserted] = words.try_emplace(word, current_word); inserted) {
#ifndef NDEBUG
        std::clog << "detected new word: " << it->second << ": " << it->first
                  << std::endl;
#endif
        ++current_word;
      }
    }

    return true;
  });

  classifier = std::unique_ptr<tag_classifier, void (*)(tag_classifier*)>(
    nb::build_tag_model(dataset, std::move(words), std::move(label_to_tag)),
    &nb::destroy_tag_model);
}

auto database::clear_tag_model() -> void { classifier = nullptr; }

auto database::suggest_tag(const char* desc) -> std::optional<tag>
{
  if (!classifier)
    return std::nullopt;

#ifndef NDEBUG
  std::clog << "\nClassifying: " << desc << std::endl;
#endif

  std::locale locale = boost::locale::generator{}("");
  const auto description = boost::locale::to_upper(desc, locale);

  namespace bb = boost::locale::boundary;
  bb::ssegment_index map(bb::word, description.begin(), description.end(), locale);
  map.rule(bb::word_letter);

  return tag(nb::classify(classifier.get(), {map.begin(), map.end()}),
             shared_from_this());
}

} // namespace npl