// File       : gemm.cpp
// Created    : Tue Oct 16 2018 10:45:45 AM (+0200)
// Description: General Matrix-Matrix multiplication with ISPC
// Copyright 2018 ETH Zurich. All Rights Reserved.
#include <cassert>
#include <iostream>
#include <chrono>
#include <string>
#include <random>
#include <cassert>
#include <cstring>
#include <cmath>
using namespace std;

#include "common.h"
#ifdef _USE_ISPC_
#include "gemm_sse2.h" // kernel signature for the ISPC SSE2 code
#include "gemm_avx2.h" // kernel signature for the ISPC AVX2 code
using namespace ispc;
#endif /* _USE_ISPC_ */

#ifdef _WITH_EIGEN_
#include <Eigen/Core>

template <typename T>
using EMat = Eigen::Map< Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor> >;

// compiled in separate compilation unit with optimization flags tuned for
// Haswell architecture, see the gemm_eigen.o target in the Makefile
void gemm_eigen(const EMat<Real>&, const EMat<Real>&, EMat<Real>&);
#endif /* _WITH_EIGEN_ */

/**
 * @brief General matrix-matrix multiplication kernel (GEMM). Computes C = AB.
 * Naive implementation.
 *
 * @tparam T Real type parameter
 * @param A Matrix dimension p x r
 * @param B Matrix dimension r x q
 * @param C Matrix dimension p x q
 * @param p Dimensional parameter
 * @param r Dimensional parameter
 * @param q Dimensional parameter
 */
template <typename T>
static void gemm_serial(const T* const A, const T* const B, T* const C,
        const int p, const int r, const int q)
{
    for (int i = 0; i < p; ++i)
        for (int j = 0; j < q; ++j)
        {
            T sum = 0.0;
            for (int k = 0; k < r; ++k)
                sum += A[i*r + k] * B[k*q + j]; // Note: The access pattern
                                                // into matrix B is bad!
            C[i*q + j] = sum;
        }
}

/**
 * @brief General matrix-matrix multiplication kernel (GEMM). Computes C = AB.
 * Implementation with optimized memory access.
 *
 * @tparam T Real type parameter
 * @param A Matrix dimension p x r
 * @param B Matrix dimension r x q
 * @param C Matrix dimension p x q
 * @param p Dimensional parameter
 * @param r Dimensional parameter
 * @param q Dimensional parameter
 */
template <typename T>
static void gemm_serial_tile(const T* const A, const T* const B, T* const C,
        const int p, const int r, const int q)
{
    assert(q%_HTILE_ == 0);
    assert(p%_VTILE_ == 0);
    T tile[_VTILE_][_HTILE_]; // tmp storage to exploit temporal locality.
                              // This must fit into L1 cache, otherwise
                              // performance will degrade

    // outer loops over dimension of matrix C.  Note the necessary stride on
    // the loop counters because of our cached data structure tile (above).
    for (int i = 0; i < p; i += _VTILE_)
        for (int j = 0; j < q; j += _HTILE_)
        {
            // initialize tile to zero
            for (int tv = 0; tv < _VTILE_; ++tv)
                for (int th = 0; th < _HTILE_; ++th)
                    tile[tv][th] = (T)0.0;

            // start of inner product
            for (int k = 0; k < r; ++k)
            {
                for (int tv = 0; tv < _VTILE_; ++tv)
                {
                    const T Aik = A[(i+tv)*r + k];
                    for (int th = 0; th < _HTILE_; ++th)
                        tile[tv][th] += Aik * B[k*q + j + th]; // Optimized memory access
                                                               // for matrix B!
                }
            }

            for (int tv = 0; tv < _VTILE_; ++tv)
                for (int th = 0; th < _HTILE_; ++th)
                    C[(i+tv)*q + j + th] = tile[tv][th]; // Optimized writes
                                                         // into C also
        }
}

/**
 * @brief Compute the Frobenius norm between the difference of two input
 * matrices
 *
 * @tparam T Real type parameter
 * @param M Test matrix dimension p x q
 * @param truth Reference matrix dimension p x q
 * @param p Dimensional parameter
 * @param q Dimensional parameter
 *
 * @return
 */
template <typename T>
static T validate(const T* const M, const T* const truth,
        const int p, const int q)
{
    T res = 0.0;
    for (int i = 0; i < p; ++i)
        for (int j = 0; j < q; ++j)
        {
            const T diff = M[i*q + j] - truth[i*q + j];
            res += diff * diff;
        }
    return std::sqrt(res/(p*q));
}

/**
 * @brief Initialize matrices A and B to uniform random values
 *
 * @tparam T Real type parameter
 * @param A Matrix dimension p x r
 * @param B Matrix dimension r x q
 * @param p Dimensional parameter
 * @param r Dimensional parameter
 * @param q Dimensional parameter
 * @param seed Used for random number generator
 */
template <typename T>
static void initialize(T* const A, T* const B,
        const int p, const int r, const int q, const int seed=101)
{
    default_random_engine gen(seed);
    uniform_real_distribution<T> dist(0.0, 1.0);
    for (int i = 0; i < p; ++i)
        for (int k = 0; k < r; ++k)
            A[i*r + k] = dist(gen);

    for (int k = 0; k < r; ++k)
        for (int j = 0; j < q; ++j)
            B[k*q + j] = dist(gen);
}

/**
 * @brief Benchmark a test kernel versus a baseline kernel
 *
 * @tparam T Real type parameter
 * @tparam MEMORY_OPTIMIZED Flag to switch between naive and memory optimized
 * base kernel
 * @tparam WITH_EIGEN Flag to enable Eigen kernel for benchmarking
 * @param p Dimensional parameter
 * @param r Dimensional parameter
 * @param q Dimensional parameter
 * @param func Function pointer to test kernel
 * @param test_name String to describe additional output
 */
template <typename T, bool MEMORY_OPTIMIZED=false, bool WITH_EIGEN=false>
void benchmark(const int p, const int r, const int q,
        void (*func)(const T* const, const T* const, T* const, const int, const int, const int),
        const string test_name)
{
    T *A, *B, *C, *truth;
    posix_memalign((void**)&A, 32, p*r*sizeof(T));
    posix_memalign((void**)&B, 32, r*q*sizeof(T));
    posix_memalign((void**)&C, 32, p*q*sizeof(T));
    posix_memalign((void**)&truth, 32, p*q*sizeof(T));
    typedef chrono::steady_clock Clock;

    // initialize random matrices
    initialize(A, B, p, r, q);
    memset(C, 0, p*q*sizeof(T));
    memset(truth, 0, p*q*sizeof(T));

    // compute truth
    auto t1 = Clock::now();
    if (WITH_EIGEN)
    {
        (*func)(A, B, truth, p, r, q);
    }
    else
    {
        if (MEMORY_OPTIMIZED)
            gemm_serial_tile(A, B, truth, p, r, q);
        else
            gemm_serial(A, B, truth, p, r, q);
    }
    auto t2 = Clock::now();
    const double t_gold = chrono::duration_cast<chrono::nanoseconds>(t2 - t1).count();
    const T norm_truth = validate(truth, C, p, q);

    // test kernel
#ifdef _WITH_EIGEN_
    // Matrix wrappers
    const EMat<T> Ap(A, p, r);
    const EMat<T> Bp(B, r, q);
    EMat<T> Cp(C, p, q);
#endif /* _WITH_EIGEN_ */

    auto tt1 = Clock::now();
#ifdef _WITH_EIGEN_
    if (WITH_EIGEN)
        gemm_eigen(Ap, Bp, Cp);
    else
#endif /* _WITH_EIGEN_ */
        (*func)(A, B, C, p, r, q);
    auto tt2 = Clock::now();
    const double t = chrono::duration_cast<chrono::nanoseconds>(tt2 - tt1).count();

    // validate
    const T diff = validate(C, truth, p, q);

    cout << test_name << ":" << endl;
    cout << "  Data type size:     " << sizeof(T) << " byte" << endl;
    cout << "  Number of elements: A=" << p*r << "; B=" << r*q << "; C=" << p*q << endl;
    cout << "  Norm of truth:      " << norm_truth << endl;
    cout << "  Error:              " << diff << endl;
    cout << "  Time reference:     " << t_gold*1.0e-6 << " millisec" << endl;
    cout << "  Time test kernel:   " << t*1.0e-6 << " millisec" << endl;
    cout << "  Speedup:            " << t_gold/t << endl;

    // clean up
    free(A);
    free(B);
    free(C);
    free(truth);
}

int main(void)
{
    // problem size:
    // Matrix $A \in\mathbb{R}^{ p \times r }$
    // Matrix $B \in\mathbb{R}^{ r \times q }$
    // Matrix $C \in\mathbb{R}^{ p \times q }$
    //
    // We solve C = A * B
    constexpr int p = 512;
    constexpr int r = 1024;
    constexpr int q = 1024;

    // benchmark serial case naive vs. memory optimized
    benchmark<Real>(p, r, q, gemm_serial_tile, "GEMM serial access optimized");

#ifdef _USE_ISPC_
    // to be fair, we must use our best serial implementation to benchmark
    // against the vectorized implementations --> use memory optimized version
    // of serial GEMM
    constexpr bool MEMORY_OPTIMIZED = true;
    benchmark<Real,MEMORY_OPTIMIZED>(p, r, q, gemm_sse2, "GEMM ISPC SSE2");
    benchmark<Real,MEMORY_OPTIMIZED>(p, r, q, gemm_avx2, "GEMM ISPC AVX2");
#endif /* _USE_ISPC_ */

#ifdef _WITH_EIGEN_
    // benchmark against the Eigen library
    constexpr bool USE_EIGEN = true;
    benchmark<Real,true,USE_EIGEN>(p, r, q, gemm_avx2, "GEMM Eigen vs. ISPC AVX2");
#endif /* _WITH_EIGEN_ */

    return 0;
}


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// 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 <iostream>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>

#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/make_connected.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_maximal_planar.hpp>

#include "graph.hpp"
#include "range.hpp"

//a class to hold the coordinates of the straight line embedding
struct coord_t {
  std::size_t x;
  std::size_t y;
};

template<class G>
std::vector<coord_t> straight_line_drawing(G const & gIn) {
  using namespace boost;

  typedef adjacency_list<vecS, vecS, undirectedS, property<vertex_index_t, int>,
      property<edge_index_t, int> > Graph;

  Graph g;
  copy_edges(gIn, g);

  //Define the storage type for the planar embedding
  typedef std::vector<std::vector<graph_traits<Graph>::edge_descriptor>> embedding_storage_t;
  typedef iterator_property_map<embedding_storage_t::iterator,
      property_map<Graph, vertex_index_t>::type> embedding_t;

  // Create the planar embedding
  embedding_storage_t embedding_storage(num_vertices(g));
  embedding_t embedding(embedding_storage.begin(), get(vertex_index, g));

  boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                               boyer_myrvold_params::embedding = embedding);

  make_connected(g);
  make_biconnected_planar(g, embedding);
  make_maximal_planar(g, embedding);

  // Find a canonical ordering
  std::vector<typename graph_traits<Graph>::vertex_descriptor> ordering;
  planar_canonical_ordering(g, embedding, std::back_inserter(ordering));

  //Set up a property map to hold the mapping from vertices to coord_t's
  typedef std::vector<coord_t> drawing_storage_t;
  typedef boost::iterator_property_map<drawing_storage_t::iterator,
      property_map<Graph, vertex_index_t>::type> drawing_t;

  drawing_storage_t drawing_storage(num_vertices(g));
  drawing_t drawing(drawing_storage.begin(), get(vertex_index, g));

  // Compute the straight line drawing
  chrobak_payne_straight_line_drawing(g, embedding, ordering.begin(),
                                      ordering.end(), drawing);

  return drawing_storage;
}

int main(int argc, char** argv) {
  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> graph;

  // you can use the functions make_connected, make_biconnected_planar,
  // and make_maximal planar in sequence to add a set of edges
  // to any undirected planar graph to make it maximal planar.

  graph g(7);
  add_edge(0, 1, g);
  add_edge(1, 2, g);
  add_edge(2, 3, g);
  add_edge(3, 0, g);
  //add_edge(3, 4, g);
  add_edge(4, 5, g);
  add_edge(5, 6, g);
  //add_edge(6, 3, g);
  add_edge(0, 4, g);
  add_edge(1, 3, g);
  add_edge(3, 5, g);
  add_edge(2, 6, g);
  add_edge(1, 4, g);
  add_edge(1, 5, g);
  add_edge(1, 6, g);

  auto coords = straight_line_drawing(g);

  std::cout << "The straight line drawing is: " << std::endl;
  for (auto v : ::range(vertices(g))) {
    coord_t coord(coords[v]);
    std::cout << v << " -> (" << coord.x << ", " << coord.y << ")" << std::endl;
  }

  return 0;
}


#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <math.h>

