#include <iostream>
#include <fstream>
#include <complex>
#include <vector>
#include <string>
#include <armadillo>

const int _Nomega=100,_Niterations=200,_Nk=100;

typedef arma::Mat< std::complex<double> > arma_t;

const arma_t ZEROS = arma::Mat< std::complex<double> >(_Nomega,_Nk,arma::fill::zeros);

class HubbardExt{
    public:
        HubbardExt(double,double,double,double,int,int,int);
        //void operator()(int,std::complex<double>);
        void init_self();
        void Gup_kAA(int,int,int,int);
        void Sup_kAA(int,int);
        void Gdo_kAA(int,int,int,int);
        void Sdo_kAA(int,int);
        void Gup_kBB(int,int,int,int);
        void Sup_kBB(int,int);
        void Gdo_kBB(int,int,int,int);
        void Sdo_kBB(int,int);
        std::complex<double> w(int);
        double get_nAA();
        double get_double_occupancy_AA();
    private:
        double _u,_v,_beta,_mu;
        std::vector<double> _kArr;
        arma_t _Gup_kAA, _Gdo_kAA, _Gup_kBB, _Gdo_kBB;
        arma_t _Sup_kAA, _Sdo_kAA, _Sup_kBB, _Sdo_kBB;
        std::vector< std::complex<double>* > _G_vec_ptr;
        std::vector< std::complex<double>* > _S_vec_ptr;
        // Static members
        static std::complex<double> self_init;
};

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

	double n,d;
    double beta_init=5.0, beta_step=5.0, beta_max=100.0;
    double u_init=2.0, u_step=0.2, u_max=4.0;
    double v=0.5;
    double mu=0.0;

    std::ofstream output;
    std::ofstream outputDO;
    std::string strOutput("T_vs_U_beta_"+std::to_string(beta_init)+"_"+std::to_string(beta_max)+"_"+std::to_string(beta_step)+"_vs_u_"+std::to_string(u_init)+"_"+std::to_string(u_max)+"_"+std::to_string(u_step)+"_v_"+std::to_string(v)+"_Nk_"+std::to_string(_Nk)+"_Nomega_"+std::to_string(_Nomega)+"_Nit_"+std::to_string(_Niterations)+"_Fock.dat");
    std::string strOutputDO("DO_beta_"+std::to_string(beta_init)+"_"+std::to_string(beta_max)+"_"+std::to_string(beta_step)+"_vs_u_"+std::to_string(u_init)+"_"+std::to_string(u_max)+"_"+std::to_string(u_step)+"_v_"+std::to_string(v)+"_Nk_"+std::to_string(_Nk)+"_Nomega_"+std::to_string(_Nomega)+"_Nit_"+std::to_string(_Niterations)+"_Fock.dat");
    for (double beta=beta_init; beta<=beta_max; beta+=beta_step){

        for (double u=u_init; u<=u_max; u+=u_step) {
            mu=u/2.0+2.0*v; // chemical potential in the 1D case for the extended Hubbard model.

            HubbardExt hubbardExtObj(u,v,beta,mu,_Nomega,_Niterations,_Nk);
            // Initialize the self-energies.
            hubbardExtObj.init_self();

            for (int i=0; i<_Niterations; i++){

                for (int kn=0; kn<_Nomega; kn++){

                    for (int k=0; k<_Nk; k++){
                        // First updating the AA self-energies
                        hubbardExtObj.Sup_kAA(k,kn);
                        hubbardExtObj.Sdo_kAA(k,kn);
                    }
                }
                for (int kn=0; kn<_Nomega; kn++){

                    for (int k=0; k<_Nk; k++){
                        // Then updating the BB self-energies
                        hubbardExtObj.Sup_kBB(k,kn);
                        hubbardExtObj.Sdo_kBB(k,kn);
                    }
                }
                std::cout << "it: " << i << std::endl;
                n = hubbardExtObj.get_nAA();
                d = hubbardExtObj.get_double_occupancy_AA();
                std::cout << "d: " << d << std::endl;
            }
            output.open(strOutput, std::ofstream::out | std::ofstream::app);
            output << n << " ";
            output.close();

            outputDO.open(strOutputDO, std::ofstream::out | std::ofstream::app);
            outputDO << d << " ";
            outputDO.close();
        }
        std::cout << "\n";
        output.open(strOutput, std::ofstream::out | std::ofstream::app);
        output << "\n";
        output.close();

        outputDO.open(strOutputDO, std::ofstream::out | std::ofstream::app);
        outputDO << " ";
        outputDO.close();
    }
  return 0;
}

std::complex<double> HubbardExt::self_init = std::complex<double>(0.2,-0.1);

HubbardExt::HubbardExt(double u, double v, double beta, double mu, int Nomega, int Nit, int Nk) : _u(u), _v(v), _beta(beta), _mu(mu){
    for (int k=0; k<=_Nk; k++){
        this->_kArr.push_back(-M_PI/2.0 + 2.0*(double)k*M_PI/(2.0*(double)Nk));
    }
    _Gup_kAA=ZEROS; _Gdo_kAA=ZEROS; _Gup_kBB=ZEROS; _Gdo_kBB=ZEROS;
    _Sup_kAA=ZEROS; _Sdo_kAA=ZEROS; _Sup_kBB=ZEROS; _Sdo_kBB=ZEROS;
    this->_G_vec_ptr = { _Gup_kAA.memptr(), _Gdo_kAA.memptr(), _Gup_kBB.memptr(), _Gdo_kBB.memptr() };
    this->_S_vec_ptr = { _Sup_kAA.memptr(), _Sdo_kAA.memptr(), _Sup_kBB.memptr(), _Sdo_kBB.memptr() };
}

void HubbardExt::init_self(){
    for (size_t it=0; it<_S_vec_ptr.size(); it++){
        for (int j=0; j<_Nomega; j++){
            std::complex<double> w = std::complex<double>(0.0,(2.0*(double)j+1.0)*M_PI/_beta);
            for (int k=0; k<_kArr.size(); k++){
                *(_S_vec_ptr[it] + j*_Nk + k) = std::complex<double>(0.4,-0.1);// /(w+_mu-self_init);
            }
        }
        self_init += std::complex<double>(0.1,0.1);
    }
}

void HubbardExt::Gup_kAA(int k, int kn, int q, int qn){
    double epsk=-2.0*cos(_kArr[k]+_kArr[q]);
    std::complex<double> wj = w(kn)+w(kn);
    // if (Nit != 0)
    //     Sup_kAA(k,kn,); // If Nit !=
    *(_G_vec_ptr[0] + qn*_Nk + q) = 1.0/( wj + _mu - *(_S_vec_ptr[0] + qn*_Nk + q) - epsk*epsk/( wj + _mu - *(_S_vec_ptr[2] + qn*_Nk + q) ) );
}

void HubbardExt::Sup_kAA(int k, int kn){
    for (int qn=0; qn<_Nomega; qn++){
        for (int q=0; q<_Nk; q++){
            Gup_kAA(k,kn,q,qn);
            *(_S_vec_ptr[0] + kn*_Nk + k) += _v*cos(_kArr[q])*( -1.0/(_Nk*_beta) )*( *(_G_vec_ptr[0] + qn*_Nk + q) );
        }
    }
}

void HubbardExt::Gdo_kAA(int k, int kn, int q, int qn){
    double epsk=-2.0*cos(_kArr[k]+_kArr[q]);
    std::complex<double> wj = w(kn)+w(kn);
    *(_G_vec_ptr[1] + qn*_Nk + q) = 1.0/( wj + _mu - *(_S_vec_ptr[1] + qn*_Nk + q) - epsk*epsk/( wj + _mu - *(_S_vec_ptr[3] + qn*_Nk + q) ) );
}

void HubbardExt::Sdo_kAA(int k, int kn){
    for (int qn=0; qn<_Nomega; qn++){
        for (int q=0; q<_Nk; q++){
            Gdo_kAA(k,kn,q,qn);
            *(_S_vec_ptr[1] + kn*_Nk + kn) += _v*cos(_kArr[q])*( -1.0/(_Nk*_beta) )*( *(_G_vec_ptr[1] + qn*_Nk + q) );
        }
    }
}

// // BB quantities are computed after AA quantities have been computed.

void HubbardExt::Gup_kBB(int k, int kn, int q, int qn){
    double epsk=-2.0*cos(_kArr[k]+_kArr[q]);
    std::complex<double> wj = w(kn)+w(kn);
    *(_G_vec_ptr[2] + qn*_Nk + q) = 1.0/( wj + _mu - *(_S_vec_ptr[2] + qn*_Nk + q) - epsk*epsk/( wj + _mu - *(_S_vec_ptr[0] + qn*_Nk + q) ) );
}

void HubbardExt::Sup_kBB(int k, int kn){
    for (int qn=0; qn<_Nomega; qn++){
        for (int q=0; q<_Nk; q++){
            Gup_kBB(k,kn,q,qn);
            *(_S_vec_ptr[2] + kn*_Nk + k) += _v*cos(_kArr[q])*( -1.0/(_Nk*_beta) )*( *(_G_vec_ptr[2] + qn*_Nk + q) );
        }
    }
}

void HubbardExt::Gdo_kBB(int k, int kn, int q, int qn){
    double epsk=-2.0*cos(_kArr[k]+_kArr[q]);
    std::complex<double> wj = w(kn)+w(kn);
    *(_G_vec_ptr[3] + qn*_Nk + q) = 1.0/( wj + _mu - *(_S_vec_ptr[3] + qn*_Nk + q) - epsk*epsk/( wj + _mu - *(_S_vec_ptr[1] + qn*_Nk + q) ) );
}

void HubbardExt::Sdo_kBB(int k, int kn){
    for (int qn=0; qn<_Nomega; qn++){
        for (int q=0; q<_Nk; q++){
            Gup_kBB(k,kn,q,qn);
            *(_S_vec_ptr[3] + kn*_Nk + k) += _v*cos(_kArr[q])*( -1.0/(_Nk*_beta) )*( *(_G_vec_ptr[3] + qn*_Nk + q) );
        }
    }
}

// Once all the self-energies of one iteration have been calculated, build the new Green's functions out of the latter.

double HubbardExt::get_nAA(){
    double n=0.0;
    for (int j=0; j<_Nomega; j++){
        std::complex<double> n_k(0.0,0.0);
        std::complex<double> wj = w(j);
        for (int k=0; k<_kArr.size(); k++){ // Summing over G(k)
            double epsk = -2.0*cos(_kArr[k]);
            if ( (k==0) || (k==_Nk) ){
                n_k += 0.5/( wj + _mu - *(_S_vec_ptr[0] + j*_Nk + k) - epsk*epsk/( wj + _mu - *(_S_vec_ptr[2] + j*_Nk + k) ) );
            }
            else{
                n_k += 1.0/( wj + _mu - *(_S_vec_ptr[0] + j*_Nk + k) - epsk*epsk/( wj + _mu - *(_S_vec_ptr[2] + j*_Nk + k) ) );
            }
        }
        n_k /= (_Nk);
        n += (2.0/_beta)*( n_k - 1.0/wj ).real();
    }
    n -= 0.5;
    n *= -1.0;

    std::cout << "beta: " << _beta << " u: " << _u << " v: " << _v << " n: " << n << std::endl;

    return n;
}

double HubbardExt::get_double_occupancy_AA(){
/* This function computes U<n_{up}n_{down}> = 1/(\beta*V)*\sum_{k,ikn} \Sigma(k,ikn)G(k,ikn)e^{-ikn0^-} */
    double d=0.0;
    for (int j=0; j<_Nomega; j++){
        std::complex<double> d_k(0.0,0.0);
        std::complex<double> wj = w(j);
        for (int k=0; k<_kArr.size(); k++){ // Summing over G(k)
            double epsk = -2.0*cos(_kArr[k]);
            if ( (k==0) || (k==_Nk) ){
                d_k += *(_S_vec_ptr[0] + j*_Nk + k)*0.5/( wj + _mu - *(_S_vec_ptr[0] + j*_Nk + k) - epsk)//*epsk/( wj + _mu - *(_S_vec_ptr[2] + j*_Nk + k) ) );
            }
            else{
                d_k += *(_S_vec_ptr[0] + j*_Nk + k)*1.0/( wj + _mu - *(_S_vec_ptr[0] + j*_Nk + k) - epsk)//*epsk/( wj + _mu - *(_S_vec_ptr[2] + j*_Nk + k) ) );
            }
        }
        d_k /= (_Nk);
        d += (2.0/_beta)*( d_k ).real();
    }
    d *= -1.0;

    return d;
}

std::complex<double> HubbardExt::w(int j){
    return std::complex<double>(0.0,(2.0*(double)j+1.0)*M_PI/_beta);
}


#ifndef _RYSQ_KERNEL_ERI_BRA_HPP_
#define _RYSQ_KERNEL_ERI_BRA_HPP_

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <math.h>
#include <boost/mpl/int.hpp>
#include <rysq/core.hpp>
#include "vector.hpp"

#include "kernel/eri.hpp"
#include "kernel/primitives.hpp"
#include "cxx/utility/permute.hpp"

#include "kernel/transform.hpp"

BEGIN_NAMESPACE(rysq, kernel)

template<class bra_, int N>
struct eri<bra_, boost::mpl::int_<N> > : public Eri
{
    typedef bra_ bra;
    typedef void ket;
    typedef kernel::Transform<bra> Transform;
    typedef typename Transform::Data Data;
    eri(const Quartet<Shell> &quartet, Transform *transform)
	: quartet_(quartet),
	  transform_(transform),
	  primitives_(quartet) {}

    ~eri() { delete transform_; }

    void operator()(const Quartet<Center> &r,
		    Data &data,
		    const Parameters &parameters) {
	double scale = rysq::SQRT_4PI5;
	double cutoff = parameters.cutoff/(scale*quartet_.K()*10);
	apply(quartet_, r[0], r[1], r[2], r[3], scale, cutoff, primitives_,
	      (*transform_)(data));
    }

private:

    const Quartet<Shell> quartet_;
    Transform *transform_;
    eri_::Primitives<double> primitives_;

    template<typename F>
    static void apply(const Quartet<Shell> &quartet,
		      const Vector<3> &ri, const Vector<3> &rj,
		      const Vector<3> &rk, const Vector<3> &rl,
		      double scale, double cutoff,
		      eri_::Primitives<F> &primitives,
		      Transform &transform);//  {

    // 	typedef kernel::eri_::quadrature<bra> quadrature;

