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

/*
 Copyright (C) 2008 Jose Aparicio

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

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

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/quantlib.hpp>

#include <boost/timer.hpp>
#include <iostream>
#include <iomanip>

using namespace std;
using namespace QuantLib;

#ifdef BOOST_MSVC
#  ifdef QL_ENABLE_THREAD_SAFE_OBSERVER_PATTERN
#    include <ql/auto_link.hpp>
#    define BOOST_LIB_NAME boost_system
#    include <boost/config/auto_link.hpp>
#    undef BOOST_LIB_NAME
#    define BOOST_LIB_NAME boost_thread
#    include <boost/config/auto_link.hpp>
#    undef BOOST_LIB_NAME
#  endif
#endif

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {

    Integer sessionId() { return 0; }

}
#endif

int main(int, char* []) {

    try {

        boost::timer timer;
        std::cout << std::endl;

        /*********************
         ***  MARKET DATA  ***
         *********************/

        Calendar calendar = TARGET();
        Date todaysDate(15, May, 2007);
        // must be a business day
        todaysDate = calendar.adjust(todaysDate);

        Settings::instance().evaluationDate() = todaysDate;

        // dummy curve
        boost::shared_ptr<Quote> flatRate(new SimpleQuote(0.01));
        Handle<YieldTermStructure> tsCurve(
              boost::shared_ptr<FlatForward>(
                      new FlatForward(todaysDate, Handle<Quote>(flatRate),
                                      Actual365Fixed())));

        /*
          In Lehmans Brothers "guide to exotic credit derivatives"
          p. 32 there's a simple case, zero flat curve with a flat CDS
          curve with constant market spreads of 150 bp and RR = 50%
          corresponds to a flat 3% hazard rate. The implied 1-year
          survival probability is 97.04% and the 2-years is 94.18%
        */

        // market
        Real recovery_rate = 0.5;
        Real quoted_spreads[] = { 0.0150, 0.0150, 0.0150, 0.0150 };
        vector<Period> tenors;
        tenors.push_back(3*Months);
        tenors.push_back(6*Months);
        tenors.push_back(1*Years);
        tenors.push_back(2*Years);
        vector<Date> maturities;
        for (Size i=0; i<4; i++) {
            maturities.push_back(calendar.adjust(todaysDate + tenors[i],
                                                 Following));
        }

        std::vector<boost::shared_ptr<DefaultProbabilityHelper> > instruments;
        for (Size i=0; i<4; i++) {
            instruments.push_back(boost::shared_ptr<DefaultProbabilityHelper>(
                new SpreadCdsHelper(
                              Handle<Quote>(boost::shared_ptr<Quote>(
                                         new SimpleQuote(quoted_spreads[i]))),
                              tenors[i],
                              0,
                              calendar,
                              Quarterly,
                              Following,
                              DateGeneration::TwentiethIMM,
                              Actual365Fixed(),
                              recovery_rate,
                              tsCurve)));
        }

        // Bootstrap hazard rates
        boost::shared_ptr<PiecewiseDefaultCurve<HazardRate, BackwardFlat> >
           hazardRateStructure(
               new PiecewiseDefaultCurve<HazardRate, BackwardFlat>(
                                                           todaysDate,
                                                           instruments,
                                                           Actual365Fixed()));
        vector<pair<Date, Real> > hr_curve_data = hazardRateStructure->nodes();

        cout << "Calibrated hazard rate values: " << endl ;
        for (Size i=0; i<hr_curve_data.size(); i++) {
            cout << "hazard rate on " << hr_curve_data[i].first << " is "
                 << hr_curve_data[i].second << endl;
        }
        cout << endl;

        cout << "Some survival probability values: " << endl ;
        cout << "1Y survival probability: "
             << io::percent(hazardRateStructure->survivalProbability(
                                                        todaysDate + 1*Years))
             << endl
             << "               expected: "
             << io::percent(0.9704)
             << endl;
        cout << "2Y survival probability: "
             << io::percent(hazardRateStructure->survivalProbability(
                                                        todaysDate + 2*Years))
             << endl
             << "               expected: "
             << io::percent(0.9418)
             << endl;

        cout << endl << endl;

        // reprice instruments
        Real nominal = 1000000.0;
        Handle<DefaultProbabilityTermStructure> probability(hazardRateStructure);
        boost::shared_ptr<PricingEngine> engine(
                  new MidPointCdsEngine(probability, recovery_rate, tsCurve));

        Schedule cdsSchedule =
            MakeSchedule().from(todaysDate).to(maturities[0])
                          .withFrequency(Quarterly)
                          .withCalendar(calendar)
                          .withTerminationDateConvention(Unadjusted)
                          .withRule(DateGeneration::TwentiethIMM);
        CreditDefaultSwap cds_3m(Protection::Seller,
                                 nominal,
                                 quoted_spreads[0],
                                 cdsSchedule,
                                 Following,
                                 Actual365Fixed());

        cdsSchedule =
            MakeSchedule().from(todaysDate).to(maturities[1])
                          .withFrequency(Quarterly)
                          .withCalendar(calendar)
                          .withTerminationDateConvention(Unadjusted)
                          .withRule(DateGeneration::TwentiethIMM);
        CreditDefaultSwap cds_6m(Protection::Seller,
                                 nominal,
                                 quoted_spreads[1],
                                 cdsSchedule,
                                 Following,
                                 Actual365Fixed());

        cdsSchedule =
            MakeSchedule().from(todaysDate).to(maturities[2])
                          .withFrequency(Quarterly)
                          .withCalendar(calendar)
                          .withTerminationDateConvention(Unadjusted)
                          .withRule(DateGeneration::TwentiethIMM);
        CreditDefaultSwap cds_1y(Protection::Seller,
                                 nominal,
                                 quoted_spreads[2],
                                 cdsSchedule,
                                 Following,
                                 Actual365Fixed());

        cdsSchedule =
            MakeSchedule().from(todaysDate).to(maturities[3])
                          .withFrequency(Quarterly)
                          .withCalendar(calendar)
                          .withTerminationDateConvention(Unadjusted)
                          .withRule(DateGeneration::TwentiethIMM);
        CreditDefaultSwap cds_2y(Protection::Seller,
                                 nominal,
                                 quoted_spreads[3],
                                 cdsSchedule,
                                 Following,
                                 Actual365Fixed());

        cds_3m.setPricingEngine(engine);
        cds_6m.setPricingEngine(engine);
        cds_1y.setPricingEngine(engine);
        cds_2y.setPricingEngine(engine);

        cout << "Repricing of quoted CDSs employed for calibration: " << endl;
        cout << "3M fair spread: " << io::rate(cds_3m.fairSpread()) << endl
             << "   NPV:         " << cds_3m.NPV() << endl
             << "   default leg: " << cds_3m.defaultLegNPV() << endl
             << "   coupon leg:  " << cds_3m.couponLegNPV() << endl
             << endl;

        cout << "6M fair spread: " << io::rate(cds_6m.fairSpread()) << endl
             << "   NPV:         " << cds_6m.NPV() << endl
             << "   default leg: " << cds_6m.defaultLegNPV() << endl
             << "   coupon leg:  " << cds_6m.couponLegNPV() << endl
             << endl;

        cout << "1Y fair spread: " << io::rate(cds_1y.fairSpread()) << endl
             << "   NPV:         " << cds_1y.NPV() << endl
             << "   default leg: " << cds_1y.defaultLegNPV() << endl
             << "   coupon leg:  " << cds_1y.couponLegNPV() << endl
             << endl;

        cout << "2Y fair spread: " << io::rate(cds_2y.fairSpread()) << endl
             << "   NPV:         " << cds_2y.NPV() << endl
             << "   default leg: " << cds_2y.defaultLegNPV() << endl
             << "   coupon leg:  " << cds_2y.couponLegNPV() << endl
             << endl;

        cout << endl << endl;

        Real seconds  = timer.elapsed();
        Integer hours = Integer(seconds/3600);
        seconds -= hours * 3600;
        Integer minutes = Integer(seconds/60);
        seconds -= minutes * 60;
        cout << "Run completed in ";
        if (hours > 0)
            cout << hours << " h ";
        if (hours > 0 || minutes > 0)
            cout << minutes << " m ";
        cout << fixed << setprecision(0)
             << seconds << " s" << endl;

        return 0;
    } catch (exception& e) {
        cerr << e.what() << endl;
        return 1;
    } catch (...) {
        cerr << "unknown error" << endl;
        return 1;
    }
}


//
//  IntervalBounder.h
//  YRoots
//
//  Created by Erik Hales Parkinson on 8/14/21.
//  Copyright © 2021 Erik Hales Parkinson. All rights reserved.
//

#ifndef IntervalBounder_h
#define IntervalBounder_h

#include "Approximation/ChebyshevApproximation.hpp"
#include "Utilities/Timer.hpp"
#include "Utilities/utilities.hpp"
#include <Eigen/Dense>

#include "IntervalChecking/BoundingIntervalUtilities.hpp"

template <int Rank>
class IntervalBounder {
public:
    IntervalBounder(size_t _rank);
    double computeBoundingInterval(std::vector<ChebyshevApproximation<Rank> >& _chebyshevApproximations, const std::vector<bool>& _allowedToReduceDim);

    const Interval& getBoundingInterval() {
        return m_boundingInterval;
    }

protected:
    double updateBoundingIntervalLinearErrorSolve(std::vector<ChebyshevApproximation<Rank> >& _chebyshevApproximations, const std::vector<bool>& _allowedToReduceDim);
    double updateBoundingIntervalLipshitzSolve(std::vector<ChebyshevApproximation<Rank> >& _chebyshevApproximations, const std::vector<bool>& _allowedToReduceDim);

    void preconditionPolynomials(std::vector<ChebyshevApproximation<Rank> >& _chebyshevApproximations);

    double computeLipshitzConstant(const Eigen::VectorXd& poly, size_t dim);
    void chebValReduce(const Eigen::VectorXd& poly, Eigen::VectorXd& result, size_t dim, double value);
    double getLiphsitzBoundIncreaseND(const Eigen::VectorXd& poly, double error, double lipshitzConstant, const Interval& boundingInterval, size_t dim);
    double getExtremeAbsVal(const Eigen::VectorXd& poly, const Interval& boundingInterval, size_t dim);
protected:
    size_t                  m_rank;
    Interval                m_boundingInterval;

    //For Bounding Intervals
    typename EigenTypes<Rank>::Matrix m_linearTerms;
    typename EigenTypes<Rank>::Vector m_constantTerms;
    typename EigenTypes<Rank>::Vector m_errorTerms;
    typename EigenTypes<Rank>::Vector m_errorTermsOfLinear;
    typename EigenTypes<Rank>::MatrixPowerColumns m_rightHandSideOfLinearSystemErrors;
    typename Eigen::ColPivHouseholderQR<typename EigenTypes<Rank>::Matrix> m_linearTermsQR;
    typename EigenTypes<Rank>::MatrixPowerColumns m_linearSystemWithErrorResult;
    typename EigenTypes<Rank>::Matrix m_linearTermsInverse;

    //The Preconditioned Polynomials
    size_t m_preconditionPolysDegree;
    typename EigenTypes<Rank>::Vector m_preconditionedErrors;
    Eigen::MatrixXd     m_preconditionedPolys;

    //For Timing
    static size_t           m_timerLinearErrorSolveIndex;
    static size_t           m_timerPreconditionPolysIndex;
    static size_t           m_timerLipshitzSolveIndex;
    static size_t           m_timerChebValReduceIndex;
    Timer&                  m_timer = Timer::getInstance();
};

template<int Rank>
size_t IntervalBounder<Rank>::m_timerLinearErrorSolveIndex = -1;
template<int Rank>
size_t IntervalBounder<Rank>::m_timerPreconditionPolysIndex = -1;
template<int Rank>
size_t IntervalBounder<Rank>::m_timerLipshitzSolveIndex = -1;
template<int Rank>
size_t IntervalBounder<Rank>::m_timerChebValReduceIndex = -1;

#include "IntervalBounder1D.ipp"
#include "IntervalBounderND.ipp"

#endif /* IntervalBounder_h */


#ifndef POUF_VEC_HPP
#define POUF_VEC_HPP

#include <Eigen/Core>

#include "real.hpp"

template<int N = Eigen::Dynamic, class U = real>
using vector = Eigen::Matrix<U, N, 1>;

using vec1 = vector<1>;
using vec2 = vector<2>;
using vec3 = vector<3>;
using vec4 = vector<4>;
using vec6 = vector<6>;
using vec = vector<>;

template<int N, class U>
struct traits< vector<N, U> > {

  using scalar_type = scalar<U>;
  using deriv_type = vector<N, deriv<U> >;

  static const std::size_t dim = N * traits<U>::dim;

  static scalar_type dot(const vector<N, U>& x,
                         const vector<N, U>& y) {
    return x.dot(y);
  }

