/*********************************************************************************/
/*                                                                               */
/*     Command-line tool to access support vector machine                        */
/*                          classification functionality.                        */
/*                                                                               */
/*     Alle Meije Wink                                                           */
/*                                                                               */
/*********************************************************************************/

/*
  Update history

  Who    When       What
  AMW    13-10-12   creation

*/

#define DLIB_PNG_SUPPORT

#include <dlib/image_io.h>                              //        -- be able to write png
#include <dlib/svm.h>                                   // n-dimensional vectors (n!=3) should use matrix in dlib
#include <dlib/matrix.h>                                //        -- see http://dlib.net/linear_algebra.html#vector
#include <dlib/svm/svm_c_linear_dcd_trainer.h>          // use svm trainer that supports "warm starting"
                                                        // see http://dlib.net/dlib/svm/active_learning.h.html

#include "combisDesign.hpp"

using namespace dlib;

int combisSVM(bis::bisnifti<value_type> *currentImage, std::string designfile)
{
    // test combis with
    // -i ~/work/documents/memorabel/PRNI2016/vumc/ECM/allmask_fmri.nii.gz --svm ~/work/documents/memorabel/PRNI2016/vumc/ECM/image_matrix.json -o ~/work/documents/memorabel/PRNI2016/vumc/ECM/weights.nii.gz

    ////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////
    // main data types

    // classification table
    std::vector<value_type>                   y;
    std::vector<dlib::matrix<value_type,0,1>> x;

    ////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////
    // data input from file

    // read the design
    auto design_isbinary=combisSVMdesign(designfile, &x, &y, currentImage);   // read text / binary design

    ////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////
    // kernel design

    // type of sample (1 image) and kernel
    typedef dlib::matrix<value_type,0,1> sample_type;                              // one sample == one row of x
    typedef dlib::linear_kernel<sample_type> kernel_type;                         // kernel data type

    std::vector<sample_type> samples;                                             // sample set container
    std::vector<value_type> labels;                                                // label set container

    samples.assign(x.begin(), x.end());
    labels.assign(y.begin(), y.end());

    for (auto l: labels)
        std::cout << l << std::endl;
    if (!design_isbinary)
        for (auto s: samples)
            std::cout << s << std::endl;

    // trainer for this type of kernel
    // This trainer solves the "C" formulation of the SVM.  See the documentation for
    // details.
    dlib::svm_c_linear_dcd_trainer<kernel_type> linear_dcd_trainer;
    linear_dcd_trainer.set_c(1000);

    // normalise samples of x -- see http://dlib.net/svm_ex.cpp.html
    //dlib::vector_normalizer<sample_type> normalizer;
    //normalizer.train(samples);
    //for (auto sx:samples)
    //    sx = normalizer(sx);

    // preserve the state of the classifier for warm-starting (see active_learing.h)
    typedef typename dlib::svm_c_linear_dcd_trainer<kernel_type>::optimizer_state optimizer_state;
    optimizer_state state;

    ////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////
    // construct decision / projection vector
    typedef decision_function< kernel_type > dftype;
    dftype decision_function = linear_dcd_trainer.train(samples, labels, state);

    sample_type m(2);
    m(0)=m(1)=0;
    for (long i = 0; i < decision_function.alpha.nr(); ++i) {
        std::cout << i << std::endl;
        std::cout << decision_function.alpha(i) << std::endl;
        std::cout << decision_function.basis_vectors(i) << std::endl;
        m += decision_function.alpha(i) * decision_function.basis_vectors(i);
    }
    std::cout << "b:" << std::endl;
    std::cout << decision_function.b << std::endl;

    #define RED     "\033[31m"      /* Red */
    #define GREEN   "\033[32m"      /* Green */
    #define YELLOW  "\033[33m"      /* Yellow */
    #define RESET   "\033[0m"

    dlib::array2d<float> map_image;
    float top=5,step=.49;
    map_image.set_size(top/step+1,top/step+1);
    for (float j=top; j>=0.; j-=step) {
        for (float i=0.1; i<=top; i+=step) {
            m(0)=i; m(1)=j;
            auto f = decision_function(m);
            map_image[i][j]=f;
            if (f<-1.)
                printf("%1.01f,%1.01f -> %s%5.02f%s  ",i,j,GREEN,-1.,RESET);
            else if (f>1.)
                printf("%1.01f,%1.01f -> %s%5.02f%s  ",i,j,RED,1.,RESET);
            else
                printf("%1.01f,%1.01f -> %s%5.02f%s  ",i,j,YELLOW,0.,RESET);
            }
        std::cout << std::endl; }
    //dlib::save_png(map_image, "/tmp/map_image.png");

    #undef RED
    #undef GREEN
    #undef YELLOW
    #undef RESET

    /*

    if (!my_machine.margin_set.empty())
        for (size_t k=0; k<my_machine.margin_set.size(); k++) {
            if (my_machine.output(my_machine.margin_key[k]))
                projection += (my_machine.weight[k] * x[my_machine.margin_key[k]]);
            else
                projection -= (my_machine.weight[k] * x[my_machine.margin_key[k]]);
        }
    if (!my_machine.error_set.empty())
        for (size_t i=0; i<my_machine.error_set.size(); i++) {
                projection -= (C * x[my_machine.every_key[my_machine.error_set[i]]]);
        }
    if (!my_machine.error_star_set.empty())
        for (size_t i=0; i<my_machine.error_star_set.size(); i++) {
                projection -= (C * x[my_machine.every_key[my_machine.error_star_set[i]]]);
        }
    auto projectionbias=my_machine.bias;

    ////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////
    // demonstrate output

    if (y.size() == 4)
    {

        // test data with 0..5 x 0..5 grid and print classes
        double stp=1.0;//                                                             // sample the input space and show the classification
        for ( double y=5.; y>=0.; y-=stp )                                            // at all sampled points
        {
            for ( double x=0.; x<=5.; x+=stp )
            {
                ublas::vector <value_type> testxy(2);
                testxy[0]=x;
                testxy[1]=y;
                std::cout << "("
                        << std::fixed << std::setprecision(1) << x << ","
                        << std::fixed << std::setprecision(1) << y << ") -> "
                        << my_machine (testxy) << ", " ;
            }
            std::cout << std::endl;
        }
        std::cout << std::endl;

        // sample the input space and show the classification
        for ( double y=5.; y>=0.; y-=stp )                                            // at all sampled points
        {
            for ( double x=0.; x<=5.; x+=stp )
            {
                ublas::vector <value_type> testxy(2);
                testxy[0]=x;
                testxy[1]=y;
                std::cout << "("
                        << std::fixed << std::setprecision(1) << x << ","
                        << std::fixed << std::setprecision(1) << y << ") -> "
                        << projectionbias + blas::dot ( projection, testxy ) << ", " ;
            }
            std::cout << std::endl;
        }

        std::cout << "weights:\n";
        auto alpha=my_machine.weight;
        for (auto a: alpha) std::cout << a << " ";
        std::cout << std::endl;
        std::cout << "margin vectors:\n";
        {auto vecset=my_machine.margin_set;
        for (auto v: vecset) std::cout << v << " ";}
        std::cout << std::endl;
        std::cout << "bias:\n" << my_machine.bias << std::endl;
        std::cout << "C:\n" << my_machine.C << std::endl;

        std::cout << "error vectors:\n";
        {auto vecset=my_machine.error_set;
        for (auto v: vecset) std::cout << v << " ";}
        std::cout << std::endl;
        std::cout << "remaining vectors:\n";
        {auto vecset=my_machine.remaining_set;
        for (auto v: vecset) std::cout << v << " ";}
        std::cout << std::endl;

    }

    // if images -> make a projection image (weights map)
    // that shows the voting rights of each brain region
    if (design_isbinary) { // images design

        currentImage->bisArray::operator*=(0);
        {size_t i=0;
            for (auto m: mask)
                currentImage->my_data[m]=projection[i++]/mstd;
        }
        std::cout << "mask size: " << mask.size() << std::endl;

        std::cout << "projections of training images on weight map: " << std::endl;
        for (auto k:keys)
            std::cout << "specimen " << k << ", class " << y[k] << " SVM output " << my_machine(x[k]) << " projection " << projectionbias + blas::dot ( projection, x[k] ) << std::endl;

        auto iminfo=nifti_copy_nim_info(currentImage->getHeader());

        // std::cout << "storing bias in toffset " << std::endl;
        iminfo->toffset=float(projectionbias);
        //iminfo->scl_inter=float(projectionbias);

        // std::cout << "map in current image " << std::endl;
        currentImage->setHeader(iminfo);

        //nifti_image_infodump(iminfo);

    }

    ////////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////////////////////////////////////////////////////
    // cross-validation

    auto cv=1;

    if (cv) {

    std::vector<double> Cs = {1./16000., 1./8000, 1./4000., 1./2000., 1./1000.,};

    for (auto C2: Cs) {

        std::cout << "C value = " << std::setw(6) << C2 << std::endl;

        unsigned n=0,p=0,tn=0,tp=0,sv=0;
        for (auto k2:keys) {
            auto keys2 = keys;
            keys2.erase(keys2.begin()+k2);
            onlinesvm_machine_type my_machine2( C2, my_kernel, training_data );        // C = 1.0
            my_machine2.learn( keys2.begin(), keys2.end() );                           // start the learning
            auto in2=y[k2];
            auto out2=2*my_machine2(x[k2])-1;
            std::cout << "crossval " << k2 << ", class " << in2 << " SVM output " << out2
                      << ", #SV " << my_machine2.margin_set.size()+my_machine2.margin_set.size()
                      << ", bias " << my_machine2.bias << std::endl;
            if (in2<0)
                {n++; tn+=(out2<0);}
            else
                {p++; tp+=(out2>0);}
            sv=sv+my_machine2.margin_set.size();
        }

        sv/=(keys.size()-1);
        auto tnr=double(tn)/double(n);
        auto tpr=double(tp)/double(p);
        std::cout << "  true negative ratio = " << tnr
                  << ", true positive ratio = " << tpr
                  << ", balanced accuracy   = " << (tnr+tpr)/2.
                  << ", average #SV = " << sv << std::endl;

    }

    }
    */

	return 0;

}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

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

#ifndef BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_ROTATION_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_ROTATION_HPP

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

#include <boost/geometry/extensions/algebra/algorithms/detail.hpp>

#include <boost/geometry/extensions/algebra/geometries/concepts/rotation_quaternion_concept.hpp>

// TODO - for multiplication of coordinates
// if coordinate_type is_integral - use double as the result type

namespace boost { namespace geometry {

namespace detail { namespace rotation {

template <typename V1, typename V2, typename Rotation, typename Tag1, typename Tag2, std::size_t Dimension>
struct matrix
{
    BOOST_MPL_ASSERT_MSG(false, NOT_IMPLEMENTED_FOR_THIS_DIMENSION, (Rotation));
};

template <typename V1, typename V2, typename Rotation>
struct matrix<V1, V2, Rotation, vector_tag, vector_tag, 3>
{
    static const bool cs_check =
        ::boost::is_same<typename traits::coordinate_system<V1>::type, cs::cartesian>::value &&
        ::boost::is_same<typename traits::coordinate_system<V2>::type, cs::cartesian>::value;

    BOOST_MPL_ASSERT_MSG(cs_check, NOT_IMPLEMENTED_FOR_THOSE_SYSTEMS, (V1, V2));

    typedef typename geometry::select_most_precise<
        typename traits::coordinate_type<V1>::type,
        typename traits::coordinate_type<V2>::type
    >::type cv_type;

    typedef typename geometry::select_most_precise<
        cv_type,
        typename traits::coordinate_type<Rotation>::type
    >::type cr_type;

    typedef model::vector<cv_type, 3> vector_type;

