/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2007, 2008 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

/*! \file matrix.hpp
    \brief matrix used in linear algebra.
*/

#include <ql/math/matrix.hpp>
#if defined(QL_PATCH_MSVC)
#pragma warning(push)
#pragma warning(disable:4180)
#pragma warning(disable:4127)
#endif

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif

#if defined(__clang__)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wunused-function"
#endif

#if !defined(QL_NO_UBLAS_SUPPORT)
#if BOOST_VERSION > 106300
#include <boost/serialization/array_wrapper.hpp>
#endif
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#endif

#if defined(QL_PATCH_MSVC)
#pragma warning(pop)
#endif

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif

#if defined(__clang__)
#pragma clang diagnostic pop
#endif

namespace QuantLib {

    Disposable<Matrix> inverse(const Matrix& m) {
        #if !defined(QL_NO_UBLAS_SUPPORT)

        QL_REQUIRE(m.rows() == m.columns(), "matrix is not square");

        boost::numeric::ublas::matrix<Real> a(m.rows(), m.columns());

        std::copy(m.begin(), m.end(), a.data().begin());

        boost::numeric::ublas::permutation_matrix<Size> pert(m.rows());

        // lu decomposition
        Size singular = 1;
        try {
            singular = lu_factorize(a, pert);
        } catch (const boost::numeric::ublas::internal_logic& e) {
            QL_FAIL("lu_factorize error: " << e.what());
        } catch (const boost::numeric::ublas::external_logic& e) {
            QL_FAIL("lu_factorize error: " << e.what());
        }
        QL_REQUIRE(singular == 0, "singular matrix given");

        boost::numeric::ublas::matrix<Real>
            inverse = boost::numeric::ublas::identity_matrix<Real>(m.rows());

        // backsubstitution
        try {
            boost::numeric::ublas::lu_substitute(a, pert, inverse);
        } catch (const boost::numeric::ublas::internal_logic& e) {
            QL_FAIL("lu_substitute error: " << e.what());
        }

        Matrix retVal(m.rows(), m.columns());
        std::copy(inverse.data().begin(), inverse.data().end(),
                  retVal.begin());

        return retVal;

        #else
        QL_FAIL("this version of gcc does not support "
                "the Boost uBLAS library");
        #endif
    }

    Real determinant(const Matrix& m) {
        #if !defined(QL_NO_UBLAS_SUPPORT)
        QL_REQUIRE(m.rows() == m.columns(), "matrix is not square");

        boost::numeric::ublas::matrix<Real> a(m.rows(), m.columns());
        std::copy(m.begin(), m.end(), a.data().begin());

        // lu decomposition
        boost::numeric::ublas::permutation_matrix<Size> pert(m.rows());
        /* const Size singular = */ lu_factorize(a, pert);

        Real retVal = 1.0;

        for (Size i=0; i < m.rows(); ++i) {
            if (pert[i] != i)
                retVal *= -a(i,i);
            else
                retVal *=  a(i,i);
        }
        return retVal;

        #else
        QL_FAIL("this version of gcc does not support "
                "the Boost uBLAS library");
        #endif
    }
}


#include <array>
#include <vector>
#include <iostream>
#include <fstream>
#include <utility>
#include <algorithm>
#include <boost/progress.hpp>
#include <boost/program_options.hpp>
#include <boost/python/numpy.hpp>
#include "latte.h"

using namespace std;
using namespace latte;
namespace py = boost::python;
namespace np = boost::python::numpy;

#if 0
void test_auc () {
    vector<pair<float, int>> all;
    size_t n0 = 113;
    size_t n1 = 119;
    for (unsigned i = 0; i < n0; ++i) {
        all.emplace_back(0, 0);
    }
    for (unsigned i = 0; i < n1; ++i) {
        all.emplace_back(0, 1);
    }
    sort(all.begin(), all.end(), [](pair<float, int> const &p1, pair<float, int> const &p2) { return p1.second < p2.second;});
    if (auc(all, n0, n1) != 1) throw 0;
    cerr << "Test 1 OK" << endl;
    sort(all.begin(), all.end(), [](pair<float, int> const &p1, pair<float, int> const &p2) { return p1.second > p2.second;});
    if (auc(all, n0, n1) != 0) throw 0;
    cerr << "Test 2 OK" << endl;
    random_shuffle(all.begin(), all.end());
    float v = auc(all, n0, n1);
    if (v < 0.3) throw 1;
    if (v > 0.7) throw 1;
    cerr << "Test 3 OK" << endl;
}
#endif

template <typename T>
void compare (np::ndarray v0, np::ndarray v1, vector<float> *ft) {
    size_t n0 = v0.shape(0);
    size_t n1 = v1.shape(0);
    size_t ns = n0 + n1;
    cerr << n0 << '\t' << n1 << endl;
    if (v0.shape(1) != Ref::GENES) throw 0;
    if (v1.shape(1) != Ref::GENES) throw 0;

    T const *p0 = (T const *)(v0.get_data());
    T const *p1 = (T const *)(v1.get_data());

    ft->resize(Ref::GENES);
    boost::progress_display progress(Ref::GENES, cerr);
#pragma omp parallel
    {
        vector<pair<T, int>> all;
        all.reserve(ns);
        // for each gene
#pragma omp for
        for (size_t gene = 0; gene < Ref::GENES; ++gene) {
            all.clear();
            for (size_t i = 0; i < n0; ++i) {
                all.emplace_back(p0[Ref::GENES * i + gene], 0);
            }
            for (size_t i = 0; i < n1; ++i) {
                all.emplace_back(p1[Ref::GENES * i + gene], 1);
            }
            sort(all.begin(), all.end());
            ft->at(gene) = auc(all, n0, n1);
#pragma omp critical
            ++progress;
        }
    }
}

int main (int argc, char *argv[]) {
    string input1_path;
    string input2_path;
    string output_path("output");
    {
        namespace po = boost::program_options;
        po::options_description desc_visible("Allowed options");
        desc_visible.add_options()
            ("help,h", "produce help message.")
            ("input1", po::value(&input1_path), "")
            ("input2", po::value(&input2_path), "")
            ("output", po::value(&output_path), "")
            ;

        po::options_description desc("Allowed options");
        desc.add(desc_visible);

        po::positional_options_description p;
        p.add("input1", 1);
        p.add("input2", 1);
        p.add("output", 1);

        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).
                         options(desc).positional(p).run(), vm);
        po::notify(vm);
        if (vm.count("help") || input1_path.empty() || input2_path.empty()) {
            cout << "Usage:" << endl;
            cout << desc_visible;
            cout << endl;
            return 0;
        }
	}
    Py_Initialize();
    np::initialize();
    Ref genes("data/ref");
    py::object np_load = py::import("numpy").attr("load");
    np::ndarray rank0 = np::array(np_load(input1_path));
    np::ndarray rank = np::array(np_load(input2_path));

    vector<float> ft;
    if (np::dtype::get_builtin<float>() == rank0.get_dtype()) {
        compare<float>(rank0, rank, &ft);
    }
    else if (np::dtype::get_builtin<uint16_t>() == rank0.get_dtype()) {
        compare<uint16_t>(rank0, rank, &ft);
    }
    else {
        cerr << "dtype not supported." << endl;
        throw 0;
    }

    if (ft.size() != genes.size()) throw 0;
    ofstream os(output_path);
    for (unsigned i = 0; i < Ref::GENES; ++i) {
        os << genes[i] << '\t' << ft[i] << endl;
    }

    return 0;
}


#include <vector>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <unsupported/Eigen/MatrixFunctions>

#include "ihgp/InfiniteHorizonGP.hpp"

const double InfiniteHorizonGP::DARE_EPS = 1e-10;
const int InfiniteHorizonGP::DARE_MAXIT = 100;

InfiniteHorizonGP::InfiniteHorizonGP(const double dt, const Eigen::MatrixXd &F, const Eigen::MatrixXd &HH, const Eigen::MatrixXd &Pinf, const double &R, const std::vector<Eigen::MatrixXd> &dF, const std::vector<Eigen::MatrixXd> &dPinf, const std::vector<double> &dR)
{
    // Solve A and Q (stationary systems)
    A = (F*dt).exp();
    Q = Pinf - A*Pinf*A.transpose();
    Rc = R;

    // Assign measurement model
    H = HH;

    // Solve the discrete algebraic Riccati equation
    Eigen::MatrixXd PP = InfiniteHorizonGP::DARE(A,H,Q,R);
    PP_update = PP;

    // Stationary innovation variance
    S = (H*PP*H.transpose())(0) + R;

    // Stationary gain
    K = PP*H.transpose()/S;

    // State covariance
    PF = PP - K*H*PP;

    // Pre-calculate
    HA = (H*A).transpose();
    AKHA = A-K*H*A;

    // Number of parameters
    nparam = (int)dF.size();

    // State dimensionality
    int dim = (int)F.rows();

    // Initialize mean and helpers
    m.setZero(dim);

    // Prepare
    Eigen::MatrixXd AK = A*K;
    Eigen::MatrixXd FF(2*dim,2*dim);
    FF.setZero();
    FF.topLeftCorner(dim,dim) = F;
    FF.bottomRightCorner(dim,dim) = F;

    // Allocate the arrays
    HdA  = new Eigen::VectorXd[nparam];
    dK = new Eigen::VectorXd[nparam];
    dAKHA = new Eigen::MatrixXd[nparam];
    dS = new double[nparam];
    dm = new Eigen::VectorXd[nparam];

    // Pre-calculate the needed derivative matrices
    for (int j=0; j<nparam; j++)
    {
        // Assign derivative of drift model matrix
        FF.bottomLeftCorner(dim,dim) = dF[j];

        // Solve the matrix exponential (see ...)
        Eigen::MatrixXd AA = (FF*dt).exp();

        // Extract the derivative of the discrete-time dynamic model
        Eigen::MatrixXd dA = AA.bottomLeftCorner(dim,dim);
        Eigen::MatrixXd dQ = dPinf[j] - dA*Pinf*A.transpose() - A*dPinf[j]*A.transpose() - A*Pinf*dA.transpose();
        dQ = .5*(dQ+dQ.transpose()).eval();

        // Precalculate C
        Eigen::MatrixXd C = dA*PP*A.transpose() + A*PP*dA.transpose() - dA*PP*H.transpose()*AK.transpose() - AK*H*PP*dA.transpose() + AK*dR[j]*AK.transpose() + dQ;
        C = .5*(C+C.transpose()).eval();

        // Solve DARE
        Eigen::MatrixXd dPP = InfiniteHorizonGP::DARE(A-AK*H,Eigen::MatrixXd::Zero(dim,dim),C,0.0);

        // Evaluate dS and dK
        dS[j] = (H*dPP*H.transpose())(0) + dR[j];
        dK[j] = dPP*H.transpose()/S - PP*H.transpose()*(((H*dPP*H.transpose())(0)+dR[j])/S/S);
        dAKHA[j] = dA - dK[j]*H*A - K*H*dA;
        HdA[j] = (H*dA).transpose();

        // Initial dm
        dm[j] = Eigen::VectorXd::Zero(dim);

    }

    // Initialize log likelihood and its gradient
    edata = 0;
    gdata = Eigen::VectorXd::Zero(nparam);
}

InfiniteHorizonGP::~InfiniteHorizonGP()
{
    delete[] HdA;
    delete[] dK;
    delete[] dAKHA;
    delete[] dS;
    delete[] dm;
}

void InfiniteHorizonGP::init_step()
{
    // Initialize log likelihood and its gradient
    edata = 0;
    gdata = Eigen::VectorXd::Zero(nparam);

    // Update
    // Stationary innovation variance
    S = (H*PP_update*H.transpose())(0) + Rc;

    // Stationary gain
    K = PP_update*H.transpose()/S;

    // State covariance
    PF = PP_update - K*H*PP_update;

    // Pre-calculate
    HA = (H*A).transpose();
    AKHA = A-K*H*A;

    //
    MF.clear();
}

void InfiniteHorizonGP::update(const double &y)
{
    // Define constants
    const double PI = 3.141592654;

    // Innovation mean
    double v = y - HA.dot(m);

    // Update marginal likelihood
    edata += .5*v*v/S + .5*log(2*PI) + .5*log(S);

    // Update derivatives
    for (int j=0; j<nparam; j++)
    {
        // Derivatives of innovation mean
        double dv = -HdA[j].dot(m) - HA.dot(dm[j]);

        // Derivatives of marginal likelihood
        gdata(j) += v*dv/S - .5*v*v*dS[j]/S/S + .5*dS[j]/S;

        // Derivatives of state mean
        dm[j] = dAKHA[j]*m + AKHA*dm[j] + dK[j]*y;
    }

    // Recursion for the state mean
    m = AKHA*m + K*y;

    // Store state mean for possible backward pass
    MF.push_back(m);

}

