/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

#include <cstdlib>
#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
#include <cmath>
#include <limits>
#include <vector>

#include <boost/numeric/odeint.hpp>

#include "NAOS/constants.hpp"
#include "NAOS/basicMath.hpp"
#include "NAOS/basicAstro.hpp"
#include "NAOS/misc.hpp"
#include "NAOS/ellipsoidGravitationalAcceleration.hpp"

namespace naos
{

//! equations of motion (for a particle around the asteroid modelled as an Ellipsoid)
/*!
 * first order differential equations describing the motion of a particle or spacecraft around a
 * central body modeled as an ellipsoid using ellipsoid gravitational model.
 */
class equationsOfMotionParticleAroundEllipsoid
{
    // declare parameters, gravitational parameter and the semi major axes of the ellipsoid
    const double gravParameter;
    const double alpha;
    const double beta;
    const double gamma;
    const double zRotation;

public:
    // Default constructor with member initializer list
    equationsOfMotionParticleAroundEllipsoid(
               const double aGravParameter,
               const double aAlpha,
               const double aBeta,
               const double aGamma,
               const double aZRotation )
            : gravParameter( aGravParameter ),
              alpha( aAlpha ),
              beta( aBeta ),
              gamma( aGamma ),
              zRotation( aZRotation )
    { }
    void operator() ( const std::vector< double > &stateVector,
                      std::vector< double > &dXdt,
                      const double currentTime )
    {
        // calculate the gravitational accelerations first
        std::vector< double > gravAcceleration( 3, 0.0 );

        computeEllipsoidGravitationalAcceleration( alpha,
                                                   beta,
                                                   gamma,
                                                   gravParameter,
                                                   stateVector[ xPositionIndex ],
                                                   stateVector[ yPositionIndex ],
                                                   stateVector[ zPositionIndex ],
                                                   gravAcceleration );

        // now calculate the derivatives
        dXdt[ xPositionIndex ] = stateVector[ xVelocityIndex ];
        dXdt[ yPositionIndex ] = stateVector[ yVelocityIndex ];
        dXdt[ zPositionIndex ] = stateVector[ zVelocityIndex ];

        dXdt[ xVelocityIndex ] = gravAcceleration[ xPositionIndex ]
                                + 2.0 * zRotation * stateVector[ yVelocityIndex ]
                                + zRotation * zRotation * stateVector[ xPositionIndex ];

        dXdt[ yVelocityIndex ] = gravAcceleration[ yPositionIndex ]
                                - 2.0 * zRotation * stateVector[ xVelocityIndex ]
                                + zRotation * zRotation * stateVector[ yPositionIndex ];

        dXdt[ zVelocityIndex ] = gravAcceleration[ zPositionIndex ];
    }
};

//! Store intermediate state values and time( if needed )
/*!
 * This structure contains members that will save all intermediate state values and times when
 * an object of this structure is passed as an argument to the integrator function.
 */
struct pushBackStateAndTime
{
    // declare containers to store state and time
    std::vector< std::vector< double > > &stateContainer;
    std::vector< double > &timeContainer;

    //member initializer list
    pushBackStateAndTime( std::vector< std::vector< double > > &aState,
                          std::vector< double > &aTime )
                : stateContainer( aState ),
                  timeContainer( aTime )
    { }

    void operator() ( const std::vector< double > &singleStateVector, const double singleTime )
    {
        // store the intermediate state and time values in the containers
        stateContainer.push_back( singleStateVector );
        timeContainer.push_back( singleTime );
    }
};

//! particle around ellipsoid integration
/*!
 * integrate the equations of motion for a particle around an ellipsoid. The gravitational accelerations
 * calculated using the ellipsoid gravitational potential model.
 */
void executeParticleAroundEllipsoid( const double alpha,
                                     const double beta,
                                     const double gamma,
                                     const double gravParameter,
                                     std::vector< double > asteroidRotationVector,
                                     std::vector< double > &initialOrbitalElements,
                                     const double initialStepSize,
                                     const double startTime,
                                     const double endTime,
                                     std::ostringstream &outputFilePath,
                                     const int dataSaveIntervals )
{
    //! open the output csv file to save data. Declare file headers.
    std::ofstream outputFile;
    outputFile.open( outputFilePath.str( ) );
    outputFile << "x" << ",";
    outputFile << "y" << ",";
    outputFile << "z" << ",";
    outputFile << "vx" << ",";
    outputFile << "vy" << ",";
    outputFile << "vz" << ",";
    outputFile << "t" << std::endl;
    outputFile.precision( 16 );

    //! convert the initial orbital elements to cartesian state
    std::vector< double > initialStateInertial( 6, 0.0 );
    initialStateInertial = convertKeplerianElementsToCartesianCoordinates( initialOrbitalElements,
                                                                           gravParameter );

    //! account for non-zero start time value and calculate the initial state in body frame
    double phi = asteroidRotationVector[ zPositionIndex ] * startTime;

    std::vector< double > initialState( 6, 0.0 );

    // get the initial body frame position
    initialState[ xPositionIndex ]
            = initialStateInertial[ xPositionIndex ] * std::cos( phi )
            + initialStateInertial[ yPositionIndex ] * std::sin( phi );

    initialState[ yPositionIndex ]
            = -1.0 * initialStateInertial[ xPositionIndex ] * std::sin( phi )
            + initialStateInertial[ yPositionIndex ] * std::cos( phi );

    initialState[ zPositionIndex ] = initialStateInertial[ zPositionIndex ];

    // get the initial body frame velocity
    std::vector< double > inertialPositionVector = { initialStateInertial[ xPositionIndex ],
                                                     initialStateInertial[ yPositionIndex ],
                                                     initialStateInertial[ zPositionIndex ] };

    std::vector< double > omegaCrossPosition( 3, 0.0 );
    omegaCrossPosition = crossProduct( asteroidRotationVector, inertialPositionVector );

    double xbodyFrameVelocityInertialCoordinates
                = initialStateInertial[ xVelocityIndex ] - omegaCrossPosition[ 0 ];

    double ybodyFrameVelocityInertialCoordinates
                = initialStateInertial[ yVelocityIndex ] - omegaCrossPosition[ 1 ];

    double zbodyFrameVelocityInertialCoordinates
                = initialStateInertial[ zVelocityIndex ] - omegaCrossPosition[ 2 ];

    initialState[ xVelocityIndex ]
            = xbodyFrameVelocityInertialCoordinates * std::cos( phi )
            + ybodyFrameVelocityInertialCoordinates * std::sin( phi );

    initialState[ yVelocityIndex ]
            = -1.0 * xbodyFrameVelocityInertialCoordinates * std::sin( phi )
            + ybodyFrameVelocityInertialCoordinates * std::cos( phi );

    initialState[ zVelocityIndex] = zbodyFrameVelocityInertialCoordinates;

    // set up boost odeint
    const double absoluteTolerance = 1.0e-15;
    const double relativeTolerance = 1.0e-15;
    typedef boost::numeric::odeint::runge_kutta_fehlberg78< std::vector< double > > stepperType;

    // state step size guess (at each step this initial guess will be used)
    double stepSizeGuess = initialStepSize;

    // initialize the ode system
    const double zRotation = asteroidRotationVector[ zPositionIndex ];
    equationsOfMotionParticleAroundEllipsoid particleAroundEllipsoidProblem( gravParameter,
                                                                             alpha,
                                                                             beta,
                                                                             gamma,
                                                                             zRotation );

    // initialize current state vector and time
    std::vector< double > currentStateVector = initialState;
    double currentTime = startTime;
    double intermediateEndTime = currentTime + dataSaveIntervals;

    // save the initial state vector
    outputFile << currentStateVector[ xPositionIndex ] << ",";
    outputFile << currentStateVector[ yPositionIndex ] << ",";
    outputFile << currentStateVector[ zPositionIndex ] << ",";
    outputFile << currentStateVector[ xVelocityIndex ] << ",";
    outputFile << currentStateVector[ yVelocityIndex ] << ",";
    outputFile << currentStateVector[ zVelocityIndex ] << ",";
    outputFile << currentTime << std::endl;

    // start the integration outer loop
    while( intermediateEndTime <= endTime )
    {
        // perform integration, integrated result stored in currentStateVector
        size_t steps = boost::numeric::odeint::integrate_adaptive(
                            make_controlled( absoluteTolerance, relativeTolerance, stepperType( ) ),
                            particleAroundEllipsoidProblem,
                            currentStateVector,
                            currentTime,
                            intermediateEndTime,
                            stepSizeGuess );

        // update the time variables
        currentTime = intermediateEndTime;
        intermediateEndTime = currentTime + dataSaveIntervals;

        // save data
        outputFile << currentStateVector[ xPositionIndex ] << ",";
        outputFile << currentStateVector[ yPositionIndex ] << ",";
        outputFile << currentStateVector[ zPositionIndex ] << ",";
        outputFile << currentStateVector[ xVelocityIndex ] << ",";
        outputFile << currentStateVector[ yVelocityIndex ] << ",";
        outputFile << currentStateVector[ zVelocityIndex ] << ",";
        outputFile << currentTime << std::endl;

    } // end of outer while loop for integration

    outputFile.close( );
}

//! Trajectory calculation for regolith around an asteroid (modelled as ellipsoid here)
/*!
 * Same as the previous function, except that the initial conditions are now given as a cartesian
 * state. The initial cartesian state should be given in body fixed frame of the asteroid.
 */
void singleRegolithTrajectoryCalculator( const double alpha,
                                         const double beta,
                                         const double gamma,
                                         const double gravParameter,
                                         std::vector< double > asteroidRotationVector,
                                         std::vector< double > &initialCartesianStateVector,
                                         const double initialStepSize,
                                         const double startTime,
                                         const double endTime,
                                         std::ostringstream &outputFilePath,
                                         const int dataSaveIntervals )
{
    //! open the output csv file to save data. Declare file headers.
    std::ofstream outputFile;
    outputFile.open( outputFilePath.str( ) );
    outputFile << "x" << ",";
    outputFile << "y" << ",";
    outputFile << "z" << ",";
    outputFile << "vx" << ",";
    outputFile << "vy" << ",";
    outputFile << "vz" << ",";
    outputFile << "t" << std::endl;
    outputFile.precision( 16 );

    //! get the initial cartesian state vector in a seperate container
    std::vector< double > initialState = initialCartesianStateVector;

    // set up boost odeint
    const double absoluteTolerance = 1.0e-15;
    const double relativeTolerance = 1.0e-15;
    typedef boost::numeric::odeint::runge_kutta_fehlberg78< std::vector< double > > stepperType;

    // state step size guess (at each step this initial guess will be used)
    double stepSizeGuess = initialStepSize;

    // initialize the ode system
    const double zRotation = asteroidRotationVector[ zPositionIndex ];
    equationsOfMotionParticleAroundEllipsoid particleAroundEllipsoidProblem( gravParameter,
                                                                             alpha,
                                                                             beta,
                                                                             gamma,
                                                                             zRotation );

    // initialize current state vector and time
    std::vector< double > currentStateVector = initialState;
    double currentTime = startTime;
    double intermediateEndTime = currentTime + dataSaveIntervals;

    // save the initial state vector
    outputFile << currentStateVector[ xPositionIndex ] << ",";
    outputFile << currentStateVector[ yPositionIndex ] << ",";
    outputFile << currentStateVector[ zPositionIndex ] << ",";
    outputFile << currentStateVector[ xVelocityIndex ] << ",";
    outputFile << currentStateVector[ yVelocityIndex ] << ",";
    outputFile << currentStateVector[ zVelocityIndex ] << ",";
    outputFile << currentTime << std::endl;

    // start the integration outer loop
    while( intermediateEndTime <= endTime )
    {
        // save the last know state vector for when the particle is outside the asteroid
        std::vector< double > lastStateVector = currentStateVector;

        // perform integration, integrated result stored in currentStateVector
        size_t steps = boost::numeric::odeint::integrate_adaptive(
                            make_controlled( absoluteTolerance, relativeTolerance, stepperType( ) ),
                            particleAroundEllipsoidProblem,
                            currentStateVector,
                            currentTime,
                            intermediateEndTime,
                            stepSizeGuess );

        //! check if the particle is inside the surface of the asteroid
        double xSquare = currentStateVector[ xPositionIndex ] * currentStateVector[ xPositionIndex ];
        double ySquare = currentStateVector[ yPositionIndex ] * currentStateVector[ yPositionIndex ];
        double zSquare = currentStateVector[ zPositionIndex ] * currentStateVector[ zPositionIndex ];

        double crashCheck = xSquare / ( alpha * alpha )
                            + ySquare / ( beta * beta )
                            + zSquare / ( gamma * gamma )
                            - 1.0;

        if( crashCheck == 0.0 )
        {
            // particle is on the surface of the asteroid, save data and stop integration
            // update the time variables
            currentTime = intermediateEndTime;

            // save data
            outputFile << currentStateVector[ xPositionIndex ] << ",";
            outputFile << currentStateVector[ yPositionIndex ] << ",";
            outputFile << currentStateVector[ zPositionIndex ] << ",";
            outputFile << currentStateVector[ xVelocityIndex ] << ",";
            outputFile << currentStateVector[ yVelocityIndex ] << ",";
            outputFile << currentStateVector[ zVelocityIndex ] << ",";
            outputFile << currentTime << std::endl;

            break;
        }

        if( crashCheck < 0.0 )
        {
            double stepSize = 1.0;
            const double machinePrecision = std::numeric_limits< double >::epsilon( );
            // if the particle is on the surface of the asteroid, then the while condition
            // will be false, for all other cases it will be true. Within the while loop, the
            // inside or outside differnetiation takes place.
            while( std::fabs( crashCheck ) > machinePrecision )
            {
                // std::cout << "crash check value = " << crashCheck << std::endl;
                if( crashCheck < 0.0 ) // particle is still inside the surface
                {
                    // std::cout << "particle inside the surface" << std::endl << std::endl;
                    // particle is inside the surface of the asteroid. restart the integration from last
                    // known state external to the asteroid
                    currentStateVector = lastStateVector;
                    stepSize = 0.5 * stepSize;
                }
                else // particle is outside the asteroid at the end of last integration step
                {
                    // std::cout << "particle outside the surface" << std::endl << std::endl;
                    lastStateVector = currentStateVector;
                    currentTime = intermediateEndTime;
                }

                typedef boost::numeric::odeint::runge_kutta_fehlberg78< std::vector< double > > errorStepperType;
                intermediateEndTime =  boost::numeric::odeint::integrate_n_steps(
                                                errorStepperType( ),
                                                particleAroundEllipsoidProblem,
                                                currentStateVector,
                                                currentTime,
                                                stepSize,
                                                1 );

                xSquare = currentStateVector[ xPositionIndex ] * currentStateVector[ xPositionIndex ];
                ySquare = currentStateVector[ yPositionIndex ] * currentStateVector[ yPositionIndex ];
                zSquare = currentStateVector[ zPositionIndex ] * currentStateVector[ zPositionIndex ];

                crashCheck = xSquare / ( alpha * alpha )
                            + ySquare / ( beta * beta )
                            + zSquare / ( gamma * gamma )
                            - 1.0;
            }

            // particle is on the surface of the asteroid, save data and stop integration
            // update the time variables
            currentTime = intermediateEndTime;

            // save data
            outputFile << currentStateVector[ xPositionIndex ] << ",";
            outputFile << currentStateVector[ yPositionIndex ] << ",";
            outputFile << currentStateVector[ zPositionIndex ] << ",";
            outputFile << currentStateVector[ xVelocityIndex ] << ",";
            outputFile << currentStateVector[ yVelocityIndex ] << ",";
            outputFile << currentStateVector[ zVelocityIndex ] << ",";
            outputFile << currentTime << std::endl;

            break;
        }

        // update the time variables
        currentTime = intermediateEndTime;
        intermediateEndTime = currentTime + dataSaveIntervals;

        // save data
        outputFile << currentStateVector[ xPositionIndex ] << ",";
        outputFile << currentStateVector[ yPositionIndex ] << ",";
        outputFile << currentStateVector[ zPositionIndex ] << ",";
        outputFile << currentStateVector[ xVelocityIndex ] << ",";
        outputFile << currentStateVector[ yVelocityIndex ] << ",";
        outputFile << currentStateVector[ zVelocityIndex ] << ",";
        outputFile << currentTime << std::endl;

    } // end of outer while loop for integration

    outputFile.close( );
}

} // namespace naos


