#ifndef SPLINE_SPLINE_HPP
#define SPLINE_SPLINE_HPP

#include <Eigen/Dense>
#include <utility>
#include <vector>
#include <iostream>

class QuadraticSplineSegment {
public:

    void setCtrlPointsAbscissa(double x_1, double x_2) {
        x1 = x_1;
        x2 = x_2;
    }

    void setCtrlPointsOrdinate(double y_1, double y_2) {
        y1 = y_1;
        y2 = y_2;
    }

    void setTangent(double tangent) {
        t1 = tangent;
    }

    bool computeParams() {
        Eigen::Matrix3d A;
        Eigen::Vector3d Y, X;

        A << pow(x1, 2), x1, 1,
                pow(x2, 2), x2, 1,
                2 * x1, 1, 0;

        Y << y1, y2, t1;

        X = A.colPivHouseholderQr().solve(Y);

        a2 = X[0];
        a1 = X[1];
        a0 = X[2];
        std::cout << a2 << " " << a1 << " " << a0 << std::endl;
    }

    void getParams(double &a_2, double &a_1, double &a_0) const {
        a_2 = a2;
        a_1 = a1;
        a_0 = a0;
    }

    double getTangent() const {
        return t1;
    }

    double arcLength(double gamma) {
        computeParams();
        double yi = y1, yi1, xi = x1, xi1, len, ti;
        while (norm(xi - x2, yi - y2) > gamma * 3) {
            ti = evalDerivative(xi);
            yi = evalFunction(xi);
            xi1 = xi + gamma / norm(1, ti);
            yi1 = evalFunction(xi1);
            len += norm(xi1 - xi, yi1 - yi);
            std::cout << ((norm(xi - x2, yi - y2) - gamma) / gamma - 1.0) * 100 << " " << xi << std::endl;
            xi = xi1;
        }
        return len;
    }

    double evalFunction(double x) const {
        return a2 * pow(x, 2) + a1 * x + a0;
    }

    double evalDerivative(double x) const {
        return a2 * x + a1;
    }

protected:

    static inline double norm(double x, double y) {
        return sqrt(pow(x, 2) + pow(y, 2));
    }

    double t1, x1, x2, y1, y2;
    double a2, a1, a0;
};

class QuadraticSplineCurve {
public:

    explicit QuadraticSplineCurve(std::vector<std::vector<double>> coordinates, double tangent) : coord(
            std::move(coordinates)), t(tangent) {}

    void compute() {
        QuadraticSplineSegment segment{};
        double x1, y1, x2, y2;

        for (int i = 0; i < coord.size() - 1; ++i) {
            auto pts1 = coord[i];
            auto pts2 = coord[i + 1];
            x1 = pts1[0];
            y1 = pts1[1];
            x2 = pts2[0];
            y2 = pts2[1];

            segment.setCtrlPointsAbscissa(x1, x2);
            segment.setCtrlPointsOrdinate(y1, y2);

            if (i == 0) {
                segment.setTangent(t);
            } else {
                segment.setTangent(curve[curve.size() - 1].evalDerivative(x2));
            }

            segment.computeParams();

            curve.push_back(segment);
        }
    }

    double evalFunction(double x) {
        double x1, x2;

        for (int i = 0; i < coord.size() - 1; ++i) {

            auto pts1 = coord[i];
            auto pts2 = coord[i + 1];

            x1 = pts1[0];
            x2 = pts2[0];

            if (x1 < x2) {
                if ((x >= x1) and (x <= x2)) {
                    return curve[i].evalFunction(x);
                }
            } else {
                if ((x <= x1) and (x >= x2)) {
                    return curve[i].evalFunction(x);
                }
            }
        }
    }

    double length(double gamma) {
        cLength = 0;
        for (auto segment : curve) {
            cLength += segment.arcLength(gamma/1000.0);
        }
    }

protected:
    std::vector<std::vector<double>> coord;
    std::vector<QuadraticSplineSegment> curve;
    double t, cLength{};
};

#endif //SPLINE_SPLINE_HPP


// g2o sphere
// based on https://github.com/RainerKuemmerle/g2o/blob/master/g2o/examples/sphere/create_sphere.cpp
// reference 1 :  https://malcolmmielle.wordpress.com/2016/06/20/g2o-optimization/
// reference 2 : http://fzheng.me/2016/03/15/g2o-demo/
// Created by Seung-Chan Kim on 12/29/16.

#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>

#include <Eigen/Core>
#include <Eigen/StdVector>

#include "g2o/types/slam3d/vertex_se3.h"
#include "g2o/types/slam3d/edge_se3.h"
#include "g2o/stuff/sampler.h"
#include "g2o/stuff/command_args.h"
#include "g2o/core/factory.h"

#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"

using namespace std;
using namespace g2o;

int main(int argc, char** argv)
{
    // command line parsing
    int nodesPerLevel;
    int numLaps;
    bool randomSeed;
    double radius;
    std::vector<double> noiseTranslation;
    std::vector<double> noiseRotation;
    string outFilename;
    CommandArgs arg;
    arg.param("o", outFilename, "-", "output filename");
    arg.param("nodesPerLevel", nodesPerLevel, 50, "how many nodes per lap on the sphere");
    arg.param("laps", numLaps, 50, "how many times the robot travels around the sphere");
    arg.param("radius", radius, 100., "radius of the sphere");
    arg.param("noiseTranslation", noiseTranslation, std::vector<double>(), "set the noise level for the translation, separated by semicolons without spaces e.g: \"0.1;0.1;0.1\"");
    arg.param("noiseRotation", noiseRotation, std::vector<double>(), "set the noise level for the rotation, separated by semicolons without spaces e.g: \"0.001;0.001;0.001\"");
    arg.param("randomSeed", randomSeed, false, "use a randomized seed for generating the sphere");
    arg.parseArgs(argc, argv);

    if (noiseTranslation.size() == 0)
    {
        cerr << "using default noise for the translation" << endl;
        noiseTranslation.push_back(0.01);
        noiseTranslation.push_back(0.01);
        noiseTranslation.push_back(0.01);
    }
    cerr << "Noise for the translation:";
    for (size_t i = 0; i < noiseTranslation.size(); ++i)
        cerr << " " << noiseTranslation[i];
    cerr << endl;
    if (noiseRotation.size() == 0)
    {
        cerr << "using default noise for the rotation" << endl;
        noiseRotation.push_back(0.005);
        noiseRotation.push_back(0.005);
        noiseRotation.push_back(0.005);
    }
    cerr << "Noise for the rotation:";
    for (size_t i = 0; i < noiseRotation.size(); ++i)
        cerr << " " << noiseRotation[i];
    cerr << endl;

    Eigen::Matrix3d transNoise = Eigen::Matrix3d::Zero();
    /*
     0 0 0
     0 0 0
     0 0 0
     */

    for (int i = 0; i < 3; ++i)
    transNoise(i, i) = std::pow(noiseTranslation[i], 2);

    Eigen::Matrix3d rotNoise = Eigen::Matrix3d::Zero();
    for (int i = 0; i < 3; ++i)
        rotNoise(i, i) = std::pow(noiseRotation[i], 2);
    cout << "transNoise =" <<endl;
    cout << transNoise << endl;

    cout << "rotNoise =" <<endl;
    cout << rotNoise << endl;

    Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Zero();
    information.block<3,3>(0,0) = transNoise.inverse();
    information.block<3,3>(3,3) = rotNoise.inverse();

    // information matrix represents how reliable this measurement is. Therefore, the more precisely the measurement is made or the more you trust in this measurement, the larger values in the information matrix you can set. (see http://fzheng.me/2016/03/15/g2o-demo/ )
    cout << "information =" <<endl;
    cout << information << endl;

    cout << "numLaps (how many times the robot travels around the sphere) = " << numLaps << endl;
    cout << "nodesPerLevel (how many nodes per lap on the sphere) = " << nodesPerLevel << endl;

    vector<VertexSE3*> vertices;
    vector<EdgeSE3*> odometryEdges;
    vector<EdgeSE3*> edges;

    // create the optimizer to load the data and carry out the optimization
    SparseOptimizer optimizer;

    int id = 0;
    FILE *fp = fopen("out_t.txt", "w+");
    for (int f = 0; f < numLaps; ++f)
    {
        for (int n = 0; n < nodesPerLevel; ++n)
        {
            VertexSE3* v = new VertexSE3;
            v->setId(id++);

            if(id==1)
            v->setFixed( true );

            Eigen::AngleAxisd rotz(-M_PI + 2*n*M_PI / nodesPerLevel, Eigen::Vector3d::UnitZ());
            Eigen::AngleAxisd roty(-0.5*M_PI + id*M_PI / (numLaps * nodesPerLevel), Eigen::Vector3d::UnitY());
            Eigen::Matrix3d rot = (rotz * roty).toRotationMatrix();

            Eigen::Isometry3d t;
            t = rot;
            t.translation() = t.linear() * Eigen::Vector3d(radius, 0, 0);
            //cout << t.translation().transpose() <<endl;;
            v->setEstimate(t);
            vertices.push_back(v);

            optimizer.addVertex(v);
            //row-wise
            fprintf(fp, "%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n",
                        t(0,0), t(0,1),t(0,2), t(0,3),
                        t(1,0), t(1,1),t(1,2), t(1,3),
                        t(2,0), t(2,1),t(2,2), t(2,3),
                        t(3,0), t(3,1),t(3,2), t(3,3) );
        }
    }
    fclose(fp);

    // generate odometry edges
    for (size_t i = 1; i < vertices.size(); ++i) {
        VertexSE3* prev = vertices[i-1];
        VertexSE3* cur  = vertices[i];
        Eigen::Isometry3d t = prev->estimate().inverse() * cur->estimate();
        EdgeSE3* e = new EdgeSE3;
        e->setVertex(0, prev);
        e->setVertex(1, cur);
        e->setMeasurement(t);
        e->setInformation(information);
        odometryEdges.push_back(e);
        edges.push_back(e);
        optimizer.addEdge(e);
    }

    // generate loop closure edges
    fp = fopen("out_loopclosure_from+to.txt", "w+");
    for (int f = 1; f < numLaps; ++f) {
        for (int nn = 0; nn < nodesPerLevel; ++nn) {
            VertexSE3* from = vertices[(f-1)*nodesPerLevel + nn];
            //row-wise
            Eigen::Isometry3d tf = from->estimate();

            for (int n = -1; n <= 1; ++n) // set three vertices
            {
                if (f == numLaps-1 && n == 1)
                    continue;
            //  VertexSE3* from = vertices[(f-1)*nodesPerLevel + nn];
                VertexSE3* to   = vertices[f*nodesPerLevel + nn + n]; // one-level위에서 3개의 vertices와 연결
                Eigen::Isometry3d t = from->estimate().inverse() * to->estimate();
                EdgeSE3* e = new EdgeSE3;
                e->setVertex(0, from);
                e->setVertex(1, to);
                e->setMeasurement(t);
                e->setInformation(information);
                edges.push_back(e);
                optimizer.addEdge(e); // what happens if commented out?

                Eigen::Isometry3d tt = to->estimate();
                fprintf(fp, "%f\t%f\t%f\t%f\t\t",   tf(0,3),tf(1,3),tf(2,3),tf(3,3) );
                fprintf(fp, "%f\t%f\t%f\t%f\n",     tt(0,3),tt(1,3),tt(2,3),tt(3,3) );
            }
            //fprintf(fp, "\n");
        }
    }
    fclose(fp);

    GaussianSampler<Eigen::Vector3d, Eigen::Matrix3d> transSampler;
    transSampler.setDistribution(transNoise);
    GaussianSampler<Eigen::Vector3d, Eigen::Matrix3d> rotSampler;
    rotSampler.setDistribution(rotNoise);

#if 0
    if (randomSeed)
    {
        std::random_device r;
        std::seed_seq seedSeq{r(), r(), r(), r(), r()};
        vector<int> seeds(2);
        seedSeq.generate(seeds.begin(), seeds.end());
        cerr << "using seeds:";
        for (size_t i = 0; i < seeds.size(); ++i)
            cerr << " " << seeds[i];
        cerr << endl;
        transSampler.seed(seeds[0]); // error ?
        rotSampler.seed(seeds[1]);
    }
#endif

    // noise for all the edges
    for (size_t i = 0; i < edges.size(); ++i)
    {
        EdgeSE3* e = edges[i];
        Eigen::Quaterniond gtQuat = (Eigen::Quaterniond)e->measurement().linear();
        Eigen::Vector3d gtTrans = e->measurement().translation();

        Eigen::Vector3d quatXYZ = rotSampler.generateSample();
        double qw = 1.0 - quatXYZ.norm();
        if (qw < 0)
        {
            qw = 0.;
            cerr << "x";
        }
        Eigen::Quaterniond rot(qw, quatXYZ.x(), quatXYZ.y(), quatXYZ.z());
        rot.normalize();
        Eigen::Vector3d trans = transSampler.generateSample();
        rot = gtQuat * rot;
        trans = gtTrans + trans;

        Eigen::Isometry3d noisyMeasurement = (Eigen::Isometry3d) rot;
        noisyMeasurement.translation() = trans;
        e->setMeasurement(noisyMeasurement);
    }

    // concatenate all the odometry constraints to compute the initial state
    // An hyper graph is a graph where an edge can connect one or more nodes.
    for (size_t i =0; i < odometryEdges.size(); ++i)
    {
        EdgeSE3* e = edges[i];
        VertexSE3* from = static_cast<VertexSE3*>(e->vertex(0));
        VertexSE3* to = static_cast<VertexSE3*>(e->vertex(1));
        HyperGraph::VertexSet aux;
        aux.insert(from);
        e->initialEstimate(aux, to);
    }

    // write output
    ofstream fileOutputStream;
    if (outFilename != "-")
    {
        //cerr << "Writing into " << outFilename << endl;
        //fileOutputStream.open(outFilename.c_str());
    }
    else
    {
        //cerr << "writing to stdout" << endl;
        outFilename = "out_vertex+edge.g2o";

    }
    cerr << "Writing into " << outFilename << endl;
    fileOutputStream.open(outFilename.c_str());

    string vertexTag = Factory::instance()->tag(vertices[0]);
    string edgeTag = Factory::instance()->tag(edges[0]);
    cout << "vertexTag = "<< vertexTag << endl;
    cout << "edgeTag = "<< edgeTag << endl;

    ostream& fout = outFilename != "-" ? fileOutputStream : cout;
    for (size_t i = 0; i < vertices.size(); ++i)
    {
        VertexSE3* v = vertices[i];
        fout << vertexTag << " " << v->id() << " ";
        v->write(fout);
        fout << endl;
    }

    for (size_t i = 0; i < edges.size(); ++i)
    {
        EdgeSE3* e = edges[i];
        VertexSE3* from = static_cast<VertexSE3*>(e->vertex(0));
        VertexSE3* to = static_cast<VertexSE3*>(e->vertex(1));
        fout << edgeTag << " " << from->id() << " " << to->id() << " ";
        e->write(fout);
        fout << endl;
    }

    // write output
    fp = fopen("out_vertices_before.txt", "w");
    for(int i=0; i<vertices.size(); i++)
    {

        Eigen::Isometry3d t = vertices[i]->estimate();
        //cout<<"vertex pose(before)="<<endl<<t.matrix()<<endl;

        fprintf(fp, "%f\t%f\t%f\t%f\n",   t(0,3),t(1,3),t(2,3),t(3,3) );

    }
    fclose(fp);

    // create the linear solver
    BlockSolverX::LinearSolverType * linearSolver = new LinearSolverCSparse<BlockSolverX::PoseMatrixType>();

    // create the block solver on top of the linear solver
    BlockSolverX* blockSolver = new BlockSolverX(linearSolver);

    // create the algorithm to carry out the optimization
    //OptimizationAlgorithmGaussNewton* optimizationAlgorithm = new OptimizationAlgorithmGaussNewton(blockSolver);
    OptimizationAlgorithmLevenberg* optimizationAlgorithm = new OptimizationAlgorithmLevenberg(blockSolver);

    optimizer.setVerbose(true);
    optimizer.setAlgorithm(optimizationAlgorithm);

    int maxIterations = 10;

    optimizer.initializeOptimization();
    optimizer.optimize(maxIterations);

    // Visulizing only points (w/o pose)
    fp = fopen("out_vertices_after.txt", "w");
    for(size_t i=0; i<vertices.size(); i++)
    //for(int i=0; i<2; i++)
    {
        g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>( optimizer.vertex(i) );
        Eigen::Isometry3d t = v->estimate();
        //cout<<"vertex pose(after)="<<endl<<t.matrix()<<endl;

        fprintf(fp, "%f\t%f\t%f\t%f\n",   t(0,3),t(1,3),t(2,3),t(3,3) );

    }
    fclose(fp);

    cout << "Done!!! " << endl;
    return 0;
}


