#include "mex.h"        // to compile in Matlab
#include <iostream> 	// to check outputs here and there
#include <cstdlib>    	// to use srand (if you uncomment the relevant line)
#include <string>   	// to read in the kernel name
#include <vector>       // to construct the index list
#include <map>	    	// to map kernels to integers for the switch
#include <Eigen/Core>   // to use basic Eigen structures
using namespace Eigen;
using namespace std;

//Commented lines contain various checks on integrity

//Date of last editing: Saturday 8th of September 2012

float LinearInterpolation (VectorXf X , VectorXf Y, float X_PointOfInterest){
//Produce Y_point_of_interest given X,Y and target X_point_of_interest
//X : vector containing the X variables of the interpolant
//Y : vector containing the Y variables of the interpolant
//PointOfInterest : Point of X to estimate the new point of Y

  float   xk, xkp1, yk, ykp1;  //Points adjecent to the point of interpolation
  if ( X.size() != Y.size() ){cout << "Problem with vector sizes" << endl; return(-1);}
//cout <<  " X(0): " <<  X(0) <<" X(Y.size()-1): " <<X(Y.size()-1)   <<   " Point of interest: " << X_PointOfInterest<< endl;
  if ( X_PointOfInterest < X(0) || X_PointOfInterest > X(Y.size()-1) ){cout << "You interpolate out of the curve boundaries" << endl; return(-1);}
//Find the points right before and right after the point of interest
  for (int i=1; i<X.size() ; i++){
    if (X(i)>= X_PointOfInterest){
      xkp1 = X(i);
      xk = X(i-1);
      ykp1 = Y(i);
      yk = Y(i-1);
      break;}
  }
//point-slope form for a line formula
  float t = (X_PointOfInterest -xk)/(xkp1 -xk);
  float yPOI = (1-t) * yk + t * ykp1;  // estimate point of interest
// cout << "(" << xk << ",  " << X_PointOfInterest << " , " << xkp1 << ") & (" << yk << ", " << yPOI  << ", " << ykp1 << ")"<< endl;
  return (yPOI);
}

VectorXf fnvalspapi(VectorXf X, VectorXf Y, VectorXf X_target){
  //evaluate Y_target for X_target given X and Y

	int N =  X_target.size();
	VectorXf rr(N);
	for (int i=0; i<N ; i++){
      rr(i) =  LinearInterpolation(X, Y, X_target(i)) ;  }
	return(rr);
}

int MonotonicityCheck(VectorXf X){
   // evaluate vector monotonicity of vector X
   int N =  X.size();
   for (int i=0; i<(N-1) ; i++){
     if ( X(i) >X (1+i) ) return (-1);   }
   return (1);
}

float rttemp1( VectorXf t_reg,  VectorXf curvei, VectorXf curvek,int nknots, float lambda, VectorXf initial){
  //Calculate the cost of the given warping
  // t_reg  : time grid of y_reg
  // curvei : query curve
  // curvek : reference curve
  // nknots : number of knots
  // lambda : time distortion penalty parameter
  // initial: position of knots on the simplex

  int N = t_reg.size();
  VectorXf struct_ = VectorXf::LinSpaced( nknots+2, t_reg(0)  , t_reg.maxCoeff() );
  VectorXf hik(N);
  VectorXf Q(2+ initial.size()) ;           //Solution with the placement of the knots on the simplex
  Q(0) = t_reg(0) ; Q(1+ initial.size()) = t_reg.maxCoeff()  ; Q.segment(1, initial.size()) = initial;
  hik =  fnvalspapi( struct_, Q , t_reg);   // compute the new internal time-scale

  //cout << "hik: " << hik.transpose() << endl;
  //cout << "Monotonicity Checked on Hik: " << MonotonicityCheck(hik) << endl;
  //if(  MonotonicityCheck(hik) == -1) { cout <<" Q.transpose()  is :"<< Q.transpose()  << endl;}

  return ( (fnvalspapi(t_reg,curvei,hik)-fnvalspapi(t_reg,curvek ,t_reg)).array().pow(2).sum() + lambda * (hik - t_reg).array().pow(2).sum() );
}

VectorXf NewSolution( VectorXf x0 , float Step, int point, VectorXf t_reg){
    //generate new solution on the simplex defined in [t_reg(0)-x0-t_reg(N-1)] using a displacement of size Step
    // x0   : initial solution
    // Step : displacement size
    // point: knot to perturb
    // t_reg: time_grid

    VectorXf InitConf (2+ x0.size());
    InitConf(0) = t_reg(0); InitConf( x0.size()+1) = t_reg.maxCoeff()  ; InitConf.segment(1, x0.size()) = x0;
    float LowBou =  InitConf(point-1);
    float UppBou =  InitConf(point+1);
    float New_State = (UppBou - LowBou) * Step + LowBou;
    InitConf(point) = New_State;
    //if (  MonotonicityCheck( InitConf.segment(1, x0.size()) ) == -1) {
    //    cout << "We generated a unacceptable solutiion" << endl << "Initial seed was : " << x0.transpose()
    //         << endl << " and we produced :"<<InitConf.segment(1, x0.size()).transpose() << endl;}
    return (InitConf.segment(1, x0.size()) ) ;
  }

//====
typedef struct { float Val; VectorXf Mapping;} rthink_Output; //define structure that holds the results.
//====

rthink_Output rthik_SA(VectorXf t_reg,  VectorXf curvei, VectorXf curvek,int nknots, float lambda){
  // Random Search solver for the optimization problem of pairwise warping
  // t_reg : time_grid
  // curvei: query curve
  // curvek: reference curve
  // nknots : number of knots
  // lambda : time distortion penalty parameter

  int k=0; float OldSol, bk;
  VectorXf xk(nknots);
  bk = (t_reg.maxCoeff() - t_reg(0))/(1+ float(nknots ));   //Distance between adjacent knots and edges-knots
  xk  = VectorXf::LinSpaced(nknots , t_reg(0) + bk  ,  - bk + t_reg.maxCoeff()  ); //Initial candidate solution with equispaced knots
  VectorXf xn(nknots);   VectorXf help(2+nknots);

  float NewSol;
  OldSol =  rttemp1(t_reg, curvei, curvek, nknots , lambda, xk);  //Cost of initial solution
  int z= 99*nknots;                                               //Number of random search to do (proportional to the # of knots)
  //srand(1);                                                     //Fix the seed to have reproducable behaviour
  VectorXf Steps(z); Steps = (ArrayXf::Random(z)+1.)/2.;          //Generate possible random pertubations magnitude
  VectorXi Posit(z); for (int u=0; u <z; u++ ) Posit(u) =1+ rand()%(nknots+0); //Generate list of positions to purturb

  k=0;
  while((OldSol > .0001) && (k<z)) {
    xn = NewSolution(xk,Steps(k), Posit(k), t_reg );              //Get a new solution
    NewSol = rttemp1(t_reg, curvei, curvek, nknots, lambda, xn);  //Cost of new solution
    if (  (NewSol < OldSol)  ) {                                  //If it's better than the old one, use it.
      OldSol= NewSol; xk= xn;
    }
  k++;
}

  VectorXf x3 =  VectorXf::LinSpaced(2+nknots, t_reg(0), t_reg( t_reg.size()-1));
  help(0) =  t_reg(0) ; help(nknots+1) = t_reg( t_reg.size()-1) ; help.segment(1,nknots) = xk;

  rthink_Output G;
  G.Val = OldSol;
  G.Mapping = fnvalspapi(  x3 , help  , t_reg);
  return G ;
}

//Use with syntax :  [hik(indd,:),ftemp]=rthik_E( curvei.coefs,curvek.coefs,t_reg,nknots,lambda);
#define IS_REAL_1D_FULL_DOUBLE(P) (!mxIsComplex(P) && mxGetNumberOfDimensions(P) == 2 && !mxIsSparse(P) && mxIsDouble(P))

