#include "ros/ros.h"
// 话题同步处理
#include "message_filters/subscriber.h"
#include "message_filters/synchronizer.h"
#include "message_filters/sync_policies/approximate_time.h"
// opencv接口
#include "cv_bridge/cv_bridge.h"
#include "image_transport/image_transport.h"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include <opencv2/calib3d/calib3d.hpp>
//ros消息类型头文件
#include "sensor_msgs/Image.h"
#include "sensor_msgs/image_encodings.h"
//系统指令头文件
#include <boost/thread/thread.hpp>
#include <boost/foreach.hpp>
// pcl点云库
#include <pcl/io/pcd_io.h>
#include "pcl_ros/point_cloud.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/conversions.h>
#include <pcl_ros/transforms.h>
//内部时钟
#include <chrono>
#include "sensor_msgs/Imu.h"
#include "eigen3/Eigen/Core"
#include <sophus/se3.hpp>
#include <iostream>
//g2o
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>

using namespace std;
using namespace cv;

sensor_msgs::Image image_;
cv::Mat cvColorImgMat;
cv::Mat cvColorImgMat2;
pcl::PCLPointCloud2 pcl_pc2;
pcl::PCLPointCloud2 pcl_pc2l;
uint8_t flag=0;
long int cnt = 0;
typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

/// vertex and edges used in g2o ba
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  virtual void setToOriginImpl() override {
    _estimate = Sophus::SE3d();
  }

  /// left multiplication on SE3
  virtual void oplusImpl(const double *update) override {
    Eigen::Matrix<double, 6, 1> update_eigen;
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
  }

  virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}
};
/// g2o edge
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d &point) : _point(point) {}

  virtual void computeError() override {
    const VertexPose *pose = static_cast<const VertexPose *> ( _vertices[0] );
    _error = _measurement - pose->estimate() * _point;
  }

  virtual void linearizeOplus() override {
    VertexPose *pose = static_cast<VertexPose *>(_vertices[0]);
    Sophus::SE3d T = pose->estimate();
    Eigen::Vector3d xyz_trans = T * _point;
    _jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
    _jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans);
  }

  bool read(istream &in) {}

  bool write(ostream &out) const {}

protected:
  Eigen::Vector3d _point;
};
void pose_estimation_3d3d(const vector<Point3f> &pts1,
                          const vector<Point3f> &pts2,
                          Mat &R, Mat &t) {
  Point3f p1, p2;     // center of mass
  int N = pts1.size();
  for (int i = 0; i < N; i++) {
    p1 += pts1[i];
    p2 += pts2[i];
  }
  p1 = Point3f(Vec3f(p1) / N);
  p2 = Point3f(Vec3f(p2) / N);
  vector<Point3f> q1(N), q2(N); // remove the center
  for (int i = 0; i < N; i++) {
    q1[i] = pts1[i] - p1;
    q2[i] = pts2[i] - p2;
  }

  // compute q1*q2^T
  Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
  for (int i = 0; i < N; i++) {
    W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
  }
  cout << "W=" << W << endl;

  // SVD on W
  Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d U = svd.matrixU();
  Eigen::Matrix3d V = svd.matrixV();

  cout << "U=" << U << endl;
  cout << "V=" << V << endl;

  Eigen::Matrix3d R_ = U * (V.transpose());
  if (R_.determinant() < 0) {
    R_ = -R_;
  }
  Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

  // convert to cv::Mat
  R = (Mat_<double>(3, 3) <<
    R_(0, 0), R_(0, 1), R_(0, 2),
    R_(1, 0), R_(1, 1), R_(1, 2),
    R_(2, 0), R_(2, 1), R_(2, 2)
  );
  t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

void bundleAdjustmentGaussNewton(const VecVector3d &points_3d,const VecVector2d &points_2d,const cv::Mat &K,Sophus::SE3d &pose) {
    typedef Eigen::Matrix<double,6,1> Vector6d;
    const int iterations = 10;
    double cost = 0,lastCost = 0;
    double fx = K.at<double>(0,0);
    double fy = K.at<double>(1,1);
    double cx = K.at<double>(0,2);
    double cy = K.at<double>(1,2);
    for(int iter = 0;iter < iterations; iter++){
        Eigen::Matrix<double,6,6> H = Eigen::Matrix<double,6,6>::Zero();
        Vector6d b = Vector6d::Zero();
        cost = 0;
        //compute cost
        for(int i = 0;i < points_3d.size(); i++){
            Eigen::Vector3d pc = pose * points_3d[i];
            double inv_z = 1.0 / pc[2];
            double inv_z2 = inv_z * inv_z;
            Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx,fy * pc[1] / pc[2] + cy);
            Eigen::Vector2d e = points_2d[i] - proj;
            cost+=e.squaredNorm();

            Eigen::Matrix<double,2,6> J;
            J << -fx * inv_z,
            0,
            fx * pc[0] * inv_z2,
            fx * pc[0] * pc[1] * inv_z2,
            -fx - fx * pc[0] * pc[0] * inv_z2,
            fx * pc[1] * inv_z,
            0,
            -fy * inv_z,
            fy * pc[1] * inv_z2,
            fy + fy * pc[1] * pc[1] * inv_z2,
            -fy * pc[0] * pc[1] * inv_z2,
            -fy * pc[0] * inv_z;

            H += J.transpose() * J;
            b += -J.transpose() * e;
        }
        Vector6d dx;
        dx = H.ldlt().solve(b);
        if(isnan(dx[0])){
            std::cout << "result is nan!" << std::endl;
            break;
        }

        if(iter > 0 && cost >= lastCost){
            // cost increase, update is not good
            std::cout << "cost: " << cost << ", last cost: " << lastCost << std::endl;
            break;
        }

        //update estimation
        pose = Sophus::SE3d::exp(dx) * pose;
        lastCost = cost;
        std::cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << std::endl;
        if(dx.norm() < 1e-6){
            break;
        }
    }
    std::cout << "pose by g-n \n" << pose.matrix() <<std::endl;
}