/* @copyright The code is licensed under the MIT License
 *            <https://opensource.org/licenses/MIT>,
 *            Copyright (c) 2020 Christian Eskil Vaugelade Berg
 * @author Christian Eskil Vaugelade Berg
*/
#pragma once

#include <utility>

#include <Eigen/Dense>

namespace orient {

// \brief Calculate rotation matrix from quaternion
//        Quaternion is expressed as a 4D-vector [w,a,b,c] such that
//        q = w + a*i + b*j + c*k
// \param q Source quaternion
// \return Rotation matrix
template<typename Scalar>
Eigen::Matrix<Scalar, 3, 3> rotationMatrixFromQuaternion(Eigen::Matrix<Scalar, 4, 1> const& q);

// \brief Calculate rotation matrix and partial derivatives from quaternion
//        See above for more detail
// \param q Source quaternion
// \return A pair containg first the rotation matrix then secondly the Jacobian matrix
template<typename Scalar>
std::pair<Eigen::Matrix<Scalar, 3, 3>, Eigen::Matrix<Scalar, 9, 4>> rotationMatrixFromQuaternionWD(Eigen::Matrix<Scalar, 4, 1> const& q);

// \brief Calculate angle axis derivatives from quaternion
//        See above for more detail
// \param q Source quaternion
// \return Angle axis
template<typename Scalar>
Eigen::Matrix<Scalar, 3, 1> angleAxisFromQuaternion(Eigen::Matrix<Scalar, 4, 1> const& q);

// \brief Calculate angle axis and partial derivatives from quaternion
//        See above for more detail
// \param q Source quaternion
// \return A pair containg first the angle axis secondly the Jacobian matrix
template<typename Scalar>
std::pair<Eigen::Matrix<Scalar, 3, 1>, Eigen::Matrix<Scalar, 3, 4>> angleAxisFromQuaternionWD(Eigen::Matrix<Scalar, 4, 1> const& q);

}

#include <orient/impl/from_quaternion.hpp>


#pragma once
#include <Eigen/Core>
#include <boost/math/constants/constants.hpp>
#include "ear/common_types.hpp"
#include "ear/metadata.hpp"

namespace ear {

  template <typename T>
  inline T radians(T d) {
    return d * static_cast<T>(boost::math::constants::pi<double>() / 180.0);
  }

  template <typename T>
  inline T degrees(T r) {
    return r * static_cast<T>(180.0 / boost::math::constants::pi<double>());
  }

  /** @brief Assuming y is clockwise from x, increment y by 360 until it's not
   * less than x.
   *
   * @param x Start angle in degrees.
   * @param y End angle in degrees.
   *
   * @returns y shifted such that it represents the same angle but is greater
   *  than x.
   */
  inline double relativeAngle(double x, double y) {
    while (y - 360.0 >= x) {
      y -= 360.0;
    }
    while (y < x) {
      y += 360.0;
    }
    return y;
  }

  /** @brief Assuming end is clockwise from start, is the angle x inside
   * [start,end] within some tolerance?
   *
   * @param x Angle to test, in degrees
   * @param start Start angle of range, in degrees
   * @param end End angle of range, in degrees
   * @param tol Tolerance in degrees to check within
   */
  bool insideAngleRange(double x, double start, double end, double tol = 0.0);

  /** @brief Order the vertices of a convex, approximately planar polygon.
   *
   * @param vertices Vertices to order (n, 3)
   *
   * @returns Indices to put the vertices into the right order.
   */
  Eigen::VectorXi ngonVertexOrder(Eigen::MatrixXd vertices);

  double azimuth(Eigen::Vector3d position);
  double elevation(Eigen::Vector3d position);
  double distance(Eigen::Vector3d position);

  Eigen::Vector3d cart(double azimuth, double elevation, double distance);

  inline Eigen::RowVector3d cartT(double azimuth, double elevation,
                                  double distance) {
    return cart(azimuth, elevation, distance).transpose();
  }

  PolarPosition toPolarPosition(CartesianPosition position);
  CartesianPosition toCartesianPosition(PolarPosition position);
  Eigen::Vector3d toNormalisedVector3d(PolarPosition position);

  Eigen::Vector3d toCartesianVector3d(PolarPosition position);
  Eigen::Vector3d toCartesianVector3d(CartesianPosition position);
  Eigen::Vector3d toCartesianVector3d(SpeakerPosition position);
  Eigen::Vector3d toCartesianVector3d(PolarSpeakerPosition position);
  Eigen::Vector3d toCartesianVector3d(CartesianSpeakerPosition position);

  Eigen::MatrixXd toPositionsMatrix(
      const std::vector<PolarPosition>& positions);

  /** @brief Calc local coordinate system
   *
   * @param azimuth: ADM format azimuth
   * @param elevation: ADM format elevation
   *
   * @return Vectors pointing along x, y and z, rotated so that +y points at
   * cart(az, el, 1).
   */
  inline Eigen::Matrix3d localCoordinateSystem(double azimuth,
                                               double elevation) {
    Eigen::Matrix3d ret;
    ret << cartT(azimuth - 90.0, 0.0, 1.0),  //
        cartT(azimuth, elevation, 1.0),  //
        cartT(azimuth, elevation + 90.0, 1.0);
    return ret;
  }

}  // namespace ear


/** @file 38.cpp Problem 38: Pandigital multiples
 *
 * Take the number 192 and multiply it by each of 1, 2, and 3:
 *
 *      192 x 1 = 192
 *      192 x 2 = 384
 *      192 x 3 = 576
 *
 * By concatenating each product we get the 1 to 9 pandigital, 192384576. We
 * will call 192384576 the concatenated product of 192 and (1,2,3)
 *
 * The same can be achieved by starting with 9 and multiplying by 1, 2, 3, 4,
 * and 5, giving the pandigital, 918273645, which is the concatenated product
 * of 9 and (1,2,3,4,5).
 *
 * What is the largest 1 to 9 pandigital 9-digit number that can be formed as
 * the concatenated product of an integer with (1,2, ... , n) where n > 1?
 */

// At most eight values of n, 2 through 9, are worth considering; n < 2 is
// explicitly prohibited by the problem description, and n > 9 will produce a
// number that exceeds the specified (9-digit) length.
//
// Similarly, we can limit the values of the multiplicand `m` to those that
// produce 9-digit concatenated products.

#include "int_util.hpp"                         // pandigital

/// @cond
#include <boost/iterator/counting_iterator.hpp> // counting_iterator
#include <boost/multiprecision/cpp_int.hpp>     // cpp_int

#include <algorithm>                            // all_of
#include <iostream>                             // cout
#include <vector>
/// @endcond

using boost::make_counting_iterator;
using boost::multiprecision::cpp_int;
using int_util::length;
using int_util::pandigital;

cpp_int cat_prod(cpp_int const& m, std::vector<int> const& v)
{
    std::string s;
    for (auto const& x : v)
        s += cpp_int(m * x).str();
    return cpp_int(s);
}

int main()
{
    cpp_int r = 0;
    for (int n = 2; n <= 9; ++n) {
        std::vector<int> v(
                make_counting_iterator(1),
                make_counting_iterator(n + 1));
        cpp_int m = 1;
        auto p = cat_prod(m, v);
        while (p.str().size() < 9) {  // Skip products that are too short.
            ++m;
            p = cat_prod(m, v);
        }
        while (p.str().size() == 9) {
            if (p > r && pandigital(p))
                r = p;
            p = cat_prod(++m, v);
        }
    }
    std::cout << r << std::endl;
}


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

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>

#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
using namespace std;
using Sophus::SE3;
using Sophus::SO3;

/************************************************
 * 本程序演示如何用g2o solver进行位姿图优化
 * sphere.g2o是人工生成的一个Pose graph，我们来优化它。
 * 尽管可以直接通过load函数读取整个图，但我们还是自己来实现读取代码，以期获得更深刻的理解
 * 本节使用李代数表达位姿图，节点和边的方式为自定义
 * **********************************************/

typedef Eigen::Matrix<double,6,6> Matrix6d;

// 给定误差求J_R^{-1}的近似
Matrix6d JRInv( SE3 e )
{
    Matrix6d J;
    J.block(0,0,3,3) = SO3::hat(e.so3().log());
    J.block(0,3,3,3) = SO3::hat(e.translation());
    J.block(3,0,3,3) = Eigen::Matrix3d::Zero(3,3);
    J.block(3,3,3,3) = SO3::hat(e.so3().log());
    J = J*0.5 + Matrix6d::Identity();
    return J;
}
// 李代数顶点
typedef Eigen::Matrix<double, 6, 1> Vector6d;
class VertexSE3LieAlgebra: public g2o::BaseVertex<6, SE3>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    bool read ( istream& is )
    {
        double data[7];
        for ( int i=0; i<7; i++ )
            is>>data[i];
        setEstimate ( SE3 (
                Eigen::Quaterniond ( data[6],data[3], data[4], data[5] ),
                Eigen::Vector3d ( data[0], data[1], data[2] )
        ));
    }

    bool write ( ostream& os ) const
    {
        os<<id()<<" ";
        Eigen::Quaterniond q = _estimate.unit_quaternion();
        os<<_estimate.translation().transpose()<<" ";
        os<<q.coeffs()[0]<<" "<<q.coeffs()[1]<<" "<<q.coeffs()[2]<<" "<<q.coeffs()[3]<<endl;
        return true;
    }

    virtual void setToOriginImpl()
    {
        _estimate = Sophus::SE3();
    }
    // 左乘更新
    virtual void oplusImpl ( const double* update )
    {
        Sophus::SE3 up (
            Sophus::SO3 ( update[3], update[4], update[5] ),
            Eigen::Vector3d ( update[0], update[1], update[2] )
        );
        _estimate = up*_estimate;
    }
};