    // 	const Shell &a = quartet[0];
    // 	const Shell &b = quartet[1];
    // 	const Shell &c = quartet[2];
    // 	const Shell &d = quartet[3];

    // 	Vector<3> rij = ri - rj;
    // 	Vector<3> rkl = rk - rl;

    // 	double rkl2 = rkl.inner();
    // 	double rij2 = rij.inner();

    // 	double eij[a.K*b.K];
    // 	for (int Kj = 0, Kij = 0; Kj < b.K; ++Kj) {
    // 	    for (int Ki = 0; Ki < a.K; ++Ki, ++Kij) {
    // 		double A = a(Ki) + b(Kj);
    // 		double A1 = 1.0/A;
    // 		eij[Kij] = exp(-a(Ki)*b(Kj)*A1*rij2);
    // 	    }
    // 	}

    // 	int K = 0;

    // 	for (int Kl = 0; Kl < d.K; ++Kl) {
    // 	    for (int Kk = 0; Kk < c.K; ++Kk) {
    // 		double ak = c(Kk);
    // 		double al = d(Kl);
    // 		double B = ak + al;

    // 		Vector<3> rB = Vector<3>::center(ak, rk, al, rl);
    // 		Vector<3> rBk = rB - rk;

    // 		double ekl = exp(-ak*al*rkl2/B);

    // 		double Ckl[4] __attribute__ ((aligned(16)));
    // 		double Ckl_max = 0.0;
    // 		for(int l = 0, kl = 0; l < d.nc; ++l) {
    // 		    for(int k = 0; k < c.nc; ++k, ++kl) {
    // 			Ckl[kl] = c(Kk,k)*d(Kl,l);
    // 			Ckl_max = std::max(Ckl_max, fabs(Ckl[kl]));
    // 		    }
    // 		}

    // 		for(int Kj = 0, Kij = 0; Kj < b.K; ++Kj) {
    // 		    for(int Ki = 0; Ki < a.K; ++Ki, ++Kij) {
    // 			double ai = a(Ki);
    // 			double aj = b(Kj);
    // 			double A = ai + aj;
    // 			double e = eij[Ki+Kj*a.K]*ekl;
    // 			double eAB = e/(A*B*sqrt(A+B));

    // 			double Cij[bra::nc] __attribute__ ((aligned(16)));
    // 			double Cij_max = 0.0;

    // 			for(int j = 0, ij = 0; j < bra::B::nc; ++j) {
    // 			    for(int i = 0; i < bra::A::nc; ++i, ++ij) {
    // 				Cij[ij] = eAB*a(Ki,i)*b(Kj,j);
    // 				Cij_max = std::max(fabs(Cij[ij]), Cij_max);
    // 			    }
    // 			}
    // 			if (Cij_max*Ckl_max < cutoff) continue;

    // 			for(int kl = 0; kl < (c.nc*d.nc); ++kl) {
    // 			    for(int ij = 0; ij < bra::nc; ++ij) {
    // 				primitives.C[kl][ij + K*bra::nc] = Cij[ij]*Ckl[kl];
    // 			    }
    // 			}

    // 			Vector<3> rA = Vector<3>::center(ai, ri, aj, rj);
    // 			Vector<3> rAi = rA - ri;
    // 			Vector<3> rAB = rA - rB;

    // 			double rho = A*B/(A + B);
    // 			double X =  rho*rAB.inner();
    // 			Vector<N> W, t2;
    // 			rysq::roots<N>(X, t2, W);
    // 			t2 /= (A + B);

    // 			static const int LA = bra::A::L;
    // 			static const int LB = bra::B::L;

    // 			double *Ix = primitives.Ix(K);
    // 			double *Iy = primitives.Iy(K);
    // 			double *Iz = primitives.Iz(K);

    // 			if (LB + d.L == 0) {
    // 			    rysq::recurrence<bra::L,N>(c.L + d.L, A, B, rAB, rAi, rBk,
    // 						       t2, W, Ix, Iy, Iz);
    // 			}
    // 			else {
    // 			    F *Gx = primitives.template Gx<F>(K);
    // 			    F *Gy = primitives.template Gy<F>(K);
    // 			    F *Gz = primitives.template Gz<F>(K);
    // 			    F *tmp = primitives.template transfer<F>();

    // 			    rysq::recurrence<bra::L,N>(c.L + d.L, A, B, rAB, rAi, rBk,
    // 						       t2, W, Gx, Gy, Gz);

    // 			    rysq::transfer<LA,LB,N>(c.L, d.L, rij[0], rkl[0], Gx, Ix, tmp);
    // 			    rysq::transfer<LA,LB,N>(c.L, d.L, rij[1], rkl[1], Gy, Iy, tmp);
    // 			    rysq::transfer<LA,LB,N>(c.L, d.L, rij[2], rkl[2], Gz, Iz, tmp);
    // 			}

    // 			++K;

    // 			// contract primitives
    // 			if (K == primitives.Kmax) {
    // 			    primitives.K = K;
    // 			    apply(c, d, primitives, scale, transform);
    // 			    K = 0;
    // 			}

    // 		    }
    // 		}
    // 	    }
    // 	}

    // 	// Final contraction
    // 	primitives.K = K;
    // 	if (primitives.K) apply(c, d, primitives, scale, transform);

    // }

private:

    template<typename F>
    static void apply(rysq::type c, rysq::type d,
		      const eri_::Primitives<F> &primitives,
		      double scale, Transform &transform);//  {

    // 	static const int ldN = (sizeof(F) > 8) ? N : N + N%2;
    // 	static const int Nij = ldN*(bra::A::L+1)*(bra::B::L+1);

    // 	const int Lc = abs(c);
    // 	const int Ld = abs(d);

    // 	const int dim2d = Nij*(Lc+1)*(Ld+1);

    // 	const int K = primitives.K;
    // 	const double* const *C = primitives.C;
    // 	const F *Ix = primitives.Ix(0);
    // 	const F *Iy = primitives.Iy(0);
    // 	const F *Iz = primitives.Iz(0);

    // 	int spk = (c < 0);
    // 	int spl = (d < 0);

    // 	const int c_first = shell(c).begin();
    // 	const int d_first = shell(d).begin();
    // 	const int c_last = shell(c).end() - 1;
    // 	const int d_last = shell(d).end() - 1;

    // 	for(int l = d_first, kl = 0; l <= d_last; ++l) {
    // 	    const int lx = (Lc+1)*LX[l];
    // 	    const int ly = (Lc+1)*LY[l];
    // 	    const int lz = (Lc+1)*LZ[l];

    // 	    const int lsp = (spl && l) << spk;

    // 	    for(int k = c_first; k <= c_last; ++k, ++kl) {
    // 		const double *Ckl = C[(spk && k) + lsp];

    // 		const int klx = Nij*(lx + LX[k]);
    // 		const int kly = Nij*(ly + LY[k]);
    // 		const int klz = Nij*(lz + LZ[k]);

    // 		static const int ni = bra::A::size;
    // 		static const int nj = bra::B::size;
    // 		double I[ni*nj] __attribute__((aligned(16))) = { 0.0 };

    // 		int flags = 0;
    // 		double screen = 0.0;
    // 		quadrature::template apply<N>(flags, screen, K, Ckl, dim2d,
    // 					      &Ix[klx], &Iy[kly], &Iz[klz], 1.0, I);

    // 		transform(k - c_first, l - d_first, kl, I, scale);
    // 	    }
    // 	}

    // }

};

END_NAMESPACE(rysq, kernel)

#endif // _RYSQ_KERNEL_ERI_BRA_HPP_


/**
 * @file
 * @license BSD 3-clause
 * @copyright Copyright (c) 2021, New York University and Max Planck
 * Gesellschaft
 *
 * @brief Bolt biped low level drivers.
 */

#pragma once

#include <math.h>

#include <Eigen/Eigen>

#include "blmc_drivers/serial_reader.hpp"
#include "odri_control_interface/calibration.hpp"
#include "odri_control_interface/robot.hpp"

namespace Eigen
{
/** @brief Eigen shortcut for vector of size 6. */
typedef Matrix<double, 6, 1> Vector6d;
}  // namespace Eigen

namespace bolt
{
#define BOLT_NB_MOTOR_BOARD 3
#define BOLT_NB_MOTOR 6
#define BOLT_NB_SLIDER 4

/** @brief Control state of the robot. */
enum BoltControlState
{
    initial,
    ready,
    calibrate
};

/**
 * @brief Driver for the Bolt biped robot.
 */
class Bolt
{
public:
    /**
     * @brief Bolt is the constructor of the class.
     */
    Bolt();

    /**
     * @brief initialize the robot by setting aligning the motors and calibrate
     * the sensors to 0
     */
    void initialize(const std::string& network_id);

    /**
     * @brief send_target_torques sends the target currents to the motors
     */
    void send_target_joint_torque(
        const Eigen::Ref<const Eigen::Vector6d> target_joint_torque);

    /**
     * @brief acquire_sensors acquire all available sensors, WARNING !!!!
     * this method has to be called prior to any getter to have up to date data.
     */
    void acquire_sensors();

    /**
     * @brief Wait until the hardware is ready to be controlled.
     */
    void wait_until_ready();

    /**
     * @brief Fill attitude quaternion.
     */
    void fill_base_attitude_quaternion();

    /**
     * @brief Request calibration of the joints by moving to the next joint
     * index position. The control is made inside the send_target_joint_torque.
     *
     * @param home_offset_rad This is the angle between the index and the zero
     * pose.
     * @return true
     * @return false
     */
    void request_calibration(
        const Eigen::Ref<const Eigen::VectorXd> home_offset_rad);

    /**
     * @brief Request calibration of the joints by moving to the next joint
     * index position. The control is made inside the send_target_joint_torque.
     * Use the yaml information in order to get the offset from the nearest
     * motor index.
     *
     * @return true
     * @return false
     */
    void request_calibration();

    /**
     * Sensor Data
     */

    /**
     * @brief get_joint_positions
     * @return  the joint angle of each module
     * WARNING !!!!
     * The method <acquire_sensors>"()" has to be called
     * prior to any getter to have up to date data.
     */
    const Eigen::Ref<const Eigen::Vector6d> get_joint_positions()
    {
        return joint_positions_;
    }

    /**
     * @brief get_joint_velocities
     * @return the joint velocities
     * WARNING !!!!
     * The method <acquire_sensors>"()" has to be called
     * prior to any getter to have up to date data.
     */
    const Eigen::Ref<const Eigen::Vector6d> get_joint_velocities()
    {
        return joint_velocities_;
    }

    /**
     * @brief get_joint_torques
     * @return the joint torques
     * WARNING !!!!
     * The method <acquire_sensors>"()" has to be called
     * prior to any getter to have up to date data.
     */
    const Eigen::Ref<const Eigen::Vector6d> get_joint_torques()
    {
        return joint_torques_;
    }

    /**
     * @brief get_joint_torques
     * @return the target joint torques
     * WARNING !!!!
     * The method <acquire_sensors>"()" has to be called
     * prior to any getter to have up to date data.

     */
    const Eigen::Ref<const Eigen::Vector6d> get_joint_target_torques()
    {
        return joint_target_torques_;
    }

    /**
     * @brief get_base_accelerometer
     * @return the base_accelerometer
     * WARNING !!!!
     * The method <acquire_sensors>"()" has to be called
     * prior to any getter to have up to date data.
     */
    const Eigen::Ref<const Eigen::Vector3d> get_base_accelerometer()
    {
        return base_accelerometer_;
    }

    /**
     * @brief get_base_accelerometer
     * @return the base_accelerometer
     * WARNING !!!!
     * The method <acquire_sensors>"()" has to be called
     * prior to any getter to have up to date data.
     */
    const Eigen::Ref<const Eigen::Vector3d> get_base_gyroscope()
    {
        return base_gyroscope_;
    }

    /**
     * @brief get_base_accelerometer
     * @return the base_accelerometer
     * WARNING !!!!
     * The method <acquire_sensors>"()" has to be called
     * prior to any getter to have up to date data.
     */
    const Eigen::Ref<const Eigen::Vector3d> get_base_attitude()
    {
        return base_attitude_;
    }

    /**
     * @brief get_base_accelerometer
     * @return the base_accelerometer
     * WARNING !!!!
     * The method <acquire_sensors>"()" has to be called
     * prior to any getter to have up to date data.
     */
    const Eigen::Ref<const Eigen::Vector3d> get_base_linear_acceleration()
    {
        return base_linear_acceleration_;
    }

    /**
     * @brief get_base_accelerometer
     * @return the base_accelerometer
     * WARNING !!!!
     * The method <acquire_sensors>"()" has to be called
     * prior to any getter to have up to date data.
     */
    const Eigen::Ref<const Eigen::Vector4d> get_base_attitude_quaternion()
    {
        return base_attitude_quaternion_;
    }

    /**
     * Hardware Status
     */

    /**
     * @brief get_motor_enabled
     * @return This gives the status (enabled/disabled) of each motors using the
     * joint ordering convention.
     */
    const Eigen::Ref<const Eigen::Matrix<bool, BOLT_NB_MOTOR, 1> >
    get_motor_enabled()
    {
        return motor_enabled_;
    }

    /**
     * @brief get_motor_ready
     * @return This gives the status (enabled/disabled) of each motors using the
     * joint ordering convention.
     */
    const Eigen::Ref<const Eigen::Matrix<bool, BOLT_NB_MOTOR, 1> >
    get_motor_ready()
    {
        return motor_ready_;
    }

    /**
     * @brief get_motor_board_enabled
     * @return This gives the status (enabled/disabled of the onboard control
     * cards).
     */
    const Eigen::Ref<const Eigen::Matrix<bool, BOLT_NB_MOTOR_BOARD, 1> >
    get_motor_board_enabled()
    {
        return motor_board_enabled_;
    }

    /**
     * @brief get_motor_board_errors
     * @return This gives the status (enabled/disabled of the onboard control
     * cards).
     */
    const Eigen::Ref<const Eigen::Matrix<int, BOLT_NB_MOTOR_BOARD, 1> >
    get_motor_board_errors()
    {
        return motor_board_errors_;
    }

    /**
     * @brief has_error
     * @return Returns true if the robot hardware has an error, false otherwise.
     */
    bool has_error() const
    {
        return robot_->HasError();
    }

    /*
     * Additional data
     */

