/*
 * This timing code is based on the benchmarking code as written in the Pinocchio repository
 * clang++-12 -std=c++11 -o timePinocchio.exe timePinocchio.cpp -O3 -DPINOCCHIO_URDFDOM_TYPEDEF_SHARED_PTR -DPINOCCHIO_WITH_URDFDOM -lboost_system -lpinocchio -lurdfdom_model -lpthread -ldl
 * eample usage: timePinocchio.exe urdfs/atlas.urdf
 */
#include "util/experiment_helpers.h" // include constants and other experiment consistency helpers
#include "ReusableThreads/ReusableThreads.h" // multi-threading wrapper

#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/algorithm/cholesky.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
#include "pinocchio/algorithm/rnea-derivatives.hpp"
#include "pinocchio/algorithm/aba-derivatives.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"

#include "pinocchio/codegen/cppadcg.hpp"
#include "pinocchio/codegen/code-generator-algo.hpp"

#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/sample-models.hpp"

#include "pinocchio/container/aligned-vector.hpp"

#include <Eigen/StdVector>
// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)

using namespace Eigen;
using namespace pinocchio;

#define time_delta_us_timespec(start,end) (1e6*static_cast<double>(end.tv_sec - start.tv_sec)+1e-3*static_cast<double>(end.tv_nsec - start.tv_nsec))

template<typename T>
void inverseDynamicsThreaded_codegen_inner(CodeGenRNEA<T> *rnea_code_gen, int nq, int nv, \
                                           Matrix<T, Dynamic, 1> *qs, Matrix<T, Dynamic, 1> *qds, int tid, int kStart, int kMax){
    Matrix<T, Dynamic, 1> zeros = Matrix<T, Dynamic, 1>::Zero(nv);
    for(int k = kStart; k < kMax; k++){
        rnea_code_gen->evalFunction(qs[k],qds[k],zeros);
    }
}

template<typename T, int NUM_THREADS, int NUM_TIME_STEPS>
void inverseDynamicsThreaded_codegen(CodeGenRNEA<T> **rnea_code_gen_arr, int nq, int nv, \
                                     Matrix<T, Dynamic, 1> *qs, Matrix<T, Dynamic, 1> *qds, ReusableThreads<NUM_THREADS> *threads){
        for (int tid = 0; tid < NUM_THREADS; tid++){
            int kStart = NUM_TIME_STEPS/NUM_THREADS*tid; int kMax = NUM_TIME_STEPS/NUM_THREADS*(tid+1);
            if(tid == NUM_THREADS-1){kMax = NUM_TIME_STEPS;}
            threads->addTask(tid, &inverseDynamicsThreaded_codegen_inner<T>, std::ref(rnea_code_gen_arr[tid]), nq, nv,
                                                                             std::ref(qs), std::ref(qds), tid, kStart, kMax);
        }
        threads->sync();
}

template<typename T>
void minvThreaded_codegen_inner(CodeGenMinv<T> *minv_code_gen, int nq, int nv, Matrix<T, Dynamic, 1> *qs, int tid, int kStart, int kMax){
    for(int k = kStart; k < kMax; k++){
        minv_code_gen->evalFunction(qs[k]);
    }
}

template<typename T, int NUM_THREADS, int NUM_TIME_STEPS>
void minvThreaded_codegen(CodeGenMinv<T> **minv_code_gen_arr, int nq, int nv, Matrix<T, Dynamic, 1> *qs, ReusableThreads<NUM_THREADS> *threads){
        for (int tid = 0; tid < NUM_THREADS; tid++){
            int kStart = NUM_TIME_STEPS/NUM_THREADS*tid; int kMax = NUM_TIME_STEPS/NUM_THREADS*(tid+1);
            if(tid == NUM_THREADS-1){kMax = NUM_TIME_STEPS;}
            threads->addTask(tid, &minvThreaded_codegen_inner<T>, std::ref(minv_code_gen_arr[tid]), nq, nv, std::ref(qs), tid, kStart, kMax);
        }
        threads->sync();
}

template<typename T>
void forwardDynamicsThreaded_codegen_inner(CodeGenMinv<T> *minv_code_gen, CodeGenRNEA<T> *rnea_code_gen, int nq, int nv, \
                                           Matrix<T, Dynamic, 1> *qs, Matrix<T, Dynamic, 1> *qds, Matrix<T, Dynamic, 1> *qdds, \
                                           Matrix<T, Dynamic, 1> *us, int tid, int kStart, int kMax){
    Matrix<T, Dynamic, 1> zeros = Matrix<T, Dynamic, 1>::Zero(nv);
    for(int k = kStart; k < kMax; k++){
        minv_code_gen->evalFunction(qs[k]);
        minv_code_gen->Minv.template triangularView<Eigen::StrictlyLower>() =
            minv_code_gen->Minv.transpose().template triangularView<Eigen::StrictlyLower>();
        rnea_code_gen->evalFunction(qs[k],qds[k],zeros);
        qdds[k].noalias() = minv_code_gen->Minv*(us[k] - rnea_code_gen->res);
    }
}

template<typename T, int NUM_THREADS, int NUM_TIME_STEPS>
void forwardDynamicsThreaded_codegen(CodeGenMinv<T> **minv_code_gen_arr, CodeGenRNEA<T> **rnea_code_gen_arr, int nq, int nv, \
                                     Matrix<T, Dynamic, 1> *qs, Matrix<T, Dynamic, 1> *qds, Matrix<T, Dynamic, 1> *qdds, \
                                     Matrix<T, Dynamic, 1> *us, ReusableThreads<NUM_THREADS> *threads){
        for (int tid = 0; tid < NUM_THREADS; tid++){
            int kStart = NUM_TIME_STEPS/NUM_THREADS*tid; int kMax = NUM_TIME_STEPS/NUM_THREADS*(tid+1);
            if(tid == NUM_THREADS-1){kMax = NUM_TIME_STEPS;}
            threads->addTask(tid, &forwardDynamicsThreaded_codegen_inner<T>, std::ref(minv_code_gen_arr[tid]),
                                                                             std::ref(rnea_code_gen_arr[tid]), nq, nv,
                                                                             std::ref(qs), std::ref(qds), std::ref(qdds), std::ref(us),
                                                                             tid, kStart, kMax);
        }
        threads->sync();
}

template<typename T>
void inverseDynamicsGradientThreaded_codegen_inner(CodeGenRNEADerivatives<T> *rnea_derivatives_code_gen, \
                                                   int nq, int nv, Matrix<T, Dynamic, 1> *qs, Matrix<T, Dynamic, 1> *qds, \
                                                   int tid, int kStart, int kMax){
    Matrix<T, Dynamic, 1> zeros = Matrix<T, Dynamic, 1>::Zero(nv);
    for(int k = kStart; k < kMax; k++){
        rnea_derivatives_code_gen->evalFunction(qs[k],qds[k],zeros);
    }
}

template<typename T, int NUM_THREADS, int NUM_TIME_STEPS>
void inverseDynamicsGradientThreaded_codegen(CodeGenRNEADerivatives<T> **rnea_derivatives_code_gen_arr, \
                                             int nq, int nv, Matrix<T, Dynamic, 1> *qs, Matrix<T, Dynamic, 1> *qds,
                                             ReusableThreads<NUM_THREADS> *threads){
        for (int tid = 0; tid < NUM_THREADS; tid++){
            int kStart = NUM_TIME_STEPS/NUM_THREADS*tid; int kMax = NUM_TIME_STEPS/NUM_THREADS*(tid+1);
            if(tid == NUM_THREADS-1){kMax = NUM_TIME_STEPS;}
            threads->addTask(tid, &inverseDynamicsGradientThreaded_codegen_inner<T>, std::ref(rnea_derivatives_code_gen_arr[tid]),
                                                                                nq, nv, std::ref(qs), std::ref(qds), tid, kStart, kMax);
        }
        threads->sync();
}

template<typename T>
void forwardDynamicsGradientThreaded_codegen_inner(CodeGenRNEADerivatives<T> *rnea_derivatives_code_gen, \
                                                   CodeGenMinv<T> *minv_code_gen, CodeGenRNEA<T> *rnea_code_gen, \
                                                   int nq, int nv, Matrix<T, Dynamic, Dynamic> *dqdd_dqs, Matrix<T, Dynamic, Dynamic> *dqdd_dvs, \
                                                   Matrix<T, Dynamic, 1> *qs, Matrix<T, Dynamic, 1> *qds, Matrix<T, Dynamic, 1> *us, \
                                                   int tid, int kStart, int kMax){
    Matrix<T, Dynamic, 1> zeros = Matrix<T, Dynamic, 1>::Zero(nv);
    for(int k = kStart; k < kMax; k++){
        minv_code_gen->evalFunction(qs[k]);
        minv_code_gen->Minv.template triangularView<Eigen::StrictlyLower>() =
            minv_code_gen->Minv.transpose().template triangularView<Eigen::StrictlyLower>();
        rnea_code_gen->evalFunction(qs[k],qds[k],zeros);
        Matrix<T, Dynamic, 1> qdd = minv_code_gen->Minv*(us[k] - rnea_code_gen->res);
        rnea_derivatives_code_gen->evalFunction(qs[k],qds[k],qdd);
        dqdd_dqs[k].noalias() = -(minv_code_gen->Minv)*(rnea_derivatives_code_gen->dtau_dq);
        dqdd_dvs[k].noalias() = -(minv_code_gen->Minv)*(rnea_derivatives_code_gen->dtau_dv);
    }
}

template<typename T, int NUM_THREADS, int NUM_TIME_STEPS>
void forwardDynamicsGradientThreaded_codegen(CodeGenRNEADerivatives<T> **rnea_derivatives_code_gen_arr, \
                                             CodeGenMinv<T> **minv_code_gen_arr, CodeGenRNEA<T> **rnea_code_gen_arr, \
                                             int nq, int nv, Matrix<T, Dynamic, Dynamic> *dqdd_dqs, Matrix<T, Dynamic, Dynamic> *dqdd_dvs, \
                                             Matrix<T, Dynamic, 1> *qs, Matrix<T, Dynamic, 1> *qds, Matrix<T, Dynamic, 1> *us, \
                                             ReusableThreads<NUM_THREADS> *threads){
        for (int tid = 0; tid < NUM_THREADS; tid++){
            int kStart = NUM_TIME_STEPS/NUM_THREADS*tid; int kMax = NUM_TIME_STEPS/NUM_THREADS*(tid+1);
            if(tid == NUM_THREADS-1){kMax = NUM_TIME_STEPS;}
            threads->addTask(tid, &forwardDynamicsGradientThreaded_codegen_inner<T>, std::ref(rnea_derivatives_code_gen_arr[tid]),
                                                                                std::ref(minv_code_gen_arr[tid]),
                                                                                std::ref(rnea_code_gen_arr[tid]), nq, nv,
                                                                                std::ref(dqdd_dqs), std::ref(dqdd_dvs),
                                                                                std::ref(qs), std::ref(qds), std::ref(us),
                                                                                tid, kStart, kMax);
    }
        threads->sync();
}

