//
// Project: Delaunay
// File: Delaunay.hpp
//
// Copyright (c) 2021 Miika 'Lehdari' Lehtimäki
// You may use, distribute and modify this code under the terms
// of the licence specified in file LICENSE which is distributed
// with this source code package.
//

#ifndef DELAUNAY_DELAUNAY_HPP
#define DELAUNAY_DELAUNAY_HPP

#include <vector>
#include <cstdint>

#define DELAUNAY_BACKEND_EIGEN 1
//#define DELAUNAY_BACKEND_OTHER 2 // add more enumerations for more supported backends

// definitions for Eigen backend
#if DELAUNAY_BACKEND == DELAUNAY_BACKEND_EIGEN
    #include <Eigen/Dense>

    #define DELAUNAY_VEC Eigen::Matrix<T_Scalar, 2, 1>
    #define DELAUNAY_VEC_ACCESS(V, D) V(D)
    #define DELAUNAY_DETERMINANT
#endif

// Check for macro definitions
#ifndef DELAUNAY_VEC
    #error "DELAUNAY_VEC not defined"
#endif
#ifndef DELAUNAY_VEC_ACCESS
    #error "DELAUNAY_VEC_ACCESS not defined"
#endif
#ifndef DELAUNAY_DETERMINANT
    #error "DELAUNAY_DETERMINANT not defined"
#endif

#ifdef DELAUNAY_INLINE
    #error "DELAUNAY_INLINE already defined"
#endif
#define DELAUNAY_INLINE inline __attribute__((always_inline))

namespace delaunay {

namespace {

struct Triangle {
    int neighbours[3]; // indices of neighbouring triangles
    int vertices[3]; // indices of corner vertices

    Triangle(int t1, int t2, int t3, int v1, int v2, int v3) :
        neighbours  {t1, t2, t3},
        vertices    {v1, v2, v3}
    {}
};

template <typename T_Scalar>
DELAUNAY_INLINE bool ccw(
    const DELAUNAY_VEC& a,
    const DELAUNAY_VEC& b,
    const DELAUNAY_VEC& c)
{
#if DELAUNAY_BACKEND == DELAUNAY_BACKEND_EIGEN
    Eigen::Matrix<T_Scalar, 3, 3>   m;
    m <<
        a.transpose(),  1.0,
        b.transpose(),  1.0,
        c.transpose(),  1.0;
    return m.determinant() > 0.0;
#else
    return DELAUNAY_DETERMINANT(
        DELAUNAY_VEC_ACCESS(a,0), DELAUNAY_VEC_ACCESS(a,1), (T_Scalar)1.0,
        DELAUNAY_VEC_ACCESS(b,0), DELAUNAY_VEC_ACCESS(b,1), (T_Scalar)1.0,
        DELAUNAY_VEC_ACCESS(c,0), DELAUNAY_VEC_ACCESS(c,1), (T_Scalar)1.0) > 0.0;
#endif
}

// is d inside of circumcircle of abc?
template <typename T_Scalar>
DELAUNAY_INLINE bool inCircle(
    const DELAUNAY_VEC& a,
    const DELAUNAY_VEC& b,
    const DELAUNAY_VEC& c,
    const DELAUNAY_VEC& d)
{
    T_Scalar dx2 = DELAUNAY_VEC_ACCESS(d,0)*DELAUNAY_VEC_ACCESS(d,0);
    T_Scalar dy2 = DELAUNAY_VEC_ACCESS(d,1)*DELAUNAY_VEC_ACCESS(d,1);
#if DELAUNAY_BACKEND == DELAUNAY_BACKEND_EIGEN
    Eigen::Matrix<T_Scalar, 3, 3>   m;
    m <<
        DELAUNAY_VEC_ACCESS(a,0)-DELAUNAY_VEC_ACCESS(d,0),
        DELAUNAY_VEC_ACCESS(a,1)-DELAUNAY_VEC_ACCESS(d,1),
        (DELAUNAY_VEC_ACCESS(a,0)*DELAUNAY_VEC_ACCESS(a,0)-dx2)+(DELAUNAY_VEC_ACCESS(a,1)*DELAUNAY_VEC_ACCESS(a,1)-dy2),
        DELAUNAY_VEC_ACCESS(b,0)-DELAUNAY_VEC_ACCESS(d,0),
        DELAUNAY_VEC_ACCESS(b,1)-DELAUNAY_VEC_ACCESS(d,1),
        (DELAUNAY_VEC_ACCESS(b,0)*DELAUNAY_VEC_ACCESS(b,0)-dx2)+(DELAUNAY_VEC_ACCESS(b,1)*DELAUNAY_VEC_ACCESS(b,1)-dy2),
        DELAUNAY_VEC_ACCESS(c,0)-DELAUNAY_VEC_ACCESS(d,0),
        DELAUNAY_VEC_ACCESS(c,1)-DELAUNAY_VEC_ACCESS(d,1),
        (DELAUNAY_VEC_ACCESS(c,0)*DELAUNAY_VEC_ACCESS(c,0)-dx2)+(DELAUNAY_VEC_ACCESS(c,1)*DELAUNAY_VEC_ACCESS(c,1)-dy2);
    return m.determinant() > 0.0;
#else
    return DELAUNAY_DETERMINANT(
        DELAUNAY_VEC_ACCESS(a,0)-DELAUNAY_VEC_ACCESS(d,0),
        DELAUNAY_VEC_ACCESS(a,1)-DELAUNAY_VEC_ACCESS(d,1),
        (DELAUNAY_VEC_ACCESS(a,0)*DELAUNAY_VEC_ACCESS(a,0)-dx2)+(DELAUNAY_VEC_ACCESS(a,1)*DELAUNAY_VEC_ACCESS(a,1)-dy2),
        DELAUNAY_VEC_ACCESS(b,0)-DELAUNAY_VEC_ACCESS(d,0),
        DELAUNAY_VEC_ACCESS(b,1)-DELAUNAY_VEC_ACCESS(d,1),
        (DELAUNAY_VEC_ACCESS(b,0)*DELAUNAY_VEC_ACCESS(b,0)-dx2)+(DELAUNAY_VEC_ACCESS(b,1)*DELAUNAY_VEC_ACCESS(b,1)-dy2),
        DELAUNAY_VEC_ACCESS(c,0)-DELAUNAY_VEC_ACCESS(d,0),
        DELAUNAY_VEC_ACCESS(c,1)-DELAUNAY_VEC_ACCESS(d,1),
        (DELAUNAY_VEC_ACCESS(c,0)*DELAUNAY_VEC_ACCESS(c,0)-dx2)+(DELAUNAY_VEC_ACCESS(c,1)*DELAUNAY_VEC_ACCESS(c,1)-dy2))
        > 0.0;
#endif
}

// initial primitive construction functions for edges and triangles
template <template <typename, typename> class T_Vector, typename T_Scalar, typename T_Allocator>
DELAUNAY_INLINE void createEdge(const T_Vector<DELAUNAY_VEC, T_Allocator>& points,
    std::vector<Triangle>& triangles, int v1, int v2, int& firstTriangle, int& lastTriangle)
{
    int s = triangles.size();
    triangles.emplace_back(s+1, s+1, s+1, v1, v2, -1); // edge is presented by 2 ghost triangles (3rd vertex -1)
    triangles.emplace_back(s, s, s, v2, v1, -1);

    firstTriangle = s;
    lastTriangle = s;
}

template <template <typename, typename> class T_Vector, typename T_Scalar, typename T_Allocator>
DELAUNAY_INLINE void createTriangle(
    T_Vector<DELAUNAY_VEC, T_Allocator>& points,
    std::vector<Triangle>& triangles, int v1, int v2, int v3, int& firstTriangle, int& lastTriangle)
{
    int s = triangles.size();
    if (!ccw(points[v1], points[v2], points[v3])) {
        std::swap(v2, v3);
        lastTriangle = s+2;
    }
    else
        lastTriangle = s+3;
    triangles.emplace_back(s+1, s+2, s+3, v1, v2, v3);
    triangles.emplace_back(s, s+3, s+2, v2, v1, -1);
    triangles.emplace_back(s, s+1, s+3, v3, v2, -1);
    triangles.emplace_back(s, s+2, s+1, v1, v3, -1);

    firstTriangle = s+3;
}

// update neighbour of a triangle
// t: triangle neighbour of which is to be updated
// current: current neighbour triangle id to be changed
// updated: triangle id of the new neighbour
DELAUNAY_INLINE void updateNeighbour(Triangle& t, int current, int updated)
{
    if (t.neighbours[0] == current) {
        t.neighbours[0] = updated;
        return;
    }
    if (t.neighbours[1] == current) {
        t.neighbours[1] = updated;
        return;
    }
    if (t.neighbours[2] == current) {
        t.neighbours[2] = updated;
        return;
    }
}

// rotate triangle indices so that vertex id of -1 is at index 2, no-op for non-ghost or correct ghost triangles
DELAUNAY_INLINE void correctGhost(Triangle& t)
{
    if (t.vertices[0] == -1) {
        int tempVertex = t.vertices[2];
        int tempNeighbour = t.neighbours[2];
        t.vertices[2] = t.vertices[0];
        t.neighbours[2] = t.neighbours[0];
        t.vertices[0] = t.vertices[1];
        t.neighbours[0] = t.neighbours[1];
        t.vertices[1] = tempVertex;
        t.neighbours[1] = tempNeighbour;
    }
    else if (t.vertices[1] == -1) {
        int tempVertex = t.vertices[2];
        int tempNeighbour = t.neighbours[2];
        t.vertices[2] = t.vertices[1];
        t.neighbours[2] = t.neighbours[1];
        t.vertices[1] = t.vertices[0];
        t.neighbours[1] = t.neighbours[0];
        t.vertices[0] = tempVertex;
        t.neighbours[0] = tempNeighbour;
    }
}

// flip an edge between two triangles
inline void flip(std::vector<Triangle>& triangles, int t1, int t2)
{
    Triangle& triangle1 = triangles[t1];
    Triangle& triangle2 = triangles[t2];

    int t1EdgeId = 0; // id of the t1 edge connecting to t2
    if (triangle1.neighbours[1] == t2)
        t1EdgeId = 1;
    else if (triangle1.neighbours[2] == t2)
        t1EdgeId = 2;

    int t2EdgeId = 0; // id of the t2 edge connecting to t1
    if (triangle2.neighbours[1] == t1)
        t2EdgeId = 1;
    else if (triangle2.neighbours[2] == t1)
        t2EdgeId = 2;

    Triangle triangle1temp = triangle1;
    Triangle triangle2temp = triangle2;

    // update neighbour indexing
    updateNeighbour(triangles[triangle1temp.neighbours[(t1EdgeId+1)%3]], t1, t2);
    updateNeighbour(triangles[triangle2temp.neighbours[(t2EdgeId+1)%3]], t2, t1);

    // update vertex and neighbour indexing of t1 and t2
    triangle1.vertices[(t1EdgeId+1)%3] = triangle2temp.vertices[(t2EdgeId+2)%3];
    triangle1.neighbours[t1EdgeId] = triangle2temp.neighbours[(t2EdgeId+1)%3];
    triangle1.neighbours[(t1EdgeId+1)%3] = t2;

    triangle2.vertices[(t2EdgeId+1)%3] = triangle1temp.vertices[(t1EdgeId+2)%3];
    triangle2.neighbours[t2EdgeId] = triangle1temp.neighbours[(t1EdgeId+1)%3];
    triangle2.neighbours[(t2EdgeId+1)%3] = t1;

    // correct ghost indexing in case either of the triangles is a ghost
    correctGhost(triangle1);
    correctGhost(triangle2);
}

// get vertex of triangle t opposing neighbour neighbour
DELAUNAY_INLINE int getOpposingVertex(Triangle& t, int neighbour)
{
    if (t.neighbours[0] == neighbour)
        return t.vertices[2];
    if (t.neighbours[1] == neighbour)
        return t.vertices[0];
    if (t.neighbours[2] == neighbour)
        return t.vertices[1];

    // should never be reached
    return -1; // neighbour not neighbour of t
}

// get the triangle ID of right neighbour w.r.t to a vertex id
DELAUNAY_INLINE int getRightNeighbour(Triangle& t, int v)
{
    if (t.vertices[0] == v)
        return t.neighbours[2];
    if (t.vertices[1] == v)
        return t.neighbours[0];
    if (t.vertices[2] == v)
        return t.neighbours[1];

    // should never be reached
    return -1; // v not a vertex of t
}

// get the triangle ID of left neighbour w.r.t to a vertex id
DELAUNAY_INLINE int getLeftNeighbour(Triangle& t, int v)
{
    if (t.vertices[0] == v)
        return t.neighbours[0];
    if (t.vertices[1] == v)
        return t.neighbours[1];
    if (t.vertices[2] == v)
        return t.neighbours[2];

    // should never be reached
    return -1; // v not a vertex of t
}

// ghost triangle adding functions required in ends of a "seam"
DELAUNAY_INLINE int addGhostTriangle(std::vector<Triangle>& triangles, int v1, int v2, int n2, int n3)
{
    int s = triangles.size();
    triangles.emplace_back(-1, n2, n3, v1, v2, -1);
    triangles[n2].neighbours[2] = s;
    triangles[n3].neighbours[1] = s;
    return s;
}

DELAUNAY_INLINE int addGhostTriangle(std::vector<Triangle>& triangles, int v1, int v2, int n1, int n2, int n3)
{
    int s = triangles.size();
    triangles.emplace_back(n1, n2, n3, v1, v2, -1);
    triangles[n2].neighbours[2] = s;
    triangles[n3].neighbours[1] = s;
    return s;
}

template <template <typename, typename> class T_Vector, typename T_Scalar, typename T_Allocator>
void merge(T_Vector<DELAUNAY_VEC, T_Allocator>& points,
    std::vector<Triangle>& triangles, int& firstLeft, int lastLeft, int firstRight, int& lastRight)
{
    // keep track of end vertices in case the end triangles change
    int firstVertex = triangles[firstLeft].vertices[0];
    int lastVertex = triangles[lastRight].vertices[1];

    // find the lower common tangent
    int iterId = 0;
    while (true) {
        int nOps = 0;
        int leftNext = triangles[lastLeft].neighbours[1]; // next ghost triangle on the left mesh boundary
        int rightNext = triangles[firstRight].neighbours[2]; // next ghost triangle on the right mesh boundary
        if (ccw(
            points[triangles[leftNext].vertices[0]],
            points[triangles[leftNext].vertices[1]],
            points[triangles[firstRight].vertices[0]])) {
            lastLeft = leftNext;
            ++nOps;
        }
        if (ccw(
            points[triangles[rightNext].vertices[0]],
            points[triangles[rightNext].vertices[1]],
            points[triangles[lastLeft].vertices[1]])) {
            firstRight = rightNext;
            ++nOps;
        }
        if (nOps == 0) {
            // opposing triangles are right of the edge of both ghost triangles,
            // lower common tangent has been found
            break;
        }
        if (++iterId > points.size()) // maximum number of iterations reached, sign of numerical instability
            break;
    }

    bool leftValid = true;
    bool rightValid = true;
    int baseLeftVertex = triangles[lastLeft].vertices[1];
    int baseRightVertex = triangles[firstRight].vertices[0];

    // add new ghost triangle to the beginning of the seam
    int firstGhost = addGhostTriangle(triangles, baseRightVertex, baseLeftVertex,
        triangles[lastLeft].neighbours[1], triangles[firstRight].neighbours[2]);
    // check if the ghost overrides either of the end triangles - in that case update their indices
    if (triangles[firstGhost].vertices[0] == firstVertex)
        firstLeft = firstGhost;
    if (triangles[firstGhost].vertices[1] == lastVertex)
        lastRight = firstGhost;

    int lastConnectedSide = -1; // required for adding the end ghost, 0: left, 1: right
    int lastConnectedTriangle = -1;

    // merge loop
    iterId = 0;
    while (leftValid || rightValid) {
        auto& baseLeft = points[baseLeftVertex];
        auto& baseRight = points[baseRightVertex];

        // find connective candidates for both sides, delete non-delaunay edges by flipping
        int leftCand = triangles[lastLeft].vertices[0];
        leftValid = ccw(baseLeft, baseRight, points[leftCand]);
        if (leftValid) {
            int leftNeighbour = getRightNeighbour(triangles[lastLeft], baseLeftVertex);
            int leftNeighbourCand = getOpposingVertex(triangles[leftNeighbour], lastLeft);
            while (leftNeighbourCand != -1 &&
                inCircle(baseLeft, baseRight, points[leftCand], points[leftNeighbourCand])) {
                flip(triangles, lastLeft, leftNeighbour);
                lastLeft = leftNeighbour;
                leftCand = leftNeighbourCand;
                leftNeighbour = getRightNeighbour(triangles[lastLeft], baseLeftVertex);
                leftNeighbourCand = getOpposingVertex(triangles[leftNeighbour], lastLeft);
            }
        }

        int rightCand = triangles[firstRight].vertices[1];
        rightValid = ccw(baseLeft, baseRight, points[rightCand]);
        if (rightValid) {
            int rightNeighbour = getLeftNeighbour(triangles[firstRight], baseRightVertex);
            int rightNeighbourCand = getOpposingVertex(triangles[rightNeighbour], firstRight);
            while (rightNeighbourCand != -1 &&
                inCircle(baseLeft, baseRight, points[rightCand], points[rightNeighbourCand])) {
                flip(triangles, firstRight, rightNeighbour);
                rightCand = rightNeighbourCand;
                rightNeighbour = getLeftNeighbour(triangles[firstRight], baseRightVertex);
                rightNeighbourCand = getOpposingVertex(triangles[rightNeighbour], firstRight);
            }
        }

        if (!leftValid && !rightValid)
            break;

        if (!leftValid || (rightValid && inCircle(points[leftCand], baseLeft, baseRight, points[rightCand]))) {
            int rightNext = triangles[firstRight].neighbours[1];

            if (firstGhost != -1) { // update first ghost
                triangles[firstGhost].neighbours[0] = firstRight;
                triangles[firstRight].neighbours[2] = firstGhost;
                firstGhost = -1;
            }
            else { // connect left side triangle in case it was previously added
                if (lastConnectedSide == 0) {
                    triangles[lastConnectedTriangle].neighbours[2] = firstRight;
                    triangles[firstRight].neighbours[2] = lastConnectedTriangle;
                }
            }

            // connect edge
            triangles[firstRight].vertices[2] = baseLeftVertex;
            lastConnectedTriangle = firstRight;
            firstRight = rightNext;
            baseRightVertex = rightCand;

            lastConnectedSide = 1;
        }
        else {
            int leftNext = triangles[lastLeft].neighbours[2];

            if (firstGhost != -1) { // update first ghost
                triangles[firstGhost].neighbours[0] = lastLeft;
                triangles[lastLeft].neighbours[1] = firstGhost;
                firstGhost = -1;
            }
            else { // connect right side triangle in case it was previously added
                if (lastConnectedSide == 1) {
                    triangles[lastConnectedTriangle].neighbours[1] = lastLeft;
                    triangles[lastLeft].neighbours[1] = lastConnectedTriangle;
                }
            }

            // connect edge
            triangles[lastLeft].vertices[2] = baseRightVertex;
            lastConnectedTriangle = lastLeft;
            lastLeft = leftNext;
            baseLeftVertex = leftCand;

            lastConnectedSide = 0;
        }

        if (++iterId > points.size()) // maximum number of iterations reached, sign of numerical instability
            break;
    }

    // add new ghost triangle to the end of the seam
    int lastGhost;
    if (lastConnectedSide == 0) {
        int lastConnect = triangles[lastLeft].neighbours[1];
        lastGhost = addGhostTriangle(triangles, baseLeftVertex, baseRightVertex, lastConnect, firstRight, lastLeft);
        triangles[lastConnect].neighbours[2] = lastGhost;
    }
    else {
        int lastConnect = triangles[firstRight].neighbours[2];
        lastGhost = addGhostTriangle(triangles, baseLeftVertex, baseRightVertex, lastConnect, firstRight, lastLeft);
        triangles[lastConnect].neighbours[1] = lastGhost;
    }

    // check if the ghost overrides either of the end triangles - in that case update their indices
    if (triangles[lastGhost].vertices[0] == firstVertex)
        firstLeft = lastGhost;
    if (triangles[lastGhost].vertices[1] == lastVertex)
        lastRight = lastGhost;
}

template <template <typename, typename> class T_Vector, typename T_Scalar, typename T_Allocator>
void construct(T_Vector<DELAUNAY_VEC, T_Allocator>& points,
    int begin, int end, std::vector<Triangle>& triangles, int& firstTriangle, int& lastTriangle)
{
    int d = end-begin;
    if (d == 2) { // initial primitives, edge
        createEdge(points, triangles, begin, begin+1, firstTriangle, lastTriangle);
    }
    else if (d == 3) { // initial primitives, triangle
        createTriangle(points, triangles, begin, begin+1, begin+2, firstTriangle, lastTriangle);
    }
    else {
        // recursive construction and merge
        auto half = begin+d/2;
        int firstLeft; int lastLeft;
        construct(points, begin, half, triangles, firstLeft, lastLeft);
        int firstRight; int lastRight;
        construct(points, half, end, triangles, firstRight, lastRight);

        merge(points, triangles, firstLeft, lastLeft, firstRight, lastRight);

        firstTriangle = firstLeft;
        lastTriangle = lastRight;
    }
}

} // namespace

template <template <typename, typename> class T_Vector, typename T_Scalar, typename T_Allocator>
std::vector<int32_t> triangulate(T_Vector<DELAUNAY_VEC, T_Allocator>& points)
{
    // sort points primarily along x-axis (and along y-axis in case of equal x)
    std::sort(points.begin(), points.end(), [](
        const DELAUNAY_VEC& a,
        const DELAUNAY_VEC& b){
        return DELAUNAY_VEC_ACCESS(a,0) == DELAUNAY_VEC_ACCESS(b,0) ?
            DELAUNAY_VEC_ACCESS(a,1) < DELAUNAY_VEC_ACCESS(b,1) :
            DELAUNAY_VEC_ACCESS(a,0) < DELAUNAY_VEC_ACCESS(b,0);
    });

    std::vector<Triangle> triangles;
    triangles.reserve(2*points.size());

    int firstTriangle, lastTriangle;
    construct(points, 0, points.size(), triangles, firstTriangle, lastTriangle);

    // list indices into output vector
    std::vector<int32_t> indices;
    indices.reserve(triangles.size()*3);
    for (auto& t : triangles) {
        indices.emplace_back(t.vertices[0]);
        indices.emplace_back(t.vertices[1]);
        indices.emplace_back(t.vertices[2]);
    }
    return indices;
}

} // namespace delaunay