// SPDX-License-Identifier: MIT
// SPDX-FileCopyrightText: Copyright 2019-2022 Heal Research

#include <numeric>
#include "operon/operators/non_dominated_sorter.hpp"
#include "operon/core/individual.hpp"
#include <Eigen/Core>

namespace Operon {
    using Vec = Eigen::Matrix<int64_t, -1, 1, Eigen::ColMajor>;
    using Mat = Eigen::Matrix<int64_t, -1, -1, Eigen::ColMajor>;

    inline auto ComputeComparisonMatrix(Operon::Span<Operon::Individual const> pop, Mat const& idx, Eigen::Index colIdx) noexcept
    {
        auto const n = static_cast<Eigen::Index>(pop.size());
        Mat c = Mat::Zero(n, n);
        Mat::ConstColXpr b = idx.col(colIdx);
        c.row(b(0)).fill(1); // NOLINT
        for (auto i = 1; i < n; ++i) {
            if (pop[b(i)][colIdx] == pop[b(i-1)][colIdx]) {
                c.row(b(i)) = c.row(b(i-1));
            } else {
                for (auto j = i; j < n; ++j) {
                    c(b(i), b(j)) = 1;
                }
            }
        }
        return c;
    }

    inline auto ComparisonMatrixSum(Operon::Span<Operon::Individual const> pop, Mat const& idx) noexcept {
        Mat d = ComputeComparisonMatrix(pop, idx, 0);
        for (int i = 1; i < idx.cols(); ++i) {
            d.noalias() += ComputeComparisonMatrix(pop, idx, i);
        }
        return d;
    }

    inline auto ComputeDegreeMatrix(Operon::Span<Operon::Individual const> pop, Mat const& idx) noexcept
    {
        auto const n = static_cast<Eigen::Index>(pop.size());
        auto const m = static_cast<Eigen::Index>(pop.front().Fitness.size());
        Mat d = ComparisonMatrixSum(pop, idx);
        for (auto i = 0; i < n; ++i) {
            for (auto j = i; j < n; ++j) {
                if (d(i, j) == m && d(j, i) == m) {
                    d(i, j) = d(j, i) = 0;
                }
            }
        }
        return d;
    }

    auto DominanceDegreeSorter::Sort(Operon::Span<Operon::Individual const> pop, Operon::Scalar eps) const -> NondominatedSorterBase::Result
    {
        auto const n = static_cast<Eigen::Index>(pop.size());
        auto const m = static_cast<Eigen::Index>(pop.front().Fitness.size());

        Operon::Less cmp;
        Mat idx = Vec::LinSpaced(n, 0, n-1).replicate(1, m);
        for (auto i = 0; i < m; ++i) {
            auto *data = idx.col(i).data();
            std::sort(data, data + n, [&](auto a, auto b) { return cmp(pop[a][i], pop[b][i], eps); });
        }
        Mat d = ComputeDegreeMatrix(pop, idx);
        auto count = 0L; // number of assigned solutions
        std::vector<std::vector<size_t>> fronts;
        std::vector<size_t> tmp(n);
        std::iota(tmp.begin(), tmp.end(), 0UL);

        std::vector<size_t> remaining;
        while (count < n) {
            std::vector<size_t> front;
            for (auto i : tmp) {
                if (std::all_of(tmp.begin(), tmp.end(), [&](auto j) { return d(j, i) < m; })) {
                    front.push_back(i);
                } else {
                    remaining.push_back(i);
                }
            }
            tmp.swap(remaining);
            remaining.clear();
            count += static_cast<int64_t>(front.size());
            fronts.push_back(front);
        }
        return fronts;
    }
} // namespace Operon


#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <pangolin/pangolin.h>
#include <string>
#include <unistd.h>
#include <vector>

using namespace std;
using namespace Eigen;

// Load the images
string left_file = "./left.png";
string right_file = "./right.png";

// Method in pangolin to plot the grayscale pointcloud
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);

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

  // Intrinsics
  double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
  // Baseline
  double b = 0.573;

  // Read the images as grayscales and get the disparity map
  cv::Mat left = cv::imread(left_file, 0);
  cv::Mat right = cv::imread(right_file, 0);
  // Semi global block matching to get the disparity maps
  // Very parameter dependent, read the paper to understand
  cv::Ptr<cv::StereoSGBM> sgbm =
      cv::StereoSGBM::create(0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);
  cv::Mat disparity_sgbm, disparity;
  sgbm->compute(left, right, disparity_sgbm);
  disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);

  // Vector to store the pointcloud
  vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;

  // Cycle through th pixels
  for (int v = 0; v < left.rows; v++)
    for (int u = 0; u < left.cols; u++) {
      // Check if the disparity (which is in pixels) is valid
      if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;

      // Initialize the position an the grayscale value
      Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0);

      // Steps to get pointcloud:
      // - Get the depth using the fx (in pixels), the disparity (in pixels) and
      // the baseline (in meters)
      // Get the normalized coordinates which are basically projections on plane z=1
      // Multiply the x,y coordinates with the depth and th z coordinate is the depth
      double depth = fx * b / (disparity.at<float>(v, u));
      double x = (u - cx) / fx;
      double y = (v - cy) / fy;
      point[0] = x * depth;
      point[1] = y * depth;
      point[2] = depth;

      pointcloud.push_back(point);
    }

  cv::imshow("disparity", disparity / 96.0);
  cv::waitKey(0);
  // Show the pointcloud in pangolin
  showPointCloud(pointcloud);
  return 0;
}