template<typename T, int TEST_ITERS, int NUM_THREADS, int NUM_TIME_STEPS>
void test(std::string urdf_filepath){
    // Setup timer
    struct timespec start, end;

    // Matrix typedefs
    typedef Matrix<T, Dynamic, Dynamic> MatrixXT;
    typedef Matrix<T, Dynamic, 1> VectorXT;

    // Import URDF model and prepare pinnochio
    Model model;
    pinocchio::urdf::buildModel(urdf_filepath,model);
    // model.gravity.setZero();
    model.gravity.linear(Eigen::Vector3d(0,0,-9.81));
    Data datas[NUM_THREADS];
    for(int i = 0; i < NUM_THREADS; i++){datas[i] = Data(model);}

    // generate the code_gen
    CodeGenRNEA<T> rnea_code_gen(model.cast<T>());
    rnea_code_gen.initLib();
    rnea_code_gen.loadLib();

    CodeGenRNEA<T> *rnea_code_gen_arr[NUM_THREADS];
    for (int i = 0; i < NUM_THREADS; i++){
        rnea_code_gen_arr[i] = new CodeGenRNEA<T>(model.cast<T>());
        rnea_code_gen_arr[i]->initLib();
        rnea_code_gen_arr[i]->loadLib();
    }

    CodeGenMinv<T> minv_code_gen(model.cast<T>());
    minv_code_gen.initLib();
    minv_code_gen.loadLib();

    CodeGenMinv<T> *minv_code_gen_arr[NUM_THREADS];
    for (int i = 0; i < NUM_THREADS; i++){
        minv_code_gen_arr[i] = new CodeGenMinv<T>(model.cast<T>());
        minv_code_gen_arr[i]->initLib();
        minv_code_gen_arr[i]->loadLib();
    }

    CodeGenRNEADerivatives<T> rnea_derivatives_code_gen(model.cast<T>());
    rnea_derivatives_code_gen.initLib();
    rnea_derivatives_code_gen.loadLib();

    CodeGenRNEADerivatives<T> *rnea_derivatives_code_gen_arr[NUM_THREADS];
    for (int i = 0; i < NUM_THREADS; i++){
        rnea_derivatives_code_gen_arr[i] = new CodeGenRNEADerivatives<T>(model.cast<T>());
        rnea_derivatives_code_gen_arr[i]->initLib();
        rnea_derivatives_code_gen_arr[i]->loadLib();
    }

    // allocate and load on CPU
    VectorXT qs[NUM_TIME_STEPS];
    VectorXT qds[NUM_TIME_STEPS];
    VectorXT qdds[NUM_TIME_STEPS];
    VectorXT us[NUM_TIME_STEPS];
    MatrixXT dqdd_dqs[NUM_TIME_STEPS];
    MatrixXT dqdd_dvs[NUM_TIME_STEPS];
    for(int i = 0; i < NUM_TIME_STEPS; i++){
        qs[i] = VectorXT::Zero(model.nq);
        qds[i] = VectorXT::Zero(model.nv);
        qdds[i] = VectorXT::Zero(model.nv);
        us[i] = VectorXT::Zero(model.nv);
        dqdd_dqs[i] = MatrixXT::Zero(model.nv,model.nq);
        dqdd_dvs[i] = MatrixXT::Zero(model.nv,model.nv);
        for(int j = 0; j < model.nq; j++){qs[i][j] = getRand<T>(); qds[i][j] = getRand<T>(); us[i][j] = getRand<T>();}
    }

    #if TEST_FOR_EQUIVALENCE
        std::cout << "q,qd,u" << std::endl;
        std::cout << qs[0].transpose() << std::endl;
        std::cout << qds[0].transpose() << std::endl;
        std::cout << us[0].transpose() << std::endl;
        // Minv
        computeMinverse(model,datas[0],qs[0]);
        std::cout << "Minv" << std::endl << datas[0].Minv << std::endl;
        datas[0].Minv.template triangularView<Eigen::StrictlyLower>() =
            datas[0].Minv.transpose().template triangularView<Eigen::StrictlyLower>();
        MatrixXT Minv = datas[0].Minv;
        // qdd
        aba(model,datas[0],qs[0],qds[0],us[0]);
        qdds[0] = datas[0].ddq;
        std::cout << "qdd" << std::endl << qdds[0].transpose() << std::endl;
        // dc/du with qdd=0
        MatrixXT drnea_dq = MatrixXT::Zero(model.nq,model.nq);
        MatrixXT drnea_dv = MatrixXT::Zero(model.nv,model.nv);
        MatrixXT drnea_da = MatrixXT::Zero(model.nv,model.nv);
        computeRNEADerivatives(model,datas[0],qs[0],qds[0],VectorXT::Zero(model.nv),drnea_dq,drnea_dv,drnea_da);
        std::cout << "dc_dq" << std::endl << drnea_dq << std::endl;
        std::cout << "dc_dqd" << std::endl << drnea_dv << std::endl;
        // df/du (via dc/du with qdd)
        computeRNEADerivatives(model,datas[0],qs[0],qds[0],qdds[0],drnea_dq,drnea_dv,drnea_da);
        dqdd_dqs[0] = -Minv*drnea_dq;
        dqdd_dvs[0] = -Minv*drnea_dv;
        std::cout << "df_dq" << std::endl << dqdd_dqs[0] << std::endl;
        std::cout << "df_dqd" << std::endl << dqdd_dvs[0] << std::endl;
    #else
        // Single call
        if(NUM_TIME_STEPS == 1){
            VectorXT zeros = VectorXT::Zero(model.nv);

            clock_gettime(CLOCK_MONOTONIC,&start);
            for(int i = 0; i < TEST_ITERS; i++){
                rnea_code_gen.evalFunction(qs[0],qds[0],qdds[0]);
            }
            clock_gettime(CLOCK_MONOTONIC,&end);
            printf("ID codegen %fus\n",time_delta_us_timespec(start,end)/static_cast<double>(TEST_ITERS));

            clock_gettime(CLOCK_MONOTONIC,&start);
            for(int i = 0; i < TEST_ITERS; i++){
                minv_code_gen.evalFunction(qs[0]);
            }
            clock_gettime(CLOCK_MONOTONIC,&end);
            printf("Minv codegen %fus\n",time_delta_us_timespec(start,end)/static_cast<double>(TEST_ITERS));

            clock_gettime(CLOCK_MONOTONIC,&start);
            for(int i = 0; i < TEST_ITERS; i++){
                minv_code_gen.evalFunction(qs[0]);
                minv_code_gen.Minv.template triangularView<Eigen::StrictlyLower>() =
                    minv_code_gen.Minv.transpose().template triangularView<Eigen::StrictlyLower>();
                rnea_code_gen.evalFunction(qs[0],qds[0],zeros);
                qdds[0].noalias() = minv_code_gen.Minv*(us[0] - rnea_code_gen.res);
            }
            clock_gettime(CLOCK_MONOTONIC,&end);
            printf("FD codegen %fus\n",time_delta_us_timespec(start,end)/static_cast<double>(TEST_ITERS));

            clock_gettime(CLOCK_MONOTONIC,&start);
            for(int i = 0; i < TEST_ITERS; i++){
                rnea_derivatives_code_gen.evalFunction(qs[0],qds[0],qdds[0]);
            }
            clock_gettime(CLOCK_MONOTONIC,&end);
            printf("ID_DU codegen %fus\n",time_delta_us_timespec(start,end)/static_cast<double>(TEST_ITERS));

            clock_gettime(CLOCK_MONOTONIC,&start);
            for(int i = 0; i < TEST_ITERS; i++){
                minv_code_gen.evalFunction(qs[0]);
                minv_code_gen.Minv.template triangularView<Eigen::StrictlyLower>() =
                    minv_code_gen.Minv.transpose().template triangularView<Eigen::StrictlyLower>();
                rnea_code_gen.evalFunction(qs[0],qds[0],zeros);
                VectorXT qdd = minv_code_gen.Minv*(us[0] - rnea_code_gen.res);
                rnea_derivatives_code_gen.evalFunction(qs[0],qds[0],qdd);
                dqdd_dqs[0].noalias() = -minv_code_gen.Minv*rnea_derivatives_code_gen.dtau_dq;
                dqdd_dvs[0].noalias() = -minv_code_gen.Minv*rnea_derivatives_code_gen.dtau_dv;
            }
            clock_gettime(CLOCK_MONOTONIC,&end);
            printf("FD_DU codegen %fus\n",time_delta_us_timespec(start,end)/static_cast<double>(TEST_ITERS));

        }
        // multi call with threadPools
        else{
            ReusableThreads<NUM_THREADS> threads;
            std::vector<double> times = {};

            for(int iter = 0; iter < TEST_ITERS; iter++){
                clock_gettime(CLOCK_MONOTONIC,&start);
                inverseDynamicsThreaded_codegen<T,NUM_THREADS,NUM_TIME_STEPS>(rnea_code_gen_arr,
                                                                              model.nq,model.nv,qs,qds,&threads);
                clock_gettime(CLOCK_MONOTONIC,&end);
                times.push_back(time_delta_us_timespec(start,end));
            }
            printf("[N:%d]: ID codegen: ",NUM_TIME_STEPS); printStats(&times); times.clear();
            printf("----------------------------------------\n");

            for(int iter = 0; iter < TEST_ITERS; iter++){
                clock_gettime(CLOCK_MONOTONIC,&start);
                minvThreaded_codegen<T,NUM_THREADS,NUM_TIME_STEPS>(minv_code_gen_arr,model.nq,model.nv,qs,&threads);
                clock_gettime(CLOCK_MONOTONIC,&end);
                times.push_back(time_delta_us_timespec(start,end));
            }
            printf("[N:%d]: Minv codegen: ",NUM_TIME_STEPS); printStats(&times); times.clear();
            printf("----------------------------------------\n");

            for(int iter = 0; iter < TEST_ITERS; iter++){
                clock_gettime(CLOCK_MONOTONIC,&start);
                forwardDynamicsThreaded_codegen<T,NUM_THREADS,NUM_TIME_STEPS>(minv_code_gen_arr,rnea_code_gen_arr,
                                                                              model.nq,model.nv,qs,qds,qdds,us,&threads);
                clock_gettime(CLOCK_MONOTONIC,&end);
                times.push_back(time_delta_us_timespec(start,end));
            }
            printf("[N:%d]: FD codegen: ",NUM_TIME_STEPS); printStats(&times); times.clear();
            printf("----------------------------------------\n");

            for(int iter = 0; iter < TEST_ITERS; iter++){
                clock_gettime(CLOCK_MONOTONIC,&start);
                inverseDynamicsGradientThreaded_codegen<T,NUM_THREADS,NUM_TIME_STEPS>(rnea_derivatives_code_gen_arr,
                                                                                model.nq,model.nv,qs,qds,&threads);
                clock_gettime(CLOCK_MONOTONIC,&end);
                times.push_back(time_delta_us_timespec(start,end));
            }
            printf("[N:%d]: ID_DU codegen: ",NUM_TIME_STEPS); printStats(&times); times.clear();
            printf("----------------------------------------\n");

            for(int iter = 0; iter < TEST_ITERS; iter++){
                clock_gettime(CLOCK_MONOTONIC,&start);
                forwardDynamicsGradientThreaded_codegen<T,NUM_THREADS,NUM_TIME_STEPS>(rnea_derivatives_code_gen_arr,
                                                                                minv_code_gen_arr,rnea_code_gen_arr,
                                                                                model.nq,model.nv,dqdd_dqs,dqdd_dvs,
                                                                                qs,qds,us,&threads);
                clock_gettime(CLOCK_MONOTONIC,&end);
                times.push_back(time_delta_us_timespec(start,end));
            }
            printf("[N:%d]: FD_DU codegen: ",NUM_TIME_STEPS); printStats(&times); times.clear();
            printf("----------------------------------------\n");
        }
    #endif

    // make sure to delete objs
    for (int i = 0; i < NUM_THREADS; i++){delete rnea_derivatives_code_gen_arr[i];}// delete rnea_code_gen_arr[i]; delete minv_code_gen_arr[i];}
}

template<typename T, int TEST_ITERS, int CPU_THREADS>
void run_all_tests(std::string urdf_filepath){
    test<T,10*TEST_ITERS,CPU_THREADS,1>(urdf_filepath);
    #if !TEST_FOR_EQUIVALENCE
        test<T,TEST_ITERS,CPU_THREADS,16>(urdf_filepath);
        test<T,TEST_ITERS,CPU_THREADS,32>(urdf_filepath);
        test<T,TEST_ITERS,CPU_THREADS,64>(urdf_filepath);
        test<T,TEST_ITERS,CPU_THREADS,128>(urdf_filepath);
        test<T,TEST_ITERS,CPU_THREADS,256>(urdf_filepath);
    #endif
}

int main(int argc, const char ** argv){
    std::string urdf_filepath;
    if(argc>1){urdf_filepath = argv[1];}
    else{printf("Usage is: urdf_filepath\n"); return 1;}

    run_all_tests<float,TEST_ITERS_GLOBAL,CPU_THREADS_GLOBAL>(urdf_filepath);
    return 0;
}