std::vector<double> InfiniteHorizonGP::getEft()
{
    // Solve backward smoother gain
    int dim = (int)A.rows();
    Eigen::MatrixXd PP = A*PF*A.transpose()+Q;
    Eigen::LDLT<Eigen::MatrixXd> PPldl = PP.ldlt();
    Eigen::MatrixXd G = PPldl.solve(A*PF).transpose();

    // Solve smoother state covariance
    // Eigen::MatrixXd QQ = PF-G*PP*G.transpose();
    // QQ = .5*(QQ+QQ.transpose()).eval();
    // P = InfiniteHorizonGP::DARE(G,Eigen::MatrixXd::Zero(dim,dim),QQ,0.0);

    // Output vector
    std::vector<double> Eft;

    // Initialize with last element
    m = MF[MF.size()-1];
    Eft.push_back((H*m)(0));

    // Run backward pass
    for (int k=(int)MF.size()-2; k>=0; k--)
    {
        m = MF[k] + G*(m-A*MF[k]);
        Eft.push_back((H*m)(0));
    }

    // Reverse
    std::reverse(Eft.begin(),Eft.end());

    return Eft;

}

double InfiniteHorizonGP::getVarft()
{
    return (H*P*H.transpose())(0);
}

double InfiniteHorizonGP::getLik()
{
    return edata;
}

Eigen::VectorXd InfiniteHorizonGP::getLikDeriv()
{
    return gdata;
}

Eigen::MatrixXd InfiniteHorizonGP::DARE(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B, const Eigen::MatrixXd &Q, const double &R)
{

    // Initial guess
    int dim = (int)A.rows();
    Eigen::MatrixXd X(dim,dim);
    Eigen::MatrixXd X_prev(dim,dim);
    Eigen::MatrixXd K(dim,B.rows());
    X.setIdentity();

    // Number of loops
    int n = 0;

    // Iterate a maximum of 100 iterations
    while (n < DARE_MAXIT)
    {
        // Step
        n++;
        X_prev = X;

        // Gain (NB: Does not work in the general case, but for scalar R and possibly zero B)
        if (abs(R) < 1e-15)
        {
            K.setZero();
        } else {
            K = A*(X*B.transpose() / ((B*X*B.transpose())(0)+R));
        }

        // Recursion
        X = (A - K*B)*X*(A - K*B).transpose() + K*R*K.transpose() + Q;

        // Check if we should break (use the Frobenius norm)
        if ((X-X_prev).norm() < DARE_EPS)
        {
            break;
        }
    }

    return X;
}


#include <Eigen/Dense>
#include <lie_mpc_bidirectional.hpp>
#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>
#include <chrono>
#include <iostream>

TEST_CASE("Hover")
{
  Eigen::VectorXd x_lin = Eigen::VectorXd(18);
  Eigen::VectorXd u_lin = Eigen::VectorXd(4);
  Eigen::VectorXd Y = Eigen::VectorXd::Zero(8 * 12);
  x_lin << 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0.;
  u_lin << 2.4525, 2.4525, 2.4525, 2.4525;
  for (int i = 0; i < 8; i++) {
    Y(i * 12 + 3) = 1.;
    Y(i * 12 + 7) = 1.;
    Y(i * 12 + 11) = 1.;
  }
  LieMPC test_controller;
  test_controller.x = x_lin;
  test_controller.u_lin = u_lin;
  test_controller.Y = Y;
  std::chrono::steady_clock::time_point tic = std::chrono::steady_clock::now();
  test_controller.linearize();
  test_controller.discretize();
  test_controller.build_mpc();
  test_controller.solve();
  std::chrono::steady_clock::time_point toc = std::chrono::steady_clock::now();
  std::cout << "Time elapsed = " << std::chrono::duration_cast<std::chrono::microseconds>(toc - tic).count() << "[µs]" << std::endl;
  REQUIRE((std::abs(test_controller.u(0) - 2.4525) < 0.05 && std::abs(test_controller.u(1) - 2.4525) < 0.05 && std::abs(test_controller.u(2) - 2.4525) < 0.05 && std::abs(test_controller.u(3) - 2.4525) < 0.05));
};

TEST_CASE("Inverted Hover")
{
  Eigen::VectorXd x_lin = Eigen::VectorXd(18);
  Eigen::VectorXd u_lin = Eigen::VectorXd(4);
  Eigen::VectorXd Y = Eigen::VectorXd::Zero(8 * 12);
  x_lin << 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., -1., 0., 0., 0., -1., 0., 0., 0.;
  u_lin << -2.4525, -2.4525, -2.4525, -2.4525;
  for (int i = 0; i < 8; i++) {
    Y(i * 12 + 3) = 1.;
    Y(i * 12 + 7) = -1.;
    Y(i * 12 + 11) = -1.;
  }
  LieMPC test_controller;
  test_controller.x = x_lin;
  test_controller.u_lin = u_lin;
  test_controller.Y = Y;
  std::chrono::steady_clock::time_point tic = std::chrono::steady_clock::now();
  test_controller.linearize();
  test_controller.discretize();
  test_controller.build_mpc();
  test_controller.solve();
  std::chrono::steady_clock::time_point toc = std::chrono::steady_clock::now();
  std::cout << "Time elapsed = " << std::chrono::duration_cast<std::chrono::microseconds>(toc - tic).count() << "[µs]" << std::endl;
  REQUIRE((std::abs(test_controller.u(0) + 2.4525) < 0.05 && std::abs(test_controller.u(1) + 2.4525) < 0.05 && std::abs(test_controller.u(2) + 2.4525) < 0.05 && std::abs(test_controller.u(3) + 2.4525) < 0.05));
};

TEST_CASE("Runtime")
{
  Eigen::VectorXd x_lin = Eigen::VectorXd(18);
  Eigen::VectorXd u_lin = Eigen::VectorXd(4);
  Eigen::VectorXd Y = Eigen::VectorXd::Zero(8 * 12);
  x_lin << 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0.;
  u_lin << 2.4525, 2.4525, 2.4525, 2.4525;
  for (int i = 0; i < 8; i++) {
    Y(i * 12 + 3) = 1.;
    Y(i * 12 + 7) = 1.;
    Y(i * 12 + 11) = 1.;
  }
  LieMPC test_controller;
  test_controller.x = x_lin;
  test_controller.u_lin = u_lin;
  test_controller.Y = Y;
  std::chrono::steady_clock::time_point tic = std::chrono::steady_clock::now();
  test_controller.linearize();
  test_controller.discretize();
  test_controller.build_mpc();
  test_controller.solve();
  std::chrono::steady_clock::time_point toc = std::chrono::steady_clock::now();
  std::cout << "Time elapsed = " << std::chrono::duration_cast<std::chrono::microseconds>(toc - tic).count() << "[µs]" << std::endl;
  REQUIRE(std::chrono::duration_cast<std::chrono::microseconds>(toc - tic).count() < 20000);
};

TEST_CASE("Consistency")
{
  Eigen::VectorXd x_lin = Eigen::VectorXd(18);
  Eigen::VectorXd u_lin = Eigen::VectorXd(4);
  Eigen::VectorXd u_init = Eigen::VectorXd(4);
  Eigen::VectorXd Y = Eigen::VectorXd::Zero(8 * 12);
  x_lin << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.7071, -0.7071, 0., 0.6830, 0.6830, -0.2588, 0.1830, 0.1830, 0.9659, 0.1, 0.1, 0.1;
  u_lin << 2.4525, 2.4525, 2.4525, 2.4525;
  for (int i = 0; i < 8; i++) {
    Y(i * 12 + 3) = 1.;
    Y(i * 12 + 7) = 1.;
    Y(i * 12 + 11) = 1.;
  }
  LieMPC test_controller;
  test_controller.x = x_lin;
  test_controller.u_lin = u_lin;
  test_controller.Y = Y;
  std::chrono::steady_clock::time_point tic = std::chrono::steady_clock::now();
  test_controller.linearize();
  test_controller.discretize();
  test_controller.build_mpc();
  test_controller.solve();
  u_init = test_controller.u;
  for (int i = 0; i < 10; i++) {
    test_controller.linearize();
    test_controller.discretize();
    test_controller.build_mpc();
    test_controller.solve();
  }
  std::chrono::steady_clock::time_point toc = std::chrono::steady_clock::now();
  std::cout << "Time elapsed = " << std::chrono::duration_cast<std::chrono::microseconds>(toc - tic).count() << "[µs]" << std::endl;
  REQUIRE((std::abs(test_controller.u(0) - u_init(0)) < 1e-8 && std::abs(test_controller.u(1) - u_init(1)) < 1e-8 && std::abs(test_controller.u(2) - u_init(2)) < 1e-8 && std::abs(test_controller.u(3) - u_init(3)) < 1e-8));
};


#include <iostream>
#include <opencv2/opencv.hpp>
#include <string>
#include <vector>
#include <algorithm>
#include <tuple>

#include "LoomoOdo.h"
#include "LoomoCSVReader.h"
#include "Utils.h"
#include "PoseGraph.h"
#include "LoomoVision.h"

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <opencv2/core/eigen.hpp>

using namespace cv;
using namespace std;
using std::cout; using std::endl;
using loomo::Pose; using loomo::kMatrix; using loomo::dCoeff;