using namespace std;
using namespace Eigen;
ros::Publisher odom_pub;
MatrixXd Q = MatrixXd::Identity(12, 12);
MatrixXd Rt = MatrixXd::Identity(6,6);
//Define some para
int img_checktemp=0;
int temp_ff=0;
Eigen::Vector3d p,q,p_dot,b_g,b_a;
Eigen::Vector3d u_a,u_g;
Eigen::VectorXd ut(6);
Eigen::VectorXd x_dot_prev(15),x_prev(15);
Eigen::VectorXd x_dot(15)     ,x(15);
Eigen::VectorXd Mu(15) ,Mu_bar(15), Mu_prev(15);
Eigen::MatrixXd Sigma(15,15) ,Sigma_bar(15,15), Sigma_prev(15,15);
Eigen::VectorXd process_model(15);
Eigen::Matrix3d R_WorldinCAM;
Eigen::Vector3d T_WorldinCAM;
Eigen::VectorXd Q_WorldinCAM(4);
Eigen::Matrix3d R_IMUinWorld;
Eigen::Vector3d T_IMUinWorld;
Eigen::VectorXd Q_IMUinWorld(4);
double dt, time_old,grav;
//Define some Matrix
Eigen::MatrixXd At(15,15);
Eigen::MatrixXd Ut(15,12);
Eigen::MatrixXd Ft(15,15);
Eigen::MatrixXd Vt(15,12);
Eigen::MatrixXd Ct(6,15);
Eigen::MatrixXd Kt(15,6);
Eigen::MatrixXd H_CAMinIMU(4,4);
Eigen::MatrixXd H_WorldinCAM(4,4);
Eigen::MatrixXd H_IMUinWorld(4,4);
void imu_callback(const sensor_msgs::Imu::ConstPtr &msg)
{
    //your code for propagation
	//Get process model
/*Get Row Data as u*/
	u_a(0) = msg->linear_acceleration.x;
	u_a(1) = msg->linear_acceleration.y;
	u_a(2) = msg->linear_acceleration.z;
	//cout << "u_a(0):" << u_a(0) << "u_a(1):" << u_a(1) << "u_a(2):" << u_a(2) << endl;
	u_g(0) = msg->angular_velocity.x;//as roll
	u_g(1) = msg->angular_velocity.y;//as pitch
	u_g(2) = msg->angular_velocity.z;//as yaw
/*Get Row Data as u*/
}

//Rotation from the camera frame to the IMU frame
Eigen::Matrix3d Rcam;
void odom_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
    //your code for update
    //camera position in the IMU frame = (0, -0.04, -0.02)
    //camera orientaion in the IMU frame = Quaternion(0, 0, 1, 0); w x y z, respectively
    //					   RotationMatrix << -1, 0, 0,
    //							      0, 1, 0,
    //                                                        0, 0, -1;
/*Get the dt from stamp*/
{
	if (!img_checktemp)
    {
	time_old = msg->header.stamp.toSec()-0.03;
	Mu.setZero();
	Sigma.setIdentity();
	//img_checktemp = 1;
	//cout << "Mu_prev" << endl << Mu_prev << endl;
    }
    dt=msg->header.stamp.toSec() - time_old;
    time_old = msg->header.stamp.toSec();
    //cout << "dt" << endl << sin(1.5) << endl;
}
/*Get the dt from stamp*/
/*Prediction Step*/
{
	//move forward
	x_prev     = x;
	x_dot_prev = x_dot;
	Mu_prev    = Mu;
	Sigma_prev = Sigma;
	//get ut from IMU
	ut        << u_a , u_g;
	//cout << "u_a(0):" << u_a(0) << "u_a(1):" << u_a(1) << "u_a(2):" << u_a(2) << endl;
	//cout << "Rcam:"   << Rcam   << endl;
	{//three matrix
	//get matrix At
	At            <<0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,
					0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,
                    0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,
  0, 0, 0,0,Mu_prev(9)*sin(Mu_prev(4)) - Mu_prev(11)*cos(Mu_prev(4)) + ut(5)*cos(Mu_prev(4)) - ut(3)*sin(Mu_prev(4)),                                                                                0, 0, 0, 0,                                               -cos(Mu_prev(4)),  0,                                              -sin(Mu_prev(4)),                                                                                0,                                            0,                                                                                0,
  0, 0, 0,(Mu_prev(11)*cos(Mu_prev(4)) - Mu_prev(9)*sin(Mu_prev(4)) - ut(5)*cos(Mu_prev(4)) + ut(3)*sin(Mu_prev(4)))/pow(cos(Mu_prev(3)),2),                                                                                -(sin(Mu_prev(3))*(Mu_prev(9)*cos(Mu_prev(4)) + Mu_prev(11)*sin(Mu_prev(4)) - ut(3)*cos(Mu_prev(4)) - ut(5)*sin(Mu_prev(4))))/cos(Mu_prev(3)),                                                                                0, 0, 0, 0, -(sin(Mu_prev(3))*sin(Mu_prev(4)))/cos(Mu_prev(3)), -1, (cos(Mu_prev(4))*sin(Mu_prev(3)))/cos(Mu_prev(3)),                                                                                0,                                            0,                                                                                0,
  0, 0, 0,-(sin(Mu_prev(3))*(Mu_prev(11)*cos(Mu_prev(4)) - Mu_prev(9)*sin(Mu_prev(4)) - ut(5)*cos(Mu_prev(4)) + ut(3)*sin(Mu_prev(4))))/pow(cos(Mu_prev(3)),2),                                                                                (Mu_prev(9)*cos(Mu_prev(4)) + Mu_prev(11)*sin(Mu_prev(4)) - ut(3)*cos(Mu_prev(4)) - ut(5)*sin(Mu_prev(4)))/cos(Mu_prev(3)),                                                                                0, 0, 0, 0,                          sin(Mu_prev(4))/cos(Mu_prev(3)),  0,                        -cos(Mu_prev(4))/cos(Mu_prev(3)),                                                                                0,                                            0,                                                                                0,
  0, 0, 0, cos(Mu_prev(3))*sin(Mu_prev(4))*sin(Mu_prev(5))*(Mu_prev(12) - ut(0)) - cos(Mu_prev(3))*cos(Mu_prev(4))*sin(Mu_prev(5))*(Mu_prev(14) - ut(2)) - sin(Mu_prev(3))*sin(Mu_prev(5))*(Mu_prev(13) - ut(1)), (Mu_prev(12) - ut(0))*(cos(Mu_prev(5))*sin(Mu_prev(4)) + cos(Mu_prev(4))*sin(Mu_prev(3))*sin(Mu_prev(5))) - (Mu_prev(14) - ut(2))*(cos(Mu_prev(4))*cos(Mu_prev(5)) - sin(Mu_prev(3))*sin(Mu_prev(4))*sin(Mu_prev(5))), (Mu_prev(12) - ut(0))*(cos(Mu_prev(4))*sin(Mu_prev(5)) + cos(Mu_prev(5))*sin(Mu_prev(3))*sin(Mu_prev(4))) + (Mu_prev(14) - ut(2))*(sin(Mu_prev(4))*sin(Mu_prev(5)) - cos(Mu_prev(4))*cos(Mu_prev(5))*sin(Mu_prev(3))) + cos(Mu_prev(3))*cos(Mu_prev(5))*(Mu_prev(13) - ut(1)), 0, 0, 0,                                                                    0,  0,                                                                   0,   sin(Mu_prev(3))*sin(Mu_prev(4))*sin(Mu_prev(5)) - cos(Mu_prev(4))*cos(Mu_prev(5)),  cos(Mu_prev(3))*sin(Mu_prev(5)), - cos(Mu_prev(5))*sin(Mu_prev(4)) - cos(Mu_prev(4))*sin(Mu_prev(3))*sin(Mu_prev(5)),
  0, 0, 0, cos(Mu_prev(5))*sin(Mu_prev(3))*(Mu_prev(13) - ut(1)) - cos(Mu_prev(3))*cos(Mu_prev(5))*sin(Mu_prev(4))*(Mu_prev(12) - ut(0)) + cos(Mu_prev(3))*cos(Mu_prev(4))*cos(Mu_prev(5))*(Mu_prev(14) - ut(2)), (Mu_prev(12) - ut(0))*(sin(Mu_prev(4))*sin(Mu_prev(5)) - cos(Mu_prev(4))*cos(Mu_prev(5))*sin(Mu_prev(3))) - (Mu_prev(14) - ut(2))*(cos(Mu_prev(4))*sin(Mu_prev(5)) + cos(Mu_prev(5))*sin(Mu_prev(3))*sin(Mu_prev(4))), cos(Mu_prev(3))*sin(Mu_prev(5))*(Mu_prev(13) - ut(1)) - (Mu_prev(14) - ut(2))*(cos(Mu_prev(5))*sin(Mu_prev(4)) + cos(Mu_prev(4))*sin(Mu_prev(3))*sin(Mu_prev(5))) - (Mu_prev(12) - ut(0))*(cos(Mu_prev(4))*cos(Mu_prev(5)) - sin(Mu_prev(3))*sin(Mu_prev(4))*sin(Mu_prev(5))), 0, 0, 0,                                                                    0,  0,                                                                   0, - cos(Mu_prev(4))*sin(Mu_prev(5)) - cos(Mu_prev(5))*sin(Mu_prev(3))*sin(Mu_prev(4)), -cos(Mu_prev(3))*cos(Mu_prev(5)),   cos(Mu_prev(4))*cos(Mu_prev(5))*sin(Mu_prev(3)) - sin(Mu_prev(4))*sin(Mu_prev(5)),
  0, 0, 0,cos(Mu_prev(4))*sin(Mu_prev(3))*(Mu_prev(14) - ut(2)) - cos(Mu_prev(3))*(Mu_prev(13) - ut(1)) - sin(Mu_prev(3))*sin(Mu_prev(4))*(Mu_prev(12) - ut(0)),                                                                                cos(Mu_prev(3))*cos(Mu_prev(4))*(Mu_prev(12) - ut(0)) + cos(Mu_prev(3))*sin(Mu_prev(4))*(Mu_prev(14) - ut(2)),                                                                                0, 0, 0, 0,                                                                    0,  0,                                                                   0,                                                                       cos(Mu_prev(3))*sin(Mu_prev(4)),                       -sin(Mu_prev(3)),                                                                      -cos(Mu_prev(3))*cos(Mu_prev(4)),
                    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
                    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
                    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
                    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
					0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
					0,0,0,0,0,0,0,0,0,0,0,0,0,0,0;
	//get matrix Ut
	Ut            <<0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0,                                               -cos(Mu_prev(4)),  0,                                              -sin(Mu_prev(4)), 0, 0, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0, -(sin(Mu_prev(3))*sin(Mu_prev(4)))/cos(Mu_prev(3)), -1, (cos(Mu_prev(4))*sin(Mu_prev(3)))/cos(Mu_prev(3)), 0, 0, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0,                          sin(Mu_prev(4))/cos(Mu_prev(3)),  0,                        -cos(Mu_prev(4))/cos(Mu_prev(3)), 0, 0, 0, 0, 0, 0,
    sin(Mu_prev(3))*sin(Mu_prev(4))*sin(Mu_prev(5)) - cos(Mu_prev(4))*cos(Mu_prev(5)),  cos(Mu_prev(3))*sin(Mu_prev(5)), - cos(Mu_prev(5))*sin(Mu_prev(4)) - cos(Mu_prev(4))*sin(Mu_prev(3))*sin(Mu_prev(5)),                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 0, 0,
  - cos(Mu_prev(4))*sin(Mu_prev(5)) - cos(Mu_prev(5))*sin(Mu_prev(3))*sin(Mu_prev(4)), -cos(Mu_prev(3))*cos(Mu_prev(5)),   cos(Mu_prev(4))*cos(Mu_prev(5))*sin(Mu_prev(3)) - sin(Mu_prev(4))*sin(Mu_prev(5)),                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 0, 0,
                                                                        cos(Mu_prev(3))*sin(Mu_prev(4)),                       -sin(Mu_prev(3)),                                                                      -cos(Mu_prev(3))*cos(Mu_prev(4)),                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 0, 0, 1, 0, 0,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 1, 0,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 0, 0, 0, 0, 1,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 1, 0, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 1, 0, 0, 0, 0,
                                                                                0,                                            0,                                                                                0,                                                                    0,  0,                                                                   0, 0, 0, 1, 0, 0, 0;

	//get Mu_bar
	grav = 9.8;
	process_model << Mu_prev(6),
                     Mu_prev(7),
                     Mu_prev(8),
  ut(3)*cos(Mu_prev(4)) - Mu_prev(11)*sin(Mu_prev(4)) - Mu_prev(9)*cos(Mu_prev(4)) + ut(5)*sin(Mu_prev(4)),
-(Mu_prev(10)*cos(Mu_prev(3)) - ut(4)*cos(Mu_prev(3)) - Mu_prev(11)*cos(Mu_prev(4))*sin(Mu_prev(3)) + Mu_prev(9)*sin(Mu_prev(3))*sin(Mu_prev(4)) + ut(5)*cos(Mu_prev(4))*sin(Mu_prev(3)) - ut(3)*sin(Mu_prev(3))*sin(Mu_prev(4)))/cos(Mu_prev(3)),
-(Mu_prev(11)*cos(Mu_prev(4)) - Mu_prev(9)*sin(Mu_prev(4)) - ut(5)*cos(Mu_prev(4)) + ut(3)*sin(Mu_prev(4)))/cos(Mu_prev(3)),
  - (Mu_prev(12) - ut(0))*(cos(Mu_prev(4))*cos(Mu_prev(5)) - sin(Mu_prev(3))*sin(Mu_prev(4))*sin(Mu_prev(5))) - (Mu_prev(14) - ut(2))*(cos(Mu_prev(5))*sin(Mu_prev(4)) + cos(Mu_prev(4))*sin(Mu_prev(3))*sin(Mu_prev(5))) + cos(Mu_prev(3))*sin(Mu_prev(5))*(Mu_prev(13) - ut(1)),
  - (Mu_prev(12) - ut(0))*(cos(Mu_prev(4))*sin(Mu_prev(5)) + cos(Mu_prev(5))*sin(Mu_prev(3))*sin(Mu_prev(4))) - (Mu_prev(14) - ut(2))*(sin(Mu_prev(4))*sin(Mu_prev(5)) - cos(Mu_prev(4))*cos(Mu_prev(5))*sin(Mu_prev(3))) - cos(Mu_prev(3))*cos(Mu_prev(5))*(Mu_prev(13) - ut(1)),
grav - sin(Mu_prev(3))*(Mu_prev(13) - ut(1)) - cos(Mu_prev(3))*cos(Mu_prev(4))*(Mu_prev(14) - ut(2)) + cos(Mu_prev(3))*sin(Mu_prev(4))*(Mu_prev(12) - ut(0)),
                     0,
                     0,
                     0,
					 0,
					 0,
					 0;
	}
	Mu_bar     = Mu_prev + dt * process_model;
	//get Sigma_bar
	Ft         = MatrixXd::Identity(15, 15) + dt * At;
	Vt         = dt * Ut;
	Sigma_bar  = Ft * Sigma_prev * Ft.transpose() + Vt * Q * Vt.transpose();

	//cout << "process_model" << endl << process_model << endl;
}
/*Prediction Step*/
/*Updata Step*/
{
	//transform imu to world using camera
	/*H_CAMinIMU	<<
	-1,0,0 ,0    ,
	0 ,1,0 ,-0.04,
	0 ,0,-1,-0.02,
	0 ,0,0 ,1    ;*/
	H_CAMinIMU.col(0) << Rcam.col(0)  , 0;
	H_CAMinIMU.col(1) << Rcam.col(1)  , 0;
	H_CAMinIMU.col(2) << Rcam.col(2)  , 0;
	H_CAMinIMU.col(3) << 0,-0.04,-0.02, 1;
	T_WorldinCAM(0) = msg->pose.pose.position.x;
	T_WorldinCAM(1) = msg->pose.pose.position.y;
	T_WorldinCAM(2) = msg->pose.pose.position.z;
	Q_WorldinCAM(0) = msg->pose.pose.orientation.w;
	Q_WorldinCAM(1) = msg->pose.pose.orientation.x;
	Q_WorldinCAM(2) = msg->pose.pose.orientation.y;
	Q_WorldinCAM(3) = msg->pose.pose.orientation.z;
    R_WorldinCAM = Quaterniond(Q_WorldinCAM(0),Q_WorldinCAM(1),Q_WorldinCAM(2),Q_WorldinCAM(3)).toRotationMatrix();

	H_WorldinCAM.col(0) << R_WorldinCAM.col(0), 0;
	H_WorldinCAM.col(1) << R_WorldinCAM.col(1), 0;
	H_WorldinCAM.col(2) << R_WorldinCAM.col(2), 0;
	H_WorldinCAM.col(3) << T_WorldinCAM       , 1;

	H_IMUinWorld = H_WorldinCAM.inverse() * H_CAMinIMU.inverse()  ;//????????????????
	R_IMUinWorld = H_IMUinWorld.topLeftCorner(3, 3);
	T_IMUinWorld = H_IMUinWorld.topRightCorner(3, 1);
	//cout << "H_IMUinWorld" << endl << H_IMUinWorld << endl;
	//get matrix Ct
	Ct   << MatrixXd::Identity(6, 15);
	//get matrix Kt
	Kt   =  Sigma_bar * Ct.transpose() * ((Ct * Sigma_bar * Ct.transpose() + MatrixXd::Identity(6, 6) * Rt * MatrixXd::Identity(6, 6)).inverse());
	//get Mu & Sigma
	x.head(3) = T_IMUinWorld;
	x(3) = asin(R_IMUinWorld(2,1));
	//x(4) = atan(-R_IMUinWorld(2,0)/R_IMUinWorld(2,2));
	//x(5) = atan(-R_IMUinWorld(0,1)/R_IMUinWorld(1,1));
	//x(4) = acos(R_IMUinWorld(2,2)/cos(x(3)));
	x(4) = asin(-R_IMUinWorld(0,2)/cos(x(3)));
	if ((R_IMUinWorld(0,2)/cos(x(3)))>1)
	{
		x(4) = asin(1);
	}
	if ((R_IMUinWorld(0,2)/cos(x(3)))<-1)
	{
		x(4) = asin(-1);
	}
	x(5) = acos(R_IMUinWorld(1,1)/cos(x(3)));
	//x(5) = asin(-R_IMUinWorld(0,1)/cos(x(3)));
	//x.segment(3,3) = R_IMUinWorld.eulerAngles(1, 2, 0);

	Mu   =  Mu_bar + Kt * (Ct*x - Ct*Mu_bar);
	Sigma=  Sigma_bar - Kt*Ct*Sigma_bar;

	if(std::isnan(Mu(0))&&temp_ff==0)
	{
		cout << "asin(-R_IMUinWorld(0,2)/cos(x(3)))" << endl << asin(-R_IMUinWorld(0,2)/cos(x(3))) << endl;
		cout << "R_IMUinWorld(0,2)" << endl << R_IMUinWorld(0,2) << endl;
		cout << "cos(x(3))" << endl << cos(x(3)) << endl;
		cout << "x" << endl << x << endl;
		cout << "Kt" << endl << Kt << endl;
		cout << "Mu_bar" << endl << Mu_prev << endl;
		/*cout << "Mu_prev" << endl << Mu_prev << endl;
		cout << "Ft" << endl << Ft << endl;
		cout << "Sigma_prev" << endl << Sigma_prev << endl;
		cout << "Sigma_bar" << endl << Sigma_bar << endl;*/
		cout << "dt" << endl << dt << endl;
		cout << "img_checktemp" << endl << img_checktemp << endl;
		temp_ff = 1;
	}
	//cout << "Mu" << endl << Mu << endl;
	if (img_checktemp>=0&&img_checktemp<=1)
    {
	//cout << "Sigma_bar" << endl << Sigma_bar << endl;
	//cout << "Mu_bar" << endl << Mu_bar << endl;
	//cout << "Ut" << endl << Mu_prev << endl;
   // cout << "At" << endl << At << endl;
    //cout << "H_WorldinCAM" << endl << H_WorldinCAM << endl;
	cout << "H_CAMinIMU * H_WorldinCAM" << endl << H_CAMinIMU * H_WorldinCAM << endl;
	cout << "H_WorldinCAM * H_CAMinIMU" << endl << H_WorldinCAM * H_CAMinIMU << endl;
    cout << "H_IMUinWorld" << endl << H_IMUinWorld << endl;
	//cout << "H_IMUinWorld.topRightCorner(3, 1)" << endl << H_IMUinWorld.topRightCorner(3, 1) << endl;
	//cout << "H_IMUinWorld.topLeftCorner(3, 3)"  << endl << H_IMUinWorld.topLeftCorner(3, 3)  << endl;
	//cout << "(Ct * Sigma_bar * Ct.transpose())" << endl << (Ct * Sigma_bar * Ct.transpose()) << endl;
	//cout << "Kt" << endl << Kt << endl;
	cout <<endl<< "***********************************"<<endl;
    }
	img_checktemp ++;
	//cout << "Kt" << endl << Kt << endl;
	Matrix3d m_temp;//??????????????
	m_temp <<
	cos(Mu(4))*cos(Mu(5))-sin(Mu(3))*sin(Mu(4))*sin(Mu(5)), -cos(Mu(3))*sin(Mu(5)), sin(Mu(4))*cos(Mu(5))+sin(Mu(3))*cos(Mu(4))*sin(Mu(5)),
    cos(Mu(4))*sin(Mu(5))+sin(Mu(3))*sin(Mu(4))*cos(Mu(5)), cos(Mu(3))*cos(Mu(5)) , sin(Mu(4))*sin(Mu(5))-sin(Mu(3))*cos(Mu(4))*cos(Mu(5)),
    -cos(Mu(3))*sin(Mu(4))                                , sin(Mu(3))            , cos(Mu(3))*cos(Mu(4))                                 ;
	Quaterniond Q_ekf;
    Q_ekf = m_temp;
    nav_msgs::Odometry odom_ekf;
    odom_ekf.header.stamp = msg->header.stamp;
    odom_ekf.header.frame_id = "world";
    odom_ekf.pose.pose.position.x = Mu(0);
    odom_ekf.pose.pose.position.y = Mu(1);
    odom_ekf.pose.pose.position.z = Mu(2);
    odom_ekf.pose.pose.orientation.w = Q_ekf.w();
    odom_ekf.pose.pose.orientation.x = Q_ekf.x();
    odom_ekf.pose.pose.orientation.y = Q_ekf.y();
    odom_ekf.pose.pose.orientation.z = Q_ekf.z();
	odom_ekf.twist.twist.linear.x    = Mu(6);
	odom_ekf.twist.twist.linear.y    = Mu(7);
	odom_ekf.twist.twist.linear.z    = Mu(8);
    odom_pub.publish(odom_ekf);
}
/*Updata Step*/
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ekf");
    ros::NodeHandle n("~");
    ros::Subscriber s1 = n.subscribe("imu", 1000, imu_callback);
    ros::Subscriber s2 = n.subscribe("tag_odom", 1000, odom_callback);
    odom_pub = n.advertise<nav_msgs::Odometry>("ekf_odom", 100);
    Rcam = Quaterniond(0, 0, -1, 0).toRotationMatrix();
    cout << "R_cam" << endl << Rcam << endl;
    // Q imu covariance matrix; Rt visual odomtry covariance matrix
    Q.topLeftCorner(6, 6) = 0.01 * Q.topLeftCorner(6, 6);
    Q.bottomRightCorner(6, 6) = 0.01 * Q.bottomRightCorner(6, 6);
    Rt.topLeftCorner(3, 3) = 0.5 * Rt.topLeftCorner(3, 3);
    Rt.bottomRightCorner(3, 3) = 0.5 * Rt.bottomRightCorner(3, 3);
    Rt.bottomRightCorner(1, 1) = 0.1 * Rt.bottomRightCorner(1, 1);

    ros::spin();
}


