#include <iostream>
#include <chrono>
#include <random>
#include <map>

#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

// #include "g2o/core/auto_differentiation.h"
// #include "g2o/core/base_unary_edge.h"
// #include "g2o/core/base_vertex.h"
// #include "g2o/core/optimization_algorithm_factory.h"
// #include "g2o/core/sparse_optimizer.h"
// #include "g2o/stuff/command_args.h"
// #include "g2o/stuff/sampler.h"
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/auto_differentiation.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include "matplotlibcpp.h"

G2O_USE_OPTIMIZATION_LIBRARY(dense);

using namespace std;
using namespace Eigen;
namespace plt = matplotlibcpp;

template <class T, class TT>
T g(TT x, T a, T b, T c)
{
  return ceres::exp(a * x * x + b * x + c);
}

class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d> {
  public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  virtual void setToOriginImpl() override {
    _estimate << 0.0, 0.0, 0.0;
  }
  virtual void oplusImpl(const double* update) override {
    _estimate += Eigen::Vector3d(update);
  }

  virtual bool read(std::istream&) {}
  virtual bool write(std::ostream&) const {}
};

class CurveFittingEdge: public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    CurveFittingEdge(double x): _x(x) {}

    // Without auto-diff
    virtual void computeError() override {
      const CurveFittingVertex* v = static_cast<CurveFittingVertex*>(_vertices[0]);
      const Eigen::Vector3d abc = v->estimate();
      _error(0, 0) = _measurement - g(_x, abc[0], abc[1], abc[2]);
    }

    // Analytical jacobian
    virtual void linearizeOplus() override {
      const CurveFittingVertex *v = static_cast<CurveFittingVertex*>(_vertices[0]);
      const Eigen::Vector3d abc = v->estimate();
      double y = g(_x, abc[0], abc[1], abc[2]);
      _jacobianOplusXi[0] = -_x * _x * y;
      _jacobianOplusXi[1] = -_x * y;
      _jacobianOplusXi[2] = -y;
    }

    // with auto-diff
    // template <class T>
    // bool operator() (const T* abc, T *error) const {
    //   error[0] = measurement() - g(_x, abc[0], abc[1], abc[2]);
    //   return true;
    // }

    virtual bool read(istream&) {}
    virtual bool write(ostream&) const {}

  private:
    double _x;

  // G2O_MAKE_AUTO_AD_FUNCTIONS  // use autodiff
};

int main(int argc, char **argv) {
  std::cout << "Gauss-Newton \n";

  double aa = 1.0, bb = 2.0, cc = 1.0;
  double a = 2.0, b = -1.0, c = 5.5;

  double obs_sigma = 1.0;
  std::default_random_engine generator;
  std::normal_distribution<double> obs_noise_distrib(0.0, obs_sigma);

  int N = 100;
  std::vector<double> x_data(N), y_data(N);
  for (int i = 0; i < N; ++i)
  {
    x_data[i] = static_cast<double>(i) / 100.0;
    y_data[i] = g(x_data[i], aa, bb, cc) + obs_noise_distrib(generator);

  }

  typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;

  // auto solver = new g2o::OptimizationAlgorithmGaussNewton(
  //   g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())
  // );

  g2o::SparseOptimizer optimizer;
  // optimizer.setAlgorithm(solver);
  g2o::OptimizationAlgorithmProperty solverProperty;
  optimizer.setAlgorithm(g2o::OptimizationAlgorithmFactory::instance()->construct("lm_dense", solverProperty));
  optimizer.setVerbose(true);

  // Create vertex
  CurveFittingVertex *v = new CurveFittingVertex();
  v->setEstimate(Eigen::Vector3d(a, b, c));
  v->setId(0);
  optimizer.addVertex(v);

  // Add edges
  for (int i = 0; i < N; ++i)
  {
    CurveFittingEdge * edge = new CurveFittingEdge(x_data[i]);
    edge->setId(i);
    edge->setVertex(0, v);
    edge->setMeasurement(y_data[i]);
    edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() / (obs_sigma * obs_sigma));
    optimizer.addEdge(edge);
  }

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

  optimizer.initializeOptimization();
  optimizer.optimize(10);

  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  a = v->estimate()[0];
  b = v->estimate()[1];
  c = v->estimate()[2];

  std::cout << "final = " << a << " " << b << " " << c << "\n";

  vector<double> final_y_data(N);
  for (int i = 0; i < N; ++i)
  {
    final_y_data[i] = g(x_data[i], a, b, c);
  }

  plt::scatter(x_data, y_data);
  std::map<std::string, std::string> parameters;
  parameters["c"] = "red";
  plt::scatter(x_data, final_y_data, 1.0, parameters);
  plt::show();

  return 0;
}


#define BOOST_TEST_MODULE "test_JacobiMethod"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include "../src/JacobiMethod.hpp"
#include "../src/InverseMatrix.hpp"
#include "../src/io.hpp"

#include "test_Defs.hpp"
using ax::test::seed;

#include <random>

BOOST_AUTO_TEST_CASE(matrix_2x2)
{
    constexpr std::size_t msize = 2;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    ax::Matrix<double, msize,msize> mat;
    double det(0e0);

    while(det == 0e0)
    {
        for(std::size_t i=0; i<msize; ++i)
            for(std::size_t j=i; j<msize; ++j)
                if(i==j)
                    mat(i,j) = randreal(mt);
                else
                    mat(i,j) = mat(j,i) = randreal(mt);
        det = ax::determinant(mat);
   }

    const ax::Matrix<double, msize,msize> values = mat;
    const ax::Matrix<double, msize,msize> E(1e0);

//     ax::JacobiMethod<ax::Matrix<double, msize,msize>> jacobi(mat);
//     auto eigenpair = jacobi.solve();
    auto eigenpair = Jacobimethod(mat);

    for(std::size_t i=0; i<msize; ++i)
    {
        const double eigenvalue = eigenpair.at(i).first;
        const ax::Vector<double, msize> eigenvector = eigenpair.at(i).second;

        std::cout << "eval " << eigenvalue << std::endl;
        std::cout << "evec " << eigenvector << std::endl;

        const ax::Vector<double, msize> zeros = (values - eigenvalue * E) * eigenvector;
        for(std::size_t j=0; j<msize; ++j)
            BOOST_CHECK_SMALL(zeros[j], 1e-7);
    }
}

BOOST_AUTO_TEST_CASE(matrix_3x3)
{
    constexpr std::size_t msize = 3;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(0e0, 1e0);

    ax::Matrix<double, msize,msize> mat;
    double det(0e0);

    while(det == 0e0)
    {
        for(std::size_t i=0; i<msize; ++i)
            for(std::size_t j=i; j<msize; ++j)
                if(i==j)
                    mat(i,j) = randreal(mt);
                else
                    mat(i,j) = mat(j,i) = randreal(mt);
        det = ax::determinant(mat);
    }

    const ax::Matrix<double, msize,msize> values = mat;
    const ax::Matrix<double, msize,msize> E(1e0);

//     ax::JacobiMethod<ax::Matrix<double, msize,msize>> jacobi(mat);
//     const auto eigenpair = jacobi.solve();
    auto eigenpair = Jacobimethod(mat);

    for(std::size_t i=0; i<msize; ++i)
    {
        const double eigenvalue = eigenpair.at(i).first;
        const ax::Vector<double, msize> eigenvector = eigenpair.at(i).second;

        std::cout << "eval " << eigenvalue << std::endl;
        std::cout << "evec " << eigenvector << std::endl;

        const ax::Vector<double, msize> zeros = (values - eigenvalue * E) * eigenvector;
        for(std::size_t j=0; j<msize; ++j)
            BOOST_CHECK_SMALL(zeros[j], 1e-7);
    }
}