int main()
{
    string path = __FILE__; //gets source code path, including file name
    path = path.substr(0, 1 + path.find_last_of('\\')); //removes file name
    string pathLoomo1 = path + "LoomoRecordings\\0069\\";

    vector<CSVData> dataLoomo1 = LoomoCSVReader::string2data(LoomoCSVReader::getData(pathLoomo1 + "sensor_data.csv"));

    VideoCapture capFisheyeLoomo1(pathLoomo1 + "hallway_00_fisheye.avi");
    //VideoCapture capFisheyeLoomo1(0);
    if (!capFisheyeLoomo1.isOpened()) {
        cout << "Error openeing video file" << endl;
        return -1;
    }
    Mat capFrame, greyFrame, comp1, comp2, compImg;
    vector<KeyPoint> kp, kp1, kp2;
    Mat descriptors, dscr1, dscr2;
    //vector<DMatch> matches;
    loomo::LoomoVision mVision;

    loomo::LoomoOdo loomo1;
    vector<loomo::Pose> odoTraj = { {0.0, 0.0, 0.0} };
    vector<loomo::Pose> tmpOdoTraj = { {0.0, 0.0, 0.0} };

    poseGraph::PoseGraph graph1(1U);
    map<uint64_t, poseGraph::Node> *nodeMap = graph1.getNodeMap();
    uint64_t currentNodeKey = nodeMap->begin()->first;
    uint64_t prevNodeKey;

    Pose odoIncrement = { 0.0, 0.0, 0.0 };
    double grossTraveledDistance = 0;
    double incrementDistance = 0;
    //Eigen::Matrix3d odoUncertainty;

    for (size_t i = 1; i < dataLoomo1.size(); ++i) {
        int dTickL = dataLoomo1[i].tick_left - dataLoomo1[i - 1].tick_left;
        int dTickR = dataLoomo1[i].tick_right - dataLoomo1[i - 1].tick_right;
        loomo1.incrementPose(dTickL, dTickR);

        //odoUncertainty = loomo1.getUncertainty();

        odoIncrement += loomo1.getIncrement();
        tmpOdoTraj.push_back(odoIncrement);
        incrementDistance += loomo1.getIncrement().norm();
        grossTraveledDistance += loomo1.getIncrement().norm();

        if (dataLoomo1[i].fisheye_idx != dataLoomo1[i - 1].fisheye_idx) {
            capFisheyeLoomo1 >> capFrame;
            if (capFrame.empty()) {
                cout << "End of video" << endl;
                break;
            }
            cvtColor(capFrame, greyFrame, COLOR_BGR2GRAY);
        }

        //// add new node every 1m
        if ((incrementDistance > 1000.0) || (abs(odoIncrement.theta) > loomo::PI / 4)) {
            prevNodeKey = currentNodeKey;

            // append to odoTraj
            {
                double theta = odoTraj.back().theta;
                Eigen::Vector3d prevPose(odoTraj.back().x, odoTraj.back().y, theta);
                Eigen::Matrix3d rot;
                rot << cos(theta), -sin(theta), 0,
                    sin(theta), cos(theta), 0,
                    0, 0, 1;
                for (auto & pose : tmpOdoTraj) {
                    Eigen::Vector3d tmp(pose.x, pose.y, pose.theta);
                    tmp = rot * tmp;
                    tmp += prevPose;
                    pose = { tmp(0), tmp(1), tmp(2) };
                }
                odoTraj.insert(odoTraj.end(), tmpOdoTraj.begin(), tmpOdoTraj.end());
            }
            currentNodeKey = graph1.addNodeFromOdo(loomo1.getPose(), loomo1.getUncertainty());

            // iterator (pointer) to nodes
            auto itCurrentNode = nodeMap->find(currentNodeKey);
            auto itPrevNode = nodeMap->find(prevNodeKey);

            mVision.detectFeatures(
                greyFrame,
                itCurrentNode->second.keyPoints,
                itCurrentNode->second.descriptors
            );

            //Match current and previous node
            if (!(itPrevNode->second.descriptors).empty()) {
                vector<DMatch> matches;
                mVision.matchImages(
                    itCurrentNode->second.descriptors,
                    itPrevNode->second.descriptors,
                    matches
                );
                // need at least 6 points to estimate the essential matrix
                //if (matches.size() > 5) {
                Mat E, R, t;
                if (mVision.tryRecoverPose(itCurrentNode->second.keyPoints, itPrevNode->second.keyPoints, matches, E, R, t)) {
                    //    //Eigen::Vector3d pose;
                    //    //pose << itCurrentNode->second.pose.x, itCurrentNode->second.pose.y, itCurrentNode->second.pose.theta;
                    //    ////Eigen::Matrix4d rotPitch, rot, T;
                    //    //Mat rotPitch, rot, T = Mat::zeros(4,4, CV_64FC1);
                    //    //rotPitch = (cv::Mat_<double>(4, 4) <<
                    //    //    0, 0, -1, 0,
                    //    //    0, 1, 0, 0,
                    //    //    1, 0, 0, 0,
                    //    //    0, 0, 0, 1);
                    //    //rot = (Mat_<double>(4,4) <<
                    //    //    cos(pose(2)), -sin(pose(2)), 0, 0,
                    //    //    sin(pose(2)), cos(pose(2)), 0, 0,
                    //    //    0, 0, 1, 0,
                    //    //    0, 0, 0, 1);
                    //    //Rect srcRectR(Point(0, 0), Size(R.rows, R.cols));
                    //    //Rect dstRectR(Point(0, 0), srcRectR.size());
                    //    //R(srcRectR).copyTo(T(dstRectR));
                    //    //Rect srcRectT(Point(0, 0), Size(R.rows, R.cols));
                    //    //Rect dstRectT(Point(0, 0), srcRectT.size());
                    //    //t(srcRectT).copyTo(T(dstRectT));

                    //    //Mat constraint6dof = rot * rotPitch*T;

                    //    //t = R * t;
                    double ang = asin(R.at<double>(0, 1));
                    cout << "Ang = " << ang << ", theta = " << odoIncrement.theta << endl;
                    //if (R.at<double>(1, 0) < 0)
                    //    ang *= -1;
                    double scale = odoIncrement.norm();
                    Pose tmp = { t.at<double>(2)*scale, t.at<double>(1)*scale, ang };
                    Eigen::Matrix3d covar = Eigen::Matrix3d::Zero();
                    covar.block<2, 2>(0, 0) = loomo1.getUncertainty().block<2, 2>(0, 0);
                    covar.block(0, 0, 2, 2) *= 1.5;
                    covar(2, 2) = loomo1.getUncertainty()(2, 2);
                    //covar.block<2, 1>(0, 2) = loomo1.getUncertainty().block<2, 1>(0, 2) * 0.5;
                    //covar.block<1, 2>(2, 0) = loomo1.getUncertainty().block<1, 2>(2, 0) * 0.5;
                    double scale1 = 4.8 * exp(-0.04*(matches.size() - 6)) + 0.2;
                    double scale2 = 50.0 / matches.size();
                    //cout << "Covar theta odo: " << covar(2, 2) << endl;
                    covar *= scale1;
                    //cout << "Covar theta scaled: " << covar(2, 2) << endl;
                    //covar(2, 2) = scale1;
                    graph1.addVirtualEdge(tmp, prevNodeKey, covar);

                    //graph1.optimizeGraph();
                //    //cout << "R:" << endl << R << endl;
                //    //cout << "Matches: " << matches.size() << endl;
                //    //cout << "Scale log: " << scale2 << ", scale exp: " << scale1 << endl;
                //    //cout << "t:" << endl << t << endl;
                //    //cout << "img matching: (" << tmp.x << ", " << tmp.y << ", " << tmp.theta << ")" << endl;
                //    //cout << "odo: (" << odoIncrement.x << ", " << odoIncrement.y << ", " << odoIncrement.theta << ")" << endl;
                }
                else {
                    cout << "Recover pose unsuccessful. N.o. matches = " << matches.size() << endl;
                }

            }

            odoIncrement = { 0.0, 0.0, 0.0 };
            tmpOdoTraj.clear();
            loomo1.reset();
            incrementDistance = 0;
        }

    }

    cout << "Gross distance: " << grossTraveledDistance / 1000 << " m" << endl;
    cout << "Odo \"miss\": " << odoTraj.back().norm() << " mm" << endl;

    /*
    check for matches
    */
    //for (auto node_i = nodeMap->begin(); node_i != nodeMap->end(); node_i++) {
    //    int i = 0;
    //    //cout << "Node " << (uint32_t)node_i->first << endl;
    //    for (auto node_j = nodeMap->begin(); node_j != nodeMap->end(); node_j++) {
    //        vector<DMatch> matches;
    //        ++i;

    //        if (!(node_j->second.descriptors).empty() && !(node_i->second.descriptors).empty()) {
    //            mVision.matchImages(
    //                node_i->second.descriptors,
    //                node_j->second.descriptors,
    //                matches
    //            );
    //            cout << matches.size();
    //        }
    //        else {
    //            cout << "0";
    //        }
    //        //cout << (uint32_t)node_j->first;
    //        cout << ",";
    //    }
    //    cout << endl;
    //    //cout << --i << endl;
    //}

    poseGraph::PoseGraph graph2 = graph1;
    //Eigen::Matrix3d covar = Eigen::Matrix3d::Identity() *1000;
    //Pose tmp = { 0.0, 0.0, loomo::PI };
    //graph2.addVirtualEdge(tmp, nodeMap->begin()->first, covar);
    cv::imshow("Graph trajectory w/o final edge", slamUtils::drawGraph(graph2));
    graph2.optimizeGraph();
    graph2.optimizeGraph();
    graph2.optimizeGraph();
    graph2.optimizeGraph();
    graph2.optimizeGraph();
    graph2.optimizeGraph();
    cv::imshow("Graph trajectory w/o final edge, w. post optimization", slamUtils::drawGraph(graph2));

    Eigen::Matrix3d covar = Eigen::Matrix3d::Identity();// *0.1;
    Pose tmp = { 0.0, 0.0, loomo::PI };
    //Pose tmp = { 0.0, 0.0, 0.0 };
    graph1.addVirtualEdge(tmp, nodeMap->begin()->first, covar);
    cv::imshow("Graph trajectory", slamUtils::drawGraph(graph1));
    graph1.optimizeGraph();
    graph1.optimizeGraph();
    graph1.optimizeGraph();
    graph1.optimizeGraph();
    graph1.optimizeGraph();
    graph1.optimizeGraph();
    cv::imshow("Graph trajectory w. post optimization", slamUtils::drawGraph(graph1));

    cout << "\"odo\"-Graph \"miss\": " << graph2.getNodeMap()->find(currentNodeKey)->second.pose.norm() << " mm" << endl;
    cout << "Graph \"miss\": " << graph1.getNodeMap()->find(currentNodeKey)->second.pose.norm() << " mm" << endl;

    cv::imshow("Odometry trajectory", slamUtils::drawOdoTrajectory(odoTraj));
    cv::waitKey(0);
}


#include <iostream>
#include <fstream>
#include <algorithm>    // std::swap
#include <vector>       // std::vector
#include <cstdlib>
#include <windows.h>

#include <dlib/svm_threaded.h>
#include <dlib/rand.h>
#include <dlib/matrix.h>

using namespace std;
using namespace dlib;

template <typename T>
void remove(std::vector<T>& vec, size_t pos)
{
	std::vector<T>::iterator it = vec.begin();
	std::advance(it, pos);
	vec.erase(it);
}

double calculateaccuracy(matrix<double>  confusionmatrix)
{
	int TValue = 0;
	int FValue = 0;
	for (int i = 0; i < 11; i++) {
		for (int j = 0; j < 11; j++)
		{
			if (i == j)
			{
				TValue += confusionmatrix(i, j);
			}
			else
			{
				FValue += confusionmatrix(i, j);
			}

		}

	}
	printf("T: %d, F :%d", TValue, FValue);
	return (TValue / double(TValue + FValue));
}

/*double dlib_svm_Multi_Leaveoneout(std::vector<std::vector<double>> features_, std::vector<double> label_, int feature_size_, int noSamples, double alfa)
{
	typedef matrix<double, 703, 1> sample_type; // 4 = Feature Size, cannot be set to variable !!!!

	std::vector<sample_type> feature_dlib;
	std::vector<sample_type> feature_train_dlib;
	std::vector<sample_type> feature_test_dlib;

	std::vector<double> train_label;
	std::vector<double> test_label;

	//Filling Vector_Matrix
	double TotAccuracy = 0;	//# true positives
	double accuracy = 0;

	//printf("started.. \n");
	for (int m = 0; m < noSamples; m++)
	{
		sample_type temp;
		temp.set_size(feature_size_);
		//cout << "Set size " << temp.size  << endl;

		for (int j = 0; j < feature_size_; j++)
			temp(j, 0) = features_[m][j];

		feature_dlib.push_back(temp);

	}

	clock_t begin = clock();

	//Leave-One-out
	for (int testid = 0; testid < noSamples; testid++)
	{

		double  error = 0;
		accuracy = 0;
		//printf("Feature dlib %d", feature_dlib.data()[testid]);
		feature_test_dlib.push_back(feature_dlib.data()[testid]);
		test_label.push_back(label_.data()[testid]);
		feature_train_dlib = feature_dlib;
		remove(feature_train_dlib, testid);
		train_label = label_;
		remove(train_label, testid);

		typedef linear_kernel<sample_type> linear_kernel;

		typedef svm_multiclass_linear_trainer <linear_kernel, double> svm_mc_trainer;
		svm_mc_trainer trainer;
		//printf("Thread %d \n", trainer.get_num_threads());

		multiclass_linear_decision_function<linear_kernel, double> df = trainer.train(feature_train_dlib, train_label);

		//randomize_samples(feature_dlib, label_);
		//accuracy = cross_validate_multiclass_trainer(trainer, feature_dlib, label_, 5);
		//printf("accuracy %5.1f", accuracy);
		double prediction = df(feature_test_dlib[0]);
		printf("Prediction %d, Expecct %d \n",prediction,test_label[0]);
		if (prediction != test_label[0])
		{
			error = 1;
			//printf("ERROR \n");
		}
		accuracy = 1 - (error);
		TotAccuracy += accuracy;
	}
	clock_t end = clock();
	printf("has completed in %4.1f seconds and accuracy %f \n", double(end - begin) / CLOCKS_PER_SEC, TotAccuracy);
	return TotAccuracy / noSamples;

}

*/

double dlib_svm_multiclass_kfold(std::vector<std::vector<double>> features_, std::vector<double> label_, int feature_size_, int noSamples, double alfa)
{
	typedef matrix<double, 703, 1> sample_type; // 4 = Feature Size, cannot be set to variable !!!!

	std::vector<sample_type> feature_dlib;
	double accuracy = 0;

	printf("started.. \n");
	for (int m = 0; m < noSamples; m++)
	{
		sample_type temp;
		temp.set_size(feature_size_);
		//cout << "Set size " << temp.size  << endl;

		for (int j = 0; j < feature_size_; j++)
			temp(j, 0) = features_[m][j];

		feature_dlib.push_back(temp);

	}

	clock_t begin = clock();

	typedef linear_kernel<sample_type> linear_kernel;

	typedef svm_multiclass_linear_trainer <linear_kernel, double> svm_mc_trainer;
	svm_mc_trainer trainer;

	//multiclass_linear_decision_function<linear_kernel,double> df = trainer.train(feature_dlib, label_);

	matrix<double, 11, 11>  confusionmatrix;
	confusionmatrix.set_size(11, 11);
	confusionmatrix = cross_validate_multiclass_trainer(trainer, feature_dlib, label_, 5);
	accuracy = calculateaccuracy(confusionmatrix);

	clock_t end = clock();
	printf("has completed in %4.1f seconds and accuracy %f\n", double(end - begin) / CLOCKS_PER_SEC, accuracy);
	return accuracy;

}