    /**
     * @brief Get the slider positions in [0, 1]
     *
     * @return Eigen::Ref<Eigen::Matrix<double, BOLT_NB_SLIDER, 1> >
     */
    const Eigen::Ref<const Eigen::Matrix<double, BOLT_NB_SLIDER, 1> >
    get_slider_positions()
    {
        return slider_positions_;
    }

    /**
     * @brief Get the active estop value
     *
     * @return true if the button is pressed.
     * @return false otherwize.
     */
    bool get_active_estop() const
    {
        return active_estop_;
    }

    /**
     * @brief is_calibrating()
     * @return Returns true if the calibration procedure is running right now.
     */
    bool is_calibrating()
    {
        return (control_state_ == BoltControlState::calibrate) || calibrate_request_;
    }

private:
    /*
     * Hardware status
     */

    /** @brief Motor status (enabled/disabled). */
    Eigen::Matrix<bool, BOLT_NB_MOTOR, 1> motor_enabled_;

    /** @brief Motor readiness to receive commands (ready/not ready). */
    Eigen::Matrix<bool, BOLT_NB_MOTOR, 1> motor_ready_;

    /** @brief Motor Board status (enabled/disabled). */
    Eigen::Matrix<bool, BOLT_NB_MOTOR_BOARD, 1> motor_board_enabled_;

    /** @brief Motor Board Error code (int).
     */
    Eigen::Matrix<int, BOLT_NB_MOTOR_BOARD, 1> motor_board_errors_;

    /*
     * Joint data
     */

    /** @brief Joint positions. */
    Eigen::Vector6d joint_positions_;

    /** @brief Joint velocities. */
    Eigen::Vector6d joint_velocities_;

    /** @brief Joint torques. */
    Eigen::Vector6d joint_torques_;

    /** @brief Target joint torques, reference from the controller. */
    Eigen::Vector6d joint_target_torques_;

    /*
     * Additional data
     */

    /** @brief Name of the network lan: Left column in ifconfig output. */
    std::string network_id_;

    /** @brief Slider position in [0, 1]. */
    Eigen::Matrix<double, BOLT_NB_SLIDER, 1> slider_positions_;

    /** @brief E-stop from the slider box. */
    bool active_estop_;

    /** @brief base accelerometer. */
    Eigen::Vector3d base_accelerometer_;

    /** @brief base accelerometer. */
    Eigen::Vector3d base_gyroscope_;

    /** @brief base accelerometer. */
    Eigen::Vector3d base_attitude_;

    /** @brief base accelerometer. */
    Eigen::Vector3d base_linear_acceleration_;

    /** @brief base attitude quaternion. */
    Eigen::Vector4d base_attitude_quaternion_;

    /** @brief Integers from the serial port.
     * - 4 sliders in [0, 1024]
     * - emergency stop
     */
    std::vector<int> slider_box_data_;

    /*
     * Controllers
     */

    /** @brief Controller to run the calibration procedure */
    std::shared_ptr<odri_control_interface::JointCalibrator> calib_ctrl_;

    /*
     * Finite state machine of the controbolller.
     */

    /** @brief Control state Initial, Ready, Calibration */
    BoltControlState control_state_;

    /** @brief Check if the user called for the joint calibration. */
    bool calibrate_request_;

    /** @brief Number of time we acquire the sensor readings, this is used
     *  to prevent spamming prints */
    long int nb_time_we_acquired_sensors_;

    /*
     * Drivers communication objects
     */

    /**
     * @brief Robot complete interface.
     * Wrapper around the MasterBoardInterface.
     *
     * PC <------------------> main board <-------> Motor Board
     *       Ethernet/Wifi                   SPI
     */
    std::shared_ptr<odri_control_interface::Robot> robot_;

    /**
     * @brief Reader for serial port to read arduino slider values.
     */
    std::shared_ptr<blmc_drivers::SerialReader> serial_reader_;
};

}  // namespace bolt


#pragma once

#include <Eigen/Sparse>

namespace EiCOS
{

    enum class exitcode
    {
        optimal = 0,           /* Problem solved to optimality              */
        primal_infeasible = 1, /* Found certificate of primal infeasibility */
        dual_infeasible = 2,   /* Found certificate of dual infeasibility   */
        maxit = -1,            /* Maximum number of iterations reached      */
        numerics = -2,         /* Search direction unreliable               */
        outcone = -3,          /* s or z got outside the cone, numerics?    */
        fatal = -7,            /* Unknown problem in solver                 */
        close_to_optimal = 10,
        close_to_primal_infeasible = 11,
        close_to_dual_infeasible = 12,
        not_converged_yet = -87
    };

    struct Settings
    {
        const double gamma = 0.99;         // scaling the final step length
        const double delta = 2e-7;         // regularization parameter
        const double deltastat = 7e-8;     // static regularization parameter
        const double eps = 1e13;           // regularization threshold
        const double feastol = 1e-8;       // primal/dual infeasibility tolerance
        const double abstol = 1e-8;        // absolute tolerance on duality gap
        const double reltol = 1e-8;        // relative tolerance on duality gap
        const double feastol_inacc = 1e-4; // primal/dual infeasibility relaxed tolerance
        const double abstol_inacc = 5e-5;  // absolute relaxed tolerance on duality gap
        const double reltol_inacc = 5e-5;  // relative relaxed tolerance on duality gap
        const size_t nitref = 9;           // maximum number of iterative refinement steps
        const size_t maxit = 100;          // maximum number of iterations
        bool verbose = false;              // print solver output
        const double linsysacc = 1e-14;    // rel. accuracy of search direction
        const double irerrfact = 6;        // factor by which IR should reduce err
        const double stepmin = 1e-6;       // smallest step that we do take
        const double stepmax = 0.999;      // largest step allowed, also in affine dir.
        const double sigmamin = 1e-4;      // always do some centering
        const double sigmamax = 1.;        // never fully center
        const size_t equil_iters = 3;      // eqilibration iterations
        const size_t iter_max = 100;       // maximum solver iterations
        const size_t safeguard = 500;      // Maximum increase in PRES before NUMERICS is thrown.
    };

    struct Information
    {
        double pcost;
        double dcost;
        double pres;
        double dres;
        bool pinf;
        bool dinf;
        std::optional<double> pinfres;
        std::optional<double> dinfres;
        double gap;
        std::optional<double> relgap;
        double sigma;
        double mu;
        double step;
        double step_aff;
        double kapovert;
        size_t iter;
        size_t iter_max;
        size_t nitref1;
        size_t nitref2;
        size_t nitref3;

        bool isBetterThan(Information &other) const;
    };

    struct LPCone
    {
        Eigen::VectorXd w; // size n_lc
        Eigen::VectorXd v; // size n_lc
    };

    struct SOCone
    {
        size_t dim;            // dimension of cone
        Eigen::VectorXd skbar; // temporary variables to work with
        Eigen::VectorXd zkbar; // temporary variables to work with
        double a;              // = wbar(1)
        double d1;             // first element of D
        double w;              // = q'*q
        double eta;            // eta = (sres / zres)^(1/4)
        double eta_square;     // eta^2 = (sres / zres)^(1/2)
        Eigen::VectorXd q;     // = wbar(2:end)
        double u0;             // eta
        double u1;             // u = [u0; u1 * q]
        double v1;             // v = [0; v1 * q]
    };

    struct Work
    {
        void allocate(size_t n_var, size_t n_eq, size_t n_ineq);
        Eigen::VectorXd x;      // Primal variables  size n_var
        Eigen::VectorXd y;      // Multipliers for equality constaints  (size n_eq)
        Eigen::VectorXd z;      // Multipliers for conic inequalities   (size n_ineq)
        Eigen::VectorXd s;      // Slacks for conic inequalities        (size n_ineq)
        Eigen::VectorXd lambda; // Scaled variable                      (size n_ineq)

        // Homogeneous embedding
        double kap; // kappa
        double tau; // tau

        // Temporary storage
        double cx, by, hz;

        Information i;
    };

    class Solver
    {
        /**
     *
     *    ..---''''---..
     *    \ '''----''' /
     *     \          /
     *      \      ########  ##  ########  ########  ########
     *       \     ########  ##  ########  ########  ########
     *        \    ##            ##        ##    ##  ##
     *          \/ ########  ##  ##        ##    ##  ########
     *          /\ ########  ##  ##        ##    ##  ########
     *        /    ##        ##  ##        ##    ##        ##
     *       /     ########  ##  ########  ########  ########
     *      /      ########  ##  ########  ########  ########
     *     /          \
     *    /            \
     *    `'---....---'´
     *
     */

    public:
        Solver(const Eigen::SparseMatrix<double> &G,
               const Eigen::SparseMatrix<double> &A,
               const Eigen::VectorXd &c,
               const Eigen::VectorXd &h,
               const Eigen::VectorXd &b,
               const Eigen::VectorXi &soc_dims);
        void updateData(const Eigen::SparseMatrix<double> &G,
                        const Eigen::SparseMatrix<double> &A,
                        const Eigen::VectorXd &c,
                        const Eigen::VectorXd &h,
                        const Eigen::VectorXd &b);

        // traditional interface for compatibility
        Solver(int n, int m, int p, int l, int ncones, int *q,
               double *Gpr, int *Gjc, int *Gir,
               double *Apr, int *Ajc, int *Air,
               double *c, double *h, double *b);
        void updateData(double *Gpr, double *Apr,
                        double *c, double *h, double *b);

        exitcode solve(bool verbose = false);

        const Eigen::VectorXd &solution() const;

        Settings &getSettings();
        const Information &getInfo() const;

        // void saveProblemData(const std::string &path = "problem_data.hpp");

    private:
        void build(const Eigen::SparseMatrix<double> &G,
                   const Eigen::SparseMatrix<double> &A,
                   const Eigen::VectorXd &c,
                   const Eigen::VectorXd &h,
                   const Eigen::VectorXd &b,
                   const Eigen::VectorXi &soc_dims);

        Settings settings;
        Work w, w_best;

        size_t n_var;  // Number of variables (n)
        size_t n_eq;   // Number of equality constraints (p)
        size_t n_ineq; // Number of inequality constraints (m)
        size_t n_lc;   // Number of linear constraints (l)
        size_t n_sc;   // Number of second order cone constraints (ncones)
        size_t dim_K;  // Dimension of KKT matrix

        LPCone lp_cone;
        std::vector<SOCone> so_cones;

        Eigen::SparseMatrix<double> G;
        Eigen::SparseMatrix<double> A;
        Eigen::SparseMatrix<double> Gt;
        Eigen::SparseMatrix<double> At;
        Eigen::VectorXd c;
        Eigen::VectorXd h;
        Eigen::VectorXd b;

        // Residuals
        Eigen::VectorXd rx; // (size n_var)
        Eigen::VectorXd ry; // (size n_eq)
        Eigen::VectorXd rz; // (size n_ineq)
        double hresx, hresy, hresz;
        double rt;

        // Norm iterates
        double nx, ny, nz, ns;

        // Equilibration vectors
        Eigen::VectorXd x_equil; // (size n_var)
        Eigen::VectorXd A_equil; // (size n_eq)
        Eigen::VectorXd G_equil; // (size n_ineq)
        bool equibrilated;

        // The problem data scaling parameters
        double resx0, resy0, resz0;

        Eigen::VectorXd dsaff_by_W, W_times_dzaff, dsaff;

        // KKT
        Eigen::VectorXd rhs1; // The right hand side in the first  KKT equation.
        Eigen::VectorXd rhs2; // The right hand side in the second KKT equation.
        Eigen::SparseMatrix<double> K;
        using LDLT_t = Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::Upper>;
        LDLT_t ldlt;
        std::vector<double *> KKT_V_ptr;  // Pointer to scaling/regularization elements for fast update
        std::vector<double *> KKT_AG_ptr; // Pointer to A/G elements for fast update
        void setupKKT();
        void resetKKTScalings();
        void updateKKTScalings();
        void updateKKTAG();
        size_t solveKKT(const Eigen::VectorXd &rhs,
                        Eigen::VectorXd &dx,
                        Eigen::VectorXd &dy,
                        Eigen::VectorXd &dz,
                        bool initialize);

        void allocate();

        void bringToCone(const Eigen::VectorXd &r, Eigen::VectorXd &s);
        void computeResiduals();
        void updateStatistics();
        exitcode checkExitConditions(bool reduced_accuracy);
        bool updateScalings(const Eigen::VectorXd &s,
                            const Eigen::VectorXd &z,
                            Eigen::VectorXd &lambda);
        void RHSaffine();
        void RHScombined();
        void scale2add(const Eigen::VectorXd &x, Eigen::VectorXd &y);
        void scale(const Eigen::VectorXd &z, Eigen::VectorXd &lambda);
        double lineSearch(Eigen::VectorXd &lambda,
                          Eigen::VectorXd &ds,
                          Eigen::VectorXd &dz,
                          double tau,
                          double dtau,
                          double kap,
                          double dkap);
        double conicProduct(const Eigen::VectorXd &u,
                            const Eigen::VectorXd &v,
                            Eigen::VectorXd &w);
        void conicDivision(const Eigen::VectorXd &u,
                           const Eigen::VectorXd &w,
                           Eigen::VectorXd &v);
        void backscale();
        void setEquilibration();
        void unsetEquilibration();
        void cacheIndices();
        void printSummary();
    };

} // namespace EiCOS


// Copyright (c) 2011-2013 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.
#include "arith_uint256.h"
#include "uint256.h"
#include "version.h"

#include <boost/test/unit_test.hpp>
#include <stdint.h>
#include <sstream>
#include <iomanip>
#include <limits>
#include <cmath>
#include <string>
#include <stdio.h>

BOOST_AUTO_TEST_SUITE(uint256_tests)

const unsigned char g_kuchR1Array[] =
    "\x9c\x52\x4a\xdb\xcf\x56\x11\x12\x2b\x29\x12\x5e\x5d\x35\xd2\xd2"
    "\x22\x81\xaa\xb5\x33\xf0\x08\x32\xd5\x56\xb1\xf9\xea\xe5\x1d\x7d";
const char g_kchR1ArrayHex[] = "7D1DE5EAF9B156D53208F033B5AA8122D2d2355d5e12292b121156cfdb4a529c";
const uint256 g_kcR1L = uint256(std::vector<unsigned char>(g_kuchR1Array,g_kuchR1Array+32));
const uint160 g_kcR1S = uint160(std::vector<unsigned char>(g_kuchR1Array,g_kuchR1Array+20));