// 两个李代数节点之边
class EdgeSE3LieAlgebra: public g2o::BaseBinaryEdge<6, SE3, VertexSE3LieAlgebra, VertexSE3LieAlgebra>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    bool read ( istream& is )
    {
        double data[7];
        for ( int i=0; i<7; i++ )
            is>>data[i];
        Eigen::Quaterniond q ( data[6], data[3], data[4], data[5] );
        q.normalize();
        setMeasurement (
            Sophus::SE3 ( q, Eigen::Vector3d ( data[0], data[1], data[2] ) )
        );
        for ( int i=0; i<information().rows() && is.good(); i++ )
            for ( int j=i; j<information().cols() && is.good(); j++ )
            {
                is >> information() ( i,j );
                if ( i!=j )
                    information() ( j,i ) =information() ( i,j );
            }
        return true;
    }
    bool write ( ostream& os ) const
    {
        VertexSE3LieAlgebra* v1 = static_cast<VertexSE3LieAlgebra*> (_vertices[0]);
        VertexSE3LieAlgebra* v2 = static_cast<VertexSE3LieAlgebra*> (_vertices[1]);
        os<<v1->id()<<" "<<v2->id()<<" ";
        SE3 m = _measurement;
        Eigen::Quaterniond q = m.unit_quaternion();
        os<<m.translation().transpose()<<" ";
        os<<q.coeffs()[0]<<" "<<q.coeffs()[1]<<" "<<q.coeffs()[2]<<" "<<q.coeffs()[3]<<" ";
        // information matrix
        for ( int i=0; i<information().rows(); i++ )
            for ( int j=i; j<information().cols(); j++ )
            {
                os << information() ( i,j ) << " ";
            }
        os<<endl;
        return true;
    }

    // 误差计算与书中推导一致
    virtual void computeError()
    {
        Sophus::SE3 v1 = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->estimate();
        Sophus::SE3 v2 = (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->estimate();
        _error = (_measurement.inverse()*v1.inverse()*v2).log();
    }

    // 雅可比计算
    virtual void linearizeOplus()
    {
        Sophus::SE3 v1 = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->estimate();
        Sophus::SE3 v2 = (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->estimate();
        Matrix6d J = JRInv(SE3::exp(_error));
        // 尝试把J近似为I？
        _jacobianOplusXi = - J* v2.inverse().Adj();
        _jacobianOplusXj = J*v2.inverse().Adj();
    }
};

int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"Usage: pose_graph_g2o_SE3_lie sphere.g2o"<<endl;
        return 1;
    }
    ifstream fin ( argv[1] );
    if ( !fin )
    {
        cout<<"file "<<argv[1]<<" does not exist."<<endl;
        return 1;
    }

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,6>> Block;  // BlockSolver为6x6
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    // 试试G-N或Dogleg？
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton ( solver_ptr );

    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm ( solver );  // 设置求解器

    int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量

    vector<VertexSE3LieAlgebra*> vectices;
    vector<EdgeSE3LieAlgebra*> edges;
    while ( !fin.eof() )
    {
        string name;
        fin>>name;
        if ( name == "VERTEX_SE3:QUAT" )
        {
            // 顶点
            VertexSE3LieAlgebra* v = new VertexSE3LieAlgebra();
            int index = 0;
            fin>>index;
            v->setId( index );
            v->read(fin);
            optimizer.addVertex(v);
            vertexCnt++;
            vectices.push_back(v);
            if ( index==0 )
                v->setFixed(true);
        }
        else if ( name=="EDGE_SE3:QUAT" )
        {
            // SE3-SE3 边
            EdgeSE3LieAlgebra* e = new EdgeSE3LieAlgebra();
            int idx1, idx2;     // 关联的两个顶点
            fin>>idx1>>idx2;
            e->setId( edgeCnt++ );
            e->setVertex( 0, optimizer.vertices()[idx1] );
            e->setVertex( 1, optimizer.vertices()[idx2] );
            e->read(fin);
            optimizer.addEdge(e);
            edges.push_back(e);
        }
        if ( !fin.good() ) break;
    }

    cout<<"read total "<<vertexCnt<<" vertices, "<<edgeCnt<<" edges."<<endl;

    cout<<"prepare optimizing ..."<<endl;
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    cout<<"calling optimizing ..."<<endl;
    optimizer.optimize(30);

    cout<<"saving optimization results ..."<<endl;
    // 因为用了自定义顶点且没有向g2o注册，这里保存自己来实现
    // 伪装成 SE3 顶点和边，让 g2o_viewer 可以认出
    ofstream fout("result_lie.g2o");
    for ( VertexSE3LieAlgebra* v:vectices )
    {
        fout<<"VERTEX_SE3:QUAT ";
        v->write(fout);
    }
    for ( EdgeSE3LieAlgebra* e:edges )
    {
        fout<<"EDGE_SE3:QUAT ";
        e->write(fout);
    }
    fout.close();
    return 0;
}


//  (C) Copyright Matt Borland 2021.
//  Use, modification and distribution are 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_MATH_STATISTICS_Z_TEST_HPP
#define BOOST_MATH_STATISTICS_Z_TEST_HPP

#include <boost/math/distributions/normal.hpp>
#include <boost/math/statistics/univariate_statistics.hpp>
#include <iterator>
#include <type_traits>
#include <utility>
#include <cmath>

namespace boost { namespace math { namespace statistics { namespace detail {

template<typename ReturnType, typename T>
ReturnType one_sample_z_test_impl(T sample_mean, T sample_variance, T sample_size, T assumed_mean)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;
    using std::sqrt;
    using no_promote_policy = boost::math::policies::policy<boost::math::policies::promote_float<false>, boost::math::policies::promote_double<false>>;

    Real test_statistic = (sample_mean - assumed_mean) / (sample_variance / sqrt(sample_size));
    auto z = boost::math::normal_distribution<Real, no_promote_policy>(sample_size - 1);
    Real pvalue;
    if(test_statistic > 0)
    {
        pvalue = 2*boost::math::cdf<Real>(z, -test_statistic);
    }
    else
    {
        pvalue = 2*boost::math::cdf<Real>(z, test_statistic);
    }

    return std::make_pair(test_statistic, pvalue);
}

template<typename ReturnType, typename ForwardIterator>
ReturnType one_sample_z_test_impl(ForwardIterator begin, ForwardIterator end, typename std::iterator_traits<ForwardIterator>::value_type assumed_mean)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;
    std::pair<Real, Real> temp = mean_and_sample_variance(begin, end);
    Real mu = std::get<0>(temp);
    Real s_sq = std::get<1>(temp);
    return one_sample_z_test_impl<ReturnType>(mu, s_sq, Real(std::distance(begin, end)), Real(assumed_mean));
}

template<typename ReturnType, typename T>
ReturnType two_sample_z_test_impl(T mean_1, T variance_1, T size_1, T mean_2, T variance_2, T size_2)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;
    using std::sqrt;
    using no_promote_policy = boost::math::policies::policy<boost::math::policies::promote_float<false>, boost::math::policies::promote_double<false>>;

    Real test_statistic = (mean_1 - mean_2) / sqrt(variance_1/size_1 + variance_2/size_2);
    auto z = boost::math::normal_distribution<Real, no_promote_policy>(size_1 + size_2 - 1);
    Real pvalue;
    if(test_statistic > 0)
    {
        pvalue = 2*boost::math::cdf<Real>(z, -test_statistic);
    }
    else
    {
        pvalue = 2*boost::math::cdf<Real>(z, test_statistic);
    }

    return std::make_pair(test_statistic, pvalue);
}

template<typename ReturnType, typename ForwardIterator>
ReturnType two_sample_z_test_impl(ForwardIterator begin_1, ForwardIterator end_1, ForwardIterator begin_2, ForwardIterator end_2)
{
    using Real = typename std::tuple_element<0, ReturnType>::type;
    using std::sqrt;
    auto n1 = std::distance(begin_1, end_1);
    auto n2 = std::distance(begin_2, end_2);

    ReturnType temp_1 = mean_and_sample_variance(begin_1, end_1);
    Real mean_1 = std::get<0>(temp_1);
    Real variance_1 = std::get<1>(temp_1);

    ReturnType temp_2 = mean_and_sample_variance(begin_2, end_2);
    Real mean_2 = std::get<0>(temp_2);
    Real variance_2 = std::get<1>(temp_2);

    return two_sample_z_test_impl<ReturnType>(mean_1, variance_1, Real(n1), mean_2, variance_2, Real(n2));
}

} // detail

template<typename Real, typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline auto one_sample_z_test(Real sample_mean, Real sample_variance, Real sample_size, Real assumed_mean) -> std::pair<double, double>
{
    return detail::one_sample_z_test_impl<std::pair<double, double>>(sample_mean, sample_variance, sample_size, assumed_mean);
}

template<typename Real, typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline auto one_sample_z_test(Real sample_mean, Real sample_variance, Real sample_size, Real assumed_mean) -> std::pair<Real, Real>
{
    return detail::one_sample_z_test_impl<std::pair<Real, Real>>(sample_mean, sample_variance, sample_size, assumed_mean);
}

template<typename ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline auto one_sample_z_test(ForwardIterator begin, ForwardIterator end, Real assumed_mean) -> std::pair<double, double>
{
    return detail::one_sample_z_test_impl<std::pair<double, double>>(begin, end, assumed_mean);
}

template<typename ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline auto one_sample_z_test(ForwardIterator begin, ForwardIterator end, Real assumed_mean) -> std::pair<Real, Real>
{
    return detail::one_sample_z_test_impl<std::pair<Real, Real>>(begin, end, assumed_mean);
}

template<typename Container, typename Real = typename Container::value_type,
         typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline auto one_sample_z_test(Container const & v, Real assumed_mean) -> std::pair<double, double>
{
    return detail::one_sample_z_test_impl<std::pair<double, double>>(std::begin(v), std::end(v), assumed_mean);
}

template<typename Container, typename Real = typename Container::value_type,
         typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline auto one_sample_z_test(Container const & v, Real assumed_mean) -> std::pair<Real, Real>
{
    return detail::one_sample_z_test_impl<std::pair<Real, Real>>(std::begin(v), std::end(v), assumed_mean);
}

template<typename ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline auto two_sample_z_test(ForwardIterator begin_1, ForwardIterator end_1, ForwardIterator begin_2, ForwardIterator end_2) -> std::pair<double, double>
{
    return detail::two_sample_z_test_impl<std::pair<double, double>>(begin_1, end_1, begin_2, end_2);
}

template<typename ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline auto two_sample_z_test(ForwardIterator begin_1, ForwardIterator end_1, ForwardIterator begin_2, ForwardIterator end_2) -> std::pair<Real, Real>
{
    return detail::two_sample_z_test_impl<std::pair<Real, Real>>(begin_1, end_1, begin_2, end_2);
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<std::is_integral<Real>::value, bool>::type = true>
inline auto two_sample_z_test(Container const & u, Container const & v) -> std::pair<double, double>
{
    return detail::two_sample_z_test_impl<std::pair<double, double>>(std::begin(u), std::end(u), std::begin(v), std::end(v));
}

template<typename Container, typename Real = typename Container::value_type, typename std::enable_if<!std::is_integral<Real>::value, bool>::type = true>
inline auto two_sample_z_test(Container const & u, Container const & v) -> std::pair<Real, Real>
{
    return detail::two_sample_z_test_impl<std::pair<Real, Real>>(std::begin(u), std::end(u), std::begin(v), std::end(v));
}

}}} // boost::math::statistics

#endif // BOOST_MATH_STATISTICS_Z_TEST_HPP


#pragma once

#include <Eigen/Dense>

namespace ilqr
{

// Helper class to store the terms of the quadratic value function.
class QuadraticValue
{
public:
    // Inititalizes the Value function terms V_, G_, W_ to zeros.
    QuadraticValue(const int state_dim);

    Eigen::MatrixXd& V() { return V_; }
    Eigen::MatrixXd& G() { return G_; }
    double& W() { return W_; }

    const Eigen::MatrixXd& V() const { return V_; }
    const Eigen::MatrixXd& G() const { return G_; }
    const double& W() const { return W_; }

private:
    // Quadratic term [dim(x)] x [dim(x)]
    Eigen::MatrixXd V_;
    // Linear term [1] x [dim(x)]
    Eigen::MatrixXd G_;
    // Constant term [1] x [1]
    double W_;
};

// Dynamics Function Prototype. Takes state, control and returns the next state.
using DynamicsFunc = std::function<Eigen::VectorXd(const Eigen::VectorXd&,const Eigen::VectorXd&)>;

// Cost Function Prototype. Takes state, control and returns a real-valued cost.
using CostFunc = std::function<double(const Eigen::VectorXd&,const Eigen::VectorXd&)>;

// Linearized dynamics parameters in terms of the state [x].
struct Dynamics
{
    // Linear dynamics matrix. [dim(x)] x [dim(x)]
    // (extension is last row is [\vec{0}, 1])
    Eigen::MatrixXd A;

    // Controls matrix. [dim(x)] x [dim(u)]
    // (extension is last row is [\vec{0}])
    Eigen::MatrixXd B;
};

// Quadratic cost parameters in terms of the state x and control u.
struct Cost
{
    // Quadratic state-cost matrix. [dim(x)] x [dim(x)]
    Eigen::MatrixXd Q;

    // Cost cross-term matrix. [dim(x)] x [dim(u)]
    Eigen::MatrixXd P;

    // Quadratic control-cost matrix. [dim(u)] x [dim(u)]
    Eigen::MatrixXd R;

    // Linear control-cost  matrix. [dim(u)] x [1]
    Eigen::VectorXd g_u;

    // Linear control-cost matrix. [dim(x)] x [1]
    Eigen::VectorXd g_x;

    // Constant offset term
    double c;
};

ilqr::Dynamics linearize_dynamics(const DynamicsFunc &dynamics_func,
                        const Eigen::VectorXd &x,
                        const Eigen::VectorXd &u);

ilqr::Cost quadraticize_cost(const CostFunc &cost_func,
                        const Eigen::VectorXd &x,
                        const Eigen::VectorXd &u);

} // namespace ilqr