double dlib_svm_kfold(std::vector<std::vector<double>> features_, std::vector<double> label_, int feature_size_, int noSamples, double alfa)
{
	typedef matrix<double, 703, 1> sample_type; // 4 = Feature Size, cannot be set to variable !!!!

	std::vector<sample_type> feature_dlib;
	double accuracy = 0;

	printf("started.. \n");
	for (int m = 0; m < noSamples; m++)
	{
		sample_type temp;
		temp.set_size(feature_size_);
		//cout << "Set size " << temp.size  << endl;

		for (int j = 0; j < feature_size_; j++)
			temp(j, 0) = features_[m][j];

		feature_dlib.push_back(temp);

	}

	clock_t begin = clock();

/*	typedef linear_kernel<sample_type> linear_kernel;

	typedef svm_multiclass_linear_trainer <linear_kernel, double> svm_mc_trainer;
	svm_mc_trainer trainer;

	multiclass_linear_decision_function<linear_kernel,double> df = trainer.train(feature_dlib, label_); */

	typedef one_vs_one_trainer<any_trainer<sample_type> > ovo_trainer;

	ovo_trainer trainer;
	typedef linear_kernel<sample_type> linear_kernel;

	krr_trainer<linear_kernel> linear_trainer;

	linear_trainer.set_kernel(linear_kernel());
	trainer.set_trainer(linear_trainer);        // linear_trainer
	randomize_samples(feature_dlib, label_);
   //printf("Training start \n");
	//one_vs_one_decision_function<ovo_trainer> df = trainer.train(feature_dlib, label_);

	matrix<double, 11,11>  confusionmatrix;
	confusionmatrix.set_size(11, 11);
	confusionmatrix=cross_validate_multiclass_trainer(trainer, feature_dlib, label_, 5);
	accuracy= calculateaccuracy(confusionmatrix);

	clock_t end = clock();
	printf("has completed in %4.1f seconds and accuracy %f\n", double(end - begin) / CLOCKS_PER_SEC,accuracy);
	return accuracy;

}
double dlib_svm(std::vector<std::vector<double>> features_, std::vector<double> label_, int feature_size_, int noSamples, double alfa)
{
	typedef matrix<double, 703, 1> sample_type; // 4 = Feature Size, cannot be set to variable !!!!

	std::vector<sample_type> feature_dlib;

	std::vector<sample_type> feature_train_dlib;
	std::vector<sample_type> feature_test_dlib;

	std::vector<double> train_label;
	std::vector<double> test_label;

	//Filling Vector_Matrix
	double TotAccuracy = 0;	//# true positives
	double accuracy = 0;

	printf("started.. \n");
	for (int m = 0; m < noSamples; m++)
	{
		sample_type temp;
		temp.set_size(feature_size_);
		//cout << "Set size " << temp.size  << endl;

		for (int j = 0; j < feature_size_; j++)
			temp(j, 0) = features_[m][j];

		feature_dlib.push_back(temp);

	}

	clock_t begin = clock();

	//Leave-One-out
	for (int testid = 0; testid < noSamples; testid++)
	{

		double  error = 0;
		accuracy = 0;
		//printf("Feature dlib %d", feature_dlib.data()[testid]);
		feature_test_dlib.push_back(feature_dlib.data()[testid]);
		test_label.push_back(label_.data()[testid]);
		feature_train_dlib = feature_dlib;
		remove(feature_train_dlib, testid);
		train_label = label_;
		remove(train_label, testid);

	/*	for (int m = 0; m < noSamples; m++)
		{
				sample_type temp;
				for (int j = 0; j < feature_size_; j++)
					temp(j, 0) = features_[m][j];
				if (m == testid) {
					feature_test_dlib.push_back(temp);
					test_label.push_back(label_[m]);
				}
				else
				{
					feature_train_dlib.push_back(temp);
					train_label.push_back(label_[m]);
				}

		}*/

		typedef one_vs_one_trainer<any_trainer<sample_type> > ovo_trainer;

		ovo_trainer trainer;
		//typedef radial_basis_kernel<sample_type> rbf_kernel;
		typedef linear_kernel<sample_type> linear_kernel;
		//krr_trainer<rbf_kernel> rbf_trainer;

		//svm_nu_trainer <linear_kernel> linear_trainer;

		krr_trainer<linear_kernel> linear_trainer;
		//svm_multiclass_linear_trainer <rbf_kernel> rbf_trainer;
		int prediction, test_gt;

		//rbf_trainer.set_kernel(rbf_kernel(alfa));
		linear_trainer.set_kernel(linear_kernel());
		//trainer.set_trainer(rbf_trainer);        // rbf_trainer
		 trainer.set_trainer(linear_trainer);        // linear_trainer
		 trainer.set_num_threads(7);
		//randomize_samples(feature_train_dlib, train_label);
		//printf("Training start \n");
		one_vs_one_decision_function<ovo_trainer> df = trainer.train(feature_train_dlib, train_label);

		//std::cout << "Testing started" << endl;
		prediction = df(feature_test_dlib[0]);
		test_gt = test_label[0];

		if (prediction != test_gt) error=1;

		accuracy = 1 - (error);
		//std::cout << "Local accuracy: " << accuracy << endl;

		feature_test_dlib.clear();
		feature_train_dlib.clear();
		test_label.clear();
		train_label.clear();

		TotAccuracy += accuracy;

	}
	clock_t end = clock();
	printf("has completed in %4.1f seconds\n", double(end - begin) / CLOCKS_PER_SEC);

	//printf("One Leave out accuracy %f",(TotAccuracy/ noSamples));
	/*for (int i = 0; i < train_features_.size()-100; i++)
	{
		sample_type temp;
		for (int j = 0; j <feature_size_; j++)
			temp(j, 0) = train_features_[i][j];
		feature_train_dlib.push_back(temp);
		train_label_1.push_back(train_label_[i]);
	}

	for (int i = test_features_.size()-100; i < test_features_.size(); i++)
	{
		sample_type temp;
		for (int j = 0; j <feature_size_; j++)
			temp(j, 0) = test_features_[i][j];
		feature_test_dlib.push_back(temp);
		test_label_1.push_back(test_label_[i]);
	}*/

	//cout << "feature_train_dlib.size: " << feature_train_dlib.size() << endl;
	//cout << "feature_train_dlib[0].size: " << feature_train_dlib[0].size() << endl;
	//cout << "\nfeature_test_dlib.size: " << feature_test_dlib.size() << endl;
	//cout << "feature_test_dlib[0].size: " << feature_test_dlib[0].size() << endl;
	//cout << "\ntrain labels size: " << train_label_1.size() << endl;
	//cout << "test labels size: " << test_label_1.size() << endl;

	feature_dlib.clear();
	return (TotAccuracy / noSamples);

}

int main()
{
	string resultsFilename = "Results_svm.txt";
	string dataFilename = "Pathogen_VOC_Dataset_v4.txt";
	FILE *outFile = fopen(resultsFilename.c_str(), "a+");	//reset file
	fclose(outFile);

	int noClasses = 12;
	int noSamples = 336;
	int noFeatures = 703;
	std::vector<string> classLabels;
	std::vector<double> classIDs;
	std::vector<std::vector<double>> data;
	//open & read data file
	FILE *dataFile = fopen(dataFilename.c_str(), "r");

	int tmpInt1, tmpInt2;
	string tmpStr;
	char* buffer = new char[200];

	fscanf(dataFile, "%d", &noClasses);	//# of classes
	fgets(buffer, 100, dataFile);		//dummy read till read EOL

	classLabels.push_back("");	//dummy push to start index from 1 for class labels
	for (int i = 1; i <= noClasses; i++)
	{
		fscanf(dataFile, "%d  %s", &tmpInt1, buffer);
		classLabels.push_back(string(buffer));
	}

	fscanf(dataFile, "%d", &noSamples);		//reads # of classes from the file
	fgets(buffer, 100, dataFile);			//dummy read till read EOL

	fscanf(dataFile, "%d", &noFeatures);	//reads the feature dimension from the file
	fgets(buffer, 100, dataFile);			//dummy read till read EOL

	for (int i = 0; i < noSamples; i++)
	{
		fscanf(dataFile, "%d %d", &tmpInt1, &tmpInt2);	//read sample ID (dummy read) & class ID of the current feature vector
		classIDs.push_back(tmpInt2);	//store class ID

		data.push_back(std::vector<double>());
		for (int j = 0; j < noFeatures; j++)
		{
			fscanf(dataFile, "%d", &tmpInt1);	//read feature vector
			data[i].push_back(tmpInt1);
		}
	}

	delete buffer;
	double alfa = 0.1;  // For SVM, standart range 0-1

	std::vector<int> selFeatIDs;
	double Allbest = 0.0;

	//select all features in a circular manner
	for (int j = 0; j < noFeatures; j++)
		selFeatIDs.push_back(j);

	for (int j = noFeatures; j >= 0; j--)
	{
		int eliminated = 0;
		if (j < noFeatures)	//no elimination in the first loop
		{
			eliminated = selFeatIDs[j];
			selFeatIDs.erase(selFeatIDs.begin() + j);	//jth feature is appended
			noFeatures -= 1;
		}

		std::vector<std::vector<double>> filtereddata;

		for (int m = 0; m < noSamples; m++)
		{
			filtereddata.push_back(std::vector<double>());
			for (int p = 0; p < selFeatIDs.size(); p++)
			{
				filtereddata[m].push_back(data[m][selFeatIDs[p]]);

			}
			//printf("Filtered Feature size %d \n", (filtereddata[m].size()));
		}
		// SVM Classifer
		//printf("Feature removal :%d", j);

		double accuracy = dlib_svm(filtereddata, classIDs, noFeatures, noSamples, alfa);
		// double accuracy =  dlib_svm_kfold(filtereddata, classIDs, noFeatures, noSamples, alfa);
		// double accuracy = dlib_svm_multiclass_kfold(filtereddata, classIDs, noFeatures, noSamples, alfa);
		//double accuracy =  dlib_svm_Multi_Leaveoneout(filtereddata, classIDs, noFeatures, noSamples, alfa);
		//cout << "Accuracy:\n" << endl << accuracy << endl;
		if (accuracy < Allbest)
		{
			//printf("Acuracy reduced");
			selFeatIDs.insert(selFeatIDs.begin() + j, eliminated);
			noFeatures += 1;
		}
		else
		{
			//printf("Acuracy improved or same");
			Allbest = accuracy;
		}
		printf("Accuracy :%5.3f, Overall accuracy :%5.3f and feature removal round: %d", accuracy,Allbest, (703-j));
		outFile = fopen(resultsFilename.c_str(), "a+");	//reset file

		fprintf(outFile, "%d /703 is done ;No. Selected Features: %d \n", j , selFeatIDs.size());

		fprintf(outFile, "#####  Accuracy: %5.3f ##### Best Accuracy: %5.3f #####\nSelected Feature IDs:", accuracy, Allbest);

		for (int j = 0; j < selFeatIDs.size(); j++) fprintf(outFile, "%d, ", selFeatIDs[j]);

		fprintf(outFile, "\n\n");

		fclose(outFile);
	}

	system("PAUSE");
	return 0;
}


#include <ostream>

#include "gtest/gtest.h"
#include <Eigen/Dense>

#include "libqhull_r/qhull_ra.h"

#include "libqhullcpp/RboxPoints.h"
#include "libqhullcpp/QhullError.h"
#include "libqhullcpp/QhullQh.h"
#include "libqhullcpp/QhullFacet.h"
#include "libqhullcpp/QhullFacetList.h"
#include "libqhullcpp/QhullFacetSet.h"
#include "libqhullcpp/QhullLinkedList.h"
#include "libqhullcpp/QhullPoint.h"
#include "libqhullcpp/QhullUser.h"
#include "libqhullcpp/QhullVertex.h"
#include "libqhullcpp/QhullVertexSet.h"
#include "libqhullcpp/Qhull.h"
#include "polytope.hpp"