const unsigned char g_kuchR2Array[] =
    "\x70\x32\x1d\x7c\x47\xa5\x6b\x40\x26\x7e\x0a\xc3\xa6\x9c\xb6\xbf"
    "\x13\x30\x47\xa3\x19\x2d\xda\x71\x49\x13\x72\xf0\xb4\xca\x81\xd7";
const uint256 g_kcR2L = uint256(std::vector<unsigned char>(g_kuchR2Array,g_kuchR2Array+32));
const uint160 g_kcR2S = uint160(std::vector<unsigned char>(g_kuchR2Array,g_kuchR2Array+20));

const unsigned char g_kuchZeroArray[] =
    "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"
    "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00";
const uint256 g_kcZeroL = uint256(std::vector<unsigned char>(g_kuchZeroArray,g_kuchZeroArray+32));
const uint160 g_kcZeroS = uint160(std::vector<unsigned char>(g_kuchZeroArray,g_kuchZeroArray+20));

const unsigned char g_kuchOneArray[] =
    "\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"
    "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00";
const uint256 g_kcOneL = uint256(std::vector<unsigned char>(g_kuchOneArray,g_kuchOneArray+32));
const uint160 g_kcOneS = uint160(std::vector<unsigned char>(g_kuchOneArray,g_kuchOneArray+20));

const unsigned char g_kuchMaxArray[] =
    "\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff"
    "\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff";
const uint256 g_kcMaxL = uint256(std::vector<unsigned char>(g_kuchMaxArray,g_kuchMaxArray+32));
const uint160 g_kcMaxS = uint160(std::vector<unsigned char>(g_kuchMaxArray,g_kuchMaxArray+20));

std::string ArrayToString(const unsigned char A[], unsigned int width) {
	std::stringstream Stream;
	Stream << std::hex;
	for (unsigned int i = 0; i < width; ++i) {
		Stream << std::setw(2) << std::setfill('0') << (unsigned int) A[width - i - 1];
	}
	return Stream.str();
}

inline uint160 uint160S(const char *str) {
	uint160 cRv;
	cRv.SetHex(str);
	return cRv;
}

inline uint160 uint160S(const std::string& str) {
	uint160 cRv;
	cRv.SetHex(str);
	return cRv;
}

// constructors, equality, inequality
BOOST_AUTO_TEST_CASE( basics ) {
    BOOST_CHECK(1 == 0+1);
    // constructor uint256(vector<char>):
    BOOST_CHECK(g_kcR1L.ToString() == ArrayToString(g_kuchR1Array,32));
    BOOST_CHECK(g_kcR1S.ToString() == ArrayToString(g_kuchR1Array,20));
    BOOST_CHECK(g_kcR2L.ToString() == ArrayToString(g_kuchR2Array,32));
    BOOST_CHECK(g_kcR2S.ToString() == ArrayToString(g_kuchR2Array,20));
    BOOST_CHECK(g_kcZeroL.ToString() == ArrayToString(g_kuchZeroArray,32));
    BOOST_CHECK(g_kcZeroS.ToString() == ArrayToString(g_kuchZeroArray,20));
    BOOST_CHECK(g_kcOneL.ToString() == ArrayToString(g_kuchOneArray,32));
    BOOST_CHECK(g_kcOneS.ToString() == ArrayToString(g_kuchOneArray,20));
    BOOST_CHECK(g_kcMaxL.ToString() == ArrayToString(g_kuchMaxArray,32));
    BOOST_CHECK(g_kcMaxS.ToString() == ArrayToString(g_kuchMaxArray,20));
    BOOST_CHECK(g_kcOneL.ToString() != ArrayToString(g_kuchZeroArray,32));
    BOOST_CHECK(g_kcOneS.ToString() != ArrayToString(g_kuchZeroArray,20));

    // == and !=
    BOOST_CHECK(g_kcR1L != g_kcR2L && g_kcR1S != g_kcR2S);
    BOOST_CHECK(g_kcZeroL != g_kcOneL && g_kcZeroS != g_kcOneS);
    BOOST_CHECK(g_kcOneL != g_kcZeroL && g_kcOneS != g_kcZeroS);
    BOOST_CHECK(g_kcMaxL != g_kcZeroL && g_kcMaxS != g_kcZeroS);

    // String Constructor and Copy Constructor
    BOOST_CHECK(uint256S("0x"+g_kcR1L.ToString()) == g_kcR1L);
    BOOST_CHECK(uint256S("0x"+g_kcR2L.ToString()) == g_kcR2L);
    BOOST_CHECK(uint256S("0x"+g_kcZeroL.ToString()) == g_kcZeroL);
    BOOST_CHECK(uint256S("0x"+g_kcOneL.ToString()) == g_kcOneL);
    BOOST_CHECK(uint256S("0x"+g_kcMaxL.ToString()) == g_kcMaxL);
    BOOST_CHECK(uint256S(g_kcR1L.ToString()) == g_kcR1L);
    BOOST_CHECK(uint256S("   0x"+g_kcR1L.ToString()+"   ") == g_kcR1L);
    BOOST_CHECK(uint256S("") == g_kcZeroL);
    BOOST_CHECK(g_kcR1L == uint256S(g_kchR1ArrayHex));
    BOOST_CHECK(uint256(g_kcR1L) == g_kcR1L);
    BOOST_CHECK(uint256(g_kcZeroL) == g_kcZeroL);
    BOOST_CHECK(uint256(g_kcOneL) == g_kcOneL);

    BOOST_CHECK(uint160S("0x"+g_kcR1S.ToString()) == g_kcR1S);
    BOOST_CHECK(uint160S("0x"+g_kcR2S.ToString()) == g_kcR2S);
    BOOST_CHECK(uint160S("0x"+g_kcZeroS.ToString()) == g_kcZeroS);
    BOOST_CHECK(uint160S("0x"+g_kcOneS.ToString()) == g_kcOneS);
    BOOST_CHECK(uint160S("0x"+g_kcMaxS.ToString()) == g_kcMaxS);
    BOOST_CHECK(uint160S(g_kcR1S.ToString()) == g_kcR1S);
    BOOST_CHECK(uint160S("   0x"+g_kcR1S.ToString()+"   ") == g_kcR1S);
    BOOST_CHECK(uint160S("") == g_kcZeroS);
    BOOST_CHECK(g_kcR1S == uint160S(g_kchR1ArrayHex));

    BOOST_CHECK(uint160(g_kcR1S) == g_kcR1S);
    BOOST_CHECK(uint160(g_kcZeroS) == g_kcZeroS);
    BOOST_CHECK(uint160(g_kcOneS) == g_kcOneS);
}

// <= >= < >
BOOST_AUTO_TEST_CASE( comparison ) {
	uint256 cLastL;
	for (int i = 255; i >= 0; --i) {
		uint256 TmpL;
		*(TmpL.begin() + (i >> 3)) |= 1 << (7 - (i & 7));
		BOOST_CHECK(cLastL < TmpL);
		cLastL = TmpL;
	}

    BOOST_CHECK( g_kcZeroL < g_kcR1L );
    BOOST_CHECK( g_kcR2L < g_kcR1L );
    BOOST_CHECK( g_kcZeroL < g_kcOneL );
    BOOST_CHECK( g_kcOneL < g_kcMaxL );
    BOOST_CHECK( g_kcR1L < g_kcMaxL );
    BOOST_CHECK( g_kcR2L < g_kcMaxL );

	uint160 cLastS;
	for (int i = 159; i >= 0; --i) {
		uint160 cTmpS;
		*(cTmpS.begin() + (i >> 3)) |= 1 << (7 - (i & 7));
		BOOST_CHECK(cLastS < cTmpS);
		cLastS = cTmpS;
	}
    BOOST_CHECK( g_kcZeroS < g_kcR1S );
    BOOST_CHECK( g_kcR2S < g_kcR1S );
    BOOST_CHECK( g_kcZeroS < g_kcOneS );
    BOOST_CHECK( g_kcOneS < g_kcMaxS );
    BOOST_CHECK( g_kcR1S < g_kcMaxS );
    BOOST_CHECK( g_kcR2S < g_kcMaxS );
}

BOOST_AUTO_TEST_CASE( methods ) // GetHex SetHex begin() end() size() GetLow64 GetSerializeSize, Serialize, Unserialize
{
    BOOST_CHECK(g_kcR1L.GetHex() == g_kcR1L.ToString());
    BOOST_CHECK(g_kcR2L.GetHex() == g_kcR2L.ToString());
    BOOST_CHECK(g_kcOneL.GetHex() == g_kcOneL.ToString());
    BOOST_CHECK(g_kcMaxL.GetHex() == g_kcMaxL.ToString());
    uint256 cTmpL(g_kcR1L);
    BOOST_CHECK(cTmpL == g_kcR1L);
    cTmpL.SetHex(g_kcR2L.ToString());   BOOST_CHECK(cTmpL == g_kcR2L);
    cTmpL.SetHex(g_kcZeroL.ToString()); BOOST_CHECK(cTmpL == uint256());

    cTmpL.SetHex(g_kcR1L.ToString());
    BOOST_CHECK(memcmp(g_kcR1L.begin(), g_kuchR1Array, 32)==0);
    BOOST_CHECK(memcmp(cTmpL.begin(), g_kuchR1Array, 32)==0);
    BOOST_CHECK(memcmp(g_kcR2L.begin(), g_kuchR2Array, 32)==0);
    BOOST_CHECK(memcmp(g_kcZeroL.begin(), g_kuchZeroArray, 32)==0);
    BOOST_CHECK(memcmp(g_kcOneL.begin(), g_kuchOneArray, 32)==0);
    BOOST_CHECK(g_kcR1L.size() == sizeof(g_kcR1L));
    BOOST_CHECK(sizeof(g_kcR1L) == 32);
    BOOST_CHECK(g_kcR1L.size() == 32);
    BOOST_CHECK(g_kcR2L.size() == 32);
    BOOST_CHECK(g_kcZeroL.size() == 32);
    BOOST_CHECK(g_kcMaxL.size() == 32);
    BOOST_CHECK(g_kcR1L.begin() + 32 == g_kcR1L.end());
    BOOST_CHECK(g_kcR2L.begin() + 32 == g_kcR2L.end());
    BOOST_CHECK(g_kcOneL.begin() + 32 == g_kcOneL.end());
    BOOST_CHECK(g_kcMaxL.begin() + 32 == g_kcMaxL.end());
    BOOST_CHECK(cTmpL.begin() + 32 == cTmpL.end());
    BOOST_CHECK(g_kcR1L.GetSerializeSize(0,g_sProtocolVersion) == 32);
    BOOST_CHECK(g_kcZeroL.GetSerializeSize(0,g_sProtocolVersion) == 32);

    std::stringstream ss;
    g_kcR1L.Serialize(ss,0,g_sProtocolVersion);
    BOOST_CHECK(ss.str() == std::string(g_kuchR1Array,g_kuchR1Array+32));
    cTmpL.Unserialize(ss,0,g_sProtocolVersion);
    BOOST_CHECK(g_kcR1L == cTmpL);
    ss.str("");
    g_kcZeroL.Serialize(ss,0,g_sProtocolVersion);
    BOOST_CHECK(ss.str() == std::string(g_kuchZeroArray,g_kuchZeroArray+32));
    cTmpL.Unserialize(ss,0,g_sProtocolVersion);
    BOOST_CHECK(g_kcZeroL == cTmpL);
    ss.str("");
    g_kcMaxL.Serialize(ss,0,g_sProtocolVersion);
    BOOST_CHECK(ss.str() == std::string(g_kuchMaxArray,g_kuchMaxArray+32));
    cTmpL.Unserialize(ss,0,g_sProtocolVersion);
    BOOST_CHECK(g_kcMaxL == cTmpL);
    ss.str("");

    BOOST_CHECK(g_kcR1S.GetHex() == g_kcR1S.ToString());
    BOOST_CHECK(g_kcR2S.GetHex() == g_kcR2S.ToString());
    BOOST_CHECK(g_kcOneS.GetHex() == g_kcOneS.ToString());
    BOOST_CHECK(g_kcMaxS.GetHex() == g_kcMaxS.ToString());
    uint160 TmpS(g_kcR1S);
    BOOST_CHECK(TmpS == g_kcR1S);
    TmpS.SetHex(g_kcR2S.ToString());   BOOST_CHECK(TmpS == g_kcR2S);
    TmpS.SetHex(g_kcZeroS.ToString()); BOOST_CHECK(TmpS == uint160());

    TmpS.SetHex(g_kcR1S.ToString());
    BOOST_CHECK(memcmp(g_kcR1S.begin(), g_kuchR1Array, 20)==0);
    BOOST_CHECK(memcmp(TmpS.begin(), g_kuchR1Array, 20)==0);
    BOOST_CHECK(memcmp(g_kcR2S.begin(), g_kuchR2Array, 20)==0);
    BOOST_CHECK(memcmp(g_kcZeroS.begin(), g_kuchZeroArray, 20)==0);
    BOOST_CHECK(memcmp(g_kcOneS.begin(), g_kuchOneArray, 20)==0);
    BOOST_CHECK(g_kcR1S.size() == sizeof(g_kcR1S));
    BOOST_CHECK(sizeof(g_kcR1S) == 20);
    BOOST_CHECK(g_kcR1S.size() == 20);
    BOOST_CHECK(g_kcR2S.size() == 20);
    BOOST_CHECK(g_kcZeroS.size() == 20);
    BOOST_CHECK(g_kcMaxS.size() == 20);
    BOOST_CHECK(g_kcR1S.begin() + 20 == g_kcR1S.end());
    BOOST_CHECK(g_kcR2S.begin() + 20 == g_kcR2S.end());
    BOOST_CHECK(g_kcOneS.begin() + 20 == g_kcOneS.end());
    BOOST_CHECK(g_kcMaxS.begin() + 20 == g_kcMaxS.end());
    BOOST_CHECK(TmpS.begin() + 20 == TmpS.end());
    BOOST_CHECK(g_kcR1S.GetSerializeSize(0,g_sProtocolVersion) == 20);
    BOOST_CHECK(g_kcZeroS.GetSerializeSize(0,g_sProtocolVersion) == 20);

    g_kcR1S.Serialize(ss,0,g_sProtocolVersion);
    BOOST_CHECK(ss.str() == std::string(g_kuchR1Array,g_kuchR1Array+20));
    TmpS.Unserialize(ss,0,g_sProtocolVersion);
    BOOST_CHECK(g_kcR1S == TmpS);
    ss.str("");
    g_kcZeroS.Serialize(ss,0,g_sProtocolVersion);
    BOOST_CHECK(ss.str() == std::string(g_kuchZeroArray,g_kuchZeroArray+20));
    TmpS.Unserialize(ss,0,g_sProtocolVersion);
    BOOST_CHECK(g_kcZeroS == TmpS);
    ss.str("");
    g_kcMaxS.Serialize(ss,0,g_sProtocolVersion);
    BOOST_CHECK(ss.str() == std::string(g_kuchMaxArray,g_kuchMaxArray+20));
    TmpS.Unserialize(ss,0,g_sProtocolVersion);
    BOOST_CHECK(g_kcMaxS == TmpS);
    ss.str("");
}