BOOST_AUTO_TEST_CASE(matrix_4x4)
{
    constexpr std::size_t msize = 4;

    std::mt19937 mt(seed);
    std::uniform_real_distribution<double> randreal(1e0, 2e0);

    ax::Matrix<double, msize,msize> mat;

    for(std::size_t i=0; i<msize; ++i)
        for(std::size_t j=i; j<msize; ++j)
            if(i==j)
                mat(i,j) = randreal(mt);
            else
                mat(i,j) = mat(j,i) = randreal(mt);

    const ax::Matrix<double, msize,msize> values = mat;
    const ax::Matrix<double, msize,msize> E(1e0);

//     ax::JacobiMethod<ax::Matrix<double, msize,msize>> jacobi(mat);
//     auto eigenpair = jacobi.solve();

    auto eigenpair = Jacobimethod(mat);

    for(std::size_t i=0; i<msize; ++i)
    {
        const double eigenvalue = eigenpair.at(i).first;
        const ax::Vector<double, msize> eigenvector = eigenpair.at(i).second;

        std::cout << "eval " << eigenvalue << std::endl;
        std::cout << "evec " << eigenvector << std::endl;

        const ax::Vector<double, msize> zeros = (values - eigenvalue * E) * eigenvector;
        for(std::size_t j=0; j<msize; ++j)
            BOOST_CHECK_SMALL(zeros[j], 1e-7);
    }
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      100903    K. Kumar          File created.
 *      100916    L. Abdulkadir     File checked.
 *      100929    K. Kumar          Checked code by D. Dirkx added.
 *      101110    K. Kumar          Added raiseToExponentPower() function.
 *      102410    D. Dirkx          Minor comment changes as code check.
 *      101213    K. Kumar          Bugfix raiseToIntegerExponent(); renamed raiseToIntegerPower().
 *                                  Added computeAbsoluteValue() functions.
 *      110202    K. Kumar          Added overload for State* for computeLinearInterpolation().
 *      110111    J. Melman         Added computeModulo() function.
 *      110411    K. Kumar          Added convertCartesianToSpherical() function.
 *      110606    J. Melman         Removed possible singularity from
 *                                  convertCartesianToSpherical.
 *      110707    K. Kumar          Added computeSampleMean(), computeSampleVariance() functions.
 *      110905    S. Billemont      Reorganized includes.
 *                                  Moved (con/de)structors and getter/setters to header.
 *      120127    D. Dirkx          First version branched from basic mathematics in Tudat Core.
 *      120127    K. Kumar          Minor comment edits.
 *      120118    D. Gondelach      Added new convertCylindricalToCartesian functions.
 *      120214    K. Kumar          Branched from old Tudat trunk for new coordinate conversions.
 *      120217    K. Kumar          Updated computeModuloForSignedValues() to computeModulo()
 *                                  from Tudat Core.
 *      120926    E. Dekens         Added spherical gradient to Cartesian conversion.
 *      131022    T. Roegiers       Added conversion from spherical state to Cartesian state.
 *                                  Added conversion from Cartesian state to spherical state.
 *      140114    E. Brandon        Reorganized includes.
 *                                  Minor changes during code check.
 *
 *    References
 *      Press W.H., et al. Numerical Recipes in C++: The Art of Scientific Computing. Cambridge
 *          University Press, February 2002.
 *      Torok, J.S. Analytical Mechanics: with an Introduction to Dynamical Systems, John Wiley and
 *          Sons, Inc., 2000.
 *      Vallado, D.A. Fundamentals of Astrodynamics and Applications. Microcosm Press, 2001.
 *
 *    Notes
 *
 */

#include <cmath>
#include <iostream>
#include <limits>

#include <boost/math/special_functions/sign.hpp>

#include "Tudat/Mathematics/BasicMathematics/basicMathematicsFunctions.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h"

namespace tudat
{

namespace coordinate_conversions
{

//! Convert spherical (radius_, zenith, azimuth) to Cartesian (x,y,z) coordinates.
Eigen::Vector3d convertSphericalToCartesian( const Eigen::Vector3d& sphericalCoordinates )
{
    // Create local variables.
    double radius_ = sphericalCoordinates( 0 );
    double zenithAngle_ = sphericalCoordinates( 1 );
    double azimuthAngle_ = sphericalCoordinates( 2 );

    // Declaring sine which has multiple usages to save computation time.
    double sineOfZenithAngle_ = std::sin( sphericalCoordinates( 1 ) );

    // Create output Vector3d.
    Eigen::Vector3d convertedCartesianCoordinates_ = Eigen::Vector3d::Zero( 3 );

    // Perform transformation.
    convertedCartesianCoordinates_( 0 ) = radius_ * std::cos( azimuthAngle_ ) * sineOfZenithAngle_;
    convertedCartesianCoordinates_( 1 ) = radius_ * std::sin( azimuthAngle_ ) * sineOfZenithAngle_;
    convertedCartesianCoordinates_( 2 ) = radius_ * std::cos( zenithAngle_ );

    return convertedCartesianCoordinates_;
}

//! Convert Cartesian (x,y,z) to spherical (radius, zenith, azimuth) coordinates.
Eigen::Vector3d convertCartesianToSpherical( const Eigen::Vector3d& cartesianCoordinates )
{
    // Create output Vector3d.
    Eigen::Vector3d convertedSphericalCoordinates_ = Eigen::Vector3d::Zero( 3 );

    // Compute transformation of Cartesian coordinates to spherical coordinates.
    convertedSphericalCoordinates_( 0 ) = cartesianCoordinates.norm( );

    // Check if coordinates are at origin.
    if ( convertedSphericalCoordinates_( 0 ) < std::numeric_limits< double >::epsilon( ) )
    {
        convertedSphericalCoordinates_( 1 ) = 0.0;
        convertedSphericalCoordinates_( 2 ) = 0.0;
    }
    // Else compute coordinates using trigonometric relationships.
    else
    {
        convertedSphericalCoordinates_( 1 ) = std::acos( cartesianCoordinates( 2 )
                                                         / convertedSphericalCoordinates_( 0 ) );
        convertedSphericalCoordinates_( 2 ) = std::atan2( cartesianCoordinates( 1 ),
                                                          cartesianCoordinates( 0 ) );
    }

    return convertedSphericalCoordinates_;
}

//! Convert cylindrical to Cartesian coordinates.
Eigen::Vector3d convertCylindricalToCartesian( const double radius,
                                               const double azimuthAngle, const double z )
{
    // Create Cartesian coordinates vector.
    Eigen::Vector3d cartesianCoordinates;

    // If radius < 0, then give warning.
    if ( radius < 0.0 )
    {
        std::cerr << "Warning: cylindrical radial coordinate is negative!\n"
                  << "This could give incorrect results!\n";
    }

    // Compute and set Cartesian coordinates.
    cartesianCoordinates << radius * std::cos( azimuthAngle ),   // x-coordinate
                            radius * std::sin( azimuthAngle ),   // y-coordinate
                            z;                                   // z-coordinate

    return cartesianCoordinates;
}

//! Convert cylindrical to cartesian coordinates.
Eigen::Vector3d convertCylindricalToCartesian( const Eigen::Vector3d& cylindricalCoordinates )
{
    // Create Cartesian coordinates vector.
    Eigen::Vector3d cartesianCoordinates;

    // If radius < 0, then give warning.
    if ( cylindricalCoordinates( 0 ) < 0.0 )
    {
        std::cerr << "Warning: cylindrical radial coordinate is negative! "
                  << "This could give incorrect results!\n";
    }

    // Compute and set Cartesian coordinates.
    cartesianCoordinates
            << cylindricalCoordinates( 0 )
               * std::cos( cylindricalCoordinates( 1 ) ),    // x-coordinate
               cylindricalCoordinates( 0 )
               * std::sin( cylindricalCoordinates( 1 ) ),    // y-coordinate
               cylindricalCoordinates( 2 );                  // z-coordinate

    return cartesianCoordinates;
}

//! Convert cylindrical to Cartesian state.
basic_mathematics::Vector6d convertCylindricalToCartesianState(
        const basic_mathematics::Vector6d& cylindricalState )
{
    // Create Cartesian state vector, initialized with zero entries.
    basic_mathematics::Vector6d cartesianState = basic_mathematics::Vector6d::Zero( );

    // Get azimuth angle, theta.
    double azimuthAngle = cylindricalState( 1 );

    // Compute and set Cartesian coordinates.
    cartesianState.head( 3 ) = convertCylindricalToCartesian(
                Eigen::Vector3d( cylindricalState.head( 3 ) ) );

    // If r = 0 AND Vtheta > 0, then give warning and assume Vtheta=0.
    if ( std::fabs(cylindricalState( 0 )) <= std::numeric_limits< double >::epsilon( )
         && std::fabs(cylindricalState( 4 )) > std::numeric_limits< double >::epsilon( ) )
    {
        std::cerr << "Warning: cylindrical velocity Vtheta (r*thetadot) does not equal zero while "
                  << "the radius (r) is zero! Vtheta is taken equal to zero!\n";

        // Compute and set Cartesian velocities.
        cartesianState.tail( 3 )
                << cylindricalState( 3 ) * std::cos( azimuthAngle ),   // xdot
                   cylindricalState( 3 ) * std::sin( azimuthAngle ),   // ydot
                   cylindricalState( 5 );                              // zdot
    }

    else
    {
        // Compute and set Cartesian velocities.
        cartesianState.tail( 3 )
                << cylindricalState( 3 ) * std::cos( azimuthAngle )
                   - cylindricalState( 4 ) * std::sin( azimuthAngle ),   // xdot
                   cylindricalState( 3 ) * std::sin( azimuthAngle )
                   + cylindricalState( 4 ) * std::cos( azimuthAngle ),   // ydot
                   cylindricalState( 5 );                                // zdot
    }

    return cartesianState;
}

//! Convert Cartesian to cylindrical coordinates.
Eigen::Vector3d convertCartesianToCylindrical( const Eigen::Vector3d& cartesianCoordinates )
{
    // Create cylindrical coordinates vector.
    Eigen::Vector3d cylindricalCoordinates;

    // Declare new variable, the azimuth angle.
    double azimuthAngle;

    // Compute azimuth angle, theta.
    /* If x = 0, then azimuthAngle = pi/2 (y>0) or 3*pi/2 (y<0) or 0 (y=0),
       else azimuthAngle = arctan(y/x).
    */
    using mathematical_constants::PI;
    if ( std::fabs(cartesianCoordinates( 0 ) ) <= std::numeric_limits< double >::epsilon( ) )
    {
        azimuthAngle = basic_mathematics::computeModulo(
                    static_cast< double >( boost::math::sign( cartesianCoordinates( 1 ) ) )
                    * 0.5 * PI, 2.0 * PI );
    }

    else
    {
        azimuthAngle = basic_mathematics::computeModulo(
                    std::atan2( cartesianCoordinates( 1 ),
                                cartesianCoordinates( 0 ) ), 2.0 * PI );
    }

    // Compute and set cylindrical coordinates.
    cylindricalCoordinates <<
        std::sqrt( pow( cartesianCoordinates( 0 ), 2 )
                   + pow( cartesianCoordinates( 1 ), 2 ) ), // Radius
        azimuthAngle,                                       // Azimuth angle, theta
        cartesianCoordinates( 2 );                          // z-coordinate

    return cylindricalCoordinates;
}

//! Convert Cartesian to cylindrical state.
basic_mathematics::Vector6d convertCartesianToCylindricalState(
        const basic_mathematics::Vector6d& cartesianState )
{
    // Create cylindrical state vector, initialized with zero entries.
    basic_mathematics::Vector6d cylindricalState = basic_mathematics::Vector6d::Zero( );

    // Compute and set cylindrical coordinates.
    cylindricalState.head( 3 ) = convertCartesianToCylindrical(
                Eigen::Vector3d( cartesianState.head( 3 ) ) );

    // Compute and set cylindrical velocities.
    /* If radius = 0, then Vr = sqrt(xdot^2+ydot^2) and Vtheta = 0,
       else Vr = (x*xdot+y*ydot)/radius and Vtheta = (x*ydot-y*xdot)/radius.
    */
    if ( cylindricalState( 0 ) <= std::numeric_limits< double >::epsilon( ) )
    {
        cylindricalState.tail( 3 ) <<
            std::sqrt( pow( cartesianState( 3 ), 2 ) + pow( cartesianState( 4 ), 2 ) ), // Vr
            0.0,                                                                        // Vtheta
            cartesianState( 5 );                                                        // Vz
    }

    else
    {
        cylindricalState.tail( 3 ) <<
            ( cartesianState( 0 ) * cartesianState( 3 )
              + cartesianState( 1 ) * cartesianState( 4 ) ) / cylindricalState( 0 ),    // Vr
            ( cartesianState( 0 ) * cartesianState( 4 )
              - cartesianState( 1 ) * cartesianState( 3 ) ) / cylindricalState( 0 ),    // Vtheta
                cartesianState( 5 );                                                    // Vz
    }

    return cylindricalState;
}

//! Convert spherical to Cartesian gradient.
Eigen::Vector3d convertSphericalToCartesianGradient( const Eigen::Vector3d& sphericalGradient,
                                                     const Eigen::Vector3d& cartesianCoordinates )
{
    // Compute radius.
    const double radius = std::sqrt( cartesianCoordinates( 0 ) * cartesianCoordinates( 0 )
                                     + cartesianCoordinates( 1 ) * cartesianCoordinates( 1 )
                                     + cartesianCoordinates( 2 ) * cartesianCoordinates( 2 ) );

    // Compute square of distance within xy-plane.
    const double xyDistanceSquared = cartesianCoordinates( 0 ) * cartesianCoordinates( 0 )
            + cartesianCoordinates( 1 ) * cartesianCoordinates( 1 );

    // Compute distance within xy-plane.
    const double xyDistance = std::sqrt( xyDistanceSquared );

    // Compute transformation matrix.
    const Eigen::Matrix3d transformationMatrix = (
                Eigen::Matrix3d( 3, 3 ) <<
                cartesianCoordinates( 0 ) / radius,
                - cartesianCoordinates( 0 ) * cartesianCoordinates( 2 )
                / ( radius * radius * xyDistance ),
                - cartesianCoordinates( 1 ) / xyDistanceSquared,
                cartesianCoordinates( 1 ) / radius,
                - cartesianCoordinates( 1 ) * cartesianCoordinates( 2 )
                / ( radius * radius * xyDistance ),
                + cartesianCoordinates( 0 ) / xyDistanceSquared,
                cartesianCoordinates( 2 ) / radius,
                xyDistance / ( radius * radius ),
                0.0
                ).finished( );

    // Return Cartesian gradient.
    return transformationMatrix * sphericalGradient;
}

//! Convert spherical to Cartesian state.
basic_mathematics::Vector6d convertSphericalToCartesianState(
        const basic_mathematics::Vector6d& sphericalState )
{
    // Create Cartesian state vector, initialized with zero entries.
    basic_mathematics::Vector6d convertedCartesianState = basic_mathematics::Vector6d::Zero( );

    // Create local variables.
    const double radius = sphericalState( 0 );
    const double azimuthAngle = sphericalState( 1 );
    const double elevationAngle = sphericalState( 2 );

    // Precompute sine/cosine of angles, which has multiple usages, to save computation time.
    const double cosineOfElevationAngle = std::cos( elevationAngle );
    const double sineOfElevationAngle = std::sin( elevationAngle );
    const double cosineOfAzimuthAngle = std::cos( azimuthAngle );
    const double sineOfAzimuthAngle = std::sin( azimuthAngle );

    // Set up transformation matrix for spherical to cylindrical conversion.
    Eigen::Matrix3d transformationMatrixSphericalToCylindrical = Eigen::Matrix3d::Zero( );
    transformationMatrixSphericalToCylindrical( 0, 0 ) = cosineOfElevationAngle;
    transformationMatrixSphericalToCylindrical( 0, 2 ) = -sineOfElevationAngle;
    transformationMatrixSphericalToCylindrical( 1, 1 ) = 1.0;
    transformationMatrixSphericalToCylindrical( 2, 0 ) = sineOfElevationAngle;
    transformationMatrixSphericalToCylindrical( 2, 2 ) = cosineOfElevationAngle;

    // Set up transformation matrix for cylindrical to Cartesian conversion.
    Eigen::Matrix3d transformationMatrixCylindricalToCartesian = Eigen::Matrix3d::Zero( );
    transformationMatrixCylindricalToCartesian( 0, 0 ) = cosineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 0, 1 ) = -sineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 1, 0 ) = sineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 1, 1 ) = cosineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 2, 2 ) = 1.0;

    // Compute transformation matrix for spherical to Cartesian conversion.
    const Eigen::Matrix3d transformationMatrixSphericalToCartesian
            = transformationMatrixCylindricalToCartesian
            * transformationMatrixSphericalToCylindrical;

    // Perform transformation of position coordinates.
    convertedCartesianState( 0 ) = radius * cosineOfAzimuthAngle * cosineOfElevationAngle;
    convertedCartesianState( 1 ) = radius * sineOfAzimuthAngle * cosineOfElevationAngle;
    convertedCartesianState( 2 ) = radius * sineOfElevationAngle;

    // Perform transformation of velocity vector.
    convertedCartesianState.segment( 3, 3 ) =
        transformationMatrixSphericalToCartesian * sphericalState.segment( 3, 3 );

    // Return Cartesian state vector.
    return convertedCartesianState;
}