void callback(const sensor_msgs::ImuConstPtr& imu,const sensor_msgs::PointCloud2ConstPtr& point){
    std::cout <<"this is quternion"   << imu-> orientation.x << std::endl;
    cv_bridge::CvImagePtr cvImagePtr;
    pcl_conversions::toPCL(*point,pcl_pc2);
    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud_last(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);
    ROS_INFO("Cloud:width = %d,height = %d",point->width,point->height);
    try{
        pcl::toROSMsg(*point,image_);
    }catch(std::runtime_error e){
        ROS_ERROR_STREAM("Error in converting cloud to image message: " << e.what());
    }
    try{
        cvImagePtr = cv_bridge::toCvCopy(image_,sensor_msgs::image_encodings::BGR8);
    }catch(cv_bridge::Exception e){
        ROS_ERROR_STREAM("Cv_bridge Exception:" << e.what());
        return;
    }
    cvColorImgMat = cvImagePtr->image;

    if(flag==0){//如果是第一次进入，则和第一帧解算相同
        // pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud_last(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud_last);
        cvColorImgMat2 = cvImagePtr->image;
        flag =2;
    }else{
        // pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud_last(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromPCLPointCloud2(pcl_pc2l,*temp_cloud_last);
    }

    std::vector<cv::KeyPoint> keypoints,keypoints2;
    cv::Mat descriptors,descriptors2;
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    detector->detect(cvColorImgMat,keypoints);
    detector->detect(cvColorImgMat2,keypoints2);
    descriptor->compute(cvColorImgMat,keypoints,descriptors);
    descriptor->compute(cvColorImgMat2,keypoints2,descriptors2);
    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    ROS_INFO("extract ORB cost = %f seconds",time_used.count());

    // cvColorImgMat2 = cvColorImgMat;
    // cv::Mat outimg,outimg2;
    // cv::drawKeypoints(cvColorImgMat,keypoints,outimg,cv::Scalar::all(-1),cv::DrawMatchesFlags::DEFAULT);
    // cv::drawKeypoints(cvColorImgMat2,keypoints2,outimg2,cv::Scalar::all(-1),cv::DrawMatchesFlags::DEFAULT);
    // cv::imshow("colorview",outimg2);
    // cv::imshow("ORB feature",outimg);

    //match
    std::vector<cv::DMatch> matches;
    t1 = std::chrono::steady_clock::now();
    try
    {
        matcher->match(descriptors,descriptors2,matches);
    }
    catch(const cv::Exception& e)
    {
        ROS_INFO(e.what());
    }

    t2 = std::chrono::steady_clock::now();
    time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    ROS_INFO("match ORB cost %f seconds",time_used.count());

    //choise better match
    auto min_max = std::minmax_element(matches.begin(),matches.end(),
        [](const cv::DMatch &m1,const cv::DMatch &m2){return m1.distance < m2.distance;});
    double min_dist = min_max.first->distance;
    double max_dist = min_max.second->distance;
    std::vector<cv::DMatch> good_matches;
    for (int i = 0; i < descriptors.rows; i++){
        if(matches[i].distance <= std::max(2 * min_dist,30.0))
            good_matches.push_back(matches[i]);
    }

    //获取3D点
    cv::Point3f d;
    std::vector<cv::Point3f> pts_3d,pts_3d_last;
    std::vector<cv::Point2f> pts_2d,pts_2d_last;
    BOOST_FOREACH(cv::DMatch m,good_matches){
        d.x = temp_cloud_last->points[int(keypoints2[m.queryIdx].pt.y)*640 + int(keypoints2[m.queryIdx].pt.x)].x;
        d.y = temp_cloud_last->points[int(keypoints2[m.queryIdx].pt.y)*640 + int(keypoints2[m.queryIdx].pt.x)].y;
        d.z = temp_cloud_last->points[int(keypoints2[m.queryIdx].pt.y)*640 + int(keypoints2[m.queryIdx].pt.x)].z;
        if(isnan(d.x)||isnan(d.y)||isnan(d.z))
            continue;
        pts_3d.push_back(d);
        pts_2d.push_back(keypoints[m.queryIdx].pt);
        // ROS_INFO("%f %f %f",d.x,d.y,d.z);
        // ROS_INFO("%d %d",int(keypoints[m.queryIdx].pt.x),int(keypoints[m.queryIdx].pt.y));
    }
    std::cout << "3d-2s pairs: " << pts_3d.size() << std::endl;
    cv::Mat K = (cv::Mat_<double>(3, 3) << 525.0, 0, 319.5, 0, 525.0, 239.5, 0, 0, 1);

    //Opencv 解位姿
    // t1 = std::chrono::steady_clock::now();
    // cv::Mat r, t;
    // cv::solvePnP(pts_3d, pts_2d, K, cv::Mat(), r, t,false,CV_ITERATIVE); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    // cv::Mat R;
    // cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵
    // t2 = std::chrono::steady_clock::now();
    // time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    // std::cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << std::endl;

    // std::cout << "R=" << std::endl << R << std::endl;
    // std::cout << "t=" << std::endl << t << std::endl;

    //gaussNewton 解位姿
    VecVector3d pts_3d_eigen;
    VecVector2d pts_2d_eigen;
    for (size_t i = 0; i < pts_3d.size(); ++i) {
        pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
        pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
    }
    std::cout << "calling bundle adjustment by gauss newton " << std::endl;
    Sophus::SE3d pose_gn;
    t1 = std::chrono::steady_clock::now();
    bundleAdjustmentGaussNewton(pts_3d_eigen,pts_2d_eigen,K,pose_gn);
    t2 = std::chrono::steady_clock::now();
    time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    std::cout << "solve pnp by gauss newton cost time: " << time_used.count() << " seconds." << std::endl;

    //保存上一帧的数据
    pcl_pc2l = pcl_pc2;
    cvColorImgMat2 = cvColorImgMat;

    //draw the answer
    cv::Mat img_match;
    cv::Mat img_goodmatch;
    cv::drawMatches(cvColorImgMat,keypoints,cvColorImgMat2,keypoints2,good_matches,img_goodmatch);
    cv::imshow("good matches",img_goodmatch);
    // cv::imshow("grayview",cvGrayImgMat);
    cv::waitKey(5);
}
int main(int argc, char *argv[])
{
    ros::init(argc,argv,"grayView");
    ros::NodeHandle nh_;
    message_filters::Subscriber<sensor_msgs::Imu> sub(nh_,"/imu_data",1);
    message_filters::Subscriber<sensor_msgs::PointCloud2> sub2(nh_,"/camera/depth_registered/points",1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Imu,sensor_msgs::PointCloud2> MySync;
    message_filters::Synchronizer<MySync> sync(MySync(10),sub,sub2);
    sync.registerCallback(boost::bind(&callback,_1,_2));
    // cv::namedWindow("colorview",cv::WINDOW_NORMAL);
    // cv::moveWindow("colorview",100,100);
    // cv::namedWindow("grayview",cv::WINDOW_NORMAL);
    // cv::moveWindow("grayview",600,100);
    ros::spin();
    return 0;
}


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#ifndef TURI_LINE_SEARCH_H_
#define TURI_LINE_SEARCH_H_

// Types
#include <core/data/flexible_type/flexible_type.hpp>

// Optimization
#include <ml/optimization/optimization_interface.hpp>
#include <ml/optimization/regularizer_interface.hpp>
#include <Eigen/Core>
#include <core/logging/assertions.hpp>

// TODO: List of todo's for this file
//------------------------------------------------------------------------------
// 1. Feature: Armijo cubic interpolation line search.
// 2. Feature: Wolfe cubic interpolation line search.
// 3. Optimization: Add accepted function value to the ls_return structure
//    (reduces 1 func eval per iteration)
//

namespace turi {

namespace optimization {

/**
 * \ingroup group_optimization
 * \addtogroup line_search Line Search
 * \{
 */

/** "Zoom" phase for More and Thuente line seach.
 *
 * \note Applicable for smooth functions only.
 *
 * This code is a C++ port of Jorge Nocedal's implementaiton of More and
 * Thuente [2] line search. This code was availiable at
 * http://www.ece.northwestern.edu/~nocedal/lbfgs.html
 *
 * Nocedal's Condition for Use: This software is freely available for
 * educational or commercial purposes. We expect that all publications
 * describing work using this software quote at least one of the references
 * given below. This software is released under the BSD License
 *
 * The purpose of cstep is to compute a safeguarded step for a linesearch and
 * to update an interval of uncertainty for a minimizer of the function.
 *
 * The parameter stx contains the step with the least function value. The
 * parameter stp contains the current step. It is assumed that the derivative
 * at stx is negative in the direction of the step. If brackt is set true then
 * a minimizer has been bracketed in an interval of uncertainty with endpoints
 * stx and sty.
 *
 *
 * \param[in] stx  Best step obtained so far.
 * \param[in] fx   Function value at the best step so far.
 * \param[in] dx   Directional derivative at the best step so far.
 * \param[in] sty  Step at the end point of the uncertainty interval.
 * \param[in] fy   Function value at end point of uncertainty interval.
 * \param[in] dy   Directional derivative at end point of uncertainty interval.
 * \param[in] stp  Current step.
 * \param[in] fp   Function value at current step.
 * \param[in] dp   Directional derivative at the current step.
 * \param[in] brackt Has the minimizer has been bracketed?
 * \param[in] stpmin Min step size
 * \param[in] stpmax Max step size.
 *
 *
 */
inline bool cstep(double &stx, double &fx, double &dx,
           double &sty, double &fy, double &dy,
           double &stp, double &fp,  double &dp,
           bool &brackt, double stpmin, double stpmax){

  const double p66 = 0.66;
  bool info = false;

  // Check the input parameters for errors.
  // This should not happen.
  if ((brackt && (stp <= std::min(stx,sty) || stp >= std::max(stx,sty)))
      || (dx*(stp-stx) >= LS_ZERO) || (stpmax < stpmin)){
     return info;
  }

  // Determine if the derivatives have opposite sign.
  double sgnd = dp*(dx/std::abs(dx));

  // Local variables
  bool bound;
  double theta, s, gamma, p, q, r, stpc, stpq, stpf;

  // First case.
  // -------------------------------------------------------------------------
  // A higher function value. The minimum is bracketed. If the cubic step is
  // closer to stx than the quadratic step, the cubic step is taken, else the
  // average of the cubic and quadratic steps is taken.
  if (fp > fx){

    info = true;
    bound = true;
    theta = 3*(fx - fp)/(stp - stx) + dx + dp;
    s = std::max(std::abs(theta), std::max(std::abs(dx),std::abs(dp)));
    gamma = s*sqrt(pow(theta/s,2) - (dx/s)*(dp/s));

    if (stp < stx)
     gamma = -gamma;

    p = (gamma - dx) + theta;
    q = ((gamma - dx) + gamma) + dp;
    r = p/q;
    stpc = stx + r*(stp - stx);
    stpq = stx + ((dx/((fx-fp)/(stp-stx)+dx))/2)*(stp - stx);

    if (std::abs(stpc-stx) < std::abs(stpq-stx)){
       stpf = stpc;
    } else{
      stpf = stpc + (stpq - stpc)/2;
    }

    brackt = true;

  }
  // Second case.
  // -------------------------------------------------------------------------
  // A lower function value and derivatives of opposite sign. The
  // minimum is bracketed. If the cubic step is closer to stx than the
  // quadratic (secant) step, the cubic step is taken, else the quadratic step
  // is taken.
  else if(sgnd <= 0){

    info = true;
    bound = false;

    theta = 3*(fx - fp)/(stp - stx) + dx + dp;
    s = std::max(std::abs(theta), std::max(std::abs(dx),std::abs(dp)));
    gamma = s*sqrt(pow(theta/s,2) - (dx/s)*(dp/s));

    if (stp > stx)
       gamma = -gamma;

    p = (gamma - dp) + theta;
    q = ((gamma - dp) + gamma) + dx;
    r = p/q;
    stpc = stp + r*(stx - stp);
    stpq = stp + (dp/(dp-dx))*(stx - stp);
    if (std::abs(stpc-stp) > std::abs(stpq-stp)){
       stpf = stpc;
    } else {
       stpf = stpq;
    }
    brackt = true;
  }
  // Third case.
  // -------------------------------------------------------------------------
  // A lower function value, derivatives of the same sign, and the
  // magnitude of the derivative decreases.  The cubic step is only used if the
  // cubic tends to infinity in the direction of the step or if the minimum of
  // the cubic is beyond stp. Otherwise the cubic step is defined to be either
  // stpmin or stpmax. The quadratic (secant) step is also computed and if the
  // minimum is bracketed then the the step closest to stx is taken, else the
  // step farthest away is taken.
  else if (std::abs(dp) < std::abs(dx)){

    info = true;
    bound = true;
    theta = 3*(fx - fp)/(stp - stx) + dx + dp;
    s = std::max(std::abs(theta), std::max(std::abs(dx),std::abs(dp)));

    // The case gamma = 0 only arises if the cubic does not tend to infinity
    // in the direction of the step.

    gamma = s*sqrt(std::max(0., pow((theta/s),2) - (dx/s)*(dp/s)));
    if (stp > stx){
        gamma = -gamma;
    }

    p = (gamma - dp) + theta;
    q = (gamma + (dx - dp)) + gamma;
    r = p/q;
    if((r < 0.0) && (std::abs(gamma) <= OPTIMIZATION_ZERO)){
       stpc = stp + r*(stx - stp);
    }else if (stp > stx){
       stpc = stpmax;
    }else{
       stpc = stpmin;
    }

    stpq = stp + (dp/(dp-dx))*(stx - stp);
    if (brackt){
       if (std::abs(stp-stpc) < std::abs(stp-stpq)){
          stpf = stpc;
       }else{
          stpf = stpq;
       }
    }else{
       if (std::abs(stp-stpc) > std::abs(stp-stpq)){
          stpf = stpc;
       }else{
          stpf = stpq;
       }
    }

  }
  // Fourth case.
  // -------------------------------------------------------------------------
  // A lower function value, derivatives of the same sign, and the magnitude of
  // the derivative does not decrease. If the minimum is not bracketed, the
  // step is either stpmin or stpmax, else the cubic step is taken.
  else{
     info = true;
     bound = false;
     if (brackt){
        theta = 3*(fp - fy)/(sty - stp) + dy + dp;
        s = std::max(std::abs(theta), std::max(std::abs(dy),std::abs(dp)));
        gamma = s*sqrt(pow(theta/s,2) - (dy/s)*(dp/s));

        if (stp > sty){
            gamma = -gamma;
        }
        p = (gamma - dp) + theta;
        q = ((gamma - dp) + gamma) + dy;
        r = p/q;
        stpc = stp + r*(sty - stp);
        stpf = stpc;
     }else if (stp > stx){
        stpf = stpmax;
     }else {
        stpf = stpmin;
     }
  }

  // Update the interval of uncertainty.
  // This update does not depend on the new step or the case analysis above.
  if (fp > fx){
     sty = stp;
     fy = fp;
     dy = dp;
  }else{

     if (sgnd < 0.0){
        sty = stx;
        fy = fx;
        dy = dx;
     }
     stx = stp;
     fx = fp;
     dx = dp;
  }

  // Compute the new step and safeguard it.
  stpf = std::min(stpmax,stpf);
  stpf = std::max(stpmin,stpf);
  stp = stpf;
  if (brackt && bound){
     if (sty > stx){
        stp = std::min(stx+p66*(sty-stx),stp);
     }else{
        stp = std::max(stx+p66*(sty-stx),stp);
     }
  }

  // cstep-failed
  if (info == false){
     logprogress_stream << "Warning:"
                        << " Unable to interpolate step size intervals."
                        << std::endl;
  }
  return info;

}

/**
 *
 * Compute step sizes for line search methods to satisfy Strong Wolfe conditions.
 *
 * \note Applicable for smooth functions only.
 *
 * This code is a C++ port of Jorge Nocedal's implementaiton of More and
 * Thuente [2] line search. This code was availiable at
 * http://www.ece.northwestern.edu/~nocedal/lbfgs.html
 *
 *   Line search based on More' and Thuente [1] to find a step which satisfies
 *   a Wolfe conditions for sufficient decrease condition and a curvature
 *   condition.
 *
 *   At each stage the function updates an interval of uncertainty with
 *   endpoints. The interval of uncertainty is initially chosen so that it
 *   contains a minimizer of the modified function
 *
 *        f(x+stp*s) - f(x) - ftol*stp*(gradf(x)'s).
 *
 *   If a step is obtained for which the modified function has a nonpositive
 *   function value and nonnegative derivative, then the interval of
 *   uncertainty is chosen so that it contains a minimizer of f(x+stp*s).
 *
 *   The algorithm is designed to find a step which satisfies the sufficient
 *   decrease condition
 *         f(x+stp*s) .le. f(x) + ftol*stp*(gradf(x)'s),          [W1]
 *   and the curvature condition
 *         std::abs(gradf(x+stp*s)'s)) .le. gtol*std::abs(gradf(x)'s).      [W2]
 *
 *  References:
 *
 *  (1) More, J. J. and D. J. Thuente. "Line Search Algorithms with
 *  Guaranteed Sufficient Decrease." ACM Transactions on Mathematical Software
 *  20, no. 3 (1994): 286-307.
 *
 * (2) Wright S.J  and J. Nocedal. Numerical optimization. Vol. 2.
 *                         New York: Springer, 1999.
 * \param[in] model Any model with a first order optimization interface.
 * \param[in] init_step Initial step size
 * \param[in] point Starting point for the solver.
 * \param[in] gradient Gradient value at this point (saves computation)
 * \param[in]    reg   Shared ptr to an interface to a smooth regularizer.
 * \returns stats Line searnorm ch return object
 *
*/
template <typename Vector>
inline ls_return more_thuente(
    first_order_opt_interface& model,
    double init_step,
    double init_func_value,
    DenseVector point,
    Vector gradient,
    DenseVector direction,
    double function_scaling = 1.0,
    const std::shared_ptr<smooth_regularizer_interface> reg=NULL,
    size_t max_function_evaluations = LS_MAX_ITER){

    // Initialize the return object
    ls_return stats;

    // Input checking: Initial step size can't be zero.
    if (init_step <= LS_ZERO){
      logprogress_stream << " Error:"
                         <<" \nInitial step step less than "<< LS_ZERO
                         << "." << std::endl;
      return stats;
    }

    // Check that the initia direction is a descent direction.
    // This can only occur of your gradients were computed incorrectly
    // or the problem is non-convex.
    DenseVector x0 = point;
    double Dphi0 = gradient.dot(direction);
    if ( (init_step <= 0) | (Dphi0 >= OPTIMIZATION_ZERO) ){
      logprogress_stream << " Error: Search direction is not a descent direction."
                          <<" \nDetected numerical difficulties." << std::endl;
    }

    // Initializing local variables
    // stx, fx, dgx: Values of the step, function, and derivative at the best
    //               step.
    //
    // sty, fy, dgy: Values of the step, function, and derivative at the other
    //               endpoint of the interval of uncertainty.
    //
    // st, f, dg   : Values of the step, function, and derivative at current
    //               step
    //
    // g           : Gradient w.r.t x and step (vector) at the current point
    //
    double stx = LS_ZERO;
    double fx = init_func_value;
    double dgx = Dphi0;                   // Derivative of f(x + s d) w.r.t s

    double sty = LS_ZERO;
    double fy = init_func_value;
    double dgy = Dphi0;

    double stp = init_step;
    double f = init_func_value;
    double dg = Dphi0;
    Vector g = gradient;
    DenseVector reg_gradient(gradient.size());

    // Interval [stmax, stmin] of uncertainty
    double stmax = LS_ZERO;
    double stmin = LS_MAX_STEP_SIZE;

    // Flags and local variables
    bool brackt = false;                 // Bracket or Zoon phase?
    bool stage1 = true;

    double wolfe_func_dec  = LS_C1*Dphi0;         // Wolfe condition [W1]
    double wolfe_curvature = LS_C2*Dphi0;         // Wolfe condition [W2]
    double width = stmax - stmin;
    double width2 = 2*width;

    // Constants used in this code.
    // (Based on http://www.ece.northwestern.edu/~nocedal/lbfgs.html)
    const double p5 = 0.5;
    const double p66 = 0.66;
    const double xtrapf = 4;
    bool infoc = true;                            // Zoom status

    // Start searching
    while (true){

      // Set the min and max steps based on the current interval of uncertainty.
      if (brackt == true){
        stmin = std::min(stx,sty);
        stmax = std::max(stx,sty);
      }else{
        stmin = stx;
        stmax = stp + xtrapf*(stp - stx);
      }

      // Force the step to be within the bounds
      stp = std::max(stp,LS_ZERO);
      stp = std::min(stp,LS_MAX_STEP_SIZE);

      // If an unusual termination is to occur then let 'stp' be the lowest point
      // obtained so far.
      if ( (infoc == false)
          || (brackt && (stmax-stmin <= LS_ZERO))){

         logprogress_stream << "Warning:"
            << " Unusual termination criterion reached."
            << "\nReturning the best step found so far."
            << " This typically happens when the number of features is much"
            << " larger than the number of training samples. Consider pruning"
            << " features manually or increasing the regularization value."
            << std::endl;
         stp = stx;
      }

      // Reached func evaluation limit -- return the best one so far.
      if (size_t(stats.func_evals) >= max_function_evaluations){
        stats.step_size = stx;
        stats.status = true;
        return stats;
      }

      // Evaluate the function and gradient at stp and compute the directional
      // derivative.
      point = x0 + stp * direction;
      model.compute_first_order_statistics(point, g, f);
      stats.num_passes++;
      stats.func_evals++;
      stats.gradient_evals++;
      if (reg != NULL){
        reg->compute_gradient(point, reg_gradient);
        f += reg->compute_function_value(point);
        g += reg_gradient;
      }

      if(function_scaling != 1.0) {
        f *= function_scaling;
        g *= function_scaling;
      }

      dg = g.dot(direction);

      double ftest = init_func_value + stp*wolfe_func_dec;

      // Termination checking
      // Note: There are many good checks and balances used in Nocedal's code
      // Some of them are overly defensive and should not happen.

      // Rounding errors
      if ( (brackt && ((stp <= stmin) || (stp >= stmax))) || (infoc == false)){
          logprogress_stream << "Warning: Rounding errors"
            << " prevent further progress. \nThere may not be a step which"
            << " satisfies the sufficient decrease and curvature conditions."
            << " \nTolerances may be too small or dataset may be poorly scaled."
            << " This typically happens when the number of features is much"
            << " larger than the number of training samples. Consider pruning"
            << " features manually or increasing the regularization value."
            << std::endl;
          stats.step_size = stp;
          stats.status = false;
          return stats;
      }

      // Step is more than LS_MAX_STEP_SIZE
      if ((stp >= LS_MAX_STEP_SIZE) && (f <= ftest) && (dg <= wolfe_func_dec)){
        logprogress_stream << "Warning: Reached max step size."
                           << std::endl;
        stats.step_size = stp;
        stats.status = true;
        return stats;
      }

      // Step is smaller than LS_ZERO
      if ((stp <= LS_ZERO) && ((f > ftest) || (dg >= wolfe_func_dec))){
        logprogress_stream << "Error: Reached min step size."
                           << " Cannot proceed anymore."
                           << std::endl;
        stats.step_size = stp;
        stats.status = false;
        return stats;
      }

      // Relative width of the interval of uncertainty is reached.
      if (brackt && (stmax-stmin <= LS_ZERO)){
        logprogress_stream << "Error: \nInterval of uncertainty"
                           << "lower than step size limit." << std::endl;
        stats.status = false;
        return stats;
      }

      // Wolfe conditions W1 and W2 are satisfied! Woo!
      if ((f <= ftest) && (std::abs(dg) <= -wolfe_curvature)){
        stats.step_size = stp;
        stats.status = true;
        return stats;
      }

      // Stage 1 is a search for steps for which the modified function has
      // a nonpositive value and nonnegative derivative.
      if ( stage1 && (f <= ftest) && (dg >= wolfe_curvature)){
             stage1 = false;
      }

      // A modified function is used to predict the step only if we have not
      // obtained a step for which the modified function has a nonpositive
      // function value and nonnegative derivative, and if a lower function
      // value has been  obtained but the decrease is not sufficient.

      if (stage1 && (f <= fx) && (f > ftest)){

         // Define the modified function and derivative values.
         double fm = f - stp*wolfe_func_dec;
         double fxm = fx - stx*wolfe_func_dec;
         double fym = fy - sty*wolfe_func_dec;
         double dgm = dg - wolfe_func_dec;
         double dgxm = dgx - wolfe_func_dec;
         double dgym = dgy - wolfe_func_dec;

         // Call cstep to update the interval of uncertainty and to compute the
         // new step.
         infoc = cstep(stx,fxm, dgxm,
                       sty, fym, dgym,
                       stp, fm, dgm,
                       brackt,
                       stmin,stmax);

         // Reset the function and gradient values for f.
         fx = fxm + stx*wolfe_func_dec;
         fy = fym + sty*wolfe_func_dec;
         dgx = dgxm + wolfe_func_dec;
         dgy = dgym + wolfe_func_dec;

      }else{

         // Call cstep to update the interval of uncertainty and to compute the
         // new step.
         infoc = cstep(stx,fx, dgx,
                       sty, fy, dgy,
                       stp, f, dg,
                       brackt,
                       stmin,stmax);

      }

      // Force a sufficient decrease in the size of the interval of uncertainty.
      if (brackt){
         if (std::abs(sty-stx) >= p66*width2){
           stp = stx + p5*(sty - stx);
         }

         width2 = width;
         width = std::abs(sty-stx);
      }

    } // end-of-while

    return stats;

}

/**
 *
 * Armijo backtracking to compute step sizes for line search methods.
 *
 * \note Applicable for smooth functions only.
 *
 * Line search based on an backtracking strategy to ensure sufficient
 * decrease in function values at each iteration.
 *
 * References:
 * (1) Wright S.J  and J. Nocedal. Numerical optimization. Vol. 2.
 *                         New York: Springer, 1999.
 *
 * \param[in] model Any model with a first order optimization interface.
 * \param[in] init_step Initial step size.
 * \param[in] init_func Initial function value.
 * \param[in] point     Starting point for the solver.
 * \param[in] gradient  Gradient value at this point.
 * \param[in] direction Direction of the next step .
 *
 * \returns stats Line searnorm ch return object
 *
*/
template <typename Vector>
inline ls_return armijo_backtracking(
    first_order_opt_interface& model,
    double init_step,
    double init_func_value,
    DenseVector point,
    Vector gradient,
    DenseVector direction){

    // Step 1: Initialize the function
    // ------------------------------------------------------------------------
    // Min function decrease according to Armijo conditions.
    // The choice of constants are based on Nocedal and Wright [1].
    double sufficient_decrease = LS_C1*(gradient.dot(direction));
    ls_return stats;
    double step_size = init_step;
    DenseVector new_point = point;

    // Step 2: Backtrack
    // -----------------------------------------------------------------------
    while (stats.func_evals <= LS_MAX_ITER && step_size >= LS_ZERO){

      // Check for sufficient decrease
      new_point = point + step_size * direction;
      if (model.compute_function_value(new_point) <= init_func_value
          + step_size * sufficient_decrease){

        stats.step_size = step_size;
        stats.status = true;
        return stats;

      }

      step_size *= 0.5;
      stats.func_evals += 1;

    }

    return stats;
}

/**
 *
 * Backtracking to compute step sizes for line search methods.
 *
 * \note Applicable for non-smooth functions.
 *
 * Line search based on an backtracking strategy to "some" decrease in function
 * values at each iteration.
 *
 * References:
 * (1) Wright S.J  and J. Nocedal. Numerical optimization. Vol. 2.
 *                         New York: Springer, 1999.
 *
 * \param[in] model Any model with a first order optimization interface.
 * \param[in] init_step Initial step size.
 * \param[in] init_func Initial function value.
 * \param[in] point     Starting point for the solver.
 * \param[in] gradient  Gradient value at this point.
 * \param[in] direction Direction of the next step .
 * \param[in] reg       Regularizer interface!
 *
 * \returns stats Line searnorm ch return object
 *
*/
template <typename Vector>
inline ls_return backtracking(
    first_order_opt_interface& model,
    double init_step,
    double init_func_value,
    DenseVector point,
    Vector gradient,
    DenseVector direction,
    const std::shared_ptr<regularizer_interface> reg=NULL){

    // Step 1: Initialize the function
    // ------------------------------------------------------------------------
    ls_return stats;
    double step_size = init_step;
    DenseVector delta_point(point.size());;
    DenseVector new_point(point.size());;

    // Step 2: Backtrack
    // -----------------------------------------------------------------------
    while (stats.func_evals <= LS_MAX_ITER && step_size >= LS_ZERO){

      // Check for sufficient decrease
      new_point = point + step_size * direction;
      if(reg != NULL){
        reg->apply_proximal_operator(new_point, step_size);
      }

      delta_point = new_point - point;
      if (model.compute_function_value(new_point) <=
             init_func_value + gradient.dot(delta_point)
                            + 0.5 * delta_point.squaredNorm()/step_size){

        stats.step_size = step_size;
        stats.status = true;
        return stats;

      }

      step_size *= 0.5;
      stats.func_evals += 1;

    }

    return stats;
}

/** This function estimates a minumum point between
 *  two function values with known gradients.
 *
 *  The minimum value must lie between the two function values, f1 and f2, and the
 *  gradients must indicate the minimum lies between the two values and that the
 *  function is convex.
 *
 *  Under these situations, a third degree polynomial / cubic spline can be fit
 *  to the two function points, and this is gauranteed to have a single minimum
 *  between f1 and f2.  This is what this function returns.
 *
 *  \param[in] dist  The distance between the points f1 and f2.
 *  \param[in] f1    The value of the function at the left point.
 *  \param[in] f2    The value of the function at the right point.
 *  \param[in] g1    The derivative of the function at the left point.
 *  \param[in] g2    The derivative of the function at the right point.
 *
 */
inline double gradient_bracketed_linesearch(double dist, double f1, double f2,
                                            double g1, double g2) {
  // Assume f1 is evaluated at 0, f2 is evaluated at 1.  To make the latter true,
  // we need to multiply the gradients by the appropriate scaling factor.
  g1 *= dist;
  g2 *= dist;

  // We have to have this such that a minimum point is between f1 and f2, which
  DASSERT_LE(g1, 1e-4);   // Condition 1
  DASSERT_GE(g2, -1e-4);  // Condition 2

  // Also make sure the function is convex;
  DASSERT_LE(f2 - g2, f1 + 1e-4);  // Condition 3
  DASSERT_LE(f1 + g1, f2 + 1e-4);  // Condition 4

  // Now, we have 4 known variables, so construct a 3rd order polynomial to
  // approximate the solution between the two points.  Then find the minimum of
  // that.

  // With the convexity coefficients above, a third order polynomial, given as
  //
  //   p(t) = a*t^3 + b*t^2 + c*t + d
  //
  // will have exactly one minima between 0 and 1 by the conditions above.
  //
  // The coefficients can be easily derived by:
  // p(0) = f1
  // p(1) = f2
  // p'(0) = g1
  // p'(1) = g2
  //
  // Some algebra yields:
  //
  const double a = -2*(f2 - f1) + (g2 + g1);
  const double b = 3*(f2 - f1) - g2 - 2*g1;
  const double c = g1;
  const double d = f1;

  // std::cout << "coeff = (" << a << ", " << b << ", " << c << std::endl;

  // Starting iterate
  double left = 0;
  double right = 1;

  double best_value = std::min(f1, f2);
  double best_loc = f1 < f2 ? 0 : 1;

  for(size_t m_iter = 0; m_iter < 32; ++m_iter) {

    double t = 0.5 * (right + left);

    // Make sure that the recent value is indeed better
    double v = d + c*t + b*t*t + a*t*t*t;
    // double vpp = 2*b + 6*a*t;

    if(v < best_value) {
      best_value = v;
      best_loc = t;
    }

    if(right - left < 1e-6) {
      break;
    }

    double vp = c + 2*b*t + 3*a*t*t;

    if(vp > 0) {
      right = t;
    } else {
      left = t;
    }

  }

  return dist * best_loc;
}

/// \}

} // optimizaiton

} // turicreate

#endif


//////////////////////////////////////////////////////////////////////////////////
// MIT License
//
// Copyright (c) 2017 Vicon Motion Systems Ltd
//
// 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.
//////////////////////////////////////////////////////////////////////////////////
#include "RetimerUtils.h"

#include <ViconDataStreamSDKCoreUtils/ClientUtils.h>

#include <boost/math/constants/constants.hpp>

namespace bmc = boost::math::constants;

namespace ClientUtils
{
  typedef std::array< double, 4 > Quaternion;
  typedef std::array< double, 3 > Axis;
  typedef std::array< double, 3 > Displacement;

  Quaternion Conjugate( const Quaternion & i_rInput )
  {
    Quaternion conj = { -i_rInput[0], -i_rInput[1], -i_rInput[2], i_rInput[3] };
    return conj;
  }

  Quaternion Inverse( const Quaternion & i_rInput )
  {
    Quaternion inv = Conjugate( i_rInput );
    double DotProd = 0;
    for( unsigned i = 0; i < 4; ++i ) DotProd += ( i_rInput[i] * i_rInput[i] );
    for( unsigned i = 0; i < 4; ++i ) inv[i] /= DotProd;
    return inv;
  }

  Axis Imaginary( const Quaternion & i_rInput )
  {
    Axis axis = { i_rInput[0], i_rInput[1], i_rInput[2] };
    return axis;
  }

  double Magnitude( const Axis & i_rInput )
  {
    double mag = 0;
    for( unsigned int i = 0; i < 3; ++i )
    {
      mag += i_rInput[i] * i_rInput[i];
    }
    return sqrt( mag );
  }

  void Normalize( Axis & io_rInput )
  {
    double Mag = Magnitude( io_rInput );
    io_rInput /= Mag;
  }

  bool ToAxisAngle( const Quaternion & i_rInput, Axis & o_rAxis, double & o_rAngle )
  {
    bool success = true;

    // angle is always positive
    Axis imag = Imaginary( i_rInput );
    double mag = Magnitude( imag );
    o_rAngle = 2 * std::atan2( mag, i_rInput[3] );

    // direc parallel to imag. part
    o_rAxis = imag;
    if( mag == 0.0 )
    {
      // or signal exception here.
      o_rAxis[2] = 1.0;
      success = false;
    }
    else
    {
      // normalize direction vector
      for( unsigned int i = 0; i < 3; ++i ) o_rAxis[i] /= mag;
    }
    return success;
  }

  Quaternion FromAxisAngle( const Axis & i_rAxis, const double & i_rAngle )
  {
    // This should take a normalized vector
    Axis AxisCopy = i_rAxis;
    Normalize( AxisCopy );

    Quaternion output;

    // half angle
    double a = i_rAngle * 0.5;
    double s = std::sin( a );

    for( int i = 0; i < 3; i++ )
    {
      // imaginary vector is sine of half angle multiplied with axis
      output[i] = s * AxisCopy[i];
    }

    // real part is cosine of half angle
    output[3] = std::cos( a );

    return output;
  }

  RotationMatrix ToRotationMatrix( const Quaternion & i_rInput )
  {
    double x2 = i_rInput[0] * i_rInput[0];
    double xy = i_rInput[0] * i_rInput[1];
    double rx = i_rInput[3] * i_rInput[0];
    double y2 = i_rInput[1] * i_rInput[1];
    double yz = i_rInput[1] * i_rInput[2];
    double ry = i_rInput[3] * i_rInput[1];
    double z2 = i_rInput[2] * i_rInput[2];
    double zx = i_rInput[2] * i_rInput[0];
    double rz = i_rInput[3] * i_rInput[2];
    double r2 = i_rInput[3] * i_rInput[3];

    RotationMatrix Output;
    // fill diagonal terms
    Output[ 0 ] = r2 + x2 - y2 - z2;    //(0,0)
    Output[ 4 ] = r2 - x2 + y2 - z2; // (1,1)
    Output[ 8 ] = r2 - x2 - y2 + z2; // (2,2)

    // fill off diagonal terms (output not transposed; differing from vnl_quaternion::rotation_matrix_transposed
    Output[ 3 ] = 2 * ( xy + rz ); // (1,0)
    Output[ 6 ] = 2 * ( zx - ry ); // (2,0)
    Output[ 7 ] = 2 * ( yz + rx ); // (2,1)
    Output[ 1 ] = 2 * ( xy - rz ); // (0,1)
    Output[ 2 ] = 2 * ( zx + ry ); // (0,2)
    Output[ 5 ] = 2 * ( yz - rx ); // (1,2)

    return Output;
  }

  Axis operator*( const Axis & i_rLeft, double i_rVal )
  {
    Axis Result;
    std::transform( i_rLeft.begin(), i_rLeft.end(), Result.begin(), [&]( double x ){ return x * i_rVal; } );
    return Result;
  }

  Axis operator+=( Axis & i_rLeft, const Axis & i_rRight )
  {
    std::transform( i_rLeft.begin(), i_rLeft.end(), i_rRight.begin(), i_rLeft.begin(), []( double x, double y ){ return x + y; } );
    return i_rLeft;
  }

  Quaternion operator*( const Quaternion & i_rLeft, const Quaternion & i_rRight )
  {
    Quaternion output;
    double r1 = i_rLeft[3];                  // real and img parts of args
    double r2 = i_rRight[3];
    Axis i1 = Imaginary( i_rLeft );
    Axis i2 = Imaginary( i_rRight );
    double real_v = ( r1 * r2 ) - DotProduct( i1, i2 ); // real&img of product q1*q2
    Axis img = CrossProduct( i1, i2 );
    Axis i2r1 = i2*r1;
    Axis i1r2 = i1*r2;
    Axis i2r1i1r2 = i2r1 + i1r2;
    img += i2r1i1r2;
    output = { img[0], img[1], img[2], real_v };
    return output;
  }

  // Simple interpolation using linear interpolation and prediction
  /// Returns a rotation prediction at time t3 from 2 rotation r1 at time t1 and r2 and time t2
  /// where t3 > t2 > t1
  // From http://answers.unity3d.com/questions/168779/extrapolating-quaternion-rotation.html
  Quaternion PredictRotation( Quaternion r1, double t1, Quaternion r2, double t2, double t3 )
  {
    Quaternion rot = r2 * Inverse( r1 ); // rot is rotation from t1 to t2

    double dt = ( t3 - t1 ) / ( t2 - t1 ); // dt = extrapolation factor

    // Convert to Axis/Angle
    Axis axis;
    double angle;
    if( !ToAxisAngle( rot, axis, angle ) )
    {
      // Check the magnitude of the imaginary part is not zero, as then
      // the axis of the quaternion will not be well defined.
      return r1;
    }

    // Assume shortest path
    if( angle > bmc::pi< double >() ) angle -= ( 2 * bmc::pi< double >() );

    // Multiple angle by extrapolation factor
    angle = fmod( angle * dt, ( 2 * bmc::pi< double >() ) );

    // Combine with first rotation
    Quaternion r3 = FromAxisAngle( axis, angle ) * r1;

    return r3;
  }

  /// Returns a translation prediction at time t3 from 2 translations at time d1, t1; d2, t2 where t3 > t2 > t1
  /// Can also be used for linear interpolation where t2 > t3 > t1
  Displacement PredictDisplacement( const Displacement & d1, double t1, const Displacement & d2, double t2, double t3 )
  {

    // disp is displacement from t1 to t2
    Displacement disp( d2 - d1 );

    // extrapolation factor
    double dt = ( t3 - t1 ) / ( t2 - t1 );

    // multiply displacement by extrapolation factor
    disp *= dt;

    // Combine with first displacement
    Displacement d3 = d1 + disp;

    return d3;
  }

  double PredictVal(const double d1, const double t1, const double d2, const double t2, double t3)
  {
    // disp is displacement from t1 to t2
    double disp( d2 - d1 );

    // extrapolation factor
    double dt = ( t3 - t1 ) / ( t2 - t1 );

    // multiply displacement by extrapolation factor
    disp *= dt;

    // Combine with first displacement
    double d3 = d1 + disp;

    return d3;

  }
}


/* boost random/nierderreiter_base2.hpp header file
 *
 * Copyright Justinas Vygintas Daugmaudis 2010-2018
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_RANDOM_NIEDERREITER_BASE2_HPP
#define BOOST_RANDOM_NIEDERREITER_BASE2_HPP

#include <boost/random/detail/niederreiter_base2_table.hpp>
#include <boost/random/detail/gray_coded_qrng.hpp>

#include <boost/dynamic_bitset.hpp>

namespace boost {
namespace random {

/** @cond */
namespace qrng_detail {
namespace nb2 {

// Return the base 2 logarithm for a given bitset v
template <typename DynamicBitset>
inline typename DynamicBitset::size_type
bitset_log2(const DynamicBitset& v)
{
  if (v.none())
    boost::throw_exception( std::invalid_argument("bitset_log2") );

  typename DynamicBitset::size_type hibit = v.size() - 1;
  while (!v.test(hibit))
    --hibit;
  return hibit;
}

// Multiply polynomials over Z_2.
template <typename PolynomialT, typename DynamicBitset>
inline void modulo2_multiply(PolynomialT P, DynamicBitset& v, DynamicBitset& pt)
{
  pt.reset(); // pt == 0
  for (; P; P >>= 1, v <<= 1)
    if (P & 1) pt ^= v;
  pt.swap(v);
}

// Calculate the values of the constants V(J,R) as
// described in BFN section 3.3.
//
// pb = polynomial defined in section 2.3 of BFN.
template <typename DynamicBitset>
inline void calculate_v(const DynamicBitset& pb,
  typename DynamicBitset::size_type kj,
  typename DynamicBitset::size_type pb_degree,
  DynamicBitset& v)
{
  typedef typename DynamicBitset::size_type size_type;

  // Now choose values of V in accordance with
  // the conditions in section 3.3.
  size_type r = 0;
  for ( ; r != kj; ++r)
    v.reset(r);

  // Quoting from BFN: "Our program currently sets each K_q
  // equal to eq. This has the effect of setting all unrestricted
  // values of v to 1."
  for ( ; r < pb_degree; ++r)
    v.set(r);

  // Calculate the remaining V's using the recursion of section 2.3,
  // remembering that the B's have the opposite sign.
  for ( ; r != v.size(); ++r)
  {
    bool term = false;
    for (typename DynamicBitset::size_type k = 0; k < pb_degree; ++k)
    {
      term ^= pb.test(k) & v[r + k - pb_degree];
    }
    v[r] = term;
  }
}

} // namespace nb2

template<typename UIntType, unsigned w, typename Nb2Table>
struct niederreiter_base2_lattice
{
  typedef UIntType value_type;

  BOOST_STATIC_ASSERT(w > 0u);
  BOOST_STATIC_CONSTANT(unsigned, bit_count = w);

private:
  typedef std::vector<value_type> container_type;

public:
  explicit niederreiter_base2_lattice(std::size_t dimension)
  {
    resize(dimension);
  }

  void resize(std::size_t dimension)
  {
    typedef boost::dynamic_bitset<> bitset_type;

    dimension_assert("Niederreiter base 2", dimension, Nb2Table::max_dimension);

    // Initialize the bit array
    container_type cj(bit_count * dimension);

    // Reserve temporary space for lattice computation
    bitset_type v, pb, tmp;

    // Compute Niedderreiter base 2 lattice
    for (std::size_t dim = 0; dim != dimension; ++dim)
    {
      const typename Nb2Table::value_type poly = Nb2Table::polynomial(dim);
      if (poly > std::numeric_limits<value_type>::max()) {
        boost::throw_exception( std::range_error("niederreiter_base2: polynomial value outside the given value type range") );
      }

      const unsigned degree = multiprecision::msb(poly); // integer log2(poly)
      const unsigned space_required = degree * ((bit_count / degree) + 1); // ~ degree + bit_count

      v.resize(degree + bit_count - 1);

      // For each dimension, we need to calculate powers of an
      // appropriate irreducible polynomial, see Niederreiter
      // page 65, just below equation (19).
      // Copy the appropriate irreducible polynomial into PX,
      // and its degree into E. Set polynomial B = PX ** 0 = 1.
      // M is the degree of B. Subsequently B will hold higher
      // powers of PX.
      pb.resize(space_required); tmp.resize(space_required);

      typename bitset_type::size_type kj, pb_degree = 0;
      pb.reset(); // pb == 0
      pb.set(pb_degree); // set the proper bit for the pb_degree

      value_type j = high_bit_mask_t<bit_count - 1>::high_bit;
      do
      {
        // Now choose a value of Kj as defined in section 3.3.
        // We must have 0 <= Kj < E*J = M.
        // The limit condition on Kj does not seem to be very relevant
        // in this program.
        kj = pb_degree;

        // Now multiply B by PX so B becomes PX**J.
        // In section 2.3, the values of Bi are defined with a minus sign :
        // don't forget this if you use them later!
        nb2::modulo2_multiply(poly, pb, tmp);
        pb_degree += degree;
        if (pb_degree >= pb.size()) {
          // Note that it is quite possible for kj to become bigger than
          // the new computed value of pb_degree.
          pb_degree = nb2::bitset_log2(pb);
        }

        // If U = 0, we need to set B to the next power of PX
        // and recalculate V.
        nb2::calculate_v(pb, kj, pb_degree, v);

        // Niederreiter (page 56, after equation (7), defines two
        // variables Q and U.  We do not need Q explicitly, but we
        // do need U.

        // Advance Niederreiter's state variables.
        for (unsigned u = 0; j && u != degree; ++u, j >>= 1)
        {
          // Now C is obtained from V. Niederreiter
          // obtains A from V (page 65, near the bottom), and then gets
          // C from A (page 56, equation (7)).  However this can be done
          // in one step.  Here CI(J,R) corresponds to
          // Niederreiter's C(I,J,R), whose values we pack into array
          // CJ so that CJ(I,R) holds all the values of C(I,J,R) for J from 1 to NBITS.
          for (unsigned r = 0; r != bit_count; ++r) {
            value_type& num = cj[dimension * r + dim];
            // set the jth bit in num
            num = (num & ~j) | (-v[r + u] & j);
          }
        }
      } while (j != 0);
    }

    bits.swap(cj);
  }

  typename container_type::const_iterator iter_at(std::size_t n) const
  {
    BOOST_ASSERT(!(n > bits.size()));
    return bits.begin() + n;
  }

private:
  container_type bits;
};

} // namespace qrng_detail

typedef detail::qrng_tables::niederreiter_base2 default_niederreiter_base2_table;

/** @endcond */

//!Instantiations of class template niederreiter_base2_engine model a \quasi_random_number_generator.
//!The niederreiter_base2_engine uses the algorithm described in
//! \blockquote
//!Bratley, Fox, Niederreiter, ACM Trans. Model. Comp. Sim. 2, 195 (1992).
//! \endblockquote
//!
//!\attention niederreiter_base2_engine skips trivial zeroes at the start of the sequence. For example,
//!the beginning of the 2-dimensional Niederreiter base 2 sequence in @c uniform_01 distribution will look
//!like this:
//!\code{.cpp}
//!0.5, 0.5,
//!0.75, 0.25,
//!0.25, 0.75,
//!0.375, 0.375,
//!0.875, 0.875,
//!...
//!\endcode
//!
//!In the following documentation @c X denotes the concrete class of the template
//!niederreiter_base2_engine returning objects of type @c UIntType, u and v are the values of @c X.
//!
//!Some member functions may throw exceptions of type std::range_error. This
//!happens when the quasi-random domain is exhausted and the generator cannot produce
//!any more values. The length of the low discrepancy sequence is given by
//! \f$L=Dimension \times (2^{w} - 1)\f$.
template<typename UIntType, unsigned w, typename Nb2Table = default_niederreiter_base2_table>
class niederreiter_base2_engine
  : public qrng_detail::gray_coded_qrng<
      qrng_detail::niederreiter_base2_lattice<UIntType, w, Nb2Table>
    >
{
  typedef qrng_detail::niederreiter_base2_lattice<UIntType, w, Nb2Table> lattice_t;
  typedef qrng_detail::gray_coded_qrng<lattice_t> base_t;

public:
  //!Effects: Constructs the default `s`-dimensional Niederreiter base 2 quasi-random number generator.
  //!
  //!Throws: bad_alloc, invalid_argument, range_error.
  explicit niederreiter_base2_engine(std::size_t s)
    : base_t(s) // initialize lattice here
  {}

#ifdef BOOST_RANDOM_DOXYGEN
  //=========================Doxygen needs this!==============================
  typedef UIntType result_type;

  //!Returns: Tight lower bound on the set of values returned by operator().
  //!
  //!Throws: nothing.
  static BOOST_CONSTEXPR result_type min BOOST_PREVENT_MACRO_SUBSTITUTION ()
  { return (base_t::min)(); }

  //!Returns: Tight upper bound on the set of values returned by operator().
  //!
  //!Throws: nothing.
  static BOOST_CONSTEXPR result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
  { return (base_t::max)(); }

  //!Returns: The dimension of of the quasi-random domain.
  //!
  //!Throws: nothing.
  std::size_t dimension() const { return base_t::dimension(); }

  //!Effects: Resets the quasi-random number generator state to
  //!the one given by the default construction. Equivalent to u.seed(0).
  //!
  //!\brief Throws: nothing.
  void seed()
  {
    base_t::seed();
  }

  //!Effects: Effectively sets the quasi-random number generator state to the `init`-th
  //!vector in the `s`-dimensional quasi-random domain, where `s` == X::dimension().
  //!\code
  //!X u, v;
  //!for(int i = 0; i < N; ++i)
  //!    for( std::size_t j = 0; j < u.dimension(); ++j )
  //!        u();
  //!v.seed(N);
  //!assert(u() == v());
  //!\endcode
  //!
  //!\brief Throws: range_error.
  void seed(UIntType init)
  {
    base_t::seed(init);
  }

  //!Returns: Returns a successive element of an `s`-dimensional
  //!(s = X::dimension()) vector at each invocation. When all elements are
  //!exhausted, X::operator() begins anew with the starting element of a
  //!subsequent `s`-dimensional vector.
  //!
  //!Throws: range_error.
  result_type operator()()
  {
    return base_t::operator()();
  }

  //!Effects: Advances *this state as if `z` consecutive
  //!X::operator() invocations were executed.
  //!\code
  //!X u = v;
  //!for(int i = 0; i < N; ++i)
  //!    u();
  //!v.discard(N);
  //!assert(u() == v());
  //!\endcode
  //!
  //!Throws: range_error.
  void discard(boost::uintmax_t z)
  {
    base_t::discard(z);
  }

  //!Returns true if the two generators will produce identical sequences of outputs.
  BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(niederreiter_base2_engine, x, y)
  { return static_cast<const base_t&>(x) == y; }

  //!Returns true if the two generators will produce different sequences of outputs.
  BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(niederreiter_base2_engine)

  //!Writes the textual representation of the generator to a @c std::ostream.
  BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, niederreiter_base2_engine, s)
  { return os << static_cast<const base_t&>(s); }

  //!Reads the textual representation of the generator from a @c std::istream.
  BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, niederreiter_base2_engine, s)
  { return is >> static_cast<base_t&>(s); }

