#ifndef COORDINATE_CALCULATION
#define COORDINATE_CALCULATION

#include "util/coordinate.hpp"

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

#include <algorithm>
#include <cmath>
#include <numeric>
#include <utility>
#include <vector>

namespace osrm
{
namespace util
{
namespace coordinate_calculation
{

namespace detail
{
const constexpr double DEGREE_TO_RAD = 0.017453292519943295769236907684886;
const constexpr double RAD_TO_DEGREE = 1. / DEGREE_TO_RAD;

inline double degToRad(const double degree)
{
    using namespace boost::math::constants;
    return degree * (pi<double>() / 180.0);
}

inline double radToDeg(const double radian)
{
    using namespace boost::math::constants;
    return radian * (180.0 * (1. / pi<double>()));
}
}

//! Takes the squared euclidean distance of the input coordinates. Does not return meters!
std::uint64_t squaredEuclideanDistance(const Coordinate lhs, const Coordinate rhs);

double fccApproximateDistance(const Coordinate first_coordinate,
                              const Coordinate second_coordinate);

double haversineDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);

double greatCircleDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);

// get the length of a full coordinate vector, using one of our basic functions to compute distances
template <class BinaryOperation, typename iterator_type>
double getLength(iterator_type begin, const iterator_type end, BinaryOperation op);

// Find the closest distance and location between coordinate and the line connecting source and
// target:
//             coordinate
//                 |
//                 |
// source -------- x -------- target.
// returns x as well as the distance between source and x as ratio ([0,1])
inline std::pair<double, FloatCoordinate> projectPointOnSegment(const FloatCoordinate &source,
                                                                const FloatCoordinate &target,
                                                                const FloatCoordinate &coordinate);

// find the closest distance between a coordinate and a segment
// O(1)
double findClosestDistance(const Coordinate coordinate,
                           const Coordinate segment_begin,
                           const Coordinate segment_end);

// find the closest distance between a coordinate and a set of coordinates
// O(|coordinates|)
template <typename iterator_type>
double findClosestDistance(const Coordinate coordinate,
                           const iterator_type begin,
                           const iterator_type end);

// find the closes distance between two sets of coordinates
// O(|lhs| * |rhs|)
template <typename iterator_type>
double findClosestDistance(const iterator_type lhs_begin,
                           const iterator_type lhs_end,
                           const iterator_type rhs_begin,
                           const iterator_type rhs_end);

// checks if two sets of coordinates describe a parallel set of ways
template <typename iterator_type>
bool areParallel(const iterator_type lhs_begin,
                 const iterator_type lhs_end,
                 const iterator_type rhs_begin,
                 const iterator_type rhs_end);

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location);

double perpendicularDistance(const Coordinate segment_source,
                             const Coordinate segment_target,
                             const Coordinate query_location,
                             Coordinate &nearest_location,
                             double &ratio);

Coordinate centroid(const Coordinate lhs, const Coordinate rhs);

double bearing(const Coordinate first_coordinate, const Coordinate second_coordinate);

// Get angle of line segment (A,C)->(C,B)
double computeAngle(const Coordinate first, const Coordinate second, const Coordinate third);

// find the center of a circle through three coordinates
boost::optional<Coordinate> circleCenter(const Coordinate first_coordinate,
                                         const Coordinate second_coordinate,
                                         const Coordinate third_coordinate);

// find the radius of a circle through three coordinates
double circleRadius(const Coordinate first_coordinate,
                    const Coordinate second_coordinate,
                    const Coordinate third_coordinate);

// factor in [0,1]. Returns point along the straight line between from and to. 0 returns from, 1
// returns to
Coordinate interpolateLinear(double factor, const Coordinate from, const Coordinate to);

// compute the signed area of a triangle
double signedArea(const Coordinate first_coordinate,
                  const Coordinate second_coordinate,
                  const Coordinate third_coordinate);

// check if a set of three coordinates is given in CCW order
bool isCCW(const Coordinate first_coordinate,
           const Coordinate second_coordinate,
           const Coordinate third_coordinate);

template <typename iterator_type>
std::pair<Coordinate, Coordinate> leastSquareRegression(const iterator_type begin,
                                                        const iterator_type end);

// rotates a coordinate around the point (0,0). This function can be used to normalise a few
// computations around regression vectors
Coordinate rotateCCWAroundZero(Coordinate coordinate, double angle_in_radians);

// compute the difference vector of two coordinates lhs - rhs
Coordinate difference(const Coordinate lhs, const Coordinate rhs);

// TEMPLATE/INLINE DEFINITIONS
inline std::pair<double, FloatCoordinate> projectPointOnSegment(const FloatCoordinate &source,
                                                                const FloatCoordinate &target,
                                                                const FloatCoordinate &coordinate)
{
    const FloatCoordinate slope_vector{target.lon - source.lon, target.lat - source.lat};
    const FloatCoordinate rel_coordinate{coordinate.lon - source.lon, coordinate.lat - source.lat};
    // dot product of two un-normed vectors
    const auto unnormed_ratio = static_cast<double>(slope_vector.lon * rel_coordinate.lon) +
                                static_cast<double>(slope_vector.lat * rel_coordinate.lat);
    // squared length of the slope vector
    const auto squared_length = static_cast<double>(slope_vector.lon * slope_vector.lon) +
                                static_cast<double>(slope_vector.lat * slope_vector.lat);

    if (squared_length < std::numeric_limits<double>::epsilon())
    {
        return {0, source};
    }

    const double normed_ratio = unnormed_ratio / squared_length;
    double clamped_ratio = normed_ratio;
    if (clamped_ratio > 1.)
    {
        clamped_ratio = 1.;
    }
    else if (clamped_ratio < 0.)
    {
        clamped_ratio = 0.;
    }

    return {clamped_ratio,
            {
                FloatLongitude{1.0 - clamped_ratio} * source.lon +
                    target.lon * FloatLongitude{clamped_ratio},
                FloatLatitude{1.0 - clamped_ratio} * source.lat +
                    target.lat * FloatLatitude{clamped_ratio},
            }};
}

template <class BinaryOperation, typename iterator_type>
double getLength(iterator_type begin, const iterator_type end, BinaryOperation op)
{
    double result = 0;
    const auto functor = [&result, op](const Coordinate lhs, const Coordinate rhs) {
        result += op(lhs, rhs);
        return false;
    };
    // side-effect find adding up distances
    std::adjacent_find(begin, end, functor);

    return result;
}

template <typename iterator_type>
double
findClosestDistance(const Coordinate coordinate, const iterator_type begin, const iterator_type end)
{
    double current_min = std::numeric_limits<double>::max();

    // comparator updating current_min without ever finding an element
    const auto compute_minimum_distance = [&current_min, coordinate](const Coordinate lhs,
                                                                     const Coordinate rhs) {
        current_min = std::min(current_min, findClosestDistance(coordinate, lhs, rhs));
        return false;
    };

    std::adjacent_find(begin, end, compute_minimum_distance);
    return current_min;
}

template <typename iterator_type>
double findClosestDistance(const iterator_type lhs_begin,
                           const iterator_type lhs_end,
                           const iterator_type rhs_begin,
                           const iterator_type rhs_end)
{
    double current_min = std::numeric_limits<double>::max();

    const auto compute_minimum_distance_in_rhs = [&current_min, rhs_begin, rhs_end](
        const Coordinate coordinate) {
        current_min = std::min(current_min, findClosestDistance(coordinate, rhs_begin, rhs_end));
        return false;
    };

    std::find_if(lhs_begin, lhs_end, compute_minimum_distance_in_rhs);
    return current_min;
}

template <typename iterator_type>
std::pair<Coordinate, Coordinate> leastSquareRegression(const iterator_type begin,
                                                        const iterator_type end)
{
    // following the formulas of https://faculty.elgin.edu/dkernler/statistics/ch04/4-2.html
    const auto number_of_coordinates = std::distance(begin, end);
    BOOST_ASSERT(number_of_coordinates >= 2);
    const auto extract_lon = [](const Coordinate coordinate) {
        return static_cast<double>(toFloating(coordinate.lon));
    };

    const auto extract_lat = [](const Coordinate coordinate) {
        return static_cast<double>(toFloating(coordinate.lat));
    };

    double min_lon = extract_lon(*begin);
    double max_lon = extract_lon(*begin);
    double min_lat = extract_lat(*begin);
    double max_lat = extract_lat(*begin);

    for (auto coordinate_iterator = begin; coordinate_iterator != end; ++coordinate_iterator)
    {
        const auto c = *coordinate_iterator;
        const auto lon = extract_lon(c);
        min_lon = std::min(min_lon, lon);
        max_lon = std::max(max_lon, lon);
        const auto lat = extract_lat(c);
        min_lat = std::min(min_lat, lat);
        max_lat = std::max(max_lat, lat);
    }
    // very small difference in longitude -> would result in inaccurate calculation, check if lat is
    // better
    if ((max_lat - min_lat) > 2 * (max_lon - min_lon))
    {
        std::vector<util::Coordinate> rotated_coordinates(number_of_coordinates);
        // rotate all coordinates to the right
        std::transform(begin, end, rotated_coordinates.begin(), [](const auto coordinate) {
            return rotateCCWAroundZero(coordinate, detail::degToRad(-90));
        });
        const auto rotated_regression =
            leastSquareRegression(rotated_coordinates.begin(), rotated_coordinates.end());
        return {rotateCCWAroundZero(rotated_regression.first, detail::degToRad(90)),
                rotateCCWAroundZero(rotated_regression.second, detail::degToRad(90))};
    }

    const auto make_accumulate = [](const auto extraction_function) {
        return [extraction_function](const double sum_so_far, const Coordinate coordinate) {
            return sum_so_far + extraction_function(coordinate);
        };
    };

    const auto accumulated_lon = std::accumulate(begin, end, 0., make_accumulate(extract_lon));

    const auto accumulated_lat = std::accumulate(begin, end, 0., make_accumulate(extract_lat));

    const auto mean_lon = accumulated_lon / number_of_coordinates;
    const auto mean_lat = accumulated_lat / number_of_coordinates;
    const auto make_variance = [](const auto mean, const auto extraction_function) {
        return [extraction_function, mean](const double sum_so_far, const Coordinate coordinate) {
            const auto difference = extraction_function(coordinate) - mean;
            return sum_so_far + difference * difference;
        };
    };

    // using the unbiased version, we divide by num_samples - 1 (see
    // http://mathworld.wolfram.com/SampleVariance.html)
    const auto sample_variance_lon =
        std::sqrt(std::accumulate(begin, end, 0., make_variance(mean_lon, extract_lon)) /
                  (number_of_coordinates - 1));

    // if we don't change longitude, return the vertical line as is
    if (std::abs(sample_variance_lon) <
        std::numeric_limits<decltype(sample_variance_lon)>::epsilon())
        return {*begin, *(end - 1)};

    const auto sample_variance_lat =
        std::sqrt(std::accumulate(begin, end, 0., make_variance(mean_lat, extract_lat)) /
                  (number_of_coordinates - 1));

    if (std::abs(sample_variance_lat) <
        std::numeric_limits<decltype(sample_variance_lat)>::epsilon())
        return {*begin, *(end - 1)};
    const auto linear_correlation =
        std::accumulate(begin,
                        end,
                        0.,
                        [&](const auto sum_so_far, const auto current_coordinate) {
                            return sum_so_far +
                                   (extract_lon(current_coordinate) - mean_lon) *
                                       (extract_lat(current_coordinate) - mean_lat) /
                                       (sample_variance_lon * sample_variance_lat);
                        }) /
        (number_of_coordinates - 1);

    const auto slope = linear_correlation * sample_variance_lat / sample_variance_lon;
    const auto intercept = mean_lat - slope * mean_lon;

    const auto GetLatAtLon = [intercept,
                              slope](const util::FloatLongitude longitude) -> util::FloatLatitude {
        return {intercept + slope * static_cast<double>((longitude))};
    };

    const double offset = 0.00001;
    const Coordinate regression_first = {
        toFixed(util::FloatLongitude{min_lon - offset}),
        toFixed(util::FloatLatitude(GetLatAtLon(util::FloatLongitude{min_lon - offset})))};
    const Coordinate regression_end = {
        toFixed(util::FloatLongitude{max_lon + offset}),
        toFixed(util::FloatLatitude(GetLatAtLon(util::FloatLongitude{max_lon + offset})))};

    return {regression_first, regression_end};
}