// this example shows how to compute elastic model energy, energy gradient and energy hessian

#include <iostream>
#include <math.h>
#include <time.h>
#include <fstream>

#include "MCSFD/MCSFDCore.h"
#include "LoboVolumtricMesh/LoboTetMesh.h"
#include "ElasticModel/HyperelasticModel.h"

#include "Utils/pugixml/pugixml.hpp"

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include "Utils/SparseMatrix/SparseMatrixTopology.h"
#include <omp.h>

LOBO_MAKE_TYPEDEFS(double, t);

using namespace Lobo;

void loadmaterial(Lobo::HyperelasticModel *elasticmodel, const char *xmlfile)
{
    pugi::xml_document xml_doc;
    pugi::xml_parse_result result = xml_doc.load_file(xmlfile);
    pugi::xml_node model_node = xml_doc.child("Scene").child("HyperelasticModel");
    elasticmodel->runXMLscript(model_node);
    elasticmodel->precompute();
    elasticmodel->useMCSFD = false;
    elasticmodel->isinvertible = false;
}

int main()
{
    omp_set_dynamic(0); // Explicitly disable dynamic teams
    omp_set_num_threads(
        1); // Use 4 threads for all consecutive parallel regions

    Lobo::LoboTetMesh *tetmesh = new Lobo::LoboTetMesh();
    tetmesh->loadTetMeshAscii("2tet");
    std::cout << tetmesh->tet_indices.size() << std::endl;
    Lobo::HyperelasticModel *stvkmodel = new Lobo::HyperelasticModel(tetmesh);
    loadmaterial(stvkmodel, "FEM_stvk.xml");
    Lobo::HyperelasticModel *stvkmodel_csfd = new Lobo::HyperelasticModel(tetmesh);
    loadmaterial(stvkmodel_csfd, "FEM_stvkCSFD.xml");

    Lobo::HyperelasticModel *neohookeanmodel = new Lobo::HyperelasticModel(tetmesh);
    loadmaterial(neohookeanmodel, "FEM_neohookean.xml");
    Lobo::HyperelasticModel *neohookeanmodel_csfd = new Lobo::HyperelasticModel(tetmesh);
    loadmaterial(neohookeanmodel_csfd, "FEM_neohookeanCSFD.xml");

    int num_dofs = tetmesh->getNumVertex() * 3;
    Eigen::VectorXd u(num_dofs);
    Eigen::VectorXd internalforce(num_dofs);

    int flags_all = 0;
    flags_all |= Computeflags_energy | Computeflags_fisrt | Computeflags_second;

    srand(time(NULL));
    u.setRandom();

    stvkmodel->currentdisplacement = u;
    stvkmodel_csfd->currentdisplacement = u;
    neohookeanmodel->currentdisplacement = u;
    neohookeanmodel_csfd->currentdisplacement = u;

    int numtest = 10000;
    clock_t t1 = clock();

    //compute energy, energy gradient, energy hessian with analytical function
    for (int i = 0; i < numtest; i++)
    {
        stvkmodel->getTetForceMatrix(0, stvkmodel->internalforce_list[0], stvkmodel->stiffness_list[0], flags_all);
    }

    clock_t t2 = clock();
    //std::cout << *stvkmodel->stiffness_list[0] << std::endl;
    std::cout << "no MSCFD stvk " << (double)(t2 - t1) / CLOCKS_PER_SEC << "s" << std::endl;

    t1 = clock();

    //compute energy, energy gradient, energy hessian with MSCFD
    for (int i = 0; i < numtest; i++)
    {
        stvkmodel_csfd->getTetForceMatrix(0, stvkmodel_csfd->internalforce_list[0], stvkmodel_csfd->stiffness_list[0], flags_all);
    }

    t2 = clock();

    //std::cout << *stvkmodel_csfd->stiffness_list[0] << std::endl;
    std::cout << "MSCFD stvk " << (double)(t2 - t1) / CLOCKS_PER_SEC << "s" << std::endl;

    t1 = clock();

    //compute energy, energy gradient, energy hessian with MSCFD
    for (int i = 0; i < numtest; i++)
    {
        stvkmodel_csfd->getTetForceMatrixCSFD(0, stvkmodel_csfd->internalforce_list[0], stvkmodel_csfd->stiffness_list[0]);
    }

    t2 = clock();

    //std::cout << *stvkmodel_csfd->stiffness_list[0] << std::endl;
    std::cout << "no image MSCFD stvk " << (double)(t2 - t1) / CLOCKS_PER_SEC << "s" << std::endl;

    t1 = clock();

    //compute energy, energy gradient, energy hessian with MSCFD
    for (int i = 0; i < numtest; i++)
    {
        neohookeanmodel->getTetForceMatrix(0, neohookeanmodel->internalforce_list[0], neohookeanmodel->stiffness_list[0], flags_all);
    }

    t2 = clock();

    //std::cout << *neohookeanmodel->stiffness_list[0] << std::endl;
    std::cout << "no MSCFD neohookean " << (double)(t2 - t1) / CLOCKS_PER_SEC << "s" << std::endl;

    t1 = clock();

    //compute energy, energy gradient, energy hessian with MSCFD
    for (int i = 0; i < numtest; i++)
    {
        neohookeanmodel_csfd->getTetForceMatrix(0, neohookeanmodel_csfd->internalforce_list[0], neohookeanmodel_csfd->stiffness_list[0], flags_all);
    }

    t2 = clock();

    //std::cout << *neohookeanmodel_csfd->stiffness_list[0] << std::endl;
    std::cout << "MSCFD neohookean " << (double)(t2 - t1) / CLOCKS_PER_SEC << "s" << std::endl;
    //delete tetmesh;
    //delete hyperelasticmodel_csfd;
    //delete hyperelasticmodel;

    return 0;
}