#endif // BOOST_RANDOM_DOXYGEN
};

/**
 * @attention This specialization of \niederreiter_base2_engine supports up to 4720 dimensions.
 *
 * Binary irreducible polynomials (primes in the ring `GF(2)[X]`, evaluated at `X=2`) were generated
 * while condition `max(prime)` < 216 was satisfied.
 *
 * There are exactly 4720 such primes, which yields a Niederreiter base 2 table for 4720 dimensions.
 *
 * However, it is possible to provide your own table to \niederreiter_base2_engine should the default one be insufficient.
 */
typedef niederreiter_base2_engine<boost::uint_least64_t, 64u, default_niederreiter_base2_table> niederreiter_base2;

} // namespace random

} // namespace boost

#endif // BOOST_RANDOM_NIEDERREITER_BASE2_HPP


///
/// @file dijkstra-bgl.h
/// @author Jxtopher
/// @version 1
/// @date 2019
/// @brief Implementation dijkstra with Boost Graph Library
/// \details BGL : https://www.boost.org/doc/libs/1_46_1/libs/graph/doc/adjacency_list.html
///          Dijkstra :https://fr.wikipedia.org/wiki/Algorithme_de_Dijkstra
///

#include <cstdlib>
#include <iostream>

#include <boost/graph/graph_utility.hpp> // print_graph
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>

