/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <vector>
#include <algorithm>
#include <Eigen/Dense>

#include <dpMM/global.hpp>
#include <dpMM/sampler.hpp>

using std::vector;
using std::cout;
using std::endl;
using std::min;
using std::max;
using boost::shared_ptr;

/* clustered data */
template <class T>
class ClGMMData
{
protected:
  spVectorXu z_; // labels
  boost::shared_ptr<Matrix<T,Dynamic,Dynamic> > x_; // data

  uint32_t K_; // number of classes
  uint32_t N_;
  uint32_t D_;

  Matrix<T,Dynamic,1> Ns_; // counts
  Matrix<T,Dynamic,Dynamic> means_; // means
  vector<Matrix<T,Dynamic,Dynamic> > Ss_; //scatter matrices

public:
  ClGMMData(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& x,
      const spVectorXu& z, uint32_t K);
  virtual ~ClGMMData();

  virtual void init();

  /* after changing z_ outside - we can use update to get new statistics */
  virtual void update(uint32_t K);

  virtual void sampleGMMpdf(const Matrix<T,Dynamic,1>& pi,
      const vector<Matrix<T,Dynamic,Dynamic> >& Sigmas,
      const Matrix<T,Dynamic,1>& logNormalizers, Sampler<T> *sampler)
  {cout<<"NOT implemented"<<endl;assert(false);};

//  virtual const spVectorXu& z() const {return z_;};
  virtual VectorXu& z() {return *z_;};
  virtual uint32_t z(uint32_t i) const {return (*z_)(i);};
  virtual const spVectorXu& labels() const {return z_;};
  virtual const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& x() const {return x_;};
  virtual const Matrix<T,Dynamic,Dynamic>& xMat() const {return (*x_);};
  virtual uint32_t N() const {return N_;};
  virtual uint32_t K() const {return K_;};
  virtual uint32_t D() const {return D_;};

  virtual const Matrix<T,Dynamic,1>& counts() const {return Ns_;};
  virtual T count(uint32_t k) const {return Ns_(k);};

  virtual const Matrix<T,Dynamic,Dynamic>& means() const {return means_;};
  virtual Matrix<T,Dynamic,1> mean(uint32_t k) const {return means_.col(k);};

  virtual const vector<Matrix<T,Dynamic,Dynamic> >& scatters() const
  {return Ss_;};
  virtual const Matrix<T,Dynamic,Dynamic>& S(uint32_t k) const
  {return Ss_[k];};
};

typedef ClGMMData<float> ClGMMDataf;
typedef ClGMMData<double> ClGMMDatad;

template<typename T>
struct Spherical
{
  static T dist(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b)
  { return a.transpose()*b; };

  static T dissimilarity(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b)
  { return acos(min(static_cast<T>(1.0),max(static_cast<T>(-1.0),(a.transpose()*b)(0)))); };

  static bool closer(const T a, const T b)
  { return a > b; };
};

template<typename T>
struct Euclidean
{
  static T dist(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b)
  { return (a-b).squaredNorm(); };

  static T dissimilarity(const Matrix<T,Dynamic,1>& a, const Matrix<T,Dynamic,1>& b)
  { return (a-b).squaredNorm();};

  static bool closer(const T a, const T b) { return a<b; };
};

template<class T, class DS>
T silhouette(const ClGMMData<T>& cld)
{
  if(cld.K()<2) return -1.0;
//  assert(Ns_.sum() == N_);
  cout<<"N="<<cld.N()<<" K="<<cld.K()<<endl;
  cout<<"counts="<<cld.counts().transpose()<<endl;
  Matrix<T,Dynamic,1> sil(cld.N());
//#pragma omp parallel for
  for(uint32_t i=0; i<cld.N(); ++i)
  {
    Matrix<T,Dynamic,1> b = Matrix<T,Dynamic,1>::Zero(cld.K());
    for(uint32_t j=0; j<cld.N(); ++j)
      if(j != i)
      {
        b(cld.z(j)) += DS::dissimilarity(cld.x()->col(i),cld.x()->col(j));
      }
    for (uint32_t k=0; k<cld.K(); ++k)
      b(k) /= cld.count(k);
//    b *= Ns_.cast<T>().cwiseInverse(); // Assumes Ns are up to date!
    T a_i = b(cld.z(i)); // average dist to own cluster
    T b_i = cld.z(i)==0 ? b(1) : b(0); // avg dist do closest other cluster
    for(uint32_t k=0; k<cld.K(); ++k)
      if(k != cld.z(i) && b(k) == b(k) && b(k) < b_i && cld.count(k) > 0)
      {
        b_i = b(k);
      }
    if(a_i < b_i)
      sil(i) = 1.- a_i/b_i;
    else if(a_i > b_i)
      sil(i) = b_i/a_i - 1.;
    else
      sil(i) = 0.;
//    cout<<"@"<<i<<" b="<<b.transpose()<<" b_i="<<b_i<<" a_i="<<a_i<<endl;
  }
  cout<<" sil.sum="<<sil.sum()<<endl;
  return sil.sum()/static_cast<T>(cld.N());
};

// -------------------------- impl --------------------------------------------
template<class T>
ClGMMData<T>::ClGMMData(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& x,
    const spVectorXu& z, uint32_t K)
 : z_(z), x_(x), K_(K>0?K:z->maxCoeff()+1), N_(x->cols()), D_(x->rows()),
   Ss_(K_,Matrix<T,Dynamic,Dynamic>::Zero(D_,D_))
{
  cout<<"D="<<D_<<" N="<<N_<<" K="<<K_<<endl;
};

template<class T>
ClGMMData<T>::~ClGMMData()
{};

template<typename T>
void ClGMMData<T>::init()
{};

template<class T>
void ClGMMData<T>::update(uint32_t K)
{
  K_ = K;
  Ns_.setZero(K_);
  means_.setZero(D_,K_);

  for (uint32_t i=0; i<N_; ++i)
//  {
//    cout<<"N="<<N_<<" z_i="<<(*z_)(i)<<" i="<<i<<" K="<<K_<<endl;
    Ns_((*z_)(i))++;
//  }

  for(uint32_t i=0; i<N_; ++i)
    means_.col((*z_)(i)) += x_->col(i);
  for(uint32_t k=0; k<K_; ++k)
  {
    means_.col(k) /= Ns_(k);
    Ss_[k].setZero(D_,D_);
  }

//  for(uint32_t i=0; i<N_; ++i)
//    Ss_[(*z_)(i)] += (x_->col(i) - means_.col((*z_)(i)))*
//      (x_->col(i) - means_.col((*z_)(i))).transpose();
}


// Copyright 2018 Dr. Matthias Hölzl
// Licensed under the MIT License
//

#include <limits>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/io.hpp>
#include <boost/units/systems/cgs.hpp>
#include <boost/units/systems/cgs/io.hpp>
#include <boost/units/operators.hpp>
#include "gmock/gmock.h"
#include "unit-matcher.h"

using namespace boost::units;
using namespace boost::units::si;
using boost::units::cgs::centimeters;

using ::testing::Not;
using ::testing::Eq;
using ::testing::UnitsEq;
using ::testing::UnitsEqUlp;

TEST(LengthTest, MetersOnly)
{
    EXPECT_THAT(1.0 * meter, UnitsEq(1.0 * meter));
}

TEST(LengthTest, MetersAndCentimeters)
{
    auto one_meter{ 1.0 * meter };
    auto one_hundred_centimeters{ 100.0 * centimeters };
    EXPECT_THAT(one_meter, UnitsEq(static_cast<decltype(one_meter)>(one_hundred_centimeters)));
}

TEST(LengthTest, ApproximateMeters)
{
    EXPECT_THAT(1.0 * meter, UnitsEq((1.0 + std::numeric_limits<double>::epsilon()) * meter));
    EXPECT_THAT(1.0 * meter, Not(UnitsEq(1.01 * meter)));
    EXPECT_THAT(1.0 * meter, Not(UnitsEq(1.1 * meter)));
}

TEST(LengthTestUlp, MetersOnly)
{
    EXPECT_THAT(1.0 * meter, UnitsEqUlp(1.0 * meter));
}

TEST(LengthTestUlp, MetersAndCentimeters)
{
    auto one_meter{ 1.0 * meter };
    auto one_hundred_centimeters{ 100.0 * centimeters };
    EXPECT_THAT(one_meter, UnitsEqUlp(static_cast<decltype(one_meter)>(one_hundred_centimeters)));
}

TEST(LengthTestUlp, ApproximateMeters)
{
    EXPECT_THAT(1.0 * meter, UnitsEqUlp((1.0 + std::numeric_limits<double>::epsilon()) * meter));
    EXPECT_THAT(1.0 * meter, Not(UnitsEqUlp(1.01 * meter)));
    EXPECT_THAT(1.0 * meter, Not(UnitsEqUlp(1.1 * meter)));
}


#include <boost/multiprecision/cpp_int.hpp>

using int_type = boost::multiprecision::cpp_int;

#include <iostream>
#include <vector>