template <typename iterator_type>
bool areParallel(const iterator_type lhs_begin,
                 const iterator_type lhs_end,
                 const iterator_type rhs_begin,
                 const iterator_type rhs_end)
{
    const auto regression_lhs = leastSquareRegression(lhs_begin, lhs_end);
    const auto regression_rhs = leastSquareRegression(rhs_begin, rhs_end);

    const auto null_island = Coordinate(FixedLongitude{0}, FixedLatitude{0});
    const auto difference_lhs = difference(regression_lhs.first, regression_lhs.second);
    const auto difference_rhs = difference(regression_rhs.first, regression_rhs.second);

    // we normalise the left slope to be zero, so we rotate the coordinates around 0,0 to match 90
    // degrees
    const auto bearing_lhs = bearing(null_island, difference_lhs);

    // we rotate to have one of the lines facing horizontally to the right (bearing 90 degree)
    const auto rotation_angle_radians = detail::degToRad(bearing_lhs - 90);
    const auto rotated_difference_rhs = rotateCCWAroundZero(difference_rhs, rotation_angle_radians);

    const auto get_slope = [](const Coordinate from, const Coordinate to) {
        const auto diff_lat = static_cast<int>(from.lat) - static_cast<int>(to.lat);
        const auto diff_lon = static_cast<int>(from.lon) - static_cast<int>(to.lon);
        if (diff_lon == 0)
            return std::numeric_limits<double>::max();
        return static_cast<double>(diff_lat) / static_cast<double>(diff_lon);
    };

    const auto slope_rhs = get_slope(null_island, rotated_difference_rhs);
    // the left hand side has a slope of `0` after the rotation. We can check the slope of the right
    // hand side to ensure we only considering slight slopes
    return std::abs(slope_rhs) < 0.20; // twenty percent incline at the most
}

double computeArea(const std::vector<Coordinate> &polygon);

} // ns coordinate_calculation
} // ns util
} // ns osrm

#endif // COORDINATE_CALCULATION


/*=================================================================================
 *	                    Copyleft! 2018 William Yu
 *          Some rights reserved：CC(creativecommons.org)BY-NC-SA
 *                      Copyleft! 2018 William Yu
 *      版权部分所有，遵循CC(creativecommons.org)BY-NC-SA协议授权方式使用
 *
 * Filename                :
 * Description             : 视觉SLAM十四讲/ch4 学习记录
 *                           李代数库
 * Reference               :
 * Programmer(s)           : William Yu, windmillyucong@163.com
 * Company                 : HUST, DMET国家重点实验室FOCUS团队
 * Modification History	   : ver1.0, 2018.03.26, William Yu

=================================================================================*/
/// Include files
#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "sophus/so3.h"
#include "sophus/se3.h"

/// Global Variables

/// Function definitions

/**
 * @function main
 * @author William Yu
 * @brief
 * @param  None
 * @retval None
 */
int main( int argc, char** argv )
{
    //--------------------------------------------------------------------------------------------------
    //  旋转矩阵
    //--------------------------------------------------------------------------------------
    // 沿Z轴转90度的旋转矩阵
    // 　                                  角度　　                  轴　　罗德里格公式进行转换为旋转矩阵
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();

    //--------------------------------------------------------------------------------------------------
    //  李群
    //--------------------------------------------------------------------------------------
    Sophus::SO3 SO3_R(R);               // Sophus::SO(3)可以直接从旋转矩阵构造
    Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  // 亦可从旋转向量构造
    Eigen::Quaterniond q(R);            // 或者四元数
    Sophus::SO3 SO3_q( q );
    // 上述表达方式都是等价的
    // 输出SO(3)时，以so(3)形式输出
    cout<<"SO(3) from matrix: "<<SO3_R<<endl;
    cout<<"SO(3) from vector: "<<SO3_v<<endl;
    cout<<"SO(3) from quaternion :"<<SO3_q<<endl;

    //--------------------------------------------------------------------------------------------------
    //  李代数
    //--------------------------------------------------------------------------------------
    // 使用对数映射获得它的李代数
    Eigen::Vector3d so3 = SO3_R.log();
    cout<<"so3 = "<<so3.transpose()<<endl;
    // hat 为向量到反对称矩阵
    cout<<"so3 hat=\n"<<Sophus::SO3::hat(so3)<<endl;
    // 相对的，vee为反对称到向量
    cout<<"so3 hat vee= "<<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<<endl; // transpose纯粹是为了输出美观一些

    //--------------------------------------------------------------------------------------------------
    //  李代数求导 更新
    //--------------------------------------------------------------------------------------
    // 增量扰动模型的更新
    Eigen::Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
    Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R;
    cout<<"SO3 updated = "<<SO3_updated<<endl;

    /********************我是萌萌的分割线*****萌萌是谁？************************/
    //说实话，在这份代码里面你可能经常看到一些略微卖萌的成分，
    //要么是高翔博士的原版代码在卖萌，
    //要么是我被高博的代码带上了一条不归路，总之，开心就好：）

    cout<<"************我是分割线*************"<<endl;

    //--------------------------------------------------------------------------------------------------
    //  特殊欧式群  变换矩阵群
    //--------------------------------------------------------------------------------------
    // 对SE(3)操作大同小异
    Eigen::Vector3d t(1,0,0);           // 沿X轴平移1
    Sophus::SE3 SE3_Rt(R, t);           // 从R,t构造SE(3)
    Sophus::SE3 SE3_qt(q,t);            // 从q,t构造SE(3)
    cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;
    cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;
    // 李代数se(3) 是一个六维向量，方便起见先typedef一下
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout<<"se3 = "<<se3.transpose()<<endl;
    // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
    // 同样的，有hat和vee两个算符
    cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl;
    cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl;

    // 最后，演示一下更新
    Vector6d update_se3; //更新量
    update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;
    cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;

    return 0;
}


/* Copyright (C) 2013 Ion Torrent Systems, Inc. All Rights Reserved */

#include <iostream>
#include <algorithm>
#include "PcaSpline.h"
#include "Utils.h"
#include <malloc.h>
//#define EIGEN_USE_MKL_ALL 1
#include <Eigen/Dense>
#include <Eigen/LU>

using namespace std;
using namespace Eigen;

/**
 * Create the basis splines for order requested at a particular value of x.
 * From "A Practical Guide To Splines - Revised Edition" by Carl de Boor p 111
 * the algorithm for BSPLVB. The value of the spline coefficients can be calculated
 * based on a cool recursion. This is a simplified and numerically stable algorithm
 * that seems to be the basis of most bspline implementations including gsl, matlab,etc.
 * @param all_knots - Original knots vector augmented with order number of endpoints
 * @param n_knots - Total number of knots.
 * @param order - Order or order of polynomials, 4 for cubic splines.
 * @param i - index of value x in knots such that all_knots[i] <= x && all_knots[i+1] > x
 * @param x - value that we want to evaluate spline at.
 * @param b - memory to write our coefficients into, must be at least order long.
 */
void basis_spline_xi(float *all_knots, int n_knots, int order, int i, float x, float *b) {
  b[0] = 1;
  double kr[order];
  double kl[order];
  double term;
  double saved;
  for (int j = 0; j < order - 1; j++) {
    kr[j] = all_knots[i + j + 1] - x; // k right values
    kl[j] = x - all_knots[i - j];     // k left values
    saved = 0;
    for (int r = 0; r <= j; r++) {
      term = b[r] / (kr[r] + kl[j-r]);
      b[r] = saved + kr[r] * term;
      saved = kl[j-r] * term;
    }
    b[j+1] = saved;
  }
}

/**
 * Create the augmented knots vector and fill in matrix Xt with the spline basis vectors
 */
void basis_splines_endreps_local(float *knots, int n_knots, int order, int *boundaries, int n_boundaries,  MatrixXf &Xt) {
  assert(n_boundaries == 2 && boundaries[0] < boundaries[1]);
  int n_rows = boundaries[1] - boundaries[0]; // this is frames so evaluate at each frame we'll need
  int n_cols = n_knots + order;  // number of basis vectors we'll have at end
  Xt.resize(n_cols, n_rows); // swapped to transpose later. This order lets us use it as a scratch space
  Xt.fill(0);
  int n_std_knots = n_knots + 2 * order;
  float std_knots[n_std_knots];

  // Repeat the boundary knots on there to ensure linear out side of knots
  for (int i = 0; i < order; i++) {
    std_knots[i] = boundaries[0];
  }
  // Our original boundary knots here
  for (int i = 0; i < n_knots; i++) {
    std_knots[i+order] = knots[i];
  }
  // Repeat the boundary knots on there to ensure linear out side of knots
  for (int i = 0; i < order; i++) {
    std_knots[i+order+n_knots] = boundaries[1];
  }
  // Evaluate our basis splines at each frame.
  for (int i = boundaries[0]; i < boundaries[1]; i++) {
    int idx = -1;
    // find index such that i >= knots[idx] && i < knots[idx+1]
    float *val = std::upper_bound(std_knots, std_knots + n_std_knots - 1, 1.0f * i);
    idx = val - std_knots - 1;
    assert(idx >= 0);
    float *f = Xt.data() + i * n_cols + idx - (order - 1); //column offset
    basis_spline_xi(std_knots, n_std_knots, order, idx, i, f);
  }
  // Put in our conventional format where each column is a basis vector
  Xt.transposeInPlace();
}

void PcaSpline::FillInKnots(const std::string &strategy, int n_frame, std::vector<float> &knots) {
  if (strategy != "no-knots") {
    if (strategy == "even4") {
      float stride = n_frame / 3.0f;
      knots.push_back(stride);
      knots.push_back(stride * 2.0f);
    }
    else if (strategy == "middle") {
      float stride = n_frame / 2.0f;
      knots.push_back(stride);
    }
    else if (strategy.find("explicit:") == 0) {
      string spec = strategy.substr(9, strategy.length() - 9);
      knots = char2Vec<float>(spec.c_str(), ',');
    }
    else {
      assert(false);
    }
  }
}

int PcaSpline::NumBasisVectors(int n_pca, int n_frames, int n_order, const std::string &knot_strategy) {
  std::vector<float> knots;
  FillInKnots(knot_strategy, n_frames, knots);
  int spline_basis = 0;
  if (knots.size() > 0) {
    spline_basis = knots.size() + n_order;
  }
  return spline_basis + n_pca;
}

void PcaSpline::LossyCompress(int n_wells, int n_frame, float *image,
			      int n_sample_wells, float *image_sample,
			      PcaSplineRegion &compressed) {
  Map<MatrixXf, Aligned> Ysub(image_sample, n_sample_wells, n_frame);
  Map<MatrixXf, Aligned> Y(image, n_wells, n_frame);
  MatrixXf C = Ysub.transpose() * Ysub;
  SelfAdjointEigenSolver<MatrixXf> es;
  es.compute(C);
  MatrixXf Pca_Basis = es.eigenvectors();
  MatrixXf Spline_Basis;
  vector<float> knots;
  FillInKnots(m_knot_strategy, n_frame, knots);
  if (!knots.empty()) {
    int boundaries[2];
    boundaries[0] = 0;
    boundaries[1] = n_frame;
    basis_splines_endreps_local(&knots[0], knots.size(), m_order, boundaries, sizeof(boundaries)/sizeof(boundaries[0]), Spline_Basis);
  }
  assert(compressed.n_basis == m_num_pca + Spline_Basis.cols());
  Map<MatrixXf, Aligned> Basis(compressed.basis_vectors, compressed.n_frames, compressed.n_basis);
  for(int i = 0; i < m_num_pca; i++) {
    Basis.col(i) = Pca_Basis.col(Pca_Basis.cols() - i - 1);
  }
  for (int i = 0; i < Spline_Basis.cols(); i++) {
    Basis.col(i+m_num_pca) = Spline_Basis.col(i);
  }
  MatrixXf SX;
  if (m_tikhonov_reg > 0) {
    MatrixXf diag(Basis.cols(), Basis.cols());
    diag.fill(0);
    for (int i = 0; i < diag.rows(); i++) {
      diag(i,i) = 1.0f * m_tikhonov_reg;
    }
    SX = ((Basis.transpose() * Basis) + (diag.transpose() * diag)).inverse() * Basis.transpose();
  }
  else {
    SX = (Basis.transpose() * Basis).inverse() * Basis.transpose();
  }
  Map<MatrixXf, Aligned> B(compressed.coefficients, n_wells, Basis.cols());
  B = Y * SX.transpose();
}