    inline static void apply(V1 const& v1, V2 const& v2, Rotation & r)
    {
        namespace da = detail::algebra;

        // TODO - should store coordinates in more precise variables before the normalization?

        // angle
        cv_type d = da::dot<0, 0, 3>(v1, v2);
        cv_type l =
            math::sqrt(da::dot<0, 0, 3>(v1, v1) * da::dot<0, 0, 3>(v2, v2));
        cv_type c = d / l;

        // rotation angle == 0
        // not needed really, because in this case function still returns zero-rotation
        if ( 1 - std::numeric_limits<cv_type>::epsilon() <= c )
        {
            set<0, 0>(r, 1); set<0, 1>(r, 0); set<0, 2>(r, 0);
            set<1, 0>(r, 0); set<1, 1>(r, 1); set<1, 2>(r, 0);
            set<2, 0>(r, 0); set<2, 1>(r, 0); set<2, 2>(r, 1);
            return;
        }

        vector_type axis;

        // rotation angle = 180
        if ( c <= std::numeric_limits<cv_type>::epsilon() - 1 )
        {
            // find arbitrary rotation axis perpendicular to v1
            da::cross<0, 0, 0>(vector_type(1, 0, 0), v1, axis);
            if ( da::dot<0, 0, 3>(axis, axis) < std::numeric_limits<cr_type>::epsilon() )
                da::cross<0, 0, 0>(vector_type(0, 1, 0), v1, axis);
        }
        else
        {
            // rotation axis
            da::cross<0, 0, 0>(v1, v2, axis);
        }

        // sin
        cv_type s = math::sqrt(1 - c * c);
        cv_type t = 1 - c;
        // normalize axis
        da::normalize<0, 3>(axis);

        cv_type txx = t*get<0>(axis)*get<0>(axis);
        cv_type tyy = t*get<1>(axis)*get<1>(axis);
        cv_type tzz = t*get<2>(axis)*get<2>(axis);
        cv_type txy = t*get<0>(axis)*get<1>(axis);
        cv_type sx = s*get<0>(axis);
        cv_type txz = t*get<0>(axis)*get<2>(axis);
        cv_type sy = s*get<1>(axis);
        cv_type tyz = t*get<1>(axis)*get<2>(axis);
        cv_type sz = s*get<2>(axis);

        set<0, 0>(r, txx+c); set<0, 1>(r, txy-sz); set<0, 2>(r, txz+sy);
        set<1, 0>(r, txy+sz); set<1, 1>(r, tyy+c); set<1, 2>(r, tyz-sx);
        set<2, 0>(r, txz-sy); set<2, 1>(r, tyz+sx); set<2, 2>(r, tzz+c);
    }
};

template <typename V1, typename V2, typename Rotation>
struct matrix<V1, V2, Rotation, vector_tag, vector_tag, 2>
{
    static const bool cs_check =
        ::boost::is_same<typename traits::coordinate_system<V1>::type, cs::cartesian>::value &&
        ::boost::is_same<typename traits::coordinate_system<V2>::type, cs::cartesian>::value;

    BOOST_MPL_ASSERT_MSG(cs_check, NOT_IMPLEMENTED_FOR_THOSE_SYSTEMS, (V1, V2));

    typedef typename geometry::select_most_precise<
        typename traits::coordinate_type<V1>::type,
        typename traits::coordinate_type<V2>::type
    >::type cv_type;