// Copyright (c) 2019 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include "pyinterp/detail/gsl/interpolate1d.hpp"
#include "pyinterp/detail/math.hpp"
#include <Eigen/Core>

namespace pyinterp {
namespace detail {
namespace math {

/// Set of coordinates/values used for interpolation
///  * q11 = (x1, y1)
///  * q12 = (x1, y2)
///  * .../...
///  * q1n = (x1, yn)
///  * q21 = (x2, y1)
///  * q22 = (x2, y2).
///  * .../...
///  * q2n = (x2, yn)
///  * .../...
///  * qnn = (xn, yn)
///
/// @code
/// Array({{x1, x2, ..., xn}, {y1, y2, ..., yn}},
///        {q11, q12, ..., q21, q22, ...., qnn})
/// @endcode
class XArray {
 public:
  /// Default constructor
  XArray() = delete;

  /// Creates a new Array
  XArray(const size_t x_size, const size_t y_size) {
    auto nx = x_size << 1U;
    auto ny = y_size << 1U;
    x_.resize(nx);
    y_.resize(ny);
    q_.resize(nx, ny);
  }

  /// Default destructor
  virtual ~XArray() = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  XArray(const XArray &rhs) = default;

  /// Move constructor
  ///
  /// @param rhs right value
  XArray(XArray &&rhs) noexcept = default;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  XArray &operator=(const XArray &rhs) = default;

  /// Move assignment operator
  ///
  /// @param rhs right value
  XArray &operator=(XArray &&rhs) noexcept = default;

  /// Get the half size of the window in abscissa.
  inline size_t nx() const noexcept {
    return static_cast<size_t>(x_.size()) >> 1U;
  }

  /// Get the half size of the window in ordinate.
  inline size_t ny() const noexcept {
    return static_cast<size_t>(y_.size()) >> 1U;
  }

  /// Get x-coordinates
  inline Eigen::VectorXd &x() noexcept { return x_; }

  /// Get x-coordinates
  inline const Eigen::VectorXd &x() const noexcept { return x_; }

  /// Get y-coordinates
  inline Eigen::VectorXd &y() noexcept { return y_; }

  /// Get y-coordinates
  inline const Eigen::VectorXd &y() const noexcept { return y_; }

  /// Get the values from the array for all x and y coordinates.
  inline Eigen::MatrixXd &q() noexcept { return q_; }

  /// Get the values from the array for all x and y coordinates.
  inline const Eigen::MatrixXd &q() const noexcept { return q_; }

  /// Get the ith x-axis.
  inline double x(const size_t ix) const { return x_(ix); }

  /// Get the ith y-axis.
  inline double y(const size_t jx) const { return y_(jx); }

  /// Get the value at coordinate (ix, jx).
  inline double z(const size_t ix, const size_t jx) const { return q_(ix, jx); }

  /// Set the ith x-axis.
  inline double &x(const size_t ix) { return x_(ix); }

  /// Get the ith y-axis.
  inline double &y(const size_t jx) { return y_(jx); }

  /// Get the value at coordinate (ix, jx).
  inline double &z(const size_t ix, const size_t jx) { return q_(ix, jx); }

  /// Normalizes the angle with respect to the first value of the X axis of this
  /// array.
  inline double normalize_angle(const double xi) const {
    return math::normalize_angle(xi, x(0));
  }

  /// Returns true if this instance does not contains at least one Not A Number
  /// (NaN).
  inline bool is_valid() const { return !q_.hasNaN(); }

 private:
  Eigen::VectorXd x_{};
  Eigen::VectorXd y_{};
  Eigen::MatrixXd q_{};
};

/// Extension of cubic interpolation for interpolating data points on a
/// two-dimensional regular grid. The interpolated surface is smoother than
/// corresponding surfaces obtained by bilinear interpolation or
/// nearest-neighbor interpolation.
class Bicubic {
 public:
  /// Default constructor
  ///
  /// @param type method of calculation
  explicit Bicubic(const gsl_interp_type *type = gsl_interp_cspline)
      : type_(type) {}

  /// Return the interpolated value of y for a given point x
  double interpolate(const double x, const double y, const XArray &xr,
                     gsl::Accelerator acc = gsl::Accelerator()) const {
    return evaluate(&gsl::Interpolate1D::interpolate, x, y, xr, std::move(acc));
  }

  /// Return the derivative for a given point x
  double derivative(const double x, const double y, const XArray &xr,
                    gsl::Accelerator acc = gsl::Accelerator()) const {
    return evaluate(&gsl::Interpolate1D::derivative, x, y, xr, std::move(acc));
  }

  /// Return the second derivative for a given point x
  double second_derivative(const double x, const double y, const XArray &xr,
                           gsl::Accelerator acc = gsl::Accelerator()) const {
    return evaluate(&gsl::Interpolate1D::second_derivative, x, y, xr,
                    std::move(acc));
  }

 private:
  using InterpolateFunction =
      double (gsl::Interpolate1D::*)(const double) const;
  const gsl_interp_type *type_;

  /// Evaluation of the GSL function performing the calculation.
  double evaluate(
      const std::function<double(const gsl::Interpolate1D &, double)> &function,
      const double x, const double y, const XArray &xr,
      gsl::Accelerator &&acc) const {
    Eigen::VectorXd fy(xr.x().size());

    // Spline interpolation as function of Y-coordinate
    for (auto ix = 0; ix < xr.x().size(); ++ix) {
      // The block containing the processed row must be copied into a new
      // memory block.
      Eigen::VectorXd row = xr.q().row(ix);
      auto interpolator = gsl::Interpolate1D(type_, xr.y(), row, acc);
      fy(ix) = function(interpolator, y);
    }
    auto interpolator = gsl::Interpolate1D(type_, xr.x(), fy, acc);
    return function(interpolator, x);
  }
};

}  // namespace math
}  // namespace detail
}  // namespace pyinterp


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <math.h>
#include <limits>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include "modules/world/opendrive/plan_view.hpp"
#include "modules/world/opendrive/lane.hpp"
#include "modules/world/opendrive/odrSpiral.hpp"

namespace modules {
namespace world {
namespace opendrive {

namespace bg = boost::geometry;

bool PlanView::add_line(geometry::Point2d start_point,
                        float heading,
                        float length) {
  //! straight line
  reference_line_.add_point(start_point);
  geometry::Point2d end_point(
    bg::get<0>(start_point) + length * cos(heading),
    bg::get<1>(start_point) + length * sin(heading));
  reference_line_.add_point(end_point);
  //! calculate overall length
  length_ = bg::length(reference_line_.obj_);
  return true;
}

bool PlanView::add_spiral(
  geometry::Point2d start_point,
  float heading,
  float length,
  float curvature_start,
  float curvature_end,
  float s_inc) {
  double x = bg::get<0>(start_point);
  double y = bg::get<1>(start_point);
  double t = heading, cDot = (curvature_end - curvature_start) / length;
  double x_old = bg::get<0>(start_point), y_old = bg::get<1>(start_point);

  double s = 0.0;
  for (; s <= length;) {
    odrSpiral(s, x_old, y_old, cDot, curvature_start, heading, &x, &y, &t);
    reference_line_.add_point(geometry::Point2d(x, y));
    if ((length - s < s_inc) && (length - s > 0.))
      s_inc = length - s;
    s += s_inc;
  }
  length_ = bg::length(reference_line_.obj_);
  return true;
}

void PlanView::calc_arc_position(
  const float s,
  float initial_heading,
  float curvature,
  float &dx,
  float &dy) {
  initial_heading = fmod(initial_heading, 2 * M_PI);
  float hdg = initial_heading - M_PI / 2;
  float a = 2 / curvature * sin(s * curvature / 2);
  float alpha = (M_PI - s * curvature) / 2 - hdg;
  dx = -1 * a * cos(alpha);
  dy = a * sin(alpha);
  // tangent = initial_heading + s * initial_curvature;
}

bool PlanView::add_arc(
  geometry::Point2d start_point,
  float heading,
  float length,
  float curvature,
  float s_inc) {
  // add_spiral(start_point, heading, length, curvature, curvature, s_inc);

  float dx, dy;
  double x_old = bg::get<0>(start_point), y_old = bg::get<1>(start_point);
  double s = 0.0;
  for (; s <= length;) {
    calc_arc_position(s, heading, curvature, dx, dy);
    reference_line_.add_point(geometry::Point2d(x_old + dx, y_old + dy));
    if (length - s < s_inc && length - s > 0.)
      s_inc = length - s;
    s += s_inc;
  }
  return true;
}

bool PlanView::apply_offset_transform(float x, float y, float hdg) {
  geometry::Line rotated_line = rotate(reference_line_, hdg);
  geometry::Line transformed_line = translate(rotated_line, x, y);
  reference_line_ = transformed_line;

  return true;
}

}  // namespace opendrive
}  // namespace world
}  // namespace modules


/*
   Implementation of "Highly efficient incremental estimation of Gaussian mixture models for online data stream clustering."

   Used to merge multiple mixture models

   Author: Ryan Watson

 */

#include <omp.h>
#include <vector>
#include <numeric>
#include <stdexcept>
#include <iostream>
#include <Eigen/Dense>

#include "merge.h"
#include "libcluster.h"
#include "probutils.h"
#include "comutils.h"
#include "distributions.h"

#include <boost/math/distributions/fisher_f.hpp>
#include <boost/math/distributions/chi_squared.hpp>

using namespace std;
using namespace Eigen;
using namespace boost;
using namespace comutils;
using namespace probutils;
using namespace libcluster;
using namespace boost::math;
using namespace distributions;

using boost::math::chi_squared;
using boost::math::quantile;
using boost::math::complement;

//
// /*! \brief Merge similar mixture components as specified in the reference provided below.
//
//    REF ::
//    [1] Song, Mingzhou, and Hongbin Wang. "Highly efficient incremental estimation of Gaussian mixture models for online data stream clustering." Intelligent Computing: Theory and Applications III. Vol. 5803. International Society for Optics and Photonics, 2005.
//  */
//
//
// Author: Ryan Watson
//