/* boost random/normal_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Copyright Steven Watanabe 2010-2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_NORMAL_DISTRIBUTION_HPP
#define BOOST_RANDOM_NORMAL_DISTRIBUTION_HPP

#include <boost/config/no_tr1/cmath.hpp>
#include <istream>
#include <iosfwd>
#include <boost/assert.hpp>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/uniform_01.hpp>

namespace boost {
namespace random {

// deterministic Box-Muller method, uses trigonometric functions

/**
 * Instantiations of class template normal_distribution model a
 * \random_distribution. Such a distribution produces random numbers
 * @c x distributed with probability density function
 * \f$\displaystyle p(x) =
 *   \frac{1}{\sqrt{2\pi\sigma}} e^{-\frac{(x-\mu)^2}{2\sigma^2}}
 * \f$,
 * where mean and sigma are the parameters of the distribution.
 */
template<class RealType = double>
class normal_distribution
{
public:
    typedef RealType input_type;
    typedef RealType result_type;

    class param_type {
    public:
        typedef normal_distribution distribution_type;

        /**
         * Constructs a @c param_type with a given mean and
         * standard deviation.
         *
         * Requires: sigma >= 0
         */
        explicit param_type(RealType mean_arg = RealType(0.0),
                            RealType sigma_arg = RealType(1.0))
          : _mean(mean_arg),
            _sigma(sigma_arg)
        {}

        /** Returns the mean of the distribution. */
        RealType mean() const { return _mean; }

        /** Returns the standand deviation of the distribution. */
        RealType sigma() const { return _sigma; }

        /** Writes a @c param_type to a @c std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        { os << parm._mean << " " << parm._sigma ; return os; }

        /** Reads a @c param_type from a @c std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        { is >> parm._mean >> std::ws >> parm._sigma; return is; }

        /** Returns true if the two sets of parameters are the same. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._mean == rhs._mean && lhs._sigma == rhs._sigma; }

        /** Returns true if the two sets of parameters are the different. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:
        RealType _mean;
        RealType _sigma;
    };

    /**
     * Constructs a @c normal_distribution object. @c mean and @c sigma are
     * the parameters for the distribution.
     *
     * Requires: sigma >= 0
     */
    explicit normal_distribution(const RealType& mean_arg = RealType(0.0),
                                 const RealType& sigma_arg = RealType(1.0))
      : _mean(mean_arg), _sigma(sigma_arg),
        _r1(0), _r2(0), _cached_rho(0), _valid(false)
    {
        BOOST_ASSERT(_sigma >= RealType(0));
    }

    /**
     * Constructs a @c normal_distribution object from its parameters.
     */
    explicit normal_distribution(const param_type& parm)
      : _mean(parm.mean()), _sigma(parm.sigma()),
        _r1(0), _r2(0), _cached_rho(0), _valid(false)
    {}

    /**  Returns the mean of the distribution. */
    RealType mean() const { return _mean; }
    /** Returns the standard deviation of the distribution. */
    RealType sigma() const { return _sigma; }

    /** Returns the smallest value that the distribution can produce. */
    RealType min BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return -std::numeric_limits<RealType>::infinity(); }
    /** Returns the largest value that the distribution can produce. */
    RealType max BOOST_PREVENT_MACRO_SUBSTITUTION () const
    { return std::numeric_limits<RealType>::infinity(); }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(_mean, _sigma); }
    /** Sets the parameters of the distribution. */
    void param(const param_type& parm)
    {
        _mean = parm.mean();
        _sigma = parm.sigma();
        _valid = false;
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() { _valid = false; }

    /**  Returns a normal variate. */
    template<class Engine>
    result_type operator()(Engine& eng)
    {
        using std::sqrt;
        using std::log;
        using std::sin;
        using std::cos;

        if(!_valid) {
            _r1 = boost::uniform_01<RealType>()(eng);
            _r2 = boost::uniform_01<RealType>()(eng);
            _cached_rho = sqrt(-result_type(2) * log(result_type(1)-_r2));
            _valid = true;
        } else {
            _valid = false;
        }
        // Can we have a boost::mathconst please?
        const result_type pi = result_type(3.14159265358979323846);

        return _cached_rho * (_valid ?
                              cos(result_type(2)*pi*_r1) :
                              sin(result_type(2)*pi*_r1))
            * _sigma + _mean;
    }

    /** Returns a normal variate with parameters specified by @c param. */
    template<class URNG>
    result_type operator()(URNG& urng, const param_type& parm)
    {
        return normal_distribution(parm)(urng);
    }

    /** Writes a @c normal_distribution to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, normal_distribution, nd)
    {
        os << nd._mean << " " << nd._sigma << " "
           << nd._valid << " " << nd._cached_rho << " " << nd._r1;
        return os;
    }

    /** Reads a @c normal_distribution from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, normal_distribution, nd)
    {
        is >> std::ws >> nd._mean >> std::ws >> nd._sigma
           >> std::ws >> nd._valid >> std::ws >> nd._cached_rho
           >> std::ws >> nd._r1;
        return is;
    }

    /**
     * Returns true if the two instances of @c normal_distribution will
     * return identical sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(normal_distribution, lhs, rhs)
    {
        return lhs._mean == rhs._mean && lhs._sigma == rhs._sigma
            && lhs._valid == rhs._valid
            && (!lhs._valid || (lhs._r1 == rhs._r1 && lhs._r2 == rhs._r2));
    }

    /**
     * Returns true if the two instances of @c normal_distribution will
     * return different sequences of values given equal generators.
     */
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(normal_distribution)

private:
    RealType _mean, _sigma;
    RealType _r1, _r2, _cached_rho;
    bool _valid;

};

} // namespace random

using random::normal_distribution;

} // namespace boost

#endif // BOOST_RANDOM_NORMAL_DISTRIBUTION_HPP


#include <gtest/gtest.h>
#include "util/logger.hpp"
#include <ros/ros.h>
#include "collision_detection/MoveItCollisionHelper.h"
#include <moveit_visual_tools/moveit_visual_tools.h>
#include "opencv2/opencv.hpp"
#include "planner/PlannerMethod.hpp"
#include <boost/thread/thread.hpp>
#include <boost/thread/locks.hpp>
#include <boost/thread/shared_mutex.hpp>
inline bool collision_check(const state_space::Rn& test_state){
    static cv::Mat environment{cv::imread("/home/xcy/Cspace/SampleBasedPlanningMethods/3.png", 0)};
    Eigen::Vector2d bounds{environment.cols,environment.rows};
    return test_state.Vector().minCoeff()>=0&&
           (test_state.Vector()-bounds).maxCoeff()<=0
           &&environment.at<uchar>((int) test_state.Vector()[1], (int) test_state.Vector()[0]) == 255;
}
double vdc(int n,unsigned int bits) {
    int reverse = 0;
    while (n){
        int pos = log2(n & -n) + 1;
        reverse = reverse | (1 << (bits - pos));
        n = n & (n - 1);
    }
    return reverse;
}
std::size_t check_times;
bool isPathValid_vdc(const state_space::Rn& from, const state_space::Rn& to){
    const double min_distance=0.5;
    check_times++;
    if(!collision_check(to))
        return false;
    unsigned int K = ceil(log2(ceil(planner::distance(from,to)/min_distance)));
    unsigned int bits = K;
    K = 1<<K;
    for(int k=0; k<K;++k){
        check_times++;
        auto intermediate_state = planner::interpolate(from, to, vdc(k,bits)/K);
        if (!collision_check(intermediate_state))
            return false;
    }
    return true;
}
bool isPathValid_incremental(const state_space::Rn& from, const state_space::Rn& to){
    const double min_distance=0.5;
    unsigned int K= ceil(planner::distance(from,to)/min_distance);
    K = K? K:1;
    for(int k=0;k<=K;++k) {
        check_times++;
        auto intermediate_state = planner::interpolate(from, to, (double)k/K);
        if(!collision_check(intermediate_state))
            return false;
    }
    return true;
}
template<typename T>
void _animate(cv::Mat& draw, planner::Vertex<T> * vertex)
{
    cv::circle(draw, cv::Point((int)vertex->state().Vector()[0], (int)vertex->state().Vector()[1]), 3, cv::Scalar{255, 0, 0}, 1);
    for(auto & it : vertex->children())
    {
        cv::line(draw, cv::Point((int)vertex->state().Vector()[0], (int)vertex->state().Vector()[1]),
                 cv::Point((int)it->state().Vector()[0], (int)it->state().Vector()[1]), cv::Scalar{255, 0, 0}, 2);
        _animate(draw,it);
    }
}
template<typename T>
void animate(cv::Mat &draw, const std::vector<planner::Vertex<T>*>& roots)
{
    boost::this_thread::interruption_enabled();
    while(true){
        boost::this_thread::interruption_point();
        for(const auto& item: roots){
            _animate(draw,item);
        }
        cv::Mat canvas;
        cv::resize(draw,canvas,cv::Size{800,800});
        cv::imshow("check: ",canvas);
        cv::waitKey(10);
    }

}
/*
template<typename SPCIFIC_STATE>
void test_no_collision_func(int dimensions = -1, const Eigen::MatrixX2d *bounds_ptr = nullptr)
{
    auto start_state = planner::randomState<SPCIFIC_STATE>(dimensions, bounds_ptr);
    auto goal_state = planner::randomState<SPCIFIC_STATE>(dimensions, bounds_ptr);

    planner::RRT<SPCIFIC_STATE> rrt_2d(planner::hash<SPCIFIC_STATE>, start_state.Dimensions());

    rrt_2d.constructPlan(planner::PLAN_REQUEST<SPCIFIC_STATE>(start_state, goal_state, 50,0.1,false, 0.1));
    std::vector<SPCIFIC_STATE> path;
    clock_t start(clock());
    if (rrt_2d.planning()) {
        clock_t end(clock());
        LOG(INFO) << "planning time consumption: " << static_cast<double>(end - start) / CLOCKS_PER_SEC;
        path = rrt_2d.GetPath();
        for (const auto &it: path) {
            LOG(INFO) << it;
        }
    }
    EXPECT_TRUE(!path.empty()) << "No path found";
}

template<typename SPCIFIC_STATE>
void test_with_collision_func(int dimensions = -1, const Eigen::MatrixX2d *bounds_ptr = nullptr)
{
    ros::AsyncSpinner spinner(2);
    spinner.start();
    my_collision_detection::MoveItCollisionHelper moveItCollisionHelper("manipulator_i5",
                                                                        "/home/xcy/WorkSpace/src/NursingRobot/config/aubo_i5.yaml",
                                                                        my_kinematics::aubo_i5_analytical_IK);

    auto start_state = planner::randomState<SPCIFIC_STATE>(dimensions, bounds_ptr);
    auto goal_state = planner::randomState<SPCIFIC_STATE>(dimensions, bounds_ptr);
    clock_t start(clock());
    double time{};
    while (true) {
        time = (double) (clock() - start) / CLOCKS_PER_SEC;
        CHECK_LT(time, 5) << "Generate valid state failed out of 5 s";
        start_state = planner::randomState<SPCIFIC_STATE>(dimensions, bounds_ptr);
        goal_state = planner::randomState<SPCIFIC_STATE>(dimensions, bounds_ptr);
        if (moveItCollisionHelper.isStateValid(start_state) && moveItCollisionHelper.isStateValid(goal_state))
            break;
    }

    planner::RRT<SPCIFIC_STATE> rrt_2d(planner::hash<SPCIFIC_STATE>, start_state.Dimensions());

    rrt_2d.setStateValidator(std::function<bool(const SPCIFIC_STATE &, const SPCIFIC_STATE &)>(
            std::bind(&my_collision_detection::MoveItCollisionHelper::isPathValid<SPCIFIC_STATE>,
                      &moveItCollisionHelper, std::placeholders::_1, std::placeholders::_2)
    ));

    rrt_2d.setStepLen(0.05);
    rrt_2d.setGoalMaxDist(0.05);
    rrt_2d.constructPlan(planner::PLAN_REQUEST<SPCIFIC_STATE>(start_state, goal_state, 10));
    std::vector<SPCIFIC_STATE> path;
    if (rrt_2d.planning()) {
        clock_t end(clock());
        LOG(INFO) << "planning time consumption: " << static_cast<double>(end - start) / CLOCKS_PER_SEC;
        path = rrt_2d.GetPath();
        for (const auto &it: path) {
            LOG(INFO) << it;
        }
    }
    EXPECT_TRUE(!path.empty()) << "No path found";
    spinner.stop();
}

TEST(RRTTest, J6WithoutCollisionTest)
{
    test_no_collision_func<state_space::JointSpace>(6);
}

TEST(RRTTest, R2WithoutCollisionTest)
{
    test_no_collision_func<state_space::Rn>(2);
}

TEST(RRTTest, R3WithoutCollisionTest)
{
    test_no_collision_func<state_space::Rn>(3);
}

TEST(RRTTest, SE3WithoutCollisionTest)
{
    test_no_collision_func<state_space::SE3>();
}

TEST(RRTTest, SO3WithoutCollisionTest)
{
    test_no_collision_func<state_space::SO3>();
}

TEST(RRTTest, J6withCollisionTest)
{
    Eigen::MatrixX2d bounds;
    bounds.resize(6, 2);
    bounds << state_space::R6().setConstant(3.05),
            state_space::R6().setConstant(-3.05);
    test_with_collision_func<state_space::JointSpace>(1, nullptr);
}

TEST(RRTTest, SE3withCollisionTest)
{
    test_with_collision_func<state_space::SE3>();
}*/

