/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/type.hpp>

#include <type_traits>
using namespace boost::hana;
using namespace literals;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto odd = [](auto x) {
        return x % 2_c != 0_c;
    };

    BOOST_HANA_CONSTEXPR_ASSERT(all(list(1, 3), odd));
    BOOST_HANA_CONSTANT_ASSERT(!all(list(3_c, 4_c), odd));

    BOOST_HANA_CONSTANT_ASSERT(
        !all(list(type<void>, type<char&>), trait<std::is_void>)
    );
    BOOST_HANA_CONSTANT_ASSERT(
        all(list(type<int>, type<char>), trait<std::is_integral>)
    );
    //! [main]
}


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#ifndef BOOST_GIL_POINT_HPP
#define BOOST_GIL_POINT_HPP

#include <boost/gil/utilities.hpp>
#include <boost/gil/detail/std_common_type.hpp>

#include <boost/config.hpp>

#include <cstddef>
#include <type_traits>

namespace boost { namespace gil {

/// \addtogroup PointModel
///
/// Example:
/// \code
/// point<std::ptrdiff_t> p(3,2);
/// assert((p[0] == p.x) && (p[1] == p.y));
/// assert(axis_value<0>(p) == 3);
/// assert(axis_value<1>(p) == 2);
/// \endcode

/// \brief 2D point both axes of which have the same dimension type
/// \ingroup PointModel
/// Models: Point2DConcept
template <typename T>
class point
{
public:
    using value_type = T;

    template<std::size_t D>
    struct axis
    {
        using coord_t = value_type;
    };

    static constexpr std::size_t num_dimensions = 2;

    point() = default;
    point(T px, T py) : x(px), y(py) {}

    point operator<<(std::ptrdiff_t shift) const
    {
        return point(x << shift, y << shift);
    }

    point operator>>(std::ptrdiff_t shift) const
    {
        return point(x >> shift, y >> shift);
    }

    point& operator+=(point const& p)
    {
        x += p.x;
        y += p.y;
        return *this;
    }

    point& operator-=(point const& p)
    {
        x -= p.x;
        y -= p.y;
        return *this;
    }

    point& operator/=(double d)
    {
        if (d < 0 || 0 < d)
        {
            x = static_cast<T>(x / d);
            y = static_cast<T>(y / d);
        }
        return *this;
    }

    point& operator*=(double d)
    {
        x = static_cast<T>(x * d);
        y = static_cast<T>(y * d);
        return *this;
    }

    T const& operator[](std::size_t i) const
    {
        return this->*mem_array[i];
    }

    T& operator[](std::size_t i)
    {
        return this->*mem_array[i];
    }

    T x{0};
    T y{0};

private:
    // this static array of pointers to member variables makes operator[] safe
    // and doesn't seem to exhibit any performance penalty.
    static T point<T>::* const mem_array[num_dimensions];
};

/// Alias template for backward compatibility with Boost <=1.68.
template <typename T>
using point2 = point<T>;

/// Common type to represent 2D dimensions or in-memory size of image or view.
/// @todo TODO: rename to dims_t or dimensions_t for purpose clarity?
using point_t = point<std::ptrdiff_t>;

template <typename T>
T point<T>::* const point<T>::mem_array[point<T>::num_dimensions] =
{
    &point<T>::x,
    &point<T>::y
};

/// \ingroup PointModel
template <typename T>
BOOST_FORCEINLINE
bool operator==(const point<T>& p1, const point<T>& p2)
{
    return p1.x == p2.x && p1.y == p2.y;
}

/// \ingroup PointModel
template <typename T>
BOOST_FORCEINLINE
bool operator!=(const point<T>& p1, const point<T>& p2)
{
    return p1.x != p2.x || p1.y != p2.y;
}

/// \ingroup PointModel
template <typename T>
BOOST_FORCEINLINE
point<T> operator+(const point<T>& p1, const point<T>& p2)
{
    return { p1.x + p2.x, p1.y + p2.y };
}

/// \ingroup PointModel
template <typename T>
BOOST_FORCEINLINE
point<T> operator-(const point<T>& p)
{
    return { -p.x, -p.y };
}

/// \ingroup PointModel
template <typename T>
BOOST_FORCEINLINE
point<T> operator-(const point<T>& p1, const point<T>& p2)
{
    return { p1.x - p2.x, p1.y - p2.y };
}

/// \ingroup PointModel
template <typename T, typename D>
BOOST_FORCEINLINE
auto operator/(point<T> const& p, D d)
    -> typename std::enable_if
    <
        std::is_arithmetic<D>::value,
        point<typename detail::std_common_type<T, D>::type>
    >::type
{
    static_assert(std::is_arithmetic<D>::value, "denominator is not arithmetic type");
    using result_type = typename detail::std_common_type<T, D>::type;
    if (d < 0 || 0 < d)
    {
        double const x = static_cast<double>(p.x) / static_cast<double>(d);
        double const y = static_cast<double>(p.y) / static_cast<double>(d);
        return point<result_type>{
            static_cast<result_type>(iround(x)),
            static_cast<result_type>(iround(y))};
    }
    else
    {
        return point<result_type>{0, 0};
    }
}

/// \ingroup PointModel
template <typename T, typename M>
BOOST_FORCEINLINE
auto operator*(point<T> const& p, M m)
    -> typename std::enable_if
    <
        std::is_arithmetic<M>::value,
        point<typename detail::std_common_type<T, M>::type>
    >::type
{
    static_assert(std::is_arithmetic<M>::value, "multiplier is not arithmetic type");
    using result_type = typename detail::std_common_type<T, M>::type;
    return point<result_type>{p.x * m, p.y * m};
}

/// \ingroup PointModel
template <typename T, typename M>
BOOST_FORCEINLINE
auto operator*(M m, point<T> const& p)
    -> typename std::enable_if
    <
        std::is_arithmetic<M>::value,
        point<typename detail::std_common_type<T, M>::type>
    >::type
{
    static_assert(std::is_arithmetic<M>::value, "multiplier is not arithmetic type");
    using result_type = typename detail::std_common_type<T, M>::type;
    return point<result_type>{p.x * m, p.y * m};
}

/// \ingroup PointModel
template <std::size_t K, typename T>
BOOST_FORCEINLINE
T const& axis_value(point<T> const& p)
{
    static_assert(K < point<T>::num_dimensions, "axis index out of range");
    return p[K];
}

/// \ingroup PointModel
template <std::size_t K, typename T>
BOOST_FORCEINLINE
T& axis_value(point<T>& p)
{
    static_assert(K < point<T>::num_dimensions, "axis index out of range");
    return p[K];
}

/// \addtogroup PointAlgorithm
///
/// Example:
/// \code
/// assert(iround(point<double>(3.1, 3.9)) == point<std::ptrdiff_t>(3,4));
/// \endcode

/// \ingroup PointAlgorithm
template <typename T>
inline point<std::ptrdiff_t> iround(point<T> const& p)
{
    static_assert(std::is_integral<T>::value, "T is not integer");
    return { static_cast<std::ptrdiff_t>(p.x), static_cast<std::ptrdiff_t>(p.y) };
}

/// \ingroup PointAlgorithm
inline point<std::ptrdiff_t> iround(point<float> const& p)
{
    return { iround(p.x), iround(p.y) };
}

/// \ingroup PointAlgorithm
inline point<std::ptrdiff_t> iround(point<double> const& p)
{
    return { iround(p.x), iround(p.y) };
}

/// \ingroup PointAlgorithm
inline point<std::ptrdiff_t> ifloor(point<float> const& p)
{
    return { ifloor(p.x), ifloor(p.y) };
}

/// \ingroup PointAlgorithm
inline point<std::ptrdiff_t> ifloor(point<double> const& p)
{
    return { ifloor(p.x), ifloor(p.y) };
}

/// \ingroup PointAlgorithm
inline point<std::ptrdiff_t> iceil(point<float> const& p)
{
    return { iceil(p.x), iceil(p.y) };
}

/// \ingroup PointAlgorithm
inline point<std::ptrdiff_t> iceil(point<double> const& p)
{
    return { iceil(p.x), iceil(p.y) };
}

}} // namespace boost::gil

#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2010 Gael Guennebaud <g.gael@free.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

// import basic and product tests for deprectaed DynamicSparseMatrix
#define EIGEN_NO_DEPRECATED_WARNING

#include "main.h"
#include <Eigen/SparseExtra>
#include "unsupported/Eigen/src/SparseExtra/BlockDiagonalSparseQR.h"

template<typename Scalar, typename BlockSparseSolver>
void block_diagonal_sparse_qr(int nBlocks, int blockRows, int blockCols)
{
    typedef Eigen::Matrix<Scalar, Dynamic, Dynamic> DenseMatrix;

    std::cout << "block_diagonal_sparse_qr< " << eigen_test_nice_typename<BlockSparseSolver>() << "> " << nBlocks << " blocks of size " << blockRows << "x" << blockCols << "\n";

    // Generate random block diagonal matrix
    SparseMatrix<Scalar> mat;
    mat.resize(blockRows*nBlocks, blockCols*nBlocks);
    std::vector< Eigen::Triplet<Scalar> > triplets;
    triplets.reserve(blockRows*blockCols*nBlocks);
    for(int i=0; i<nBlocks; i++)
        for(int r=0; r<blockRows; r++)
            for(int c=0; c<blockCols; c++)
                triplets.push_back( Eigen::Triplet<Scalar>( i*blockRows+r, i*blockCols+c, Eigen::internal::random<Scalar>() ) );

    mat.setFromTriplets(triplets.begin(), triplets.end());

    // solve using BlockDiagonalSparseQR
    BlockDiagonalSparseQR<SparseMatrix<Scalar, ColMajor, Index>, BlockSparseSolver > solver;
    solver.setSparseBlockParams(blockRows, blockCols);
    solver.compute(mat);

    // check result
    SparseMatrix<Scalar> Q = solver.matrixQ();
    SparseMatrix<Scalar> R = solver.matrixR();

    // check A*P = Q*R
    DenseMatrix  Q_dot_R = Q.toDense()*R.toDense();
    DenseMatrix AP = mat.toDense();
    solver.colsPermutation().applyThisOnTheRight(AP);
    VERIFY_IS_APPROX( Q_dot_R, AP );

    // check Q*Qt = I
    DenseMatrix QQt = (Q*Q.transpose()).toDense();
    DenseMatrix I = Matrix<Scalar, Dynamic, Dynamic>::Identity(Q.rows(), Q.cols());
    VERIFY_IS_APPROX( QQt, I );

    // check R = upper triangular
    for(int i=0; i<R.rows(); i++)
        for(int j=0; j<R.cols() && j<i; j++)
            eigen_assert( fabs(R.coeff(i,j)) < 0.00001 );

}

void block_diagonal_sparse_qr_check_invalid_structure(int nBlocks, int blockRows, int blockCols) {

    // Generate random matrix with incorrect size
    SparseMatrix<double> mat;
    mat.resize(blockRows*nBlocks, blockCols*nBlocks+Eigen::internal::random<int>(1,10));

    // try to solve using BlockDiagonalSparseQR
    BlockDiagonalSparseQR<SparseMatrix<double, ColMajor, Index>, SparseQR<SparseMatrix<double>, COLAMDOrdering<int> > > solver;
    solver.setSparseBlockParams(blockRows, blockCols);
    VERIFY_RAISES_ASSERT( solver.compute(mat) );

    // again
    mat.resize(blockRows*nBlocks+Eigen::internal::random<int>(1,10), blockCols*nBlocks);
    VERIFY_RAISES_ASSERT( solver.compute(mat) );
}