// undefine macros
#undef DELAUNAY_VEC
#undef DELAUNAY_VEC_ACCESS
#undef DELAUNAY_DETERMINANT
#undef DELAUNAY_INLINE

#endif //DELAUNAY_DELAUNAY_HPP


/*
 * @Description: ceres residual block for map matching pose measurement
 * @Author: Ge Yao
 * @Date: 2020-11-29 15:47:49
 */
#ifndef LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_MAP_MATCHING_POSE_HPP_
#define LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_MAP_MATCHING_POSE_HPP_

#include <ceres/ceres.h>

#include <Eigen/Eigen>
#include <Eigen/Core>
#include <Eigen/Dense>

#include <sophus/so3.hpp>

#include "glog/logging.h"

namespace sliding_window {

class FactorPRVAGMapMatchingPose : public ceres::SizedCostFunction<6, 15> {
public:
	static const int INDEX_P = 0;
	static const int INDEX_R = 3;

  FactorPRVAGMapMatchingPose(void) {};

  void SetMeasurement(const Eigen::VectorXd &m) {
		m_ = m;
	}

  void SetInformation(const Eigen::MatrixXd &I) {
    I_ = I;
  }

  virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
    //
    // parse parameters:
    //
    // pose
    Eigen::Map<const Eigen::Vector3d>     pos(&parameters[0][INDEX_P]);
    Eigen::Map<const Eigen::Vector3d> log_ori(&parameters[0][INDEX_R]);
    const Sophus::SO3d                    ori = Sophus::SO3d::exp(log_ori);

    //
    // parse measurement:
    //
		const Eigen::Vector3d     &pos_prior = m_.block<3, 1>(INDEX_P, 0);
		const Eigen::Vector3d &log_ori_prior = m_.block<3, 1>(INDEX_R, 0);
    const Sophus::SO3d         ori_prior = Sophus::SO3d::exp(log_ori_prior);

    //
    // TODO: get square root of information matrix:
    //

    //
    // TODO: compute residual:
    //

    //
    // TODO: compute jacobians:
    //
    if ( jacobians ) {
      if ( jacobians[0] ) {
        // implement jacobian computing:
      }
    }

    //
    // TODO: correct residual by square root of information matrix:
    //

    return true;
  }

private:
  static Eigen::Matrix3d JacobianRInv(const Eigen::Vector3d &w) {
      Eigen::Matrix3d J_r_inv = Eigen::Matrix3d::Identity();

      double theta = w.norm();

      if ( theta > 1e-5 ) {
          Eigen::Vector3d k = w.normalized();
          Eigen::Matrix3d K = Sophus::SO3d::hat(k);

          J_r_inv = J_r_inv
                    + 0.5 * K
                    + (1.0 - (1.0 + std::cos(theta)) * theta / (2.0 * std::sin(theta))) * K * K;
      }

      return J_r_inv;
  }

  Eigen::VectorXd m_;
  Eigen::MatrixXd I_;
};

} // namespace sliding_window

#endif // LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_FACTOR_PRVAG_MAP_MATCHING_POSE_HPP_


#include <iostream>
#include <vector>
#include <unordered_set>
#include <set>
#include <chrono>
#include <queue>
#include <utility>
#include <new>
#include <algorithm>
#include <stack>
#include <random>
#include <cmath>
#include <fstream>
/*
used for plotting routes
#include <boost/tuple/tuple.hpp>
#include "gnuplot-iostream.h"
*/

// writes a vector of pairs to a given file name
void WriteToFile(std::string file_name, std::vector< std::pair<double, double> > array) {
  std::ofstream fout(file_name);
  for(int i = 0;i < array.size(); i++) {
    fout<<array[i].first<<','<<array[i].second<<'\n';
  }
}

// represents each solution/chromosome
struct State{
  std::vector< std::pair <double,double> > permutation;
  double cost;
  int operator ==(State b) const {
    return (permutation == b.permutation) && (cost == b.cost);
  }
};

// a hash function for random_states
struct StateHash {
    size_t operator()(const State &v) const {
        size_t seed = 0;
      	for(auto t = v.permutation.begin();t != v.permutation.end();t++) {
            size_t h1 = std::hash<double>()((*t).first);
            size_t h2 = std::hash<double>()((*t).second);
            seed ^= (h1 ^ (h2 << 1));
        }
        return seed;
    }
};

// a hash function for random_states used for unordered_set
struct PairHash {
    size_t operator()(const std::pair<double,double> &v) const {
        size_t seed = 0;
        size_t h1 = std::hash<double>()(v.first);
        size_t h2 = std::hash<double>()(v.second);
        seed = (h1 ^ (h2 << 1));
        return seed;
    }
};

// two states are compared by their path cost
bool CompareState(State first, State second) {
    return (first.cost < second.cost);
}
// forward declaring functions
double StateCost(State);

State BeamSearch(State);

State SimulatedAnnealing(State);

State GeneticAlgorithm(State);

//  lists to store best, worst and best of each generation
std::vector<std::pair<double,double>> best_of_generation;

std::vector<std::pair<double,double>> worst_of_generation;

std::vector<std::pair<double,double>> average_of_generation;

// probability bins for rank selection
std::vector< int > pool_probability_bins;