TEST(RRTTest, R2WithAnimationTest)
{
    //load environment
    cv::Mat img = cv::imread("/home/xcy/Cspace/SampleBasedPlanningMethods/map6.png");
    //set sample bounds
    Eigen::MatrixX2d bounds;
    bounds.resize(2, 2);
    bounds << Eigen::Vector2d{img.cols, img.rows},
            Eigen::Vector2d::Zero();

    //set start state
    state_space::Rn start_state{std::vector<double>{1,1}};
    //set valid goal state
    state_space::Rn goal_state{std::vector<double>{1990,1990}};
    std::size_t i=0;
    const std::size_t max_iterations=1e4;
    const std::string base_path{"/home/xcy/RRT_EXP2D/"};
    const std::string goal_list{"Goals.txt"};

    std::string final_path = base_path+goal_list;
    const std::vector<std::string> txt_names{{"/time.txt"},{"/pathLength.txt"},{"/ccTimes.txt"},
                                             {"/nodes.txt"},{"/invalidGoals.txt"},{"/performance.txt"}};

    std::size_t  iter_index=0;
    std::size_t total_time{};
    std::size_t valid_times=0;
    double total_length{};
    std::size_t total_path_size{};
    std::size_t total_nodes{};
    std::size_t total_ccTimes{};

    std::ifstream goal_src(final_path.c_str());

    auto rrt_based_planner_ptr = planner::createPlanner<state_space::Rn,flann::L2_Simple<double>>(planner::RRT_SIMPLE,start_state.Dimensions());

    rrt_based_planner_ptr->setStateValidator(std::function<bool(const state_space::Rn &, const state_space::Rn &)>(
            isPathValid_incremental));
    rrt_based_planner_ptr->setSampleBounds(&bounds);

    std::vector<std::ofstream> outfile_vector{};
    outfile_vector.resize(txt_names.size());
    for(int i=0;i<txt_names.size();++i){
        outfile_vector[i].open(base_path+rrt_based_planner_ptr->getName()+txt_names[i],std::ios::out | std::ios::trunc);
    }

    while(iter_index++<max_iterations){
        std::string s;getline(goal_src,s);
        std::istringstream stringGet(s);
        stringGet>>start_state[0]>>start_state[1]>>goal_state[0]>>goal_state[1];
        rrt_based_planner_ptr->constructPlan(planner::PLAN_REQUEST<state_space::Rn,flann::L2_Simple<double>>(start_state, goal_state, 10,200,false,30,0));
        std::vector<state_space::Rn> path;
        clock_t start(clock());
        if (rrt_based_planner_ptr->planning()) {
            path = rrt_based_planner_ptr->GetPath();
            //valid times
            valid_times++;
            //single time
            outfile_vector[0]<<(double)(clock()-start)/CLOCKS_PER_SEC<<std::endl;
            //single length
            double path_length{};
            for(int i=0; i<path.size()-1;++i){
                path_length += planner::distance(path[i],path[i+1]);
            }
            outfile_vector[1]<<path.size()<<" "<<path_length<<std::endl;
            //single cc times
            outfile_vector[2]<<check_times<<std::endl;
            //single iter times
            outfile_vector[3]<<rrt_based_planner_ptr->getTotalNodes()<<std::endl;

            //average usage
            total_time += clock() - start;
            total_length += path_length;
            total_path_size += path.size();
            total_ccTimes += check_times;
            total_nodes+=rrt_based_planner_ptr->getTotalNodes();
            check_times=0;
        }
        else{
            outfile_vector[4]<<goal_state<<std::endl;
        }
    }
    outfile_vector[5]<<"average planning time: "<<(double)total_time / valid_times / CLOCKS_PER_SEC<<"s"<<std::endl;
    outfile_vector[5]<<"average path length: "<<total_length/valid_times<<std::endl;
    outfile_vector[5]<<"average path size: "<<(double)total_path_size/valid_times<<std::endl;
    outfile_vector[5]<<"average cc Times: "<<(long double)total_ccTimes/valid_times<<std::endl;
    outfile_vector[5] << "average nodes: " << (long double)total_nodes / valid_times << std::endl;
    outfile_vector[5]<<"valid percent: "<<valid_times<<"/"<<max_iterations<<": "<<(double)valid_times/max_iterations<<std::endl;

    if(goal_src.is_open()) {
        goal_src.close();
    }
    for(int i=0; i<txt_names.size();++i){
        if(outfile_vector[i].is_open()){
            outfile_vector[i].close();
        }
    }
}
TEST(RRTTest, R2SingleTest){
    //load environment
    std::shared_ptr<boost::thread> thread_ptr_;
    cv::Mat img = cv::imread("/home/xcy/Cspace/SampleBasedPlanningMethods/3.png");
    //set sample bounds
    Eigen::MatrixX2d bounds;
    bounds.resize(2, 2);
    bounds << Eigen::Vector2d{img.cols, img.rows},
            Eigen::Vector2d::Zero();

    state_space::Rn start_state{std::vector<double>{190,245}};
    state_space::Rn goal_state{std::vector<double>{650,360}};

    auto rrt_based_planner_ptr = planner::createPlanner<state_space::Rn,flann::L2_Simple<double>>(planner::RRT_SIMPLE,start_state.Dimensions());
    rrt_based_planner_ptr->setStateValidator(std::function<bool(const state_space::Rn &, const state_space::Rn &)>(
            isPathValid_vdc));
    rrt_based_planner_ptr->setSampleBounds(&bounds);
    rrt_based_planner_ptr->constructPlan(planner::PLAN_REQUEST<state_space::Rn,flann::L2_Simple<double>>(start_state, goal_state, 5000,60,false,30,0.05));
    std::vector<state_space::Rn> path;
    clock_t before{clock()};
    //thread_ptr_.reset(new boost::thread(boost::bind(&animate<state_space::Rn>,img,rrt_based_planner_ptr->getRootVertex())));
    if (rrt_based_planner_ptr->planning()) {
        std::cout<<"time: "<<static_cast<double>((clock()-before))/CLOCKS_PER_SEC<<""
                                                                                "s"<<std::endl;
        path = rrt_based_planner_ptr->GetPath();
        auto root_vertex=rrt_based_planner_ptr->getRootVertex();
        //visualization
        for(const auto& item: root_vertex){
            _animate(img,item);
        }
        for (std::size_t i = 1; i < path.size(); ++i) {
            cv::line(img, cv::Point(path[i].Vector()[0], path[i].Vector()[1]),
                     cv::Point(path[i - 1].Vector()[0], path[i - 1].Vector()[1]), cv::Scalar{0, 0, 255}, 2);
        }
        double path_length{};
        for(int i=0; i<path.size()-1;++i){
            path_length += planner::distance(path[i],path[i+1]);
        }
        std::cout<<path.size()<<" "<<path_length<<std::endl;
        std::cout<<"nodes: "<<rrt_based_planner_ptr->getTotalNodes()<<std::endl;
        std::cout<<"cc Times: "<<check_times<<std::endl;
        cv::resize(img,img,cv::Size{800,800});
        cv::imshow("check: ",img);
        cv::waitKey(0);
        //cv::imwrite("/home/xcy/"+rrt_based_planner_ptr->getName()+".png",img);
    }
    //thread_ptr_->interrupt();
    //thread_ptr_->join();
}

int main(int argc, char **argv)
{
    logger lg(argv[0]);
    testing::InitGoogleTest(&argc, argv);
    ros::init(argc, argv, "RRTTest");
    return RUN_ALL_TESTS();
}


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

#include <boost/hana/assert.hpp>
#include <boost/hana/integral_constant.hpp>
using namespace boost::hana;

int main() {

{

//! [rem]
BOOST_HANA_CONSTANT_CHECK(rem(int_<6>, int_<4>) == int_<2>);
BOOST_HANA_CONSTANT_CHECK(rem(int_<-6>, int_<4>) == int_<-2>);
BOOST_HANA_CONSTEXPR_CHECK(rem(6, 4) == 2);
//! [rem]

}{

//! [quot]
BOOST_HANA_CONSTANT_CHECK(quot(int_<6>, int_<3>) == int_<2>);
BOOST_HANA_CONSTANT_CHECK(quot(int_<6>, int_<4>) == int_<1>);

BOOST_HANA_CONSTEXPR_CHECK(quot(6, 3) == 2);
BOOST_HANA_CONSTEXPR_CHECK(quot(6, 4) == 1);
//! [quot]

}

}


/*
This sample illustrates how to use the Fréchet Range Queries Library from a
typical Boost::Geometry project.

two versions are given:
  one is vector_of_points, which models a trajectory as a vector of boost points
  one is linestring, which models a trajectory as a linestring feature.

*/
#include <cmath>
#include <functional>
#include <iostream>
#include <vector>

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

#include "../include/frechetrange/frechetrange.hpp"

using std::cout;
using std::endl;
namespace bg = boost::geometry;

// Declare Geometry
typedef bg::model::point<double, 2, bg::cs::cartesian> point_type;
typedef bg::model::linestring<point_type> linestring_type;

struct get_coordinate {
    template <size_t dim>
    static double get(const point_type &p) {
        return p.get<dim>();
    }
};

struct squared_distance {
    double operator()(const point_type &p, const point_type &q) const {
        return bg::comparable_distance(p, q);
    }
};

int main(int argc, char **argv) {
    // Linestring version
    linestring_type t1, t2;

    // read from WKT
    bg::read_wkt("LINESTRING(0 0 , 0 1, 0 2)", t1);
    bg::read_wkt("LINESTRING(1 1, 2 2, 1 3)", t2);

    // instantiate some decider, this time fully specified.
    frechetrange::detail::dv::frechet_distance<2, get_coordinate,
                                               squared_distance>
        fd;

    cout << std::fixed;
    // a range scan
    for (double d = 1; d < 5; d += 0.25)
        cout << "Reachable at " << d << ":\t"
             << (fd.is_bounded_by(t1, t2, d) ? "yes" : "no") << endl;

    // estimate the distance by interval cutting
    {
        std::pair<double, double> interval = {0, 5};
        while ((interval.second - interval.first) > 0.001) {
            double m = (interval.first + interval.second) / 2;

            if (fd.is_bounded_by(t1, t2, m)) {
                interval.second = m;
            } else {
                interval.first = m;
            }
        }
        cout << "Final Interval: [" << interval.first << "," << interval.second
             << "]" << endl;
    }

    // Bringmann Baldus Case

    frechetrange::detail::bb::frechet_distance<2, get_coordinate,
                                               squared_distance>
        fd2;

    cout << std::fixed;
    // a range scan
    for (double d = 1; d < 5; d += 0.25)
        cout << "Reachable at " << d << ":\t"
             << (fd2.is_bounded_by(t1, t2, d) ? "yes" : "no") << endl;

    // estimate the distance by interval cutting
    {
        std::pair<double, double> interval = {0, 5};
        while ((interval.second - interval.first) > 0.001) {
            double m = (interval.first + interval.second) / 2;

            if (fd2.is_bounded_by(t1, t2, m)) {
                interval.second = m;
            } else {
                interval.first = m;
            }
        }
        cout << "Final Interval: [" << interval.first << "," << interval.second
             << "]" << endl;
    }

    return 0;
}