using namespace std;
using namespace boost;

//-----------------------------------------------------------------------------
// Définitions des types pour le graphe
//-----------------------------------------------------------------------------

///
/// @brief Définition du type pour les noeuds graphe
///
struct VertexProperties {
    float d;
    int predecessor;
    VertexProperties() : d(std::numeric_limits<float>::infinity()), predecessor(-1) {}
    VertexProperties(float d, int predecessor) : d(d), predecessor(-1) {}
};

///
/// @brief Définition du type pour les liens graphe
///
struct EdgeProperties {
    int weight;
    EdgeProperties() : weight(0) { }
    EdgeProperties(int weight) : weight(weight) { }
};

struct EdgeInfoPropertyTag {
    typedef edge_property_tag kind;
    static std::size_t const num; // ???
};

std::size_t const EdgeInfoPropertyTag::num = (std::size_t)&EdgeInfoPropertyTag::num;
typedef property<EdgeInfoPropertyTag, EdgeProperties> edge_info_prop_type;

///
/// @brief Type de graphe
///
typedef adjacency_list<
    boost::vecS, boost::vecS, boost::undirectedS,
    VertexProperties,                               // Type vertex
    edge_info_prop_type                             // Type edge
> Graph;

//-----------------------------------------------------------------------------
// En plus
//-----------------------------------------------------------------------------
void test(Graph &g) {
    // Visiter les voisins du sommet 0
    auto neighbours = boost::adjacent_vertices(0, g);

    for (auto vd : make_iterator_range(neighbours))
        std::cout << "0 has adjacent vertex " << vd << "\n";

    g[0].d = 42;
    cout<<g[0].d<<endl;

    //cout<<edge(0,2,g).second<<endl;

    EdgeProperties p = get(EdgeInfoPropertyTag(), g, edge(0,2,g).first);
    std::cout << "weight: " << p.weight << std::endl;

    get(EdgeInfoPropertyTag(), g, edge(0,2,g).first).weight = 33;
    std::cout << "weight: " << get(EdgeInfoPropertyTag(), g, edge(0,2,g).first).weight << std::endl;

    // Nombre de sommets
    cout<<num_vertices(g)<<endl;
    cout<<num_edges(g)<<endl;

    // Affiche le graphe
    print_graph(g);

}
//-----------------------------------------------------------------------------
// dijkstra
//-----------------------------------------------------------------------------
///
/// @brief Crée une instance de graphe
///
/// \param g : graphe
///
void createInstanceGraph(Graph &g) {
    add_vertex(VertexProperties(std::numeric_limits<float>::infinity(), -1), g);
    add_vertex(g);
    add_vertex(g);
    add_vertex(g);
    add_vertex(g);

    add_edge(0, 1, EdgeProperties(7), g);
    add_edge(0, 2, EdgeProperties(9), g);
    add_edge(0, 5, EdgeProperties(14), g);
    add_edge(1, 3, EdgeProperties(15), g);
    add_edge(1, 2, EdgeProperties(10), g);
    add_edge(2, 3, EdgeProperties(11), g);
    add_edge(2, 5, EdgeProperties(2), g);
    add_edge(4, 3, EdgeProperties(6), g);
    add_edge(5, 4, EdgeProperties(9), g);
}