  static scalar_type& coord(std::size_t i, vector<N, U>& v) {
	// TODO assert this is safe
    return reinterpret_cast<scalar_type*>(v.data())[i];
  }

  static const scalar_type& coord(std::size_t i, const vector<N, U>& v) {
	// TODO assert this is safe
    return reinterpret_cast<const scalar_type*>(v.data())[i];
  }

  static vector<N, U> zero() {
	return vector<N, U>::Constant(traits<U>::zero());
  }

  // additive lie group structure
  // TODO multiplicative as well?
  using group_type = vector<N, U>;

  static group_type id() { return zero(); }
  static group_type inv(const group_type& x) { return -x; }
  static group_type prod(const group_type& x, const group_type& y) { return x + y; }

  static group_type exp(const deriv_type& x) { return x; }

  static const char* name() {
    static std::string value = "vec" + ((N > 0) ? std::to_string(N) : std::string());
    return value.c_str();
  }

  static deriv_type AdT(const group_type&, const deriv_type& x) { return x; }
  static deriv_type Ad(const group_type&, const deriv_type& x) { return x; }

};

#endif


/*
Program to translate text to DNA and vice versa
*/

#include <iostream>
#include <fstream>
#include <vector>
#include <boost/random/mersenne_twister.hpp>
//#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/bernoulli_distribution.hpp>
#include <boost/program_options.hpp>
#include "../include/PFE.hpp"
#include "../include/EFE.hpp"
#include "../include/DFT.hpp"
#include "../include/helpers.hpp"
#include "../include/encodedecode.hpp"
#include "../include/ReedSolomon.hpp"
#include <string>
#include <fstream>
#include <streambuf>

using namespace std;

// define static variables

typedef int el_t;
template<> int PFE<el_t>::prime = 47;
template<> boost::bimap<el_t,el_t> PFE<el_t>::exp_el = boost::bimap<el_t,el_t>();

typedef PFE<el_t> pfe;

template<> polynomial<pfe> EFE<pfe>::prim_poly = polynomial<pfe>();
template<> unsigned EFE<pfe>::m = 30;

 template<class cf_t>
    ostream &operator<<(ostream &stream, const vector<cf_t>& x)
    {
	for(unsigned i = 0; i<x.size(); ++i) stream << x[i] << "  ";
	return stream; // must return stream
    };

namespace po = boost::program_options;

int main(int ac, char* av[])
{

// command line options

int numblocks;
string infile;
string outfile;

int opt;
po::options_description desc("Allowed options");
desc.add_options()
    ("help", "produce help message")
    ("encode", "encode")
    ("decode", "decode")
    ("disturb", "draw uniformly at random from the input lines, add errors to each line")
	("input",po::value<string>(&infile)->default_value(""),"inputfile")
	("output",po::value<string>(&outfile)->default_value(""),"outputfile")
	("numblocks",po::value<int>(&numblocks)->default_value(0),"numblocks")
;

po::variables_map vm;
po::store(po::parse_command_line(ac, av, desc), vm);
po::notify(vm);

if (vm.count("help")) {
    cout << desc << "\n";
    return 1;
}

const unsigned l = 3;
const unsigned m = 30; // EFE<pfe>::m; // GF(p^m)
DNAmap<pfe> dnamap;

// initialize the inner code
pfe::initialize_exp_el(33);
typedef EFE<pfe> efe;

const unsigned M = 50000;
const int prime = PFE<el_t>::prime;

const unsigned N = 39;
const unsigned N_u = 46; // the underlying length of the shortened inner code
const unsigned K = 33;

// primitive element of the inner code
const pfe A = pfe(33);
typedef DFT_PRIM<pfe> dftpfe;
dftpfe dftpfe_d(N,A);

RScode<pfe,dftpfe> innercode(N,K,A,dftpfe_d,N_u);

// initialize the outer code
const unsigned n = 713; // DFT length, divides 47^33-1
const unsigned P = 23;
const unsigned Q = 31; // P*Q = n
const unsigned k = 594;//

EFE<pfe>::m = m;

// initialize the primitive polynomial
pfe ppvv[m+1] = {1,13,15,22,45,  19,34,10,17,5,41,21,  12,3,23,17,27,8,  34,32,7,40,41,1,  32,26,24,32,37,5,1}; // for m=30
vector<pfe> ppv(ppvv, ppvv+m+1);
EFE<pfe>::prim_poly = polynomial<pfe>(ppv);
// initialize the element of order n, the Fourier kernel
pfe aa[m] = {26,35,41,0,24,3,31,8,12,10,9,24,44,11,24,9,43,14,26,32,7,13,15,29,25,38,8,24,40,32}; // element of order 713
vector<pfe> avv(aa, aa+m); //
efe a = efe(avv); // this is an element of order n

typedef DFT_FFT<efe> dftefe;
dftefe dftefe_d(n,a,P,Q); // Fourier transform for the outer code

RScode<efe,dftefe> outercode(n,k,a,dftefe_d);

// encoder/decoder
EnDecode<RScode<pfe,dftpfe>, RScode<efe,dftefe>, DNAmap<pfe> > endecode(innercode,outercode,dnamap,l);

/////////////////////// encode
if (vm.count("encode")) {

	cout << "start encoding.." << endl;

	if(infile == "" || outfile ==""){
		cout << "in/outfile not specified " << endl;
		return 0;
	}
	cout << "infile:  " << infile << endl;
	cout << "outfile: " << outfile << endl;

	// read data
	std::ifstream t(infile.c_str());
	std::string str((std::istreambuf_iterator<char>(t)),std::istreambuf_iterator<char>());

	// encode
	vector<string> urn(n*numblocks);
	endecode.encode(str, urn);
	numblocks = endecode.numblocks;

	cout << "encoded " << str.size() << " Bytes to " << numblocks << " blocks, resulting in "
	<< n*numblocks << " DNA segments of length " << N << " each." << endl;

	ofstream out;
	out.open(outfile.c_str());
	for(unsigned i=0;i<urn.size();++i) out << urn[i] << endl;
	out.close();
	return 0;
}

/////////////////////// decode
if (vm.count("decode")) {
	//cout << "generate DFT lookup table"<< endl;
	typedef DFT_FFT<efe> dftefe;
	dftefe dftefe_d(n,a,P,Q); // Fourier transform for the outer code

	if(infile == "" || outfile =="" || numblocks==0 ) {
		cout << "in/outfile/numblocks not specified " << endl;
		return 0;
	}
	cout << "infile:  " << infile << endl;
	cout << "outfile: " << outfile << endl;
	cout << "numblocks: "<<numblocks << endl;

	vector<string> drawnseg;

	string sLine = "";
	ifstream in;
	in.open(infile.c_str());
	while (!in.eof()){
		getline(in, sLine);
		drawnseg.push_back(sLine);
	}
	drawnseg.resize(drawnseg.size()-1); // erase the last, empty line

	string recstr;
	cout << "start decode.." << endl;
	endecode.numblocks = numblocks;
	endecode.decode(recstr, drawnseg);

	ofstream out;
	out.open(outfile.c_str());
	out << recstr;
	out.close();

}
////////////////////////// disturb
if (vm.count("disturb")) {

	if(infile == "" || outfile =="") {
		cout << "in/outfile not specified " << endl;
		return 0;
	}
	cout << "infile:  " << infile << endl;
	cout << "outfile: " << outfile << endl;

	vector<string> urn;

	string sLine = "";
	ifstream in;
	in.open(infile.c_str());
	while (!in.eof()){
		getline(in, sLine);
		urn.push_back(sLine);
	}
	urn.resize(urn.size()-1); // erase the last, empty line

	boost::mt19937 rng;
	boost::uniform_int<> unif(0,urn.size()-1); // distribution that maps to 0,..,urn.size()-1
	boost::uniform_int<> unif_N(0,N-1); // distribution that maps to 0,..,N-1
	boost::uniform_int<> unif_4(0,4-1); // distribution that maps to 0,..,N-1
	boost::bernoulli_distribution<> bern(0.013);
	boost::bernoulli_distribution<> faircoin(0.5);

	char nucl[] = "ACGT";

	vector<string> drawnseg(M);

	//write data
	ofstream out;
	out.open(outfile.c_str());

	// draw M times
	string tmpstr;
	for(unsigned i=0;i<M;++i){
		unsigned randind = unif(rng);
		tmpstr = urn[randind];

		// introduce errors

		for(unsigned j=0;j<2;++j){
			tmpstr[unif_N(rng)] = nucl[unif_4(rng)];
		}

		unsigned ctr = 0;
		for(unsigned j=0;j<tmpstr.size();++j)
			if(bern(rng)) {
				tmpstr[j] = nucl[unif_4(rng)];
				ctr++;
			}
			//cout << ctr << endl;
		//if(faircoin(rng)) flipvecdir(tmpstr); // flip every second

		// write to file
		out << tmpstr;
		if(i!= M-1) out << endl;
	}
	out.close();

}

}


#include <opencv2/opencv.hpp>
#include <yaml-cpp/yaml.h>
#include <boost/foreach.hpp>
#include <boost/bind.hpp>
#include <Eigen/Dense>

#include "calotypes/CrossValidation.hpp"
#include "calotypes/CameraCalibration.h"
#include "calotypes/CalibrationLog.h"

#include "calotypes/RandomDataSelector.hpp"

using namespace calotypes;

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

	if( argc < 2 )
	{
		std::cerr << "Please specify config file path." << std::endl;
		return -1;
	}

	std::string configPath( argv[1] );
	CalibrationLogReader reader( configPath );

	CameraTrainingData datum;
	std::vector<CameraTrainingData> data;
	while( reader.GetNext( datum ) )
	{
		data.push_back( datum );
	}

	std::vector<CameraTrainingData> subset;
	RandomCameraDataSelector selector;
// 	selector.SelectData( data, data.size(), subset );
	subset = data;

	CameraTrainingParams params;
	params.optimizeAspectRatio = true;
	params.optimizePrincipalPoint = true;
	params.enableRadialDistortion[0] = true;
	params.enableRadialDistortion[1] = false;

	typedef CrossValidationTask< CameraModel, CameraTrainingData > CameraCV;
	CameraCV::TrainFunc trainer = boost::bind( &TrainCameraModel, _1, _2,
											   data[0].imageSize, params ); // HACK heh
	CameraCV::TestFunc tester = boost::bind( &TestCameraModel, _1, _2 );
	CameraCV crossValidation( trainer, tester, subset, 4 );

	unsigned int numFolds = 4;
	std::cout << "Performing " << numFolds << "-fold cross validation..." << std::endl;
	crossValidation.Validate();
	std::vector< CameraCV::ValidationResult > results = crossValidation.GetResults();

	std::cout << "Errors: " << std::endl;
	for( unsigned int i = 0; i < numFolds; i++ )
	{
		std::cout << "\tFold " << i << " test/train: " << results[i].testError
				  << " " << results[i].trainingError << std::endl;
		std::cout << "\tModel " << results[i].model << std::endl;
	}

	return 0;
}


//==============================================================================
//         Copyright 2016        Numscale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/function/simd/nthroot.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/pack.hpp>
#include <exhaustive.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

#include <cmath>
#include <cstdlib>

struct raw_nth
{
  float operator()(float x) const
  {
    return bs::nthroot(double(x), 4);
  }
};

struct nth
{
  bs::pack<float> operator()(bs::pack<float> x) const
  {
    using pi_t = bd::as_integer_t<bs::pack<float>>;
    return bs::fast_(bs::nthroot)(x, pi_t(4));
  }
};

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

  float mini = bs::Zero<float>();
  float maxi = bs::Valmax<float>();
  if(argc >= 2) mini = std::atof(argv[1]);
  if(argc >= 3) maxi = std::atof(argv[2]);
  bs::exhaustive_test<bs::pack<float>> ( mini
                                       , maxi
                                       , nth()
                                       , raw_nth()
                                       );

  return 0;
}


#include <iostream>
#include <fstream>
#include <memory>
#include <boost/algorithm/string.hpp>

using namespace std;

/* Becasue I want to use OpenMP,
 * So I must use vector as the container for paras, because openmp require random access iterator.
 * And must use list as the container for results, because vector need to relocate, and therefore not thread-safe.
 */

// type define
typedef array<double, 3> Row;
typedef shared_ptr< list<Row> > RowList;
typedef array<double, 3> Para;
typedef shared_ptr< vector<Para> > ParaList;
typedef pair<Para, int> Score;

// function defines
RowList readFile(const string& fileName);
int sign(const Row& row, const Para& para);
int errors(const Para& para);
int predictErrors(const Para& para);
Para patch(const Para& para, const Row& row);
ParaList analysys(const Para& para);
Score deepin(const int now, const Para& para, const int limit);

// overloading define
ostream &operator<<(ostream& out, const Para& para) {
	out << "[" << para[0] << ", " << para[1] << ", " << para[2] << "]";
	return out;
}

ostream &operator<<(ostream& out, const Score& score) {
	out << "<" << score.first << ", " << score.second << ">";
	return out;
}