//! Convert Cartesian to spherical state.
basic_mathematics::Vector6d convertCartesianToSphericalState(
        const basic_mathematics::Vector6d& cartesianState )
{
    // Create spherical state vector, initialized with zero entries.
    basic_mathematics::Vector6d convertedSphericalState = basic_mathematics::Vector6d::Zero( );

    // Compute radius.
    convertedSphericalState( 0 ) = cartesianState.segment( 0, 3 ).norm( );

    // Check if radius is nonzero.
    /*
     * If r > 0, the elevation and azimuth angles are computed using trigonometric relationships.
     * If r = 0, the coordinates are at the origin, the elevation and azimuth angles equal to zero.
     * Since the state vector was initialized with zeroes, this is already the case.
     */
    if ( convertedSphericalState( 0 ) > std::numeric_limits< double >::epsilon( ) )
    {
        // Compute elevation and azimuth angles using trigonometric relationships.
        // Azimuth angle.
        convertedSphericalState( 1 ) = std::atan2( cartesianState( 1 ), cartesianState( 0 ) );
        // Elevation angle.
        convertedSphericalState( 2 ) = std::asin( cartesianState( 2 )
                                                   / convertedSphericalState( 0 ) );
    }

    // Precompute sine/cosine of angles, which has multiple usages, to save computation time.
    const double cosineOfElevationAngle = std::cos( convertedSphericalState( 2 ) );
    const double sineOfElevationAngle = std::sin( convertedSphericalState( 2 ) );
    const double cosineOfAzimuthAngle = std::cos( convertedSphericalState( 1 ) );
    const double sineOfAzimuthAngle = std::sin( convertedSphericalState( 1 ) );

    // Set up transformation matrix for cylindrical to spherical conversion.
    Eigen::Matrix3d transformationMatrixCylindricalToSpherical = Eigen::Matrix3d::Zero( );
    transformationMatrixCylindricalToSpherical( 0, 0 ) = cosineOfElevationAngle;
    transformationMatrixCylindricalToSpherical( 0, 2 ) = sineOfElevationAngle;
    transformationMatrixCylindricalToSpherical( 1, 1 ) = 1.0;
    transformationMatrixCylindricalToSpherical( 2, 0 ) = -sineOfElevationAngle;
    transformationMatrixCylindricalToSpherical( 2, 2 ) = cosineOfElevationAngle;

    // Set up transformation matrix for Cartesian to cylindrical conversion.
    Eigen::Matrix3d transformationMatrixCartesianToCylindrical = Eigen::Matrix3d::Zero( );
    transformationMatrixCartesianToCylindrical( 0, 0 ) = cosineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 0, 1 ) = sineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 1, 0 ) = -sineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 1, 1 ) = cosineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 2, 2 ) = 1.0;

    // Compute transformation matrix for Cartesian to spherical conversion.
    const Eigen::Matrix3d transformationMatrixCartesianToSpherical
            = transformationMatrixCylindricalToSpherical
            * transformationMatrixCartesianToCylindrical;

    // Perform transformation of velocity vector.
    convertedSphericalState.segment( 3, 3 )
            = transformationMatrixCartesianToSpherical * cartesianState.segment( 3, 3 );

    // Return spherical state vector.
    return convertedSphericalState;
}

} // namespace coordinate_conversions

} // namespace tudat


#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <sophus/se3.hpp>
#include <chrono>
using namespace std;
using namespace cv;
// define VecVec2d and VecVec3d
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

// Functions Get the matching points
void find_feature_matches(const Mat &img_1, const Mat &img_2,std::vector<KeyPoint> &keypoints_1,
  std::vector<KeyPoint> &keypoints_2,std::vector<DMatch> &matches);

// Functions to Change the pixel points into camera normalization points
Point2d pixel2cam(const Point2d &p, const Mat &K);

// Bundle Adjustment  By Gauss-Newton
void bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose    // need to be optimazied
);

// Bundle Adjustment By G2O
void bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose
);

int main(int argc, char const *argv[]){
    if(argc!=5){
        cout<<"Usage (bash): pose_estimation3d2d.cpp  <image01> <image02> <rgbd-image01-path>  <rgbd-image02-path> "<<endl;
       return 1;
    }
 //  Load Images
  Mat img_1 = imread(argv[1]);
  Mat img_2 = imread(argv[2]);
  assert(img_1.data && img_2.data && "Can not load images!");

 // Find keypoints matches using orb-brief
vector<KeyPoint> keypoints_1, keypoints_2;
vector<DMatch> matches;
find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
cout << "Totally found " << matches.size() << " matches" << endl;

//  Build 3d -2d  reflections

//  depth image is 16-bit unchar mono-channel picture
Mat rgbd_image01 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);
Mat camera_K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
vector<Point3f> pts_3d;
vector<Point2f> pts_2d;
for(DMatch m :matches){
 // reference : Mat.ptr<unchar>(int a)[int b] : means the pixel in row a, col b.
 // get the keypoint's deep
ushort d = rgbd_image01.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
if(d==0) continue;
float normalized_d = d /1000.0;
Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt,camera_K);
pts_3d.push_back(Point3f(p1.x*normalized_d,p1.y*normalized_d,normalized_d));  // keypint one's 3d , 相机坐标
pts_2d.push_back(keypoints_2[m.trainIdx].pt);      // keypoint2 :only 2d
}

// Solve PNP using OpenCV
Mat r, t;  // r is rotation vector , t is translation vector
solvePnP(pts_3d,pts_2d,camera_K,Mat(),r,t, false, cv::SOLVEPNP_EPNP);
Mat inliners;
bool result  = solvePnPRansac(pts_3d,pts_2d,camera_K,Mat(),r,t,false,100,4.0,0.99,inliners);
cout<<result<<endl;
Mat R;
cv::Rodrigues(r,R);
cout<<" Rotation Matrix R (3,3 ): "<<"\n"<<R<<endl;
cout<<"  translation vector  (3,1): "<<"\n"<<t<<endl;

  VecVector3d pts_3d_eigen;
  VecVector2d pts_2d_eigen;
  for (size_t i = 0; i < pts_3d.size(); ++i) {
    pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
    pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
  }
  // 使用 G-N  BA
  cout << "Calling bundle adjustment by gauss newton" << endl;
  Sophus::SE3d pose_gn;
  bundleAdjustmentGaussNewton(pts_3d_eigen, pts_2d_eigen, camera_K, pose_gn);

 Sophus::SE3d pose_g2o;
bundleAdjustmentG2O(pts_3d_eigen, pts_2d_eigen, camera_K, pose_g2o);
return 0;
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  //-- 初始化
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  //-- 第一步:检测 Oriented FAST 角点位置
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  //-- 第二步:根据角点位置计算 BRIEF 描述子
  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
  vector<DMatch> match;
  //BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  //-- 第四步:匹配点对筛选
  double min_dist = 10000, max_dist = 0;

  //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }
  //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

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

void bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose) {
 typedef Eigen::Matrix<double, 6, 1> Vector6d;   // because Se(3) is a 6 dim vector, First 3 P is translation, Second 3 Fi is rotatiom
  const int iterations = 10;
  double cost = 0, lastCost = 0;
  /// camera model//
  // X' = f(X/Z) Y'= f(Y/Z)
  // u = aX' +cx   v= bY' + cy

  // Camera inner parameter
  //         [ fx, 0 , cx ]
 //  K =  [ 0, fx,  cy ]
 //          [ 0,  0,   1  ]
  double fx = K.at<double>(0, 0);
  double fy = K.at<double>(1, 1);
  double cx = K.at<double>(0, 2);
  double cy = K.at<double>(1, 2);

  for (int iter = 0; iter < iterations; iter++) {
    Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();  //这里之所以是 6 x 6 是因为J(x)TJ(X) = H, J(X)是2 x 6
    Vector6d b = Vector6d::Zero(); // b = J(x)Tf(x), J(x)T是 6 x 2, f(x)是 x,y 2个维度的误差，所以是2 x 1.
    cost = 0;
    // compute cost
    for (int i = 0; i < points_3d.size(); i++) {
      Eigen::Vector3d pc = pose * points_3d[i];
      double inv_z = 1.0 / pc[2]; // 1/Z'
      double inv_z2 = inv_z * inv_z;  // 1/Z^2
      Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);

      Eigen::Vector2d e = points_2d[i] - proj;  // 重投影误差

      cost += e.squaredNorm();
      Eigen::Matrix<double, 2, 6> J;  // 可以直接套结论的矩阵
      J << -fx * inv_z,
        0,
        fx * pc[0] * inv_z2,
        fx * pc[0] * pc[1] * inv_z2,
        -fx - fx * pc[0] * pc[0] * inv_z2,
        fx * pc[1] * inv_z,
        0,
        -fy * inv_z,
        fy * pc[1] * inv_z2,
        fy + fy * pc[1] * pc[1] * inv_z2,
        -fy * pc[0] * pc[1] * inv_z2,
        -fy * pc[0] * inv_z;

      H += J.transpose() * J;  // n点累加
      b += -J.transpose() * e;
    }

    Vector6d dx;
    dx = H.ldlt().solve(b); // 更新 se3

    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }

    if (iter > 0 && cost >= lastCost) {
      // cost increase, update is not good
      cout << "cost: " << cost << ", last cost: " << lastCost << endl;
      break;
    }

    // update your estimation // 更新SE(3)
    pose = Sophus::SE3d::exp(dx) * pose;
    lastCost = cost;

    cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;
    if (dx.norm() < 1e-6) {  // 当需要更新的左乘se(3)足够小。
      // converge
      break;
    }
  }
  cout << "pose by g-n: \n" << pose.matrix() << endl;

}

// 定义 Vertex 和  Edge

class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // Eigen 内部处理动态分配变量的对齐问题
    // setToOriginImpl()用来重置优化变量， 这里初始化一个空 的 SE3
    // 这里使用虚函数的原因是由C++11新特性，重写了父类的虚函数
    virtual void setToOriginImpl() override {
    _estimate = Sophus::SE3d();
  }
   /// left multiplication on SE3， setToOriginImpl()用来Upfate优化变量,这里使用左乘来更新
  virtual void oplusImpl(const double *update) override {
    Eigen::Matrix<double, 6, 1> update_eigen;
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
  }
    virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}
};

/// 定义边， 也就是重新投影误差， 我们知道重投影误差为2位， 类型是vector2d, 优化对象的类型是一个VertexPose
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 // 构造函数
  EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}
// 计算重新投影误差
  virtual void computeError() override {
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    Sophus::SE3d T = v->estimate();  // 获得顶点的观测value
    Eigen::Vector3d pos_pixel = _K * (T * _pos3d); // // K TP = s[u,v]
    pos_pixel /= pos_pixel[2]; // 齐次到非齐次
    _error = _measurement - pos_pixel.head<2>(); //_measurement：存储观测值
  }
   // 更新操作, 定义Jaco矩阵即可，这里使用就是G-N法
  virtual void linearizeOplus() override {
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    Sophus::SE3d T = v->estimate();
    Eigen::Vector3d pos_cam = T * _pos3d;
    double fx = _K(0, 0);
    double fy = _K(1, 1);
    double cx = _K(0, 2);
    double cy = _K(1, 2);
    double X = pos_cam[0];
    double Y = pos_cam[1];
    double Z = pos_cam[2];
    double Z2 = Z * Z;
    _jacobianOplusXi
      << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
      0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
  };

  virtual bool read(istream &in) override {}

  virtual bool write(ostream &out) const override {}