int main() {
  // used for plotting route
  //Gnuplot gp;
  // recieve city Coordinates
  std::vector<std::pair<double, double> > points;
	int n;
	double x,y;
	std::string method;
	State nodes;
	State result;
	std::cout<<"Enter X and Y Coordinates of Initial Node:\n";
  std::cin>>x>>y;
  nodes.permutation.push_back(std::make_pair(x,y));
	std::cout<<"Enter Number of Nodes other than the Initial Node:\n";
	std::cin>>n;
    std::cout<<"Enter X and Y Coordinates of the Nodes:\n";
    for(int i = 0; i < n;i++) {
		std::cin>>x>>y;
		nodes.permutation.push_back(std::make_pair(x,y));
	}
  // create initial state / permutation
	nodes.cost = StateCost(nodes);
  // run the selected method
	std::cout<<"Enter Beam, SA or GA\n";
	std::cin>>method;
  // start timer
  auto start = std::chrono::high_resolution_clock::now();
	if(method == "Beam") {
		result = BeamSearch(nodes);
	}
	if(method == "SA") {
		result = SimulatedAnnealing(nodes);
	}
	if(method == "GA") {
		result = GeneticAlgorithm(nodes);
	}
  // stop timer and print the time it took and the best route cost
	auto finish = std::chrono::high_resolution_clock::now();
	std::chrono::duration<double>elapsed = finish - start;
	std::cout<<"Duration: "<<elapsed.count()<<'\n'<<result.cost;

  // plot best route
  /*
  for(int i = 0;i<result.permutation.size();i++) {
        points.push_back(result.permutation[i]);
	}
	points.push_back(result.permutation[0]);
*/
  //gp << "set xrange [0:1000]\nset yrange [0:100]\n";
	//gp << "plot" << gp.file1d(average_of_generation) << "with lines title 'average',"<<std::endl;
	//gp << "plot" << gp.file1d(best_of_generation) << "with lines title 'best',"<<std::endl;
	//gp << "plot" << gp.file1d(worst_of_generation) << "with lines title 'worst',"<<std::endl;

  // write generation results to files
  //WriteToFile("average_of_generation1000k1000g.txt",average_of_generation);
  //WriteToFile("best_of_generation1000k1000g.txt",best_of_generation);
  //WriteToFile("worst_of_generation1000k1000g.txt",worst_of_generation);
  return 0;
}

int Factorial(int num) {
    int res = 1;
    for(int i = num;i>1;i--) {
        res*=i;
    }
    return res;
}

// computes cost of permutation
double StateCost (State nodes) {
    double cost = 0;
    for(int i = 0;i<nodes.permutation.size()-1;i++) {
        cost += sqrt(pow((nodes.permutation[i].first - nodes.permutation[i+1].first),2) + pow((nodes.permutation[i].second - nodes.permutation[i+1].second),2));
    }
    cost += sqrt(pow((nodes.permutation[0].first - nodes.permutation[nodes.permutation.size()-1].first),2) +
                 pow((nodes.permutation[0].second - nodes.permutation[nodes.permutation.size()-1].second),2));
    return cost;
}

// a binary search to find index/bin which the input belongs to
int FindIndex(int input) {
  int l = 0;
  int r = pool_probability_bins.size()-1;
  int m;
  while(l<r) {
    m = (l+r)/2;
    if(pool_probability_bins[m] == input) {
      break;
    }
    else if(input > pool_probability_bins[m]) {
      if(input < pool_probability_bins[m+1]) {
        break;
      }
      else {
        l = m+1;
      }
    }
    else {
      if(input > pool_probability_bins[m-1]) {
        m--;
        break;
      }
      else {
        r = m-1;
      }
    }
  }
  return m;
}

// creates random permutations and adds greedy state to poputlation
std::vector < State > RandomPopulation(State nodes , int size_of_population = -1) {
	std::unordered_set < State , StateHash > random_states;
	State new_state;
	unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
	std::default_random_engine generator (seed);
	if(size_of_population == -1) {
        size_of_population = std::min(std::max(1,int(1e5/pow(nodes.permutation.size(),2))),int((nodes.permutation.size()-1 > 9) ? 1e6 : Factorial(nodes.permutation.size()-1)));
	}
	else {
	    size_of_population = std::min(size_of_population,int((nodes.permutation.size()-1 > 9) ? 1e6 : Factorial(nodes.permutation.size()-1)));
	}
	for(int k = 0;k < size_of_population-1;k++) {
	    new_state = nodes;
	    for(int i = 1;i<nodes.permutation.size()-1;i++) {
	        std::uniform_int_distribution<int> distribution(i,nodes.permutation.size()-1);
	        int j = distribution(generator);
	        std::swap(new_state.permutation[i],new_state.permutation[j]);
	    }
	    new_state.cost = StateCost(new_state);
	    random_states.insert(new_state);
  }
  // computing greedy route/permutaion by finding closest city at each step
  std::set < int > visited;
  State greedy;
  visited.insert(0);
  int current = 0;
  greedy.permutation.push_back(nodes.permutation[0]);
  for(int i = 0; i < nodes.permutation.size()-1; i++){
    int closest = -1;
    double min_distnace = 99999;
    for(int j = 0; j < nodes.permutation.size()-1; j++){
      if(!visited.count(j)){
        if(pow((nodes.permutation[current].first - nodes.permutation[j].first),2) +
           pow((nodes.permutation[current].second - nodes.permutation[j].second),2) < min_distnace){
          min_distnace = pow((nodes.permutation[current].first - nodes.permutation[j].first),2) +
                        pow((nodes.permutation[current].second - nodes.permutation[j].second),2);
          closest = j;
         }
      }
    }
    visited.insert(closest);
    greedy.permutation.push_back(nodes.permutation[closest]);
    current = closest;
  }
  greedy.cost = StateCost(greedy);
  std::cerr<<greedy.cost<<'\n';
  std::vector < State > return_states;
  return_states.push_back(greedy);
  for(auto state = random_states.begin();state != random_states.end();state++) {
    return_states.push_back(*state);
  }
	return return_states;
}

// applies HillClimb on given state/permutation
// neighbours are produced by swapping two cities
State HillClimb (State nodes) {
    State min_neighbour;
    bool min_defined = false;
    do{
        if(min_defined == true) {
            nodes = min_neighbour;
            min_defined = false;
        }
        for(int i = 1;i<nodes.permutation.size();i++) {
            for(int j = i+1;j<nodes.permutation.size();j++) {
                State neighbour_state = nodes;
                swap(neighbour_state.permutation[i],neighbour_state.permutation[j]);
                neighbour_state.cost = StateCost(neighbour_state);
                if(min_defined == false) {
                    min_neighbour = neighbour_state;
                    min_defined = true;
                }
                else if(neighbour_state.cost < min_neighbour.cost) {
                    min_neighbour = neighbour_state;
                }
            }
        }
    }while(min_neighbour.cost < nodes.cost);
    return nodes;
}

// missleading name. it basically does HillClimb on each state in RandomPopulation
State BeamSearch(State nodes) {
    std::vector < State > initial_states = RandomPopulation(nodes,1);
    State final_state;
    bool final_defined = false;
    double final_state_cost = 0;
    for(auto state = initial_states.begin();state != initial_states.end();state++) {
        State local_min = HillClimb(*state);
        if(final_defined == false) {
            final_state = local_min;
            final_defined = true;
        }
        else if(local_min.cost < final_state.cost) {
            final_state = local_min;
        }
    }
    return final_state;
}

// a simple mutation function swapping two indexes
State RandomNeighbour(State nodes) {
  // choose two random numbers to swap
	State new_state;
	unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
	std::default_random_engine generator (seed);
  new_state = nodes;
  std::uniform_int_distribution<int> distribution(1,nodes.permutation.size()-1);
  int j = distribution(generator);
  int i = distribution(generator);
  // swap chosen indexes
  std::swap(new_state.permutation[i],new_state.permutation[j]);
  new_state.cost = StateCost(new_state);
	return new_state;
}

// SimulatedAnnealing
State SimulatedAnnealing(State nodes) {
    double temperature = 400;
    double cooling_factor = 0.99999;
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> dis(0.0, 1.0);
    while(temperature > 1e-9) {
        State neighbour = RandomNeighbour(nodes);
        if(neighbour.cost < nodes.cost) {
            nodes = neighbour;
        }
        else if(exp((neighbour.cost-nodes.cost)/temperature) < dis(gen)) {
             nodes = neighbour;
        }
        temperature *= cooling_factor;
    }
    return nodes;
}

// uses rank selection to generate breeding_pool
std::vector< State > RankSelection(std::vector< State > population,int kPopulationSize) {
  std::vector<State> breeding_pool;
  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  std::default_random_engine generator (seed);
  std::uniform_int_distribution<int> distribution(0,(kPopulationSize*(kPopulationSize+1))/2);
  for(int i = 0; i<kPopulationSize;i++) {
    int j = distribution(generator);
    // finds index of chosen number in pool_probability_bins
    int chosen = FindIndex(j);
    breeding_pool.push_back(population[chosen]);
  }
  return breeding_pool;
}

// an order recombination based Cross Over function
State CrossOver(State parent1 ,State parent2) {
  // choose two random points for recombination
  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
	std::default_random_engine generator (seed);
  std::uniform_int_distribution<int> distribution(1,parent1.permutation.size()-1);
  std::unordered_set < std::pair<double,double> , PairHash > seen;
  int a = distribution(generator);
  int b = distribution(generator);
  // fill offspring with the a-b cut of first parent
  State new_state;
  for(int i = std::min(a,b);i<=std::max(a,b);i++) {
    new_state.permutation.push_back(parent1.permutation[i]);
    seen.insert(parent1.permutation[i]);
  }
  // fill the rest of offspring with rest of second parent
  int k=-1;
  for(int i = 0;i<std::min(a,b);i++) {
    while(seen.count(parent2.permutation[++k])) {}
    new_state.permutation.insert(new_state.permutation.begin(),parent2.permutation[k]);
  }
  for(int i = new_state.permutation.size();i<parent1.permutation.size();i++) {
    while(seen.count(parent2.permutation[++k])) {}
    new_state.permutation.push_back(parent2.permutation[k]);
  }
  new_state.cost = StateCost(new_state);
  return new_state;
}

State GeneticAlgorithm(State nodes) {
  const double kElitePercentage = 0.1;
  const double kMutationRate = 0.01;
  const int kGenerations = 1000;
  const int kPopulationSize = 500;
  std::vector < State > population = RandomPopulation(nodes, kPopulationSize);
  const int kEliteSize = int(kElitePercentage*population.size());
  State min_state;
  min_state.cost = 1000000;
  // rank based selection bins
  pool_probability_bins.push_back(0);
  for(int k = 0;k<kPopulationSize;k++) {
    int temp = pool_probability_bins[k];
    pool_probability_bins.push_back(temp + kPopulationSize - k);
  }

  // execute GeneticAlgorithm with specified number of generations
  for(int k = 0;k<kGenerations;k++) {
    std::vector < State > new_population;
    std::vector < State >  breeding_pool;
    std::vector < State >  offsprings;
    // sort current population
    std::sort(population.begin(),population.end(),CompareState);
    // save best and worst of population
    best_of_generation.push_back(std::make_pair(k, (*(population.begin())).cost));
    worst_of_generation.push_back(std::make_pair(k, (population[population.size()-1]).cost));

    double temp_avg = 0;
    int temp_counter = 1;
    // calculate average fitness of population
    for(auto it = population.begin();it != population.end();it++) {
      temp_avg = temp_avg / temp_counter * (temp_counter-1) + (*it).cost / temp_counter;
      temp_counter++;
    }
    // save average fitness of population
    average_of_generation.push_back(std::make_pair(k, temp_avg));
    // add elites to new population
    for(int i = 0;i<kEliteSize;i++) {
      new_population.push_back(population[i]);
    }
    // get breeding pool with RankSelection
    breeding_pool = RankSelection(population,kPopulationSize);
    for(int t = 0;t<population.size()-kEliteSize;t++) {
      // choose two random states from breeding_pool
      unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
      std::default_random_engine generator (seed);
      std::uniform_int_distribution<int> distribution(0,breeding_pool.size()-1);
      int j = distribution(generator);
      int i = distribution(generator);
      // create offspring of two parents using CrossOver
      State offspring = CrossOver(breeding_pool[i],breeding_pool[j]);
      offsprings.push_back(offspring);
    }
    // mutates an offspring with given probability
    for(auto it = offsprings.begin();it != offsprings.end();it++) {
      std::random_device rd;
      std::mt19937 gen(rd());
      std::uniform_real_distribution<> dis(0.0, 1.0);
      if(dis(gen) < kMutationRate) {
        State mutant = RandomNeighbour(*it);
        new_population.push_back(mutant);
      }
      else {
        new_population.push_back(*it);
      }
    }
    breeding_pool.erase(breeding_pool.begin(),breeding_pool.end());
    for(int it = 0;it < population.size();it++) {
      population[it]=new_population[it];
    }
    new_population.erase(new_population.begin(),new_population.end());
  }
  State result = population[0];
  for(int it = 0;it < population.size();it++) {
    if (result.cost > population[it].cost) {
      result = population[it];
    }
  }
  return result;
}


/**
 * regression test
 * @author Tobias Weber <tweber@ill.fr>
 * @date feb-19
 * @license GPLv3, see 'LICENSE' file
 *
 * g++ -std=c++20 -o leastsq leastsq.cpp
 * g++ -std=c++20 -DUSE_LAPACK -I/usr/include/lapacke -I/usr/local/opt/lapack/include -L/usr/local/opt/lapack/lib -o leastsq leastsq.cpp -llapacke
 *
 * ----------------------------------------------------------------------------
 * tlibs
 * Copyright (C) 2017-2021  Tobias WEBER (Institut Laue-Langevin (ILL),
 *                          Grenoble, France).
 * Copyright (C) 2015-2017  Tobias WEBER (Technische Universitaet Muenchen
 *                          (TUM), Garching, Germany).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
 */

#define BOOST_TEST_MODULE Least Squares Test
#include <boost/test/included/unit_test.hpp>
namespace test = boost::unit_test;
namespace testtools = boost::test_tools;

#include <iostream>
#include <vector>

#include "libs/maths.h"
using namespace tl2_ops;