//  (C) Copyright Nick Thompson 2020.
//  Use, modification and distribution are 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)
//
// Deliberately contains some unicode characters:
//
// boost-no-inspect
//
#include <iostream>
#include <boost/math/constants/constants.hpp>
#include <boost/math/tools/centered_continued_fraction.hpp>
#include <boost/multiprecision/mpfr.hpp>

using boost::math::constants::root_two;
using boost::math::constants::phi;
using boost::math::constants::pi;
using boost::math::constants::e;
using boost::math::constants::zeta_three;
using boost::math::tools::centered_continued_fraction;
using boost::multiprecision::mpfr_float;

int main()
{
    using Real = mpfr_float;
    int p = 10000;
    mpfr_float::default_precision(p);
    auto phi_cfrac = centered_continued_fraction(phi<Real>());
    std::cout << "φ ≈ " << phi_cfrac << "\n";
    std::cout << "Khinchin mean: " << std::setprecision(10) << phi_cfrac.khinchin_geometric_mean() << "\n\n\n";

    auto pi_cfrac = centered_continued_fraction(pi<Real>());
    std::cout << "π ≈ " << pi_cfrac << "\n";
    std::cout << "Khinchin mean: " << std::setprecision(10) << pi_cfrac.khinchin_geometric_mean() << "\n\n\n";

    auto rt_cfrac = centered_continued_fraction(root_two<Real>());
    std::cout << "√2 ≈ " << rt_cfrac << "\n";
    std::cout << "Khinchin mean: " << std::setprecision(10) <<  rt_cfrac.khinchin_geometric_mean() << "\n\n\n";

    auto e_cfrac = centered_continued_fraction(e<Real>());
    std::cout << "e ≈ " << e_cfrac << "\n";
    std::cout << "Khinchin mean: " << std::setprecision(10) <<  e_cfrac.khinchin_geometric_mean() << "\n\n\n";

    auto z_cfrac = centered_continued_fraction(zeta_three<Real>());
    std::cout << "ζ(3) ≈ " << z_cfrac << "\n";
    std::cout << "Khinchin mean: " << std::setprecision(10) <<  z_cfrac.khinchin_geometric_mean() << "\n\n\n";

    // http://jeremiebourdon.free.fr/data/Khintchine.pdf
    std::cout << "The expected Khinchin mean for a random centered continued fraction is 5.45451724454\n";
}


/**
 * @file finitevolumesineconslaw_main.cc
 * @brief NPDE homework "FiniteVolumeSineConsLaw" code
 * @author Oliver Rietmann
 * @date 25.04.2019
 * @copyright Developed at ETH Zurich
 */

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

#include "finitevolumesineconslaw.h"

using namespace FiniteVolumeSineConsLaw;

const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                       Eigen::DontAlignCols, ", ", "\n");

int main() {
  /* SAM_LISTING_BEGIN_1 */
  unsigned int N = 600;
  unsigned int M = 200;
  double h = 12.0 / N;
  Eigen::VectorXd x =
      Eigen::VectorXd::LinSpaced(N, -6.0 + 0.5 * h, 6.0 - 0.5 * h);

  std::ofstream file;

  // without reaction term
  Eigen::VectorXd ufinal = solveSineConsLaw(&sineClawRhs, N, M);
#if SOLUTION
  file.open("ufinal.csv");
  file << x.transpose().format(CSVFormat) << std::endl;
  file << ufinal.transpose().format(CSVFormat) << std::endl;
  file.close();

  std::cout << "Generated " CURRENT_BINARY_DIR "/ufinal.csv" << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR "/plot.py " CURRENT_BINARY_DIR
              "/ufinal.csv " CURRENT_BINARY_DIR "/ufinal.eps");
#else
  //====================
  // Your code goes here
  // Use std::ofstream to write the solution to
  // the file "ufinal.csv". To plot this
  // file you may uncomment the following line:
  // std::system("python3 " CURRENT_SOURCE_DIR "/plot.py " CURRENT_BINARY_DIR
  // "/ufinal.csv " CURRENT_BINARY_DIR "/ufinal.eps");
  //====================
#endif
  /* SAM_LISTING_END_1 */

  // with reaction term: -c * u(x, t), where c = 1.0
  double c = 1.0;
  auto bind_c = [c](const Eigen::VectorXd &mu) {
    return sineClawReactionRhs(mu, c);
  };
  Eigen::VectorXd ufinal_reaction = solveSineConsLaw(bind_c, N, M);
#if SOLUTION
  file.open("ufinal_reaction.csv");
  file << x.transpose().format(CSVFormat) << std::endl;
  file << ufinal_reaction.transpose().format(CSVFormat) << std::endl;
  file.close();

  std::cout << "Generated " CURRENT_BINARY_DIR "/ufinal_reaction.csv"
            << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR "/plot.py " CURRENT_BINARY_DIR
              "/ufinal_reaction.csv " CURRENT_BINARY_DIR
              "/ufinal_reaction.eps");
#else
  //====================
  // Your code goes here
  // Use std::ofstream to write the solution to
  // the file "ufinal_reaction.csv". To plot this
  // file you may uncomment the following line:
  // std::system("python3 " CURRENT_SOURCE_DIR "/plot.py " CURRENT_BINARY_DIR
  // "/ufinal_reaction.csv " CURRENT_BINARY_DIR "/ufinal_reaction.eps");
  //====================
#endif

  // Finding the optimal timestep (no reaction term)
  unsigned int M_small = findTimesteps();
  std::cout
      << "The smallest number of timesteps keeping the solution in [0, 2] is "
      << M_small << std::endl;

  return 0;
}


#ifndef QUAD_TANH_SINH_RULE_HPP
#define QUAD_TANH_SINH_RULE_HPP

#include <tuple>

#include <Eigen/Core>

#include <mxpfit/math/constants.hpp>

namespace quad
{

//==============================================================================
// Double exponential formula for numerical integration
//==============================================================================

///
/// ### DESinhTanhRule
///
/// \brief Double exponential quadrature for finite interval [-1, 1]
/// \tparam T the scalar type for quadrature nodes and weights
///
/// This class computes nodes and weights of the sinh-tanh quadrature rule which
/// can efficiently compute the integral over the finite interval. ///
///
/// \f[
///   I = \int_{-1}^{1}f(x) dx \approx \sum_{k=N_{-}}^{N_{+}} w_{k} f(x_k)
/// \f]
///
/// This sinh-tanh rule is obtained by applying the change of variable in the
/// form
///
/// \f[
///   x = \phi(t)
///     = \tanh\left(\frac{\pi}{2}\sinh(t)\right) ,\quad t \in (-\infty,\infty).
/// \f]
///
/// and discretizing the integral over \f$ t \f$ by the trapezoidal rule of the
/// step width \f$h\f$. The integration nodes and weighs are given as
///
/// \f[
///   x_k = \tanh\left(\frac{\pi}{2}\sinh(kh)\right), \\
///   w_k =
///   \frac{\frac{1}{2}h\pi\cosh(kh)}{\cosh^\left(\frac{1}{2}\pi\sinh(kh)\right)}.
/// \f]
///
template <typename T>
class DESinhTanhRule
{
public:
    constexpr static const T alpha = math::constant<T>::pi / 2;

    using Scalar    = T;
    using ArrayType = Eigen::Array<T, Eigen::Dynamic, 1>;
    using Index     = Eigen::Index;

private:
    ArrayType m_node;            // nodes
    ArrayType m_weight;          // weights
    ArrayType m_dist_from_lower; // distance of nodes from lower bound
    ArrayType m_dist_from_upper; // distance of nodes from upper bound
    ArrayType m_adj_diff;        // adjacent difference of nodes
public:
    /// Default constructor
    DESinhTanhRule() = default;
    /// Copy constructor
    DESinhTanhRule(const DESinhTanhRule&) = default;
    /// Move constructor
    DESinhTanhRule(DESinhTanhRule&&) = default;
    /// Destructor
    ~DESinhTanhRule() = default;

    /// Copy assignment operator
    DESinhTanhRule& operator=(const DESinhTanhRule&) = default;
    /// Move assignment operator
    DESinhTanhRule& operator=(DESinhTanhRule&&) = default;

    ///
    /// Construst `n`-points quadrature rule
    ///
    /// \param[in] n  number of integration points
    /// \param[in] tiny A threshold value that the integration weights can be
    ///    negligibly small. If the integrand is a regular function at
    ///    boundaries,
    ///
    /// \pre `n > 2` is required
    ///
    explicit DESinhTanhRule(Index n, T tiny)
    {
        compute(n, tiny);
    }

    ///
    /// \return number of integration points.
    ///
    Index size() const
    {
        return m_node.size();
    }
    ///
    /// \return const reference to the vector of integration points.
    ///
    const ArrayType& x() const
    {
        return m_node;
    }

    ///
    /// Get a node
    ///
    /// \param[in] i  index of integration point. `i < size()` required.
    /// \return `i`-th integration point.
    ///
    Scalar x(Index i) const
    {
        assert(i < size());
        return m_node[i];
    }
    ///
    /// \return const reference to the vector of integration weights.
    ///
    const ArrayType& w() const
    {
        return m_weight;
    }

    ///
    /// Get a weight
    ///
    /// \parma[in] i  index of integration point. `i < size()` required.
    /// \return `i`-th integration weight.
    ///
    Scalar w(Index i) const
    {
        assert(i < size());
        return m_weight[i];
    }
    ///
    /// Get the distance of node from lower bound, \f$ x + 1, \f$ where \f$ x
    /// \in [-1,1].\f$
    ///
    Scalar distanceFromLower(Index i) const
    {
        assert(i < size());
        return m_dist_from_lower[i];
    }
    ///
    /// Get the distance of node from upper bound, \f$ 1 - x, \f $where \f$ x
    /// \in [-1,1].\f$
    ///
    Scalar distanceFromUpper(Index i) const
    {
        assert(i < size());
        return m_dist_from_upper[i];
    }
    ///
    /// Get the adjacent difference of nodes.
    ///
    /// \param[in] i index of nodes
    ///
    /// \return `x(0)` for `i=0`, `x(i)-x(i-1)` for `i=1,2,...,size()-1.`
    ///
    Scalar adjacentDifference(Index i) const
    {
        assert(i < size());
        return m_adj_diff[i];
    }

    ///
    /// Compute nodes and weights of the \c n point Gauss-Legendre
    /// quadrature rule. Nodes are located in the interval [-1,1].
    ///
    /// \param[in] n  number of integration points
    /// \param[in] tiny A threshold value that the integration weights can be
    ///    negligibly small. If the integrand is a regular function at
    ///    boundaries,
    ///
    /// \pre `n > 2` is required
    ///
    void compute(Index n, T tiny)
    {
        using Eigen::numext::log;
        assert(T() < tiny && tiny < T(1));

        resize(n);

        const auto t_bound =
            log(log(T(2) / (alpha * tiny) * log(T(2) / tiny)) / alpha);
        const auto h     = T(2) * t_bound / (n - 1);
        const auto w_pre = h * alpha;

        const auto sh_half = std::sinh(h / T(2));
        const auto ch_half = std::cosh(h / T(2));

        T di_pre;

        {
            const auto u0        = alpha * std::sinh(-t_bound);
            const auto d0        = std::cosh(u0);
            m_node[0]            = std::tanh(u0);
            m_weight[0]          = w_pre * std::cosh(-t_bound) / (d0 * d0);
            m_dist_from_lower[0] = std::exp(u0) / d0;
            m_dist_from_upper[0] = std::exp(-u0) / d0;
            m_adj_diff[0]        = m_node[0];

            di_pre = d0;
        }

        for (Index i = 1; i < n; ++i)
        {
            const auto ti = -t_bound + T(i) * h;
            const auto si = alpha * std::sinh(ti);
            const auto ci = alpha * std::cosh(ti);
            const auto di = std::cosh(si);

            m_node[i]            = std::tanh(si);
            m_weight[i]          = w_pre * std::cosh(ti) / (di * di);
            m_dist_from_lower[i] = std::exp(si) / di;
            m_dist_from_upper[i] = std::exp(-si) / di;
            // alpha * (sinh(t[i]) - sinh(t[i-1]))
            const auto ui_diff = T(2) * sh_half * (ci * ch_half - si * sh_half);
            m_adj_diff[i]      = std::sinh(ui_diff) / (di * di_pre);
            di_pre             = di;
        }

        return;
    }

    ///
    /// Inverse of variable transformation
    ///
    /// \return \f$ t = \phi^{-1}(x) \f$
    ///
    static T inverse(T x)
    {
        return std::asinh(std::atanh(x) / alpha);
    }

private:
    void resize(Index n)
    {
        m_node.resize(n);
        m_weight.resize(n);
        m_dist_from_lower.resize(n);
        m_dist_from_upper.resize(n);
        m_adj_diff.resize(n);
    }
};

} // namespace: quad

#endif /* QUAD_TANH_SINH_RULE_HPP */


/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <ros/ros.h>
#include <Eigen/Core>
#include <Eigen/SVD>

#include <cob_twist_controller/inverse_jacobian_calculations/inverse_jacobian_calculation.h>

/**
 * Calculates the pseudoinverse of the Jacobian by using SVD technique.
 * This allows to get information about singular values and evaluate them.
 */