    inline static void apply(V1 const& v1, V2 const& v2, Rotation & r)
    {
        namespace da = detail::algebra;

        // TODO - should store coordinates in more precise variables before the normalization?

        // angle
        cv_type d = da::dot<0, 0, 2>(v1, v2);
        cv_type l =
            math::sqrt(da::dot<0, 0, 2>(v1, v1) * da::dot<0, 0, 2>(v2, v2));
        cv_type c = d / l;

        // TODO return also if l == 0;

        // rotation angle == 0
        // not needed really, because in this case function still returns zero-rotation
        if ( 1 - std::numeric_limits<cv_type>::epsilon() <= c )
        {
            set<0, 0>(r, 1); set<0, 1>(r, 0);
            set<1, 0>(r, 0); set<1, 1>(r, 1);
        }
        // rotation angle = 180
        else if ( c <= std::numeric_limits<cv_type>::epsilon() - 1 )
        {
            set<0, 0>(r, -1); set<0, 1>(r, 0);
            set<1, 0>(r, 0); set<1, 1>(r, -1);
        }
        else
        {
            // sin
            cv_type s = (get<0>(v1) * get<1>(v2) - get<1>(v1) * get<0>(v2)) / l;

            set<0, 0>(r, c); set<0, 1>(r, -s);
            set<1, 0>(r, s); set<1, 1>(r, c);
        }
    }
};

}} // namespace detail::rotation

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch {

template <typename V1, typename V2, typename Rotation,
          typename Tag1 = typename tag<V1>::type,
          typename Tag2 = typename tag<V2>::type,
          typename RTag = typename tag<Rotation>::type
>
struct rotation
{
    BOOST_MPL_ASSERT_MSG(false, NOT_IMPLEMENTED_FOR_THOSE_TAGS, (Tag1, Tag2, Rotation));
};

template <typename V1, typename V2, typename Rotation>
struct rotation<V1, V2, Rotation, vector_tag, vector_tag, rotation_quaternion_tag>
{
    static const bool cs_check =
        ::boost::is_same<typename traits::coordinate_system<V1>::type, cs::cartesian>::value &&
        ::boost::is_same<typename traits::coordinate_system<V2>::type, cs::cartesian>::value;

    BOOST_MPL_ASSERT_MSG(cs_check, NOT_IMPLEMENTED_FOR_THOSE_SYSTEMS, (V1, V2));

    typedef typename geometry::select_most_precise<
        typename traits::coordinate_type<V1>::type,
        typename traits::coordinate_type<V2>::type
    >::type cv_type;

    typedef typename geometry::select_most_precise<
        cv_type,
        typename traits::coordinate_type<Rotation>::type
    >::type cr_type;

    typedef model::vector<cv_type, 3> vector_type;

    inline static void apply(V1 const& v1, V2 const& v2, Rotation & r)
    {
        namespace da = detail::algebra;

        // TODO - should store coordinates in more precise variables before the normalization?

        cv_type d = da::dot<0, 0, 3>(v1, v2); // l1 * l2 * cos
        cv_type l = math::sqrt(da::dot<0, 0, 3>(v1, v1) * da::dot<0, 0, 3>(v2, v2)); // l1 * l2
        cv_type w = l + d; // l1 * l2 * ( 1 + cos )

        // rotation angle == 0
        // not needed really, because in this case function still returns zero-rotation
        if ( 2*l-std::numeric_limits<cv_type>::epsilon() <= w )
        {
            set<0>(r, 1); set<0>(r, 0); set<0>(r, 0); set<0>(r, 0);
        }
        // rotation angle == pi
        else if ( w <= std::numeric_limits<cv_type>::epsilon() )
        {
            set<0>(r, 0);
            // find arbitrary rotation axis perpendicular to v1
            da::cross<0, 0, 1>(vector_type(1, 0, 0), v1, r);
            if ( da::dot<1, 1, 3>(r, r) < std::numeric_limits<cr_type>::epsilon() )
                da::cross<0, 0, 1>(vector_type(0, 1, 0), v1, r);

            // normalize axis
            da::normalize<1, 3>(r);
        }
        else
        {
            set<0>(r, w); // l1 * l2 * ( 1 + cos )
            // rotation axis
            da::cross<0, 0, 1>(v1, v2, r); // l1 * l2 * sin * UNITA

            // normalize quaternion
            da::normalize<0, 4>(r);
        }
    }
};

template <typename V1, typename V2, typename Rotation>
struct rotation<V1, V2, Rotation, vector_tag, vector_tag, rotation_matrix_tag>
    : detail::rotation::matrix<V1, V2, Rotation, vector_tag, vector_tag, traits::dimension<Rotation>::value>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

template <typename V1, typename V2, typename Rotation>
inline void rotation(V1 const& v1, V2 const& v2, Rotation & r)
{
    concepts::check_concepts_and_equal_dimensions<V1 const, V2 const>();
    // TODO - replace the following by check_equal_dimensions
    concepts::check_concepts_and_equal_dimensions<V1 const, Rotation>();

    dispatch::rotation<V1, V2, Rotation>::apply(v1, v2, r);
}

template <typename Rotation, typename V1, typename V2>
inline Rotation return_rotation(V1 const& v1, V2 const& v2)
{
    Rotation r;
    translation(v1, v2, r);
    return r;
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_ROTATION_HPP


#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Core>

#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

int main (int argc, char** argv)
{
    if (argc != 2) {
        std::cout << "Usage: pose_graph_gtsam sphere.g2o" << std::endl;
        exit(EXIT_FAILURE);
    }

    std::ifstream fin(argv[1]);
    if (!fin) {
        std::cout << "file " << argv[1] << " does not exist." << std::endl;
        exit(EXIT_FAILURE);
    }

    gtsam::NonlinearFactorGraph::shared_ptr factor_graph_ptr(new gtsam::NonlinearFactorGraph);  // gtsam of factor graph
    gtsam::Values::shared_ptr initial_ptr(new gtsam::Values);                                   // values of initialized

    /**
     * get vertices & edges information form the sphere.g2o files.
     */
    int cntVertex = 0, cntEdge = 0;
    std::cout << "reading from g2o file." << std::endl;

    while (!fin.eof()) {
        std::string name;
        fin >> name;

        if (name == "VERTEX_SE3:QUAT") {
            // vertex
            gtsam::Key id;
            fin >> id;

            double data[7];
            for (int i = 0; i < 7; i++) {
                fin >> data[i];
            }
            gtsam::Rot3 R = gtsam::Rot3::quaternion(data[6], data[3], data[4], data[5]);
            gtsam::Point3 t(data[0], data[1], data[2]);
            initial_ptr->insert(id, gtsam::Pose3(R, t));        // add initial values
            cntVertex ++;

        } else if (name == "EDGE_SE3:QUAT") {
            gtsam::Matrix m = gtsam::I_6x6;             // information matrix
            gtsam::Key idx1, idx2;
            fin >>  idx1 >> idx2;
            double data[7];
            for (int i = 0; i < 7; i++)
                fin >> data[i];

            gtsam::Rot3 R = gtsam::Rot3::quaternion(data[6], data[3], data[4], data[5]);
            gtsam::Point3 t(data[0], data[1], data[2]);
            for (int i = 0; i < 6; i ++ ) {
                for (int j = i; j < 6; j ++) {
                    double mij;
                    fin >> mij;
                    m(i, j) = mij;
                    m(j, i) = mij;
                }
            }

            // information matrix
            gtsam::Matrix mgsam_information = gtsam::I_6x6;
            mgsam_information.block<3, 3>(0, 0) = m.block<3, 3>(3, 3);  // cov rotation
            mgsam_information.block<3, 3>(3, 3) = m.block<3, 3>(0, 0);  // cov translation
            mgsam_information.block<3, 3>(0, 3) = m.block<3, 3>(0, 3);  // off diagonal
            mgsam_information.block<3, 3>(3, 0) = m.block<3, 3>(3, 0);  // off diagonal

            gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgsam_information);    // Gaussian noise model
            gtsam::NonlinearFactor::shared_ptr factor(new gtsam::BetweenFactor<gtsam::Pose3>(idx1, idx2, gtsam::Pose3(R, t), model)); // add a factor
            factor_graph_ptr->push_back(factor);
            cntEdge ++;
        }

        if (!fin.good()) break;
    }

    std::cout << "read total " << cntVertex << " vertices, " << " cntEdge." << std::endl;

    /* 固定第一个顶点，在gtsam中相当于添加一个先验因子 */
    gtsam::NonlinearFactorGraph graph_with_prior = *factor_graph_ptr;
    gtsam::noiseModel::Diagonal::shared_ptr prior_model = gtsam::noiseModel::Diagonal::Variances(
        (gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6).finished()
    );

    gtsam::Key first_key = 0;

//     for(const gtsam::Values::ConstKeyValuePair& key_value : *initial_ptr) {
    for (auto key_value : *initial_ptr) {
        std::cout << "Adding prior to g2o file" << std::endl;
        graph_with_prior.add(gtsam::PriorFactor<gtsam::Pose3>(
            key_value.key, key_value.value.cast<gtsam::Pose3>(), prior_model)
        );
        break;
    }

    // 开始因子图优化，配置优化选项
    std::cout << "optimizing the factor graph." << std::endl;

    // use LM optimization
    gtsam::LevenbergMarquardtParams params_lm;
    params_lm.setVerbosity("ERROR");
    params_lm.setMaxIterations(20);
    params_lm.setLinearSolverType("MULTIFRONTAL_QR");
    gtsam::LevenbergMarquardtOptimizer optimizer_LM(graph_with_prior, *initial_ptr, params_lm);

    // try use GN
//     gtsam::GaussNewtonParams params_gn;
//     params_gn.setVerbosity("ERROR");
//     params_gn.setMaxIterations(20);
//     params_gn.setLinearSolverType("MULTIFRONTAL_QR");
//     gtsam::GaussNewtonOptimizer optimizer_GN(graph_with_prior, *initial_ptr, params_gn);

    gtsam::Values result = optimizer_LM.optimize();

    std::cout << "optimization complete." << std::endl;

    std::cout << "initial error: " << factor_graph_ptr->error(*initial_ptr) << std::endl;
    std::cout << "final error: " << factor_graph_ptr->error(result) << std::endl;

    std::cout << "done.\r\nwrite to g2o ..." << std::endl;

    std::ofstream fout("result_gtsam.g2o");
    // vertex
    for (auto key_value : result) {
//     for (const gtsam::Values::ConstKeyValuePair& key_value : result) {
        gtsam::Pose3 pose = key_value.value.cast<gtsam::Pose3>();
        gtsam::Point3 t = pose.translation();
        gtsam::Quaternion q = pose.rotation().toQuaternion();
        fout << "VERTEX_SE3:QUAT " << key_value.key << " "
             << t.x() << " " << t.y() << " " << t.z() << " "
             << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl;
    }
    // edge
    for (auto factor : *factor_graph_ptr) {
//     for (gtsam::NonlinearFactor::shared_ptr factor : *factor_graph_ptr) {
        gtsam::BetweenFactor<gtsam::Pose3>::shared_ptr f = boost::dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose3>>(factor);
//         gtsam::BetweenFactor<gtsam::Pose3>::shared_ptr f = std::dynamic_pointer_cast<gtsam::BetweenFactor<gtsam::Pose3>>(factor);    // this program is error !!!!!!
        if (f) {
            gtsam::SharedNoiseModel model = f->noiseModel();
            gtsam::noiseModel::Gaussian::shared_ptr gaussianModel = boost::dynamic_pointer_cast<gtsam::noiseModel::Gaussian>(model);
            if (gaussianModel) {
                gtsam::Matrix info = gaussianModel->R().transpose() * gaussianModel->R();
                gtsam::Pose3 pose = f->measured();
                gtsam::Point3 t = pose.translation();
                gtsam::Quaternion q = pose.rotation().toQuaternion();

                fout << "EDGE_SE3:QUAT " << f->key1() << " " << f->key2() << " "
                     << t.x() << " " << t.y() << " " << t.z() << " "
                     << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << " ";

                gtsam::Matrix infoG2O = gtsam::I_6x6;
                infoG2O.block(0, 0, 3, 3) = info.block(3, 3, 3, 3);     // cov translation
                infoG2O.block(3, 3, 3, 3) = info.block(3, 3, 3, 3);     // cov rotation
                infoG2O.block(0, 3, 3, 3) = info.block(0, 3, 3, 3);     // off diagonal
                infoG2O.block(3, 0, 3, 3) = info.block(3, 0, 3, 3);     // off diagonal

                for (int i = 0; i < 6; i++) {
                    for (int j = i; j < 6; j++) {
                        fout << infoG2O(i, j) << " ";
                    }
                }

                fout << std::endl;
            }
        }
    }

    fout.close();
    std::cout << "done." << std::endl;

    return 0;
}


#include <iostream>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <mumpscpp/mumpscpp.h>
#include <Eigen/core>
#include <shiva/Environment.h>
#include <shiva/Communicator.h>
#include <mumpscpp/UblasCoordinateAdaptor.h>
#include <mumpscpp/UblasVectorAdaptor.h>
#include <mumpscpp/EigenVector.h>

shiva::environment mpi_env;

void assemble_symmetric(mumpscpp::UblasCoordinateSparseMatrix& k)
{
  // lower triangular part
  const size_t matrix_size = 3;
  k.resize(matrix_size, matrix_size, false);
  k.clear();
  k.append_element(0, 0, 2.0);
  k.append_element(1, 0, -1.0);
  k.append_element(1, 1, 2.0);
  k.append_element(2, 0, 0.0);
  k.append_element(2, 1, -1.0);
  k.append_element(2, 2, 2.0);

  k.sort();
}

int main()
{
  shiva::communicator world;
  const size_t matrix_size = 3;

  mumpscpp::UblasCoordinateSparseMatrix k;
  assemble_symmetric(k);
  mumpscpp::EigenVector f = Eigen::VectorXd::Zero(matrix_size);
  f[0] = 1.25;
  f[1] = -2.0;
  f[2] = 1.75;

  mumpscpp::Mumps<double> mumps(mumpscpp::MatrixType::symmetric, mumpscpp::HostParallelism::involved, world.fortran_mpi_communicator());
  mumps.set_output_level(mumpscpp::OutputLevel::error);
  mumps.setDistributedInput(k);

  mumps.analyzeFactorize();
  mumps.solve(f);

  std::cout << f[0] << " should be " << 0.375 << "\n";
  std::cout << f[1] << " should be " << -0.5  << "\n";
  std::cout << f[2] << " should be " << 0.625 << "\n";

  mumps.destroy();
}


#include <cinttypes>
#include <cmath>
#include <complex>
#include <iostream>

#define png_infopp_NULL nullptr
#define int_p_NULL      nullptr
#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/png_io.hpp>

namespace gil = boost::gil;

struct mandelbrot_fn {
    using const_t           = mandelbrot_fn;
    using value_type        = gil::gray8_pixel_t;
    using reference         = value_type;
    using const_reference   = value_type;
    using point_t           = gil::point2<int>;
    using result_type       = value_type;
    using argument_type     = point_t;
    static constexpr bool is_mutable = false;

    explicit mandelbrot_fn(point_t const& size) :
        m_size{ size }
    {}

    auto operator()(point_t const& p) const -> result_type {
        auto map = [](double val, double r1_from, double r1_to, double r2_from, double r2_to) -> double {
            return ((val - r1_from) / (r1_to - r1_from)) * (r2_to - r2_from) + r2_from;
        };

        // map x to [-2, 1] and y to [1.5, -1.5]
        std::complex<double> c{ map(p.x, 0, m_size.x, -2, 1), map(p.y, 0, m_size.y, 1.5, -1.5) };
        auto lc = c;
        for (auto i = 0; i < 100; ++i) {
            if (std::pow(lc.real(), 2) + std::pow(lc.imag(), 2) > 4) {
                return result_type{ static_cast<gil::bits8>(i / 100.0 * 255) };
            }
            lc = std::pow(lc, 2) + c;
        }

        return result_type{ 0 };
    }

private:
    point_t m_size;
};

int main() {
    using point_t = mandelbrot_fn::point_t;
    using locator_t = gil::virtual_2d_locator<mandelbrot_fn, false>;
    using image_view_t = gil::image_view<locator_t>;

    point_t size{ 5000, 5000 };
    image_view_t view{ size, locator_t{ point_t{ 0, 0 }, point_t{ 1, 1 }, mandelbrot_fn{ size } } };
    gil::png_write_view("mandelbrot.png", view);
}


#pragma once
#include <Eigen/Core>
#include <cassert>
#include <stdexcept>
#include <tuple>

//! The gradient of the shape function (on the reference element)
//!
//! We have three shape functions
//!
//! @param i integer between 0 and 2 (inclusive). Decides which shape function to return.
//! @param x x coordinate in the reference element.
//! @param y y coordinate in the reference element.
inline Eigen::Vector2d gradientLambda(const int i, double x, double y) {
	// (write your solution here)
	std::ignore = x;
	std::ignore = y;
	assert(0 <= i && i <= 2);
	switch (i) {
		case 0:
			return Eigen::Vector2d(-1, -1);
		case 1:
			return Eigen::Vector2d(1, 0);
		case 2:
			return Eigen::Vector2d(0, 1);
		default:
			throw std::domain_error("i not in {0,1,2}");
	}

	return Eigen::Vector2d(0, 0); //remove when implemented
}


//
// Created by riku on 1/3/18.
//
#include <NTL/ZZ.h>
#include <HElib/NumbTh.h>
#include <HElib/FHEContext.h>
#include <iostream>
void MiniONN(long m) {
    long p = NTL::RandomPrime_long(16, 20);
    while (true) {
        long d = multOrd(p, m);
        if (d == 1)
            break;
        do {
            p = NTL::NextPrime(p + 2);
        } while (m % p == 0);
    };
    std::cout << m << " " << p << std::endl;
}

bool check(NTL::ZZX const& factor) {
	for (long i = 1; i < NTL::deg(factor); i++) {
		if (NTL::coeff(factor, i) != 0)
			return false;
	}
	return true;
}

void DoublePacking(long m, long slots) {
	long p = NTL::RandomPrime_long(17, 20);
	long phim = phi_N(m);
	assert(phim == (m >> 1));
	long count = 0;
	while (count < 10) {
		long d = multOrd(p, m);
		long s = phim / d;
		if (s == slots) {
			FHEcontext context(m, p, 1);
			const auto &ftrs = context.alMod.getFactorsOverZZ();
			bool ok = true;
			for (const auto& f : ftrs)
				ok &= check(f);
			if (ok) {
				printf("%ld %ld %f\n", m, p, std::log(p) / std::log(2.));
				count += 1;
			}
		}
		do {
            p = NTL::NextPrime(p + 2);
        } while (m % p == 0);
	};
}

int main() {
    //MiniONN(8192);
	DoublePacking(16384, 64);
	//DoublePacking(8192, 64);
	// DoublePacking(8192, 256);
	// DoublePacking(8192, 512);
	// DoublePacking(8192, 1024);
	// DoublePacking(8192, 2048);
    return 0;
}


#include <k52/optimization/conjugate_gradient_method.h>

#ifdef BUILD_WITH_MPI

#include <boost/mpi.hpp>
#include <k52/parallel/mpi/constants.h>

#endif

#include <cmath>
#include <stdexcept>
#include <iostream>

#include <k52/common/floating_point.h>
#include <k52/optimization/params/i_continuous_parameters.h>

using ::std::vector;
using ::k52::common::FloatingPoint;

namespace k52
{
namespace optimization
{

ConjugateGradientMethod::ConjugateGradientMethod(
    double precision,
    double increment_of_the_argument,
    size_t number_of_iterations)
{
    precision_ = precision;
    increment_of_the_argument_ = increment_of_the_argument;
    number_of_iterations_ = number_of_iterations;
}

ConjugateGradientMethod* ConjugateGradientMethod::Clone() const
{
    return new ConjugateGradientMethod(precision_, increment_of_the_argument_, number_of_iterations_);
}

std::string ConjugateGradientMethod::get_name() const
{
    return "Conjugate Gradient Method";
}

#ifdef BUILD_WITH_MPI
void ConjugateGradientMethod::Send(boost::mpi::communicator* communicator, int target) const
{
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, precision_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, increment_of_the_argument_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, number_of_iterations_);
}

void ConjugateGradientMethod::Receive(boost::mpi::communicator* communicator, int source)
{
    communicator->recv(source,
        k52::parallel::mpi::constants::kCommonTag,
        precision_);
    communicator->recv(source,
        k52::parallel::mpi::constants::kCommonTag,
        increment_of_the_argument_);
    communicator->recv(source,
        k52::parallel::mpi::constants::kCommonTag,
        number_of_iterations_);
}
#endif

vector<double> ConjugateGradientMethod::FindOptimalParameters(
    const vector<double>& initial_parameters)
{
    vector<double> parameters = initial_parameters;

    vector<double> gradient = CalculateGradient(parameters);
    vector<double> previous_gradient(parameters.size());

    vector<double> search_direction(parameters.size());
    vector<double> previous_search_direction(parameters.size());

    double weighting_coefficient = 0;
    double exit = 0;
    size_t iteration = 1;

    //http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method
    do
    {
        previous_search_direction = search_direction;
        search_direction = FindNextSearchDirection(gradient, previous_search_direction, weighting_coefficient, iteration);

        double minimizing_parameter = PerformOneDimensionalSearch(parameters, search_direction);
        //TODO use vector math
        for (size_t i = 0; i<parameters.size(); i++)
        {
            parameters[i] += minimizing_parameter * search_direction[i];
        }

        previous_gradient = gradient;
        gradient = CalculateGradient(parameters);

        weighting_coefficient = CalculateWeightingCoefficient(gradient, previous_gradient);

        //TODO use vector math
        exit = 0;
        for (size_t i = 0; i<parameters.size(); i++)
        {
            exit += pow(gradient[i], 2);
        }
        exit = sqrt(exit);

        iteration++;
        if (iteration == number_of_iterations_)
        {
            std::cout << "Solution not found" << std::endl;
            break;
        }
    } while (exit >= precision_);

    return parameters;
}

double ConjugateGradientMethod::CountCorrectedObjectiveFunctionValue(
    const vector<double>& parameters)
{
    //Searching for minimum in the method
    return CountObjectiveFunctionValueToMinimize(parameters);
}

double ConjugateGradientMethod::CalculateDerivative(
    const vector<double>& parameters,
    size_t index)
{
    vector<double> decrement_function (parameters);
    vector<double> increment_function (parameters);
    decrement_function[index] = parameters[index] - increment_of_the_argument_/2;
    increment_function[index] = parameters[index] + increment_of_the_argument_/2;
    double increment_function_value = CountCorrectedObjectiveFunctionValue(increment_function);
    double decrement_function_value = CountCorrectedObjectiveFunctionValue(decrement_function);
    return (increment_function_value - decrement_function_value)/increment_of_the_argument_;
}

vector<double> ConjugateGradientMethod::FindNextSearchDirection(
    const vector<double>& gradient,
    const vector<double>& previous_search_direction,
    double weighting_coefficient,
    int iteration)
{
    vector<double> search_direction = gradient;

    if (iteration > 1)
    {
        //TODO use vector math
        for (size_t i=0; i<gradient.size(); i++)
        {
            search_direction[i] += weighting_coefficient * previous_search_direction[i];
        }
    }

    return search_direction;
}

vector<double> ConjugateGradientMethod::CalculateGradient(
    const vector<double>& parameters)
{
    vector<double> gradient(parameters.size());

    //TODO use vector math
    for (size_t i=0; i<parameters.size(); i++)
    {
        gradient[i] = CalculateDerivative(parameters, i);
    }

    return gradient;
}

double ConjugateGradientMethod::PerformOneDimensionalSearch(
    const vector<double>& parameters,
    const vector<double>& search_direction)
{
    double x = 0, previous_x=0;

    //Using Newton method to find one-dim minimum
    do
    {
        previous_x = x;

        //Counting derivatives
        //TODO use vector math
        vector<double> point = parameters;
        vector<double> incremented = parameters;
        vector<double> decremented = parameters;
        for(size_t i=0; i<parameters.size(); i++)
        {
            point[i] += x*search_direction[i];
            incremented[i] += (x + increment_of_the_argument_/2)*search_direction[i];
            decremented[i] += (x - increment_of_the_argument_/2)*search_direction[i];
        }

        double f = CountCorrectedObjectiveFunctionValue(point);
        double f_incremented = CountCorrectedObjectiveFunctionValue(incremented);
        double f_decremented = CountCorrectedObjectiveFunctionValue(decremented);

        double dirivative = (f_incremented - f_decremented)/increment_of_the_argument_;
        double secound_derivative = (f_incremented - 2*f + f_decremented) / (increment_of_the_argument_*increment_of_the_argument_/4);

        x = previous_x - dirivative/secound_derivative;
    } while(std::abs(previous_x - x) > precision_);
    return x;
}

double ConjugateGradientMethod::CalculateWeightingCoefficient(
    const vector<double>& gradient,
    const vector<double>& previous_gradient)
{
    //Fletcher-Reeves coefficient
    double gradient_square=0;
    double previous_gradient_square=0;

    //TODO replace pow 2
    //TODO use vector math
    for (size_t i=0; i<gradient.size(); i++)
    {
        gradient_square += pow(gradient[i], 2);
        previous_gradient_square += pow(previous_gradient[i], 2);
    }

    if(FloatingPoint::IsZero(previous_gradient_square))
    {
        throw std::runtime_error("previous_gradient_square == 0");
    }

    return gradient_square / previous_gradient_square;
}

}/* namespace optimization */
}/* namespace k52 */


#ifndef TRIUMF_MATH_PDF_HPP
#define TRIUMF_MATH_PDF_HPP

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

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

//
namespace math {

// probability density function (PDF)
namespace pdf {

/// exponentially modified Gaussian distribution
template <typename T = double>
T exponentially_modified_gaussian(T x, T mu, T sigma, T lambda) {
  return boost::math::constants::half<T>() * lambda *
         std::exp(2.0 * mu + lambda * sigma * sigma - 2.0 * x) *
         std::erfc((mu + lambda * sigma * sigma - x) /
                   (boost::math::constants::root_two<T>() * sigma));
}

/// exponentially modified Gaussian distribution (ROOT interface)
template <typename T = double>
T exponentially_modified_gaussian(const T *x, const T *par) {
  return exponentially_modified_gaussian<T>(*x, par[0], par[1], par[2]);
}

/// skew-normal distribution
template <typename T = double>
T skew_normal_distribution(T x, T mu, T sigma, T alpha) {
  boost::math::skew_normal_distribution<T> distribution(mu, sigma, alpha);
  return boost::math::pdf(distribution, x);
}

/// skew-normal distribution (ROOT interface)
template <typename T = double>
T skew_normal_distribution(const T *x, const T *par) {
  return skew_normal_distribution<T>(*x, par[0], par[1], par[2]);
}

/// modified beta distribution - x in [0, x_max]
template <typename T = double> T modified_beta(T x, T alpha, T beta, T x_max) {
  if (x <= 0.0 or x >= x_max) {
    return 0.0;
  }
  T y = x / x_max;
  boost::math::beta_distribution<T> distribution(alpha, beta);
  return boost::math::pdf(distribution, y) / x_max;
}

/// modified beta distribution - x in [0, x_max] (ROOT interface)
template <typename T = double> T modified_beta(const T *x, const T *par) {
  T alpha = par[0];
  T beta = par[1];
  T x_max = par[2];
  return modified_beta<T>(*x, alpha, beta, x_max);
}

/// two modified beta distributions.
template <typename T = double>
T two_modified_beta(T x, T alpha_1, T beta_1, T x_max_1, T fraction_1,
                    T alpha_2, T beta_2, T x_max_2) {
  return fraction_1 * modified_beta<T>(x, alpha_1, beta_1, x_max_1) +
         (1.0 - fraction_1) * modified_beta<T>(x, alpha_2, beta_2, x_max_2);
}

/// modified non-central beta distribution - x in [0, x_max]
template <typename T = double>
T modified_non_central_beta(T x, T alpha, T beta, T lambda, T x_max) {
  if (x <= 0.0 or x >= x_max) {
    return 0.0;
  }
  T y = x / x_max;
  if (lambda == 0.0) {
      return modified_beta(x, alpha, beta, x_max);
  }
  boost::math::non_central_beta_distribution<T> distribution(alpha, beta,
                                                             lambda);
  return boost::math::pdf(distribution, y) / x_max;
}

/// modified non-central beta distribution - x in [0, x_max] (ROOT interface)
template <typename T = double>
T modified_non_central_beta(const T *x, const T *par) {
  return modified_non_central_beta<T>(*x, par[0], par[1], par[2], par[3]);
}

} // namespace pdf

} // namespace math

} // namespace triumf

#endif // TRIUMF_MATH_PDF_HPP


#pragma once

#include "perceive/foundation.hpp"
#include "perceive/utils/sdbm-hash.hpp"
#include "vector-3.hpp"
#include <Eigen/Core>

namespace perceive
{
// --------------------------------------------------------------------- Vector3

#pragma pack(push, 1)
template<typename T> class Vector4T
{
 public:
   using value_type = T;

   T x, y, z, w;

   Vector4T() noexcept
       : x(T(0.0))
       , y(T(0.0))
       , z(T(0.0))
       , w(T(0.0))
   {}
   Vector4T(T x_, T y_, T z_, T w_) noexcept
       : x(x_)
       , y(y_)
       , z(z_)
       , w(w_)
   {}
   Vector4T(const float p[4]) noexcept
   {
      x = p[0];
      y = p[1];
      z = p[2];
      w = p[3];
   }
   Vector4T(const double p[4]) noexcept
   {
      x = p[0];
      y = p[1];
      z = p[2];
      w = p[3];
   }
   Vector4T(const Vector3T<T>& p, T w_ = T(0.0)) noexcept
       : x(p.x)
       , y(p.y)
       , z(p.z)
       , w(w_)
   {}
   Vector4T(const Vector3T<T>& a,
            const Vector3T<T>& b,
            const Vector3T<T>& c) noexcept
   {
      *this = plane_from_3_points(a, b, c);
   }

   Vector4T& operator=(const Vector4T& v) = default;

   Vector4T& operator=(const Eigen::Vector4d& v) noexcept
   {
      for(int i = 0; i < 4; ++i) this->operator[](i) = v(i);
      return *this;
   }
   Vector4T& operator=(const Eigen::Vector4f& v) noexcept
   {
      for(int i = 0; i < 4; ++i) this->operator[](i) = v(i);
      return *this;
   }

   static Vector4T nan() noexcept
   {
      return Vector4T(T(NAN), T(NAN), T(NAN), T(NAN));
   }

   unsigned size() const noexcept { return 4; }

   Vector4T& normalise(T epsilon = 1e-9) noexcept
   {
      // Don't normalize if we don't have to
      T mag2 = quadrance();
      if(fabs(mag2 - T(1.0)) > epsilon) {
         T mag_inv = T(1.0) / sqrt(mag2);
         x *= mag_inv;
         y *= mag_inv;
         z *= mag_inv;
         w *= mag_inv;
      }
      return *this;
   }

   Vector4T& normalise_plane(T epsilon = 1e-9) noexcept
   {
      T mag2 = x * x + y * y + z * z;
      if(fabs(mag2 - T(1.0)) > epsilon) *this *= T(1.0) / sqrt(mag2);
      return *this;
   }

   Vector4T& normalise_point(T epsilon = 1e-9) noexcept
   {
      if(fabs(w - T(1.0)) > epsilon) *this *= T(1.0) / w;
      return *this;
   }

   Vector4T normalised(T epsilon = 1e-9) const noexcept
   {
      Vector4T res = *this;
      res.normalise(epsilon);
      return res;
   }
   Vector4T normalised_plane(T epsilon = 1e-9) const noexcept
   {
      Vector4T res = *this;
      res.normalise_plane(epsilon);
      return res;
   }
   Vector4T normalised_point(T epsilon = 1e-9) const noexcept
   {
      Vector4T res = *this;
      res.normalise_point(epsilon);
      return res;
   }

   Vector4T& normalize(T ep = 1e-9) noexcept { return normalise(ep); }
   Vector4T& normalize_plane(T ep = 1e-9) noexcept
   {
      return normalise_plane(ep);
   }
   Vector4T& normalize_point(T ep = 1e-9) noexcept
   {
      return normalise_point(ep);
   }

   Vector4T normalized(T epsilon = 1e-9) const noexcept
   {
      return normalised(epsilon);
   }
   Vector4T normalized_plane(T ep = 1e-9) const noexcept
   {
      return normalised_plane(ep);
   }
   Vector4T normalized_point(T ep = 1e-9) const noexcept
   {
      return normalised_point(ep);
   }

   T quadrance() const noexcept { return x * x + y * y + z * z + w * w; }
   T norm() const noexcept { return sqrt(quadrance()); }
   T dot(const Vector4T& rhs) const noexcept
   {
      return x * rhs.x + y * rhs.y + z * rhs.z + w * rhs.w;
   }
   T distance(const Vector4T& rhs) const noexcept
   {
      return (*this - rhs).norm();
   }

   Vector3T<T>& xyz() noexcept
   {
#ifdef DEBUG_BUILD
      assert(&x == reinterpret_cast<const T*>(this) + 0);
      assert(&y == reinterpret_cast<const T*>(this) + 1);
      assert(&z == reinterpret_cast<const T*>(this) + 2);
#endif
      return *(reinterpret_cast<Vector3T<T>*>(this));
   }
   const Vector3T<T>& xyz() const noexcept
   {
#ifdef DEBUG_BUILD
      assert(&x == reinterpret_cast<const T*>(this) + 0);
      assert(&y == reinterpret_cast<const T*>(this) + 1);
      assert(&z == reinterpret_cast<const T*>(this) + 2);
#endif
      return *(reinterpret_cast<const Vector3T<T>*>(this));
   }
   T& d() noexcept { return w; } // for plane
   const T& d() const noexcept { return w; }

   Vector4T& set_to(const T& a, const T& b, const T& c, const T& d) noexcept
   {
      x = a;
      y = b;
      z = c;
      w = d;
      return *this;
   }
   Vector4T& set_to(T a[4]) noexcept
   {
      set_to(a[0], a[1], a[2], a[3]);
      return *this;
   }

   T* copy_to(T a[4]) const noexcept
   {
      a[0] = x;
      a[1] = y;
      a[2] = z;
      a[3] = w;
      return a;
   }

   T* ptr() noexcept
   {
#ifdef DEBUG_BUILD
      assert(&x == reinterpret_cast<const T*>(this) + 0);
      assert(&y == reinterpret_cast<const T*>(this) + 1);
      assert(&z == reinterpret_cast<const T*>(this) + 2);
      assert(&w == reinterpret_cast<const T*>(this) + 3);
#endif
      return &x;
   }

   const T* ptr() const noexcept
   {
      return const_cast<Vector4T<T>*>(this)->ptr();
   }

   T& operator[](int idx) noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   const T& operator[](int idx) const noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   T& operator()(int idx) noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }
   const T& operator()(int idx) const noexcept
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 4);
#endif
      return ptr()[idx];
   }

   Vector4T round() const noexcept
   {
      return Vector4T(floor(x + T(0.499)),
                      floor(y + T(0.499)),
                      floor(z + T(0.499)),
                      floor(w + T(0.499)));
   }

   Vector4T& operator*=(T scalar) noexcept
   {
      x *= scalar;
      y *= scalar;
      z *= scalar;
      w *= scalar;
      return *this;
   }
   Vector4T& operator/=(T scalar) noexcept
   {
      x /= scalar;
      y /= scalar;
      z /= scalar;
      w /= scalar;
      return *this;
   }
   Vector4T operator*(T scalar) const noexcept
   {
      Vector4T res(*this);
      res *= scalar;
      return res;
   }
   Vector4T operator/(T scalar) const noexcept
   {
      Vector4T res(*this);
      res /= scalar;
      return res;
   }

   Vector4T& operator+=(const Vector4T& rhs) noexcept
   {
      x += rhs.x;
      y += rhs.y;
      z += rhs.z;
      w += rhs.w;
      return *this;
   }
   Vector4T& operator-=(const Vector4T& rhs) noexcept
   {
      x -= rhs.x;
      y -= rhs.y;
      z -= rhs.z;
      w -= rhs.w;
      return *this;
   }
   Vector4T operator+(const Vector4T& rhs) const noexcept
   {
      Vector4T res(*this);
      res += rhs;
      return res;
   }
   Vector4T operator-(const Vector4T& rhs) const noexcept
   {
      Vector4T res(*this);
      res -= rhs;
      return res;
   }
   Vector4T operator-() const noexcept { return Vector4T(-x, -y, -z, -w); }

   bool operator==(const Vector4T& rhs) const noexcept
   {
      return x == rhs.x and y == rhs.y and z == rhs.z and w == rhs.w;
   }
   bool operator!=(const Vector4T& rhs) const noexcept
   {
      return !(*this == rhs);
   }

   std::string to_string(const char* fmt = "{{{}, {}, {}, {}}}") const noexcept
   {
      return format(fmt, x, y, z, w);
   }

   std::string to_str() const { return format("[{}, {}, {}, {}]", x, y, z, w); }

   bool is_nan() const noexcept
   {
      return std::isnan(x) || std::isnan(y) || std::isnan(z) || std::isnan(w);
   }

   bool is_finite() const noexcept
   {
      return std::isfinite(x) && std::isfinite(y) && std::isfinite(z)
             && std::isfinite(w);
   }

   inline friend bool isfinite(const Vector4T& o) noexcept
   {
      return o.is_finite();
   }

   bool is_unit_vector(T epsilon = 1e-9) const noexcept
   {
      return is_finite() && fabs(quadrance() - 1.0) < epsilon;
   }

   void print(const char* msg = NULL, bool newline = true) const noexcept
   {
      printf("%s%s%s%s",
             (msg == NULL ? "" : msg),
             (msg == NULL ? "" : " "),
             to_string().c_str(),
             (newline ? "\n" : ""));
      fflush(stdout);
   }

   // Homogenous point
   bool pt_at_infinity(T epsilon = 1e-9) const noexcept
   {
      return fabs(w) < epsilon;
   }

   size_t hash() const noexcept { return sdbm_hash(ptr(), sizeof(T) * size()); }

   // Plane functions
   T side(const Vector3T<T>& o) const noexcept
   {
      return o.x * x + o.y * y + o.z * z + d();
   }

   // WARNING, must be normalised
   double point_plane_distance(const Vector3T<T>& point) const noexcept
   {
      assert(fabs(xyz().quadrance() - 1.0) < 1e-9);
      return fabs(side(point));
   }

   Vector3T<T> image(const Vector3T<T>& p) const noexcept
   {
      assert(fabs(xyz().quadrance() - 1.0) < 1e-9);
      return p - xyz() * side(p);
   }
   Vector3T<T> reflect(const Vector3T<T>& p) const noexcept
   {
      assert(fabs(xyz().quadrance() - 1.0) < 1e-9);
      return p - 2.0 * xyz() * side(p);
   }

   // Reflect a plane
   Vector4T<T> reflect(const Vector4T<T>& p) const noexcept
   {
      auto norm = (p.xyz() - 2.0 * p.xyz().dot(xyz()) * xyz()).normalised();
      auto C    = reflect(p.image(Vector3T<T>(0.0, 0.0, 0.0)));
      return Vector4T<T>(norm, -C.dot(norm));
   }

   friend std::string str(const Vector4T<T>& o) noexcept
   {
      return o.to_string();
   }
};
#pragma pack(pop)