void PcaSpline::SampleImageMatrix(int n_wells, int n_frames, float * __restrict orig, int *__restrict trcs_state, int target_sample,
				  int &n_sample, float ** __restrict sample) {
  int good_wells = 0;
  int * __restrict state_begin = trcs_state;
  int * __restrict state_end  = trcs_state + n_wells;
  while (state_begin != state_end) {
    if (*state_begin++ >= 0) {
      good_wells++;
    }
  }
  int sample_rate = max(1,good_wells/target_sample);
  int n_sample_rows = (int)(ceil((float)good_wells/sample_rate));
  n_sample = n_sample_rows;
  *sample = (float *) memalign(32, sizeof(float) * n_frames * n_sample_rows);
  for (int i = 0; i < n_frames; i++) {
    float *__restrict col_start = orig + i * n_wells;
    float *__restrict col_end = col_start + n_wells;
    state_begin = trcs_state;
    float *__restrict sample_col_begin = (*sample) + n_sample_rows * i;
    int check_count = 0;
    int good_count = sample_rate - 1;
    while (col_start != col_end) {
      if (*state_begin >= 0 && ++good_count == sample_rate) {
	*sample_col_begin++ = *col_start;
	good_count = 0;
	check_count++;
      }
      col_start++;
    }
    assert(check_count == n_sample_rows);
  }
}

void PcaSpline::LossyUncompress(int n_wells, int n_frame, float *image,
				const PcaSplineRegion &compressed) {
  Map<MatrixXf, Aligned> Yh(image, n_wells, compressed.n_frames);
  Map<MatrixXf, Aligned> Basis(compressed.basis_vectors, compressed.n_frames, compressed.n_basis);
  Map<MatrixXf, Aligned> B(compressed.coefficients, n_wells, compressed.n_basis);
  Yh = B * Basis.transpose();
}


/**
 * @file
 * @author Denise Ratasich
 * @date 14.10.2013
 *
 * @brief Implement sampling from specific distributions.
 *
 * Currently only Gaussian distributions can be choosen for process
 * and covariance noise. Hence only sampling from a Gaussian
 * distribution is implemented.
 */

#include "probability/sampling.h"

#include <stdexcept>
#include <cmath>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_real.hpp>

namespace probability
{
  // create the rng only once (otherwise the numbers generated are
  // always the same)
  static boost::mt19937 rng(time(NULL));

  VectorXd sampleNormalDistribution(VectorXd mean, MatrixXd covariance)
  {
    // save last covariance (so Cholesky decomposition needs not to
    // be done every time entering this function)
    static MatrixXd cov;
    static MatrixXd T;			// transform matrix when cov is not diagonal

    // additionally needed for rng
    boost::normal_distribution<> dist(0,1);	// mean = 0, standard deviation = 1
    boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > random(rng, dist);

    VectorXd sample = VectorXd::Zero(mean.size());

    // check if covariance is diagonal (set diagonal to zero and
    // compare with MatrixXD::Zero)
    if (covariance.isDiagonal())
    {
      // sample each variable independently
      for (int i = 0; i < sample.size(); i++)
	sample[i] = mean[i] + random() * sqrt(covariance(i,i));
    }
    else
    {
      // x ~ N(0,C^-1)
      // C = L*L^T
      // sample x = L^-T * z, where z ~ N(0,I)

      // same covariance as before? (do calculation of L only once!)
      if (cov.rows() != covariance.rows() ||
	  cov.cols() != covariance.cols() ||
	  cov != covariance)
	{
	  cov = covariance;

	  // Cholesky decomposition of inverse covariance to get L;
	  // the covariance must be a symmetric and positive definite
	  // matrix (a covariance has these features, but the user
	  // must configure it so)
	  covariance = covariance.inverse();
	  MatrixXd L = covariance.llt().matrixL();

	  MatrixXd checkLL = L*L.transpose();
	  for (int r = 0; r < covariance.rows(); r++)
	    for (int c = 0; c < covariance.cols(); c++)
	      if (checkLL(r,c) - covariance(r,c)  >  0.000001)
		throw std::runtime_error("sampleNormalDistribution: Cholesky decomposition failed.");

	  T = L.inverse().transpose();
	}

      // sample z: z_i ~ N(0,1)
      for (int i = 0; i < sample.size(); i++)
	sample[i] = random();

      // transform to x (and add mean!)
      sample = mean + T * sample;
    }

    return sample;
  }

  VectorXd sampleUniformDistribution(VectorXd a, VectorXd b)
  {
    VectorXd sample = VectorXd::Zero(a.size());

    if (a.size() != b.size())
      throw std::runtime_error("sampleUniformDistribution: Bounds a and b must have equal sizes.");

    // additionally needed for rng
    boost::uniform_real<> dist(0,1);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<> >
      random(rng, dist);

    // sample each variable independently
    for (int i = 0; i < sample.size(); i++)
      sample[i] = a[i] + random() * (b[i]-a[i]);

    return sample;
  }
}


// written for clarity not efficiency.

#include <iostream>
using std::cout;
using std::endl;

#include <boost/array.hpp>
#include <boost/circular_buffer.hpp>

class Subtractive_generator {
private:
    static const int param_i = 55;
    static const int param_j = 24;
    static const int initial_load = 219;
    static const int mod = 1e9;
    boost::circular_buffer<int> r;
public:
    Subtractive_generator(int seed);
    int next();
    int operator()(){return next();}
};

Subtractive_generator::Subtractive_generator(int seed)
:r(param_i)
{
    boost::array<int, param_i> s;
    s[0] = seed;
    s[1] = 1;
    for(int n = 2; n < param_i; ++n){
        int t = s[n-2]-s[n-1];
        if (t < 0 ) t+= mod;
        s[n] = t;
    }

    for(int n = 0; n < param_i; ++n){
	int i = (34 * (n+1)) % param_i;
        r.push_back(s[i]);
    }
    for(int n = param_i; n <= initial_load; ++n) next();
}

int Subtractive_generator::next()
{
    int t = r[0]-r[31];
    if (t < 0) t += mod;
    r.push_back(t);
    return r[param_i-1];
}

int main()
{
    Subtractive_generator rg(292929);

    cout << "result = " << rg() << endl;
    cout << "result = " << rg() << endl;
    cout << "result = " << rg() << endl;
    cout << "result = " << rg() << endl;
    cout << "result = " << rg() << endl;
    cout << "result = " << rg() << endl;
    cout << "result = " << rg() << endl;

    return 0;
}


#ifndef LATTICE_HPP
#define LATTICE_HPP

#include <vector>
#include <memory>
#include <utility>
#include <complex>

#include <Eigen/Dense>

/**  class representing a general lattice structure
	parameters
		- lattice basis vectors
		- which lattice sites are coupled
*/

using namespace Eigen;

//a structure given for making the lattice
struct basis{
	//matrix that contains basis indexes
	std::vector<std::pair<int,double>> pairing;
};

class Lattice{

public:

	//lattice basis vectors
	MatrixXf basis_vectors;

	//closest particle vectors
	//  R = r_aa r_ba r_ca
	//		r_ab r_bb r_cb
	//		r_ac r_bc r_cc
	std::vector<std::shared_ptr<MatrixXf>> R;

	//t_matrix << 0, -t,-t,
	//			-t, 0, -t,
	//			-t, -t, 0;
	MatrixXf t_matrix;

	//dimensions
	int bands;
	int dim;

	//all the kinetic hamiltonians
	std::vector<MatrixXcf> H_kin_matrices;

	//all the k_values
	std::vector<MatrixXf> k_vectors;

	const std::complex<float> IF = std::complex<float>(0.0f, 1.0f);

	//constructor
	Lattice(const MatrixXf& basis, const std::vector<std::shared_ptr<MatrixXf>>& r, const MatrixXf& t_mat) :
		basis_vectors(basis), bands(t_mat.cols()), dim(basis.cols()),
		R(r), t_matrix(t_mat) {}

	//destructor
	~Lattice() {};

	int Bands(){
		return bands;
	}

	int Dim(){
		return dim;
	}

	//calculates the kinetic hamiltonian for a given k
	MatrixXcf KineticHamiltonian(const VectorXf& k){
		MatrixXcf H_kin(bands,bands);
		H_kin = MatrixXcf::Zero(bands,bands);
		for(int l=0; l<R.size(); l++){
			MatrixXf R_iter = *R[l];

			for(int i = 0; i<bands; i++){
				for(int j =0; j< bands; j++){

					//make a complex 1by1 matrix
					MatrixXcf dot_ra(1,1);
					//fill the imaginary part with i*dot(k*r_ab)
					dot_ra.imag() = ( R_iter.block(i*dim,j,dim,1).transpose() * k );

					dot_ra.real() = MatrixXf::Zero(1,1);

					//taking the term to direction -r_ab
					MatrixXcf neg_dot_ra = dot_ra;
					neg_dot_ra.imag() = -neg_dot_ra.imag();

					//summing the exponentials
					MatrixXcf coupling = dot_ra.array().exp() + neg_dot_ra.array().exp();

					//the coupling is multiplied by corresponding t

					H_kin.block(i,j,1,1) += -t_matrix(i,j) * coupling;

				}
			}
		}
		return H_kin;
	}

	//this saves the h_kin_matrics
	void SaveKineticHamiltonians(const float& k_min, const float& k_max){
		int Nk = 20;
		VectorXf kx = VectorXf::LinSpaced(Nk,k_min,k_max);
		VectorXf ky = VectorXf::LinSpaced(Nk,k_min,k_max);

		for(int i = 0; i<Nk ; i++){
			for(int j = 0; j<Nk; j++){
				MatrixXf k(dim,1);
				k << kx(i),ky(j);

				k_vectors.push_back(k);
				H_kin_matrices.push_back(KineticHamiltonian(k) );

			}
		}
	}

	//solves the eigenvalues of one k by selfadjointeigensolver of Eigen package
	//@return: column vector of eigenvalues with smallest eigenvalue first
	MatrixXf EigenvaluesOneK(const VectorXf& k){

		SelfAdjointEigenSolver<MatrixXcf> es(bands);

		es.compute(KineticHamiltonian(k));

		return es.eigenvalues();

	}

	//calculates the dispersion over the first brillouin zone
	//returns a matrix of [kx ky E1 E2 E3]
	MatrixXf CalculateDispersion(const float& k_min, const float& k_max){

		int Nk = 20;
		VectorXf kx = VectorXf::LinSpaced(Nk,k_min,k_max);
		VectorXf ky = VectorXf::LinSpaced(Nk,k_min,k_max);

		MatrixXf k_values(Nk*Nk, dim);

		MatrixXf Energies(Nk*Nk, bands);

		for(int i = 0; i<Nk ; i++){
			for(int j = 0; j<Nk; j++){
				MatrixXf k(1,dim);
				k << kx(i),ky(j);
				k_values.row(i*Nk + j) = k;

				Energies.row(i*Nk + j) = EigenvaluesOneK(k.transpose()).transpose();

			}
		}

		MatrixXf data(Nk*Nk,dim+bands);
		data.leftCols(dim) = k_values;
		data.rightCols(bands) = Energies;
		return data;

	}

};

#endif


//  GAMBIT: Global and Modular BSM Inference Tool
//  *********************************************
///  \file
///
///  Multivariate Gaussian prior
///
///  *********************************************
///
///  Authors (add name and date if you modify):
///
///  \author Ben Farmer
///  (benjamin.farmer@monash.edu.au)
///  \date 2013 Dec
///
///  \author Gregory Martinez
///  (gregory.david.martinez@gmail.com)
///  \date Feb 2014
///
///  \author Andrew Fowlie
///    (andrew.j.fowlie@qq.com)
///  \date August 2020
///
///  *********************************************

#ifndef __PRIOR_GAUSSIAN_HPP__
#define __PRIOR_GAUSSIAN_HPP__

#include <algorithm>
#include <cmath>
#include <string>
#include <unordered_map>
#include <vector>

#include "gambit/ScannerBit/cholesky.hpp"
#include "gambit/ScannerBit/priors.hpp"
#include "gambit/Utils/yaml_options.hpp"

#include <boost/math/special_functions/erf.hpp>

namespace Gambit
{
  namespace Priors
  {
    /**
     * @brief  Multi-dimensional Gaussian prior
     *
     * Defined by a covariance matrix and mean.
     *
     * If the covariance matrix is diagonal, it may instead be specified by the square-roots of its
     * diagonal entries, denoted \f$\sigma\f$.
     */
    class Gaussian : public BasePrior
    {
     private:
      std::vector <double> mu;
      mutable Cholesky col;