TEST(TESTQHull, basicTest) {
    constexpr int dimension = 3;

    // box
    constexpr int numberOfPoints  = 9;
    double points[numberOfPoints*dimension]{-1, -1, -1,
            -1, -1, 1,
            -1, 1, -1,
            -1, 1, 1,
            1, -1, -1,
            1, -1, 1,
            1, 1, -1,
            1, 1, 1,
            0, 0, 0};
    orgQhull::Qhull q {"", dimension, numberOfPoints, points, "o"};
    orgQhull::QhullFacetList facets= q.facetList();
    //std::cout << facets << std::endl;

    for (auto &facet : q.facetList())
    {
        std::cout << "Facet> " << facet;
        orgQhull::QhullVertexSet vertices = facet.vertices();
        for (const auto &vertex : vertices)
      	{
            std::cout << "Vertex> " << vertex;
      	}

        Eigen::Vector3d center(facet.getCenter().coordinates());
        std::cout << " center: " << center << std::endl;
        Eigen::Vector3d normal(facet.hyperplane().coordinates());
        std::cout << " normal: " << normal << std::endl;
        orgQhull::QhullVertexSet::iterator it = vertices.begin();
        Eigen::Vector3d firstVertex((*it).point().coordinates());
        std::cout << " firstVertex: " << firstVertex  << std::endl;
        firstVertex = (firstVertex - center);
        firstVertex.normalize();
        std::cout << " firstVertex: " << firstVertex  << std::endl;;
        std::vector<std::pair<double, orgQhull::QhullVertex> > orderedVertices;
        orderedVertices.push_back(std::pair<double, orgQhull::QhullVertex> (0.0, (*it)));
        it++;
        // first element is the angle.

        for (;it < vertices.end(); it++) {
            Eigen::Vector3d vertex((*it).point().coordinates());
            std::cout << " vertex: " << vertex << " id: " << (*it).id()  << std::endl;
            vertex = vertex - center;
            vertex.normalize();
            std::cout << " vertex: " << vertex << std::endl;
            std::cout << " cos(angle) " << firstVertex.dot(vertex) << std::endl;
            std::cout << " (angle) " << acos(firstVertex.dot(vertex)) << std::endl;
            double angle = acos(firstVertex.dot(vertex));
            Eigen::Vector3d crossProductFirstAndCurrent = firstVertex.cross(vertex);
            std::cout << " cross product " << crossProductFirstAndCurrent << " is zero " << crossProductFirstAndCurrent.isZero() << std::endl;
            std::cout << " dot(crossProductFirstAndCurrent, normal) " << crossProductFirstAndCurrent.dot(normal) << std::endl;
            if (!crossProductFirstAndCurrent.isZero() && crossProductFirstAndCurrent.dot(normal) < 0)
            {
                angle = -angle + 2 * EIGEN_PI;
            }
            //std::cout << " final angle: " << angle << std::endl;
            std::pair<double, orgQhull::QhullVertex> pair(angle, (*it));
            orderedVertices.push_back(std::pair<double, orgQhull::QhullVertex> (angle, (*it)));
        }

        std::sort( std::begin(orderedVertices), std::end(orderedVertices),
                   []( const std::pair<double, orgQhull::QhullVertex> &left, const std::pair<double, orgQhull::QhullVertex> &right)
                   {
                       return  left.first < right.first;
                   } );
        for (auto &v : orderedVertices)
        {
            std::cout << " ordered vertex: " << v.second;
        }

        std::cout << " ALL vertices: " << std::endl;
        for (auto v = q.beginVertex(); v != q.endVertex(); v = v.next()){
            std::cout << "ID: " << v.id() << "->" << v;
        }

        std::cout << "ALL vertices again: " << q.vertexList() << std::endl;
        std::cout << "ALL points: " << q.points() << std::endl;

    }

    //spdlog::info("A basic test");
    //spdlog::info("sizeof realT {} double {}", sizeof(realT), sizeof(double));
    // orgQhull::RboxPoints eg("100");
    // orgQhull::Qhull q(eg, "");

    ASSERT_EQ(true, true);
}

void print_summary(qhT *qh) {
    facetT *facet;
    vertexT *vertex, **vertexp;
    int k;

    printf("\n%d vertices and %d facets with normals:\n",
           qh->num_vertices, qh->num_facets);
    FORALLfacets {
        printf("FACET\n");
        for (k=0; k < qh->hull_dim; k++)
        {
            printf("%6.2g ", facet->normal[k]);

        }
        printf("\n");
        printf("center "); for (k=0; k < qh->hull_dim; k++) {printf("%6.2g", facet->center[k]);} printf("\n");
        printf(" toporient %i\n", facet->toporient);
        FOREACHvertex_(facet->vertices) {
            printf("vertex ");
            for (int k=0; k < qh->hull_dim; k++)
                printf("%5.2f ", vertex->point[k]);
            printf("\n");
        }
    }

}

TEST(TESTQHull, DISABLED_cTest) {
    constexpr int dim = 3;
    constexpr int numpoints = 8;
    coordT points[numpoints*dim] = {-1, -1, -1,
                                    -1, -1, 1,
                                    -1, 1, -1,
                                    -1, 1, 1,
                                    1, -1, -1,
                                    1, -1, 1,
                                    1, 1, -1,
                                    1, 1, 1};

    boolT ismalloc= False; /* True if qhull should free points in qh_freeqhull() or reallocation */
    //char flags[] = "qhull QJ";
    char flags[255];
    sprintf(flags, "qhull s Tcv Fx");
    FILE *outfile= stdout;
    FILE *errfile= stderr;
    int exitcode;
    facetT *facet;
    int curlong, totlong;

    qhT qh_qh;
    qhT *qh= &qh_qh;

    QHULL_LIB_CHECK

        qh_zero(qh, errfile);

    exitcode= qh_new_qhull(qh, dim, numpoints, points, ismalloc,
                           flags, outfile, errfile);
    fflush(NULL);

    if (!exitcode) {                  /* if no error */
        print_summary(qh);
        facetT *facet;
        vertexT *vertex, **vertexp;
        FORALLfacets {
            /* ... your code ... */
            FOREACHvertex_(facet->vertices) {
                // for (int k=0; k < dim; k++)
                //       printf("%5.2f ", vertex->point[k]);
            }
        }
    }

    qh_freeqhull(qh, !qh_ALL);                   /* free long memory  */
    qh_memfreeshort(qh, &curlong, &totlong);    /* free short memory and memory allocator */
    if (curlong || totlong)
        fprintf(errfile, "qhull internal warning (user_eg, #1): did not free %d bytes of long memory (%d pieces)\n", totlong, curlong);
}

TEST(TESTQHull, qhullPrism) {
    constexpr int dimension = 3;

    // box
    constexpr int numberOfPoints  = 6;
    double points[numberOfPoints*dimension]{
                     0, 0, 1,
                         0, 1, 1,
                         0.866025, 0, -0.5,
                         0.866025, 1, -0.5,
                         -0.866025, 0, -0.5,
                         -0.866025, 1, -0.5};
    orgQhull::Qhull q {"", dimension, numberOfPoints, points, "o"};

    std::cout << " ALL facets: " << std::endl;
    orgQhull::QhullFacetList facets= q.facetList();
    std::cout << facets << std::endl;

    std::cout << " ALL vertices: " << std::endl;
    for (auto v = q.beginVertex(); v != q.endVertex(); v = v.next()){
        std::cout << "ID: " << v.id() << "->" << v;
    }

}

TEST(TESTQHull, rbox)
{
    orgQhull::RboxPoints rbox {"10 s D3"}; /// 10 points in a 3d sphere

    auto itbegin = rbox.beginCoordinates();
    auto itend = rbox.endCoordinates();

    int count = 0;
    for (auto it = rbox.beginCoordinates(); it != rbox.endCoordinates(); ++it)
    {
        std::cout << "coordinate: " << (*it) << std::endl;
        count++;
    }
    std::cout << "number coordinates: " << count;

    orgQhull::Qhull q {rbox, "o"};

    std::vector<OB::Point> points;
    for (auto it = rbox.beginCoordinates(); it != rbox.endCoordinates(); ++it) {
        OB::Real x = static_cast<OB::Real>(*(it++));
        OB::Real y = static_cast<OB::Real>(*(it++));
        OB::Real z = static_cast<OB::Real>(*(it));

        points.push_back(OB::Point(x, y, z));

    }

    for (auto& p : points) {
        std::cout << " Points " << p.transpose() << std::endl;
    }

    int numberOfPoints = points.size();
    std::vector<realT> pointCoordinates(numberOfPoints * 3);

    for (size_t i = 0; i < points.size(); i++)
    {
        const OB::Point& p = points[i];
        pointCoordinates[i*3+0] = static_cast<realT>(p(0));
        pointCoordinates[i*3+1] = static_cast<realT>(p(1));
        pointCoordinates[i*3+2] = static_cast<realT>(p(2));
    }
    orgQhull::Qhull q2 {"", 3, numberOfPoints, pointCoordinates.data(), "o"};

    // std::cout << " ALL facets: " << std::endl;
    // orgQhull::QhullFacetList facets= q.facetList();
    // std::cout << facets << std::endl;

    // std::cout << " ALL vertices: " << std::endl;
    // for (auto v = q.beginVertex(); v != q.endVertex(); v = v.next()){
    //     std::cout << "ID: " << v.id() << "->" << v;
    // }
}


// Copyright (c) 2021 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <pybind11/numpy.h>
#include <pybind11/stl.h>

#include <Eigen/Core>
#include <boost/geometry.hpp>
#include <iostream>
#include <optional>

#include "pyinterp/axis.hpp"
#include "pyinterp/detail/broadcast.hpp"
#include "pyinterp/detail/isviewstream.hpp"
#include "pyinterp/detail/math/binning.hpp"
#include "pyinterp/detail/math/streaming_histogram.hpp"
#include "pyinterp/eigen.hpp"

namespace pyinterp {

/// Group a number of more or less continuous values into a smaller number of
/// "bins" located on a grid.
template <typename T>
class Histogram2D {
 public:
  /// Statistics handled by this object.
  using StreamingHistogram = detail::math::StreamingHistogram<T>;

  /// Default constructor
  ///
  /// @param x Definition of the bin centers for the X axis of the grid.
  /// @param y Definition of the bin centers for the Y axis of the grid.
  Histogram2D(std::shared_ptr<Axis<double>> x, std::shared_ptr<Axis<double>> y,
              const std::optional<size_t>& bin_count)
      : x_(std::move(x)), y_(std::move(y)), histogram_(x_->size(), y_->size()) {
    if (bin_count) {
      for (int ix = 0; ix < histogram_.rows(); ++ix) {
        for (int jx = 0; jx < histogram_.cols(); ++jx) {
          histogram_(ix, jx).resize(*bin_count);
        }
      }
    }
  }

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

  /// Copy constructor
  ///
  /// @param rhs right value
  Histogram2D(const Histogram2D& rhs) = delete;

  /// Move constructor
  ///
  /// @param rhs right value
  Histogram2D(Histogram2D&& rhs) noexcept = delete;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  auto operator=(const Histogram2D& rhs) -> Histogram2D& = delete;

  /// Move assignment operator
  ///
  /// @param rhs right value
  auto operator=(Histogram2D&& rhs) noexcept -> Histogram2D& = delete;

  /// Inserts new values in the grid from Z values for X, Y data
  /// coordinates.
  void push(const pybind11::array_t<T>& x, const pybind11::array_t<T>& y,
            const pybind11::array_t<T>& z) {
    detail::check_array_ndim("x", 1, x, "y", 1, y, "z", 1, z);
    detail::check_ndarray_shape("x", x, "y", y, "z", z);

    auto _x = x.template unchecked<1>();
    auto _y = y.template unchecked<1>();
    auto _z = z.template unchecked<1>();

    {
      pybind11::gil_scoped_release release;

      const auto& x_axis = static_cast<pyinterp::detail::Axis<double>&>(*x_);
      const auto& y_axis = static_cast<pyinterp::detail::Axis<double>&>(*y_);

      for (pybind11::ssize_t idx = 0; idx < x.size(); ++idx) {
        auto value = _z(idx);

        if (!std::isnan(value)) {
          auto ix = x_axis.find_index(_x(idx), true);
          auto iy = y_axis.find_index(_y(idx), true);

          if (ix != -1 && iy != -1) {
            histogram_(ix, iy)(value);
          }
        }
      }
    }
  }

  /// Reset the statistics.
  void clear() {
    for (Eigen::Index ix = 0; ix < histogram_.rows(); ++ix) {
      for (Eigen::Index jx = 0; jx < histogram_.cols(); ++jx) {
        histogram_(ix, jx).clear();
      }
    }
  }

  /// Compute the count of points within each bin.
  [[nodiscard]] auto count() const -> pybind11::array_t<uint64_t> {
    return calculate_statistics<decltype(&StreamingHistogram::count),
                                uint64_t>(&StreamingHistogram::count);
  }