BOOST_AUTO_TEST_CASE( conversion ) {
    BOOST_CHECK(ArithToUint256(UintToArith256(g_kcZeroL)) == g_kcZeroL);
    BOOST_CHECK(ArithToUint256(UintToArith256(g_kcOneL)) == g_kcOneL);
    BOOST_CHECK(ArithToUint256(UintToArith256(g_kcR1L)) == g_kcR1L);
    BOOST_CHECK(ArithToUint256(UintToArith256(g_kcR2L)) == g_kcR2L);
    BOOST_CHECK(UintToArith256(g_kcZeroL) == 0);
    BOOST_CHECK(UintToArith256(g_kcOneL) == 1);
    BOOST_CHECK(ArithToUint256(0) == g_kcZeroL);
    BOOST_CHECK(ArithToUint256(1) == g_kcOneL);
    BOOST_CHECK(arith_uint256(g_kcR1L.GetHex()) == UintToArith256(g_kcR1L));
    BOOST_CHECK(arith_uint256(g_kcR2L.GetHex()) == UintToArith256(g_kcR2L));
    BOOST_CHECK(g_kcR1L.GetHex() == UintToArith256(g_kcR1L).GetHex());
    BOOST_CHECK(g_kcR2L.GetHex() == UintToArith256(g_kcR2L).GetHex());
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/accumulators/statistics/tail.hpp>


#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>


#include <boost/math/concepts/distributions.hpp>


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file Jacobian.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_JACOBIAN_HPP
#define STEAM_JACOBIAN_HPP

#include <Eigen/Dense>
#include <steam/state/StateVariableBase.hpp>

namespace steam {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Simple structure to hold Jacobian information
//////////////////////////////////////////////////////////////////////////////////////////////
template <int LHS_DIM = Eigen::Dynamic,
          int MAX_STATE_DIM = Eigen::Dynamic>
struct Jacobian
{

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Default constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  Jacobian() {}

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  Jacobian(const StateKey& key, const Eigen::Matrix<double, LHS_DIM, MAX_STATE_DIM>& jac)
    : key(key), jac(jac) {
  }

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Go through vector of Jacobians and check for Jacobians which are with respect to
  ///        the same state variable, and merge them.
  ///
  /// For efficiency, specify a hintIndex, which specifies that Jacobians before hintIndex
  /// cannot be multiples of eachother.
  //////////////////////////////////////////////////////////////////////////////////////////////
  static void merge(std::vector<Jacobian<LHS_DIM,MAX_STATE_DIM> >* outJacobians, unsigned int hintIndex) {

    // Check inputs
    if (hintIndex > outJacobians->size()) {
      throw std::invalid_argument("The specified hintIndex is beyond the size of outJacobians");
    }

    // Iterate over the 'safe' non-duplicate Jacobians
    for (unsigned int j = 0; j < hintIndex; j++) {

      // Iterate over the branched (other) Jacobians
      for (unsigned int k = hintIndex; k < outJacobians->size();) {

        // Check if Jacobian j and k are w.r.t the same state variable.
        // If so, we must merge them and erase the second entry
        if (outJacobians->at(j).key.equals(outJacobians->at(k).key)) {

          // Merge
          outJacobians->at(j).jac += outJacobians->at(k).jac;

          // Erase duplicate
          outJacobians->erase(outJacobians->begin() + k);

          // Assuming merge has been called consistently, there should not exist
          // more than one duplicate in second half.. so we can go to the next 'j'
          break;

        } else {
          k++;
        }
      }
    }
  }

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Key of the associated state variable
  //////////////////////////////////////////////////////////////////////////////////////////////
  StateKey key;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Jacobian matrix
  //////////////////////////////////////////////////////////////////////////////////////////////
  Eigen::Matrix<double, LHS_DIM, MAX_STATE_DIM> jac;
};

} // steam

#endif // STEAM_JACOBIAN_HPP


/******************************************************************************
 * Copyright 2017 Baidu Robotic Vision Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include "param.h"
#include <glog/logging.h>
#include <opencv2/core.hpp>
#include <Eigen/Eigen>
#include <fstream>
#include <list>
#include <algorithm>

using std::vector;
using Eigen::Matrix4f;
namespace YAML {
// Define templated helper functions for YAML to encode/decode from custom data type.
// We need special care for emitters of Vector and Matrix!
template<>
struct convert<Eigen::Vector3f> {
  static Node encode(const Eigen::Vector3f& rhs) {
    Node node;
    node.push_back(rhs(0));
    node.push_back(rhs(1));
    node.push_back(rhs(2));
    return node;
  }

  static bool decode(const Node& node, Eigen::Vector3f& rhs) {  // NOLINT
    if (!node.IsSequence() || node.size() != 3) {
      return false;
    }
    rhs(0) = node[0].as<float>();
    rhs(1) = node[1].as<float>();
    rhs(2) = node[2].as<float>();
    return true;
  }
};

Emitter& operator << (Emitter& out, const Eigen::Vector3f& v) {
  out << YAML::Flow;
  out << YAML::BeginSeq << v(0) << v(1) << v(2) << YAML::EndSeq;
  return out;
}
template<>
struct convert<cv::Size> {
  static Node encode(const cv::Size& rhs) {
    Node node;
    node.push_back(rhs.width);
    node.push_back(rhs.height);
    return node;
  }

  static bool decode(const Node& node, cv::Size& rhs) {  // NOLINT
    if (!node.IsSequence() || node.size() != 2) {
      return false;
    }
    rhs.width = node[0].as<int>();
    rhs.height = node[1].as<int>();
    return true;
  }
};
Emitter& operator << (Emitter& out, const cv::Size& v) {
  out << YAML::Flow;
  out << YAML::BeginSeq << v.width << v.height << YAML::EndSeq;
  return out;
}
template<int M, int N>
struct convert<Eigen::Matrix<float, M, N>> {
// Encode Matrix3f as three sequences of 3-element sequence (row-major)
static Node encode(const Eigen::Matrix<float, M, N>& rhs) {
  Node node;
  for (int r = 0; r < M; r++) {
    Node row;
    for (int c = 0; c < N; c++) {
      row.push_back(rhs(r, c));
    }
    node.push_back(row);
  }
  return node;
}

static bool decode(const Node& node, Eigen::Matrix<float, M, N>& rhs) {  // NOLINT
  if (!node.IsSequence() || node.size() != M) {
    return false;
  }
  for (int r = 0; r < M; r++) {
    if (!node[r].IsSequence() || node[r].size() != N) {
      return false;
    }
    for (int c = 0 ; c < N; c++) {
      rhs(r, c) = node[r][c].as<float>();
    }
  }
  return true;
}
};
template<int M, int N>
Emitter& operator << (Emitter& out, const Eigen::Matrix<float, M, N>& v) {
  out << YAML::BeginSeq;
  for (int r = 0; r < M; r++) {
    out << YAML::Flow;
    out << YAML::BeginSeq;
    for (int c = 0; c < N; c++) {
      out << v(r, c);
    }
    out << YAML::EndSeq;
  }
  out << YAML::EndSeq;
  return out;
}
template<int M, int N>
struct convert<cv::Matx<float, M, N>> {
  static Node encode(const cv::Matx<float, M, N>& rhs) {
    Node node;
    for (int r = 0; r < M; r++) {
      Node row;
      for (int c = 0; c < N; c++) {
        row.push_back(rhs(r, c));
      }
      node.push_back(row);
    }
    return node;
  }
  static bool decode(const Node& node, cv::Matx<float, M, N>& rhs) {  // NOLINT
    if (!node.IsSequence() || node.size() != M) {
      return false;
    }
    for (int r = 0; r < M; r++) {
      if (!node[r].IsSequence()) {
        return false;
      }
      if (node[r].size() != N) {
        return false;
      }
      for (int c = 0 ; c < N; c++) {
        rhs(r, c) = node[r][c].as<float>();
      }
    }
    return true;
  }
};
template<>
struct convert<cv::Mat_<float>> {
  static Node encode(const cv::Mat_<float>& rhs) {
    Node node;
    for (int r = 0; r < rhs.rows; r++) {
      Node row;
      for (int c = 0; c < rhs.cols; c++) {
        row.push_back(rhs(r, c));
      }
      node.push_back(row);
    }
    return node;
  }
  static bool decode(const Node& node, cv::Mat_<float>& rhs) {  // NOLINT
    if (!node.IsSequence()) {
      return false;
    }
    const int M = node.size();
    if (!node[0].IsSequence()) {
      return false;
    }
    const int N = node[0].size();
    rhs.create(M, N);
    for (int r = 0; r < M; r++) {
      if (!node[r].IsSequence()) {
        return false;
      }
      if (node[r].size() != N) {
        return false;
      }
      for (int c = 0 ; c < N; c++) {
        rhs(r, c) = node[r][c].as<float>();
      }
    }
    return true;
  }
};

template<int M, int N>
Emitter& operator << (Emitter& out, const cv::Matx<float, M, N>& v) {
  out << YAML::BeginSeq;
  for (int r = 0; r < M; r++) {
    out << YAML::Flow;
    out << YAML::BeginSeq;
    for (int c = 0; c < N; c++) {
      out << v(r, c);
    }
    out << YAML::EndSeq;
  }
  out << YAML::EndSeq;
  return out;
}

template<>
struct convert<XP::AlgorithmParam::FeatDetParam_t> {
  static Node encode(const XP::AlgorithmParam::FeatDetParam_t& rhs) {
    Node node;
    node["request_feat_num"] = rhs.request_feat_num;
    node["pyra_level"] = rhs.pyra_level;
    node["uniform_radius"] = rhs.uniform_radius;
    node["fast_det_thresh"] = rhs.fast_det_thresh;
    node["min_feature_distance_over_baseline_ratio"] = rhs.min_feature_distance_over_baseline_ratio;
    node["max_feature_distance_over_baseline_ratio"] = rhs.max_feature_distance_over_baseline_ratio;
    node["feature_track_length_thresh"] = rhs.feature_track_length_thresh;
    node["feature_track_dropout_rate"] = rhs.feature_track_dropout_rate;
    return node;
  }
  static bool decode(const Node& node, XP::AlgorithmParam::FeatDetParam_t& rhs) {  // NOLINT
    if (!node.IsMap()) {
      return false;
    }
    rhs.request_feat_num = node["request_feat_num"].as<int>();
    rhs.pyra_level = node["pyra_level"].as<int>();
    rhs.uniform_radius = node["uniform_radius"].as<int>();
    rhs.fast_det_thresh = node["fast_det_thresh"].as<int>();
    rhs.min_feature_distance_over_baseline_ratio =
        node["min_feature_distance_over_baseline_ratio"].as<float>();
    rhs.max_feature_distance_over_baseline_ratio =
        node["max_feature_distance_over_baseline_ratio"].as<float>();
    rhs.feature_track_length_thresh = node["feature_track_length_thresh"].as<int>();
    rhs.feature_track_dropout_rate = node["feature_track_dropout_rate"].as<float>();
    return true;
  }
};

Emitter& operator << (Emitter& out, const XP::AlgorithmParam::FeatDetParam_t& v) {
  Node node;
  node = v;
  out << node;
  return out;
}
template<>
struct convert<XP::AlgorithmParam::Tracking_t> {
  static Node encode(const XP::AlgorithmParam::Tracking_t& rhs) {
    Node node;
    node["feature_uncertainty"] = rhs.feature_uncertainty;
    node["use_of_id"] = rhs.use_of_id;
    node["slave_det"] = rhs.slave_det;
    node["undistort_before_vio"] = rhs.undistort_before_vio;
    return node;
  }

  static bool decode(const Node& node, XP::AlgorithmParam::Tracking_t& rhs) {  // NOLINT
    if (!node.IsMap()) {
      return false;
    }
    rhs.feature_uncertainty = node["feature_uncertainty"].as<float>();
    rhs.use_of_id = node["use_of_id"].as<bool>();
    rhs.slave_det = node["slave_det"].as<std::string>();
    if (rhs.slave_det != "epi" && rhs.slave_det != "OF" && rhs.slave_det != "direct") {
      LOG(ERROR) << "slave_det mode " << rhs.slave_det << " not supported";
      return false;
    }
    rhs.undistort_before_vio = node["undistort_before_vio"].as<bool>();
    if (rhs.undistort_before_vio && !rhs.use_of_id) {
      // if we feed undistorted pnts to vio, we have to let vio use the OF id
      LOG(ERROR) << "undistort_before_vio && !use_of_id";
      return false;
    }
    return true;
  }
};

Emitter& operator << (Emitter& out, const XP::AlgorithmParam::Tracking_t& v) {
  Node node;
  node = v;
  out << node;
  return out;
}

template<>
struct convert<XP::DuoCalibParam::Imu_t> {
  static Node encode(const XP::DuoCalibParam::Imu_t& rhs) {
    Node node;
    node["accel_TK"] = rhs.accel_TK;
    node["accel_bias"] = rhs.accel_bias;
    node["gyro_TK"] = rhs.gyro_TK;
    node["gyro_bias"] = rhs.gyro_bias;
    node["accel_noise_var"] = rhs.accel_noise_var;
    node["angv_noise_var"] = rhs.angv_noise_var;
    node["D_T_I"] = rhs.D_T_I;
    return node;
  }
  static bool decode(const Node& node, XP::DuoCalibParam::Imu_t& rhs) {  // NOLINT
    if (!node.IsMap()) {
      return false;
    }
    rhs.accel_TK = node["accel_TK"].as<Eigen::Matrix3f>();
    rhs.accel_bias = node["accel_bias"].as<Eigen::Vector3f>();
    rhs.gyro_TK = node["gyro_TK"].as<Eigen::Matrix3f>();
    rhs.gyro_bias = node["gyro_bias"].as<Eigen::Vector3f>();
    rhs.accel_noise_var = node["accel_noise_var"].as<Eigen::Vector3f>();
    rhs.angv_noise_var = node["angv_noise_var"].as<Eigen::Vector3f>();
    if (node["D_T_I"]) {
      rhs.D_T_I = node["D_T_I"].as<Eigen::Matrix4f>();
    } else {
      VLOG(1) << "Set D_T_I as default val (I)";
      rhs.D_T_I = Eigen::Matrix4f::Identity();
    }
    return true;
  }
};

// Imu_t need special care because of the formatting of Matrix and Vector
Emitter& operator << (Emitter& out, const XP::DuoCalibParam::Imu_t& v) {
  out << YAML::BeginMap;
  out << YAML::Key << "accel_TK"
      << YAML::Value << v.accel_TK;
  out << YAML::Key << "accel_bias"
      << YAML::Value << v.accel_bias;
  out << YAML::Key << "gyro_TK"
      << YAML::Value << v.gyro_TK;
  out << YAML::Key << "gyro_bias"
      << YAML::Value << v.gyro_bias;
  out << YAML::Key << "accel_noise_var"
      << YAML::Value << v.accel_noise_var;
  out << YAML::Key << "angv_noise_var"
      << YAML::Value << v.angv_noise_var;
  out << YAML::Key << "D_T_I"
      << YAML::Value << v.D_T_I;
  out << YAML::EndMap;
  return out;
}

template<>
struct convert<XP::DuoCalibParam::Camera_t> {
  static Node encode(const XP::DuoCalibParam::Camera_t& rhs) {
    Node node;
    node["D_T_C_l"] = rhs.D_T_C_lr[0];
    node["D_T_C_r"] = rhs.D_T_C_lr[1];
    node["cameraK_l"] = rhs.cameraK_lr[0];
    node["cameraK_r"] = rhs.cameraK_lr[1];
    node["dist_coeff_l"] = rhs.cv_dist_coeff_lr[0];
    node["dist_coeff_r"] = rhs.cv_dist_coeff_lr[1];
    node["img_size"] = rhs.img_size;
    return node;
  }

  static bool decode(const Node& node, XP::DuoCalibParam::Camera_t& rhs) {  // NOLINT
    if (!node.IsMap()) {
      return false;
    }
    rhs.D_T_C_lr.resize(2);
    rhs.D_T_C_lr[0] = node["D_T_C_l"].as<Eigen::Matrix4f>();
    rhs.D_T_C_lr[1] = node["D_T_C_r"].as<Eigen::Matrix4f>();
    rhs.cameraK_lr.resize(2);
    rhs.cameraK_lr[0] = node["cameraK_l"].as<Eigen::Matrix3f>();
    rhs.cameraK_lr[1] = node["cameraK_r"].as<Eigen::Matrix3f>();
    rhs.cv_camK_lr.resize(2);
    rhs.cv_camK_lr[0] = node["cameraK_l"].as<cv::Matx33f>();
    rhs.cv_camK_lr[1] = node["cameraK_r"].as<cv::Matx33f>();
    rhs.cv_dist_coeff_lr.resize(2);
    cv::Matx<float, 8, 1> dist_l =
        node["dist_coeff_l"].as<cv::Matx<float, 8, 1>>();
    if (dist_l(7) < 1e-7 && dist_l(7) > -1e-7 &&
        dist_l(6) < 1e-7 && dist_l(6) > -1e-7 &&
        dist_l(5) < 1e-7 && dist_l(5) > -1e-7) {
      if (dist_l(4) < 1e-7 && dist_l(4) > -1e-7) {
        rhs.cv_dist_coeff_lr[0].create(4, 1);
      } else {
        rhs.cv_dist_coeff_lr[0].create(5, 1);
      }
    } else {
      rhs.cv_dist_coeff_lr[0].create(8, 1);
    }
    for (int i = 0; i < rhs.cv_dist_coeff_lr[0].rows; ++i) {
      rhs.cv_dist_coeff_lr[0](i) = dist_l(i);
    }
    cv::Matx<float, 8, 1> dist_r =
        node["dist_coeff_r"].as<cv::Matx<float, 8, 1>>();
    if (dist_r(7) < 1e-7 && dist_r(7) > -1e-7 &&
        dist_r(6) < 1e-7 && dist_r(6) > -1e-7 &&
        dist_r(5) < 1e-7 && dist_r(5) > -1e-7) {
      if (dist_r(4) < 1e-7 && dist_r(4) > -1e-7) {
        rhs.cv_dist_coeff_lr[1].create(4, 1);
      } else {
        rhs.cv_dist_coeff_lr[1].create(5, 1);
      }
    } else {
      rhs.cv_dist_coeff_lr[1].create(8, 1);
    }
    for (int i = 0; i < rhs.cv_dist_coeff_lr[1].rows; ++i) {
      rhs.cv_dist_coeff_lr[1](i) = dist_r(i);
    }
    if (node["img_size"]) {
      rhs.img_size = node["img_size"].as<cv::Size>();
    } else {
      rhs.img_size.width = 752;  // default param
      rhs.img_size.height = 480;  // default param
      std::cout << "no img_size is found in calib file. Using default value "
                << rhs.img_size << std::endl;
    }
    return true;
  }
};
// Imu_t need special care because of the formatting of Matrix and Vector
Emitter& operator << (Emitter& out, const XP::DuoCalibParam::Camera_t& v) {
  out << YAML::BeginMap;
  out << YAML::Key << "D_T_C_l"
      << YAML::Value << v.D_T_C_lr[0];
  out << YAML::Key << "D_T_C_r"
      << YAML::Value << v.D_T_C_lr[1];
  out << YAML::Key << "cameraK_l" << YAML::Value << v.cameraK_lr[0];
  out << YAML::Key << "cameraK_r" << YAML::Value << v.cameraK_lr[1];
  cv::Matx<float, 8, 1> dist_l = cv::Matx<float, 8, 1>::zeros();
  CHECK_EQ(v.cv_dist_coeff_lr.size(), 2);
  // CHECK_LE(v.cv_dist_coeff_lr[0].rows, dist_l.rows);
  for (int i = 0; i < std::min(8, v.cv_dist_coeff_lr[0].rows); ++i) {
    dist_l(i) = v.cv_dist_coeff_lr[0](i);
  }
  out << YAML::Key << "dist_coeff_l" << YAML::Value << dist_l;
  cv::Matx<float, 8, 1> dist_r = cv::Matx<float, 8, 1>::zeros();
  // CHECK_LE(v.cv_dist_coeff_lr[0].rows, dist_r.rows);
  for (int i = 0; i < std::min(8, v.cv_dist_coeff_lr[1].rows); ++i) {
    dist_r(i) = v.cv_dist_coeff_lr[1](i);
  }
  out << YAML::Key << "dist_coeff_r" << YAML::Value << dist_r;
  out << YAML::Key << "img_size" << YAML::Value << v.img_size;
  out << YAML::EndMap;
  return out;
}

}  // namespace YAML

namespace XP {

YAML::Node ParamBase::deserialize(const std::string& filename) {
  std::ifstream in(filename);
  CHECK(in.is_open()) << "Cannot load file " << filename;
  in.close();
  YAML::Node node = YAML::LoadFile(filename);
  return node;
}

void ParamBase::serialize(const std::string& filename, const YAML::Emitter& emitter) {
  std::ofstream fout(filename);
  fout << emitter.c_str();
}

bool AlgorithmParam::LoadFromYaml(const std::string& filename) {
  YAML::Node node = deserialize(filename);
  FeatDetParam = node["FeatDetParam"].as<FeatDetParam_t>();
  Tracking = node["Tracking"].as<Tracking_t>();
  return true;
}

bool AlgorithmParam::LoadFromCvYaml(const std::string& filename) {
  LOG(FATAL) << "Not Implemented.";
  return false;
}

bool AlgorithmParam::WriteToYaml(const std::string& filename) {
  YAML::Emitter emitter;
  emitter << YAML::BeginMap;
  emitter << YAML::Key << "FeatDetParam"
          << YAML::Value << FeatDetParam;
  emitter << YAML::Key << "Tracking"
          << YAML::Value << Tracking;
  emitter << YAML::EndMap;

  serialize(filename, emitter);
  return true;
}

bool AlgorithmParam::WriteToCvYaml(const std::string& filename) {
  LOG(FATAL) << "Not Implemented.";
  return false;
}

DuoCalibParam::DuoCalibParam() :
    C_R_B(Eigen::Matrix3f::Identity()),
    C_p_B(Eigen::Vector3f::Zero()),
    device_id("n/acc"),
    sensor_type(SensorType::UNKNOWN) {
  this->Camera.D_T_C_lr.resize(2, Eigen::Matrix4f::Identity());
  this->Camera.cameraK_lr.resize(2, Eigen::Matrix3f::Identity());
  this->Camera.cv_camK_lr.resize(2, cv::Matx33f::eye());
  this->Camera.lurd_lr.resize(2, Eigen::Vector4f::Zero());
  this->Camera.cv_dist_coeff_lr.resize(2);
  this->Imu.accel_TK = Eigen::Matrix3f::Identity();
  this->Imu.accel_bias.setZero();
  this->Imu.gyro_TK = Eigen::Matrix3f::Identity();
  this->Imu.gyro_bias.setZero();
  this->Imu.accel_noise_var.setConstant(0.04 * 0.04);
  this->Imu.angv_noise_var.setConstant(0.01 * 0.01);
  this->Imu.D_T_I = Eigen::Matrix4f::Identity();;
}

bool DuoCalibParam::LoadFromCvYaml(const std::string& filename) {
  cv::FileStorage cvfs(filename, cv::FileStorage::READ);
  if (!cvfs.isOpened()) {
    LOG(ERROR) << "Can't open file: " << filename;
    return false;
  }
  // loading device id
  cvfs["DeviceId"] >> device_id;
  // loading sensor type
  std::string str_sensor_type = cvfs["SensorType"];
  if (str_sensor_type == "UNKNOWN") {
    sensor_type = XP::DuoCalibParam::SensorType::UNKNOWN;
  } else if (str_sensor_type == "LI") {
    sensor_type = XP::DuoCalibParam::SensorType::LI;
  } else if (str_sensor_type == "XP") {
    sensor_type = XP::DuoCalibParam::SensorType::XP;
  } else if (str_sensor_type == "XP2") {
    sensor_type = XP::DuoCalibParam::SensorType::XP2;
  } else if (str_sensor_type == "XP3") {
    sensor_type = XP::DuoCalibParam::SensorType::XP3;
  } else {
    LOG(ERROR) << "Unknown sensor type";
    cvfs.release();
    return false;
  }

  // IMU
  cv::Mat accel_TK;
  cvfs["accel_TK"] >> accel_TK;
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      Imu.accel_TK(i, j) = accel_TK.at<double>(i, j);
    }
  }

  cv::Mat accel_bias;
  cvfs["accel_bias"] >> accel_bias;
  for (int i = 0; i < 3; ++i) {
    Imu.accel_bias(i, 0) = accel_bias.at<double>(i, 0);
  }

  cv::Mat gyro_TK;
  cvfs["gyro_TK"] >> gyro_TK;
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      Imu.gyro_TK(i, j) = gyro_TK.at<double>(i, j);
    }
  }

  cv::Mat gyro_bias;
  cvfs["gyro_bias"] >> gyro_bias;
  for (int i = 0; i < 3; ++i) {
    Imu.gyro_bias(i, 0) = gyro_bias.at<double>(i, 0);
  }

  cv::Mat accel_noise_var;
  cvfs["accel_noise_var"] >> accel_noise_var;
  for (int i = 0; i < 3; ++i) {
    Imu.accel_noise_var(i, 0) = accel_noise_var.at<double>(i, 0);
  }

  cv::Mat angv_noise_var;
  cvfs["angv_noise_var"] >> angv_noise_var;
  for (int i = 0; i < 3; ++i) {
    Imu.angv_noise_var(i, 0) = angv_noise_var.at<double>(i, 0);
  }

  cv::Mat D_T_I;
  cvfs["D_T_I"] >> D_T_I;
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      Imu.D_T_I(i, j) = D_T_I.at<double>(i, j);
    }
  }

  // CAMERA
  cv::Mat d_t_c[2];
  cvfs["D_T_C_l"] >> d_t_c[0];
  cvfs["D_T_C_r"] >> d_t_c[1];
  // COLUMN MAJOR ORDER
  Eigen::Matrix4f& dtcl = Camera.D_T_C_lr[0];
  Eigen::Matrix4f& dtcr = Camera.D_T_C_lr[1];
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      dtcl(i, j) = d_t_c[0].at<double>(i, j);
      dtcr(i, j) = d_t_c[1].at<double>(i, j);
    }
  }

  cv::Mat cameraK[2];
  cvfs["cameraK_l"] >> cameraK[0];
  cvfs["cameraK_r"] >> cameraK[1];
  // COLUMN MAJOR ORDER
  Eigen::Matrix3f& camera_k_l = Camera.cameraK_lr[0];
  Eigen::Matrix3f& camera_k_r = Camera.cameraK_lr[1];
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      camera_k_l(i, j) = cameraK[0].at<double>(i, j);
      camera_k_r(i, j) = cameraK[1].at<double>(i, j);
    }
  }

  cv::Mat dist_coeff[2];
  cvfs["dist_coeff_l"] >> dist_coeff[0];
  cvfs["dist_coeff_r"] >> dist_coeff[1];
  dist_coeff[0].convertTo(Camera.cv_dist_coeff_lr[0], CV_32FC1);
  dist_coeff[1].convertTo(Camera.cv_dist_coeff_lr[1], CV_32FC1);

  cvfs["img_size"] >> Camera.img_size;
  cvfs.release();
  return true;
}

bool DuoCalibParam::LoadFromYaml(const std::string& filename) {
  return (this->LoadImuCalibFromYaml(filename) &&
      this->LoadCamCalibFromYaml(filename));
}

bool DuoCalibParam::LoadImuCalibFromYaml(const std::string& filename) {
  YAML::Node node = deserialize(filename);
  if (!node["Imu"].IsDefined()) {
    return false;
  }
  this->Imu = node["Imu"].as<Imu_t>();
  return true;
}

bool DuoCalibParam::LoadCamCalibFromYaml(const std::string& filename) {
  YAML::Node node = deserialize(filename);
  this->device_id = "";
  if (node["DeviceId"]) {
    this->device_id = node["DeviceId"].as<std::string>();
  }
  this->sensor_type = SensorType::UNKNOWN;
  if (node["SensorType"]) {
    std::string type = node["SensorType"].as<std::string>();
    if (type == "LI") {
      this->sensor_type = SensorType::LI;
    } else if (type == "XP") {
      this->sensor_type = SensorType::XP;
    } else if (type == "XP2") {
      this->sensor_type = SensorType::XP2;
    } else if (type == "XP3") {
      this->sensor_type = SensorType::XP3;
    } else if (type == "UNKNOWN") {
      this->sensor_type = SensorType::UNKNOWN;
    } else {
      LOG(ERROR) << "sensor_type in yaml " << type << " is illegal";
      return false;
    }
  }
  if (!node["Camera"].IsDefined()) {
    return false;
  }
  this->Camera = node["Camera"].as<Camera_t>();
  // generate undistort map
  CHECK(this->initUndistortMap(this->Camera.img_size));
  return true;
}

bool DuoCalibParam::WriteToCvYaml(const std::string& filename) {
  cv::FileStorage cvfs(filename, cv::FileStorage::WRITE | cv::FileStorage::FORMAT_YAML);
  if (!cvfs.isOpened()) {
    LOG(ERROR) << "Can't open file: " << filename;
    return false;
  }
  // device id
  cvfs << "DeviceId" << device_id;
  // sensor type
  switch (sensor_type) {
    case XP::DuoCalibParam::SensorType::UNKNOWN:
      cvfs << "SensorType" << "UNKNOWN"; break;
    case XP::DuoCalibParam::SensorType::LI:
      cvfs << "SensorType" << "LI"; break;
    case XP::DuoCalibParam::SensorType::XP:
      cvfs << "SensorType" << "XP"; break;
    case XP::DuoCalibParam::SensorType::XP2:
      cvfs << "SensorType" << "XP2"; break;
    case XP::DuoCalibParam::SensorType::XP3:
      cvfs << "SensorType" << "XP3"; break;
    default:
      LOG(ERROR) << "sensor type invalid";
      return -1;
  }
  double param_buffer[4 * 4 * sizeof(double)];
  // IMU
  // accel_TK
  cv::Mat accel_TK(3, 3, CV_64FC1, reinterpret_cast<void*>(param_buffer));
  float* tmp_ptr = Imu.accel_TK.data();
  for (int i = 0; i < 9; ++i) {
    param_buffer[i] = static_cast<double>(*tmp_ptr++);
  }
  cvfs << "accel_TK" << accel_TK;

  // accel_bias
  cv::Mat accel_bias(3, 1, CV_64FC1, reinterpret_cast<void*>(param_buffer));
  tmp_ptr = Imu.accel_bias.data();
  for (int i = 0; i < 3; ++i) {
    param_buffer[i] = static_cast<double>(*tmp_ptr++);
  }
  cvfs << "accel_bias" << accel_bias;

  // gyro_TK
  cv::Mat gyro_TK(3, 3, CV_64FC1, reinterpret_cast<void*>(param_buffer));
  tmp_ptr = Imu.gyro_TK.data();
  for (int i = 0; i < 9; ++i) {
    param_buffer[i] = static_cast<double>(*tmp_ptr++);
  }
  cvfs << "gyro_TK" << gyro_TK;

  // gyro_bias
  cv::Mat gyro_bias(3, 1, CV_64FC1, reinterpret_cast<void*>(param_buffer));
  tmp_ptr = Imu.gyro_bias.data();
  for (int i = 0; i < 3; ++i) {
    param_buffer[i] = static_cast<double>(*tmp_ptr++);
  }
  cvfs << "gyro_bias" << gyro_bias;

  // accel_noise_var
  cv::Mat accel_noise_var(3, 1, CV_64FC1, reinterpret_cast<void*>(param_buffer));
  tmp_ptr = Imu.accel_noise_var.data();
  for (int i = 0; i < 3; ++i) {
    param_buffer[i] = static_cast<double>(*tmp_ptr++);
  }
  cvfs << "accel_noise_var" << accel_noise_var;

  // angv_noise_var
  cv::Mat angv_noise_var(3, 1, CV_64FC1, reinterpret_cast<void*>(param_buffer));
  tmp_ptr = Imu.angv_noise_var.data();
  for (int i = 0; i < 3; ++i) {
    param_buffer[i] = static_cast<double>(*tmp_ptr++);
  }
  cvfs << "angv_noise_var" << angv_noise_var;

  // D_T_I
  cv::Mat D_T_I(4, 4, CV_64FC1, reinterpret_cast<void*>(param_buffer));
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      param_buffer[i * 4 + j] = static_cast<double>(Imu.D_T_I(i, j));
    }
  }
  cvfs << "D_T_I" << D_T_I;

  // CAMERA
  cv::Mat d_t_c_l(4, 4, CV_64FC1, reinterpret_cast<void*>(param_buffer));
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      param_buffer[i * 4 + j] = static_cast<double>(Camera.D_T_C_lr[0](i, j));
    }
  }
  cvfs << "D_T_C_l" << d_t_c_l;

  cv::Mat d_t_c_r(4, 4, CV_64FC1, reinterpret_cast<void*>(param_buffer));
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      param_buffer[i * 4 + j] = static_cast<double>(Camera.D_T_C_lr[1](i, j));
    }
  }
  cvfs << "D_T_C_r" << d_t_c_r;

  cv::Mat camera_l(3, 3, CV_64FC1, reinterpret_cast<void*>(param_buffer));
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      param_buffer[i * 3 + j] = static_cast<double>(Camera.cameraK_lr[0](i, j));
    }
  }
  cvfs << "cameraK_l" << camera_l;

  cv::Mat camera_r(3, 3, CV_64FC1, reinterpret_cast<void*>(param_buffer));
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      param_buffer[i * 3 + j] = static_cast<double>(Camera.cameraK_lr[1](i, j));
    }
  }
  cvfs << "cameraK_r" << camera_r;

  cv::Mat dist_coeff_l(8, 1, CV_64FC1, reinterpret_cast<void*>(param_buffer));
  tmp_ptr = reinterpret_cast<float*>(Camera.cv_dist_coeff_lr[0].data);
  for (int i = 0; i < 8; ++i) {
    param_buffer[i] = static_cast<double>(*tmp_ptr++);
  }
  cvfs << "dist_coeff_l" << dist_coeff_l;

  cv::Mat dist_coeff_r(8, 1, CV_64FC1, reinterpret_cast<void*>(param_buffer));
  tmp_ptr = reinterpret_cast<float*>(Camera.cv_dist_coeff_lr[1].data);
  for (int i = 0; i < 8; ++i) {
    param_buffer[i] = static_cast<double>(*tmp_ptr++);
  }
  cvfs << "dist_coeff_r" << dist_coeff_r;
  cvfs << "img_size" << Camera.img_size;
  cvfs.release();
  return true;
}

bool DuoCalibParam::WriteToYaml(const std::string& filename) {
  YAML::Emitter emitter;
  emitter << YAML::BeginMap;
  emitter << YAML::Key << "DeviceId"
          << YAML::Value << this->device_id;
  if (this->sensor_type == SensorType::UNKNOWN) {
    emitter << YAML::Key << "SensorType"
            << YAML::Value << "UNKNOWN";
  } else if (this->sensor_type == SensorType::LI) {
    emitter << YAML::Key << "SensorType"
            << YAML::Value << "LI";
  } else if (this->sensor_type == SensorType::XP) {
    emitter << YAML::Key << "SensorType"
            << YAML::Value << "XP";
  } else if (this->sensor_type == SensorType::XP2) {
    emitter << YAML::Key << "SensorType"
            << YAML::Value << "XP2";
  } else if (this->sensor_type == SensorType::XP3) {
    emitter << YAML::Key << "SensorType"
            << YAML::Value << "XP3";
  } else {
    LOG(ERROR) << "this->sensor_type == "
               << this->sensor_type << " not supported";
    return false;
  }
  emitter << YAML::Key << "Imu"
          << YAML::Value << this->Imu;
  emitter << YAML::Key << "Camera"
          << YAML::Value << this->Camera;
  emitter << YAML::EndMap;
  serialize(filename, emitter);
  return true;
}

// init undistort map from camera K and distort
bool DuoCalibParam::initUndistortMap(const cv::Size& new_img_size) {
  // compute the boundary of images in uv coordinate
  this->Camera.lurd_lr.resize(2);
  //遍历左右相机
  for (int lr = 0; lr < 2; ++lr)
  {
      std::vector<cv::Point2f> lurd(4);
      //图像左边缘中心点
      lurd[0].x = 0;
      lurd[0].y = this->Camera.img_size.height / 2;
      //图像上边缘中心点
      lurd[1].x = this->Camera.img_size.width / 2;
      lurd[1].y = 0;
      //图像右边缘中心点
      lurd[2].x = this->Camera.img_size.width;
      lurd[2].y = this->Camera.img_size.height / 2;
      //图像下边缘中心点
      lurd[3].x = this->Camera.img_size.width / 2;
      lurd[3].y = this->Camera.img_size.height;
      std::vector<cv::Point2f> lurd_undistort(4);
      //去畸变
      cv::undistortPoints(lurd, lurd_undistort,
                          this->Camera.cv_camK_lr[lr],
                          this->Camera.cv_dist_coeff_lr[lr]);
      this->Camera.lurd_lr[lr][0] = lurd_undistort[0].x;
      this->Camera.lurd_lr[lr][1] = lurd_undistort[1].y;
      this->Camera.lurd_lr[lr][2] = lurd_undistort[2].x;
      this->Camera.lurd_lr[lr][3] = lurd_undistort[3].y;
  }
  this->Camera.undistort_map_op1_lr.resize(2);
  this->Camera.undistort_map_op2_lr.resize(2);
  //左右目的校正变换矩阵
  vector<cv::Mat> R(2);
  //左右目在新的映射下的投影矩阵
  vector<cv::Mat> P(2);
  Matrix4f C0_T_C1 = this->Camera.D_T_C_lr[0].inverse() * this->Camera.D_T_C_lr[1];//Tc0c1
  Matrix4f C1_T_C0 = C0_T_C1.inverse();//Tc1c0
  //cv形式为了立体矫正
  cv::Mat C0_R_C1(3, 3, CV_64F);
  cv::Mat C1_R_C0(3, 3, CV_64F);
  cv::Mat C1_t_C0(3, 1, CV_64F);
  cv::Mat C0_t_C1(3, 1, CV_64F);
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      C0_R_C1.at<double>(i, j) = C0_T_C1(i, j);
      C1_R_C0.at<double>(i, j) = C1_T_C0(i, j);
    }
    C0_t_C1.at<double>(i, 0) = C0_T_C1(i, 3);
    C1_t_C0.at<double>(i, 0) = C1_T_C0(i, 3);
  }
  cv::Matx44d Q;
  //立体矫正
  cv::stereoRectify(this->Camera.cv_camK_lr[0],
                    this->Camera.cv_dist_coeff_lr[0],
                    this->Camera.cv_camK_lr[1],
                    this->Camera.cv_dist_coeff_lr[1],
                    this->Camera.img_size,
                    C1_R_C0, C1_t_C0,
                    R[0], R[1],
                    P[0], P[1],
                    Q, cv::CALIB_ZERO_DISPARITY, 0, new_img_size);
  //设置一下视差-深度映射矩阵
  this->Camera.Q = cv::Matx44f(Q);
  this->Camera.cv_undist_K_lr.resize(2);
  this->Camera.undist_D_T_C_lr.resize(2);

  //设置左右两目畸变矫正后的映射,以及设置新的内参,外参
  for (int lr = 0; lr < 2; lr++)
  {
      //新的内参
    cv::Mat newK(3, 3, CV_32F);
    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        // note P[lr] doesn't work
        newK.at<float>(i, j) = P[lr].at<double>(i, j);
      }
    }
    cv::initUndistortRectifyMap(this->Camera.cv_camK_lr[lr],
                                this->Camera.cv_dist_coeff_lr[lr],
                                R[lr],
                                newK,  // this->Camera.cv_camK_lr[lr],  // newK
                                new_img_size,
                                CV_32FC1,
                                this->Camera.undistort_map_op1_lr[lr],
                                this->Camera.undistort_map_op2_lr[lr]);
    this->Camera.cv_undist_K_lr[lr] = cv::Matx33f(newK);
    this->Camera.undist_D_T_C_lr[lr].setIdentity();
  }

  // Compute the new *rectified* extrinsics
  // In undist_D_T_C_lr, we assume D_T_C_l is identity, which means,
  // the device {D} is *moved* to the new C_l (R[0] is actually Cl_new_R_Cl)
  // Therefore, D_T_I needs to be adjusted as well to undist_D_T_I.
  //这里设备坐标系被移动到了新的双目矫正后的坐标系下,所以需要重新计算tdc1
  cv::Mat new_C0_t_C1 = R[0] * C0_t_C1;
  this->Camera.undist_D_T_C_lr[1](0, 3) = new_C0_t_C1.at<double>(0);
  this->Camera.undist_D_T_C_lr[1](1, 3) = new_C0_t_C1.at<double>(1);
  this->Camera.undist_D_T_C_lr[1](2, 3) = new_C0_t_C1.at<double>(2);
  //旧的设备坐标系到新的设备坐标系下的变换
  Eigen::Matrix4f new_D_T_D = Eigen::Matrix4f::Identity();
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      new_D_T_D(i, j) = static_cast<float>(R[0].at<double>(i, j));
    }
  }
  //设置一下新的imu到设备坐标系的变换
  this->Imu.undist_D_T_I = new_D_T_D * this->Imu.D_T_I;
  return true;
}
bool get_calib_file_from_device_id(const std::string& device_id,
                                   std::string* calib_file_ptr) {
  CHECK_NOTNULL(calib_file_ptr);
  // todo: auto-parse device_id from string...
  std::list<std::string> known_device_ids;
  known_device_ids.push_back(std::string("93312E057D22"));  // this one has an IR pass filter
  known_device_ids.push_back(std::string("8219C930E5FC"));
  known_device_ids.push_back(std::string("DFCDC2D10CD9"));
  known_device_ids.push_back(std::string("BAAFF337329A"));
  known_device_ids.push_back(std::string("BDC4FA571539"));
  known_device_ids.push_back(std::string("7E92FAF26363"));
  known_device_ids.push_back(std::string("8E1BC83F0421"));
  known_device_ids.push_back(std::string("4924E006C82A"));
  known_device_ids.push_back(std::string("054AC6A9044F"));
  known_device_ids.push_back(std::string("408C392FF83F"));
  known_device_ids.push_back(std::string("505FC7C0497A"));
  known_device_ids.push_back(std::string("A72F4DA6A92D"));
  known_device_ids.push_back(std::string("FC329CAD406E"));
  known_device_ids.push_back(std::string("XP0000001"));
  char* p_path;
  p_path = getenv("MASTER_DIR");
  CHECK_NOTNULL(p_path);
  std::string dir_path(p_path);
  dir_path += "/calib_params/";
  for (const auto& known_device_id : known_device_ids) {
    if (device_id.find(known_device_id) != std::string::npos) {
      *calib_file_ptr = dir_path + known_device_id + ".calib.yaml";
      return true;
    }
  }
  return false;
}

bool load_imu_calib_param(const std::string& device_id,
                          DuoCalibParam* duo_calib_param_ptr) {
  CHECK_NOTNULL(duo_calib_param_ptr);
  std::string calib_file;
  if (!get_calib_file_from_device_id(device_id, &calib_file)) {
    LOG(ERROR) << "unknown device id: " << device_id;
    return false;
  }
  return duo_calib_param_ptr->LoadFromYaml(calib_file);
}

//读取相机以及imu的参数,同时对双目做畸变矫正,去畸变映射
bool load_camera_calib_param(const std::string& calib_file,
                             DuoCalibParam* duo_calib_param_ptr) {
  cv::FileStorage calib_fs(calib_file, cv::FileStorage::READ);
  if (!calib_fs.isOpened()) {
    return false;
  }
  CHECK_NOTNULL(duo_calib_param_ptr);
  XP::DuoCalibParam& duo_calib_param = *duo_calib_param_ptr;
  cv::Mat K_l, K_r, dist_l, dist_r, R_l, t_l;
  calib_fs["K_l"] >> K_l;
  calib_fs["K_r"] >> K_r;
  CHECK_EQ(K_l.type(), CV_32F);
  CHECK_EQ(K_r.type(), CV_32F);
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      duo_calib_param.Camera.cameraK_lr[0](i, j) = K_l.at<float>(i, j);
      duo_calib_param.Camera.cameraK_lr[1](i, j) = K_r.at<float>(i, j);
      duo_calib_param.Camera.cv_camK_lr[0](i, j) = K_l.at<float>(i, j);
      duo_calib_param.Camera.cv_camK_lr[1](i, j) = K_r.at<float>(i, j);
    }
  }
  calib_fs["dist_l"] >> dist_l;
  calib_fs["dist_r"] >> dist_r;
  CHECK_EQ(dist_l.type(), CV_32F);
  CHECK_EQ(dist_l.type(), CV_32F);
  duo_calib_param.Camera.cv_dist_coeff_lr[0].create(dist_l.rows, 1);
  for (int i = 0; i < dist_l.rows; i++) {
    duo_calib_param.Camera.cv_dist_coeff_lr[0](i) = dist_l.at<float>(i);
  }
  duo_calib_param.Camera.cv_dist_coeff_lr[1].create(dist_r.rows, 1);
  for (int i = 0; i < dist_r.rows; i++) {
    duo_calib_param.Camera.cv_dist_coeff_lr[1](i) = dist_r.at<float>(i);
  }
  cv::Mat Cl_T_Cr;
  calib_fs["Cl_T_Cr"] >> Cl_T_Cr;
  if (Cl_T_Cr.rows != 0) {
    CHECK_EQ(Cl_T_Cr.type(), CV_32F);
    for (int i = 0; i < 4; ++i) {
      for (int j = 0; j < 4; ++j) {
        duo_calib_param.Camera.D_T_C_lr[1](i, j) = Cl_T_Cr.at<float>(i, j);
      }
    }
  } else {
    // calib file is saved by reading duo mlx
    calib_fs["R_l"] >> R_l;
    calib_fs["t_l"] >> t_l;
    CHECK_EQ(R_l.type(), CV_32F);
    CHECK_EQ(R_l.rows, 3);
    CHECK_EQ(R_l.cols, 3);
    CHECK_EQ(t_l.type(), CV_32F);
    // This is needed by 1.0.50 driver
    // For new log recorded by 1.0.80 driver, Rl and tl is no longer used
    R_l = R_l.t();
    Eigen::Matrix4f Cr_T_D = Eigen::Matrix4f::Identity();
    for (int i = 0; i < 3; i++) {
      for (int j = 0; j < 3; j++) {
        Cr_T_D(i, j) = R_l.at<float>(i, j);
      }
      Cr_T_D(i, 3) = t_l.at<float>(i, 0) / 1e3;
    }
    duo_calib_param.Camera.D_T_C_lr[1] = Cr_T_D.inverse();
  }
  CHECK_NEAR(duo_calib_param.Camera.D_T_C_lr[1](3, 0), 0, 1e-5);
  CHECK_NEAR(duo_calib_param.Camera.D_T_C_lr[1](3, 1), 0, 1e-5);
  CHECK_NEAR(duo_calib_param.Camera.D_T_C_lr[1](3, 2), 0, 1e-5);
  CHECK_NEAR(duo_calib_param.Camera.D_T_C_lr[1](3, 3), 1, 1e-5);
  VLOG(1) << "duo_calib_param.Camera.D_T_C_lr[1] " << duo_calib_param.Camera.D_T_C_lr[1];
  // load size
  if (calib_fs["img_size"].isNone()) {
    duo_calib_param.Camera.img_size = cv::Size(752, 480);  // default param
    std::cout << "No img size info is found. Using default "
              << duo_calib_param.Camera.img_size << std::endl;
  } else {
    calib_fs["img_size"] >> duo_calib_param.Camera.img_size;
  }
  // generate undistort map
  //双目矫正&&去畸变映射
  CHECK(duo_calib_param.initUndistortMap(duo_calib_param.Camera.img_size));
  return true;
}
bool save_camera_calib_param(const std::string& calib_file,
                             const DuoCalibParam& duo_calib_param) {
  cv::FileStorage calib_fs(calib_file, cv::FileStorage::WRITE);
  if (!calib_fs.isOpened()) {
    return false;
  }
  cv::Mat K_l, K_r, dist_l, dist_r, R_l, t_l;
  calib_fs << "K_l" << cv::Mat(duo_calib_param.Camera.cv_camK_lr[0]);
  calib_fs << "dist_l" << cv::Mat(duo_calib_param.Camera.cv_dist_coeff_lr[0]);
  calib_fs << "K_r" << cv::Mat(duo_calib_param.Camera.cv_camK_lr[1]);
  calib_fs << "dist_r" << cv::Mat(duo_calib_param.Camera.cv_dist_coeff_lr[0]);
  cv::Mat Cl_T_Cr(4, 4, CV_32F);
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      Cl_T_Cr.at<float>(i, j) = duo_calib_param.Camera.D_T_C_lr[1](i, j);
    }
  }
  calib_fs << "Cl_T_Cr" << Cl_T_Cr;
  calib_fs << "img_size" << duo_calib_param.Camera.img_size;
  return true;
}

}  // namespace XP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_FDLIBM_FUNCTIONS_SCALAR_MODF_HPP_INCLUDED
#define NT2_TOOLBOX_FDLIBM_FUNCTIONS_SCALAR_MODF_HPP_INCLUDED
#include <nt2/toolbox/fdlibm/functions/modf.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <boost/fusion/include/at.hpp>
#include <boost/fusion/include/vector.hpp>
#include <nt2/sdk/meta/strip.hpp>
  extern "C"{
    extern double fd_modf ( double,double* );
  }
/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is double_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION(nt2::fdlibm::tag::modf_, tag::cpu_,
                      (A0),
                      (scalar_<double_<A0> > )
	              (scalar_<double_<A0> > )
                     )
  {
    typedef A0       result_type;

    inline result_type operator()(A0 const& a0,A0 & a1) const
    {
      return ::fd_modf(a0, &a1);
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::fdlibm::tag::modf_, tag::cpu_,
                      (A0),
                      (scalar_<double_<A0> > )
                     )
  {
    typedef boost::fusion::vector<A0, A0>        result_type;
    NT2_FUNCTOR_CALL(1)
    {
      result_type res;
      boost::fusion::at_c<0>(res) = nt2::fdlibm::modf(a0, boost::fusion::at_c<1>(res));
      return res;
    }
  };
} }
#endif


// Boost.Geometry
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2015 Adam Wulkiewicz, Lodz, Poland.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[ring
//` Declaration and use of the Boost.Geometry model::ring, modelling the Ring Concept

#include <iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>

namespace bg = boost::geometry;

int main()
{
    typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
    typedef bg::model::ring<point_t> ring_t; /*< Default parameters, clockwise, closed ring. >*/

    ring_t ring1; /*< Default-construct a ring. >*/

#if !defined(BOOST_NO_CXX11_UNIFIED_INITIALIZATION_SYNTAX) \
 && !defined(BOOST_NO_CXX11_HDR_INITIALIZER_LIST)

    ring_t ring2{{0.0, 0.0}, {0.0, 5.0}, {5.0, 5.0}, {5.0, 0.0}, {0.0, 0.0}}; /*< Construct a ring containing four points plus one closing point, using C++11 unified initialization syntax. >*/

#endif

    bg::append(ring1, point_t(0.0, 0.0)); /*< Append point. >*/
    bg::append(ring1, point_t(0.0, 5.0));
    bg::append(ring1, point_t(5.0, 5.0));
    bg::append(ring1, point_t(5.0, 0.0));
    bg::append(ring1, point_t(0.0, 0.0));

    double a = bg::area(ring1);

    std::cout << a << std::endl;

    return 0;
}