// -------------- test the visual odometry -------------
#ifndef SFMTEST_INCLUDE_H
#define SFMTEST_INCLUDE_H
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <fstream>
#include <Eigen/Dense>
// #include "myslam/common_include.h"
#include <boost/timer.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/viz.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>

#include <map>
#include <opencv2/video/tracking.hpp>

#include "myslam/config.h"
#include "myslam/visual_odometry.h"

using namespace std;
using namespace cv;
using namespace Eigen;
struct SFMFeature
{
    bool state;
    int id;
    vector<pair<int, Vector2d>> observation;//这个就可以表示在每帧听到的位置
    double position[3];
    double depth;
};
struct FrameInfo
{
  int id;
  Mat img;
  vector<KeyPoint> frameKeypoints;
  Mat FrameDescriptors;
  vector<Point3d> depth;
};
// 像素坐标转相机归一化坐标
Point2f pixel2cam(const Point2d &p, const Mat &K);

struct ReprojectionError3D
{
    ReprojectionError3D(double observed_u, double observed_v)
        : observed_u(observed_u), observed_v(observed_v)
    {
    }

    template <typename T>
    bool operator()(const T *const camera_R, const T *const camera_T, const T *point, T *residuals) const
    {
        T p[3];
        ceres::QuaternionRotatePoint(camera_R, point, p);
        p[0] += camera_T[0];
        p[1] += camera_T[1];
        p[2] += camera_T[2];
        T xp = p[0] / p[2];
        T yp = p[1] / p[2];
        residuals[0] = xp - T(observed_u);
        residuals[1] = yp - T(observed_v);
        return true;
    }

    static ceres::CostFunction *Create(const double observed_x,
                                       const double observed_y)
    {
        return (new ceres::AutoDiffCostFunction<
                ReprojectionError3D, 2, 4, 3, 3>(
            new ReprojectionError3D(observed_x, observed_y)));
    }

    double observed_u;
    double observed_v;
};
template <class T> void reduceVector(vector<T> &v, vector<uchar> status)
{
    int j = 0;
    for (int i = 0; i < int(v.size()); i++)
        if (status[i])
            v[j++] = v[i];
    v.resize(j);
}
void triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
						Vector2d &point0, Vector2d &point1, Vector3d &point_3d);
//找到两帧之间的共视点
void triangulateTwoFrames(int frame0, Eigen::Matrix<double, 3, 4> &Pose0,
									 int frame1, Eigen::Matrix<double, 3, 4> &Pose1,
									 vector<SFMFeature> &sfm_f);
bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l,int WINDOW_SIZE,vector<SFMFeature> &sfm_f);

void rejectWithF(vector<cv::Point2f> &forw_pts, vector<cv::Point2f> &cur_pts)
{

        vector<uchar> status;
        //Calculates a fundamental matrix from the corresponding points in two images.
        //根据两队点算F,以及status再次筛除一些点
        cv::findFundamentalMat(forw_pts, cur_pts, cv::FM_RANSAC, 3.0, 0.99, status);
        int size_a = cur_pts.size();
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        // ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);
        // ROS_DEBUG("FM ransac costs: %fms", t_f.toc());

}

vector<pair<Vector3d, Vector3d>> getCorresponding(int frame_count_l, int frame_count_r,vector<SFMFeature> &sfm_f);
//5帧法恢复rt，是从前面找到的特征点
bool solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
{
    if (corres.size() >= 15)
    {
      int ptCount = (int)corres.size();
      Mat p1(ptCount, 2, CV_32F);
      Mat p2(ptCount, 2, CV_32F);

      // 把Keypoint转换为Mat
      for (int i=0; i<ptCount; i++)
      {

	  p1.at<float>(i, 0) = corres[i].first(0);
	  p1.at<float>(i, 1) = corres[i].first(1);

	  p2.at<float>(i, 0) = corres[i].second(0);
	  p2.at<float>(i, 1) = corres[i].second(1);
// 	  cout<<p1.at<float>(i, 0)<<"----"<<p1.at<float>(i, 1)<<endl;
// 	  cout<<p2.at<float>(i, 0)<<"----"<<p2.at<float>(i, 1)<<endl;
      }

        cv::Mat mask;

        //找基础质矩阵
        cv::Mat E = cv::findFundamentalMat(p1, p2, cv::FM_RANSAC, 3., 0.99, mask);

        cv::Mat cameraMatrix =  ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );

        cv::Mat rot, trans;
        //判断相位符合的点

        int inlier_cnt = cv::recoverPose(E, p1, p2, cameraMatrix, rot, trans, mask);
        cout << "inlier_cnt " << inlier_cnt << endl;

        Eigen::Matrix3d R;
        Eigen::Vector3d T;
        for (int i = 0; i < 3; i++)
        {
            T(i) = trans.at<double>(i, 0);
            for (int j = 0; j < 3; j++)
                R(i, j) = rot.at<double>(i, j);
        }

        Rotation = R.transpose();
        Translation = -R.transpose() * T;
        if(inlier_cnt > 12)//如果点合符要求，就返回
            return true;
        else
            return false;
    }
    return false;
}

bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i,vector<SFMFeature> &sfm_f);