  /// Compute the minimum of values for points within each bin.
  [[nodiscard]] auto min() const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::min);
  }

  /// Compute the maximum of values for points within each bin.
  [[nodiscard]] auto max() const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::max);
  }

  /// Compute the mean of values for points within each bin.
  [[nodiscard]] auto mean() const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::mean);
  }

  /// Compute the variance of values for points within each bin.
  [[nodiscard]] auto variance() const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::variance);
  }

  /// Compute the kurtosis of values for points within each bin.
  [[nodiscard]] auto kurtosis() const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::kurtosis);
  }

  /// Compute the quantile of values for points within each bin.
  [[nodiscard]] auto quantile(const T& q) const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::quantile, q);
  }

  /// Compute the skewness of values for points within each bin.
  [[nodiscard]] auto skewness() const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::skewness);
  }
  /// Compute the sum of weights within each bin.
  [[nodiscard]] auto sum_of_weights() const -> pybind11::array_t<T> {
    return calculate_statistics(&StreamingHistogram::sum_of_weights);
  }

  /// Gets the X-Axis
  [[nodiscard]] inline auto x() const -> std::shared_ptr<Axis<double>> {
    return x_;
  }

  /// Gets the Y-Axis
  [[nodiscard]] inline auto y() const -> std::shared_ptr<Axis<double>> {
    return y_;
  }

  /// Pickle support: get state of this instance
  [[nodiscard]] auto getstate() const -> pybind11::tuple {
    return pybind11::make_tuple(x_->getstate(), y_->getstate(), marshal());
  }

  /// Pickle support: set state of this instance
  static auto setstate(const pybind11::tuple& state)
      -> std::unique_ptr<Histogram2D<T>> {
    if (state.size() != 3) {
      throw std::invalid_argument("invalid state");
    }

    // Unmarshalling X-Axis
    auto x = std::make_shared<Axis<double>>();
    *x = Axis<double>::setstate(state[0].cast<pybind11::tuple>());

    // Unmarshalling Y-Axis
    auto y = std::make_shared<Axis<double>>();
    *y = Axis<double>::setstate(state[1].cast<pybind11::tuple>());

    // Unmarshalling instance
    auto result = std::make_unique<Histogram2D<T>>(x, y, 40);
    auto marshal_data = state[2].cast<pybind11::bytes>();
    Histogram2D::unmarshal(marshal_data.cast<std::string_view>(),
                           result->histogram_);
    return result;
  }

  /// Aggregation of statistics
  auto operator+=(const Histogram2D& other) -> Histogram2D& {
    if (*x_ != *(other.x_) || *y_ != *(other.y_)) {
      throw std::invalid_argument("Unable to combine different grids");
    }
    for (Eigen::Index ix = 0; ix < histogram_.rows(); ++ix) {
      for (Eigen::Index iy = 0; iy < histogram_.cols(); ++iy) {
        auto& lhs = histogram_(ix, iy);
        auto& rhs = other.histogram_(ix, iy);

        // Statistics are defined only in the other instance.
        if (lhs.size() == 0 && rhs.size() != 0) {
          lhs = rhs;
          // If the statistics are defined in both instances they can be
          // combined.
        } else if (lhs.size() != 0 && rhs.size() != 0) {
          lhs += rhs;
        }
      }
    }
    return *this;
  }

  /// Returns the histogram for each bin.
  auto histograms() const -> pybind11::array_t<detail::math::Bin<T>> {
    auto bins_count = size_t(0);
    for (Eigen::Index ix = 0; ix < histogram_.rows(); ++ix) {
      for (Eigen::Index iy = 0; iy < histogram_.cols(); ++iy) {
        bins_count = std::max(
            bins_count, histogram_(ix, iy).size());
      }
    }
    auto result =
        pybind11::array_t<detail::math::Bin<T>>(pybind11::array::ShapeContainer(
            {x_->size(), y_->size(),
             static_cast<pybind11::ssize_t>(bins_count)}));
    auto _result = result.template mutable_unchecked<3>();
    {
      auto gil = pybind11::gil_scoped_release();

      for (Eigen::Index ix = 0; ix < histogram_.rows(); ++ix) {
        for (Eigen::Index iy = 0; iy < histogram_.cols(); ++iy) {
          auto iz = size_t(0);
          auto& bins = histogram_(ix, iy).bins();
          for (iz = 0; iz < bins.size(); ++iz) {
            _result(ix, iy, iz) = bins[iz];
          }
          for (; iz < bins_count; ++iz) {
            _result(ix, iy, iz) =
                detail::math::Bin<T>{std::numeric_limits<T>::quiet_NaN(), T(0)};
          }
        }
      }
    }
    return result;
  }

 private:
  /// Grid axis
  std::shared_ptr<Axis<double>> x_;
  std::shared_ptr<Axis<double>> y_;

  /// Statistics grid
  Matrix<StreamingHistogram> histogram_;

  /// Calculation of a given statistical variable.
  template <typename Func, typename Type = T, typename... Args>
  [[nodiscard]] auto calculate_statistics(const Func& func, Args... args) const
      -> pybind11::array_t<Type> {
    pybind11::array_t<Type> z({x_->size(), y_->size()});
    auto _z = z.template mutable_unchecked<2>();
    {
      pybind11::gil_scoped_release release;

      for (Eigen::Index ix = 0; ix < histogram_.rows(); ++ix) {
        for (Eigen::Index iy = 0; iy < histogram_.cols(); ++iy) {
          _z(ix, iy) = (histogram_(ix, iy).*func)(args...);
        }
      }
    }
    return z;
  }

  [[nodiscard]] auto marshal() const -> pybind11::bytes {
    auto gil = pybind11::gil_scoped_release();
    auto ss = std::stringstream();
    ss.exceptions(std::stringstream::failbit);
    auto rows = histogram_.rows();
    ss.write(reinterpret_cast<const char*>(&rows), sizeof(rows));
    auto cols = histogram_.cols();
    ss.write(reinterpret_cast<const char*>(&cols), sizeof(cols));
    for (int ix = 0; ix < histogram_.rows(); ++ix) {
      for (int jx = 0; jx < histogram_.cols(); ++jx) {
        auto marshal_hist = static_cast<std::string>(histogram_(ix, jx));
        auto size = marshal_hist.size();
        ss.write(reinterpret_cast<const char*>(&size), sizeof(size));
        ss.write(marshal_hist.c_str(), size);
      }
    }
    return ss.str();
  }

  static auto unmarshal(const std::string_view& data,
                        Matrix<StreamingHistogram>& histogram) -> void {
    auto gil = pybind11::gil_scoped_release();
    auto ss = detail::isviewstream(data);
    ss.exceptions(std::stringstream::failbit);

    try {
      auto rows = Eigen::Index(0);
      auto cols = Eigen::Index(0);
      ss.read(reinterpret_cast<char*>(&rows), sizeof(rows));
      ss.read(reinterpret_cast<char*>(&cols), sizeof(cols));
      if (rows != histogram.rows() || cols != histogram.cols()) {
        throw std::invalid_argument("invalid state");
      }
      for (int ix = 0; ix < rows; ++ix) {
        for (int jx = 0; jx < cols; ++jx) {
          auto size = size_t(0);
          ss.read(reinterpret_cast<char*>(&size), sizeof(size));
          auto marshal_hist = std::string(size, '\0');
          ss.read(marshal_hist.data(), size);
          histogram(ix, jx) = std::move(StreamingHistogram(marshal_hist));
        }
      }
    } catch (std::ios_base::failure&) {
      throw std::invalid_argument("invalid state");
    }
  }
};

}  // namespace pyinterp


/*

    HUANG Jing
    haungjing@mae.cuhk.edu.hk
    Track contour in an image.
*/

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/photo.hpp>    // denoising module
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <vector>
#include <math.h>

namespace enc = sensor_msgs::image_encodings;

static const char WINDOW[] = "New OpenCV Image";

class ImageConverter
{
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    image_transport::Subscriber image_sub_;
    image_transport::Publisher image_pub_;
    float focal_x = 683;
    float focal_y = 683;
    float depth = 58;

public:
    ImageConverter(char* ros_image_stream): it_(nh_)
    {
        // image_pub_ = it_.advertise("correll_ros2opencv", 1);
        image_sub_ = it_.subscribe(ros_image_stream, 1, &ImageConverter::imageCb, this);

        cv::namedWindow(WINDOW);
    }

    ~ImageConverter()
    {
        cv::destroyWindow(WINDOW);
    }

    void imageCb(const sensor_msgs::ImageConstPtr& msg)
    {
      cv_bridge::CvImagePtr cv_ptr;
      try
      {
        cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
      }
      catch (cv_bridge::Exception& e)
      {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
      }

   // Find countors of red shape
    cv::Point imageOrigin(320, 240),
              xAxisEnd(220, 240), yAxisEnd(320, 340);
    cv::Scalar redLow(0, 173, 152), redHigh(10, 255, 255), black_low(0, 0, 0), black_high(149, 218, 71);
    std::vector< std::vector<cv::Point>> contours;
    cv::Mat trackedImage, trackedImage_denoised, dst, trackedImage_undist;
    cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 683.9731, 0, 320., 0, 683.9731, 240, 0, 0, 1);
    cv::Mat distCoef = (cv::Mat_<double>(1, 5) << -0.7175, 2.8528, 0, 0, -5.1548);

    cv::cvtColor(cv_ptr->image, trackedImage, CV_BGR2HSV);
    // cv::fastNlMeansDenoisingColored(trackedImage,trackedImage_denoised, 3, 3, 7, 21); // This function costs much time.
    cv::blur(trackedImage, trackedImage, cv::Size(3, 3));   // blurring is very important to get rid of noise.
    cv::inRange(trackedImage, black_low, black_high, dst);
    cv::imshow("Binary Image", dst);

    /* Use bgr difference to generate binary image rather than inRange()
    cv::Mat bgr_thr = cv::Mat::zeros(trackedImage.size(), CV_8UC1);
    int Rows = bgr_thr.rows, Cols = bgr_thr.cols;
    for (int r = 0; r < Rows; r ++)
        for (int c = 0; c < Cols; c++)
        {
            cv::Vec3b intensity = cv_ptr->image.at<cv::Vec3b>(cv::Point(c, r));
            if (intensity[2] - intensity[0] > 100 && intensity[2] - intensity[1] > 100)
            {
                bgr_thr.at<uchar>(cv::Point(c, r)) = 255;
            }
        }
    */

    cv::Moments m_dst = moments(dst, true);
    cv::Point p(m_dst.m10 / m_dst.m00, m_dst.m01 / m_dst.m00);

    if (p.x < 0 || p.y < 0)
    {
      std::cout << "No feature shape detected." << std::endl;
    }
    else
    {
      // cv::findContours(bgr_thr, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE, cv::Point(0, 0));
      cv::findContours(dst, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE, cv::Point(0, 0));
      int largestContourIndex = 0;
      for (int i = 0; i < contours.size(); i++)
        largestContourIndex = (contours[i].size() > contours[largestContourIndex].size()) ? i : largestContourIndex;
      cv::Moments shapeMoments = moments(contours[largestContourIndex]);
      cv::Point2f shapeCenter(static_cast<float>(shapeMoments.m10 / shapeMoments.m00),
                              static_cast<float>(shapeMoments.m01 / shapeMoments.m00));
      Eigen::Matrix2f I;        // inertia tensor
      I <<  shapeMoments.mu20, shapeMoments.mu11,
            shapeMoments.mu11, shapeMoments.mu02;
      Eigen::EigenSolver<Eigen::Matrix2f> es(I);
      Eigen::Vector2d eigenVector1(es.eigenvectors().col(0).real()[0], es.eigenvectors().col(0).real()[1]),
                      eigenVector2(es.eigenvectors().col(1).real()[0], es.eigenvectors().col(1).real()[1]);

      // ***The procedure is to get rid of the possible random switch of the principal axis display.
      // ***There is a need of look at the characters of the retrun eigenvectors first to know how to
      // ***better set the processing.
      if (eigenVector1[0] < 0 )
      {
        Eigen::Vector2d tempVector = eigenVector1;
        eigenVector1 = eigenVector2;
      }
      eigenVector2 = Eigen::Vector2d(eigenVector1[1], -eigenVector1[0]);
      /* float angleX1 = atan2(eigenVector1[1], eigenVector1[0]),
            angleX2 = atan2(eigenVector2[1], eigenVector2[0]);
      if (angleX1 > angleX2)
      {
        Eigen::Vector2d tempVector = eigenVector1;
        eigenVector1 = eigenVector2;
        eigenVector2 = tempVector;
      } */
      // std::cout << es.eigenvectors().col(0).real()[0] << "*****" << std::endl;

      cv::RotatedRect rectShape = cv::minAreaRect(contours[largestContourIndex]);
      cv::Point2f rectShapeVertices[4];
      rectShape.points(rectShapeVertices);    // order of vertices: bottomLeft, topLeft, topRight, bottomRight. And the bottom
                                              // is the most bottom.
      //std::vector<std::vector<cv::Point2f>> rectShapeContour;
      //rectShapeContour[0].assign(rectShapeVertices, rectShapeVertices + 4);  // this cause segmentation fault (core dumped)
      // cv::drawContours(cv_ptr->image, rectShapeContour, 0, cv::Scalar(0, 255, 0), 2);
      cv::Point2f rotateOrigin;
      if (rectShapeVertices[2].x > rectShapeVertices[0].x)
      {
        rotateOrigin.x = (rectShapeVertices[0].x + rectShapeVertices[1].x) / 2;
        rotateOrigin.y = (rectShapeVertices[0].y + rectShapeVertices[1].y) / 2;
      }
      else
      {
        rotateOrigin.x = (rectShapeVertices[1].x + rectShapeVertices[2].x) / 2;
        rotateOrigin.y = (rectShapeVertices[1].y + rectShapeVertices[2].y) / 2;
      }
      for (int i = 0; i < 4; i++)
      {
        cv::line(cv_ptr->image, rectShapeVertices[i], rectShapeVertices[(i+1) % 4], cv::Scalar(0, 255, 0), 2);
      }
      cv::circle(cv_ptr->image, rotateOrigin, 10, cv::Scalar(0, 0, 255), -1);

      cv::Point2f principleAxie1 = shapeCenter + 80 * cv::Point2f( eigenVector1[0], eigenVector1[1]),
                  principleAxie2 = shapeCenter + 80 * cv::Point2f( eigenVector2[0], eigenVector2[1]);
      cv::arrowedLine(cv_ptr->image, shapeCenter, principleAxie1, cv::Scalar(0, 0, 255), 2);
      cv::arrowedLine(cv_ptr->image, shapeCenter, principleAxie2, cv::Scalar(0, 255, 0), 2);
      cv::circle(cv_ptr->image, shapeCenter, 5, cv::Scalar(0, 0, 0), -1);
      cv::drawContours(cv_ptr->image, contours, largestContourIndex, cv::Scalar(255, 0, 0), 2);
    }

