#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <boost/format.hpp>
#include <pangolin/pangolin.h>

using namespace std;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// Camera intrinsics
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// baseline
double baseline = 0.573;
// paths
string left_file = "../../images/left.png";
string disparity_file = "../../images/disparity.png";
boost::format fmt_others("../../images/%06d.png");    // other files

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

/// class for accumulator jacobians in parallel
class JacobianAccumulator {
public:
    JacobianAccumulator(
        const cv::Mat &img1_,
        const cv::Mat &img2_,
        const VecVector2d &px_ref_,
        const vector<double> depth_ref_,    //FIXME: Use &?//FIXME: Use &?
        Sophus::SE3d &T21_) :
        img1(img1_), img2(img2_), px_ref(px_ref_), depth_ref(depth_ref_), T21(T21_) {
        projection = VecVector2d(px_ref.size(), Eigen::Vector2d(0, 0));
    }

    /// accumulate jacobians in a range
    void accumulate_jacobian(const cv::Range &range);

    /// get hessian matrix
    Matrix6d hessian() const { return H; }

    /// get bias
    Vector6d bias() const { return b; }

    /// get total cost
    double cost_func() const { return cost; }

    /// get projected points
    VecVector2d projected_points() const { return projection; }

    /// reset h, b, cost to zero
    void reset() {
        H = Matrix6d::Zero();
        b = Vector6d::Zero();
        cost = 0;
    }

private:
    const cv::Mat &img1;
    const cv::Mat &img2;
    const VecVector2d &px_ref;
    const vector<double> depth_ref;  //FIXME: Use &?
    Sophus::SE3d &T21;
    VecVector2d projection; // projected points

    std::mutex hessian_mutex;
    Matrix6d H = Matrix6d::Zero();
    Vector6d b = Vector6d::Zero();
    double cost = 0;
};

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationMultiLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21
);

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21
);

// bilinear interpolation
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    // boundary check
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols) x = img.cols - 1;
    if (y >= img.rows) y = img.rows - 1;
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    return float(
        (1 - xx) * (1 - yy) * data[0] +
        xx * (1 - yy) * data[1] +
        (1 - xx) * yy * data[img.step] +
        xx * yy * data[img.step + 1]
    );
}

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

    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng;
    int nPoints = 2000;
    int boarder = 20;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++) {
        int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    Sophus::SE3d T_cur_ref;

    for (int i = 1; i < 6; i++) {  // 1~10
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
        // try single layer by uncomment this line
        // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
        DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
    }
    return 0;
}

void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,  //FIXME: Use &?
    Sophus::SE3d &T21) {

    const int iterations = 10;
    double cost = 0, lastCost = 0;
    auto t1 = chrono::steady_clock::now();
    JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);

    for (int iter = 0; iter < iterations; iter++) {
        jaco_accu.reset();
        cv::parallel_for_(cv::Range(0, px_ref.size()),
                          std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, std::placeholders::_1));
        Matrix6d H = jaco_accu.hessian();
        Vector6d b = jaco_accu.bias();

        // solve update and put it into estimation
        Vector6d update = H.ldlt().solve(b);;
        T21 = Sophus::SE3d::exp(update) * T21;
        cost = jaco_accu.cost_func();

        if (std::isnan(update[0])) {
            // sometimes occurred when we have a black or white patch and H is irreversible
            cout << "update is nan" << endl;
            break;
        }
        if (iter > 0 && cost > lastCost) {
            cout << "cost increased: " << cost << ", " << lastCost << endl;
            break;
        }
        if (update.norm() < 1e-3) {
            // converge
            break;
        }

        lastCost = cost;
        cout << "iteration: " << iter << ", cost: " << cost << endl;
    }

    cout << "T21 = \n" << T21.matrix() << endl;
    auto t2 = chrono::steady_clock::now();
    auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "direct method for single layer: " << time_used.count() << endl;

    // plot the projected pixels here
    cv::Mat img2_show;
    cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
    VecVector2d projection = jaco_accu.projected_points();
    for (size_t i = 0; i < px_ref.size(); ++i) {
        auto p_ref = px_ref[i];
        auto p_cur = projection[i];
        if (p_cur[0] > 0 && p_cur[1] > 0) {
            cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
                     cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("current", img2_show);
    cv::waitKey();
}

void JacobianAccumulator::accumulate_jacobian(const cv::Range &range) {

    // parameters
    const int half_patch_size = 1;
    int cnt_good = 0;
    Matrix6d hessian = Matrix6d::Zero();
    Vector6d bias = Vector6d::Zero();
    double cost_tmp = 0;

    for (size_t i = range.start; i < range.end; i++) {

        // compute the projection in the second image
        Eigen::Vector3d point_ref =
            depth_ref[i] * Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
        Eigen::Vector3d point_cur = T21 * point_ref;
        if (point_cur[2] < 0)   // depth invalid
            continue;

        float u = fx * point_cur[0] / point_cur[2] + cx, v = fy * point_cur[1] / point_cur[2] + cy;
        if (u < half_patch_size || u > img2.cols - half_patch_size || v < half_patch_size ||
            v > img2.rows - half_patch_size)
            continue;

        projection[i] = Eigen::Vector2d(u, v);
        double X = point_cur[0], Y = point_cur[1], Z = point_cur[2],
            Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;
        cnt_good++;

        // and compute error and jacobian
        for (int x = -half_patch_size; x <= half_patch_size; x++)
            for (int y = -half_patch_size; y <= half_patch_size; y++) {

                double error = GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) -
                               GetPixelValue(img2, u + x, v + y);
                Matrix26d J_pixel_xi;
                Eigen::Vector2d J_img_pixel;

                J_pixel_xi(0, 0) = fx * Z_inv;
                J_pixel_xi(0, 1) = 0;
                J_pixel_xi(0, 2) = -fx * X * Z2_inv;
                J_pixel_xi(0, 3) = -fx * X * Y * Z2_inv;
                J_pixel_xi(0, 4) = fx + fx * X * X * Z2_inv;
                J_pixel_xi(0, 5) = -fx * Y * Z_inv;

                J_pixel_xi(1, 0) = 0;
                J_pixel_xi(1, 1) = fy * Z_inv;
                J_pixel_xi(1, 2) = -fy * Y * Z2_inv;
                J_pixel_xi(1, 3) = -fy - fy * Y * Y * Z2_inv;
                J_pixel_xi(1, 4) = fy * X * Y * Z2_inv;
                J_pixel_xi(1, 5) = fy * X * Z_inv;

                J_img_pixel = Eigen::Vector2d(
                    0.5 * (GetPixelValue(img2, u + 1 + x, v + y) - GetPixelValue(img2, u - 1 + x, v + y)),
                    0.5 * (GetPixelValue(img2, u + x, v + 1 + y) - GetPixelValue(img2, u + x, v - 1 + y))
                );

                // total jacobian
                Vector6d J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose();

                hessian += J * J.transpose();
                bias += -error * J;
                cost_tmp += error * error;
            }
    }

    if (cnt_good) {
        // set hessian, bias and cost
        unique_lock<mutex> lck(hessian_mutex);
        H += hessian;
        b += bias;
        cost += cost_tmp / cnt_good;
    }
}

void DirectPoseEstimationMultiLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,  //FIXME: Use &?
    Sophus::SE3d &T21) {

    // parameters
    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    // create pyramids
    vector<cv::Mat> pyr1, pyr2; // image pyramids
    for (int i = 0; i < pyramids; i++) {
        if (i == 0) {
            pyr1.push_back(img1);
            pyr2.push_back(img2);
        } else {
            cv::Mat img1_pyr, img2_pyr;
            cv::resize(pyr1[i - 1], img1_pyr,
                       cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
            cv::resize(pyr2[i - 1], img2_pyr,
                       cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
            pyr1.push_back(img1_pyr);
            pyr2.push_back(img2_pyr);
        }
    }

    double fxG = fx, fyG = fy, cxG = cx, cyG = cy;  // backup the old values
    for (int level = pyramids - 1; level >= 0; level--) {
        VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
        for (auto &px: px_ref) {
            px_ref_pyr.push_back(scales[level] * px);
        }

        // scale fx, fy, cx, cy in different pyramid levels
        fx = fxG * scales[level];
        fy = fyG * scales[level];
        cx = cxG * scales[level];
        cy = cyG * scales[level];
        DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
    }

}


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

#pragma once

#include "FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>

namespace fluid {
namespace algorithm {

class WeightedStats
{
public:
  Eigen::ArrayXd process(Eigen::Ref<Eigen::ArrayXd> input,
                         Eigen::Ref<Eigen::ArrayXd> weights, double low,
                         double mid, double high)
  {
    using namespace Eigen;
    using namespace std;
    index   length = input.size();
    ArrayXd out = ArrayXd::Zero(7);
    double  mean = (weights * input).sum();
    double  stdev = sqrt((weights * (input - mean).square()).sum());
    double  skewness =
        (weights * ((input - mean) / (stdev == 0 ? 1 : stdev)).cube()).sum();
    double kurtosis =
        (weights * ((input - mean) / (stdev == 0 ? 1 : stdev)).pow(4)).sum();
    ArrayXd sorted = input;
    ArrayXidx perm = ArrayXidx::LinSpaced(length, 0, length - 1);
    std::sort(perm.data(), perm.data() + length,
              [&](index i, index j) { return input(i) < input(j); });
    index  level = 0;
    double lowVal{input(perm(0))};
    double midVal{input(perm(lrint(length - 1) / 2))};
    double hiVal{input(perm(lrint(length - 1)))};
    double acc = weights(perm(0)), prevAcc = 0;
    for (index i = 1; i < length; i++)
    {
      acc += weights(perm(i));
      if (level == 0 && acc >= low)
      {
        lowVal = abs(prevAcc - low) <= abs(acc - low) ? input(perm(i - 1))
                                                      : input(perm(i));
        level = 1;
      }
      if (level == 1 && acc >= mid)
      {
        midVal = abs(prevAcc - mid) < abs(acc - mid) ? input(perm(i - 1))
                                                     : input(perm(i));
        level = 2;
      }
      if (level == 2 && acc >= high)
      {
        hiVal = abs(prevAcc - high) < abs(acc - high) ? input(perm(i - 1))
                                                      : input(perm(i));
        break;
      }
      prevAcc = acc;
    }
    out << mean, stdev, skewness, kurtosis, lowVal, midVal, hiVal;
    return out;
  }
};
} // namespace algorithm
} // namespace fluid


#include <geom/aabb.h>
#include <geom/matrix.h>
#include <geom/primitives.h>
#include <geom/properties.h>

#include <Eigen/Dense>

double SignedTriangleVolume6(double3 a, double3 b, double3 c) {
    double z = a.z;
    z += b.z;
    z += c.z;
    double s;
    s = (a.y + b.y) * (a.x - b.x);
    s += (b.y + c.y) * (b.x - c.x);
    s += (c.y + a.y) * (c.x - a.x);
    return s * z;
}

double SignedRingVolume6(cspan<double3> ring) {
    double z = 0;
    for (auto e : ring) z += e.z;
    double s = 0;
    for (auto e : Edges(ring)) s += (e.a.y + e.b.y) * (e.a.x - e.b.x);
    return s * z;
}

double SignedVolume(const mesh3& mesh) {
    double v = 0;
    for (const triangle3& f : mesh) v += SignedTriangleVolume6(f.a, f.b, f.c);
    return v / 6.0;
}

double SignedVolume(const xmesh3& mesh) {
    double v = 0;
    for (face f : mesh)
        for (auto ring : f) v += SignedRingVolume6(ring);
    return v / 6.0;
}

// TODO test with volume of cube (randomly rotated)

// Center of mass of a valid polyhedron
double3 CenterOfMass(const mesh3& mesh) {
    double3 P = {0, 0, 0};
    double V = 0;
    for (const triangle3& f : mesh) {
        double v = dot(f.a, cross(f.b, f.c));
        P += (f.a + f.b + f.c) * v;
        V += v;
    }
    return P / (V * 4);
}

double2 centroid(cspan<double2> poly) {
    double2 P = {0, 0};
    double2 V = 0;
    for (auto i : range(poly.size())) {
        double2 a = poly[i];
        double2 b = poly[(i + 1) % poly.size()];
        double v = a.x * b.y - a.y * b.x;
        P += (a + b) * v;
        V += v;
    }
    return P / (V * 3);
}

double2 CenterOfMass(const polygon2& poly) {
    double2 P = {0, 0};
    double2 V = 0;
    for (auto [a, b] : Edges(poly)) {
        double v = a.x * b.y - a.y * b.x;
        P += (a + b) * v;
        V += v;
    }
    return P / (V * 3);
}

// Moment of inertia of a valid polyhedron with Center of Mass = 0 and Density = 1
double33 moment_of_inertia(const mesh3& mesh) {
    constexpr double a = 1 / 60., b = 1 / 120.;
    const double33 canonical = {{a, b, b}, {b, a, b}, {b, b, a}};
    auto C = double33::make(0);  // covariance
    for (const auto& f : mesh) {
        double33 A;
        // TODO setting rows or columns?
        A.a = f[0];
        A.b = f[1];
        A.c = f[2];
        C += transpose(A) * canonical * A * det(A);
    }
    return double33::make(trace(C)) - C;  // C -> I
}

bool is_aabb(const mesh3& mesh) {
    aabb3 box(mesh);

    // All vertices must be made from extreme coordinates
    for (auto f : mesh)
        for (double3 v : f)
            if (v.x != box.min[0] && v.x != box.max[0] && v.y != box.min[1] && v.y != box.max[1] && v.z != box.min[2] &&
                v.z != box.max[2])
                return false;

    // Every face must have one coordinate constant
    for (auto f : mesh) {
        bool xx = f.a.x == f.b.x && f.b.x == f.c.x;
        bool yy = f.a.y == f.b.y && f.b.y == f.c.y;
        bool zz = f.a.z == f.b.z && f.b.z == f.c.z;
        if (!xx && !yy && !zz) return false;
    }
    return true;
}

double3 eigen_vector(cspan<double3> points) {
    // compute mean
    double3 m = {0, 0, 0};
    for (auto p : points) m += p;
    m /= points.size();

    // compute matrix
    double3 ss = {0, 0, 0};
    double xy = 0, xz = 0, yz = 0;
    for (auto p : points) {
        p -= m;
        ss += p * p;
        xy += p.x * p.y;
        xz += p.x * p.z;
        yz += p.y * p.z;
    }

    Eigen::MatrixXd a = Eigen::MatrixXd::Zero(3, 3);
    a(0, 0) = ss.x;
    a(1, 1) = ss.y;
    a(2, 2) = ss.z;
    a(0, 1) = a(1, 0) = xy;
    a(0, 2) = a(2, 0) = xz;
    a(1, 2) = a(2, 1) = yz;

    Eigen::EigenSolver<Eigen::MatrixXd> es(a, true);
    auto values = es.eigenvalues();
    int i = 0;
    if (values(1).real() < values(i).real()) i = 1;
    if (values(2).real() < values(i).real()) i = 2;

    auto vectors = es.eigenvectors();
    return {vectors(0, i).real(), vectors(1, i).real(), vectors(2, i).real()};
}


//######################################################################
//#   SDF_Fusion Module
//#
//#   Copyright (C) 2020 Siemens AG
//#   SPDX-License-Identifier: MIT
//#   Author 2020: This module has been developed by
//#                or under supervision of Slobodan Ilic
//#######################################################################

#include "visualizer.hpp"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/video.hpp>

using namespace cv;
using namespace Eigen;
using namespace std;

vector<Mat> transform_and_project(const vector<Mat>& points, const Mat& intrinsics, const Mat& pose)
{
	vector<Mat> projected_2d_points(points.size());
	transform(points.begin(), points.end(), projected_2d_points.begin(), [&pose, &intrinsics] (const auto &point)
	{
		Mat transformed = pose * point;
		Mat projected = intrinsics * transformed.rowRange(0, 3);
		projected /= projected.at<float>(2);
		return projected;
	});

	return projected_2d_points;
}

void draw_origin_axis(Mat &colored_img, const Mat& intrinsics, const Mat& pose, float basis_length = 0.1) {
	vector<Mat> axes_endpoints;
	axes_endpoints.push_back((Mat_<float>(4, 1) << 0, 0, 0, 1));
	axes_endpoints.push_back((Mat_<float>(4, 1) << basis_length, 0, 0, 1));
	axes_endpoints.push_back((Mat_<float>(4, 1) << 0, basis_length, 0, 1));
	axes_endpoints.push_back((Mat_<float>(4, 1) << 0, 0, basis_length, 1));

	// transform cube into camera reference frame

	vector<Mat> p2d = transform_and_project(axes_endpoints, intrinsics, pose);

	Point p0 = Point(p2d[0].at<float>(0), p2d[0].at<float>(1));
	Point px = Point(p2d[1].at<float>(0), p2d[1].at<float>(1));
	Point py = Point(p2d[2].at<float>(0), p2d[2].at<float>(1));
	Point pz = Point(p2d[3].at<float>(0), p2d[3].at<float>(1));

	line(colored_img, p0, px, cv::Scalar(0, 0, 255), 1, 8);
	line(colored_img, p0, py, cv::Scalar(0, 255, 0), 1, 8);
	line(colored_img, p0, pz, cv::Scalar(255, 0, 0), 1, 8);

	circle(colored_img, p0, 200 / 32.0, cv::Scalar(255, 255, 0), -1, 8);
}

void visualize_volume(const Mat &source_image, const Mat &intrinsics, const Mat &rotation, const Mat &translation, const Mat& llc, const Mat& upc)
{
	Mat image = source_image.clone();

	Mat pose = Mat::eye(4, 4, CV_32F);
	rotation.copyTo(pose.colRange(0, 3).rowRange(0, 3));
	translation.copyTo(pose.col(3).rowRange(0, 3));

	Point3f llc_p(llc);
	Point3f upc_p(upc);

	// Points in the world coordinates
	vector<Mat> volume_points;
	volume_points.push_back((Mat_<float>(4, 1) << llc_p.x, llc_p.y, llc_p.z, 1));
	volume_points.push_back((Mat_<float>(4, 1) << llc_p.x, llc_p.y, upc_p.z, 1));
	volume_points.push_back((Mat_<float>(4, 1) << upc_p.x, llc_p.y, upc_p.z, 1));
	volume_points.push_back((Mat_<float>(4, 1) << upc_p.x, llc_p.y, llc_p.z, 1));
	volume_points.push_back((Mat_<float>(4, 1) << llc_p.x, upc_p.y, llc_p.z, 1));
	volume_points.push_back((Mat_<float>(4, 1) << llc_p.x, upc_p.y, upc_p.z, 1));
	volume_points.push_back((Mat_<float>(4, 1) << upc_p.x, upc_p.y, upc_p.z, 1));
	volume_points.push_back((Mat_<float>(4, 1) << upc_p.x, upc_p.y, llc_p.z, 1));

	// transform cube into camera reference frame
	vector<Mat> p2d = transform_and_project(volume_points, intrinsics, pose);

	// Draw cube
	Scalar line_color = Scalar(148, 24, 248);
	// Generate edge points
	vector<pair<int, int>> edge_point_indices;
	edge_point_indices.push_back(make_pair(1, 5));
	edge_point_indices.push_back(make_pair(2, 6));
	edge_point_indices.push_back(make_pair(3, 7));
	edge_point_indices.push_back(make_pair(0, 4));

	edge_point_indices.push_back(make_pair(0, 1));
	edge_point_indices.push_back(make_pair(1, 2));
	edge_point_indices.push_back(make_pair(2, 3));
	edge_point_indices.push_back(make_pair(3, 0));

	edge_point_indices.push_back(make_pair(4, 7));
	edge_point_indices.push_back(make_pair(7, 6));
	edge_point_indices.push_back(make_pair(6, 5));
	edge_point_indices.push_back(make_pair(5, 4));

	// Draw edges

	for (const auto &point_indices : edge_point_indices)
	{
		int p0_i = point_indices.first;
		Point p0 = Point(p2d[p0_i].at<float>(0), p2d[p0_i].at<float>(1));
		int p1_i = point_indices.second;
		Point p1 = Point(p2d[p1_i].at<float>(0), p2d[p1_i].at<float>(1));
		line(image, p0, p1, line_color, 1, 8);

	}

	// Draw circles
	for (const auto& point : p2d)
	{
		Point point_to_draw = Point(point.at<float>(0), point.at<float>(1));
		circle(image, point_to_draw, 200 / 32.0, line_color, -1, 8);
	}

	draw_origin_axis(image, intrinsics, pose);

	Mat image_to_show;

	// resize if too big for showing
	if (image.cols > 800)
	{
		Size show_size = Size(image.cols / 2, image.rows / 2);
		resize(image, image_to_show, show_size);
	} else
	{
		image_to_show = image;
	}

	imshow("image", image_to_show);
}

void visualize_volume(const Mat& input_image, const Matrix3f& intrinsics, const Isometry3f& pose, const vector<float>& volume)
{
	Mat intrinsics_cv;

	eigen2cv(intrinsics, intrinsics_cv);

	Mat llc = (Mat_<float>(3, 1) << volume[0], volume[2], volume[4]);
	Mat upc = (Mat_<float>(3, 1) << volume[1], volume[3], volume[5]);

	const Matrix4f &pose_eigen = pose.matrix();
	Mat pose_cv;
	eigen2cv(pose_eigen, pose_cv);

	Mat rotation = pose_cv(Range(0, 3), Range(0, 3));
	Mat translation = pose_cv(Range(0, 3), Range(3, 4));

	visualize_volume(input_image, intrinsics_cv, rotation, translation, llc, upc);
}


#define BOOST_TEST_MODULE SparseLatticeMultTests

#include "DenseLattice.h"
#include "SparseLattice.h"
#include "MIAConfig.h"

#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

template<typename data_type>
void multwork(size_t m1, size_t n1, size_t n2, size_t p){
    typedef LibMIA::DenseLattice<data_type> denseType;
    typedef LibMIA::SparseLattice<data_type> sparseType;

    denseType DenseLat1(m1,n1,p);
    denseType DenseLat2(n1,n2,p);
    denseType DenseLat3;

    sparseType SparseLat1(m1,n1,p);
    sparseType SparseLat2(n1,n2,p);
    sparseType SparseLat3;

    DenseLat1.randu(0,10);
    DenseLat2.randu(0,10);
    for(auto it=DenseLat1.data_begin();it<DenseLat1.data_end();++it)
        if(*it<7)
            *it=0;
    for(auto it=DenseLat2.data_begin();it<DenseLat2.data_end();++it)
        if(*it<7)
            *it=0;

    SparseLat1=DenseLat1;
//    std::cout << "**************First LAT " << std::endl;
//    DenseLat1.print();
//    SparseLat1.print();
    SparseLat2=DenseLat2;
//    std::cout << "**************Second LAT " << std::endl;
//    DenseLat2.print();
//    SparseLat2.print();
    DenseLat3=DenseLat1*DenseLat2;
//    std::cout << "**************Result " << std::endl;
//    DenseLat3.print();
    SparseLat3=SparseLat1*SparseLat2;
//    SparseLat3.print();
    BOOST_CHECK_MESSAGE(DenseLat3.fuzzy_equals(SparseLat3,test_precision<data_type>()),std::string("Full Dimension Mult Test for ")+typeid(data_type).name());

    DenseLat1=denseType(1,n1,p);
    DenseLat2=denseType(n1,1,p);
    DenseLat1.randu(0,10);
    DenseLat2.randu(0,10);
    for(auto it=DenseLat1.data_begin();it<DenseLat1.data_end();++it)
        if(*it<7)
            *it=0;
    for(auto it=DenseLat2.data_begin();it<DenseLat2.data_end();++it)
        if(*it<7)
            *it=0;

    SparseLat1=DenseLat1;
    SparseLat2=DenseLat2;

    DenseLat3=DenseLat1*DenseLat2;
    SparseLat3=SparseLat1*SparseLat2;

    BOOST_CHECK_MESSAGE(DenseLat3.fuzzy_equals(SparseLat3,test_precision<data_type>()),std::string("Repeated Inner Product Mult Test for ")+typeid(data_type).name());

    DenseLat1=denseType(m1,1,p);
    DenseLat2=denseType(1,n2,p);
    DenseLat1.randu(0,10);
    DenseLat2.randu(0,10);
    for(auto it=DenseLat1.data_begin();it<DenseLat1.data_end();++it)
        if(*it<7)
            *it=0;
    for(auto it=DenseLat2.data_begin();it<DenseLat2.data_end();++it)
        if(*it<7)
            *it=0;

    SparseLat1=DenseLat1;
    SparseLat2=DenseLat2;

    DenseLat3=DenseLat1*DenseLat2;
    SparseLat3=SparseLat1*SparseLat2;
    BOOST_CHECK_MESSAGE(DenseLat3.fuzzy_equals(SparseLat3,test_precision<data_type>()),std::string("Repeated Outer Product Mult Test for ")+typeid(data_type).name());

    DenseLat1=denseType(m1,n1,1);
    DenseLat2=denseType(n1,n2,1);
    DenseLat1.randu(0,10);
    DenseLat2.randu(0,10);
    for(auto it=DenseLat1.data_begin();it<DenseLat1.data_end();++it)
        if(*it<7)
            *it=0;
    for(auto it=DenseLat2.data_begin();it<DenseLat2.data_end();++it)
        if(*it<7)
            *it=0;

    SparseLat1=DenseLat1;
    SparseLat2=DenseLat2;

    DenseLat3=DenseLat1*DenseLat2;
    SparseLat3=SparseLat1*SparseLat2;
    BOOST_CHECK_MESSAGE(DenseLat3.fuzzy_equals(SparseLat3,test_precision<data_type>()),std::string("No Depth Mult Test for ")+typeid(data_type).name());

    DenseLat1=denseType(m1,1,p);
    DenseLat2=denseType(1,1,p);
    DenseLat1.randu(0,10);
    DenseLat2.randu(0,10);

    for(auto it=DenseLat1.data_begin();it<DenseLat1.data_end();++it)
        if(*it<7)
            *it=0;
    for(auto it=DenseLat2.data_begin();it<DenseLat2.data_end();++it)
        if(*it<7)
            *it=0;

    SparseLat1=DenseLat1;
    SparseLat2=DenseLat2;

    DenseLat3=DenseLat1*DenseLat2;
    SparseLat3=SparseLat1*SparseLat2;

    BOOST_CHECK_MESSAGE(DenseLat3.fuzzy_equals(SparseLat3,test_precision<data_type>()),std::string("Outer product, one operand, test for ")+typeid(data_type).name());

}

BOOST_AUTO_TEST_CASE( SparseLatticeMultTests )
{

    //multwork<double>(5,5,5,1);
    multwork<double>(20,20,20,20);
    multwork<float>(20,20,20,20);
    multwork<int>(20,20,20,20);
    multwork<long>(20,20,20,20);

    multwork<double>(40,20,20,20);
    multwork<float>(40,20,20,20);
    multwork<int>(40,20,20,20);
    multwork<long>(40,20,20,20);

    multwork<double>(20,20,40,20);
    multwork<float>(20,20,40,20);
    multwork<int>(20,20,40,20);
    multwork<long>(20,20,40,20);

}


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, Robert Bosch GmbH
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Robert Bosch GmbH nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Leonard Bruns */

#include "ompl/base/samplers/deterministic/HaltonSequence.h"
#include "ompl/util/Console.h"
#include <iostream>
#include <cmath>
#include <map>
#include <boost/math/special_functions/prime.hpp>

namespace ompl
{
    namespace base
    {
        HaltonSequence1D::HaltonSequence1D() : i_(1), base_(2)
        {
        }

        HaltonSequence1D::HaltonSequence1D(unsigned int base) : i_(1), base_(base)
        {
        }

        void HaltonSequence1D::setBase(unsigned int base)
        {
            base_ = base;
        }

        double HaltonSequence1D::sample()
        {
            double f = 1, r = 0;
            unsigned int i = i_;

            while (i > 0)
            {
                f /= base_;
                r += f * (i % base_);
                i = std::floor(i / base_);
            }

            ++i_;
            return r;
        }

        HaltonSequence::HaltonSequence(unsigned int dimensions)
          : DeterministicSequence(dimensions), halton_sequences_1d_(dimensions)
        {
            setBasesToPrimes();
        }

        HaltonSequence::HaltonSequence(unsigned int dimensions, std::vector<unsigned int> bases)
          : DeterministicSequence(dimensions), halton_sequences_1d_(dimensions)
        {
            if (bases.size() != dimensions)
            {
                OMPL_WARN("Number of bases does not match dimensions. Using first n primes instead.");
            }
            else
            {
                int i = 0;
                for (auto base : bases)
                {
                    halton_sequences_1d_[i].setBase(base);
                    i++;
                }
            }
        }

        std::vector<double> HaltonSequence::sample()
        {
            std::vector<double> samples;
            for (auto &seq : halton_sequences_1d_)
            {
                samples.push_back(seq.sample());
            }
            return samples;
        }

        void HaltonSequence::setBasesToPrimes()
        {
            // set the base of the halton sequences to the first n prime numbers, where n is dimensions
            unsigned int current = 2;
            for (unsigned int i = 0; i < dimensions_; i++)
            {
                current = boost::math::prime(i);
                halton_sequences_1d_[i].setBase(current);
            }
        }
    }  // namespace base
}  // namespace ompl


/* Copyright (c) 2021 Grumpy Cat Software S.L.
 *
 * This Source Code is licensed under the MIT 2.0 license.
 * the terms can be found in  LICENSE.md at the root of
 * this project, or at http://mozilla.org/MPL/2.0/.
 */

#include <gauss/linalg.h>
#include <Eigen/Eigenvalues>
#include <gauss/internal/scopedHostPtr.h>
#include <tuple>

af::array gauss::linalg::eigvalsh(const af::array &m) {
    auto mtype = m.type();

    if (mtype == af::dtype::c32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cfloat>());
        auto typed = (std::complex<float>*)matHost.get();
        Eigen::MatrixXcf mat = Eigen::Map<Eigen::MatrixXcf>(typed, m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXcf> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);
        return af::array(m.dims(0), solver.eigenvalues().data());
    }

    if (mtype == af::dtype::c64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cdouble>());
        auto typed = (std::complex<double>*)matHost.get();
        Eigen::MatrixXcd mat = Eigen::Map<Eigen::MatrixXcd>(typed, m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXcd> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);
        return af::array(m.dims(0), solver.eigenvalues().data());
    }

    if (mtype == af::dtype::f64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<double>());
        Eigen::MatrixXd mat = Eigen::Map<Eigen::MatrixXd>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);
        return af::array(m.dims(0), solver.eigenvalues().data());
    }

    if (mtype == af::dtype::f32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<float>());
        Eigen::MatrixXf mat = Eigen::Map<Eigen::MatrixXf>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);
        return af::array(m.dims(0), solver.eigenvalues().data());
    }

    if (mtype == af::dtype::s64 || mtype == af::dtype::u64)
        return eigvalsh(m.as(af::dtype::f64));

    return eigvalsh(m.as(af::dtype::f32));
}