template<typename T>
Vector4T<T> operator*(float a, const Vector4T<T>& v) noexcept
{
   return v * a;
}

template<typename T>
Vector4T<T> operator/(float a, const Vector4T<T>& v) noexcept
{
   return v / a;
}

template<typename T>
Vector4T<T> operator*(double a, const Vector4T<T>& v) noexcept
{
   return v * a;
}

template<typename T>
Vector4T<T> operator/(double a, const Vector4T<T>& v) noexcept
{
   return v / a;
}

template<typename T>
Vector4T<T> plane_from_3_points(const Vector3T<T>& a,
                                const Vector3T<T>& b,
                                const Vector3T<T>& c) noexcept
{
   Vector4T<T> ret;

   auto ab   = a - b;
   auto ac   = a - c;
   ret.xyz() = ab.cross(ac);
   ret.xyz().normalise();
   ret.d() = -1.0 * dot(a, ret.xyz());

   return ret;
}

template<typename T>
inline T ray_position_relative_to_plane_t(const Vector4T<T>& p3,
                                          const Vector3T<T>& a,
                                          const Vector3T<T>& b,
                                          const T side) noexcept
{
   return (side - p3.d() - dot(p3.xyz(), a)) / dot(p3.xyz(), b - a);
}

template<typename T>
inline Vector3T<T> ray_position_relative_to_plane(const Vector4T<T>& p3,
                                                  const Vector3T<T>& a,
                                                  const Vector3T<T>& b,
                                                  const T side) noexcept
{
   const auto t = ray_position_relative_to_plane_t(p3, a, b, side);
   return a + t * (b - a);
}