    cv::circle(cv_ptr->image, imageOrigin, 5, cv::Scalar(0, 0, 0), -1);
    cv::arrowedLine(cv_ptr->image, imageOrigin, xAxisEnd, cv::Scalar(0, 0, 255), 2);
    cv::putText(cv_ptr->image, "x", xAxisEnd - cv::Point(10, 10), CV_FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 0, 255), 2);
    cv::arrowedLine(cv_ptr->image, imageOrigin, yAxisEnd, cv::Scalar(0, 255, 0), 2);
    cv::putText(cv_ptr->image, "y", yAxisEnd + cv::Point(10, 10), CV_FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 255, 0), 2);
   // end of processing
    cv::imshow(WINDOW, cv_ptr->image);
    cv::waitKey(3);

    image_pub_.publish(cv_ptr->toImageMsg());
    }

};

int main(int argc, char** argv)
{
  if (argc == 2)
  {
    ros::init(argc, argv, "image_processing");
    ImageConverter ic(argv[1]);
    ros::spin();
    return 0;
  }
  else if (argc == 1)
  {
  // set default ros image source， HUANG Jing
    char *image_topic = (char*) "/cameras/source_camera/image";
    ros::init(argc, argv, "image_processing");
    ImageConverter ic(image_topic);
    ros::spin();
    return 1;
  }
  else
  {
    std::cout << "ERROR:\tusage - RosToOpencvImage <ros_image_topic>" << std::endl;
    return 1;
  }
}


#ifndef __DYNAUTODIFF_DISTRIBUTIONS__
#define __DYNAUTODIFF_DISTRIBUTIONS__
#include "Common.hpp"
#include "Var.hpp"
#include "boost/math/special_functions/gamma.hpp"
#include <boost/math/special_functions/math_fwd.hpp>
#include <numbers>

#define UNWRAP(...) __VA_ARGS__