af::array gauss::linalg::eigvals(const af::array &m) {
    auto mtype = m.type();

    if (mtype == af::dtype::c32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cfloat>());
        auto typed = (std::complex<float>*)matHost.get();
        Eigen::MatrixXcf mat = Eigen::Map<Eigen::MatrixXcf>(typed, m.dims(0), m.dims(1));
        Eigen::ComplexEigenSolver<Eigen::MatrixXcf> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);
        auto eivals = solver.eigenvalues();
        auto eivals_af = (af::af_cfloat*) eivals.data();
        return af::array(m.dims(0), eivals_af);
    }

    if (mtype == af::dtype::c64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cdouble>());
        auto typed = (std::complex<double>*)matHost.get();
        Eigen::MatrixXcd mat = Eigen::Map<Eigen::MatrixXcd>(typed, m.dims(0), m.dims(1));
        Eigen::ComplexEigenSolver<Eigen::MatrixXcd> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);

        auto eivals = solver.eigenvalues();
        auto eivals_af = (af::af_cdouble*) eivals.data();
        return af::array(m.dims(0), eivals_af);
    }

    if (mtype == af::dtype::f64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<double>());
        Eigen::MatrixXd mat = Eigen::Map<Eigen::MatrixXd>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::EigenSolver<Eigen::MatrixXd> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);
        auto eivals = solver.eigenvalues();
        auto eivals_af = (af::af_cdouble*) eivals.data();
        return af::array(m.dims(0), eivals_af);
    }

    if (mtype == af::dtype::f32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<float>());
        Eigen::MatrixXf mat = Eigen::Map<Eigen::MatrixXf>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::EigenSolver<Eigen::MatrixXf> solver(mat, Eigen::DecompositionOptions::EigenvaluesOnly);
        auto eivals = solver.eigenvalues();
        auto eivals_af = (af::af_cfloat*) eivals.data();
        return af::array(m.dims(0), eivals_af);
    }

    if (mtype == af::dtype::s64 || mtype == af::dtype::u64)
        return eigvals(m.as(af::dtype::f64));

    return eigvals(m.as(af::dtype::f32));
}

std::tuple<af::array, af::array> gauss::linalg::eig(const af::array &m) {

    auto mtype = m.type();

    if (mtype == af::dtype::c32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cfloat>());
        auto typed = (std::complex<float>*)matHost.get();
        Eigen::MatrixXcf mat = Eigen::Map<Eigen::MatrixXcf>(typed, m.dims(0), m.dims(1));
        Eigen::ComplexEigenSolver<Eigen::MatrixXcf> solver(mat);
        auto eivals = solver.eigenvalues();
        auto eivect = solver.eigenvectors();
        auto eivals_af = (af::af_cfloat*) eivals.data();
        auto eivect_af = (af::af_cfloat*) eivect.data();
        auto eigenValues = af::array(m.dims(0), eivals_af);
        auto eigenVectors = af::array(m.dims(0), m.dims(1), eivect_af);
        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::c64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cdouble>());
        auto typed = (std::complex<double>*)matHost.get();

        Eigen::MatrixXcd mat = Eigen::Map<Eigen::MatrixXcd>(typed, m.dims(0), m.dims(1));
        Eigen::ComplexEigenSolver<Eigen::MatrixXcd> solver(mat);

        auto eivals = solver.eigenvalues();
        auto eivect = solver.eigenvectors();
        auto eivals_af = (af::af_cdouble*) eivals.data();
        auto eivect_af = (af::af_cdouble*) eivect.data();
        auto eigenValues = af::array(m.dims(0), eivals_af);
        auto eigenVectors = af::array(m.dims(0), m.dims(1), eivect_af);
        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::f64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<double>());
        Eigen::MatrixXd mat = Eigen::Map<Eigen::MatrixXd>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::EigenSolver<Eigen::MatrixXd> solver(mat);
        auto eivals = solver.eigenvalues();
        auto eivect = solver.eigenvectors();
        auto eivals_af = (af::af_cdouble*) eivals.data();
        auto eivect_af = (af::af_cdouble*) eivect.data();
        auto eigenValues = af::array(m.dims(0), eivals_af);
        auto eigenVectors = af::array(m.dims(0), m.dims(1), eivect_af);
        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::f32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<float>());
        Eigen::MatrixXf mat = Eigen::Map<Eigen::MatrixXf>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::EigenSolver<Eigen::MatrixXf> solver(mat);
        auto eivals = solver.eigenvalues();
        auto eivect = solver.eigenvectors();
        auto eivals_af = (af::af_cfloat*) eivals.data();
        auto eivect_af = (af::af_cfloat*) eivect.data();
        auto eigenValues = af::array(m.dims(0), eivals_af);
        auto eigenVectors = af::array(m.dims(0), m.dims(1), eivect_af);
        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::s64 || mtype == af::dtype::u64)
        return eig(m.as(af::dtype::f64));

    return eig(m.as(af::dtype::f32));
}

std::tuple<af::array, af::array> gauss::linalg::eigh(const af::array &m) {

    auto mtype = m.type();

    if (mtype == af::dtype::c32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cfloat>());
        auto typed = (std::complex<float>*)matHost.get();
        Eigen::MatrixXcf mat = Eigen::Map<Eigen::MatrixXcf>(typed, m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXcf> solver(mat);
        auto eigenValues = af::array(m.dims(0), solver.eigenvalues().data());

        auto eivect = solver.eigenvectors();
        auto eivect_af = (af::af_cfloat*) eivect.data();
        auto eigenVectors = af::array(m.dims(0), m.dims(1), eivect_af);

        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::c64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<af::af_cdouble>());
        auto typed = (std::complex<double>*)matHost.get();

        Eigen::MatrixXcd mat = Eigen::Map<Eigen::MatrixXcd>(typed, m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXcd> solver(mat);

        auto eigenValues = af::array(m.dims(0), solver.eigenvalues().data());

        auto eivect = solver.eigenvectors();
        auto eivect_af = (af::af_cdouble*) eivect.data();
        auto eigenVectors = af::array(m.dims(0), m.dims(1), eivect_af);

        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::f64) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<double>());
        Eigen::MatrixXd mat = Eigen::Map<Eigen::MatrixXd>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(mat);
        auto eigenValues = af::array(m.dims(0), solver.eigenvalues().data());
        auto eigenVectors = af::array(m.dims(0), m.dims(1), solver.eigenvectors().data());
        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::f32) {
        auto matHost = gauss::utils::makeScopedHostPtr(m.host<float>());
        Eigen::MatrixXf mat = Eigen::Map<Eigen::MatrixXf>(matHost.get(), m.dims(0), m.dims(1));
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> solver(mat);

        auto eigenValues = af::array(m.dims(0), solver.eigenvalues().data());
        auto eigenVectors = af::array(m.dims(0), m.dims(1), solver.eigenvectors().data());
        return std::make_tuple(eigenValues, eigenVectors);
    }

    if (mtype == af::dtype::s64 || mtype == af::dtype::u64)
        return eigh(m.as(af::dtype::f64));

    return eigh(m.as(af::dtype::f32));
}

af::array gauss::linalg::lls(const af::array &A, const af::array &b) {
    af::array U;
    af::array S;
    af::array VT;

    af::svd(U, S, VT, A);

    S = af::diag(S, 0, false);

    af::array S_dagger = (S != 0).as(S.type()) * af::inverse(S);

    long missingRows = static_cast<long>(A.dims(1) - S.dims(0));
    long missingColumns = static_cast<long>(A.dims(0) - S.dims(0));
    af::array toPadRows = af::constant(0, missingRows, A.dims(0), A.type());
    af::array toPadColumns = af::constant(0, A.dims(1), missingColumns, A.type());

    S_dagger = af::join(0, S_dagger, toPadRows);
    S_dagger = af::join(1, S_dagger, toPadColumns);

    af::array V = af::transpose(VT);
    af::array UT = af::transpose(U);
    af::array x = af::matmul(V, S_dagger, UT, b);

    return x;
}