void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {

  if (pointcloud.empty()) {
    cerr << "Point cloud is empty!" << endl;
    return;
  }

  pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
      pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
      pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0));

  pangolin::View &d_cam =
      pangolin::CreateDisplay()
          .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
          .SetHandler(new pangolin::Handler3D(s_cam));

  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

    glPointSize(2);
    glBegin(GL_POINTS);
    for (auto &p : pointcloud) {
      glColor3f(p[3], p[3], p[3]);
      glVertex3d(p[0], p[1], p[2]);
    }
    glEnd();
    pangolin::FinishFrame();
    usleep(5000);  // sleep 5 ms
  }
  return;
}


#include "libs/experiments/ROC.h"
#include <boost/range/algorithm/transform.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <math.h>
#include <utility>
#include <tuple>
#include <iostream>
#include <iterator>
#include  <stdexcept>

namespace exprs
{

value_type accuracy(value_type threshold, const std::vector<int>& true_val,
        const data_array& pred)
{
    static const value_type eps = 1.0e-10;
    int a  = 0,b = 0,c = 0, d = 0;
    for (int item = 0; item < (int)true_val.size(); item++) {
        if (true_val[item] == 1) {
            if (pred[item] >= threshold) {
                a++;
            } else {
                b++;
            }
        } else {
            if (pred[item] >= threshold) {
                c++;
            } else {
                d++;
            }
        }
    }
    return( ((value_type)(a+d)) / (((value_type)(a+b+c+d)) + eps) );
}

int partition(int p, int r, std::vector<int>& true_val, data_array& pred)
{

    value_type x = pred[p];
    if (p >= (int)pred.size() || p < 0) {
        throw std::runtime_error{"cannot partition lower limit - out of range between " + std::to_string(pred.size()) +
            " and " + std::to_string(p)};
    }
    if (r <= 0 || r >= (int)pred.size()) {
       throw std::runtime_error{"cannot partition upper limit - out of range between " + std::to_string(r) +
                " and " + std::to_string(pred.size())};
    }
    int i = p - 1;
    int j = r + 1;
    while (true) {
        do j--; while (j > -1 && pred[j] > x);
        if (j < 0) {
            throw std::runtime_error{"invalid value given no value less than " + std::to_string(x) + " found"};
        }
        do i++; while (i < (int)pred.size() && pred[i] < x );
        if (i == (int)pred.size()) {
            throw std::runtime_error{"invalid value given no value greater than " + std::to_string(x) + " found"};
        }
        if (i < j) {
            std::swap(pred[i], pred[j]);
            std::swap(true_val[i], true_val[j]);
        } else {
            return j;
        }
    }
}

void quicksort(int p, int r, std::vector<int>& true_val, data_array& pred)
{
    if (p < r) {
        int q = partition(p, r, true_val, pred);
        quicksort(p, q, true_val, pred);
        quicksort(q+1, r, true_val, pred);
    }
}

template<typename T>
value_type calculateRmse(const std::vector<T>& true_val, const data_array& pred,
        value_type& mean_true)
{
    value_type sse = 0.0;
    for(int no_item = 0; no_item < (int)true_val.size(); ++no_item) {
        value_type p1 = pred[no_item];
        sse+= (true_val[no_item]-p1)*(true_val[no_item]-p1);
        mean_true += true_val[no_item];
    }
    mean_true /= (value_type) true_val.size();
    return  sqrt(sse / ((value_type)true_val.size()));

}
std::tuple<value_type, value_type,
    data_array, data_array>
do_calculation(ROC::algo_params&& calc_params)
{
    // now let's do the ROC cruve and area
    quicksort(0, (int)(calc_params.partitions.size() - 1u), calc_params.partitions, calc_params.predictions);
    auto total_0 = calc_params.predictions.size() - calc_params.condition_pos;
    auto tt = 0;
    auto tf = calc_params.condition_pos;
    auto ft = 0;
    auto ff = total_0;

    auto sens = ((value_type) tt) / ((value_type) (tt+tf));
    auto spec = ((value_type) ff) / ((value_type) (ft+ff));
    auto tpf = sens;
    auto fpf = 1.f - spec;
    data_array true_pf, false_pf;

    true_pf.push_back(tpf);
    false_pf.push_back(fpf);
    auto roc_area = 0.f;
    auto tpf_prev = tpf;
    auto fpf_prev = fpf;

    auto no_item = calc_params.predictions.size();
    for (int item=no_item-1; item>-1; item--) {
        tt+= calc_params.partitions[item];
        tf-= calc_params.partitions[item];
        ft+= 1 - calc_params.partitions[item];
        ff-= 1 - calc_params.partitions[item];
        sens = ((value_type) tt) / ((value_type) (tt+tf));
        spec = ((value_type) ff) / ((value_type) (ft+ff));
        tpf  = sens;
        fpf  = 1.f - spec;
        if (item > 0) {
            if (calc_params.predictions[item] != calc_params.predictions[item - 1]) {
                true_pf.push_back(tpf);
                false_pf.push_back(fpf);
                roc_area += 0.5f * (tpf + tpf_prev) * (fpf-fpf_prev);
                tpf_prev = tpf;
                fpf_prev = fpf;
            }
        }
        if (item == 0) {
            true_pf.push_back(tpf);
            false_pf.push_back(fpf);
            roc_area += 0.5f * (tpf+tpf_prev) * (fpf-fpf_prev);
        }
    }
    auto acc = accuracy(0.5, calc_params.partitions, calc_params.predictions);
    return std::make_tuple(roc_area, acc, std::move(true_pf), std::move(false_pf));
}

template<typename T>
std::size_t ROC::algo_params::init(const std::vector<T>& ex,
        std::vector<int>& target,
        value_type threshold)
{
    auto pos = 0u;
    boost::transform(ex, std::back_inserter(target), [&pos, threshold](auto val) {
                if (val < threshold) {
                    return 0;
                } else {
                    ++pos;
                    return 1;
                }
            }
    );
    return pos;
}

ROC::algo_params::algo_params(const data_array& ex, const data_array& predic,
        value_type threshold) :
        predictions(predic)/*, partitions(ex.size(), 0)*/
{
    condition_pos = init(ex, partitions, threshold);
}

ROC::algo_params::algo_params(const std::vector<int>& ex,
        const data_array& p, value_type threshold) :
        predictions(p)/*, partitions(ex.size(), 0)*/
{
    condition_pos = init(ex, partitions, threshold);
}

template<typename T>
bool ROC::calc(const std::vector<T>& expected,
            const data_array& predict)
{
    if (!expected.empty() && expected.size() == predict.size()) {
        value_type mean = 0.f;
        results.rmse = calculateRmse(expected, predict, mean);
        algo_params data4calc{expected, predict, mean};
        if (data4calc.condition_pos != 0) {
            std::tie(results.area, results.accuracy, results.sensitivity, results.fall_out) =
                do_calculation(std::move(data4calc));
            return !results.sensitivity.empty();
        }
    }
    return false;
}

bool ROC::calculate(const data_array& expected,
                    const data_array& predict)
{
    return calc(expected, predict);
}

bool ROC::classes_calculate(const std::vector<int>& expected,
                const data_array& predict)
{
    return calc(expected, predict);
}

bool ROC::classes_calculate(const data_array& expected,
                const data_array& predict)
{
    return calc(expected, predict);
}

std::ostream& operator << (std::ostream& os, const ROC& roc)
{
    os<<"area: "<<std::fixed<<roc.results.area<<" RMS error: "<<roc.results.rmse<<
        " accuracy: "<<roc.results.accuracy;
    os<<"sensitivity   fall_out\n";
    boost::transform(roc.results.sensitivity, roc.results.fall_out,
                    std::ostream_iterator<std::string>(os, "\n"), [](auto s, auto f) {
                        return std::to_string(s) + "      " + std::to_string(f);
                    }
            );
    return os;
}

}   // end of namespace exprs


// Copyright (c) 2020 [Yihong Jian]. All rights reserved.

#include <mylibrary/matrixsolver.h>
#include "mylibrary/util.h"
#include <Eigenvalues>

// Taken from numcpp reference
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> EigenIntMatrix;
typedef Eigen::Map<EigenIntMatrix> EigenMatrixMap;

namespace matrixsolver {

    string Rref(const string& input) {
        // Convert to 2d array and solve
        vector<vector<double>> mat;
        try {
            mat = util::StringTo2dVec(input);
        } catch (exception e) {
            // I can't catch "int e" for some reason.
            return e.what();
        }
        size_t rank = MatReducer(mat);

        // Strech out 2dvec to a string
        string out;
        for (const auto& r:mat) {
            out += "[";
            for (const auto& c:r) {
                out += to_string(c);
                out += ", ";
            }
            out += "]\n";
        }
        return "Rank is " + to_string(rank)
               + "\nReduced Row Echelon Form is\n" + out;
    }

    // Took this from https://github.com/yicheng-w/acm-icpc-notebook/blob/master/general-algorithm/rref.cpp
    int MatReducer(vector<vector<double>>& mat) {
        int num_row = mat.size();
        int num_col = mat[0].size();
        int row = 0;
        for (int col = 0; col < num_col && row < num_row; col++) {
            // Find pivot rows, checking if next n-rows are greater than current
            // row on leading index
            int j = row;
            for (int i = row + 1; i < num_row; i++) {
                if (fabs(mat[i][col]) > fabs(mat[j][col]))
                    j = i;
            }
            if (fabs(mat[j][col]) < 1e-10)
                continue;
            swap(mat[j], mat[row]);

            // Normalize each row based with 1
            // subtract previous row.
            double s = 1.0 / mat[row][col];
            for (int j1 = 0; j1 < num_col; j1++) mat[row][j1] *= s;
            for (int i = 0; i < num_row; i++)
                if (i != row) {
                    double t = mat[i][col];
                    for (int j2 = 0; j2 < num_col; j2++) {
                        mat[i][j2] -= t * mat[row][j2];
                    }
                }
            row++;
        }
        return row;
    }

    string LUDecomp(const string& input) {
        NdArray<double> mat;
        try {
            mat = util::StringToMat(input);
        } catch (exception e) {
            // I can't catch "int e" for some reason.
            return e.what();
        }

        try {
            // LUDecomp may encounter error such as rectangular matrix
            // So we just catch whatever and print it out.
            auto lu_res = linalg::lu_decomposition(mat);
            string l = util::MatToString(get<0>(lu_res));
            string u = util::MatToString(get<1>(lu_res));
            return "L is:\n" + l + "U is\n" + u;
        } catch (exception e) {
            return e.what();
        }
    }

    string Det(const string& input) {
        vector<vector<double>> mat;
        try {
            mat = util::StringTo2dVec(input);
        } catch (exception e) {
            return e.what();
        }

        size_t n = mat.size();
        if (n != mat[0].size())
            return "Determinant Requires Square Matrix";

        double det = 1.0;

        for (int i = 0; i < n; ++i) {
            double pivotElement = mat[i][i];
            int pivotRow = i;
            for (int row = i + 1; row < n; ++row) {
                if (std::abs(mat[row][i]) > std::abs(pivotElement)) {
                    pivotElement = mat[row][i];
                    pivotRow = row;
                }
            }
            if (pivotElement == 0.0) {
                det = 0.0;
                break;
            }
            if (pivotRow != i) {
                mat[i].swap(mat[pivotRow]);
                det *= -1.0;
            }
            det *= pivotElement;

            for (int row = i + 1; row < n; ++row) {
                for (int col = i + 1; col < n; ++col) {
                    mat[row][col] -= mat[row][i] * mat[i][col] / pivotElement;
                }
            }
        }

        return "Determinant is " + to_string(det);
    }