using t_types = std::tuple<double, float>;
BOOST_AUTO_TEST_CASE_TEMPLATE(test_leastsq, t_real, t_types)
{
	//using t_cplx = std::complex<t_real>;
	using t_vec = tl2::vec<t_real, std::vector>;
	//using t_mat = tl2::mat<t_real, std::vector>;
	//using t_vec_cplx = tl2::vec<t_cplx, std::vector>;
	//using t_mat_cplx = tl2::mat<t_cplx, std::vector>;

	auto x = tl2::create<t_vec>({1, 2, 3, 4, 5, 6, 7, 8, 9, 10});
	auto y = tl2::create<t_vec>({5, 5, 7, 9, 9.5, 10.5, 10.5, 12, 13.5, 14});

	auto [params, ok] = tl2::leastsq<t_vec>(x, y, 1);
	std::cout << "ok: " << ok << ", params: " << params << std::endl;

	BOOST_TEST(ok);
	BOOST_TEST(params[0] == 3.9, testtools::tolerance(1e-3));
	BOOST_TEST(params[1] == 1.036, testtools::tolerance(1e-3));
}


/**
 * UnscentedKalmanFilterX.hpp
 * @author koide
 * 16/02/01
 **/
#ifndef KKL_UNSCENTED_KALMAN_FILTER_X_HPP
#define KKL_UNSCENTED_KALMAN_FILTER_X_HPP

#include <random>
#include <Eigen/Dense>
#include <kalman/kalman_filter.hpp>

/**
 * @brief Unscented Kalman Filter class
 * @param T        scaler type
 * @param System   system class to be estimated
 */
template<typename T, class System>
class UnscentedKalmanFilterX  : public KalmanFilter<T, System>
{
  typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXt;    //列向量
  typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> MatrixXt;
  using KalmanFilter<T, System>::state_dim;
  using KalmanFilter<T, System>::N;
  using KalmanFilter<T, System>::input_dim;
  using KalmanFilter<T, System>::measurement_dim;
  using KalmanFilter<T, System>::M;
  using KalmanFilter<T, System>::mean;
  using KalmanFilter<T, System>::cov;
  using KalmanFilter<T, System>::system;
  using KalmanFilter<T, System>::process_noise;
  using KalmanFilter<T, System>::measurement_noise;
  using KalmanFilter<T, System>::kalman_gain;
public:
  /**
   * @brief constructor
   * @param system               system to be estimated
   * @param state_dim            state vector dimension
   * @param input_dim            input vector dimension
   * @param measurement_dim      measurement vector dimension
   * @param process_noise        process noise covariance (state_dim x state_dim)
   * @param measurement_noise    measurement noise covariance (measurement_dim x measuremend_dim)
   * @param mean                 initial mean
   * @param cov                  initial covariance
   */
  UnscentedKalmanFilterX(const System& _system, int _state_dim, int _input_dim, int _measurement_dim,
                         const MatrixXt& _process_noise, const MatrixXt& _measurement_noise,
                         const VectorXt& _mean, const MatrixXt& _cov):
    KalmanFilter<T, System>(_system, _state_dim, _input_dim, _measurement_dim, _process_noise, _measurement_noise, _mean, _cov),
    S(2 * state_dim + 1),
    lambda(1)
  {
    weights.resize(S, 1);
    sigma_points.resize(S, N);
    ext_weights.resize(2 * (N + M) + 1, 1);
    ext_sigma_points.resize(2 * (N + M) + 1, N + M);
    expected_measurements.resize(2 * (N + M) + 1, M);

    // initialize weights for unscented filter
    weights[0] = lambda / (N + lambda);
    for (int i = 1; i < 2 * N + 1; i++) {
      weights[i] = 1 / (2 * (N + lambda));
    }

    // weights for extended state space which includes error variances
    ext_weights[0] = lambda / (N + M + lambda);
    for (int i = 1; i < 2 * (N + M) + 1; i++) {
      ext_weights[i] = 1 / (2 * (N + M + lambda));
    }
  }

  /**
   * @brief predict  预测函数
   * @param control  input vector
   */
  virtual void predict(const VectorXt& control) override
  {
    // calculate sigma points
    this->ensurePositiveFinite(cov);
    computeSigmaPoints(mean, cov, sigma_points); //根据上一时刻的均值和方差计算sigma点
    for (int i = 0; i < S; i++) {
      sigma_points.row(i) = system.f(sigma_points.row(i), control); //根据系统方程传播sigma点
    }

    const auto& Q = process_noise; //系统噪声|过程噪声

    // unscented transform
    VectorXt mean_pred(mean.size());
    MatrixXt cov_pred(cov.rows(), cov.cols());

    mean_pred.setZero();
    cov_pred.setZero();
    for (int i = 0; i < S; i++) {
      mean_pred += weights[i] * sigma_points.row(i);   //传播后的sigma点集均值
    }
    for (int i = 0; i < S; i++) {
      VectorXt diff = sigma_points.row(i).transpose() - mean_pred;
      cov_pred += weights[i] * diff * diff.transpose(); //传播后的sigma点集方差
    }
    cov_pred += Q;                                      //加上过程噪声

    //得到预测值和预测协方差
    mean = mean_pred;
    cov = cov_pred;
  }

  /**
   * @brief correct      校正函数
   * @param measurement  观测值
   */
  virtual void correct(const VectorXt& measurement) override
  {
    // create extended state space which includes error variances
    VectorXt ext_mean_pred = VectorXt::Zero(N + M, 1);
    MatrixXt ext_cov_pred = MatrixXt::Zero(N + M, N + M);
    ext_mean_pred.topLeftCorner(N, 1) = VectorXt(mean);
    ext_cov_pred.topLeftCorner(N, N) = MatrixXt(cov);
    ext_cov_pred.bottomRightCorner(M, M) = measurement_noise;

    this->ensurePositiveFinite(ext_cov_pred);
    computeSigmaPoints(ext_mean_pred, ext_cov_pred, ext_sigma_points); //根据预测均值和协方差以及测量噪声计算sigma点
                                                                       //此时测量误差并未添加到sigama主体,而是存放于拓展部分

    // unscented transform
    expected_measurements.setZero();
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      expected_measurements.row(i) = system.h(ext_sigma_points.row(i).transpose().topLeftCorner(N, 1));     //观测方程传播sigama点集
      expected_measurements.row(i) += VectorXt(ext_sigma_points.row(i).transpose().bottomRightCorner(M, 1));//添加测量噪声
    }

    VectorXt expected_measurement_mean = VectorXt::Zero(M);
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      expected_measurement_mean += ext_weights[i] * expected_measurements.row(i);  //传播后的sigama点集均值
    }
    MatrixXt expected_measurement_cov = MatrixXt::Zero(M, M);
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      VectorXt diff = expected_measurements.row(i).transpose() - expected_measurement_mean;
      expected_measurement_cov += ext_weights[i] * diff * diff.transpose();        //传播后的sigama点集协方差
    }

    // calculated transformed covariance
    MatrixXt sigma = MatrixXt::Zero(N + M, M);
    for (int i = 0; i < ext_sigma_points.rows(); i++) {
      auto diffA = (ext_sigma_points.row(i).transpose() - ext_mean_pred);
      auto diffB = (expected_measurements.row(i).transpose() - expected_measurement_mean);
      sigma += ext_weights[i] * (diffA * diffB.transpose());
    }

    kalman_gain = sigma * expected_measurement_cov.inverse();                       //计算卡尔曼增益

    VectorXt ext_mean = ext_mean_pred + kalman_gain * (measurement - expected_measurement_mean); //最优估计
    MatrixXt ext_cov = ext_cov_pred - kalman_gain * expected_measurement_cov * kalman_gain.transpose();    //最优估计的协方差

    mean = ext_mean.topLeftCorner(N, 1);
    cov = ext_cov.topLeftCorner(N, N);
  }

  const MatrixXt& getSigmaPoints() const { return sigma_points; }
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  T lambda;
  const int S; //sigma点个数
  VectorXt weights;
  MatrixXt sigma_points;
  VectorXt ext_weights;
  MatrixXt ext_sigma_points;
  MatrixXt expected_measurements;

private:
  /**
   * @brief compute sigma points
   * @param mean          mean
   * @param cov           covariance
   * @param sigma_points  calculated sigma points
   */
  void computeSigmaPoints(const VectorXt& mean, const MatrixXt& cov, MatrixXt& sigma_points) {
    const int n = mean.size();
    assert(cov.rows() == n && cov.cols() == n);

    Eigen::LLT<MatrixXt> llt;   //Cholesky分解，将对称正定矩阵表示成一个下三角矩阵L和其转置的乘积的分解 M = L*L.transpose()
    llt.compute((n + lambda) * cov);
    MatrixXt l = llt.matrixL();

    sigma_points.row(0) = mean;
    for (int i = 0; i < n; i++) {
      sigma_points.row(1 + i * 2) = mean + l.col(i);
      sigma_points.row(1 + i * 2 + 1) = mean - l.col(i);
    }
  }
};

#endif


// std includes
#include <vector>
#include <iostream>
#include <random> // random_device, default_random_engine, uniform_real_distribution
#include <memory> // shared_ptr
#include <cmath> // sin, cos
// thirdparty includes
#include <Eigen/Dense>
// lib includes
#include "m0sh/uniform.h"
#include "m0sh/structured_sub.h"
#include "p0l/interpolation.h"

using TypeScalar = double;
// Space
const unsigned int DIM = 2;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
// Mesh
template<typename ...Args>
using TypeContainer = std::vector<Args...>;
using TypeMeshStructured = m0sh::Structured<TypeVector, TypeRef, TypeContainer>;
using TypeMeshStructuredSub = m0sh::StructuredSub<TypeVector, TypeRef, TypeContainer>;
using TypeMeshStructuredUniform = m0sh::Uniform<TypeVector, TypeRef, TypeContainer>;
// Data
const std::size_t np = 5;
const std::size_t n = 100;
const double l = 2 * M_PI;

double f(const double x, const double y) {
    return std::cos(x) + std::sin(y);
}

void print(const std::shared_ptr<TypeMeshStructured>& sMesh, const TypeContainer<TypeScalar> q, std::uniform_real_distribution<TypeScalar>& uniform, std::default_random_engine& e) {
    const TypeVector x = {uniform(e), uniform(e)};
    const double analy = f(x[0], x[1]);
    double interp = p0l::lagrangeMeshPoint<TypeMeshStructured, TypeContainer, double, TypeVector, TypeRef, TypeMeshStructuredSub>(sMesh, q, x, np);
    double error = std::abs(analy - interp);
    double relative = error / analy;
    std::cout << "Interpolation using " << np << " grid points (" << std::pow(np, DIM) << " points)." << " x : \n" << x << "\n Analy : " << analy << " Result : " << interp << " | error = " << error << " | relative = " << relative << std::endl;
}

int main() {
    // Init
    std::shared_ptr<TypeMeshStructured> sMesh = std::make_shared<TypeMeshStructuredUniform>(TypeContainer<std::size_t>(DIM, n), TypeContainer<TypeScalar>(DIM, l), TypeVector::Constant(0.0), TypeContainer<bool>(DIM, true));
    TypeContainer<TypeScalar> q(sMesh->nbPoints());
    for(std::size_t pointIndex = 0; pointIndex < q.size(); pointIndex++) {
        TypeVector x = sMesh->positionPoint(pointIndex);
        q[pointIndex] = f(x[0], x[1]);
    }
    // Random setup
    std::random_device r;
    std::default_random_engine e(r());
    std::uniform_real_distribution<TypeScalar> uniform(0, l);
    // Print
    print(sMesh, q, uniform, e);
    print(sMesh, q, uniform, e);
    print(sMesh, q, uniform, e);
    print(sMesh, q, uniform, e);
    print(sMesh, q, uniform, e);
    print(sMesh, q, uniform, e);
}


/**
 This homework problem consists of reading a simple, gmesh generated, mesh on
 the unit square and solving a simple reaction diffusion system using LehrFEM++
 */

#include <cmath>
#include <iostream>
#include <memory>

#include <Eigen/Core>

#include <lf/base/base.h>
#include <lf/mesh/mesh.h>
#include <lf/refinement/refinement.h>

#include "linfereactdiff.h"

int main() {
  const lf::base::size_type num_levels = 5;
  std::shared_ptr<lf::refinement::MeshHierarchy> multi_mesh_p =
      LinFeReactDiff::generateMeshHierarchy(num_levels);
  lf::refinement::MeshHierarchy &multi_mesh{*multi_mesh_p};
  // get pointer to finest mesh used as ground truth
  std::shared_ptr<const lf::mesh::Mesh> mesh_p =
      multi_mesh.getMesh(num_levels - 1);
  Eigen::VectorXd finest_sol = LinFeReactDiff::solveFE(mesh_p);
  double ground_truth_energy =
      LinFeReactDiff::computeEnergy(mesh_p, finest_sol);

  // compute error for the other meshes
  for (int i = 0; i < num_levels - 1; i++) {
    mesh_p = multi_mesh.getMesh(i);
    Eigen::VectorXd sol = LinFeReactDiff::solveFE(mesh_p);
    double energy = LinFeReactDiff::computeEnergy(mesh_p, sol);
    std::cout << "Mesh " << i + 1
              << " error: " << std::abs(energy - ground_truth_energy) << "\n";
  }
}


// Author: Daisuke Kanaizumi
// Affiliation: Department of Applied Mathematics, Waseda University

// verification program for q-Laguerre polynomials
// References

// Ismail, M. E., & Zhang, R. (2016).
// Integral and Series Representations of $ q $-Polynomials and Functions: Part I.
// arXiv preprint arXiv:1604.08441.
// Y. Chen , M. E. Ismail, & K. A. Muttalib. Asymptotics of basic Bessel functions and q-Laguerre polynomials, Lemma 2
// Journal of Computational and Applied Mathematics, 54(3), 263-272 (1994).

//Koekoek, R., & Swarttouw, R. F. (1996). The Askey-scheme of hypergeometric orthogonal polynomials and its q-analogue. arXiv preprint math/9602214.

#ifndef QLAGUERRE_HPP
#define QLAGUERRE_HPP