int WINDOWSIZE=15;
int main(int argc, char **argv)
{
    // if ( argc != 2 )
    // {
    //     cout<<"usage: run_vo parameter_file"<<endl;
    //     return 1;
    // }

    myslam::Config::setParameterFile ( "../config/default.yaml" );
    myslam::VisualOdometry::Ptr vo ( new myslam::VisualOdometry );

    string dataset_dir = myslam::Config::get<string> ( "dataset_dir" );
    cout<<"dataset: "<<dataset_dir<<endl;
    ifstream fin(dataset_dir + "/associate.txt");

    if ( !fin )
    {
        cout<<"please generate the associate file called associate.txt!"<<endl;
        return 1;
    }

    vector<string> rgb_files, depth_files;
    vector<double> rgb_times, depth_times;
    while ( !fin.eof() )
    {
        string rgb_time, rgb_file, depth_time, depth_file;
        fin>>rgb_time>>rgb_file>>depth_time>>depth_file;
        rgb_times.push_back ( atof ( rgb_time.c_str() ) );
        depth_times.push_back ( atof ( depth_time.c_str() ) );
        rgb_files.push_back ( dataset_dir+"/"+rgb_file );
        depth_files.push_back ( dataset_dir+"/"+depth_file );

        if ( fin.good() == false )
            break;
    }

    // visualization
    cv::viz::Viz3d vis ( "Visual Odometry" );
    cv::viz::WCoordinateSystem world_coor ( 1.0 ), camera_coor ( 0.5 );
    cv::Point3d cam_pos ( 0, -1.0, -1.0 ), cam_focal_point ( 0,0,0 ), cam_y_dir ( 0,1,0 );
    cv::Affine3d cam_pose = cv::viz::makeCameraPose ( cam_pos, cam_focal_point, cam_y_dir );
    vis.setViewerPose ( cam_pose );

    world_coor.setRenderingProperty ( cv::viz::LINE_WIDTH, 2.0 );
    camera_coor.setRenderingProperty ( cv::viz::LINE_WIDTH, 1.0 );
    vis.showWidget ( "World", world_coor );
    vis.showWidget ( "Camera", camera_coor );

    cout<<"read total "<<rgb_files.size() <<" entries"<<endl;

    Vector3d T[WINDOWSIZE + 1];
    map<int, Vector3d> sfm_tracked_points;

    vector< cv::Point2f > keypoints;      // 因为要删除跟踪失败的点，使用list
    vector< SFMFeature > sfm_f;
    cv::Mat color, depth, last_color;

    for(int index=0;index<WINDOWSIZE;index++)
    {
        color = imread(rgb_files[index], CV_LOAD_IMAGE_COLOR);
	cout<<"read..."<<endl;
        if (index == 0 )
        {
            // 对第一帧提取FAST特征点
            vector<cv::KeyPoint> kps;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            detector->detect( color, kps );
	    int first_count=0;
            for ( auto kp:kps )
	    {
                keypoints.push_back( kp.pt );
		SFMFeature tmpsfm;
		tmpsfm.state=false;
		tmpsfm.id=first_count;
		tmpsfm.observation.push_back(make_pair(index, Eigen::Vector2d{kp.pt.x, kp.pt.y}));
		first_count++;
		sfm_f.push_back(tmpsfm);
	    }
            last_color = color;
            continue;
        }
        if ( color.data==nullptr)
            continue;
        // 对其他帧用LK跟踪特征点
        vector<cv::Point2f> next_keypoints;
        vector<cv::Point2f> prev_keypoints;
        for ( auto kp:keypoints )
            prev_keypoints.push_back(kp);

        vector<unsigned char> status;
        vector<float> error;
//         chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        cv::calcOpticalFlowPyrLK( last_color, color, prev_keypoints, next_keypoints, status, error );

        //         chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        //         chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
        //         cout<<"LK Flow use time："<<time_used.count()<<" seconds."<<endl;
        // 把跟丢的点删掉

        int keypoint_index = 0;
        for ( auto kp:next_keypoints )
	{
	  sfm_f[keypoint_index].observation.push_back(make_pair(index, Eigen::Vector2d{kp.x, kp.y}));
      ++keypoint_index;
    }
        reduceVector(keypoints,status);
	reduceVector(sfm_f,status);

    //Calculates a fundamental matrix from the corresponding points in two images.
    //根据两队点算F,以及status再次筛除一些点
    cv::findFundamentalMat(prev_keypoints, next_keypoints, cv::FM_RANSAC, 1.0, 0.99, status);
    reduceVector(keypoints, status);
    reduceVector(sfm_f, status);

    cout << "tracked keypoints: " << keypoints.size() << endl;
    if (keypoints.size() == 0)
    {
        cout << "all keypoints are lost." << endl;
        break;
        }
        // 画出 keypoints
//         cv::Mat img_show = color.clone();
//         for ( auto kp:keypoints )
//             cv::circle(img_show, kp, 10, cv::Scalar(0, 240, 0), 1);
//         cv::imshow("corners", img_show);
//         cv::waitKey(0);
        last_color = color;

    }

    int frame_num = WINDOWSIZE;
    // //cout << "set 0 and " << l << " as known " << endl;
    // // have relative_r relative_t
    // // intial two view
    // //确定最后一帧的相对值
    // //设置初始值，ql是确定的帧的，t【-1】当然是给出计算的那个部分，以l帧作为起点0
    int l=0;

    Eigen::Quaterniond q[frame_num];
    q[l].w() = 1;
    q[l].x() = 0;
    q[l].y() = 0;
    q[l].z() = 0;
    T[l].setZero();
    Matrix3d relative_R;
    Vector3d relative_T;

    if (!relativePose(relative_R, relative_T, l,WINDOWSIZE,sfm_f))
    {
        printf("Not enough features or parallax; Move device around");
        return false;
    }

    q[frame_num - 1] = q[l] * Quaterniond(relative_R);
    T[frame_num - 1] = relative_T;
    // //cout << "init q_l " << q[l].w() << " " << q[l].vec().transpose() << endl;
    // //cout << "init t_l " << T[l].transpose() << endl;

    // //rotate to cam frame
    // //为啥要定义两个部分的RT
    Matrix3d c_Rotation[frame_num];
    Vector3d c_Translation[frame_num];
    Quaterniond c_Quat[frame_num];
    double c_rotation[frame_num][4];
    double c_translation[frame_num][3];
    Eigen::Matrix<double, 3, 4> Pose[frame_num]; //位姿用于三角化
//第一帧的值
    c_Quat[l] = q[l].inverse();//旋转是什么意思？？？
    c_Rotation[l] = c_Quat[l].toRotationMatrix(); //最后一帧当然是算出来的那个
    c_Translation[l] = -1 * (c_Rotation[l] * T[l]);//意思是以最后一帧载体系处理吗
    Pose[l].block<3, 3>(0, 0) = c_Rotation[l];
    Pose[l].block<3, 1>(0, 3) = c_Translation[l];
//最后一帧的值
    c_Quat[frame_num - 1] = q[frame_num - 1].inverse();
    c_Rotation[frame_num - 1] = c_Quat[frame_num - 1].toRotationMatrix();
    c_Translation[frame_num - 1] = -1 * (c_Rotation[frame_num - 1] * T[frame_num - 1]);
    Pose[frame_num - 1].block<3, 3>(0, 0) = c_Rotation[frame_num - 1];
    Pose[frame_num - 1].block<3, 1>(0, 3) = c_Translation[frame_num - 1];

    for (int i = l; i < WINDOWSIZE-1; i++)
    {
        // cout<<"****** loop "<<i<<" ******"<<rgb_files[i] <<endl;
        // Mat color = cv::imread ( rgb_files[i] );
        // solve pnp跳过前面那些不用的帧
	if (i > l)
        {
            //就是用上一帧的作为初值去求解pnp
            Matrix3d R_initial = c_Rotation[i - 1];
            Vector3d P_initial = c_Translation[i - 1];
            if (!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
                return false;
            c_Rotation[i] = R_initial;
            c_Translation[i] = P_initial;
            c_Quat[i] = c_Rotation[i];
            Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
            Pose[i].block<3, 1>(0, 3) = c_Translation[i];
        }

    //     // triangulate point based on the solve pnp result
    //     //前面条件不满足，就是首先进来当i=l时第i帧和最后一帧之间的三角化，得到了这两帧之间的共视点
    //     //然后重复进行从i+1帧到最后一帧之间的三角化，sfm里对应的部分的位姿
    //     //经过这一步，可以在一开始定义的那些在l和最后一帧之间的共视点上加上深度
        triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);

    }

    for (int i = l + 1; i < frame_num-1; i++)
    {
        triangulateTwoFrames(l, Pose[l], i, Pose[i], sfm_f);
    }
    //4: solve pnp l-1; triangulate l-1 ----- l
    //             l-2              l-2 ----- l
    //三角化所有窗口后部分的帧和地l帧
    //l帧已经被认定为0了，那么其他帧自然就是和他确定

    for (int i = l - 1; i >= 0; i--)
    {
        //solve pnp
        Matrix3d R_initial = c_Rotation[i + 1];
        Vector3d P_initial = c_Translation[i + 1];
        if (!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
            return false;
        c_Rotation[i] = R_initial;
        c_Translation[i] = P_initial;
        c_Quat[i] = c_Rotation[i];
        Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
        Pose[i].block<3, 1>(0, 3) = c_Translation[i];
        //triangulate
        triangulateTwoFrames(i, Pose[i], l, Pose[l], sfm_f);
    }
    //5: triangulate all other points
    for (int j = 0; j < sfm_f.size(); j++)
    {
        if (sfm_f[j].state == true)
            continue;
        if ((int)sfm_f[j].observation.size() >= 2)
        {
            Vector2d point0, point1;
            int frame_0 = sfm_f[j].observation[0].first;
            point0 = sfm_f[j].observation[0].second;
            int frame_1 = sfm_f[j].observation.back().first;
            point1 = sfm_f[j].observation.back().second;
            Vector3d point_3d;
            triangulatePoint(Pose[frame_0], Pose[frame_1], point0, point1, point_3d);
            sfm_f[j].state = true;
            sfm_f[j].position[0] = point_3d(0);
            sfm_f[j].position[1] = point_3d(1);
            sfm_f[j].position[2] = point_3d(2);
            //cout << "trangulated : " << frame_0 << " " << frame_1 << "  3d point : "  << j << "  " << point_3d.transpose() << endl;
        }

    }
    //full BA
    //full BA
    //全优化方式
    ceres::Problem problem;
    ceres::LocalParameterization *local_parameterization = new ceres::QuaternionParameterization();
    //cout << " begin full BA " << endl;
    for (int i = 0; i < frame_num; i++)
    {
        //double array for ceres
        c_translation[i][0] = c_Translation[i].x();
        c_translation[i][1] = c_Translation[i].y();
        c_translation[i][2] = c_Translation[i].z();
        c_rotation[i][0] = c_Quat[i].w();
        c_rotation[i][1] = c_Quat[i].x();
        c_rotation[i][2] = c_Quat[i].y();
        c_rotation[i][3] = c_Quat[i].z();
        problem.AddParameterBlock(c_rotation[i], 4, local_parameterization);
        problem.AddParameterBlock(c_translation[i], 3);
        if (i == l)
        {
            problem.SetParameterBlockConstant(c_rotation[i]);
        }
        if (i == l || i == frame_num - 1)
        {
            problem.SetParameterBlockConstant(c_translation[i]);
        }
    }

    for (int i = 0; i < sfm_f.size(); i++)
    {
        if (sfm_f[i].state != true)
            continue;
        for (int j = 0; j < int(sfm_f[i].observation.size()); j++)
        {
            int l = sfm_f[i].observation[j].first;
            ceres::CostFunction *cost_function = ReprojectionError3D::Create(
                sfm_f[i].observation[j].second.x(),
                sfm_f[i].observation[j].second.y());

            problem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l],
                                     sfm_f[i].position);
        }
    }
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    //options.minimizer_progress_to_stdout = true;
    options.max_solver_time_in_seconds = 0.2;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    //std::cout << summary.BriefReport() << "\n";
    if (summary.termination_type == ceres::CONVERGENCE || summary.final_cost < 5e-03)
    {
        cout << "vision only BA converge" << endl;
    }
    else
    {
        cout << "vision only BA not converge " << endl;
        // return false;
    }
    for (int i = 0; i < frame_num; i++)
    {
        q[i].w() = c_rotation[i][0];
        q[i].x() = c_rotation[i][1];
        q[i].y() = c_rotation[i][2];
        q[i].z() = c_rotation[i][3];
        q[i] = q[i].inverse();
        // cout << "final  q" << " i " << i <<"  " <<q[i].w() << "  " << q[i].vec().transpose() << endl;
        cout<<"final q  "<<endl;
        cout<<q[i].toRotationMatrix()<<endl;

        T[i] = -1 * (q[i] * Vector3d(c_translation[i][0], c_translation[i][1], c_translation[i][2]));
        cout << "final t  "<<endl;
        cout << T[i](0) << "  " << T[i](1) << "  " << T[i](2) << endl;
    }

    for (int i = 0; i < (int)sfm_f.size(); i++)
    {
        if (sfm_f[i].state)
            sfm_tracked_points[sfm_f[i].id] = Vector3d(sfm_f[i].position[0], sfm_f[i].position[1], sfm_f[i].position[2]);
    }
    // return true;

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

    //visual
      Matrix3d newtmp=q[i].toRotationMatrix();
      cv::Affine3d M(
          cv::Affine3d::Mat3(
              newtmp(0, 0), newtmp(0, 1), newtmp(0, 2),
              newtmp(1, 0), newtmp(1, 1), newtmp(1, 2),
              newtmp(2, 0), newtmp(2, 1), newtmp(2, 2)),
          cv::Affine3d::Vec3(
              T[i](0), T[i](1), T[i](2)));

      cv::Mat img_show = imread(rgb_files[i], CV_LOAD_IMAGE_COLOR);
      vector<KeyPoint> kp;
      for (auto sfm : sfm_f)
      {
          for (unsigned sfmindex = 0; sfmindex < sfm.observation.size(); sfmindex++)
          {
              if (sfm.observation[sfmindex].first == i)
              {
                  cv::circle(img_show, Point(sfm.observation[sfmindex].second(0), sfm.observation[sfmindex].second(1)), 10, cv::Scalar(0, 240, 0), 1);
              }
          }
	    }
        // cv::imshow("corners", img_show);
        cv::imshow ( "image", img_show );

        cv::waitKey ( 0);
        vis.setWidgetPose ( "Camera", M );
        vis.spinOnce ( 10, false );
        cout<<endl;
    }

    return 0;
}
void triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
						Vector2d &point0, Vector2d &point1, Vector3d &point_3d)
{
	Matrix4d design_matrix = Matrix4d::Zero();
	design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);
	design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);
	design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);
	design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);
	Vector4d triangulated_point;
	triangulated_point =
		      design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
	point_3d(0) = triangulated_point(0) / triangulated_point(3);
	point_3d(1) = triangulated_point(1) / triangulated_point(3);
	point_3d(2) = triangulated_point(2) / triangulated_point(3);
}
//找到两帧之间的共视点
void triangulateTwoFrames(int frame0, Eigen::Matrix<double, 3, 4> &Pose0,
									 int frame1, Eigen::Matrix<double, 3, 4> &Pose1,
									 vector<SFMFeature> &sfm_f)
{
	assert(frame0 != frame1);
	for (int j = 0; j < sfm_f.size(); j++)
	{
		if (sfm_f[j].state == true)
			continue;
		bool has_0 = false, has_1 = false;
		Vector2d point0;
		Vector2d point1;
		for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)
		{
			if (sfm_f[j].observation[k].first == frame0)
			{
				point0 = sfm_f[j].observation[k].second;
				has_0 = true;
			}
			if (sfm_f[j].observation[k].first == frame1)
			{
				point1 = sfm_f[j].observation[k].second;
				has_1 = true;
			}
		}
		//在所有的点中，这两帧之间存在共视点
		if (has_0 && has_1)
		{
			Vector3d point_3d;
			triangulatePoint(Pose0, Pose1, point0, point1, point_3d);
			sfm_f[j].state = true;
			sfm_f[j].position[0] = point_3d(0);
			sfm_f[j].position[1] = point_3d(1);
			sfm_f[j].position[2] = point_3d(2);
			//cout << "trangulated : " << frame1 << "  3d point : "  << j << "  " << point_3d.transpose() << endl;
		}
	}
}
//看起来初始化的时候这里也没用k，ｔ
bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i,vector<SFMFeature> &sfm_f)
{
	vector<cv::Point2f> pts_2_vector;
	vector<cv::Point3f> pts_3_vector;
	for (int j = 0; j < sfm_f.size(); j++)
	{
		if (sfm_f[j].state != true)
			continue;
		Vector2d point2d;
		for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)
		{
			if (sfm_f[j].observation[k].first == i)
			{
				Vector2d img_pts = sfm_f[j].observation[k].second;
				cv::Point2f pts_2(img_pts(0), img_pts(1));
				pts_2_vector.push_back(pts_2);
				cv::Point3f pts_3(sfm_f[j].position[0], sfm_f[j].position[1], sfm_f[j].position[2]);
				pts_3_vector.push_back(pts_3);
				break;
			}
		}
	}
	if (int(pts_2_vector.size()) < 15)
	{
		printf("unstable features tracking, please slowly move you device!\n");
		if (int(pts_2_vector.size()) < 10)
			return false;
	}
	cv::Mat r, rvec, t, D, tmp_r;
	cv::eigen2cv(R_initial, tmp_r);

// int  cvRodrigues2cvRodr ( const CvMat* src, CvMat* dst, CvMat* jacobian=0 );
//      src为输入的旋转向量（3x1或者1x3）或者旋转矩阵（3x3）。
//      dst为输出的旋转矩阵（3x3）或者旋转向量（3x1或者1x3）。
//      jacobian为可选的输出雅可比矩阵（3x9或者9x3），是输入与输出数组的偏导数。

	cv::Rodrigues(tmp_r, rvec);
	cv::eigen2cv(P_initial, t);
    cv::Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    bool pnp_succ;
    //K在这里内参默认是I都可以吗？？
	pnp_succ = cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1);
	if(!pnp_succ)
	{
		return false;
	}
	cv::Rodrigues(rvec, r);
	//cout << "r " << endl << r << endl;
	MatrixXd R_pnp;
	cv::cv2eigen(r, R_pnp);
	MatrixXd T_pnp;
	cv::cv2eigen(t, T_pnp);
	R_initial = R_pnp;
	P_initial = T_pnp;
	return true;

}