template<typename T>
inline T plane_ray_intersection_t(const Vector4T<T>& p,
                                  const Vector3T<T>& a,
                                  const Vector3T<T>& b) noexcept
{
   static_assert(std::is_floating_point<T>::value);
   return ray_position_relative_to_plane_t(p, a, b, T(0.0));
}

template<typename T>
inline Vector3T<T> plane_ray_intersection(const Vector4T<T>& p,
                                          const Vector3T<T>& a,
                                          const Vector3T<T>& b) noexcept
{
   static_assert(std::is_floating_point<T>::value);
   return a + plane_ray_intersection_t(p, a, b) * (b - a);
}

// String shim
template<typename T> std::string str(const Vector4T<T>& v) noexcept
{
   return v.to_string();
}
template<typename T>
std::ostream& operator<<(std::ostream& out, const Vector4T<T>& v) noexcept
{
   out << v.to_string();
   return out;
}

} // namespace perceive


#ifndef CANNON_CONTROL_LQR_H
#define CANNON_CONTROL_LQR_H

/*!
 * \file cannon/control/lqr.hpp
 * \brief File containing utility functions for computing LQR controllers in
 * discrete and continuous time.
 */

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace control {

    /*!
     * \brief Class representing an LQR controller for a continuous or
     * discrete-time system.
     */
    class LQRController {
      public:

        using LinearizationFunction =
            std::function<void(const Ref<const VectorXd> &, Ref<MatrixXd>,
                               Ref<MatrixXd>)>;

        LQRController() = delete;

        /*!
         * \brief Constructor taking state to stabilize around, control
         * dimension, whether the system to be controlled is continuous-time,
         * and linearization function for the system to be controlled.
         *
         * Initializes cost matrices to identity.
         */
        LQRController(const Ref<const VectorXd> &q0,
                      unsigned int control_dim,
                      LinearizationFunction f,
                      bool continuous = true);

        /*!
         * \brief Constructor taking state to stabilize around, state
         * cost matrix, control cost matrix, whether the system to be
         * controlled is continuous-time, and linearization function for the
         * system to be controlled.
         */
        LQRController(const Ref<const VectorXd> &q0,
                      const Ref<const MatrixXd> &Q,
                      const Ref<const MatrixXd> &R, LinearizationFunction f,
                      bool continuous = true);

        /*!
         * \brief Compute the control to apply at the input state.
         *
         * \param q The state to compute control for.
         *
         * \returns The computed control.
         */
        VectorXd compute_control(const Ref<const VectorXd>& q) const;

        /*!
         * \brief Get the linear portion of the control law represented by this
         * controller.
         *
         * \returns Gain matrix.
         */
        MatrixXd get_linear_gain() const;

        /*!
         * \brief Get the constant offset portion of the control law
         * represented by this controller.
         *
         * \returns Offset vector.
         */
        VectorXd get_control_offset() const;

        /*!
         * \brief Set the state that this controller attempts to stabilize to.
         *
         * \param q The new state to stabilize to.
         */
        void set_target(const Ref<const VectorXd>& q);

      private:

        /*!
         * \brief Compute LQR gain for controller represented by this object.
         */
        void compute_lqr_gain_();

        VectorXd q0_; //!< State to stabilize toward
        MatrixXd Q_; //!< State cost matrix
        MatrixXd K_; //!< Control gain matrix
        MatrixXd R_; //!< Control cost matrix
        LLT<MatrixXd> R_cholesky_; //!< Cholesky factorization of control cost matrix
        LinearizationFunction linearization_; //!< Linearization function for system to be controlled
        bool continuous_; //!< Whether the system to be controlled is continuous-time
    };

    // Free Functions
    /*!
     * \brief Solve the continuous-time algebraic riccati equation for
     * the input state and control derivative matrices A and B, and
     * return the resulting steady-state LQR gain.
     *
     * \param A Partial derivatives of ode with respect to state.
     * \param B Partial derivatives of ode with respect to control.
     * \param Q State cost matrix
     * \param R Control cost matrix Cholesky decomposition
     *
     * \returns CARE solution control gain matrix.
     */
  MatrixXd continuous_algebraic_riccati_equation(const Ref<const MatrixXd> &A,
                                                 const Ref<const MatrixXd> &B,
                                                 const Ref<const MatrixXd> &Q,
                                                 const LLT<MatrixXd> &R_cholesky);

  } // namespace control
} // namespace cannon

#endif /* ifndef CANNON_CONTROL_LQR_H */


//
//  Copyright Markus Rickert 2008
//
// 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)
//

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/bindings/blas/blas.hpp>
#include <boost/numeric/bindings/traits/c_array.hpp>
#include <boost/numeric/bindings/traits/c_array2.hpp>
#include <boost/numeric/bindings/traits/dense_traits.hpp>
#include <boost/numeric/bindings/traits/std_valarray.hpp>
#include <boost/numeric/bindings/traits/std_valarray2.hpp>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#include <boost/numeric/bindings/traits/std_vector2.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_vector2.hpp>

int
main(int argc, char** argv)
{
	{
		// a * b' = C ; a' * b = d

		boost::numeric::ublas::vector<double> a(3);
		for (std::size_t i = 0; i < a.size(); ++i) a(i) = i;
		std::cout << "a=" << a << std::endl;

		boost::numeric::ublas::vector<double> b(3);
		for (std::size_t i = 0; i < b.size(); ++i) b(i) = i;
		std::cout << "b=" << b << std::endl;

		boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> c(3, 3);
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			boost::numeric::bindings::traits::TRANSPOSE,
			1.0, a, b, 0.0, c
		);
		std::cout << "c=" << c << std::endl;

		boost::numeric::ublas::vector<double> d(1);
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::TRANSPOSE,
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			1.0, a, b, 0.0, d
		);
		std::cout << "d=" << d << std::endl;
	}

	std::cout << std::endl;

	{
		// a * b' = C ; a' * b = d

		std::vector<double> a(3);
		for (std::size_t i = 0; i < a.size(); ++i) a[i] = i;
		std::cout << "a=[" << a.size() << "](";
		for (std::size_t i = 0; i < a.size(); ++i) std::cout << (i > 0 ? "," : "") << a[i];
		std::cout << ")" << std::endl;

		std::valarray<double> b(3);
		for (std::size_t i = 0; i < b.size(); ++i) b[i] = i;
		std::cout << "b=[" << b.size() << "](";
		for (std::size_t i = 0; i < b.size(); ++i) std::cout << (i > 0 ? "," : "") << b[i];
		std::cout << ")" << std::endl;

		boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> c(3, 3);
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			boost::numeric::bindings::traits::TRANSPOSE,
			1.0, a, b, 0.0, c
		);
		std::cout << "c=" << c << std::endl;

		std::vector<double> d(1);
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::TRANSPOSE,
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			1.0, a, b, 0.0, d
		);
		std::cout << "d=[" << d.size() << "](";
		for (std::size_t i = 0; i < d.size(); ++i) std::cout << (i > 0 ? "," : "") << d[i];
		std::cout << ")" << std::endl;
	}

	std::cout << std::endl;

	{
		// a * b' = C ; a' * b = d

		double a[3];
		for (std::size_t i = 0; i < 3; ++i) a[i] = i;
		std::cout << "a=[" << 3 << "](";
		for (std::size_t i = 0; i < 3; ++i) std::cout << (i > 0 ? "," : "") << a[i];
		std::cout << ")" << std::endl;

		std::vector<double> b(3);
		for (std::size_t i = 0; i < b.size(); ++i) b[i] = i;
		std::cout << "b=[" << b.size() << "](";
		for (std::size_t i = 0; i < b.size(); ++i) std::cout << (i > 0 ? "," : "") << b[i];
		std::cout << ")" << std::endl;

		boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> c(3, 3);
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			boost::numeric::bindings::traits::TRANSPOSE,
			1.0, a, b, 0.0, c
		);
		std::cout << "c=" << c << std::endl;

		std::valarray<double> d(1);
		boost::numeric::bindings::blas::gemm(
			boost::numeric::bindings::traits::TRANSPOSE,
			boost::numeric::bindings::traits::NO_TRANSPOSE,
			1.0, a, b, 0.0, d
		);
		std::cout << "d=[" << d.size() << "](";
		for (std::size_t i = 0; i < d.size(); ++i) std::cout << (i > 0 ? "," : "") << d[i];
		std::cout << ")" << std::endl;
	}

	return 0;
}


/*
 * COPYRIGHT AND PERMISSION NOTICE
 * Penn Software MSCKF_VIO
 * Copyright (C) 2017 The Trustees of the University of Pennsylvania
 * All rights reserved.
 */

// The original file belongs to MSCKF_VIO (https://github.com/KumarRobotics/msckf_vio/)
// Some changes have been made to use it in livox_slam_ware

#ifndef MATH_UTILS_HPP
#define MATH_UTILS_HPP

#include <cmath>
#include <Eigen/Dense>