    string Eig(const string& input) {
        NdArray<double> mat;
        try {
            mat = util::StringToMat(input);
        } catch (exception e) {
            // I can't catch "int e" for some reason.
            return e.what();
        }

        if (mat.numCols() != mat.numRows())
            return "Can't perform eigen calculation with non-square matrix";
        // Map numcpp array to Eigen Matrix
        auto eigen_mat = EigenMatrixMap(mat.data(), mat.numRows(), mat.numCols());
        // Retrieve Eigen values
        Eigen::EigenSolver<Eigen::MatrixXd> es(eigen_mat);
        Eigen::VectorXd eigen_values = es.eigenvalues().real();
        Eigen::MatrixXd eigen_vectors = es.eigenvectors().real();

        // Map eigen matrix to string
        ostringstream val;
        val << eigen_values;
        ostringstream vec;
        vec << eigen_vectors;

        return "CAUTION, complex eigenvalue caused UNDEFINED behavior\n"
               "Eigenvalues are\n" + val.str() +
               "\nEigenvectors(in columns) are\n" + vec.str();
    }

    string SVD(const string& input) {
        NdArray<double> mat;
        try {
            mat = util::StringToMat(input);
        } catch (exception e) {
            // I can't catch "int e" for some reason.
            return e.what();
        }

        nc::NdArray<double> u;
        nc::NdArray<double> s;
        nc::NdArray<double> vt;
        try {
            linalg::svd(mat, u, s, vt);
        } catch (exception e) {
            // I can't catch "int e" for some reason.
            return e.what();
        }

        return "U is:\n" + util::MatToString(u)
               + "S is\n" + util::MatToString(s)
               + "V-Transpose is\n" + util::MatToString(vt);
    }

    string Inv(const string& input) {
        NdArray<double> mat;
        try {
            mat = util::StringToMat(input);
        } catch (exception e) {
            // I can't catch "int e" for some reason.
            return e.what();
        }

        try {
            return "Inverse is\n" + util::MatToString(linalg::inv(mat));
        } catch (exception e) {
            return e.what();
        }
    }

    pair<string, string> PowerIter(const string& input, const string& init_guess) {
        NdArray<double> mat;
        NdArray<double> vec;
        try {
            mat = util::StringToMat(input).astype<double>();
            vec = util::StringToMat(init_guess, true).astype<double>();
        } catch (exception e) {
            return make_pair(e.what(), init_guess);
        }

        try {
            // Normalize input to prevent overflow
            // Broadcasting doesn't work too well between arrays, so
            // I extracted the double contained in array for broadcasting
            NdArray<double> vec_normed = vec / vec.norm()(0, 0);
            NdArray<double> y = mat.dot(vec_normed);
            NdArray<double> res = y / y.norm()(0, 0);
            return make_pair("Initial guess after 1 iteration is\n" + util::MatToString(res),
                             util::VecToLine(res));
        } catch (exception e) {
            return make_pair(e.what(), init_guess);
        }
    }

    string LstSq(const string& input, const string& init_guess) {
        NdArray<double> A;
        // Assume b is only one column
        NdArray<double> b;

        try {
            A = util::StringToMat(input);
            b = util::StringToMat(init_guess, true);
        } catch (exception e) {
            return e.what();
        }

        if (A.numRows() != b.numRows())
            return "For Ax = b, rows of A and b does not match";

        try {
            return util::MatToString(nc::linalg::lstsq(A, b));
        } catch (exception e) {
            return e.what();
        }
    }

}  // namespace matrixsolver


#include <iostream>
#include <limits>

#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <unsupported/Eigen/NonLinearOptimization>

#include <opencv2/calib3d/calib3d.hpp>
#include <sys/stat.h>

#include "Optimization.h"
#include "math.hh"

bool translation_gauss_newton(const std::vector<cv::Point3d>& world_pts, const std::vector<cv::Point2d>& image_pts,
                              const Eigen::Matrix3d &K, const Eigen::Quaterniond& Q, Eigen::Vector3d &T,
                              double& residual, int& iter)
//----------------------------------------------------------------------------------------------------------------------
{
   double tx = T[0], ty = T[1], tz = T[2];
   const Eigen::Matrix3d KI = K.inverse();
   size_t m = std::min(world_pts.size(), image_pts.size());
   std::vector<Eigen::Quaterniond> world_quaternions;
   std::vector<Eigen::Vector3d> image_rays;
   const Eigen::Quaterniond QI = Q.inverse();
   for (size_t row = 0; row < m; row++)
   {
      cv::Point2d &pt = const_cast<cv::Point2d &>(image_pts[row]);
      Eigen::Vector3d pt2d = KI * Eigen::Vector3d(pt.x, pt.y, 1);
      pt2d /= pt2d[2];
      image_rays.push_back(pt2d);
      const cv::Point3d pt3d = world_pts[row];
      const Eigen::Quaterniond QR = Q * Eigen::Quaterniond(0, pt3d.x, pt3d.y, pt3d.z) * QI;
      world_quaternions.push_back(QR);
   }
   iter = 0;
   double prev_min_residual = std::numeric_limits<double>::max();
   double eps = 0.0000001;
   do
   {
      Eigen::MatrixXd J(m, 3);
//      Eigen::MatrixXd r(m, 3);
      Eigen::MatrixXd r(m, 1);
      double min_residual = std::numeric_limits<double>::max();
      for (size_t row = 0; row < m; row++)
      {
         Eigen::Vector3d pt2d = image_rays[row];
         double u = pt2d[0], v = pt2d[1];
         const Eigen::Vector3d Rv = world_quaternions[row].vec();
         //  Eigen::Vector3d Rr = R*Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z);
         Eigen::Vector3d pt3d = Rv + Eigen::Vector3d(tx, ty, tz);
         pt3d /= pt3d[2];
         Eigen::Vector3d diff = pt3d - pt2d;
         residual = diff.dot(diff);
         if (residual < min_residual)
            min_residual = residual;
//         std::cout << "refine: " << pt3d.transpose() << " " << pt2d.transpose() << " " << diff.transpose() << " " << residual << std::endl;
         Eigen::Vector3d d;
         J.row(row) << 2 * (Rv[0] + tx - u), 2 * (Rv[1] + ty - v), 2 * (Rv[2] + tz - 1);
         r.row(row) << residual;
      }
//      std::cout << "minmax " << min_residual << " " << (min_residual - prev_min_residual) <<  std::endl;
      if (min_residual > prev_min_residual) break;
      prev_min_residual = min_residual;
      auto Jt = J.transpose();
//      std::cout << "JTr: " << std::endl << (Jt * r) << std::endl << "==================== " << std::endl;

      //auto llt = (Jt * J).ldlt();
      auto llt = (Jt * J).llt();
      double dx, dy, dz;
      if (llt.info() == Eigen::Success)
      {
         auto delta =  llt.solve(Jt * r * (-1.0));
         dx = delta(0, 0), dy = delta(1, 0), dz = delta(2, 0);
//         std::cout << delta << std::endl;
      }
      else
      {
         auto JtJI = (Jt*J).inverse();
         auto dd = -(JtJI*Jt);
         auto delta = dd*r;
         dx = delta(0, 0), dy = delta(1, 0), dz = delta(2, 0);
      }
      if ( (mut::near_zero(dx, eps)) && (mut::near_zero(dy, eps)) && (mut::near_zero(dz, eps)) ) break;
      tx += dx;
      ty += dy;
      tz += dz;
   } while (iter++ < 200);
   if ( (iter > 1) && (iter < 100) )
   {
      T[0] = tx; T[1] = ty; T[2] = tz;
      return true;
   }
   return false;
}

bool translation_levenberg_marquardt3d(const std::vector<cv::Point3d>& world_pts, const std::vector<cv::Point2d>& image_pts,
                                     const Eigen::Matrix3d &K, const Eigen::Quaterniond& Q, Eigen::Vector3d &T,
                                     int& iterations)
//-----------------------------------------------------------------------------------------------------------------------
{
   TranslationLevenbergMarquardt3D functor(world_pts, image_pts, K, Q);
   Eigen::LevenbergMarquardt<TranslationLevenbergMarquardt3D, double> lm(functor);
   Eigen::VectorXd x(3);
   x << T[0], T[1], T[2];
   iterations = 0;
   Eigen::LevenbergMarquardtSpace::Status status = lm.minimizeInit(x);
   if (status == Eigen::LevenbergMarquardtSpace::ImproperInputParameters)
      return false;
   do
   {
      status = lm.minimizeOneStep(x);
      if (status == Eigen::LevenbergMarquardtSpace::Running)
         iterations++;
   } while (status == Eigen::LevenbergMarquardtSpace::Running);
   if (iterations > 1)
   {
      T = x;
      return true;
   }
   return false;
}

bool translation_levenberg_marquardt2d_depth(const std::vector<cv::Point3d>& world_pts,
                                             const std::vector<cv::Point3d>& image_pts, const Eigen::Matrix3d &K,
                                             const Eigen::Quaterniond& Q, Eigen::Vector3d &T, const double depth,
                                             int& iterations)
//--------------------------------------------------------------------------------------------------------------
{
   TranslationLevenbergMarquardt2DDepth functor(world_pts, image_pts, K, Q, depth);
   Eigen::LevenbergMarquardt<TranslationLevenbergMarquardt2DDepth, double> lm(functor);
   Eigen::VectorXd x(3);
   x << T[0], T[1], T[2];
   iterations = 0;
   Eigen::LevenbergMarquardtSpace::Status status = lm.minimizeInit(x);
   if (status == Eigen::LevenbergMarquardtSpace::ImproperInputParameters)
      return false;
   do
   {
      status = lm.minimizeOneStep(x);
      if (status == Eigen::LevenbergMarquardtSpace::Running)
         iterations++;
   } while (status == Eigen::LevenbergMarquardtSpace::Running);
   if (iterations > 1)
   {
      T = x;
      return true;
   }
   return false;
}


// TinyVector<T,N> DAXPY benchmark

//#define BZ_DISABLE_KCC_COPY_PROPAGATION_KLUDGE

#include <blitz/array.h>
#include <blitz/timer.h>
#include <random/uniform.h>

BZ_USING_NAMESPACE(blitz)

ranlib::Uniform<double> rnd;

template<class T>
void optimizationSink(T&);