Point2f pixel2cam(const Point2d &p, const Mat &K)
{
    return Point2f(
        (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
        (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}

//获得corres焦点
vector<pair<Vector3d, Vector3d>> getCorresponding(int frame_count_l, int frame_count_r,vector<SFMFeature> &sfm_f)
{
    vector<pair<Vector3d, Vector3d>> corres;
    for (auto &it : sfm_f)
    {

            Vector3d a , b ;
	    for(auto &frameId:it.observation)
	    {
	      if(frameId.first==frame_count_l)
	      {
		a=Vector3d( frameId.second(0),frameId.second(1),0);
	      }
	      if(frameId.first==frame_count_r)
	      {
		b=Vector3d( frameId.second(0),frameId.second(1),0);
	      }
	    }

            corres.push_back(make_pair(a, b));

    }
    return corres;
}

bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l,int WINDOW_SIZE,vector<SFMFeature> &sfm_f)
{
    //corres考察视差
    // find previous frame which contians enough correspondance and parallex with newest frame
    for (int i = 0; i < WINDOW_SIZE; i++)
    {
        vector<pair<Vector3d, Vector3d>> corres;
        corres = getCorresponding(i, WINDOW_SIZE-1,sfm_f);
        //判断窗口内所有的帧和最后的帧之间的交点个数
        //判断交点个数，要求大于20个

        if (corres.size() > 20)
        {
            double sum_parallax = 0;
            double average_parallax;
            for (int j = 0; j < int(corres.size()); j++)
            {
                Vector2d pts_0(corres[j].first(0), corres[j].first(1));
                Vector2d pts_1(corres[j].second(0), corres[j].second(1));
                double parallax = (pts_0 - pts_1).norm();
                sum_parallax = sum_parallax + parallax;

            }
            average_parallax = 1.0 * sum_parallax / int(corres.size());
            //满足特征点之间的平均距离要符合一个要求，然后使用5点法求解RT
            //这里求出的是基于第i帧的位姿
// 	    cout<<average_parallax<<endl;
            if(average_parallax * 460 > 30 && solveRelativeRT(corres, relative_R, relative_T))
            {
	      cout<<corres.size()<<endl;
                l = i;
//                 ROS_DEBUG("average_parallax %f choose l %d and newest frame to triangulate the whole structure", average_parallax * 460, l);
                return true;
            }
        }

    }
    return false;
}
#endif


// Boost.Geometry

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014.
// Modifications copyright (c) 2014 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_VINCENTY_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_VINCENTY_HPP

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/strategies/distance.hpp>

#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/algorithms/detail/vincenty_inverse.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Distance calculation formulae on latlong coordinates, after Vincenty, 1975
\ingroup distance
\tparam Spheroid The reference spheroid model
\tparam CalculationType \tparam_calculation
\author See
    - http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
    - http://www.icsm.gov.au/gda/gdav2.3.pdf
\author Adapted from various implementations to get it close to the original document
    - http://www.movable-type.co.uk/scripts/LatLongVincenty.html
    - http://exogen.case.edu/projects/geopy/source/geopy.distance.html
    - http://futureboy.homeip.net/fsp/colorize.fsp?fileName=navigation.frink

*/
template
<
    typename Spheroid,
    typename CalculationType = void
>
class vincenty
{
public :
    template <typename Point1, typename Point2>
    struct calculation_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point1,
                      Point2,
                      CalculationType
                  >::type
          >
    {};

    typedef Spheroid model_type;

    inline vincenty()
        : m_spheroid()
    {}

    explicit inline vincenty(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    template <typename Point1, typename Point2>
    inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& point1, Point2 const& point2) const
    {
        return geometry::detail::vincenty_inverse
                <
                    typename calculation_type<Point1, Point2>::type
                >(get_as_radian<0>(point1),
                  get_as_radian<1>(point1),
                  get_as_radian<0>(point2),
                  get_as_radian<1>(point2),
                  m_spheroid).distance();
    }

    inline Spheroid const& model() const
    {
        return m_spheroid;
    }

private :
    Spheroid m_spheroid;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename Spheroid, typename CalculationType>
struct tag<vincenty<Spheroid, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct return_type<vincenty<Spheroid, CalculationType>, P1, P2>
    : vincenty<Spheroid, CalculationType>::template calculation_type<P1, P2>
{};

template <typename Spheroid, typename CalculationType>
struct comparable_type<vincenty<Spheroid, CalculationType> >
{
    typedef vincenty<Spheroid, CalculationType> type;
};

template <typename Spheroid, typename CalculationType>
struct get_comparable<vincenty<Spheroid, CalculationType> >
{
    static inline vincenty<Spheroid, CalculationType> apply(vincenty<Spheroid, CalculationType> const& input)
    {
        return input;
    }
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct result_from_distance<vincenty<Spheroid, CalculationType>, P1, P2 >
{
    template <typename T>
    static inline typename return_type<vincenty<Spheroid, CalculationType>, P1, P2>::type
        apply(vincenty<Spheroid, CalculationType> const& , T const& value)
    {
        return value;
    }
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

// We might add a vincenty-like strategy also for point-segment distance, but to calculate the projected point is not trivial

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_VINCENTY_HPP


/*!  @author Michael Brand
*    @Excercise 10 - final
*    @date 26.07.2017
*
*   Algorithm to find Steiner tree on a graph where primes count as termainals.
*
*   I decided to leave all code in one file since its mainly consistent of two bigger algorithms
*   1. Dijkstra_mod
*   2. Analyzing Dijkstra output in main-function
*   I don't think it gets to complicated reading it from top to bottom.
*
*   Parallel computing
*   Take #n random source nodes from parameter input and start the heurisitic parallely from these sources
*/

#include <iostream>
#include <fstream>
#include <stdexcept>
#include <cstdio>
#include <ctime>
#include <chrono>
#include <string>
#include <vector>
#include <climits>
#include <utility>                          // for std::pair
#include <random>

#include <boost/graph/adjacency_list.hpp>

#ifdef _OPENMP
#include <omp.h>
#endif

using namespace std;

using vertex_ = int;
using weight_ = int;

/**Edge
 * pair of ints containing the weight and
 * the vertex pointed to. This is useful
 * for the adjacency list
 */
using Edge = pair<vertex_, weight_>;

/**Graph
 * An adjacency list representing the graph.
 * graph[i] returns a list with Edge elements.
 *
 */
using Graph = vector< vector<Edge> >;

/**struct pq_compare
 * compare structure for edges
 * allows me to compare two edges in a graph
 * this way I can sort edges for any vertex under consideration of their weight
 */
struct pq_compare {
    bool operator() (const Edge i, const Edge j) const{
    return (i.second <= j.second); }
};

/*! \fn bool isPrime(int number)
    \brief checks if number is a prime.
    \param number number to be checked.
*/
bool isPrime(int number){
    if(number < 2) return false;
    if(number == 2) return true;
    if(number % 2 == 0) return false;
    for(int i=3; (i*i)<=number; i+=2){
        if(number % i == 0 ) return false;
    }
    return true;

}

/*! \fn std::vector<int> getRequiredPrimes(int numV)
 *  \brief lists all primes <= numV in a vector.
 *  \param numV number to be checked.
*/
std::vector<int> getRequiredPrimes(int numV){
  std::vector<int> primes;
  for(int i=2; i<=numV; i++){
    if(isPrime(i)){
      primes.push_back(i);
    }
  }
  return primes;
}

/*! \fn bool edgeInSteinerAlEx(Graph &stGph, vertex_ u, vertex_ v){
*   \brief Checks if Edge (u,v) already exists in SteinerGraph
*	  \param stGph Steiner Tree Graph that is checked for existing edge
*   \param u source-vertex
*   \param v dest-vertex
*/
bool edgeInSteinerAlEx(Graph &stGph, vertex_ u, vertex_ v){
	for(vector<Edge>::const_iterator ni = stGph[u].begin(); ni != stGph[u].end(); ni++){
		if(ni->first==v)
			return true;
	}
	return false;
}
/*! \fn std::vector<vertex_> dijkstra_mod(const Graph &graph, vertex_ root, vector<int> &remPrimes)
*   \brief modified dijkstra algorithm used for steiner tree problem
*   \param &graph the graph we are working on.
*   \param &steinerGraph an empty graph of size(graph) that will be used to build steinerGraph
*   \param root root-vertex
*   \return objective Value of SteinerTree
*/
int dijkstra_mod(const Graph &graph, Graph &steinerGraph, vertex_ root) {
  int wghtLocTree = 0; /*!< return value */
  std::vector<int> remPrimes = getRequiredPrimes(graph.size());
  std::vector<weight_> dist(graph.size(), INT_MAX);
  /* A set helps insertion and insert/erase/find operations in logarithmic time.
   * This set maintains Edge(distance,vertex number) sorted on basis of distance
   */
  set< Edge , pq_compare> pq;
  set< Edge , pq_compare > ::iterator it;

  vector<vertex_> pre(graph.size(), (-1)); /*!< vevtor of predecessors */
  int u,v,wt;

  dist[root] = 0;
  pq.insert(Edge(root,0));
  while(pq.size() != 0){
  	if(!(remPrimes.size()>0)) break;
    bool pFound = false;
    it = pq.begin();
    u = it->first;
    pq.erase(it);
    if((u != root) && (isPrime(u+1))){
      for(unsigned int chk=0; chk<remPrimes.size(); chk++){
        if((remPrimes[chk])==(u+1)){
          //NEW PRIME FOUND
          pFound = true;
          //delete u from remainingPrimes
          remPrimes.erase(remPrimes.begin()+chk);
          pq.insert(Edge(u,0));
          dist[u]=0;
          //Add Path to u to SteinerTree Structure
          int lV = u;
          do{
            int preV = pre[lV];
            for(unsigned int eIndx = 0; eIndx < graph[lV].size(); eIndx++){
              if(graph[lV][eIndx].first==preV){
              	if(!(edgeInSteinerAlEx(steinerGraph, lV,preV))){
            			steinerGraph[lV].push_back(Edge(preV, graph[lV][eIndx].second));
	                steinerGraph[preV].push_back(Edge(lV, graph[lV][eIndx].second));
	                wghtLocTree += graph[lV][eIndx].second;
	                pq.insert(Edge(preV,0));
            		}
                else{
                  //abort inserting edges since they are all part of graph
                  break;
                }
              }
            }
	          lV = pre[lV];
          }while(dist[lV] != 0);
          break;
        }
      }
    }
    if(pFound){
      continue;
    }
    for(vector<Edge>::const_iterator ni = graph[u].begin(); ni != graph[u].end(); ni++){
      v  = ni->first;
      wt = ni->second;
      if(dist[v] > dist[u] + wt){
        pre[v] = u;
        if(dist[v] != INT_MAX){
        	pq.erase(Edge(v,dist[v]));
        }
        dist[v] = dist[u] + wt;
        pq.insert(Edge(v,dist[v]));
      }

    }
  }
  return wghtLocTree;
}

/*! \fn int validateStGraph(&gphToChk)
*   \brief tests if gphToChk is connected and inherits all primes
*   \param &grphToChk minimal SteinerTree found
*   \return 1 if successful, 0 if unsuccessful
*/
int validateStGraph(Graph &gphToChk){

	std::vector<int> remPrimes = getRequiredPrimes(gphToChk.size());
	int totPrimes = remPrimes.size();
	int stTreePrimeCount = 0;

	bool visited[gphToChk.size()];
	for(int i=0; i<gphToChk.size(); i++)
		visited[i]=false;

  if(!(gphToChk[1].empty())){
		set< Edge , pq_compare> pq;
		set< Edge , pq_compare > ::iterator it;
		int u,v,wt;
		pq.insert(Edge(1,0));
		while(pq.size() != 0){
	    it = pq.begin();
		  u = it->first;
		  pq.erase(it);
		  if(visited[u]){
		  	return 0;
		  }
			visited[u] = true;
			for(vector<Edge>::const_iterator ni = gphToChk[u].begin(); ni != gphToChk[u].end(); ni++){
				v  = ni->first;
	     	wt = ni->second;
	     	pq.insert(Edge(v,wt));
	     	//Delete ṕarrallel edge from minSteinerGraph
	     	for(int k=0; k<gphToChk[v].size(); k++){
	     		if(gphToChk[v][k].first == u){
	     			gphToChk[v].erase(gphToChk[v].begin()+k);
	     		}
	     	}
			}
    }

    for(int j=1; j<gphToChk.size(); j++){
    	if(isPrime(j-1)){
    		if(!(gphToChk[j-1].empty()))
    			stTreePrimeCount++;
    	}
    }

    if(stTreePrimeCount = totPrimes){
    	return 1;
    }
    else{
    	return 0;
    }
  }
  else{
		return 0;
	}
}

/*!
*
*		INPUT n_root_nodes gives an int-number that defines the number of chosen terminals
*/
int main (int argc, char* argv[]) {
  /**
  * start timers for cpu and wall time
  */
  clock_t cpu0 = clock();
  bool output = false;

  /*
  *   Check input arguments
  */
  if( argc < 3){
      fprintf(stderr, "Call the program as: %s 'filename.gph' 'n_root_nodes' (-s)\n", argv[0]);
      exit(EXIT_FAILURE);
  }
  if(std::string(argv[argc-1]) == "-s"){
  	output=true;
  }

  ifstream    file(argv[1]);
  string      line;

  if(!file){
    fprintf(stderr, "Could not open file.\n");
    return -1;
  }

  /**
  * read number of vertices and edges
  */
  getline(file, line, ' ');
  const int numV = stoi(line);
  getline(file, line, '\n');
  const int numE = stoi(line);

  /**
  *	check if no nodes doesnt exceed vertices of graph
  */
  int n_nodes = 0;

  try{
  	if((stoi(argv[2]) > 0) && (stoi(argv[2]) <= numV))
  		n_nodes = stoi(argv[2]);
  	else{
  		fprintf(stderr, "Make sure parameter n_root_nodes is positive and does not exceed graph size.\n");
      exit(EXIT_FAILURE);
  	}
  }
  catch(...){
  	fprintf(stderr, "Call the program as: %s 'filename.gph' 'n_root_nodes' (-s)\n", argv[0]);
    exit(EXIT_FAILURE);
  }

  /*!
  *   FILL UP sourceVert by random numbers in range 0<->graph.size())
  */
  vector<int> sourceVert;
	std::random_device rd;     // only used once to initialise (seed) engine
	std::mt19937 rng(rd());    // random-number engine used (Mersenne-Twister in this case)
	for(int k=0; k<n_nodes; k++){
		std::uniform_int_distribution<int> uni(0,numV-1);
		sourceVert.push_back(uni(rng));
	}

  /**
  * create graph
  */
  Graph graph(numV);

  /**
  * read ín given gph file
  */
  while( getline(file, line) ){
    stringstream linestream(line);
    string       vertex1, vertex2, weight;
    try{
      getline(linestream, vertex1, ' ');
      getline(linestream, vertex2, ' ');
      getline(linestream, weight, '\n');

      /**
      * add both directions of edge to the graph since undirected
      * index switch applies: 1 --> 0, 1 --> 2, etc.
      */
      graph[stoi(vertex1)-1].push_back(Edge(stoi(vertex2)-1, stoi(weight)));
      graph[stoi(vertex2)-1].push_back(Edge(stoi(vertex1)-1, stoi(weight)));
    }catch (...){
    	fprintf(stderr, "Corrupt data in .gph file.");
      exit(EXIT_FAILURE);
    }

  }
  file.close();

  //Input Done. Start Wall Time counter
  auto   wall0 = chrono::system_clock::now();

  int minStVal = INT_MAX;
  Graph minSteiner(numV);
  int locStTrWght[sourceVert.size()];

  //Parallel section (ex10)
  #pragma omp parallel for shared(locStTrWght, minStVal, minSteiner)

  for(unsigned int initroot=0; initroot<sourceVert.size(); initroot++){
    //Create local SteinerGraph as emty Graph
    Graph stGraph(numV);
    //Check if initroot is in Graph-range
    if(sourceVert[initroot]>=graph.size()){
      fprintf(stderr, "Invalid source node: %i. Please make sure that root node indices are lower than graph size!\n", sourceVert[initroot]+1);
      exit(EXIT_FAILURE);
    }
    //Call Dijkstra with initroot-vertex as source
    locStTrWght[initroot] = dijkstra_mod(graph, stGraph, sourceVert[initroot]);

    //If return value is new min, save SteinerGraph
    if(locStTrWght[initroot]<minStVal){
    	minSteiner = stGraph;
    	minStVal = locStTrWght[initroot];
    }
  }

  //End Wall-Timer
  chrono::duration<double> wallDur = (chrono::system_clock::now() - wall0);
  double wallTime = wallDur.count();

  /*!
  *   RUN EXTRA TEST
  *   Checks if graph is a tree + if all primes are found in the tree
  *   returns 1 if successful, 0 if unsuccessful
  */
  int validate = validateStGraph(minSteiner);

  if(validate==0) fprintf(stdout, "ERROR.\nSteiner Graph is not a tree\n");

  /*
  *   OUTPUT
  */
  int ocount=0;
  fprintf(stdout, "TLEN:\t %i\n", minStVal);

  //Output of MinSteiner-Edges (only in manual call (-s).)
  if(output){
  	fprintf(stdout, "TREE:\t");
  	for(unsigned int i=0; i<minSteiner.size(); i++){
  		if(!(minSteiner[i].empty())){
  			for(unsigned int j=0; j<minSteiner[i].size(); j++){
  				fprintf(stdout, "(%i,%i), ", i+1, minSteiner[i][j].first+1);
  				ocount++;
  				if((ocount%10)==0) fprintf(stdout, "\n\t");
  			}
  		}
  	}
  	fprintf(stdout, "\n");
  }
  double cpuTime = (clock() - cpu0) / (double) CLOCKS_PER_SEC;
  fprintf(stdout, "TIME:\t %f\n", cpuTime);
  fprintf(stdout, "WALL:\t %f\n", wallTime);
  return 0;
}


#ifndef _HASH_HPP
#define _HASH_HPP
#include <string>
#include <vector>
#include <stdint.h>
#include <boost/multiprecision/gmp.hpp>
#include <boost/multiprecision/cpp_int.hpp>

namespace usebitcoins {
namespace hash {

// Generate BTC addresses deterministically by hashing the customer's ID
// (e.g., an email address)

using email_address_t = std::string;

/// Perform straightforward hash of arbitrary data
std::vector<uint8_t> blake2_hash(std::vector<uint8_t> src);

// Hash an arbitrary string via blake2 to a byte array
std::vector<uint8_t> hash_string(std::string_view src);

/// Hash email address to a 64-bit integer
uint64_t hash_email(const email_address_t& email_address);

/// Fit a byte array into a bignum
/// Assumes that `src` is arranged in little-endian order
boost::multiprecision::uint256_t pack_byte_array(const std::vector<uint8_t>& src);

} // namespace hash
} // namespace usebitcoins

#endif // _HASH_HPP


#ifndef BIN_PACKING
#define BIN_PACKING

#include <iostream>
#include <fstream>
#include <cassert>
#include <vector>
#include <deque>
#include <iomanip>
#include <cmath>
#include <string>
#include <cstring>
#include <chrono>

#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/overlaps.hpp>
#include <boost/geometry/geometries/geometries.hpp>

namespace boost_geo = boost::geometry;
namespace trans = boost::geometry::strategy::transform;

typedef boost_geo::model::point<double, 2, boost_geo::cs::cartesian> Point;
typedef boost_geo::model::polygon<Point> Polygon;
typedef boost_geo::model::multi_polygon<Polygon> MultiPolygon;
typedef boost_geo::model::box<Point> Box;
typedef std::vector<std::pair<double, double>> PolygonInput;
typedef Point Vector;

#define x get<0>()
#define y get<1>()

const double EPS = 1e-8;
const double INF = 4e18;
const double PI = acos(-1);

static int frameno;

/**
 * customized geometry utility methods using boost
 * */
namespace boost_geo_util
{
    Polygon constructBGPolygon(PolygonInput &);
    bool isPolygonIntersectPolygon(MultiPolygon &, MultiPolygon &);
    bool isPointInsidePolygons(MultiPolygon &, Point &);
    void visualize(MultiPolygon &, std::string);
}; // namespace boost_geo_util

namespace bin_packing
{
    void runDataset(std::string, std::string, double);
    std::vector<PolygonInput> readDataset(std::string, std::vector<int> &, double &);
    void binPacking(std::vector<PolygonInput> &, double, double &, std::string);
    void placeItem(MultiPolygon &, Polygon &, double, double &);
    void normalize(PolygonInput &);
    void normalizePolygon(Polygon &);
    double getLength(Polygon &);
    double getWidth(Polygon &);
}; // namespace bin_packing

#endif // BIN_PACKING


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      131204    S. Hirsh          File created.
 *      140120    T. Roegiers       Codecheck small changes.
 *      140410    S. Hirsh          Minor revisions.
 *      140411    T. Roegiers       Minor textual changes during codecheck.
 *
 *    References
 *
 *
 *    Notes
 *
 */

#include <cmath>

#include <Eigen/Geometry>

#include "Tudat/Mathematics/BasicMathematics/rotationAboutArbitraryAxis.h"

namespace tudat
{
namespace basic_mathematics
{

//! Compute rotation of point about arbitrary axis
Eigen::Vector3d computeRotationOfPointAboutArbitraryAxis(
        const Eigen::Vector3d& originOfRotation,
        const double angleOfRotation,
        const Eigen::Vector3d& axisOfRotation,
        const Eigen::Vector3d& initialPositionOfPoint )
{

    //Declare and initialize rotation matrix
    Eigen::Matrix3d rotationMatrix = Eigen::Matrix3d::Zero( );

    // Compute rotation matrix using AngleAxis object.
    rotationMatrix = Eigen::AngleAxisd( angleOfRotation, axisOfRotation.normalized( ) );

    // Compute initial of position of point with respect to origin of rotation.
    const Eigen::Vector3d initialPositionOfPointWithRespectToOriginOfRotation =
            initialPositionOfPoint - originOfRotation;

    // Compute rotation of point about axis of rotation with respect to origin of rotation.
    const Eigen::Vector3d rotatedPositionWithRespectToOriginOfRotation =
            rotationMatrix * initialPositionOfPointWithRespectToOriginOfRotation;

    //Return position with respect to the chosen arbitrary origin after rotation about
    //arbitrary axis.
    return rotatedPositionWithRespectToOriginOfRotation + originOfRotation;

}

//! Compute rotation of vector about arbitrary axis
Eigen::Vector3d computeRotationOfVectorAboutArbitraryAxis(
        const Eigen::Vector3d& originOfRotation,
        const double angleOfRotation,
        const Eigen::Vector3d& axisOfRotation,
        const Eigen::Vector3d& initialPositionOfVectorTail,
        const Eigen::Vector3d& initialVector )
{

    // Compute rotation of the tail of vector. Resulted position is with respect to the chosen
    // arbitrary origin.
    Eigen::Vector3d rotatedPositionOfVectorTail =
            computeRotationOfPointAboutArbitraryAxis( originOfRotation, angleOfRotation,
                                                      axisOfRotation, initialPositionOfVectorTail );

    // Compute rotation of the head of vector. Resulted position is with respect to the chosen
    // arbitrary origin.
    Eigen::Vector3d rotatedPositionOfVectorHead =
            computeRotationOfPointAboutArbitraryAxis( originOfRotation, angleOfRotation,
                                                      axisOfRotation,
                                                      initialPositionOfVectorTail + initialVector );

    // Return rotated vector with respect to the chosen arbitrary origin
    return rotatedPositionOfVectorHead - rotatedPositionOfVectorTail;

}

} // namespace basic_mathematics
} // namespace tudat


#include <complex>
#include <iostream>
#include <string>
#include <boost/spirit/include/qi.hpp>

// parse complex number in the form of a + bi
bool parse_complex_number(const std::string& str, std::complex<double>& val) {
    namespace qi = boost::spirit::qi;
    namespace ascii = boost::spirit::ascii;

    using qi::double_;
    using qi::phrase_parse;
    using ascii::space;

    // construct the grammar
    auto real_part = double_;
    auto imag_part = double_ >> 'i';
    auto grammar = real_part >> -imag_part;

    double real = 0;
    double imag = 0;
    auto begin = str.begin();
    auto end = str.end();
    bool r = phrase_parse(begin, end, grammar, space, real, imag);

    // fail if we did not get a full match
    if (begin != end)
        return false;

    // set the value if success
    val = { real, imag };
    return r;
}

int main() {
    std::cout << "Complex number parser.\n";
    std::cout << "Type 'quit' to quit.\n\n";

    for (std::string line; getline(std::cin, line);) {
        if (line == "quit")
            break;

        std::complex<double> val;
        if (parse_complex_number(line, val)) {
            std::cout << "Parsing succeeded.\n";
            std::cout << "You entered (" << val.real() << "; " << val.imag() << ")\n\n";
        }
        else {
            std::cout << "Parsing failed\n\n";
        }
    }
}