void mexFunction (int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[])
{
    #define p_curvei	prhs[0]
	#define p_curvek	prhs[1]
	#define p_treg		prhs[2]
	#define p_knots		prhs[3]
	#define p_lambda	prhs[4]

    #define p_hik         plhs[0]
    #define p_f           plhs[1]

    if(nrhs != 5)       // Check the number of input arguments
    mexErrMsgTxt("Wrong number of input arguments.");
    else if(!IS_REAL_1D_FULL_DOUBLE(p_curvei))   //  Check lw
     mexErrMsgTxt("curvei needs be a real 2D full double array.");
    else if(!IS_REAL_1D_FULL_DOUBLE(p_curvek))   //  Check lw
     mexErrMsgTxt("curvek needs be a real 2D full double array.");
    else if(!IS_REAL_1D_FULL_DOUBLE(p_treg))   //  Check lw
     mexErrMsgTxt("t_reg needs be a real 2D full double array.");

   const int LengthOfVector = mxGetN(p_treg);

   //Read in the variables
   Map<VectorXd> CurvI(mxGetPr(p_curvei) , LengthOfVector);
   Map<VectorXd> CurvK(mxGetPr(p_curvek) , LengthOfVector);
   Map<VectorXd> T_Reg(mxGetPr(p_treg)   , LengthOfVector);

   int knots = *mxGetPr(p_knots);
   double lamdba = *mxGetPr(p_lambda);
   srand(12);
   //Past as floats cause really nobody cares about the small digits there.
   rthink_Output Res = rthik_SA(T_Reg.cast<float>(),CurvI.cast<float>(),CurvK.cast<float>(), knots, float(lamdba));

  //Declare the outputs
  p_hik = mxCreateDoubleMatrix(1, LengthOfVector, mxREAL);
  p_f   = mxCreateDoubleMatrix(1, 1, mxREAL);

  double *hik = mxGetPr(p_hik );
  double *f = mxGetPr(p_f );
  for (int u=0; u< LengthOfVector; u++) { hik[u] = Res.Mapping(u);}
  f[0] = Res.Val;

}


#include "RBGL.hpp"
#include "Basic2DMatrix.hpp"
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>

extern "C"
{

#include <Rdefines.h>

    using namespace std;
    using namespace boost;

    typedef std::set<int> SubgraphAsSet;
    typedef std::vector< SubgraphAsSet > CliqueVector;
    typedef std::vector< CliqueVector > ResultCliquesType;

    static void addNewClique(CliqueVector& cliques, int i, int j)
    {
        SubgraphAsSet s;
        s.insert(i);
        s.insert(j);
        cliques.push_back(s);
    }

    static void findAllCliques(ResultCliquesType& rCliques,
                               Basic2DMatrix<double>& D)
    {
        CliqueVector cliques;
        SubgraphAsSet::iterator s;
        CliqueVector::iterator ci, cj;
        int i, j, k, N=0;
        const int nv = D.numrows();

        // N: max distance in given graph
        for ( i = 0; i < nv; i++ )
            for ( j = i+1; j < nv; j++ )
            {
                N = max(N, (int)D[i][j]);
                // each edge is 1-clique
                if ( D[i][j] == 1 ) addNewClique(cliques, i, j);
            }

        for ( k = 1; k <= N; k++ )
        {
            for ( i = 0; i < nv; i++ )
            {
                for ( ci = cliques.begin(); ci != cliques.end(); ci++ )
                {
                    // i is already in this clique
                    if ( (*ci).find(i) != (*ci).end() ) continue;

                    for ( s = (*ci).begin(); s != (*ci).end(); s++ )
                    {
                        if ( D[i][*s] > k || D[*s][i] > k ) break;
                    }

                    // add i to this clique
                    if ( s == (*ci).end() )
                    {
                        (*ci).insert(i);

                        // eliminate its subsequent subsets
                        for ( cj = ci + 1; cj != cliques.end(); )
                        {
                            if (includes((*ci).begin(), (*ci).end(),
                                         (*cj).begin(), (*cj).end()) )
                                cj = cliques.erase(cj);
                            else
                                cj++;
                        }
                    }
                }
            }
            rCliques.push_back(cliques);
        }

#if DEBUG
        cout << " Cliques: " << endl;
        for ( i = 0; i < rCliques.size(); i++ )
        {
            cout << i+1 << " cliques: " << endl;
            for ( ci=rCliques[i].begin(); ci!=rCliques[i].end(); ci++ )
            {
                cout << "    ";
                for ( s = (*ci).begin(); s != (*ci).end(); s++ )
                    cout << (*s)+1 << " ";
                cout << endl;
            }
        }
#endif
    }

    SEXP kCliques(SEXP num_verts_in, SEXP num_edges_in,
                  SEXP R_edges_in, SEXP R_weights_in)
    {
        // R_weights_in has to be INTEGER now
        int nv = INTEGER(num_verts_in)[0];

        Graph_ud g(num_verts_in, num_edges_in, R_edges_in, R_weights_in);

        Basic2DMatrix<double> D(nv, nv);

        // find out the shortest distance between any two nodes
        johnson_all_pairs_shortest_paths(g, D);

        // find k-cliques now
        ResultCliquesType rCliques;
        findAllCliques(rCliques, D);

        ResultCliquesType::iterator ci;
        CliqueVector::iterator vi;
        SubgraphAsSet::iterator si;
        int i, j, k;

        SEXP ansList, cList, sList;
        PROTECT(ansList = allocVector(VECSXP, (int)rCliques.size()));

        for ( i = 0, ci = rCliques.begin(); ci != rCliques.end(); i++, ci++)
        {
            PROTECT(cList = allocVector(VECSXP, (*ci).size()));
            for ( j = 0, vi = (*ci).begin(); vi != (*ci).end(); j++, vi++ )
            {
                PROTECT(sList = allocVector(INTSXP, (*vi).size()));
                for ( k = 0, si = (*vi).begin(); si != (*vi).end(); k++, si++ )
                {
                    INTEGER(sList)[k] = *si;
                }
                SET_VECTOR_ELT(cList,j,sList);
                UNPROTECT(1);
            }
            SET_VECTOR_ELT(ansList,i,cList);
            UNPROTECT(1);
        }
        UNPROTECT(1);
        return(ansList);
    }

    SEXP lambdaSets(SEXP num_verts_in, SEXP num_edges_in,
                    SEXP R_edges_in, SEXP R_capacity_in)
    {
        using namespace boost;

        typedef adjacency_list_traits<vecS, vecS, directedS> Tr;
        typedef Tr::edge_descriptor Tr_edge_desc;

        typedef adjacency_list<vecS, vecS, directedS, no_property,
        property<edge_capacity_t, double,
        property<edge_residual_capacity_t, double,
        property<edge_reverse_t, Tr_edge_desc> > > >
        FlowGraph;

        typedef graph_traits<FlowGraph>::vertex_descriptor vertex_descriptor;
	typedef graph_traits<FlowGraph>::edge_descriptor edge_descriptor;

        FlowGraph flow_g;

        property_map < FlowGraph, edge_capacity_t >::type
        cap = get(edge_capacity, flow_g);
        property_map < FlowGraph, edge_residual_capacity_t >::type
        res_cap = get(edge_residual_capacity, flow_g);
        property_map < FlowGraph, edge_reverse_t >::type
        rev_edge = get(edge_reverse, flow_g);

        edge_descriptor e1, e2;
        bool in1, in2;

        if (!isInteger(R_edges_in)) error("R_edges_in should be integer");

        int NV = INTEGER(num_verts_in)[0];
        int NE = asInteger(num_edges_in);
        int* edges_in = INTEGER(R_edges_in);
	int i, j, k, MaxC=0;

        for (i = 0; i < NE ; i++, edges_in += 2)
        {
            tie(e1, in1) = boost::add_edge(*edges_in, *(edges_in+1), flow_g);
            tie(e2, in2) = boost::add_edge(*(edges_in+1), *edges_in, flow_g);
            if ( !in1 || !in2 )
                error("unable to add edge: (%d, %d)", *edges_in, *(edges_in+1));

            // fill in capacity_map
            cap[e1] = 1;
            cap[e2] = 1;

            // fill in reverse_edge_map
            rev_edge[e1] = e2;
            rev_edge[e2] = e1;
        }

        // ASSUMPTION: max_flow(u, v) = max_flow(v, u)
        // compute edge connectivities between u and v, (u < v)
	// we only need lower-left triangle of the matrix w/o diagonal
        Basic2DMatrix<int> CC(NV, NV);

        for ( i = 0; i < NV; i++ )
        {
            vertex_descriptor s = vertex(i, flow_g);

            for ( j = 0; j < i; j++ )
            {
                vertex_descriptor t = vertex(j, flow_g);

                CC[i][j] = (int)edmonds_karp_max_flow(flow_g, s, t);
                MaxC = max(MaxC, CC[i][j]);
            }
        }

        // calc lambda sets by successively partition V
        Basic2DMatrix<int> P(MaxC+1, NV);
        for ( k = 0; k <= MaxC; k++ )
        {
            for ( i = 0; i < NV; i++ ) P[k][i] = i;

            for ( i = 1; i < NV; i++ )
                for ( j = 0; j < i; j++ )
                    if ( CC[i][j] >= k )  P[k][i] = P[k][j];
        }

#if DEBUG
        cout << " edge-connectivity matrix: " << endl;
        for ( i = 0; i < NV; i++ )
        {
            for ( j = 0; j < NV; j++ ) cout << CC[i][j] << " ";
            cout << endl;
        }

        cout << " P matrix: " << endl;
        for ( k = 0; k <= MaxC; k++ )
        {
            cout << " k = " << k << ": ";
            for ( j = 0; j < NV; j++ ) cout << P[k][j] << " ";
            cout << endl;
        }
#endif

        SEXP ansList, conn, eList;
        PROTECT(ansList = allocVector(VECSXP,2));
        PROTECT(conn = NEW_NUMERIC(1));
        PROTECT(eList = allocMatrix(INTSXP, MaxC+1, NV));

        REAL(conn)[0] = MaxC;

        for ( i = 0, j = 0; j < NV; j++ )
            for ( k = 0; k <= MaxC; k++ )
                INTEGER(eList)[i++] = P[k][j];

        SET_VECTOR_ELT(ansList,0,conn);
        SET_VECTOR_ELT(ansList,1,eList);
        UNPROTECT(3);

        return(ansList);
    }
}