bool merge::checkCov(Eigen::MatrixXd data, Eigen::MatrixXd qZ, mixtureComponents priorModel, GaussWish currModel, double currWeight, int currModelIdx, double alpha)
{
        Eigen::MatrixXd transObs, chol, chol_inv, tmp;

        chol = (priorModel.get<4>()).llt().matrixL();
        chol_inv = chol.inverse();

        int n = currModel.getN() + 1;
        int d = (currModel.getmean()).size() + 1;

        chi_squared dist((d*(d+1))/2);
        double ucv = quantile(complement(dist, alpha));
        double ucv2 = quantile(complement(dist, alpha/2.0));
        double lcv = quantile(dist, alpha);
        double lcv2 = quantile(dist, alpha/2.0);

        ArrayXi mapidx = partobs(data, (qZ.col(currModelIdx).array()>0.5), tmp);
        transObs.setZero(tmp.rows(),d);
        for (int k = 0; k < tmp.rows(); k++)
        {
                transObs.row(k) = chol_inv * tmp.row(k);
        }

        if (transObs.rows() < 3) {return false; }
        auto transCov = probutils::cov(transObs);
        auto covDiff = transCov - Eigen::MatrixXd::Identity(d,d);
        auto covDiff_sq = covDiff * covDiff;
        double dd = static_cast<double>(d);
        double nn = static_cast<double>(n);
        double f = (1.0/dd) * ( (covDiff_sq).trace() );
        double s = (dd/nn) * pow( (1.0/dd)*(transCov.trace()),2 );
        double W = f - s + dd/nn;
        double t_stat = ((nn*W*dd)/2.0);
        if ( t_stat < ucv2 || t_stat > lcv2 ) { return true; }
        return false;
}

bool merge::checkCovGMM(mixtureComponents prior, mixtureComponents test, double alpha)
{
        Eigen::MatrixXd test_cov, chol, chol_inv;

        chol = (test.get<4>()).llt().matrixL();
        chol_inv = chol.inverse();
        test_cov = chol_inv * test.get<4>() * chol_inv.transpose();

        int n = prior.get<1>() + 1;
        int d = (prior.get<3>()).size() + 1;

        chi_squared dist((d*(d+1))/2);
        double ucv = quantile(complement(dist, alpha));
        double ucv2 = quantile(complement(dist, alpha/2.0));
        double lcv = quantile(dist, alpha);
        double lcv2 = quantile(dist, alpha/2.0);

        auto covDiff = test_cov - Eigen::MatrixXd::Identity(d,d);
        auto covDiff_sq = covDiff * covDiff;
        double dd = static_cast<double>(d);
        double nn = static_cast<double>(n);
        double f = (1.0/dd) * ( (covDiff_sq).trace() );
        double s = (dd/nn) * pow( (1.0/dd)*(test_cov.trace()),2 );
        double W = f - s + dd/nn;
        double t_stat = ((nn*W*dd)/2.0);
        if ( t_stat < ucv2 || t_stat > lcv2 ) { return true; }
        return false;
}

bool merge::checkMean(mixtureComponents priorModel, GaussWish currModel, double alpha)
{

        int n = currModel.getN() + 1;
        int d = (currModel.getmean()).size() + 1;
        if (n-d <= 1) {return false; }
        fisher_f dist(d, n-d);

        double ucv = quantile(complement(dist, alpha));
        double ucv2 = quantile(complement(dist, alpha/2.0));
        double lcv = quantile(dist, alpha);
        double lcv2 = quantile(dist, alpha/2.0);

        Eigen::RowVectorXd meanDiff = currModel.getmean() - priorModel.get<3>();
        double t = (meanDiff * currModel.getcov() * meanDiff.transpose());
        double t_stat = (double)(n-d)/(double)(d*(n-1)) * pow(t,2.0);
        if (t_stat < ucv) {  return true; }
        return false;
}

bool merge::checkMeanGMM(mixtureComponents prior, mixtureComponents test, double alpha)
{

        int n = test.get<1>() + 1;
        int d = (test.get<3>()).size() + 1;
        if (n-d <= 1) {return false; }

        fisher_f dist(d, n-d);

        double ucv = quantile(complement(dist, alpha));
        double ucv2 = quantile(complement(dist, alpha/2.0));
        double lcv = quantile(dist, alpha);
        double lcv2 = quantile(dist, alpha/2.0);

        Eigen::RowVectorXd meanDiff = prior.get<3>() - test.get<3>();
        double t = (meanDiff * test.get<4>() * meanDiff.transpose());
        double t_stat = (double)(n-d)/(double)(d*(n-1)) * pow(t,2.0);
        if (t_stat < ucv) {  return true; }
        return false;
}

vector<double> merge::getPriorWeights(vector<mixtureComponents> gmm)
{
        vector<double> weights;
        for (unsigned int i=0; i<gmm.size(); i++)
        {
                weights.push_back(gmm[i].get<2>());
        }
        return weights;
}

bool merge::checkComponent(Eigen::MatrixXd data, Eigen::MatrixXd qZ, mixtureComponents priorModel, GaussWish currModel, double currWeight, int currModelIdx, double alpha)
{
        if (checkCov(data, qZ, priorModel, currModel, currWeight, currModelIdx, alpha))
        {
                if (checkMean(priorModel, currModel, alpha))
                        return true;
                else
                        return false;
        }
        return false;
}

bool merge::checkComponentGMM(mixtureComponents prior, mixtureComponents test, double alpha)
{
        if (checkCovGMM(prior, test, alpha))
        {
                if (checkMeanGMM(prior, test, alpha))
                        return true;
                else
                        return false;
        }
        return false;
}

bool merge::sortbyobs(merge::mixtureComponents a, merge::mixtureComponents b)
{
        return (a.get<1>() < b.get<1>());
}

vector<merge::mixtureComponents> merge::updateObs(vector<merge::mixtureComponents> gmm, vector<int> numObs)
{
        unsigned int N(0);
        for (unsigned int i=0; i<gmm.size(); i++)
        {
                N += numObs.at(i);
        }

        for (unsigned int i=0; i<gmm.size(); i++)
        {
                // update total num of obs.
                gmm[i].get<0>() += N;
                if (numObs.at(i) != 0 )
                {
                        // update num of obs in component
                        gmm[i].get<1>() += numObs.at(i);
                        // update components weight.
                        gmm[i].get<2>() = gmm[i].get<1>() / double(gmm[i].get<0>());
                }
        }
        return gmm;
}

vector<merge::mixtureComponents> merge::pruneMixtureModel(vector<merge::mixtureComponents> gmm, int truncLevel)
{
        if (gmm.size() < truncLevel)
                return gmm;

        merge::mixtureComponents currComponent;
        sort(gmm.begin(), gmm.end(), sortbyobs);
        while (gmm.size() > truncLevel)
        {
                currComponent = gmm[0];
                int n = currComponent.get<1>();
                for (unsigned int i=1; i<gmm.size(); i++)
                {
                        // update number of obs. for each component
                        gmm[i].get<0>() = gmm[i].get<0>() - n;
                        // update weight for each component
                        gmm[i].get<2>() = (double)gmm[i].get<1>() / (double)gmm[i].get<0>();
                }
                gmm.erase(gmm.begin());
        }

        return gmm;
}

vector<merge::mixtureComponents> merge::mergeMixtureModel(Eigen::MatrixXd data, Eigen::MatrixXd qZ, vector<merge::mixtureComponents> priorModel, vector<GaussWish> currModel, StickBreak currWeight, double alpha, int truncLevel)
{
        vector<merge::mixtureComponents> gmm;
        int dataCard = data.rows();

        auto pWeights =  getPriorWeights(priorModel);
        auto cWeights =  currWeight.Elogweight().exp().transpose();
        int cc = 0;
        vector<int> pModelMatches;
        vector<int> cModelMatches;

        if ( priorModel.size() == 0)
        {
                for (vector<GaussWish>::iterator j = currModel.begin(); j < currModel.end(); ++j)
                {
                        // num obs.
                        int n = j->getN();
                        // cluster weights
                        double w = cWeights[cc];
                        // cluster mean
                        auto m = j->getmean();
                        // cluster cov
                        auto c = j->getcov();
                        // add new component to mixture model
                        gmm.push_back(boost::make_tuple(dataCard, n, w, m, c));
                        cc+=1;
                }
                return gmm;
        }

        // remove cluster without sufficient obs.
        int m_inc = 0;
        for (vector<GaussWish>::iterator j = currModel.begin(); j < currModel.end(); ++j)
        {
                if (j->getN() < 2.0) {
                        currModel.erase(currModel.begin() + m_inc);
                }
                m_inc++;
        }

        for (unsigned int i=0; i<priorModel.size(); i++)
        {

                for (vector<GaussWish>::iterator j = currModel.begin(); j < currModel.end(); ++j)
                {
                        if( checkComponent(data, qZ, priorModel[i], *j, cWeights[cc], cc, alpha)
                            && !(std::find(pModelMatches.begin(), pModelMatches.end(), i) != pModelMatches.end())
                            && !(std::find(cModelMatches.begin(), cModelMatches.end(), cc) != cModelMatches.end()) )
                        {
                                // merge components
                                mixtureComponents pModel = priorModel[i];
                                // update mean and cov based on eq.

                                int NP = pModel.get<0>();
                                int n = pModel.get<1>() +j->getN();
                                double WP = pModel.get<2>();
                                Eigen::RowVectorXd MP = pModel.get<3>();
                                Eigen::MatrixXd CM = pModel.get<4>();
                                double NPWP = NP*WP;

                                Eigen::RowVectorXd MC = j->getmean();
                                Eigen::MatrixXd CC = j->getcov();
                                int NC = j->getN();

                                auto denom = NPWP + NC;

                                /// update mean --> REF:: [1] Eq. 6
                                Eigen::RowVectorXd n_mean = (NPWP*MP + NC*MC )*(1/denom);

                                Eigen::MatrixXd A = (NPWP*CM + NC*CC)*(1/denom);

                                Eigen::MatrixXd cov_p = MP.transpose()*MP;
                                Eigen::MatrixXd cov_c = MC.transpose()*MC;

                                Eigen::MatrixXd B = ( NPWP*cov_p + NC*cov_c )*(1/denom);

                                Eigen::MatrixXd C = n_mean.transpose() * n_mean;

                                /// update cov --> REF:: [1] Eq. 7
                                Eigen::MatrixXd n_cov = A + B - C;

                                double w =  (NPWP + NC) /(double) (NP + dataCard);

                                gmm.push_back(boost::make_tuple(NP+dataCard, n, w, n_mean, n_cov));
                                pModelMatches.push_back(i);
                                cModelMatches.push_back(cc);
                        }
                        cc+=1;
                }
                cc=0;
        }

        // Add all unmerged components from new GMM
        int idx = 0;
        for (vector<GaussWish>::iterator j = currModel.begin(); j < currModel.end(); ++j)
        {
                if (!(std::find(cModelMatches.begin(), cModelMatches.end(), idx) != cModelMatches.end()))
                {
                        Eigen::RowVectorXd MC = j->getmean();
                        Eigen::MatrixXd CC = j->getcov();
                        int NC = j->getN();
                        int n = priorModel[0].get<0>() + dataCard;

                        double w = NC/(double)n;

                        gmm.push_back(boost::make_tuple(n, NC, w, MC, CC));
                }

                idx+=1;
        }

        // Add all unmerged components from previous GMM
        idx = 0;
        for (unsigned int i=0; i<priorModel.size(); i++)
        {
                mixtureComponents pModel;
                pModel = priorModel[i];
                if (!(std::find(pModelMatches.begin(), pModelMatches.end(), idx) != pModelMatches.end()))
                {
                        int NP = pModel.get<1>();
                        int n = pModel.get<0>() + dataCard;
                        double w = NP/(double)n;
                        Eigen::RowVectorXd MP = pModel.get<3>();
                        Eigen::MatrixXd CM = pModel.get<4>();

                        gmm.push_back(boost::make_tuple(n, NP, w, MP, CM));
                }

                idx+=1;
        }

        // Merge equilavent components in new GMM
        for (unsigned int i=0; i<gmm.size(); i++)
        {
                for (unsigned int j=0; j<gmm.size(); j++)
                {
                        if (i==j) {continue; }
                        if (checkComponentGMM(gmm[i], gmm[j], alpha))
                        {

                                int NP = gmm[i].get<0>();
                                int n = gmm[i].get<1>();
                                double WP = gmm[i].get<2>();
                                Eigen::RowVectorXd MP = gmm[i].get<3>();
                                Eigen::MatrixXd CM = gmm[i].get<4>();
                                double NPWP = NP*WP;

                                int NH = gmm[j].get<0>();
                                int nh = gmm[j].get<1>();
                                double WH = gmm[j].get<2>();
                                Eigen::RowVectorXd MH = gmm[j].get<3>();
                                Eigen::MatrixXd CH = gmm[j].get<4>();
                                double NHWH = NH*WH;

                                auto denom = NPWP + NHWH;

                                /// update mean --> REF:: [1] Eq. 6
                                Eigen::RowVectorXd n_mean = (NPWP*MP + NHWH*MH )*(1/denom);

                                Eigen::MatrixXd A = (NPWP*CM + NHWH*CH)*(1/denom);

                                Eigen::MatrixXd cov_p = MP.transpose()*MP;
                                Eigen::MatrixXd cov_c = MH.transpose()*MH;

                                Eigen::MatrixXd B = ( NPWP*cov_p + NHWH*cov_c )*(1/denom);

                                Eigen::MatrixXd C = n_mean.transpose() * n_mean;

                                gmm[i].get<1>() = (n + nh);
                                gmm[i].get<2>() = gmm[i].get<1>()/(double) (NP);
                                gmm[i].get<3>() = n_mean;
                                /// update cov --> REF:: [1] Eq. 7
                                gmm[i].get<4>() = A + B - C;

                                // remove merged component from global GMM
                                gmm.erase(gmm.begin() + j);
                        }
                }

        }

        // prune the model to specified truncation level
        gmm = pruneMixtureModel(gmm, truncLevel);

        return gmm;
}