template<int N_rank>
void tinyDAXPYBenchmark(TinyVector<double,N_rank>, int iters, double a)
{
    Timer timer;

    TinyVector<double,N_rank> ta, tb, tc, td, te, tf, tg, th, ti, tj;
    for (int i=0; i < N_rank; ++i)
    {
        ta[i] = rnd.random()+1;
        tb[i] = rnd.random()+1;
        tc[i] = rnd.random()+1;
        td[i] = rnd.random()+1;
        te[i] = rnd.random()+1;
        tf[i] = rnd.random()+1;
        tg[i] = rnd.random()+1;
        th[i] = rnd.random()+1;
        ti[i] = rnd.random()+1;
        tj[i] = rnd.random()+1;
    }

    double b = -a;

    double numFlops = 0;

    if (N_rank < 20)
    {
      timer.start();
      for (int i=0; i < iters; ++i)
      {
        ta += a * tb;
        tc += a * td;
        te += a * tf;
        tg += a * th;
        ti += a * tj;
        tb += b * ta;
        td += b * tc;
        tf += b * te;
        th += b * tg;
        tj += b * ti;
        ta += a * tb;
        tc += a * td;
        te += a * tf;
        tg += a * th;
        ti += a * tj;
        tb += b * ta;
        td += b * tc;
        tf += b * te;
        th += b * tg;
        tj += b * ti;
      }
      timer.stop();
      numFlops = 40.0 * N_rank * double(iters);
    }
    else {
      timer.start();
      for (int i=0; i < iters; ++i)
      {
        ta += a * tb;
        tb += b * ta;
      }
      timer.stop();
      numFlops = 4.0 * N_rank * double(iters);
    }

    optimizationSink(ta);
    optimizationSink(tb);
    optimizationSink(tc);
    optimizationSink(td);
    optimizationSink(te);
    optimizationSink(tf);
    optimizationSink(tg);
    optimizationSink(th);
    optimizationSink(ti);
    optimizationSink(tj);

    timer.stop();
    float Gflops = numFlops / (1e9*timer.elapsed());

    if (iters > 1)
    {
    cout << setw(5) << N_rank << '\t' << Gflops << endl;
    }
}

double a = 0.3429843;

template<class T>
void optimizationSink(T&)
{
}

int main()
{
    cout << "TinyVector<double,N> DAXPY benchmark" << endl
         << setw(5) << "N" << '\t' << "Gflops/" << Timer::indep_var() << endl;
    tinyDAXPYBenchmark(TinyVector<double,1>(), 800000, a);
    tinyDAXPYBenchmark(TinyVector<double,2>(), 800000, a);
    tinyDAXPYBenchmark(TinyVector<double,3>(), 800000, a);
    tinyDAXPYBenchmark(TinyVector<double,4>(), 700000, a);
    tinyDAXPYBenchmark(TinyVector<double,5>(), 600000, a);
    tinyDAXPYBenchmark(TinyVector<double,6>(), 500000, a);
    tinyDAXPYBenchmark(TinyVector<double,7>(), 500000, a);
    tinyDAXPYBenchmark(TinyVector<double,8>(), 500000, a);
    tinyDAXPYBenchmark(TinyVector<double,9>(), 500000, a);
    tinyDAXPYBenchmark(TinyVector<double,10>(), 500000, a);

    return 0;
}


#ifndef HOPS_NORMALIZEPOLYTOPE_HPP
#define HOPS_NORMALIZEPOLYTOPE_HPP

#include <Eigen/Core>

namespace hops {

    /**
     * @brief Normalizes polytope defined by Ax < b
     * @tparam Derived1
     * @tparam Derived2
     * @param A Dense representation of A
     * @param b
     */
    template<typename Derived1, typename Derived2>
    void normalizePolytope(Eigen::MatrixBase<Derived1> &A, Eigen::MatrixBase<Derived2> &b) {
        for (int i = 0; i < A.rows(); ++i) {
            const double norm = A.row(i).template lpNorm<2>();
            A.row(i) /= norm;
            b(i) /= norm;
        }
    }
}

#endif //HOPS_NORMALIZEPOLYTOPE_HPP


#include "CubicInterpolation/InterpolantBuilder.h"

#include <boost/math/differentiation/finite_difference.hpp>
#include <Eigen/Dense>

namespace cubic_splines {

/* template <> */
/* BicubicSplines */
/* InterpolantBuilder<BicubicSplines>::build(BicubicSplines::Definition const &def, */
/*                                           std::string save_path, */
/*                                           std::string filename) { */
/*   using boost::math::differentiation::finite_difference_derivative; */
/*   try { */
/*     return load(save_path, filename); */
/*   } catch (std::system_error const &ex) { */
/*     if (ex.code().value() != ENOENT) */
/*       throw ex; */
/*   } */
/*   auto x1nodes = def.axis[0]->required_nodes(); */
/*   auto x2nodes = def.axis[1]->required_nodes(); */
/*   auto data = BicubicSplines::RuntimeData(); */
/*   auto y = Eigen::MatrixXf(x1nodes, x2nodes); */
/*   auto dydx1 = Eigen::MatrixXf(x1nodes, x2nodes); */
/*   auto dydx2 = Eigen::MatrixXf(x1nodes, x2nodes); */
/*   auto d2ydx1dx2 = Eigen::MatrixXf(x1nodes, x2nodes); */
/*   auto func = [this, &def](double x1, double x2) { */
/*     return transform(def, x1, x2); */
/*   }; */
/*   for (size_t n1 = 0; n1 < x1nodes; ++n1) { */
/*     for (size_t n2 = 0; n2 < x2nodes; ++n2) { */
/*       auto x1 = def.axis[0]->back_transform(n1); */
/*       auto x2 = def.axis[1]->back_transform(n2); */

/*       auto dfdx1 = def.axis[0]->derive(x1); */
/*       auto dfdx2 = def.axis[1]->derive(x2); */

/*       y(n1, n2) = func(x1, x2); */
/*       dydx1(n1, n2) = */
/*           finite_difference_derivative( */
/*               [this, &func, x2](double x) { return func(x, x2); }, x1) * */
/*           dfdx1; */
/*       dydx2(n1, n2) = */
/*           finite_difference_derivative( */
/*               [this, &func, x1](double x) { return func(x1, x); }, x2) * */
/*           dfdx2; */
/*       d2ydx1dx2(n1, n2) = */
/*           finite_difference_derivative( */
/*               [this, &func, x1, x2, dfdx1, dfdx2](double x_1) { */
/*                 return finite_difference_derivative( */
/*                            [this, &func, x_1, dfdx2](double x_2) { */
/*                              return func(x_1, x_2); */
/*                            }, */
/*                            x2) * */
/*                        dfdx2; */
/*               }, */
/*               x1) * */
/*           dfdx1; */
/*     } */
/*   } */
/*   bool sucess = save(save_path, filename, y, dydx1, dydx2, d2ydx1dx2); */
/*   if (not sucess) */
/*     std::cout << "storage of tables have failed" << std::endl; */
/*   return BicubicSplines(y, dydx1, dydx2, d2ydx1dx2); */
/* } */

/* template <> */
/* CubicSplines */
/* InterpolantBuilder<CubicSplines>::build(CubicSplines::Definition const &def, */
/*                                         std::string path, */
/*                                         std::string filename) { */
/*   using boost::math::differentiation::finite_difference_derivative; */
/*   try { */
/*     return load(path, filename); */
/*   } catch (std::system_error const &ex) { */
/*     if (ex.code().value() != ENOENT) */
/*       throw ex; */
/*   } */
/*   auto y = std::vector<double>(def.axis->required_nodes()); */
/*   auto func = [this, &def](double x) { return transform(def, x); }; */
/*   /1* auto func = [&def](double x) { *1/ */
/*   /1*   auto fx = def.f(x); *1/ */
/*   /1*   if (def.f_trafo) *1/ */
/*   /1*     fx = def.f_trafo->transform(fx); *1/ */
/*   /1*   return fx; *1/ */
/*   /1* }; *1/ */
/*   for (size_t n = 0; n < y.size(); ++n) */
/*     y[n] = func(def.axis->back_transform(n)); */
/*   auto low = def.axis->back_transform(0); */
/*   auto low_lim_derivate = */
/*       finite_difference_derivative(func, low) * def.axis->derive(low); */
/*   auto up = def.axis->back_transform(y.size() - 1); */
/*   auto up_lim_derivate = finite_difference_derivative( */
/*                              func, def.axis->back_transform(y.size() - 1)) * */
/*                          def.axis->derive(up); */
/*   bool sucess = save(path, filename, y, low_lim_derivate, up_lim_derivate); */
/*   if (not sucess) */
/*     std::cout << "storage of tables have failed" << std::endl; */
/*   return CubicSplines(y, low_lim_derivate, up_lim_derivate); */
/* } */

} // namespace cubic_splines


#include <armadillo>
#include <iostream>
#include <ostream>
#include <target/odesolver.hpp>
#include <target/utils.hpp>

using arma::vec;
using arma::mat;
using std::cout;
using std::endl;

vec dy(const vec &input,  // time (first element) and input variables
       const vec &x,      // state variables
       const vec &par) {
  return par(0) + par(1)*x;
}

int main(int argc, char **argv) {
    cout << target::BLUE << "RK4 test\n\n";

    target::RK4 MyODE(dy);
    vec t = arma::linspace(0, 2, 20);
    vec y0 = arma::zeros(1);
    vec par = { 1.0, 1.0 };
    vec y = MyODE.solve(t, y0, par);
    cout << y << endl;
    std::cout << target::COL_RESET;

    mat ty = arma::join_horiz(t, y);
    ty.save("y.csv", arma::csv_ascii);

    return 0;
}


#include <iostream>
#include <stdio.h>

#include <Eigen/Dense>
//using namespace Eigen;

//#include <boost/array.hpp>
//#include <boost/numeric/odeint.hpp>

//using namespace boost::numeric;
//
//#include "filters.h"
//
//namespace syllo {
//
//     Kalman::Kalman()
//     {
//     }
//
//     Kalman::Kalman(const Eigen::MatrixXf &A, const Eigen::MatrixXf &B, const Eigen::VectorXf &C, const Eigen::MatrixXf &R, const Eigen::MatrixXf &Q)
//     {
//	  setModel(A, B, C, R, Q);
//     }
//
//     int Kalman::setModel(const Eigen::MatrixXf &A, const Eigen::MatrixXf &B, const Eigen::VectorXf &C, const Eigen::MatrixXf &R, const Eigen::MatrixXf &Q)
//     {
//	  this->A = A;
//	  this->B = B;
//	  this->C = C;
//	  this->R = R;
//	  this->Q = Q;
//	  eye = Eigen::MatrixXf::Identity(A.rows(), A.cols());
//
//	  return 0;
//     }
//
//     int Kalman::init(const Eigen::VectorXf &mu0, const Eigen::MatrixXf &covar0)
//     {
//	  this->mu = mu0;
//	  this->covar = covar0;
//	  return 0;
//     }
//
//     int Kalman::step(Eigen::VectorXf &mu_prev, Eigen::MatrixXf &covar_prev, const Eigen::VectorXf &u)
//     {
//	  Eigen::VectorXf mu_dx;
//
//	  mu_dx = A*mu_prev + B*u;
//
//	  mu_prev += mu_dx;
//
//	  covar = A*covar_prev*A.transpose() + R;
//	  //K = covar*C.transpose() * (C*covar*C.transpose() + Q).inverse();
//	  ////mu = mu + K*(z-C*mu);
//	  //covar = (eye - K*C) * covar;
//
//
//
//	  return 0;
//     }
//
//     int Kalman::step(Eigen::VectorXf &mu_prev, Eigen::MatrixXf &covar_prev, const Eigen::VectorXf &u, const Eigen::VectorXf &z)
//     {
//	  mu = A*mu_prev + B*u;
//	  covar = A*covar_prev*A.transpose() + R;
//
//	  std::cout << "covar: \n" << covar << std::endl;
//	  std::cout << "C: \n" << C << std::endl;
//	  std::cout << "C': \n" << C.transpose() << std::endl;
//	  std::cout << "Q: \n" << Q << std::endl;
//
//	  //K = covar*C.transpose() * (C*covar*C.transpose() + Q).inverse();
//
//          K = covar*C * (C.transpose()*covar*C + Q).inverse();
//
//	  std::cout << "mu: \n" <<  mu << std::endl;
//	  std::cout << "K: \n" <<  K << std::endl;
//	  std::cout << "z: \n" <<  z << std::endl;
//	  std::cout << "C: \n" <<  C << std::endl;
//
//	  //mu = mu + K*(z-C*mu);
//	  //mu = mu + K*(z-C*mu);
//	  z-C.transpose()*mu;
//
//          //covar = (eye - K*C) * covar;
//
//	  return 0;
//     }
//
//     Eigen::VectorXf Kalman::getMu()
//     {
//	  return mu;
//     }
//
//     Eigen::MatrixXf Kalman::getCovar()
//     {
//	  return covar;
//     }
//}


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