#include <iostream>
#include <iomanip>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <random>
#include <set>

constexpr int seed = 1;

using Mat33_t = Eigen::Matrix3d;
using Vec3_t = Eigen::Vector3d;
using Vec2_t = Eigen::Vector2d;

template<typename T>
using eigen_vector = std::vector<T, Eigen::aligned_allocator<T>>;

eigen_vector<Vec3_t> gen_3Dpoints(const int n) {

    std::mt19937 mt(seed);
    const double z_dist = 3;
    const double r = 1;
    std::uniform_real_distribution<> rand(-r, r);

    eigen_vector<Vec3_t> pts;
    pts.reserve(n);
    for (unsigned int i = 0; i < n; i++) {
        const double x = rand(mt);
        const double y = rand(mt);
        const double z = rand(mt) + z_dist;
        pts.emplace_back(x, y, z);
    }
    return pts;
}

template<typename T>
void generate_keypts(const int num_pts, const double outlier_ratio,
                     T &kps_1, T &kps_2, std::vector<bool> &is_inliers) {

    double data[9];
    data[0] = 0.99875035875531126;
    data[1] = 0.043232155011259037;
    data[2] = 0.025073923889563768;
    data[3] = -0.043243275321131029;
    data[4] = 0.9990645671709939;
    data[5] = -9.8807568748168046e-05;
    data[6] = -0.025054740582133875;
    data[7] = -0.00098559449940248617;
    data[8] = 0.99968559486362751;

    Mat33_t R(data);
    Vec3_t t{-0.49, 0.12, -0.05};

    eigen_vector<Vec3_t> pts = gen_3Dpoints(num_pts);

    Mat33_t K;
    K << 500, 0, 500,
            0, 500, 250,
            0, 0, 1;

    kps_1.clear();
    kps_1.reserve(num_pts);
    kps_2.clear();
    kps_2.reserve(num_pts);

    std::mt19937 mt(seed);
    const double r = 1;
    std::uniform_real_distribution<> rand(-r, r);
    const auto try_bernoulli = [&mt](const double p) {
        std::uniform_real_distribution<> rand(0, 1);
        return rand(mt) < p;
    };

    for (const auto &pos_w : pts) {

        const double x = rand(mt);
        const double y = rand(mt);
        const double z = rand(mt);
        const Vec3_t noise{x, y, z};

        const bool is_outlier = try_bernoulli(outlier_ratio);

        const Vec3_t &pos_1 = pos_w;
        const Vec3_t pos_2 = R * pos_w + t
                             + (is_outlier ? noise : Vec3_t::Zero());

        kps_1.emplace_back((K * pos_1).hnormalized());
        kps_2.emplace_back((K * pos_2).hnormalized());
        is_inliers.push_back(!is_outlier);
    }
}