///
/// @brief donne le plus petit élément de la liste l
///
/// \param g : graphe
/// \param l : liste de noeud
///
/// \return le plus petit élément de la liste l
///
unsigned int extrait_min(Graph const &g, list<unsigned int> &l) {
    list<unsigned int>::iterator iter = min_element(l.begin(), l.end());
    l.erase(iter);
    return *iter;
}

///
/// @brief *
///
/// \param u : *
/// \param v : *
///
void relacher(Graph &g, unsigned int u, unsigned int v) {
    EdgeProperties p = get(EdgeInfoPropertyTag(), g, edge(u, v, g).first);
    //std::cout << "weight: " << p.weight << std::endl;

    if ((g[u].d + p.weight) < g[v].d ) {
        g[v].d = g[u].d + p.weight;
        g[v].predecessor = u;
    }
}

///
/// @brief algo de dijkstra
///
/// \param g : graphe
///
/// \return le plus court chemin
///
list<unsigned int> dijkstra(Graph &g) {
    g[0].d = 0;             // Sommet de début
    list<unsigned int> F;   // liste des sommets à visiter
    list<unsigned int> E;   // liste des sommets avec longueurs finales de plus court chemin à partir de l’origine s
    for (unsigned int i = 0 ; i < num_vertices(g) ; i++)
        F.push_back(i);

    while (!F.empty()) {
        unsigned int u = extrait_min(g, F);
        E.push_back(u);

        // Visite les voisins du sommet u
        auto neighbours = boost::adjacent_vertices(u, g);
        for (auto v : make_iterator_range(neighbours)) {
            //std::cout << "0 has adjacent vertex " << v << "\n";
            relacher(g, u, v);
        }
    }

    // Lecture du résultat
    list<unsigned int> resultat;
    int x = num_vertices(g) - 1;
    resultat.push_back(x);
    while (0 < x) {
        x = g[x].predecessor;
        resultat.push_back(x);
    }

    return resultat;
}