namespace DynAutoDiff {
namespace bm = boost::math;
#define DISTFUNCTIONTEMPLATE(functionname, structname, functionargs, funinput, assertstmt,         \
                             requires_grad_stmt)                                                   \
    template <Reduction R = Sum, typename T = double>                                              \
    std::shared_ptr<Var<T>> functionname(std::shared_ptr<Var<T>> X functionargs) {                 \
        assertstmt std::vector<std::shared_ptr<Var<T>>> input_nodes{funinput};                     \
        if constexpr (R == Reduction::None) {                                                      \
            return std::make_shared<Var<T>>(X->rows(), 1, requires_grad_stmt, input_nodes,         \
                                            std::make_unique<structname##EvalGrad<T>>(R));         \
        } else {                                                                                \
            return std::make_shared<Var<T>>(1, 1, requires_grad_stmt, input_nodes,                 \
                                            std::make_unique<structname##EvalGrad<T>>(R));         \
        }                                                                                \
    };

template <typename T = double> struct LnMVNormalDenEvalGrad : EvalGradFunctionBase<T> {
    TMat<T> iS;
    TMat<T> Xmu;
    int N, d;
    int R;
    LnMVNormalDenEvalGrad(int R) : R(R){};
    double Sd; // Sigma determinant
    std::string get_name() const override { return "LnMVNormalDenEvalGrad"; };
    boost::json::object to_json() const override {
        boost::json::object res;
        res["name"] = "ln_mvnormal_den";
        res["Reduction"] = R;
        return res;
    };
    void eval(TMap<T> &dest, const std::vector<TMap<T>> &inputs) override {
        N = inputs[0].rows();
        d = inputs[0].cols();
        iS = inputs[2].inverse();
        Sd = inputs[2].determinant();

        Xmu.resize(N, d);
        TMat<T> res(N, 1);

        for (int i = 0; i < N; ++i) {
            Xmu.row(i) = (inputs[0].row(i).transpose() - inputs[1]).transpose();
            res.coeffRef(i, 0) = -Xmu.row(i) * iS * Xmu.row(i).transpose();
        };

        if (R == Reduction::None) {
            dest = 0.5 * (res.array() - std::log(Sd) - d * log(2 * std::numbers::pi));
        } else {
            dest.coeffRef(0, 0) =
                0.5 * (res.sum() - N * std::log(Sd) - N * d * log(2 * std::numbers::pi));

            if (R == Reduction::Mean) {
                dest = dest.array() / N;
            }
        }
    };
    std::vector<TMat<T>> grad(const std::shared_ptr<Var<T>> &current) override {
        const auto &X = current->input_node(0)->val();
        const auto &mu = current->input_node(1)->val();
        const auto &Sigma = current->input_node(2)->val();

        std::vector<TMat<T>> res;

        // X
        if (current->input_node(0)->requires_grad()) {
            TMat<T> Xg(N, d);
            if (R == Reduction::None) {
                for (int i = 0; i < N; ++i) {
                    Xg.row(i) =
                        -(iS * Xmu.row(i).transpose()).transpose() * current->grad().coeff(i, 0);
                }
            } else {
                Xg = -(iS * Xmu.transpose()).transpose() * current->grad().coeff(0, 0);
            }
            // std::cout << Xg << std::endl;
            if (R == Reduction::Mean) {
                Xg = Xg / N;
            }
            res.emplace_back(Xg);
        } else {
            res.emplace_back(TMat<T>());
        };
        // mu
        if (current->input_node(1)->requires_grad()) {
            TMat<T> mug(d, 1);
            // mu's gradient is negation of X's.
            if (current->input_node(0)->requires_grad()) {
                mug = -res[0].colwise().sum().transpose();
            } else {
                if (R == Reduction::None) {
                    TMat<T> Xg(N, d);
                    for (int i = 0; i < N; ++i) {
                        Xg.row(i) =
                            (iS * Xmu.row(i).transpose()).transpose() * current->grad().coeff(i, 0);
                    }
                    mug = Xg.colwise().sum().transpose();
                } else {
                    mug = (iS * Xmu.transpose()).rowwise().sum() * current->g();
                }
            }
            if (R == Reduction::Mean) {
                mug = mug / N;
            }
            res.emplace_back(mug);

        } else {
            res.emplace_back(TMat<T>());
        }
        // Sigma
        if (current->input_node(2)->requires_grad()) {
            TMat<T> Sg(d, d);

            if (R == Reduction::None) {
                Sg.setZero();
                for (int i = 0; i < N; ++i) {
                    Sg = Sg + iS.transpose() * Xmu.row(i).transpose() * Xmu.row(i) *
                                  iS.transpose() * current->grad().coeff(i, 0);
                }
                Sg = Sg - iS.transpose() * current->grad().sum();
            } else {
                Sg = iS.transpose() * Xmu.transpose() * Xmu * iS.transpose() *
                     current->grad().coeff(0, 0);
                Sg = Sg - iS.transpose() * N;
            }

            Sg = Sg * 0.5;

            if (R == Reduction::Mean) {
                Sg = Sg / N;
            }
            res.emplace_back(Sg);
        } else {
            res.emplace_back(TMat<T>());
        }

        return res;
    }
};

template <typename T = double> struct LnNormalDenEvalGrad : EvalGradFunctionBase<T> {
    TMat<T> Xmu, Xmu2;
    int N;
    int R;
    LnNormalDenEvalGrad(int R) : R(R){};
    std::string get_name() const override { return "LnNormalDenEvalGrad"; };
    boost::json::object to_json() const override {
        boost::json::object res;
        res["name"] = "ln_normal_den";
        res["Reduction"] = R;
        return res;
    };
    void eval(TMap<T> &dest, const std::vector<TMap<T>> &inputs) override {
        N = inputs[0].rows();
        T mu = inputs[1].coeff(0, 0);
        T sigma = inputs[2].coeff(0, 0);
        const auto &X = inputs[0];

        TMat<T> res(N, 1);
        Xmu = X.array() - mu;
        Xmu2 = Xmu.array().pow(2);

        if (R == Reduction::None) {
            dest = -0.5 * std::log(2 * std::numbers::pi) - std::log(sigma) -
                   0.5 * Xmu2.array() / (sigma * sigma);
        } else {
            dest.coeffRef(0, 0) = -0.5 * N * std::log(2 * std::numbers::pi) - N * std::log(sigma) -
                                  0.5 * Xmu2.sum() / (sigma * sigma);
            if (R == Reduction::Mean) {
                dest = dest.array() / N;
            }
        }
    };
    std::vector<TMat<T>> grad(const std::shared_ptr<Var<T>> &current) override {
        const auto &X = current->input_node(0)->val();
        T mu = current->input_node(1)->val().coeff(0, 0);
        T sigma = current->input_node(2)->val().coeff(0, 0);

        std::vector<TMat<T>> res;

        // X
        if (current->input_node(0)->requires_grad()) {
            TMat<T> Xg(N, 1);
            if (R == Reduction::None) {
                Xg = -Xmu.array() / (sigma * sigma) * current->grad().array();
            } else {
                Xg = -Xmu.array() / (sigma * sigma) * current->grad().coeff(0, 0);

                if (R == Reduction::Mean) {
                    Xg = Xg / N;
                }
            }
            // std::cout << Xg << std::endl;
            res.emplace_back(Xg);
        } else {
            res.emplace_back(TMat<T>());
        };
        // mu
        if (current->input_node(1)->requires_grad()) {
            TMat<T> mug(1, 1);
            // mu's gradient is negation of X's.
            if (current->input_node(0)->requires_grad()) {
                mug.coeffRef(0, 0) = -res[0].sum();
            } else {
                if (R == Reduction::None) {
                    mug.coeffRef(0, 0) = (Xmu.array() * current->grad().array()).sum() / (sigma * sigma);
                } else {
                    mug.coeffRef(0, 0) = Xmu.sum() / (sigma * sigma) * current->grad().coeff(0, 0);
                    if (R == Reduction::Mean) {
                        mug = mug / N;
                    }
                }
            }
            res.emplace_back(mug);
        } else {
            res.emplace_back(TMat<T>());
        }
        // Sigma
        if (current->input_node(2)->requires_grad()) {
            TMat<T> sg(1, 1);

            if (R == Reduction::None) {
                sg.coeffRef(0, 0) =
                    ((-1.0 / sigma + Xmu2.array() / (std::pow(sigma, 3))) * current->grad().array()).sum();
            } else {
                sg.coeffRef(0, 0) = (-N / sigma + Xmu2.sum() / (std::pow(sigma, 3))) *
                                    current->grad().coeff(0, 0);
                if (R == Reduction::Mean) {
                    sg = sg / N;
                }
            }
            res.emplace_back(sg);
        } else {
            res.emplace_back(TMat<T>());
        }

        return res;
    }
};

template <typename T = double> struct LnTDenEvalGrad : EvalGradFunctionBase<T> {
    TMat<T> Xmu;
    int N;
    int R;
    TMat<T> xm, xm2, fx, lfx;
    LnTDenEvalGrad(int R) : R(R){};
    std::string get_name() const override { return "LnTDenEvalGrad"; };
    boost::json::object to_json() const override {
        boost::json::object res;
        res["name"] = "ln_t_den";
        res["Reduction"] = R;
        return res;
    };
    void eval(TMap<T> &dest, const std::vector<TMap<T>> &inputs) override {
        N = inputs[0].rows();
        fx.resize(N, 1);
        xm.resize(N, 1);
        xm2.resize(N, 1);
        lfx.resize(N, 1);
        T mu = inputs[1].coeff(0, 0);
        T sigma = inputs[2].coeff(0, 0);
        T nu = inputs[3].coeff(0, 0);
        const auto &X = inputs[0];

        xm = X.array() - mu;
        xm2 = xm.array().pow(2);
        fx = xm2.array() / (sigma * sigma) / nu + 1;
        lfx = fx.array().log();

        if (R == Reduction::None) {
            dest = bm::lgamma(nu / 2 + 0.5) - bm::lgamma(nu / 2) -
                   0.5 * std::log(nu * std::numbers::pi) - std::log(sigma) -
                   (nu + 1) / 2 * lfx.array();
        } else {
            dest.coeffRef(0, 0) = N * (bm::lgamma(nu / 2 + 0.5) - bm::lgamma(nu / 2) -
                                       0.5 * std::log(nu * std::numbers::pi) - std::log(sigma)) -
                                  (nu + 1) / 2 * lfx.array().sum();
            if (R == Reduction::Mean) {
                dest = dest.array() / N;
            }
        }
    };
    std::vector<TMat<T>> grad(const std::shared_ptr<Var<T>> &current) override {
        const auto &X = current->input_node(0)->val();
        T mu = current->input_node(1)->val().coeff(0, 0);
        T sigma = current->input_node(2)->val().coeff(0, 0);
        T nu = current->input_node(3)->val().coeff(0, 0);

        std::vector<TMat<T>> res;

        // X
        if (current->input_node(0)->requires_grad()) {
            TMat<T> Xg(N, 1);
            if (R == Reduction::None) {
                Xg = -(1 + 1.0 / nu) / (sigma * sigma) * xm.array() / fx.array() *
                     current->grad().array();
            } else {
                Xg = -(1 + 1.0 / nu) / (sigma * sigma) * (xm.array() / fx.array()) *
                     current->grad().coeff(0, 0);

                if (R == Reduction::Mean) {
                    Xg = Xg / N;
                }
            }
            // std::cout << Xg << std::endl;
            res.emplace_back(Xg);
        } else {
            res.emplace_back(TMat<T>());
        };
        // mu
        if (current->input_node(1)->requires_grad()) {
            TMat<T> mug(1, 1);
            // mu's gradient is negation of X's.
            if (current->input_node(0)->requires_grad()) {
                mug.coeffRef(0, 0) = -res[0].sum();
            } else {
                if (R == Reduction::None) {
                    mug.coeffRef(0, 0) = (1 + 1.0 / nu) / (sigma * sigma) *
                                         (xm.array() / fx.array() * current->grad().array()).sum();
                } else {
                    mug.coeffRef(0, 0) = (1 + 1.0 / nu) / (sigma * sigma) *
                                         (xm.array() / fx.array()).sum() *
                                         current->grad().coeff(0, 0);
                    if (R == Reduction::Mean) {
                        mug = mug / N;
                    }
                }
            }
            res.emplace_back(mug);
        } else {
            res.emplace_back(TMat<T>());
        }
        // sigma
        if (current->input_node(2)->requires_grad()) {
            TMat<T> sg(1, 1);

            if (R == Reduction::None) {
                sg.coeffRef(0, 0) = ((-1.0 / sigma + (1 + 1.0 / nu) / (sigma * sigma * sigma) *
                                                         xm2.array() / fx.array()) *
                                     current->grad().array())
                                        .sum();
            } else {
                sg.coeffRef(0, 0) = (-N / sigma + (1 + 1.0 / nu) / (sigma * sigma * sigma) *
                                                      (xm2.array() / fx.array()).sum()) *
                                    current->grad().coeff(0, 0);
                if (R == Reduction::Mean) {
                    sg = sg / N;
                }
            }
            res.emplace_back(sg);
        } else {
            res.emplace_back(TMat<T>());
        }
        // nu
        if (current->input_node(3)->requires_grad()) {
            TMat<T> nug(1, 1);

            if (R == Reduction::None) {
                nug.coeffRef(0, 0) =
                    0.5 * ((bm::digamma((nu + 1) / 2) - bm::digamma(nu / 2) - 1.0 / nu -
                            (lfx.array() -
                             (nu + 1) / (nu * nu) / (sigma * sigma) * xm2.array() / fx.array())) *
                           current->grad().array())
                              .sum();
            } else {
                nug.coeffRef(0, 0) =
                    0.5 *
                    (N * (bm::digamma((nu + 1) / 2) - bm::digamma(nu / 2) - 1.0 / nu) -
                     (lfx.array() -
                      (nu + 1) / (nu * nu) / (sigma * sigma) * xm2.array() / fx.array())
                         .sum()) *
                    current->grad().coeff(0, 0);
                if (R == Reduction::Mean) {
                    nug = nug / N;
                }
            }
            res.emplace_back(nug);
        } else {
            res.emplace_back(TMat<T>());
        }
        return res;
    }
};

DISTFUNCTIONTEMPLATE(
    ln_mvnormal_den, LnMVNormalDen,
    UNWRAP(, std::shared_ptr<Var<T>> mu, std::shared_ptr<Var<T>> Sigma), UNWRAP(X, mu, Sigma),
    UNWRAP(assert(((mu->cols() == 1) && (Sigma->rows() == Sigma->cols())) &&
                  "mu should be a column vector. Sigma should be a square matrix.");),
    X->requires_grad() || mu->requires_grad() || Sigma->requires_grad())
DISTFUNCTIONTEMPLATE(
    ln_normal_den, LnNormalDen, UNWRAP(, std::shared_ptr<Var<T>> mu, std::shared_ptr<Var<T>> sigma),
    UNWRAP(X, mu, sigma),
    UNWRAP(assert(((X->cols() == 1) && (mu->size() == 1) && (sigma->size() == 1)) &&
                  "X must be a column vector. mu and sigma must be a scalar.");),
    X->requires_grad() || mu->requires_grad() || sigma->requires_grad())
DISTFUNCTIONTEMPLATE(
    ln_t_den, LnTDen,
    UNWRAP(, std::shared_ptr<Var<T>> mu, std::shared_ptr<Var<T>> sigma, std::shared_ptr<Var<T>> nu),
    UNWRAP(X, mu, sigma, nu),
    UNWRAP(assert(((X->cols() == 1) && (mu->size() == 1) && (sigma->size() == 1) &&
                   (nu->size() == 1)) &&
                  "X must be a column vector. mu, sigma and nu must be scalars.");),
    X->requires_grad() || mu->requires_grad() || sigma->requires_grad() || nu->requires_grad())
#undef DISTFUNCTIONTEMPLATE
#undef UNWRAP
};     // namespace DynAutoDiff
#endif // !


/*!
 * @file
 * Defines the @ref Integer datatype.
 *
 *
 * @copyright Louis Dionne 2014
 * Distributed under the Boost Software License, Version 1.0.
 *         (See accompanying file LICENSE.md or copy at
 *             http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_MPL11_INTEGER_HPP
#define BOOST_MPL11_INTEGER_HPP

#include <boost/mpl11/fwd/integer.hpp>

#include <boost/mpl11/bitwise.hpp>
#include <boost/mpl11/comparable.hpp>
#include <boost/mpl11/detail/logical_or/strict.hpp>
#include <boost/mpl11/enumerable.hpp>
#include <boost/mpl11/group.hpp>
#include <boost/mpl11/integral_domain.hpp>
#include <boost/mpl11/logical.hpp>
#include <boost/mpl11/monoid.hpp>
#include <boost/mpl11/orderable.hpp>
#include <boost/mpl11/ring.hpp>

namespace boost { namespace mpl11 {
    namespace integral_detail {
        template <typename ...T>
        struct or_impl : true_ { };

        template <typename ...T>
        struct or_impl<integer_c<T, false>...> : false_ { };

        template <typename ...T>
        struct and_impl : false_ { };

        template <typename ...T>
        struct and_impl<integer_c<T, true>...> : true_ { };
    } // end namespace integral_detail

    template <typename T, T ...v>
    struct or_<integer_c<T, v>...>
        : detail::logical_or::strict<bool_<(bool)v>...>
    { };

    template <typename T, T ...v>
    struct and_<integer_c<T, v>...> // DeMorgan
        : bool_<!detail::logical_or::strict<bool_<!v>...>::value>
    { };

    template <>
    struct Monoid<Integer> : instantiate<Monoid>::with<Integer> {
        template <typename x, typename y>
        using plus_impl = integer_c<
            decltype(x::value + y::value),
            x::value + y::value
        >;

        template <typename ...>
        using zero_impl = int_<0>;
    };

    template <>
    struct Group<Integer> : instantiate<Group>::with<Integer> {
        template <typename x, typename y>
        using minus_impl = integer_c<
            decltype(x::value - y::value),
            x::value - y::value
        >;

        template <typename x>
        using negate_impl = integer_c<
            decltype(-x::value),
            -x::value
        >;
    };

    template <>
    struct Ring<Integer> : instantiate<Ring>::with<Integer> {
        template <typename x, typename y>
        using mult_impl = integer_c<
            decltype(x::value * y::value),
            x::value * y::value
        >;

        template <typename ...>
        using one_impl = int_<1>;
    };

    template <>
    struct IntegralDomain<Integer>
        : instantiate<IntegralDomain>::with<Integer>
    {
        template <typename x, typename y>
        using div_impl = integer_c<
            decltype(x::value / y::value),
            x::value / y::value
        >;

        template <typename x, typename y>
        using mod_impl = integer_c<
            decltype(x::value % y::value),
            x::value % y::value
        >;
    };

    template <>
    struct Enumerable<Integer> : instantiate<Enumerable>::with<Integer> {
        template <typename x>
        using succ_impl = integer_c<
            decltype(x::value + 1),
            x::value + 1
        >;

        template <typename x>
        using pred_impl = integer_c<
            decltype(x::value - 1),
            x::value - 1
        >;
    };

    template <>
    struct Comparable<Integer> : instantiate<Comparable>::with<Integer> {
        template <typename x, typename y>
        using equal_impl = bool_<x::value == y::value>;

        template <typename x, typename y>
        using not_equal_impl = bool_<x::value != y::value>;
    };

    template <>
    struct Orderable<Integer> : instantiate<Orderable>::with<Integer> {
        template <typename x, typename y>
        using less_impl = bool_<(x::value < y::value)>;

        template <typename x, typename y>
        using less_equal_impl = bool_<(x::value <= y::value)>;

        template <typename x, typename y>
        using greater_impl = bool_<(x::value > y::value)>;

        template <typename x, typename y>
        using greater_equal_impl = bool_<(x::value >= y::value)>;

        template <typename x, typename y>
        using max_impl = if_c<(x::value < y::value), y, x>;

        template <typename x, typename y>
        using min_impl = if_c<(x::value < y::value), x, y>;
    };

    template <>
    struct Bitwise<Integer> : instantiate<Bitwise>::with<Integer> {
        template <typename x, typename y>
        using bitand_impl = integer_c<
            decltype(x::value & y::value),
            x::value & y::value
        >;

        template <typename x, typename y>
        using bitor_impl = integer_c<
            decltype(x::value | y::value),
            x::value | y::value
        >;

        template <typename x, typename y>
        using bitxor_impl = integer_c<
            decltype(x::value ^ y::value),
            x::value ^ y::value
        >;

        template <typename x, typename n>
        using shift_left_impl = integer_c<
            decltype(x::value << n::value),
            (x::value << n::value)
        >;

        template <typename x, typename n>
        using shift_right_impl = integer_c<
            decltype(x::value >> n::value),
            (x::value >> n::value)
        >;

        template <typename x>
        using compl_impl = integer_c<
            decltype(~x::value),
            ~x::value
        >;
    };
}} // end namespace boost::mpl11

#endif // !BOOST_MPL11_INTEGER_HPP


#pragma once

#include <Eigen/Dense>

namespace polyfem {
namespace autogen {
void p_nodes_2d(const int p, Eigen::MatrixXd &val);

void p_basis_value_2d(const int p, const int local_index, const Eigen::MatrixXd &uv, Eigen::MatrixXd &val);

void p_grad_basis_value_2d(const int p, const int local_index, const Eigen::MatrixXd &uv, Eigen::MatrixXd &val);

void p_nodes_3d(const int p, Eigen::MatrixXd &val);

void p_basis_value_3d(const int p, const int local_index, const Eigen::MatrixXd &uv, Eigen::MatrixXd &val);

void p_grad_basis_value_3d(const int p, const int local_index, const Eigen::MatrixXd &uv, Eigen::MatrixXd &val);

static const int MAX_P_BASES = 4;

}}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_EXPX2_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_EXPX2_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-exponential
    This function object returns the exponential of the square of its
    argument or its inverse: \f$e^{x^2}\f$ or \f$e^{-x^2}\f$.
    The sign chosen is -1 if and only if the sign bit of the second argument is not zero.

    @par Header <boost/simd/function/expx2.hpp>

    @par Note:
    provisions are made for obtaining more accurate results for large @c x.
    The second argument @c s defaults to 1.

    @see exp

    @par Example:

      @snippet expx2.cpp expx2

    @par Possible output:

      @snippet expx2.txt expx2

  **/
   IEEEValue expx2(IEEEValue const& x, IEEEValue const& s = 1);

} }
#endif

#include <boost/simd/function/scalar/expx2.hpp>
#include <boost/simd/function/simd/expx2.hpp>

#endif