#include "ekf/ekf.h"
#include "ekf/utils.h"

#include <Eigen/Core>
#include <Eigen/LU>

namespace ekf {

void Predict(Ekf& ekf, double dt) {
    auto new_state = ekf.state;
    new_state(0) = ekf.state(3) * dt * std::cos(ekf.state(2)) + ekf.state(0);
    new_state(1) = ekf.state(3) * dt * std::sin(ekf.state(2)) + ekf.state(1);
    new_state(2) = wrapAngle(ekf.state(4) * dt + ekf.state(2));

    Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(5, 5);
    // x
    jacobian(0,0) = 1;
    jacobian(0,2) = -ekf.state(3) * dt * std::sin(ekf.state(2));
    jacobian(0,3) = dt * std::cos(ekf.state(2));

    // y
    jacobian(1,1) = 1;
    jacobian(1,2) = ekf.state(3) * dt * std::cos(ekf.state(2));
    jacobian(1,3) = dt * std::sin(ekf.state(2));

    // theta
    jacobian(2,2) = 1;
    jacobian(2,4) = dt;

    // v
    jacobian(3,3) = 1;

    // w
    jacobian(4,4) = 1;

    Eigen::MatrixXd process_noise = Eigen::MatrixXd::Identity(5, 5);
    process_noise = process_noise * 0.1;

    ekf.state = new_state;
    ekf.covariance = jacobian * ekf.covariance * jacobian.transpose() + process_noise;
}

bool Update(Ekf& ekf,
        const Eigen::VectorXd& z,
        const Eigen::MatrixXd& H,
        const Eigen::MatrixXd& R) {
    Eigen::VectorXd y = z - H * ekf.state;
    Eigen::MatrixXd S = H * ekf.covariance * H.transpose() + R;

    Eigen::FullPivLU<Eigen::MatrixXd> lu(S);
    if (!lu.isInvertible()) {
        return false;
    }

    Eigen::MatrixXd K = ekf.covariance * H.transpose() * S.inverse();
    ekf.state = ekf.state + K * y;
    Eigen::MatrixXd KH = K * H;
    ekf.covariance = (Eigen::MatrixXd::Identity(KH.rows(), KH.cols()) - KH) * ekf.covariance;
    return true;
}
}


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

#include "tools.hpp"

using namespace Eigen;
using namespace std;

// For converting back and forth between radians and degrees.
double pi() { return M_PI; }

double deg2rad(double x) { return x * pi() / 180.; }
double rad2deg(double x) { return x * 180. / pi(); }

double distance(double x1, double y1, double x2, double y2) {
	return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
}

double norm(double x, double y) {
	return sqrt(x*x + y*y);
}

double norm(double x, double y, double z) {
	return sqrt(x*x + y*y + z*z);
}

double mph2mps(double mph) {
	return mph / 2.2369362920544;
}

double mps2mph(double mps) {
	return mps * 2.2369362920544;
}

// Evaluate a polynomial.
vector<double> polyeval(vector<double> &coeffs, vector<double> &x)
{
    vector<double> result(x.size());
    for (int j = 0; j < x.size(); j++)
    {
        result[j] = 0;
        for (int i = 0; i < coeffs.size(); i++) {
            result[j] += coeffs[i] * pow(x[j], i);
        }
    }
    return result;
}

double polyeval(vector<double> &coeffs, double x)
{
    double result = 0;
    for (int i = 0; i < coeffs.size(); i++) {
        result += coeffs[i] * pow(x, i);
    }
    return result;
}

double polyeval(Eigen::VectorXd &coeffs, double x)
{
    double result = 0.0;
    for (int i = 0; i < coeffs.size(); i++) {
        result += coeffs[i] * pow(x, i);
    }
    return result;
}

vector<double> polyfit(vector<double> &xvals, vector<double> &yvals, int order) {
    assert(xvals.size() == yvals.size());
    assert(order >= 1 && order <= xvals.size() - 1);
    Eigen::VectorXd xvals_eig = Eigen::VectorXd::Map(xvals.data(), xvals.size());
    Eigen::VectorXd yvals_eig = Eigen::VectorXd::Map(yvals.data(), yvals.size());
	Eigen::VectorXd result_eig = polyfit(xvals_eig, yvals_eig, order);
	vector<double> result(result_eig.data(), result_eig.data() + result_eig.size());
    return result;
}

// Fit a polynomial.
// Adapted from
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
Eigen::VectorXd polyfit(Eigen::VectorXd &xvals, Eigen::VectorXd &yvals, int order) {
    assert(xvals.size() == yvals.size());
    assert(order >= 1 && order <= xvals.size() - 1);
    Eigen::MatrixXd A(xvals.size(), order + 1);

    for (int i = 0; i < xvals.size(); i++) {
        A(i, 0) = 1.0;
    }

    for (int j = 0; j < xvals.size(); j++) {
        for (int i = 0; i < order; i++) {
            A(j, i + 1) = A(j, i) * xvals(j);
        }
    }

    auto Q = A.householderQr();
    auto result = Q.solve(yvals);
    return result;
}

vector<double> polyfit_wp(int wp_start, int wp_stop, int order,
                          vector<double> &map_x, vector<double> &map_y)
{
    assert(map_x.size() == map_y.size());
    int wp_count = wp_stop - wp_start;
    Eigen::VectorXd xvals_eig(wp_count);
    Eigen::VectorXd yvals_eig(wp_count);
    Eigen::MatrixXd A(wp_count, order+1);
    // make sure indicies for map_x and map_y wrap around map size!
    wp_start = wp_start % map_x.size();
    wp_stop  = wp_stop  % map_x.size();

    for (int idx = 0; idx < wp_count; idx++) {
        xvals_eig(idx) = map_x[wp_start+idx];
        yvals_eig(idx) = map_y[wp_start+idx];
    }

    for (int i = 0; i < wp_count; i++) {
        A(i, 0) = 1.0;
    }

    for (int j = 0; j < wp_count; j++) {
        for (int i = 0; i < order; i++) {
            A(j, i + 1) = A(j, i) * xvals_eig(j);
        }
    }
    /*
    IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
    cout << "A: " << endl << A.format(CleanFmt) << endl;
    */
    auto Q = A.householderQr();
    VectorXd result_eig = Q.solve(yvals_eig);
    vector<double> result(result_eig.data(), result_eig.data() + result_eig.size());
    return result;
}

int getLane(const double d, const double laneWidth) {
    for (int lane = 0; lane <= 2; lane++) {
        if (d > laneWidth*lane && d <= laneWidth*(lane+1)) {
            return lane;
        }
    }
    cout << "couldn't find lane for d=" << d << endl;
    return 0;
}

double getLaneOffsetD(const int lane_number, const double laneWidth) {
    return (laneWidth*lane_number) + 2.0;
}

vector<double> JMT(vector< double> start, vector <double> end, double T)
{
    /*
    Calculate the Jerk Minimizing Trajectory that connects the initial state
    to the final state in time T.

    INPUTS

    start - the vehicles start location given as a length three array
            corresponding to initial values of [s, s_dot, s_double_dot]

    end   - the desired end state for vehicle. Like "start" this is a
            length three array.

    T     - The duration, in seconds, over which this maneuver should occur.

    OUTPUT
    an array of length 6, each value corresponding to a coefficent in the polynomial
    s(t) = a_0 + a_1 * t + a_2 * t**2 + a_3 * t**3 + a_4 * t**4 + a_5 * t**5

    */

    double T2, T3, T4, T5;
    T2 = T*T;
    T3 = T2*T;
    T4 = T3*T;
    T5 = T4*T;

    MatrixXd c(3,1);
    c << end[0] - (start[0] + start[1]*T + 0.5*start[2]*T2),
         end[1] - (start[1] + start[2]*T),
         end[2] -  start[2];

    MatrixXd A(3,3);
    A <<   T3,    T4,    T5,
         3*T2,  4*T3,  5*T4,
         6*T,  12*T2, 20*T3;

    MatrixXd b = A.inverse() * c;

    return {start[0], start[1], 0.5*start[2],
            b(0),     b(1),     b(2)};
}


#include <iostream>
#include <cstdlib>

#include <Eigen/QR>
#include <unsupported/Eigen/MatrixFunctions>

//#define TVMTL_MANIFOLD_DEBUG
//#define TVMTL_MANIFOLD_DEBUG_GRASSMANN
#include <mtvmtl/core/manifold.hpp>

using namespace tvmtl;

const int N=3;
const int P=2;

typedef Manifold<GRASSMANN, N, P> mf_t;

typedef typename mf_t::value_type mat;