int main() {
    // Crée un graphe g
    Graph g;

    // Crée une instance de graphe
    createInstanceGraph(g);

    // Affiche le graphe
    cout<<"[+] liste d'adjacence :"<<endl;
    print_graph(g);

    // Appel de l'algo dijkstra
    list<unsigned int> r = dijkstra(g);

    // Affiche le plus court chemin
    cout<<endl<<"[+] le plus court chemin :"<<endl;
    for (auto v : r)
        std::cout << v<<" ";
    cout<<endl;

    return EXIT_SUCCESS;
}


/*

 * @file LinearInterpolationVectorSpaceCurve.hpp
 * @date Aug 17, 2014
 * @author Paul Furgale, Abel Gawel, Renaud Dube

#include <curves/LinearInterpolationVectorSpaceCurve.hpp>
#include <iostream>
#include <boost/bind.hpp>
#include <boost/function.hpp>

namespace curves {

template<int N>
LinearInterpolationVectorSpaceCurve<N>::LinearInterpolationVectorSpaceCurve() : VectorSpaceCurve<N>() {}

template<int N>
LinearInterpolationVectorSpaceCurve<N>::~LinearInterpolationVectorSpaceCurve() {}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::print(const std::string& str) const {
  std::cout << "=========================================" << std::endl;
  std::cout << "=======LINEAR INTERPOLATION CURVE========" << std::endl;
  std::cout << str << std::endl;
  std::cout << "number of coefficients: " << manager_.size() << std::endl;
  std::cout << "dimension: " << N << std::endl;
  std::stringstream ss;
  std::vector<Key> keys;
  std::vector<Time> times;
  manager_.getTimes(&times);
  manager_.getKeys(&keys);
  std::cout << "curve defined between times: " << manager_.getMinTime() <<
      " and " << manager_.getMaxTime() <<std::endl;
  std::cout <<"=========================================" <<std::endl;
  for (size_t i = 0; i < manager_.size(); i++) {
    ss << "coefficient " << keys[i] << ": ";
    std::cout << ss.str() << manager_.getCoefficientByKey(keys[i]).transpose() << std::endl;
    std::cout << " | time: " << times[i];
    std::cout << std::endl;
    ss.str("");
  }
  std::cout <<"=========================================" <<std::endl;
}

template<int N>
Time LinearInterpolationVectorSpaceCurve<N>::getMaxTime() const {
  return manager_.getMaxTime();
}

template<int N>
Time LinearInterpolationVectorSpaceCurve<N>::getMinTime() const {
  return manager_.getMinTime();
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::fitCurve(const std::vector<Time>& times,
                                                      const std::vector<ValueType>& values,
                                                      std::vector<Key>* outKeys) {
  CHECK_EQ(times.size(), values.size());

  if(times.size() > 0) {
    manager_.clear();
    if (outKeys != NULL) {
      outKeys->clear();
      outKeys->reserve(times.size());
    }
    std::vector<Coefficient> coefficients;
    coefficients.reserve(times.size());
    size_t vsize = values[0].size();
    for(size_t i = 0; i < values.size(); ++i) {
      CHECK_EQ(vsize, values[i].size()) << "The vectors must be uniform length.";
      coefficients.push_back(Coefficient(values[i]));
    }
    manager_.insertCoefficients(times,coefficients,outKeys);
  }
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::extend(const std::vector<Time>& times,
                                                    const std::vector<ValueType>& values,
                                                    std::vector<Key>* outKeys) {

  CHECK_EQ(times.size(), values.size()) << "number of times and number of coefficients don't match";
  std::vector<Coefficient> coefficients(values.size());
  for (size_t i = 0; i < values.size(); ++i) {
    coefficients[i] = Coefficient(values[i]);
  }
  manager_.insertCoefficients(times, coefficients, outKeys);
}

template<int N>
typename LinearInterpolationVectorSpaceCurve<N>::ValueType
LinearInterpolationVectorSpaceCurve<N>::evaluate(Time time) const {
  CoefficientIter rval0, rval1;
  bool success = manager_.getCoefficientsAt(time, &rval0, &rval1);
  CHECK(success) << "Unable to get the coefficients at time " << time;

  Time dt = rval1->first - rval0->first;
  Time t = rval1->first - time;
  // Alpha goes from zero to one.
  double alpha = double(t)/double(dt);

  return alpha * rval0->second.coefficient + (1.0 - alpha) * rval1->second.coefficient;
}

template<int N>
typename LinearInterpolationVectorSpaceCurve<N>::DerivativeType
LinearInterpolationVectorSpaceCurve<N>::evaluateDerivative(Time time,
                                                           unsigned derivativeOrder) const {

  // time is out of bound --> error
  CHECK_GE(time, this->getMinTime()) << "Time out of bounds";
  CHECK_LT(time, this->getMaxTime()) << "Time out of bounds";
  CHECK_GT(derivativeOrder, 0) << "DerivativeOrder must be greater than 0";

  typename LinearInterpolationVectorSpaceCurve<N>::DerivativeType dCoeff;
  Time dt;
  CoefficientIter rval0, rval1;
  bool success = manager_.getCoefficientsAt(time, &rval0, &rval1);
  CHECK(success) << "Unable to get the coefficients at time " << time;
  // first derivative
  if (derivativeOrder == 1) {
    dCoeff = rval1->second.coefficient - rval0->second.coefficient;
    dt = rval1->first - rval0->first;
    return dCoeff/dt;
  } else { // order of derivative > 1 returns vector of zeros
    dCoeff.Zero();
    return dCoeff;
  }
}

// Evaluation function in functional form. To be passed to the expression
template<int N>
Eigen::Matrix<double,N,1> linearInterpolation(Eigen::Matrix<double,N,1>  v1,
                                              Eigen::Matrix<double,N,1>  v2, double alpha,
                                              gtsam::OptionalJacobian<N,N> H1,
                                              gtsam::OptionalJacobian<N,N> H2) {
  if (H1) { *H1 = Eigen::Matrix<double,N,N>::Identity()*(1-alpha); }
  if (H2) { *H2 = Eigen::Matrix<double,N,N>::Identity()*alpha; }

  return v1*(1-alpha) + v2*alpha;
}

template<int N>
gtsam::Expression<typename LinearInterpolationVectorSpaceCurve<N>::ValueType>
LinearInterpolationVectorSpaceCurve<N>::getValueExpression(const Time& time) const {
  typedef typename LinearInterpolationVectorSpaceCurve<N>::ValueType ValueType;
  using namespace gtsam;
  CoefficientIter rval0, rval1;
  bool success = manager_.getCoefficientsAt(time, &rval0, &rval1);
  CHECK(success) << "Unable to get the coefficients at time " << time;

  Expression<ValueType> leaf1(rval0->second.key);
  Expression<ValueType> leaf2(rval1->second.key);

  double alpha = double(time - rval0->first)/double(rval1->first - rval0->first);

  Expression<ValueType> rval(boost::bind(&linearInterpolation<N>, _1, _2, alpha, _3, _4),
                             leaf1, leaf2);

  return rval;
}

template<int N>
gtsam::Expression<typename LinearInterpolationVectorSpaceCurve<N>::DerivativeType>
LinearInterpolationVectorSpaceCurve<N>::getDerivativeExpression(const Time& time, unsigned derivativeOrder) const {
  // \todo Abel and Renaud
  CHECK(false) << "Not implemented";
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::initializeGTSAMValues(gtsam::KeySet keys, gtsam::Values* values) const {
  manager_.initializeGTSAMValues(keys, values);
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::initializeGTSAMValues(gtsam::Values* values) const {
  manager_.initializeGTSAMValues(values);
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::updateFromGTSAMValues(const gtsam::Values& values) {
  manager_.updateFromGTSAMValues(values);
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::clear() {
  manager_.clear();
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::addPriorFactors(gtsam::NonlinearFactorGraph* graph, Time priorTime) const {
  gtsam::noiseModel::Constrained::shared_ptr priorNoise = gtsam::noiseModel::Constrained::All(gtsam::traits<Coefficient>::dimension);

  // Constraint the coefficients which influence the curve value at priorTime
  CoefficientIter rVal0, rVal1;
  manager_.getCoefficientsAt(priorTime, &rVal0, &rVal1);

  gtsam::ExpressionFactor<Coefficient> factor0(priorNoise,
                                          rVal0->second.coefficient,
                                          gtsam::Expression<Coefficient>(rVal0->second.key));
  gtsam::ExpressionFactor<Coefficient> factor1(priorNoise,
                                          rVal1->second.coefficient,
                                          gtsam::Expression<Coefficient>(rVal1->second.key));
  graph->push_back(factor0);
  graph->push_back(factor1);
}

template<int N>
void LinearInterpolationVectorSpaceCurve<N>::transformCurve(const ValueType T) {
  //todo
}

template<int N>
Time LinearInterpolationVectorSpaceCurve<N>::getTimeAtKey(gtsam::Key key) const {
  return manager_.getCoefficientTimeByKey(key);
}

} // namespace curves
*/