namespace livox_slam_ware {

/*
 *  @brief Create a skew-symmetric matrix from a 3-element vector.
 *  @note Performs the operation:
 *  w   ->  [  0 -w3  w2]
 *          [ w3   0 -w1]
 *          [-w2  w1   0]
 */
inline Eigen::Matrix3d skewSymmetric(const Eigen::Vector3d& w) {
  Eigen::Matrix3d w_hat;
  w_hat(0, 0) = 0;
  w_hat(0, 1) = -w(2);
  w_hat(0, 2) = w(1);
  w_hat(1, 0) = w(2);
  w_hat(1, 1) = 0;
  w_hat(1, 2) = -w(0);
  w_hat(2, 0) = -w(1);
  w_hat(2, 1) = w(0);
  w_hat(2, 2) = 0;
  return w_hat;
}

/*
 * @brief Normalize the given quaternion to unit quaternion.
 */
inline void quaternionNormalize(Eigen::Vector4d& q) {
  double norm = q.norm();
  q = q / norm;
  return;
}

/*
 * @brief Perform q1 * q2.
 *
 *    Format of q1 and q2 is as [x,y,z,w]
 */
inline Eigen::Vector4d quaternionMultiplication(
    const Eigen::Vector4d& q1,
    const Eigen::Vector4d& q2) {
  Eigen::Matrix4d L;

  // QXC: Hamilton
  L(0, 0) =  q1(3); L(0, 1) = -q1(2); L(0, 2) =  q1(1); L(0, 3) =  q1(0);
  L(1, 0) =  q1(2); L(1, 1) =  q1(3); L(1, 2) = -q1(0); L(1, 3) =  q1(1);
  L(2, 0) = -q1(1); L(2, 1) =  q1(0); L(2, 2) =  q1(3); L(2, 3) =  q1(2);
  L(3, 0) = -q1(0); L(3, 1) = -q1(1); L(3, 2) = -q1(2); L(3, 3) =  q1(3);

  Eigen::Vector4d q = L * q2;
  quaternionNormalize(q);
  return q;
}

/*
 * @brief Convert the vector part of a quaternion to a
 *    full quaternion.
 * @note This function is useful to convert delta quaternion
 *    which is usually a 3x1 vector to a full quaternion.
 *    For more details, check Section 3.2 "Kalman Filter Update" in
 *    "Indirect Kalman Filter for 3D Attitude Estimation:
 *    A Tutorial for quaternion Algebra".
 */
inline Eigen::Vector4d smallAngleQuaternion(
    const Eigen::Vector3d& dtheta) {

  Eigen::Vector3d dq = dtheta / 2.0;
  Eigen::Vector4d q;
  double dq_square_norm = dq.squaredNorm();

  if (dq_square_norm <= 1) {
    q.head<3>() = dq;
    q(3) = std::sqrt(1-dq_square_norm);
  } else {
    q.head<3>() = dq;
    q(3) = 1;
    q = q / std::sqrt(1+dq_square_norm);
  }

  return q;
}

/*
 * @brief Convert the vector part of a quaternion to a
 *    full quaternion.
 * @note This function is useful to convert delta quaternion
 *    which is usually a 3x1 vector to a full quaternion.
 *    For more details, check Section 3.2 "Kalman Filter Update" in
 *    "Indirect Kalman Filter for 3D Attitude Estimation:
 *    A Tutorial for quaternion Algebra".
 */
inline Eigen::Quaterniond getSmallAngleQuaternion(
    const Eigen::Vector3d& dtheta) {

  Eigen::Vector3d dq = dtheta / 2.0;
  Eigen::Quaterniond q;
  double dq_square_norm = dq.squaredNorm();

  if (dq_square_norm <= 1) {
    q.x() = dq(0);
    q.y() = dq(1);
    q.z() = dq(2);
    q.w() = std::sqrt(1-dq_square_norm);
  } else {
    q.x() = dq(0);
    q.y() = dq(1);
    q.z() = dq(2);
    q.w() = 1;
    q.normalize();
  }

  return q;
}

/*
 * @brief Convert a quaternion to the corresponding rotation matrix
 * @note Pay attention to the convention used. The function follows the
 *    conversion in "Indirect Kalman Filter for 3D Attitude Estimation:
 *    A Tutorial for Quaternion Algebra", Equation (78).
 *
 *    The input quaternion should be in the form
 *      [q1, q2, q3, q4(scalar)]^T
 */
inline Eigen::Matrix3d quaternionToRotation(
    const Eigen::Vector4d& q) {
  // QXC: Hamilton
  const double& qw = q(3);
  const double& qx = q(0);
  const double& qy = q(1);
  const double& qz = q(2);
  Eigen::Matrix3d R;
  R(0, 0) = 1-2*(qy*qy+qz*qz);  R(0, 1) =   2*(qx*qy-qw*qz);  R(0, 2) =   2*(qx*qz+qw*qy);
  R(1, 0) =   2*(qx*qy+qw*qz);  R(1, 1) = 1-2*(qx*qx+qz*qz);  R(1, 2) =   2*(qy*qz-qw*qx);
  R(2, 0) =   2*(qx*qz-qw*qy);  R(2, 1) =   2*(qy*qz+qw*qx);  R(2, 2) = 1-2*(qx*qx+qy*qy);

  return R;
}

/*
 * @brief Convert a rotation matrix to a quaternion.
 * @note Pay attention to the convention used. The function follows the
 *    conversion in "Indirect Kalman Filter for 3D Attitude Estimation:
 *    A Tutorial for Quaternion Algebra", Equation (78).
 *
 *    The input quaternion should be in the form
 *      [q1, q2, q3, q4(scalar)]^T
 */
inline Eigen::Vector4d rotationToQuaternion(
    const Eigen::Matrix3d& R) {
  Eigen::Vector4d score;
  score(0) = R(0, 0);
  score(1) = R(1, 1);
  score(2) = R(2, 2);
  score(3) = R.trace();

  int max_row = 0, max_col = 0;
  score.maxCoeff(&max_row, &max_col);

  Eigen::Vector4d q = Eigen::Vector4d::Zero();

  // QXC: Hamilton
  if (max_row == 0) {
    q(0) = std::sqrt(1+2*R(0, 0)-R.trace()) / 2.0;
    q(1) = (R(0, 1)+R(1, 0)) / (4*q(0));
    q(2) = (R(0, 2)+R(2, 0)) / (4*q(0));
    q(3) = (R(2, 1)-R(1, 2)) / (4*q(0));
  } else if (max_row == 1) {
    q(1) = std::sqrt(1+2*R(1, 1)-R.trace()) / 2.0;
    q(0) = (R(0, 1)+R(1, 0)) / (4*q(1));
    q(2) = (R(1, 2)+R(2, 1)) / (4*q(1));
    q(3) = (R(0, 2)-R(2, 0)) / (4*q(1));
  } else if (max_row == 2) {
    q(2) = std::sqrt(1+2*R(2, 2)-R.trace()) / 2.0;
    q(0) = (R(0, 2)+R(2, 0)) / (4*q(2));
    q(1) = (R(1, 2)+R(2, 1)) / (4*q(2));
    q(3) = (R(1, 0)-R(0, 1)) / (4*q(2));
  } else {
    q(3) = std::sqrt(1+R.trace()) / 2.0;
    q(0) = (R(2, 1)-R(1, 2)) / (4*q(3));
    q(1) = (R(0, 2)-R(2, 0)) / (4*q(3));
    q(2) = (R(1, 0)-R(0, 1)) / (4*q(3));
  }

  if (q(3) < 0) q = -q;
  quaternionNormalize(q);
  return q;
}

} // end namespace livox_slam_ware

#endif // MATH_UTILS_HPP


#include "SimpleIndex.h"
#include "cryptoTools/Crypto/PRNG.h"
#include <random>
#include "cryptoTools/Common/Log.h"
#include "cryptoTools/Common/CuckooIndex.h"
#include <numeric>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

namespace osuCrypto
{

    void SimpleIndex::print()
    {

        for (u64 i = 0; i < mBins.size(); ++i)
            //	for (u64 i = 0; i <1; ++i)
        {
            std::cout << "Bin #" << i << std::endl;

            std::cout << " contains " << mBinSizes[i] << " elements" << std::endl;

            for (u64 j = 0; j < mBinSizes[i]; ++j)
            {
                std::cout << "    idx=" << mBins(i, j).idx() << "  hIdx=" << mBins(i, j).hashIdx() << std::endl;
                //	std::cout << "    " << mBins[i].first[j] << "  " << mBins[i].second[j] << std::endl;

            }

            std::cout << std::endl;
        }

        std::cout << std::endl;
    }

    //template<unsigned int N = 16>
    double getBinOverflowProb(u64 numBins, u64 numBalls, u64 getBinSize, double epsilon = 0.0001)
    {
        if (numBalls <= getBinSize)
            return std::numeric_limits<double>::max();

        if (numBalls > std::numeric_limits<i32>::max())
        {
            auto msg = ("boost::math::binomial_coefficient(...) only supports " + std::to_string(sizeof(unsigned) * 8) + " bit inputs which was exceeded." LOCATION);
            std::cout << msg << std::endl;
            throw std::runtime_error(msg);
        }

        //std::cout << numBalls << " " << numBins << " " << binSize << std::endl;
        typedef boost::multiprecision::number<boost::multiprecision::backends::cpp_bin_float<16>> T;
        T sum = 0.0;
        T sec = 0.0;// minSec + 1;
        T diff = 1;
        u64 i = getBinSize + 1;

        while (diff > T(epsilon) && numBalls >= i /*&& sec > minSec*/)
        {
            sum += numBins * boost::math::binomial_coefficient<T>(i32(numBalls), i32(i))
                * boost::multiprecision::pow(T(1.0) / numBins, i) * boost::multiprecision::pow(1 - T(1.0) / numBins, numBalls - i);

            //std::cout << "sum[" << i << "] " << sum << std::endl;

            T sec2 = boost::multiprecision::log2(sum);
            diff = boost::multiprecision::abs(sec - sec2);
            //std::cout << diff << std::endl;
            sec = sec2;

            i++;
        }

        return std::max<double>(0, (double)-sec);
    }

    u64 SimpleIndex::get_bin_size(u64 numBins, u64 numBalls, u64 statSecParam)
    {

        auto B = std::max<u64>(1, numBalls / numBins);

        double currentProb = getBinOverflowProb(numBins, numBalls, B);
        u64 step = 1;

        bool doubling = true;

        while (currentProb < statSecParam || step > 1)
        {
            if (!step)
                throw std::runtime_error(LOCATION);

            if (statSecParam > currentProb)
            {
                if (doubling) step = std::max<u64>(1, step * 2);
                else          step = std::max<u64>(1, step / 2);

                B += step;
            }
            else
            {
                doubling = false;
                step = std::max<u64>(1, step / 2);
                B -= step;
            }
            currentProb = getBinOverflowProb(numBins, numBalls, B);
        }

        return B;
    }

    void SimpleIndex::init(u64 numBins, u64 numBalls, u64 statSecParam, u64 numHashFunction)
    {
        mNumHashFunctions = numHashFunction;
        mMaxBinSize = get_bin_size(numBins, numBalls * numHashFunction, statSecParam);
        mBins.resize(numBins, mMaxBinSize);
        mBinSizes.resize(numBins, 0);
        mItemToBinMap.resize(numBalls, numHashFunction);
        mNumBins = numBins;
    }