merge::observationModel merge::getMixtureComponent(vector<merge::mixtureComponents> gmm, Eigen::VectorXd observation)
{

        int idx = 0;
        double minProb = 0, modelProb;
        Eigen::RowVectorXd mean;
        Eigen::RowVectorXd obs = observation.transpose();
        Eigen::MatrixXd cov;
        double doublePi = M_PI * 2.0;

        for (int i=0; i<gmm.size(); i++)
        {

                mean = gmm[i].get<3>();
                cov = gmm[i].get<4>();
                double err = (obs-mean)*cov.inverse()*(observation-mean.transpose());
                modelProb = -1 * log(err);

                if (modelProb < minProb)
                {
                        minProb = modelProb;
                        idx = i;
                }
        }

        merge::mixtureComponents gmmComp = gmm[idx];
        merge::observationModel obsModel = boost::make_tuple(gmmComp, minProb);

        return obsModel;
}


/*! \file hydrogen_fem.cpp
    \brief FEMで水素原子に対するSchrödinger方程式を解くクラスの実装

    Copyright © 2019 @dc1394 All Rights Reserved.
    (but this is originally adapted by sunsetyuhi for fem1d_poisson.py from https://github.com/sunsetyuhi/fem_py/blob/master/fem1d_poisson )
    This software is released under the BSD 2-Clause License.
*/

#include "hydrogen_fem.h"
#include <cmath>                // for std::exp
#include <cstdio>               // for FILE, std::fclose, std::fopen, std::fprintf
#include <memory>               // for std::unique_ptr
#include <boost/assert.hpp>     // for BOOST_ASSERT
#include <Eigen/Eigenvalues>    // for Eigen::GeneralizedSelfAdjointEigenSolver

namespace hydrogen_fem {
    // #region コンストラクタ

    Hydrogen_FEM::Hydrogen_FEM()
        :   hg_(Eigen::MatrixXd::Zero(NODE_TOTAL, NODE_TOTAL)),
            length_(ELE_TOTAL),
            mat_A_ele_(boost::extents[ELE_TOTAL][2][2]),
            mat_B_ele_(boost::extents[ELE_TOTAL][2][2]),
            node_num_seg_(boost::extents[ELE_TOTAL][2]),
            node_r_ele_(boost::extents[ELE_TOTAL][2]),
            node_r_glo_(NODE_TOTAL),
            ug_(Eigen::MatrixXd::Zero(NODE_TOTAL, NODE_TOTAL))
    {
    }

    // #endregion コンストラクタ

    // #region publicメンバ関数

    double Hydrogen_FEM::do_run()
    {
        // 各種データの生成
        make_data();

        // 要素行列の生成
        make_element_matrix();

        // 全体行列を生成
        make_global_matrix();

        // 境界条件処理を行う
        boundary_conditions();

        // 一般化固有値問題を解く
        Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> es(hg_, ug_);

        // エネルギー固有値を取得
        eigenval_ = es.eigenvalues();

        // 基底状態の固有関数（波動関数）（波動関数）を取得
        phi_ = es.eigenvectors().col(0);

        // 固有ベクトル（波動関数）のN要素目を追加
        phi_.resize(NODE_TOTAL);

        // 固有ベクトル（波動関数）を規格化
        normalize();

        return eigenval_[0];
    }

    void Hydrogen_FEM::save_result() const
    {
        std::unique_ptr<FILE, decltype(&std::fclose)> fp_eigenfunc(std::fopen(Hydrogen_FEM::EIGENFUNC_FILENAME, "w"), std::fclose);

        for (auto i = 0; i < NODE_TOTAL; i++) {
            auto const r = node_r_glo_[i];
            // 厳密な結果と比較
            std::fprintf(fp_eigenfunc.get(), "%.14f, %.14f, %.14f\n", r, phi_[i], 2.0 * std::exp(-r));
        }

        std::unique_ptr<FILE, decltype(&std::fclose)> fp_eigenval(std::fopen(Hydrogen_FEM::EIGENVAL_FILENAME, "w"), std::fclose);

        for (auto i = 0; i < NODE_TOTAL - 1; i++) {
            // 厳密な結果と比較
            std::fprintf(fp_eigenval.get(), "%d, %.14f, %.14f\n", i + 1, eigenval_[i], - 0.5 / static_cast<double>((i + 1) * (i + 1)));
        }

    }

    // #endregion publicメンバ関数

    // #region privateメンバ関数

    void Hydrogen_FEM::boundary_conditions()
    {
        // 左辺の全体行列のN + 1行とN + 1列を削る
        hg_.conservativeResize(hg_.rows() - 1, hg_.cols() - 1);

        // 右辺の全体行列のN + 1行とN + 1列を削る
        ug_.conservativeResize(ug_.rows() - 1, ug_.cols() - 1);
    }

    double Hydrogen_FEM::get_A_matrix_element(std::int32_t e, double le, std::int32_t p, std::int32_t q) const
    {
        auto const ed = static_cast<double>(e);
        switch (p) {
        case 0:
            switch (q) {
            case 0:
                return 0.5 * le * (ed * ed + ed + 1.0 / 3.0) - le * le * (ed / 3.0 + 1.0 / 12.0);

            case 1:
                return -0.5 * le * (ed * ed + ed + 1.0 / 3.0) - le * le * (ed / 6.0 + 1.0 / 12.0);

            default:
                BOOST_ASSERT(!"heの添字が2以上！");
                return 0.0;
            }

        case 1:
            switch (q) {
            case 0:
                return -0.5 * le * (ed * ed + ed + 1.0 / 3.0) - le * le * (ed / 6.0 + 1.0 / 12.0);

            case 1:
                return 0.5 * le * (ed * ed + ed + 1.0 / 3.0) - le * le * (ed / 3.0 + 0.25);

            default:
                BOOST_ASSERT(!"heの添字が2以上！");
                return 0.0;
            }

        default:
            BOOST_ASSERT(!"heの添字が2以上！");
            return 0.0;
        }
    }

    double Hydrogen_FEM::get_B_matrix_element(std::int32_t e, double le, std::int32_t p, std::int32_t q) const
    {
        auto const ed = static_cast<double>(e);
        switch (p) {
        case 0:
            switch (q) {
            case 0:
                return le * le * le * (ed * ed / 3.0 + ed / 6.0 + 1.0 / 30.0);

            case 1:
                return le * le * le * (ed * ed / 6.0 + ed / 6.0 + 0.05);

            default:
                BOOST_ASSERT(!"ueの添字が2以上！");
                return 0.0;
            }

        case 1:
            switch (q) {
            case 0:
                return le * le * le * (ed * ed / 6.0 + ed / 6.0 + 0.05);

            case 1:
                return le * le * le * (ed * ed / 3.0 + ed / 2.0 + 0.2);

            default:
                BOOST_ASSERT(!"ueの添字が2以上！");
                return 0.0;
            }

        default:
            BOOST_ASSERT(!"ueの添字が2以上！");
            return 0.0;
        }
    }

    void Hydrogen_FEM::make_element_matrix()
    {
        // 各線分要素の長さを計算
        for (auto e = 0; e < ELE_TOTAL; e++) {
            length_[e] = std::fabs(node_r_ele_[e][1] - node_r_ele_[e][0]);
        }

        // 要素行列の各成分を計算
        for (auto e = 0; e < ELE_TOTAL; e++) {
            auto const le = length_[e];
            for (auto i = 0; i < 2; i++) {
                for (auto j = 0; j < 2; j++) {
                    mat_A_ele_[e][i][j] = get_A_matrix_element(e, le, i, j);
                    mat_B_ele_[e][i][j] = get_B_matrix_element(e, le, i, j);
                }
            }
        }
    }

    void Hydrogen_FEM::make_data()
    {
        // Global節点のx座標を定義(R_MIN～R_MAX）
        auto const dr = (R_MAX - R_MIN) / static_cast<double>(ELE_TOTAL);
        for (auto i = 0; i <= ELE_TOTAL; i++) {
            // 計算領域を等分割
            node_r_glo_[i] = R_MIN + static_cast<double>(i) * dr;
        }

        for (auto e = 0; e < ELE_TOTAL; e++) {
            node_num_seg_[e][0] = e;
            node_num_seg_[e][1] = e + 1;
        }

        for (auto e = 0; e < ELE_TOTAL; e++) {
            for (auto i = 0; i < 2; i++) {
                node_r_ele_[e][i] = node_r_glo_[node_num_seg_[e][i]];
            }
        }
    }

    void Hydrogen_FEM::make_global_matrix()
    {
        for (auto e = 0; e < ELE_TOTAL; e++) {
            for (auto i = 0; i < 2; i++) {
                for (auto j = 0; j < 2; j++) {
                    hg_(node_num_seg_[e][i], node_num_seg_[e][j]) += mat_A_ele_[e][i][j];
                    ug_(node_num_seg_[e][i], node_num_seg_[e][j]) += mat_B_ele_[e][i][j];
                }
            }
        }
    }

    void Hydrogen_FEM::normalize()
    {
        auto sum = 0.0;
        auto const size = phi_.size();
        auto const max = size - 2;

        // Simpsonの公式によって数値積分する
        for (auto i = 0; i < max; i += 2) {
            auto const f0 = phi_[i] * phi_[i] * node_r_glo_[i] * node_r_glo_[i];
            auto const f1 = phi_[i + 1] * phi_[i + 1] * node_r_glo_[i + 1] * node_r_glo_[i + 1];
            auto const f2 = phi_[i + 2] * phi_[i + 2] * node_r_glo_[i + 2] * node_r_glo_[i + 2];
            sum += (f0 + 4.0 * f1 + f2);
        }

        auto const a_1 = 1.0 / std::sqrt(sum * length_[0] / 3.0);

        for (auto i = 0; i < size; i++) {
            phi_[i] *= -a_1;
        }
    }

    // #endregion privateメンバ関数
}


#include <map>
#include <vector>
#include <iostream>
#include <iomanip>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

#include "DatasetAR.h"
#include "GaussSeq.h"
#include "ARSeq.h"
#include "utils.h"

using utils::my_float;

int main() {
    using boost::multiprecision::cpp_bin_float_50;
    using boost::random::uniform_real_distribution;

    boost::random::mt19937 gen {};
    uniform_real_distribution<my_float> u (0, 4);

    // prepare target
    std::map<int, my_float> coeff {
        {3, 0.2},
        {20, 0.5},
    };
    std::vector<my_float> v;
    for (auto i = 0; i < 20; i++) {
        v.push_back(u(gen));
    }

    // AR process
    ARSeq targ_seq(coeff, 0);
    targ_seq.seed_prev_vals(v);

    // Dataset write
    DatasetAR dat_0("testing_zero_background", targ_seq, 0);
    DatasetAR dat_1("testing_one_background", targ_seq, 1);
    DatasetAR dat_2("testing_two_background", targ_seq, 2);
    DatasetAR dat_3("testing_three_background", targ_seq, 3);
    DatasetAR dat_4("testing_four_background", targ_seq, 4);

    dat_0.write_csv();
    dat_1.write_csv();
    dat_2.write_csv();
    dat_3.write_csv();
    dat_4.write_csv();

    return 0;
}