private:
  Eigen::Vector3d _pos3d;
  Eigen::Matrix3d _K;
};

void bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose
){
  // 构建图优化，先设定g2o
  // pose is 6 ， 3 for rotation, 3 for translation. landmark is 3 : X,Y,Z,观测点的维度
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  // pose is 6, landmark is 3
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器, 去优化Pose Matrix
    // 梯度下降方法，可以从GN, LM, DogLeg 中选， 接受一个BlockSolverType,用Linear
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));

    g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

  // 下面开始往图里面添加 Vertex 和 Edge
  // vertex， 只有一个R,t
  VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose
  vertex_pose->setId(0);
  vertex_pose->setEstimate(Sophus::SE3d());
  optimizer.addVertex(vertex_pose);

    //  Camera K
  Eigen::Matrix3d K_eigen;
  K_eigen <<
          K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
    K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
    K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);

  // edges
  int index = 1;
  for (size_t i = 0; i < points_2d.size(); ++i) {
    auto p2d = points_2d[i];
    auto p3d = points_3d[i];
    EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);
    edge->setId(index);
    edge->setVertex(0, vertex_pose); // 对应成员函数_vertex
    edge->setMeasurement(p2d); // 对应成员函数_mesurement
    edge->setInformation(Eigen::Matrix2d::Identity()); // 图中的Q就是信息矩阵，为了表示我们对误差各分量重视程度的不一样。
    // 一般情况下，我们都设置这个矩阵为单位矩阵，表示我们对所有的误差分量的重视程度都一样。
    optimizer.addEdge(edge);
    index++;
  }
  optimizer.setVerbose(true);
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  cout << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;
  pose = vertex_pose->estimate();
}


#include <iostream>
#include <chrono>
#include <dlib/matrix.h>

using namespace dlib;
using namespace std;

#define LOOP_COUNT 10

int main()
{
    int m = 2000;
    int p = 200;
    int n = 1000;

    matrix<double> A(m,p);
    matrix<double> B(p,n);

    for (int i = 0; i < m; i++)
    {
        for (int j = 0; j < p; j++)
        {
            A(i, j) = i * p + j + 1;
        }
    }
    for (int i = 0; i < p; i++)
    {
        for (int j = 0; j < n; j++)
        {
            B(i, j) = -(i * n + j) - 1;
        }
    }

    cout << "Making the first run of matrix product using the * operator" << endl;
    cout << "to get stable run time measurements." << endl << endl;
    matrix<double> C = A * B;

    cout << "Measuring performance of matrix product using the * operator." << endl << endl;
    auto start = chrono::steady_clock::now();
    for (int i = 0; i < LOOP_COUNT;i++)
    {
        C = A * B;
    }
    auto end = chrono::steady_clock::now();
    double elapsed_time = chrono::duration_cast<chrono::milliseconds>(end -start).count() / LOOP_COUNT;
    cout << "Elapsed time: " << elapsed_time << " milliseconds." << endl;
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_euler_angles.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

using namespace OpenTissue;

void do_test( double const & phi_in, double const & psi_in, double const & theta_in)
{
  OpenTissue::math::Quaternion<double> Q_in;
  OpenTissue::math::Quaternion<double> Q_out;
  OpenTissue::math::Quaternion<double> identity;
  OpenTissue::math::Quaternion<double> Qz1;
  OpenTissue::math::Quaternion<double> Qy;
  OpenTissue::math::Quaternion<double> Qz2;

  double const too_small = 10e-7;

  double phi_out   = 0.0;
  double psi_out   = 0.0;
  double theta_out = 0.0;

  Qz1.Rz(theta_in);
  Qy.Ry(psi_in);
  Qz2.Rz(phi_in);

  Q_in = OpenTissue::math::prod( Qz2 , OpenTissue::math::prod( Qy , Qz1) );

  OpenTissue::math::ZYZ_euler_angles(Q_in,phi_out,psi_out,theta_out);

  if(psi_in > 0.0)
  {
    // we only want to do this if we are not in a gimbal lock
    Qz1.Rz(theta_out);
    Qy.Ry(psi_out);
    Qz2.Rz(phi_out);
    Q_out = OpenTissue::math::prod( Qz2 , OpenTissue::math::prod( Qy , Qz1) );
    identity = OpenTissue::math::prod( OpenTissue::math::conj(Q_out), Q_in );

    double const s = fabs( fabs(identity.s())  - 1.0 );
    double const v0 = fabs(identity.v()(0));
    double const v1 = fabs(identity.v()(1));
    double const v2 = fabs(identity.v()(2));

    BOOST_CHECK( s < too_small);
    BOOST_CHECK( v0 < too_small);
    BOOST_CHECK( v1 < too_small);
    BOOST_CHECK( v2 < too_small);

    double const dphi = fabs(phi_in - phi_out);
    double const dpsi = fabs(psi_in - psi_out);
    double const dtheta = fabs(theta_in - theta_out);
    BOOST_CHECK( dphi < too_small);
    BOOST_CHECK( dpsi < too_small);
    BOOST_CHECK( dtheta < too_small);
  }
  else
  {
    // In gimbal lock phi and theta behaves strangely
    BOOST_CHECK_CLOSE( 0.0, theta_out, 0.01);
    double const dpsi = fabs(psi_out);
    BOOST_CHECK( dpsi < too_small);

    double new_phi = phi_in + theta_in;
    double const pi     = 3.1415926535897932384626433832795;
    double const two_pi = 2.0*pi;
    while(new_phi>pi) new_phi -= two_pi;
    while(new_phi<-pi) new_phi += two_pi;

    double const dphi = fabs(new_phi - phi_out);
    BOOST_CHECK( dphi < too_small);
  }
}

BOOST_AUTO_TEST_SUITE(opentissue_math_euler_angles);

BOOST_AUTO_TEST_CASE(ZYZ)
{
  size_t N = 15;

  double const pi     = 3.1415926535897932384626433832795;
  double const two_pi = 2.0*pi;
  double const delta = (two_pi)/(N-1);

  double phi = -pi+delta;
  for(;phi<pi;)
  {
    double psi = 0.0;
    for(;psi<pi;)
    {
      double theta = -pi+delta;
      for(;theta<pi;)
      {
        do_test( phi, psi, theta );
        theta += delta;
      }
      psi += delta;
    }
    phi += delta;
  }
}

BOOST_AUTO_TEST_SUITE_END();


#ifndef SEGWAY_SIM_COMMON_H
#define SEGWAY_SIM_COMMON_H

#include "segway_sim/CyberTimer.hpp"
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <math.h>
#include <vector>
#include <map>
#include <functional>
#include <signal.h>

static const uint32_t STATE_LENGTH = 7;
static const uint32_t INPUT_LENGTH = 2;
static const uint32_t CMD_LENGTH = 2;

inline void quat2eulZYX(const Eigen::Quaterniond &q,
                              Eigen::Vector3d &eul)
{
	double eul_tmp = 2.0 * (q.y() * q.y());
	eul(0) = atan2(2.0 * q.w() * q.x() + 2.0 * q.y() * q.z(), (1.0 - 2.0 * (q.x() * q.x())) - eul_tmp);

	double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
	if (abs(sinp) >= 1)
		eul(1) = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
	else
		eul(1) = asin(sinp);

	eul(2) = atan2(2.0 * q.w() * q.z() + 2.0 * q.x() * q.y(), (1.0 - eul_tmp) - 2.0 * (q.z() * q.z()));
}

inline void quat2rotm(const Eigen::Quaterniond &q,
                            Eigen::Matrix3d &rotm)
{
	rotm(0,0) = 1-2*q.y()*q.y()-2*q.z()*q.z();
	rotm(0,1) = 2*q.x()*q.y()-2*q.z()*q.w();
	rotm(0,2) = 2*q.x()*q.z()+2*q.y()*q.w();
	rotm(1,0) = 2*q.x()*q.y()+2*q.z()*q.w();
	rotm(1,1) = 1-2*q.x()*q.x()-2*q.z()*q.z();
	rotm(1,2) = 2*q.y()*q.z()-2*q.x()*q.w();
	rotm(2,0) = 2*q.x()*q.z()-2*q.y()*q.w();
	rotm(2,1) = 2*q.y()*q.z()+2*q.x()*q.w();
	rotm(2,2) = 1-2*q.x()*q.x()-2*q.y()*q.y();
}

inline void eul2quatZYX(const Eigen::Vector3d &eul,
                              Eigen::Quaterniond &q)
{
	double cy = cos(eul(2) * 0.5);
	double sy = sin(eul(2) * 0.5);
	double cp = cos(eul(1) * 0.5);
	double sp = sin(eul(1) * 0.5);
	double cr = cos(eul(0) * 0.5);
	double sr = sin(eul(0) * 0.5);

	q.w() = cy * cp * cr + sy * sp * sr;
	q.x() = cy * cp * sr - sy * sp * cr;
	q.y() = sy * cp * sr + cy * sp * cr;
	q.z() = sy * cp * cr - cy * sp * sr;
}

inline void removeYaw(Eigen::Quaterniond &q)
{
	Eigen::Vector3d eul;
	quat2eulZYX(q,eul);
	eul(2) = 0;
	eul2quatZYX(eul,q);
}

inline Eigen::Matrix3d rotx(double angle)
{
	Eigen::Matrix3d rotm;
	rotm(0,0) =  1; rotm(0,1) =           0; rotm(0,2) =           0;
	rotm(1,0) =  0; rotm(1,1) =  cos(angle); rotm(1,2) = -sin(angle);
	rotm(2,0) =  0; rotm(2,1) =  sin(angle); rotm(2,2) =  cos(angle);
	return rotm;
}

inline Eigen::Matrix3d roty(double angle)
{
	Eigen::Matrix3d rotm;
	rotm(0,0) =  cos(angle); rotm(0,1) =  0; rotm(0,2) = sin(angle);
	rotm(1,0) =           0; rotm(1,1) =  1; rotm(1,2) = 0;
	rotm(2,0) = -sin(angle); rotm(2,1) =  0; rotm(2,2) = cos(angle);
	return rotm;
}

inline Eigen::Matrix3d rotz(double angle)
{
	Eigen::Matrix3d rotm;
	rotm(0,0) =  cos(angle); rotm(0,1) = -sin(angle); rotm(0,2) = 0;
	rotm(1,0) =  sin(angle); rotm(1,1) =  cos(angle); rotm(1,2) = 0;
	rotm(2,0) =           0; rotm(2,1) =           0; rotm(2,2) = 1;
	return rotm;
}

inline double deg2rad(double angle)
{
	return angle*M_PI/180.0;
}

inline double rad2deg(double angle)
{
	return angle*180.0/M_PI;
}

inline double saturate(double in, double min, double max)
{
	double out;
	if(min>max)
	{
		double tmp = max;
		max = min;
		min = tmp;
	}

	if(in>max)
		out = max;
	else if(in<min)
		out = min;
	else
		out = in;

	return out;
}

inline void saturateInPlace(double &in, double min, double max)
{
	if(min>max)
	{
		double tmp = max;
		max = min;
		min = tmp;
	}

	if(in>max)
		in = max;
	else if(in<min)
		in = min;
}

inline void saturateInPlace(double in[], double min, double max, uint32_t len)
{
	if(min>max)
	{
		double tmp = max;
		max = min;
		min = tmp;
	}

	for(uint32_t i = 0; i<len; i++)
	{
		if(in[i]>max)
			in[i] = max;
		else if(in[i]<min)
			in[i] = min;
	}

}

inline double constrainAngle(double x){
    x = fmod(x + M_PI,2*M_PI);
    if (x < 0)
        x += M_PI;
    return x - M_PI;
}

inline void omega2eulRatesZYX(const Eigen::Vector3d &omega,
                              const Eigen::Quaterniond &q,
                                    Eigen::Vector3d &eulDot)
{
	Eigen::Vector3d eul;
	quat2eulZYX(q,eul);
	const double x = eul(0);
	const double y = eul(1);

	Eigen::Matrix3d M;
	M(0,0) = 1; M(0,1) = sin(x)*tan(y); M(0,2) = cos(x)*tan(y);
	M(1,0) = 0; M(1,1) =        cos(x); M(1,2) =       -sin(x);
	M(2,0) = 0; M(2,1) = sin(x)/cos(y); M(2,2) = cos(x)/cos(y);

	eulDot = M*omega;
}

inline void eulRates2omegaZYX(const Eigen::Vector3d &eulDot,
                              const Eigen::Quaterniond &q,
                                    Eigen::Vector3d &omega)
{
	Eigen::Vector3d eul;
	quat2eulZYX(q,eul);
	const double x = eul(0);
	const double y = eul(1);

	Eigen::Matrix3d M;
	M(0,0) = 1; M(0,1) =       0; M(0,2) =       -sin(y);
	M(1,0) = 0; M(1,1) =  cos(x); M(1,2) = sin(x)*cos(y);
	M(2,0) = 0; M(2,1) = -sin(x); M(2,2) = cos(x)*cos(y);

	omega = M*eulDot;
}
#endif


// Array stencil benchmark

#include <blitz/array.h>
#include <blitz/traversal.h>
#include <blitz/benchext.h>
#include <blitz/rand-uniform.h>
#include <blitz/array/stencil-et.h>

BZ_USING_NAMESPACE(blitz)

#if defined(BZ_FORTRAN_SYMBOLS_WITH_TRAILING_UNDERSCORES)
  #define stencilf stencilf_
  #define stencilftiled stencilftiled_
  #define stencilf90 stencilf90_
#elif defined(BZ_FORTRAN_SYMBOLS_CAPS)
  #define stencilf       STENCILF
  #define stencilftiled  STENCILFTILED
  #define stencilf90     STENCILF90
#endif

extern "C" {
    void stencilf(double* A, double* B, int& N, int& iters);
    void stencilftiled(double* A, double* B, int& N, int& iters);
    void stencilf90(double* A, double* B, int& N, int& iters);
}

#ifdef FORTRAN_90
void stencilFortran90Version(BenchmarkExt<int>& bench);
#endif
void stencilFortran77Version(BenchmarkExt<int>& bench);
void stencilFortran77VersionTiled(BenchmarkExt<int>& bench);
void stencilBlitzVersion(BenchmarkExt<int>& bench);
void stencilBlitzExpressionVersion(BenchmarkExt<int>& bench);
void stencilBlitzProductVersion(BenchmarkExt<int>& bench);
void stencilBlitzProductVersion2(BenchmarkExt<int>& bench);
void stencilBlitzProductVersion3(BenchmarkExt<int>& bench);
void stencilBlitzStencilVersion(BenchmarkExt<int>& bench);
void stencilBlitzIndexVersion(BenchmarkExt<int>& bench);

int main()
{
    int numBenchmarks = 10;
#ifndef FORTRAN_90
		numBenchmarks--;   // No fortran 90
#endif

    BenchmarkExt<int> bench("Array stencil", numBenchmarks);

    const int numSizes = 28;

    bench.setNumParameters(numSizes);
    bench.setRateDescription("Mflops/s");

    Vector<int> parameters(numSizes);
    Vector<long> iters(numSizes);
    Vector<double> flops(numSizes);

    for (int i=0; i < numSizes; ++i)
    {
        parameters[i] = (i+1) * 8;
        iters[i] = 32*8*8*8/(i+1)/(i+1)/(i+1)/4;
        if (iters[i] < 2)
            iters[i] = 2;
        int npoints = parameters[i] - 2;
        flops[i] = npoints * npoints * npoints * 7 * 2;
    }

    bench.setParameterVector(parameters);
    bench.setIterations(iters);
    bench.setOpsPerIteration(flops);

    bench.beginBenchmarking();
#ifdef FORTRAN_90
    stencilFortran90Version(bench);
#endif
    stencilBlitzVersion(bench);
    stencilBlitzStencilVersion(bench);
    stencilBlitzExpressionVersion(bench);
    stencilBlitzProductVersion(bench);
    stencilBlitzProductVersion2(bench);
    stencilBlitzProductVersion3(bench);
    stencilBlitzIndexVersion(bench);
    stencilFortran77Version(bench);
    stencilFortran77VersionTiled(bench);
    bench.endBenchmarking();

    bench.saveMatlabGraph("stencil.m","plot");

    return 0;
}

void initializeRandomDouble(double* data, int numElements, int stride = 1)
{
    static Random<Uniform> rnd;

    for (int i=0; i < numElements; ++i)
        data[i*stride] = rnd.random();
}

void stencilBlitzVersion(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Blitz++ Range Expr");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "Blitz++: N = " << N << endl;
        cout.flush();

        long iters = bench.getIterations();

        Array<double,3> A(N,N,N), B(N,N,N);
        initializeRandomDouble(A.data(), N*N*N, A.stride(thirdDim));
        initializeRandomDouble(B.data(), N*N*N, B.stride(thirdDim));
        TinyVector<int,2> size = N-2;
        generateFastTraversalOrder(size);
        double c = 1/7.;

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            Range I(1,N-2), J(1,N-2), K(1,N-2);

            A(I,J,K) = c * (B(I,J,K) + B(I+1,J,K) + B(I-1,J,K) + B(I,J+1,K)
                + B(I,J-1,K) + B(I,J,K+1) + B(I,J,K-1));

            B(I,J,K) = c * (A(I,J,K) + A(I+1,J,K) + A(I-1,J,K) + A(I,J+1,K)
                + A(I,J-1,K) + A(I,J,K+1) + A(I,J,K-1));
        }
        bench.stop();
    }

    bench.endImplementation();
}