/* Computes the joint distribution over all the latent variables in the model using Gibbs Sampling.
 *
 * This latent variable model adapted from the one described in "Learning an Input Filter for
 * Argument Structure Acquisition" by Perkins, Feldman, and Lidz. See the paper for details.
 */

#include "metropolis_hastings.hpp"
#include "sample_models.hpp"
#include "utils.hpp"

#include <CLI/CLI.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/log/core.hpp>
#include <boost/log/expressions.hpp>
#include <boost/log/trivial.hpp>
#include <chrono>
#include <fstream>
#include <iostream>
#include <random>
#include <string>
#include <tuple>
#include <utility>

using namespace FilterModel;
namespace logging = boost::log;

/**
 * Jointly inferrs the alpha vector of each object, the epsilon, and the delta value for some
 * objects by using a fixed number of iterations of gibbs sampling.
 *
 * The sampling uses the following flow:
 *   alpha -> epsilon -> delta.
 *
 * This function takes as input a vector of category count vectors of each object and the number of
 * iterations or time steps to sample over.
 *
 * This function outputs a tuple of three vectors. The first vector contains the sampled
 * alpha of each object in the data at each time step. So the first element of outer vector
 * is a vector of alphas sampled at the first time step, the second element is the
 * alphas at the second time step, etc. The second and third vectors are the sampled epsilon
 * and delta values at each time step respectively.
 */
std::tuple<std::vector<std::vector<alpha_t>>, std::vector<double>, std::vector<delta_t>,
           std::vector<double>>