#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/constants.hpp>
#include <kv/Pochhammer.hpp>
#include <kv/QHypergeometric.hpp>
#include <cmath>
#include <limits>
#include <kv/convert.hpp> // this was included to use complex numbers
#include <kv/complex.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/psa.hpp>
namespace ub = boost::numeric::ublas;
namespace kv {
  template <class T> complex<interval<T> > qLpoly(const complex<interval<T> >& x,const interval<T>& q,const int n,const complex<interval<T> >& alpha){
    complex<interval<T> >res,sum;
    if(n<=1000){
      sum=0.;
      for(int k=0;k<=n;k++){
	sum=sum+pow(q,alpha*k+k*k)*pow(-x,k)
	  /qPochhammer(q,q,k)/qPochhammer(q,q,n-k)/qPochhammer(pow(q,alpha+1),q,k);
      }
      sum=sum*qPochhammer(pow(q,alpha+1),q,n);
      res=sum;
    }
    else{
      ub::vector<complex<interval<T> > >a(1),b(2);
      a(0)=pow(q,alpha+n+1);
      b(0)=pow(q,alpha+1);
      b(1)=-x*pow(q,n+alpha+1);

      res=qPochhammer(pow(q,alpha+1),q,n)/qPochhammer(q,q,n)
	*infinite_qPochhammer(complex<interval<T> >(-x*pow(q,alpha+n+1)),interval<T>(q))
	*QHypergeom(ub::vector<complex<interval<T> > >(a),ub::vector<complex<interval<T> > >(b),interval<T>(q),complex<interval<T> >(-x*pow(q,alpha+1)));

      //std::cout<<QHypergeom(ub::vector<complex<interval<T> > >(a),ub::vector<complex<interval<T> > >(b),interval<T>(q),complex<interval<T> >(-x*pow(q,alpha+1)))<<std::endl;
    }
    if((abs(res)).upper()==std::numeric_limits<T>::infinity()){
      res=infinite_qPochhammer(complex<interval<T> >(-x*pow(q,n+alpha+1)),interval<T> (q))
	/infinite_qPochhammer(complex<interval<T> >(pow(q,n+alpha+1)),interval<T> (q))/qPochhammer(q,q,n)
	*_1phi_1(complex<interval<T> >(-x),complex<interval<T> >(-x*pow(q,n+alpha+1)),interval<T>(q),complex<interval<T> >(pow(q,alpha+1)));
    }
    return res;
  }
  template <class T> interval<T>  qLpoly(const interval<T> & x,const interval<T>& q,const int n,const interval<T> & alpha){
    interval<T> res,sum;
    if(n<=1000){
      sum=0.;
      for(int k=0;k<=n;k++){
	sum=sum+pow(q,alpha*k+k*k)*pow(-x,k)
	  /qPochhammer(q,q,k)/qPochhammer(q,q,n-k)/qPochhammer(pow(q,alpha+1),q,k);
      }
      sum=sum*qPochhammer(pow(q,alpha+1),q,n);
      res=sum;
    }
    else{
      ub::vector<interval<T>  >a(1),b(2);
      a(0)=pow(q,alpha+n+1);
      b(0)=pow(q,alpha+1);
      b(1)=-x*pow(q,n+alpha+1);

      res=qPochhammer(pow(q,alpha+1),q,n)/qPochhammer(q,q,n)
	*infinite_qPochhammer(interval<T> (-x*pow(q,alpha+n+1)),interval<T>(q))
	*QHypergeom(ub::vector<interval<T>  >(a),ub::vector<interval<T>  >(b),interval<T>(q),interval<T> (-x*pow(q,alpha+1)));

      //std::cout<<QHypergeom(ub::vector<complex<interval<T> > >(a),ub::vector<complex<interval<T> > >(b),interval<T>(q),complex<interval<T> >(-x*pow(q,alpha+1)))<<std::endl;
    }
    if((abs(res)).upper()==std::numeric_limits<T>::infinity()){
      res=infinite_qPochhammer(interval<T> (-x*pow(q,n+alpha+1)),interval<T> (q))
	/infinite_qPochhammer(interval<T> (pow(q,n+alpha+1)),interval<T> (q))/qPochhammer(q,q,n)
	*_1phi_1(interval<T> (-x),interval<T> (-x*pow(q,n+alpha+1)),interval<T>(q),interval<T> (pow(q,alpha+1)));
    }
    return res;
  }
  template <class T> interval<T>  qLpoly_psa(const interval<T> & x,const interval<T>& q,const int n,const interval<T> & alpha){
    interval<T> res;

    kv::psa<interval<T> >a,b,c;
    a.v.resize(n+1);
    b.v.resize(n+1);
    c.v.resize(n+1);
    for(int i=0;i<=n;i++){
      a.v(i)=qPochhammer(-x,q,i)*std::pow(-1,i)*pow(q,i*(i+1)*0.5)*pow(q,alpha*i)/qPochhammer(q,q,i);
    }
    for(int j=0;j<=n;j++){
      b.v(j)=std::pow(-1,j)*pow(q,j*(j-1)*0.5)/qPochhammer(q,q,j);
    }
    c=a/b;
    res=c.v(n);
    return res;
}
  template <class T> interval<T>  qLpoly_rec(const interval<T> & x,const interval<T>& q,const int n,const interval<T> & alpha){
    interval<T> res,xx;
    xx=mid(x);
    ub::vector<interval<T>  >L(2*n);
    L(0)=1.;
    L(1)=(1-pow(q,alpha+1))*(1+pow(q,alpha+1)/(1-pow(q,alpha+1))*(-x))/(1-q);
    for(int i=1;i<=n-1;i++){
      L(i+1)=((1-pow(q,i+1)+q*(1-pow(q,i+alpha))-pow(q,2*i+alpha+1)*x)*L(i)-q*(1-pow(q,i+alpha))*L(i-1))/(1-pow(q,i+1));
    }
  res=L(n);
  return res;
  }
}
#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

inline bool close(double x, double y)
{
    using std::abs;
    return abs(x - y) < 0.001;
}

int main(int , char**)
{
    using namespace std;
    using mtl::lazy;

    double                d, rho, alpha= 7.8, beta, gamma;
    std::complex<double>  z;

    mtl::dense_vector<double> v(6, 1.0), w(6), r(6, 6.0), q(6, 2.0), x(6);
    mtl::dense2D<double>      A(6, 6);
    A= 2.0;
    mtl::compressed2D<double>      B(6, 6);
    B= 2.0;

    (lazy(w)= A * v) || (lazy(d) = lazy_dot(w, v));
    cout << "w = " << w << ", d = " << d << "\n";
    MTL_THROW_IF(!close(d, 12), mtl::runtime_error("wrong dot"));

    (lazy(w)= B * v) || (lazy(d) = lazy_dot(w, v));
    cout << "w = " << w << ", d = " << d << "\n";
    MTL_THROW_IF(!close(d, 12), mtl::runtime_error("wrong dot"));

    (lazy(r)-= alpha * q) || (lazy(rho)= lazy_unary_dot(r));
    cout << "r = " << r << ", rho = " << rho << "\n";
    MTL_THROW_IF(!close(rho, 552.96), mtl::runtime_error("wrong unary_dot"));

    (lazy(x)= 7.0) || (lazy(beta)= lazy_unary_dot(x));
    cout << "x = " << x << ", beta = " << beta << "\n";
    MTL_THROW_IF(!close(beta, 294), mtl::runtime_error("wrong unary_dot"));

    (lazy(x)= 7.0) || (lazy(beta)= lazy_one_norm(x));
    cout << "x = " << x << ", beta = " << beta << "\n";
    MTL_THROW_IF(!close(beta, 42), mtl::runtime_error("wrong one_norm"));

    (lazy(x)= 7.0) || (lazy(beta)= lazy_two_norm(x));
    cout << "x = " << x << ", beta = " << beta << "\n";
    MTL_THROW_IF(!close(beta, 17.1464), mtl::runtime_error("wrong two_norm"));

    (lazy(x)= 7.0) || (lazy(beta)= lazy_infinity_norm(x));
    cout << "x = " << x << ", beta = " << beta << "\n";
    MTL_THROW_IF(!close(beta, 7), mtl::runtime_error("wrong one_norm"));

    (lazy(x)= 7.0) || (lazy(beta)= lazy_sum(x));
    cout << "x = " << x << ", beta = " << beta << "\n";
    MTL_THROW_IF(!close(beta, 42), mtl::runtime_error("wrong sum"));

    (lazy(x)= 7.0) || (lazy(beta)= lazy_product(x));
    cout << "x = " << x << ", beta = " << beta << "\n";
    MTL_THROW_IF(!close(beta, 117649), mtl::runtime_error("wrong sum"));

    (lazy(x)= 2.0) || (lazy(gamma)= lazy_dot(r, x));
    cout << "x = " << x << ", gamma = " << gamma << "\n";
    MTL_THROW_IF(!close(gamma, -115.2), mtl::runtime_error("wrong dot"));

    (lazy(r)= alpha * q) || (lazy(rho)= lazy_dot(r, q));
    cout << "r = " << r << ", rho = " << rho << "\n";
    MTL_THROW_IF(!close(rho, 187.2), mtl::runtime_error("wrong dot"));

    (lazy(r)= alpha * q) || (lazy(v)= 8.6 * q) || (lazy(x)= 2.2 * q);
    cout << "r = " << r << ", v = " << v << ", x = " << x << "\n";
    MTL_THROW_IF(!close(r[0], 15.6) || !close(v[0], 17.2) || !close(x[0], 4.4),
		 mtl::runtime_error("wrong vector scaling"));

    return 0;
}


#ifndef SOPHUS_ROTATION_MATRIX_HPP
#define SOPHUS_ROTATION_MATRIX_HPP

#include <Eigen/Dense>
#include <Eigen/SVD>

#include "types.hpp"

namespace Sophus {

// Takes in arbiray square matrix and returns true if it is
// orthogonal.
template <class D>
SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase<D> const& R) {
  using Scalar = typename D::Scalar;
  static int const N = D::RowsAtCompileTime;
  static int const M = D::ColsAtCompileTime;

  static_assert(N == M, "must be a square matrix");
  static_assert(N >= 2, "must have compile time dimension >= 2");

  return (R * R.transpose() - Matrix<Scalar, N, N>::Identity()).norm() <
         Constants<Scalar>::epsilon();
}

// Takes in arbiray square matrix and returns true if it is
// "scaled-orthogonal" with positive determinant.
//
template <class D>
SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<D> const& sR) {
  using Scalar = typename D::Scalar;
  static int const N = D::RowsAtCompileTime;
  static int const M = D::ColsAtCompileTime;
  using std::pow;
  using std::sqrt;

  Scalar det = sR.determinant();

  if (det <= Scalar(0)) {
    return false;
  }

  Scalar scale_sqr = pow(det, Scalar(2. / N));

  static_assert(N == M, "must be a square matrix");
  static_assert(N >= 2, "must have compile time dimension >= 2");

  return (sR * sR.transpose() - scale_sqr * Matrix<Scalar, N, N>::Identity())
             .template lpNorm<Eigen::Infinity>() <
         sqrt(Constants<Scalar>::epsilon());
}

// Takes in arbiray square matrix (2x2 or larger) and returns closest
// orthogonal matrix with positive determinant.
template <class D>
SOPHUS_FUNC enable_if_t<
    std::is_floating_point<typename D::Scalar>::value,
    Matrix<typename D::Scalar, D::RowsAtCompileTime, D::RowsAtCompileTime>>
makeRotationMatrix(Eigen::MatrixBase<D> const& R) {
  using Scalar = typename D::Scalar;
  static int const N = D::RowsAtCompileTime;
  static int const M = D::ColsAtCompileTime;

  static_assert(N == M, "must be a square matrix");
  static_assert(N >= 2, "must have compile time dimension >= 2");

  Eigen::JacobiSVD<Matrix<Scalar, N, N>> svd(
      R, Eigen::ComputeFullU | Eigen::ComputeFullV);

  // Determine determinant of orthogonal matrix U*V'.
  Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant();
  // Starting from the identity matrix D, set the last entry to d (+1 or
  // -1),  so that det(U*D*V') = 1.
  Matrix<Scalar, N, N> Diag = Matrix<Scalar, N, N>::Identity();
  Diag(N - 1, N - 1) = d;
  return svd.matrixU() * Diag * svd.matrixV().transpose();
}

}  // namespace Sophus

#endif  // SOPHUS_ROTATION_MATRIX_HPP


#include <aslam/cameras/Triangulation.hpp>
#include <Eigen/Dense>

namespace aslam {
namespace cameras {

void triangulate(const Eigen::Vector3d & point1, const Eigen::Vector3d & ray1,
                 const Eigen::Vector3d & point2, const Eigen::Vector3d & ray2,
                 Eigen::Vector3d & outTriangulatedPoint, double & outGap,
                 double & outS1, double & outS2) {

  Eigen::Vector3d t12 = point2 - point1;

  Eigen::Vector2d b;
  b[0] = t12.dot(ray1);
  b[1] = t12.dot(ray2);
  Eigen::Matrix2d A;
  A(0, 0) = ray1.dot(ray1);
  A(1, 0) = ray1.dot(ray2);
  A(0, 1) = -A(1, 0);
  A(1, 1) = -ray2.dot(ray2);
  Eigen::Vector2d lambda = A.inverse() * b;
  Eigen::Vector3d xm = point1 + lambda[0] * ray1;
  Eigen::Vector3d xn = point2 + lambda[1] * ray2;
  t12 = (xm - xn);

  outGap = t12.norm();
  outTriangulatedPoint = xn + 0.5 * t12;
  outS1 = lambda[0];
  outS2 = lambda[1];

}

}  // namespace cameras
}  // namespace aslam


#include <iostream>
#include <functional>

#include <Eigen/Dense>
#include <EigenRand/EigenRand>

using namespace std;
using namespace Eigen;