// preset data defines
Para initPara = {0, 0, 0};
auto trainFileContent = readFile("train_1_5.csv");
auto testFileContent = readFile("test_1_5.csv");

// main
int main(void) {
	Score trainResult = deepin(0, initPara, 1);
	int predictionError = predictErrors(trainResult.first);

	cout << "Train Result: " << trainResult << endl;
	cout << "Prediction Error: " << predictionError << endl;
}

// function implementation
RowList readFile(const string& fileName) {
	ifstream inFile(fileName);
	string line;
	vector<string> words;
	Row nums;
	RowList fileContent = static_cast<RowList>(new list<Row>);

	while(getline(inFile, line)){
		boost::split(words, line, boost::is_any_of(","));
		nums = {
			stod(words[0]),
			stod(words[1]),
			stod(words[2])
		};
		fileContent->push_back(nums);
	}

	return fileContent;
}

inline int sign(const Row& row, const Para& para) {
	double result = para[0] * row[0] + para[1] * row[1] + para[2];
	if(result >= 0){
		return 1;
	} else{
		return -1;
	}
}

int errors(const Para& para) {
	int sum = 0;

	for(auto it = trainFileContent->begin(); it != trainFileContent->end(); it++){
		Row& row = *it;
		if(sign(row, para) != row[2]){
			sum++;
		}
	}

	return sum;
}

int predictErrors(const Para& para) {
	int sum = 0;

	for(auto it = testFileContent->begin(); it != testFileContent->end(); it++){
		Row& row = *it;
		if(sign(row, para) != row[2]){
			sum++;
		}
	}

	return sum;
}

Para patch(const Para& para, const Row& row) {
	Para tryPara = {
		para[0] + row[2] * row[0],
		para[1] + row[2] * row[1],
		para[2] + row[2]
	};

	return tryPara;
}

ParaList analysys(const Para& para) {
	ParaList candidatePara = static_cast<ParaList>(new vector<Para>);

	for(auto it = trainFileContent->begin(); it != trainFileContent->end(); it++){
		Row& row = *it;
		if(sign(row, para) != row[2]){
			candidatePara->push_back(patch(para, row));
		}
	}

	return candidatePara;
}

/* @parameter: init para
 * @return: the best para among the child and sub-child para created by this para.
 */
Score deepin(const int now, const Para& para, const int limit) {
	list<Score> thisResult;
	thisResult.push_back(Score(para, errors(para)));

	if(now < limit){
		auto tryParas = *analysys(para);
		const unsigned int size = tryParas.size();

		//#pragma omp parallel for
		for(unsigned int i = 0; i < size; i++){
			Para& childPara = tryParas[i];
			thisResult.push_back(deepin(now+1, childPara, limit));
			if(now == 0){
				cout << "No: " << i << ", Score: " << *thisResult.rbegin() << endl;
			}
		}
	} else{
		auto tryParas = *analysys(para);
		const unsigned int size = tryParas.size();

		for(unsigned int i = 0; i < size; i++){
			Para& subPara = tryParas[i];
			thisResult.push_back(Score(subPara, errors(subPara)));
		}
	}

	Para bestPara = thisResult.begin()->first;
	int bestError = thisResult.begin()->second;

	for(auto it = thisResult.begin(); it != thisResult.end(); it++){
		if(it->second < bestError){
			bestPara = it->first;
			bestError = it->second;
		}
	}

	return(Score(bestPara, bestError));
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file   testGaussianFactorGraphB.cpp
 *  @brief  Unit tests for Linear Factor Graph
 *  @author Christian Potthast
 **/

#include <tests/smallExample.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/tuple/tuple.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;
#include <boost/range/adaptor/map.hpp>
namespace br { using namespace boost::range; using namespace boost::adaptors; }

#include <string.h>
#include <iostream>

using namespace std;
using namespace gtsam;
using namespace example;

double tol=1e-5;

using symbol_shorthand::X;
using symbol_shorthand::L;

static auto kUnit2 = noiseModel::Unit::Create(2);

/* ************************************************************************* */
TEST( GaussianFactorGraph, equals ) {

  GaussianFactorGraph fg = createGaussianFactorGraph();
  GaussianFactorGraph fg2 = createGaussianFactorGraph();
  EXPECT(fg.equals(fg2));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, error ) {
  GaussianFactorGraph fg = createGaussianFactorGraph();
  VectorValues cfg = createZeroDelta();

  // note the error is the same as in testNonlinearFactorGraph as a
  // zero delta config in the linear graph is equivalent to noisy in
  // non-linear, which is really linear under the hood
  double actual = fg.error(cfg);
  DOUBLES_EQUAL( 5.625, actual, 1e-9 );
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, eliminateOne_x1) {
  GaussianFactorGraph fg = createGaussianFactorGraph();

  GaussianConditional::shared_ptr conditional;
  auto result = fg.eliminatePartialSequential(Ordering(list_of(X(1))));
  conditional = result.first->front();

  // create expected Conditional Gaussian
  Matrix I = 15 * I_2x2, R11 = I, S12 = -0.111111 * I, S13 = -0.444444 * I;
  Vector d = Vector2(-0.133333, -0.0222222);
  GaussianConditional expected(X(1), 15 * d, R11, L(1), S12, X(2), S13);

  EXPECT(assert_equal(expected, *conditional, tol));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, eliminateOne_x2) {
  Ordering ordering;
  ordering += X(2), L(1), X(1);
  GaussianFactorGraph fg = createGaussianFactorGraph();
  auto actual = EliminateQR(fg, Ordering(list_of(X(2)))).first;

  // create expected Conditional Gaussian
  double sigma = 0.0894427;
  Matrix I = I_2x2 / sigma, R11 = I, S12 = -0.2 * I, S13 = -0.8 * I;
  Vector d = Vector2(0.2, -0.14) / sigma;
  GaussianConditional expected(X(2), d, R11, L(1), S12, X(1), S13, kUnit2);

  EXPECT(assert_equal(expected, *actual, tol));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, eliminateOne_l1) {
  Ordering ordering;
  ordering += L(1), X(1), X(2);
  GaussianFactorGraph fg = createGaussianFactorGraph();
  auto actual = EliminateQR(fg, Ordering(list_of(L(1)))).first;

  // create expected Conditional Gaussian
  double sigma = sqrt(2.0) / 10.;
  Matrix I = I_2x2 / sigma, R11 = I, S12 = -0.5 * I, S13 = -0.5 * I;
  Vector d = Vector2(-0.1, 0.25) / sigma;
  GaussianConditional expected(L(1), d, R11, X(1), S12, X(2), S13, kUnit2);

  EXPECT(assert_equal(expected, *actual, tol));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, eliminateOne_x1_fast) {
  GaussianFactorGraph fg = createGaussianFactorGraph();
  GaussianConditional::shared_ptr conditional;
  JacobianFactor::shared_ptr remaining;
  boost::tie(conditional, remaining) = EliminateQR(fg, Ordering(list_of(X(1))));

  // create expected Conditional Gaussian
  Matrix I = 15 * I_2x2, R11 = I, S12 = -0.111111 * I, S13 = -0.444444 * I;
  Vector d = Vector2(-0.133333, -0.0222222);
  GaussianConditional expected(X(1), 15 * d, R11, L(1), S12, X(2), S13, kUnit2);

  // Create expected remaining new factor
  JacobianFactor expectedFactor(
      L(1), (Matrix(4, 2) << 6.87184, 0, 0, 6.87184, 0, 0, 0, 0).finished(),
      X(2),
      (Matrix(4, 2) << -5.25494, 0, 0, -5.25494, -7.27607, 0, 0, -7.27607)
          .finished(),
      (Vector(4) << -1.21268, 1.73817, -0.727607, 1.45521).finished(),
      noiseModel::Unit::Create(4));

  EXPECT(assert_equal(expected, *conditional, tol));
  EXPECT(assert_equal(expectedFactor, *remaining, tol));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, eliminateOne_x2_fast) {
  GaussianFactorGraph fg = createGaussianFactorGraph();
  auto actual = EliminateQR(fg, Ordering(list_of(X(2)))).first;

  // create expected Conditional Gaussian
  double sigma = 0.0894427;
  Matrix I = I_2x2 / sigma, R11 = -I, S12 = 0.2 * I, S13 = 0.8 * I;
  Vector d = Vector2(-0.2, 0.14) / sigma;
  GaussianConditional expected(X(2), d, R11, L(1), S12, X(1), S13, kUnit2);

  EXPECT(assert_equal(expected, *actual, tol));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, eliminateOne_l1_fast) {
  GaussianFactorGraph fg = createGaussianFactorGraph();
  auto actual = EliminateQR(fg, Ordering(list_of(L(1)))).first;

  // create expected Conditional Gaussian
  double sigma = sqrt(2.0) / 10.;
  Matrix I = I_2x2 / sigma, R11 = -I, S12 = 0.5 * I, S13 = 0.5 * I;
  Vector d = Vector2(0.1, -0.25) / sigma;
  GaussianConditional expected(L(1), d, R11, X(1), S12, X(2), S13, kUnit2);

  EXPECT(assert_equal(expected, *actual, tol));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, copying) {
  // Create a graph
  GaussianFactorGraph actual = createGaussianFactorGraph();

  // Copy the graph !
  GaussianFactorGraph copy = actual;

  // now eliminate the copy
  GaussianBayesNet actual1 = *copy.eliminateSequential();

  // Create the same graph, but not by copying
  GaussianFactorGraph expected = createGaussianFactorGraph();

  // and check that original is still the same graph
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet) {
  GaussianFactorGraph fg = createGaussianFactorGraph();

  // render with a given ordering
  GaussianBayesNet CBN = *fg.eliminateSequential();

  // True GaussianFactorGraph
  GaussianFactorGraph fg2(CBN);
  GaussianBayesNet CBN2 = *fg2.eliminateSequential();
  EXPECT(assert_equal(CBN, CBN2));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, optimize_Cholesky) {
  // create a graph
  GaussianFactorGraph fg = createGaussianFactorGraph();

  // optimize the graph
  VectorValues actual = fg.optimize(boost::none, EliminateCholesky);

  // verify
  VectorValues expected = createCorrectDelta();
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, optimize_QR )
{
  // create a graph
  GaussianFactorGraph fg = createGaussianFactorGraph();

  // optimize the graph
  VectorValues actual = fg.optimize(boost::none, EliminateQR);

  // verify
  VectorValues expected = createCorrectDelta();
  EXPECT(assert_equal(expected,actual));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, combine) {
  // create a test graph
  GaussianFactorGraph fg1 = createGaussianFactorGraph();

  // create another factor graph
  GaussianFactorGraph fg2 = createGaussianFactorGraph();

  // get sizes
  size_t size1 = fg1.size();
  size_t size2 = fg2.size();

  // combine them
  fg1.push_back(fg2);

  EXPECT(size1 + size2 == fg1.size());
}

/* ************************************************************************* */
// print a vector of ints if needed for debugging
void print(vector<int> v) {
  for (size_t k = 0; k < v.size(); k++) cout << v[k] << " ";
  cout << endl;
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, createSmoother) {
  GaussianFactorGraph fg1 = createSmoother(2);
  LONGS_EQUAL(3, fg1.size());
  GaussianFactorGraph fg2 = createSmoother(3);
  LONGS_EQUAL(5, fg2.size());
}

/* ************************************************************************* */
double error(const VectorValues& x) {
  GaussianFactorGraph fg = createGaussianFactorGraph();
  return fg.error(x);
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, multiplication) {
  GaussianFactorGraph A = createGaussianFactorGraph();
  VectorValues x = createCorrectDelta();
  Errors actual = A * x;
  Errors expected;
  expected += Vector2(-1.0, -1.0);
  expected += Vector2(2.0, -1.0);
  expected += Vector2(0.0, 1.0);
  expected += Vector2(-1.0, 1.5);
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
// Extra test on elimination prompted by Michael's email to Frank 1/4/2010
TEST(GaussianFactorGraph, elimination) {
  // Create Gaussian Factor Graph
  GaussianFactorGraph fg;
  Matrix Ap = I_1x1, An = I_1x1 * -1;
  Vector b = (Vector(1) << 0.0).finished();
  SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1, 2.0);
  fg += JacobianFactor(X(1), An, X(2), Ap, b, sigma);
  fg += JacobianFactor(X(1), Ap, b, sigma);
  fg += JacobianFactor(X(2), Ap, b, sigma);

  // Eliminate
  Ordering ordering;
  ordering += X(1), X(2);
  GaussianBayesNet bayesNet = *fg.eliminateSequential();

  // Check matrix
  Matrix R;
  Vector d;
  boost::tie(R, d) = bayesNet.matrix();
  Matrix expected =
      (Matrix(2, 2) << 0.707107, -0.353553, 0.0, 0.612372).finished();
  Matrix expected2 =
      (Matrix(2, 2) << 0.707107, -0.353553, 0.0, -0.612372).finished();
  EXPECT(assert_equal(expected, R, 1e-6));
  EXPECT(equal_with_abs_tol(expected, R, 1e-6) ||
         equal_with_abs_tol(expected2, R, 1e-6));
}

/* ************************************************************************* */
// Tests ported from ConstrainedGaussianFactorGraph
/* ************************************************************************* */
TEST(GaussianFactorGraph, constrained_simple) {
  // get a graph with a constraint in it
  GaussianFactorGraph fg = createSimpleConstraintGraph();
  EXPECT(hasConstraints(fg));

  // eliminate and solve
  VectorValues actual = fg.eliminateSequential()->optimize();

  // verify
  VectorValues expected = createSimpleConstraintValues();
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, constrained_single) {
  // get a graph with a constraint in it
  GaussianFactorGraph fg = createSingleConstraintGraph();
  EXPECT(hasConstraints(fg));

  // eliminate and solve
  VectorValues actual = fg.eliminateSequential()->optimize();

  // verify
  VectorValues expected = createSingleConstraintValues();
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, constrained_multi1) {
  // get a graph with a constraint in it
  GaussianFactorGraph fg = createMultiConstraintGraph();
  EXPECT(hasConstraints(fg));

  // eliminate and solve
  VectorValues actual = fg.eliminateSequential()->optimize();

  // verify
  VectorValues expected = createMultiConstraintValues();
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */

static SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1);

/* ************************************************************************* */
TEST(GaussianFactorGraph, replace)
{
  Ordering ord; ord += X(1),X(2),X(3),X(4),X(5),X(6);
  SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0));

  GaussianFactorGraph::sharedFactor f1(new JacobianFactor(
      X(1), I_3x3, X(2), I_3x3, Z_3x1, noise));
  GaussianFactorGraph::sharedFactor f2(new JacobianFactor(
      X(2), I_3x3, X(3), I_3x3, Z_3x1, noise));
  GaussianFactorGraph::sharedFactor f3(new JacobianFactor(
      X(3), I_3x3, X(4), I_3x3, Z_3x1, noise));
  GaussianFactorGraph::sharedFactor f4(new JacobianFactor(
      X(5), I_3x3, X(6), I_3x3, Z_3x1, noise));

  GaussianFactorGraph actual;
  actual.push_back(f1);
  actual.push_back(f2);
  actual.push_back(f3);
  actual.replace(0, f4);

  GaussianFactorGraph expected;
  expected.push_back(f4);
  expected.push_back(f2);
  expected.push_back(f3);

  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, hasConstraints)
{
  FactorGraph<GaussianFactor> fgc1 = createMultiConstraintGraph();
  EXPECT(hasConstraints(fgc1));

  FactorGraph<GaussianFactor> fgc2 = createSimpleConstraintGraph() ;
  EXPECT(hasConstraints(fgc2));

  GaussianFactorGraph fg = createGaussianFactorGraph();
  EXPECT(!hasConstraints(fg));
}