BZ_DECLARE_STENCIL_OPERATOR1(test1,B)
return (1./7) * ( (*B) + B.shift(1,0) + B.shift(-1,0) + B.shift(1,1)
		  + B.shift(-1,1) + B.shift(1,2) + B.shift(-1,2));
BZ_END_STENCIL_OPERATOR

BZ_ET_STENCIL(test1, double, double,shape(-1,-1,-1),shape(1,1,1))

BZ_DECLARE_STENCIL2(test1stencil,A,B)
  A=test1(B);
BZ_END_STENCIL_WITH_SHAPE(shape(-1,-1,-1),shape(1,1,1))

void stencilBlitzExpressionVersion(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Blitz++ StencilOp");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "Blitz++ Stencil Operator: N = " << N << endl;
        cout.flush();

        long iters = bench.getIterations();

        Array<double,3> A(N,N,N), B(N,N,N);
        initializeRandomDouble(A.data(), N*N*N, A.stride(thirdDim));
        initializeRandomDouble(B.data(), N*N*N, B.stride(thirdDim));
        TinyVector<int,2> size = N-2;
        generateFastTraversalOrder(size);
        double c = 1/7.;

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            Range I(1,N-2), J(1,N-2), K(1,N-2);
	    A(I,J,K) = test1(B);

	    B(I,J,K) = test1(A);
        }
        bench.stop();
    }

    bench.endImplementation();
}

void stencilBlitzProductVersion(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Blitz++ product");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "Blitz++ Stencil Operator on product: N = " << N << endl;
        cout.flush();

        long iters = bench.getIterations();

        Array<double,3> A(N,N,N), B(N,N,N),C(N,N,N);
        initializeRandomDouble(A.data(), N*N*N, A.stride(thirdDim));
        initializeRandomDouble(B.data(), N*N*N, B.stride(thirdDim));
        TinyVector<int,2> size = N-2;
        generateFastTraversalOrder(size);
        double c = 1/7.;

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            Range I(1,N-2), J(1,N-2), K(1,N-2);
	    C=B*B;
	    A(I,J,K) = test1(C);
	    C=A*A;
	    B(I,J,K) = test1(C);
        }
        bench.stop();
    }

    bench.endImplementation();
}

void stencilBlitzProductVersion2(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Blitz++ product w alloc");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "Blitz++ Stencil Operator on product: N = " << N << endl;
        cout.flush();

        long iters = bench.getIterations();

        Array<double,3> A(N,N,N), B(N,N,N);
        initializeRandomDouble(A.data(), N*N*N, A.stride(thirdDim));
        initializeRandomDouble(B.data(), N*N*N, B.stride(thirdDim));
        TinyVector<int,2> size = N-2;
        generateFastTraversalOrder(size);
        double c = 1/7.;

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            Range I(1,N-2), J(1,N-2), K(1,N-2);
	    {
	      Array<double,3>C(B*B);
	      A(I,J,K) = test1(C);
	    }
	    {
	      Array<double,3>C(A*A);
	      B(I,J,K) = test1(C);
	    }
        }
        bench.stop();
    }

    bench.endImplementation();
}

void stencilBlitzProductVersion3(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Blitz++ product expr");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "Blitz++ Stencil Operator on product expr: N = " << N << endl;
        cout.flush();

        long iters = bench.getIterations();

        Array<double,3> A(N,N,N), B(N,N,N);
        initializeRandomDouble(A.data(), N*N*N, A.stride(thirdDim));
        initializeRandomDouble(B.data(), N*N*N, B.stride(thirdDim));
        TinyVector<int,2> size = N-2;
        generateFastTraversalOrder(size);
        double c = 1/7.;

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            Range I(1,N-2), J(1,N-2), K(1,N-2);
	    A(I,J,K) = test1(B*B);

	    B(I,J,K) = test1(A*A);
        }
        bench.stop();
    }

    bench.endImplementation();
}

void stencilBlitzIndexVersion(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Blitz++ Indexed StencilOp");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "Blitz++ Indexed Stencil Operator: N = " << N << endl;
        cout.flush();

        long iters = bench.getIterations();

        Array<double,3> A(N,N,N), B(N,N,N);
        initializeRandomDouble(A.data(), N*N*N, A.stride(thirdDim));
        initializeRandomDouble(B.data(), N*N*N, B.stride(thirdDim));
        TinyVector<int,2> size = N-2;
        generateFastTraversalOrder(size);
        double c = 1/7.;

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            Range I(1,N-2), J(1,N-2), K(1,N-2);
	    A(I,J,K) = test1(B(tensor::i, tensor::j, tensor::k));

	    B(I,J,K) = test1(A(tensor::i, tensor::j, tensor::k));
        }
        bench.stop();
    }

    bench.endImplementation();
}

void stencilBlitzStencilVersion(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Blitz++ Stencil");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "Blitz++ Stencil: N = " << N << endl;
        cout.flush();

        long iters = bench.getIterations();

        Array<double,3> A(N,N,N), B(N,N,N);
        initializeRandomDouble(A.data(), N*N*N, A.stride(thirdDim));
        initializeRandomDouble(B.data(), N*N*N, B.stride(thirdDim));
        TinyVector<int,2> size = N-2;
        generateFastTraversalOrder(size);
        double c = 1/7.;

	;        bench.start();
        for (long i=0; i < iters; ++i)
        {
            Range I(1,N-2), J(1,N-2), K(1,N-2);
	    applyStencil(test1stencil(),A,B);
	    applyStencil(test1stencil(),B,A);
        }
        bench.stop();
    }

    bench.endImplementation();
}

void stencilFortran77Version(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Fortran 77");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "Fortran 77: N = " << N << endl;
        cout.flush();

        int iters = (int)bench.getIterations();

        size_t arraySize = size_t(N) * size_t(N) * N;

        double* A = new double[arraySize];
        double* B = new double[arraySize];

        initializeRandomDouble(A, arraySize);
        initializeRandomDouble(B, arraySize);

        bench.start();
        stencilf(A, B, N, iters);
        bench.stop();

        delete [] A;
        delete [] B;
    }

    bench.endImplementation();
}