int main()
{
	Rand::Vmt19937_64 urng{4212};
	Vector4f mean{0, 1, 2, 3};
	Matrix4f cov;
	cov << 1, 1, 0, 0,
		1, 2, 0, 0,
		0, 0, 3, 1,
		0, 0, 1, 2;

	// constructs MvNormalGen with Scalar=float, Dim=4
	Rand::MvNormalGen<float, 4> gen1{mean, cov};

	// or you can use `make-` helper function. It can deduce the type of generator to be created.
	auto gen2 = Rand::makeMvNormGen(mean, cov);

	// generates one sample ( shape (4, 1) )
	Vector4f sample = gen1.generate(urng);

	// generates 10 samples ( shape (4, 10) )
	MatrixXf samples = gen1.generate(urng, 10);
	// or you can just use `MatrixXf` type

	cout << sample << endl;
	cout << samples << endl;

	Rand::StdNormalGen<float> stdnorm;
	for (int i = 0; i < 10; ++i)
	{
		cout << stdnorm.generate<Matrix<float, 4, -1>>(4, 1, urng) << endl;
	};
};


//
// Created by cheyulin on 12/17/16.
//

#include <cassert>

#include <boost/type_traits.hpp>
#include <boost/mpl/arithmetic.hpp>
#include <boost/mpl/logical.hpp>
#include <boost/mpl/comparison.hpp>
#include <boost/mpl/next_prior.hpp>
#include <boost/mpl/if.hpp>

using namespace boost;
using namespace boost::mpl;

void DemoIntegerUsage() {
    using i2=int_<2>;
    using s2=integral_c<short, 2>;

    assert(i2::value == 2);
    assert(i2::value == s2::value);

    assert((is_same<i2::type, i2>::value));
    assert((is_same<s2::value_type, short>::value));

    assert(i2::next::value == 3);
    assert(prior<s2>::type::value == 1);

    i2 two1;
    s2 two2;
    int i = two1 + two2;
    assert(i == int_<4>());
}

void DemoBoolUsage() {
    assert(true_::value == true);
    assert(false_::value == false);

    assert((is_same<true_::type, bool_<true>>::value));
    assert((is_same<false_::value_type, bool>::value));
}

void DemoCalculation() {
    using i2= int_<2>;
    using i5= int_<5>;
    using i7= int_<7>;

    assert((plus<i2, i5, i7>::type::value == 14));
    assert((equal_to<minus<i7, i5>::type, i2>::type::value));

    assert((less<i2, i7>::type::value));
    assert((is_same<greater<i5, i2>::type, true_>::value));

    assert((not_<and_<true_, false_>::type>::type::value));
    assert((or_<true_, false_>::type()));
}

void DemoBranching() {
    using mdata1= if_c<true, int, long>::type;
    assert((is_same<mdata1, int>::value));

    using mdata2= if_<false_, float, double>::type;
    assert((is_same<mdata2, double>::value));

    using mdata3 = if_<is_integral<mdata2>, integral_promotion<mdata2>::type, floating_point_promotion<mdata2>::type>::type;
    assert((is_same<mdata3, double>::value));
}

void DemoEvalBranching() {
    using mdata1=eval_if_c<true, identity<int>, identity<long>>::type;
    assert((is_same<mdata1, int>::value));

    using mdata2=eval_if<false_, identity<float>, identity<double>>::type;
    assert((is_same<mdata2, double>::value));

    using mdata3= eval_if<is_integral<mdata2>, integral_promotion<mdata2>, floating_point_promotion<mdata2>>::type;
    assert((is_same<mdata3, double>::value));
}

int main() {
    DemoIntegerUsage();
    DemoBoolUsage();
    DemoCalculation();
    DemoBranching();
    DemoEvalBranching();
}


/*--
  Open3DMotion
  Copyright (c) 2004-2012.
  All rights reserved.
  See LICENSE.txt for more information.
--*/

#include "Open3DMotion/Maths/Matrix3x3.h"

#ifndef OPEN3DMOTION_LINEAR_ALGEBRA_EIGEN
extern "C"
{
#include <f2clibs/f2c.h>
#include <clapack.h>
}
#else
#include <Eigen/Dense>
#include <Eigen/SVD>
#endif

namespace Open3DMotion
{
	void Matrix3x3::SVD(double* U, double* s, double* VT, const double* A)
  {
#ifndef OPEN3DMOTION_LINEAR_ALGEBRA_EIGEN
    long three(3);
    Matrix3x3 Acpy(A);
    long lwork(256);
    double work[256];
    long info(0);

    // use lapack routine
    // - note U and VT are swapped
    // - this is because of the fortran column-major
    //   ordering for matrices - it turns out that
    //   using a row major matrix here corresponds to
    //   swapping U and VT
    dgesvd_(
      "A",  // all of U
      "A",  // all of VT
      &three, // rows
      &three, // cols
      Acpy,   // input/output matrix
      &three, // leading dimension of Acpy
      s,      // singular values
      VT,      // left orthonormal matrix
      &three, // leading dimension of left
      U,      // right orthonormal matrix
      &three, // leading dimension of right
      work,   // workspace
      &lwork, // size of workspace
      &info);   // returned error codes

#else
		Eigen::Map< const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > _A(A, 3, 3);
    Eigen::Map< Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > _U(U, 3, 3);
    Eigen::Map< Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > _VT(VT, 3, 3);
    Eigen::Map< Eigen::Matrix<double, 3, 1> > _s(s, 3, 1);
    Eigen::JacobiSVD< Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > svd(_A, Eigen::ComputeFullU | Eigen::ComputeFullV);
    _U = svd.matrixU();
    _VT = svd.matrixV().transpose();
    _s = svd.singularValues();
#endif

  }
}


#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <cstdlib>
#include <ctime>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <cfloat>

#include "time_bench.hpp"

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>

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

template <class T>
inline double norm(ublas::vector<T> v){
  /* ベクトルのノルムを計算する */
  double sum = 0;
  for(unsigned int i = 0; i < v.size(); ++i){
    sum += v[i] * v[i];
  }
  return sqrt(sum);
}

template <class T>
inline double dist(ublas::vector<T> x, ublas::vector<T> y){
  /* ベクトルのノルムから距離を定義 */
  ublas::vector<T> v = x - y;
  return norm(v);
}

inline double dist(ublas::matrix<double> mat1, unsigned int r1,
		   ublas::matrix<double> mat2, unsigned int r2){
  /* mat1のr1行目のベクトルと, mat2のr2行目のベクトルの距離を計算 */
  if(mat1.size2() != mat2.size2()){
    return -1;
  }else{
    double sum = 0;
    for(unsigned int i = 0; i < mat1.size2(); ++i){
      sum += (mat1(r1, i) - mat2(r2, i)) * (mat1(r1, i) - mat2(r2, i));
    }
    return sqrt(sum);
  }
}

template <class T>
inline unsigned int argmin(ublas::vector<T> v){
  if(v.empty()){ return -1; /* エラー */
  }else{
    int argmin = 0; T minval = v[argmin];
    for(unsigned int i = 0; i < v.size(); ++i){
      if(v[i] < minval){
	argmin = i; minval = v[i];
      }
    }
    return argmin;
  }
}

template <class T>
inline unsigned int argmax(ublas::vector<T> v){
  if(v.empty()){ return -1; /* エラー */
  }else{
    int argmax = 0; T maxval = v[argmax];
    for(unsigned int i = 0; i < v.size(); ++i){
      if(v[i] > maxval){
	argmax = i; maxval = v[i];
      }
    }
    return argmax;
  }
}

template <class T>
inline T min(ublas::vector<T> v){
  if(v.empty()){ return -1; /* エラー */
  }else{
    T minval = v[0];
    for(unsigned int i = 0; i < v.size(); ++i){
      if(v.at(i) < minval){
	minval = v.at(i);
      }
    }
    return minval;
  }
}

template <class T>
inline T min2(ublas::vector<T> v){
  /* vector vの要素で2番目に小さい要素を見つける */
  if(v.size() < 2){ return -1; /* エラー */
  }else{
    T min1 = v[0], min2 = v[1];
    if(min1 > min2){ /* swap する */
      T temp = min1; min1 = min2; min2 = temp;
    }
    for(unsigned int i = 2; i < v.size(); ++i){
      if(v[i] <= min1){       /* 最小値が発見された */
	min2 = min1; min1 = v[i];
      }else if(v[i] <= min2){ /* 準最小値が発見された */
	min2 = v[i];
      }
    }
    return min2;
  }
}
inline void Hamerly_update_ul(const unsigned int n, const unsigned int k,
			      const ublas::matrix <double> data,
			      const ublas::matrix <double> c,       /* クラスタの重心 */
			      const ublas::vector <unsigned int> a, /* 各点x_iが属するクラスタ */
			      ublas::vector<double> &u,            /* upper bound */
			      ublas::vector<double> &l             /* lower bound */){
  ublas::vector<double> dist_xc(k, 0); /* x[i] とクラスター中心との距離*/
  for(unsigned int i = 0; i < n; ++i){
    for(unsigned int j = 0; j < k; ++j){
      dist_xc[j] = dist(data, i, c, j);
    }
    u[i] = dist_xc[a[i]]; /* u[i] = d(x[i], c[a[i]])*/
    l[i] = min2(dist_xc); /* l[i] = min2_j d(x[i], c[j]) */
    /* lはx[i]から2番目に近いクラスタ重心までの距離 */
    //cerr << i << ": " << u[i] << "\t" << l[i] << "\t" << dist_xc << endl;
  }
  //cerr << u << endl;
  //cerr << l << endl;
  return;
}

bool Hamerly_repeat(const unsigned int n, const unsigned int d, const unsigned int k,
		    const ublas::matrix <double> data,
		    ublas::vector<unsigned int> &q, /* クラスタ中の点の数 */
		    ublas::matrix<double> &c,       /* クラスタの重心 */
		    ublas::matrix<double> &c_sum,   /* クラスタのベクトル和*/
		    ublas::vector<double> &s,       /* 最近接クラスタの重心との距離 */
		    ublas::vector<unsigned int> &a, /* 各点x_iが属するクラスタ */
		    ublas::vector<double> &u,       /* upper bound */
		    ublas::vector<double> &l        /* lower bound */){
  /* Hamerlyの繰り返しステップ クラスタ重心の更新の有無をbooleanで返す */
  /* この繰り返しステップで何らかの更新があったか */
  volatile bool updated = false;

  /* クラスター中心の変化があったか */
  ublas::vector<bool> cluster_unchanged(k, true);

  /* まずs[j]を更新
   * s[j] = min_{jj != j} dist(c[jj], c[j])
   *      = min2_jj dist(c[j], c[jj]) */
  for(unsigned int j = 0; j < k; ++j){
    ublas::vector<double> distance(k, 0);
    for(unsigned int jj = 0; jj < k; ++jj){
      distance[jj] = dist(c, j, c, jj);
    }
    s[j] = min2(distance);
  }

  //cerr << "s: " << s << endl;

  /* 各点x[i]について，Hamerlyの命題の条件をみたすか確認しながら
   * (つまり，適宜枝刈りを実行しながら)
   * 最近接クラスタの再割当てを行う
   * 同時に，のちのクラスタ重心の更新のために
   * c_sum[], q[] を正しい値に保つ */
  for(unsigned int i = 0; i < n; ++i){
    double m = max((s[a[i]] / 2.0), l[i]);
    //cerr << "i = " << i << ", m = " << m << ", u[i] = " << u[i] << endl;
    if(u[i] > m){     /* Hamerlyの命題の条件を満たさない */
      //cerr << i << u[i] << " -> ";
      u[i] = dist(data,i,c,a[i]);  /* upper boundを厳密な値に更新 */
      //cerr << u[i] << endl;
      if(u[i] > m){   /* Hamerlyの命題の条件を満たさない */
	unsigned int aa = a[i]; /* 最近接クラスタが変化したか調べる */
	{ /* 最近接クラスタを再計算 */
	  ublas::vector<double> distance(k);
	  for(unsigned int j = 0; j < k; ++j){
	    distance[j] = dist(data,i,c,j);
	  }
	  a[i] = argmin(distance);
	}
	if(aa != a[i]){ /* 最近接クラスタの変化によるパラメタの更新*/
	  /* 点x[i]の最近接クラスタは，aa から a[i] に変化した */
	  updated = true; /* この繰り返しステップ更新があった */
	  /* 後のクラスター重心の更新のために q, c_sum の更新が必要
	   * 同時に、更新があったクラスターのindexをメモしておく */
	  row(c_sum,aa)   -= row(data,i); q[aa]--;
	  row(c_sum,a[i]) += row(data,i); q[a[i]]++;
	  cluster_unchanged[aa] = false;
	  cluster_unchanged[a[i]] = false;
	  {
	    ublas::vector<double> dist_xc(k, 0);
	    /* x[i] とクラスター中心との距離*/
	    for(unsigned int j = 0; j < k; ++j){
	      dist_xc[j] = dist(data, i, c, j);
	    }
	    u[i] = dist_xc[a[i]]; /* u[i] = d(x[i], c[a[i]])*/
	    l[i] = min2(dist_xc); /* l[i] = min2_j d(x[i], c[j]) */
	  }
	}
      }
    }
  }

  ublas::matrix<double> c_prev = c; /* 変化前のクラスタ重心 */
  if(updated){ /* いずれかのデータ点の最近接クラスタが変化した場合 */
    ublas::vector<double> p(k, 0); /* 各クラスタの重心の移動距離 */
    for(unsigned int j = 0; j < k; ++j){
      if(!cluster_unchanged[j]){ /* j番目のクラスタ重心が移動していた場合 */
	row(c,j) = row(c_sum,j) / q[j]; /* クラスタ重心を更新 */
	p[j] = dist(c_prev, j, c, j); /* クラスター重心の移動距離 */
      }
    }
    unsigned int r = argmax(p);
    for(unsigned int i = 0; i < n; ++i){
      u[i] += p[a[i]];  l[i] -= p[r];
    }
  }

  return updated; /* データ点の最近接クラスタに変化があったかを返す */
}