    void SimpleIndex::insertItems(span<block> items, block hashingSeed)
    {

        std::array<block, 8> hashs;
        AES hasher(hashingSeed);

        auto mainSteps = items.size() / hashs.size();
        auto remSteps = items.size() % hashs.size();
        u64 itemIdx = 0;
        if (mNumHashFunctions == 3 )
        {
            for (u64 i = 0; i < mainSteps; ++i, itemIdx += 8)
            {
                auto min = std::min<u64>(items.size() - itemIdx, hashs.size());

                hasher.ecbEncBlocks(items.data() + itemIdx, min, hashs.data());

                auto itemIdx0 = itemIdx + 0;
                auto itemIdx1 = itemIdx + 1;
                auto itemIdx2 = itemIdx + 2;
                auto itemIdx3 = itemIdx + 3;
                auto itemIdx4 = itemIdx + 4;
                auto itemIdx5 = itemIdx + 5;
                auto itemIdx6 = itemIdx + 6;
                auto itemIdx7 = itemIdx + 7;

                hashs[0] = hashs[0] ^ items[itemIdx0];
                hashs[1] = hashs[1] ^ items[itemIdx1];
                hashs[2] = hashs[2] ^ items[itemIdx2];
                hashs[3] = hashs[3] ^ items[itemIdx3];
                hashs[4] = hashs[4] ^ items[itemIdx4];
                hashs[5] = hashs[5] ^ items[itemIdx5];
                hashs[6] = hashs[6] ^ items[itemIdx6];
                hashs[7] = hashs[7] ^ items[itemIdx7];

                auto bIdx00 = CuckooIndex<>::getHash(hashs[0], 0, mNumBins);
                auto bIdx10 = CuckooIndex<>::getHash(hashs[1], 0, mNumBins);
                auto bIdx20 = CuckooIndex<>::getHash(hashs[2], 0, mNumBins);
                auto bIdx30 = CuckooIndex<>::getHash(hashs[3], 0, mNumBins);
                auto bIdx40 = CuckooIndex<>::getHash(hashs[4], 0, mNumBins);
                auto bIdx50 = CuckooIndex<>::getHash(hashs[5], 0, mNumBins);
                auto bIdx60 = CuckooIndex<>::getHash(hashs[6], 0, mNumBins);
                auto bIdx70 = CuckooIndex<>::getHash(hashs[7], 0, mNumBins);

                mBins(bIdx00, mBinSizes[bIdx00]++).set(itemIdx0, 0, false);
                mBins(bIdx10, mBinSizes[bIdx10]++).set(itemIdx1, 0, false);
                mBins(bIdx20, mBinSizes[bIdx20]++).set(itemIdx2, 0, false);
                mBins(bIdx30, mBinSizes[bIdx30]++).set(itemIdx3, 0, false);
                mBins(bIdx40, mBinSizes[bIdx40]++).set(itemIdx4, 0, false);
                mBins(bIdx50, mBinSizes[bIdx50]++).set(itemIdx5, 0, false);
                mBins(bIdx60, mBinSizes[bIdx60]++).set(itemIdx6, 0, false);
                mBins(bIdx70, mBinSizes[bIdx70]++).set(itemIdx7, 0, false);

                mItemToBinMap(itemIdx0, 0) = bIdx00;
                mItemToBinMap(itemIdx1, 0) = bIdx10;
                mItemToBinMap(itemIdx2, 0) = bIdx20;
                mItemToBinMap(itemIdx3, 0) = bIdx30;
                mItemToBinMap(itemIdx4, 0) = bIdx40;
                mItemToBinMap(itemIdx5, 0) = bIdx50;
                mItemToBinMap(itemIdx6, 0) = bIdx60;
                mItemToBinMap(itemIdx7, 0) = bIdx70;

                auto bIdx01 = CuckooIndex<>::getHash(hashs[0], 1, mNumBins);
                auto bIdx11 = CuckooIndex<>::getHash(hashs[1], 1, mNumBins);
                auto bIdx21 = CuckooIndex<>::getHash(hashs[2], 1, mNumBins);
                auto bIdx31 = CuckooIndex<>::getHash(hashs[3], 1, mNumBins);
                auto bIdx41 = CuckooIndex<>::getHash(hashs[4], 1, mNumBins);
                auto bIdx51 = CuckooIndex<>::getHash(hashs[5], 1, mNumBins);
                auto bIdx61 = CuckooIndex<>::getHash(hashs[6], 1, mNumBins);
                auto bIdx71 = CuckooIndex<>::getHash(hashs[7], 1, mNumBins);

                bool c01 = bIdx00 == bIdx01;
                bool c11 = bIdx10 == bIdx11;
                bool c21 = bIdx20 == bIdx21;
                bool c31 = bIdx30 == bIdx31;
                bool c41 = bIdx40 == bIdx41;
                bool c51 = bIdx50 == bIdx51;
                bool c61 = bIdx60 == bIdx61;
                bool c71 = bIdx70 == bIdx71;

                mBins(bIdx01, mBinSizes[bIdx01]++).set(itemIdx0, 1, c01);
                mBins(bIdx11, mBinSizes[bIdx11]++).set(itemIdx1, 1, c11);
                mBins(bIdx21, mBinSizes[bIdx21]++).set(itemIdx2, 1, c21);
                mBins(bIdx31, mBinSizes[bIdx31]++).set(itemIdx3, 1, c31);
                mBins(bIdx41, mBinSizes[bIdx41]++).set(itemIdx4, 1, c41);
                mBins(bIdx51, mBinSizes[bIdx51]++).set(itemIdx5, 1, c51);
                mBins(bIdx61, mBinSizes[bIdx61]++).set(itemIdx6, 1, c61);
                mBins(bIdx71, mBinSizes[bIdx71]++).set(itemIdx7, 1, c71);

                mItemToBinMap(itemIdx0, 1) = bIdx01 | ((u8)c01 & 1) * u64(-1);
                mItemToBinMap(itemIdx1, 1) = bIdx11 | ((u8)c11 & 1) * u64(-1);
                mItemToBinMap(itemIdx2, 1) = bIdx21 | ((u8)c21 & 1) * u64(-1);
                mItemToBinMap(itemIdx3, 1) = bIdx31 | ((u8)c31 & 1) * u64(-1);
                mItemToBinMap(itemIdx4, 1) = bIdx41 | ((u8)c41 & 1) * u64(-1);
                mItemToBinMap(itemIdx5, 1) = bIdx51 | ((u8)c51 & 1) * u64(-1);
                mItemToBinMap(itemIdx6, 1) = bIdx61 | ((u8)c61 & 1) * u64(-1);
                mItemToBinMap(itemIdx7, 1) = bIdx71 | ((u8)c71 & 1) * u64(-1);

                auto bIdx02 = CuckooIndex<>::getHash(hashs[0], 2, mNumBins);
                auto bIdx12 = CuckooIndex<>::getHash(hashs[1], 2, mNumBins);
                auto bIdx22 = CuckooIndex<>::getHash(hashs[2], 2, mNumBins);
                auto bIdx32 = CuckooIndex<>::getHash(hashs[3], 2, mNumBins);
                auto bIdx42 = CuckooIndex<>::getHash(hashs[4], 2, mNumBins);
                auto bIdx52 = CuckooIndex<>::getHash(hashs[5], 2, mNumBins);
                auto bIdx62 = CuckooIndex<>::getHash(hashs[6], 2, mNumBins);
                auto bIdx72 = CuckooIndex<>::getHash(hashs[7], 2, mNumBins);

                bool c02 = bIdx00 == bIdx02 || bIdx01 == bIdx02;
                bool c12 = bIdx10 == bIdx12 || bIdx11 == bIdx12;
                bool c22 = bIdx20 == bIdx22 || bIdx21 == bIdx22;
                bool c32 = bIdx30 == bIdx32 || bIdx31 == bIdx32;
                bool c42 = bIdx40 == bIdx42 || bIdx41 == bIdx42;
                bool c52 = bIdx50 == bIdx52 || bIdx51 == bIdx52;
                bool c62 = bIdx60 == bIdx62 || bIdx61 == bIdx62;
                bool c72 = bIdx70 == bIdx72 || bIdx71 == bIdx72;

                mBins(bIdx02, mBinSizes[bIdx02]++).set(itemIdx0, 2, c02);
                mBins(bIdx12, mBinSizes[bIdx12]++).set(itemIdx1, 2, c12);
                mBins(bIdx22, mBinSizes[bIdx22]++).set(itemIdx2, 2, c22);
                mBins(bIdx32, mBinSizes[bIdx32]++).set(itemIdx3, 2, c32);
                mBins(bIdx42, mBinSizes[bIdx42]++).set(itemIdx4, 2, c42);
                mBins(bIdx52, mBinSizes[bIdx52]++).set(itemIdx5, 2, c52);
                mBins(bIdx62, mBinSizes[bIdx62]++).set(itemIdx6, 2, c62);
                mBins(bIdx72, mBinSizes[bIdx72]++).set(itemIdx7, 2, c72);

                mItemToBinMap(itemIdx0, 2) = bIdx02 | ((u8)c02 & 1) * u64(-1);
                mItemToBinMap(itemIdx1, 2) = bIdx12 | ((u8)c12 & 1) * u64(-1);
                mItemToBinMap(itemIdx2, 2) = bIdx22 | ((u8)c22 & 1) * u64(-1);
                mItemToBinMap(itemIdx3, 2) = bIdx32 | ((u8)c32 & 1) * u64(-1);
                mItemToBinMap(itemIdx4, 2) = bIdx42 | ((u8)c42 & 1) * u64(-1);
                mItemToBinMap(itemIdx5, 2) = bIdx52 | ((u8)c52 & 1) * u64(-1);
                mItemToBinMap(itemIdx6, 2) = bIdx62 | ((u8)c62 & 1) * u64(-1);
                mItemToBinMap(itemIdx7, 2) = bIdx72 | ((u8)c72 & 1) * u64(-1);
            }

            hasher.ecbEncBlocks(items.data() + itemIdx, remSteps, hashs.data());
            for (u64 i = 0; i < remSteps; i += hashs.size())
            {
                hashs[i] = hashs[i] ^ items[itemIdx + i];

                std::vector<u64> bIdxs(mNumHashFunctions);
                for (u64 h = 0; h < mNumHashFunctions; ++h)
                {
                    auto bIdx = CuckooIndex<>::getHash(hashs[i], (u8)h, mNumBins);
                    bool collision = false;

                    bIdxs[h] = bIdx;
                    for (u64 hh = 0; hh < h; ++hh)
                        collision |= (bIdxs[hh] == bIdx);

                    mBins(bIdx, mBinSizes[bIdx]++).set(itemIdx, u8(h), collision);
                    mItemToBinMap(itemIdx + i, h) = bIdx | ((u8)collision & 1) * u64(-1);
                }
            }
        }
        else
        {
            std::vector<u64> bIdxs(mNumHashFunctions);
            for (u64 i = 0; i < u64(items.size()); i += u64(hashs.size()))
            {
                auto min = std::min<u64>(items.size() - i, hashs.size());

                hasher.ecbEncBlocks(items.data() + i, min, hashs.data());

                for (u64 j = 0, itemIdx = i; j < min; ++j, ++itemIdx)
                {
                    hashs[j] = hashs[j] ^ items[itemIdx];

                    for (u64 h = 0; h < mNumHashFunctions; ++h)
                    {
                        auto bIdx = CuckooIndex<>::getHash(hashs[j], (u8)h, mNumBins);
                        bool collision = false;

                        bIdxs[h] = bIdx;
                        for (u64 hh = 0; hh < h; ++hh)
                            collision |= (bIdxs[hh] == bIdx);

                        mBins(bIdx, mBinSizes[bIdx]++).set(itemIdx, u8(h), collision);
                        mItemToBinMap(itemIdx + i, h) = bIdx | ((u8)collision & 1) * u64(-1);

                    }
                }
            }
        }
    }

}


#ifndef POINT_TRIANGLE_HPP
#define POINT_TRIANGLE_HPP

#include <Eigen/Core>

template<class U>
class projector {
public:
  using real = U;
  using vec3 = Eigen::Matrix<real, 3, 1>;
private:
  vec3 p1, p2, p3, p1p2, p1p3;
  real distp1p2;
  real fa, fb, fc;
  real fdet;

  // singular case
  bool singular;
  real e0, e1, sign, ff;

  static constexpr real epsilon = 1e-10;
public:
  projector(vec3 p1, vec3 p2, vec3 p3):
      p1(p1),
      p2(p2),
      p3(p3),
      p1p2(p2 - p1),
      p1p3(p3 - p1),
      fa(p1p2.dot(p1p2)),
      fb(p1p2.dot(p1p3)),
      fc(p1p3.dot(p1p3)),
      fdet(fa * fc - fb * fb),
      singular(fdet < epsilon && fdet > -epsilon) {

  }

  static vec3 project_edge(vec3 origin, vec3 dir, vec3 p) {
    return origin + dir * dir.dot(p - origin) / dir.dot(dir);
  }

  vec3 operator()(vec3 p) const {
    if(singular) {
      if(fa + fc < epsilon) {
        // single point: easy peasy
        return p1;
      }

      if(fb > 0) {
        // both edges point in the same direction: project on the longest
        if(fa > fc) {
          return project_edge(p1, p1p2, p);
        } else {
          return project_edge(p1, p1p3, p);
        }
      } else {
        // edges pointing in opposite directions
        return project_edge(p2, p3 - p2, p);
      }
    }

    // non-singular case
    const vec3 pp1 = p1 - p;
    const real fd = p1p2.dot(pp1), fe = p1p3.dot(pp1);

    // minimize squared distance to source point
    real fs = fb * fe - fc * fd, ft = fb * fd - fa * fe;

    if(fs + ft <= fdet) {
      if(fs < 0) {
        if(ft < 0) {
          // region 4
          if(fd < 0) {
            ft = 0;
            if(-fd >= fa) {
              fs = 1;
            } else {
              fs = -fd / fa;
            }
          } else {
            fs = 0;
            if(fe >= 0) {
              ft = 0;
            } else if(-fe >= fc) {
              ft = 1;
            } else {
              ft = -fe / fc;
            }
          }
        } else {
          // region 3
          fs = 0;
          if(fe >= 0) {
            ft = 0;
          } else if(-fe >= fc) {
            ft = 1;
          } else {
            ft = -fe / fc;
          }
        }
      } else if(ft < 0) {
        // region 5
        ft = 0;
        if(fd >= 0) {
          fs = 0;
        } else if(-fd >= fa) {
          fs = 1;
        } else {
          fs = -fd / fa;
        }

      } else {
        // region 0
        // minimum at interior point
        fs /= fdet;
        ft /= fdet;
      }
    } else {
      real ftmp0, ftmp1, fNumer, fDenom;

      if(fs < 0) {
        // region 2
        ftmp0 = fb + fd;
        ftmp1 = fc + fe;
        if(ftmp1 > ftmp0) {
          fNumer = ftmp1 - ftmp0;
          fDenom = fa - 2 * fb + fc;
          if(fNumer >= fDenom) {
            fs = 1;
            ft = 0;
          } else {
            fs = fNumer / fDenom;
            ft = 1 - fs;
          }
        } else {
          fs = 0;
          if(ftmp1 <= 0) {
            ft = 1;
          } else if(fe >= 0) {
            ft = 0;
          } else {
            ft = -fe / fc;
          }
        }
      } else if(ft < 0) {
        // region 6
        ftmp0 = fb + fe;
        ftmp1 = fa + fd;
        if(ftmp1 > ftmp0) {
          fNumer = ftmp1 - ftmp0;
          fDenom = fa - 2 * fb + fc;
          if(fNumer >= fDenom) {
            ft = 1;
            fs = 0;
          } else {
            ft = fNumer / fDenom;
            fs = 1 - ft;
          }
        } else {
          ft = 0;
          if(ftmp1 <= 0) {
            fs = 1;
          } else if(fd >= 0) {
            fs = 0;
          } else {
            fs = -fd / fa;
          }
        }
      } else {
        // region 1
        fNumer = fc + fe - fb - fd;
        if(fNumer <= 0) {
          fs = 0;
          ft = 1;
        } else {
          fDenom = fa - 2 * fb + fc;
          if(fNumer >= fDenom) {
            fs = 1;
            ft = 0;
          } else {
            fs = fNumer / fDenom;
            ft = 1 - fs;
          }
        }
      }
    }

    return (1 - fs - ft) * p1 + fs * p2 + ft * p3;
  }
};