//]

//[ring_output
/*`
Output:
[pre
25
]
*/
//]


#include "grey_wolf_optimizer.hpp"
#include "particle_swarm_optimization.hpp"
#include "benchmark_functions.hpp"
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>

BOOST_PYTHON_MODULE(optimization)
{
    using namespace boost::python;

    class_<std::vector<double>>("vector_double")
        .def(vector_indexing_suite<std::vector<double>>());

    enum_<optimization::function>("function")
        .value("bent_cigar", optimization::function::bent_cigar)
        .value("rosenbrock", optimization::function::rosenbrock)
        .value("rastrigin", optimization::function::rastrigin)
        .value("zakharov", optimization::function::zakharov);

    class_<optimization::solution>("solution")
        .def_readonly("convergence", &optimization::solution::convergence)
        .def_readonly("time_of_execution", &optimization::solution::time_of_execution)
        .def_readonly("best", &optimization::solution::best);

    def("bent_cigar", &optimization::bent_cigar_pywrapper);
    def("rosenbrock", &optimization::rosenbrock_pywrapper);
    def("rastrigin", &optimization::rastrigin_pywrapper);
    def("zakharov", &optimization::zakharov_pywrapper);

    enum_<optimization::calculation_type>("calculation_type")
        .value("lin", optimization::calculation_type::lin)
        .value("exp", optimization::calculation_type::exp)
        .value("rand", optimization::calculation_type::rand);

    def("grey_wolf_optimizer", &optimization::grey_wolf_optimizer);
    def("particle_swarm_optimization", &optimization::particle_swarm_optimization);
}