#include "structured.h"
#include "config.h"
#include "gadget.h"
#include "optimizer.h"
#include "orbit.h"
#include "rational.h"

int main() {
  std::cout << "Analyzing orbits of the symmetry group..." << std::endl;
  OrbitInfo orbitInfo;

  std::cout << "Finding all structured sets" << std::endl;
  std::vector<bool> structuredSets = getAllStructuredSets();
  long long i = 0;
  for (long long ind = 0; ind < n_nodes; ++ind)
    i += structuredSets[ind];
  std::cout << "Found " << i << " nodes to consider" << std::endl;

  EdgeOrbitInfo edgeOrbitInfo(orbitInfo, structuredSets);
  Optimizer<int_type> optimizer;
  Evaluator<int_type> evaluator(orbitInfo, edgeOrbitInfo);

  std::cout << "Optimizing gadget..." << std::endl;

  auto gadget = optimizer.gadgetSearch(orbitInfo, edgeOrbitInfo);

  std::cout << "Finished optimizing gadget" << std::endl;

  std::cout << gadget;

  auto randomCost = evaluator.relaxedRandomCost(gadget);

  int_type totalWeight = 0;
  for (auto edgeOrbit : edgeOrbitInfo.edgeOrbits)
    totalWeight += edgeOrbit.size() * gadget.getWeight(edgeOrbit[0]);
  auto dictCost = Rational<int_type>(totalWeight, dimension);

  auto inapproximabilityFactor = randomCost / dictCost;

  std::cout << "The found gadget has inapproximability factor "
            << inapproximabilityFactor.a << "/" << inapproximabilityFactor.b
            << std::endl;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_MAIN

#include <limits>
#include <string>
#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>

#include <Eigen/Core>

#include "Tudat/Astrodynamics/ReferenceFrames/referenceFrameTransformations.h"
#include "Tudat/Mathematics/BasicMathematics/linearAlgebra.h"
#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Mathematics/BasicMathematics/rotationRepresentations.h"

namespace tudat
{
namespace unit_tests
{

using namespace tudat::basic_mathematics;

BOOST_AUTO_TEST_SUITE( test_rotation_partials )

Eigen::Matrix3d getTestRotationMatrix(
        const double perturbation )
{
    return ( Eigen::AngleAxisd( -( -1.2 + perturbation ), Eigen::Vector3d::UnitZ( ) ) *
             Eigen::AngleAxisd( -( 0.3 + perturbation ), Eigen::Vector3d::UnitX( ) ) *
             Eigen::AngleAxisd( -( -0.4 + perturbation ), Eigen::Vector3d::UnitZ( ) ) ).toRotationMatrix( );
}

Eigen::Matrix3d getTestRotationMatrix(
        const double psiPerturbation,
        const double thetaPerturbation,
        const double phiPerturbation )
{
    return ( Eigen::AngleAxisd( -( -1.2 + psiPerturbation ), Eigen::Vector3d::UnitZ( ) ) *
             Eigen::AngleAxisd( -( 0.3 + thetaPerturbation ), Eigen::Vector3d::UnitX( ) ) *
             Eigen::AngleAxisd( -( -0.4 + phiPerturbation ), Eigen::Vector3d::UnitZ( ) ) ).toRotationMatrix( );
}

Eigen::Quaterniond perturbQuaternionEntry(
        const Eigen::Quaterniond originalQuaternion,
        const int index,
        const double perturbation )
{
    double perturbedValue, newq0;
    Eigen::Quaterniond newQuaternion;
    if( index == 1 )
    {
        perturbedValue = originalQuaternion.x( ) + perturbation;
        newq0 = std::sqrt( 1.0 - perturbedValue * perturbedValue -
                           originalQuaternion.y( ) * originalQuaternion.y( ) -
                           originalQuaternion.z( ) * originalQuaternion.z( ) );
        newQuaternion = Eigen::Quaterniond( newq0, perturbedValue, originalQuaternion.y( ), originalQuaternion.z( ) );

    }
    else if( index == 2 )
    {
        perturbedValue = originalQuaternion.y( ) + perturbation;
        newq0 = std::sqrt( 1.0 - perturbedValue * perturbedValue -
                           originalQuaternion.x( ) * originalQuaternion.x( ) -
                           originalQuaternion.z( ) * originalQuaternion.z( ) );
        newQuaternion = Eigen::Quaterniond( newq0, originalQuaternion.x( ), perturbedValue, originalQuaternion.z( ) );
    }
    else if( index == 3 )
    {
        perturbedValue = originalQuaternion.z( ) + perturbation;
        newq0 = std::sqrt( 1.0 - perturbedValue * perturbedValue -
                           originalQuaternion.y( ) * originalQuaternion.y( ) -
                           originalQuaternion.x( ) * originalQuaternion.x( ) );
        newQuaternion = Eigen::Quaterniond( newq0, originalQuaternion.x( ), originalQuaternion.y( ), perturbedValue );

    }
    return newQuaternion;
}

Eigen::Matrix3d getTestRotationMatrix(
        const double perturbation,
        const int angleIndex )
{
    if( angleIndex == 0 )
    {
        return getTestRotationMatrix( perturbation, 0.0, 0.0 );
    }
    else if( angleIndex == 1 )
    {
        return getTestRotationMatrix( 0.0, perturbation, 0.0 );
    }
    else if( angleIndex == 2 )
    {
        return getTestRotationMatrix( 0.0, 0.0, perturbation );
    }
    else
    {
        return Eigen::Matrix3d::Zero( );
    }
}

BOOST_AUTO_TEST_CASE( testEulerAngles )
{
    Eigen::Quaterniond quaternion  = Eigen::Quaterniond( getTestRotationMatrix( 0.0 ) );
    Eigen::Vector3d eulerAngles = get313EulerAnglesFromQuaternion( quaternion );

    BOOST_CHECK_SMALL( std::fabs( eulerAngles( 0 ) + 1.2 ), 1.0E-15 );
    BOOST_CHECK_SMALL( std::fabs( eulerAngles( 1 ) - 0.3 ), 1.0E-15 );
    BOOST_CHECK_SMALL( std::fabs( eulerAngles( 2 ) + 0.4 ), 1.0E-15 );

    Eigen::Quaterniond recomputedQuaternion  = getQuaternionFrom313EulerAngles( eulerAngles );

    Eigen::Quaterniond manualQuaternion = Eigen::Quaterniond(
                Eigen::AngleAxisd( -eulerAngles( 0 ), Eigen::Vector3d::UnitZ( ) ) *
                Eigen::AngleAxisd( -eulerAngles( 1 ), Eigen::Vector3d::UnitX( ) ) *
                Eigen::AngleAxisd( -eulerAngles( 2 ), Eigen::Vector3d::UnitZ( ) ) );
//    Eigen::Matrix3d rotationMatrix = manualQuaternion.toRotationMatrix( );

    Eigen::Vector3d newEulerAngles = get313EulerAnglesFromQuaternion( recomputedQuaternion );
//    std::cout<<eulerAngles.transpose( )<<std::endl;
//    std::cout<<newEulerAngles.transpose( )<<std::endl;

//    std::cout<<quaternion.w( )<<" "<<recomputedQuaternion.w( )<<" "<<manualQuaternion.w( )<<std::endl;
//    std::cout<<quaternion.x( )<<" "<<recomputedQuaternion.x( )<<" "<<manualQuaternion.x( )<<std::endl;
//    std::cout<<quaternion.y( )<<" "<<recomputedQuaternion.y( )<<" "<<manualQuaternion.y( )<<std::endl;
//    std::cout<<quaternion.z( )<<" "<<recomputedQuaternion.z( )<<" "<<manualQuaternion.z( )<<std::endl;

    BOOST_CHECK_SMALL( std::fabs( newEulerAngles.x( ) - eulerAngles.x( ) ), 1.0E-15 );
    BOOST_CHECK_SMALL( std::fabs( newEulerAngles.y( ) - eulerAngles.y( ) ), 1.0E-15 );
    BOOST_CHECK_SMALL( std::fabs( newEulerAngles.z( ) - eulerAngles.z( ) ), 1.0E-15 );

    BOOST_CHECK_SMALL( std::fabs( recomputedQuaternion.w( ) - quaternion.w( ) ), 1.0E-15 );
    BOOST_CHECK_SMALL( std::fabs( recomputedQuaternion.x( ) - quaternion.x( ) ), 1.0E-15 );
    BOOST_CHECK_SMALL( std::fabs( recomputedQuaternion.y( ) - quaternion.y( ) ), 1.0E-15 );
    BOOST_CHECK_SMALL( std::fabs( recomputedQuaternion.z( ) - quaternion.z( ) ), 1.0E-15 );

    BOOST_CHECK_SMALL( std::fabs( manualQuaternion.w( ) - recomputedQuaternion.w( ) ), 1.0E-15 );
    BOOST_CHECK_SMALL( std::fabs( manualQuaternion.x( ) - recomputedQuaternion.x( ) ), 1.0E-15 );
    BOOST_CHECK_SMALL( std::fabs( manualQuaternion.y( ) - recomputedQuaternion.y( ) ), 1.0E-15 );
    BOOST_CHECK_SMALL( std::fabs( manualQuaternion.z( ) - recomputedQuaternion.z( ) ), 1.0E-15 );
}

BOOST_AUTO_TEST_CASE( testEulerAngleRetrieval )
{
    {
        const double angleX = 2.1;
        const double angleZ = -0.3;
        const double angleY = 1.45;

        Eigen::Matrix3d rotationMatrix =
                ( Eigen::AngleAxisd( -angleX, Eigen::Vector3d::UnitX( ) ) *
                  Eigen::AngleAxisd( -angleZ, Eigen::Vector3d::UnitZ( ) ) *
                  Eigen::AngleAxisd( -angleY, Eigen::Vector3d::UnitY( ) ) ).toRotationMatrix( );

        Eigen::Vector3d eulerAngles = get132EulerAnglesFromRotationMatrix(
                    rotationMatrix );

        BOOST_CHECK_CLOSE_FRACTION( angleX, eulerAngles( 0 ), 1.0E-15 );
        BOOST_CHECK_CLOSE_FRACTION( angleZ, eulerAngles( 1 ), 1.0E-15 );
        BOOST_CHECK_CLOSE_FRACTION( angleY, eulerAngles( 2 ), 1.0E-15 );
    }

    {
        const double angleZ2 = -2.1;
        const double angleX = 0.3;
        const double angleZ1 = 1.45;

        Eigen::Matrix3d rotationMatrix =
                ( Eigen::AngleAxisd( -angleZ2, Eigen::Vector3d::UnitZ( ) ) *
                  Eigen::AngleAxisd( -angleX, Eigen::Vector3d::UnitX( ) ) *
                  Eigen::AngleAxisd( -angleZ1, Eigen::Vector3d::UnitZ( ) ) ).toRotationMatrix( );
        Eigen::Matrix3d manualMatrix =
                ( Eigen::Matrix3d( ) <<
                  std::cos( angleZ2 ), std::sin( angleZ2 ), 0.0,
                  -std::sin( angleZ2 ), std::cos( angleZ2 ), 0.0,
                  0.0, 0.0, 1.0 ).finished( ) *
                ( Eigen::Matrix3d( ) <<
                  1.0, 0.0, 0.0,
                  0.0, std::cos( angleX ), std::sin( angleX ),
                  0.0, -std::sin( angleX ), std::cos( angleX ) ).finished( ) *
                ( Eigen::Matrix3d( ) <<
                  std::cos( angleZ1 ), std::sin( angleZ1 ), 0.0,
                  -std::sin( angleZ1 ), std::cos( angleZ1 ), 0.0,
                  0.0, 0.0, 1.0 ).finished( );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( rotationMatrix, manualMatrix, 1.0E-15 );

        Eigen::Quaterniond rotationQuaternion =
                Eigen::Quaterniond( manualMatrix );
        double quaternionEntryW = 0.5 * std::sqrt( 1.0 + manualMatrix( 0, 0 ) + manualMatrix( 1, 1 ) + manualMatrix( 2, 2 ) );
        Eigen::Quaterniond manualQuaternion = Eigen::Quaterniond(
                    quaternionEntryW,
                    0.5 * ( manualMatrix( 1, 2 ) - manualMatrix( 2, 1 ) ) / ( 2.0 * quaternionEntryW ),
                    0.5 * ( manualMatrix( 2, 0 ) - manualMatrix( 0, 2 ) ) / ( 2.0 *quaternionEntryW ),
                    0.5 * ( manualMatrix( 0, 1 ) - manualMatrix( 1, 0 ) ) / ( 2.0 *quaternionEntryW ) );
        BOOST_CHECK_CLOSE_FRACTION( manualQuaternion.w( ), rotationQuaternion.w( ), 1.0E-15 );
        BOOST_CHECK_CLOSE_FRACTION( manualQuaternion.x( ), -rotationQuaternion.x( ), 1.0E-15 );
        BOOST_CHECK_CLOSE_FRACTION( manualQuaternion.y( ), -rotationQuaternion.y( ), 1.0E-15 );
        BOOST_CHECK_CLOSE_FRACTION( manualQuaternion.z( ), -rotationQuaternion.z( ), 1.0E-15 );

        Eigen::Vector3d eulerAngles = get313EulerAnglesFromRotationMatrix( rotationMatrix );

        BOOST_CHECK_CLOSE_FRACTION( angleZ2, eulerAngles( 0 ), 1.0E-14 );
        BOOST_CHECK_CLOSE_FRACTION( angleX, eulerAngles( 1 ), 1.0E-14 );
        BOOST_CHECK_CLOSE_FRACTION( angleZ1, eulerAngles( 2 ), 1.0E-14 );

        eulerAngles = get313EulerAnglesFromQuaternion( rotationQuaternion );

        BOOST_CHECK_CLOSE_FRACTION( angleZ2, eulerAngles( 0 ), 1.0E-14 );
        BOOST_CHECK_CLOSE_FRACTION( angleX, eulerAngles( 1 ), 1.0E-14 );
        BOOST_CHECK_CLOSE_FRACTION( angleZ1, eulerAngles( 2 ), 1.0E-14 );

    }
}

BOOST_AUTO_TEST_CASE( testEulerAnglePartials )
{
    Eigen::Quaterniond quaternion  = Eigen::Quaterniond( getTestRotationMatrix( 0.0 ) );

    Eigen::Matrix< double, 3, 4 > eulerAnglePartial;
    eulerAnglePartial = calculateEulerAngle313WrtQuaternionPartial( quaternion );

    Eigen::Matrix< double, 3, 4 > eulerAnglePartial2;
    eulerAnglePartial2 = calculateEulerAngle313WrtQuaternionPartialFromEulerAngles(
                get313EulerAnglesFromQuaternion( quaternion ) );

    Eigen::Vector4d quaternionInVectorForm = linear_algebra::convertQuaternionToVectorFormat( quaternion );

    Eigen::Matrix< double, 3, 4 > manualAnglePartial = Eigen::Matrix< double, 3, 4 >::Zero( );
    Eigen::Matrix< double, 3, 4 > manualQ0PartialContribution = Eigen::Matrix< double, 3, 4 >::Zero( );

    double perturbation = 1.0E-6;
    Eigen::Quaterniond upperturbedQuaternion, downperturbedQuaternion;
    for( int i = 1; i < 4; i++ )
    {
        upperturbedQuaternion = perturbQuaternionEntry( quaternion, i, perturbation );
        downperturbedQuaternion = perturbQuaternionEntry( quaternion, i, -perturbation );

        manualAnglePartial.block( 0, i, 3, 1 ) =
                ( get313EulerAnglesFromQuaternion( upperturbedQuaternion ) -
                  get313EulerAnglesFromQuaternion( downperturbedQuaternion ) ) / ( 2.0 * perturbation );
        manualQ0PartialContribution.block( 0, i, 3, 1 ) = eulerAnglePartial.block( 0, 0, 3, 1 ) *
                -quaternionInVectorForm( i ) / quaternionInVectorForm( 0 );
    }

    for( unsigned int i = 0; i < 3; i++ )
    {
        for( unsigned int j = 1; j < 4; j++ )
        {
            BOOST_CHECK_SMALL( std::fabs( eulerAnglePartial( i, j )  - eulerAnglePartial2( i, j ) ), 1.0E-14 );
            BOOST_CHECK_SMALL( std::fabs( eulerAnglePartial( i, j ) -
                                          ( manualAnglePartial( i, j ) - manualQ0PartialContribution( i, j ) ) ), 1.0E-9 );
        }
        BOOST_CHECK_SMALL( std::fabs( eulerAnglePartial( i, 0 )  -eulerAnglePartial2( i, 0 ) ), 1.0E-14 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


#include <boost/graph/betweenness_centrality.hpp>


// Petter Strandmark 2012.

#include <cstdio>
#include <iomanip>
#include <iostream>
#include <limits>
#include <stdexcept>

#include <Eigen/Dense>

#include <spii/spii.h>
#include <spii/solver.h>

namespace spii {

void LBFGSSolver::solve(const Function& function,
                        SolverResults* results) const
{
	double global_start_time = wall_time();

	// Dimension of problem.
	size_t n = function.get_number_of_scalars();

	if (n == 0) {
		results->exit_condition = SolverResults::FUNCTION_TOLERANCE;
		return;
	}

	// Current point, gradient and Hessian.
	double fval   = std::numeric_limits<double>::quiet_NaN();
	double fprev  = std::numeric_limits<double>::quiet_NaN();
	double normg0 = std::numeric_limits<double>::quiet_NaN();
	double normg  = std::numeric_limits<double>::quiet_NaN();
	double normdx = std::numeric_limits<double>::quiet_NaN();

	Eigen::VectorXd x, g;

	// Copy the user state to the current point.
	function.copy_user_to_global(&x);
	Eigen::VectorXd x2(n);

	// L-BFGS history.
	std::vector<Eigen::VectorXd>  s_data(this->lbfgs_history_size),
	                              y_data(this->lbfgs_history_size);
	std::vector<Eigen::VectorXd*> s(this->lbfgs_history_size),
	                              y(this->lbfgs_history_size);
	for (int h = 0; h < this->lbfgs_history_size; ++h) {
		s_data[h].resize(function.get_number_of_scalars());
		s_data[h].setZero();
		y_data[h].resize(function.get_number_of_scalars());
		y_data[h].setZero();
		s[h] = &s_data[h];
		y[h] = &y_data[h];
	}

	Eigen::VectorXd rho(this->lbfgs_history_size);
	rho.setZero();

	Eigen::VectorXd alpha(this->lbfgs_history_size);
	alpha.setZero();
	Eigen::VectorXd q(n);
	Eigen::VectorXd r(n);

	// Needed from the previous iteration.
	Eigen::VectorXd x_prev(n), s_tmp(n), y_tmp(n);

	CheckExitConditionsCache exit_condition_cache;

	//
	// START MAIN ITERATION
	//
	results->startup_time   += wall_time() - global_start_time;
	results->exit_condition = SolverResults::INTERNAL_ERROR;
	int iter = 0;
	bool last_iteration_successful = true;
	int number_of_line_search_failures = 0;
	int number_of_restarts = 0;
	while (true) {

		//
		// Evaluate function and derivatives.
		//
		double start_time = wall_time();
		// y[0] should contain the difference between the gradient
		// in this iteration and the gradient from the previous.
		// Therefore, update y before and after evaluating the
		// function.
		if (iter > 0) {
			y_tmp = -g;
		}
		fval = function.evaluate(x, &g);

		normg = std::max(g.maxCoeff(), -g.minCoeff());
		if (iter == 0) {
			normg0 = normg;
		}
		results->function_evaluation_time += wall_time() - start_time;

		//
		// Update history
		//
		start_time = wall_time();

		if (iter > 0 && last_iteration_successful) {
			s_tmp = x - x_prev;
			y_tmp += g;

			double sTy = s_tmp.dot(y_tmp);
			if (sTy > 1e-16) {
				// Shift all pointers one step back, discarding the oldest one.
				Eigen::VectorXd* sh = s[this->lbfgs_history_size - 1];
				Eigen::VectorXd* yh = y[this->lbfgs_history_size - 1];
				for (int h = this->lbfgs_history_size - 1; h >= 1; --h) {
					s[h]   = s[h - 1];
					y[h]   = y[h - 1];
					rho[h] = rho[h - 1];
				}
				// Reuse the storage of the discarded data for the new data.
				s[0] = sh;
				y[0] = yh;

				*y[0] = y_tmp;
				*s[0] = s_tmp;
				rho[0] = 1.0 / sTy;
			}
		}

		results->lbfgs_update_time += wall_time() - start_time;

		//
		// Test stopping criteriea
		//
		start_time = wall_time();
		if (iter > 1 && this->check_exit_conditions(fval, fprev, normg,
		                                            normg0, x.norm(), normdx,
		                                            last_iteration_successful,
		                                            &exit_condition_cache, results)) {
			break;
		}
		if (iter >= this->maximum_iterations) {
			results->exit_condition = SolverResults::NO_CONVERGENCE;
			break;
		}

		if (this->callback_function) {
			CallbackInformation information;
			information.objective_value = fval;
			information.x = &x;
			information.g = &g;

			if (!callback_function(information)) {
				results->exit_condition = SolverResults::USER_ABORT;
				break;
			}
		}

		results->stopping_criteria_time += wall_time() - start_time;

		//
		// Compute search direction via L-BGFS two-loop recursion.
		//
		start_time = wall_time();
		bool should_restart = false;

		double H0 = 1.0;
		if (iter > 0) {
			// If the gradient is identical two iterations in a row,
			// y will be the zero vector and H0 will be NaN. In this
			// case the line search will fail and L-BFGS will be restarted
			// with a steepest descent step.
			H0 = s[0]->dot(*y[0]) / y[0]->dot(*y[0]);

			// If isinf(H0) || isnan(H0)
			if (H0 ==  std::numeric_limits<double>::infinity() ||
			    H0 == -std::numeric_limits<double>::infinity() ||
			    H0 != H0) {
				should_restart = true;
			}
		}

		q = -g;

		for (int h = 0; h < this->lbfgs_history_size; ++h) {
			alpha[h] = rho[h] * s[h]->dot(q);
			q = q - alpha[h] * (*y[h]);
		}

		r = H0 * q;

		for (int h = this->lbfgs_history_size - 1; h >= 0; --h) {
			double beta = rho[h] * y[h]->dot(r);
			r = r + (*s[h]) * (alpha[h] - beta);
		}

		// If the function improves very little, the approximated Hessian
		// might be very bad. If this is the case, it is better to discard
		// the history once in a while. This allows the solver to correctly
		// solve some badly scaled problems.
		double restart_test = std::fabs(fval - fprev) /
		                      (std::fabs(fval) + std::fabs(fprev));
		if (iter > 0 && iter % 100 == 0 && restart_test
		                                   < this->lbfgs_restart_tolerance) {
			should_restart = true;
		}
		if (! last_iteration_successful) {
			should_restart = true;
		}

		if (should_restart) {
			if (this->log_function) {
				char str[1024];
				if (number_of_restarts <= 10) {
					std::sprintf(str, "Restarting: fval = %.3e, deltaf = %.3e, max|g_i| = %.3e, test = %.3e",
								 fval, std::fabs(fval - fprev), normg, restart_test);
					this->log_function(str);
				}
				if (number_of_restarts == 10) {
					this->log_function("NOTE: No more restarts will be reported.");
				}
				number_of_restarts++;
			}
			r = -g;
			for (int h = 0; h < this->lbfgs_history_size; ++h) {
				(*s[h]).setZero();
				(*y[h]).setZero();
			}
			rho.setZero();
			alpha.setZero();
			// H0 is not used, but its value will be printed.
			H0 = std::numeric_limits<double>::quiet_NaN();
		}

		results->lbfgs_update_time += wall_time() - start_time;

		//
		// Perform line search.
		//
		start_time = wall_time();
		double start_alpha = 1.0;
		// In the first iteration, start with a much smaller step
		// length. (heuristic used by e.g. minFunc)
		if (iter == 0) {
			double sumabsg = 0.0;
			for (size_t i = 0; i < n; ++i) {
				sumabsg += std::fabs(g[i]);
			}
			start_alpha = std::min(1.0, 1.0 / sumabsg);
		}
		double alpha_step = this->perform_linesearch(function, x, fval, g,
		                                             r, &x2, start_alpha);

		if (alpha_step <= 0) {
			if (this->log_function) {
				this->log_function("Line search failed.");
				char str[1024];
				std::sprintf(str, "%4d %+.3e %9.3e %.3e %.3e %.3e %.3e",
					iter, fval, std::fabs(fval - fprev), normg, alpha_step, H0, rho[0]);
				this->log_function(str);
			}
			if (! last_iteration_successful || number_of_line_search_failures++ > 10) {
				// This happens quite seldom. Every time it has happened, the function
				// was actually converged to a solution.
				results->exit_condition = SolverResults::GRADIENT_TOLERANCE;
				break;
			}

			last_iteration_successful = false;
		}
		else {
			// Record length of this step.
			normdx = alpha_step * r.norm();
			// Compute new point.
			x_prev = x;
			x = x + alpha_step * r;

			last_iteration_successful = true;
		}

		results->backtracking_time += wall_time() - start_time;

		//
		// Log the results of this iteration.
		//
		start_time = wall_time();

		int log_interval = 1;
		if (iter > 30) {
			log_interval = 10;
		}
		if (iter > 200) {
			log_interval = 100;
		}
		if (iter > 2000) {
			log_interval = 1000;
		}
		if (this->log_function && iter % log_interval == 0) {
			if (iter == 0) {
				this->log_function("Itr       f       deltaf   max|g_i|   alpha      H0       rho");
			}

			this->log_function(
				to_string(
					std::setw(4), iter, " ",
					std::setw(10), std::setprecision(3), std::scientific, std::showpos, fval, std::noshowpos, " ",
					std::setw(9),  std::setprecision(3), std::scientific, std::fabs(fval - fprev), " ",
					std::setw(9),  std::setprecision(3), std::setprecision(3), std::scientific, normg, " ",
					std::setw(9),  std::setprecision(3), std::scientific, alpha_step, " ",
					std::setw(9),  std::setprecision(3), std::scientific, H0, " ",
					std::setw(9),  std::setprecision(3), std::scientific, rho[0]
				)
			);
		}
		results->log_time += wall_time() - start_time;

		fprev = fval;
		iter++;
	}

	function.copy_global_to_user(x);
	results->total_time += wall_time() - global_start_time;

	if (this->log_function) {
		char str[1024];
		std::sprintf(str, " end %+.3e           %.3e", fval, normg);
		this->log_function(str);
	}
}

}  // namespace spii


#include "ouster/image_processing.h"

#include <Eigen/Eigen>
#include <algorithm>
#include <vector>

namespace ouster {
namespace viz {

namespace {
const double ae_percentile = 0.1;

/*
 * damping makes the autoexposure smooth and avoids flickering however, it
 * becomes slower to update.
 * 1.0 --> slowest, smoothest
 * 0.0 --> fastest, prone to flickering
 */
const double ae_damping = 0.90;

/*
 * for performance reasons, we may not want to update every frame but rather
 * every few frames
 */
const int ae_update_every = 3;

/* for performance reasons, only consider a subset of points */
const size_t ae_stride = 4;

/* if there are too few points, do nothing */
const size_t ae_min_nonzero_points = 100;

}  // namespace

void AutoExposure::operator()(Eigen::Ref<img_t<double>> image) {
    Eigen::Map<Eigen::ArrayXd> key_eigen(image.data(), image.size());

    if (counter == 0) {
        const size_t n = key_eigen.rows();
        std::vector<size_t> indices;
        indices.reserve(n);
        for (size_t i = 0; i < n; i += ae_stride) {
            // ignore 0 values, which are often due to dropped packets etc
            if (key_eigen[i] > 0) {
                indices.push_back(i);
            }
        }
        if (indices.size() < ae_min_nonzero_points) {
            // too few nonzero values, nothing to do
            return;
        }
        const size_t kth_extreme = indices.size() * ae_percentile;
        auto cmp = [&](const size_t a, const size_t b) {
            return key_eigen(a) < key_eigen(b);
        };
        std::nth_element(indices.begin(), indices.begin() + kth_extreme,
                         indices.end(), cmp);
        lo = key_eigen[*(indices.begin() + kth_extreme)];
        std::nth_element(indices.begin() + kth_extreme,
                         indices.end() - kth_extreme - 1, indices.end(), cmp);
        hi = key_eigen[*(indices.end() - kth_extreme - 1)];

        if (!initialized) {
            initialized = true;
            lo_state = lo;
            hi_state = hi;
        }
    }
    if (!initialized) {
        return;
    }
    // we use exponential smoothing
    lo_state = ae_damping * lo_state + (1.0 - ae_damping) * lo;
    hi_state = ae_damping * hi_state + (1.0 - ae_damping) * hi;
    counter = (counter + 1) % ae_update_every;
    key_eigen += ae_percentile - lo_state;
    key_eigen *= (1.0 - 2 * ae_percentile) / (hi_state - lo_state);

    // clamp
    key_eigen = key_eigen.max(0.0).min(1.0);
}

namespace {

/*
 * damping makes the correction smooth and avoids flickering.
 * however, it becomes slower to update.
 * 1.0 --> slowest, smoothest
 * 0.0 --> fastest, prone to flickering
 */
const double buc_damping = 0.92;

/*
 * for performance reasons, we may not want to update every frame
 * but rather every 8 or so frames.
 */
const int buc_update_every = 8;

}  // namespace

/*
 * computes the dark count, i.e. an additive offset in the brightness of the
 * image, to smoothe the difference between rows
 */
static std::vector<double> compute_dark_count(
    const Eigen::Ref<const img_t<double>>& image) {
    const size_t image_h = image.rows();
    const size_t image_w = image.cols();

    std::vector<double> tmp(image_w);
    std::vector<double> new_dark_count(image_h, 0);

    img_t<double> row_diffs =
        image.bottomRows(image_h - 1) - image.topRows(image_h - 1);

    // compute the median of differences between rows
    for (size_t i = 1; i < image_h; i++) {
        Eigen::Map<Eigen::Matrix<double, -1, 1>> tmp_map(tmp.data(), image_w);
        tmp_map = row_diffs.row(i - 1);
        std::nth_element(tmp.begin(), tmp.begin() + image_w / 2, tmp.end());
        new_dark_count[i] = new_dark_count[i - 1] + tmp[image_w / 2];
    }

    // remove gradients in the entire height of image by doing linear fit
    Eigen::Matrix<double, -1, 2> A(image_h, 2);
    for (size_t i = 0; i < image_h; i++) {
        A(i, 0) = 1;
        A(i, 1) = i;
    }

    Eigen::Vector2d x = A.fullPivLu().solve(
        Eigen::Map<Eigen::VectorXd>(new_dark_count.data(), image_h, 1));

    Eigen::Map<Eigen::ArrayXd>(new_dark_count.data(), image_h, 1) -=
        (A * x).array();

    // subtract minimum value
    double min_el =
        *std::min_element(new_dark_count.begin(), new_dark_count.end());
    Eigen::Map<Eigen::ArrayXd>(new_dark_count.data(), image_h, 1) -= min_el;
    return new_dark_count;
}

/*
 * updates the dark count by using exponential smoothing:
 * https://en.wikipedia.org/wiki/Exponential_smoothing
 * and then updates the dark_count member variable with the new result
 */
static void update_dark_count(const Eigen::Ref<const img_t<double>>& image,
                              std::vector<double>& dark_count) {
    const auto new_dark_count = compute_dark_count(image);
    const size_t image_h = image.rows();
    auto dark_count_map =
        Eigen::Map<Eigen::ArrayXd>(dark_count.data(), image_h);
    const auto new_dark_count_map =
        Eigen::Map<const Eigen::ArrayXd>(new_dark_count.data(), image_h);
    dark_count_map *= buc_damping;
    dark_count_map += new_dark_count_map * (1.0 - buc_damping);
}

void BeamUniformityCorrector::operator()(Eigen::Ref<img_t<double>> image) {
    const size_t image_h = image.rows();

    if (counter == 0) {
        if (dark_count.size() == 0) {
            dark_count = compute_dark_count(image);
        } else {
            update_dark_count(image, dark_count);
        }
    }
    counter = (counter + 1) % buc_update_every;

    // apply the dark count correction row by row
    for (size_t i = 0; i < image_h; i++) {
        // contains a view of the current row
        image.row(i) -= dark_count[i];
        image.row(i) = image.row(i).unaryExpr([](double x) {
            x = std::max(x, 0.0);
            x = std::min(x, (double)UINT32_MAX);
            return x;
        });
    }
}

}  // namespace viz
}  // namespace ouster


#ifndef INTEGRATION_HPP
#define INTEGRATION_HPP

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

typedef SparseVector<double> SparseVectorXd;
typedef SparseMatrix<double> SparseMatrixXd;

class PhysicalSystem {
    // parent class for physical systems that support implicit integration
public:
    // return number of *positional* degrees of freedom (not velocity)
    virtual int getDOFs() = 0;
    // write position and velocity into vectors
    virtual void getState(VectorXd &x, VectorXd &v) = 0;
    // read position and velocity from vectors
    virtual void setState(const VectorXd &x, const VectorXd &v) = 0;
    // write mass matrix
    virtual void getInertia(MatrixXd &M) = 0;
    // write inverse mass matrix
    virtual void getInverseInertia(MatrixXd &Mi) = 0;
    // write forces
    virtual void getForces(VectorXd &f) = 0;
    // write Jacobians
    virtual void getJacobians(MatrixXd &Jx, MatrixXd &Jv) = 0;
};

class ConstrainedSystem: public PhysicalSystem {
    // parent class for physical systems with constraints
public:
    // return number of constraints
    virtual int getConstraints() = 0;
    // write constraint values into vector
    virtual void getConstraintValues(VectorXd &c) = 0;
    // write constraint gradients into matrix
    virtual void getConstraintJacobian(MatrixXd &J) = 0;
};

// perform a forward Euler step of length dt
void forwardEulerStep(PhysicalSystem *system, double dt);

// perform a backward Euler step of length dt
void backwardEulerStep(PhysicalSystem *system, double dt);

// perform a constrained forward Euler step of length dt
void constrainedForwardEulerStep(ConstrainedSystem *system, double dt);

// perform a constrained backward Euler step of length dt
void constrainedBackwardEulerStep(ConstrainedSystem *system, double dt);

#endif


#pragma once
#include <dynamic_msgs/Trajectory.h>
#include <dynamic_msgs/Obstacle.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <sp_const.hpp>
#include <polynomial.hpp>
#include <utility>
#include <random>

namespace DynamicPlanning {
    class ObstacleBase {
    public:
        //DynamicObstacle has ellipsoidal shape
        ObstacleBase(double _radius, double _max_acc, double _downwash)
        : radius(_radius), max_acc(_max_acc), downwash(_downwash) {

        }

        dynamic_msgs::Obstacle getObstacle(double t){
            dynamic_msgs::Obstacle obstacle = getObstacle_impl(t);
            if(type == "static"){
                obstacle.type = ObstacleType::STATICOBSTACLE;
            }
            else{
                obstacle.type = ObstacleType::DYNAMICOBSTACLE;
            }
            obstacle.max_acc = max_acc;
            obstacle.radius = radius;
            obstacle.downwash = downwash;
            return obstacle;
        }

        virtual dynamic_msgs::Obstacle getObstacle_impl(double t) {
            dynamic_msgs::Obstacle obstacle;
            return obstacle;
        }

        double getRadius() const{
            return radius;
        }

        double getMaxAcc() const{
            return max_acc;
        }

        std::string getType() const{
            return type;
        }

        double getDownwash() const{
            return downwash;
        }

        void setRadius(double _radius) {
            radius = _radius;
        }

    protected:
        std::string type;
        double radius;
        double max_acc;
        double downwash;
    };

    class SpinObstacle : public ObstacleBase {
    public:
        SpinObstacle(geometry_msgs::PoseStamped _axis, geometry_msgs::Point _start, double _radius, double _speed,
                     double _max_acc, double _downwash)
                : axis(std::move(_axis)), start(_start), speed(_speed), ObstacleBase(_radius, _max_acc, _downwash) {
            type = "spin";
        }

        dynamic_msgs::Obstacle getObstacle_impl(double t) override {
            return getSpinObstacle(t);
        }

    private:
        geometry_msgs::PoseStamped axis;
        geometry_msgs::Point start;
        double speed;

        dynamic_msgs::Obstacle getSpinObstacle(double t) {
            dynamic_msgs::Obstacle spinObstacle;
            Eigen::Vector3d a(start.x - axis.pose.position.x, start.y - axis.pose.position.y,
                              start.z - axis.pose.position.z);
            Eigen::Vector3d n(axis.pose.orientation.x, axis.pose.orientation.y, axis.pose.orientation.z);
            n.normalize();
            Eigen::Vector3d r = a - a.dot(n) * n;
            double spin_radius = r.norm();
            double w = speed / spin_radius;
            double theta = w * t;

            if (max_acc < speed * speed / spin_radius){
                ROS_WARN("[Obstacle] max_acc is smaller than real acc!");
//                throw(PlanningReport::INVALIDARGS);
            }

            // position
            Eigen::Quaterniond q(cos(theta / 2), sin(theta / 2) * n.x(), sin(theta / 2) * n.y(),
                                 sin(theta / 2) * n.z());
            Eigen::Quaterniond p(0, a.x(), a.y(), a.z());
            p = q * p * q.inverse();
            spinObstacle.pose.position.x = axis.pose.position.x + p.x();
            spinObstacle.pose.position.y = axis.pose.position.y + p.y();
            spinObstacle.pose.position.z = axis.pose.position.z + p.z();

            // velocity
            theta = M_PI / 2;
            Eigen::Quaterniond q_vel(cos(theta / 2), sin(theta / 2) * n.x(), sin(theta / 2) * n.y(),
                                     sin(theta / 2) * n.z());
            p = q_vel * p * q_vel.inverse();
            spinObstacle.velocity.linear.x = w * p.x();
            spinObstacle.velocity.linear.y = w * p.y();
            spinObstacle.velocity.linear.z = w * p.z();

            return spinObstacle;
        }
    };

    class StraightObstacle : public ObstacleBase {
    public:
        StraightObstacle() : ObstacleBase(0, 0, 1) {
            type = "straight";
            speed = 0;
            flight_time = 0;
        }

        StraightObstacle(geometry_msgs::Point _start, geometry_msgs::Point _goal, double _radius, double _speed,
                         double _max_acc, double _downwash)
                : start(_start), goal(_goal), speed(_speed), ObstacleBase(_radius, _max_acc, _downwash) {
            type = "straight";

            Eigen::Vector3d delta(goal.x - start.x, goal.y - start.y, goal.z - start.z);
            Eigen::Vector3d n = delta.normalized();
            v = speed * n;
            double distance = delta.norm();
            flight_time = distance / speed;
        }

        dynamic_msgs::Obstacle getObstacle_impl(double t) override {
            return getStraightObstacle(t);
        }

    private:
        geometry_msgs::Point start, goal;
        double speed;
        Eigen::Vector3d v;
        double flight_time;

        dynamic_msgs::Obstacle getStraightObstacle(double t) {
            dynamic_msgs::Obstacle straightObstacle;

            // position
            if(t < flight_time) {
                straightObstacle.pose.position.x = start.x + v(0) * t;
                straightObstacle.pose.position.y = start.y + v(1) * t;
                straightObstacle.pose.position.z = start.z + v(2) * t;
                straightObstacle.velocity.linear.x = v(0);
                straightObstacle.velocity.linear.y = v(1);
                straightObstacle.velocity.linear.z = v(2);
            }
            else{
                straightObstacle.pose.position = goal;
                straightObstacle.velocity.linear.x = 0;
                straightObstacle.velocity.linear.y = 0;
                straightObstacle.velocity.linear.z = 0;
            }
            return straightObstacle;
        }
    };

    class PatrolObstacle : public ObstacleBase {
    public:
        PatrolObstacle(std::vector<geometry_msgs::Point> _waypoints, double _radius, double _speed, double _max_acc, double _downwash)
                : waypoints(std::move(_waypoints)), speed(_speed), ObstacleBase(_radius, _max_acc, _downwash) {
            type = "multisim_patrol";
            flight_time.resize(waypoints.size());
            straightObstacles.resize(waypoints.size());
            for (int i = 0; i < waypoints.size(); i++) {
                double distance;
                if (i == waypoints.size() - 1) {
                    Eigen::Vector3d delta(waypoints[0].x - waypoints[i].x, waypoints[0].y - waypoints[i].y,
                                          waypoints[0].z - waypoints[i].z);
                    distance = delta.norm();
                    flight_time[i] = distance / speed;
                    straightObstacles[i] = StraightObstacle(waypoints[i], waypoints[0],
                                                            radius, speed, max_acc, downwash);
                } else {
                    Eigen::Vector3d delta(waypoints[i + 1].x - waypoints[i].x, waypoints[i + 1].y - waypoints[i].y,
                                          waypoints[i + 1].z - waypoints[i].z);
                    distance = delta.norm();
                    flight_time[i] = distance / speed;
                    straightObstacles[i] = StraightObstacle(waypoints[i], waypoints[i + 1],
                                                            radius, speed, max_acc, downwash);
                }
            }
        }

        dynamic_msgs::Obstacle getObstacle_impl(double t) override {
            return getPatrolObstacle(t);
        }

    private:
        std::vector<geometry_msgs::Point> waypoints;
        std::vector<double> flight_time;
        std::vector<StraightObstacle> straightObstacles;
        double speed;

        dynamic_msgs::Obstacle getPatrolObstacle(double t) {
            dynamic_msgs::Obstacle patrolObstacle;

            double current_time = t;
            int current_idx = 0;
            while (current_time >= flight_time[current_idx]){
                current_time -= flight_time[current_idx];
                if(current_idx == waypoints.size() - 1){
                    current_idx = 0;
                }
                else{
                    current_idx++;
                }
            }

            patrolObstacle = straightObstacles[current_idx].getObstacle(current_time);
            return patrolObstacle;
        }
    };

    //TODO: cannot used in oracle prediction
    class ChasingObstacle : public ObstacleBase {
    public:
        ChasingObstacle() : ObstacleBase(0, 0, 1) {
            type = "chasing";
            t_last_called = 0;
            max_vel = 0;
            gamma_target = 0;
            gamma_obs = 0;
        }

        ChasingObstacle(const dynamic_msgs::Obstacle& _start_state, double _radius, double _max_vel, double _max_acc,
                        double _gamma_target, double _gamma_obs, double _downwash)
                : ObstacleBase(_radius, _max_acc, _downwash) {
            type = "chasing";
            current_state.pose.position = _start_state.pose.position;
            current_state.velocity.linear.x = 0;
            current_state.velocity.linear.y = 0;
            current_state.velocity.linear.z = 0;
            max_vel = _max_vel;
            gamma_target = _gamma_target;
            gamma_obs = _gamma_obs;
            goal_point = current_state.pose.position;
            t_last_called = 0;
        }

        void setGoalPoint(const geometry_msgs::Point& _goal_point) {
            goal_point = _goal_point;
        }

        void setObstacles(const std::vector<dynamic_msgs::Obstacle>& _obstacles){
            obstacles = _obstacles;
        }

        dynamic_msgs::Obstacle getObstacle_impl(double t) override {
            return getChasingObstacle(t);
        }

    private:
        dynamic_msgs::Obstacle current_state;
        double t_last_called, max_vel, gamma_target, gamma_obs;
        geometry_msgs::Point goal_point;
        std::vector<dynamic_msgs::Obstacle> obstacles;
        int chasing_target_id;

        dynamic_msgs::Obstacle getChasingObstacle(double t) {
            dynamic_msgs::Obstacle chasingObstacle;
            Eigen::Vector3d delta_goal(goal_point.x - current_state.pose.position.x,
                                       goal_point.y - current_state.pose.position.y,
                                       goal_point.z - current_state.pose.position.z);

            Eigen::Vector3d a = gamma_target * delta_goal;
            Eigen::Vector3d v(current_state.velocity.linear.x,
                              current_state.velocity.linear.y,
                              current_state.velocity.linear.z);
            double dt = t - t_last_called;

            for(const auto & obstacle : obstacles){
                Eigen::Vector3d delta_obstacle(obstacle.pose.position.x - current_state.pose.position.x,
                                               obstacle.pose.position.y - current_state.pose.position.y,
                                               obstacle.pose.position.z - current_state.pose.position.z);
                double dist_to_obs = delta_obstacle.norm();
                if(dist_to_obs < SP_EPSILON_FLOAT){
                    continue;
                }
                double Q_star = 2 * (radius + obstacle.radius); //TODO: parameterization
                if(dist_to_obs < Q_star) {
//                    a += gamma_obs * (1/Q_star - 1/dist_to_obs) / (1/(dist_to_obs * dist_to_obs)) * delta_obstacle.normalized();
                    a += gamma_obs * (1 - dist_to_obs/Q_star) * (1/(dist_to_obs * Q_star)) * (-delta_obstacle.normalized());
                }
            }

            if(a.norm() > (max_acc-0.01)){
                a = a / a.norm() * (max_acc-0.01);
            }
            v = v + a * dt;
            if(v.norm() > max_vel){ //TODO: parameterization
                v = v / v.norm() * max_vel;
            }

            //update current_state
            current_state.pose.position.x = current_state.pose.position.x + v(0) * dt;
            current_state.pose.position.y = current_state.pose.position.y + v(1) * dt;
            current_state.pose.position.z = current_state.pose.position.z + v(2) * dt;
            current_state.velocity.linear.x = v(0);
            current_state.velocity.linear.y = v(1);
            current_state.velocity.linear.z = v(2);

            chasingObstacle.pose.position = current_state.pose.position;
            chasingObstacle.velocity.linear = current_state.velocity.linear;
            chasingObstacle.radius = radius;

            t_last_called = t;
            return chasingObstacle;
        }
    };

    class GaussianObstacle : public ObstacleBase {
    public:
        GaussianObstacle() : ObstacleBase(0, 0, 1) {
            type = "gaussian";
            acc_history_horizon = 0;
        }

        GaussianObstacle(geometry_msgs::Point _start, double _radius,
                         geometry_msgs::Vector3 _initial_vel, double _max_vel,
                         double _stddev_acc, double _max_acc, double _acc_update_cycle,
                         double _downwash)
                : start(_start), initial_vel(_initial_vel), max_vel(_max_vel),
                  stddev_acc(_stddev_acc), max_acc(_max_acc), acc_update_cycle(_acc_update_cycle),
                  ObstacleBase(_radius, _max_acc, _downwash)
        {
            type = "gaussian";
            acc_history_horizon = 0;
            update_acc_history(10);
        }

        dynamic_msgs::Obstacle getObstacle_impl(double t) override {
            return getGaussianObstacle(t);
        }

        void set_acc_history(std::vector<Eigen::Vector3d> _acc_history){
            acc_history = std::move(_acc_history);
        }

    private:
        geometry_msgs::Point start;
        std::vector<Eigen::Vector3d> acc_history;
        geometry_msgs::Vector3 initial_vel;
        double max_vel;
        double stddev_acc, max_acc, acc_update_cycle;
        double acc_history_horizon;

        void update_acc_history(double desired_horizon){
            if(acc_history_horizon < desired_horizon){
                std::random_device rd;
                std::mt19937 generator(rd());
                std::normal_distribution<double> distribution(0.0, stddev_acc);
                int n = ceil((desired_horizon - acc_history_horizon) / acc_update_cycle);
                acc_history_horizon += n * acc_update_cycle;

                for(int i = 0; i < n; i++){
                    Eigen::Vector3d acc;
                    for(int j = 0; j < 3; j++){
                        acc(j) = distribution(generator);
                    }
                    if(acc.norm() > max_acc){
                       acc = acc.normalized() * max_acc;
                    }

                    acc_history.emplace_back(acc);
                }
            }
        }

        dynamic_msgs::Obstacle getGaussianObstacle(double t) {
            dynamic_msgs::Obstacle gaussianObstacle;
            gaussianObstacle.pose.position = start;
            gaussianObstacle.velocity.linear = initial_vel;

            // update acc history
            if(t >= acc_history_horizon){
                double horizon_update = 10.0;
                update_acc_history(acc_history_horizon + horizon_update);
                acc_history_horizon += horizon_update;
            }

            // backtrack to find current state
            int n = floor((t + SP_EPSILON_FLOAT) / acc_update_cycle);
            if(acc_history.size() < n + 1){
                ROS_ERROR("acc history size error");
            }
            Eigen::Vector3d v(initial_vel.x, initial_vel.y, initial_vel.z);
            Eigen::Vector3d v_next;
            double dt = acc_update_cycle;
            for(int i = 0; i < n + 1; i++){
                if(i == n){
                    dt = t - n * acc_update_cycle;
                }

                Eigen::Vector3d acc = acc_history[i];
                v_next = v + acc * dt;
                if(v_next.norm() > max_vel){
                    // if v is over max_vel then ignore acc
                    //TODO: better max vel
                    gaussianObstacle.pose.position.x += v(0) * dt;
                    gaussianObstacle.pose.position.y += v(1) * dt;
                    gaussianObstacle.pose.position.z += v(2) * dt;
                }
                else{
                    gaussianObstacle.pose.position.x += v(0) * dt + 0.5 * acc(0) * dt * dt;
                    gaussianObstacle.pose.position.y += v(1) * dt + 0.5 * acc(1) * dt * dt;
                    gaussianObstacle.pose.position.z += v(2) * dt + 0.5 * acc(2) * dt * dt;
                    gaussianObstacle.velocity.linear.x += acc(0) * dt;
                    gaussianObstacle.velocity.linear.y += acc(1) * dt;
                    gaussianObstacle.velocity.linear.z += acc(2) * dt;
                    v = v_next;
                }
            }

            return gaussianObstacle;
        }
    };

    class StaticObstacle : public ObstacleBase {
    public:
        StaticObstacle() : ObstacleBase(0, 0, 1) {
            type = "static";
        }

        StaticObstacle(geometry_msgs::Point _pose, geometry_msgs::Point _dimensions)
                : pose(_pose), dimensions(_dimensions), ObstacleBase(0, 0, 1)
        {
            type = "static";
        }

        dynamic_msgs::Obstacle getObstacle_impl(double t) override {
            return getStaticObstacle();
        }

    private:
        geometry_msgs::Point pose;
        geometry_msgs::Point dimensions;

        dynamic_msgs::Obstacle getStaticObstacle() {
            dynamic_msgs::Obstacle staticObstacle;
            staticObstacle.pose.position = pose;

            staticObstacle.dimensions.resize(3);
            staticObstacle.dimensions[0] = dimensions.x;
            staticObstacle.dimensions[1] = dimensions.y;
            staticObstacle.dimensions[2] = dimensions.z;

            staticObstacle.velocity.linear.x = 0;
            staticObstacle.velocity.linear.y = 0;
            staticObstacle.velocity.linear.z = 0;

            staticObstacle.goal_point = pose;
            staticObstacle.max_acc = 0;
            staticObstacle.downwash = 1;
            staticObstacle.id = -1;
            staticObstacle.radius = -1;

            return staticObstacle;
        }
    };

    class BernsteinObstacle : public ObstacleBase {
    public:
        BernsteinObstacle(std::vector<std::vector<octomap::point3d>> _control_points, std::vector<double> _time_segments,
                double _radius, double _max_acc, double _downwash)
                : control_points(std::move(_control_points)), time_segments(_time_segments), ObstacleBase(_radius, _max_acc, _downwash) {
            type = "bernstein";

            M = control_points.size();
            n = control_points[0].size() - 1;
        }

        dynamic_msgs::Obstacle getObstacle_impl(double t) override {
            return getBernsteinObstacle(t);
        }

        dynamic_msgs::State getObstacleState(double t) {
            dynamic_msgs::State state;
            state = getStateFromControlPoints(control_points, t, M, n, time_segments);
            return state;
        }

    private:
        std::vector<std::vector<octomap::point3d>> control_points;
        std::vector<double> time_segments;
        int M, n;

        dynamic_msgs::Obstacle getBernsteinObstacle(double t) {
            dynamic_msgs::Obstacle bernsteinObstacle;
            dynamic_msgs::State state;
            state = getStateFromControlPoints(control_points, t, M, n, time_segments);
            bernsteinObstacle.pose = state.pose;
            bernsteinObstacle.velocity = state.velocity;

            return bernsteinObstacle;
        }
    };
}


#include <iostream>
//
#include <polyvec/api.hpp>
#include <polyvec/curve-tracer/bezier_merging.hpp>
#include <polyvec/curve-tracer/curve_bezier.hpp>
#include <polyvec/core/log.hpp>
#include <polyvec/misc.hpp>
#include <polyvec/utils/num.hpp>
#include <polyvec/utils/string.hpp>
#include <polyvec/io/vtk_curve_writer.hpp>
#include <polyvec/geometry/winding_number.hpp>
//
#include <Eigen/Core>
#include <Eigen/Geometry>

NAMESPACE_BEGIN()
::polyvec::BezierCurve build_bezier(const Eigen::Matrix<double, 2, 4> ctrl) {
  ::polyvec::BezierCurve bz;
  bz.set_control_points(ctrl);
  return bz;
};
NAMESPACE_END()

NAMESPACE_BEGIN(polyvectest)
NAMESPACE_BEGIN(BezierMerging)

int merge_one_curve(int, char**) {
  using Mat24 = Eigen::Matrix<double, 2, 4>;
  using Mat22 = Eigen::Matrix<double, 2, 2>;

  std::vector<Mat22> line_begin;
  std::vector<Mat22> line_end;
  std::vector<Mat24> beziers;
  Mat24 merged_bezier;
  polyfit::BezierMerging::PotraceParameters potparams;
  bool doable;

  int dump_counter = 0;
  auto write_results = [&](const bool doable) {
    // Write the unmerged
    polyvec::VtkCurveWriter writer;

    if (line_begin.size()) {
      writer.add_polyline(line_begin[0]);
      writer.add_point(line_begin[0].col(0));
    }

    if (line_end.size()) {
      writer.add_polyline(line_end[0]);
      writer.add_point(line_end[0].col(0));
      writer.add_point(line_end[0].col(1));
    } else {
      writer.add_point(beziers.back().col(3));
    }

    for (const Mat24& bzc : beziers) {
      writer.add_polyline(build_bezier(bzc).get_tesselation2());
      writer.add_point(bzc.col(0));
      writer.add_point(bzc.col(1));
      writer.add_point(bzc.col(2));
    }

    writer.dump(polyfit::StringUtils::fmt("test_dump/merge_one_curve_%04d_%d.vtk",
                                          0, dump_counter));
    writer.clear();

    // Write the merged

    Eigen::MatrixXd todraw(2, 3);

    if (doable) {
      todraw.col(0) = potparams.get_yy().control_points.col(0);
      todraw.col(1) = potparams.get_oy();
      todraw.col(2) = potparams.get_yy().control_points.col(3);
      writer.add_polyline(todraw);
      //
      writer.add_point(potparams.get_yy().control_points.col(0));
      writer.add_point(potparams.get_yy().control_points.col(1));
      writer.add_point(potparams.get_yy().control_points.col(2));
      writer.add_point(potparams.get_yy().control_points.col(3));
      //
      writer.add_polyline(
          build_bezier(potparams.get_yy().control_points).get_tesselation2());
    }

    //
    writer.dump(polyfit::StringUtils::fmt("test_dump/merge_one_curve_%04d_%d.vtk",
                                          1, dump_counter));
    writer.clear();

    ++dump_counter;
  };

  auto attempt_merge = [&]() {
    polyfit::BezierMerging::merged_curve(line_begin, beziers, line_end, stdout,
                                         doable, merged_bezier);

    if (doable) {
      potparams = ::polyfit::BezierMerging::PointParameters({merged_bezier})
                      .as_potrace_params();
      const double tolerance = 1.0;
      const double alphamax = 0.9;
      double penalty = 0, potrace_penalty=0;
      bool is_acceptable = false;
      ::polyfit::BezierMerging::is_merge_acceptable(beziers, merged_bezier, tolerance,alphamax,
                                              potrace_penalty, is_acceptable);
      ::polyfit::BezierMerging::merge_penalty(beziers, merged_bezier, penalty);
      printf("Case %d | acceptable %d penalty_potrace %.8g penalty %.8g \n", dump_counter,
             is_acceptable, potrace_penalty, penalty);
    } else {
      printf("Case %d | cannot fit merged bezier \n", dump_counter);
    }

    write_results(doable);
  };

  //
  // First try a single bezier
  //
  line_begin.clear();
  line_end.clear();
  beziers.push_back(Mat24());
  beziers.back().col(0) << -1, 0;
  beziers.back().col(1) << -0.25, 0.75;
  beziers.back().col(2) << 0.7, 0.3;
  beziers.back().col(3) << 1, 0;
  attempt_merge();

  //
  // Now  single bezier with one line before failes
  //
  line_end.clear();
  //
  line_begin.clear();
  line_begin.push_back(Mat22());
  line_begin.front().col(0) << -2, -1;
  line_begin.front().col(1) << -1, 0;
  //
  beziers.clear();
  beziers.push_back(Mat24());
  beziers.back().col(0) << -1, 0;
  beziers.back().col(1) << -0.25, 0.75;
  beziers.back().col(2) << 0.7, 0.3;
  beziers.back().col(3) << 1, 0;
  attempt_merge();

  //
  //  Now  single bezier with one line before and one after
  //
  line_end.clear();
  line_end.push_back(Mat22());
  line_end.front().col(0) << 1, 0;
  line_end.front().col(1) << 1.5, -0.5;
  //
  line_begin.clear();
  line_begin.push_back(Mat22());
  line_begin.front().col(0) << -1.5, -0.5;
  line_begin.front().col(1) << -1, 0;
  //
  beziers.clear();
  beziers.push_back(Mat24());
  beziers.back().col(0) << -1, 0;
  beziers.back().col(1) << -0.25, 0.75;
  beziers.back().col(2) << 0.7, 0.3;
  beziers.back().col(3) << 1, 0;
  attempt_merge();

  //
  // Same as before just transform
  //
  line_end.clear();
  line_end.push_back(Mat22());
  line_end.front().col(0) << 1, 0;
  line_end.front().col(1) << 1.1, -0.1;
  //
  line_begin.clear();
  line_begin.push_back(Mat22());
  line_begin.front().col(0) << -1.25, -0.25;
  line_begin.front().col(1) << -1, 0;
  //
  beziers.clear();
  beziers.push_back(Mat24());
  beziers.back().col(0) << -1, 0;
  beziers.back().col(1) << -0.25, 0.75;
  beziers.back().col(2) << 0.7, 0.3;
  beziers.back().col(3) << 1, 0;
  {
    Eigen::Matrix2d A;
    A << -3, 1, 2, -4;
    Eigen::Vector2d b;
    b << -1, 4;
    line_end.front() = ((A * line_end.front()).colwise() + b).eval();
    line_begin.front() = ((A * line_begin.front()).colwise() + b).eval();

    for (Mat24& bz : beziers) {
      bz = ((A * bz).colwise() + b).eval();
    }
  }
  attempt_merge();

  //
  // Now add two beziers
  //
  line_end.clear();
  line_end.push_back(Mat22());
  line_end.front().col(0) << 1, 0;
  line_end.front().col(1) << 1.1, -0.1;
  //
  line_begin.clear();
  line_begin.push_back(Mat22());
  line_begin.front().col(0) << -1.25, -0.25;
  line_begin.front().col(1) << -1, 0;
  //
  beziers.clear();
  beziers.push_back(Mat24());
  beziers.back().col(0) << -1, 0;  //
  beziers.back().col(1) << -0.6, 0.4;
  beziers.back().col(2) << -0.2, 0.6;  //
  beziers.back().col(3) << 0, 0.6;     //
  beziers.push_back(Mat24());
  beziers.back().col(0) << 0, 0.6;    //
  beziers.back().col(1) << 0.3, 0.6;  //
  beziers.back().col(2) << 0.65, 0.35;
  beziers.back().col(3) << 1, 0;  //
  {
    Eigen::Matrix2d A;
    A << -3, 1, 2, -4;
    Eigen::Vector2d b;
    b << -1, 4;
    line_end.front() = ((A * line_end.front()).colwise() + b).eval();
    line_begin.front() = ((A * line_begin.front()).colwise() + b).eval();

    for (Mat24& bz : beziers) {
      bz = ((A * bz).colwise() + b).eval();
    }
  }
  attempt_merge();

  //
  // This was a problem with potrace incoming curves
  //
  line_end.clear();
  //
  line_begin.clear();
  //
  beziers.clear();
  beziers.push_back(Mat24());
  beziers.push_back(Mat24());
  beziers.push_back(Mat24());
  beziers.push_back(Mat24());
  beziers.push_back(Mat24());
 beziers[0] <<   21, 22.5238,  23.119,    23.5,      30,      30 , 30.8333 ,   33.5;
  beziers[1] << 23.5, 23.9952, 24.0476,      29,    33.5 ,36.9667  ,    37,      37;
 beziers[2] <<   29,  31.75, 34.225,   34.5,     37,     37, 36.775  , 36.5;
 beziers[3] << 34.5, 34.775, 35.225,   35.5 ,  36.5, 36.225 ,30.825 ,  24.5;
 beziers[4] <<  35.5,  35.881, 35.6429,    34.5,    24.5, 15.7381, 12.0476 ,      9;
  attempt_merge();

  //
  // This was a problem with potrace incoming curves
  //
  line_end.clear();
  //
  line_begin.clear();
  //
  beziers.clear();
  beziers.push_back(Mat24());
  beziers.push_back(Mat24());
  beziers[0] <<   29,  31.75, 34.225,   34.5,     37,     37, 36.775,   36.5;
  beziers[1] << 34.5, 34.775, 35.225,   35.5,   36.5, 36.225, 30.825,   24.5;
  attempt_merge();

  return EXIT_FAILURE;
}

NAMESPACE_END(BezierMerging)
NAMESPACE_END(polyvectest)