     public:
      // Constructor defined in gaussian.cpp
      Gaussian(const std::vector<std::string>&, const Options&);

      /** @brief Transformation from unit interval to the Gaussian */
      void transform(const std::vector <double> &unitpars, std::unordered_map<std::string, double> &outputMap) const
      {
        std::vector<double> vec(unitpars.size());

        auto v_it = vec.begin();
        for (auto elem_it = unitpars.begin(), elem_end = unitpars.end(); elem_it != elem_end; elem_it++, v_it++)
        {
          *v_it = M_SQRT2 * boost::math::erf_inv(2. * (*elem_it) - 1.);
        }

        col.ElMult(vec);

        v_it = vec.begin();
        auto m_it = mu.begin();
        for (auto str_it = param_names.begin(), str_end = param_names.end(); str_it != str_end; str_it++)
        {
          outputMap[*str_it] = *(v_it++) + *(m_it++);
        }
      }

      std::vector<double> inverse_transform(const std::unordered_map<std::string, double> &physical) const override
      {
        // subtract mean
        std::vector<double> central;
        for (int i = 0, n = this->size(); i < n; i++)
        {
          central.push_back(physical.at(param_names[i]) - mu[i]);
        }

        // invert rotation by Cholesky matrix
        std::vector<double> rotated = col.invElMult(central);

        // now diagonal; invert Gaussian CDF
        std::vector<double> u;
        for (const auto& v : rotated)
        {
          u.push_back(0.5 * (boost::math::erf(v / M_SQRT2) + 1.));
        }
        return u;
      }

      double operator()(const std::vector<double> &vec) const
      {
        static double norm = 0.5 * std::log(2. * M_PI * std::pow(col.DetSqrt(), 2));
        return -0.5 * col.Square(vec, mu) - norm;
      }
    };

    LOAD_PRIOR(gaussian, Gaussian)

  }  // namespace Priors
}  // namespace Gambit

#endif  // __PRIOR_GAUSSIAN_HPP__


#include <itomp_cio_planner/util/exponential_map.h>
#include <boost/math/special_functions/sinc.hpp>

namespace itomp_cio_planner
{
namespace exponential_map
{

Eigen::Vector3d RotationToExponentialMap(const Eigen::Matrix3d& matrix, const Eigen::Vector3d* close_to)
{
    if (close_to == NULL)
        return QuaternionToExponentialMap(Eigen::Quaterniond(matrix));
    else
    {
        Eigen::Quaterniond q0 = Eigen::Quaterniond(matrix);
        Eigen::Vector3d exp_map0 = QuaternionToExponentialMap(q0);

        Eigen::Quaterniond q1 = q0;
        q1.coeffs() = -q0.coeffs();
        Eigen::Vector3d exp_map1 = QuaternionToExponentialMap(q1);

        if ((exp_map0 - *close_to).norm() <= (exp_map1 - *close_to).norm())
            return exp_map0;
        else
            return exp_map1;
    }
}

Eigen::Matrix3d ExponentialMapToRotation(const Eigen::Vector3d& exponential_rotation)
{
	return ExponentialMapToQuaternion(exponential_rotation).toRotationMatrix();
}

Eigen::Vector3d QuaternionToExponentialMap(const Eigen::Quaterniond& quaternion)
{
	Eigen::Vector3d vec = quaternion.vec();
    if (vec.norm() < ITOMP_EPS)
		return Eigen::Vector3d::Zero();

	double theta = 2.0 * std::acos(quaternion.w());
	vec.normalize();
	return theta * vec;
}

Eigen::Quaterniond ExponentialMapToQuaternion(const Eigen::Vector3d& exponential_rotation)
{
	double angle = 0.5 * exponential_rotation.norm();
	Eigen::Quaterniond quaternion;
	quaternion.w() = std::cos(angle);
	quaternion.vec() = 0.5 * boost::math::sinc_pi(angle) * exponential_rotation;
	return quaternion;
}

}
}


// Implementing the class that is defined in the header file: Option.hpp
//
// (c) Sudhansh Dua

#include "EuropeanOption.hpp"
#include <cmath>
#include <boost/math/distributions.hpp>

using namespace std;
using namespace boost::math;

//	Gaussian functions using boost libraries
double EuropeanOption::N(double x) const
{
	normal_distribution<> Standard_normal(0.0, 1.0);
	return cdf(Standard_normal, x);
}

double EuropeanOption::n(double x) const
{
	normal_distribution<> Standard_normal(0.0, 1.0);
	return pdf(Standard_normal, x);
}

//	Kernel functions
double EuropeanOption::CallPrice() const
{
	return ::CallPrice(S, K, T, r, sig, b);
}

double EuropeanOption::PutPrice() const
{
	return ::PutPrice(S, K, T, r, sig, b);
}

double EuropeanOption::CallDelta() const
{
	return ::CallDelta(S, K, T, r, sig, b);
}

double EuropeanOption::PutDelta() const
{
	return ::PutDelta(S, K, T, r, sig, b);
}

double EuropeanOption::CallGamma() const
{
	return ::CallGamma(S, K, T, r, sig, b);
}

double EuropeanOption::PutGamma() const
{
	return ::PutGamma(S, K, T, r, sig, b);
}

double EuropeanOption::CallVega() const
{
	return ::CallVega(S, K, T, r, sig, b);
}

double EuropeanOption::PutVega() const
{
	return ::PutVega(S, K, T, r, sig, b);
}

double EuropeanOption::CallTheta() const
{
	return ::CallTheta(S, K, T, r, sig, b);
}

double EuropeanOption::PutTheta() const
{
	return ::PutTheta(S, K, T, r, sig, b);
}

double EuropeanOption::CallRho() const
{
	return ::CallRho(S, K, T, r, sig, b);
}

double EuropeanOption::PutRho() const
{
	return ::PutRho(S, K, T, r, sig, b);
}

double EuropeanOption::CallCoc() const
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	return T * S * exp((b - r) * T) * N(d1);
}

double EuropeanOption::PutCoc() const
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	return -T * S * exp((b - r) * T) * N(-d1);
}

//	Initialising all the default values
void EuropeanOption::init()
{
	//	Default values
	r = 0.03;
	sig = 0.2;
	K = 100;
	S = 95;				//	Default stock price
	T = 1;
	b = r;				//	Black - Scholes(1973) stock option model : b = r
	type = "C";			//	Call option as the default
}

void EuropeanOption::copy(const EuropeanOption& option)
{
	S = option.S;
	K = option.K;
	T = option.T;
	r = option.r;
	sig = option.sig;
	b = option.b;
	type = option.type;
}

//	Constructors and destructor
EuropeanOption::EuropeanOption() : Option()						//	Default constructor
{
	init();
}

EuropeanOption::EuropeanOption(const EuropeanOption& option) : Option(option)		//	Copy constructor
{
	copy(option);
}

//	Constructor that accepts values
EuropeanOption::EuropeanOption(const double& S1, const double& K1, const double& T1, const double& r1,
	const double& sig1, const double& b1, const string type1) : Option(), S(S1), K(K1), T(T1), r(r1), sig(sig1), b(b1), type(type1) {}

EuropeanOption::~EuropeanOption() {}							//	Destructor

//	Assignment operator
EuropeanOption& EuropeanOption::operator = (const EuropeanOption& option)
{
	if (this == &option)
	{
		return *this;
	}
	Option::operator = (option);
	copy(option);
	return *this;
}

//	Functions that calculate option price and sensitivities
double EuropeanOption::Price() const
{
	if (type == "C")
	{
		return CallPrice();
	}
	else
	{
		return PutPrice();
	}
}

double EuropeanOption::Delta() const
{
	if (type == "C")
	{
		return CallDelta();
	}
	else
	{
		return PutDelta();
	}
}

double EuropeanOption::Gamma() const
{
	if (type == "C")
	{
		return CallGamma();
	}
	else
	{
		return PutGamma();
	}
}

double EuropeanOption::Vega() const
{
	if (type == "C")
	{
		return CallVega();
	}
	else
	{
		return PutVega();
	}
}

double EuropeanOption::Theta() const
{
	if (type == "C")
	{
		return CallTheta();
	}
	else
	{
		return PutTheta();
	}
}

double EuropeanOption::Rho() const
{
	if (type == "C")
	{
		return CallRho();
	}
	else
	{
		return PutRho();
	}
}

//	Functions that calculates the Cost of carry
double EuropeanOption::Coc() const
{
	if (type == "C")
	{
		return CallCoc();
	}
	else
	{
		return PutCoc();
	}
}

// Modifier functions
void EuropeanOption::toggle()				//	Change the option type
{
	type = ((type == "C") ? "P" : "C");
}

//	Global Functions
double CallDelta(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));

	normal_distribution<> standard_normal(0.0, 1.0);
	return exp((b - r) * T) * cdf(standard_normal, d1);

}

double PutDelta(const double S, const double K, const double T, const double r, const double sig, const double b)
{

	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	return exp((b - r) * T) * (cdf(standard_normal, d1) - 1.0);
}