/**  \file mcmc_sampler.hpp \brief Markov Chain Monte Carlo algorithms */
/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#ifndef  _MCMC_SAMPLER_HPP_
#define  _MCMC_SAMPLER_HPP_

#include <boost/scoped_ptr.hpp>
#include "randgen.hpp"
#include "optimizable.hpp"

namespace bayesopt {

  // We plan to add more in the future
  typedef enum {
    SLICE_MCMC           ///< Slice sampling
  } McmcAlgorithms;

  /**
   * \brief Markov Chain Monte Carlo sampler
   *
   * It generates a set of particles that are distributed according to
   * an arbitrary pdf. IMPORTANT: As it should be a replacement for
   * the optimization (ML or MAP) estimation, it also assumes a
   * NEGATIVE LOG PDF.
   *
   * @see NLOPT_Optimization
   */
  class MCMCSampler
  {
  public:
    /**
     * \brief Constructor (Note: default constructor is private)
     *
     * @param rbo point to RBOptimizable type of object with the PDF
     *            to sample from. IMPORTANT: We assume that the
     *            evaluation of rbo is the NEGATIVE LOG PDF.
     * @param dim number of input dimensions
     * @param eng random number generation engine (boost)
     */
    MCMCSampler(RBOptimizable* rbo, size_t dim, randEngine& eng);
    virtual ~MCMCSampler();