template <class T>
void test(T& vec1, T& vec2){

	std::cout << mf_t::MyType << std::endl;

	std::cout << "Vector 1:\n" << vec1 << std::endl;
	std::cout << "\nVector 2:\n" << vec2 << std::endl;

	std::cout<< "\n\n==========DISTANCES TEST==========" << std::endl;
	std::cout << "Geodesic distance: " << mf_t::distGeod_squared(vec1, vec2) << std::endl;
	std::cout << "Projection F distance: " << mf_t::distPF_squared(vec1, vec2) << std::endl;

	std::cout<< "\n\n==========PERMUTATION MATRIX TEST==========" << std::endl;

	auto vec1t = vec1.transpose();
	Eigen::VectorXd vecvec1 = Eigen::Map<Eigen::VectorXd>(vec1.data(), vec1.size());
	Eigen::VectorXd vecvec1t = Eigen::Map<Eigen::VectorXd>(vec1t.data(), vec1t.size());

	std::cout << "\n\nTest of the Permutation Matrix Knp:" <<std::endl;
	std::cout << "s1:\n" << vec1 << std::endl;
	std::cout << "s1^t:\n" << vec1t << std::endl;
	std::cout << "\nVectorized s1:\n" << vecvec1 << std::endl;
	std::cout << "\nVectorized s1^t:\n" << vecvec1t << std::endl;
	std::cout << "\nPermutation Matrix:\n" << mf_t::permutation_matrix.toDenseMatrix() << std::endl;
	std::cout << "\nPermutation Matrix Indices:\n" << mf_t::permutation_matrix.indices() << std::endl;
	std::cout << "\nP * Vectorized s1:\n" << mf_t::permutation_matrix * vecvec1 << std::endl;

	std::cout<< "\n\n==========DERIVATIVE COMPUTATION TEST==========" << std::endl;
	typedef typename mf_t::deriv2_type mat9x9;

	mat d1x,d1y;
	mf_t::deriv1x_dist_squared(vec1, vec2, d1x);
	mf_t::deriv1y_dist_squared(vec1, vec2, d1y);
	mat9x9 d2xx, d2xy, d2yy;
	mf_t::deriv2xx_dist_squared(vec1, vec2, d2xx);
	mf_t::deriv2xy_dist_squared(vec1, vec2, d2xy);
	mf_t::deriv2yy_dist_squared(vec1, vec2, d2yy);

	std::cout << "\nFirst Derivative:" << std::endl;
	std::cout << d1x << std::endl;
	std::cout << std::endl;
	std::cout << d1y << std::endl;

	std::cout << "\nSecond Derivative:" << std::endl;
	std::cout << d2xx << std::endl;
	std::cout << std::endl;
	std::cout << d2xy << std::endl;
	std::cout << std::endl;
	std::cout << d2yy << std::endl;

	std::cout<< "\n\n==========TANGENT SPACE BASIS TEST==========" << std::endl;
	mf_t::tm_base_type t, t2;
	mf_t::tangent_plane_base(vec1, t);
	std::cout << "\nTangent Base Restriction:" << std::endl;
	std::cout << t << std::endl;
	std::cout << "\nTangent Base Restriction alternative calculatiopn: " << std::endl;
	Eigen::HouseholderQR<mf_t::value_type> qr(vec1);
	Eigen::Matrix<mf_t::scalar_type, N, N> Q = qr.householderQ();
	Eigen::Matrix<mf_t::scalar_type, N, N - P> vec1orth = Q.rightCols(N-P);
	t2 = Eigen::kroneckerProduct(Eigen::Matrix<mf_t::scalar_type, P, P>::Identity(), vec1orth.transpose());
	std::cout << t2 << std::endl;

	std::cout<< "\n\n==========RESTRICTED DERIVIATIVES COMPUTATION TEST==========" << std::endl;
	mf_t::restricted_deriv2_type rd2xx, rd2xy, rd2yy;
	rd2xx = t.transpose() * d2xx * t;
	rd2xy = t.transpose() * d2xy * t;
	rd2yy = t.transpose() * d2yy * t;
	std::cout << "\nRestricted second Derivatives:" << std::endl;
	std::cout << rd2xx << std::endl;
	std::cout << std::endl;
	std::cout << rd2xy << std::endl;
	std::cout << std::endl;
	std::cout << rd2yy << std::endl;

	std::cout<< "\n\n==========EXPONENTIAL LOGARITHM CONSISTENCY CHECK==========" << std::endl;
	mf_t::value_type u, z, v;
	std::cout << "\n\nExponential map test:" << std::endl;
	mf_t::exp(vec1, vec2, z);
	std::cout << "Exp(s1,s2) = \n" << z << std::endl;

	mf_t::log(vec1, vec2, u);
	mf_t::exp(vec1, u, z);
	mf_t::log(vec1, z, v);
	std::cout << "\nLogarithm map test:" << std::endl;
	std::cout << "U = Log(s1, s2) = \n" << u << std::endl;
	std::cout << "\nThe next two expression should be the same:" << std::endl;
	std::cout << "distGeod_squared(s1, s2) = " << mf_t::distGeod_squared(vec1,vec2) << std::endl;
	std::cout << "tr U^TU = " << (u.transpose()*u).trace() << std::endl;

	std::cout << "\nZ = Exp(s1, U) = \n"  << z << "\n and s2  =\n" << vec2 << "\nshould have distance close to zero for geodesic and projection F-norm :\n";
	std::cout << "\ndistGeod_squared(Z, s2) = " << mf_t::distGeod_squared(z,vec2) << std::endl;
	std::cout << "distPF_squared(Z, s2) = " << mf_t::distPF_squared(z,vec2) << std::endl;
//	std::cout << "\nV = Log(s1, Z) = \n" << v << std::endl;

	std::cout<< "\n\n==========KARCHER MEAN CONSISTENCY TEST==========" << std::endl;
	mf_t::value_type kmean, d1, d2, sum;
	mf_t::karcher_mean(kmean, vec1, vec2);

	std::cout << "\nK = K(s1, s2) =\n " << kmean << std::endl;
	mf_t::log(kmean, vec1, d1);
	mf_t::log(kmean, vec2, d2);
	sum = d1 + d2;
	std::cout << "\n||Sum_i Log(K, si)|| should be close to zero:" << (sum.transpose()*sum).trace() << std::endl;
	std::cout << "\nThe following four distances should all be approximately equal, the upper pair and the lower pair should be exactly equal:\n";
	std::cout << "Geodesic squared distance between s1 and K: " << mf_t::distGeod_squared(vec1, kmean) << std::endl;
	std::cout << "Geodesic squared distance between s2 and K: " << mf_t::distGeod_squared(vec2, kmean) << std::endl;
	std::cout << "Projection F squared distance between s1 and K: " << mf_t::distPF_squared(vec1, kmean) << std::endl;
	std::cout << "Projection F squared distance between s2 and K: " << mf_t::distPF_squared(vec2, kmean) << std::endl;

	std::cout<< "\n\n==========KARCHER MEAN NEWTON TEST==========" << std::endl;
	mf_t::value_type grad, X, Y, s;
	mat9x9 H;
	Eigen::VectorXd G, S;

	typedef Eigen::Matrix<mf_t::scalar_type, N, N> matn;
	typedef Eigen::Matrix<mf_t::scalar_type, P, P> matp;
	typedef Eigen::Matrix<mf_t::scalar_type, P, N> matpn;

	matn I = Eigen::Matrix<mf_t::scalar_type, N, N>::Identity();
	matp Ip = Eigen::Matrix<mf_t::scalar_type, P, P>::Identity();

	matn XorthP, H1;
	matp H2;

	X=vec1;

	for (int i=0; i<8; ++i){
	    XorthP = I - X * X.transpose();
	    grad = -XorthP * vec1 * vec1.transpose() * X - XorthP * vec2 * vec2.transpose() * X;
	    H1 = XorthP * vec1 * vec1.transpose() + XorthP * vec2 * vec2.transpose();
	    H2 = X.transpose() * vec1 * vec1.transpose() * X +  X.transpose() * vec2 * vec2.transpose() * X;
	    H = kroneckerProduct(Ip, H1) - kroneckerProduct(H2.transpose(), I);
	    G = Eigen::Map<Eigen::VectorXd>(grad.data(), grad.size());
	    S = H.fullPivLu().solve(G);
	    s = Eigen::Map<mf_t::value_type>(S.data());
	    mf_t::exp(X,s,X);
	}
	std::cout << "\nK = K(s1, s2) =\n " << X << std::endl;

	std::cout << "\n||grad|| should be close to zero:" << (grad.transpose()*grad).trace() << std::endl;
	std::cout << "\nThe following four distances should all be approximately equal, the upper pair and the lower pair should be exactly equal:\n";
	std::cout << "Geodesic squared distance between s1 and K: " << mf_t::distGeod_squared(vec1, X) << std::endl;
	std::cout << "Geodesic squared distance between s2 and K: " << mf_t::distGeod_squared(vec2, X) << std::endl;
	std::cout << "Projection F squared distance between s1 and K: " << mf_t::distPF_squared(vec1, X) << std::endl;
	std::cout << "Projection F squared distance between s2 and K: " << mf_t::distPF_squared(vec2, X) << std::endl;

	std::cout << "\n\n------WITH TANGENT SPACE RESTRICTION-----"    << std::endl;
	X=vec1;

	mf_t::restricted_deriv2_type HR;
	Eigen::VectorXd GR, SR;
	mf_t::tm_base_type tb;

	for (int i=0; i<8; ++i){
	    XorthP = I - X * X.transpose();
	    grad = -XorthP * vec1 * vec1.transpose() * X - XorthP * vec2 * vec2.transpose() * X;
	    H1 = XorthP * vec1 * vec1.transpose() + XorthP * vec2 * vec2.transpose();
	    H2 = X.transpose() * vec1 * vec1.transpose() * X +  X.transpose() * vec2 * vec2.transpose() * X;
	    H = kroneckerProduct(Ip, H1) - kroneckerProduct(H2.transpose(), I);
	    G = Eigen::Map<Eigen::VectorXd>(grad.data(), grad.size());
	    mf_t::tangent_plane_base(X, tb);
	    HR = tb.transpose() * H * tb;
	    GR = tb.transpose() * G;
	    SR = HR.fullPivLu().solve(GR);
	    S = tb * SR;
	    s = Eigen::Map<mf_t::value_type>(S.data());
	    mf_t::exp(X,s,X);
	}

	std::cout << "\nK = K(s1, s2) =\n " << X << std::endl;

	std::cout << "\n||grad|| should be close to zero:" << (grad.transpose()*grad).trace() << std::endl;
	std::cout << "\nThe following four distances should all be approximately equal, the upper pair and the lower pair should be exactly equal:\n";
	std::cout << "Geodesic squared distance between s1 and K: " << mf_t::distGeod_squared(vec1, X) << std::endl;
	std::cout << "Geodesic squared distance between s2 and K: " << mf_t::distGeod_squared(vec2, X) << std::endl;
	std::cout << "Projection F squared distance between s1 and K: " << mf_t::distPF_squared(vec1, X) << std::endl;
	std::cout << "Projection F squared distance between s2 and K: " << mf_t::distPF_squared(vec2, X) << std::endl;

	std::cout<< "\n\n==========2ND ORDER TAYLOR DERIVATIVE CONSISTENCY CHECK==========" << std::endl;
	double h = 1e-4;
	std::cout << "\n\nTaylor expansion Derivative Tests with perturbation O(h) = O(" << h << ")" <<std::endl;
	mf_t::value_type dx, dy;

	matn HXproj = I - vec1 * vec1.transpose();
	matn HYproj = I - vec2 * vec2.transpose();

	std::cout << "\n------Tangent vector tests-----"    << std::endl;
	mat Xpdx, Ypdy;
	dx = mat::Random(); mf_t::projector(dx); dx = h * HXproj * dx;
	mf_t::exp(vec1, dx, Xpdx);
	Eigen::VectorXd vecdx = Eigen::Map<Eigen::VectorXd>(dx.data(), dx.size());
	std::cout << "\ndx should be in the horizontal space at X, i.e. X^Tdx=0:\n "<< (vec1.transpose()*dx).norm() << std::endl;

	dy = mat::Random(); mf_t::projector(dy); dy = h * HYproj * dy;
	mf_t::exp(vec2, dy, Ypdy);
	Eigen::VectorXd vecdy = Eigen::Map<Eigen::VectorXd>(dy.data(), dy.size());
	std::cout << "\ndy should be in the horizontal space at Y, i.e. Y^Tdy=0:\n "<< (vec2.transpose()*dy).norm() << std::endl;

	double exact = mf_t::dist_squared(vec1 + dx, vec2 + dy);
	double exact2 = mf_t::dist_squared(Xpdx, Ypdy);
	double taylor_order1 = mf_t::dist_squared(vec1, vec2) + d1x.cwiseProduct(dx).sum() + d1y.cwiseProduct(dy).sum();
	double taylor_order2 = taylor_order1 + 0.5 * d2xx.cwiseProduct(vecdx * vecdx.transpose()).sum() + 0.5 * d2yy.cwiseProduct(vecdy * vecdy.transpose()).sum() + d2xy.cwiseProduct(vecdx * vecdy.transpose()).sum();

	std::cout << "\n\nError of first order Taylor " << std::abs(taylor_order1 - exact) << " = O(h^"<< std::log10(std::abs(taylor_order1 - exact))/std::log10(h)  << ") " <<std::endl;
	std::cout << "Error of first order Taylor " << std::abs(taylor_order1 - exact2) << " = O(h^"<< std::log10(std::abs(taylor_order1 - exact2))/std::log10(h)  << ") " <<std::endl;
	std::cout << "\nError of second order Taylor " << std::abs(taylor_order2 - exact) << " = O(h^"<< std::log10(std::abs(taylor_order2 - exact))/std::log10(h)  << ") " <<std::endl;
	std::cout << "Error of second order Taylor " << std::abs(taylor_order2 - exact2) << " = O(h^"<< std::log10(std::abs(taylor_order2 - exact2))/std::log10(h)  << ") " <<std::endl;

	std::cout<< "\n\n==========2ND ORDER TAYLOR DERIVATIVE ALTERNATIVE CHECK==========" << std::endl;
	h = 1e-4;
	std::cout << "\n\nTaylor expansion Derivative Tests with perturbation O(h) = O(" << h << ")" <<std::endl;
	std::cout << "\n------Single variable, fixed Y-----"    << std::endl;

	X=vec1;
	Y=vec2;

	XorthP = I - X * X.transpose();
	grad = -XorthP * Y * Y.transpose() * X;

	H1 = XorthP * Y * Y.transpose();
	H2 = X.transpose() * Y * Y.transpose() * X;
	H = kroneckerProduct(Ip, H1) - kroneckerProduct(H2.transpose(), I);

	double exact1, exact3, exact4;
	double taylor_firstorder = mf_t::dist_squared(X, Y) + grad.cwiseProduct(dx).sum();

	exact1 = mf_t::distGeod_squared(Xpdx,Y);
	exact2 = mf_t::distGeod_squared(X+dx,Y);
	exact3 = mf_t::dist_squared(Xpdx,Y);
	exact4 = mf_t::dist_squared(X+dx,Y);

	std::cout << "\nGeodesic distances at (X,Y): " << mf_t::distGeod_squared(X,Y) << std::endl;
	std::cout << "1)Geodesic distances at (exp_X(dx),Y): " << mf_t::distGeod_squared(Xpdx,Y) << std::endl;
	std::cout << "2)Geodesic distances at (X+dx,Y): " << mf_t::distGeod_squared(X+dx,Y) << std::endl;
	std::cout << "\nProjection F distance at (X,Y): " << mf_t::dist_squared(X,Y) << std::endl;
	std::cout << "3)Projection F distance at (exp_X(dx),Y): " << mf_t::dist_squared(Xpdx,Y) << std::endl;
	std::cout << "4)Projection F distance at (X+dx,Y): " << mf_t::dist_squared(X+dx,Y) << std::endl;

	std::cout << "\n1)Error of first order Taylor " << std::abs(taylor_firstorder - exact1) << " = O(h^"<< std::log10(std::abs(taylor_firstorder - exact1))/std::log10(h)  << ") " <<std::endl;
	std::cout << "2)Error of first order Taylor " << std::abs(taylor_firstorder - exact2) << " = O(h^"<< std::log10(std::abs(taylor_firstorder - exact2))/std::log10(h)  << ") " <<std::endl;
	std::cout << "\n3)Error of first order Taylor " << std::abs(taylor_firstorder - exact3) << " = O(h^"<< std::log10(std::abs(taylor_firstorder - exact3))/std::log10(h)  << ") " <<std::endl;
	std::cout << "4)Error of first order Taylor " << std::abs(taylor_firstorder - exact4) << " = O(h^"<< std::log10(std::abs(taylor_firstorder - exact4))/std::log10(h)  << ") " <<std::endl;

}