/**
 * \file boost/numeric/ublasx/operation/svd.hpp
 *
 * \brief Singular Value Decomposition problem.
 *
 * The singular value decomposition (SVD) of a m-by-n real/complex
 * matrix \f$A\f$ is:
 * \f[
 *   A = U \Sigma V^{H}
 * \f]
 * where \f$\Sigma\f$ is an m-by-n matrix which is zero except for its
 * \f$\min(m,n)\f$ diagonal elements, \f$U\f$ is an m-by-m unitary matrix, and
 * \f$V\f$ is an n-by-n unitary matrix.
 * The diagonal elements of \f$\Sigma\f$ are the singular values of
 * \f$A\f$; they are real and non-negative, and are returned in descending order.
 * The first \f$\min(m,n)\f$ columns of \f$U\f$ and \f$V\f$ are the left and
 * right singular vectors of \f$A\f$.
 *
 * When an economy-size SVD is requested, if \f$k=\min(m.n)\f$, it results that
 * \f$\Sigma\f$ is a k-by-k diagonal matrix, \f$U\f$ is an m-by-k unitary matrix
 * and \f$V\f$ is an n-by-k unitary matrix.
 * In this case the original matrix \f$A\f$ cannot be reconstructed.
 *
 * \note
 *  For real m-by-n matrix \f$A\f$ the associated SVD is:
 * \f[
 *   A = U \Sigma V^{T}
 * \f]
 *
 *

 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_SVD_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_SVD_HPP

#include <algorithm>
#include <boost/numeric/bindings/lapack/driver/gesvd.hpp>
#include <boost/numeric/bindings/ublas.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/detail/lapack.hpp>
#include <boost/numeric/ublasx/operation/diag.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <boost/numeric/ublasx/traits/layout_type.hpp>
#include <boost/type_traits/is_complex.hpp>
#include <boost/utility/enable_if.hpp>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail { namespace /*<unnamed>*/ {

template <
	typename AMatrixT,
	typename SVectorT,
	typename UMatrixT,
	typename VTMatrixT
>
void svd_impl(AMatrixT const& A, SVectorT& s, bool want_U, bool full_U, UMatrixT& U, bool want_VT, bool full_VT, VTMatrixT& VT, column_major_tag)
{
	typedef typename matrix_traits<AMatrixT>::value_type value_type;
	typedef typename matrix_traits<AMatrixT>::size_type size_type;
	typedef matrix<value_type, column_major> work_matrix_type;

	char jobu = 'N';
	char jobvt = 'N';
	size_type m = num_rows(A);
	size_type n = num_columns(A);
	size_type k = ::std::min(m, n);
	size_type U_nr = detail::lapack::min_array_size;
	size_type U_nc = detail::lapack::min_array_size;
	size_type VT_nr = detail::lapack::min_array_size;
	size_type VT_nc = detail::lapack::min_array_size;

	if (want_U)
	{
		U_nr = m;
		if (full_U)
		{
			jobu = 'A';
			U_nc = m;
		}
		else
		{
			jobu = 'S';
			U_nc = k;
		}
	}
	if (want_VT)
	{
		VT_nc = n;
		if (full_VT)
		{
			jobvt = 'A';
			VT_nr = n;
		}
		else
		{
			jobvt = 'S';
			VT_nr = k;
		}
	}

	if (size(s) != k)
	{
		s.resize(k, false);
	}
	if (num_rows(U) != U_nr || num_columns(U) != U_nc)
	{
		U.resize(U_nr, U_nc, false);
	}
	if (num_rows(VT) != VT_nr || num_columns(VT) != VT_nc)
	{
		VT.resize(VT_nr, VT_nc, false);
	}

	work_matrix_type tmp_A(A);

	::boost::numeric::bindings::lapack::gesvd(
		jobu,
		jobvt,
		tmp_A,
		s,
		U,
		VT
	);
}

template <
	typename AMatrixT,
	typename SVectorT,
	typename UMatrixT,
	typename VTMatrixT
>
void svd_impl(AMatrixT const& A, SVectorT& s, bool want_U, bool full_U, UMatrixT& U, bool want_VT, bool full_VT, VTMatrixT& VT, row_major_tag)
{
	typedef typename matrix_traits<AMatrixT>::value_type value_type;
	typedef matrix<value_type, column_major> colmaj_matrix_type;

	colmaj_matrix_type tmp_A(A);
	colmaj_matrix_type tmp_U;
	colmaj_matrix_type tmp_VT;

	svd_impl(tmp_A, s, want_U, full_U, tmp_U, want_VT, full_VT, tmp_VT, column_major_tag());

	if (want_U)
	{
		U = tmp_U;
	}
	if (want_VT)
	{
		VT = tmp_VT;
	}
}

template <typename MatrixT>
typename ::boost::enable_if<
	::boost::is_complex<typename matrix_traits<MatrixT>::value_type>,
	MatrixT
>::type make_V(MatrixT const& VH)
{
	return herm(VH);
}

template <typename MatrixT>
typename ::boost::disable_if<
	::boost::is_complex<typename matrix_traits<MatrixT>::value_type>,
	MatrixT
>::type make_V(MatrixT const& VT)
{
	return trans(VT);
}

}} // Namespace detail::<unnamed>

/**
 * \brief Computes the singular value decomposition (SVD) of a matrix.
 *
 * Computes the singular value decomposition (SVD) of a m-by-n matrix,
 * optionally computing the left and/or right singular vectors.
 * The SVD is written as:
 * \f[
 *   A = U \Sigma V^{H}
 * \f]
 * where \f$\Sigma\f$ is an m-by-n matrix which is zero except for its
 * \f$\min(m,n)\f$ diagonal elements, \f$U\f$ is an m-by-m unitary matrix, and
 * \f$V\f$ is an n-by-n unitary matrix.
 * The diagonal elements of \f$\Sigma\f$ are the singular values of
 * \f$A\f$; they are real and non-negative, and are returned in descending order.
 * The first \f$\min(m,n)\f$ columns of \f$U\f$ and \f$V\f$ are the left and
 * right singular vectors of \f$A\f$.
 *
 * When full mode is disabled, an economy-size SVD is computed, such
 * that, if \f$k=\min(m.n)\f$, \f$\Sigma\f$ is a k-by-k diagonal matrix,
 * \f$U\f$ is an m-by-k unitary matrix and \f$V\f$ is an n-by-k unitary matrix.
 *
 * \author Marco Guazzone, marco.guazzoe@gmail.com
 */
template <typename ValueT>
class svd_decomposition
{
	public: typedef ValueT value_type;
	public: typedef typename type_traits<value_type>::real_type real_type;
	public: typedef vector<real_type> vector_type;
	public: typedef matrix<value_type, column_major> matrix_type;
	public: typedef matrix<real_type, column_major> real_matrix_type;
	private: typedef typename matrix_traits<matrix_type>::size_type size_type;

	/// Default constructor
	public: svd_decomposition()
		: full_(false),
		  m_(0),
		  n_(0),
		  k_(0)
	{
	}

	/// A constructor.
	public: template <typename MatrixExprT>
		svd_decomposition(matrix_expression<MatrixExprT> const& A, bool full = true)
	{
		decompose(A, full);
	}

	/// Compute the SVD \f$A=U \Sigma V^H\f$
	public: template <typename MatrixExprT>
		void decompose(matrix_expression<MatrixExprT> const& A, bool full = true)
	{
		// Cache some values (useful for later info retrieval)
		full_ = full;
		m_ = num_rows(A);
		n_ = num_columns(A);
		k_ = ::std::min(m_, n_);

		detail::svd_impl(A(), s_, true, full, U_, true, full, VH_, column_major_tag());
	}

	/// Return the U matrix of the SVD \f$U \Sigma V^H\f$.
	public: matrix_type const& U() const
	{
		return U_;
	}

	/// Return the \f$\operatorname{diag}(\Sigma)\f$ vector of the SVD \f$U \Sigma V^H\f$.
	public: vector_type const& s() const
	{
		return s_;
	}

	/// Return the \f$\Sigma\f$ matrix of the SVD \f$U \Sigma V^H\f$.
	public: real_matrix_type S() const
	{
		if (full_)
		{
			//return diag(s_, m_, n_);
			return diag<vector_type,column_major>(s_, m_, n_);
		}
		//return diag(s_, k_, k_);
		return diag<vector_type,column_major>(s_, k_, k_);
	}

	/// Return the \f$V^H\f$ matrix (\f$V^T\f$ for real types) of the SVD
	/// \f$U \Sigma V^H\f$.
	public: matrix_type const& VH() const
	{
		return VH_;
	}

	/// Return the \f$V\f$ matrix of the SVD \f$U \Sigma V^H\f$.
	public: matrix_type V() const
	{
		return detail::make_V(VH_);
	}

//	TODO: interesting but not urgent.
//	/**
//	 * \brief  Compute the approximate error bound for the computed singular
//	 *  values.
//	 * \note
//	 *  For the 2-norm, \f$\Sigma(1,1) = \operatorname{norm}(A)\f$.
//	 */
//	public: real_type s_error() const
//	{
//		return ::std::numeric_limits<real_type>::epsilon()*s_(0);
//	}

//	TODO: interesting but not urgent.
//	/**
//	 * \brief  Compute the approximate error bound for the computed singular
//	 *  values.
//	 * \note
//	 *  For the 2-norm, \f$\Sigma(1,1) = \operatorname{norm}(A)\f$.
//	 */
//	public: vector_type U_error() const
//	{
//		// Estimate reciprocal condition numbers for the singular vectors
//		vector_type rcond(n);
//		DDISNA('Left',M,N,s_,rcond)
//		// Compute the error estimates for the singular vectors
//		real_type s_err = s_error();
//		vector_type err(n);
//		for (size_type i = 0; i < n; ++i)
//		{
//			err(i) = s_err/rcond(i)
//		}
//
//		return err;
//	}

//	TODO: interesting but not urgent.
//	/**
//	 * \brief  Compute the approximate error bound for the computed singular
//	 *  values.
//	 * \note
//	 *  For the 2-norm, \f$\Sigma(1,1) = \operatorname{norm}(A)\f$.
//	 */
//	public: vector_type VH_error() const
//	{
//		// Estimate reciprocal condition numbers for the singular vectors
//		vector_type rcond(n);
//		DDISNA('Right',M,N,s_,rcond)
//		// Compute the error estimates for the singular vectors
//		real_type s_err = s_error();
//		vector_type err(n);
//		for (size_type i = 0; i < n; ++i)
//		{
//			err(i) = s_err/rcond(i)
//		}
//
//		return err;
//	}

	/// Tell if the current SVD is in full or economy mode.
	bool full_;
	/// The number of rows of the original decomposed matrix.
	size_type m_;
	/// The number of columns of the original decomposed matrix.
	size_type n_;
	/// The minimum between the number of rows and columns of the original
	/// decomposed matrix.
	size_type k_;
	/// The vector of singular values.
	private: vector_type s_;
	/// The matrix containing the left singular vectors.
	private: matrix_type U_;
	/// The matrix containing the right singular vectors.
	private: matrix_type VH_;
};

/// Compute the singular values of matrix \a A.
template <typename MatrixExprT>
vector<
	typename type_traits<
		typename matrix_traits<MatrixExprT>::value_type
	>::real_type
> svd_values(matrix_expression<MatrixExprT> const& A)
{
	typedef typename matrix_traits<MatrixExprT>::orientation_category orientation_category;
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;
	typedef typename type_traits<value_type>::real_type real_type;
	typedef vector<real_type> vector_type;
	typedef typename layout_type<MatrixExprT>::type layout_type;
	typedef matrix<value_type, layout_type> work_matrix_type;

	vector_type s;
	work_matrix_type dummy_U;
	work_matrix_type dummy_VT;

	detail::svd_impl(A(), s, false, false, dummy_U, false, false, dummy_VT, orientation_category());

	return s;
}

/// Compute the singular value decomposition of matrix \a A.
template <typename MatrixExprT>
svd_decomposition<typename matrix_traits<MatrixExprT>::value_type> svd_decompose(matrix_expression<MatrixExprT> const& A, bool full = true)
{
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;

	return svd_decomposition<value_type>(A, full);
}

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

#endif // BOOST_NUMERIC_UBLASX_OPERATION_SVD_HPP


#include "Vector3d.hpp"
#include "utils.hpp"
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

namespace fragdock {
  void getNormalizeRot(const Vector3d& v1, const Vector3d& _v2, fltype& theta, fltype& phi, fltype& psi) {
    theta = 0;
    phi = 0;
    psi = 0;
    Vector3d vec1 = v1;
    Vector3d vec2 = v1.cross(_v2);
    psi = Vector3d(0, 1, 0).getAngle(Vector3d(vec2.x, vec2.y, 0));
    phi = Vector3d(0, 0, 1).getAngle(Vector3d(0, sqrt(vec2.x * vec2.x + vec2.y * vec2.y), vec2.z));
    if (vec2.x < 0) psi = -psi;
    vec1.rotate(0, phi, psi);
    theta = -Vector3d(1, 0, 0).getAngle(vec1);
    if (vec1.y < 0) theta = -theta;
    // rotate(theta, phi, psi);
  }

  Vector3d getRot(const Vector3d& v1, const Vector3d& _v2) {
    fltype theta, phi, psi;
    getNormalizeRot(v1, _v2, theta, phi, psi);
    return Vector3d(-psi, -phi, -theta);
  }

  void Vector3d::axisRotate(const Vector3d &axis, double th) {
    Vector3d n = axis/axis.abs();
    fltype B = cos(th), C = sin(th);
    fltype A = 1-B;
    fltype ox = x, oy = y, oz = z;
    x = (n.x*n.x*A +     B)*ox + (n.x*n.y*A - n.z*C)*oy + (n.z*n.x*A + n.y*C)*oz;
    y = (n.x*n.y*A + n.z*C)*ox + (n.y*n.y*A +     B)*oy + (n.y*n.z*A - n.x*C)*oz;
    z = (n.z*n.x*A - n.y*C)*ox + (n.y*n.z*A + n.x*C)*oy + (n.z*n.z*A +     B)*oz;
  }

  // zxz rotation
  void Vector3d::rotate(fltype theta, fltype phi, fltype psi) {
    const fltype cosT = cos(theta), sinT = sin(theta), cosH = cos(phi), sinH = sin(phi), cosS = cos(psi), sinS = sin(psi);
    fltype ox = x, oy = y, oz = z;

    x = (cosT*cosS  - sinT*cosH*sinS) * ox + (-cosT*sinS - sinT*cosH*cosS) * oy + (sinT*sinH ) * oz;
    y = (sinT*cosS  + cosT*cosH*sinS) * ox + (-sinT*sinS + cosT*cosH*cosS) * oy + (-cosT*sinH) * oz;
    z = (sinH*sinS                  ) * ox + (sinH*cosS                  ) * oy + (cosH      ) * oz;
  }
  void Vector3d::rotate(const Vector3d& v) {
    rotate(v.x, v.y, v.z);
  }