joint_inference(std::vector<category_counts_t> &data, int iterations,
                std::function<void(std::vector<std::vector<alpha_t>>, std::vector<double>,
                                   std::vector<delta_t>, std::vector<double>)>
                    write_batch,
                int batch_size, Options options) {
    BOOST_LOG_TRIVIAL(info) << "Starting Gibbs Sampling";

    int n_categories = 0;
    if (data.size() > 0) {
        n_categories = data.at(0).size();
    }

    std::default_random_engine generator;
    generator.seed(std::chrono::system_clock::now().time_since_epoch().count());

    std::uniform_real_distribution<double> parameter_distribution(0.0, 1.0);
    std::vector<double> epsilons = {parameter_distribution(generator)};
    std::vector<delta_t> deltas = {
        sample_symmetric_simplex(parameter_distribution, generator, n_categories)};
    // Each item contains a vector of the estimated alpha of each object at each timestep.
    std::vector<std::vector<alpha_t>> models;
    models.push_back(std::vector<alpha_t>());

    ModelDistribution model_distribution(data, generator, options);
    ModelSampler sampler(data, generator, options);

    std::vector<double> log_likelyhoods;

    for (int iteration = 1; iteration <= iterations; ++iteration) {
        BOOST_LOG_TRIVIAL(info) << "Iteration " << iteration;

        if (options.fixed_alphas.empty()) {
            std::vector<alpha_t> new_models =
                sampler.sample(epsilons.at(iteration - 1), deltas.at(iteration - 1));
            models.push_back(new_models);
        } else {
            models.push_back(options.fixed_alphas);
        }

        epsilons.push_back(MetropolisHastingsSampler::sample<double, std::default_random_engine>(
            10,  // Iterations
            [delta = deltas.at(iteration - 1), model = models.at(iteration),
             model_distribution](double epsilon) {
                std::vector<std::vector<alpha_t>> alphas_per_object(model.size());
                for (int i = 0; i < alphas_per_object.size(); ++i) {
                    alphas_per_object.at(i) = std::vector<alpha_t>(1, model.at(i));
                }
                std::vector<std::vector<double>> log_alpha_likelyhoods_per_object =
                    model_distribution.distribution(alphas_per_object, epsilon, delta);
                std::vector<double> log_alpha_likelyhoods(log_alpha_likelyhoods_per_object.size());
                std::transform(log_alpha_likelyhoods_per_object.begin(),
                               log_alpha_likelyhoods_per_object.end(),
                               log_alpha_likelyhoods.begin(),
                               [](std::vector<double> v) { return v[0]; });

                return std::accumulate(log_alpha_likelyhoods.begin(), log_alpha_likelyhoods.end(),
                                       0.0);
            },  // log_pdf
            [&parameter_distribution](std::default_random_engine &generator) {
                return parameter_distribution(generator);
            },  // uniform_sampler
            [](double center, std::default_random_engine &generator) {
                std::normal_distribution<> dist(center, 0.25);
                return sample_probability(dist, generator);
            },  // conditional_sampler
            generator));

        deltas.push_back(MetropolisHastingsSampler::sample<delta_t, std::default_random_engine>(
            10,  // Iterations
            [epsilon = epsilons.at(iteration), model = models.at(iteration),
             model_distribution](delta_t delta) {
                std::vector<std::vector<alpha_t>> alphas_per_object(model.size());
                for (int i = 0; i < alphas_per_object.size(); ++i) {
                    alphas_per_object.at(i) = std::vector<alpha_t>(1, model.at(i));
                }
                std::vector<std::vector<double>> log_alpha_likelyhoods_per_object =
                    model_distribution.distribution(alphas_per_object, epsilon, delta);
                std::vector<double> log_alpha_likelyhoods =
                    flatten<double>(log_alpha_likelyhoods_per_object);

                return accumulate(log_alpha_likelyhoods.begin(), log_alpha_likelyhoods.end(), 0.0);
            },  // log_pdf
            [parameter_distribution, n_categories](std::default_random_engine &generator) {
                return sample_symmetric_simplex(parameter_distribution, generator, n_categories);
            },  // uniform_sampler
            [](delta_t center, std::default_random_engine &generator) {
                return sample_gaussian_simplex<>(center, 0.25, generator);
            },  // Conditional sampler
            generator));

        if (options.record_likelyhood) {
            std::vector<double> log_likelyhood_per_object =
                flatten<double>(model_distribution.distribution(
                    models.at(iteration), epsilons.at(iteration), deltas.at(iteration)));
            log_likelyhoods.push_back(std::accumulate(log_likelyhood_per_object.begin(),
                                                      log_likelyhood_per_object.end(), 0.0));
        }

        if (iteration % batch_size == batch_size - 1 || iteration == iterations) {
            int batch = iteration / batch_size + 1;
            int start_index = (batch - 1) * batch_size;
            int end_index = std::min(batch * batch_size, iterations + 1);

            std::vector<std::vector<alpha_t>> alpha_batch(models.begin() + start_index,
                                                          models.begin() + end_index);
            std::vector<double> epsilon_batch(epsilons.begin() + start_index,
                                              epsilons.begin() + end_index);
            std::vector<delta_t> delta_batch(deltas.begin() + start_index,
                                             deltas.begin() + end_index);
            std::vector<double> log_likelyhood_batch;
            if (options.record_likelyhood) {
                log_likelyhood_batch = std::vector<double>(log_likelyhoods.begin() + start_index,
                                                           log_likelyhoods.begin() + end_index);
            }

            write_batch(alpha_batch, epsilon_batch, delta_batch, log_likelyhood_batch);
        }
    }

    return std::make_tuple(models, epsilons, deltas, log_likelyhoods);
}

/**
 * Reads in a csv file of category_count_t objects in order.
 * See category_count_t in types.hpp for details.
 * Expects a header line.
 */
std::vector<category_counts_t> read_category_counts_file(std::string in_path) {
    std::ifstream in_file(in_path);
    std::string line;
    std::vector<category_counts_t> data;
    if (in_file.is_open()) {
        // Trim the header line
        getline(in_file, line);
        while (getline(in_file, line)) {
            std::vector<std::string> items;
            boost::split(items, line, boost::is_any_of(","));
            category_counts_t datum = {std::stoi(items.at(1)), std::stoi(items.at(2)),
                                       std::stoi(items.at(3))};
            data.push_back(datum);
        }
        in_file.close();
    } else {
        BOOST_LOG_TRIVIAL(fatal) << "File " << in_path << " not found.";
        assert(false);
    }

    return data;
}

std::vector<alpha_t> read_alphas(std::string alpha_path) {
    std::ifstream in_file(alpha_path);
    std::string line;
    std::vector<alpha_t> alphas;
    if (in_file.is_open()) {
        // Trim the header line
        getline(in_file, line);
        while (getline(in_file, line)) {
            std::vector<std::string> items;
            boost::split(items, line, boost::is_any_of(","));
            alpha_t alpha = {bool(std::stoi(items.at(1))), bool(std::stoi(items.at(2))),
                             bool(std::stoi(items.at(3)))};
            alphas.push_back(alpha);
        }
        in_file.close();
    } else {
        BOOST_LOG_TRIVIAL(fatal) << "File " << alpha_path << " not found.";
        assert(false);
    }

    return alphas;
}

std::ofstream setup_output(std::string out_path) {
    return std::ofstream(out_path, std::ios::out | std::ios::trunc);
}

/**
 * Sets the current logging level.
 */
void initLogging() {
    logging::core::get()->set_filter(logging::trivial::severity >= logging::trivial::info);
}

int main(int argc, char *const *argv) {
    initLogging();

    // Setting up command line flags.
    CLI::App app{"Input Filter Model"};

    std::string in_path = "input.csv";
    app.add_option("-i,--in", in_path,
                   "The path to a csv file containing pairs of direct object and object counts.")
        ->required();

    std::string out_path = "output.csv";
    app.add_option("-o,--output", out_path, "The path of the output file.")->required();

    int iterations = 1;
    app.add_option("--iterations", iterations,
                   "The number of iterations to run Gibbs sampling for.");

    std::string alpha_path;
    CLI::Option *alpha_path_option = app.add_option(
        "--alpha-path", alpha_path, "Path to fixed alpha values to use, only if provided.");

    std::string message;
    CLI::Option *message_option =
        app.add_option("-m, --message", message, "Message to be written at the top of the output.")
            ->required();

    bool record_likelyhood = false;
    CLI::Option *record_likelyhood_flag =
        app.add_flag("--record-likelyhood", record_likelyhood,
                     "Additionally record the likelyhood of the sampled parameters.");

    bool exact = false;
    CLI::Option *exact_flag =
        app.add_flag("--exact", exact, "Prevent approximations in calcualtion.");

    bool comparison = false;
    app.add_flag("--comparison", comparison,
                 "If both integration methods shosuld be used and compared.")
        ->excludes(exact_flag);

    bool use_smaller_alphas = false;
    CLI::Option *use_smaller_alphas_flag = app.add_flag("--use-smaller-alphas", use_smaller_alphas,
                                                        "Only consider alphas that are possible.")
                                               ->excludes(alpha_path_option);

    CLI11_PARSE(app, argc, argv);

    std::vector<alpha_t> alphas;
    if (!alpha_path.empty()) {
        alphas = read_alphas(alpha_path);
    }

    Options options;
    options.comparison = comparison;
    options.exact = exact;
    options.use_smaller_alphas = use_smaller_alphas;
    options.record_likelyhood = record_likelyhood;
    options.fixed_alphas = alphas;

    auto data = read_category_counts_file(in_path);

    std::ofstream out_file = setup_output(out_path);

    out_file << "Comment: " << message << ", Input path: " << in_path
             << ", Output path: " << out_path << ", Alpha path: " << alpha_path
             << ", Iterations: " << std::to_string(iterations)
             << ", Comparison: " << std::to_string(options.comparison)
             << ", Exact: " << std::to_string(options.exact)
             << ", Use smaller alphas: " << std::to_string(options.use_smaller_alphas)
             << ", Record likelyhood: " << std::to_string(options.record_likelyhood) << std::endl;

    auto write_batch = [&out_file](std::vector<std::vector<alpha_t>> alpha_batch,
                                   std::vector<double> epsilon_batch,
                                   std::vector<delta_t> delta_batch,
                                   std::vector<double> log_likelyhood_batch) {
        for (int i = 0; i < alpha_batch.size(); ++i) {
            if (!log_likelyhood_batch.empty()) {
                out_file << vector_of_vector_to_string<>(alpha_batch.at(i)) << ","
                         << epsilon_batch.at(i) << "," << vector_to_string<>(delta_batch.at(i))
                         << "," << log_likelyhood_batch.at(i) << std::endl;
            } else {
                out_file << vector_of_vector_to_string<>(alpha_batch.at(i)) << ","
                         << epsilon_batch.at(i) << "," << vector_to_string<>(delta_batch.at(i))
                         << std::endl;
            }
        }
    };

    joint_inference(data, iterations, write_batch, 100, options);

    BOOST_LOG_TRIVIAL(info) << "Inference complete.";

    out_file.close();
}