int main(int argc, const char *argv[])
{
	srand(42);

	mat s1, s2;
	s1 = mat::Random();
	mf_t::projector(s1);

	s2 = mat::Random();
	mf_t::projector(s2);

	std::cout << "s1=\n" << s1 << std::endl;

	std::cout << "\n\nRANDOM Matrices" << std::endl;
	test(s1,s2);

	return 0;
}


// An interval object.
#pragma once

#include <array>

#include <Eigen/Core>

#include <tight_inclusion/types.hpp>

namespace ticcd {
    // calculate a*(2^b)
    uint64_t power(const uint64_t a, const uint8_t b);

    // calculate 2^exponent
    inline uint64_t pow2(const uint8_t exponent) { return power(1l, exponent); }

    // return power t. n=result*2^t
    uint8_t reduction(const uint64_t n, uint64_t &result);

    //<k,n> pair present a number k/pow(2,n)
    struct NumCCD {
        uint64_t numerator;
        uint8_t denom_power;

        NumCCD() {}

        NumCCD(uint64_t p_numerator, uint8_t p_denom_power)
            : numerator(p_numerator), denom_power(p_denom_power)
        {
        }

        NumCCD(Scalar x);

        ~NumCCD() {}

        uint64_t denominator() const { return pow2(denom_power); }

        // convert NumCCD to double number
        Scalar value() const { return Scalar(numerator) / denominator(); }

        operator double() const { return value(); }

        NumCCD operator+(const NumCCD &other) const;

        bool operator==(const NumCCD &other) const
        {
            return numerator == other.numerator
                   && denom_power == other.denom_power;
        }
        bool operator!=(const NumCCD &other) const { return !(*this == other); }
        bool operator<(const NumCCD &other) const;
        bool operator<=(const NumCCD &other) const
        {
            return (*this == other) || (*this < other);
        }
        bool operator>=(const NumCCD &other) const { return !(*this < other); }
        bool operator>(const NumCCD &other) const { return !(*this <= other); }

        bool operator<(const Scalar other) const { return value() < other; }
        bool operator>(const Scalar other) const { return value() > other; }
        bool operator==(const Scalar other) const { return value() == other; }

        static bool is_sum_leq_1(const NumCCD &num1, const NumCCD &num2);
    };

    // an interval represented by two double numbers
    struct Interval {
        NumCCD lower;
        NumCCD upper;

        Interval() {}

        Interval(const NumCCD &p_lower, const NumCCD &p_upper)
            : lower(p_lower), upper(p_upper)
        {
        }

        ~Interval() {}

        std::pair<Interval, Interval> bisect() const;

        bool overlaps(const Scalar r1, const Scalar r2) const;
    };

    typedef std::array<Interval, 3> Interval3;
    Array3 width(const Interval3 &x);

} // namespace ticcd


/*
 * Common.hpp
 *
 *  Created on: Feb 9, 2014
 *      Author: Bloeschm
 */

#ifndef LWF_COMMON_HPP_
#define LWF_COMMON_HPP_

#include <map>
#include <type_traits>
#include <tuple>
#include <Eigen/Dense>
#include <iostream>
#include "lightweight_filtering/PropertyHandler.hpp"

typedef Eigen::Quaterniond QPD;
typedef Eigen::Matrix3d MPD;
typedef Eigen::Vector3d V3D;
typedef Eigen::Matrix3d M3D;
typedef Eigen::VectorXd VXD;
typedef Eigen::MatrixXd MXD;

inline M3D gSM(const V3D& vec){
  M3D mat;
  mat << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0;
  return mat;
}

static void enforceSymmetry(MXD& mat){
  mat = 0.5*(mat+mat.transpose()).eval();
}

inline M3D Lmat (const V3D& a) {
  const double norm = a.norm();
  const M3D skewMatrix = gSM(a);
  if (norm < 1.0e-4) {
    return M3D::Identity() + 0.5*skewMatrix;
  }
  return M3D::Identity() + (double(1.0) - cos(norm))/(norm*norm)*skewMatrix + (norm - sin(norm))/(norm*norm*norm)*(skewMatrix*skewMatrix);
}

inline V3D log_map(const QPD& q) {
    using std::acos;
    using std::sqrt;

    // define these compile time constants to avoid std::abs:
    static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
    NearlyNegativeOne = -1.0 + 1e-10;

    V3D omega;

    const double qw = q.w();
    // See Quaternion-Logmap.nb in doc for Taylor expansions
    if (qw > NearlyOne) {
      // Taylor expansion of (angle / s) at 1
      // (2 + 2 * (1-qw) / 3) * q.vec();
      omega = ( 8. / 3. - 2. / 3. * qw) * q.vec();
    } else if (qw < NearlyNegativeOne) {
      // Taylor expansion of (angle / s) at -1
      // (-2 - 2 * (1 + qw) / 3) * q.vec();
      omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
    } else {
      // Normal, away from zero case
      double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
      // Important:  convert to [-pi,pi] to keep error continuous
      if (angle > M_PI)
      angle -= twoPi;
      else if (angle < -M_PI)
      angle += twoPi;
      omega = (angle / s) * q.vec();
    }

    return omega;
}

inline QPD exp_map(const V3D& omega) {
    using std::cos;
    using std::sin;

    double theta2 = omega.dot(omega);
    if (theta2 > std::numeric_limits<double>::epsilon()) {
      double theta = std::sqrt(theta2);
      double ha = 0.5 * theta;
      V3D vec = (sin(ha) / theta) * omega;
      return QPD(cos(ha), vec.x(), vec.y(), vec.z());
    } else {
      // first order approximation sin(theta/2)/theta = 0.5
      V3D vec = 0.5 * omega;
      return QPD(1.0, vec.x(), vec.y(), vec.z());
    }
}

inline QPD box_plus(const QPD& q, const V3D& v) {
  return exp_map(v) * q;
}

inline V3D box_minus(const QPD& q1, const QPD& q2) {
  return log_map(q1 * q2.inverse());
}

namespace LWF{
  enum FilteringMode{
    ModeEKF,
    ModeUKF,
    ModeIEKF
  };
}

#endif /* LWF_COMMON_HPP_ */