    /** Sets the sampling algorithm (slice, MH, etc.) */
    void setAlgorithm(McmcAlgorithms newAlg);

    /** Sets the number of particles that are stored */
    void setNParticles(size_t nParticles);

    /**Usually, the initial samples of any MCMC method are biased and
     *	they are discarded. This phase is called the burnout. This
     *	method sets the number of particles to be discarded
     */
    void setNBurnOut(size_t nParticles);

    /** Compute the set of particles according to the target PDF.
     * @param Xnext input: initial point of the Markov Chain,
     *              output: last point of the Markov Chain
     */
    void run(vectord &Xnext);

    vectord getParticle(size_t i);

    void printParticles();

  private:
    void randomJump(vectord &x);
    void burnOut(vectord &x);
    void sliceSample(vectord &x);

    boost::scoped_ptr<RBOptimizableWrapper> obj;

    McmcAlgorithms mAlg;
    size_t mDims;
    size_t nBurnOut;
    size_t nSamples;
    bool mStepOut;

    vectord mSigma;
    vecOfvec mParticles;
    randEngine& mtRandom;

  private: //Forbidden
    MCMCSampler();
    MCMCSampler(MCMCSampler& copy);
  };

  inline void MCMCSampler::setAlgorithm(McmcAlgorithms newAlg)
  { mAlg = newAlg; };

  inline void MCMCSampler::setNParticles(size_t nParticles)
  { nSamples = nParticles; };

  inline void MCMCSampler::setNBurnOut(size_t nParticles)
  { nBurnOut = nParticles; };

  inline vectord MCMCSampler::getParticle(size_t i)
  { return mParticles[i]; };

  inline void MCMCSampler::printParticles()
  {
    for(size_t i=0; i<mParticles.size(); ++i)
      {
	FILE_LOG(logDEBUG) << i << "->" << mParticles[i]
			   << " | Log-lik " << -obj->evaluate(mParticles[i]);
      }
  }

} //namespace bayesopt

#endif