void stencilFortran77VersionTiled(BenchmarkExt<int>& bench)
{
    bench.beginImplementation("Fortran 77 (tiled)");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "Fortran 77: N = " << N << endl;
        cout.flush();

        int iters = (int)bench.getIterations();

        size_t arraySize = size_t(N) * size_t(N) * N;

        double* A = new double[arraySize];
        double* B = new double[arraySize];

        initializeRandomDouble(A, arraySize);
        initializeRandomDouble(B, arraySize);

        bench.start();
        stencilftiled(A, B, N, iters);
        bench.stop();

        delete [] A;
        delete [] B;
    }

    bench.endImplementation();
}

#ifdef FORTRAN_90
void stencilFortran90Version(BenchmarkExt<int>& bench)
{
   bench.beginImplementation("Fortran 90");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();

        cout << "Fortran 90: N = " << N << endl;
        cout.flush();

        int iters = (int)bench.getIterations();

        size_t arraySize = size_t(N) * size_t(N) * N;

        double* A = new double[arraySize];
        double* B = new double[arraySize];

        initializeRandomDouble(A, arraySize);
        initializeRandomDouble(B, arraySize);

        bench.start();
        stencilf90(A, B, N, iters);
        bench.stop();

        delete [] A;
        delete [] B;
    }

    bench.endImplementation();
}
#endif


/**
 * @brief 2D配准 闭式解算法
 * @author hqy
 * @date 2021.3.15
 */

#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/features2d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

template<typename T>
void printMat(const T& mat, int size){
    for (int i = 0; i < size; i++){
        for (int j = 0; j < size - 1; j++){
            std::cout << mat(i, j) << ", ";
        }
        std::cout << mat(i, size - 1) << std::endl;
    }
}

void hMatrixSolver(
    const std::vector<cv::DMatch>& matches,
    const std::vector<cv::KeyPoint>& pts1,
    const std::vector<cv::KeyPoint>& pts2,
    Eigen::Matrix3d& H
) {
    int size = matches.size();
    Eigen::Matrix3Xd P(3, size);
    Eigen::Matrix3Xd Q(3, size);
    P.setZero();
    Q.setZero();
    #pragma omp parallel for num_threads(8)
    for (size_t i = 0; i < matches.size(); i++) {
        const cv::Point2f& _p1 = pts1[matches[i].queryIdx].pt;
        const cv::Point2f& _p2 = pts2[matches[i].trainIdx].pt;
        P.block<3, 1>(0, i) = Eigen::Vector3d(_p1.x, _p1.y, 1);
        Q.block<3, 1>(0, i) = Eigen::Vector3d(_p2.x, _p2.y, 1);
    }
    Eigen::Matrix3d PPT = P * P.transpose();
    Eigen::Matrix3d inv = (PPT).ldlt().solve(Eigen::Matrix3d::Identity());    // LDLT分解求逆矩阵 (PP^T)
    Eigen::Matrix3d res = PPT * inv;
    H = Q * P.transpose() * inv;
}

void featureExtract(
    const cv::Mat& src1,
    const cv::Mat& src2,
    cv::Mat& dst,
    Eigen::Matrix3d& H
){
    std::vector<cv::DMatch> matches;
    std::vector<cv::KeyPoint> pts1, pts2;
    #define POINT_NUM 64
    cv::Mat dscp1, dscp2;
    #pragma omp parallel sections
    {
        #pragma omp section
        {
            cv::Ptr<cv::FeatureDetector> det1 = cv::ORB::create(POINT_NUM);
            cv::Ptr<cv::DescriptorExtractor> des1 = cv::ORB::create(POINT_NUM);
            det1->detectAndCompute(src1, cv::noArray(), pts1, dscp1);
        }
        #pragma omp section
        {
            cv::Ptr<cv::FeatureDetector> det2 =  cv::ORB::create(POINT_NUM);
            cv::Ptr<cv::DescriptorExtractor> des2 =  cv::ORB::create(POINT_NUM);
            det2->detectAndCompute(src2, cv::noArray(), pts2, dscp2);
        }
    }
    std::cout << "Parallel ORB descriptors found.\n";
    cv::Ptr<cv::DescriptorMatcher> match = cv::DescriptorMatcher::create(cv::DescriptorMatcher::BRUTEFORCE_HAMMINGLUT);
    match->match(dscp1, dscp2, matches);
    std::cout << "ORB descriptors matched.\n";
    std::vector<cv::Point2f> crn1, crn2;
    for (const cv::DMatch& mt: matches){
        crn1.push_back(pts1[mt.queryIdx].pt);
        crn2.push_back(pts2[mt.trainIdx].pt);
    }
    cv::Mat mask;

    cv::Mat cvH = cv::findHomography(crn1, crn2, mask, cv::RANSAC, 16);
    printf("Homography found. mask is %d, %d, %d, %d\n", mask.cols, mask.rows, mask.type(), CV_8UC1);
    printf("The original match num is %lu.\n", matches.size());
    std::vector<cv::DMatch> truth;
    for (size_t i = 0; i < matches.size(); i++){
        if (mask.at<uchar>(i) == 1){
            truth.push_back(matches[i]);
        }
    }
    cv::drawMatches(src1, pts1, src2, pts2, truth, dst);
    hMatrixSolver(truth, pts1, pts2, H);
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            printf("%lf, ", cvH.at<double>(i, j));
        }
        printf("\n");
    }
}

/// 可视化
void alignmentVisualize(const cv::Mat& src, cv::Mat&dst, Eigen::Matrix3d H) {
    dst.create(src.rows, src.cols, CV_8UC3);
    #pragma for num_threads(8);
    for (int i = 0; i < src.rows; i++) {
        for (int j = 0; j < src.cols; j++) {
            Eigen::Vector3d now(j, i, 1.0);
            Eigen::Vector3d trans = H * now;
            int px = trans[0], py = trans[1];
            if (px >= 0 && px < dst.cols && py >= 0 && py < dst.rows) {
                dst.at<cv::Vec3b>(py, px) = src.at<cv::Vec3b>(i, j);
            }
        }
    }
}

int main() {
    cv::Mat src1 = cv::imread("../data/ImageA.jpg");
    cv::Mat src2 = cv::imread("../data/ImageB.jpg");
    cv::Mat gray1, gray2;
    cv::cvtColor(src1, gray1, cv::COLOR_BGR2GRAY);
    cv::cvtColor(src2, gray2, cv::COLOR_BGR2GRAY);
    cv::equalizeHist(gray1, gray1);
    cv::equalizeHist(gray2, gray2);
    cv::Mat aligned;
    cv::Mat feats;

    Eigen::Matrix3d H;

    featureExtract(gray1, gray2, feats, H);
    std::cout << "H matrix is:\n";
    printMat<Eigen::Matrix3d>(H, 3);
    cv::imwrite("../data/features.jpg", feats);

    alignmentVisualize(src1, aligned, H);
    cv::imwrite("../data/result.jpg", aligned);
    return 0;
}


/**
 * Kalman Filter
 * Skeleton code for teaching
 * A3M33MKR
 * Czech Technical University
 * Faculty of Electrical Engineering
 * Intelligent and Mobile Robotics group
 *
 * Authors: Zdeněk Kasl, Karel Košnar kosnar@labe.felk.cvut.cz
 *
 * this code is inspired by tutorial on Kalman Filter by Greg Czerniak
 *
 * Licence: MIT (see LICENSE file)
 **/
/*TODO set up the convarience matrices better and re-measure the noice constants (process noise/measurenment noice)*/

#include<cmath>
#include<cassert>
#include<cstdlib>
#include<fstream>
#include<iostream>
#include <Eigen/Dense>

#include "gui/gui.h"
#include "systemSimulator/system.h"

using namespace imr;
using namespace gui;
using namespace Eigen;
using namespace std;

#define PRINT(name) Print(#name, (name))

void Print(const char *name, Matrix4f m);

Point KalmanFilter(Point measuredPosition);

IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");

Matrix4f A, B, Sigma, R, Q, E, C;
//Matrix2f Q;
//Matrix<float, 2, 4> C;
Vector4f mu, u;
float dt = 0.1;

//noice constants
double q = 100;
double r = 0.1;

//step constant
int step = 10;

void initMatrices() {
    A << 1, 0, dt, 0,
            0, 1, 0, dt,
            0, 0, 1, 0,
            0, 0, 0, 1;

    B << 0, 0, 0, 0,
            0, -0.5 * dt * dt, 0, 0,
            0, 0, 0, 0,
            0, 0, 0, -dt;

    u << 9.8, 9.8, 9.8, 9.8;

    mu << -300, 100, 0, 0;

    Sigma << 0.1, 0, 0, 0,
            0, 0.1, 0, 0,
            0, 0, 0.1, 0,
            0, 0, 0, 0.1;

// /Noice consts.
//-------------------------------------------------------------------
    C <<    1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, 0,
            0, 0, 0, 1;

    Q <<    q, 0, 0, 0,
            0, q, 0, 0,
            0, 0, q, 0,
            0, 0, 0, q;

    R <<    r, 0, 0, 0,
            0, r, 0, 0,
            0, 0, r, 0,
            0, 0, 0, r;
//-------------------------------------------------------------------
    E <<    1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, 0,
            0, 0, 0, 1;
}

void help(char **argv) {
    std::cout << "\nUsage of the program " << argv[0] + 2 << std::endl
              << "Parameter [-h or -H] displays this message." << std::endl
              << " Parametr [-n or -N] number of simulation steps."
              << std::endl;
}

int main(int argc, char **argv) {
    int nSteps = 1000;
    char *dataFile;

    // Parse all console parameters
    for (int i = 0; i < argc; i++) {
        if (argv[i][0] == '-') {
            switch (argv[i][1]) {
                //> HELP
                case 'H' :
                case 'h' :
                    help(argv);
                    break;
                case 'N':
                case 'n':
                    assert(i + 1 < argc);
                    assert(atoi(argv[i + 1]) > 1);
                    step = atoi(argv[i + 1]);
                    break;
                default :
                    std::cout << "Parameter \033[1;31m" << argv[i] << "\033[0m is not valid!\n"
                              << "Use parameter -h or -H for help." << std::endl;
                    break;
            }
        }
    }
    // All parameters parsed

    Gui gui;
    System system;

    //> comment line below in order to let the program
    //> continue right away
    gui.startInteractor();

    initMatrices();

    Point measurement;
    Point truth;
    Point kfPosition;

    for (int i = 1; i < nSteps; i++) {
        system.makeStep();
        truth = system.getTruthPosition();
        measurement = system.getMeasurement();
        kfPosition = KalmanFilter(measurement);
        gui.setPoints(truth, measurement, kfPosition);

        //> comment line below in order to let the program
        //> continue right away
        //if (i % step == 0) gui.startInteractor();

    }
    gui.startInteractor();
    return EXIT_SUCCESS;
}

/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//                               implement Kalman Filter here
/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

Point KalmanFilter(const Point measuredPosition) {
    Point ret;
    Vector4f mu_actual, z, y;
    Matrix4f Sigma_actual, K, S, S_inv;

    z << measuredPosition.x,
            measuredPosition.y,
            0,
            0;

    PRINT(A);
    PRINT(C);
    //-------------------------------------------------------------------------------
    mu_actual = A * mu + B * u;

    //Error estimation
    //-------------------------------------------------------------------------------
    Sigma_actual = A * Sigma * A.transpose() + R;
    PRINT(Sigma_actual);

    //K gain
    //-------------------------------------------------------------------------------
    S = C * Sigma_actual * C.transpose() + Q;
    PRINT(S);

    K = Sigma_actual * C.transpose() * S.inverse();
    PRINT(K);

    //improvement
    //-------------------------------------------------------------------------------
    y = z - C * mu_actual;
    mu = mu_actual + K * y;
    Sigma = (E - K * C) * Sigma_actual;
    PRINT(Sigma);

    ret.x = mu(0);
    ret.y = mu(1);

    //Simulation of system
    //ret.x = mu_actual(0);
    //ret.y = mu_actual(1);
    //mu = mu_actual;

    cout << "X: " << ret.x << "    Y: " << ret.y <<
             "    Vx: " << mu(2) << "    Vy: " << mu(3) << endl;
    cout << "-------------------------------------------------------------------" << endl;

    return ret;
}