af::array gauss::linalg::levinsonDurbin(af::array acv, int order) {
    af::array result = af::constant(0, order + 1, acv.dims(1), acv.type());

    for (int i = 0; i < acv.dims(1); i++) {
        af::array phi = af::constant(0, order + 1, order + 1, acv.type());
        af::array sig = af::constant(0, order + 1, acv.type());

        phi(1, 1) = acv(1, i) / acv(0, i);
        sig(1) = acv(0, i) - (phi(1, 1) * acv(1, i));

        // First iteration, to avoid problems with negative sequences with 1 element
        int k = 2;
        if (k < (order + 1)) {
            phi(k, k) = (acv(k, i) - af::dot(phi(af::seq(1, k - 1), k - 1), acv(af::seq(1, k - 1), i))) / sig(k - 1);
            for (int j = 1; j < k; j++) {
                phi(j, k) = phi(j, k - 1) - (phi(k, k) * phi(k - j, k - 1));
            }
            sig(k) = sig(k - 1) * (1.0 - phi(k, k) * phi(k, k));
        }

        // Second and subsequent iterations
        for (int l = 3; l < (order + 1); l++) {
            af::array aux = acv(af::seq(1, l - 1), i);
            phi(l, l) = (acv(l, i) - af::dot(phi(af::seq(1, l - 1), l - 1),
                                             aux(af::seq(static_cast<double>(aux.dims(0)) - 1, 0, -1)))) /
                        sig(l - 1);
            for (int j = 1; j < l; j++) {
                phi(j, l) = phi(j, l - 1) - (phi(l, l) * phi(l - j, l - 1));
            }
            sig(l) = sig(l - 1) * (1.0 - phi(l, l) * phi(l, l));
        }

        af::array pac = af::diag(phi);
        pac(0) = 1.0;
        result(af::span, i) = pac;
    }
    return result;
}


#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iostream>
#include <fstream>
#include <vector>

using namespace std;

void vector2transform(vector<double> &vec, Eigen::Matrix4d &t){
    t(0,3) = vec[0];
    t(1,3) = vec[1];
    t(2,3) = vec[2];
    Eigen::Quaterniond temp;
    temp.w() = vec[3];
    temp.x() = vec[4];
    temp.y() = vec[5];
    temp.z() = vec[6];
    t.block(0,0,3,3) = temp.toRotationMatrix();
}

int main(){
    string path_in = "/home/chenchr/ttx.txt";
    fstream file_in(path_in, ios::in);
    vector<vector<double> > all;
    for(int i=0; i<10; ++i){
        double x, y, z, qw, qx, qy, qz;
        char temp;
        file_in >> x >> temp >> y >> temp >> z >> temp >> qw >> temp >> qx >> temp >> qy >> temp >> qz;
        all.push_back({x, y, z, qw, qx, qy, qz});
    }
    vector<Eigen::Matrix4d> all_T;
    for(int i=0; i<10; ++i){
        all_T.push_back(Eigen::Matrix4d::Identity());
        vector2transform(all[i], all_T[i]);
    }
    for(int i=0; i<10; ++i){
        cout << i << ": " << endl;
        for(int j=0; j<7; ++j)
            cout << all[i][j] << " ";
        cout << endl;
        cout << all_T[i] << endl;
    }
    return 0;
}


//
// Created by joshua on 9/6/17.
//
#include <mps/planner/util/Math.h>
#include <cmath>
#include <boost/math/constants/constants.hpp>

void mps::planner::util::math::normalize_orientation(double& value) {
    // from ompl::base::SO2StateSpace::enforceBounds
    double v = fmod(value, 2.0 * boost::math::constants::pi<double>());
    if (v < -boost::math::constants::pi<double>())
        v += 2.0 * boost::math::constants::pi<double>();
    else if (v >= boost::math::constants::pi<double>())
        v -= 2.0 * boost::math::constants::pi<double>();
    value = v;
}

void mps::planner::util::math::normalize_orientation(float& value) {
    float v = fmod(value, 2.0f * boost::math::constants::pi<float>());
    if (v < -boost::math::constants::pi<float>())
        v += 2.0f * boost::math::constants::pi<float>();
    else if (v >= boost::math::constants::pi<float>())
        v -= 2.0f * boost::math::constants::pi<float>();
    value = v;
}

/**
 * Returns the shortest direction from val_1 to val_2 assuming that both values
 * are angles in radian in [-pi, pi)
 * @param val_1
 * @param val_2
 * @return the smallest change in angle da such that val_1 + da = val_2 mod 2pi
 */
float mps::planner::util::math::shortest_direction_so2(float val_1, float val_2) {
    float value = val_2 - val_1;
    if (std::abs(value) > boost::math::constants::pi<float>()) {
        if (value > 0.0f) { // val_2 > val_1
            value -= 2.0f * boost::math::constants::pi<float>();
        } else {
            value += 2.0f * boost::math::constants::pi<float>();
        }
    }
    return value;
}


#include <armadillo>
#include <iostream>

using namespace arma;
using namespace std;

int main() {
    mat A {{1.0, 2.0}, {3.0, 4.0}};
    A.print("A:");
    mat B = A*A.i();
    B.print("B:");
    mat C = eye<mat>(2, 2);
    C.print("C:");
    cout << "abs. sum: " <<  accu(abs(B - C)) << endl;
    return 0;
}


#include "geo_calculator.h"
#include <Eigen/Sparse>
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>
#include <assert.h>

#include "cgal_predef.h"

//// CGAL
// Compute OBB
#include <CGAL/Polygon_2.h>
#include <CGAL/min_quadrilateral_2.h>
// convex hull
#include <CGAL/convex_hull_2.h>
// polyline simplification
#include <CGAL/Polyline_simplification_2/simplify.h>
#include <CGAL/Polyline_simplification_2/Squared_distance_cost.h>
// curve reconstruction from unorganized point set
#include <CGAL/Optimal_transportation_reconstruction_2.h>
// random point generator
#include <CGAL/point_generators_2.h>

double CGeoCalculator::ComputeDistFromPoint2Plane(COpenMeshT::Point p, COpenMeshT::Point b, COpenMeshT::Point n)
{
	// assert n is a unit vector
	assert((n.norm() - 1.0) < 10e-5);

	COpenMeshT::Point v = p - b;
	return innerProduct(v, n);
}

double CGeoCalculator::ComputeLinearity(std::vector<COpenMeshT::Point> p_list)
{
	if (p_list.size() < 3)
	{
		return 1.0;
	}
	else
	{
		std::vector<std::vector<double>> eigenvectors;
		std::vector<double> lambda;
		ComputePCAwithSVD(p_list, eigenvectors, lambda);
		//ComputePCAwithEIG(p_list, eigenvectors, lambda);
		return lambda[0] / (lambda[0] + lambda[1] + lambda[2]);
	}
}

double CGeoCalculator::ComputeLineLinearity(std::vector<COpenMeshT::Point> p_list, COpenMeshT::Point ctr, COpenMeshT::Point dir)
{
	double sum_dev = 0.0;
	for (int i = 0; i < p_list.size(); i++)
	{
		double dev = (p_list[i] - ctr - innerProduct(p_list[i] - ctr, dir)*dir).norm();
		sum_dev += dev;
	}
	std::cout << "center = " << ctr << std::endl;
	std::cout << "direction = " << dir << std::endl;
	std::cout << sum_dev <<" "<< p_list.size() << std::endl;
	sum_dev /= double(p_list.size());
	return sum_dev;
}

void CGeoCalculator::FittingLineSegmentToPointSet(std::vector<COpenMeshT::Point> p_list, COpenMeshT::Point & pstart, COpenMeshT::Point & pend)
{
	// base
	COpenMeshT::Point base(0, 0, 0);
	for (int i = 0; i < p_list.size(); i++)
	{
		base += p_list[i];
	}
	base = base / double(p_list.size());
	for (int i = 0; i < p_list.size(); i++)
	{
		p_list[i] -= base;
	}

	// get line direction
	std::vector<double> direction;
	ComputePrincipalDirection(p_list, direction);

	std::cout << "line direction: ";
	std::copy(direction.begin(), direction.end(), std::ostream_iterator<double>(std::cout, " "));
	std::cout << std::endl;

	// project every point to direction and find the max and min parameters
	double max_t = 0, min_t = 0;
	COpenMeshT::Point v(direction[0], direction[1], direction[2]);
	for (int i = 0; i < p_list.size(); i++) {
		double t = innerProduct(v, p_list[i]);
		std::cout << "t = " << t << std::endl;
		if (t > max_t)
			max_t = t;
		if (t < min_t)
			min_t = t;
	}
	std::cout << "min_t = "<< min_t << ", max_t = " << max_t << std::endl;
	// output
	pstart = base + v*min_t;
	pend = base + v*max_t;
}

void CGeoCalculator::FittingLineSegmentToPointSet(std::vector<COpenMeshT::Point> p_list,
	COpenMeshT::Point base, COpenMeshT::Point dir,
	COpenMeshT::Point & pstart, COpenMeshT::Point & pend)
{
	// base
	for (int i = 0; i < p_list.size(); i++)
	{
		p_list[i] -= base;
	}
	// project every point to direction and find the max and min parameters
	double max_t = 0, min_t = 0;
	for (int i = 0; i < p_list.size(); i++) {
		double t = innerProduct(dir, p_list[i]);
		std::cout << "t = " << t << std::endl;
		if (t > max_t)
			max_t = t;
		if (t < min_t)
			min_t = t;
	}
	std::cout << "min_t = " << min_t << ", max_t = " << max_t << std::endl;
	// output
	pstart = base + dir*min_t;
	pend = base + dir*max_t;
}

void CGeoCalculator::FittingPlaneToPointSet(std::vector<COpenMeshT::Point> p_list, std::vector<double>& transform)
{
	transform.clear();
	int Dim = 3;
	Eigen::MatrixXd P(p_list.size(), Dim);
	Eigen::MatrixXd V(Dim, Dim);

	COpenMeshT::Point center_p(0, 0, 0);
	for (int i = 0; i < p_list.size(); i++)
	{
		center_p += p_list[i];
	}
	center_p = center_p / double(p_list.size());
	for (int i = 0; i < p_list.size(); i++)
	{
		P(i, 0) = p_list[i][0] - center_p[0];
		P(i, 1) = p_list[i][1] - center_p[1];
		P(i, 2) = p_list[i][2] - center_p[2];
	}

	Eigen::JacobiSVD<Eigen::MatrixXd> svd(P, Eigen::ComputeFullU | Eigen::ComputeFullV);
	V = svd.matrixV();

	Eigen::Vector3d n = V.col(2);
	n.normalized();
	transform.push_back(n[0]);
	transform.push_back(n[1]);
	transform.push_back(n[2]);
}

void CGeoCalculator::ComputePrincipalDirection(std::vector<COpenMeshT::Point> p_list, std::vector<double>& transform)
{
	transform.clear();
	int Dim = 3;
	Eigen::MatrixXd P(p_list.size(), Dim);
	Eigen::MatrixXd V(Dim, Dim);

	COpenMeshT::Point center_p(0, 0, 0);
	for (int i = 0; i < p_list.size(); i++)
	{
		center_p += p_list[i];
	}
	center_p = center_p / double(p_list.size());
	for (int i = 0; i < p_list.size(); i++)
	{
		P(i, 0) = p_list[i][0] - center_p[0];
		P(i, 1) = p_list[i][1] - center_p[1];
		P(i, 2) = p_list[i][2] - center_p[2];
	}

	Eigen::JacobiSVD<Eigen::MatrixXd> svd(P, Eigen::ComputeThinU | Eigen::ComputeThinV);
	V = svd.matrixV();

	//std::cout << "P = [" << P << "]" << std::endl;
	//std::cout << "V = [" << V << "]" << std::endl;

	Eigen::Vector3d n = V.col(0);
	n.normalized();
	transform.push_back(n[0]);
	transform.push_back(n[1]);
	transform.push_back(n[2]);
}

void CGeoCalculator::ComputePCAwithEIG(std::vector<COpenMeshT::Point> p_list,
	std::vector<std::vector<double>>& eigenvectors, std::vector<double>& eigenvalues)
{
	eigenvectors.clear();
	int Dim = 3;
	Eigen::MatrixXd P(p_list.size(), Dim);
	Eigen::MatrixXd V(Dim, Dim);
	Eigen::MatrixXd S(Dim, Dim);

	// construct
	COpenMeshT::Point center_p(0, 0, 0);
	for (int i = 0; i < p_list.size(); i++)
	{
		center_p += p_list[i];
	}
	center_p = center_p / double(p_list.size());
	for (int i = 0; i < p_list.size(); i++)
	{
		P(i, 0) = p_list[i][0] - center_p[0];
		P(i, 1) = p_list[i][1] - center_p[1];
		P(i, 2) = p_list[i][2] - center_p[2];
	}

	Eigen::MatrixXd Pt = P.transpose();
	Eigen::MatrixXd PtP = P.transpose() * P;
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigen_solver(PtP);
	S = eigen_solver.eigenvalues().asDiagonal();
	V = eigen_solver.eigenvectors();

	//std::cout << "using ComputePCAwithEIG function" << std::endl;
	//std::cout << "S = [" << S << "]" << std::endl;
	//std::cout << "V = [" << V << "]" << std::endl;

	// make it in the descending order
	for (int i = Dim; i > 0; i--)
	{
		std::vector<double> tmp_vector;
		Eigen::Vector3d n = V.col(i-1);
		n.normalized();
		tmp_vector.push_back(n[0]);
		tmp_vector.push_back(n[1]);
		tmp_vector.push_back(n[2]);
		eigenvectors.push_back(tmp_vector);
		eigenvalues.push_back(S(i-1, i-1));
	}
}