void Hamerly_init(const unsigned int n, const unsigned int d, const unsigned int k,
		  const ublas::matrix <double> data,
		  ublas::vector<unsigned int> &q,   /* クラスタ中の点の数 */
		  ublas::matrix<double> &c,         /* クラスタの重心 */
		  ublas::matrix<double> &c_sum,     /* クラスタのベクトル和*/
		  ublas::vector<unsigned int> &a,   /* 各点x_iが属するクラスタ */
		  ublas::vector<double> &u,         /* upper bound */
		  ublas::vector<double> &l          /* lower bound */){
  /* 各クラスタの代表点をランダムに選択 */
  for(unsigned int j = 0; j < k; ++j){
    ublas::zero_vector<double> zero(d);
    row(c,j) = row(data, j);
    /* j番目のクラスタ中心をj番目のデータ点と一致させて初期化した */
  }

  /* 全てのデータ点について，初期クラスタa[i]を計算 */
  for(unsigned int i = 0; i < n; ++i){
    { /* argmin_j dist(x(i), c(j))の計算 */
      ublas::vector<double> distance(k);
      for(unsigned int j = 0; j < k; ++j){
	distance[j] = dist(data,i,c,j);
      }
      a[i] = argmin(distance);
    }
    /* クラスタ内の点の数，クラスタの点のベクトル和も更新 */
    q[a[i]]++;  row(c_sum,a[i]) += row(data,i);
  }

  /* upper bound, lower bound の更新 */
  Hamerly_update_ul(n, k, data, c, a, u, l);

  /* 初期クラスタ中心に対するデータ割り当てに対応して，
   * クラスター中心の更新や，各データ点のu[i],l[i]の更新が必要 */

  ublas::matrix<double> c_prev = c; /* 変化前のクラスタ重心 */

  ublas::vector<double> p(k, 0); /* 各クラスタの重心の移動距離 */
  for(unsigned int j = 0; j < k; ++j){
    row(c,j) = row(c_sum,j) / q[j]; /* クラスタ重心を更新 */
    p[j] = dist(c_prev, j, c, j); /* クラスター重心の移動距離 */
  }
  unsigned int r = argmax(p);

  for(unsigned int i = 0; i < n; ++i){
    u[i] += p[a[i]];  l[i] -= p[r];
  }

  return;
}

double Hamerly_sqerr(const unsigned int n, const unsigned int k,
		     const ublas::matrix<double>data,
		     const ublas::matrix<double> c,       /* クラスタの重心 */
		     const ublas::vector<unsigned int> a  /* 各点x_iが属するクラスタ */){
  /* クラスタリング結果に基づき二乗平均二乗誤差を計算して返す */
  double sum = 0;
  for(unsigned int i = 0; i < n; ++i){
    sum += dist(data,i,c,a[i]) * dist(data,i,c,a[i]);
  }
  return (sum / n);
}

void Hamerly(const unsigned int n, const unsigned int d, const unsigned int k,
	     const ublas::matrix<double> data){
  if(n < k){
    cerr << "data size n is smaller than cluster size d" << endl;
    exit(1);
  }else{
    struct timeval t_start, t_end;
    gettimeofday(&t_start, NULL); /* 時間計測開始 */

    /* j番目のクラスタを主語とする記号  */
    ublas::vector<unsigned int> q(k,0); /* クラスタ中の点の数 */
    ublas::matrix<double> c(k,d,0);     /* クラスタの重心 */
    ublas::matrix<double> c_sum(k,d,0); /* クラスタ中のベクトルの総和 */
    ublas::vector<double> s(k,0);       /* 最近接クラスタの重心との距離 */

    /* i番目のデータ点を主語とする記号 */
    ublas::vector<unsigned int> a(n, 0);  /* 各点x_iが属するクラスタ */
    ublas::vector<double> u(n, 0);        /* upper bound */
    ublas::vector<double> l(n, 0);        /* lower bound */

    Hamerly_init(n, d, k, data, q, c, c_sum, a, u, l);

    int t = 0; /* 繰り返しステップを何回繰り返したか */

    //cerr << "a: " << a << ": t = " << t << endl;

    while(Hamerly_repeat(n, d, k, data, q, c, c_sum, s, a, u, l)){
      t++;
      //cerr << "a: " << a << ": t = " << t << endl;
    }
    t++; /* 更新されなかった最後の1回も実行されている */

    gettimeofday(&t_end, NULL); /* 時間計測終了 */

    { /* 結果を出力 */
      ofstream fs("Hamerly_"
		  + std::to_string(n) + "_"
		  + std::to_string(d) + "_"
		  + std::to_string(k) + ".txt");
      if(fs.fail()){ exit(1); }
      fs << "a: " << a << endl;
      fs << c << endl;
      fs.close();
    }

    cout << n << "\t"                               /* サンプル数 */
	 << d << "\t"                               /* 次元 */
      	 << k << "\t"                               /* クラスタ数 */
	 << diff_timeval(t_start, t_end) << "\t"    /* 実行時間(us) */
	 << Hamerly_sqerr(n, k, data, c, a) << "\t" /* 平均二乗誤差 */
	 << t << endl;                              /* 繰り返し回数*/

    return;
  }
}

int main(int argc, char *argv[]){
  if(argc < 5){
    cerr << "usage: " << argv[0]
	 << " <n: data num> <d: dimension> <k: cluster num> <data file>" << endl;
    exit(1);
  }else{
    unsigned int n = atoi(argv[1]), d = atoi(argv[2]), k = atoi(argv[3]);
    char *file = argv[4];
    ublas::matrix<double> data(n, d); /* n個のd次元データを読み込む */
    {
      ifstream fs(file);
      if(fs.fail()){ exit(1); }
      for(unsigned int i = 0; i < n; ++i){
	for(unsigned int j = 0; j < d; ++j){
	  fs >> data(i,j);
	}
      }
      fs.close();
    }
    Hamerly(n, d, k, data);
    return 0;
  }
}


#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

int main() {
    //
    // check multiprecision/doc/html/boost_multiprecision/tut/ints/cpp_int.html
    // for checked & unchecked types
    //

    // integer number type
    using boost::multiprecision::cpp_int;   // unlimited precision
    using boost::multiprecision::int1024_t;
    using boost::multiprecision::uint1024_t;

    using boost::multiprecision::checked_cpp_int;
    using boost::multiprecision::checked_int1024_t;
    using boost::multiprecision::checked_uint1024_t;

    // rational number type
    using boost::multiprecision::cpp_rational;

    // floating point number type
    using boost::multiprecision::cpp_dec_float_100;
    using cpp_dec_float_200 = boost::multiprecision::number < boost::multiprecision::cpp_dec_float < 200 > > ;

    //
    // real code below
    //

    // calculate factorial of 50
    cpp_int factorial{ 1 };
    // cpp_int factorial{ "1" }; also works
    for (cpp_int i{ 1 }; i <= 50; ++i)
        factorial *= i;

    std::cout << "Factorial of 50 is\n\t" << factorial << "\n\n";

    cpp_rational rational{ cpp_int{ 321 }, cpp_int{ 123 } };
    // cpp_rational rational{ "321/123" }; also works
    rational *= 10;
    // print 50 numbers of rational
    std::cout << rational << " is \n\t"
        << rational.convert_to<cpp_dec_float_100>().str(50) << "\n\n";

    // calculate square root of 2
    std::cout << "sqrt(2)\n\t"
        << std::setprecision(std::numeric_limits<cpp_dec_float_200>::digits10)
        << boost::multiprecision::sqrt(cpp_dec_float_200{ 2 });
}


#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <time.h>
#include "re3q3/re3q3.h"

#define TEST(FUNC) if(!FUNC()) { std::cout << #FUNC"\033[1m\033[31m FAILED!\033[0m\n"; } else { std::cout << #FUNC"\033[1m\033[32m PASSED!\033[0m\n"; passed++;} num_tests++;
#define REQUIRE(COND) if(!(COND)) { std::cout << "Failure: "#COND" was not satisfied.\n"; return false; }

void compute_equation_residuals(const Eigen::Matrix<double, 3, 10> & coeffs, const Eigen::Matrix<double, 3, 8> & solution, int n_sols, double res[8]) {
	Eigen::Matrix<double, 10, 1> mons;

	for (int i = 0; i < n_sols; i++) {
		double x = solution(0, i);
		double y = solution(1, i);
		double z = solution(2, i);
		mons << x * x, x* y, x* z, y* y, y* z, z* z, x, y, z, 1.0;
		Eigen::Matrix<double, 3, 1> residuals = coeffs * mons;
		res[i] = residuals.cwiseAbs().maxCoeff();
	}
}

bool verify_solutions(const Eigen::Matrix<double, 3, 10> &coeffs,
					const Eigen::Matrix<double, 3, 8> &solutions,
					int n_sols, double tol) {
	bool ok = true;

	double res[8];
	compute_equation_residuals(coeffs, solutions, n_sols, res);

	for (int i = 0; i < n_sols; i++) {
		ok &= res[i] < tol;
	}
	return ok;
}

bool test_random_coefficients() {
	Eigen::Matrix<double, 3, 10> coeffs;
	Eigen::Matrix<double, 3, 8> solutions;

	coeffs.setRandom();

	//std::cout << "coeffs: " << coeffs << "\n";

	int n_sols = re3q3::re3q3(coeffs, &solutions);

	return verify_solutions(coeffs, solutions, n_sols, 1e-8);
}

bool test_degenerate_for_x() {
	Eigen::Matrix<double, 3, 10> coeffs;
	Eigen::Matrix<double, 3, 8> solutions;

	coeffs.setRandom();
	coeffs.col(3) = 0.5 * (coeffs.col(5) + coeffs.col(4));

	int n_sols = re3q3::re3q3(coeffs, &solutions);

	return verify_solutions(coeffs, solutions, n_sols, 1e-8);
}

bool test_degenerate_for_y() {
	Eigen::Matrix<double, 3, 10> coeffs;
	Eigen::Matrix<double, 3, 8> solutions;

	coeffs.setRandom();
	coeffs.col(0) = 0.5 * (coeffs.col(5) + coeffs.col(2));

	int n_sols = re3q3::re3q3(coeffs, &solutions);

	return verify_solutions(coeffs, solutions, n_sols, 1e-8);
}

bool test_degenerate_for_z() {
	Eigen::Matrix<double, 3, 10> coeffs;
	Eigen::Matrix<double, 3, 8> solutions;

	coeffs.setRandom();
	coeffs.col(0) = 0.5 * (coeffs.col(1) + coeffs.col(3));

	int n_sols = re3q3::re3q3(coeffs, &solutions);

	return verify_solutions(coeffs, solutions, n_sols, 1e-8);
}

bool test_degenerate_for_xy() {
	Eigen::Matrix<double, 3, 10> coeffs;
	Eigen::Matrix<double, 3, 8> solutions;

	coeffs.setRandom();
	coeffs.col(0) = 0.5 * (coeffs.col(5) + coeffs.col(2));
	coeffs.col(3) = 0.5 * (coeffs.col(5) + coeffs.col(4));

	int n_sols = re3q3::re3q3(coeffs, &solutions);

	return verify_solutions(coeffs, solutions, n_sols, 1e-8);
}

bool test_pure_squares() {
	Eigen::Matrix<double, 3, 10> coeffs;
	Eigen::Matrix<double, 3, 8> solutions;

	coeffs.setZero();
	coeffs(0, 0) = 1.0;
	coeffs(0, 9) = -1.0;
	coeffs(1, 3) = 1.0;
	coeffs(1, 9) = -1.0;
	coeffs(2, 5) = 1.0;
	coeffs(2, 9) = -1.0;

	int ok = 0;

	// We run multiple tests here since the change of variables is random.
	for(int test = 0; test < 1000; ++test) {
		int n_sols = re3q3::re3q3(coeffs, &solutions);

		REQUIRE(n_sols == 8);

		if(verify_solutions(coeffs, solutions, n_sols, 1e-8))
			++ok;
	}
	return ok == 1000;
}

bool benchmark_random_coeffs() {

	std::vector<double> residuals;
	residuals.reserve(10000 * 8);

	for (int iter = 0; iter < 10000; ++iter) {
		Eigen::Matrix<double, 3, 10> coeffs;
		Eigen::Matrix<double, 3, 8> solutions;
		coeffs.setRandom();

		int n_sols = re3q3::re3q3(coeffs, &solutions);

		double res[8];
		compute_equation_residuals(coeffs, solutions, n_sols, res);
		for (int i = 0; i < n_sols; ++i)
			residuals.push_back(std::log10(res[i]));
	}

	std::sort(residuals.begin(), residuals.end());

	double q90 = residuals[static_cast<int>(residuals.size() * 0.90)];
	double q95 = residuals[static_cast<int>(residuals.size() * 0.95)];
	double q99 = residuals[static_cast<int>(residuals.size() * 0.99)];

	std::cout << "q90: " << q90 << ", q95: " << q95 << ", q99: " << q99 << "\n";

	return q99 < -6;
}

bool benchmark_degen_rotation_homogeneous() {

	std::vector<double> residuals;
	residuals.reserve(10000 * 8);

	Eigen::Matrix<double, 3, 10> coeffs;
	Eigen::Matrix<double, 3, 9> Rcoeffs;
	Eigen::Matrix<double, 4, 8> solutions;

	for (int iter = 0; iter < 10000; ++iter) {

		// Generate random 180 degree rotation
		Eigen::Quaterniond q_gt;
		q_gt.coeffs().setRandom();
		q_gt.coeffs()(0) = 0.0;
		q_gt.coeffs().normalize();

		// Problem is x1'*R*x2 = 0
		// => kron(x2',x1') * vec(R) = 0

		for (int i = 0; i < 3; ++i) {
			Eigen::Vector3d x1, x2, v;
			x2.setRandom().normalize();
			v.setRandom();
			x1 = v.cross(q_gt.toRotationMatrix() * x2).normalized();

			Rcoeffs.row(i) << x2(0) * x1.transpose(), x2(1)* x1.transpose(), x2(2)* x1.transpose();
		}

		int n_sols = re3q3::re3q3_rotation(Rcoeffs, &solutions);

		double res = 1.0;
		for (int i = 0; i < n_sols; ++i) {
			Eigen::Vector4d q = solutions.col(i);
			Eigen::Matrix3d R = Eigen::Quaterniond(q).toRotationMatrix();
			res = std::min(res, (R - q_gt.toRotationMatrix()).norm());
		}
		residuals.push_back(std::log10(res));
	}

	std::sort(residuals.begin(), residuals.end());

	double q90 = residuals[static_cast<int>(residuals.size() * 0.90)];
	double q95 = residuals[static_cast<int>(residuals.size() * 0.95)];
	double q99 = residuals[static_cast<int>(residuals.size() * 0.99)];

	std::cout << "q90: " << q90 << ", q95: " << q95 << ", q99: " << q99 << "\n";

	return q99 < -6;
}