#endif


#include <dlib/optimization.h>
#include <fbxpstate.h>

namespace BezierFitter {
    using namespace dlib;

    // dlib reference fo the solver:
    // http://dlib.net/least_squares_ex.cpp.html

    typedef matrix< double, 3, 1 >                 BezierFitterInput;  // double t, P0, P3;
    typedef matrix< double, 2, 1 >                 BezierFitterParams; // double P1, P2;
    typedef std::pair< BezierFitterInput, double > BezierFitterSample; // double t, P0, P3 + Bt;

    inline double Squared( double v ) {
        return v * v;
    }

    inline double Cubed( double v ) {
        return v * v * v;
    }

    inline double BezierFitterModel( const BezierFitterInput input, const BezierFitterParams params ) {
        const double t  = input( 0 );
        const double P0 = input( 1 );
        const double P1 = params( 0 );
        const double P2 = params( 1 );
        const double P3 = input( 2 );
        return Cubed( 1.0 - t ) * P0 + 3 * Squared( 1.0 - t ) * t * P1 + 3 * ( 1.0 - t ) * Squared( t ) * P2 + Cubed( t ) * P3;
    }

    inline double BezierFitterResidual( const BezierFitterSample input, const BezierFitterParams params ) {
        const double Bt = input.second;
        return BezierFitterModel( input.first, params ) - Bt;
    }

    inline BezierFitterParams BezierFitterResidualDerivative( const BezierFitterSample input,
                                                              const BezierFitterParams params ) {
        const double       t = input.first( 0 );
        BezierFitterParams derivative;
        derivative( 0 ) = 3 * Squared( 1.0 - t ) * t;
        derivative( 1 ) = 3 * ( 1.0 - t ) * Squared( t );
        return derivative;
    }

    static BezierFitterParams SolveBezier( std::vector< BezierFitterSample > samples ) {
        BezierFitterParams bezierSolverParams;
        bezierSolverParams = 0;

        // Use the Levenberg-Marquardt method to determine the parameters which
        // minimize the sum of all squared residuals.
        solve_least_squares_lm( objective_delta_stop_strategy( 1e-7 ),
                                BezierFitterResidual,
                                BezierFitterResidualDerivative,
                                samples,
                                bezierSolverParams );

        // If we didn't create the residual_derivative function then we could
        // have used this method which numerically approximates the derivatives for you.
        // solve_least_squares_lm( objective_delta_stop_strategy( 1e-7 ).be_verbose( ),
        //                         BezierFitterResidual,
        //                         derivative( BezierFitterResidual ),
        //                         samples,
        //                         x );

        // This version of the solver uses a method which is appropriate for problems
        // where the residuals don't go to zero at the solution.  So in these cases
        // it may provide a better answer.
        // solve_least_squares( objective_delta_stop_strategy( 1e-7 ).be_verbose( ),
        // solve_least_squares( objective_delta_stop_strategy( 1e-7 ),
        //                      BezierFitterResidual,
        //                      BezierFitterResidualDerivative,
        //                      samples,
        //                      bezierSolverParams );

        return bezierSolverParams;
    }

    static void ExtractSamples( FbxAnimCurve*                      pAnimCurve,
                                const int                          /*startIndex*/,
                                const double                       P0X,
                                const double                       P0Y,
                                const double                       P3X,
                                const double                       P3Y,
                                std::vector< BezierFitterSample >& samples ) {

        // TODO: Scan only the relevan region, break the loop when the end time is reached.
        for ( int i = 0; i < pAnimCurve->KeyGetCount(); ++i ) {
            const double time = pAnimCurve->KeyGetTime( i ).GetSecondDouble( );

            if ( time >= P0X && time <= P3X ) {
                const double t  = ( time - P0X ) / ( P3X - P0X );
                const double Bt = pAnimCurve->KeyGetValue( i );

                BezierFitterSample sample;
                sample.first( 0 ) = t;
                sample.first( 1 ) = P0Y;
                sample.first( 2 ) = P3Y;
                sample.second     = Bt;

                samples.push_back( sample );
            }
        }
    }

} // namespace BezierFitter

bool BezierFitterFitSamples( FbxAnimCurve* pAnimCurve,
                             const int     startIndex,
                             const double  BezP0X,
                             const double  BezP0Y,
                             const double  BezP3X,
                             const double  BezP3Y,
                             double&       BezP1Y,
                             double&       BezP2Y ) {
    std::vector< BezierFitter::BezierFitterSample > samples;
    BezierFitter::ExtractSamples( pAnimCurve, startIndex, BezP0X, BezP0Y, BezP3X, BezP3Y, samples );

    auto & s = apemode::State::Get( );
    if ( !samples.empty( ) ) {
        auto params = BezierFitter::SolveBezier( std::move( samples ) );
        BezP1Y = params( 0 );
        BezP2Y = params( 1 );
        s.console->debug( "Samples taken: {}", samples.size( ) );
        s.console->debug( "Solved Bezier: {} {}", BezP1Y, BezP2Y );
        return true;
    }

    s.console->error("Failed to find the samples for fitting the Bezier control points.");
    return false;
}

void BezierFitterFitSamples( FbxAnimCurve* pAnimCurve, int keyIndex, double& OutFittedBezier1, double& OutFittedBezier2 ) {
    assert( pAnimCurve && ( keyIndex < ( pAnimCurve->KeyGetCount( ) - 1 ) ) );
    auto& s = apemode::State::Get( );

    const FbxString copiedCurveName = pAnimCurve->GetNameOnly() + " [FbxPipeline-Copy]";
    if ( FbxAnimCurve* pCopiedAnimCurve = FbxAnimCurve::Create( s.manager, copiedCurveName.Buffer() ) ) {
        pCopiedAnimCurve->CopyFrom( *pAnimCurve );

        auto resampleStartTime = pAnimCurve->KeyGet( keyIndex ).GetTime( );
        auto resampleStopTime  = pAnimCurve->KeyGet( keyIndex + 1 ).GetTime( );

        FbxTime resamplePeriodTime;
        resamplePeriodTime.SetMilliSeconds( ( FbxLongLong )( 1000.0f / 180.0f ) );

        FbxAnimCurveFilterResample animCurveFilterResample;
        animCurveFilterResample.SetPeriodTime( resamplePeriodTime );
        animCurveFilterResample.SetStartTime( resampleStartTime );
        animCurveFilterResample.SetStopTime( resampleStopTime );
        animCurveFilterResample.Apply( *pCopiedAnimCurve );

        BezierFitterFitSamples( pCopiedAnimCurve,
                                keyIndex,
                                pAnimCurve->KeyGet( keyIndex ).GetTime( ).GetSecondDouble( ),
                                pAnimCurve->KeyGet( keyIndex ).GetValue( ),
                                pAnimCurve->KeyGet( keyIndex + 1 ).GetTime( ).GetSecondDouble( ),
                                pAnimCurve->KeyGet( keyIndex + 1 ).GetValue( ),
                                OutFittedBezier1,
                                OutFittedBezier2 );

        pCopiedAnimCurve->Destroy( );
    }
}


#ifndef STAN_MATH_PRIM_FUN_FFT_HPP
#define STAN_MATH_PRIM_FUN_FFT_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/fun/Eigen.hpp>
#include <unsupported/Eigen/FFT>
#include <Eigen/Dense>
#include <complex>
#include <type_traits>
#include <vector>

namespace stan {
namespace math {

/**
 * Return the discrete Fourier transform of the specified complex
 * vector.
 *
 * Given an input complex vector `x[0:N-1]` of size `N`, the discrete
 * Fourier transform computes entries of the resulting complex
 * vector `y[0:N-1]` by
 *
 * ```
 * y[n] = SUM_{i < N} x[i] * exp(-n * i * 2 * pi * sqrt(-1) / N)
 * ```
 *
 * If the input is of size zero, the result is a size zero vector.
 *
 * @tparam V type of complex vector argument
 * @param[in] x vector to transform
 * @return discrete Fourier transform of `x`
 */
template <typename V, require_eigen_vector_vt<is_complex, V>* = nullptr>
inline Eigen::Matrix<scalar_type_t<V>, -1, 1> fft(const V& x) {
  // copy because fft() requires Eigen::Matrix type
  Eigen::Matrix<scalar_type_t<V>, -1, 1> xv = x;
  if (xv.size() <= 1)
    return xv;
  Eigen::FFT<base_type_t<V>> fft;
  return fft.fwd(xv);
}

/**
 * Return the inverse discrete Fourier transform of the specified
 * complex vector.
 *
 * Given an input complex vector `y[0:N-1]` of size `N`, the inverse
 * discrete Fourier transform computes entries of the resulting
 * complex vector `x[0:N-1]` by
 *
 * ```
 * x[n] = SUM_{i < N} y[i] * exp(n * i * 2 * pi * sqrt(-1) / N)
 * ```
 *
 * If the input is of size zero, the result is a size zero vector.
 * The only difference between the discrete DFT and its inverse is
 * the sign of the exponent.
 *
 * @tparam V type of complex vector argument
 * @param[in] y vector to inverse transform
 * @return inverse discrete Fourier transform of `y`
 */
template <typename V, require_eigen_vector_vt<is_complex, V>* = nullptr>
inline Eigen::Matrix<scalar_type_t<V>, -1, 1> inv_fft(const V& y) {
  // copy because fft() requires Eigen::Matrix type
  Eigen::Matrix<scalar_type_t<V>, -1, 1> yv = y;
  if (y.size() <= 1)
    return yv;
  Eigen::FFT<base_type_t<V>> fft;
  return fft.inv(yv);
}

/**
 * Return the two-dimensional discrete Fourier transform of the
 * specified complex matrix.  The 2D discrete Fourier transform first
 * runs the discrete Fourier transform on the each row, then on each
 * column of the result.
 *
 * @tparam M type of complex matrix argument
 * @param[in] x matrix to transform
 * @return discrete 2D Fourier transform of `x`
 */
template <typename M, require_eigen_dense_dynamic_vt<is_complex, M>* = nullptr>
inline Eigen::Matrix<scalar_type_t<M>, -1, -1> fft2(const M& x) {
  Eigen::Matrix<scalar_type_t<M>, -1, -1> y(x.rows(), x.cols());
  for (int i = 0; i < y.rows(); ++i)
    y.row(i) = fft(x.row(i));
  for (int j = 0; j < y.cols(); ++j)
    y.col(j) = fft(y.col(j));
  return y;
}

/**
 * Return the two-dimensional inverse discrete Fourier transform of
 * the specified complex matrix.  The 2D inverse discrete Fourier
 * transform first runs the 1D inverse Fourier transform on the
 * columns, and then on the resulting rows.  The composition of the
 * FFT and inverse FFT (or vice-versa) is the identity.
 *
 * @tparam M type of complex matrix argument
 * @param[in] y matrix to inverse trnasform
 * @return inverse discrete 2D Fourier transform of `y`
 */
template <typename M, require_eigen_dense_dynamic_vt<is_complex, M>* = nullptr>
inline Eigen::Matrix<scalar_type_t<M>, -1, -1> inv_fft2(const M& y) {
  Eigen::Matrix<scalar_type_t<M>, -1, -1> x(y.rows(), y.cols());
  for (int j = 0; j < x.cols(); ++j)
    x.col(j) = inv_fft(y.col(j));
  for (int i = 0; i < x.rows(); ++i)
    x.row(i) = inv_fft(x.row(i));
  return x;
}

}  // namespace math
}  // namespace stan

#endif