void CGeoCalculator::ComputePCAwithSVD(std::vector<COpenMeshT::Point> p_list,
	std::vector<std::vector<double>>& eigenvectors, std::vector<double>& eigenvalues)
{
	eigenvectors.clear();
	eigenvalues.clear();

	int Dim = 3;
	Eigen::MatrixXd P(p_list.size(), Dim);
	Eigen::MatrixXd V(Dim, Dim);
	Eigen::VectorXd S(Dim);

	COpenMeshT::Point center_p(0, 0, 0);
	for (int i = 0; i < p_list.size(); i++)
	{
		center_p += p_list[i];
	}
	center_p = center_p / double(p_list.size());
	for (int i = 0; i < p_list.size(); i++)
	{
		P(i, 0) = p_list[i][0] - center_p[0];
		P(i, 1) = p_list[i][1] - center_p[1];
		P(i, 2) = p_list[i][2] - center_p[2];
	}
	Eigen::MatrixXd CovP = P.transpose() * P;
	CovP = CovP / double(p_list.size());
	Eigen::JacobiSVD<Eigen::MatrixXd> svd(CovP, Eigen::ComputeThinU | Eigen::ComputeThinV);
	V = svd.matrixV();
	S = svd.singularValues();

	//std::cout << "using ComputePCAwithSVD function" << std::endl;
	//std::cout << "S = [" << S << "]" << std::endl;
	//std::cout << "V = [" << V << "]" << std::endl;

	for (int i = 0; i < 3; i++)
	{
		std::vector<double> tmp_vector;
		Eigen::Vector3d n = V.col(i);
		n.normalized();
		tmp_vector.push_back(n[0]);
		tmp_vector.push_back(n[1]);
		tmp_vector.push_back(n[2]);
		eigenvectors.push_back(tmp_vector);
		double s = sqrt(S(i));
		eigenvalues.push_back(s);
	}
}

void CGeoCalculator::ProjectPointToPlane(COpenMeshT::Point point, std::vector<double> plane, COpenMeshT::Point & projection)
{
	double d = point[0] * plane[0] + point[1] * plane[1] + point[2] * plane[2] + plane[3];
	COpenMeshT::Point v(plane[0] * d, plane[1] * d, plane[2] * d);
	projection = point - v;
}

double CGeoCalculator::ComputeDistPointToLine(COpenMeshT::Point q, COpenMeshT::Point sp, COpenMeshT::Point tp)
{
	COpenMeshT::Point dir = tp - sp;
	dir /= dir.norm();
	return (q - sp - innerProduct(q - sp, dir)*dir).norm();
}

void CGeoCalculator::givensTransform(double x, double y, std::vector<double>& g)
{
	double absx = fabs(x);
	double c, s;

	if (absx == 0.0)
	{
		c = 0.0; s = 1.0;
	}
	else {
		double nrm = Eigen::Vector2d(x, y).norm();
		c = absx / nrm;
		s = x / absx*(y / nrm);
	}
	g.push_back(c);
	g.push_back(s);
}

void CGeoCalculator::computeOBB(const std::vector<COpenMeshT::Point> & pts, std::vector<double> & out_obb)
{
	typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
	typedef K::Point_2								Point_2;
	typedef CGAL::Polygon_2<K>						Polygon_2;

	std::vector<Point_2> pointset;
	for (int i = 0; i < pts.size(); i++)
	{
		pointset.push_back(Point_2(pts[i][1], pts[i][2]));
	}

	Polygon_2 ch, obb;
	CGAL::convex_hull_2(pointset.begin(), pointset.end(), std::back_inserter(ch));
	CGAL::min_rectangle_2(ch.vertices_begin(), ch.vertices_end(), std::back_inserter(obb));
	out_obb.clear();
	out_obb.push_back(obb.bbox().xmin());
	out_obb.push_back(obb.bbox().xmax());
	out_obb.push_back(obb.bbox().ymin());
	out_obb.push_back(obb.bbox().xmax());
}

std::vector<double>  CGeoCalculator::getBernsteinBasis(int n, double step, Eigen::MatrixXd & Bmat)
{
	// parameters
	std::vector<double> t;
	double it = 0.0;
	t.push_back(it);
	while (it < 1)
	{
		it += step;
		t.push_back(it);
	}
	if (fabs(t.back() - 1.0) > 0.00001)
		t.push_back(1.0);

	//for (int i = 0; i < t.size(); i++)
	//	std::cout << t[i] << std::endl;

	Bmat.resize(t.size(), n);
	for (int j = 0; j < n; j++)
	{
		for (int i = 0; i < t.size(); i++) {
			double c = CGeoCalculator::nchoosek(n - 1, j);
			Bmat(i, j) = c*pow(t[i], j)*pow((1.0-t[i]), (n - 1 - j));
		}
	}

	return t;
}

void CGeoCalculator::getBernsteinBasis(int n, std::vector<double> t, Eigen::MatrixXd & Bmat)
{
	Bmat.resize(t.size(), n);
	Bmat.setZero();
	for (int j = 0; j < n; j++)
	{
		for (int i = 0; i < t.size(); i++) {
			double c = CGeoCalculator::nchoosek(n - 1, j);
			Bmat(i, j) = c*pow(t[i], j)*pow((1.0 - t[i]), (n - 1 - j));
		}
	}
}

void CGeoCalculator::getSampleFromBezier(
	Eigen::MatrixXd & sample_pts,
	Eigen::MatrixXd & ctrl_pts,
	double step)
{
	int numCPs = ctrl_pts.rows();
	Eigen::MatrixXd Bmat;
	getBernsteinBasis(numCPs, step, Bmat);
	sample_pts = Bmat*ctrl_pts;
}

void CGeoCalculator::getEqualArcLengthSampleFromBezier(
	std::vector<COpenMeshT::Point>& om_sample_pts,
	std::vector<double>& tparas,
	const std::vector<COpenMeshT::Point>& om_ctrl_pts, int Nsamples)
{
	om_sample_pts.clear();
	tparas.clear();

	Eigen::MatrixXd ctrl_pts;
	convertOMptToEigenMat(om_ctrl_pts, ctrl_pts);

	int numCPs = ctrl_pts.rows();
	Eigen::MatrixXd Bmat;
	double dt = 0.0001;
	std::vector<double> ts;
	ts = getBernsteinBasis(numCPs, dt, Bmat);
	// get total arc length
	Eigen::MatrixXd dense_pts;
	dense_pts = Bmat*ctrl_pts;
	double total_length = 0.0;
	for (int i = 1; i < dense_pts.rows(); i++)
	{
		total_length += (dense_pts.row(i) - dense_pts.row(i - 1)).norm();
	}
	double dD = total_length / double(Nsamples);
	double accum_length = 0.0;
	std::vector<double> t_samples;
	t_samples.push_back(0);
	for (int i = 1; i < dense_pts.rows(); i++)
	{
		if (accum_length > dD) {
			//std::cout << accum_length << ", " << dD << std::endl;
			t_samples.push_back(ts[i]);
			accum_length = 0.0;
		}
		else {
			accum_length += (dense_pts.row(i) - dense_pts.row(i - 1)).norm();
		}
	}
	t_samples.push_back(1.0);
	getBernsteinBasis(numCPs, t_samples, Bmat);
	Eigen::MatrixXd sample_pts;
	sample_pts = Bmat*ctrl_pts;

	// convert sample_pts to om_pts;
	convertEigenMatToOMpt(sample_pts, om_sample_pts);
	tparas = t_samples;
}

double CGeoCalculator::nchoosek(int n, int k)
{
	if (k < 0 || k > n) {
		std::cerr << "Error: k should be less than n" << std::endl;
		return 0;
	}
	if (k == 0) {
		return 1;
	}
	return (n * nchoosek(n - 1, k - 1)) / k;
}

void CGeoCalculator::convertOMptToEigenMat(const std::vector<COpenMeshT::Point> om_pts, Eigen::MatrixXd & Mat)
{
	std::vector<double> om_pt_vec;
	for (int i = 0; i < om_pts.size(); i++)
	{
		om_pt_vec.push_back(om_pts[i][0]);
		om_pt_vec.push_back(om_pts[i][1]);
		om_pt_vec.push_back(om_pts[i][2]);
	}
	Mat = Eigen::Map<Eigen::MatrixXd, Eigen::Unaligned>
		(om_pt_vec.data(), 3, om_pt_vec.size() / 3);
	Mat.transposeInPlace();
}

void CGeoCalculator::convertEigenMatToOMpt(const Eigen::MatrixXd & Mat, std::vector<COpenMeshT::Point>& om_pts)
{
	om_pts.clear();
	for (int i = 0; i < Mat.rows(); i++)
	{
		COpenMeshT::Point pt(Mat(i, 0), Mat(i, 1), Mat(i, 2));
		om_pts.push_back(pt);
	}
}

void CGeoCalculator::pts_sorting_alg(std::vector<COpenMeshT::Point>& pts)
{
	// use delaunay to compute the visibility graph
	double xCoord = pts[0][0];
	DelaunayT dt;
	for (int i = 0; i < pts.size(); i++)
	{
		KPoint2 kp(pts[i][1], pts[i][2]);
		dt.insert(kp);
	}

	COpenMeshT triMesh;
	std::map<DelaunayT::Vertex_handle, COpenMeshT::VertexHandle> cgal_om_vh_map;
	std::vector<std::vector<DelaunayT::Vertex_handle>> f_vhandles_cgal;
	for (DelaunayT::Finite_faces_iterator fiter = dt.finite_faces_begin();
		fiter != dt.finite_faces_end(); ++fiter)
	{
		std::vector<COpenMeshT::VertexHandle> vhandles;
		for (int i = 0; i < 3; i++) {
			DelaunayT::Vertex_handle vh_cgal = fiter->vertex(i);
			if (cgal_om_vh_map.find(vh_cgal) == cgal_om_vh_map.end()) {
				COpenMeshT::VertexHandle vh_om =
					triMesh.add_vertex(COpenMeshT::Point(0, vh_cgal->point().x(), vh_cgal->point().y()));
				cgal_om_vh_map[vh_cgal] = vh_om;
				vhandles.push_back(vh_om);
			}
			else
			{
				COpenMeshT::VertexHandle vh_om = cgal_om_vh_map[vh_cgal];
				vhandles.push_back(vh_om);
			}
		}
		triMesh.add_face(vhandles);
	}

	COpenMeshT::VertexHandle seed_vh;
	for (COpenMeshT::VertexIter viter = triMesh.vertices_begin(); viter != triMesh.vertices_end(); ++viter)
	{
		if (triMesh.is_boundary(*viter))
		{
			seed_vh = *viter;
			break;
		}
	}

	std::vector<COpenMeshT::VertexHandle> b_vertices;
	auto next_vh = seed_vh;
	do {
		b_vertices.push_back(next_vh);
		for (COpenMeshT::VertexOHalfedgeCCWIter voh_ccwiter = triMesh.voh_ccwbegin(next_vh);
			voh_ccwiter != triMesh.voh_ccwend(next_vh); ++voh_ccwiter)
		{
			if (triMesh.is_boundary(*voh_ccwiter))
			{
				next_vh = triMesh.to_vertex_handle(*voh_ccwiter);
				break;
			}
		}
	} while (next_vh != seed_vh);

	// output
	pts.clear();
	//std::cout << b_vertices.size() << std::endl;
	for (int i = 0; i < b_vertices.size(); i++)
	{
		COpenMeshT::Point p = triMesh.point(b_vertices[i]);
		p[0] = xCoord;
		pts.push_back(p);
		//std::cout << p[1] << " " << p[2] << std::endl;
	}

}

void CGeoCalculator::simplify_polygon(std::vector<COpenMeshT::Point>& pts, double dT)
{
	namespace PS = CGAL::Polyline_simplification_2;
	typedef PS::Stop_above_cost_threshold		Stop;
	typedef PS::Squared_distance_cost			Cost;

	double xCoord = pts[0][0];
	KPolygon2 polygon;
	for (int i = 0; i < pts.size(); i++)
		polygon.push_back(KPoint2(pts[i][1], pts[i][2]));

	Cost cost;
	polygon = PS::simplify(polygon, cost, Stop(dT));

	pts.clear();
	for (int i = 0; i < polygon.size(); i++) {
		pts.push_back(COpenMeshT::Point(xCoord, polygon[i].x(), polygon[i].y()));
	}
}