Eigen::MatrixXd PInvBySVD::calculate(const Eigen::MatrixXd& jacobian) const
{
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);
    double eps_truncation = DIV0_SAFE;  // prevent division by 0.0
    Eigen::VectorXd singularValues = svd.singularValues();
    Eigen::VectorXd singularValuesInv = Eigen::VectorXd::Zero(singularValues.rows());

    // small change to ref: here quadratic damping due to Control of Redundant Robot Manipulators : R.V. Patel, 2005, Springer [Page 13-14]
    for (uint32_t i = 0; i < singularValues.rows(); ++i)
    {
        double denominator = singularValues(i) * singularValues(i);
        // singularValuesInv(i) = (denominator < eps_truncation) ? 0.0 : singularValues(i) / denominator;
        singularValuesInv(i) = (singularValues(i) < eps_truncation) ? 0.0 : singularValues(i) / denominator;
    }
    Eigen::MatrixXd result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();

    return result;
}

/**
 * Calculates the pseudoinverse of the Jacobian by using SVD technique.
 * This allows to get information about singular values and evaluate them.
 */
Eigen::MatrixXd PInvBySVD::calculate(const TwistControllerParams& params,
                                     boost::shared_ptr<DampingBase> db,
                                     const Eigen::MatrixXd& jacobian) const
{
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);
    double eps_truncation = params.eps_truncation;
    Eigen::VectorXd singularValues = svd.singularValues();
    Eigen::VectorXd singularValuesInv = Eigen::VectorXd::Zero(singularValues.rows());
    Eigen::MatrixXd lambda = db->getDampingFactor(singularValues, jacobian);

    if (params.numerical_filtering)
    {
        // Formula 20 Singularity-robust Task-priority Redundandancy Resolution
        // Sum part
        for (uint32_t i = 0; i < singularValues.rows()-1; ++i)
        {
            // pow(beta, 2) << pow(lambda, 2)
            singularValuesInv(i) = singularValues(i) / (pow(singularValues(i), 2) + pow(params.beta, 2));
        }
        // Formula 20 - additional part - numerical filtering for least singular value m
        uint32_t m = singularValues.rows()-1;
        singularValuesInv(m) = singularValues(m) / (pow(singularValues(m), 2) + pow(params.beta, 2) + lambda(m, m));
    }
    else
    {
        // small change to ref: here quadratic damping due to Control of Redundant Robot Manipulators : R.V. Patel, 2005, Springer [Page 13-14]
        for (uint32_t i = 0; i < singularValues.rows(); ++i)
        {
            double denominator = (singularValues(i) * singularValues(i) + lambda(i, i) );
            // singularValuesInv(i) = (denominator < eps_truncation) ? 0.0 : singularValues(i) / denominator;
            singularValuesInv(i) = (singularValues(i) < eps_truncation) ? 0.0 : singularValues(i) / denominator;
        }

        //// Formula from Advanced Robotics : Redundancy and Optimization : Nakamura, Yoshihiko, 1991, Addison-Wesley Pub. Co [Page 258-260]
        // for(uint32_t i = 0; i < singularValues.rows(); ++i)
        // {
        //       // damping is disabled due to damping factor lower than a const. limit
        //       singularValues(i) = (singularValues(i) < eps_truncation) ? 0.0 : 1.0 / singularValues(i);
        // }
    }

    Eigen::MatrixXd result = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();

    return result;
}

/**
 * Calculates the pseudoinverse by means of left/right pseudo inverse respectively.
 */
Eigen::MatrixXd PInvDirect::calculate(const Eigen::MatrixXd& jacobian) const
{
    Eigen::MatrixXd result;
    Eigen::MatrixXd jac_t = jacobian.transpose();
    uint32_t rows = jacobian.rows();
    uint32_t cols = jacobian.cols();

    if (cols >= rows)
    {
        result = jac_t * (jacobian * jac_t).inverse();
    }
    else
    {
        result = (jac_t * jacobian).inverse() * jac_t;
    }

    return result;
}

/**
 * Calculates the pseudoinverse by means of left/right pseudo inverse respectively.
 */
Eigen::MatrixXd PInvDirect::calculate(const TwistControllerParams& params,
                                      boost::shared_ptr<DampingBase> db,
                                      const Eigen::MatrixXd& jacobian) const
{
    Eigen::MatrixXd result;
    Eigen::MatrixXd jac_t = jacobian.transpose();
    uint32_t rows = jacobian.rows();
    uint32_t cols = jacobian.cols();
    if (params.damping_method == LEAST_SINGULAR_VALUE)
    {
        ROS_ERROR("PInvDirect does not support SVD. Use PInvBySVD class instead!");
    }

    Eigen::MatrixXd lambda = db->getDampingFactor(Eigen::VectorXd::Zero(1, 1), jacobian);
    if (cols >= rows)
    {
        Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(rows, rows);
        Eigen::MatrixXd temp = jacobian * jac_t + lambda * ident;
        result = jac_t * temp.inverse();
    }
    else
    {
        Eigen::MatrixXd ident = Eigen::MatrixXd::Identity(cols, cols);
        Eigen::MatrixXd temp = jac_t * jacobian + lambda * ident;
        result = temp.inverse() * jac_t;
    }

    return result;
}


#include <iostream>
#include <cmath>
#include <vector>
#include <Eigen/Eigen>

int binomial(int n, int k) {
//    if (c < 0)
//        std::cout << "c: " << c << std::endl;
//    if (n < 0)
//        std::cout << "n: " << n << std::endl;
    if (k > n)
        return 0;
    if (k == 0 || k == n)
        return 1;
    return binomial(n-1, k-1) + binomial(n-1, k);
}

int factorial(int n) {
    assert(n >= 0);
    return (n == 1 || n == 0) ? 1 : factorial(n-1) * n;
}

double k_pfac(double pfac, int n, int l, int m, double alpha,
         double beta, double gamma) {
    if (pfac == 0.0)
        return 0;
    double sum_term = 0.0;
    //std::cout << "n,l,m: " << n << "," << l << "," << m << std::endl;
    //std::cout << "alpha: " << alpha << " beta: " << beta << " gamma: " << gamma << std::endl;
    for (std::size_t a = 0; a != (n+2); ++a) {
        for (std::size_t b = 0; b != (l+2); ++b) {
            for (std::size_t c = 0; c != (m+2); ++c) {
                double sum_term_part = binomial(l + 1 - b + a, a) *
                            binomial(m + 1 - c + b, b) * binomial(n + 1 - a + c, c)
                            / (pow((alpha + beta)*2, l - b + a + 2)
                               * pow((alpha + gamma)*2, n - a + c + 2)
                               * pow((beta + gamma)*2, m - c + b + 2));
                //std::cout << "a,b,c: " << a << "," << b << "," << c << " sum term contrib: " << sum_term_part << std::endl;
                sum_term += sum_term_part;
            }
        }
    }

    return pfac * 16.0 * M_PI * M_PI * factorial(n+1) * factorial(l+1) * factorial(m+1) * sum_term;
}

double k(int n, int l, int m, double alpha,
        double beta, double gamma) {

    double sum_term = 0.0;
    for (std::size_t a = 0; a != n+2; ++a) {
        for (std::size_t b = 0; b != l+2; ++b) {
            for (std::size_t c = 0; c != m+2; ++c) {
                sum_term += binomial(l + 1 - b + a, a) *
                    binomial(m + 1 - c + b, b) * binomial(n + 1 - a + c, c)
                    / (pow((alpha + beta)*2, l - b + a + 2)
                    * pow((alpha + gamma)*2, n - a + c + 2)
                    * pow((beta + gamma)*2, m - c + b + 2));
            }
        }
    }

    return 16.0 * M_PI * M_PI * factorial(n+1) * factorial(l+1) * factorial(m+1) * sum_term;
}

double s(int ni, int li, int mi, int nj,
        int lj, int mj, double alpha, double beta, double gamma) {
    //std::cout << "ni: " << ni << " nj: " << nj << " li: " << li << " lj: " << lj << " mi: " <<
    //mi << " mj: " << mj << std::endl;
    //std::cout << "alpha: " << alpha << " beta: " << beta << " gamma: " << gamma << std::endl;
    return k_pfac(1, ni + nj, li + lj, mi + mj, alpha, beta, gamma);
}

double v_ne(int ni, int li, int mi, int nj,
        int lj, int mj, double alpha, double beta, double gamma,
        int z) {
    return -z * (k(ni + nj - 1, li + lj, mi + mj, alpha, beta, gamma)
        + k(ni + nj, li + lj - 1, mi + mj, alpha, beta, gamma));
}

double v_ee(int ni, int li, int mi, int nj,
        int lj, int mj, double alpha, double beta, double gamma) {
    return k(ni + nj, li + lj, mi + mj - 1, alpha, beta, gamma);
}

double t_e(int ni, int li, int mi, int nj,
           int lj, int mj, double alpha, double beta, double gamma) {
    return -((alpha*2.0) * (alpha*2.0) + (beta*2.0) * (beta*2.0) + 2.0 * (gamma*2.0) * (gamma*2.0)) * s(ni, li, mi, nj, lj, mj, alpha, beta, gamma) / 8.0
        + (nj * (alpha*2.0) / 2.0) * k(ni + nj - 1, li + lj, mi + mj, alpha, beta, gamma)
        + (lj * (beta*2.0) / 2.0) * k(ni + nj, li + lj - 1, mi + mj, alpha, beta, gamma)
        + (mj * (gamma*2.0)) * k(ni + nj, li + lj, mi + mj - 1, alpha, beta, gamma)
//        - (nj * (nj - 1) / 2) * k(ni + nj - 2, li + lj, mi + mj, alpha, beta, gamma)
        - k_pfac((nj * (nj - 1) / 2.0), ni + nj - 2, li + lj, mi + mj, alpha, beta, gamma)
//        - (lj * (lj - 1) / 2) * k(ni + nj, li + lj - 2, mi + mj, alpha, beta, gamma)
        - k_pfac((lj * (lj - 1) / 2.0), ni + nj, li + lj - 2, mi + mj, alpha, beta, gamma)
//
        - k_pfac((mj * (mj - 1)), ni + nj, li + lj, mi + mj - 2, alpha, beta, gamma)
        + ((alpha*2.0) / 2.0) * k(ni + nj - 1, li + lj, mi + mj, alpha, beta, gamma)
        + ((beta*2.0) / 2.0) * k(ni + nj, li + lj - 1, mi + mj, alpha, beta, gamma)
        + ((gamma*2.0)) * k(ni + nj, li + lj, mi + mj - 1, alpha, beta, gamma)
//        - (nj) * k(ni + nj - 2, li + lj, mi + mj, alpha, beta, gamma)
        - k_pfac((nj), ni + nj - 2, li + lj, mi + mj, alpha, beta, gamma)
//        - (lj) * k(ni + nj, li + lj - 2, mi + mj, alpha, beta, gamma)
        - k_pfac((lj), ni + nj, li + lj - 2, mi + mj, alpha, beta, gamma)
//        - (2 * mj) * k(ni + nj, li + lj, mi + mj - 2, alpha, beta, gamma)
        - k_pfac((2.0 * mj), ni + nj, li + lj, mi + mj - 2, alpha, beta, gamma)
        - ((alpha*2.0) * (gamma*2.0) / 8.0) * (k(ni + nj - 1, li + lj, mi + mj + 1, alpha, beta, gamma)
            + k(ni + nj + 1, li + lj, mi + mj - 1, alpha, beta, gamma)
            - k(ni + nj - 1, li + lj + 2, mi + mj - 1, alpha, beta, gamma))
        - ((beta*2.0) * (gamma*2.0) / 8.0) * (k(ni + nj, li + lj - 1, mi + mj + 1, alpha, beta, gamma)
            + k(ni + nj, li + lj + 1, mi + mj - 1, alpha, beta, gamma)
            - k(ni + nj + 2, li + lj - 1, mi + mj - 1, alpha, beta, gamma))
//        + (nj * gamma / 4) * (k(ni + nj - 2, li + lj, mi + mj + 1, alpha, beta, gamma))
        + (k_pfac((nj * (gamma*2.0) / 4.0), ni + nj - 2, li + lj, mi + mj + 1, alpha, beta, gamma)
            + k_pfac((nj * (gamma*2.0) / 4.0), ni + nj, li + lj, mi + mj - 1, alpha, beta, gamma)
            - k_pfac((nj * (gamma*2.0) / 4.0), ni + nj - 2, li + lj + 2, mi + mj - 1, alpha, beta, gamma))
//        + (mj * alpha / 4) * (k(ni + nj - 1, li + lj, mi + mj, alpha, beta, gamma)
        + (k_pfac((mj * (alpha*2.0) / 4.0), ni + nj - 1, li + lj, mi + mj, alpha, beta, gamma)
            + k_pfac((mj * (alpha*2.0) / 4.0), ni + nj + 1, li + lj, mi + mj - 2, alpha, beta, gamma)
            - k_pfac((mj * (alpha*2.0) / 4.0), ni + nj - 1, li + lj + 2, mi + mj - 2, alpha, beta, gamma))
//        - (nj * mj / 2) * (k(ni + nj - 2, li + lj, mi + mj + 1, alpha, beta, gamma)
        - (k_pfac((nj * mj / 2.0), ni + nj - 2, li + lj, mi + mj, alpha, beta, gamma)
            + k_pfac((nj * mj / 2.0), ni + nj, li + lj, mi + mj - 2, alpha, beta, gamma)
            - k_pfac((nj * mj / 2.0), ni + nj - 2, li + lj + 2, mi + mj - 2, alpha, beta, gamma))
//        + (lj * gamma / 4) * (k(ni + nj, li + lj - 2, mi + mj + 1, alpha, beta, gamma)
        + (k_pfac((lj * (gamma*2.0) / 4.0), ni + nj, li + lj - 2, mi + mj + 1, alpha, beta, gamma)
            + k_pfac((lj * (gamma*2.0) / 4.0), ni + nj, li + lj, mi + mj - 1, alpha, beta, gamma)
            - k_pfac((lj * (gamma*2.0) / 4.0), ni + nj + 2, li + lj - 2, mi + mj - 1, alpha, beta, gamma))
//        + (mj * beta / 4) * (k(ni + nj, li + lj - 1, mi + mj, alpha, beta, gamma)
        + (k_pfac((mj * (beta*2.0) / 4.0), ni + nj, li + lj - 1, mi + mj, alpha, beta, gamma)
            + k_pfac((mj * (beta*2.0) / 4.0), ni + nj, li + lj + 1, mi + mj - 2, alpha, beta, gamma)
            - k_pfac((mj * (beta*2.0) / 4.0), ni + nj + 2, li + lj - 1, mi + mj - 2, alpha, beta, gamma))
//        - (lj * mj / 2) * (k(ni + nj, li + lj - 2, mi + mj, alpha, beta, gamma)
        - (k_pfac((lj * mj / 2.0), ni + nj, li + lj - 2, mi + mj, alpha, beta, gamma)
            + k_pfac((lj * mj / 2.0), ni + nj, li + lj, mi + mj - 2, alpha, beta, gamma)
            - k_pfac((lj * mj / 2.0), ni + nj + 2, li + lj - 2, mi + mj - 2, alpha, beta, gamma));
}