// Fundamental solver; Modified version of eightpt solver from opengv
// Reference:
// https://github.com/laurentkneip/opengv/blob/91f4b19c73450833a40e463ad3648aae80b3a7f3/src/relative_pose/methods.cpp#L424
template<typename T>
Mat33_t solve_F(const T &kps_1, const T &kps_2) {
    const int num_pts = kps_1.size();
    Eigen::MatrixXd A(num_pts, 9);

    for (unsigned int i = 0; i < num_pts; i++) {
        A.block<1, 3>(i, 0) = kps_2.at(i)(0) * kps_1.at(i).homogeneous();
        A.block<1, 3>(i, 3) = kps_2.at(i)(1) * kps_1.at(i).homogeneous();
        A.block<1, 3>(i, 6) = kps_1.at(i).homogeneous();
    }

    const Eigen::JacobiSVD<Eigen::MatrixXd> SVD(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
    const Eigen::Matrix<Mat33_t::Scalar, 9, 1> f = SVD.matrixV().col(8);
    Mat33_t F_temp(3, 3);
    F_temp.row(0) = f.block<3, 1>(0, 0).transpose();
    F_temp.row(1) = f.block<3, 1>(3, 0).transpose();
    F_temp.row(2) = f.block<3, 1>(6, 0).transpose();

    const Eigen::JacobiSVD<Mat33_t> SVD2(F_temp, Eigen::ComputeFullU | Eigen::ComputeFullV);

    Vec3_t s = SVD2.singularValues();
    s(2) = 0.0;

    const Mat33_t &U = SVD2.matrixU();
    const Mat33_t S = s.asDiagonal();
    const Mat33_t &V_T = SVD2.matrixV().transpose();

    return U * S * V_T;
}

template<typename T>
std::vector<bool> check_inlier(const Mat33_t &F, const T &kps_1, const T &kps_2) {
    std::vector<bool> inl_flag(kps_1.size(), false);
    for (int i = 0; i < kps_1.size(); i++) {
        const double err = kps_2.at(i).homogeneous().dot(F * kps_1.at(i).homogeneous());
        inl_flag.at(i) = std::abs(err) < 1e-8;
    }
    return inl_flag;
}

// Find F matrix by ransac
template<typename T>
Mat33_t estimate_F(const T &kps_1, const T &kps_2, const int num_trial) {
    const auto num_pts = kps_1.size();
    const auto num_sample = std::min<int>(8, num_pts);

    std::uniform_int_distribution<> dist(0, num_pts - 1);
    std::mt19937 mt(seed);

    Mat33_t F;
    int max_inl = 0;

    for (unsigned int i = 0; i < num_trial; i++) {
        std::set<int> sample_id;
        while (sample_id.size() < num_sample) {
            sample_id.insert(dist(mt));
        }

        T smpl1, smpl2;
        for (const auto id: sample_id) {
            smpl1.push_back(kps_1.at(id));
            smpl2.push_back(kps_2.at(id));
        }
        const Mat33_t F_cand = solve_F(smpl1, smpl2);
        const auto inl_flag = check_inlier(F_cand, kps_1, kps_2);
        const int num_inl = std::count(inl_flag.begin(), inl_flag.end(), true);
        if (max_inl < num_inl) {
            max_inl = num_inl;
            F = F_cand;
        }
    }

    return F;
}

template<typename T>
void print_test_result(const T &is_inliers, const T &inl_flag) {
    int true_pos = 0;
    int true_neg = 0;
    int false_pos = 0;
    int false_neg = 0;

    const auto num_pts = is_inliers.size();
    for (int i = 0; i < num_pts; i++) {
        const bool gt = is_inliers.at(i);
        const bool est = inl_flag.at(i);
        if (gt == est) {
            if (gt) true_pos++;
            else true_neg++;
        } else {
            if (est) false_pos++;
            else false_neg++;
        }
    }

    std::cout << std::endl;
    std::cout << "<< Confusion Matrix >>" << std::endl;
    std::cout << " GT \\ est | inlier | outlier |" << std::endl;

    std::cout << "  inlier  |  ";
    std::cout << std::setw(4) << true_pos << "  |  ";
    std::cout << std::setw(4) << false_neg << "   |" << std::endl;

    std::cout << " outlier  |  ";
    std::cout << std::setw(4) << false_pos << "  |  ";
    std::cout << std::setw(4) << true_neg << "   |" << std::endl;
}

int main() {
    const int num_pts = 1000;
    const double outlier_ratio = 0.3;

    // テスト用の特徴点と外れ値かどうかの正解
    eigen_vector<Vec2_t> kps_1, kps_2;
    std::vector<bool> is_inliers;
    generate_keypts(num_pts, outlier_ratio, kps_1, kps_2, is_inliers);

    // F行列を推定
    // num_trial は (1/(1-outlier_ratio)) ^ 8 より大きく
    const Mat33_t F = estimate_F(kps_1, kps_2, 100);

    // 推定したF行列を用いて特徴点マッチのインライア判定（インデックスでマッチ）
    const auto inl_flag = check_inlier(F, kps_1, kps_2);

    // F行列を用いたインライア判定の結果の表示
    const auto num_inls = std::count(inl_flag.begin(), inl_flag.end(), true);
    std::cout << "#inlier: " << num_inls << std::endl;

    // F行列推定による外れ値検出の正当性の表示
    print_test_result(is_inliers, inl_flag);
}


#include "eigen-helpers.hpp"

#include "perceive/geometry/rotation.hpp"

#include <Eigen/SVD>

using std::stringstream;

namespace perceive
{
// ------------------------------------------------------------------- Is finite
// Checks that an Eigen Matrix is finite
template<typename T> bool is_finiteT(const T& M)
{
   int n_rows = int(M.rows());
   int n_cols = int(M.cols());
   for(int i = 0; i < n_rows; ++i)
      for(int j = 0; j < n_cols; ++j)
         if(!std::isfinite(M(i, j))) return false;
   return true;
}

bool is_finite(const Vector2r& M) { return is_finiteT(M); }
bool is_finite(const Vector3r& M) { return is_finiteT(M); }
bool is_finite(const Vector4r& M) { return is_finiteT(M); }
bool is_finite(const Matrix3r& M) { return is_finiteT(M); }
bool is_finite(const Matrix34r& M) { return is_finiteT(M); }
bool is_finite(const MatrixXr& M) { return is_finiteT(M); }

// -------------------------------------------------------------------- str(...)

std::string str(const Vector2r& M) { return format("[{}, {}]", M(0), M(1)); }
std::string str(const Vector3r& M)
{
   return format("[{}, {}, {}]", M(0), M(1), M(2));
}
std::string str(const Vector4r& M)
{
   return format("[{}, {}, {}, {}]", M(0), M(1), M(2), M(3));
}
std::string str(const Matrix3r& M)
{
   stringstream ss("");
   ss << M;
   return ss.str();
}
std::string str(const Matrix34r& M)
{
   stringstream ss("");
   ss << M;
   return ss.str();
}
std::string str(const MatrixXr& M)
{
   stringstream ss("");
   ss << M;
   return ss.str();
}

std::string str(std::string name, const Matrix3r& M)
{
   stringstream ss("");
   ss << name << " = \n" << M << "\n";
   return ss.str();
}
std::string str(std::string name, const Matrix34r& M)
{
   stringstream ss("");
   ss << name << " = \n" << M << "\n";
   return ss.str();
}
std::string str(std::string name, const MatrixXr& M)
{
   stringstream ss("");
   ss << name << " = \n" << M << "\n";
   return ss.str();
}

// ------------------------------------------------------------------------- SVD

// -- Thin

real svd_thin(const MatrixXr& M, VectorXr& out)
{
   Eigen::JacobiSVD<MatrixXr> svd(M, Eigen::ComputeThinV);
   out = svd.matrixV().col(M.cols() - 1);
   return svd.singularValues()(M.cols() - 1);
}

real svd_thin(const Matrix3r& M, Vector3r& out)
{
   Eigen::JacobiSVD<MatrixXr> svd(M, Eigen::ComputeThinV);
   out = svd.matrixV().col(M.cols() - 1);
   return svd.singularValues()(M.cols() - 1);
}

// real svd_thin(const Matrix3d& M, Vector3d& out)
// {
//     Eigen::JacobiSVD<MatrixXd> svd(M, Eigen::ComputeThinV);
//     out = svd.matrixV().col(M.cols() - 1);
//     return svd.singularValues()(M.cols() - 1);
// }

// real svd_thin(const MatrixXd& M, VectorXd& out)
// {
//     Eigen::JacobiSVD<MatrixXd> svd(M, Eigen::ComputeThinV);
//     out = svd.matrixV().col(M.cols() - 1);
//     return svd.singularValues()(M.cols() - 1);
// }

real svd_thin(const MatrixXr& M, Vector6r& out)
{
   Eigen::JacobiSVD<MatrixXr> svd(M, Eigen::ComputeThinV);
   out = svd.matrixV().col(M.cols() - 1);
   return svd.singularValues()(M.cols() - 1);
}

// -- UD

void svd_UV(const MatrixXr& M, MatrixXr& U, MatrixXr& V)
{
   Eigen::JacobiSVD<MatrixXr> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
   U = svd.matrixU();
   V = svd.matrixV();
}

void svd_UV(const Matrix3r& M, Matrix3r& U, Matrix3r& V)
{
   Eigen::JacobiSVD<MatrixXr> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
   U = svd.matrixU();
   V = svd.matrixV();
}

// void svd_UV(const MatrixXd& M, MatrixXd& U, MatrixXd& V)
// {
//     Eigen::JacobiSVD<MatrixXd> svd(M,
//     Eigen::ComputeThinU|Eigen::ComputeThinV); U = svd.matrixU(); V =
//     svd.matrixV();
// }

// -- UDV

void svd_UDV(const MatrixXr& M, MatrixXr& U, VectorXr& D, MatrixXr& V)
{
   Eigen::JacobiSVD<MatrixXr> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
   U      = svd.matrixU();
   V      = svd.matrixV();
   auto s = svd.singularValues();
   auto n = s.rows();
   D.resize(n);
   for(uint i = 0; i < n; ++i) D(i) = s(i);
}

void svd_UDV(const Matrix3r& M, Matrix3r& U, Vector3r& D, Matrix3r& V)
{
   Eigen::JacobiSVD<MatrixXr> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
   U      = svd.matrixU();
   V      = svd.matrixV();
   auto s = svd.singularValues();
   for(uint i = 0; i < 3; ++i) D(i) = s(i);
}

void svd_UDV(const Matrix3r& M, Matrix3r& U, Matrix3r& D, Matrix3r& V)
{
   Vector3r d;
   svd_UDV(M, U, d, V);
   D = Matrix3r::Zero();
   for(auto i = 0; i < 3; ++i) D(i, i) = d(i);
}

// void svd_UDV(const MatrixXd& M, MatrixXd& U, VectorXd& D, MatrixXd& V)
// {
//     Eigen::JacobiSVD<MatrixXd> svd(M,
//     Eigen::ComputeThinU|Eigen::ComputeThinV); U = svd.matrixU(); V =
//     svd.matrixV(); auto s = svd.singularValues(); auto n = s.rows();
//     D.resize(n);
//     for(uint i = 0; i < n; ++i)
//         D(i) = s(i);
// }

Matrix3r SVD3DRet::Dm() const noexcept
{
   Matrix3r X = Matrix3r::Identity();
   for(auto i = 0; i < 3; ++i) X(i, i) = D(i);
   return X;
}

string SVD3DRet::to_string() const noexcept
{
   std::stringstream ss{""};
   ss << format("U = \n") << U << "\n\n"
      << format("D = \n") << Dm() << "\n\n"
      << format("V = \n") << V << "\n\n";
   return ss.str();
}

Vector3r SVD3DRet::eigen_vector(int ind) const noexcept
{
   Expects(ind >= 0 && ind < 3);
   return Vector3r(V(ind, 0), V(ind, 1), V(ind, 2));
}

Quaternion SVD3DRet::rot_vec() const noexcept
{
   return rot3x3_to_quaternion(V.transpose());
}

template<typename J>
using MatrixXc_J
    = Eigen::Matrix<std::complex<J>, Eigen::Dynamic, Eigen::Dynamic>;
template<typename J>
using VectorXc_J = Eigen::Matrix<std::complex<J>, Eigen::Dynamic, 1>;

template<typename T>
void svd_UDV_T(const MatrixXc_J<T>& M,
               MatrixXc_J<T>& U,
               VectorXc_J<T>& D,
               MatrixXc_J<T>& V)
{
   using MatrixXc = MatrixXc_J<T>;
   // using VectorXc = VectorXc_J<T>;
   Eigen::JacobiSVD<MatrixXc> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
   U      = svd.matrixU();
   V      = svd.matrixV();
   auto s = svd.singularValues();
   auto n = s.rows();
   D.resize(n, 1);
   for(uint i = 0; i < n; ++i) D(i) = s(i);
}

void svd_UDV(const MatrixXc_J<long double>& M,
             MatrixXc_J<long double>& U,
             VectorXc_J<long double>& D,
             MatrixXc_J<long double>& V)
{
   svd_UDV_T<long double>(M, U, D, V);
}

void svd_UDV(const MatrixXc_J<double>& M,
             MatrixXc_J<double>& U,
             VectorXc_J<double>& D,
             MatrixXc_J<double>& V)
{
   svd_UDV_T<double>(M, U, D, V);
}

void svd_UDV(const MatrixXc_J<float>& M,
             MatrixXc_J<float>& U,
             VectorXc_J<float>& D,
             MatrixXc_J<float>& V)
{
   svd_UDV_T<float>(M, U, D, V);
}

template<typename T>
void svd_UDV_T(const Eigen::Matrix<std::complex<T>, 3, 3>& M,
               Eigen::Matrix<std::complex<T>, 3, 3>& U,
               Eigen::Matrix<std::complex<T>, 3, 1>& D,
               Eigen::Matrix<std::complex<T>, 3, 3>& V)
{
   using MatrixXc = MatrixXc_J<T>;
   // using VectorXc = VectorXc_J<T>;

   Eigen::JacobiSVD<MatrixXc> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
   U      = svd.matrixU();
   V      = svd.matrixV();
   auto s = svd.singularValues();
   auto n = s.rows();
   D.resize(n, 1);
   for(uint i = 0; i < n; ++i) D(i) = s(i);
}

void svd_UDV(const Eigen::Matrix<std::complex<long double>, 3, 3>& M,
             Eigen::Matrix<std::complex<long double>, 3, 3>& U,
             Eigen::Matrix<std::complex<long double>, 3, 1>& D,
             Eigen::Matrix<std::complex<long double>, 3, 3>& V)
{
   svd_UDV_T(M, U, D, V);
}

void svd_UDV(const Eigen::Matrix<std::complex<double>, 3, 3>& M,
             Eigen::Matrix<std::complex<double>, 3, 3>& U,
             Eigen::Matrix<std::complex<double>, 3, 1>& D,
             Eigen::Matrix<std::complex<double>, 3, 3>& V)
{
   svd_UDV_T(M, U, D, V);
}

void svd_UDV(const Eigen::Matrix<std::complex<float>, 3, 3>& M,
             Eigen::Matrix<std::complex<float>, 3, 3>& U,
             Eigen::Matrix<std::complex<float>, 3, 1>& D,
             Eigen::Matrix<std::complex<float>, 3, 3>& V)
{
   svd_UDV_T(M, U, D, V);
}

template<typename T, typename S> double bdcsvd_thin_TS(const T& M, S& out)
{
   Eigen::BDCSVD<T> svd(M, Eigen::ComputeThinV);
   out = svd.matrixV().col(M.cols() - 1);
   return svd.singularValues()(M.cols() - 1);
}

// double bdcsvd_thin(const MatrixXd& M, VectorXd& out)
// { return bdcsvd_thin_TS(M, out); }
real bdcsvd_thin(const MatrixXr& M, VectorXr& out)
{
   return bdcsvd_thin_TS(M, out);
}

// ------------------------------------------------------------ condition-number

template<typename T> real condition_number_T(const T& M)
{
   Eigen::JacobiSVD<T> svd(M);
   auto vals = svd.singularValues();
   return vals(0) / vals(vals.rows() - 1);
}

real condition_number(const MatrixXr& M) { return condition_number_T(M); }

real condition_number(const Matrix3r& M) { return condition_number_T(M); }

// -------------------------------------------------------- Cross-product Matrix

template<typename T> void to_cross_product_matrix_T(const T& X, Matrix3r& M)
{
   M(0, 0) = M(1, 1) = M(2, 2) = 0.0;
   M(0, 1)                     = -X(2);
   M(1, 0)                     = X(2);
   M(0, 2)                     = X(1);
   M(2, 0)                     = -X(1);
   M(1, 2)                     = -X(0);
   M(2, 1)                     = X(0);
}

void to_cross_product_matrix(const Vector3r& X, Matrix3r& M)
{
   to_cross_product_matrix_T(X, M);
}

void to_cross_product_matrix(const Vector3& X, Matrix3r& M)
{
   to_cross_product_matrix_T(X, M);
}

Matrix3r make_cross_product_matrix(const Vector3& X)
{
   Matrix3r M;
   to_cross_product_matrix_T(X, M);
   return M;
}

Matrix3r make_cross_product_matrix(const Vector3r& X)
{
   Matrix3r M;
   to_cross_product_matrix_T(X, M);
   return M;
}

// --------------------------------------------------- Quadratic with Constraint

// Minimize xGtGx such that Hx = h
//  * G must have more rows than columns.
//  * G.cols() == H.cols()
MatrixXr minimize_with_constraint(const MatrixXr& G, const MatrixXr& H, real h)
{
   const unsigned n_vars      = unsigned(G.cols());
   const unsigned n_equations = unsigned(G.rows());
   MatrixXr sol               = MatrixXr::Zero(n_vars, 1);

   if(G.rows() < G.cols()) {
      LOG_ERR(format("Must have more equations than unknowns!"));
      return sol;
   }

   if(H.rows() != n_vars) {
      LOG_ERR(format("G and H must have the same number of columns!"));
      return sol;
   }

   MatrixXr A = MatrixXr::Zero(n_vars + 1, n_vars + 1);
   MatrixXr b = MatrixXr::Zero(n_vars + 1, 1); // Ax - b = 0

   b(n_vars) = h;
   for(unsigned row = 0; row < n_vars; ++row) {
      A(row, n_vars) = H(row);
      A(n_vars, row) = H(row);
   }
   A.block(0, 0, n_vars, n_vars) = G.transpose() * G;

   if(false) {
      MatrixXr U, V;
      VectorXr D;
      svd_UDV(A, U, D, V);

      cout << str("A\n") << A << endl << endl;
      cout << str("U\n") << U << endl << endl;
      cout << str("D\n") << D << endl << endl;
      cout << str("V\n") << V << endl << endl;
   }

   MatrixXr Ainv       = A.inverse();
   MatrixXr sol_lambda = Ainv * b;
   sol                 = sol_lambda.block(0, 0, n_vars, 1);

   return sol;
}

} // namespace perceive


#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iostream>
#include <armadillo>
#include <cmath>
#include <cstdlib>
#include <iomanip>
#include <time.h>
using namespace std;

arma::vec init_func(arma::vec Y, double alpha, double t);
double dx1dt(double x2);
double dx2dt(double x1, double p2, double alpha, double t);
double dp1dt(double x1, double p2, double alpha, double t);
double dp2dt(double p1);

arma::vec init_func(arma::vec Function, double alpha, double t) {
    arma::vec ODE(static_cast<arma::uword>(4), arma::fill::zeros);

    ODE(0) = dx1dt(Function(1));
    ODE(1) = dx2dt(Function(0), Function(3), alpha, t);
    ODE(2) = dp1dt(Function(0), Function(3), alpha, t);
    ODE(3) = dp2dt(Function(2));

    return ODE;
}

double dx1dt(double x2) {
    return x2;
}

double dx2dt(double x1, double p2, double alpha, double t) {
    return p2 / 2 - sqrt(2) * x1 * exp(-alpha*t);
}

double dp1dt(double x1, double p2, double alpha, double t) {
    return 2*x1 + sqrt(2) * p2 * exp(-alpha*t);
}

double dp2dt(double p1) {
    return -p1;
}


/***************************************************************************
/* Javier Juan Albarracin - jajuaal1@ibime.upv.es                         */
/* Universidad Politecnica de Valencia, Spain                             */
/*                                                                        */
/* Copyright (C) 2014 Javier Juan Albarracin                              */
/*                                                                        */
/***************************************************************************
* Convex Non-Negative Matrix Factorization                                 *
***************************************************************************/

#ifndef CNNMF_HPP
#define CNNMF_HPP

#include <Eigen/Dense>
#include <cmath>
#include "NNMF.hpp"

// [1] C. Ding and T. Li and M. Jordan, "Convex and Semi-Nonnegative Matrix
//     Factorizations", IEEE Transactions on Pattern Analysis and Machine
//     Intelligence, Vol. 99(1), 2008.

using namespace Eigen;

class CNNMF
{
public:
	CNNMF(const int maxIterations = 200, const double threshold = 1e-4, const double tolerance = 1e-4, const bool verbose = false);

	template<class Derived>
	void compute(const MatrixBase<Derived> &dataset, const int K);
	template<class Derived>
	void compute(const MatrixBase<Derived> &dataset, const MatrixXd &G0, const MatrixXd &H0);

	MatrixXd G() const { return m_G; }
	MatrixXd H() const { return m_H; }

private:
	void cnnmf_mur(const MatrixXd &dataset);

	MatrixXd m_G;
	MatrixXd m_H;
	int m_components;
	int m_maxIterations;
	double m_threshold;
	double m_tolerance;
	bool m_verbose;
};

/***************************** Implementation *****************************/

CNNMF::CNNMF(const int maxIterations, const double threshold, const double tolerance, const bool verbose)
: m_G(MatrixXd()), m_H(MatrixXd()), m_components(0), m_maxIterations(maxIterations), m_threshold(threshold), m_tolerance(tolerance), m_verbose(verbose)
{
}

template<class Derived>
void CNNMF::compute(const MatrixBase<Derived> &dataset, const int K)
{
	if (K >= dataset.cols() || K >= dataset.rows())
	{
		m_G = MatrixXd::Identity(dataset.cols(), K);
		m_H = MatrixXd::Identity(K, dataset.cols());
		return;
	}

	// Initialize G and H matrices
	NNMF nnmf(m_maxIterations, m_threshold, m_tolerance, true);
	nnmf.compute(dataset, K);

	m_H = nnmf.H();
	m_G = m_H.transpose().fullPivHouseholderQr().solve(MatrixXd::Identity(dataset.cols(), dataset.cols())).transpose().cwiseMax(0);

	// Set components
	m_components = K;

	// Compute Convex Non-Negative Matrix Factorization decomposition
	cnnmf_mur(dataset.cast<double>());
}

template<typename T>
void CNNMF::compute(const MatrixBase<T> &dataset, const MatrixXd &G0, const MatrixXd &H0)
{
	// Initialize W and H matrices
	m_components = G0.cols();
	m_G = G0;
	m_H = H0;

	// Compute Convex Non-Negative Matrix Factorization decomposition
	cnnmf_mur(dataset.cast<double>());
}

// Multiplicative Updated Rules
void CNNMF::cnnmf_mur(const MatrixXd &dataset)
{
	// Set dimension variables
	const double NM = (double) (dataset.rows() * dataset.cols());

	// Set usefull matrices
	const MatrixXd VtV = dataset.transpose() * dataset;
	const MatrixXd Yp = (VtV.cwiseAbs() + VtV) / 2.0;
	const MatrixXd Yn = (VtV.cwiseAbs() - VtV) / 2.0;
	MatrixXd num, den;

	if (m_verbose)
	{
		mexPrintf("Iteration\t\tMax iterations\t\tRMSE\t\t\tRatio\t\tThreshold\n");
		mexPrintf("-----------------------------------------------------------------------------\n");
	}

	// Declare previous iteration matrices and error
	double dnorm0 = 0;

	for (int i = 0; i < m_maxIterations; ++i)
	{
		// Update G
		num = (Yp + (Yn * m_G * m_H)) * m_H.transpose();
		den = (Yn + (Yp * m_G * m_H)) * m_H.transpose();
		m_G = m_G.cwiseProduct(num.cwiseQuotient(den).cwiseSqrt()).cwiseMax(0).eval();

		// Update H
		num = m_G.transpose() * (Yp + (Yn * m_G * m_H));
		den = m_G.transpose() * (Yn + (Yp * m_G * m_H));
		m_H = m_H.cwiseProduct(num.cwiseQuotient(den).cwiseSqrt()).cwiseMax(0).eval();

		// Compute squared error
		const ArrayXXd d = dataset - (dataset * m_G * m_H.transpose());
		const double dnorm = std::sqrt((d * d).sum() / NM);

		if (m_verbose)
		{
			mexPrintf("%d\t\t\t\t%d\t\t\t\t\t%.2e\t\t%.2e\t\t%.2e\n", i+1, m_maxIterations, dnorm, (dnorm0 - dnorm), (m_threshold * std::max(1.0, dnorm0)));
			mexEvalString("drawnow");
		}

		if (i > 0)
		{
			if (dnorm0 - dnorm <= (m_threshold * std::max(1.0, dnorm0)))
				return;
		}

		dnorm0 = dnorm;
	}
}

#endif


/*
 * Author: Mario Lüder
 * Date: Nov. 2018
 */

#ifndef UKF_HPP
#define UKF_HPP

#include <Eigen/Dense>

class UkfBase
{

};

template<typename PROCESSMODEL_T>
class Ukf
{
public:
   Ukf()
      : m_n_x(PROCESSMODEL_T::m_n_x)
      , m_n_x_aug(PROCESSMODEL_T::m_n_x_aug)
      , m_lambda(3 - PROCESSMODEL_T::m_n_x_aug)
      , m_weights(calculateWeights())
      , m_x(m_n_x)
      , m_P(m_n_x, m_n_x)
      , m_previousTimestampUs(0)
   {
      m_x.setZero();
      m_P.setIdentity();
   }

   ~Ukf() {}

   // delete this as this class as an read only interface
   Ukf(const Ukf & rhs) = delete;
   Ukf & operator=(Ukf & ukf) = delete;
   Ukf & operator=(Ukf && ukf) = delete;

   template<typename MEASUREMENT_T, typename MEASUREMENTMODEL_T>
   void processMeasurement(const MEASUREMENT_T & z_measurement, MEASUREMENTMODEL_T & measurementModel, int64_t timestamp_us)
   {
      const int64_t dt_us = setNewTime(timestamp_us);

      if (dt_us > 0)
      {
         // do update step
         // if some time passed since last measurement
         const double dt_s = dt_us / 1000000.0;	//dt - expressed in seconds

         // generate augumented sigma points from last state
         Eigen::MatrixXd XSigmaPoints;
         generateAugumentedSigmaPoints(m_x, m_P, m_std, XSigmaPoints);

         // predict sigma points
         PROCESSMODEL_T::predictSigmaPoints(XSigmaPoints, dt_s, m_XSigmaPointsPred);

         // predict object covariance and
         // predict object state
         m_x_pred.setZero(PROCESSMODEL_T::m_n_x);
         m_P_pred.setZero(PROCESSMODEL_T::m_n_x, PROCESSMODEL_T::m_n_x);

         predict<PROCESSMODEL_T>(m_XSigmaPointsPred, m_x_pred, m_P_pred);
      }

      Eigen::MatrixXd Z_pred;
      measurementModel.predictMeasurement(m_XSigmaPointsPred, Z_pred);

      Eigen::VectorXd z_pred;
      weightedMean(Z_pred, z_pred);

      Eigen::MatrixXd S_cov;
      measurementModel.predictCovar(*this, Z_pred, z_pred, S_cov);

      //
      // update
      //

      // Cross Correlation Matrix
      Eigen::MatrixXd Tc;
      calculateCrosscorrelationMatix<MEASUREMENTMODEL_T>(m_XSigmaPointsPred, Z_pred, m_x_pred, z_pred, Tc);

      // Kalman Gain
      Eigen::MatrixXd K;
      calculateKalmanGain(Tc, S_cov, K);

      // update state and covariance matrix
      Eigen::VectorXd z_residual = z_measurement.value - z_pred;
      MEASUREMENTMODEL_T::normalize(z_residual);

      updateState(K, z_residual, m_x_pred);
      updateCovariance(K, S_cov, m_P_pred);

      // compute NIS
      measurementModel.m_nis = computeNIS(z_residual, S_cov);

      std::swap(m_x_pred, m_x);
      std::swap(m_P_pred, m_P);
   }

   template<typename MODEL_T>
   void predictCovar(const Eigen::MatrixXd & SigmaPoints, const Eigen::VectorXd & vector, Eigen::MatrixXd & Covariance) const
   {
      Eigen::MatrixXd SigmaPointsDiff = SigmaPoints.colwise() - vector;

      for (long colIdx = 0; colIdx < PROCESSMODEL_T::m_nSigmaPoints; ++colIdx)
      {
         MODEL_T::normalize(colIdx, SigmaPointsDiff);

         Eigen::MatrixXd cov_pred_sigmaPoint = (SigmaPointsDiff.col(colIdx).array() * m_weights(colIdx)).matrix() *
               SigmaPointsDiff.col(colIdx).transpose();

         Covariance += cov_pred_sigmaPoint;
      }
   }

   // set standard deviation - process noise
   void setStd(const Eigen::VectorXd & std) { m_std = std; }

   // constant 2 * PI
   static constexpr double TWO_PI = 2 * M_PI;

private:
   //set state dimension
   const long m_n_x;

   //set augumented state dimension
   const long m_n_x_aug;

public:
   // Read only interface
   const Eigen::VectorXd & x = m_x;
   const Eigen::MatrixXd & P = m_P;

protected:
   void generateSigmaPoints(const Eigen::VectorXd &x_, const Eigen::MatrixXd & P_, Eigen::MatrixXd & XSigmaPoints)
   {
      //create sigma point matrix
      XSigmaPoints.setZero(m_n_x, 2 * m_n_x + 1);

      //calculate square root of P
      Eigen::MatrixXd A = P_.llt().matrixL();

      //set first column of sigma point matrix
      XSigmaPoints.col(0)  = x_;

      //set remaining sigma points
      for (int i = 0; i < m_n_x; i++)
      {
         XSigmaPoints.col(i+1)       = x_ + sqrt(m_lambda+m_n_x) * A.col(i);
         XSigmaPoints.col(i+1+m_n_x) = x_ - sqrt(m_lambda+m_n_x) * A.col(i);
      }
   }

   void generateAugumentedSigmaPoints(const Eigen::VectorXd &x_, const Eigen::MatrixXd & P_, const Eigen::VectorXd &std, Eigen::MatrixXd & XSigmaPoints)
   {
      //create augmented mean vector
      Eigen::VectorXd x_aug;
      x_aug.setZero(m_n_x_aug);
      x_aug.head(m_n_x) = x_;

      //create augmented state covariance
      Eigen::MatrixXd P_aug;
      P_aug.setZero(m_n_x_aug, m_n_x_aug);
      P_aug.topLeftCorner(m_n_x, m_n_x) = P_;

      for (long i = m_n_x; i < m_n_x_aug; ++i)
      {
         P_aug(i, i) = pow(std(i - m_n_x),2);
      }

      //create sigma point matrix
      XSigmaPoints.setZero(m_n_x_aug, 2 * m_n_x_aug + 1);

      //calculate square root of P
      Eigen::MatrixXd A = P_aug.llt().matrixL();

      //set first column of sigma point matrix
      XSigmaPoints.col(0)  = x_aug;

      //set remaining sigma points
      for (int i = 0; i < m_n_x_aug; i++)
      {
         XSigmaPoints.col(i+1)           = x_aug + sqrt(m_lambda+m_n_x_aug) * A.col(i);
         XSigmaPoints.col(i+1+m_n_x_aug) = x_aug - sqrt(m_lambda+m_n_x_aug) * A.col(i);
      }
   }

   inline void weightedMean(const Eigen::MatrixXd & XSigmaPoints, Eigen::VectorXd &x_pred)
   {
      x_pred = (XSigmaPoints.array().rowwise() * m_weights.transpose()).rowwise().sum();
   }

   template<typename MEASUREMENTMODEL_T>
   inline void calculateCrosscorrelationMatix(const Eigen::MatrixXd & XSigmaPointsPred, const Eigen::MatrixXd & ZSigmaPointsPred, const Eigen::VectorXd & x_mean, const Eigen::VectorXd & z_mean, Eigen::MatrixXd & T)
   {
      Eigen::MatrixXd X = (XSigmaPointsPred.colwise() - x_mean);
      // MEASUREMENTMODEL_T::normalize(X);
      Eigen::MatrixXd Z = (ZSigmaPointsPred.colwise() - z_mean);
      // MEASUREMENTMODEL_T::normalize(Z);
      const Eigen::MatrixXd X_weighted = X.array().rowwise() * m_weights.transpose();
      T = (X_weighted * Z.transpose());
   }

   inline void calculateKalmanGain(const Eigen::MatrixXd & T, const Eigen::MatrixXd & S, Eigen::MatrixXd & K)
   {
      K = T * S.inverse();
   }

   inline void updateState(const Eigen::MatrixXd & K, const Eigen::VectorXd & z_residual, Eigen::VectorXd & x_pred)
   {
      Eigen::VectorXd & x_update = x_pred;
      x_update = x_pred + K * z_residual;
   }

   inline void updateCovariance(const Eigen::MatrixXd & K, const Eigen::MatrixXd & S, Eigen::MatrixXd & P_pred) const
   {
      Eigen::MatrixXd & P_update = P_pred;
      P_update = P_pred - K * S * K.transpose();
   }

   template<typename MODEL_T>
   inline void predict(const Eigen::MatrixXd & XSigmaPoints, Eigen::VectorXd &x_pred, Eigen::MatrixXd & P_pred)
   {
      weightedMean(XSigmaPoints, x_pred);
      predictCovar<MODEL_T>(XSigmaPoints, x_pred, P_pred);
   }

   // set the new measurement time and compute the difference
   inline int64_t setNewTime(const int64_t timestamp_us)
   {
      //compute the time elapsed between the current and previous measurements
      int64_t dt = timestamp_us - m_previousTimestampUs;
      m_previousTimestampUs = timestamp_us;
      return dt;
   }

   inline double computeNIS(const Eigen::VectorXd z_diff, const Eigen::MatrixXd & S)
   {
      const double nis = z_diff.transpose() * S.inverse() * z_diff;
      return nis;
   }

   //define spreading parameter
   const double m_lambda;

   // weights for prediction
   const Eigen::ArrayXd m_weights;

   // set standard deviation - process noise
   Eigen::VectorXd m_std;

   // object state
   Eigen::VectorXd m_x;

   // object covariance matrix
   Eigen::MatrixXd m_P;

   // predicted object state
   Eigen::VectorXd m_x_pred;

   // predicted object covariance matrix
   Eigen::MatrixXd m_P_pred;

   // predicted Sigma Points
   Eigen::MatrixXd m_XSigmaPointsPred;

private:
   const Eigen::ArrayXd calculateWeights()
   {
      Eigen::ArrayXd weights(PROCESSMODEL_T::m_nSigmaPoints);

      weights(0) = m_lambda / (m_lambda + m_n_x_aug);

      for (int i = 1; i < PROCESSMODEL_T::m_nSigmaPoints; ++i)
      {
         weights(i) = 0.5 / (m_lambda + m_n_x_aug);
      }

      return  weights;
   }

   int64_t m_previousTimestampUs;
};

#endif // UKF_HPP


#include <algorithm>
#include <fstream>
#include <vector>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>
#include <Eigen/Core>
#include <Eigen/Householder>
#include <Eigen/SparseCore>
#include <Euclid/Descriptor/HKS.h>
#include <Euclid/Descriptor/Histogram.h>
#include <Euclid/IO/OffIO.h>
#include <Euclid/IO/PlyIO.h>
#include <Euclid/MeshUtil/CGALMesh.h>
#include <Euclid/Geometry/Spectral.h>
#include <Euclid/Util/Color.h>

#include <config.h>

using Kernel = CGAL::Simple_cartesian<double>;
using Point_3 = Kernel::Point_3;
using Mesh = CGAL::Surface_mesh<Point_3>;
using Mat = Eigen::MatrixXd;
using Vec = Eigen::VectorXd;
using SpMat = Eigen::SparseMatrix<double>;
using Arr = Eigen::ArrayXXd;

void eigs_to_color(const std::string& prefix,
                   const std::vector<double>& positions,
                   const std::vector<unsigned>& indices,
                   const Mat& eigenfunctions)
{
    auto nv = positions.size() / 3;
    for (int i = 0; i < 6; ++i) {
        std::vector<double> eigenvector(nv);
        Vec::Map(eigenvector.data(), nv) = eigenfunctions.col(i);
        std::vector<uint8_t> colors;
        Euclid::colormap(igl::COLOR_MAP_TYPE_JET, eigenvector, colors, true);
        std::string fout(TMP_DIR);
        fout.append(prefix)
            .append("_eigenfunction_")
            .append(std::to_string(i))
            .append(".ply");
        Euclid::write_ply<3>(
            fout, positions, nullptr, nullptr, &indices, &colors);
    }
}

void eigs_recon(const std::string& prefix,
                const std::vector<double>& positions,
                const std::vector<unsigned>& indices,
                const Mat& eigenfunctions)
{
    auto nv = positions.size() / 3;
    Eigen::Map<const Mat> xyz(positions.data(), 3, nv);
    EASSERT(xyz(1, 0) == positions[1]);
    EASSERT(xyz(0, 1) == positions[3]);
    Mat embedding = xyz * eigenfunctions;
    std::vector<int> neigs{ 5, 20, 50, 100, 200, 300 };
    for (auto ne : neigs) {
        Mat xyz_recon =
            embedding.leftCols(ne) * eigenfunctions.transpose().topRows(ne);
        std::vector<double> recon(nv * 3);
        Mat::Map(recon.data(), 3, nv) = xyz_recon;
        std::string fout(TMP_DIR);
        fout.append(prefix)
            .append("_recon_")
            .append(std::to_string(ne))
            .append(".ply");
        Euclid::write_ply<3>(fout, recon, nullptr, nullptr, &indices, nullptr);
    }
}

void hks_to_color(const std::vector<double>& positions,
                  const std::vector<unsigned>& indices,
                  const Arr& signatures,
                  int vidx)
{
    std::vector<double> distances(signatures.cols());
    for (int i = 0; i < signatures.cols(); ++i) {
        distances[i] = Euclid::chi2(signatures.col(vidx), signatures.col(i));
    }
    std::vector<uint8_t> colors;
    Euclid::colormap(
        igl::COLOR_MAP_TYPE_JET, distances, colors, true, false, true);
    std::string fout(TMP_DIR);
    fout.append("hks_").append(std::to_string(vidx)).append(".ply");
    Euclid::write_ply<3>(fout, positions, nullptr, nullptr, &indices, &colors);
}

void hks_to_csv(const Arr& signatures)
{
    std::string fout(TMP_DIR);
    fout.append("hks.csv");
    std::ofstream ofs(fout);
    for (int i = 0; i < signatures.rows(); ++i) {
        for (int j = 0; j < signatures.cols(); ++j) {
            ofs << signatures(i, j) << ",";
        }
        ofs << std::endl;
    }
}

int main()
{
    // read and build mesh
    std::vector<double> positions;
    std::vector<unsigned> indices;
    std::string fmesh(DATA_DIR);
    fmesh.append("bumpy.off");
    Euclid::read_off<3>(fmesh, positions, nullptr, &indices, nullptr);
    Mesh mesh;
    Euclid::make_mesh<3>(mesh, positions, indices);

    // solving eigenvalue problem
    int k = 300;
    Vec lambdas_lbo, lambdas_gl;
    Mat phis_lbo, phis_gl;
    Euclid::spectrum(
        mesh, k, lambdas_lbo, phis_lbo, Euclid::SpecOp::mesh_laplacian);
    Euclid::spectrum(
        mesh, k, lambdas_gl, phis_gl, Euclid::SpecOp::graph_laplacian);

    // eigenvectors of mesh laplacian need to be orthonormalized for recon
    phis_lbo = phis_lbo.householderQr().householderQ();

    // output eigenfunctions to colors
    eigs_to_color("lbo", positions, indices, phis_lbo);
    eigs_to_color("gl", positions, indices, phis_gl);

    // eigen embedding and reconstruction
    eigs_recon("lbo", positions, indices, phis_lbo);
    eigs_recon("gl", positions, indices, phis_gl);

    // use eigenstructures to compute heat kernel signature
    Euclid::HKS<Mesh> hks;
    hks.build(mesh, &lambdas_lbo, &phis_lbo);
    Eigen::ArrayXXd hks_sigs;
    hks.compute(hks_sigs, 100);
    hks_to_color(positions, indices, hks_sigs, 240); // tip
    hks_to_color(positions, indices, hks_sigs, 3);   // cube corner
    hks_to_csv(hks_sigs);
}


#include <inttypes.h>
#include <iostream>
#include <iomanip>
#include <cstdlib>
#include <cerrno>
#include <NTL/mat_GF2.h>
#include <NTL/GF2.h>
#include <NTL/matrix.h>
#include <vector>
#include "calc_tvalue.hpp"

using namespace std;
using namespace NTL;

//#define DEBUG

namespace {
    Mat<GF2> Ident;
    bool make_mat(Mat<GF2>& mat, uint64_t num);
    size_t calc_order(Mat<GF2>& mat, size_t max);
}

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

    if (argc < 4) {
        cout << argv[0] << " s m max_tvalue" << endl;
        return -1;
    }
    errno = 0;
    int s = strtoul(argv[1], NULL, 10);
    int m = strtoul(argv[2], NULL, 10);
    int max_tvalue = strtoul(argv[3], NULL, 10);
    if (errno) {
        cout << " s, m and max_tvalue should be numbers" << endl;
        return -1;
    }
    cout << "# s = " << dec << s << endl;
    cout << "# m = " << dec << m << endl;
    cout << "# max_tvalue = " << dec << max_tvalue << endl;
    ident(Ident, m);
#if defined(DEBUG)
    cout << "main step 0.1" << endl;
#endif
    int64_t max = 1 << (m * m);
    Mat<GF2> mat;
    mat.SetDims(m, m);
    Mat<GF2> array[s];
#if defined(DEBUG)
    cout << "main step 0.2" << endl;
#endif
    vector<vector<int> >to;
    to.resize(max_tvalue + 1);
#if defined(DEBUG)
    cout << "main step 1" << endl;
#endif
    for (int i = 0; i <= max_tvalue; i++) {
        to[i].resize(max);
        for (int64_t j = 0; j < max; j++) {
            to[i][j] = 0;
        }
    }
#if defined(DEBUG)
    cout << "main step 2" << endl;
#endif
    for (int64_t cnt = 1; cnt < max; cnt++) {
        if (!make_mat(mat, cnt)) {
            continue;
        }
        int order = calc_order(mat, max - 1);
        if (order < s) {
            continue;
        }
        array[0] = mat;
        for (int j = 1; j < s; j++) {
            array[j] = array[j - 1] * mat;
        }
        int tvalue = calc_tvalue(array, s, max_tvalue);
        if (tvalue > max_tvalue || tvalue < 0) {
            continue;
        }
        to[tvalue][order] += 1;
        if (tvalue == 0 && order == max -1) {
            cout << "found!" << endl;
            cout << "cnt = " << dec << cnt << endl;
            cout << "tvalue = " << dec << tvalue << endl;
            cout << "order = " << dec << order << endl;
            cout << mat;
        }
    }
    cout << "(tvalue, order) = cnt" << endl;
    for (int i = 0; i <= max_tvalue; i++) {
        for (int64_t j = 0; j < max; j++) {
            if (to[i][j] == 0) {
                continue;
            }
            cout << "(" << dec << i << ","
                 << j << ") = " << to[i][j] << endl;
        }
    }
    return 0;
}