bool benchmark_degen_rotation_inhomogeneous() {

	std::vector<double> residuals;
	residuals.reserve(10000 * 8);

	Eigen::Matrix<double, 3, 10> coeffs;
	Eigen::Matrix<double, 3, 10> Rcoeffs;
	Eigen::Matrix<double, 4, 8> solutions;

	for (int iter = 0; iter < 10000; ++iter) {

		// Generate random 180 degree rotation
		Eigen::Quaterniond q_gt;
		q_gt.coeffs().setRandom();
		q_gt.coeffs()(0) = 0.0;
		q_gt.coeffs().normalize();

		// Problem is x1'*R*x2 = d
		// => kron(x2',x1') * vec(R) = d

		for (int i = 0; i < 3; ++i) {
			Eigen::Vector3d x1, x2, v;
			x2.setRandom().normalize();
			x1.setRandom().normalize();
			double d = -x1.dot(q_gt.toRotationMatrix() * x2);
			Rcoeffs.row(i) << x2(0) * x1.transpose(), x2(1)* x1.transpose(), x2(2)* x1.transpose(), d;
		}

		int n_sols = re3q3::re3q3_rotation(Rcoeffs, &solutions);

		double res = 1.0;
		for (int i = 0; i < n_sols; ++i) {
			Eigen::Vector4d q = solutions.col(i);
			Eigen::Matrix3d R = Eigen::Quaterniond(q).toRotationMatrix();
			res = std::min(res, (R - q_gt.toRotationMatrix()).norm());
		}
		residuals.push_back(std::log10(res));
	}

	std::sort(residuals.begin(), residuals.end());

	double q90 = residuals[static_cast<int>(residuals.size() * 0.90)];
	double q95 = residuals[static_cast<int>(residuals.size() * 0.95)];
	double q99 = residuals[static_cast<int>(residuals.size() * 0.99)];

	std::cout << "q90: " << q90 << ", q95: " << q95 << ", q99: " << q99 << "\n";

	return q99 < -6;
}

int main() {

	unsigned int seed = (unsigned int)time(0);
	srand(seed);

	std::cout << "Running tests... (seed = " << seed << ")\n\n";

	int passed = 0;
	int num_tests = 0;

	TEST(test_random_coefficients);
	TEST(test_degenerate_for_x);
	TEST(test_degenerate_for_y);
	TEST(test_degenerate_for_z);
	TEST(test_degenerate_for_xy);
	TEST(test_pure_squares);
	TEST(benchmark_random_coeffs);
	TEST(benchmark_degen_rotation_homogeneous);
	TEST(benchmark_degen_rotation_inhomogeneous);

	std::cout << "\nDone! Passed " << passed << "/" << num_tests << " tests.\n";
}


//!
//! Contains the interface for the multivariate Gaussian distribution.
//!
//! \file distrib/multigaussian.hpp
//! \author Alistair Reid
//! \date April 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#pragma once

#include <cmath>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Dense>
#include <Eigen/Cholesky>

namespace obsidian
{
  namespace distrib
  {
    //! Represents a multivariate Gaussian distribution.
    //!
    struct MultiGaussian
    {
      //! Create a multivariate Gaussian distribution.
      //!
      //! \param mu The means of each dimension of the distribution.
      //! \param sigma The covariance matrix.
      //!
      MultiGaussian(const Eigen::VectorXd& mu, const Eigen::MatrixXd& sigma);

      //! Create a multivariate Gaussian distribution.
      //!
      //! \param mu The means of each dimension of the distribution.
      //! \param sigma The covariance matrix.
      //! \param w, h The shape of the distribution.
      //!
      MultiGaussian(const Eigen::VectorXd& mu, const Eigen::MatrixXd& sigma, int w, int h);

      //! The means of each dimension.
      Eigen::VectorXd mu;
      Eigen::MatrixXd sigLInv;
      Eigen::MatrixXd sigL;

      //! The covariance matrix.
      Eigen::MatrixXd sigma;

      //! The shape of the distribution.
      std::pair<uint, uint> shape;
    };

    //!
    MultiGaussian coupledGaussianBlock(const Eigen::MatrixXd& mean, double coupledSD, double decoupledSD);

    double crankNicolsonLogPDF(const Eigen::VectorXd& theta, const MultiGaussian& input, const Eigen::VectorXd& thetaMin, const Eigen::VectorXd& thetaMax, const double& ro);

    //! Compute the log PDF of a multivariate Gaussian distribution
    double logPDF(const Eigen::MatrixXd& theta, const MultiGaussian& input, const Eigen::MatrixXd& thetaMin,
                  const Eigen::MatrixXd& thetaMax);

    //! Compute the log PDF of a multivariate Gaussian distribution
    double logPDF(const Eigen::VectorXd& theta, const MultiGaussian& input, const Eigen::VectorXd& thetaMin,
                  const Eigen::VectorXd& thetaMax);

    double uniformLogPDF(const Eigen::MatrixXd& theta, const MultiGaussian& input, const Eigen::MatrixXd& thetaMins,
                         const Eigen::MatrixXd& thetaMaxs);

    //! Draw a sample from a multivariate Gaussian distribution.
    //
    Eigen::MatrixXd drawValues(const MultiGaussian &input, std::mt19937 &gen);

    //! Draw a sample from a multivariate Gaussian distribution.
    //
    std::vector<Eigen::MatrixXd> drawFrom(const std::vector<distrib::MultiGaussian>& prior, std::mt19937& gen,
                                          const std::vector<Eigen::MatrixXd>& mins, const std::vector<Eigen::MatrixXd>& maxs,
                                          const std::vector<bool>& uniformFlags);

    //! Draw a sample from a multivariate Gaussian distribution.
    //
    std::vector<Eigen::VectorXd> drawVectorFrom(const std::vector<distrib::MultiGaussian>& prior, std::mt19937& gen,
                                                const std::vector<Eigen::VectorXd>& mins, const std::vector<Eigen::VectorXd>& maxs);

  }
}


//==============================================================================
//         Copyright 2015 J.T. Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/arithmetic/include/functions/tenpower.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/oneo_10.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/hundred.hpp>
#include <boost/simd/include/functions/sqr.hpp>

NT2_TEST_CASE_TPL ( tenpower_unsigned_int, BOOST_SIMD_UNSIGNED_TYPES)
{

  using boost::simd::tenpower;
  using boost::simd::tag::tenpower_;
  typedef typename boost::dispatch::meta::call<tenpower_(T)>::type r_t;
  typedef typename boost::dispatch::meta::as_floating<T>::type  wished_r_t;

  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::One<T>()), boost::simd::Ten<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::Two<T>()), boost::simd::Hundred<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::Zero<T>()), boost::simd::One<r_t>(), 0.5);
} // end of test for unsigned_int_

NT2_TEST_CASE_TPL ( tenpower_signed_int,  BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{

  using boost::simd::tenpower;
  using boost::simd::tag::tenpower_;
  typedef typename boost::dispatch::meta::call<tenpower_(T)>::type r_t;
  typedef typename boost::dispatch::meta::as_floating<T>::type  wished_r_t;

  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::Mone<T>()), boost::simd::Oneo_10<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::One<T>()), boost::simd::Ten<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::Two<T>()), boost::simd::Hundred<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::Mtwo<T>()), boost::simd::sqr(boost::simd::Oneo_10<r_t>()), 0.5);
  NT2_TEST_ULP_EQUAL(tenpower(boost::simd::Zero<T>()), boost::simd::One<r_t>(), 0.5);
} // end of test for signed_int_


#include <Eigen/Dense>

template<typename T>
DenseMatrix<T>::DenseMatrix(int m, int n)
{
    data = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(m, n);
}

template<typename T>
DenseMatrix<T>::DenseMatrix(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& data_):
data(data_)
{

}

template<typename T>
DenseMatrix<T>::DenseMatrix(const DenseMatrix<T>& B):
data(B.copy())
{

}

template<typename T>
DenseMatrix<T>& DenseMatrix<T>::operator=(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& data_)
{
    if (&data != &data_) data = data_;

    return *this;
}

template<typename T>
DenseMatrix<T>& DenseMatrix<T>::operator=(const DenseMatrix<T>& B)
{
    if (this != &B) data = B.copy();

    return *this;
}

template<typename T>
DenseMatrix<T> DenseMatrix<T>::identity(int m, int n)
{
    return DenseMatrix<T>(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Identity(m, n));
}

template<typename T>
DenseMatrix<T> DenseMatrix<T>::ones(int m, int n)
{
    return DenseMatrix<T>(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Ones(m, n));
}

template<typename T>
DenseMatrix<T> DenseMatrix<T>::constant(int m, int n, const T& x)
{
    return DenseMatrix<T>(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Constant(m, n, x));
}

template<typename T>
DenseMatrix<T> DenseMatrix<T>::random(int m, int n)
{
    return DenseMatrix<T>(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Random(m, n));
}

template<typename T>
DenseMatrix<T> DenseMatrix<T>::transpose() const
{
    return DenseMatrix<T>(data.transpose());
}

template<typename T>
DenseMatrix<T> DenseMatrix<T>::conjugate() const
{
    return DenseMatrix<T>(data.conjugate());
}

template<typename T>
int DenseMatrix<T>::nRows() const
{
    return (int)data.rows();
}

template<typename T>
int DenseMatrix<T>::nCols() const
{
    return (int)data.cols();
}

template<typename T>
double DenseMatrix<T>::norm(int n) const
{
    if (n == 0) return data.template lpNorm<Eigen::Infinity>();
    else if (n == 1) return data.template lpNorm<1>();
    return data.norm();
}

template<typename T>
double DenseMatrix<T>::rank() const
{
    Eigen::ColPivHouseholderQR<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>> qr(data);
    return qr.rank();
}

template<typename T>
T DenseMatrix<T>::sum() const
{
    return data.sum();
}

template<typename T>
DenseMatrix<T> DenseMatrix<T>::subMatrix(int r0, int r1, int c0, int c1) const
{
    return DenseMatrix<T>(data.block(r0, c0, r1 - r0, c1 - c0));
}

template<typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> DenseMatrix<T>::copy() const
{
    return data;
}

template<typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& DenseMatrix<T>::toEigen()
{
    return data;
}

template<typename T>
DenseMatrix<T> DenseMatrix<T>::operator*(const T& s)
{
    return DenseMatrix<T>(data*s);
}

template<typename T>
DenseMatrix<T> DenseMatrix<T>::operator+(DenseMatrix<T> *B)
{
    return DenseMatrix<T>(data + B->data);
}

template<typename T>
DenseMatrix<T> DenseMatrix<T>::operator-(DenseMatrix<T> *B)
{
    return DenseMatrix<T>(data - B->data);
}

template<typename T>
DenseMatrix<T> DenseMatrix<T>::operator*(DenseMatrix<T> *B)
{
    return DenseMatrix<T>(data*B->data);
}

template<typename T>
DenseMatrix<T> DenseMatrix<T>::operator-()
{
    return DenseMatrix<T>(data*T(-1.0));
}

template<typename T>
void DenseMatrix<T>::operator*=(const T& s)
{
    data *= s;
}

template<typename T>
void DenseMatrix<T>::operator+=(DenseMatrix<T> *B)
{
    data += B->data;
}

template<typename T>
void DenseMatrix<T>::operator-=(DenseMatrix<T> *B)
{
    data -= B->data;
}

template<typename T>
T DenseMatrix<T>::get(int r, int c) const
{
    return data(r, c);
}

template<typename T>
void DenseMatrix<T>::set(int r, int c, const T& s)
{
    data(r, c) = s;
}

template<typename T>
DenseMatrix<T> DenseMatrix<T>::hcat(DenseMatrix<T> *B)
{
    int m = data.rows();
    int n1 = data.cols();
    int n2 = B->data.cols();
    DenseMatrix<T> C(m, n1 + n2);

    for (int i = 0; i < m; i++) {
        for (int j = 0; j < n1; j++) {
            C.set(i, j, data(i, j));
        }

        for (int j = 0; j < n2; j++) {
            C.set(i, n1 + j, B->data(i, j));
        }
    }

    return C;
}

template<typename T>
DenseMatrix<T> DenseMatrix<T>::vcat(DenseMatrix<T> *B)
{
    int m1 = data.rows();
    int m2 = B->data.rows();
    int n = data.cols();
    DenseMatrix<T> C(m1 + m2, n);

    for (int j = 0; j < n; j++) {
        for (int i = 0; i < m1; i++) {
            C.set(i, j, data(i, j));
        }

        for (int i = 0; i < m2; i++) {
            C.set(m1 + i, j, B->data(i, j));
        }
    }

    return C;
}


/* **********************************************************************
 * Demonstration code for NPDE lecture & homeworks
 ********************************************************************** */

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

using namespace std;

int main(int, char **) {
  cout << "Demonstration of initialization of sparse matrix in eigen" << endl;

  /* SAM_LISTING_BEGIN_1 */
  const int n = 20, m = 10;
  // Set up zero sparse matrix with row major storage format
  // This format is essential for being able to set the maximal
  // number of non-zero entries \textbf{per row}.
  Eigen::SparseMatrix<int, Eigen::RowMajor> X(n, m);
  // Reserve space for at most nnz\_row non-zero entries per row
  const std::size_t nnz_row = 3;
  X.reserve(Eigen::VectorXi::Constant(n, nnz_row));
  // Initialize nnz\_row  entries per row
  for (int row_idx = 0; row_idx < n; ++row_idx) {
    for (int k = 0; k < nnz_row; ++k) {
      X.coeffRef(row_idx, (row_idx * k) % m) += 1;
    }
  }
  /* SAM_LISTING_END_1 */
  Eigen::MatrixXi X_dense = X;
  cout << "Matrix X = " << endl << X_dense << endl;
  return 0;
}