Eigen::MatrixXd s_matrix_builder(std::vector<std::pair<std::vector<int>, std::vector<double> > > basis) {
    Eigen::MatrixXd result;
    result.setZero(basis.size(), basis.size());
    for (std::size_t i = 0; i != basis.size(); ++i) {
        for (std::size_t j = 0; j != basis.size(); ++j) {
            //std::vector<int> q_nos_i, q_nos_j;
            //std::vector<double> exp_i, exp_j;
            //auto [q_nos_i, exp_i] = basis[i];
            //auto [q_nos_j, exp_j] = basis[j];
            std::vector<int> q_nos_i = basis[i].first;
            std::vector<double> exp_i = basis[i].second;
            std::vector<int> q_nos_j = basis[j].first;
            //std::cout << "S(" << i << "," << j << ") = s(" << q_nos_i[0] << "," <<  q_nos_i[1] << "," << q_nos_i[2] << "," << q_nos_j[0] << "," <<  q_nos_j[1] << "," << q_nos_j[2] << "," << exp_i[0] << "," << exp_i[1] << "," << exp_i[2] << std::endl;
            result(i,j) = s(q_nos_i[0], q_nos_i[1], q_nos_i[2], q_nos_j[0], q_nos_j[1], q_nos_j[2], exp_i[0], exp_i[1], exp_i[2]);
        }
    }
    return result;
}

Eigen::MatrixXd h_matrix_builder(std::vector<std::pair<std::vector<int>, std::vector<double> > > basis, int z) {
    Eigen::MatrixXd result;
    result.setZero(basis.size(), basis.size());
    for (std::size_t i = 0; i != basis.size(); ++i) {
        for (std::size_t j = 0; j != basis.size(); ++j) {
            //std::vector<int> q_nos_i, q_nos_j;
            //std::vector<double> exp_i, exp_j;
            //auto [q_nos_i, exp_i] = basis[i];
            //auto [q_nos_j, exp_j] = basis[j];
            std::vector<int> q_nos_i = basis[i].first;
            std::vector<double> exp_i = basis[i].second;
            std::vector<int> q_nos_j = basis[j].first;
            result(i,j) = (v_ne(q_nos_i[0], q_nos_i[1], q_nos_i[2], q_nos_j[0], q_nos_j[1], q_nos_j[2], exp_i[0], exp_i[1], exp_i[2], z)
                + v_ee(q_nos_i[0], q_nos_i[1], q_nos_i[2], q_nos_j[0], q_nos_j[1], q_nos_j[2], exp_i[0], exp_i[1], exp_i[2])
                + t_e(q_nos_i[0], q_nos_i[1], q_nos_i[2], q_nos_j[0], q_nos_j[1], q_nos_j[2], exp_i[0], exp_i[1], exp_i[2]));
//            if (result(i,j) == nan(0)) {
//                result(i,j) = 0;
//            }
        }
    }
    return result;
}

//template<typename T,typename U>
//std::pair<T, U>
std::pair<Eigen::MatrixXd, Eigen::MatrixXd> hylleraas(std::vector<std::pair<std::vector<int>, std::vector<double> > > basis, int z) {
    auto H = h_matrix_builder(basis, z);
    auto S = s_matrix_builder(basis);

    Eigen::GeneralizedEigenSolver<Eigen::MatrixXd> ges;
    ges.compute(H,S);

    Eigen::MatrixXd evecs = ges.eigenvectors().real();
    Eigen::MatrixXd evals = ges.eigenvalues().real();

    return std::pair(evecs, evals);
}

std::vector<std::pair<std::vector<int>, std::vector<double> > > generate_basis(std::size_t N, double alpha, double gamma) {
    std::vector<std::pair<std::vector<int>, std::vector<double> > > result;
    for (int n = 0; n != N; ++n) {
        for (int l = 0; l != (N-n); ++l) {
            for (int m = 0; m != (N-n-l); ++m) {
                if ((n + l + m) <= N)
                    result.push_back(std::pair<std::vector<int>, std::vector<double>>{std::vector<int>{n,l,m}, std::vector<double>{alpha, alpha, gamma}});
            }
        }
    }
    return result;
}

template<typename T,typename U>
std::pair<T, U> hylleraas_generic(int N, int z, double alpha, double gamma) {
    auto basis = generate_basis(N, alpha, gamma);
    auto [evecs, evals] = hylleraas(basis, z); //<Eigen::MatrixXd, Eigen::MatrixXd>
    return std::pair(evecs, evals);
}

int main() {
    std::vector<std::pair<std::vector<int>, std::vector<double> > > basis =
            {std::pair(std::vector<int>{0,0,0}, std::vector<double>{1.6875, 1.6875, 0.0})};
    Eigen::MatrixXd S = s_matrix_builder(basis);
    std::cout << "S with single basis fn: \n" << S << std::endl;

    Eigen::MatrixXd H = h_matrix_builder(basis, 2);
    std::cout << "H with single basis fn: \n" << H << std::endl;

//    double k_test = k(0, 0, 0, 0.5, 0.5, 0.5);
//    std::cout << "k(-1,-1,-1,0.5,0.5,0.5): " << k_test << std::endl;

//    double k_test_pfac = k_pfac(1,0, 0, 0, 1, 1, 1);
//    std::cout << "k(1,0,0,0,1,1,1): " << k_test_pfac << std::endl;
//    double k_test_pfac2 = k_pfac(1,0, 0, 0, 1.6875, 1.6875, 0);
//    std::cout << "k(1,0,0,0,1.6875,1.6875,0): " << k_test_pfac2 << std::endl;

//    int binom_test = binomial(2,1);
//    std::cout << "binom test C(2,1)\n" << binom_test;

    std::vector<std::pair<std::vector<int>, std::vector<double> > > func3_basis =
            {std::pair(std::vector<int>{0,0,0}, std::vector<double>{1.8, 1.8, 0.0}), std::pair(std::vector{1,1,0}, std::vector{1.8, 1.8, 0.0}), std::pair(std::vector{0,0,1}, std::vector{1.8, 1.8, 0.0})};
    Eigen::MatrixXd S_3func = s_matrix_builder(func3_basis);
    std::cout << "S_3func: \n" << S_3func << std::endl;

    Eigen::MatrixXd H_3func = h_matrix_builder(func3_basis, 2);
    std::cout << "H_3func: \n" << H_3func << std::endl;

    auto [evecs_3func, evals_3func] = hylleraas(func3_basis, 2);
    std::cout << "3basis evecs: \n" << evecs_3func << "\n\n3basis evals: \n" << evals_3func << std::endl;

    //hylleraas generic
    auto [evecs_gen_func, evals_gen_func] = hylleraas_generic<Eigen::MatrixXd, Eigen::MatrixXd>(3, 2, 1.8, 0.0);
    std::cout << "gen basis evecs: \n" << evecs_gen_func << "\n\ngen basis evals: \n" << evals_gen_func << std::endl;

//    std::vector<double> evals; //(15);
//    for (size_t i = 1; i != 16; ++i) {
//        auto [evecs_gen_func, evals_gen_func] = hylleraas_generic<Eigen::MatrixXd, Eigen::MatrixXd>(i, 2, 1.8, 0.0);
//
//        evals.push_back(evals_gen_func.minCoeff());
//        std::cout << "Completed " << i << "th computation: " << evals_gen_func.minCoeff() << "\n";
//    }
//
//    for (std::size_t i = 1; i != 16; ++i) {
//        std::cout << "He energy computed with i=" << i << ": " << evals[i] << "\n";
//    }

    auto [evecs_7_func, evals_7_func] = hylleraas_generic<Eigen::MatrixXd, Eigen::MatrixXd>(8, 2, 1.8, 0.0);
    std::cout << "7-basis evecs: \n" << evecs_7_func << "\n\n7-basis evals: \n" << evals_7_func << std::endl;

    auto [evecs_8_func, evals_8_func] = hylleraas_generic<Eigen::MatrixXd, Eigen::MatrixXd>(8, 2, 1.8, 0.0);
    std::cout << "8-basis evecs: \n" << evecs_8_func << "\n\n8-basis evals: \n" << evals_8_func << std::endl;

    return 0;
}