void Print(const char *name, Matrix4f m){
    cout << name << ": " << endl;
    cout << m.format(CleanFmt) << endl;
    cout << "-------------------------------------------------------------------" << endl;
}

/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
/// >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Shikhar Vashistha
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/utility.hpp>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

double f(double) { /* cout << "double\n"; */ return 1.0; }
complex<double> f(complex<double>)
{
    //cout << "complex\n";
    return complex<double>(1.0, -1.0);
}

int main(int, char**)
{
    using namespace mtl; using mtl::io::tout;
    unsigned size = 4, row = size + 1, col = size;

    double tol(0.00001);
    dense_vector<double>                vec(size), vec1(size);
    dense2D<double>                     A(row, col), Q(row, row), R(row, col), A_test(row, col),
        A_t(col, row), Q_t(col, col), R_t(col, row), A_t_test(col, row);
    dense2D<complex<double> >                            dz(row, col), Qz(row, row), Rz(row, col);
    dense2D<double, mat::parameters<col_major> >         dc(size, size);
    compressed2D<double>                                 Ac(size, size), Qc(size, size), Rc(size, size), A_testc(size, size);
    A = 0;

    A[0][0] = 1;    A[0][1] = 1;    A[0][2] = 1;
    A[1][0] = 3;    A[1][1] = -1;   A[1][2] = -2;
    A[2][0] = 1;    A[2][1] = 7;    A[2][2] = 1;
    A[3][3] = -10;  A[4][0] = 4;    A[4][2] = 3;
    tout << "A=\n" << A << "\n";
    laplacian_setup(Ac, 2, 2);

    tout << "START-----dense2d---------row > col\n";

    dense2D<double> A1(A[iall][iall]), A2(A);
    boost::tie(Q, R) = lq(A1);
    tout << "R=\n" << R << "\n";
    tout << "Q=\n" << Q << "\n";
    A_test = Q * R - A2;
    tout << "Q*R=\n" << Q * R << "\n";

    tout << "one_norm(Rest A)=" << one_norm(A_test) << "\n";
    MTL_THROW_IF(one_norm(A_test) > tol, mtl::logic_error("wrong LQ decomposition of matrix A"));

    tout << "START------dense2d-------row < col\n";

    A_t = trans(A);
    boost::tie(Q_t, R_t) = lq(A_t);
    tout << "R_t=\n" << R_t << "\n";
    tout << "Q_t=\n" << Q_t << "\n";
    A_t_test = Q_t * R_t - A_t;
    tout << "Q_t*R_t=\n" << Q_t * R_t << "\n";

    tout << "one_norm(Rest A')=" << one_norm(A_t_test) << "\n";
    MTL_THROW_IF(one_norm(A_t_test) > tol, mtl::logic_error("wrong LQ decomposition of matrix trans(A)"));

    tout << "START-------compressed2d-------row > col\n";
#if 1
    boost::tie(Qc, Rc) = lq(Ac);
    tout << "R=\n" << Rc << "\n";
    tout << "Q=\n" << Qc << "\n";
    A_testc = Qc * Rc - Ac;
    tout << "Q*R=\n" << Qc * Rc << "\n";
    tout << "A=\n" << Ac << "\n";

    tout << "one_norm(Rest A)=" << one_norm(A_testc) << "\n";
    MTL_THROW_IF(one_norm(A_testc) > tol, mtl::logic_error("wrong LQ decomposition of matrix A"));
#endif

#if 0
    dz[0][0] = complex<double>(1.0, 0.0);
    dz[0][1] = complex<double>(1.0, 0.0);
    dz[0][2] = complex<double>(1, 0);
    dz[1][0] = complex<double>(1, 0);
    dz[1][1] = complex<double>(-1, 0);
    dz[1][2] = complex<double>(-2, 0);
    dz[2][0] = complex<double>(1, 0);
    dz[2][1] = complex<double>(-2, 0);
    dz[2][2] = complex<double>(1, 0);
    dz[3][3] = complex<double>(-10, 0);
    tout << "MAtrix complex=\n" << dz << "\n";

    tout << "START-----complex---------" << dz[0][0] << "\n";
    //AxB==BxA(For square matrix) LQ==QR(Transpose(A)
    boost::tie(Qz, Rz) = lq(dz);
    // Rz= lq_zerl(dz).second;
    // tout<<"MAtrix  R="<< Rz <<"\n";
    // tout<<"MAtrix  Q="<< Qz <<"\n";
    // tout<<"MAtrix  A=Q*R--outside"<< Qz*Rz <<"\n";
#endif
    /*
    Normal equations
    A^T A x = A^T b

    These equations are called the normal equations of the least squares problem

    Coefficient matrix  A^TA is the Gram matrix of A

    Equivalent to del f(x) = 0 where f(x) = ||Ax-b||^2

    All solutions of the least squares problem satisfy the normal equations.

    if A has linearly independent columns, then:

    A^T is non-singular

    Normal equations have unique solution xcap=(A^T A)^-1 A^T b
     */
    //Rewriting least squares solution using QR/LQ factorization A = QR, QR==LQ(transpose(A)).
    //  x = R^-1 Q^T x b
    /*  1. compute QR factorization A = QR(2mn^2 flops if A is m × n)
        2. matrix - vector product d = Q^T b(2mn flops)
        3. solve Rx = d by back substitution(n^2 flops)
        complexity: 2mn^2 flops
    */
    /*
       Example

         | 3 -6|    |-1 |
       A=| 4 -8|, b=| 7 |
	 | 0  1|    | 2 |

    1. QR/LQ factorization A = QR/LQ with

          |3/5 0|
    	Q=|4/5 0|, R=|5 -10|
     	  | 0  1|    |0	 1 |

    2. calculate d=Q^Tb = (5,2)

    3. solve Rx=d

    	|5 -10| |x1| = |5|
	|0  1 | |x2|   |2|

	solution is x1=5, x2=2

     */
    //Problem with regular method occurs when forming Gram matrix A^T A as it is unstable
    //QR factorization method is more stable because it avoids forming A^T A
    dense2D<double> x(A[iall][iall]),d(A[iall][iall]),b(A[iall][iall]);
    hessian_setup(x, 3.0); hessian_setup(d, 1.0);
    hessian_setup(b, 3.0);
    b[0][0] = -1, b[1][0] = 7, b[2][0] = 2;
    boost::tie(Q, R) = lq(A);
    mult(Q,b,d);
    d = trans(Q) * b;
    //d = trans(Q) * b;
    //lq decomposition is performed here so R==L here
    x = inv(R) * d;
    tout << "Solution to least square's problem is:" << x;
    return 0;
}


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

int main()
{
  Eigen::Vector4d v;
  Eigen::Matrix4d C;

  v << 1,2,3,4;
  C = v.asDiagonal();

  std::cout << C << std::endl;

  return 0;
}


//
// Created by Sabar Nimmagadda on 4/25/20.
//
#include "Linear Algebra/computations.h"
#include <Eigen/LU>

#include <Eigen/QR>
#include <Eigen/Householder>
#include <Eigen/Eigenvalues>

using Eigen::MatrixXd;
using std::string;

  MatrixXd Computations::ComputeL(MatrixXd m) {
     typedef Eigen::Matrix<double, 3, 3 > L_Matrix;
     Eigen::FullPivLU<L_Matrix> lu(m);
     //Since the L matrix is a square matrix with the rowsize of the original matrix.
     L_Matrix l = L_Matrix::Identity();
     l.block<3,3>(0,0).triangularView<Eigen::StrictlyLower>() = lu.matrixLU();
     return l;
  }
  MatrixXd Computations::ComputeU(MatrixXd m) {
     typedef Eigen::Matrix<double, 3, 3> U_Matrix;
     Eigen::FullPivLU<U_Matrix> lu(m);
     //Since the U matrix has the exact dimensions of the original matrix.
     U_Matrix u = lu.matrixLU().triangularView<Eigen::Upper>();
     return u;
  }
  MatrixXd Computations::ComputePermutationMatrix(MatrixXd m) {
      typedef Eigen::Matrix<double, 3, 3> P_Matrix;
      Eigen::FullPivLU<P_Matrix> lu(m);
      P_Matrix p = lu.matrixLU();
      return p;
  }
  MatrixXd Computations::ComputeInverse(const MatrixXd& m) {
      return m.inverse();
  }

  MatrixXd Computations::ComputeRREF(const MatrixXd& m) {
      if (m.rows() >= 1 && m.cols() >= 1) {
          int lead = 0;
          MatrixXd out = m;
          while (lead < m.rows()) {
              float divisor, multiplier;
              for (int r = 0; r < m.rows(); r++) {
                  if (out(lead, lead) != 0) {
                      divisor = out(lead, lead); //What divides a cell to make its value = 1.
                      multiplier = out(r, lead)/ out(lead, lead); //What is multiplied to a cell, so that when it is subtracted the value becomes 0.
                  } else {
                      divisor = 1;
                      multiplier = 1;
                  }
                  for(int c = 0; c < m.cols(); c++) {
                      if (lead == r) {
                          out(r,c) /= divisor;//This makes the pivots = 1.
                      } else {
                          out(r,c) -= out(lead, c) * multiplier; //Making everything that is not a pivot = 0;
                      }
                  }
              }
              lead++;
          }
          return out;
      }
  }

  MatrixXd Computations::ComputeMultiply(MatrixXd matrix1, MatrixXd matrix2) {
      if (matrix1.cols() == matrix2.rows()) {
          return matrix1*matrix2;
      }
  }

  MatrixXd Computations::ComputeR(const MatrixXd& matrix) {
      Eigen::HouseholderQR<M3X3> qr(matrix.rows(), matrix.cols());
      qr.compute(matrix);
      MatrixXd R = qr.matrixQR().triangularView<Eigen::Upper>();
      return R;
  }

  MatrixXd Computations::ComputeQ(const MatrixXd& matrix) {
      Eigen::HouseholderQR<M3X3> qr(matrix.rows(), matrix.cols());
      qr.compute(matrix);
      MatrixXd Q = qr.householderQ();
      return Q;
  }

  double Computations::ComputeDotProduct(MatrixXd matrix1, MatrixXd matrix2, int dim) {
      double product = 0;
      for (int i = 0; i < dim; i++) {
          for (int j = 0; j < dim; j++) {
              product += matrix1(i, j) * matrix2(i, j);
          }
      }
      return product;
  }

  VectorXcd Computations::ComputeEigenValues(const MatrixXd& matrix) {
      VectorXcd ret = matrix.eigenvalues();
      return ret;
  }

  MatrixXcd Computations::ComputeEigenVectors(const MatrixXd& matrix) {
      Eigen::EigenSolver<MatrixXd> es(matrix);
      return es.eigenvectors();
  }

  int Computations::ComputeDeterminant(const MatrixXd& matrix) {
      return matrix.determinant();
  }
  MatrixXd Computations::ComputeColSpace(MatrixXd matrix) {
      if (matrix.size() >= 1) {
          Eigen::ColPivHouseholderQR<MatrixXd> qr(matrix);
          MatrixXd new_mat;
          if (qr.rank() != 0) {
              for (auto i : qr.colsPermutation().indices()) {
                  new_mat.col(i) = matrix.col(i);
              }
              return new_mat;
          }
          return new_mat;
      }
  }

  MatrixXd Computations::ComputeRowSpace(MatrixXd matrix) {
      MatrixXd transp = matrix.transpose();
      Eigen::ColPivHouseholderQR<MatrixXd> qr(transp);
      MatrixXd new_mat;
      for (auto i : qr.colsPermutation().indices()) {
          new_mat.col(i) = transp.col(i);
      }
      return new_mat;
  }


// =============================================================================
//                    MetaProg Chapter III Solution 1
//                         Author : Xin Liu
// =============================================================================

#include <iostream>
#include <limits>
#include <type_traits>
#include <boost/mpl/apply.hpp>
#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/multiplies.hpp>

namespace mpl = boost::mpl;