void CGeoCalculator::sample_polygon(std::vector<COpenMeshT::Point>& pts, double spacing, bool is_closed)
{
	if (is_closed)
		pts.push_back(pts.front());
	std::vector<COpenMeshT::Point> samples;

	// step size = ssz
	double alength = 0.0;
	for (int i = 0; i < pts.size() - 1; i++)
	{
		alength += (pts[i] - pts[i + 1]).norm();
	}
	double ssz = alength*spacing;

	// sample
	for (int i = 0; i < pts.size() - 1; i++)
	{
		double t_alength = (pts[i] - pts[i + 1]).norm();
		int cnt = 0;
		double t = 0.0;
		while (t < 1.0)
		{
			COpenMeshT::Point pt = pts[i] + t * (pts[i + 1] - pts[i]);
			if ((pt - pts[i + 1]).norm() > ssz) {
				samples.push_back(pt);
			}
			cnt++;
			t = double(cnt)*ssz / t_alength;
		}
	}

	//if (is_closed)
	//	samples.push_back(samples.front());
	pts = samples;
}

void CGeoCalculator::sample_polygon_parameters(std::vector<double> &tparas,
	const std::vector<COpenMeshT::Point> pts, double spacing, bool is_closed)
{
	tparas.clear();
	std::vector<COpenMeshT::Point> in_pts;
	if (is_closed)
		in_pts.push_back(pts.front());

	// step size = ssz
	double alength = 0.0;
	for (int i = 0; i < pts.size() - 1; i++)
	{
		alength += (pts[i] - pts[i + 1]).norm();
	}
	double ssz = alength*spacing;

	// sample
	for (int i = 0; i < pts.size() - 1; i++)
	{
		double t_alength = (pts[i] - pts[i + 1]).norm();
		int cnt = 0;
		double t = 0.0;
		while (t < 1.0)
		{
			COpenMeshT::Point pt = pts[i] + t * (pts[i + 1] - pts[i]);
			if ((pt - pts[i + 1]).norm() > ssz) {
				tparas.push_back(t);
			}
			cnt++;
			t = double(cnt)*ssz / t_alength;
		}
	}
	//tparas.push_back(1.0);
}

void CGeoCalculator::sample_polygon_with_parameters(std::vector<double> tparas,
	std::vector<COpenMeshT::Point> cs_pts, std::vector<COpenMeshT::Point> &out_pts, bool is_closed)
{
	//
	out_pts.clear();
	if (is_closed)
		cs_pts.push_back(cs_pts.front());

	// sample
	int pid = 0;
	for (int i = 0; i < tparas.size(); i++)
	{
		double t = tparas[i];
		if (t == 0.0) pid++;
		COpenMeshT::Point pt = cs_pts[pid-1] + t * (cs_pts[pid] - cs_pts[pid-1]);
		out_pts.push_back(pt);
	}
}

void CGeoCalculator::reconstruct_curve_from_pointset(std::vector<COpenMeshT::Point> &pts, float tolOMT)
{
	typedef CGAL::Optimal_transportation_reconstruction_2<K>    Otr_2;
	std::vector<KPoint2> points;
	for (int i = 0; i < pts.size(); i++)
		points.push_back(KPoint2(pts[i][1], pts[i][2]));

	std::cout << "init pts: " << points.size() << std::endl;
	if (points.size() == 0)
		return;

	std::vector<double> obb;
	computeOBB(pts, obb);
	double dl = (std::max)((obb[1] - obb[0]) / 2.,
		(obb[3] - obb[2]) / 2.);
	tolOMT = dl*tolOMT;
	std::cout << "tolOMT: " << tolOMT << std::endl;
	Otr_2 otr2(points);
	//otr2.set_random_sample_size(15);
	//otr2.set_relevance(0.3);
	std::vector<KPoint2> ptss;
	std::vector<std::size_t> isolated_vertices;
	std::vector<std::pair<std::size_t, std::size_t> > edges;
	int cntIter = 0;
	//float tolOMT = 0.6;
	otr2.run_under_wasserstein_tolerance(tolOMT);

	otr2.indexed_output(
		std::back_inserter(ptss),
		std::back_inserter(isolated_vertices),
		std::back_inserter(edges));
	std::cout << "(-------------List output---------- )" << tolOMT << " " << cntIter << std::endl;
	// points
	std::vector<KPoint2>::iterator pit;
	for (pit = ptss.begin(); pit != ptss.end(); pit++)
		std::cout << *pit << std::endl;
	// isolated vertices
	std::vector<std::size_t>::iterator vit;
	for (vit = isolated_vertices.begin(); vit != isolated_vertices.end(); vit++)
		std::cout << "1 " << *vit << std::endl;
	// edges
	std::vector<std::pair<std::size_t, std::size_t> >::iterator eit;
	for (eit = edges.begin(); eit != edges.end(); eit++)
		std::cout << "2 " << eit->first << " " << eit->second << std::endl;

	//KPoint2 orig(0.0, 0.0);
	//for (eit = edges.begin(); eit != edges.end(); eit++)
	//{
	//	KVector2 v12 = ptss[eit->second] - ptss[eit->first];
	//	KVector2 v01 = ptss[eit->first] - orig;
	//	double a = v01.x*v12*
	//}

}


#include "NewtonMethodEqNode.h"
#include <cmath>
#include <Eigen/Dense>

void NewtonEqNode::solve(EqNodePtr& root, EqNodePtr var)
{
	EqNodePtr derivRoot;
	double x1, x2, f1, tol, df1, err;
	int kmax, k;

	tol = 1e-8;
	kmax = 100;

	x1 = var->getValue();
	f1 = root->getValue();
	df1 = root->getDerValue(*var);

	for (k = 0; k < kmax; k++) {
		x2 = x1 - f1 / df1;
		err = abs(x2 - x1);

		var->v = x2;
		x1 = x2;
		if (err < tol)
			return;

		f1 = root->getValue();
		df1 = root->getDerValue(*var);
	}
}

void NewtonEqNode::NLASolve(const int& n, std::vector< EqNodePtr >& eqs,
	std::vector < EqNodePtr >& vars, const double& tol)
{
	int kmax, k;
	Eigen::MatrixXd Jac(n, n);
	Eigen::VectorXd F(n), x(n);
	Eigen::VectorXd delta(n);

	kmax = 300;
	double err = 0;

	for (const auto& var : vars)
		var->correctValueInsideBoundaries();

	for (k = 0; k < kmax; ++k) {

		for (int i = 0; i < n; ++i) {
			x[i] = vars[i]->v;
			F[i] = eqs[i]->getValue();
			for (int j = 0; j < n; ++j)
				Jac(i, j) = eqs[i]->getDerValue(*vars[j]);
		}

		// Solve linear system
		delta = Jac.fullPivLu().solve(-F);
		// Update x values
		x = x + delta;

		for (int i = 0; i < n; ++i) vars[i]->v = x[i];

		// criterion for stop
		// get max abs value of function
		err = 0.0;
		for (int i = 0; i < n; ++i) {
			err = err < std::abs(F[i]) ? abs(F[i]) : err;
			err = err < std::abs(delta[i]) ? abs(delta[i]) : err;
		}

		if (err < tol) return;

		for (auto& var : vars) var->correctValueInsideBoundaries();
	}
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/math/matrix/componentwise_equal.hpp>
#include <fcppt/math/matrix/inverse.hpp>
#include <fcppt/math/matrix/row.hpp>
#include <fcppt/math/matrix/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <limits>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_matrix_inverse
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::matrix::static_<
		double,
		3,
		3
	>
	large_matrix_type;

	BOOST_CHECK((
		fcppt::math::matrix::componentwise_equal(
			fcppt::math::matrix::inverse(
				large_matrix_type(
					fcppt::math::matrix::row(
						0.,1.,2.
					),
					fcppt::math::matrix::row(
						1.,0.,3.
					),
					fcppt::math::matrix::row(
						4.,-3.,8.
					)
				)
			),
			large_matrix_type(
				fcppt::math::matrix::row(
					-9./2.,7.,-3./2.
				),
				fcppt::math::matrix::row(
					-2.,4.,-1.
				),
				fcppt::math::matrix::row(
					3./2.,-2.,1./2.
				)
			),
			std::numeric_limits<
				double
			>::epsilon()
		)
	));
}


#include <iostream>
#include <vector>
#include <fstream>
#include <string>
#include <boost/numeric/odeint.hpp>
#include "helper_func.h"
#include "LongDynamics.h"
#include "EKF.h"

int main()
{
    // Define the dataset path
    const std::string dataset_path{"../data/aircraft_long_dynamics_dataset.txt"};

    // Load dataset
    const int dataset_size = 1001;
    Eigen::VectorXd elev_input(dataset_size);
    Eigen::VectorXd throttle_input(dataset_size);
    Eigen::VectorXd alpha_true(dataset_size);
    Eigen::VectorXd alpha_measured(dataset_size);
    Eigen::VectorXd pitch_rate_true(dataset_size);
    Eigen::VectorXd pitch_rate_measured(dataset_size);
    LoadDataSet(dataset_path, elev_input, throttle_input, alpha_true,
                alpha_measured, pitch_rate_true, pitch_rate_measured);

    // Create a csv file to save output data
    std::ofstream outputs_file;
    std::string filename = "outputs.csv";
    outputs_file.open(filename);
    outputs_file << "time" << "," << "true_alpha" << "," << "measured_alpha"
                << "," << "estimated_alpha" << "," << "true_q" << ","
                << "measured_q" << "," << "estimated_q" << std::endl;

    // Define dynamic system parameters
    int num_state = 4;
    int num_input = 2;
    int num_output = 2;
    const double sample_time = 0.01;
    Eigen::MatrixXd Q(num_state, num_state); // Process noise covariance matrix
    Q << 0.0001, 0.0   , 0.0     , 0.0,
         0.0   , 0.0001, 0.0     , 0.0,
         0.0   , 0.0   , 0.000001, 0.0,
         0.0   , 0.0   , 0.0     , 0.0;

    Eigen::MatrixXd R(num_output, num_output); // Measurment noise covariance matrix
    R << 0.00001, 0.0,
         0.0    , 0.00001;

    Eigen::MatrixXd P0(num_state, num_state); // Initial estimate error covariance matrix
    P0 << 0.0001, 0.0   , 0.0     , 0.0,
         0.0   , 0.0001, 0.0     , 0.0,
         0.0   , 0.0   , 0.000001, 0.0,
         0.0   , 0.0   , 0.0     , 0.0;

    // Construct EKF object
    EKF ekf(Q, R, P0, sample_time);

    // Define integration class and a stepper integrator
    state_type x(num_state);
    LongDynamics long_dyn_object;
    boost::numeric::odeint::runge_kutta4<state_type> rk4_stepper;

    // Define vectors to hold the estimated outputs and the integrator predicted state
    Eigen::VectorXd alpha_estimated(dataset_size);
    Eigen::VectorXd pitch_rate_estimated(dataset_size);
    Eigen::VectorXd predicted_state(num_state);

    // Initialize the state
    predicted_state << 74.9167 + 0.03,
                       3.5330 + 0.03,
                       0.0 +0.001,
                       2.7 * PI / 180;

    // Run the filter on the dataset of I/O
    Eigen::VectorXd y(num_output);
    Eigen::VectorXd u(num_input);
    double t = 0.0;
    std::cout << "Processing dataset..." << std::endl;
    for (int i = 0; i < dataset_size; i++)
    {
        // Run EKF for one step
        y << alpha_measured(i),
             pitch_rate_measured(i);
        u << elev_input(i),
             throttle_input(i);
        ekf.Update(y, u, predicted_state);
        alpha_estimated(i) = ekf.GetEstimatedOutput()(0);
        pitch_rate_estimated(i) = ekf.GetEstimatedOutput()(1);

        // Save outputs to the csv file
        outputs_file << sample_time * i << ","
                     << alpha_true(i) * 180.0 / PI << ","
                     << alpha_measured(i) * 180.0 / PI << ","
                     << alpha_estimated(i) * 180.0 / PI << ","
                     << pitch_rate_true(i) * 180.0 / PI << ","
                     << pitch_rate_measured(i) * 180.0 / PI << ","
                     << pitch_rate_estimated(i) * 180.0 / PI << std::endl;

        // Pass the updated state back to the integrator
        x[0] = ekf.GetState()(0);
        x[1] = ekf.GetState()(1);
        x[2] = ekf.GetState()(2);
        x[3] = ekf.GetState()(3);

        // Run the integrator for one step and get the new state
        t = sample_time * i;
        rk4_stepper.do_step(long_dyn_object, x, t, sample_time);
        predicted_state(0) = x[0];
        predicted_state(1) = x[1];
        predicted_state(2) = x[2];
        predicted_state(3) = x[3];
    }
    std::cout << "Finished processing dataset" << std::endl;

    // Close the output csv file
    outputs_file.close();

    // Compute and print root mean square error for the estimated outputs (angle of attack and pitch rate)
    std::cout << "RMSE for the estimated AoA is: " << ComputeRMSE(alpha_true, alpha_estimated) << std::endl;
    std::cout << "RMSE for the estimated pitch rate is: " << ComputeRMSE(pitch_rate_true, pitch_rate_estimated) << std::endl;
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

#include <iostream>
#include <Eigen/Eigen>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXf A(MatrixXf::Random(5,3)), thinQ(MatrixXf::Identity(5,3)), Q;
A.setRandom();
HouseholderQR<MatrixXf> qr(A);
Q = qr.householderQ();
thinQ = qr.householderQ() * thinQ;
std::cout << "The complete unitary matrix Q is:\n" << Q << "\n\n";
std::cout << "The thin matrix Q is:\n" << thinQ << "\n\n";

  return 0;
}


// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>
#include <OpenTissue/core/spline/spline.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>

// Boost Test declaration and Checking macros
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>
#include <boost/test/floating_point_comparison.hpp>

typedef OpenTissue::spline::MathTypes<double, size_t>    math_types;
typedef math_types::vector_type                                vector_type;
typedef std::vector<double>                                    knot_container;
typedef std::vector<vector_type>                               point_container;

typedef OpenTissue::spline::NUBSpline<knot_container, point_container> spline_type;

BOOST_AUTO_TEST_SUITE(opentissue_spline_insert_knot);

BOOST_AUTO_TEST_CASE(test_insert_knot)
{
  knot_container U;

  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(0.0);  //k = 4
  U.push_back(1.0);
  U.push_back(2.0);
  U.push_back(3.0);  // n = 6  => |P| = 7
  U.push_back(4.0);
  U.push_back(4.0);
  U.push_back(4.0);
  U.push_back(4.0);  // m = 10  => |U| = 11

  point_container P;
  vector_type p0(2);  p0(0) = 0.0; p0(1) = 0.0;
  vector_type p1(2);  p1(0) = 1.0; p1(1) = 0.0;
  vector_type p2(2);  p2(0) = 2.0; p2(1) = 0.0;
  vector_type p3(2);  p3(0) = 3.0; p3(1) = 0.0;
  vector_type p4(2);  p4(0) = 4.0; p4(1) = 0.0;
  vector_type p5(2);  p5(0) = 5.0; p5(1) = 0.0;
  vector_type p6(2);  p6(0) = 6.0; p6(1) = 0.0;

  P.push_back(p0);
  P.push_back(p1);
  P.push_back(p2);
  P.push_back(p3);
  P.push_back(p4);
  P.push_back(p5);
  P.push_back(p6);

  spline_type spline(4,U,P);

  double const tolerance = 0.00001;
  double const u = 2.5;

  knot_container const & UU = spline.get_knot_container();
  point_container const & PP = spline.get_control_container();

  //--- Before insertion
  BOOST_CHECK_CLOSE(UU[0], 0.0,  tolerance);
  BOOST_CHECK_CLOSE(UU[1], 0.0,  tolerance);
  BOOST_CHECK_CLOSE(UU[2], 0.0,  tolerance);
  BOOST_CHECK_CLOSE(UU[3], 0.0,  tolerance);
  BOOST_CHECK_CLOSE(UU[4], 1.0,  tolerance);
  BOOST_CHECK_CLOSE(UU[5], 2.0,  tolerance);
  BOOST_CHECK_CLOSE(UU[6], 3.0,  tolerance);
  BOOST_CHECK_CLOSE(UU[7], 4.0,  tolerance);
  BOOST_CHECK_CLOSE(UU[8], 4.0,  tolerance);
  BOOST_CHECK_CLOSE(UU[9], 4.0,  tolerance);
  BOOST_CHECK_CLOSE(UU[10], 4.0,  tolerance);
  BOOST_CHECK( UU.size() == 11 );
  BOOST_CHECK( PP.size() == 7 );

  //--- After insertion
  OpenTissue::spline::insert_knot(u,spline);

  knot_container const & UUU = spline.get_knot_container();
  point_container const & PPP = spline.get_control_container();

  BOOST_CHECK_CLOSE(UUU[0], 0.0,  tolerance);
  BOOST_CHECK_CLOSE(UUU[1], 0.0,  tolerance);
  BOOST_CHECK_CLOSE(UUU[2], 0.0,  tolerance);
  BOOST_CHECK_CLOSE(UUU[3], 0.0,  tolerance);
  BOOST_CHECK_CLOSE(UUU[4], 1.0,  tolerance);
  BOOST_CHECK_CLOSE(UUU[5], 2.0,  tolerance);
  BOOST_CHECK_CLOSE(UUU[6], 2.5,  tolerance);
  BOOST_CHECK_CLOSE(UUU[7], 3.0,  tolerance);
  BOOST_CHECK_CLOSE(UUU[8], 4.0,  tolerance);
  BOOST_CHECK_CLOSE(UUU[9], 4.0,  tolerance);
  BOOST_CHECK_CLOSE(UUU[10], 4.0,  tolerance);
  BOOST_CHECK_CLOSE(UUU[11], 4.0,  tolerance);

  for(int i=0;i<8;++i)
    BOOST_CHECK_CLOSE(PPP[i](1), 0.0,  tolerance);

  BOOST_CHECK_CLOSE(PPP[0](0), 0.0,  tolerance);
  BOOST_CHECK_CLOSE(PPP[1](0), 1.0,  tolerance);
  BOOST_CHECK_CLOSE(PPP[2](0), 2.0,  tolerance);
  BOOST_CHECK_CLOSE(PPP[3](0), 2.833333333333333,  tolerance);
  BOOST_CHECK_CLOSE(PPP[4](0), 3.5,  tolerance);
  BOOST_CHECK_CLOSE(PPP[5](0), 4.25,  tolerance);
  BOOST_CHECK_CLOSE(PPP[6](0), 5.0,  tolerance);
  BOOST_CHECK_CLOSE(PPP[7](0), 6.0,  tolerance);

  BOOST_CHECK( UUU.size() == 12 );
  BOOST_CHECK( PPP.size() == 8 );
}

BOOST_AUTO_TEST_CASE(test_bezier_curve_case)
{
  knot_container U;

  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(0.0);
  U.push_back(1.0);
  U.push_back(1.0);
  U.push_back(1.0);
  U.push_back(1.0);

  // k = 4
  // n = 3  => |P| = 4
  // m = 7  => |U| = 8

  point_container P;
  vector_type p0(2);  p0(0) = 0.0; p0(1) = 0.0;
  vector_type p1(2);  p1(0) = 1.0; p1(1) = 1.0;
  vector_type p2(2);  p2(0) = 2.0; p2(1) = 1.0;
  vector_type p3(2);  p3(0) = 3.0; p3(1) = 0.0;

  P.push_back(p0);
  P.push_back(p1);
  P.push_back(p2);
  P.push_back(p3);

  spline_type bezier(4,U,P);

  double const tolerance = 10e-15;

  // Now we have constructed the equivalent of a bezier curve
  //
  //   B(t) =     a t^3 +   b t^2 + c t + d
  //   B'(t) =  3 a t^2 + 2 b t   + c
  //
  //   B(0) = p0  = d
  //   B(1) = p3  = a + b + c + d
  //   B'(0) = 3*(p1-p0) = c
  //   B'(1) = 3*(p1-p0) = 3 a + 2 b + c
  //
  //  working out the equations one would get
  vector_type a = -1*p0 + 3*p1 - 3*p2 + 1*p3;
  vector_type b =  3*p0 - 6*p1 + 3*p2;
  vector_type c = -3*p0 + 3*p1;
  vector_type d =  1*p0;

  double u = 0.5;

  OpenTissue::spline::insert_knot(u,bezier);
  OpenTissue::spline::insert_knot(u,bezier);
  OpenTissue::spline::insert_knot(u,bezier);
  OpenTissue::spline::insert_knot(u,bezier);

  vector_type m0 = 0.5*(p1+p0);
  vector_type m1 = 0.5*(p2+p1);
  vector_type m2 = 0.5*(p3+p2);
  vector_type n0 = 0.5*(m1+m0);
  vector_type n1 = 0.5*(m2+m1);
  vector_type q0 = 0.5*(n0+n1);

  knot_container  const & newU = bezier.get_knot_container();
  point_container const & newP = bezier.get_control_container();

  BOOST_CHECK_CLOSE(newU[0], 0.0,  tolerance);
  BOOST_CHECK_CLOSE(newU[1], 0.0,  tolerance);
  BOOST_CHECK_CLOSE(newU[2], 0.0,  tolerance);
  BOOST_CHECK_CLOSE(newU[3], 0.0,  tolerance);
  BOOST_CHECK_CLOSE(newU[4], 0.5,  tolerance);
  BOOST_CHECK_CLOSE(newU[5], 0.5,  tolerance);
  BOOST_CHECK_CLOSE(newU[6], 0.5,  tolerance);
  BOOST_CHECK_CLOSE(newU[7], 0.5,  tolerance);
  BOOST_CHECK_CLOSE(newU[8], 1.0,  tolerance);
  BOOST_CHECK_CLOSE(newU[9], 1.0,  tolerance);
  BOOST_CHECK_CLOSE(newU[10], 1.0,  tolerance);
  BOOST_CHECK_CLOSE(newU[11], 1.0,  tolerance);
  BOOST_CHECK_CLOSE(newP[0](0), p0(0),  tolerance);
  BOOST_CHECK_CLOSE(newP[0](1), p0(1),  tolerance);
  BOOST_CHECK_CLOSE(newP[1](0), m0(0),  tolerance);
  BOOST_CHECK_CLOSE(newP[1](1), m0(1),  tolerance);
  BOOST_CHECK_CLOSE(newP[2](0), n0(0),  tolerance);
  BOOST_CHECK_CLOSE(newP[2](1), n0(1),  tolerance);
  BOOST_CHECK_CLOSE(newP[3](0), q0(0),  tolerance);
  BOOST_CHECK_CLOSE(newP[3](1), q0(1),  tolerance);
  BOOST_CHECK_CLOSE(newP[4](0), q0(0),  tolerance);
  BOOST_CHECK_CLOSE(newP[4](1), q0(1),  tolerance);
  BOOST_CHECK_CLOSE(newP[5](0), n1(0),  tolerance);
  BOOST_CHECK_CLOSE(newP[5](1), n1(1),  tolerance);
  BOOST_CHECK_CLOSE(newP[6](0), m2(0),  tolerance);
  BOOST_CHECK_CLOSE(newP[6](1), m2(1),  tolerance);
  BOOST_CHECK_CLOSE(newP[7](0), p3(0),  tolerance);
  BOOST_CHECK_CLOSE(newP[7](1), p3(1),  tolerance);
}

BOOST_AUTO_TEST_SUITE_END();


#ifndef ITERATOR_HPP
#define ITERATOR_HPP

#include <string>
#include <Eigen/Dense>

template<typename T, int D>
class iterator {
public:

    iterator()  {
    }

    uint dim() const {return D;}
    uint num() const {return dataPtr.size();}

    // insert a consecutive batch of vectors
    void insertBatch(T *addr, uint n) {
        for (uint i = 0; i < n; ++i) {
            insert(addr + i * D, i);
        }
    }
    // insert a single vector
    void insert(T *addr, uint id) {
        dataPtr.push_back(addr);
        dataIdx.push_back(id);
    }

    T* addr(uint pos) const {
        return dataPtr[pos];
    }

    const Eigen::Ref< const Eigen::Matrix< T, D, 1>> operator[](const uint i)  {
        return get(i);
    }
    const Eigen::Ref< const Eigen::Matrix< T, D, 1>> get(const uint i)  {
        return Eigen::Map<Eigen::Matrix<T, D, 1>>(dataPtr[i]);
    }

    const Eigen::Matrix< T, D, 1>  center() {
        T *c = new T[D];
        Eigen::Map<Eigen::Matrix<T, D, 1>> avgVector(c);
        avgVector = Eigen::Matrix<T, D, 1>::Zero();

        for (uint n = 0; n < num(); ++n) {
            avgVector += get(n);
        }

        avgVector /= static_cast<T>(num());
        return avgVector;

    }

    std::vector<T*> dataPtr;
    std::vector<uint> dataIdx;

};

#endif


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* 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 ROKKO_UTILITY_FRANK_MATRIX_HPP
#define ROKKO_UTILITY_FRANK_MATRIX_HPP

#include <cmath>
#include <stdexcept>
#include <boost/throw_exception.hpp>
#include <rokko/config.h>
#include <rokko/localized_matrix.hpp>
#if defined(ROKKO_HAVE_PARALLEL_DENSE_SOLVER)
# include <rokko/distributed_matrix.hpp>
#endif

namespace rokko {

class frank_matrix {
public:
  template<typename T, typename MATRIX_MAJOR>
  static void generate(rokko::localized_matrix<T, MATRIX_MAJOR>& mat) {
    if (mat.rows() != mat.cols())
      BOOST_THROW_EXCEPTION(std::invalid_argument("frank_matrix::generate() : non-square matrix"));
    int n = mat.rows();
    for(int i = 0; i < n; ++i) {
      for(int j = 0; j < n; ++j) {
        mat(i,j) = n - std::max(i, j);
      }
    }
  }

#if defined(ROKKO_HAVE_PARALLEL_DENSE_SOLVER)
  template<typename T, typename MATRIX_MAJOR>
  static void generate(rokko::distributed_matrix<T, MATRIX_MAJOR>& mat) {
    if (mat.get_m_global() != mat.get_n_global())
      BOOST_THROW_EXCEPTION(std::invalid_argument("frank_matrix::generate() : non-square matrix"));
    for(int local_i = 0; local_i < mat.get_m_local(); ++local_i) {
      for(int local_j = 0; local_j < mat.get_n_local(); ++local_j) {
        int global_i = mat.translate_l2g_row(local_i);
        int global_j = mat.translate_l2g_col(local_j);
        mat.set_local(local_i, local_j, mat.get_m_global() - std::max(global_i, global_j));
      }
    }
  }