namespace {
    bool make_mat(Mat<GF2>& mat, uint64_t num)
    {
#if defined(DEBUG)
        cout << "start make_mat" << endl;
#endif
        //cout << "num = " << dec << num << endl;
        int m = mat.NumRows();
        const uint64_t mask = ~UINT64_C(0) >> (64 - m);
        for (int i = 0; i < m; i++) {
            uint64_t row = num & mask;
            if (row == 0) {
#if defined(DEBUG)
                cout << "end make_mat" << endl;
#endif
                return false;
            }
            for (int j = 0; j < m; j++) {
                //cout << "mat.put(" << i << "," << j << "," << (row & 1) << ")"
                //     << endl;
                mat.put(i, j, row & 1);
                row = row >> 1;
            }
            num = num >> m;
        }
        //cout << "make_mat 1:" << mat << endl;
        Mat<GF2> w = mat;
        if (gauss(w) != m) {
#if defined(DEBUG)
            cout << "end make_mat" << endl;
#endif
            return false;
        }
        //cout << "make_mat 2:\n" << mat << endl;
#if defined(DEBUG)
        cout << "end make_mat" << endl;
#endif
        return true;
    }

    size_t calc_order(Mat<GF2>& mat, size_t max)
    {
        Mat<GF2> w = mat;
        for (size_t i = 1; i <= max; i++) {
            w = w * mat;
            if (w == mat) {
                return i;
            }
        }
        return max;
    }
}