/******************************************************************************
 * Copyright (C) 2014 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

#include "aslam/calibration/car/geo/geodetic.h"

#include <Eigen/Core>

#include <cmath>

#include <sm/kinematics/quaternion_algebra.hpp>

using namespace sm::kinematics;

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

    sm::kinematics::Transformation ecef2enu(double x, double y, double z, double
        latitude, double longitude) {
      return enu2ecef(x, y, z, latitude, longitude).inverse();
    }

    sm::kinematics::Transformation enu2ecef(double x, double y, double z, double
        latitude, double longitude) {

      const double slat = std::sin(latitude);
      const double clat = std::cos(latitude);
      const double slong = std::sin(longitude);
      const double clong = std::cos(longitude);

      Eigen::Matrix3d enu_R_ecef;
      enu_R_ecef << -slong, clong, 0,
        -slat * clong, -slat * slong, clat,
        clat * clong, clat * slong, slat;

      return Transformation(
        r2quat(enu_R_ecef.transpose()), Eigen::Vector3d(x, y, z));
    }

    sm::kinematics::Transformation ecef2ned(double x, double y, double z, double
        latitude, double longitude) {
      return ned2ecef(x, y, z, latitude, longitude).inverse();
    }

    sm::kinematics::Transformation ned2ecef(double x, double y, double z, double
        latitude, double longitude) {

      const double slat = std::sin(latitude);
      const double clat = std::cos(latitude);
      const double slong = std::sin(longitude);
      const double clong = std::cos(longitude);

      Eigen::Matrix3d ned_R_ecef;
      ned_R_ecef << -slat * clong, -slat * slong, clat,
        -slong, clong, 0,
        -clat * clong, -clat * slong, -slat;

      return Transformation(
        r2quat(ned_R_ecef.transpose()), Eigen::Vector3d(x, y, z));
    }

    void wgs84ToEcef(double latitude, double longitude, double altitude,
        double& x, double& y, double& z) {
      const double slat = std::sin(latitude);
      const double clat = std::cos(latitude);
      const double slong = std::sin(longitude);
      const double clong = std::cos(longitude);
      const double a = 6378137;
      const double e2 = 0.006694380004260827;
      const double R = a / std::sqrt(1 - e2 * slat * slat);
      x = (R + altitude) * clat * clong;
      y = (R + altitude) * clat * slong;
      z = (R * (1 - e2) + altitude) * slat;
    }

  }
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef PLANARQUAD_DYNAMICS_HPP_
#define PLANARQUAD_DYNAMICS_HPP_

#include <Eigen/Dense>

// template here over some type
template<typename T1, typename T2>
auto planarquad_dynamics(const Eigen::MatrixBase<T1> & x, const Eigen::MatrixBase<T2> & u)
{
  // Define nx, nu
  constexpr auto nx = T1::RowsAtCompileTime;
  constexpr auto nu = T2::RowsAtCompileTime;

  // Initialize Parameters
  constexpr double I = 0.2;
  constexpr double m = 0.5;
  constexpr double r = 0.25;
  constexpr double gr = 9.81;

  using T = typename decltype(x * u.transpose())::EvalReturnType::Scalar;

  // xDot = f(x) + g(x)*u
  Eigen::Matrix<T, nx, 1> xDot;
  Eigen::Matrix<typename T1::Scalar, nx, 1> f;

  Eigen::Matrix<typename T1::Scalar, nx, nu> g;
  Eigen::Map<Eigen::Matrix<typename T1::Scalar, nx, 1>> g1(g.data());
  Eigen::Map<Eigen::Matrix<typename T1::Scalar, nx, 1>> g2(g.data() + nx);

  // Define States
  // const auto & X = x[0];
  // const auto & Y = x[1];
  const auto & theta = x[2];

  // Dynamics
  f[0] = x[3];
  f[1] = x[4];
  f[2] = x[5];
  f[3] = 0.;
  f[4] = -gr;
  f[5] = 0.;

  g1[0] = 0;                g2[0] = 0;
  g1[1] = 0;                g2[1] = 0;
  g1[2] = 0;                g2[2] = 0;
  g1[3] = -sin(theta) / m;  g2[3] = -sin(theta) / m;
  g1[4] = cos(theta) / m;   g2[4] = cos(theta) / m;
  g1[5] = r / I;            g2[5] = -r / I;

  xDot = f + g * u;

  return xDot;
}

#endif  // PLANARQUAD_DYNAMICS_HPP_


#include <iomanip>
#include <iostream>
#include <boost/math/constants/constants.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

using big_float = boost::multiprecision::cpp_dec_float_100;

big_float f(unsigned int n) {
    big_float pi(boost::math::constants::pi<big_float>());
    return exp(sqrt(big_float(n)) * pi);
}

int main() {
    std::cout << "Ramanujan's constant using formula f(N) = exp(pi*sqrt(N)):\n"
        << std::setprecision(80) << f(163) << '\n';
    std::cout << "\nResult with last four Heegner numbers:\n";
    std::cout << std::setprecision(30);
    for (unsigned int n : {19, 43, 67, 163}) {
        auto x = f(n);
        auto c = ceil(x);
        auto pc = 100.0 * (x/c);
        std::cout << "f(" << n << ") = " << x << " = "
            << pc << "% of " << c << '\n';
    }
    return 0;
}


#include "UtilEOL.h"

#include "external\ArcSim\geometry.hpp"

#include <iostream>

#include <Eigen\SVD>
#include <unsupported/Eigen/MatrixFunctions>

using namespace std;
using namespace Eigen;

MatrixXd deform_grad(const Face *f)
{
	MatrixXd Dx(3, 2);
	Dx(0, 0) = f->v[1]->node->x[0] - f->v[0]->node->x[0];
	Dx(0, 1) = f->v[2]->node->x[0] - f->v[0]->node->x[0];
	Dx(1, 0) = f->v[1]->node->x[1] - f->v[0]->node->x[1];
	Dx(1, 1) = f->v[2]->node->x[1] - f->v[0]->node->x[1];
	Dx(2, 0) = f->v[1]->node->x[2] - f->v[0]->node->x[2];
	Dx(2, 1) = f->v[2]->node->x[2] - f->v[0]->node->x[2];
	Matrix2d DX;
	DX(0, 0) = f->v[1]->u[0] - f->v[0]->u[0];
	DX(0, 1) = f->v[2]->u[0] - f->v[0]->u[0];
	DX(1, 0) = f->v[1]->u[1] - f->v[0]->u[1];
	DX(1, 1) = f->v[2]->u[1] - f->v[0]->u[1];
	return Dx * DX.inverse();
}

MatrixXd deform_grad_v(const Vert* v)
{
	double tot_ang = 0.0;
	Matrix3d Q = Matrix3d::Zero();
	Matrix2d P = Matrix2d::Zero();

	for (int f = 0; f < v->adjf.size(); f++) {
		Face* face = v->adjf[f];

		MatrixXd F = deform_grad(face);
		JacobiSVD<MatrixXd> svd(F, ComputeFullU | ComputeFullV);

		MatrixXd V3(2, 3);
		V3 << svd.matrixV(), Vector2d::Zero();

		MatrixXd Qx = svd.matrixU() * V3.transpose();
		Matrix3d Qxrot;
		Qxrot << Qx.col(0), Qx.col(1), Qx.block<3,1>(0,0).cross(Qx.block<3,1>(0,1));

		Matrix2d S2;
		S2 << svd.singularValues(), svd.singularValues();

		Matrix2d Px = svd.matrixV() * S2 * svd.matrixV().transpose();

		Q += incedent_angle(v, face) * Qxrot.log();

		P += incedent_angle(v, face) * Px;

		tot_ang += incedent_angle(v, face);
	}

	Q /= tot_ang;
	Q = Q.exp();

	P /= tot_ang;

	return Q.block<3, 2>(0, 0) * P;
}


#include <OGRE/OgreVector3.h>
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>

#include <ros/console.h>
#include <rviz/ogre_helpers/axes.h>
#include <rviz/ogre_helpers/shape.h>

#include <Eigen/Dense>

#include "covariance_visual.h"

template<typename Derived>
inline bool isfinite(const Eigen::MatrixBase<Derived>& x)
{
  return ( (x - x).array() == (x - x).array()).all();
}

template<typename Derived>
inline bool isnan(const Eigen::MatrixBase<Derived>& x)
{
  return !((x.array() == x.array())).all();
}

std::pair<Eigen::Matrix3d, Eigen::Vector3d> computeEigenValuesAndVectors(const geometry_msgs::PoseWithCovariance& msg,  unsigned offset)
{
  Eigen::Matrix3d covariance   = Eigen::Matrix3d::Zero();
  Eigen::Vector3d eigenValues  = Eigen::Vector3d::Identity();
  Eigen::Matrix3d eigenVectors = Eigen::Matrix3d::Zero();

  for (size_t i = 0; i < 3; ++i)
  {
    for (size_t j = 0; j < 3; ++j)
    {
      covariance (i, j) = msg.covariance[(i + offset) * 6 + j + offset];
    }
  }

  // Compute eigen values and eigen vectors.
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(covariance);

  if (eigensolver.info() == Eigen::Success)
  {
    eigenValues  = eigensolver.eigenvalues();
    eigenVectors = eigensolver.eigenvectors();
  }
  else
  {
    ROS_WARN_THROTTLE(1, "Failed to compute eigen vectors/values. Is the covariance matrix correct?");
  }

  return std::make_pair(eigenVectors, eigenValues);
}

Ogre::Quaternion computeRotation(const geometry_msgs::PoseWithCovariance& msg, std::pair<Eigen::Matrix3d, Eigen::Vector3d>& pair)
{
  Ogre::Matrix3 rotation;

  for (size_t i = 0; i < 3; ++i)
  {
    for (size_t j = 0; j < 3; ++j)
    {
      rotation[i][j] = pair.first(i, j);
    }
  }

  return Ogre::Quaternion(rotation);
}

namespace rviz_plugin_covariance
{

CovarianceVisual::CovarianceVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node)
  : frame_node_(parent_node->createChildSceneNode())
  , scene_manager_(scene_manager)
  , scale_(1.0)
{
  axes_.reset(new rviz::Axes(scene_manager_, frame_node_));

  position_node_    = axes_->getSceneNode()->createChildSceneNode();
  orientation_node_ = axes_->getSceneNode()->createChildSceneNode();

  position_shape_.reset(new rviz::Shape(rviz::Shape::Sphere, scene_manager_, position_node_));
  orientation_shape_.reset(new rviz::Shape(rviz::Shape::Cone, scene_manager_, orientation_node_));

  axes_->getSceneNode()->setVisible(show_axis_);
  position_node_->setVisible(show_position_);
  orientation_node_->setVisible(use_6dof_ && show_orientation_);
}

CovarianceVisual::~CovarianceVisual()
{
  scene_manager_->destroySceneNode(orientation_node_);
  scene_manager_->destroySceneNode(position_node_);
  scene_manager_->destroySceneNode(frame_node_);
}

void CovarianceVisual::setMessage(const geometry_msgs::PoseWithCovariance& msg)
{
  // Construct pose position and orientation.
  const geometry_msgs::Point& p = msg.pose.position;
  Ogre::Vector3 position(p.x, p.y, p.z);
  Ogre::Quaternion orientation(msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z);

  // Set position and orientation for axes scene node.
  if (!position.isNaN())
  {
    axes_->setPosition(position);
  }
  else
  {
    ROS_WARN_STREAM_THROTTLE(1, "Position contains NaN: " << position);
  }

  if (!orientation.isNaN())
  {
    axes_->setOrientation(orientation);
  }
  else
  {
    ROS_WARN_STREAM_THROTTLE(1, "Orientation contains NaN: " << orientation);
  }

  if (use_6dof_)
  {
    // Check for NaN in covariance
    for (size_t i = 0; i < 3; ++i)
    {
      if (isnan(msg.covariance[i]))
      {
        ROS_WARN_THROTTLE(1, "Covariance contains NaN");
        return;
      }
    }

    // Compute eigen values and vectors for both shapes.
    std::pair<Eigen::Matrix3d, Eigen::Vector3d> positionEigenVectorsAndValues(computeEigenValuesAndVectors(msg, 0));
    std::pair<Eigen::Matrix3d, Eigen::Vector3d> orientationEigenVectorsAndValues(computeEigenValuesAndVectors(msg, 3));

    Ogre::Quaternion positionQuaternion(computeRotation(msg, positionEigenVectorsAndValues));
    Ogre::Quaternion orientationQuaternion(computeRotation(msg, orientationEigenVectorsAndValues));

    position_node_->setOrientation(positionQuaternion);
    orientation_node_->setOrientation(orientationQuaternion);

    // Compute scaling.
    Ogre::Vector3 positionScaling(
        std::sqrt(positionEigenVectorsAndValues.second[0]),
        std::sqrt(positionEigenVectorsAndValues.second[1]),
        std::sqrt(positionEigenVectorsAndValues.second[2]));
    positionScaling *= scale_;

    Ogre::Vector3 orientationScaling(
        std::sqrt(orientationEigenVectorsAndValues.second[0]),
        std::sqrt(orientationEigenVectorsAndValues.second[1]),
        std::sqrt(orientationEigenVectorsAndValues.second[2]));
    orientationScaling *= scale_;

    // Set the scaling.
    if (!positionScaling.isNaN())
    {
      position_node_->setScale(positionScaling);
    }
    else
    {
      ROS_WARN_STREAM_THROTTLE(1, "PositionScaling contains NaN: " << positionScaling);
    }

    if (!orientationScaling.isNaN())
    {
      orientation_node_->setScale(orientationScaling);
    }
    else
    {
      ROS_WARN_STREAM_THROTTLE(1, "OrientationScaling contains NaN: " << orientationScaling);
    }

    // Debugging.
    ROS_DEBUG_STREAM_THROTTLE(1,
       "Position:\n"
       << position << "\n"
       << "Positional part 3x3 eigen values:\n"
       << positionEigenVectorsAndValues.second << "\n"
       << "Positional part 3x3 eigen vectors:\n"
       << positionEigenVectorsAndValues.first << "\n"
       << "Sphere orientation:\n"
       << positionQuaternion << "\n"
       << positionQuaternion.getYaw() << "\n"
       << "Sphere scaling:\n"
       << positionScaling << "\n"
       << "Rotational part 3x3 eigen values:\n"
       << orientationEigenVectorsAndValues.second << "\n"
       << "Rotational part 3x3 eigen vectors:\n"
       << orientationEigenVectorsAndValues.first << "\n"
       << "Cone orientation:\n"
       << orientationQuaternion << "\n"
       << orientationQuaternion.getRoll() << " "
       << orientationQuaternion.getPitch() << " "
       << orientationQuaternion.getYaw() << "\n"
       << "Cone scaling:\n"
       << orientationScaling);
  }
  else // 3DOF
  {
    // Take (x, y, th) part from the covariance matrix:
    //   x y z R P Y
    // x * *       x
    // y * *       x
    // z
    // R
    // P
    // Y x x       *
    //
    // Actually, we only take the elements marked with '*', although we could
    // also take a 3x3 matrix with the '*' and 'x' elements.
    Eigen::Matrix2d cov_xy; cov_xy << msg.covariance[0], msg.covariance[1],
                                      msg.covariance[6], msg.covariance[7];
    const double cov_yaw = msg.covariance[35];

    // Check for NaN in covariance
    if (isnan(cov_xy) || isnan(cov_yaw))
    {
      ROS_WARN_STREAM_THROTTLE(1, "Covariance contains NaN: " <<
          "C_xy = " << cov_xy << ", C_yaw = " << cov_yaw);
      return;
    }

    // Compute eigen values and vectors for xy
    Eigen::Vector2d eigen_values  = Eigen::Vector2d::Identity();
    Eigen::Matrix2d eigen_vectors = Eigen::Matrix2d::Zero();

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(cov_xy);
    if (eigensolver.info() == Eigen::Success)
    {
      eigen_values  = eigensolver.eigenvalues();
      eigen_vectors = eigensolver.eigenvectors();
    }
    else
    {
      ROS_WARN_THROTTLE(1, "Failed to compute eigen values/vectors. Is the covariance matrix correct?");
    }

    // Compute ellipsoid angle, and axes
    const double yaw = atan2(eigen_vectors(1, 0), eigen_vectors(0, 0));
    const double axis_major = sqrt(eigen_values[0]);
    const double axis_minor = sqrt(eigen_values[1]);
    const double axis_yaw   = sqrt(cov_yaw);

    // Compute the ellipsoid orientation
    Ogre::Quaternion positionQuaternion;
    Ogre::Matrix3 R;
    R.FromEulerAnglesXYZ(Ogre::Radian(0.0), Ogre::Radian(0.0), Ogre::Radian(yaw));
    positionQuaternion.FromRotationMatrix(R);

    // Set the orientation
    position_node_->setOrientation(positionQuaternion);

    // Compute the ellipsoid scale
    Ogre::Vector3 positionScaling(
        std::isnormal(axis_major) ? axis_major : 0.001,
        std::isnormal(axis_minor) ? axis_minor : 0.001,
        std::isnormal(axis_yaw  ) ? axis_yaw   : 0.001);
    positionScaling *= scale_;

    // Set the scaling
    if (!positionScaling.isNaN())
    {
      position_node_->setScale(positionScaling);
    }
    else
    {
      ROS_WARN_STREAM_THROTTLE(1, "PositionScaling contains NaN: " << positionScaling);
    }

    // Debugging
    ROS_DEBUG_STREAM_THROTTLE(1,
       "Position:\n"
       << position << "\n"
       << "C_xy:\n"
       << cov_xy << "\n"
       << "C_yaw:\n"
       << cov_yaw << "\n"
       << "axis (major, minor, yaw): (" << axis_major << ", " << axis_minor << ", " << axis_yaw << ")\n"
       << "yaw: " << yaw << "\n"
       << "Positional part 2x2 eigen values:\n"
       << eigen_values << "\n"
       << "Positional part 2x2 eigen vectors:\n"
       << eigen_vectors << "\n"
       << "Sphere orientation:\n"
       << positionQuaternion << "\n"
       << positionQuaternion.getRoll() << " "
       << positionQuaternion.getPitch() << " "
       << positionQuaternion.getYaw() << "\n"
       << "Sphere scaling:\n"
       << positionScaling);
  }
}

void CovarianceVisual::setMessage(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
  setMessage(msg->pose);
}

void CovarianceVisual::setMessage(const nav_msgs::OdometryConstPtr& msg)
{
  setMessage(msg->pose);
}

void CovarianceVisual::setFramePosition(const Ogre::Vector3& position)
{
  frame_node_->setPosition(position);
}

void CovarianceVisual::setFrameOrientation(const Ogre::Quaternion& orientation)
{
  frame_node_->setOrientation(orientation);
}

void CovarianceVisual::setColor(float r, float g, float b, float a)
{
  position_shape_->setColor(r, g, b, a);
  orientation_shape_->setColor(r, g, b, a);
}

void CovarianceVisual::setScale(float scale)
{
  scale_ = scale;
}

void CovarianceVisual::setShowAxis(bool show_axis)
{
  show_axis_ = show_axis;

  axes_->getSceneNode()->setVisible(show_axis);
  position_node_->setVisible(show_position_);
  orientation_node_->setVisible(use_6dof_ && show_orientation_);
}

void CovarianceVisual::setShowPosition(bool show_position)
{
  show_position_ = show_position;

  position_node_->setVisible(show_position_);
}

void CovarianceVisual::setShowOrientation(bool show_orientation)
{
  show_orientation_ = show_orientation;

  orientation_node_->setVisible(use_6dof_ && show_orientation_);
}

void CovarianceVisual::setUse6DOF(bool use_6dof)
{
  use_6dof_ = use_6dof;

  orientation_node_->setVisible(use_6dof && show_orientation_);
}

} // end namespace rviz_plugin_covariance


#pragma once

#include <iomanip>
#include <vector>
#include <cmath>

#include <Eigen/Core>
#include <opencv2/core/core.hpp>

#include "Bounds.hh"
#include "../Math/math.hh"
#include "../util/assert.hh"
#include "../util/Maybe.hh"

#include "LineSegment/linesegment.hh"

namespace bold
{
  /**
   * Line, specified using radius/distance-from-origin parameters (normal form.)
   *
   * Equations defining the line are:
   *
   *   r = x*sin(theta) + y*cos(theta)
   *
   *   x = (r - y*cos(theta))/sin(theta)
   *   y = (r - x*sin(theta))/cos(theta)
   */
  class Line
  {
  // TODO rename to NormalLine2d?
  public:
    Line(double const radius, double const theta, const ushort votes = 0)
    : d_radius(radius),
      d_theta(theta)
    {
      ASSERT(theta >= 0);
      ASSERT(theta <= M_PI);
      ASSERT(!std::isnan(theta) && !std::isnan(radius) && !std::isinf(theta) && !std::isinf(radius));
    };

    double radius() const { return d_radius; }
    double theta() const { return d_theta; }
    double thetaDegrees() const { return Math::radToDeg(d_theta); }

    double gradient() const { return tanh(d_theta); }
    double yIntersection() const { return d_radius / cos(d_theta); }

    void draw(cv::Mat& mat, cv::Scalar const& color) const
    {
      Maybe<LineSegment<int,2>> line(intersectWith(Bounds2i(Eigen::Vector2i::Zero(), Eigen::Vector2i(mat.cols, mat.rows))));

      if (line.hasValue())
      {
        Eigen::Vector2i const& p1 = line->p1();
        Eigen::Vector2i const& p2 = line->p2();
        cv::line(mat, cv::Point(p1.x(), p1.y()), cv::Point(p2.x(), p2.y()), color);
      }
    }

    template<typename T>
    Maybe<LineSegment<T,2>> intersectWith(Bounds<T,2> bounds) const
    {
      ASSERT(d_theta >= 0 && d_theta <= M_PI);

      double tsin = sin(d_theta);
      double tcos = cos(d_theta);

      int minX = bounds.min().x();
      int minY = bounds.min().y();
      int maxX = bounds.max().x();
      int maxY = bounds.max().y();

      // r = x*sin(theta) + y*cos(theta)
      // x = (r - y*cos(theta))/sin(theta)
      // y = (r - x*sin(theta))/cos(theta)

      double xWhenYMin = (d_radius-minY*tcos)/tsin;
      double yWhenXMin = (d_radius-minX*tsin)/tcos;

      double xWhenYMax = (d_radius-maxY*tcos)/tsin;
      double yWhenXMax = (d_radius-maxX*tsin)/tcos;

      std::vector<Eigen::Matrix<T,2,1>> edgeContactPoints;

      if (xWhenYMin >= minX && xWhenYMin <= maxX)
        edgeContactPoints.emplace_back(xWhenYMin, minY);

      if (xWhenYMax >= minX && xWhenYMax <= maxX)
        edgeContactPoints.emplace_back(xWhenYMax, maxY);

      if (yWhenXMin >= minY && yWhenXMin <= maxY)
        edgeContactPoints.emplace_back(minX, yWhenXMin);

      if (yWhenXMax >= minY && yWhenXMax <= maxY)
        edgeContactPoints.emplace_back(maxX, yWhenXMax);

      if (edgeContactPoints.size() == 0)
        return Maybe<LineSegment<T,2>>::empty();

      ASSERT(edgeContactPoints.size() == 2);

      if (edgeContactPoints.size() != 2)
        return Maybe<LineSegment<T,2>>::empty();

      return Maybe<LineSegment<T,2>>(LineSegment<T,2>(edgeContactPoints[0], edgeContactPoints[1]));
    }

    bool operator==(Line const& other) const
    {
      const double epsilon = 0.0000001;
      return fabs(d_radius - other.d_radius) < epsilon
          && fabs(d_theta - other.d_theta) < epsilon;
    }

    template<typename T>
    static Line fromSegment(LineSegment2<T> const& segment)
    {
      double theta = atan2(segment.p2().y() - segment.p1().y(),
                           segment.p1().x() - segment.p2().x());

      double radius = segment.p1().x() * std::sin(theta) +
        segment.p1().y() * std::cos(theta);

      while (theta < 0)
      {
        theta += M_PI;
        radius = -radius;
      }

      while (theta > M_PI)
      {
        theta -= M_PI;
        radius = -radius;
      }

      return Line(radius, theta);
    }

    friend std::ostream& operator<<(std::ostream& stream, Line const& line)
    {
      return stream << std::setprecision(13) << "Line (radius=" << line.radius() << " theta=" << line.theta() << ")";
    }

  private:
    double d_radius;
    double d_theta;
  };
}