double CallGamma(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	double d2 = d1 - (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	return (pdf(standard_normal, d1) * exp((b - r) * T)) / (S * sig * sqrt(T));
}

double PutGamma(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	return CallGamma(S, K, T, r, sig, b);
}

double CallVega(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	return (S * sqrt(T) * exp((b - r) * T) * pdf(standard_normal, d1));
}

double PutVega(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	return CallVega(S, K, T, r, sig, b);
}

double CallTheta(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	double d2 = d1 - (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	double t1 = (S * sig * exp((b - r) * T) * pdf(standard_normal, d1)) / (2 * sqrt(T));
	double t2 = ((b - r) * S * exp((b - r) * T) * cdf(standard_normal, d1));
	double t3 = (r * K * exp(-r * T) * cdf(standard_normal, d1));
	return -(t1 + t2 + t3);
}

double PutTheta(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	double d2 = d1 - (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	double t1 = (S * sig * exp((b - r) * T) * pdf(standard_normal, d1)) / (2 * sqrt(T));
	double t2 = ((b - r) * S * exp((b - r) * T) * cdf(standard_normal, d1));
	double t3 = (r * K * exp(-r * T) * cdf(standard_normal, d1));
	return t2 + t3 - t1;
}

double CallRho(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	double d2 = d1 - (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	if (b != 0.0)
	{
		return T * K * exp(-r * T) * cdf(standard_normal, d2);
	}
	else
	{
		return -T * CallPrice(S, K, T, r, sig, b);
	}
}

double PutRho(const double S, const double K, const double T, const double r, const double sig, const double b)
{
	double d1 = (log(S / K) + (b + (sig * sig) * 0.5) * T) / (sig * sqrt(T));
	double d2 = d1 - (sig * sqrt(T));
	normal_distribution<> standard_normal(0.0, 1.0);
	if (b != 0.0)
	{
		return T * K * exp(-r * T) * cdf(standard_normal, -d2);
	}
	else
	{
		return -T * PutPrice(S, K, T, r, sig, b);
	}
}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/trace.hpp
 *
 * \brief Compute the trace of a matrix.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * 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_NUMERIC_UBLASX_OPERATION_TRACE_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_TRACE_HPP

#include <algorithm>
#include <boost/numeric/ublas/detail/config.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>

namespace boost { namespace numeric { namespace ublasx {

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

/**
 * \brief Compute the trace of a given matrix.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param A The input matrix expression.
 * \return The trace of \a A.
 *
 * The trace of a m-by-n matrix \f$A\f$ is defined as
 * \f[
 *   \operatorname{tr}(A) = \sum_{i=1}^k a_{ii}
 * \f]
 * where \f$k=\min(m,n)\f$.
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename ublas::matrix_traits<MatrixExprT>::value_type trace(ublas::matrix_expression<MatrixExprT> const& A)
{
    typedef typename ublas::matrix_traits<MatrixExprT>::value_type value_type;
    typedef typename ublas::matrix_traits<MatrixExprT>::size_type size_type;

    size_type n = ::std::min(num_rows(A), num_columns(A));
    value_type res = 0;

    for (size_type i = 0; i < n; ++i)
    {
        res += A()(i,i);
    }

    return res;
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_TRACE_HPP


#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <Eigen/Dense>
#include <algorithm>
#include <ctime>
#include <fstream>
#include <iostream>
#include <math.h>
#include <random>
#include <string>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned, K> Vb;
typedef CGAL::Triangulation_data_structure_2<Vb> Tds;
typedef CGAL::Delaunay_triangulation_2<K, Tds> Delaunay;
typedef Delaunay::Face_circulator Face_circulator;
typedef Delaunay::Face_handle Face_handle;
typedef Delaunay::Point Point;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> MapM3Xd;
typedef Eigen::Ref<Matrix3Xd> RefM3Xd;

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

  // We expect three command line arguments:
  // 1. Path to the data file
  // 2. Output directory where output files will be created
  if (argc < 3) {
    std::cout << "Usage: ./calcVolume <data_file> <output_dir>" << std::endl;
    return 0;
  }

  clock_t t = clock();

  // Open the data file
  std::ifstream inputfile(argv[1]);
  if (!inputfile.is_open()) {
    std::cout << "Unable to open file " << argv[1] << std::endl;
    return 0;
  }

  //**********************************************************************//
  //

  auto outputFile = std::string(argv[2]) + "/volume.dat";
  std::ofstream vol_file(outputFile);

  if (!vol_file) {
    std::cout << "Failed to create output file " << outputFile << std::endl;
    return 0;
  }

  //**********************************************************************//
  // Now read the first( or the header ) line of the data file
  // The first line contains N, gamma and beta information
  std::string line, ignore;
  size_t N;
  double_t gamma = 0, beta = 0;
  std::getline(inputfile, line);
  std::istringstream header(line);
  header >> ignore >> N >> ignore >> gamma >> ignore >> beta;
  int lmax = std::floor(std::sqrt(N) - 1);

  // Write column names in the output files
  vol_file << "volume" << std::endl;
  vol_file.precision(7);

  //**********************************************************************//
  // Some useful lambda functions
  //

  // A function to read N rows of a 3xN matrix from a stringstream
  auto readMatrix3Xd = [](std::istringstream &ss, RefM3Xd X) {
    auto N = X.cols();
    size_t valCount = 0;
    while (valCount < 3 * N && ss.good()) {
      std::string value;
      std::getline(ss, value, ',');
      X(valCount % 3, valCount / 3) = std::stod(value);
      valCount++;
    }
  };
  //**********************************************************************//

  //***************************** MAIN LOOP ******************************//
  // Skip the first row which is just zero temperature data
  std::getline(inputfile, line);

  // Now iterate over the remaining rows
  auto rowCount = 1;
  while (std::getline(inputfile, line)) {

    std::istringstream rowStream(line);

    // We will extract 3*N doubles representing particle positions
    // from the stream and ignore the 3*N rotation vector components
    Matrix3Xd Xt(3, N), X0(3, N);
    readMatrix3Xd(rowStream, Xt);

    // Project the points to a unit sphere
    X0 = Xt.colwise().normalized();

    // Rotate all points of the shell so that the 0th point is along z-axis
    Vector3d c = X0.col(0);
    double_t cos_t = c(2);
    double_t sin_t = std::sin(std::acos(cos_t));
    Vector3d axis;
    axis << c(1), -c(0), 0.;
    axis.normalize();
    Matrix3d rotMat, axis_cross, outer;
    axis_cross << 0., -axis(2), axis(1), axis(2), 0., -axis(0), -axis(1),
        axis(0), 0.;
    outer.noalias() = axis * axis.transpose();
    rotMat =
        cos_t * Matrix3d::Identity() + sin_t * axis_cross + (1 - cos_t) * outer;
    Matrix3Xd rPts(3, N);
    rPts = rotMat * X0;

    // Calculate the stereographic projections
    Vector3d p0;
    p0 << 0, 0, -1.0; // Point on the plane of projection
    c = rPts.col(0);  // The point from which we are projecting

    MapM3Xd l0(&(rPts(0, 1)), 3, N - 1);
    Matrix3Xd l(3, N - 1), proj(3, N - 1);
    l = (l0.colwise() - c).colwise().normalized(); // dirns of projections
    for (auto j = 0; j < N - 1; ++j) {
      proj.col(j) = ((p0(2) - l0(2, j)) / l(2, j)) * l.col(j) + l0.col(j);
    }

    // Insert the projected points in a CGAL vertex_with_info vector
    std::vector<std::pair<Point, unsigned>> verts;
    for (auto j = 0; j < N - 1; ++j) {
      verts.push_back(std::make_pair(Point(proj(0, j), proj(1, j)), j + 1));
    }

    // Triangulate
    Delaunay dt(verts.begin(), verts.end());

    // Iterate over the triangles to calculate volume
    auto volume = 0.0;
    for (auto ffi = dt.finite_faces_begin(); ffi != dt.finite_faces_end();
         ++ffi) {
      auto i = ffi->vertex(0)->info();
      auto j = ffi->vertex(2)->info();
      auto k = ffi->vertex(1)->info();
      volume += 0.166666667 * (Xt.col(i).dot(Xt.col(j).cross(Xt.col(k))));
    }

    // Iterate over infinite faces
    Face_circulator fc = dt.incident_faces(dt.infinite_vertex()), done(fc);
    if (fc != 0) {
      do {
        auto i = dt.is_infinite(fc->vertex(0)) ? 0 : fc->vertex(0)->info();
        auto j = dt.is_infinite(fc->vertex(2)) ? 0 : fc->vertex(2)->info();
        auto k = dt.is_infinite(fc->vertex(1)) ? 0 : fc->vertex(1)->info();
        volume += 0.166666667 * (Xt.col(i).dot(Xt.col(j).cross(Xt.col(k))));
      } while (++fc != done);
    }
    vol_file << volume << std::endl;
    rowCount++;
  }
  inputfile.close();
  vol_file.close();

  std::cout << "Time elapsed = " << ((float)(clock() - t)) / CLOCKS_PER_SEC
            << " seconds." << std::endl;

  return 1;
}


//
// Created by kazem on 2020-05-18.
//

#include "includes/sympiler_cholesky.h"
#include <Eigen/Core>
#include <Eigen/Sparse>

 namespace sympiler
 {
  //  Solving Ax=b
  // Inputs:
  //   H  n by n sparse Hessian matrix **lower triangle only** (see
  //     .triangularView<Eigen::Lower>() )
  //   q  n by 1 vector
  // Outputs:
  //   x  n by 1 solution vector
  // Returns sympiler exit flag
  //
  int sympiler_spd_linsolve(
    // Pass inputs by copy so we get non-const and casted data
    Eigen::SparseMatrix<double,Eigen::ColMajor,int> A,
    Eigen::Matrix<double,Eigen::Dynamic,1> b,
    Eigen::Matrix<double,Eigen::Dynamic,1> & x){
   assert(A.isApprox(A.triangularView<Eigen::Lower>(),0) &&
          "P should be lower triangular");
   assert(A.isCompressed());
   assert(A.rows()==A.cols());
   assert(A.rows()==b.rows());

   auto *H = new sym_lib::parsy::CSC;
   H->nzmax = A.nonZeros();
   H->ncol= H->nrow=A.rows();
   H->p = A.outerIndexPtr();
   H->i = A.innerIndexPtr();
   H->x = A.valuePtr();
   H->stype=-1;
   H->xtype=SYMPILER_REAL;
   H->packed=TRUE;
   H->nz = NULL;
   H->sorted = TRUE;

   auto *sym_chol = new sym_lib::parsy::SolverSettings(H,b.data());
   sym_chol->ldl_variant = 1;
   sym_chol->solver_mode = 0;
   sym_chol->symbolic_analysis();
   sym_chol->numerical_factorization();
   double *sol = sym_chol->solve_only();
   //lbl->compute_norms();
   //std::cout<<"residual: "<<lbl->res_l1<<"\n";

   x = Eigen::Map< Eigen::Matrix<double,Eigen::Dynamic,1> >(
     sol,A.rows(),1);

   // Exitflag TODO
   int exitflag = 0;
   delete sym_chol;
   delete H;
   return exitflag;
  }

  sym_lib::parsy::CSC* sympiler_csc_format(
    // Pass inputs by copy so we get non-const and casted data
    Eigen::SparseMatrix<double,Eigen::ColMajor,int> A) {
   assert(A.isApprox(A.triangularView<Eigen::Lower>(), 0) &&
          "P should be lower triangular");
   assert(A.isCompressed());
   assert(A.rows() == A.cols());

   auto *H = new sym_lib::parsy::CSC;
   H->nzmax = A.nonZeros();
   H->ncol = H->nrow = A.rows();
   H->p = A.outerIndexPtr();
   H->i = A.innerIndexPtr();
   H->x = A.valuePtr();
   H->stype = -1;
   H->xtype = SYMPILER_REAL;
   H->packed = TRUE;
   H->nz = NULL;
   H->sorted = TRUE;
   return H;
  }

  sym_lib::parsy::SolverSettings* sympiler_chol_symbolic(
    // Pass inputs by copy so we get non-const and casted data
    sym_lib::parsy::CSC *A){

   auto *sym_chol = new sym_lib::parsy::SolverSettings(A);
   sym_chol->ldl_variant = 1;
   sym_chol->solver_mode = 0;
   sym_chol->symbolic_analysis();
   return sym_chol;
  }

  sym_lib::parsy::SolverSettings* sympiler_chol_symbolic(
    // Pass inputs by copy so we get non-const and casted data
    sym_lib::parsy::CSC *A, int num_threads, sym_lib::parsy::SYM_ORDER  so, int mode){

   auto *sym_chol = new sym_lib::parsy::SolverSettings(A);
   sym_chol->ldl_variant = mode;
   sym_chol->num_thread = num_threads;
   sym_chol->solver_mode = 0;
   sym_chol->sym_order = so;
   sym_chol->symbolic_analysis();
   return sym_chol;
  }

 }


//
// Created by jianping on 17-9-1.
//

#ifndef DEPTHPROBABILITY_ESTIMATOR_HPP
#define DEPTHPROBABILITY_ESTIMATOR_HPP

#include <ceres/ceres.h>
#include <Eigen/Dense>
#include <algorithm>
#include <math.h>

class DistributionEstimator {
public:
    struct distributionCostFunctor {

        distributionCostFunctor(double ob_x,double min, double max)
        :_ob_x(ob_x),_min(min),_max(max)
        {

        }

        template <typename T>
        bool operator()(const T* const params, T* residual) const {
            T part_normal = T(1.0)/(T(std::sqrt(2.0*M_PI))*params[2])
                            *ceres::exp(-(T(_ob_x)-params[1])*(T(_ob_x)-params[1])/(T(2)*params[2]*params[2]))*params[0];
            T part_uniform = T(1)/(_max-_min)*(T(1.0)-params[0]);
            residual[0] = -ceres::log(part_normal+part_uniform);
            if(params[0] < T(0.3) || params[0]>T(1))
            {
                return false;
            }
            return true;
        }

        double _ob_x;
        double _min;
        double _max;

    };

public:
    DistributionEstimator(const std::vector<double>& data)
    :_data(data)
    {
        _min = *std::min_element(_data.begin(),_data.end());
        _max = *std::max_element(_data.begin(),_data.end());

        //init
        params.resize(3);
        params[0] = 1;
        params[1] = std::accumulate(_data.begin(),_data.end(),0.0)/_data.size();
        params[2] = 0;
        for (int i = 0; i < _data.size(); ++i) {
            params[2] += (_data[i]-params[1])*(_data[i]-params[1]);
        }
        params[2]/=_data.size();
        params[2] = std::sqrt(params[2]);
        std::cout<<" mu:"<<params[1]<<std::endl;
        std::cout<<" sigma:"<<params[2]<<std::endl;
    }

    void estimate();
    std::vector<double> params;//pi mu sigma
private:
    const std::vector<double>& _data;
    double _min;
    double _max;

};

#endif //DEPTHPROBABILITY_ESTIMATOR_HPP


#include "mex.h"
#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include "drakeMexUtil.h"
#include <drakeGradientUtil.h>

using namespace Eigen;
using namespace std;

/*
 * c++ version of
 *     function [H,C,B] = manipulatorDynamics(obj,q,qd)
 */

template<typename DerivedQ>
void manipulatorDynamics(const mxArray *pobj, const MatrixBase<DerivedQ>& q, const MatrixBase<DerivedQ>& qd,
                         mxArray **plhs)
{
  // keep it readable:
  double m1 = mxGetScalar(mxGetProperty(pobj, 0, "m1"));
  double m2 = mxGetScalar(mxGetProperty(pobj, 0, "m2"));
  double l1 = mxGetScalar(mxGetProperty(pobj, 0, "l1"));
  double g = mxGetScalar(mxGetProperty(pobj, 0, "g"));
  double lc1 = mxGetScalar(mxGetProperty(pobj, 0, "lc1"));
  double lc2 = mxGetScalar(mxGetProperty(pobj, 0, "lc2"));
  double b1 = mxGetScalar(mxGetProperty(pobj, 0, "b1"));
  double b2 = mxGetScalar(mxGetProperty(pobj, 0, "b2"));
  double I1 = mxGetScalar(mxGetProperty(pobj, 0, "Ic1")) + m1 * lc1 * lc1;
  double I2 = mxGetScalar(mxGetProperty(pobj, 0, "Ic2")) + m2 * lc2 * lc2;
  double m2l1lc2 = m2 * l1 * lc2;  // occurs often!

  auto c2 = cos(q(1));
  auto s1 = sin(q(0)), s2 = sin(q(1));
  auto s12 = sin(q(0) + q(1));

  auto h12 = I2 + m2l1lc2 * c2;

  typedef typename DerivedQ::Scalar Scalar;
  Matrix<Scalar, 2, 2> H;
  Matrix<Scalar, 2, 1> C;
  Matrix<Scalar, 2, 1> B;

  H << I1 + I2 + m2 * l1 * l1 + 2 * m2l1lc2 * c2, h12, h12, I2;

  //C = [ -2*m2l1lc2*s(2)*qd(2), -m2l1lc2*s(2)*qd(2); m2l1lc2*s(2)*qd(1), 0 ];
  //G = g*[ m1*lc1*s(1) + m2*(l1*s(1)+lc2*s12); m2*lc2*s12 ];

  C << -2 * m2l1lc2 * s2 * qd(1) * qd(0) + -m2l1lc2 * s2 * qd(1) * qd(1), m2l1lc2 * s2 * qd(0) * qd(0);

  // add in G terms
  C(0) += g * m1 * lc1 * s1 + g * m2 * (l1 * s1 + lc2 * s12);
  C(1) += g * m2 * lc2 * s12;

  // damping terms
  C(0) += b1 * qd(0);
  C(1) += b2 * qd(1);

  // input matrix
  // multiplying by q(0) just to get the size of the derivatives vector right; Matlab TaylorVar operations will complain otherwise. We could handle this case on the Matlab side instead.
  B << 0.0 * q(0), 1.0;

  plhs[0] = eigenToMatlabGeneral<2, 2>(H);
  plhs[1] = eigenToMatlabGeneral<2, 1>(C);
  plhs[2] = eigenToMatlabGeneral<2, 1>(B);
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  const mxArray *pobj = prhs[0];

  if (mxIsDouble(prhs[1]) && mxIsDouble(prhs[2])) {
    auto q = matlabToEigenMap<2, 1>(prhs[1]);
    auto qd = matlabToEigenMap<2, 1>(prhs[2]);
    manipulatorDynamics(pobj, q, qd, plhs);
  } else if (isa(prhs[1], "TrigPoly") && isa(prhs[2], "TrigPoly")) {
    auto q = trigPolyToEigen(prhs[1]);
    auto qd = trigPolyToEigen(prhs[2]);
    manipulatorDynamics(pobj, q, qd, plhs);
  } else if (isa(prhs[1], "TaylorVar") && isa(prhs[2], "TaylorVar")) {
    auto q = taylorVarToEigen<Dynamic, 1>(prhs[1]);
    auto qd = taylorVarToEigen<Dynamic, 1>(prhs[2]);
    manipulatorDynamics(pobj, q, qd, plhs);
  } else {
    mexErrMsgIdAndTxt("Drake:AcrobotPLantCpp:UnknownType",
                      "don't know how to handle the datatypes passed in for q and/or qd (yet)");
  }
}


/*! \file main.cpp
    \brief The main file performing the spectral numerical integration.

    In this file, we perform the computation from the PDF.
*/

#include <iostream>
#include <vector>
#include <algorithm>
#include <numeric>
#include <chrono>
#include <fstream>

#include <Eigen/Dense>

#include "chebyshev_differentiation.h"
#include "spectral_integration_utilities.h"

#include "tictoc.h"

//  Number od admitted strain fields and number of mode per strain field
static constexpr unsigned int na = 3;  //  Kirkhoff rod
static constexpr unsigned int ne = 3;  // dimesion of qe

//  Number of Chebyshev nodes
static constexpr unsigned int number_of_chebyshev_nodes = 11;

/*!
 * \brief getA Compute the matrix A for the system x' = Ax + b
 * \param t_qe The current generalized strains coordinates
 * \tparam t_state_dimension The dimension of the state x
 * \tparam t_number_of_chebyshev_nodes The number of Chebyshev nodes (which also account for the first one)
 * \tparam t_na The number of allowed strain coordinates
 * \tparam t_ne The number of modes per strain coordinate
 * \return
 */
template<unsigned int t_state_dimension, unsigned int t_number_of_chebyshev_nodes, unsigned int t_na, unsigned int t_ne>
Eigen::MatrixXd getA(const Eigen::Matrix<double, ne*ne, 1> &t_qe)
{

    //  Define the Chebyshev points on the unit circle
    const auto x = ComputeChebyshevPoints<t_number_of_chebyshev_nodes>();

    //  Definition of the problem dymension
    static constexpr unsigned int problem_dimension = t_state_dimension * t_number_of_chebyshev_nodes;

    std::array<Eigen::Matrix<double, t_state_dimension, t_state_dimension>, t_number_of_chebyshev_nodes> A_stack;

    Eigen::Vector3d K;
    Eigen::Matrix<double, t_state_dimension, t_state_dimension> A_at_chebyshev_point;
    for(unsigned int i=0; i<x.size(); i++){

        //  Extract the curvature from the strain
        K = Phi<t_na, t_ne>(x[i])*t_qe;

        //  Compute the A matrix of Q' = 1/2 A(K) Q
        A_at_chebyshev_point <<      0, -K(0),  -K(1),  -K(2),
                                  K(0),     0,   K(2),  -K(1),
                                  K(1), -K(2),      0,   K(0),
                                  K(2),  K(1),  -K(0),      0;

        A_stack[i] = 0.5*A_at_chebyshev_point;
    }

    //  Declare the matrix for the system Ax = b
    Eigen::Matrix<double, problem_dimension, problem_dimension> A = Eigen::Matrix<double, problem_dimension, problem_dimension>::Zero();
    A.setConstant(0);

    //  Define a vector containing the indexes of the top left corners of the blocks composing the matrix A
    Eigen::VectorXi block_indexes = Eigen::VectorXi::LinSpaced(t_state_dimension, 0, t_number_of_chebyshev_nodes*(t_state_dimension-1));

    //  Populate this matrix with all the elements in the right order
    for(unsigned int chebyshev_point=0; chebyshev_point<t_number_of_chebyshev_nodes; chebyshev_point++){

        //  Get the current set of indexes for the coefficients of the matrix A at the current chebyshev point
        A(block_indexes, block_indexes) = A_stack[chebyshev_point] ;

        block_indexes += Eigen::VectorXi::Constant(t_state_dimension, 1, 1);
    }

    return A;
}

/*!
 * \brief writeToFile writes a Eigen matrix into file
 * \param t_name    name of the file
 * \param t_matrix  the Eigen matrix to write into the file
 * \param t_relative_path_from_build the relative path from the build folder to the file location. Default is none so the file is written in the build directory)
 * \param t_format  the specification for writing. (Default in column major allignment, with comma column separator and 8 digits precision)
 */
void writeToFile(std::string t_name,
                 const Eigen::MatrixXd &t_matrix,
                 std::string t_relative_path_from_build="",
                 const Eigen::IOFormat &t_format=Eigen::IOFormat(16, 0, ","));

Eigen::MatrixXd GetQuaternions(const Eigen::Vector4d &t_initial_state, const Eigen::Matrix<double, ne*ne, 1> &t_qe)
{
    /*  The state dimension and the number of nodes are known. The state dimension will not change
     *  (a quaternion will remain a quaternion, a twist will remain a twist ecc..) and the number of
     *  nodes are not changed at runtime and neither between runs. Typically, once a satisfactory number
     *  of nodes has been found, it is kept for the whole lifetime of the project.
     *
     *  Thus, we can enforce this concept with constexpr classifier so that the values are known at
     *  compile time and we can use templated function for our matrices and vector, which are a bit faster.
     */
        //  Dimension of the state
        constexpr unsigned int state_dimension = 4;

        //  Problem size is the total number of elements
        constexpr unsigned int prob_dimension = state_dimension * number_of_chebyshev_nodes;
        //  The subset of unknows in the problem
        constexpr unsigned int unknow_state_dimension = state_dimension * (number_of_chebyshev_nodes - 1);

    /*  These are a set of type definition in order to have a more neat algorithm in
     *  terms of matrix and vector dymensions
     */
        typedef Eigen::Matrix<double, prob_dimension, prob_dimension> MatrixNpNp;
        typedef Eigen::Matrix<double, prob_dimension, 1> VectorNp;

        typedef Eigen::Matrix<double, prob_dimension, state_dimension> MatrixNpNs;

        typedef Eigen::Matrix<double, unknow_state_dimension, unknow_state_dimension> MatrixNuNu;
        typedef Eigen::Matrix<double, unknow_state_dimension, 1> VectorNu;

        typedef Eigen::Matrix<double, number_of_chebyshev_nodes, number_of_chebyshev_nodes> MatrixNchebNcheb;

        typedef Eigen::Matrix<double, number_of_chebyshev_nodes, state_dimension, Eigen::ColMajor> MatrixNchebNs;

    /*  In this first part, we precompute the matrices P and Dn. In fact, once we know the state dimension and the
     *  number of nodes, these matrices are constant and thus me can compute them beforehand
     *
     *  P.S. As we know the number of nodes, the matrix Phi can be computed at every chebyshev point to be stacked
     *  into a vector. As a result, instead of computing the matrices, we directly access their value in the vector position.
     *  This can actually be something more than 200 times faster.
     *
     */

        const MatrixNpNp  P = getP<state_dimension, number_of_chebyshev_nodes>();

        const MatrixNchebNcheb Dn = getDn<number_of_chebyshev_nodes>();
        const MatrixNpNp D = Eigen::KroneckerProduct(Eigen::MatrixXd::Identity(state_dimension, state_dimension), Dn);

    /*  In this part we compute the matrix A and the vector b.
     *  In this case, the elements of the matrix A depends on the strain. As we change the strains during
     *  simulation, we have to compute the components of A at runtime.
     *
     *  Similarly, the vector b contains the values not related to the derivating variable, but that depends on other parameters.
     *  For example when computing r' = R(Q)*Γ it does not depend on r but only on Q and Γ.
     *
     *  We can interpret everything before this point as the setup and everything after as the actual run-time operations.
     *
     *  In the following there are some matrices and operation that could be moved in the setup. However I choose to left them
     *  there for clarity, but feel free to move where you thing it's better.
     *
     *  The following is the translation into C++ of the equations presented in the PDF
     *
     */
        //tictoc.tic();
        const MatrixNpNp A = getA<state_dimension, number_of_chebyshev_nodes, na, ne>(t_qe);
        //tictoc.toc("Time to compute A : ");

        const VectorNp b = Eigen::Matrix<double, prob_dimension, 1>::Zero();

        //  Apply transformation of initial condition onto ODE's matrices

        const MatrixNpNp Ap = P.transpose() * A * P;
        const MatrixNpNp Dp = P.transpose() * D * P;    //  Can be moved in setup
        const VectorNp bp   = P * b;

        //  Compute the ivp
        const MatrixNpNs D_IT = Dp.block<prob_dimension, state_dimension>(0, 0);    //  Can be moved in setup
        const MatrixNpNs A_IT = Ap.block<prob_dimension, state_dimension>(0, 0);
        const VectorNp b_IT = ( D_IT - A_IT ) * t_initial_state;

        //  Obtain the section related to the unknows of the problem
        const MatrixNuNu D_NN = Dp.block<unknow_state_dimension, unknow_state_dimension>(state_dimension, state_dimension);
        const MatrixNuNu A_NN = Ap.block<unknow_state_dimension, unknow_state_dimension>(state_dimension, state_dimension);
        const VectorNu ivp = b_IT.block<unknow_state_dimension, 1>(state_dimension, 0);
        const VectorNu b_NN   = bp.block<unknow_state_dimension, 1>(state_dimension, 0);

        //  Finally compute the states at the unknows Chebyshev points
        const VectorNu X_NN = (D_NN - A_NN).inverse() * (b_NN - ivp);

        //  We now stack together the initial state on top of the other we just compute
        //  Then we need to map back to a more readable stack of states
        const VectorNp X_tilde = P * (VectorNp() << t_initial_state, X_NN).finished();

        //  Then we write the element row-wise
        const MatrixNchebNs X_stack = Eigen::Map<const MatrixNchebNs>(X_tilde.data());

        return X_stack;

}

Eigen::MatrixXd IntegratePositions(const Eigen::Vector3d &t_initial_state,
                                   const Eigen::Matrix<double, ne*ne, 1> &t_qe,
                                   const Eigen::MatrixXd &t_quaternions_stack)
{
    /*  The state dimension and the number of nodes are known. The state dimension will not change
     *  (a quaternion will remain a quaternion, a twist will remain a twist ecc..) and the number of
     *  nodes are not changed at runtime and neither between runs. Typically, once a satisfactory number
     *  of nodes has been found, it is kept for the whole lifetime of the project.
     *
     *  Thus, we can enforce this concept with constexpr classifier so that the values are known at
     *  compile time and we can use templated function for our matrices and vector, which are a bit faster.
     */
        //  Dimension of the state
        constexpr unsigned int state_dimension = 3;

        //  Problem size is the total number of elements
        constexpr unsigned int prob_dimension = state_dimension * number_of_chebyshev_nodes;
        //  The subset of unknows in the problem
        constexpr unsigned int unknow_state_dimension = state_dimension * (number_of_chebyshev_nodes - 1);

    /*  These are a set of type definition in order to have a more neat algorithm in
     *  terms of matrix and vector dymensions
     */
        typedef Eigen::Matrix<double, prob_dimension, prob_dimension> MatrixNpNp;
        typedef Eigen::Matrix<double, prob_dimension, 1> VectorNp;

        typedef Eigen::Matrix<double, prob_dimension, state_dimension> MatrixNpNs;

        typedef Eigen::Matrix<double, unknow_state_dimension, unknow_state_dimension> MatrixNuNu;
        typedef Eigen::Matrix<double, unknow_state_dimension, 1> VectorNu;

        typedef Eigen::Matrix<double, number_of_chebyshev_nodes, number_of_chebyshev_nodes> MatrixNchebNcheb;

        typedef Eigen::Matrix<double, number_of_chebyshev_nodes, state_dimension, Eigen::ColMajor> MatrixNchebNs;

    MatrixNchebNs positions_stack;

    return positions_stack;
}

int main()
{
    tictoc tictoc;

    //  Const curvature strain field
    Eigen::Matrix<double, ne*na, 1> qe;
    //  Here we give some value for the strain
    qe <<   0,
            0,
            0,
            1.2877691307032,
           -1.63807499160786,
            0.437406679142598,
            0,
            0,
            0;

//  Let's deal with quaternion

    //  Define the initial state
    const Eigen::Vector4d initial_state(1, 0, 0, 0); // Quaternion case
    const auto ruaternions_stack = GetQuaternions(initial_state, qe);

//  Let's deal with positions
     const Eigen::Vector3d initial_position(0, 0, 0);
     const auto positions_stack = IntegratePositions(initial_position, qe, ruaternions_stack);

    Eigen::Quaterniond q;
    Eigen::Matrix3d R = q.toRotationMatrix();

    std::cout<< "X_stack = " << std::endl << ruaternions_stack <<std::endl << std::endl;

    writeToFile("Q_stack", ruaternions_stack);

    return 0;

    /*
     * Workflow
     *
     * 1) Read through, test some values qe Cheb points
     *
     * 2) Move the computations in main into a function IntegrateQuaternion (it returns quaternion stack)
     *
     * 3) Extend to positions (stack of r)
     *
     */

}

void writeToFile(std::string t_name,
                 const Eigen::MatrixXd &t_matrix,
                 std::string t_relative_path_from_build,
                 const Eigen::IOFormat &t_format)
{
    if(not t_relative_path_from_build.empty()){
        //  Ensure relative path ends with a backslash only if a path is given
        if(not t_relative_path_from_build.ends_with('/'))
            t_relative_path_from_build.append("/");
    }

    //  Ensure it ends with .csv
    if(t_name.find(".csv") == std::string::npos)
        t_name.append(".csv");

    //  The file will be created in the location given by the realtive path and with the given name
    const auto file_name_and_location = t_relative_path_from_build + t_name;

    //  Create file in given location with given name
    std::ofstream file(file_name_and_location.c_str());

    //  Put matrix in this file
    file << t_matrix.format(t_format);

    //  Close the file
    file.close();
 }


#pragma once

#include <Eigen/Core>
using namespace Eigen;

#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>
using namespace autodiff;

#include <samson/so3.hpp>

namespace samson::se3
{

    using Vector6dual = Matrix<dual, 6, 1, 0, 6, 1>;
    using Matrix4dual = Matrix<dual, 4, 4, 0, 4, 4>;

    auto se3(const Vector6dual &v) -> Matrix4dual
    {
        Matrix4dual X;
        X << 0.0, -v(2), v(1), v(3),
            v(2), 0.0, -v(0), v(4),
            -v(1), v(0), 0.0, v(5),
            0.0, 0.0, 0.0, 0.0;
        return X;
    }

    auto se3(const Matrix4dual &X) -> Vector6dual
    {
        Vector6dual v;
        v << X(2, 1), X(0, 2), X(1, 0), X(0, 3), X(1, 3), X(2, 3);
        return v;
    }

    auto exp(Vector6dual &x) -> Matrix4dual
    {
        using samson::so3::from_axis_angle;
        using samson::so3::so3;

        Vector3dual w = x.head<3>();
        Vector3dual v = x.tail<3>();

        dual theta = w.norm();
        Matrix4dual ret = Matrix4dual::Identity();
        if (theta == 0.0)
        {
            ret = Matrix4dual::Identity();
            ret.block<3, 1>(0, 3) = v;
            return ret;
        }

        Vector3dual k = w.normalized();
        dual sin_theta = sin(theta);
        dual one_minus_cos_theta = 1.0 - cos(theta);
        Matrix3dual K = so3(k);
        Matrix3dual KK = K * K;

        Matrix3dual I = Matrix3dual::Identity();

        ret.block<3, 3>(0, 0) = from_axis_angle(k, theta);
        ret.block<3, 1>(0, 3) = (I * theta + one_minus_cos_theta * K + (theta - sin_theta) * KK) * (v / theta);

        return ret;
    }

    auto exp(Matrix4dual &X) -> Matrix4dual
    {
        Vector6dual x = se3(X);
        return exp(x);
    }
} // namespace samson::se3


#include <ql/quantlib.hpp>

#include <boost/assign/std/vector.hpp>

#include <iostream>
#include <fstream>

using namespace QuantLib;
using namespace  boost::assign;

void bdkSmile() {

    std::ofstream out;
    out.open("bdk.dat");

    Real forward = 0.03;
    Real expiryTime = 20.0;

    std::vector<Real> sabrParams;
    sabrParams.push_back(0.15); // alpha
    sabrParams.push_back(0.8);    // beta
    sabrParams.push_back(0.50);   // nu
    sabrParams.push_back(-0.48);  // rho

    boost::shared_ptr<SabrSmileSection> sabr(
        new SabrSmileSection(expiryTime, forward, sabrParams));

    boost::shared_ptr<BdkSmileSection> bdk(new BdkSmileSection(sabr, 1.0, 1.0, 0.025, 0.06));

    Real strike = 0.0001;
    while (strike <= 0.10) {
        out << strike << " " << sabr->optionPrice(strike) << " "
            << bdk->optionPrice(strike) << " "
            << sabr->digitalOptionPrice(strike) << " "
            << bdk->digitalOptionPrice(strike) << " " << sabr->density(strike)
            << " " << bdk->density(strike) << " " << sabr->volatility(strike) << " " <<
            bdk->volatility(strike) << std::endl;
        strike += 0.0001;
    }

    out.close();

}

void zabrExamples() {

    Real forward = 0.03;
    Real expiryTime = 15;

    Real alpha = 0.10;
    Real beta = 0.7;
    Real nu = 0.20;
    Real rho = -0.4;
    Real gamma = 1.0;

    std::vector<Real> sabrParams, zabrParams;

    sabrParams += alpha, beta, nu, rho;
    zabrParams += alpha, beta, nu, rho, gamma;

    boost::shared_ptr<SabrSmileSection> sabr = boost::make_shared<SabrSmileSection>(expiryTime, forward, sabrParams);

    boost::shared_ptr<ZabrSmileSection> zabrln =
        boost::make_shared<ZabrSmileSection>(
            expiryTime, forward, zabrParams,
            ZabrSmileSection::ShortMaturityLognormal);

    boost::shared_ptr<ZabrSmileSection> zabrfull =
        boost::make_shared<ZabrSmileSection>(
            expiryTime, forward, zabrParams,
            ZabrSmileSection::FullFd, std::vector<Real>(), 2);

    boost::shared_ptr<ZabrSmileSection> zabrfd =
        boost::make_shared<ZabrSmileSection>(
            expiryTime, forward, zabrParams,
            ZabrSmileSection::LocalVolatility, std::vector<Real>(), 2);

    // only for debug
    boost::shared_ptr<ZabrModel> tmpZabrFull = boost::make_shared<ZabrModel>(
        expiryTime, forward, alpha, beta, nu, rho, gamma);

    std::ofstream out;
    out.open("smiles.dat");

    // debug full fd
    std::vector<Real> tmpFullFd(100);
    Size tmpIdx = 0;
#pragma omp parallel for
    for(Size i=0;i<100;++i)
        tmpFullFd[i] = 0.0;//tmpZabrFull->fullFdPrice(0.0001+i*0.0010);
    // end debug full fd

    Real strike = 0.0001;
    while (strike <= 1.00) {
        // 1 strike
        out << strike << " ";
        // 2, 3, 4, 5  sabr
        out << sabr->volatility(strike) << " "
                  << sabr->optionPrice(strike) << " "
                  << sabr->digitalOptionPrice(strike) << " "
                  << sabr->density(strike) << " ";
        // 6, 7, 8, 9  zabr (lognormal short mat)
        out << zabrln->volatility(strike) << " "
                  << zabrln->optionPrice(strike) << " "
                  << zabrln->digitalOptionPrice(strike) << " "
                  << zabrln->density(strike) << " ";
        // 10, 11, 12, 13  zabr (full fd)
        out <<zabrfull->volatility(strike) << " "
                  << zabrfull->optionPrice(strike) << " "
                  << zabrfull->digitalOptionPrice(strike) << " "
                  << zabrfull->density(strike) << " ";
        // 14, 15, 16, 17  zabr (fd)
        out <<zabrfd->volatility(strike) << " "
                  << zabrfd->optionPrice(strike) << " "
                  << zabrfd->digitalOptionPrice(strike) << " "
                  << zabrfd->density(strike) << " ";
        // 14 zabr price full fd (debug)
        out << tmpFullFd[tmpIdx++];
        strike += 0.0010;
        out << std::endl;
    }

    out.close();

}

void zabrPaper() {

    // compare different SABR formulas

    std::ofstream out;
    out.open("smiles.dat");

    // this is an example from the paper, forward is a guess (since not
    // mentioned there)

    Real forward = 0.0325;
    Real expiryTime = 10.0;

    std::vector<Real> sabrParams, zabrParams;

    sabrParams.push_back(0.0873); // alpha
    sabrParams.push_back(0.7);    // beta
    sabrParams.push_back(0.47);   // nu
    sabrParams.push_back(-0.48);  // rho

    // Black Scholes with 20%
    // sabrParams.push_back(0.20); // alpha
    // Sabrparams.push_back(1.0);    // beta
    // sabrParams.push_back(0.0001);   // nu
    // sabrParams.push_back(0.0);  // rho

    // BS beta = 0.7
    // sabrParams.push_back(0.08); // alpha
    // sabrParams.push_back(0.7);    // beta
    // sabrParams.push_back(0.0001);   // nu
    // sabrParams.push_back(0.0);  // rho

    // BScholes nu = 0.50
    // sabrParams.push_back(0.20); // alpha
    // sabrParams.push_back(1.0);    // beta
    // sabrParams.push_back(0.50);   // nu
    // sabrParams.push_back(0.0);  // rho

    zabrParams.push_back(sabrParams[0]);
    zabrParams.push_back(sabrParams[1]);
    zabrParams.push_back(sabrParams[2]);
    zabrParams.push_back(sabrParams[3]);
    zabrParams.push_back(1.0); // gamma

    std::vector<Real> zabrParamsg0(zabrParams);
    zabrParamsg0[4] = 0.0;
    std::vector<Real> zabrParamsg05(zabrParams);
    zabrParamsg05[4] = 0.5;
    std::vector<Real> zabrParamsg15(zabrParams);
    zabrParamsg15[4] = 1.5;
    std::vector<Real> zabrParamsg17(zabrParams);
    zabrParamsg17[4] = 1.7;

    boost::shared_ptr<SabrSmileSection> sabr(
        new SabrSmileSection(expiryTime, forward, sabrParams));

    boost::shared_ptr<ZabrSmileSection> zabrln(
        new ZabrSmileSection(expiryTime, forward, zabrParams,
                             ZabrSmileSection::ShortMaturityLognormal));

    boost::shared_ptr<ZabrSmileSection> zabrnm(
        new ZabrSmileSection(expiryTime, forward, zabrParams,
                             ZabrSmileSection::ShortMaturityNormal));

    boost::shared_ptr<ZabrSmileSection> zabrlv(new ZabrSmileSection(
        expiryTime, forward, zabrParams, ZabrSmileSection::LocalVolatility));

    boost::shared_ptr<ZabrSmileSection> zabrnmg0(
        new ZabrSmileSection(expiryTime, forward, zabrParamsg0,
                             ZabrSmileSection::ShortMaturityNormal));
    boost::shared_ptr<ZabrSmileSection> zabrnmg05(
        new ZabrSmileSection(expiryTime, forward, zabrParamsg05,
                             ZabrSmileSection::ShortMaturityNormal));
    boost::shared_ptr<ZabrSmileSection> zabrnmg15(
        new ZabrSmileSection(expiryTime, forward, zabrParamsg15,
                             ZabrSmileSection::ShortMaturityNormal));
    boost::shared_ptr<ZabrSmileSection> zabrnmg17(
        new ZabrSmileSection(expiryTime, forward, zabrParamsg17,
                             ZabrSmileSection::ShortMaturityNormal));

    // boost::shared_ptr<ZabrSmileSection> zabrfd(new ZabrSmileSection(
    //     expiryTime, forward, zabrParams, ZabrSmileSection::FullFd));

    boost::shared_ptr<ZabrModel> zabrmodel = zabrln->model();

    Real strike = 0.0001; // we start at 1bp ...

    while (strike <= 0.50) { // ... and go to 50% ...

        // debug: localvol function for sabr hardcoded
        Real ytmp = strike >= 0.0
                        ? 1.0 / (sabrParams[0] * (1.0 - sabrParams[1])) *
                              (pow(forward, 1.0 - sabrParams[1]) -
                               pow(strike, 1.0 - sabrParams[1]))
                        : 1.0 / (sabrParams[0] * (1.0 - sabrParams[1])) *
                              (pow(-strike, 1.0 - sabrParams[1]) +
                               pow(forward, 1.0 - sabrParams[1]));
        Real Jtmp = sqrt(1.0 + sabrParams[2] * sabrParams[2] * ytmp * ytmp -
                         2.0 * sabrParams[3] * sabrParams[2] * ytmp);
        Real localVol =
            Jtmp * pow(std::fabs(strike), sabrParams[1]) * sabrParams[0];
        // end debug

        out << strike << " "                           // 1  SABR hagan
            << sabr->volatility(strike) << " "         // 2
            << sabr->optionPrice(strike) << " "        // 3
            << sabr->digitalOptionPrice(strike) << " " // 4
            << sabr->density(strike) << " "            // 5
            << zabrln->volatility(strike)
            << " " // 6  ZABR short maturity lognormal
            << zabrln->optionPrice(strike) << " "        // 7
            << zabrln->digitalOptionPrice(strike) << " " // 8
            << zabrln->density(strike) << " "            // 9
            << zabrnm->volatility(strike)
            << " " // 10  ZABR short maturity normal
            << zabrnm->optionPrice(strike) << " "        // 11
            << zabrnm->digitalOptionPrice(strike) << " " // 12
            << zabrnm->density(strike) << " "            // 13
            << zabrmodel->localVolatility(strike)
            << " " // 14  *** Equivalent deterministic volatility
            << localVol
            << " " // 15  *** same as 14, but hardcoded for SABR (debug)
            << zabrlv->volatility(strike) << " "  // 16  ZABR local vol
            << zabrlv->optionPrice(strike) << " " // 17
            << zabrlv->digitalOptionPrice(strike, Option::Call, 1.0, 1E-4)
            << " "                                                // 18
            << zabrlv->density(strike, Option::Call, 1E-4) << " " // 19
            // << zabrfd->volatility(strike) << " "         // 20  ZABR fd
            // << zabrfd->optionPrice(strike) << " "        // 21
            // << zabrfd->digitalOptionPrice(strike) << " " // 22
            // << zabrfd->density(strike) << " "            // 23
            << zabrnmg0->volatility(strike)
            << " " // 20  ZABR short maturity normal gamma=0.0
            << zabrnmg05->volatility(strike)
            << " " // 21  ZABR short maturity normal gamma=0.5
            << zabrnmg15->volatility(strike)
            << " " // 22  ZABR short maturity normal gamma=1.5
            << zabrnmg17->volatility(strike)
            << " " // 23  ZABR short maturity normal gamma=1.7
            << std::endl;

        strike += 0.0001; // ... in steps of 5bp
    }

    out.close();
}

void splineSmiles() {

    std::ofstream out;
    out.open("smiles2.dat");

    // same example as in zabrPaper

    Real forward = 0.0325;
    Real expiryTime = 10.0;

    std::vector<Real> sabrParams;

    sabrParams.push_back(0.0873); // alphacd
    sabrParams.push_back(0.7);    // beta
    sabrParams.push_back(0.47);   // nu
    sabrParams.push_back(-0.48);  // rho

    std::vector<Real> money;
    for(Size i=0;i<40;i++) {
        money.push_back(i/40.0);
    }
    for(Size i=10;i<=100;i++) {
        money.push_back(i*0.1);
    }
    for(Size i=11;i<=100;i++) {
        money.push_back(i*1.0);
    }

    boost::shared_ptr<SabrSmileSection> sabr(
        new SabrSmileSection(expiryTime, forward, sabrParams));

    boost::shared_ptr<SmileSection> flatSmileSection(new FlatSmileSection(expiryTime,0.10,Actual365Fixed(),forward));

    boost::shared_ptr<SplineDensitySmileSection> spline(
        new SplineDensitySmileSection(flatSmileSection, forward, 0.0, 1.00,
                                      false, money));

    std::cout << "arbitrage free region is " << spline->leftCoreStrike()
              << " ... " << spline->rightCoreStrike() << std::endl;

    Real strike = 0.00001;

    while (strike <= 3.0) {

        Real vol = 0.10;
        Real mu = std::log(forward) + vol*vol*expiryTime/2.0;
        Real referencePrice = blackFormula(Option::Call,strike,forward,vol*sqrt(expiryTime));
        Real referenceDigital = blackFormulaCashItmProbability(Option::Call,strike,forward,vol*sqrt(expiryTime));
        NormalDistribution norm(mu,vol*sqrt(expiryTime));
        Real referenceDensity = norm(std::log(strike))/strike;

        out << strike << " "                                    // 1
            // << sabr->volatility(strike) << " "                  // 2
            // << sabr->optionPrice(strike) << " "                 // 3
            // << sabr->digitalOptionPrice(strike) << " "          // 4
            // << sabr->density(strike) << " "                     // 5
            << flatSmileSection->volatility(strike) << " "                  // 2
            << flatSmileSection->optionPrice(strike) << " "                 // 3
            << flatSmileSection->digitalOptionPrice(strike) << " "          // 4
            << flatSmileSection->density(strike) << " "                     // 5
            << spline->volatility(strike) << " "                // 6
            << spline->optionPrice(strike) << " "               // 7
            << spline->digitalOptionPrice(strike) << " "        // 8
            << spline->density(strike) << " "                   // 9
            // << referencePrice << " " // 5
            // << referenceDigital << " " //6
            // << referenceDensity << " "  //7
            // << referenceDigital - spline->digitalOptionPrice(strike) << " " // 8
            << std::endl;
        strike += 0.0010;
    }

    out.close();
}

void sbSmiles() {

    std::ofstream out;
    out.open("smiles3.dat");

    Real forward = 0.0325;
    Real expiryTime = 10.0;

    boost::shared_ptr<SbSmileSection> sb(new SbSmileSection(expiryTime,forward,
                                                            0.0,0.50,1.0,1.0));

    Real strike = -1.0;

    while (strike <= 1.0) {

        out << strike << " "                                    // 1
            //<< sb->volatility(strike) << " "                  // 2
            << sb->optionPrice(strike) << " "                 // 3
            << sb->digitalOptionPrice(strike) << " "          // 4
            << sb->density(strike) << " "                     // 5
            << std::endl;
        strike += 0.0010;
    }

    out.close();

}

void interpolation() {

    std::ofstream out;
    out.open("sample.dat");

    Real x[] = { 0.0, 0.5, 1.0, 1.5, 2.0 };
    Real y1[] = { 0.0, 0.5, 1.0, 0.5, 0.0 };
    Real y2[] = { 0.0, 0.5, 0.2, 0.5, 0.0 };

    std::vector<Real> xv(x,x+5);
    std::vector<Real> yv1(y1,y1+5);
    std::vector<Real> yv2(y2,y2+5);

    CubicInterpolation i1(xv.begin(), xv.end(), yv1.begin(), CubicInterpolation::Parabolic, false,
                         CubicInterpolation::FirstDerivative, 0.0, CubicInterpolation::FirstDerivative, 0.0);

    CubicInterpolation i2(xv.begin(), xv.end(), yv2.begin(), CubicInterpolation::Parabolic, false,
                         CubicInterpolation::FirstDerivative, 0.0, CubicInterpolation::FirstDerivative, 0.0);

    Real h = 0.0;
    while(h <= 2.01) {
        out << h << " " << i1(h) << " " << i2(h) << std::endl;
        h+=0.01;
    }

    out.close();

}

int main(int, char * []) {
    zabrExamples();
    // splineSmiles();
}


#include <boost/numeric/odeint.hpp>
#include <iostream>
#include <fstream>
#include <vector>
#include <cassert>

using namespace boost::numeric::odeint;

using state_t = std::vector<double>;

int mod_subtract(int a, int b, int bound){
	//computes a - b mod bound
	int res = a - b;
	if(res < 0){
		while(res < 0){
			res += bound;
		}
	}
	else if(res >= bound){
		res -= bound;
	}
	return res;
}

void time_differentiate(const state_t& x, state_t& dxdt, const double /* type param */)
{
	auto J = x.size();

	//precondition: x and dx/dt have the same length
	assert(J == dxdt.size());

	auto bound = static_cast<long long>(J);

	double F = 8;

	for(int i = 0; i < bound; i++){
		dxdt[i] = ( x[(i+1) % bound] - x[mod_subtract(i, 2, bound)] ) * x[mod_subtract(i,1,bound)] - x[i] + F;
	}
}

void apply_initial_conditions(state_t& x){
	//apply initial conditions from the HW
	for(auto& val : x){
		val = 0;
	}
	x[1] = 1.0;
}

struct state_storage
{
	std::vector<state_t>& m_states;

	std::vector<double>& m_times;

	state_storage(std::vector<state_t>& states, std::vector<double>& times) :
																		m_states(states),
																		m_times(times) {};

	void operator()(const state_t& x, double t){
		m_states.push_back(x);
		m_times.push_back(t);
	}

};

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

	state_t x(10);

	std::vector<state_t> x_stored;
	std::vector<double> times;

	apply_initial_conditions(x);

	double time_length = 10.0;

	double init_step_size = 0.05;

	std::cout << "Integrating ODE over 10 second total time length.\n";

	//solve with runge_kutta54_cash_karp stepper
	size_t steps = integrate(time_differentiate, x, 0.0,
						   	 time_length, init_step_size,
							 state_storage(x_stored, times) );

	std::cout << "Solved ODE in " << steps << " timesteps.\n";

	std::ofstream filewriter;
	filewriter.open("hw1.csv");
	for(const auto& x : x_stored){
		for(const auto& val : x){
			filewriter << val << ", ";
		}
		filewriter << "\n";
	}

	filewriter.close();

	return 0;
}