#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/sam/RangeFactor.h>

/* ************************************************************************* */
TEST( GaussianFactorGraph, conditional_sigma_failure) {
  // This system derives from a failure case in DDF in which a Bayes Tree
  // has non-unit sigmas for conditionals in the Bayes Tree, which
  // should never happen by construction

  // Reason for the failure: using Vector_() is dangerous as having a non-float gets set to zero, resulting in constraints
  gtsam::Key xC1 = 0, l32 = 1, l41 = 2;

  // noisemodels at nonlinear level
  gtsam::SharedNoiseModel priorModel = noiseModel::Diagonal::Sigmas((Vector(6) << 0.05, 0.05, 3.0, 0.2, 0.2, 0.2).finished());
  gtsam::SharedNoiseModel measModel = kUnit2;
  gtsam::SharedNoiseModel elevationModel = noiseModel::Isotropic::Sigma(1, 3.0);

  double fov = 60; // degrees
  int imgW = 640; // pixels
  int imgH = 480; // pixels
  gtsam::Cal3_S2::shared_ptr K(new gtsam::Cal3_S2(fov, imgW, imgH));

  typedef GenericProjectionFactor<Pose3, Point3> ProjectionFactor;

  double relElevation = 6;

  Values initValues;
  initValues.insert(xC1,
      Pose3(Rot3(
          -1.,           0.0,  1.2246468e-16,
          0.0,             1.,           0.0,
          -1.2246468e-16,           0.0,            -1.),
          Point3(0.511832102, 8.42819594, 5.76841725)));
  initValues.insert(l32,  Point3(0.364081507, 6.89766221, -0.231582751) );
  initValues.insert(l41,  Point3(1.61051523, 6.7373052, -0.231582751)   );

  NonlinearFactorGraph factors;
  factors += PriorFactor<Pose3>(xC1,
      Pose3(Rot3(
          -1.,           0.0,  1.2246468e-16,
          0.0,             1.,           0.0,
          -1.2246468e-16,           0.0,            -1),
          Point3(0.511832102, 8.42819594, 5.76841725)), priorModel);
  factors += ProjectionFactor(Point2(333.648615, 98.61535), measModel, xC1, l32, K);
  factors += ProjectionFactor(Point2(218.508, 83.8022039), measModel, xC1, l41, K);
  factors += RangeFactor<Pose3,Point3>(xC1, l32, relElevation, elevationModel);
  factors += RangeFactor<Pose3,Point3>(xC1, l41, relElevation, elevationModel);

  // Check that sigmas are correct (i.e., unit)
  GaussianFactorGraph lfg = *factors.linearize(initValues);

  GaussianBayesTree actBT = *lfg.eliminateMultifrontal();

  // Check that all sigmas in an unconstrained bayes tree are set to one
  for(const GaussianBayesTree::sharedClique& clique: actBT.nodes() | br::map_values) {
    GaussianConditional::shared_ptr conditional = clique->conditional();
    //size_t dim = conditional->rows();
    //EXPECT(assert_equal(gtsam::Vector::Ones(dim), conditional->get_model()->sigmas(), tol));
    EXPECT(!conditional->get_model());
  }
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */


//
// Created by wei on 10/3/18.
//

#include <Cuda/Common/TransformCuda.h>
#include <Eigen/Eigen>
#include <Open3D/Open3D.h>

#include <gtest/gtest.h>

using namespace open3d;
using namespace open3d::utility;
using namespace open3d::cuda;

TEST(TransformCuda, Transform) {
    using namespace open3d;

    for (int i = 0; i < 1000; ++i) {
        /* Generate random R & t */
        Eigen::Vector3d w = Eigen::Vector3d::Random();
        float theta = w.norm();
        w = w / theta;
        Eigen::Matrix3d w_tilde;
        w_tilde << 0, -w(2), w(1),
            w(2), 0, -w(0),
            -w(1), w(0), 0;
        Eigen::Matrix3d R =
            Eigen::Matrix3d::Identity()
            + sin(theta) * w_tilde + (1 - cos(theta)) * (w_tilde * w_tilde);

        Eigen::Vector3d t = Eigen::Vector3d::Random();
        Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
        T.block<3, 3>(0, 0) = R;
        T.block<3, 1>(0, 3) = t;

        TransformCuda transform_cuda;
        transform_cuda.FromEigen(R, t);
        float
            matrix_norm = (T.inverse() - transform_cuda.Inverse().ToEigen()).norm();
        EXPECT_LE(matrix_norm, 1e-6);

        Vector3f v_cuda;
        Eigen::Vector3d v = Eigen::Vector3d::Random();
        v_cuda.FromEigen(v);

        Vector3f Tv_cuda = transform_cuda * v_cuda;
        float vector_norm = ((T * v.homogeneous()).hnormalized() -
            (transform_cuda * v_cuda).ToEigen()).norm();
        EXPECT_LE(vector_norm, 1e-6);
    }
    LogInfo("Transform tests passed\n");

    Vector1f v;
    v(0) = 1.0f;
    v = 1.0f * v;
}

int main(int argc, char **argv) {
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


/*****************************************************************************
*
*     Program: EPTlib
*     Author: Alessandro Arduino <a.arduino@inrim.it>
*
*  MIT License
*
*  Copyright (c) 2020-2021  Alessandro Arduino
*  Istituto Nazionale di Ricerca Metrologica (INRiM)
*  Strada delle cacce 91, 10135 Torino
*  ITALY
*
*  Permission is hereby granted, free of charge, to any person obtaining a copy
*  of this software and associated documentation files (the "Software"), to deal
*  in the Software without restriction, including without limitation the rights
*  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
*  copies of the Software, and to permit persons to whom the Software is
*  furnished to do so, subject to the following conditions:
*
*  The above copyright notice and this permission notice shall be included in all
*  copies or substantial portions of the Software.
*
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
*  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
*  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
*  SOFTWARE.
*
*****************************************************************************/

#include "eptlib/finite_difference.h"

#include <complex>
#include <iostream>

#include <Eigen/Dense>

using namespace eptlib;

// FDSavitzkyGolayFilter constructor
FDSavitzkyGolayFilter::
FDSavitzkyGolayFilter(const Shape &shape) :
    shape_(shape),
    m_vox_(std::accumulate(shape.GetSize().begin(),shape.GetSize().end(),1,std::multiplies<int>())) {
    double toll = 1e-10;
    std::array<int,NDIM> mm = shape_.GetSize();
    // check to have odd shape size
    assert(m_vox_%2);
    // get the central voxel address
    int idx0 = m_vox_/2;
    std::array<int,NDIM> ii0;
    for (int d = 0; d<NDIM; ++d) {
        ii0[d] = mm[d]/2;
    }
    // initialise the design matrix
    int n_row = shape_.GetVolume();
    int n_col2 = 1 + NDIM;
    int n_col1 = (NDIM*(NDIM+1))/2;
    Eigen::MatrixXd F2(n_row,n_col2);
    Eigen::MatrixXd F1(n_row,n_col1);
    // fill the design matrix
    std::array<int,NDIM> ii;
    std::array<double,NDIM> di;
    int r = 0;
    for (ii[2] = 0; ii[2]<mm[2]; ++ii[2]) {
        for (ii[1] = 0; ii[1]<mm[1]; ++ii[1]) {
            for (ii[0] = 0; ii[0]<mm[0]; ++ii[0]) {
                if (shape_[ii]) {
                    for (int d = 0; d<NDIM; ++d) {
                        di[d] = ii[d]-ii0[d];
                    }
                    // design matrix for even quantities
                    F2(r,0) = 1.0;
                    for (int d = 0; d<NDIM; ++d) {
                        F2(r,1+d) = di[d]*di[d];
                    }
                    // design matrix for odd quantities
                    int c = 0;
                    for (int d = 0; d<NDIM; ++d) {
                        F1(r,d) = di[d];
                        for (int d2 = d+1; d2<NDIM; ++d2) {
                            F1(r,NDIM+c) = di[d]*di[d2];
                            ++c;
                        }
                    }
                    ++r;
                }
            }
        }
    }
    // check that all lines are filled
    Eigen::VectorXd v = F2.cwiseAbs().colwise().sum();
    double ref = v.maxCoeff();
    for (int d = 0; d<NDIM; ++d) {
        if (v[d]<toll*ref) {
            throw std::runtime_error("Impossible to set-up the Savitzky-Golay filter with the provided shape: no points along direction "+std::to_string(d)+".");
        }
    }
    v = F1.cwiseAbs().colwise().sum();
    ref = v.maxCoeff();
    for (int d = 0; d<NDIM; ++d) {
        if (v[d]<toll*ref) {
            throw std::runtime_error("Impossible to set-up the Savitzky-Golay filter with the provided shape: no points along direction "+std::to_string(d)+".");
        }
    }
    for (int c = NDIM; c<n_col1; ++c) {
        if (v[c]<toll*ref) {
            --n_col1;
            F1.col(c).swap(F1.col(n_col1));
            --c;
        }
    }
    F1.conservativeResize(Eigen::NoChange, n_col1);
    // solve the normal equations
    Eigen::MatrixXd A;
    if (shape_.IsSymmetric()) {
        A = (F2.transpose()*F2).inverse()*F2.transpose();
    } else {
        Eigen::MatrixXd F(n_row,n_col2+n_col1);
        F << F2,F1;
        A = (F.transpose()*F).inverse()*F.transpose();
    }
    // Compute the kernel for the laplacian
    for (int d = 0; d<NDIM; ++d) {
        lapl_kernel_[d].resize(m_vox_,0.0);
        r = 0;
        for (int idx = 0; idx<m_vox_; ++idx) {
            if (shape_[idx]) {
                lapl_kernel_[d][idx] = 2.0*A(1+d,r);
                ++r;
            }
        }
    }
    // Compute the kernel for the gradient
    int c_base = 1+NDIM;
    if (shape_.IsSymmetric()) {
        A = (F1.transpose()*F1).inverse()*F1.transpose();
        c_base = 0;
    }
    for (int d = 0; d<NDIM; ++d) {
        grad_kernel_[d].resize(m_vox_,0.0);
        r = 0;
        for (int idx = 0; idx<m_vox_; ++idx) {
            if (shape_[idx]) {
                grad_kernel_[d][idx] = A(c_base+d,r);
                ++r;
            }
        }
    }
    return;
}

// FDSavitzkyGolayFilter apply
template <typename NumType>
EPTlibError FDSavitzkyGolayFilter::
Apply(const DifferentialOperator diff_op, NumType *dst, const NumType *src, const std::array<int,NDIM> &nn, const std::array<double,NDIM> &dd) const {
    const int n_vox = std::accumulate(nn.begin(),nn.end(),1,std::multiplies<int>());
    std::array<int,NDIM> ii;
    std::array<int,NDIM> rr;
    std::array<int,NDIM> inc;
    for (int d = 0; d<NDIM; ++d) {
        rr[d] = shape_.GetSize()[d]/2;
    }
    inc[0] = 1;
    inc[1] = nn[0]-shape_.GetSize()[0];
    inc[2] = nn[0]*(nn[1]-shape_.GetSize()[1]);
    // loop over field voxels
    for (ii[2] = rr[2]; ii[2]<nn[2]-rr[2]; ++ii[2]) {
        for (ii[1] = rr[1]; ii[1]<nn[1]-rr[1]; ++ii[1]) {
            for (ii[0] = rr[0]; ii[0]<nn[0]-rr[0]; ++ii[0]) {
                std::array<int,NDIM> ii_l;
                std::vector<NumType> field_crop(m_vox_,0.0);
                // inner loop over kernel voxels
                int idx_l = 0;
                int idx_g = ii[0]-rr[0] + nn[0]*(ii[1]-rr[1] + nn[1]*(ii[2]-rr[2]));
                for (ii_l[2] = -rr[2]; ii_l[2]<=rr[2]; ++ii_l[2]) {
                    for (ii_l[1] = -rr[1]; ii_l[1]<=rr[1]; ++ii_l[1]) {
                        for (ii_l[0] = -rr[0]; ii_l[0]<=rr[0]; ++ii_l[0]) {
                            if (shape_[idx_l]) {
                                // set the field_crop to the field value
                                field_crop[idx_l] = src[idx_g];
                            }
                            ++idx_l;
                            idx_g += inc[0];
                        }
                        idx_g += inc[1];
                    }
                    idx_g += inc[2];
                }
                // compute the derivative
                int idx = ii[0] + nn[0]*(ii[1] + nn[1]*ii[2]);
                if (diff_op==DifferentialOperator::GradientX||
                    diff_op==DifferentialOperator::GradientY||
                    diff_op==DifferentialOperator::GradientZ) {
                    int d = static_cast<int>(diff_op) -
                        static_cast<int>(DifferentialOperator::GradientX);
                    dst[idx] = FirstOrder(d,field_crop,dd);
                } else if (diff_op==DifferentialOperator::GradientXX||
                    diff_op==DifferentialOperator::GradientYY||
                    diff_op==DifferentialOperator::GradientZZ) {
                    int d = static_cast<int>(diff_op) -
                        static_cast<int>(DifferentialOperator::GradientXX);
                    dst[idx] = SecondOrder(d,field_crop,dd);
                } else if (diff_op==DifferentialOperator::Laplacian) {
                    dst[idx] = Laplacian(field_crop,dd);
                }
            }
        }
    }
    return EPTlibError::Success;
}

// FDSavitzkyGolayFilter apply kernel first order derivative
template <typename NumType>
NumType FDSavitzkyGolayFilter::
FirstOrder(const int d, const std::vector<NumType> &field_crop, const std::array<double,NDIM> &dd) const {
    NumType dst = std::inner_product(grad_kernel_[d].begin(),grad_kernel_[d].end(),field_crop.begin(),static_cast<NumType>(0.0))/dd[d];
    return dst;
}
// FDSavitzkyGolayFilter apply kernel second order derivative
template <typename NumType>
NumType FDSavitzkyGolayFilter::
SecondOrder(const int d, const std::vector<NumType> &field_crop, const std::array<double,NDIM> &dd) const {
    NumType dst = std::inner_product(lapl_kernel_[d].begin(),lapl_kernel_[d].end(),field_crop.begin(),static_cast<NumType>(0.0))/dd[d]/dd[d];
    return dst;
}
// FDSavitzkyGolayFilter apply kernel laplacian
template <typename NumType>
NumType FDSavitzkyGolayFilter::
Laplacian(const std::vector<NumType> &field_crop, const std::array<double,NDIM> &dd) const {
    NumType dst = 0.0;
    for (int d = 0; d<NDIM; ++d) {
        dst += std::inner_product(lapl_kernel_[d].begin(),lapl_kernel_[d].end(),field_crop.begin(),static_cast<NumType>(0.0))/dd[d]/dd[d];
    }
    return dst;
}

// FDSavitzkyGolayFilter getters
const Shape& FDSavitzkyGolayFilter::
GetShape() const {
    return shape_;
}
const std::array<std::vector<double>,NDIM>& FDSavitzkyGolayFilter::
GetLaplKernel() const {
    return lapl_kernel_;
}
const std::array<std::vector<double>,NDIM>& FDSavitzkyGolayFilter::
GetGradKernel() const {
    return grad_kernel_;
}

// FDSavitzkyGolayFilter specialisations
template EPTlibError FDSavitzkyGolayFilter::Apply<double>(const DifferentialOperator diff_op,double *dst, const double *src, const std::array<int,NDIM> &nn, const std::array<double,NDIM> &dd) const;
template EPTlibError FDSavitzkyGolayFilter::Apply<std::complex<double> >(const DifferentialOperator diff_op,std::complex<double> *dst, const std::complex<double> *src, const std::array<int,NDIM> &nn, const std::array<double,NDIM> &dd) const;
template double FDSavitzkyGolayFilter::FirstOrder<double>(const int d,const std::vector<double> &field_crop,const std::array<double,NDIM> &dd) const;
template std::complex<double> FDSavitzkyGolayFilter::FirstOrder<std::complex<double> >(const int d,const std::vector<std::complex<double> > &field_crop,const std::array<double,NDIM> &dd) const;
template double FDSavitzkyGolayFilter::SecondOrder<double>(const int d,const std::vector<double> &field_crop,const std::array<double,NDIM> &dd) const;
template std::complex<double> FDSavitzkyGolayFilter::SecondOrder<std::complex<double> >(const int d,const std::vector<std::complex<double> > &field_crop,const std::array<double,NDIM> &dd) const;
template double FDSavitzkyGolayFilter::Laplacian<double>(const std::vector<double> &field_crop,const std::array<double,NDIM> &dd) const;
template std::complex<double> FDSavitzkyGolayFilter::Laplacian<std::complex<double> >(const std::vector<std::complex<double> > &field_crop,const std::array<double,NDIM> &dd) const;

// Apply the FD filter to an wrapped phase input field.
EPTlibError eptlib::WrappedPhaseDerivative(const DifferentialOperator diff_op,
    double *dst, const double *src, const std::array<int,NDIM> &nn,
    const std::array<double,NDIM> &dd, const FDSavitzkyGolayFilter &fd_filter) {
    int n_vox = std::accumulate(nn.begin(),nn.end(),1,std::multiplies<int>());
    // initialize the exponential map
    std::vector<std::complex<double> > emap(n_vox);
    for (int idx = 0; idx<n_vox; ++idx) {
        emap[idx] = std::exp(std::complex<double>(0,src[idx]));
    }
    // compute the derivative...
    if (diff_op==DifferentialOperator::GradientX||
        diff_op==DifferentialOperator::GradientY||
        diff_op==DifferentialOperator::GradientZ) {
        // ...first order
        std::vector<std::complex<double> > d_emap(n_vox);
        fd_filter.Apply(diff_op,d_emap.data(),emap.data(),nn,dd);
        for (int idx = 0; idx<n_vox; ++idx) {
            dst[idx] = std::imag(d_emap[idx]/emap[idx]);
        }
    } else if (diff_op==DifferentialOperator::GradientXX||
        diff_op==DifferentialOperator::GradientYY||
        diff_op==DifferentialOperator::GradientZZ) {
        // ...second order
        DifferentialOperator diff_op1 = static_cast<DifferentialOperator>(static_cast<int>(diff_op)-static_cast<int>(DifferentialOperator::GradientXX));
        std::vector<std::complex<double> > d_emap(n_vox);
        std::vector<std::complex<double> > dd_emap(n_vox);
        fd_filter.Apply(diff_op1,d_emap.data(),emap.data(),nn,dd);
        fd_filter.Apply(diff_op,dd_emap.data(),emap.data(),nn,dd);
        for (int idx = 0; idx<n_vox; ++idx) {
            dst[idx] = std::imag(d_emap[idx]*d_emap[idx]/emap[idx]/emap[idx] + dd_emap[idx]/emap[idx]);
        }
    } else if (diff_op==DifferentialOperator::Laplacian) {
        // ...laplacian
        std::vector<std::complex<double> > dx_emap(n_vox);
        std::vector<std::complex<double> > dy_emap(n_vox);
        std::vector<std::complex<double> > dz_emap(n_vox);
        std::vector<std::complex<double> > dd_emap(n_vox);
        fd_filter.Apply(DifferentialOperator::GradientX,dx_emap.data(),emap.data(),nn,dd);
        fd_filter.Apply(DifferentialOperator::GradientY,dy_emap.data(),emap.data(),nn,dd);
        fd_filter.Apply(DifferentialOperator::GradientZ,dz_emap.data(),emap.data(),nn,dd);
        fd_filter.Apply(DifferentialOperator::Laplacian,dd_emap.data(),emap.data(),nn,dd);
        for (int idx = 0; idx<n_vox; ++idx) {
            dst[idx] = std::imag((dx_emap[idx]*dx_emap[idx]+dy_emap[idx]*dy_emap[idx]+dz_emap[idx]*dz_emap[idx])/emap[idx]/emap[idx] + dd_emap[idx]/emap[idx]);
        }
    }
    return EPTlibError::Success;
}


#include "../r1_proof.h"
#include "../r1_proof_generator.h"
#include "../sigma_primitives.h"

#include "../../test/test_bitcoin.h"

#include <boost/test/unit_test.hpp>

using  namespace secp_primitives;
using  namespace sigma;

namespace {

struct sigma_unit_tests_fixture {
    // struct sigma_unit_tests_fixture : public TestingSetup {
    // sigma_unit_tests_fixture() = default;
    int N;
    int n;
    int m;
    int index;
    GroupElement g;
    std::vector<GroupElement> h_gens;
    Scalar rB;
    std::vector <Scalar> sigma;
    std::unique_ptr<R1ProofGenerator<Scalar, GroupElement>> r1prover;
    R1Proof<Scalar, GroupElement> r1proof;
    Scalar x;
    std::vector <std::vector<Scalar>> P_i_k;
    std::vector<Scalar> f_;
    std::vector<Scalar> a;
    std::vector <Scalar> Pk;
    secp_primitives::Scalar r;
    std::vector<secp_primitives::GroupElement> commits;

    sigma_unit_tests_fixture() {
        // sigma_unit_tests_fixture() : TestingSetup(CBaseChainParams::REGTEST) {
        N = 16;
        n = 4;
        index = 13;
        m = (int)(log(N) / log(n));
        g.randomize();

        for(int i = 0; i < n * m; ++i ){
            h_gens.push_back(secp_primitives::GroupElement());
            h_gens[i].randomize();
        }
        rB.randomize();
        SigmaPrimitives<Scalar,GroupElement>::convert_to_sigma(index, n, m, sigma);
        r1prover.reset(new R1ProofGenerator<Scalar, GroupElement>(g, h_gens, sigma, rB, n, m));

        Pk.resize(m);
        for (int k = 0; k < m; ++k) {
            Pk[k].randomize();
        }
        r.randomize();
        for(int i = 0; i < N; ++i){
            if(i == (index)){
                secp_primitives::GroupElement c;
                secp_primitives::Scalar zero(uint64_t(0));
                c = sigma::SigmaPrimitives<Scalar,GroupElement>::commit(g, zero, h_gens[0], r);
                commits.push_back(c);
            }
            else{
                commits.push_back(secp_primitives::GroupElement());
                commits[i].randomize();
            }
        }
        (*r1prover).proof(a, r1proof);
        x = (*r1prover).x_;
        P_i_k.resize(N);
        for (int i = 0; i < N; ++i) {
            std::vector <Scalar>& coefficients = P_i_k[i];
            std::vector<uint64_t> I = SigmaPrimitives<Scalar,GroupElement>::convert_to_nal(i, n, m);
            coefficients.push_back(sigma[I[0]]);
            coefficients.push_back(a[I[0]]);
            for (int j = 1; j < m; ++j) {
                SigmaPrimitives<Scalar,GroupElement>::new_factor(sigma[j * n + I[j]], a[j * n + I[j]], coefficients);
            }
            std::reverse(coefficients.begin(), coefficients.end());
        }
        f_ = r1proof.f_;
        std::vector<Scalar> f;
        for(int j = 0; j < m; ++j){
            f.push_back(sigma[j * n] * x + a[j * n]);
            int k = n - 1;
            for(int i = 0; i < k; ++i){
                f.push_back(r1proof.f_[j * k + i]);
            }
        }
        f_= f;
    }

    ~sigma_unit_tests_fixture(){}
};

BOOST_FIXTURE_TEST_SUITE(sigma_unit_tests,sigma_unit_tests_fixture)

BOOST_AUTO_TEST_CASE(unit_f_and_p_x)
{
    for(int i = 0; i < N; ++i){
        std::vector<uint64_t> I = SigmaPrimitives<Scalar,GroupElement>::convert_to_nal(i, n, m);
        Scalar f_i(uint64_t(1));
        Scalar p_i_x(uint64_t(0));
        for(int j = 0; j < m; ++j){
            f_i *= f_[j*n + I[j]];
            p_i_x += (P_i_k[i][j]*x.exponent(j));
        }
        if(i==index)
            p_i_x += (P_i_k[i][m]*x.exponent(m));
        BOOST_CHECK(f_i==p_i_x);
    }
}

BOOST_AUTO_TEST_CASE(unit_commits)
{
    Scalar z;
    z = r * x.exponent(uint64_t(m));
    Scalar sum;
    Scalar x_k(uint64_t(1));
    for (int k = 0; k < m; ++k) {
        sum += (Pk[k] * x_k);
        x_k *= x;
    }
    z -= sum;
    GroupElement coommit = SigmaPrimitives<Scalar,GroupElement>::commit(g, Scalar(uint64_t(0)), h_gens[0], z);
    GroupElement commits_;
    for(int k = 0; k< m; ++k){
        commits_ += (SigmaPrimitives<Scalar,GroupElement>::commit(
                g, Scalar(uint64_t(0)), h_gens[0], Pk[k])) * (x.exponent(k)).negate();
    }
    commits_ += (commits[index] * x.exponent(m));

    BOOST_CHECK(coommit == commits_);
}

BOOST_AUTO_TEST_CASE(unit_G_k_prime)
{
    std::vector<Scalar> f_i_;
    for(int i = 0; i < N; ++i){
        std::vector<uint64_t> I = SigmaPrimitives<Scalar,GroupElement>::convert_to_nal(i, n, m);
        Scalar f_i(uint64_t(1));
        for(int j = 0; j < m; ++j){
            f_i *= f_[j*n + I[j]];
        }
        f_i_.push_back(f_i);
    }

    secp_primitives::MultiExponent mult(commits, f_i_);
    GroupElement C = mult.get_multiple();

    GroupElement G;
    for(int k = 0; k < m; ++k){
        GroupElement Gk_prime;
        for(int i = 0; i < N; ++i)
            Gk_prime += commits[i] * P_i_k[i][k];
            G += (Gk_prime)* ((x.exponent(k)).negate());
    }
    BOOST_CHECK((C + G) == (commits[index] * (x.exponent(m))));
}

BOOST_AUTO_TEST_SUITE_END()
}


/*
Copyright (c) 2020 Inverse Palindrome
ProceduralX - ECS/SteeringBehaviors.cpp
https://inversepalindrome.com/
*/

#include "ECS/SteeringBehaviors.hpp"
#include "App/Random.hpp"
#include "App/Constants.hpp"

#include <boost/math/constants/constants.hpp>

#include <cmath>

b2Vec2 ECS::SteeringBehaviors::move(const b2Vec2& bodyPosition, Direction direction, const b2Vec2& bodyVelocity, float acceleration, float maxSpeed, float mass)
{
    switch (direction)
    {
    case Direction::Up:
        return { 0.f, mass * (b2Min(bodyVelocity.y + acceleration, maxSpeed) - bodyVelocity.y) };
    case Direction::Down:
        return { 0.f, mass * (b2Max(bodyVelocity.y - acceleration, -maxSpeed) - bodyVelocity.y) };
    case Direction::Right:
        return { mass * (b2Min(bodyVelocity.x + acceleration, maxSpeed) - bodyVelocity.x), 0.f };
    case Direction::Left:
        return { mass * (b2Max(bodyVelocity.x - acceleration, -maxSpeed) - bodyVelocity.x), 0.f };
    default:
        return { 0.f, 0.f };
    }
}

b2Vec2 ECS::SteeringBehaviors::seek(const b2Vec2& bodyPosition, const b2Vec2& targetPosition, const b2Vec2& bodyVelocity, float maxSpeed)
{
    return desiredVelocity(bodyPosition, targetPosition, maxSpeed) - bodyVelocity;
}

b2Vec2 ECS::SteeringBehaviors::flee(const b2Vec2& bodyPosition, const b2Vec2& targetPosition, const b2Vec2& bodyVelocity, float maxSpeed)
{
    return desiredVelocity(targetPosition, bodyPosition, maxSpeed) - bodyVelocity;
}

b2Vec2 ECS::SteeringBehaviors::pursue(const b2Vec2& bodyPosition, const b2Vec2& targetPosition, const b2Vec2& bodyVelocity, const b2Vec2& targetVelocity, float maxSpeed)
{
    auto predictionFrames = (targetPosition - bodyPosition).Length() / maxSpeed;

    return seek(bodyPosition, targetPosition + predictionFrames * targetVelocity, bodyVelocity, maxSpeed);
}

b2Vec2 ECS::SteeringBehaviors::evade(const b2Vec2& bodyPosition, const b2Vec2& targetPosition, const b2Vec2& bodyVelocity, const b2Vec2& targetVelocity, float maxSpeed)
{
    auto predictionFrames = (targetPosition - bodyPosition).Length() / maxSpeed;

    return flee(bodyPosition, targetPosition + predictionFrames * targetVelocity, bodyVelocity, maxSpeed);
}

b2Vec2 ECS::SteeringBehaviors::arrive(const b2Vec2& bodyPosition, const b2Vec2& targetPosition, const b2Vec2& bodyVelocity, float slowRadius, float maxSpeed)
{
    if (auto radius = (targetPosition - bodyPosition).Length(); radius < slowRadius)
    {
        return seek(bodyPosition, targetPosition, bodyVelocity, maxSpeed * radius / slowRadius);
    }

    return seek(bodyPosition, targetPosition, bodyVelocity, maxSpeed);
}

b2Vec2 ECS::SteeringBehaviors::wander(const b2Vec2& bodyPosition, const b2Vec2& bodyVelocity, float wanderDistance, float wanderRadius, float wanderRate, float& wanderAngle, float maxSpeed)
{
    auto wanderCenter = bodyVelocity;
    wanderCenter.Normalize();
    wanderCenter *= wanderDistance;
    wanderCenter += { wanderRadius* std::cos(wanderAngle), wanderRadius* std::sin(wanderAngle) };

    wanderAngle += App::Random::get(-1.f, 1.f) * wanderRate;

    return seek(bodyPosition, bodyPosition + wanderCenter, bodyVelocity, maxSpeed);
}

b2Vec2 ECS::SteeringBehaviors::orbit(const b2Vec2& satellitePosition, const b2Vec2& primaryPosition, const b2Vec2& bodyVelocity, float maxSpeed)
{
    auto radius = primaryPosition - satellitePosition;

    auto steeringForce = radius.Skew();
    steeringForce.Normalize();
    steeringForce *= maxSpeed;

    return steeringForce - bodyVelocity;
}

b2Vec2 ECS::SteeringBehaviors::alignForce(const b2Vec2& agentPosition, const std::vector<b2Vec2>& neighborVelocities, float alignmentForce)
{
    b2Vec2 steeringForce(0.f, 0.f);

    for (const auto& neighborVelocity : neighborVelocities)
    {
        steeringForce += neighborVelocity;
    }

    if (!neighborVelocities.empty())
    {
        steeringForce *= 1.f / neighborVelocities.size();
        steeringForce.Normalize();
        steeringForce *= alignmentForce;
    }

    return steeringForce;
}

b2Vec2 ECS::SteeringBehaviors::cohesionForce(const b2Vec2& agentPosition, const std::vector<b2Vec2>& neighborPositions, float cohesionForce)
{
    b2Vec2 steeringForce(0.f, 0.f);

    for (const auto& neighborPosition : neighborPositions)
    {
        steeringForce += neighborPosition;
    }

    if (!neighborPositions.empty())
    {
        steeringForce *= 1.f / neighborPositions.size();
        steeringForce -= agentPosition;
        steeringForce.Normalize();
        steeringForce *= cohesionForce;
    }

    return steeringForce;
}

b2Vec2 ECS::SteeringBehaviors::separateForce(const b2Vec2& agentPosition, const std::vector<b2Vec2>& neighborPositions, float separationForce)
{
    b2Vec2 steeringForce(0.f, 0.f);

    for (const auto& neighborPosition : neighborPositions)
    {
        steeringForce += neighborPosition - agentPosition;
    }

    if (!neighborPositions.empty())
    {
        steeringForce *= -1.f / neighborPositions.size();
        steeringForce.Normalize();
        steeringForce *= separationForce;
    }

    return steeringForce;
}

b2Vec2 ECS::SteeringBehaviors::desiredVelocity(const b2Vec2& bodyPosition, const b2Vec2& targetPosition, float maxSpeed)
{
    auto desiredVelocity = targetPosition - bodyPosition;
    desiredVelocity.Normalize();
    desiredVelocity *= maxSpeed;

    return desiredVelocity;
}

float ECS::SteeringBehaviors::face(float desiredAngle, float bodyAngle, float bodyAngularVelocity, float bodyInertia)
{
    auto nextAngle = bodyAngle + bodyAngularVelocity / App::FRAMES_PER_SECOND;
    auto totalRotation = std::remainderf(desiredAngle - nextAngle, 2 * boost::math::constants::pi<float>());

    return bodyInertia * totalRotation * App::FRAMES_PER_SECOND;
}

float ECS::SteeringBehaviors::face(const b2Vec2& bodyPosition, const b2Vec2& targetPosition, float bodyAngle, float bodyAngularVelocity, float bodyInertia)
{
    auto desiredAngle = std::atan2f(targetPosition.y - bodyPosition.y, targetPosition.x - bodyPosition.x);

    return face(desiredAngle, bodyAngle, bodyAngularVelocity, bodyInertia);
}


#include <boost/graph/minimum_degree_ordering.hpp>


#include "evaluation.h"
#include "predeal.h"
#include <boost/graph/graph_concepts.hpp>

using namespace std;

Eigen::Vector3f posediff(const Eigen::Vector3f& x, const Eigen::Vector3f &y)
{
  Eigen::Vector3f v;
  v(0)=abs(x(0)-y(0));
  v(1)=abs(x(1)-y(1));
  float x2,y2;
  if(x(2)<0)
  {
    x2=2*M_PI+x(2);
  }
  else
  {
    x2=x(2);
  }
  if(y(2)<0)
  {
    y2=2*M_PI+y(2);
  }
  else
  {
    y2=y(2);
  }
  v(2)=abs(x2-y2);
  return v;
}

int main()
{
   string posedir="/home/shaoan/projects/slamEvaluation/data/rgspose.txt";
  vector<Eigen::Vector3f> v_pose;

  predeal::readposefile(posedir,v_pose);
  string maposedir="/home/shaoan/projects/slamEvaluation/data/maPose.txt";
  vector<Eigen::Vector3f> v_mapose;

  predeal::readposefile(maposedir,v_mapose);

 assert(v_mapose.size()==v_pose.size());
 assert(v_pose.size()>0);

     vector<Eigen::Vector3f> differnce;
     for(int i=0;i<v_pose.size();i++)
     {
       Eigen::Vector3f v;
       v=posediff(v_mapose[i],v_pose[i]);
       //v=v_mapose[i]-v_pose[i];
       //cout<<v<<endl;
       differnce.push_back(v);
    }

    evaluation eval(differnce);
    Eigen::Vector2f v2;
   v2= eval.computeMSE();
   cout<<"平均绝对误差："<<endl;
   cout<<v2<<endl;
   Eigen::Vector3f v3;
   v3=eval.computeMAE();
  cout<<"平均绝对误差："<<endl;
  cout<<v3<<endl;

}


#include <random>
#include "Eigen/Dense"
#define _USE_MATH_DEFINES
#include <math.h>

#include "scalar-typedef.hpp"

#include "shared-probability-distributions.hpp"

#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif

using namespace Eigen;

// Explicit template instantiation(s)
template double lpdf_imulti_normal(const Eigen::Matrix<double,Eigen::Dynamic,1> &x,
                            const Eigen::Matrix<double,Eigen::Dynamic,1> &meanVec,
                            const Eigen::Matrix<double,Eigen::Dynamic,1> &sdVec);

template double lpdf_imulti_lognormal(const Eigen::Matrix<double,Eigen::Dynamic,1> &x,
                            const Eigen::Matrix<double,Eigen::Dynamic,1> &meanVec,
                                      const Eigen::Matrix<double,Eigen::Dynamic,1> &sdVec,
                                      unsigned int & ifail);

#ifdef USE_DCO_TYPES
template gt1s_scalar lpdf_imulti_normal(const Eigen::Matrix<gt1s_scalar,Eigen::Dynamic,1> &x,
                            const Eigen::Matrix<gt1s_scalar,Eigen::Dynamic,1> &meanVec,
                            const Eigen::Matrix<gt1s_scalar,Eigen::Dynamic,1> &sdVec);
template ga1s_scalar lpdf_imulti_normal(const Eigen::Matrix<ga1s_scalar,Eigen::Dynamic,1> &x,
                            const Eigen::Matrix<ga1s_scalar,Eigen::Dynamic,1> &meanVec,
                            const Eigen::Matrix<ga1s_scalar,Eigen::Dynamic,1> &sdVec);
template gt2s_ga1s_scalar lpdf_imulti_normal(const Eigen::Matrix<gt2s_ga1s_scalar,Eigen::Dynamic,1> &x,
                            const Eigen::Matrix<gt2s_ga1s_scalar,Eigen::Dynamic,1> &meanVec,
                            const Eigen::Matrix<gt2s_ga1s_scalar,Eigen::Dynamic,1> &sdVec);

template gt1s_scalar lpdf_imulti_lognormal(const Eigen::Matrix<gt1s_scalar,Eigen::Dynamic,1> &x,
                                           const Eigen::Matrix<gt1s_scalar,Eigen::Dynamic,1> &meanVec,
                                           const Eigen::Matrix<gt1s_scalar,Eigen::Dynamic,1> &sdVec,
                                           unsigned int & ifail);
template ga1s_scalar lpdf_imulti_lognormal(const Eigen::Matrix<ga1s_scalar,Eigen::Dynamic,1> &x,
                            const Eigen::Matrix<ga1s_scalar,Eigen::Dynamic,1> &meanVec,
                                           const Eigen::Matrix<ga1s_scalar,Eigen::Dynamic,1> &sdVec,
                                           unsigned int & ifail);
template gt2s_ga1s_scalar lpdf_imulti_lognormal(const Eigen::Matrix<gt2s_ga1s_scalar,Eigen::Dynamic,1> &x,
                                                const Eigen::Matrix<gt2s_ga1s_scalar,Eigen::Dynamic,1> &meanVec,
                                                const Eigen::Matrix<gt2s_ga1s_scalar,Eigen::Dynamic,1> &sdVec,
                                                unsigned int & ifail);
#endif

template<typename SCALAR_T>
SCALAR_T lpdf_univariate_normal(const SCALAR_T & x, const SCALAR_T & mu, const SCALAR_T & sd)
{
  const SCALAR_T logSqrt2Pi = 0.5*std::log(2*M_PI);
  const SCALAR_T z = x - mu;
  return - logSqrt2Pi - log(sd) - 0.5 * (z * z) / (sd * sd);
}

template<typename SCALAR_T>
SCALAR_T lpdf_univariate_lognormal(const SCALAR_T & x, const SCALAR_T & mu, const SCALAR_T & sd)
{
  const SCALAR_T logSqrt2Pi = 0.5*std::log(2*M_PI);
  const SCALAR_T z = log(x) - mu;
  return -log(x) - logSqrt2Pi - log(sd) - 0.5 * (z * z) / (sd * sd);
}

template<typename SCALAR_T>
SCALAR_T lpdf_imulti_lognormal(const Eigen::Matrix<SCALAR_T,Eigen::Dynamic,1> &x,
                               const Eigen::Matrix<SCALAR_T,Eigen::Dynamic,1> &meanVec,
                               const Eigen::Matrix<SCALAR_T,Eigen::Dynamic,1> &sdVec,
                               unsigned int & ifail)
{
  SCALAR_T lp = 0;
  for (int i=0; i < x.size(); ++i){
    if (x[i] < 0){
      ifail = 7;
      return - std::numeric_limits<SCALAR_T>::infinity();;
    }
    SCALAR_T mu = log( meanVec[i] / sqrt(1 + pow( sdVec[i] / meanVec[i] , 2.0) ) );
    SCALAR_T log_sd = sqrt( log( 1 + pow( sdVec[i] / meanVec[i] , 2.0)  ) );
    lp += lpdf_univariate_lognormal(x[i], mu, log_sd);
  }
  return lp;
}

template<typename SCALAR_T>
SCALAR_T lpdf_imulti_normal(const Eigen::Matrix<SCALAR_T,Eigen::Dynamic,1> &x,
                            const Eigen::Matrix<SCALAR_T,Eigen::Dynamic,1> &meanVec,
                            const Eigen::Matrix<SCALAR_T,Eigen::Dynamic,1> &sdVec)
{
  SCALAR_T lp = 0;
  for (int i=0; i < x.size(); ++i){
    lp += lpdf_univariate_normal(x[i], meanVec[i], sdVec[i]);
  }
  return lp;
}

// downloaded from https://stackoverflow.com/questions/41538095/evaluate-multivariate-normal-gaussian-density-in-c
double lpdf_multi_normal(const Eigen::VectorXd &x, const Eigen::VectorXd &meanVec,
                         const Eigen::MatrixXd &covMat)
{
    // avoid magic numbers in your code. Compilers will be able to compute this at compile time:
    const double logSqrt2Pi = 0.5*std::log(2*M_PI);
    typedef Eigen::LLT<Eigen::MatrixXd> Chol;
    Chol chol(covMat);
    // Handle non positive definite covariance somehow:
    if(chol.info()!=Eigen::Success) throw "decomposition failed!";
    const Chol::Traits::MatrixL& L = chol.matrixL();
    Eigen::VectorXd centredVec = (x - meanVec);
    // solve  L y = (x - mu) for y
    double quadform = (L.solve(centredVec)).squaredNorm();
    // log(|Sigma|^{-1/2} = - 0.5 log( |Sigma| )
    return -x.rows()*logSqrt2Pi - 0.5*quadform - 0.5 * covMat.colPivHouseholderQr().logAbsDeterminant() ;
}

// Note - this is not an optimal implementation if generating many samples from dist with the same covariance matrix
Matrix<double,Dynamic,1> rand_multi_normal(const Matrix<double,Dynamic,1> mean,
                                           Matrix<double,Dynamic,Dynamic> cov,
                                           std::mt19937 & gen)
{
  std::normal_distribution<> std_normal_rng;
  Eigen::LLT<Eigen::MatrixXd> llt_of_cov = cov.llt();
  Eigen::VectorXd z(cov.cols());
  for (int i = 0; i < cov.cols(); ++i){
    z(i) = std_normal_rng(gen);
  }
  MatrixXd L = llt_of_cov.matrixL();
  return mean + L  * z;
}

// Adapted from https://stackoverflow.com/questions/41538095/evaluate-multivariate-normal-gaussian-density-in-c
#undef MYPREC_VERSION
#ifdef MYPREC_VERSION
double lpdf_multi_normal(const Eigen::VectorXd &x, const Eigen::VectorXd &meanVec, const Eigen::MatrixXd &precMat)
{
    // avoid magic numbers in your code. Compilers will be able to compute this at compile time:
    // const double logSqrt2Pi = 0.5*std::log(2*M_PI);
    typedef Eigen::LLT<Eigen::MatrixXd> llt_of_prec;
    llt_of_prec chol(precMat);
    // Handle non positive definite covariance somehow:
    if(chol.info()!=Eigen::Success) throw "decomposition failed!";
    const llt_of_prec::Traits::MatrixL& L = chol.matrixL();
    double quadform = (L * (x - meanVec)).squaredNorm();
    // return std::exp(-x.rows()*logSqrt2Pi - 0.5*quadform) / L.determinant();
    return - 0.5*quadform +  0.5 * precMat.colPivHouseholderQr().logAbsDeterminant();
}
#endif

#undef STACKOVERFLOW_VERSION
#ifdef STACKOVERFLOW_VERSION
// From https://stackoverflow.com/questions/6142576/sample-from-multivariate-normal-gaussian-distribution-in-c
// October 2019
struct normal_random_variable
{
    normal_random_variable(Eigen::MatrixXd const& covar)
        : normal_random_variable(Eigen::VectorXd::Zero(covar.rows()), covar)
    {}

    normal_random_variable(Eigen::VectorXd const& mean, Eigen::MatrixXd const& covar)
        : mean(mean)
    {
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(covar);
        transform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();
    }

    Eigen::VectorXd mean;
    Eigen::MatrixXd transform;

    Eigen::VectorXd operator()() const
    {
        static std::mt19937 gen{ std::random_device{}() };
        static std::normal_distribution<> dist;

        return mean + transform * Eigen::VectorXd{ mean.size() }.unaryExpr([&](auto x) { return dist(gen); });
    }
};
#endif

#undef MY_COV_VERSION
#ifdef MY_COV_VERSION
// Note - this is less efficient than stackoverflow version if generating many samples from dist with the same covariance matrix
Matrix<double,Dynamic,1> rand_multi_normal(const Matrix<double,Dynamic,1> mean,
                                           Matrix<double,Dynamic,Dynamic> covar,
                                           std::mt19937 & gen,
                                           std::normal_distribution<> & dist)
{
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(covar);
  Matrix<double,Dynamic,Dynamic> transform m= eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseSqrt().asDiagonal();
  return mean + transform * Eigen::VectorXd{ mean.size() }.unaryExpr([&](double x) { return dist(gen); });
}
#endif

#undef STAN_PREC_VERSION
#ifdef STAN_PREC_VERSION
// From https://mc-stan.org/math/d4/d28/multi__normal__prec__rng_8hpp_source.html
// October 2019

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/mat/err/check_pos_definite.hpp>
#include <stan/math/prim/mat/err/check_symmetric.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
  namespace math {

    template <typename T_loc, class RNG>
    inline typename StdVectorBuilder<true, Eigen::VectorXd, T_loc>::type
    multi_normal_prec_rng(const T_loc &mu, const Eigen::MatrixXd &S, RNG &rng) {
      using boost::normal_distribution;
      using boost::variate_generator;

      static const char *function = "multi_normal_prec_rng";

      check_positive(function, "Precision matrix rows", S.rows());
      check_finite(function, "Precision matrix", S);
      check_symmetric(function, "Precision matrix", S);

      Eigen::LLT<Eigen::MatrixXd> llt_of_S = S.llt();
      check_pos_definite(function, "precision matrix argument", llt_of_S);

      vector_seq_view<T_loc> mu_vec(mu);
      check_positive(function, "number of location parameter vectors",
                     mu_vec.size());
      size_t size_mu = mu_vec[0].size();

      size_t N = mu_vec.size();

      for (size_t i = 1; i < N; i++) {
        int size_mu_new = mu_vec[i].size();
        check_size_match(function,
                         "Size of one of the vectors of "
                         "the location variable",
                         size_mu_new,
                         "Size of another vector of the "
                         "location variable",
                         size_mu);
      }

      for (size_t i = 0; i < N; i++) {
        check_finite(function, "Location parameter", mu_vec[i]);
      }

      check_size_match(function, "Rows of location parameter", size_mu, "Rows of S",
                       S.rows());

      StdVectorBuilder<true, Eigen::VectorXd, T_loc> output(N);

      variate_generator<RNG &, normal_distribution<>> std_normal_rng(
                                                                     rng, normal_distribution<>(0, 1));

      for (size_t n = 0; n < N; ++n) {
        Eigen::VectorXd z(S.cols());
        for (int i = 0; i < S.cols(); i++)
          z(i) = std_normal_rng();

        output[n] = Eigen::VectorXd(mu_vec[n]) + llt_of_S.matrixU().solve(z);
      }

      return output.data();
    }

  }  // namespace math
}  // namespace stan
#endif


#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include <cmath>
#include <vector>

#include <boost/lexical_cast.hpp>
#include <boost/assign/list_of.hpp>
using boost::assign::list_of;
using boost::assign::map_list_of;

#include <evaluation/core/PerformanceMeasureUtil.h>
using namespace evaluation;

#include <tvgutil/containers/MapUtil.h>
using namespace tvgutil;

//#################### TESTS ####################

BOOST_AUTO_TEST_SUITE(test_PerformanceMeasureUtil)

BOOST_AUTO_TEST_CASE(average_measures_test)
{
  const float TOL = 1e-5f;

  #define CHECK_CLOSE(L,R) BOOST_CHECK_CLOSE(L,R,TOL)

  PerformanceMeasure m1 = PerformanceMeasureUtil::average_measures(list_of(1.0f)(2.0f)(3.0f));
  CHECK_CLOSE(m1.get_mean(), 2.0f);
  BOOST_CHECK_EQUAL(m1.get_sample_count(), 3);
  CHECK_CLOSE(m1.get_std_dev(), sqrtf(2.0f/3.0f));
  CHECK_CLOSE(m1.get_variance(), 2.0f/3.0f);

  PerformanceMeasure m2 = PerformanceMeasureUtil::average_measures(list_of(4.0f)(5.0f));
  CHECK_CLOSE(m2.get_mean(), 4.5f);
  BOOST_CHECK_EQUAL(m2.get_sample_count(), 2);
  CHECK_CLOSE(m2.get_std_dev(), 0.5f);
  CHECK_CLOSE(m2.get_variance(), 0.25f);

  PerformanceMeasure avg = PerformanceMeasureUtil::average_measures(list_of(m1)(m2));
  CHECK_CLOSE(avg.get_mean(), 3.0f);
  BOOST_CHECK_EQUAL(avg.get_sample_count(), 5);
  CHECK_CLOSE(avg.get_std_dev(), sqrtf(2.0f));
  CHECK_CLOSE(avg.get_variance(), 2.0f);

  #undef CHECK_CLOSE
}

BOOST_AUTO_TEST_CASE(average_results_test)
{
  PerformanceResult r1 = map_list_of("A",1.0f)("B",2.0f);
  PerformanceResult r2 = map_list_of("A",2.0f)("B",1.0f);
  PerformanceResult r3 = map_list_of("C",3.0f);

  std::vector<PerformanceResult> results = list_of(r1)(r2)(r3);
  PerformanceResult rAvg = PerformanceMeasureUtil::average_results(results);

  BOOST_CHECK_EQUAL(boost::lexical_cast<std::string>(MapUtil::lookup(rAvg, "A")),"1.5 +/- 0.5 (2 samples)");
  BOOST_CHECK_EQUAL(boost::lexical_cast<std::string>(MapUtil::lookup(rAvg, "B")),"1.5 +/- 0.5 (2 samples)");
  BOOST_CHECK_EQUAL(boost::lexical_cast<std::string>(MapUtil::lookup(rAvg, "C")),"3 +/- 0 (1 samples)");
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Tsai algorithm 1989, initially ported to Eigen from VISP and then manually modified for efficiency, clearness and more Eigenability
 *
 * Copyright Scuola Superiore Sant'Anna (2016)
 * Emanuele Ruffaldi, e.ruffaldi@sssup.it
 *
 * TODO: exclude collinear pairs
 * TODO: compute residuals for providing a measure of the error
 *
 * Structure optimization (double the memory but faster)
 *    A1,A2,b1,b2,tmp
 *    loop all pairs
 *      exclude bad pairs
 *      write A1 and A2
 *      write b1 and partial b2 the part that doesn't require Rcg
 *    solve Rcg using A1,b1
 *    update b2 using eRcg*cijo.matrix().template block<3,1>(0,3) (so we need to store cijo translation part into tmp)
 *    solve translation for A2,b2
 */
#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include "tsai.hpp"

/// extracts the translation part
template <class T>
static Vector3<T> gettx(const Matrix4<T> & x)
{
  return x.template block<3,1>(0,3);
}

/// extracts the translation part
template <class T>
static Vector3<T> gettx(const Eigen::Transform<T,3,Eigen::Affine> & x)
{
  return x.matrix().template block<3,1>(0,3);
}

/// extracts rotation
template <class T>
static Matrix3<T> getrot(const Eigen::Transform<T,3,Eigen::Affine> & x)
{
  return x.matrix().template block<3,3>(0,0);
}

/// extracts rotation
template <class T>
static Matrix3<T> getrot(const Matrix4<T> & x)
{
  return x.template block<3,3>(0,0);
}

/// extracts quat and transform from matrix 4x4
template <class T>
static void extract(const Matrix4<T> & x,Eigen::Quaternion<T> & q, Vector3<T> & t)
{
  q = x.template block<3,3>(0,0);
  t = x.template block<3,1>(0,3);
}

/// extract rotation
template <class T>
static void extract(const Matrix4<T> & x,Eigen::Quaternion<T> & q)
{
  q = x.template block<3,3>(0,0);
}

/// makes affine3f from matrix using extraction
template <class T>
static void extract(const Matrix4<T> & x,Eigen::Transform<T,3,Eigen::Affine> & a)
{
  auto q = x.template block<3,3>(0,0);
  auto t = x.template block<3,1>(0,3);
  a.fromPositionOrientationScale(t,q,Vector3<T>(1,1,1));
}

template <class T>
static inline T sinc(T sx,T x)
{
  return fabs(x) < 1e-8 ? 1 : sx/x;
}

template <class T>
static inline T sinc(T x)
{
  return fabs(x) < 1e-8 ? 1 : sin(x)/x;
}

// modified tsai is: 2 sin(theta/2) axis
// BUT quaternion real part is:  sin(theta/2) * axis
// equation 9
// THIS IS FROM VISP
template <class T>
Vector3<T> quat2paratsai(const Eigen::Quaternion<T> & q)
{
  //Eigen::AngleAxisf aa(q);
  //return 2*sin(aa.angle()/2)*aa.axis();
  //return 2*Vector3<T>(q.x(),q.y(),q.z());
  const double minimum = 0.0001;
  Matrix3<T> R(q);
  Vector3<T> r;

  double s = (R(1,0)-R(0,1))*(R(1,0)-R(0,1))
    + (R(2,0)-R(0,2))*(R(2,0)-R(0,2))
    + (R(2,1)-R(1,2))*(R(2,1)-R(1,2));
   s = sqrt(s)/2.0;
   double c = (R(0,0)+R(1,1)+R(2,2)-1.0)/2.0;
   double theta=atan2(s,c);  /* theta in [0, PI] since s > 0 */

  // General case when theta != pi. If theta=pi, c=-1
  if ( (1+c) > minimum) // Since -1 <= c <= 1, no fabs(1+c) is required
  {
    double si = sinc(s,theta);

    r[0] = (R(2,1)-R(1,2))/(2*si);
    r[1] = (R(0,2)-R(2,0))/(2*si);
    r[2] = (R(1,0)-R(0,1))/(2*si);
  }
  else /* theta near PI */
  {
    if ( (R(0,0)-c) < std::numeric_limits<double>::epsilon() )
      r[0] = 0.;
    else
      r[0] = theta*(sqrt((R(0,0)-c)/(1-c)));
    if ((R(2,1)-R(1,2)) < 0) r[0] = -r[0];

    if ( (R(1,1)-c) < std::numeric_limits<double>::epsilon() )
      r[1] = 0.;
    else
      r[1] = theta*(sqrt((R(1,1)-c)/(1-c)));

    if ((R(0,2)-R(2,0)) < 0) r[1] = -r[1];

    if ( (R(2,2)-c) < std::numeric_limits<double>::epsilon() )
      r[2] = 0.;
    else
      r[2] = theta*(sqrt((R(2,2)-c)/(1-c)));

    if ((R(1,0)-R(0,1)) < 0) r[2] = -r[2];
  }
  return r;

}

template <class T>
T paratsaiprime2theta(const Vector3<T> & x)
{
  return 2*atan(x.norm()); // eq. 13
}

// port and improve: https://github.com/thomas-moulard/visp-deb/blob/master/src/camera/calibration/vpCalibrationTools.cpp
template<class T>
T calibrationTsaiT(const std::vector<Matrix4<T> > & cMo,const std::vector<Matrix4<T> > & rMe, Matrix4<T> &eMc, bool computeResidual)
{
  assert(cMo.size() == rMe.size() && !cMo.empty() && "not empty calibration matrices");
  const unsigned int nbPose = cMo.size();
  Vector3<T> x; // tsaipara result
  Eigen::Matrix<T,Eigen::Dynamic,3> A((nbPose*(nbPose-1)/2)*3,3); // reused between the two loops over all pairs
  Eigen::Matrix<T,Eigen::Dynamic,1> B(A.rows(),1);
  {
    unsigned int k = 0 ;
    // for all couples ij
    for (unsigned int i=0 ; i < nbPose ; i++)
    {
      Eigen::Quaternion<T> eiR,ioR;
      extract(rMe[i],eiR);
      extract(cMo[i],ioR);

      for (unsigned int j=i+1; j < nbPose ; j++, k+= 3)
      {
          Eigen::Quaternion<T> ejR,joR;
          extract(rMe[j],ejR);
          extract(cMo[j],joR);

          Eigen::Quaternion<T> rRgij = ejR.conjugate() * eiR;
          Eigen::Quaternion<T>  cRijo = joR * ioR.conjugate();

          // CODE: theta = sqrt(|rotationalpart of rRgij|) then multiply rotation part by sinc(theta/2)
          // going from norm theta to scaled by sinc

          // rotation axis (eq 11a,11b,11c)
          Vector3<T>  rPgij = quat2paratsai<T>(rRgij);
          Vector3<T>  cPijo = quat2paratsai<T>(cRijo);

          // TODO: in paper we can remove pair if not good

          // skewtsai(Pgij+Pcij) Pcg' = Pcij - Pgij  equation 12
          A.template block<3,3>(k,0) = skewtsai(Vector3<T>(rPgij + cPijo)) ;
          B.template block<3,1>(k,0) = cPijo - rPgij;

#if 0
          if(k == 0)
          {
            std::cout << "rPeij0 R\n" << Matrix3<T>(rRgij) << std::endl;
            std::cout << "rPeij0\n" << rPgij.transpose() << std::endl;
            std::cout << "rPeij0\n" << Eigen::AngleAxisf(rRgij).angle() << "|" << Eigen::AngleAxisf(rRgij).axis() << std::endl;
            std::cout << "cijPo\n" << cPijo.transpose() << std::endl;

            std::cout << "As0\n " << A.template block<3,3>(0,0) << " \n b0" << B.template block<3,1>(0,0).transpose() << std::  endl;
          }
#endif
      }
    }

    // the output is in the MODIFIED form
    x = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);

    // convert output to PARAMETRIC eq. 14
    // Pcg' = 1/(4-|Pcg|^2) Pcg
    // Pcg  = 2 Pcg' /(1+|Pcg|^2)
    x = paratsaiprime2paratsai(x);

    // verification of the quality of the vector
    const double theta = paratsai2theta(x);

    if (std::fabs(theta) < std::numeric_limits<double>::epsilon())
      x.setZero();
    // x *= theta/(2*sin(theta/2));     // paratsai => para NOT NEEDED we work in paratsai
  }

  // Building of the rotation matrix eRc using eq. 10
  Matrix3<T>  eRcg = paratsai2rot(x);

  {
    // Building of the system for the translation estimation
    // for all couples ij
    unsigned int k = 0 ;
    for (unsigned int i = 0 ; i < nbPose ; i++)
    {
      /*
      Eigen::Affine3f eiA,ioA;
      extract(rMe[i],eiA);
      extract(cMo[i],ioA);
      */

      Vector3<T>  eiT = gettx(rMe[i]);
      Vector3<T>  ioT = gettx(cMo[i]);
      Matrix3<T>  eiR = getrot(rMe[i]);

      for (unsigned int j = i+1 ; j < nbPose ; j++, k+= 3)
      {
          /*
          Eigen::Affine3f ejA,joA;
          extract(rMe[j],rejA);
          extract(cMo[j],joA);
          Eigen::Affine3f jiA = ejA.inverse() * eiA; // gij of paper
          */
          Vector3<T>  ejT = gettx(rMe[j]);
          Vector3<T> joT = gettx(cMo[j]);
          Matrix3<T> ejR = getrot(rMe[j]);
          Matrix3<T> jiR = ejR.transpose()*eiR;

          // TODO: in paper we can remove pair if not good COLLINEARITY ISSUE

          // equation 15: (Rgij-I) T = Rcg Tcij - Tgij
          A.template block<3,3>(k,0) = jiR-Matrix3<T>::Identity();
          B.template block<3,1>(k,0) = eRcg*joT - jiR*eRcg*ioT + ejR.transpose()*(ejT-eiT);
      }
    }

    eMc.setIdentity();
    eMc.template block<3,3>(0,0) = eRcg;
    eMc.template block<3,1>(0,3) = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);

    // A (n*3,3) x(3,1) - B(n*3,1)
    return computeResidual ? (A*eMc.template block<3,1>(0,3)-B).squaredNorm()/A.rows() : 0; // this is the variance of result
 }
}

float calibrationTsai(const std::vector<Eigen::Matrix4f> & cMo,
                      const std::vector<Eigen::Matrix4f> & rMe,
                      Eigen::Matrix4f &eMc,
                      bool computeResidual)
{
    return calibrationTsaiT<float>(cMo,rMe,eMc,computeResidual);
}

double calibrationTsai(const std::vector<Eigen::Matrix4d> & cMo,
                       const std::vector<Eigen::Matrix4d> & rMe,
                       Eigen::Matrix4d &eMc,bool computeResidual)
{
    return calibrationTsaiT<double>(cMo,rMe,eMc,computeResidual);
}