/* Boost libs/numeric/odeint/examples/openmp/lorenz_ensemble_nested.cpp

 Copyright 2013 Karsten Ahnert
 Copyright 2013 Pascal Germroth
 Copyright 2013 Mario Mulansky

 Parallelized Lorenz ensembles using nested omp algebra

 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 <omp.h>
#include <vector>
#include <iostream>
#include <iterator>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/openmp/openmp.hpp>
#include <boost/lexical_cast.hpp>
#include "point_type.hpp"

using namespace std;
using namespace boost::numeric::odeint;

typedef point<double, 3> point_type;
typedef vector< point_type > state_type;

const double sigma = 10.0;
const double b = 8.0 / 3.0;

struct sys_func {
    const vector<double> &R;
    sys_func( vector<double> &R ) : R(R) {}

    void operator()( const state_type &x , state_type &dxdt , double t ) const {
#       pragma omp parallel for
        for(size_t i = 0 ; i < x.size() ; i++) {
            dxdt[i][0] = -sigma * (x[i][0] - x[i][1]);
            dxdt[i][1] = R[i] * x[i][0] - x[i][1] - x[i][0] * x[i][2];
            dxdt[i][2] = -b * x[i][2] + x[i][0] * x[i][1];
        }
    }
};

int main(int argc, char **argv) {
    size_t n = 1024;
    if(argc > 1) n = boost::lexical_cast<size_t>(argv[1]);

    vector<double> R(n);
    const double Rmin = 0.1, Rmax = 50.0;
#   pragma omp parallel for
    for(size_t i = 0 ; i < n ; i++)
        R[i] = Rmin + (Rmax - Rmin) / (n - 1) * i;

    state_type state( n , point_type(10, 10, 10) );

    typedef runge_kutta4< state_type, double , state_type , double ,
                          openmp_nested_algebra<vector_space_algebra> > stepper;

    const double t_max = 10.0, dt = 0.01;

    integrate_const(
        stepper(),
        sys_func(R),
        state,
        0.0, t_max, dt
    );

    std::copy( state.begin(), state.end(), ostream_iterator<point_type>(cout, "\n") );

    return 0;
}


#pragma once

#include <armadillo>
#include <vector>
#include <tuple>

#define MAX_ITERS 100

struct KMedNode
{
	KMedNode(size_t n, size_t k) : N(n), K(k),
						medoids(K,arma::fill::zeros),
						multiplicity(K,arma::fill::zeros),
						assignments(N,arma::fill::zeros) { }
	size_t N;
	size_t K;
	arma::uvec medoids;
	arma::uvec multiplicity;
	arma::uvec assignments;
};

void
assign_to_medoids(const arma::mat& dists, KMedNode& kmn)
{
	arma::mat sub_dists = dists.rows(kmn.medoids);
	kmn.multiplicity.zeros();
	for(size_t i = 0; i < kmn.N; ++i)
	{
		arma::uvec idx = arma::find(kmn.medoids==i,1);
		if(idx.n_elem == 1)
		{
			size_t loc = idx[0];
			kmn.multiplicity[loc]++;
			kmn.assignments[i] = kmn.medoids[loc];
			continue;
		}

		size_t min_idx = sub_dists.col(i).index_min();
		kmn.assignments[i] = kmn.medoids[min_idx];
		kmn.multiplicity[min_idx]++;
	}
}

void
update_medoids(const arma::mat& dists, KMedNode& kmn)
{
	arma::uvec set;
	for(size_t k = 0; k < kmn.K; ++k)
	{
		set = arma::find(kmn.assignments == kmn.medoids[k]);
		arma::mat sub_dist = dists(set,set);
		arma::vec row_sums = arma::sum(sub_dist,1);
		size_t min_idx = row_sums.index_min();
		if(kmn.medoids[k] != set[min_idx])
			printf("Swapping %llu for %llu\n",set[min_idx],kmn.medoids[k]);
		kmn.medoids[k] = set[min_idx];
	}
}

arma::uvec
kmpp_init(const arma::mat& dists, size_t k, const arma::uvec& seeds)
{
	arma::uvec medoids(k);
	size_t start;
	printf("Initializing random center\n");
	if(seeds.n_elem > k)
		throw std::runtime_error("kmeans++ called with seed count greater than k");
	if(seeds.n_elem > 0)
	{
		for(size_t i = 0; i < seeds.n_elem; ++i)
			medoids(i) = seeds(i);
		start = seeds.n_elem;
	} else {
		// randomly initialize one value
		auto init = arma::randi<arma::uvec>(1,arma::distr_param(0,dists.n_rows-1));
		medoids(0)=init(0);
		start = 1;
	}
	for(size_t i = start; i < k; ++i)
	{
		// Dsq(x) = min_i D(x,medoid_i)^2
		// so minimize across columns to find the min value of D(x,medoids_i)
		arma::uvec assigned = medoids(arma::span(0,i-1));
		arma::vec Dsq = arma::square(arma::min(dists.cols(assigned),1));

		arma::vec cdf = arma::cumsum(Dsq/arma::accu(Dsq));
		arma::vec r = arma::randu<arma::vec>(1);
		arma::uvec loc = arma::find(cdf > r[0],1,"first");
		if(loc.is_empty())
		{
			medoids[i] = dists.n_rows-1;
		}
		else
		{
			medoids[i] = loc[0];
		}
	}

	return medoids;
}
// uses Voronoi iteration
KMedNode
select_kmedoids(const arma::mat& dists, size_t k, const arma::uvec& seeds = arma::uvec() )
{
	int N = dists.n_rows;
	KMedNode kmn(N, k);

	kmn.medoids = arma::sort(kmpp_init(dists,k,seeds));
	arma::uvec old_medoids = kmn.medoids;
	arma::uvec diff, sindex;

	bool converged = false;
	for(size_t i = 0; i < MAX_ITERS && !converged; ++i)
	{
		printf("Iteration %lu\n",i);
		assign_to_medoids(dists,kmn);
		update_medoids(dists,kmn);
		kmn.medoids = arma::sort(kmn.medoids);

		diff = arma::find(old_medoids != kmn.medoids);
		if(diff.is_empty())
			converged = true;
		else
			old_medoids = kmn.medoids;
	}

	assign_to_medoids(dists,kmn);
	return kmn;
}

std::tuple<pyarr_u,pyarr_u>
kmedoids(pyarr_d dists, size_t k, pyarr_u seeds = pyarr_u(0,nullptr))
{
	arma::arma_rng::set_seed_random();
	arma::mat d = py_to_mat(dists);
	arma::uvec seed_medoid = py_to_uvec(seeds);
	seed_medoid.print("Seed Medoids");
	KMedNode kmn = select_kmedoids(d,k, seed_medoid);
	return std::make_tuple(uvec_to_py(kmn.medoids),uvec_to_py(kmn.multiplicity));
}