  // another (slower) implementation using set_global function
  template<typename T, typename MATRIX_MAJOR>
  static void generate_global(rokko::distributed_matrix<T, MATRIX_MAJOR>& mat) {
    if (mat.m_global != mat.n_global)
      BOOST_THROW_EXCEPTION(std::invalid_argument("frank_matrix::generate() : non-square matrix"));
    for(int global_i=0; global_i<mat.m_global; ++global_i) {
      for(int global_j=0; global_j<mat.n_global; ++global_j) {
        mat.set_global(global_i, global_j, mat.m_global - std::max(global_i, global_j) );
      }
    }
  }
#endif

  // calculate k-th smallest eigenvalue of dim-dimensional Frank matrix (k=0...dim-1)
  static double eigenvalue(int dim, int k) {
    return 1 / (2 * (1 - std::cos(M_PI * (2 * k + 1) / (2 * dim + 1))));
  }
};

} // namespace rokko

#endif // ROKKO_UTILITY_FRANK_MATRIX_HPP


////////////////////////////////////////////////////////////////////////////////
// EigSensitivity.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  First and second derivatives of symmetric 2x2 matrix eigenvalues.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Created:  04/05/2019 10:32:17
////////////////////////////////////////////////////////////////////////////////
#ifndef EIGSENSITIVITY_HH
#define EIGSENSITIVITY_HH

#include <Eigen/Dense>
#include <array>
#include <MeshFEM/EnergyDensities/Tensor.hh>

// 2x2 case only for now, also can be sped up.
template<typename Real>
struct EigSensitivity {
    using V2d  = Eigen::Matrix<Real, 2, 1>;
    using M2d  = Eigen::Matrix<Real, 2, 2>;

    EigSensitivity() { }

    template<typename Derived>
    EigSensitivity(const Eigen::MatrixBase<Derived> &A) { setMatrix(A); }

    template<typename Derived>
    void setMatrix(const Eigen::MatrixBase<Derived> &A) {
        static_assert((Derived::RowsAtCompileTime == 2) && (Derived::ColsAtCompileTime == 2), "Only 2x2 supported for now");

        const Real a_minus_c = A(0, 0) - A(1, 1);
        const Real b = A(0, 1);
        if (std::abs(b - A(1, 0)) > 1e-15) throw std::runtime_error("Only symmetric matrices are supported");
        // d := descriminant of characteristic quadratic
        const Real sqrt_d = std::sqrt(a_minus_c * a_minus_c + 4 * b * b);
        m_Lambda << 0.5 * (A.trace() + sqrt_d),
                    0.5 * (A.trace() - sqrt_d);

        V2d q0(a_minus_c + sqrt_d, 2 * b);
        q0.normalize();
        m_Q.col(0) = q0;
        m_Q.col(1) << -q0[1], q0[0];

        if (sqrt_d < 1e-14) m_degenerate = true;
        else                m_degenerate = false;
    }

    auto      q(size_t i) const { return m_Q.col(i); }
    Real lambda(size_t i) const { return m_Lambda[i]; }

    // Access Eigendecomposition
    const M2d &     Q() const { return m_Q; }
    const V2d &Lambda() const { return m_Lambda; }

    V2d dLambda(const M2d &dA) const { return (m_Q.transpose() * dA * m_Q).diagonal(); }
    M2d dLambda(size_t i) const { return q(i) * q(i).transpose(); }
    Real dLambda(size_t i, const M2d &dA) const { return q(i).dot(dA * q(i)); }

    // Only correct for symmetric dA_b!
    V2d d2Lambda(const M2d &dA_a, const M2d &dA_b) const {
        if (m_degenerate) return V2d(0.0, 0.0);
        Real d2lambda_0 = (2.0 / (m_Lambda[0] - m_Lambda[1])) * (q(0).dot(dA_a * q(1))) * (q(0).dot(dA_b * q(1)));
        return V2d(d2lambda_0, -d2lambda_0);
    }

    // Only correct for symmetric dA!
    M2d delta_dLambda(size_t i, const M2d &dA) const {
        if (m_degenerate) return M2d::Zero();
        double sign = (i == 0) ? 1.0 : -1.0;
        M2d result = ((sign * (2.0 / (m_Lambda[0] - m_Lambda[1])) * (q(0).dot(dA * q(1)))) * q(0)) * q(1).transpose();
        symmetrize(result);
        return result;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    M2d m_Q;
    V2d m_Lambda;

    bool m_degenerate;
};

#endif /* end of include guard: EIGSENSITIVITY_HH */


#ifndef SPLX_INTERNAL_BEZIER_H
#define SPLX_INTERNAL_BEZIER_H
#include <Eigen/Dense>
#include <splx/types.hpp>
#include <splx/internal/combinatorics.hpp>

namespace splx {
namespace internal {
namespace bezier {

/*
* given a bezier curve degree and max parameter, return row r so that
* multiplying the row with control points gives the kth derivative
* of the curve at u
*
* f^k(u) = \sum_{i=0}^degree r(i) * p(i)
* return r
*/
template<typename T>
Row<T> getBasisRow(unsigned int degree, T maxParameter, T u, unsigned int k) {
    if(u < 0 || u > maxParameter) {
    throw std::domain_error(
        std::string("u is outside of the range [0, ")
        + std::to_string(maxParameter)
        + std::string("]")
    );
    }

    if(maxParameter == 0) {
        Row<T> result(degree+1);
        result.setZero();
        if(k == 0 && degree >= 0) {
            result(0) = 1.0;
        }
        return result;
    }

    Row<T> result(degree + 1);

    T oneOverA = 1/maxParameter;
    for(unsigned int i = 0; i <= degree; i++) {
    T base = 0.0;
    T mult = 1.0;
    for(unsigned int j = 0; j+k <= degree; j++, mult *= u) {
        if(j+k >= i) {
        // base += pow(oneOverA, i) * this->comb(degree-i, j+k-i)
        //       * pow(-oneOverA, j+k-i) * this->perm(j+k, k)
        //       * mult;

        base += splx::internal::comb(degree-i, j+k-i)
            * splx::internal::pow(oneOverA, j+k) * splx::internal::perm(j+k, k)
            * mult * ((j+k-i)%2 == 0 ? 1 : -1);
        }
    }
    base *= splx::internal::comb(degree, i);
    result(i) = base;
    }
    return result;
}

/*
    Coefficient matrix for the k^th derivative bernstein base functions where each row r
    contains coefficients where k^th derivative of i^th bernstein polynomial of degree d
    can be expressed as r(0) + r(1)u + r(2)u^2 + ... + r(d)u^d
*/
template<typename T>
Matrix<T> bernsteinCoefficientMatrix(unsigned int degree, T maxParameter, unsigned int k) {
    Matrix<T> bernsteinMtr(degree+1, degree+1);
    bernsteinMtr.setZero();

    if(maxParameter == 0) {
        if(k == 0 && degree >= 0) {
            bernsteinMtr(0, 0) = 1.0;
        }
        return bernsteinMtr;
    }

    unsigned int dcombi = 1;
    for(Index i = 0;

        i < degree+1;

        dcombi *= (degree-i),
        dcombi /= (i+1),
        i++) {

        unsigned int dminicombjmini = 1;
        T min1 = 1;
        T oneOverAPowj = splx::internal::pow(1/maxParameter, i);

        for(Index j = i;

            j < degree+1;

            dminicombjmini *= (degree - j),
            dminicombjmini/= (j+1-i),
            j++,
            min1 *= -1,
            oneOverAPowj *= (1/maxParameter)) {

            bernsteinMtr(i, j) = dcombi * dminicombjmini * min1 * oneOverAPowj;

        }
    }

    Matrix<T> derivative(degree+1, degree+1);
    derivative.setZero();

    unsigned int jpermk = splx::internal::fac(k);
    for(unsigned int j = k;

        j < degree+1;

        jpermk *= (j+1),
        jpermk /= (j+1-k),
        j++) {
        derivative(j, j-k) = jpermk;
    }

    return bernsteinMtr * derivative;
}

} // namespace bezier
} // namespace internal
} // namespace splx

#endif


//test_ptf_mc_var.cpp

//test portfolio VaR through MC simulation

#include <iostream>
#include <string>
#include <fstream>
#include <sstream>
#include <vector>
#include<memory>

#include <Eigen/Dense>

#include "compute_returns_eigen.h"
#include "compute_var.h"
#include "path.h"
#include "mc_engine.h"
#include "rng.h"
#include "pca.h"
#include "portfolio.h"
#include "ptf_var.h"

#include <boost/math/special_functions/erf.hpp>
using boost::math::erfc_inv;

using namespace Eigen;
using namespace std;

void readCSV(std::istream &input, std::vector< std::vector<std::string> > &output)
//https://www.gamedev.net/topic/444193-c-how-to-load-in-a-csv-file/
{
	std::string csvLine;
	// read every line from the stream
	while( std::getline(input, csvLine) )
	{
		std::istringstream csvStream(csvLine);
		std::vector<std::string> csvColumn;
		std::string csvElement;
		// read every element from the line that is seperated by commas
		// and put it into the vector or strings
		while( std::getline(csvStream, csvElement, ',') )
		{
			csvColumn.push_back(csvElement);
		}
		output.push_back(csvColumn);
	}
}

int main()
{

    try{

    // Read daily price history for crypto assets, in USD
    //Date,EOS,DAPP
    //2018-11-19,3.5,0.5
    //2018-11-20,3.6,0.6

	std::fstream file("/home/gg/VaR/test/data.csv", ios::in);
	if(!file.is_open())
	{
		std::cout << "File not found!\n";
		return 1;
	}
	// typedef to save typing for the following object
	typedef std::vector< std::vector<std::string> > csvVector;
	csvVector csvData;

	readCSV(file, csvData);

    //test
    for(size_t i = 0;i < 5; ++i){
        for(size_t j = 0;j < csvData[i].size();++j){
            cout << csvData[i][j] << '\t';

        }

        cout << endl;
    }
    cout << endl;

    // Remove lines with missing values

    size_t n(csvData.size() - 1);
    size_t m(csvData[0].size() - 1);

    Mat _prices;
    _prices.resize(m,Vec(n));

    for(size_t i = 1;i < n+1;++i){
        for(size_t j = 1;j < csvData[i].size();++j){
            std::string tmp = csvData[i][j];
            if(tmp.empty()){
                _prices[j-1][i-1] = 99999.;
            }
            else{
                _prices[j-1][i-1] = std::stod(tmp);
            }
        }
    }

    std::vector<std::string> indexNames(csvData[0].size() - 1);

    for(size_t i = 1;i < csvData[0].size();++i){
        indexNames[i-1] = csvData[0][i];
    }

	//Remove missing values to compute trailling returns
    //Asynchornous time series. Shift to the next value
    Mat prices;
    prices.resize(m,Vec(0));

    bool goodrow = true;
    for(size_t j = 0;j < _prices[0].size();++j){
        for(size_t i = 0;i < _prices.size();++i){
            if((_prices[i][j] == 99999) || (_prices[i][j] == 0))
                goodrow = false;
        }
        if (goodrow)
            for(size_t i = 0;i < _prices.size();++i){
                prices[i].push_back(_prices[i][j]);
            }
            goodrow = true;
        }
    unsigned int windowsize = prices[0].size()-2;
    std::shared_ptr<ComputeReturn> cr(new ComputeReturn(prices,1,windowsize,true));

    // create portfolio
	double a = double(1./m);
	std::vector<double> weights{a,a}; //initialization. Equi-weighted asset for mere convenience
	Ptf _ptf;
	for(size_t i = 0;i < m;++i){
        shared_ptr<Instrument> instrument(new DeltaOne());
        auto p = std::make_pair(i,instrument);
		_ptf.push_back(p);
	}
	shared_ptr<Portfolio> ptf(new Portfolio(_ptf, weights, cr, false, 1.e+07));

    HistoricalVaR model;

    double alphatest = .95; // set alpha level, example 0.9 for 95% CVaR

    // CVaRVarianceCovariance
    // as currently used in VIGOR
    double CVaRVarianceCovariance = -100.0 * std::log(1.0 + (((std::exp(-1.0*(std::pow(-1.0*std::sqrt(2.0)*erfc_inv(2.0*alphatest),2.0))/2.0)/(std::sqrt(2.0*M_PI)))/(1.0-alphatest)) * (ptf->getPtfSdev()/100)));
    cout << "CVaRVarianceCovariance: " << CVaRVarianceCovariance << endl;

    return 0;

    } catch (const std::exception& e) { // caught by reference to base
        std::cout << " a standard exception was caught, with message '"
                  << e.what() << "'\n";
    }

}