////////////////////////////////////////////////////////////////////////////////
namespace dimension_analysis
{
    using mass                  = mpl::vector_c<int, 1, 0, 0, 0, 0, 0, 0>;
    using length                = mpl::vector_c<int, 0, 1, 0, 0, 0, 0, 0>;
    using time                  = mpl::vector_c<int, 0, 0, 1, 0, 0, 0, 0>;
    using charge                = mpl::vector_c<int, 0, 0, 0, 1, 0, 0, 0>;
    using temperature           = mpl::vector_c<int, 0, 0, 0, 0, 1, 0, 0>;
    using intensity             = mpl::vector_c<int, 0, 0, 0, 0, 0, 1, 0>;
    using amount_of_substance   = mpl::vector_c<int, 0, 0, 0, 0, 0, 0, 1>;

    using velocity              = mpl::vector_c<int, 0, 1, -1, 0, 0, 0, 0>; // l/t
    using acceleration          = mpl::vector_c<int, 0, 1, -2, 0, 0, 0, 0>; // l/(t^2)
    using momentum              = mpl::vector_c<int, 1, 1, -1, 0, 0, 0, 0>; // ml/t
    using force                 = mpl::vector_c<int, 1, 1, -2, 0, 0, 0, 0>; // ml/(t^2)

    using scalar                = mpl::vector_c<int, 0, 0, 0, 0, 0, 0, 0>;

    template<typename T, typename Dimensions>
    class quantity
    {
    public:
        explicit quantity(T x)
                : m_value(x)
        { }

        template <typename OtherDimensions>
        quantity(quantity<T, OtherDimensions> const& q)
                : m_value(q.value())
        {
            static_assert(mpl::equal<Dimensions, OtherDimensions>::type::value, "OtherDimensions should be same as Dimensions.");
        }

    public:
        T value() const
        {
            return m_value;
        }

    private:
        T m_value;
    };

    template <typename T, typename D>
    quantity<T, D>
    operator+ (const quantity<T, D>& lhs, const quantity<T, D>& rhs)
    {
        //static_assert(mpl::equal<D1, D2>(), "D1 and D2 dimensions should be same.");
        return quantity<T, D>(lhs.value() + rhs.value());
    }

    template <typename T, typename D>
    quantity<T, D>
    operator- (const quantity<T, D>& lhs, const quantity<T, D>& rhs)
    {
        //static_assert(mpl::equal<D1, D2>(), "D1 and D2 dimensions should be same.");
        return quantity<T, D>(lhs.value() - rhs.value());
    }

    using mpl::placeholders::_;

    template <typename D1, typename D2>
    struct multiply_dimensions
            : mpl::transform<D1, D2, mpl::plus<_, _> >
    { };

    template <typename T, typename D1, typename D2>
    quantity<T, typename multiply_dimensions<D1, D2>::type>
    operator* (const quantity<T, D1>& lhs, const quantity<T, D2>& rhs)
    {
        return quantity<T, typename multiply_dimensions<D1, D2>::type>(lhs.value() * rhs.value());
    }

    template <typename D1, typename D2>
    struct divide_dimensions
            : mpl::transform<D1, D2, mpl::minus<_, _>>
    { };

    template <typename T, typename D1, typename D2>
    quantity<T, typename divide_dimensions<D1, D2>::type>
    operator/ (const quantity<T, D1>& lhs, const quantity<T, D2>& rhs)
    {
        return quantity<T, typename divide_dimensions<D1, D2>::type>(lhs.value() / rhs.value());
    }
} // namespace dimension_analysis

int main()
{
    using namespace dimension_analysis;

    quantity<float, length> l{ 1.0f };
    quantity<float, mass> m{ 2.0f };

    // expected compile error
    // m = l;

    quantity<float, length> len1{ 1.0f };
    quantity<float, length> len2{ 2.0f };

    len1 = len1 + len2;

    // expected compile error
    //len1 = len2 + quantity<float, mass>{ 3.7f };

    quantity<float, mass> m1{ 5.0f };
    quantity<float, mass> m3{ 10.0f };
    quantity<float, acceleration> a1{ 9.8f };

    quantity<float, force> f1 = m1 * a1;

    quantity<float, mass> m2 = f1 / a1;
    float rounding_error = std::abs((m2 - m1).value());
    std::cout << "error: " << rounding_error << "\n";

    f1 = f1 + (quantity<float, force>)(m3 * a1);
    float rounding_error2 = std::abs((m2 - m1).value());
    std::cout << "error2: " << rounding_error2 << "\n";
    bool res = rounding_error2 <= std::numeric_limits<float>::min();
    if(res) return 0;
    else return -1;
}


#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <map>
#include <utility>
#include <vector>
// this header only for ch8
#ifndef STATISTICS_HPP
#define STATISTICS_HPP
using namespace std;

double genZValue(double alpha) {
    boost::math::normal Ndistribution(0, 1);
    auto Z = boost::math::quantile(complement(Ndistribution, alpha / 2));
    return Z;
}

double genTValue(int degree, double upperTailArea) {
    boost::math::students_t Tdistribution(degree);
    auto T = boost::math::quantile(complement(Tdistribution, upperTailArea));
    return T;
}

double genMean(vector<double>& _dataSet) {
    double sum = 0;
    for (auto i : _dataSet)
        sum += i;
    return sum / _dataSet.size();
}

double genSampleStandardDeviation(vector<double>& _dataSet, double sampleMean, double size) {
    double sxx = 0;
    for (auto i : _dataSet)
        sxx += (i - sampleMean) * (i - sampleMean);
    return sqrt(1 / (size - 1) * sxx);
}

double errorRadius(vector<double>& _dataSet, int degree, int sampleSize, double upperTailArea) {
    double mean = genMean(_dataSet);
    double ssd = genSampleStandardDeviation(_dataSet, mean, sampleSize);
    return ssd / sqrt(sampleSize) * genTValue(degree, upperTailArea);
}

double errorRadius(vector<double>& _dataSet, double upperTailArea = 0.025) {
    /*
    * giving data set and the upper tail area of Student-T distribution
    * return the error radius
    * for too lazy to only input data set
    */
    int sampleSize = _dataSet.size();
    return errorRadius(_dataSet, sampleSize - 1, sampleSize, upperTailArea);
}

double errorRadius(double knownSigma, double alpha, int sampleSize) {
    /*
    * @knownSigma: the population sigma, which over sqrt(n)
    * giving alpha return the error radius for known Sigma of Z distribution
    * return the error radius
    */
    return knownSigma / sqrt(sampleSize) * genZValue(alpha);
}

pair<double, double> genConfidenceInterval(double theta, double errorRadius) {
    /*
     * @theta: the sample variable
     * @errorRadius: a radius of error, which might greater than 1,
     *               means the standardDeviation times (z or t) over sqrt(n)
     */
    return make_pair(theta - errorRadius, theta + errorRadius);
}

template <typename T, typename S>
ostream& operator<<(ostream& os, const pair<T, S>& v) {
    os << "[" << v.first << ", " << v.second << "]";
    return os;
}

namespace needingSampleSize {
int p(double alpha, double p = 0.5, double marginError = 0.95) {
    /*
    * @p: the p bar of sample proportions, which might be iid Ber(P) (Bernoulli distribution)
    * @marginError: the margin error of confidence interval
    */
    double q = 1 - p, z = genZValue(alpha);
    auto sampleSize = p * q * z * z / marginError / marginError;
    return ceil(sampleSize);
}

int x_bar(double alpha, double knownTheta, double marginError) {
    /*
    * @knownTheta: the sigma which is the population standard deviation
    * @marginError: the margin error of confidence interval
    */
    double z = genZValue(alpha);
    auto sampleSize = z * z * knownTheta * knownTheta / marginError / marginError;
    return ceil(sampleSize);
}
}  // namespace needingSampleSize

template <class T>
string readSingleLineCSV(vector<T>& _dataSet, string fileName) {
    /* will return the file's title
    * read only single line csv file
    * @_dataSet: a container you want to storge at
    * @fileName: fileName
    */
    ifstream inFile(fileName, ios::in);
    if (inFile.fail()) {
        cerr << "Open file failed" << endl;
        return "";
    }
    string title;
    T rawdata;
    getline(inFile, title);
    while (inFile >> rawdata)
        _dataSet.push_back(rawdata);
    inFile.close();
    return title;
}

template <class T>
string readSingleLineCSV(map<T, int>& proportionDataSet, string fileName) {
    /*
    * only for reading proportion Data
    * will return the file's title
    * read only single line csv file
    * @proportionDataSet: a container you want to storge at
    * @fileName: fileName
    */
    ifstream inFile(fileName, ios::in);
    if (inFile.fail()) {
        cerr << "Open file failed" << endl;
        return "";
    }
    string title;
    T rawData;
    getline(inFile, title);
    while (getline(inFile, rawData)) {
        auto isInMap = proportionDataSet.find(rawData);
        if (isInMap != proportionDataSet.end())
            proportionDataSet.at(isInMap->first)++;
        else
            proportionDataSet.insert(pair<T, int>(rawData, 1));
    }
    inFile.close();
    return title;
}

#endif


#ifndef TEST_UNIT_MATH_PRIM_SCAL_PROB_HPP
#define TEST_UNIT_MATH_PRIM_SCAL_PROB_HPP

#include <boost/math/distributions.hpp>
#include <gtest/gtest.h>
#include <algorithm>
#include <vector>

/**
 * Uses a chi-squared test to assert that a vector of observed counts
 * is consistent with a vector of expected counts. Useful for testing RNGs.
 */
void assert_chi_squared(const std::vector<int>& counts,
                        const std::vector<double>& expected, double tolerance) {
  int bins = counts.size();
  EXPECT_EQ(bins, expected.size());

  double chi = 0;
  for (int i = 0; i < bins; ++i) {
    double discrepancy = expected[i] - counts[i];
    chi += discrepancy * discrepancy / expected[i];
  }
  boost::math::chi_squared dist(bins - 1);
  double chi_threshold = quantile(complement(dist, tolerance));

  EXPECT_TRUE(chi < chi_threshold);
}

/**
 * Like assert_matches_quantiles, but the bins are not necessarily
 * equiprobable. Assert that approximately proportions[i] of the
 * samples are in bin i, which has lower bound bin_boundaries[i-1] and
 * upper bound bin_boundaries[i], using a chi-squared goodness of fit
 * test. bin_boundaries is assumed sorted in increasing order.
 **/
void assert_matches_bins(const std::vector<double>& samples,
                         const std::vector<double>& bin_boundaries,
                         const std::vector<double>& proportions,
                         double tolerance) {
  ASSERT_GT(samples.size(), 0);
  int N = samples.size();
  std::vector<double> mysamples = samples;
  std::sort(mysamples.begin(), mysamples.end());

  ASSERT_GT(bin_boundaries.size(), 0);
  ASSERT_TRUE(bin_boundaries.size() == proportions.size());
  int K = bin_boundaries.size();
  std::vector<double> expected;
  for (int i = 0; i < K; i++) {
    ASSERT_TRUE(proportions[i] >= 0 && proportions[i] <= 1);
    expected.push_back(proportions[i] * N);
  }

  std::vector<int> counts(K);
  size_t current_index = 0;
  for (int i = 0; i < N; ++i) {
    while (mysamples[i] >= bin_boundaries[current_index]) {
      ++current_index;
      EXPECT_TRUE(current_index < bin_boundaries.size());
    }
    ++counts[current_index];
  }
  assert_chi_squared(counts, expected, tolerance);
}

/**
 * From a collection of samples and a list of quantiles, assumed
 * ordered, assert that the samples resemble draws from a distribution
 * with those quantiles, using a chi_squared goodness of fit
 * test. That is, assert that the samples are approximately evenly
 * distributed among the quantiles.size() equiprobable bins, who's
 * upper bounds are given in quantiles in increasing order.
 */
void assert_matches_quantiles(const std::vector<double>& samples,
                              const std::vector<double>& quantiles,
                              double tolerance) {
  int K = quantiles.size();
  std::vector<double> proportions;
  for (int i = 0; i < K; ++i)
    proportions.push_back(1.0 / K);

  assert_matches_bins(samples, quantiles, proportions, tolerance);
}
#endif