void block_diagonal_sparse_qr_check_values_outside_blocks(int nBlocks, int blockRows, int blockCols) {

    // Generate full random matrix
    SparseMatrix<double> mat;
    mat.resize(blockRows*nBlocks, blockCols*nBlocks);
    std::vector< Eigen::Triplet<double> > triplets;
    for(int i=0; i<mat.rows(); i++) {
        for(int j=0; j<mat.cols(); j++) {
            triplets.push_back( Eigen::Triplet<double>( i, j, Eigen::internal::random<double>() ) );
        }
    }
    mat.setFromTriplets(triplets.begin(), triplets.end());

    // try to solve using BlockDiagonalSparseQR
    BlockDiagonalSparseQR<SparseMatrix<double, ColMajor, Index>, SparseQR<SparseMatrix<double>, COLAMDOrdering<int> > > solver;
    solver.setSparseBlockParams(blockRows, blockCols);
    VERIFY_RAISES_ASSERT( solver.compute(mat) );

}

void test_block_diagonal_sparse_qr()
{
  for(int i = 0; i < g_repeat; i++) {

    typedef double Scalar;

    // Fixed-size blocks
    CALL_SUBTEST((block_diagonal_sparse_qr<Scalar, ColPivHouseholderQR<Matrix<Scalar, 3, 2>>>(3, 3, 2)));

    // check BlockDiagonalSparseQR with sparse solver (for internal blocks)
    typedef SparseQR<SparseMatrix<Scalar>, COLAMDOrdering<int> > BlockSparseSolver;
    CALL_SUBTEST((block_diagonal_sparse_qr<Scalar, BlockSparseSolver>(2, 3, 5)));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockSparseSolver>( 1,1,1 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockSparseSolver>( 1,2,2 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockSparseSolver>( 1,3,5 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockSparseSolver>( 1,5,3 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockSparseSolver>( 2,1,1 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockSparseSolver>( 2,2,2 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockSparseSolver>( 2,5,3 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockSparseSolver>( 11,1,1 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockSparseSolver>( 11,2,2 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockSparseSolver>( 11,3,5 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockSparseSolver>( 11,5,3 ) ));

    // check BlockDiagonalSparseQR with dense solver (for internal blocks)
    typedef ColPivHouseholderQR<Matrix<Scalar,Dynamic,Dynamic> > BlockDenseSolver;
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockDenseSolver>( 1,1,1 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockDenseSolver>( 1,2,2 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockDenseSolver>( 1,3,5 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockDenseSolver>( 1,5,3 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockDenseSolver>( 2,1,1 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockDenseSolver>( 2,2,2 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockDenseSolver>( 2,3,5 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockDenseSolver>( 2,5,3 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockDenseSolver>( 11,1,1 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockDenseSolver>( 11,2,2 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockDenseSolver>( 11,3,5 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr<Scalar, BlockDenseSolver>( 11,5,3 ) ));

    // check BlockDiagonalSparseQR fails with an invalid block structure
    CALL_SUBTEST(( block_diagonal_sparse_qr_check_invalid_structure( 1,1,1 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr_check_invalid_structure( 1,2,2 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr_check_invalid_structure( 2,3,5 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr_check_invalid_structure( 11,5,3 ) ));

    // check BlockDiagonalSparseQR fails with data outside of the blocks
    CALL_SUBTEST(( block_diagonal_sparse_qr_check_values_outside_blocks( 3,1,1 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr_check_values_outside_blocks( 5,2,2 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr_check_values_outside_blocks( 7,3,5 ) ));
    CALL_SUBTEST(( block_diagonal_sparse_qr_check_values_outside_blocks( 11,5,3 ) ));

  }
}

//triplets.push_back( Eigen::Triplet<Scalar>(0,blockCols*nBlocks-1,4));


#include "NBC.h"
//#include <boost/algorithm/string.hpp>

int main(int argc, char* argv[])
{
	if (argc != 6){
		cout<< "Usuage: 5 arguments, the 1. executable followded by 2. training set, 3. training label, \
		4. test set, 5. tes label, 6. Number of Classe " << endl;
		exit(1);
	}
	int nC = atoi(argv[5]);
	cout<<"Number of Classe: " <<nC<<endl;

	NBC nbc;
	nbc.set_numClass(nC);
	//Start get Input data
	ifstream traindata(argv[1]);
	ifstream trainlable(argv[2]);
	ifstream testdata(argv[3]);
	ifstream testlable(argv[4]);
	if(trainlable.is_open() && traindata.is_open()){
		string line, lable;
		int l =-1;

		while (!trainlable.eof()){
			getline(traindata, line, '\n');
			getline(trainlable, lable, '\n');
			istringstream linestr(line);
			vector<int> sample;
			int v = 0;
			if (lable.size() ==0 )
				break;

			l = atoi(lable.c_str());
			//cout << "l: " <<l <<endl;
			nbc.ltrain.push_back(l);
			while(linestr >> v){
				sample.push_back(v);
			}
			nbc.trainset.push_back(sample);
		}
	}

	if(testlable.is_open() && testdata.is_open()){
		string line, lable;
		int l =-1;

		while (!testlable.eof()){
			getline(testdata, line, '\n');
			getline(testlable, lable, '\n');
			istringstream linestr(line);
			vector<int> sample;
			int v = 0;
			if (lable.size() ==0 )
				break;

			l = atoi(lable.c_str());
			//cout << "l: " <<l <<endl;
			nbc.ltest.push_back(l);
			while(linestr >> v){
				sample.push_back(v);
			}
			nbc.testset.push_back(sample);
		}
	}

	traindata.close();
	trainlable.close();
	testdata.close();
	testlable.close();

	// Start Training Naive Baysian Classifier

	nbc.Train(nbc.trainset, nbc.ltrain);

	//nbc.Print_Ptable();
	//nbc.check_Ptable();

	nbc.Test(nbc.testset);

	nbc.Print_confusionMatrix();

	cout<<"total correct: " << nbc.totalCorrect << " , with ratio: " <<double(nbc.totalCorrect)/nbc.totalTest << endl;
	nbc.confusionOutput(8, 9);
	cout<<"\n\n"<<endl;
	nbc.confusionOutput(7, 9);
	cout<<"\n\n"<<endl;
	nbc.confusionOutput(4, 9);
	cout<<"\n\n"<<endl;
	nbc.confusionOutput(5, 3);

/*
	int nlabel = nbc.pltest.size();

	cout << "# of lable: " <<nlabel<<endl;
	nbc.TP = 0;
	nbc.TN = 0;
	nbc.FP = 0;
	nbc.FN = 0;
    for (int n = 0; n<nlabel; n++)
	{
		cout<<"n ="<<n <<", nbc.ltest[n]: " << nbc.ltest[n] << ", nbc.pltest[n] " << nbc.pltest[n] << endl;
		if(nbc.ltest[n] ==1 && nbc.pltest[n] == 1)    nbc.TP+=1;
		if(nbc.ltest[n] ==-1 && nbc.pltest[n] == -1)  nbc.TN+=1;
		if(nbc.ltest[n] ==-1 && nbc.pltest[n] == 1)   nbc.FP+=1;
		if(nbc.ltest[n] ==1 && nbc.pltest[n] == -1)   nbc.FN+=1;
	}

    cout<<nbc.TP<<endl;
	cout<<nbc.FN<<endl;
	cout<<nbc.FP<<endl;
	cout<<nbc.TN<<endl;
*/
}


#pragma once
#include <cmath>
#include <Eigen/Eigen>
#include <random>

#include <nlohmann/json.hpp>

namespace yannq
{

class LocalSweeper
{
private:
	const uint32_t n_;
	const uint32_t nSweep_;

public:

	LocalSweeper(uint32_t n, uint32_t nSweep = 1) noexcept
		: n_(n), nSweep_(nSweep)
	{
	}

	nlohmann::json desc() const
	{
		nlohmann::json res;
		res["name"] = "Local Sweeper";
		res["num_sweep_per"] = nSweep_;
		return res;
	}

	template<class StateValue, class RandomEngine>
	uint32_t sweep(StateValue& sv, typename StateValue::RealScalar beta,
			RandomEngine& re) const noexcept
	{
		using RealScalar = typename StateValue::RealScalar;

		uint32_t acc = 0;
		std::uniform_real_distribution<RealScalar> urd(0.0, 1.0);
		std::uniform_int_distribution<int> uid_(0,n_-1);
		for(uint32_t sidx = 0; sidx < n_*nSweep_; sidx++)
		{
			int toFlip = uid_(re);
			RealScalar p = std::min(1.0,exp(beta*2.0*sv.logRatioRe(toFlip)));
			RealScalar u = urd(re);
			if(u < p)//accept
			{
				sv.flip(toFlip);
				++acc;
			}
		}
		return acc;
	}
};
} //NNQS


//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

//Distributed under the Boost Software License, Version 1.0. (See accompanying

//file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/qvm/quat_operations.hpp>

#include <boost/qvm/mat_operations.hpp>

#include "test_qvm_matrix.hpp"

#include "test_qvm_quaternion.hpp"

#include "test_qvm_vector.hpp"

#include "gold.hpp"

namespace

    {

    void

    test_x()

        {

        using namespace boost::qvm;

        test_qvm::vector<V1,3> axis; axis.a[0]=1;

        for( float r=0; r<6.28f; r+=0.5f )

            {

            test_qvm::quaternion<Q1> q1=rot_quat(axis,r);

            test_qvm::matrix<M1,3,3> x1=convert_to< test_qvm::matrix<M1,3,3> >(q1);

            test_qvm::rotation_x(x1.b,r);

            BOOST_QVM_TEST_CLOSE(x1.a,x1.b,0.000001f);

            test_qvm::quaternion<Q2> q2(42,1);

            set_rot(q2,axis,r);

            test_qvm::matrix<M2,3,3> x2=convert_to< test_qvm::matrix<M2,3,3> >(q2);

            test_qvm::rotation_x(x2.b,r);

            BOOST_QVM_TEST_CLOSE(x2.a,x2.b,0.000001f);

            test_qvm::quaternion<Q1> q3(42,1);

            test_qvm::quaternion<Q1> q4(42,1);

            rotate(q3,axis,r);

            q3 = q3*q1;

            BOOST_QVM_TEST_EQ(q3.a,q3.a);

            }

        }

    void

    test_y()

        {

        using namespace boost::qvm;

        test_qvm::vector<V1,3> axis; axis.a[1]=1;

        for( float r=0; r<6.28f; r+=0.5f )

            {

            test_qvm::quaternion<Q1> q1=rot_quat(axis,r);

            test_qvm::matrix<M1,3,3> x1=convert_to< test_qvm::matrix<M1,3,3> >(q1);

            test_qvm::rotation_y(x1.b,r);

            BOOST_QVM_TEST_CLOSE(x1.a,x1.b,0.000001f);

            test_qvm::quaternion<Q2> q2(42,1);

            set_rot(q2,axis,r);

            test_qvm::matrix<M2,3,3> x2=convert_to< test_qvm::matrix<M2,3,3> >(q2);

            test_qvm::rotation_y(x2.b,r);

            BOOST_QVM_TEST_CLOSE(x2.a,x2.b,0.000001f);

            test_qvm::quaternion<Q1> q3(42,1);

            test_qvm::quaternion<Q1> q4(42,1);

            rotate(q3,axis,r);

            q3 = q3*q1;

            BOOST_QVM_TEST_EQ(q3.a,q3.a);

            }

        }

    void

    test_z()

        {

        using namespace boost::qvm;

        test_qvm::vector<V1,3> axis; axis.a[2]=1;

        for( float r=0; r<6.28f; r+=0.5f )

            {

            test_qvm::quaternion<Q1> q1=rot_quat(axis,r);

            test_qvm::matrix<M1,3,3> x1=convert_to< test_qvm::matrix<M1,3,3> >(q1);

            test_qvm::rotation_z(x1.b,r);

            BOOST_QVM_TEST_CLOSE(x1.a,x1.b,0.000001f);

            test_qvm::quaternion<Q2> q2(42,1);

            set_rot(q2,axis,r);

            test_qvm::matrix<M2,3,3> x2=convert_to< test_qvm::matrix<M2,3,3> >(q2);

            test_qvm::rotation_z(x2.b,r);

            BOOST_QVM_TEST_CLOSE(x2.a,x2.b,0.000001f);

            test_qvm::quaternion<Q1> q3(42,1);

            test_qvm::quaternion<Q1> q4(42,1);

            rotate(q3,axis,r);

            q3 = q3*q1;

            BOOST_QVM_TEST_EQ(q3.a,q3.a);

            }

        }

    }

int

main()

    {

    test_x();

    test_y();

    test_z();

    return boost::report_errors();

    }


#ifndef DRC_CONTROL_EXPONENTIAL_FORM_HPP_
#define DRC_CONTROL_EXPONENTIAL_FORM_HPP_
#include "drake/util/Polynomial.h"
#include <Eigen/Dense>

class ExponentialForm {
  private:
    double m_a;
    double m_b;
    double m_c;

  public:
    ExponentialForm(double a, double b, double c) {
      m_a = a;
      m_b = b;
      m_c = c;
    }

    Polynomial<double> taylorExpand(int degree) const;

    ExponentialForm operator+(const double x) {
      ExponentialForm expform(this->m_a, this->m_b, this->m_c + x);
      return expform;
    }
    ExponentialForm operator-(const double x) {
      ExponentialForm expform(this->m_a, this->m_b, this->m_c - x);
      return expform;
    };

    double value(double t) const;
};

#endif


/*=============================================================================
    Spirit v1.6.0
    Copyright (c) 2002-2003 Joel de Guzman
    http://spirit.sourceforge.net/

    Permission to copy, use, modify, sell and distribute this software is
    granted provided this copyright notice appears in all copies. This
    software is provided "as is" without express or implied warranty, and
    with no claim as to its suitability for any purpose.
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  A Roman Numerals Parser (demonstrating the symbol table)
//
//  [ JDG 8/22/2002 ]
//
///////////////////////////////////////////////////////////////////////////////
#include <boost/spirit/core.hpp>
#include <boost/spirit/symbols/symbols.hpp>
#include <iostream>
#include <string>

///////////////////////////////////////////////////////////////////////////////
using namespace std;
using namespace boost::spirit;

///////////////////////////////////////////////////////////////////////////////
//
//  Parse roman hundreds (100..900) numerals using the symbol table.
//  Notice that the data associated with each slot is passed
//  to attached semantic actions.
//
///////////////////////////////////////////////////////////////////////////////
struct hundreds : symbols<unsigned>
{
    hundreds()
    {
        add
            ("C"    , 100)
            ("CC"   , 200)
            ("CCC"  , 300)
            ("CD"   , 400)
            ("D"    , 500)
            ("DC"   , 600)
            ("DCC"  , 700)
            ("DCCC" , 800)
            ("CM"   , 900)
        ;
    }

} hundreds_p;

///////////////////////////////////////////////////////////////////////////////
//
//  Parse roman tens (10..90) numerals using the symbol table.
//
///////////////////////////////////////////////////////////////////////////////
struct tens : symbols<unsigned>
{
    tens()
    {
        add
            ("X"    , 10)
            ("XX"   , 20)
            ("XXX"  , 30)
            ("XL"   , 40)
            ("L"    , 50)
            ("LX"   , 60)
            ("LXX"  , 70)
            ("LXXX" , 80)
            ("XC"   , 90)
        ;
    }

} tens_p;

///////////////////////////////////////////////////////////////////////////////
//
//  Parse roman ones (1..9) numerals using the symbol table.
//
///////////////////////////////////////////////////////////////////////////////
struct ones : symbols<unsigned>
{
    ones()
    {
        add
            ("I"    , 1)
            ("II"   , 2)
            ("III"  , 3)
            ("IV"   , 4)
            ("V"    , 5)
            ("VI"   , 6)
            ("VII"  , 7)
            ("VIII" , 8)
            ("IX"   , 9)
        ;
    }

} ones_p;

///////////////////////////////////////////////////////////////////////////////
//
//  Semantic actions
//
///////////////////////////////////////////////////////////////////////////////
struct add_1000
{
    add_1000(unsigned& r_) : r(r_) {}
    void operator()(char) const { r += 1000; }
    unsigned& r;
};

struct add_roman
{
    add_roman(unsigned& r_) : r(r_) {}
    void operator()(unsigned n) const { r += n; }
    unsigned& r;
};

///////////////////////////////////////////////////////////////////////////////
//
//  roman (numerals) grammar
//
///////////////////////////////////////////////////////////////////////////////
struct roman : public grammar<roman>
{
    template <typename ScannerT>
    struct definition
    {
        definition(roman const& self)
        {
            first
                =   +ch_p('M')  [add_1000(self.r)]
                ||  hundreds_p  [add_roman(self.r)]
                ||  tens_p      [add_roman(self.r)]
                ||  ones_p      [add_roman(self.r)];

            //  Note the use of the || operator. The expression
            //  a || b reads match a or b and in sequence. Try
            //  defining the roman numerals grammar in YACC or
            //  PCCTS. Spirit rules! :-)
        }

        rule<ScannerT> first;
        rule<ScannerT> const&
        start() const { return first; }
    };

    roman(unsigned& r_) : r(r_) {}
    unsigned& r;
};

///////////////////////////////////////////////////////////////////////////////
//
//  Main driver code
//
///////////////////////////////////////////////////////////////////////////////
int
main()
{
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "\t\tRoman Numerals Parser\n\n";
    cout << "/////////////////////////////////////////////////////////\n\n";
    cout << "Type a Roman Numeral ...or [q or Q] to quit\n\n";

    //  Start grammar definition

    string str;
    while (getline(cin, str))
    {
        if (str[0] == 'q' || str[0] == 'Q')
            break;

        unsigned n = 0;
        roman roman_p(n);
        if (parse(str.c_str(), roman_p).full)
        {
            cout << "parsing succeeded\n";
            cout << "result = " << n << "\n\n";
        }
        else
        {
            cout << "parsing failed\n\n";
        }
    }

    cout << "Bye... :-) \n\n";

    return 0;
}


#include "HelmertTransformation.h"
#include <boost/test/included/unit_test.hpp>

#include <cstdio>

using namespace boost::unit_test;

namespace
{
const double GR = M_PI / 180.0;
}

using namespace boost::unit_test;

test_suite* init_unit_test_suite(int argc, char* argv[])
{
  const char* name = "HelmertTransformationTest tester";
  unit_test_log.set_threshold_level(log_messages);
  framework::master_test_suite().p_name.value = name;
  BOOST_TEST_MESSAGE("");
  BOOST_TEST_MESSAGE(name);
  BOOST_TEST_MESSAGE(std::string(std::strlen(name), '='));
  return NULL;
}

BOOST_AUTO_TEST_CASE(toGeocentricTestZeroHeight)
{
  unit_test_log.set_threshold_level(log_messages);
  BOOST_TEST_MESSAGE("+ [Testing conversion from FMI sphere to WGS84]");

  const double R0 = 6371220.0;
  const double lat0 = 60;
  const double lon0 = 25;

  Fmi::ReferenceEllipsoid fmi(R0, 0);
  Fmi::HelmertTransformation conv, conv2;

  for (int i = 0; i <= 2; i++)
  {
    Fmi::HelmertTransformation::FmiSphereConvScalingType scaling_type =
        static_cast<Fmi::HelmertTransformation::FmiSphereConvScalingType>(i);

    conv.set_fmi_sphere_to_reference_ellipsoid_conv(
        R0, GR * lat0, GR * lon0, Fmi::ReferenceEllipsoid::wgs84, scaling_type);

    conv2.set_reference_ellipsoid_to_fmi_sphere_conv(
        R0, GR * lat0, GR * lon0, Fmi::ReferenceEllipsoid::wgs84, scaling_type);

    // printf("H: %16.5f %16.5f %16.5f %16.12f\n", conv.tx, conv.ty, conv.tz, conv.m);
    // printf("H: %16.5f %16.5f %16.5f %16.12f\n", conv2.tx, conv2.ty, conv2.tz, conv2.m);
    // printf("## %s\n", Fmi::get_fmi_sphere_towgs84_proj4_string(R0, lat0*GR, lon0*GR,
    //                                                           scaling_type).c_str());

    const int N = 1;
    double y_step = 0.01;
    double x_step = 2 * y_step;
    for (int ix = -N; ix <= N; ix++)
      for (int iy = -N; iy <= N; iy++)
      {
        double lat1 = GR * (lat0 + iy * y_step);
        double lon1 = GR * (lon0 + ix * x_step);
        const auto fmi_xyz = fmi.to_geocentric(lat1, lon1, 0.0);
        const auto wgs_xyz = conv(fmi_xyz);
        double lat2, lon2, h2;
        double lat3, lon3, h3;
        Fmi::ReferenceEllipsoid::wgs84.to_geodetic(wgs_xyz, &lat2, &lon2, &h2);

        BOOST_CHECK_CLOSE(lat2 / GR, lat1 / GR, 0.008);
        BOOST_CHECK_CLOSE(lon2 / GR, lon1 / GR, 0.008);
        BOOST_CHECK_SMALL(h2, 0.5);

        const auto fmi2_xyz = conv2(wgs_xyz);
        fmi.to_geodetic(fmi2_xyz, &lat3, &lon3, &h3);

        BOOST_CHECK_CLOSE(lat3 / GR, lat1 / GR, 1e-5);
        BOOST_CHECK_CLOSE(lon3 / GR, lon1 / GR, 1e-5);
        BOOST_CHECK_SMALL(h3, 0.001);

#if 0
            printf("## %15.8f %15.8f ", lat1/GR, lon1/GR);
            printf(" : %15.8f %15.8f %12.4f", (lat2-lat1)/GR, (lon2-lon1)/GR, h2);
            printf(" : %15.8f %15.8f %12.4f", (lat3-lat1)/GR, (lon3-lon1)/GR, h3);
            printf("\n");
            //printf(" : %15.3f %15.3f %15.3f ", fmi_xyz[0] , fmi_xyz[1], fmi_xyz[2]);
            //printf(" : %15.3f %15.3f %15.3f ", wgs_xyz[0] , wgs_xyz[1], wgs_xyz[2]);
            //printf(" : %15.3f %15.3f %15.3f ", fmi2_xyz[0] , fmi2_xyz[1], fmi2_xyz[2]);
            //printf("\n");
#endif
      }
  }
}


// This file is part of PoseEstimation.
// Copyright (c) 2021, Eijiro Shibusawa <phd_kimberlite@yahoo.co.jp>
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.
// 2. 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.

// 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 OWNER 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.

#ifndef THREE_POINT_UTIL_HPP_
#define THREE_POINT_UTIL_HPP_

#include "ThreePoint.hpp"

#include <Eigen/Geometry>

#include <random>

namespace ThreePoint
{
template <typename FloatType, typename RNG>
void getRandomRotation(RNG &rng, FloatType *R, FloatType *q = NULL)
{
	// construct random rotation
	std::uniform_real_distribution<FloatType> urd(0, static_cast<FloatType>(2 * M_PI));
	FloatType rx = urd(rng);
	FloatType ry = urd(rng);
	FloatType rz = urd(rng);
	Eigen::Quaternion<FloatType> mq;
	mq = Eigen::AngleAxis<FloatType>(rz, Eigen::Matrix<FloatType, 3, 1>::UnitZ()) *
		Eigen::AngleAxis<FloatType>(ry, Eigen::Matrix<FloatType, 3, 1>::UnitY()) *
		Eigen::AngleAxis<FloatType>(rx, Eigen::Matrix<FloatType, 3, 1>::UnitX());

	Eigen::Map<Eigen::Matrix<FloatType, 3, 3, Eigen::RowMajor> > mR(R);
	mR = mq.toRotationMatrix();

	if (q != NULL)
	{
		q[0] = mq.x();
		q[1] = mq.y();
		q[2] = mq.z();
		q[3] = mq.w();
	}
}

template <typename FloatType, typename RNG>
void getRandomTranslation(RNG &rng, FloatType *t)
{
	// cosntruct random translation
	std::uniform_real_distribution<FloatType> urd(static_cast<FloatType>(-0.5), static_cast<FloatType>(0.5));
	t[0] = 5 + urd(rng);
	t[1] = 5 + urd(rng);
	t[2] = 5 + urd(rng);
}

template <typename FloatType, typename RNG>
inline void getRandomScale(RNG &rng, FloatType &s)
{
	std::uniform_real_distribution<FloatType> urd(static_cast<FloatType>(-0.05), static_cast<FloatType>(0.05));
	s = 1 + urd(rng);
}

template <typename FloatType, typename RNG>
void getRandomPoint(int n, RNG &rng, FloatType *X)
{
	// construct random point on unit shpere
	std::uniform_real_distribution<FloatType> urd(static_cast<FloatType>(-0.5), static_cast<FloatType>(0.5));
	for (int i = 0; i < 3 * n; i++)
	{
		X[i] = urd(rng);
	}
	Eigen::Map<Eigen::Matrix<FloatType, 3, Eigen::Dynamic> > mX(X, 3, n);
	mX.colwise().normalize();
}

template <typename FloatType>
void getRandomCorrespondences(int n, FloatType *R, FloatType *t, FloatType &s, std::vector<FloatType> &pts1, std::vector<FloatType> &pts2)
{
	std::random_device rd;
	std::mt19937 rng(rd());

	// construct random rotation
	getRandomRotation(rng, R);

	// construct random translation
	getRandomTranslation(rng, t);

	// random scale
	getRandomScale(rng, s);

	// construct random point on unit shpere
	pts1.resize(3 * n);
	getRandomPoint(n, rng, &(pts1[0]));

	// transform
	pts2.resize(3 * n);
	ThreePoint<FloatType>::transformPoints(n, s, R, t, &(pts1[0]), &(pts2[0]));
}

}

#endif // THREE_POINT_UTIL_HPP_


//
//  InverseKinematics.cpp
//  Eigen_test
//
//  Created by Emil-Iliev on 5.02.20.
//  Copyright © 2020 Emil Iliev. All rights reserved.
//

#include "InverseKinematicsCommand.hpp"
#include <iostream>
#include <Eigen/Dense>
#include "AbstractSolver.hpp"

using namespace Eigen;

InverseKinematicsCommand::InverseKinematicsCommand(std::shared_ptr<AbstractSolver> solver):
    solver(solver) {
}

void InverseKinematicsCommand::execute() {
    VectorXf desired_position(6);
    float posX, posY, posZ;
    std::cout<< "Enter position (X, Y, Z): ";
    std::cin >> posX >> posY >> posZ;
    std::cout<< "Calculating ... \n";
    desired_position << posX, posY, posZ, 0.0, 0.0, 0.0;

    solver->setDesiredPosistion(desired_position);
    solver->calculateData();
}

std::string InverseKinematicsCommand::key() {
    return "inverse_kinematics_command";
}


/**
   \file lbfgs_method.hpp
   \brief large bfgs method, used to perform large scale optimization
   \author Junhua Gu
 */

#ifndef LBFGS_METHOD
#define LBFGS_METHOD
#define OPT_HEADER
#include <core/optimizer.hpp>
//#include <blitz/array.h>
#include <limits>
#include <cstdlib>
#include <core/opt_traits.hpp>
#include "../linmin/linmin.hpp"
#include <math/num_diff.hpp>
#include <cassert>
#include <cmath>
#include <ctime>
#include <vector>
#include <algorithm>
#include "lbfgs.h"
#include "lbfgs.cpp"
/*
 *
*/
#include <iostream>
using std::cerr;
using std::endl;

namespace opt_utilities
{

  template<typename rT,typename pT>
  lbfgsfloatval_t lbfgs_adapter(
				void *instance,
				const lbfgsfloatval_t *x,
				lbfgsfloatval_t *g,
				const int n,
				const lbfgsfloatval_t step
				)
  {
    pT px;
    resize(px,n);
    for(int i=0;i<n;++i)
      {
	set_element(px,i,x[i]);
      }

    lbfgsfloatval_t result=((func_obj<rT,pT>*)instance)->eval(px);
    pT grad(gradient(*static_cast<func_obj<rT,pT>*>(instance),px));
    for(int i=0;i<n;++i)
      {
	g[i]=get_element(grad,i);
      }
    return result;
  }

  template <typename rT,typename pT>
  class lbfgs_method
    :public opt_method<rT,pT>
  {
  public:
    typedef pT array1d_type;
    typedef rT T;
  private:
    func_obj<rT,pT>* p_fo;
    optimizer<rT,pT>* p_optimizer;

    //typedef blitz::Array<rT,2> array2d_type;

  private:
    array1d_type start_point;
    array1d_type end_point;

  private:
    rT threshold;
  private:
    rT func(const pT& x)
    {
      assert(p_fo!=0);
      return p_fo->eval(x);
    }

    const char* do_get_type_name()const
    {
      return "large scale bfgs";
    }
  public:
    lbfgs_method()
      :threshold(1e-4)
    {}

    virtual ~lbfgs_method()
    {
    };

    lbfgs_method(const lbfgs_method<rT,pT>& rhs)
      :p_fo(rhs.p_fo),p_optimizer(rhs.p_optimizer),
       start_point(rhs.start_point),
       end_point(rhs.end_point),
       threshold(rhs.threshold)
    {
    }

    lbfgs_method<rT,pT>& operator=(const lbfgs_method<rT,pT>& rhs)
    {
      threshold=rhs.threshold;
      p_fo=rhs.p_fo;
      p_optimizer=rhs.p_optimizer;
      opt_eq(start_point,rhs.start_point);
      opt_eq(end_point,rhs.end_point);
    }

    opt_method<rT,pT>* do_clone()const
    {
      return new lbfgs_method<rT,pT>(*this);
    }

    void do_set_start_point(const array1d_type& p)
    {
      start_point.resize(get_size(p));
      opt_eq(start_point,p);

    }

    array1d_type do_get_start_point()const
    {
      return start_point;
    }

    void do_set_precision(rT t)
    {
      threshold=t;
    }

    rT do_get_precision()const
    {
      return threshold;
    }

    void do_set_optimizer(optimizer<rT,pT>& o)
    {
      p_optimizer=&o;
      p_fo=p_optimizer->ptr_func_obj();
    }

    pT do_optimize()
    {
      lbfgs_parameter_t param;
      lbfgs_parameter_init(&param);
      param.ftol=threshold;
      std::vector<lbfgsfloatval_t> buffer(get_size(start_point));
      for(int i=0;i<buffer.size();++i)
	{
	  buffer[i]=get_element(start_point,i);
	}
      lbfgsfloatval_t fx;
      lbfgs(get_size(start_point),&buffer[0],&fx,
	    lbfgs_adapter<rT,pT>,0,p_fo,&param);
      for(int i=0;i<buffer.size();++i)
	{
	  set_element(start_point,i,buffer[i]);
	}
      return start_point;
    }
  };

}

#endif
//EOF


#include "observable.hpp"

#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>

#undef NDEBUG
#include <Eigen/Dense>
#include <array>
#include <cassert>
#include <cmath>
#include <fstream>
#include <iostream>

#include "state.hpp"
#include "utility.hpp"

void HermitianQuantumOperator::add_operator(const PauliOperator* mpt) {
    if (std::abs(mpt->get_coef().imag()) > 0) {
        std::cerr << "Error: HermitianQuantumOperator::add_operator(const "
                     "PauliOperator* mpt): PauliOperator must be Hermitian."
                  << std::endl;
        return;
    }
    GeneralQuantumOperator::add_operator(mpt);
}

void HermitianQuantumOperator::add_operator(
    CPPCTYPE coef, std::string pauli_string) {
    if (std::abs(coef.imag()) > 0) {
        std::cerr << "Error: HermitianQuantumOperator::add_operator(const "
                     "PauliOperator* mpt): PauliOperator must be Hermitian."
                  << std::endl;
        return;
    }
    GeneralQuantumOperator::add_operator(coef, pauli_string);
}

CPPCTYPE HermitianQuantumOperator::get_expectation_value(
    const QuantumStateBase* state) const {
    return GeneralQuantumOperator::get_expectation_value(state).real();
}

CPPCTYPE
HermitianQuantumOperator::solve_ground_state_eigenvalue_by_lanczos_method(
    QuantumStateBase* init_state, const UINT iter_count,
    const CPPCTYPE mu) const {
    if (this->get_term_count() == 0) {
        std::cerr << "Error: "
                     "HermitianQuantumOperator::solve_ground_state_eigenvalue_"
                     "by_lanczos_method("
                     "QuantumStateBase * state, const UINT iter_count, const "
                     "CPPCTYPE mu): At least one PauliOperator is required.";
        return 0;
    }

    // Implemented based on
    // https://files.transtutors.com/cdn/uploadassignments/472339_1_-numerical-linear-aljebra.pdf
    CPPCTYPE mu_;
    if (mu == 0.0) {
        // mu is not changed from default value.
        mu_ = this->calculate_default_mu();
    } else {
        mu_ = mu;
    }

    const auto qubit_count = this->get_qubit_count();
    QuantumState tmp_state(qubit_count);
    QuantumState mu_timed_state(qubit_count);
    // work_states: [q_{i-1}, q_i, q_{i+1}]
    // q_0, q_1, q_2,... span Krylov subspace.
    init_state->normalize(init_state->get_squared_norm());
    std::array<QuantumState, 3> work_states = {QuantumState(qubit_count),
        QuantumState(qubit_count), QuantumState(qubit_count)};
    work_states.at(1).load(init_state);

    Eigen::VectorXd alpha_v(iter_count);
    Eigen::VectorXd beta_v(iter_count - 1);
    for (UINT i = 0; i < iter_count; i++) {
        // v = (A - μI) * q_i
        mu_timed_state.load(&work_states.at(1));
        mu_timed_state.multiply_coef(-mu_);
        this->apply_to_state(&tmp_state, work_states.at(1), &work_states.at(2));
        work_states.at(2).add_state(&mu_timed_state);

        // α_i = q_i^T * v
        alpha_v(i) =
            state::inner_product(&work_states.at(1), &work_states.at(2)).real();
        // In the last iteration, no need to calculate β.
        if (i == iter_count - 1) {
            break;
        }
        // v -= α_i * q_i
        tmp_state.load(&work_states.at(1));
        tmp_state.multiply_coef(-alpha_v(i));
        work_states.at(2).add_state(&tmp_state);
        if (i != 0) {
            // v -= β_{i-1} * q_{i-1}
            tmp_state.load(&work_states.at(0));
            tmp_state.multiply_coef(-beta_v(i - 1));
            work_states.at(2).add_state(&tmp_state);
        }

        // β_i = ||v||
        beta_v(i) = std::sqrt(work_states.at(2).get_squared_norm());
        // q_{i+1} = v / β_i
        work_states.at(2).multiply_coef(1 / beta_v(i));
        work_states.at(0).load(&work_states.at(1));
        work_states.at(1).load(&work_states.at(2));
    }

    // Compute eigenvalue of a symmetric matrix T whose diagonal elements are
    // `alpha_v` and subdiagonal elements are `beta_v`.
    Eigen::SelfAdjointEigenSolver<ComplexMatrix> solver;
    solver.computeFromTridiagonal(alpha_v, beta_v);
    const auto eigenvalues = solver.eigenvalues();
    // Find ground state eigenvalue.
    UINT minimum_eigenvalue_index = 0;
    auto minimum_eigenvalue = eigenvalues(0);
    for (UINT i = 0; i < eigenvalues.size(); i++) {
        if (eigenvalues(i) < minimum_eigenvalue) {
            minimum_eigenvalue_index = i;
            minimum_eigenvalue = eigenvalues(i);
        }
    }

    auto eigenvectors = solver.eigenvectors();
    auto eigenvector_in_krylov = eigenvectors.col(minimum_eigenvalue_index);
    // Store ground state eigenvector to `init_state`.
    // If λ is an eigenvalue of T and q is the eigenvector, Tq = λq.
    // And let V be a matrix whose column vectors span Krylov subspace.
    // Then, T = V^* AV where A is this observable.
    // Tq = λq, VV^* AVq = Vλq, A(Vq) = λ(Vq).
    // So, an eigenvector of A for λ is Vq.
    // q_0 = init_state
    work_states.at(1).load(init_state);
    init_state->multiply_coef(0.0);
    assert(eigenvector_in_krylov.size() == iter_count);
    for (UINT i = 0; i < iter_count; i++) {
        // q += v_i * q_i, where q is eigenvector to compute
        tmp_state.load(&work_states.at(1));
        tmp_state.multiply_coef(eigenvector_in_krylov(i));
        init_state->add_state(&tmp_state);

        // v = (A - μI) * q_i
        mu_timed_state.load(&work_states.at(1));
        mu_timed_state.multiply_coef(-mu_);
        this->apply_to_state(&tmp_state, work_states.at(1), &work_states.at(2));
        work_states.at(2).add_state(&mu_timed_state);
        if (i == iter_count - 1) {
            break;
        }

        // v -= α_i * q_i
        tmp_state.load(&work_states.at(1));
        tmp_state.multiply_coef(-alpha_v(i));
        work_states.at(2).add_state(&tmp_state);
        if (i != 0) {
            // v -= β_{i-1} * q_{i-1}
            tmp_state.load(&work_states.at(0));
            tmp_state.multiply_coef(-beta_v(i - 1));
            work_states.at(2).add_state(&tmp_state);
        }

        // q_{i+1} = v / β_i
        work_states.at(2).multiply_coef(1 / beta_v[i]);
        work_states.at(0).load(&work_states.at(1));
        work_states.at(1).load(&work_states.at(2));
    }

    return minimum_eigenvalue + mu_;
}

std::string HermitianQuantumOperator::to_string() const {
    std::stringstream os;
    auto term_count = this->get_term_count();
    for (UINT index = 0; index < term_count; index++) {
        os << this->get_term(index)->get_coef().real() << " ";
        os << this->get_term(index)->get_pauli_string();
        if (index != term_count - 1) {
            os << " + ";
        }
    }
    return os.str();
}

namespace observable {
HermitianQuantumOperator* create_observable_from_openfermion_file(
    std::string file_path) {
    UINT qubit_count = 0;
    std::vector<CPPCTYPE> coefs;
    std::vector<std::string> ops;

    std::ifstream ifs;
    ifs.open(file_path);

    if (!ifs) {
        std::cerr << "ERROR: Cannot open file" << std::endl;
        return NULL;
    }

    // loading lines and check qubit_count
    std::string str_buf;
    std::vector<std::string> index_list;

    std::string line;
    while (getline(ifs, line)) {
        std::tuple<double, double, std::string> parsed_items =
            parse_openfermion_line(line);
        const auto coef_real = std::get<0>(parsed_items);
        const auto coef_imag = std::get<1>(parsed_items);
        str_buf = std::get<2>(parsed_items);

        CPPCTYPE coef(coef_real, coef_imag);
        coefs.push_back(coef);
        ops.push_back(str_buf);
        index_list = split(str_buf, "IXYZ ");

        for (UINT i = 0; i < index_list.size(); ++i) {
            UINT n = std::stoi(index_list[i]) + 1;
            if (qubit_count < n) qubit_count = n;
        }
    }
    if (!ifs.eof()) {
        std::cerr << "ERROR: Invalid format" << std::endl;
        return NULL;
    }
    ifs.close();

    HermitianQuantumOperator* observable =
        new HermitianQuantumOperator(qubit_count);

    for (UINT i = 0; i < ops.size(); ++i) {
        observable->add_operator(new PauliOperator(ops[i].c_str(), coefs[i]));
    }

    return observable;
}

HermitianQuantumOperator* create_observable_from_openfermion_text(
    const std::string& text) {
    UINT qubit_count = 0;
    std::vector<CPPCTYPE> coefs;
    std::vector<std::string> ops;

    std::vector<std::string> lines;
    std::string str_buf;
    std::vector<std::string> index_list;

    lines = split(text, "\n");
    for (std::string line : lines) {
        std::tuple<double, double, std::string> parsed_items =
            parse_openfermion_line(line);
        const auto coef_real = std::get<0>(parsed_items);
        const auto coef_imag = std::get<1>(parsed_items);
        str_buf = std::get<2>(parsed_items);

        CPPCTYPE coef(coef_real, coef_imag);
        coefs.push_back(coef);
        ops.push_back(str_buf);
        index_list = split(str_buf, "IXYZ ");

        for (UINT i = 0; i < index_list.size(); ++i) {
            UINT n = std::stoi(index_list[i]) + 1;
            if (qubit_count < n) qubit_count = n;
        }
    }
    HermitianQuantumOperator* hermitian_quantum_operator =
        new HermitianQuantumOperator(qubit_count);

    for (UINT i = 0; i < ops.size(); ++i) {
        hermitian_quantum_operator->add_operator(
            new PauliOperator(ops[i].c_str(), coefs[i]));
    }

    return hermitian_quantum_operator;
}

std::pair<HermitianQuantumOperator*, HermitianQuantumOperator*>
create_split_observable(std::string file_path) {
    UINT qubit_count = 0;
    std::vector<CPPCTYPE> coefs;
    std::vector<std::string> ops;

    std::ifstream ifs;
    ifs.open(file_path);

    if (!ifs) {
        std::cerr << "ERROR: Cannot open file" << std::endl;
        return std::make_pair(
            (HermitianQuantumOperator*)NULL, (HermitianQuantumOperator*)NULL);
    }

    // loading lines and check qubit_count
    std::string str_buf;
    std::vector<std::string> index_list;

    std::string line;
    while (getline(ifs, line)) {
        std::tuple<double, double, std::string> parsed_items =
            parse_openfermion_line(line);
        const auto coef_real = std::get<0>(parsed_items);
        const auto coef_imag = std::get<1>(parsed_items);
        str_buf = std::get<2>(parsed_items);

        CPPCTYPE coef(coef_real, coef_imag);
        coefs.push_back(coef);
        ops.push_back(str_buf);
        index_list = split(str_buf, "IXYZ ");

        for (UINT i = 0; i < index_list.size(); ++i) {
            UINT n = std::stoi(index_list[i]) + 1;
            if (qubit_count < n) qubit_count = n;
        }
    }
    if (!ifs.eof()) {
        std::cerr << "ERROR: Invalid format" << std::endl;
        return std::make_pair(
            (HermitianQuantumOperator*)NULL, (HermitianQuantumOperator*)NULL);
    }
    ifs.close();

    HermitianQuantumOperator* observable_diag =
        new HermitianQuantumOperator(qubit_count);
    HermitianQuantumOperator* observable_non_diag =
        new HermitianQuantumOperator(qubit_count);

    for (UINT i = 0; i < ops.size(); ++i) {
        if (ops[i].find("X") != std::string::npos ||
            ops[i].find("Y") != std::string::npos) {
            observable_non_diag->add_operator(
                new PauliOperator(ops[i].c_str(), coefs[i]));
        } else {
            observable_diag->add_operator(
                new PauliOperator(ops[i].c_str(), coefs[i]));
        }
    }

    return std::make_pair(observable_diag, observable_non_diag);
}
}  // namespace observable


#define BOOST_TEST_MODULE fastforestTests
#include <boost/test/unit_test.hpp>

#include "fastforest.h"

#include <fstream>
#include <cmath>

constexpr fastforest::FeatureType tolerance = 1e-4;
constexpr std::size_t nSamples = 100;
using RefPredictionType = float;

BOOST_AUTO_TEST_CASE(ExampleTest) {
    std::vector<std::string> features{"f0", "f1", "f2", "f3", "f4"};

    const auto fastForest = fastforest::load_txt("continuous/model.txt", features);

    std::vector<fastforest::FeatureType> input{0.0, 0.2, 0.4, 0.6, 0.8};

    fastforest::FeatureType score = fastForest(input.data());
    fastforest::FeatureType logistcScore = 1. / (1. + std::exp(-score));
}

BOOST_AUTO_TEST_CASE(BasicTest) {
    std::vector<std::string> features{"f0", "f1", "f2", "f3", "f4"};

    const auto fastForest = fastforest::load_txt("continuous/model.txt", features);

    std::ifstream fileX("continuous/X.csv");
    std::ifstream filePreds("continuous/preds.csv");

    std::vector<fastforest::FeatureType> input(5);
    fastforest::FeatureType score;
    RefPredictionType ref;

    for (std::size_t i = 0; i < nSamples; ++i) {
        for (auto& x : input) {
            fileX >> x;
        }
        score = fastForest(input.data());
        filePreds >> ref;

        BOOST_CHECK_CLOSE(score, ref, tolerance);
    }
}

BOOST_AUTO_TEST_CASE(SoftmaxTest) {
    std::vector<std::string> features{"f0", "f1", "f2", "f3", "f4"};

    const auto fastForest = fastforest::load_txt("softmax/model.txt", features);

    std::ifstream fileX("softmax/X.csv");
    std::ifstream filePreds("softmax/preds.csv");

    std::vector<fastforest::FeatureType> input(5);
    fastforest::FeatureType score;
    RefPredictionType ref;

    for (std::size_t i = 0; i < nSamples; ++i) {
        for (auto& x : input) {
            fileX >> x;
        }
        for (auto& x : fastForest.softmax(input.data(), 3)) {
            filePreds >> ref;
            BOOST_CHECK_CLOSE(x, ref, tolerance);
        }
    }
}

// This test covers the case of trees with a single leaf node.
BOOST_AUTO_TEST_CASE(SoftmaxNSamples100NFeatures100Test) {
    std::vector<std::string> features;
    for (std::size_t i = 0; i < 100; ++i) {
        features.emplace_back(std::string("f") + std::to_string(i));
    }

    const auto fastForest = fastforest::load_txt("softmax_n_samples_100_n_features_100/model.txt", features);

    std::ifstream fileX("softmax_n_samples_100_n_features_100/X.csv");
    std::ifstream filePreds("softmax_n_samples_100_n_features_100/preds.csv");

    std::vector<fastforest::FeatureType> input(features.size());
    fastforest::FeatureType score;
    RefPredictionType ref;

    for (std::size_t i = 0; i < nSamples; ++i) {
        for (auto& x : input) {
            fileX >> x;
        }
        for (auto& x : fastForest.softmax(input.data(), 3)) {
            filePreds >> ref;
            BOOST_CHECK_CLOSE(x, ref, tolerance);
        }
    }
}

BOOST_AUTO_TEST_CASE(SoftmaxArrayTest) {
    std::vector<std::string> features{"f0", "f1", "f2", "f3", "f4"};

    const auto fastForest = fastforest::load_txt("softmax/model.txt", features);

    std::ifstream fileX("softmax/X.csv");
    std::ifstream filePreds("softmax/preds.csv");

    std::vector<fastforest::FeatureType> input(5);
    fastforest::FeatureType score;
    RefPredictionType ref;

    for (std::size_t i = 0; i < nSamples; ++i) {
        for (auto& x : input) {
            fileX >> x;
        }
        for (auto& x : fastForest.softmax<3>(input.data())) {
            filePreds >> ref;
            BOOST_CHECK_CLOSE(x, ref, tolerance);
        }
    }
}

BOOST_AUTO_TEST_CASE(SerializationTest) {
    {
        std::vector<std::string> features{"f0", "f1", "f2", "f3", "f4"};
        const auto fastForest = fastforest::load_txt("continuous/model.txt", features);
        fastForest.write_bin("continuous/forest.bin");
    }

    const auto fastForest = fastforest::load_bin("continuous/forest.bin");

    std::ifstream fileX("continuous/X.csv");
    std::ifstream filePreds("continuous/preds.csv");

    std::vector<fastforest::FeatureType> input(5);
    fastforest::FeatureType score;
    RefPredictionType ref;

    for (std::size_t i = 0; i < nSamples; ++i) {
        for (auto& x : input) {
            fileX >> x;
        }
        score = fastForest(input.data());
        filePreds >> ref;

        BOOST_CHECK_CLOSE(score, ref, tolerance);
    }
}

BOOST_AUTO_TEST_CASE(DiscreteTest) {
    std::vector<std::string> features{"f0", "f1", "f2", "f3", "f4"};

    const auto fastForest = fastforest::load_txt("discrete/model.txt", features);

    std::ifstream fileX("discrete/X.csv");
    std::ifstream filePreds("discrete/preds.csv");

    std::vector<fastforest::FeatureType> input(5);
    fastforest::FeatureType score;
    RefPredictionType ref;

    for (std::size_t i = 0; i < nSamples; ++i) {
        for (auto& x : input) {
            fileX >> x;
        }
        score = fastForest(input.data());
        filePreds >> ref;

        BOOST_CHECK_CLOSE(score, ref, tolerance);
    }
}

BOOST_AUTO_TEST_CASE(ManyfeaturesTest) {
    std::vector<std::string> features{};
    for (int i = 0; i < 311; ++i) {
        features.push_back(std::string("f") + std::to_string(i));
    }

    const auto fastForest = fastforest::load_txt("manyfeatures/model.txt", features);

    std::ifstream fileX("manyfeatures/X.csv");
    std::ifstream filePreds("manyfeatures/preds.csv");

    std::vector<fastforest::FeatureType> input(features.size());
    fastforest::FeatureType score;
    RefPredictionType ref;

    for (std::size_t i = 0; i < nSamples; ++i) {
        for (auto& x : input) {
            fileX >> x;
        }
        score = fastForest(input.data());
        filePreds >> ref;

        BOOST_CHECK_CLOSE(score, ref, tolerance);
    }
}

#ifdef EXPERIMENTAL_TMVA_SUPPORT

BOOST_AUTO_TEST_CASE(BasicTMVAXMLTest) {
    std::vector<std::string> features{"f0", "f1", "f2", "f3", "f4"};

    const auto fastForest = fastforest::load_tmva_xml("continuous/model.xml", features);

    std::ifstream fileX("continuous/X.csv");
    std::ifstream filePreds("continuous/preds.csv");

    std::vector<fastforest::FeatureType> input(5);
    fastforest::FeatureType score;
    RefPredictionType ref;

    for (std::size_t i = 0; i < nSamples; ++i) {
        for (auto& x : input) {
            fileX >> x;
        }
        score = fastForest(input.data());
        filePreds >> ref;

        BOOST_CHECK_CLOSE(score, ref, tolerance);
    }
}

#endif


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE SinogramCreatorTest
#include <boost/test/unit_test.hpp>

#include "../SinogramCreatorTools.h"
#include <iostream>

BOOST_AUTO_TEST_SUITE(FirstSuite)

BOOST_AUTO_TEST_CASE(roundToNearesMultiplicity_test) {
  BOOST_REQUIRE_EQUAL(
      SinogramCreatorTools::roundToNearesMultiplicity(0.0f, 1.f), 0u);
  BOOST_REQUIRE_EQUAL(
      SinogramCreatorTools::roundToNearesMultiplicity(0.4f, 1.f), 0u);
  BOOST_REQUIRE_EQUAL(
      SinogramCreatorTools::roundToNearesMultiplicity(0.5f, 1.f), 1u);
  BOOST_REQUIRE_EQUAL(
      SinogramCreatorTools::roundToNearesMultiplicity(30.f, 1.f), 30u);
  BOOST_REQUIRE_EQUAL(
      SinogramCreatorTools::roundToNearesMultiplicity(0.00f, 0.01f), 0u);
  BOOST_REQUIRE_EQUAL(
      SinogramCreatorTools::roundToNearesMultiplicity(0.01f, 0.01f), 1u);
  BOOST_REQUIRE_EQUAL(
      SinogramCreatorTools::roundToNearesMultiplicity(0.02f, 0.01f), 2u);
}

BOOST_AUTO_TEST_CASE(test_angle_middle) {
  const float EPSILON = 0.01f;
  const float r = 10;
  const float maxDistance = 20.f;
  const float accuracy = 0.1f;

  for (int i = 0; i < 360; i++) {
    const float x1 = r * std::cos((i - 1) * (M_PI / 180.f));
    const float y1 = r * std::sin((i - 1) * (M_PI / 180.f));
    const float x2 = r * std::cos((i + 1) * (M_PI / 180.f));
    const float y2 = r * std::sin((i + 1) * (M_PI / 180.f));
    const auto result = SinogramCreatorTools::getSinogramRepresentation(
        x1, y1, x2, y2, maxDistance, accuracy,
        std::ceil(maxDistance * 2.f * (1.f / accuracy)), 180);
    BOOST_REQUIRE_EQUAL(result.second, i % 180);
    const float distance = i < 180 ? r : -r;
    const float distanceResult =
        SinogramCreatorTools::roundToNearesMultiplicity(distance + maxDistance,
                                                        accuracy);
    BOOST_REQUIRE_CLOSE(result.first, distanceResult, EPSILON);
  }
}

BOOST_AUTO_TEST_CASE(test_lor_slice) {
  const float EPSILON = 0.0001f;

  float x1 = 0.f;
  float y1 = 0.f;
  float z1 = 0.f;
  float t1 = 0.f;
  float x2 = 0.f;
  float y2 = 0.f;
  float z2 = 0.f;
  float t2 = 0.f;
  BOOST_REQUIRE_CLOSE(
      SinogramCreatorTools::calculateLORSlice(x1, y1, z1, t1, x2, y2, z2, t2),
      0.f, EPSILON);
  x1 = 1.f;
  x2 = -1.f;
  BOOST_REQUIRE_CLOSE(
      SinogramCreatorTools::calculateLORSlice(x1, y1, z1, t1, x2, y2, z2, t2),
      0.f, EPSILON);
  t1 = 10 * 3.33564095; // speed-of-light * 10 * 3.33564095 ~= 1
  t2 = 10 * 3.33564095;
  BOOST_REQUIRE_CLOSE(
      SinogramCreatorTools::calculateLORSlice(x1, y1, z1, t1, x2, y2, z2, t2),
      0.f, EPSILON);
  x1 = 0.f;
  x2 = 0.f;
  z1 = 1.f;
  z2 = -1.f;
  BOOST_REQUIRE_CLOSE(
      SinogramCreatorTools::calculateLORSlice(x1, y1, z1, t1, x2, y2, z2, t2),
      0.f, EPSILON);
  z1 = -1.f;
  z2 = 1.f;
  BOOST_REQUIRE_CLOSE(
      SinogramCreatorTools::calculateLORSlice(x1, y1, z1, t1, x2, y2, z2, t2),
      0.f, EPSILON);
}

BOOST_AUTO_TEST_SUITE_END()


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson, John B. Mains
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__FEEDBACK__NLP_HPP_
#define SMOOTH__FEEDBACK__NLP_HPP_

/**
 * @file
 * @brief Nonlinear program definition.
 */

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <smooth/diff.hpp>

#include <limits>

#include "collocation.hpp"

namespace smooth::feedback {

/**
 * @brief Nonlinear Programming Problem
 * \f[
 *  \begin{cases}
 *   \min_{x}    & f(x)                    \\
 *   \text{s.t.} & x_l \leq x \leq x_u     \\
 *               & g_l \leq g(x) \leq g_u
 *  \end{cases}
 * \f]
 * for \f$ f : \mathbb{R}^n \rightarrow \mathbb{R} \f$ and
 * \f$ g : \mathbb{R}^n \rightarrow \mathbb{R}^m \f$.
 */
struct NLP
{
  /// @brief Number of variables
  std::size_t n;

  /// @brief Number of constraints
  std::size_t m;

  /// @brief Objective function (R^n -> R)
  std::function<double(Eigen::VectorXd)> f;

  /// @brief Variable bounds (R^n)
  Eigen::VectorXd xl, xu;

  /// @brief Constraint function (R^n -> R^m)
  std::function<Eigen::VectorXd(Eigen::VectorXd)> g;

  /// @brief Constaint bounds (R^m)
  Eigen::VectorXd gl, gu;

  /// @brief Jacobian of objective function (R^n -> R^{n x n})
  std::function<Eigen::SparseMatrix<double>(Eigen::VectorXd)> df_dx;

  /// @brief Jacobian of constraint function (R^n -> R^{m x n})
  std::function<Eigen::SparseMatrix<double>(Eigen::VectorXd)> dg_dx;

  /// @brief Hessian of objective function (R^n -> R^{n x n}) [optional]
  std::optional<std::function<Eigen::SparseMatrix<double>(Eigen::VectorXd, Eigen::VectorXd)>>
    d2f_dx2 = std::nullopt;

  /**
   * @brief Projected Hessian of constraint function (R^m, R^n -> R^{n x n}) [optional]
   *
   * Should return the derivative
   * \f[
   *  H_g(\lambda, x) = \nabla^2_x \lambda^T g(x), \quad \lambda \in \mathbb{R}^m, x \in
   * \mathbb{R}^n \f]
   */
  std::optional<std::function<Eigen::SparseMatrix<double>(Eigen::VectorXd, Eigen::VectorXd)>>
    d2g_dx2 = std::nullopt;
};

struct NLPSolution
{
  /// @brief Solver status
  enum class Status {
    Optimal,
    PrimalInfeasible,
    DualInfeasible,
    MaxIterations,
    MaxTime,
    Unknown,
  } status;

  /// @brief Number of iterations
  std::size_t iter{0};

  /// @brief Variable values
  Eigen::VectorXd x;

  /// @brief Inequality multipliers
  Eigen::VectorXd zl, zu;

  /// @brief Constraint multipliers
  Eigen::VectorXd lambda;

  /// @brief Objective
  double objective{0};
};

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__NLP_HPP_


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

#include <stdint.h>
#include <iostream>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/QR>

// CUDA runtime
#include <cuda_runtime.h>
// Utilities and system includes
//#include <helper_functions.h>
#include <nvidia/helper_cuda.h>

//#include <mmf/defines.h>

using namespace Eigen;
using std::min;
using std::max;
using std::cout;
using std::endl;

template<typename T>
class OptSO3ApproxCpu
{
  public:
  OptSO3ApproxCpu(T t_max = 5.0, T dt = 0.05)
    : t_max_(t_max), dt_(dt), t_(0)
  {
    R_ = Matrix<T,3,3>::Identity();
  };

  virtual ~OptSO3ApproxCpu()
  { };

//  virtual T conjugateGradient(Matrix<T,3,3>& R, uint32_t maxIter=100);
  virtual T conjugateGradient(Matrix<T,3,3>& R,
      const Matrix<T,Dynamic,Dynamic>& qKarch,
      const Matrix<T,Dynamic,1>& Ns,
      uint32_t maxIter=100);

  /* return a skew symmetric matrix from A */
  Matrix<T,3,3> enforceSkewSymmetry(const Matrix<T,3,3> &A) const
  {return 0.5*(A-A.transpose());};

  const Matrix<T,3,3>& R() const {return R_;};
  /* matrix of the 6 directions of the MF */
  Matrix<T,Dynamic,Dynamic> M() const;

  static Matrix<T,Dynamic,Dynamic> Rot2M(const Matrix<T,3,3>& R);

protected:
  T t_max_, dt_;
  uint32_t t_; // timestep
  Matrix<T,3,3> R_; // previous rotation
  Matrix<T,3,6> qKarch_; // karcher means for all axes
  Matrix<T,1,6> Ns_; // number of normals for each axis

  virtual void conjugateGradientPostparation_impl(Matrix<T,3,3>& R);
  virtual T conjugateGradient_impl(Matrix<T,3,3>& R, T res0,
      uint32_t maxIter=100);
  /*
   * evaluate cost function for a given assignment of npormals to axes
   */
  virtual T evalCostFunction(Matrix<T,3,3>& R);
  /* compute Jacobian */
  virtual void computeJacobian(Matrix<T,3,3>&J, Matrix<T,3,3>& R);

  /*
   * updates G and H from rotation R and jacobian J
   */
  virtual void updateGandH(Matrix<T,3,3>& G, Matrix<T,3,3>& G_prev,
      Matrix<T,3,3>& H, const Matrix<T,3,3>& R, const Matrix<T,3,3>& J,
      const Matrix<T,3,3>& M_t_min, bool resetH);
  /*
   * performs line search starting at R in direction of H returns min
   * of cost function and updates R, and M_t_min
   */
  virtual T linesearch(Matrix<T,3,3>& R, Matrix<T,3,3>& M_t_min, const
      Matrix<T,3,3>& H, T t_max=1.0, T dt=0.1);
  /* convert a Rotation matrix R to a MF representaiton of the axes */
  void Rot2M(Matrix<T,3,3>& R, T *mu);
};


// Copyright (C) 2015 The Regents of the University of California (Regents).
// 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 Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/estimators/estimate_calibrated_absolute_pose.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <memory>
#include <vector>

#include "theia/sfm/create_and_initialize_ransac_variant.h"
#include "theia/sfm/estimators/feature_correspondence_2d_3d.h"
#include "theia/sfm/pose/perspective_three_point.h"
#include "theia/solvers/estimator.h"
#include "theia/solvers/sample_consensus_estimator.h"
#include "theia/util/util.h"

namespace theia {
namespace {

using Eigen::Matrix3d;
using Eigen::Vector3d;

// An estimator for computing the absolute pose from 3 feature
// correspondences. The feature correspondences should be normalized by the
// focal length with the principal point at (0, 0).
class CalibratedAbsolutePoseEstimator
    : public Estimator<FeatureCorrespondence2D3D, CalibratedAbsolutePose> {
 private:
    // note: not "fixed-size-vectorizable" so no need to use aligned_allocator
    // https://eigen.tuxfamily.org/dox/group__TopicFixedSizeVectorizable.html
    std::vector<Eigen::Matrix3d> rotations;
    std::vector<Eigen::Vector3d> translations;
 public:
  CalibratedAbsolutePoseEstimator() {}

  // 3 correspondences are needed to determine the absolute pose.
  double SampleSize() const { return 3; }

  // Estimates candidate absolute poses from correspondences.
  bool EstimateModel(
      const std::vector<FeatureCorrespondence2D3D>& correspondences,
      std::vector<CalibratedAbsolutePose>* absolute_poses) const {
    const Eigen::Vector2d features[3] = {correspondences[0].feature,
                                         correspondences[1].feature,
                                         correspondences[2].feature};
    const Eigen::Vector3d world_points[3] = {correspondences[0].world_point,
                                             correspondences[1].world_point,
                                             correspondences[2].world_point};

    // avoid massive reallocation if called frequently
    auto mutable_this = const_cast<CalibratedAbsolutePoseEstimator&>(*this);
    auto &rotations = mutable_this.rotations;
    auto &translations = mutable_this.translations;
    rotations.clear();
    translations.clear();
    if (!PoseFromThreePoints(features,
                             world_points,
                             &rotations,
                             &translations)) {
      return false;
    }

    for (int i = 0; i < rotations.size(); i++) {
      CalibratedAbsolutePose pose;
      pose.rotation = rotations[i];
      pose.position = -pose.rotation.transpose() * translations[i];
      absolute_poses->emplace_back(pose);
    }

    return absolute_poses->size() > 0;
  }

  // The error for a correspondences given an absolute pose. This is the squared
  // reprojection error.
  double Error(const FeatureCorrespondence2D3D& correspondence,
               const CalibratedAbsolutePose& absolute_pose) const {
    // The reprojected point is computed as R * (X - c) where R is the camera
    // rotation, c is the position, and X is the 3D point.
    const Eigen::Vector2d reprojected_feature =
        (absolute_pose.rotation *
         (correspondence.world_point - absolute_pose.position)).hnormalized();
    return (reprojected_feature - correspondence.feature).squaredNorm();
  }
};

class ReusableCalibratedAbsolutePoseEstimatorImpl : public ReusableCalibratedAbsolutePoseEstimator {
private:
  CalibratedAbsolutePoseEstimator absolute_pose_estimator;
  std::unique_ptr<SampleConsensusEstimator<CalibratedAbsolutePoseEstimator> > ransac;

public:
  ReusableCalibratedAbsolutePoseEstimatorImpl(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type)
  {
    ransac = CreateAndInitializeRansacVariant(ransac_type, ransac_params, absolute_pose_estimator);
  }

  bool estimate(
      const std::vector<FeatureCorrespondence2D3D>& normalized_correspondences,
      CalibratedAbsolutePose* absolute_pose,
      RansacSummary* ransac_summary) final {
      // Estimate the absolute pose.
      return ransac->Estimate(normalized_correspondences, absolute_pose, ransac_summary);
  }
};
}  // namespace

std::unique_ptr<ReusableCalibratedAbsolutePoseEstimator> ReusableCalibratedAbsolutePoseEstimator::build(
      const RansacParameters& ransac_params,
      const RansacType& ransac_type) {
  return std::make_unique<ReusableCalibratedAbsolutePoseEstimatorImpl>(ransac_params, ransac_type);
}

ReusableCalibratedAbsolutePoseEstimator::~ReusableCalibratedAbsolutePoseEstimator() = default;

bool EstimateCalibratedAbsolutePose(
    const RansacParameters& ransac_params,
    const RansacType& ransac_type,
    const std::vector<FeatureCorrespondence2D3D>& normalized_correspondences,
    CalibratedAbsolutePose* absolute_pose,
    RansacSummary* ransac_summary)
{
  return ReusableCalibratedAbsolutePoseEstimatorImpl(ransac_params, ransac_type)
    .estimate(normalized_correspondences, absolute_pose, ransac_summary);
}
}  // namespace theia


///////////////////////////////////////////////////////////////////////////////
//                                                                           //
//                 polar v1.0 - August the 15th 2019                         //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
//                                                                           //
//  Copyright 2019 Matteo Tommasini                                          //
//                                                                           //
//  Licensed under the Apache License, Version 2.0 (the "License");          //
//  you may not use this file except in compliance with the License.         //
//  You may obtain a copy of the License at                                  //
//                                                                           //
//      http://www.apache.org/licenses/LICENSE-2.0                           //
//                                                                           //
//  Unless required by applicable law or agreed to in writing, software      //
//  distributed under the License is distributed on an "AS IS" BASIS,        //
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. //
//  See the License for the specific language governing permissions and      //
//  limitations under the License.                                           //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

#ifndef ICM_H
#define ICM_H

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include "units.hpp"
#include "options.hpp"
#include "gaussiandataset.hpp"

///////////////////////////////////
// ICM: Intensity Carrying Modes //
///////////////////////////////////
class ICM
{
   private:

   bool has_ICM_IR  = false,
        has_ICM_VCD = false,
        has_ICM_MAG = false;

   Eigen::VectorXd ICM_intensity;
   Eigen::MatrixXd ICM_displacement;

   public:
   ICM() = default;
   explicit ICM(const GaussianDataset&, const std::string&);
   void Diagonalize(const Eigen::MatrixXd&, Eigen::VectorXd&, Eigen::MatrixXd&);
   void WriteMolden(const GaussianDataset&, const std::string&) const;
   void PrintReport() const;

};

#endif // ICM_H


//
// Created by Yosuke Takahashi on 10/14/18.
//

#include <gtest/gtest.h>
#include "../src/particle_filter.h"
#include <Eigen/Dense>
#include <iostream>

using namespace std;
using Eigen::MatrixXd;
using Eigen::VectorXd;

TEST(ParticleFilter, SimpleTest) {
    ASSERT_EQ(2, 2);
}

TEST(ParticleFilter, prediction) {
    std::vector<Particle> particles;

    Particle p;

    p.x = 102;
    p.y = 65;
    p.theta = 5*M_PI/8;

    double velocity = 110.0;
    double yaw_rate = M_PI/8;
    double delta_t = 0.1;

    double sigma_pos[3] = {0.0, 0.0, 0.0}; // GPS measurement uncertainty [x [m], y [m], theta [rad]]

    particles.push_back(p);

    ParticleFilter pf;
    pf.setParticles(particles);

    pf.prediction(delta_t, sigma_pos, velocity, yaw_rate);

    EXPECT_NEAR(pf.particles[0].x, 97.59, 0.01);
    EXPECT_NEAR(pf.particles[0].y, 75.08, 0.01);
    EXPECT_NEAR(pf.particles[0].theta, 51*M_PI/80, 0.01);
}

TEST(ParticleFilter, transformObservationCoordinateVehicleToMap_1) {
    ParticleFilter pf;

    Particle p;
    LandmarkObs obs;

    p.x = 4;
    p.y = 5;
    p.theta = -M_PI/2;
    obs.x = 2;
    obs.y = 2;

    LandmarkObs map;

    map = pf.transformObservationCoordinateVehicleToMap(p, obs);

    EXPECT_NEAR(map.x, 6.0, 0.1);
    EXPECT_NEAR(map.y, 3.0, 0.1);
}

TEST(ParticleFilter, transformObservationCoordinateVehicleToMap_2) {
    ParticleFilter pf;

    Particle p;
    LandmarkObs obs;

    p.x = 4;
    p.y = 5;
    p.theta = -M_PI/2;
    obs.x = 3;
    obs.y = -2;

    LandmarkObs map;

    map = pf.transformObservationCoordinateVehicleToMap(p, obs);

    EXPECT_NEAR(map.x, 2.0, 0.1);
    EXPECT_NEAR(map.y, 2.0, 0.1);
}

TEST(ParticleFilter, getNearestLandmark) {
    ParticleFilter pf;

    Particle p;
    p.x = 4;
    p.y = 5;
    p.theta = -M_PI/2;

    Map map;
    Map::single_landmark_s l1, l2, l3, l4, l5;

    l1.id_i = 1;
    l1.x_f  = 5;
    l1.y_f  = 3;
    map.landmark_list.push_back(l1);

    l2.id_i = 2;
    l2.x_f  = 2;
    l2.y_f  = 1;
    map.landmark_list.push_back(l2);

    l3.id_i = 3;
    l3.x_f  = 6;
    l3.y_f  = 1;
    map.landmark_list.push_back(l3);

    l4.id_i = 4;
    l4.x_f  = 7;
    l4.y_f  = 4;
    map.landmark_list.push_back(l4);

    l5.id_i = 5;
    l5.x_f  = 4;
    l5.y_f  = 7;
    map.landmark_list.push_back(l5);

    LandmarkObs obs1, obs2, obs3;
    obs1.x = 2;
    obs1.y = 2;

    obs2.x = 3;
    obs2.y = -2;

    obs3.x = 0;
    obs3.y = -4;

    LandmarkObs obs_map1, obs_map2, obs_map3;
    obs_map1 = pf.transformObservationCoordinateVehicleToMap(p, obs1);
    obs_map2 = pf.transformObservationCoordinateVehicleToMap(p, obs2);
    obs_map3 = pf.transformObservationCoordinateVehicleToMap(p, obs3);

    LandmarkObs nearest_obs1, nearest_obs2, nearest_obs3;
    nearest_obs1 = pf.getNearestLandmark(obs_map1, map);
    nearest_obs2 = pf.getNearestLandmark(obs_map2, map);
    nearest_obs3 = pf.getNearestLandmark(obs_map3, map);

    EXPECT_EQ(nearest_obs1.id, 1);
    EXPECT_EQ(nearest_obs2.id, 2);
    EXPECT_EQ(nearest_obs3.id, 2);

    double weight1, weight2, weight3;
    double std_landmark[3] = {0.3, 0.3, 0.0};
    weight1 = pf.getObservationWeight(obs_map1, nearest_obs1, std_landmark);
    weight2 = pf.getObservationWeight(obs_map2, nearest_obs2, std_landmark);
    weight3 = pf.getObservationWeight(obs_map3, nearest_obs3, std_landmark);

    EXPECT_NEAR(weight1, 6.84e-3, 0.01e-3);
    EXPECT_NEAR(weight2, 6.84e-3, 0.01e-3);
    EXPECT_NEAR(weight3, 9.83e-49, 0.01e-49);
}

TEST(ParticleFilter, getParticleWeight) {
    ParticleFilter pf;

    Particle p;
    p.x = 4;
    p.y = 5;
    p.theta = -M_PI/2;

    Map map;
    Map::single_landmark_s l1, l2, l3, l4, l5;

    l1.id_i = 1;
    l1.x_f  = 5;
    l1.y_f  = 3;
    map.landmark_list.push_back(l1);

    l2.id_i = 2;
    l2.x_f  = 2;
    l2.y_f  = 1;
    map.landmark_list.push_back(l2);

    l3.id_i = 3;
    l3.x_f  = 6;
    l3.y_f  = 1;
    map.landmark_list.push_back(l3);

    l4.id_i = 4;
    l4.x_f  = 7;
    l4.y_f  = 4;
    map.landmark_list.push_back(l4);

    l5.id_i = 5;
    l5.x_f  = 4;
    l5.y_f  = 7;
    map.landmark_list.push_back(l5);

    LandmarkObs obs1, obs2, obs3;
    obs1.x = 2;
    obs1.y = 2;

    obs2.x = 3;
    obs2.y = -2;

    obs3.x = 0;
    obs3.y = -4;

    std::vector<LandmarkObs> observations;
    observations.push_back(obs1);
    observations.push_back(obs2);
    observations.push_back(obs3);

    double std_landmark[3] = {0.3, 0.3, 0.0};
    double weight = pf.getParticleWeight(p, std_landmark, observations, map);

    EXPECT_NEAR(weight, 4.60e-53, 0.01e-53);
}


#ifndef __Physical_Parameters_hpp_
#define __Physical_Parameters_hpp_

#include <Eigen/Geometry>
#include <vector>

using namespace std;

//Units:
	//Energy
	extern const double GeV,eV,keV,MeV,TeV;
	//Mass
	extern const double gram,kg;
	//Length
	extern const double cm,mm,meter,km,fm,pb,parsec,kpc,Mpc;
	//Time
	extern const double sec,minute,hour,day,year;
	//Temperature
	extern const double Kelvin;
	//Others
	extern const double erg;

//Specific Parameters:
	//Masses
	extern const double mPlanck,GNewton,mProton,mElectron,mNucleon;
	//Geographic Parameters
	extern const double mEarth,rEarth,rhoEarth;
	//Solar Parameters
	extern const double mSun,rSun,rhoSun;
	//Dark Matter Halo Parameters
	extern const double v0,vesc;
	extern const double Nesc;
	extern const double ymax;
	//degree to rad
	extern const double deg;

//Unit Conversion
	extern double InUnits(double quantity, double dimension);
//Reduced Mass
	extern double Mu(double m1,double m2);

//Average inverse velocity
	extern double AverageInvVelocity(double vE);

//Nucleus Mass
	extern double NucleusMass(double A);

//Wimp Nucleus Cross-section with zero momentum transfer:
	extern double sigmaSI(double mX,double sigman0,double A);
//Total Wimp Nucleus Cross-section:
	extern double TotalsigmaSI(double mX,double sigman0,double A,double vX=0.0);
//Helm Form Factor (approximation)
	extern double FF_HelmApproximation(double qSquared,double A);
	extern double FF_HelmApproximation_Integrated(double mX,double vDM,double A);

//Coordinate System Change
	extern Eigen::Vector3d SphericalCoordinates(double r,double theta,double phi);
	extern Eigen::Vector3d Equat2Gal(Eigen::Vector3d& v,double T=0.0);
	extern Eigen::Vector3d GeoEcl2Gal(Eigen::Vector3d& v,double T=0.0);
	extern Eigen::Vector3d HelEcl2Gal(Eigen::Vector3d& v,double T=0.0);
	extern Eigen::Vector3d Gal2Equat(Eigen::Vector3d& v,double T=0.0);

//fractional days
	extern double FractionalDays(int date[3], int time[3]);
	extern double LASTinSeconds(double nJ2000,double longitude=0);
//Earth's Velocity in the galactic frame
	extern Eigen::Vector3d EarthVelocity(double n=0.0);

#endif


#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include "jefflib.h"
#include <boost/tokenizer.hpp>
#include <thread>
#include <chrono>
#include <ncurses.h>

using namespace std;
using namespace boost;

struct star{
    int x, y, dx, dy;
};

void PrintStars(vector<star>& stars, int rows, int cols, int maxX, int maxY, int minX, int minY, int time){

    // -----> Status <----- //
    mvprintw(0,0,"Screen Columns: %d",cols);
    mvprintw(1,0,"Screen Rows: %d",rows);
    mvprintw(2,0,"Nunber of Stars: %d",stars.size());
    mvprintw(4,0,"Time: %d",time);

    int miX = INT_MAX;
    int miY = INT_MAX;
    int maX = 0;
    int maY = 0;
    for(auto star : stars){
        int y = (rows*star.y)/maxY;
        int x = (cols*star.x)/maxX;
        mvaddch(y,x,'*');

        if(x<miX)miX=x;
        if(x>maX)maX=x;
        if(y>maY)maY=y;
        if(y<miY)miY=y;
    }
    mvprintw(3,0,"MinX %d, MinX %d, MaxX %d, MaxY %d", miX, miY, maX, maY);
    refresh();
}

void TranslateStars(int minX, int minY, vector<star>& stars, int& maxX, int& maxY){
    maxX = 0;
    maxY = 0;
    for(auto& star : stars){
        if(minX < 0){
            star.x += abs(minX);
        }
        if(minX > 0){
            star.x -= minX;
        }

        if(minY < 0){
            star.y += abs(minY);
        }
        if(minY > 0){
            star.y -= minY;
        }
        if(star.x>maxX) maxX = star.x;
        if(star.y>maxY) maxY = star.y;
    }
}

void UpdateStars(vector<star>& stars, int& maxX, int& maxY, int& minX, int& minY){

    // Update them
    for(auto& star : stars){
        star.x += star.dx;
        star.y += star.dy;
    }
    minX = INT_MAX;
    minY = INT_MAX;
    maxX = 0;
    maxY = 0;

    for(auto star : stars){
        if(star.x < minX) minX = star.x;
        if(star.x > maxX) maxX = star.x;
        if(star.y < minY) minY = star.y;
        if(star.y > maxY) maxY = star.y;
    }

    TranslateStars(minX, minY, stars, maxX, maxY);

}

int main()
{
    initscr();
    cbreak();
    noecho();
    clear();

    int row, col;
    getmaxyx(stdscr,row,col);

    vector<string> vect;
    GetStringInput(vect);

    // Example input line, nicely well-formed :)
    //  ..........10.....17.................36.39..
    //  position=<-31503, -52596> velocity=< 3,  5>

    vector<star> stars;
    for(auto line : vect){
        star tempStar;
        tempStar.x  = stoi(line.substr(10,6));
        tempStar.y  = stoi(line.substr(17, 7));
        tempStar.dx = stoi(line.substr(36, 2));
        tempStar.dy = stoi(line.substr(39, 3));
        stars.push_back(tempStar);
    }

    int time = 0;
    int maxX = 0;
    int maxY = 0;
    int minX = INT_MAX;
    int minY = INT_MAX;
    int delay = 0;
    for(;;){
        clear();
        time++;
        UpdateStars(stars, maxX, maxY, minX, minY);
        PrintStars(stars, row, col, maxX, maxY, minX, minY, time);
        this_thread::sleep_for(chrono::milliseconds(delay));
        if(time > 10000) delay = 10; //100
        if(time > 10500) delay = 100; //500
        if(time > 10550) delay = 500; //1000
        if(time == 10558) delay = 5000;
        if(time > 10558) delay = 500;
        /*
        if(time == 10558){
            ofstream myfile;
            myfile.open ("out-558.csv");
            for(auto star : stars){
                myfile << star.x <<","<<star.y<<endl;
            }
            myfile << "Writing this to a file.\n";
            myfile.close();
            return 0;
        }
        */
    }
}