  const std::vector<Vector3d> makeRotations60() {
    const fltype phi = (1.0 + sqrt(5.0)) / 2.0;
    const fltype pi = acos(-1.0);

    fragdock::Vector3d poleA(0, phi*phi*phi, phi*phi);
    fragdock::Vector3d poleB(0, 1.0, phi*phi);
    poleA = poleA.unit();
    poleB = poleB.unit();
    fltype p = poleA.getAngle(poleB);
    poleA = Vector3d(1, 0, 0);
    poleB = Vector3d(cos(p), sin(p), 0);

    const int ORDER_NUM = 12;
    const int order[ORDER_NUM] = {0, 0, 1, 1, 1, 2, 1, 2, 2, 2, 3, -1};

    std::vector<Vector3d> rots;
    for(int ord_id = 0; ord_id < ORDER_NUM; ord_id++) {
      int o = order[ord_id];
      for(int i = 0; i < 5; i++) {
        // fltype theta = getAxisAngle(poleA, poleB);
        rots.push_back(getRot(poleA, poleB));
        poleB.axisRotate(poleA, 2.0*pi/5.0);
      }
      poleB.axisRotate(poleA, 2.0*o*pi/5.0);
      poleA.axisRotate(poleB, 4.0*pi/3.0);
    }
    return rots;
  }

  const std::vector<Vector3d> readRotations(const std::string& filename) {
    std::ifstream ifs(filename);
    if (ifs.fail()){
      std::cerr << "opening grid file failed:" << filename << std::endl;
      abort();
    }
    std::vector<Vector3d> rots;
    while (!ifs.eof()) {
      std::string str;
      std::getline(ifs, str);
      std::vector<std::string> vals;
      boost::algorithm::split(vals, str, boost::algorithm::is_any_of(","));
      if ((int)vals.size() < 3) continue;
      boost::algorithm::trim(vals[0]);
      boost::algorithm::trim(vals[1]);
      boost::algorithm::trim(vals[2]);
      rots.push_back(Vector3d(boost::lexical_cast<fltype>(vals[0]),
                              boost::lexical_cast<fltype>(vals[1]),
                              boost::lexical_cast<fltype>(vals[2])));
    }
    return rots;
  }
}


#include "caffe/util/prediction.hpp"

#include <vector>
#include <Eigen/Core>

#include <opencv2/core/core.hpp>

// #include <cv.h>
// #include <highgui.h>

namespace caffe {

template <typename Dtype>
Grid<Dtype> rotate_voxels_prediction(const Grid<Dtype> &vox,
								   const Eigen::Matrix<Dtype,4,4> &model1,
								   const Eigen::Matrix<Dtype,4,4> &view1,
								   const Eigen::Matrix<Dtype,4,4> &model2,
								   const Eigen::Matrix<Dtype,4,4> &view2,
								   const Eigen::Matrix<Dtype,4,4> &proj)
{
int size = vox[0].rows();
//std::cout<<size<<std::endl;
	Grid<Dtype> rot_vox(size);
	for(int c = 0; c < size; c++)
	{
		rot_vox[c] = Slice<Dtype>(size, size);
		for(int i = 0; i <size; i++)
		{
			for(int j = 0; j < size; j++)
			{
				rot_vox[c](i,j) = rotated_proba_value<Dtype>(vox, model1, view1,model2, view2, proj, c, i, j);
			}
		}
	}
	return rot_vox;
}

	template <typename Dtype>
Dtype rotated_proba_value(const Grid<Dtype> &vox,
						  const Eigen::Matrix<Dtype,4,4> &model1,
						  const Eigen::Matrix<Dtype,4,4> &view1,
						  const Eigen::Matrix<Dtype,4,4> &model2,
						  const Eigen::Matrix<Dtype,4,4> &view2,
						  const Eigen::Matrix<Dtype,4,4> &proj,
						  Dtype z,Dtype x, Dtype y)
{
	int size = vox[0].rows();
	//pass coord in the unit cube and rotate it
	// std::cout<<x<<" "<<y<<" "<<z<<" -> ";
	Dtype depth = z_to_depth(z, size, proj);;
	// std::cout<<depth<<std::endl;
	Eigen::Matrix<Dtype,3,1> rotated =
		rotate_coords<Dtype>((y+0.5)/(size), 1-(x+0.5)/(size),
					  depth, model1, view1,model2, view2, proj);
	// std::cout<<rotated<<std::endl;

	int new_i = (int)std::floor((1-rotated.y())*(size));
	int new_j = (int)std::floor(rotated.x()*(size));
	int new_z = depth_to_z(rotated.z(), size, proj);
	new_z = std::min<int>(size - 1,std::max<int>(0,new_z));
	// std::cout<<new_i<<" "<<new_j<<" "<<new_z<<std::endl;
	Dtype v0 = vox[new_z](new_i, new_j);
	return v0;
}

	template <typename Dtype>
	Dtype z_to_depth(int z, int size, const Eigen::Matrix<Dtype,4,4> &proj)
{
	Dtype zf=z*0.8+(size/16);
	Dtype depth = -(zf+0.5)/size*5.5-2.5;
	depth = -proj(2,3)/depth-proj(2,2);
	depth=depth/2+0.5;
	return depth;
}

	template <typename Dtype>
int depth_to_z(Dtype depth, int size, const Eigen::Matrix<Dtype,4,4> &proj)
{
	depth = depth * 2 - 1;
	depth = -proj(2,3)/(depth+proj(2,2));
	Dtype z = (-(depth+2.5)/5.5)*size-0.5;
	return (int)std::round((z-(size/16))/0.8);
}

	template <typename Dtype>
Eigen::Matrix<Dtype,3,1> rotate_coords(Dtype x, Dtype y, Dtype z,
						  const Eigen::Matrix<Dtype,4,4> &model1,
						  const Eigen::Matrix<Dtype,4,4> &view1,
						  const Eigen::Matrix<Dtype,4,4> &model2,
						  const Eigen::Matrix<Dtype,4,4> &view2,
							  const Eigen::Matrix<Dtype,4,4> &proj)
{
	Eigen::Matrix<Dtype,4,1> NDC(x*2-1, y*2-1, z*2-1, 1.0);
	Eigen::Matrix<Dtype,4,1> position = (proj*view2*model2).inverse()*NDC;
	//rotate
	Eigen::Matrix<Dtype,4,1> coords = proj * view1 * model1 * position;
	coords /= coords.w();
	//goes back into (0,1)
	coords = coords.array()/2+0.5;
	//make sure
	coords.x() = std::min<Dtype>(0.99,std::max<Dtype>(0.01,coords.x()));
	coords.y() = std::min<Dtype>(0.99,std::max<Dtype>(0.01,coords.y()));
	coords.z() = std::min<Dtype>(0.99,std::max<Dtype>(0.01,coords.z()));
	return coords.head(3);
}

float interpolate( float val, float y0, float x0, float y1, float x1 ) {
    return (val-x0)*(y1-y0)/(x1-x0) + y0;
}

float base( float val ) {
    if ( val <= -0.75 ) return 0;
    else if ( val <= -0.25 ) return interpolate( val, 0.0, -0.75, 1.0, -0.25 );
    else if ( val <= 0.25 ) return 1.0;
    else if ( val <= 0.75 ) return interpolate( val, 1.0, 0.25, 0.0, 0.75 );
    else return 0.0;
}

cv::Vec3b jetColor(float gray)
{
	return cv::Vec3b(base( gray + 0.5 )*255,base( gray )*255, base( gray - 0.5 )*255);
}

	template <typename Dtype>
cv::Mat iso_surface(const  Grid<Dtype> &vox,
					float threshold)
{
	int size = vox[0].rows();
	cv::Mat img(size, size,CV_8UC3);
	for(int i = 0; i <size; i++)
	{
		for(int j = 0; j < size; j++)
		{
			int c_depth = -1;
			for(int c = 0; c < size; c++)
			{
float val_depth= vox[c](i,j);
				if (val_depth > threshold)
				{
					c_depth = c;
					break;
				}
			}
			float depth = (c_depth+0.5)/(size);
			cv::Vec3b color = jetColor(depth*2-1);
			img.at<cv::Vec3b>(i, j)=color;
		}
	}

	return img;
}

//takes only blobs and do the work (go to eigen and rotate) AVOID COPYING!
	template <typename Dtype>
	void rotate_blobs(const Blob<Dtype> * pred,
					  const Dtype* model1,
					  const Dtype* view_mat1,
					  const Dtype* model2,
					  const Dtype* view_mat2,
					  const Dtype* proj_mat,
					  Dtype * output)  //WARNING a voir
	{
		//go from pred to a Grid<Dtype>
		int output_channels=pred->channels();
		int output_width = pred->width();
		int output_height = pred->height();
		int size = output_width;

		Grid<Dtype> grid(size);
		const Dtype* output_data=pred->cpu_data();
		//chenger methode selon taille du blob  (unfold ou 3d)
		if (output_height == size*size) //if pred from a net, skip first classif layer
			output_data+=output_width * output_height;

		for(int c = 0; c < size; c++)
		{
			Slice<Dtype> channel(size, size);
			std::memcpy(channel.data(), output_data, size*size* sizeof(Dtype));
			output_data += size*size;
			grid[c] = channel;
		}
		//transform viewpoint, view_mat, proj_mat into eigen matrices
		Eigen::Map<const Eigen::Matrix<Dtype,4,4> > view1(view_mat1);
		Eigen::Map<const Eigen::Matrix<Dtype,4,4> > view2(view_mat2);
		Eigen::Map<const Eigen::Matrix<Dtype,4,4> > proj(proj_mat);
		Eigen::Map<const Eigen::Matrix<Dtype,4,4> > mv1(model1);
		Eigen::Map<const Eigen::Matrix<Dtype,4,4> > mv2(model2);
		// std::cout<<view1<<std::endl<<std::endl;
		// std::cout<<view2<<std::endl<<std::endl;
		// std::cout<<mv1<<std::endl<<std::endl;
		// std::cout<<mv2<<std::endl<<std::endl;
		//compute model
		// Eigen::Matrix<Dtype,4,4> model;
		// model = new_view*model_old.inverse();
		// model=model.inverse();

		//rotate
		Grid<Dtype> gt12 = rotate_voxels_prediction<Dtype>(grid, mv1, view1, mv2, view2, proj);
		// std::cout<<"rotate ok"<<std::endl;
		// cv::Mat pred1 = iso_surface(grid, 0.3);
		// cv::namedWindow( "pred1", CV_WINDOW_NORMAL );
		// cv::imshow("pred1",pred1);
		// cv::Mat pred2 = iso_surface(gt12, 0.3);
		// cv::namedWindow( "pred2", CV_WINDOW_NORMAL );
		// cv::imshow("pred2",pred2);
		// cv::waitKey(0);
		//put into output

		for(int c = 0; c < size; c++)
		{
			std::memcpy(output, gt12[c].data() ,size * size * sizeof(Dtype));
			output += size*size;
		}
	}

	template <typename Dtype>
	Grid<Dtype> unpack_pred_in_image( cv::Mat &image, int grid_rows, int grid_cols)
{
	//assert(grid_rows*grid_cols == grid.size()/4);
	int nrows = image.rows/grid_rows;
	int ncols = image.cols/grid_cols;
	int nchannels = nrows/4;//std::min<int>(grid.size(), grid_rows*grid_cols);
	//cv::Mat img(nrows*grid_rows, ncols*grid_cols,CV_8UC4);
	//std::cout<<"start to unpack\n";
	std::vector<cv::Mat> imgs;
	int from_to[4*2] = {0,2,1,1,2,0,3,3};
	std::vector<cv::Mat> img;
	img.push_back(image);
	// std::cout<<img[0].type()<<std::endl;
	//std::cout<<img[0].channels()<<std::endl;
	for(int i = 0; i<4; i++)
	{
		imgs.push_back(cv::Mat(image.size(),CV_8UC1));
	}
	cv::mixChannels(img,imgs,from_to,4);
	// for(int i = 0; i<4; i++)
	// 	imShow::show(imgs[i],"img"+std::to_string(i));
	// imShow::wait();
	// cv::namedWindow("grid");

	Grid<Dtype> grid(nrows);
	for(int c = 0; c<nchannels; c++)
	{
		int i_cell = c/grid_cols*nrows;
		int j_cell = c%grid_cols*ncols;
		//std::cout<<"c = "<<c<<", icell = "<<i_cell<<", jcell = "<<j_cell<<std::endl;
		for(int i = 0; i<4; i++)
		{
			int channel = c*4 + i;
			//std::cout<<"copying channel "<<channel<<std::endl;
			cv::Mat part = imgs[i](cv::Rect(j_cell,i_cell,nrows,ncols)),partF;
			part.convertTo(partF, CV_type<Dtype>(), 1.0/255);

			Eigen::Map<Slice<Dtype> > chan(reinterpret_cast<Dtype*>(partF.data),nrows,ncols);
			grid[channel] =chan;

		}
	}
	// cv::imshow("grid",pred_to_grid(grid,8,8));
	// cv::waitKey(0);
	return grid;
}

template <typename Dtype>
int CV_type()
{
	return 0;
}
	template <>
	int CV_type<float>()
	{
		return CV_32F;
	}
	template <>
	int CV_type<double>()
	{
		return CV_64F;
	}

	template 	Grid<float> unpack_pred_in_image( cv::Mat &image, int grid_rows, int grid_cols);
	template 	Grid<double> unpack_pred_in_image( cv::Mat &image, int grid_rows, int grid_cols);

	template void rotate_blobs(const Blob<double> * pred, const double* model1, const double* model2, const double* view_mat1, const double* view_mat2,  const double* proj_mat, double * output);
	template void rotate_blobs(const Blob<float> * pred, const float* model1, const float* model2, const float* view_mat1, const float* view_mat2,  const float* proj_mat, float * output);
}