/*    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.
 *
 *    References
 *      Montebruck O, Gill E. Satellite Orbits, Corrected Third Printing, Springer, 2005.
 *      Bate R. Fundamentals of Astrodynamics, Courier Dover Publications, 1971.
 *      Wikipedia, Sphere of Influence, accesed 121018.
 *        http://en.wikipedia.org/wiki/Sphere_of_influence_(astrodynamics)
 *      Petit et al., IERS Conventions, IERS, 2010.
 *
 */

#define BOOST_TEST_MAIN

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

#include <Eigen/Core>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/missionGeometry.h"

namespace tudat
{
namespace unit_tests
{

//! Show the functionality of the unit tests.
BOOST_AUTO_TEST_SUITE( test_Mission_Geometry )

//! Unit test for shadow function (Sun,Earth).
BOOST_AUTO_TEST_CASE( testShadowFunctionForFullShadow )
{
    // Satellite totally blocked from solar radiation by the earth. Sun and satellite located on the
    // x-axis with the Earth in between. Altitude of satellite = 1000 km.

    const Eigen::Vector3d occultedBodyPosition = -149598000.0e3 * Eigen::Vector3d( 1.0, 0.0, 0.0 );
    const Eigen::Vector3d occultingBodyPosition = Eigen::Vector3d::Zero( );
    const double occultedBodyRadius = 6.96e8; // Siedelmann 1992.
    const double occultingBodyRadius = 6378.137e3; // WGS-84.

    const Eigen::Vector3d satellitePosition = ( occultingBodyRadius + 1.0e6 )
            * Eigen::Vector3d( 1.0, 0.0, 0.0 );

    // Compute shadow function.
    const double shadowFunction = mission_geometry::computeShadowFunction(
                occultedBodyPosition,
                occultedBodyRadius,
                occultingBodyPosition,
                occultingBodyRadius,
                satellitePosition );

    // Test values.
    BOOST_CHECK_EQUAL( 0.0, shadowFunction );
}

BOOST_AUTO_TEST_CASE( testShadowFunctionForFullLight )
{
    // Satellite totally subjected to sunlight. Satellite is located on the y-axis and the sun is
    // located on the x-axis. Altitude of satellite = 1000 km.

    const Eigen::Vector3d occultedBodyPosition = -149598000.0e3 * Eigen::Vector3d( 1.0, 0.0, 0.0 );
    const Eigen::Vector3d occultingBodyPosition = Eigen::Vector3d::Zero( );
    const double occultedBodyRadius = 6.96e8; // Siedelmann 1992.
    const double occultingBodyRadius = 6378.137e3; // WGS-84.

    const Eigen::Vector3d satellitePosition = ( occultingBodyRadius + 1.0e6 )
            * Eigen::Vector3d( 0.0, 1.0, 0.0 );

    // Compute shadow function.
    const double shadowFunction = mission_geometry::computeShadowFunction(
                occultedBodyPosition,
                occultedBodyRadius,
                occultingBodyPosition,
                occultingBodyRadius,
                satellitePosition );

    // Test values
    BOOST_CHECK_EQUAL( 1.0, shadowFunction );
}

BOOST_AUTO_TEST_CASE( testShadowFunctionForPartialShadow )
{
    // Satellite partially visible. Satellite is located between the locations of the previous
    // tests in penumbra. According to analytical derivations in Matlab the shadow function should
    // be around 0.4547. Altitude of satellite = 1000 km.

    const Eigen::Vector3d occultedBodyPosition = -149598000.0e3 * Eigen::Vector3d( 1.0, 0.0, 0.0 );
    const Eigen::Vector3d occultingBodyPosition = Eigen::Vector3d::Zero( );
    const double occultedBodyRadius = 6.96e8; // Siedelmann 1992.
    const double occultingBodyRadius = 6378.137e3; // WGS-84.

    Eigen::Vector3d satelliteDirection( 0.018, 1.0, 0.0 );
    satelliteDirection.normalize( );

    const Eigen::Vector3d satellitePosition = ( occultingBodyRadius + 1.0e3 ) * satelliteDirection;

    // Compute shadow function
    const double shadowFunction = mission_geometry::computeShadowFunction(
                occultedBodyPosition,
                occultedBodyRadius,
                occultingBodyPosition,
                occultingBodyRadius,
                satellitePosition );

    // Test values.
    BOOST_CHECK_CLOSE_FRACTION( 0.4547, shadowFunction, 0.001 );
}

//! Unit test for computation of radius of sphere of influence (Earth with respect to Sun).
BOOST_AUTO_TEST_CASE( testSphereOfInfluenceEarth )
{
    const double distanceEarthSun = 1.49597870700e11;  // (IERS, 2010)
    const double massEarth = 5.972186390142457e24;     // (IERS, 2010)
    const double massSun = 1.988415860572227e30;       // (IERS, 2010)

    // Test 1: test function taking masses.
    {
        // Calculate the sphere of influence of the Earth with respect to the Sun.
        const double sphereOfInfluenceEarth
                = mission_geometry::computeSphereOfInfluence(
                    distanceEarthSun, massEarth, massSun );

        // Test values (Wikipedia, Sphere of Influence)
        BOOST_CHECK_CLOSE_FRACTION( 9.25e8, sphereOfInfluenceEarth, 5.0e-4 );
    }

    // Test 2: test function taking mass ratio.
    {
        // Calculate the sphere of influence of the Earth with respect to the Sun.
        const double sphereOfInfluenceEarth
                = mission_geometry::computeSphereOfInfluence(
                    distanceEarthSun, massEarth / massSun );

        // Test values (Wikipedia, Sphere of Influence)
        BOOST_CHECK_CLOSE_FRACTION( 9.25e8, sphereOfInfluenceEarth, 5.0e-4 );
    }
}

//! Unit test for computation of radius of sphere of influence (Moon with respect to Earth).
BOOST_AUTO_TEST_CASE( testSphereOfInfluenceMoon )
{
    const double distanceMoonEarth = 3.84400e8;     // (Horizons, NASA)
    const double massMoon = 7.345811416686730e22;   // (IERS, 2010)
    const double massEarth = 5.972186390142457e24;  // (IERS, 2010)

    // Test 1: test function taking masses.
    {
        // Calculate the sphere of influence of the Moon with respect to the Earth.
        const double sphereOfInfluenceEarth
                = mission_geometry::computeSphereOfInfluence(
                    distanceMoonEarth, massMoon, massEarth );

        // Test values (Wikipedia, Sphere of Influence)
        BOOST_CHECK_CLOSE_FRACTION( 6.61e7, sphereOfInfluenceEarth, 2.0e-3 );
    }

    // Test 2: test function taking mass ratio.
    {
        const double sphereOfInfluenceMoon
                = mission_geometry::computeSphereOfInfluence(
                    distanceMoonEarth, massMoon / massEarth );

        // Test values (Wikipedia, Sphere of Influence)
        BOOST_CHECK_CLOSE_FRACTION( 6.61e7, sphereOfInfluenceMoon, 2.0e-3 );
    }
}

//! Unit test for testing for retrogradeness.
BOOST_AUTO_TEST_CASE( testIsOrbitRetrograde )
{
    using namespace orbital_element_conversions;
    using namespace mission_geometry;
    using namespace mathematical_constants;

    // Initialize test Kepler elements.
    Eigen::Vector6d testKepler = Eigen::VectorXd::Zero( 6 );
    testKepler( semiMajorAxisIndex ) = 1.0e7;
    testKepler( eccentricityIndex ) = 0.1;
    testKepler( inclinationIndex ) = 50.0 / 180.0 * PI;
    testKepler( argumentOfPeriapsisIndex ) = 350.0 / 180.0 * PI;
    testKepler( longitudeOfAscendingNodeIndex ) = 15.0 / 180.0 * PI;
    testKepler( trueAnomalyIndex ) = 170.0 / 180.0 * PI;

    bool expectedIsOrbitRetrograde = false;

    bool calculatedIsOrbitRetrograde = true;

    // Test value for prograde orbit, not near boundary.
    {
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );

        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );
    }

    // Test value for prograde orbit, near zero boundary.
    {
        testKepler( inclinationIndex ) = std::numeric_limits< double >::epsilon( );
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );

        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );
    }

    // Test value for prograde orbit, near 90 degrees boundary.
    {
        testKepler( inclinationIndex ) = PI / 2.0 - std::numeric_limits< double >::epsilon( );
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );

        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );
    }

    // Test value for retrograde orbit, near 90 degrees boundary.
    {
        expectedIsOrbitRetrograde = 1;
        testKepler( inclinationIndex ) = PI / 2.0 + std::numeric_limits< double >::epsilon( );
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );

        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );
    }

    // Test value for retrograde orbit, not near boundary.
    {
        testKepler( inclinationIndex ) = 2.0;
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );

        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );
    }

    // Test value for retrograde orbit, near 180 degrees boundary.
    {
        testKepler( inclinationIndex ) = PI - std::numeric_limits< double >::epsilon( );
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );

        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
        BOOST_CHECK_EQUAL( expectedIsOrbitRetrograde, calculatedIsOrbitRetrograde );
    }

    // Check exception handling for inclinations out of range.
    bool isExceptionFound = false;
    testKepler( inclinationIndex ) = PI + 1.0;

    // Try to calculate retrogradeness.
    try
    {
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
    }
    // Catch the expected runtime error, and set the boolean flag to true.
    catch ( std::runtime_error )
    {
        isExceptionFound = true;
    }
    // Check value of flag.
    BOOST_CHECK( isExceptionFound );

    isExceptionFound = false;
    // Try to calculate retrogradeness.
    try
    {
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
    }
    // Catch the expected runtime error, and set the boolean flag to true.
    catch ( std::runtime_error )
    {
        isExceptionFound = true;
    }
    // Check value of flag.
    BOOST_CHECK( isExceptionFound );

    isExceptionFound = false;
    testKepler( inclinationIndex ) = -1.0;
    // Try to calculate retrogradeness.
    try
    {
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler );
    }
    // Catch the expected runtime error, and set the boolean flag to true.
    catch ( std::runtime_error )
    {
        isExceptionFound = true;
    }
    // Check value of flag.
    BOOST_CHECK( isExceptionFound );

    isExceptionFound = false;
    // Try to calculate retrogradeness.
    try
    {
        calculatedIsOrbitRetrograde = isOrbitRetrograde( testKepler( inclinationIndex ) );
    }
    // Catch the expected runtime error, and set the boolean flag to true.
    catch ( std::runtime_error )
    {
        isExceptionFound = true;
    }
    // Check value of flag.
    BOOST_CHECK( isExceptionFound );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


// demo to test BBW with custom FK for a cylinder mesh
// Executable bbw_cyl can be run to see the results.

#include <igl/boundary_conditions.h>
#include <igl/readMESH.h>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/bbw.h>
#include <igl/normalize_row_sums.h>
#include <igl/forward_kinematics.h>
#include <igl/directed_edge_parents.h>

#include <igl/lbs_matrix.h>
#include <igl/deform_skeleton.h>

#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <vector>
#include <algorithm>
#include <iostream>

using namespace Eigen;
using namespace std;

const Eigen::RowVector3d sea_green(70./255.,252./255.,167./255.);

Eigen::MatrixXd V,W,U,C,M;
Eigen::MatrixXi T,F,BE;
Eigen::VectorXi P;

int selected = 0;

Eigen::MatrixXd TV;
Eigen::MatrixXi TT;
Eigen::MatrixXi TF;

typedef std::vector<Eigen::Quaterniond,Eigen::aligned_allocator<Eigen::Quaterniond>> RotationList;

Eigen::Quaterniond
euler2Quaternion( const double roll,
                  const double pitch,
                  const double yaw )
{
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());

    Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
    return q;
}

double angle = 0;

bool pre_draw(igl::opengl::glfw::Viewer & viewer){

    Vector3d dist1 = (C.row(1) - C.row(0));
    Vector3d dist2 = (C.row(2) - C.row(1));
    Affine3d T0 = Affine3d::Identity();
    Affine3d T1 = Affine3d::Identity();
    Affine3d T2 = Affine3d::Identity();
    Affine3d T3 = Affine3d::Identity();

    T1.translate(dist1);
    T3.translate(dist2);

    angle += 0.1*M_PI;

    T0.rotate(euler2Quaternion(angle/2.0, 0.0, 0.0));
    T2.rotate(euler2Quaternion(angle, 0.0, 0.0));

    Affine3d T01 = Affine3d::Identity();
    T01.translate(Vector3d(C.row(1)));
    Affine3d T02 = Affine3d::Identity();
    T02.translate(Vector3d(C.row(2)));
    Affine3d T_ff1 = T0*T1;
    Affine3d T_ff2 = T0*T1*T2*T3;

    const int dim = C.cols();
    MatrixXd T(BE.rows()*(dim+1),dim);
    for(int e = 0;e<BE.rows();e++)
    {
        Affine3d a = Affine3d::Identity();
        if (e == 0){
            a = T_ff1*T01.inverse();
        }
        if (e == 1){
            a = T_ff2*T02.inverse();
        }
        T.block(e*(dim+1),0,dim+1,dim) =
            a.matrix().transpose().block(0,0,dim+1,dim);

    }
    // Compute deformation via LBS as matrix multiplication
    U = M*T;
    MatrixXd CT;
    MatrixXi BET;
    igl::deform_skeleton(C,BE,T,CT,BET);

    viewer.data().set_vertices(U);
    viewer.data().set_edges(CT,BET,sea_green);
};

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

    igl::readOFF("../data/cylinder.off",V,F);
    std::cout << "finished reading ..." << std::endl;

    U = V;

    C.resize(3,3);

    C.row(0) = V.row(4800);
    C.row(1) = V.row(2372);
    C.row(2) = V.row(4801);

    BE.resize(2,2);

    BE << 0, 1,
          1, 2;

    // List of boundary indices (aka fixed value indices into VV)
    VectorXi b;
    // List of boundary conditions of each weight function
    MatrixXd bc;
    igl::boundary_conditions(V,F,C,VectorXi(),BE,MatrixXi(),b,bc);

    std::cout << "vec" << VectorXi() << std::endl;

    igl::BBWData bbw_data;
    // only a few iterations for sake of demo
    bbw_data.active_set_params.max_iter = 8;
    bbw_data.verbosity = 2;
    if(!igl::bbw(V,F,b,bc,bbw_data,W))
    {
        return EXIT_FAILURE;
    }
    // Normalize weights to sum to one
    igl::normalize_row_sums(W,W);

    // precompute linear blend skinning matrix
    igl::lbs_matrix(V,W,M);

    //////////////////////////////////////////////////////////
    // bending the cylinder

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

    // std::cout << CT << std::endl;

    // std::cout << BET << std::endl;

    igl::opengl::glfw::Viewer viewer;
    viewer.data().set_mesh(U, F);
    viewer.data().set_data(W.col(selected));
    viewer.data().set_edges(C,BE,sea_green);
    viewer.callback_pre_draw = &pre_draw;
    viewer.data().show_lines = false;
    viewer.data().show_overlay_depth = false;
    viewer.data().line_width = 1;
    viewer.core().animation_max_fps = 30.;
    viewer.core().is_animating = true;
    viewer.launch();

    return 0;
}

// std::cout << "FK " << Quaterniond((T_ff*T0.inverse()).rotation()).vec() <<  std::endl;

    // RotationList anim_pose(2);
    // anim_pose[0] = euler2Quaternion(0, 0.0, 0.0);
    // anim_pose[1] = euler2Quaternion(0.5*M_PI, 0.0, 0.0);

    // // retrieve parents for forward kinematics
    // igl::directed_edge_parents(BE,P);
    // RotationList vQ;
    // vector<Vector3d> vT;
    // igl::forward_kinematics(C,BE,P,anim_pose,vQ,vT);

    // std::cout << "igl FK" << vQ[1].vec() << " " << std::endl;


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#define _USE_MATH_DEFINES
#include <string>
#include <vector>
#include <exception>

#include <boost/make_shared.hpp>

#include <cmath>

#include "rttbIntegration.h"
#include "rttbNTCPLKBModel.h"
#include "rttbDvhBasedModels.h"
#include "rttbInvalidParameterException.h"
#include "rttbExceptionMacros.h"

namespace rttb
{

	namespace models
	{
		NTCPLKBModel::NTCPLKBModel() : NTCPModel(){
			_name = "NTCPLKBModel";
			fillParameterMap();
		}

		NTCPLKBModel::NTCPLKBModel(DVHPointer aDvh, BioModelParamType aD50, BioModelParamType aM,
		                           BioModelParamType aA):
								   NTCPModel(aDvh, aD50), _m(aM), _a(aA) {
									   _name = "NTCPLKBModel";
									   fillParameterMap();
								   }

		void NTCPLKBModel::setA(const BioModelParamType aA)
		{
			_a = aA;
		}

		const BioModelParamType NTCPLKBModel::getA()
		{
			return _a;
		}

		void NTCPLKBModel::setM(const BioModelParamType aM)
		{
			_m = aM;
		}

		const BioModelParamType NTCPLKBModel::getM()
		{
			return _m;
		}

		void NTCPLKBModel::setParameterVector(const ParamVectorType& aParameterVector)
		{
			if (aParameterVector.size() != 3)
			{
				throw core::InvalidParameterException("Parameter invalid: aParameterVector.size must be 3! ");
			}
			else
			{
				_d50 = aParameterVector.at(0);
				_m = aParameterVector.at(1);
				_a = aParameterVector.at(2);
			}
		}

		void NTCPLKBModel::setParameterByID(const int aParamId, const BioModelParamType aValue)
		{
			if (aParamId == 0)
			{
				_d50 = aValue;
			}
			else if (aParamId == 1)
			{
				_m = aValue;
			}
			else if (aParamId == 2)
			{
				_a = aValue;
			}
			else
			{
				throw core::InvalidParameterException("Parameter invalid: aParamID must be 0(for d50) or 1(for m) or 2(for a)! ");
			}
		}

		const int NTCPLKBModel::getParameterID(const std::string& aParamName) const
		{
			if (aParamName == "d50")
			{
				return 0;
			}
			else if (aParamName == "m")
			{
				return 1;
			}
			else if (aParamName == "a")
			{
				return 2;
			}
			else
			{
				rttbExceptionMacro(core::InvalidParameterException,
				                   << "Parameter name " << aParamName << " invalid: it should be d50 or m or a!");

			}
		}

		std::map<std::string, double> NTCPLKBModel::getParameterMap() const{
			return parameterMap;
		}

		void NTCPLKBModel::fillParameterMap(){
			parameterMap["d50"] = getD50();
			parameterMap["m"] = getM();
			parameterMap["a"] = getA();
		}

		std::string NTCPLKBModel::getModelType() const{
			return _name;
		}

		BioModelValueType NTCPLKBModel::calcModel(const double doseFactor)
		{
			if (_a == 0)
			{
				throw core::InvalidParameterException("_a must not be zero");
			}

			if (_m == 0)
			{
				throw core::InvalidParameterException("_m must not be zero");
			}

			core::DVH variantDVH = core::DVH(_dvh->getDataDifferential(),
			                                 (DoseTypeGy)(_dvh->getDeltaD() * doseFactor),
			                                 _dvh->getDeltaV(), "temporary", "temporary");

			auto spDVH = boost::make_shared<core::DVH>(variantDVH);
			double eud = getEUD(spDVH, this->_a);
			//_m must not be zero
			double t = (eud - this->_d50) / (this->_m * this->_d50);
			double value = 1 / pow(2 * M_PI, 0.5);

			double result = integrateLKB(t);

			if (result != -100)
			{
				value *= result;

				return value;
			}
			else
			{
				return false;
			}
		}

	}//end namespace models
}//end namespace rttb


#include <tdp/testing/testing.h>
#include <tdp/cg/cg.h>
#include <tdp/utils/timer.hpp>
#include <Eigen/Dense>
#include <Eigen/Sparse>

TEST(CG, small) {
  size_t Nvar = 6*3;
  size_t Nobs = 100;
  Eigen::MatrixXf A = Eigen::MatrixXf::Random(Nobs,Nvar);
  Eigen::VectorXf b = Eigen::VectorXf::Random(Nobs);

  Eigen::MatrixXf ATA = A.transpose()*A;
  Eigen::VectorXf ATb = A.transpose()*b;

  Eigen::VectorXf x = ATA.ldlt().solve(ATb);
  std::cout << x.transpose() << std::endl;

  Eigen::SparseMatrix<float> Asp = A.sparseView();
  Eigen::VectorXf xCg = Eigen::VectorXf::Zero(Nvar);
  tdp::CG::ComputeCpu(Asp, b, 100, 1e-6, xCg);
  std::cout << xCg.transpose() << std::endl;

  Eigen::VectorXf xPcg = Eigen::VectorXf::Zero(Nvar);
  Eigen::VectorXf Mdiag = ATA.diagonal();
  tdp::PCG::ComputeCpu(Asp, b, Mdiag, 100, 1e-6, xPcg);
  std::cout << xPcg.transpose() << std::endl;

}

TEST(CG, large) {
  size_t Nvar = 6*100;
  size_t Nobs = 1000000;
  Eigen::MatrixXf A = Eigen::MatrixXf::Random(Nobs,Nvar);
  Eigen::VectorXf b = Eigen::VectorXf::Random(Nobs);

  tdp::Timer t0;
  Eigen::MatrixXf ATA = A.transpose()*A;
  Eigen::VectorXf ATb = A.transpose()*b;
  Eigen::VectorXf x = ATA.ldlt().solve(ATb);
  t0.toctic("ldlt solve");

  Eigen::SparseMatrix<float> Asp = A.sparseView();
  Eigen::VectorXf xCg = Eigen::VectorXf::Zero(Nvar);

  t0.tic();
  tdp::CG::ComputeCpu(Asp, b, 100, 1e-6, xCg);
  t0.toctic("cg");
  std::cout << "CG err:\t" << (x-xCg).norm() << std::endl;

  Eigen::VectorXf xPcg = Eigen::VectorXf::Zero(Nvar);
  Eigen::VectorXf Mdiag = ATA.diagonal();
  t0.tic();
  tdp::PCG::ComputeCpu(Asp, b, Mdiag, 100, 1e-6, xPcg);
  t0.toctic("pcg");
  std::cout << "PCG err:\t" << (x-xPcg).norm() << std::endl;
}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


/*
 * COPYRIGHT AND PERMISSION NOTICE
 * UCSD Software ORCVIO
 * Copyright (C) 2021
 * All rights reserved.
 */

#include <gtest/gtest.h>
#include <random_numbers/random_numbers.h>
#include <iostream>
#include <map>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/StdVector>

#include "orcvio/cam_state.h"
#include "orcvio/feature.h"

using namespace std;
using namespace Eigen;
using namespace orcvio;

// Static member variables in CAMState class
Isometry3d CAMState::T_cam0_cam1 = Isometry3d::Identity();

// Static member variables in Feature class
Feature::OptimizationConfig Feature::optimization_config;

TEST(FeatureInitializeTest, sphereDistribution) {
    // Set the real feature at the origin of the world frame.
    Vector3d feature(0.5, 0.0, 0.0);

    // Add 6 camera poses, all of which are able to see the
    // feature at the origin. For simplicity, the six camera
    // view are located at the six intersections between a
    // unit sphere and the coordinate system. And the z axes
    // of the camera frames are facing the origin.
    vector<Isometry3d> cam_poses(6);
    // Positive x axis.
    cam_poses[0].linear() << 0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, -1.0, 0.0;
    cam_poses[0].translation() << 1.0, 0.0, 0.0;
    // Positive y axis.
    cam_poses[1].linear() << -1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, -1.0, 0.0;
    cam_poses[1].translation() << 0.0, 1.0, 0.0;
    // Negative x axis.
    cam_poses[2].linear() << 0.0, 0.0, 1.0, -1.0, 0.0, 0.0, 0.0, -1.0, 0.0;
    cam_poses[2].translation() << -1.0, 0.0, 0.0;
    // Negative y axis.
    cam_poses[3].linear() << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0;
    cam_poses[3].translation() << 0.0, -1.0, 0.0;
    // Positive z axis.
    cam_poses[4].linear() << 0.0, -1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, -1.0;
    cam_poses[4].translation() << 0.0, 0.0, 1.0;
    // Negative z axis.
    cam_poses[5].linear() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
    cam_poses[5].translation() << 0.0, 0.0, -1.0;

    // Set the camera states
    CamStateServer cam_states;
    for (int i = 0; i < 6; ++i) {
        CAMState new_cam_state;
        new_cam_state.id = i;
        new_cam_state.time = static_cast<double>(i);
        new_cam_state.orientation = Matrix3d(cam_poses[i].linear());
        new_cam_state.position = cam_poses[i].translation();
        cam_states[new_cam_state.id] = new_cam_state;
    }

    // Compute measurements.
    random_numbers::RandomNumberGenerator noise_generator;
    vector<Vector4d, aligned_allocator<Vector4d> > measurements(6);
    for (int i = 0; i < 6; ++i) {
        Isometry3d cam_pose_inv = cam_poses[i].inverse();
        Vector3d p = cam_pose_inv.linear() * feature + cam_pose_inv.translation();
        double u = p(0) / p(2) + noise_generator.gaussian(0.0, 0.01);
        double v = p(1) / p(2) + noise_generator.gaussian(0.0, 0.01);
        //double u = p(0) / p(2);
        //double v = p(1) / p(2);
        measurements[i] = Vector4d(u, v, u, v);
    }

    for (int i = 0; i < 6; ++i) {
        cout << "pose " << i << ":" << endl;
        cout << "orientation: " << endl;
        cout << cam_poses[i].linear() << endl;
        cout << "translation: " << endl;
        cout << cam_poses[i].translation().transpose() << endl;
        cout << "measurement: " << endl;
        cout << measurements[i].transpose() << endl;
        cout << endl;
    }

    // Initialize a feature object.
    Feature feature_object;
    for (int i = 0; i < 6; ++i) feature_object.observations[i] = measurements[i];

    // Compute the 3d position of the feature.
    feature_object.initializePosition(cam_states);

    // Check the difference between the computed 3d
    // feature position and the groud truth.
    cout << "ground truth position: " << feature.transpose() << endl;
    cout << "estimated position: " << feature_object.position.transpose() << endl;
    Eigen::Vector3d error = feature_object.position - feature;
    EXPECT_NEAR(error.norm(), 0, 0.05);
}

int main(int argc, char** argv) {
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


#include <iostream>
#include <string>
#include <vector>
#include <cctype>
#include <sstream>
#include <memory>
#include <utility>
#include <exception>
#include <boost/lexical_cast.hpp>

/**
 * The `Interpreter` pattern provides a way to include language elements in a program
 */

struct Token {
    enum Type { integer, plus, minus, lparen, rparen };

    Type type;
    std::string text;

    explicit Token(Type t, std::string txt)
        : type{t}, text{std::move(txt)}
    {
    }

    friend std::ostream& operator<<(std::ostream& os, Token const& obj)
    {
        return os << "`" << obj.text << "`";
    }
};

std::vector<Token> lex(std::string const& input)
{
    std::vector<Token> result;
    for (int i = 0; i < input.size(); ++i) {
        switch (input[i]) {
        case '+':
            result.push_back(Token{Token::plus, "+"});
            break;
        case '-':
            result.push_back(Token{Token::minus, "-"});
            break;
        case '(':
            result.push_back(Token{Token::lparen, "("});
            break;
        case ')':
            result.push_back(Token{Token::rparen, ")"});
            break;
        default:
            // number
            std::ostringstream buffer;
            buffer << input[i];
            for (int j = i + 1; j < input.size(); ++j) {
                if (std::isdigit(input[j])) {
                    buffer << input[j];
                    ++i;
                }
                else {
                    result.push_back(Token{Token::integer, buffer.str()});
                    break;
                }
            }
        }
    }

    return result;
}

// parsing

struct Element {
    virtual ~Element() = default;
    virtual int eval() const = 0;
};

struct Integer : public Element {
    int value;
    explicit Integer(int const val)
        : value{val}
    {
    }

    int eval() const override { return value; }
};

struct BinaryOperation : public Element {
public:
    enum Type {addition, substraction};
    Type type;
    std::shared_ptr<Element> lhs, rhs;

    int eval() const override
    {
        if (type == addition) {
            return lhs->eval() + rhs->eval();
        }
        return lhs->eval() - rhs->eval();
    }
};

std::shared_ptr<Element> parse(std::vector<Token> const& tokens)
{
    auto result = std::make_unique<BinaryOperation>();
    bool have_lhs = false;
    for (std::size_t i = 0; i < tokens.size(); ++i) {
        auto token = tokens[i];
        switch (token.type) {
        case Token::integer:
        {
            int value = boost::lexical_cast<int>(token.text);
            auto integer = std::make_shared<Integer>(value);
            if (!have_lhs) {
                result->lhs = integer;
                have_lhs = true;
            }
            else {
                result->rhs = integer;
            }
            break;
        }
        case Token::plus:
            result->type = BinaryOperation::addition;
            break;
        case Token::minus:
            result->type = BinaryOperation::substraction;
            break;
        case Token::lparen:
        {
            int j = i;
            for ( ; j < tokens.size(); ++j) {
                if (tokens[j].type == Token::rparen) {
                    break;
                }
            }
            std::vector<Token> subexpression(&tokens[i+1], &tokens[j]);
            auto element = parse(subexpression);
            if (!have_lhs) {
                result->lhs = element;
                have_lhs = true;
            }
            else {
                result->rhs = element;
                i = j;  // advance
            }
            break;
        }
        } // switch
        return result;
    }
}

int main()
{
    std::string input{ "(13-4)-(12+1)" }; // see if you can make nested braces work
    auto tokens = lex(input);

    // let's see the tokens
    for (auto& t : tokens)
        std::cout << t << "   ";
    std::cout << std::endl;

    try {
        auto parsed = parse(tokens);
        std::cout << input << " = " << parsed->eval() << std::endl;
    }
    catch (const std::exception& e)
    {
        std::cout << e.what() << std::endl;
    }
}


//! [mandelbrot-all]
#include <chrono>
#include <iostream>
#include <numeric>
#include <vector>

#include <boost/simd/memory/allocator.hpp>
#include <boost/simd/pack.hpp>

#include <boost/simd/function/aligned_load.hpp>
#include <boost/simd/function/aligned_store.hpp>
#include <boost/simd/function/enumerate.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/if_inc.hpp>
#include <boost/simd/function/sqr.hpp>

// if you want to see the julia set
// uncomment the two include lines and the two other commented areas with the
// same front line as this one
// this suppose you have opencv installed and the proper libraries on your
// compilation command
// opencv_core opencv_imgproc opencv_highgui
// #include <opencv2/contrib/contrib.hpp>
// #include <opencv2/highgui/highgui.hpp>
// #include <opencv2/imgproc/imgproc.hpp>

typedef float T;
namespace bs = boost::simd;
using pack_t = bs::pack<T>;
using pack_i = bs::pack<int>;
using pack_l = bs::as_logical_t<pack_t>;

struct mandelbrot
{
  int size, max_iter;
  T x_min, x_max, y_min, y_max, x_range, y_range;
  std::vector<int, bs::allocator<int>> iterations;

  mandelbrot(int size_, int max_iter_)
    : size(size_)
    , max_iter(max_iter_)
  {
    x_min   = -2.5;
    x_max   = 1;
    y_min   = -1;
    y_max   = 1;
    x_range = x_max - x_min;
    y_range = y_max - y_min;
    iterations.resize(size * size);
  }

  //! [mandelbrot-scalar]
  void evaluate_scalar()
  {
    for (int i = 0; i < size; ++i) {
      T x0 = T(i) / T(size) * x_range + x_min;
      for (int j = 0; j < size; ++j) {
        int iteration = 0;
        T y0          = T(j) / T(size) * y_range + y_min;
        T x           = 0;
        T y           = 0;
        T x2          = x * x;
        T y2          = y * y;
        while (x2 + y2 < 4 && iteration < max_iter) {
          x2 = x * x;
          y2 = y * y;
          T x_temp = x2 - y2 + x0;
          y        = 2 * x * y + y0;
          x        = x_temp;
          ++iteration;
        }
        iterations[j + i * size] = iteration;
      }
    }
  }
  //! [mandelbrot-scalar]

  //! [mandelbrot-simd]
  void evaluate_simd()
  {
    pack_t step =
      bs::enumerate<pack_t>(0); // produce a vector containing {0, 1, ..., pack_t::static_size-1}
    for (int i = 0; i < size; ++i) {
      pack_t x0{T(i) / T(size) * x_range + x_min};
      pack_t fac{y_range / T(size)};
      pack_t y_min_t{y_min};
      for (int j = 0; j < size; j += pack_t::static_size) {
        int iteration = 0;

        pack_t y0 = bs::fma(step + j, fac, y_min_t);
        pack_t x{0};
        pack_t y{0};
        pack_i iter{0};
        pack_l mask;
        do {
          pack_t x2 = bs::sqr(x);
          pack_t y2 = bs::sqr(y);

          y    = bs::fma(x + x, y, y0);
          x    = x2 - y2 + x0;
          mask = x2 + y2 < 4;
          ++iteration;
          iter = bs::if_inc(mask, iter);
        } while (bs::any(mask) && iteration < max_iter);
        bs::aligned_store(iter, &iterations[j + i * size]);
      }
    }
  }
  //! [mandelbrot-simd]

  // if you want to see the julia set
  //    void display() {
  //      cv::Mat display;
  //      cv::Mat A(size, size, CV_32SC1, iterations.data());
  //      A.convertTo(display, CV_8UC1, 255.0 / 1000.0);
  //      cv::applyColorMap(display, display, cv::COLORMAP_JET);
  //      cv::imshow("Mandelbrot", display);
  //      cv::waitKey(0);
  //    }
};

int main(int argc, char** argv)
{
  namespace chr = std::chrono;
  using hrc     = chr::high_resolution_clock;

  int size          = 1024;
  int max_iteration = 1000;
  mandelbrot image(size, max_iteration);
  auto t0 = hrc::now();
  image.evaluate_scalar();
  auto t1 = hrc::now();
  std::cout << " scalar " << chr::duration_cast<chr::milliseconds>(t1 - t0).count() << std::endl;

  t0 = hrc::now();
  image.evaluate_simd();
  t1 = hrc::now();
  std::cout << " simd " << chr::duration_cast<chr::milliseconds>(t1 - t0).count() << std::endl;
  // if you want to see the julia set
  //  image.display();
}
//! [mandelbrot-all]

// This code can be compiled using
//    g++ mandelbrot.cpp -msse4.2 -std=c++11 -O3 -DNDEBUG -o mandelbrot
//    -I/pathto/boost_simd//include -I/pathto/boost
// or if you uncomment the opencv related lines to see the generated julia set
// and use:
//    g++ mandelbrot.cpp -msse4.2 -std=c++11 -O3 -DNDEBUG -o mandelbrot
//    -I/pathto/boost_simd//include -I/pathto/boost -lopencv_core
//    -lopencv_imgproc -lopencv_highgui -lopencv_contrib


#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PointStamped.h>
#include <Eigen/Dense>

//TODO: this might be better off in a util class / module

#define D2R(x) ((M_PI*x)/180.0)

void geo2ecef(Eigen::Vector3d & positionECEF, double latitude, double longitude, double ellipsoidalHeight) {
	/**WGS84 ellipsoid semi-major axis*/
	double a = 6378137.0;

	/**WGS84 ellipsoid first eccentricity squared*/
	double e2 = 0.081819190842622 * 0.081819190842622;

	double slat = sin(D2R(latitude));
	double clat = cos(D2R(latitude));
	double slon = sin(D2R(longitude));
	double clon = cos(D2R(longitude));

	double N = a / (sqrt(1 - e2 * slat * slat));
	double xTRF = (N + ellipsoidalHeight) * clat * clon;
	double yTRF = (N + ellipsoidalHeight) * clat * slon;
	double zTRF = (N * (1 - e2) + ellipsoidalHeight) * slat;

	positionECEF << xTRF, yTRF, zTRF;
};

class GeoreferenceProvider{
public:
	GeoreferenceProvider(): ecefLocalOrigin(0,0,0){
                odomPublisher=n.advertise<nav_msgs::Odometry>("odom", 50);

		depthRepublisher = n.advertise<geometry_msgs::PointStamped>("depth_ned",50);

                gpsSubscriber = n.subscribe("fix", 50, &GeoreferenceProvider::processGpsCallback,this);
                //attitudeSubscriber = n.subscribe("/imu/pos_ecef", 50, &GeoreferenceProvider::processPositionCallback,this);
		attitudeSubscriber = n.subscribe("/imu/data", 50, &GeoreferenceProvider::processAttitudeCallback,this);
		lidarSubscriber = n.subscribe("/velodyne_points",100,&GeoreferenceProvider::processLidarCallback,this);
		depthSubscriber = n.subscribe("/depth",100,&GeoreferenceProvider::processDepthCallback,this);

	}

	void processGpsCallback(const sensor_msgs::NavSatFix & msg){

		//Init ENU frame origin once we receive a first fix
		if(!gpsFixObtained && msg.status.status >= 0){
			gpsFixObtained = true;
			latitudeLocalOrigin = msg.latitude;
			longitudeLocalOrigin = msg.longitude;
			altitudeLocalOrigin = msg.altitude;

			ROS_INFO("GPS Fix obtained. Initializing NED transform at (%f,%f,%f)",latitudeLocalOrigin,longitudeLocalOrigin,altitudeLocalOrigin);

			geo2ecef(ecefLocalOrigin,latitudeLocalOrigin,longitudeLocalOrigin,altitudeLocalOrigin);

			ROS_INFO_STREAM("Local Origin:" << ecefLocalOrigin);

			//ENU
			//ecef2local << 	-sin(D2R(longitudeLocalOrigin)),	cos(D2R(longitudeLocalOrigin)),	0,
			//		-sin(D2R(latitudeLocalOrigin))*cos(D2R(longitudeLocalOrigin)), -sin(D2R(latitudeLocalOrigin))*sin(D2R(longitudeLocalOrigin)), cos(D2R(latitudeLocalOrigin)),
			//		cos(D2R(latitudeLocalOrigin))*cos(D2R(longitudeLocalOrigin)),cos(D2R(latitudeLocalOrigin))*sin(D2R(longitudeLocalOrigin)),sin(D2R(latitudeLocalOrigin));

			//NED
			ecef2local <<	-sin(D2R(latitudeLocalOrigin)) * cos(D2R(longitudeLocalOrigin)), -sin(D2R(latitudeLocalOrigin))*sin(D2R(longitudeLocalOrigin)),cos(D2R(latitudeLocalOrigin)),
				   	-sin(D2R(longitudeLocalOrigin)) , cos(D2R(longitudeLocalOrigin)), 0,
					-cos(D2R(latitudeLocalOrigin))*cos(D2R(longitudeLocalOrigin)), -cos(D2R(latitudeLocalOrigin))*sin(D2R(longitudeLocalOrigin)), -sin(D2R(latitudeLocalOrigin)) ;

			//ROS_INFO_STREAM("DCM:" << ecef2local);
		}
		else if(gpsFixObtained){
			processPosition(msg);
		}
	}

	void processAttitudeCallback(const sensor_msgs::Imu & msg){
		lastAttitude = msg;
	}

	void processLidarCallback(const sensor_msgs::PointCloud2 & msg){
		//std::cout << "Got lidar data" << std::endl;
	}

	void processDepthCallback(const geometry_msgs::PointStamped & msg){
		//FIXME: this is solved in the imagenex driver but not in the rosbag so we need to republish to NED frame
		geometry_msgs::PointStamped newMsg = msg;
		newMsg.point.z=-1 * msg.point.z;
		depthRepublisher.publish(newMsg);
	}

	/* Publish the odometry transform and Odometry message */
	void publishOdometry(Eigen::Vector3d & prpPointLocal){

		/* Broadcast the base_link to map transform */
                geometry_msgs::TransformStamped map_transform;
                map_transform.header.stamp = ros::Time::now(); //msg.header.stamp;
                map_transform.header.frame_id = "map"; //FIXME: we are assuming that the prp is the base_link origin
                map_transform.child_frame_id = "base_link";

                map_transform.transform.translation.x = prpPointLocal[0];
                map_transform.transform.translation.y = prpPointLocal[1];
                map_transform.transform.translation.z = prpPointLocal[2];
                map_transform.transform.rotation = lastAttitude.orientation;

                transformBroadcaster.sendTransform(map_transform);

                /* Publish the odometry message over ROS */
                nav_msgs::Odometry odom;
                odom.header.stamp = ros::Time::now();//msg.header.stamp;
                odom.header.frame_id = "odom";

                //set the position
                odom.pose.pose.position.x = prpPointLocal[0];
                odom.pose.pose.position.y = prpPointLocal[1];
                odom.pose.pose.position.z = prpPointLocal[2];

		//TODO: compensate attitude value using delta t, since our last attitude value dates from a little while back
                odom.pose.pose.orientation = lastAttitude.orientation;

                //set the velocity
                odom.child_frame_id = "base_link";
                //odom.twist.twist.linear.x = vx;
                //odom.twist.twist.linear.y = vy;
                //odom.twist.twist.angular.z = vth;

                //publish the message
                odomPublisher.publish(odom);
	}

	void processPosition(const sensor_msgs::NavSatFix & msg){
		if(gpsFixObtained){
			lastPosition = msg;

			//transform to ENU
			Eigen::Vector3d pointEcef;

			geo2ecef(pointEcef,msg.latitude,msg.longitude,msg.altitude);

			Eigen::Vector3d pointLocal = ecef2local * (pointEcef - ecefLocalOrigin);

			//Update odometry
			publishOdometry(pointLocal);
		}
	}

	void run(){
		ros::Rate r(10.0);

		//just spin...
		while(n.ok()){
			ros::spinOnce();
			r.sleep();
		}
	}

private:
	ros::NodeHandle n;
        ros::Publisher  odomPublisher;

	ros::Publisher  depthRepublisher;//FIXME: delete this

        ros::Subscriber gpsSubscriber;

        ros::Subscriber positionSubscriber;
	ros::Subscriber attitudeSubscriber;
	ros::Subscriber lidarSubscriber;
	ros::Subscriber depthSubscriber;

        tf2_ros::TransformBroadcaster transformBroadcaster;

	//These are used to georeference
	sensor_msgs::Imu 	lastAttitude;
	sensor_msgs::NavSatFix  lastPosition;

	bool gpsFixObtained = false;

	//This DCM is used to transform ECEF coordinates into a local NED frame centered at ecefEnuOrigin
	Eigen::Matrix3d	ecef2local;

	//The NED origin is set as the first position when a proper GPS fix is obtained
	Eigen::Vector3d ecefLocalOrigin;

	//NED origin in geodesic coordinates
	double latitudeLocalOrigin;
	double longitudeLocalOrigin;
	double altitudeLocalOrigin;
};

int main(int argc, char** argv){
  ros::init(argc, argv, "odometry_publisher");

  GeoreferenceProvider georefProvider;

  georefProvider.run();
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Array3d v(0, sqrt(2.)/2, 1);
cout << v.acos() << endl;

  return 0;
}


/*
 * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy
 * Copyright 2018 Pasquale Oppido, University of Sannio in Benevento, Italy
 * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

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

#include "bebop_simulator/transform_datatypes.h"
#include "bebop_simulator/Matrix3x3.h"
#include "bebop_simulator/Quaternion.h"

#include <math.h>
#include <ros/ros.h>
#include <ros/console.h>
#include <Eigen/Eigen>
#include <stdio.h>
#include <boost/bind.hpp>

#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseStamped.h>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <mav_msgs/eigen_mav_msgs.h>
#include <nav_msgs/Odometry.h>

#define M_PI    3.14159265358979323846  /* pi */

ros::Publisher rpy_publisher;
ros::Subscriber quat_subscriber;

void MsgCallback(const nav_msgs::Odometry odometry_msg)
{
    // the incoming geometry_msgs::PoseStamped is transformed to a tf::Quaterion
    mav_msgs::EigenOdometry odometry;
    eigenOdometryFromMsg(odometry_msg, &odometry);
    tf::Quaternion q(odometry.orientation_W_B.x(), odometry.orientation_W_B.y(), odometry.orientation_W_B.z(),
                     odometry.orientation_W_B.w());
    tf::Matrix3x3 m(q);

    // the tf::Quaternion has a method to access roll pitch and yaw
    double roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);

    double yaw_degrees = yaw * 180.0 / M_PI; // conversion to degrees
    if( yaw_degrees < 0 ) yaw_degrees += 360.0; // convert negative to positive angles

    double roll_degrees = roll * 180.0 / M_PI; // conversion to degrees
    if( roll_degrees < 0 ) roll_degrees += 360.0; // convert negative to positive angles

    double pitch_degrees = pitch * 180.0 / M_PI; // conversion to degrees
    if( pitch_degrees < 0 ) pitch_degrees += 360.0; // convert negative to positive angles

    // the found angles are written in a geometry_msgs::Vector3
    geometry_msgs::Vector3 rpy;
    rpy.x = roll_degrees;
    rpy.y = pitch_degrees;
    rpy.z = yaw_degrees;

    // this Vector is then published:
    rpy_publisher.publish(rpy);
    ROS_DEBUG("published rpy angles: roll=%f pitch=%f yaw=%f", rpy.x, rpy.y, rpy.z);
}

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

    ros::init(argc, argv, "quaternion_to_rpy");

    ros::NodeHandle n;

    rpy_publisher = n.advertise<geometry_msgs::Vector3>("orientation_rpy", 1);

    quat_subscriber = n.subscribe(mav_msgs::default_topics::ODOMETRY, 1, MsgCallback);

    // check for incoming quaternions until ctrl+c is pressed
    ROS_DEBUG("waiting for quaternion");

    ros::spin();

    return 0;
}


/*
 * This file is part of the Visual Computing Library (VCL) release under the
 * MIT license.
 *
 * Copyright (c) 2014 Basil Fierz
 *
 * 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.
 */

// VCL configuration
#include <vcl/config/global.h>

// C++ standard library
#include <fstream>
#include <iostream>
#include <random>

// Eigen library
#include <Eigen/Dense>

// VCL
#include <vcl/core/simd/vectorscalar.h>
#include <vcl/core/interleavedarray.h>
#include <vcl/math/jacobieigen33_selfadjoint.h>
#include <vcl/math/jacobieigen33_selfadjoint_quat.h>
#include <vcl/util/precisetimer.h>

template<typename Scalar>
Vcl::Core::InterleavedArray<Scalar, 3, 3, -1> createProblems(size_t nr_problems)
{
	// Random number generator
	std::mt19937_64 rng;
	std::uniform_real_distribution<float> d;

	Vcl::Core::InterleavedArray<Scalar, 3, 3, -1> F(nr_problems);

	// Initialize data
	for (int i = 0; i < (int) nr_problems; i++)
	{
		Eigen::Matrix<Scalar, 3, 3> rnd;
		rnd << d(rng), d(rng), d(rng),
			   d(rng), d(rng), d(rng),
			   d(rng), d(rng), d(rng);
		F.template at<Scalar>(i) = rnd.transpose() * rnd;
	}

	return std::move(F);
}

template<typename Scalar>
void computeReferenceSolution
(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<Scalar, 3, 3, -1>& ATA,
	Vcl::Core::InterleavedArray<Scalar, 3, 3, -1>& U,
	Vcl::Core::InterleavedArray<Scalar, 3, 1, -1>& S
)
{
	// Compute reference using Eigen
	for (int i = 0; i < static_cast<int>(nr_problems); i++)
	{
		Vcl::Matrix3f A = ATA.template at<Scalar>(i);

		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
		solver.compute(A, Eigen::ComputeEigenvectors);

		U.template at<Scalar>(i) = solver.eigenvectors();
		S.template at<Scalar>(i) = solver.eigenvalues();
	}
}

template<typename WideScalar>
void jacobiEig
(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& resU,
	Vcl::Core::InterleavedArray<float, 3, 1, -1>& resS
)
{
	using real_t = WideScalar;
	using matrix3_t = Eigen::Matrix<real_t, 3, 3>;

	size_t width = sizeof(real_t) / sizeof(float);

	int avg_nr_iter = 0;
	for (size_t i = 0; i < nr_problems / width; i++)
	{
		// Map data
		auto U = resU.at<real_t>(i);
		auto S = resS.at<real_t>(i);

		// Compute SVD using 2-sided Jacobi iterations (Brent)
		matrix3_t SV = F.at<real_t>(i);
		matrix3_t matU = matrix3_t::Identity();

		avg_nr_iter += Vcl::Mathematics::SelfAdjointJacobiEigen(SV, matU);

		// Store results
		U = matU;
		S = SV.diagonal();
	}
}

template<typename WideScalar>
void jacobiEigQuat
(
	size_t nr_problems,
	const Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& resU,
	Vcl::Core::InterleavedArray<float, 3, 1, -1>& resS
)
{
	using real_t = WideScalar;
	using matrix3_t = Eigen::Matrix<real_t, 3, 3>;

	size_t width = sizeof(real_t) / sizeof(float);

	int avg_nr_iter = 0;
	for (size_t i = 0; i < nr_problems / width; i++)
	{
		// Map data
		auto U = resU.at<real_t>(i);
		auto S = resS.at<real_t>(i);

		// Compute SVD using Jacobi iterations and QR decomposition
		matrix3_t SV = F.at<real_t>(i);
		matrix3_t matU = matrix3_t::Identity();

		avg_nr_iter += Vcl::Mathematics::SelfAdjointJacobiEigenQuat(SV, matU);

		// Store results
		U = matU;
		S = SV.diagonal();
	}
}

template<typename REAL>
void SortEigenvalues(Eigen::Matrix<REAL, 3, 1>& A, Eigen::Matrix<REAL, 3, 3>& B)
{
	// Bubble sort
	bool swapped = true;
	int j = 0;

	while (swapped)
	{
		swapped = false;
		j++;

		for (int i = 0; i < 3 - j; i++)
		{
			if (A(i) > A(i + 1))
			{
				std::swap(A(i), A(i + 1));
				B.col(i).swap(B.col(i + 1));

				swapped = true;
			}
		}
	}
}

template<typename Scalar>
void checkSolution
(
	const char* Name,
	const char* file,
	size_t nr_problems,
	Scalar tol,
	const Vcl::Core::InterleavedArray<Scalar, 3, 3, -1>& refUa,
	const Vcl::Core::InterleavedArray<Scalar, 3, 1, -1>& refSa,
	const Vcl::Core::InterleavedArray<Scalar, 3, 3, -1>& resUa,
	const Vcl::Core::InterleavedArray<Scalar, 3, 1, -1>& resSa
)
{
	using scalar_t = Scalar;

	int wrong_computations, wrong_u_computations;
	scalar_t accum_error, accum_u_error;
	std::ofstream fout;

	wrong_computations = 0;
	wrong_u_computations = 0;
	accum_error = 0;
	accum_u_error = 0;
	fout.open(file);

	for (int j = 0; j < (int) nr_problems; j++)
	{
		Vcl::Matrix3f refU = refUa.template at<scalar_t>(j);
		Vcl::Vector3f refS = refSa.template at<scalar_t>(j);
		Vcl::Matrix3f cU = resUa.template at<scalar_t>(j);
		Vcl::Vector3f cS = resSa.template at<scalar_t>(j);

		SortEigenvalues(cS, cU);

		bool eqU = refU.array().abs().isApprox(cU.array().abs(), tol);
		bool eqS = refS.array().abs().isApprox(cS.array().abs(), tol);

		if (!eqS || !eqU)
			fout << j;

		if (!eqS)
		{
			wrong_computations++;
			scalar_t err = abs((refS.array().abs() - cS.array().abs()).sum() / scalar_t(3));
			accum_error += err;
			fout << ", E: " << err;
		}
		if (!eqU)
		{
			wrong_u_computations++;
			scalar_t err = abs((refU.array().abs() - cU.array().abs()).sum() / scalar_t(9));
			accum_u_error += err;
			fout << ", U: " << err;
		}
		if (!eqS || !eqU)
			fout << std::endl;
	}

	fout.close();
	std::cout << Name << " - Errors: (" << wrong_computations << ", " << wrong_u_computations << "), "
			  << "Avg. Singular value error: " << accum_error / std::max(wrong_computations, 1) << ", "
			  << "Avg. U error: " << accum_u_error / std::max(wrong_u_computations, 1) << std::endl;
}

int main(int, char**)
{
	size_t nr_problems = 1024*1024;

	using scalar_t = float;

	Vcl::Core::InterleavedArray<scalar_t, 3, 3, -1> resU(nr_problems);
	Vcl::Core::InterleavedArray<scalar_t, 3, 1, -1> resS(nr_problems);

	Vcl::Core::InterleavedArray<scalar_t, 3, 3, -1> refU(nr_problems);
	Vcl::Core::InterleavedArray<scalar_t, 3, 1, -1> refS(nr_problems);

	auto F = createProblems<scalar_t>(nr_problems);
	computeReferenceSolution(nr_problems, F, refU, refS);

	// Test correctness: Two-sided Jacobi SVD (Brent)
	jacobiEig<float>(nr_problems, F, resU, resS);        checkSolution("JacobiEigen - float",   "jacobi_eigen_float_errors.txt",   nr_problems, 1e-5f, refU, refS, resU, resS);
	jacobiEig<Vcl::float4>(nr_problems, F, resU, resS);  checkSolution("JacobiEigen - float4",  "jacobi_eigen_float4_errors.txt",  nr_problems, 1e-5f, refU, refS, resU, resS);
	jacobiEig<Vcl::float8>(nr_problems, F, resU, resS);  checkSolution("JacobiEigen - float8",  "jacobi_eigen_float8_errors.txt",  nr_problems, 1e-5f, refU, refS, resU, resS);
	jacobiEig<Vcl::float16>(nr_problems, F, resU, resS); checkSolution("JacobiEigen - float16", "jacobi_eigen_float16_errors.txt", nr_problems, 1e-5f, refU, refS, resU, resS);

	// Test correctness: Jacobi SVD with symmetric EV computation and QR decomposition
	jacobiEigQuat<float>(nr_problems, F, resU, resS);        checkSolution("JacobiEigenQuat - float",   "jacobi_eigen_quat_float_errors.txt",   nr_problems, 1e-5f, refU, refS, resU, resS);
	jacobiEigQuat<Vcl::float8>(nr_problems, F, resU, resS);	 checkSolution("JacobiEigenQuat - float8",  "jacobi_eigen_quat_float8_errors.txt",  nr_problems, 1e-5f, refU, refS, resU, resS);
	jacobiEigQuat<Vcl::float4>(nr_problems, F, resU, resS);	 checkSolution("JacobiEigenQuat - float4",  "jacobi_eigen_quat_float4_errors.txt",  nr_problems, 1e-5f, refU, refS, resU, resS);
	jacobiEigQuat<Vcl::float16>(nr_problems, F, resU, resS); checkSolution("JacobiEigenQuat - float16", "jacobi_eigen_quat_float16_errors.txt", nr_problems, 1e-5f, refU, refS, resU, resS);
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_TWO_ADD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_TWO_ADD_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing two_add capabilities

    For any two reals @c x and @c y two_add computes two reals (in an std::pair)
    @c r0 and @c r1 such that:

    @code
    r0 = x + y
    r1 = r0 -(x + y)
    @endcode

    using perfect arithmetic.

    Its main usage is to be able to compute
    sum of reals and the residual error using IEEE  754 arithmetic.

  **/
  std::pair<Value, Value> two_add(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/two_add.hpp>
#include <boost/simd/function/simd/two_add.hpp>

#endif


#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <fstream>
#include <boost/format.hpp> // for formatting strings

using namespace cv;
using namespace std;

int main(int argc, char **argv)
{
    ifstream fin("calibdata.txt"); /* 标定所用图像文件的路径 */
	ofstream fout("caliberation_result.txt");  /* 保存标定结果的文件 */
    //读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化
	int image_count=0;  /* 图像数量 */
	int valid_image_count = 0; // 能成功提取出角点的图像数量
	Size image_size;  /* 图像的尺寸 */
	Size board_size = Size(9,6);    /* 标定板上每行、列的角点数 */
	vector<Point2f> image_points_buf;  /* 缓存每幅图像上检测到的角点 */
	vector<vector<Point2f>> image_points_seq; // 保存检测到的所有角点
	string filename;
	int count= -1 ;//用于存储角点个数。

    while (getline(fin, filename))
    {
		image_count++;
        cv::Mat rawImageInput = cv::imread(filename);
        cv::Mat imageInput = rawImageInput;
        if (image_count == 1)
        {
            image_size.width = imageInput.cols;
            image_size.height = imageInput.rows;
            cout << "image_size = " <<  image_size << endl;
			cout << "开始提取角点………………" << endl;
        }

        // 用于观察检验输出
		cout<<"image_count = "<<image_count<<endl;

        boost::format fmt("%s %d");
        // cv::imshow((fmt % "imageInput" % image_count).str(), imageInput);
        // cv::waitKey(0);

        if (0 == findChessboardCorners(imageInput,board_size,image_points_buf))
		{
			cout<<"can not find chessboard corners!\n"; //找不到角点
			// exit(1);
		}
		else
		{
			valid_image_count++;
			Mat view_gray;
			cvtColor(imageInput,view_gray,CV_RGB2GRAY);
			/* 亚像素精确化 */
			//find4QuadCornerSubpix(view_gray,image_points_buf,Size(5,5)); //对粗提取的角点进行精确化
			cornerSubPix(view_gray,image_points_buf,Size(11,11),Size(-1,-1),TermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,0.1));

			image_points_seq.push_back(image_points_buf);  //保存亚像素角点
			/* 在图像上显示角点位置 */
			drawChessboardCorners(view_gray,board_size,image_points_buf,false); //用于在图片中标记角点
			cv::imshow((fmt % "Camera Calibration" % image_count).str(),view_gray);//显示图片
		}
    }
    cv::waitKey(500);
	cout << "角点提取完成，" << "有效图片数=" << valid_image_count << endl << endl;

	// 摄像机标定
	cout << "开始标定......" << endl;
	Size square_size = Size(50, 50); // 实际测量得到的标定板上每个棋盘格的大小
	vector<vector<Point3f>> objectPoints;  // 保存标定板上角点的三维坐标

	// vector<vector<Point3f>> objectPoints(1);
    // calcBoardCornerPositions(board_size, square_size, objectPoints[0]);

    // objectPoints.resize(image_points_seq.size(), objectPoints[0]);

	Mat cameraMatrix= Mat::eye(3, 3, CV_64F); // 定义相机内参矩阵
	vector<int> point_counts;  // 每幅图像中角点的数量
	Mat distCoeffs = Mat::zeros(1, 5, CV_64F);  // 摄像机的五个畸变系数: k1 k2 p1 p2 k2
	vector<Mat> tvecsMat;  //每幅图像的评议向量
	vector<Mat> rvecsMat;  //每幅图像的旋转向量
	// 计算角点的真实坐标（假定在z=0平面上）
	for (int n=0; n < valid_image_count; n++)
	{
		vector<Point3f> tempPointSet;
		for (int u=0; u < board_size.height; ++u)
		{
			for (int v=0; v < board_size.width; ++v)
			{
				Point3f realPoint;
				realPoint.x = v * square_size.width;
				realPoint.y = u * square_size.height;
				realPoint.z = 0;
				tempPointSet.push_back(realPoint);
			}
		}
		// cout << "真实角点：" << endl << tempPointSet << endl;
		objectPoints.push_back(tempPointSet);
	}
	// 标定
	int flags = 0;
	flags = CV_CALIB_FIX_PRINCIPAL_POINT | CV_CALIB_FIX_ASPECT_RATIO;
	calibrateCamera(objectPoints, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, flags);
	cout << "标定完成!" << endl << endl;

	// 对标定结果进行评价
	cout << "开始评价标定结果......" << endl;
	vector<Point2f> image_points_new;

	for(int i=0; i < image_count; i++)
	{
		vector<Point3f> tempPointSet = objectPoints[i];
		projectPoints(objectPoints[i], rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points_new);

	}

    return 0;
}


//
// Created by krab1k on 27.08.20.
//

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

#include "sqeq0.h"
#include "../parameters.h"
#include "../geometry.h"

CHARGEFW2_METHOD(SQEq0)

std::vector<double> SQEq0::calculate_charges(const Molecule &molecule) const {

    size_t n = molecule.atoms().size();
    size_t m = molecule.bonds().size();

    Eigen::VectorXd q0 = Eigen::VectorXd::Zero(n);
    Eigen::VectorXd hardness = Eigen::VectorXd::Zero(n);

    for (size_t i = 0; i < n; i++) {
        const auto &atom = molecule.atoms()[i];
        q0(i) = atom.formal_charge();
        hardness(i) = parameters_->atom()->parameter(atom::hardness)(atom);
    }

    Eigen::MatrixXd T = Eigen::MatrixXd::Zero(m, n);
    for (size_t i = 0; i < molecule.bonds().size(); i++) {
        const auto &bond = molecule.bonds()[i];
        auto i1 = bond.first().index();
        auto i2 = bond.second().index();
        T(i, i1) = 1;
        T(i, i2) = -1;
    }

    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n, n);
    Eigen::VectorXd b = Eigen::VectorXd::Zero(n);

    /* Setup EEM part */
    for (size_t i = 0; i < n; i++) {
        const auto &atom_i = molecule.atoms()[i];
        A(i, i) = hardness(i);
        b(i) = -parameters_->atom()->parameter(atom::electronegativity)(atom_i);
        for (size_t j = i + 1; j < n; j++) {
            const auto &atom_j = molecule.atoms()[j];
            auto d = distance(atom_i, atom_j);
            auto wi = parameters_->atom()->parameter(atom::width)(atom_i);
            auto wj = parameters_->atom()->parameter(atom::width)(atom_j);
            auto d0 = sqrt(2 * wi * wi + 2 * wj * wj);
            auto x = erf(d / d0) / d;
            A(i, j) = x;
            A(j, i) = x;
        }
    }

    b = b - A * q0;
    b = b + hardness.cwiseProduct(q0);

    Eigen::MatrixXd split_A = T * A * T.transpose();
    Eigen::VectorXd split_b = T * b;

    for (size_t i = 0; i < molecule.bonds().size(); i++) {
        const auto &bond = molecule.bonds()[i];
        split_A(i, i) += parameters_->bond()->parameter(bond::kappa)(bond);
    }

    Eigen::VectorXd split_q = split_A.partialPivLu().solve(split_b);
    Eigen::VectorXd q = T.transpose() * split_q + q0;

    return std::vector<double>(q.data(), q.data() + n);
}


//
// Created by denn nevera on 2019-09-03.
//

#pragma once

#include <armadillo>

namespace dehancer{

        namespace math {

            using float2x2 = arma::fmat::fixed<2, 2>;
            using float3x3 = arma::fmat::fixed<3, 3>;
            using float4x4 = arma::fmat::fixed<4, 4>;
        }
}


#include "localizationkf.h"
#include "Storage/logController.h"

#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <Eigen/Sparse>

#include <math.h>

LocalizationKF::LocalizationKF(LogController& logcontroller):
_logcontroller(logcontroller)
{};

void LocalizationKF::setup(){
    //this current intializaiton method means the filter will take time to converge to the solution
    X = Eigen::Vector<float,6>::Zero();
    P = Eigen::Matrix<float,6,6>::Identity(); // initialize as identity matrix for ease -> can update this later
};

void LocalizationKF::predict(const Eigen::Vector3f& linear_acceleration,float dt){
    const Eigen::Matrix<float,6,6> A{{1, dt, 0, 0, 0, 0},
                                     {0, 1, 0, 0, 0, 0},
                                     {0, 0, 1, dt, 0, 0},
                                     {0, 0, 0, 1, 0, 0},
                                     {0, 0, 0, 0, 1, dt},
                                     {0, 0, 0, 0, 0, 1}}; // construct state transition sub matrix

    const float dt2 = dt*dt;//temporary for readability
    const float dt3 = dt2*dt;
    const Eigen::Matrix<float,6,3> B{{dt2/2, 0, 0},
                                     {dt, 0, 0},
                                     {0, dt2/2, 0},
                                     {0, dt, 0},
                                     {0, 0, dt2/2},
                                     {0, 0,dt}}; // construct control sub matrix

    //dt^4 terms are considered negligible so have been estimated as zero
    const Eigen::Matrix<float,6,6> Q = accelVariance * Eigen::Matrix<float,6,6> {{0, dt3/2, 0, 0, 0, 0},
                                                                                {dt3/2, dt2, 0, 0, 0, 0},
                                                                                {0, 0, 0, dt3/2, 0, 0},
                                                                                {0, 0, dt3/2, dt2, 0, 0},
                                                                                {0, 0, 0, 0, 0, dt3/2},
                                                                                {0, 0, 0, 0, dt3/2, dt2}}; // construct state transition sub matrix

    //update State
    X = (A*X) + (B*linear_acceleration);
    //update Covariance estimate
    P = A*P*A.transpose() + Q;
}

void LocalizationKF::gpsUpdate(const float lat, const float lng, const long alt,const long vn, const long ve, const long vd){
    //measurement matrix
    Eigen::Vector3f positionNED = GPStoNED(lat,lng,alt);
    const Eigen::Vector<float,6> z{{positionNED(0),
                                    ((float)vn)/1000.0f, //conversion to m/s
                                    positionNED(1),
                                    ((float)ve)/1000.0f,
                                    positionNED(2),
                                    ((float)vd)/1000.0f}};

    //System Uncertainty
    Eigen::Matrix<float,6,6> S_GPS = P+R_GPS.toDenseMatrix();
    //Kalman Gain
    Eigen::Matrix<float,6,6> K_GPS = P*(S_GPS).inverse();
    //Reisdual
    Eigen::Vector<float,6> Y_GPS = z-X;
    //State update
    X = X + (K_GPS*Y_GPS);
    //Covariance update
    P = ( (Eigen::Matrix<float,6,6>::Identity() - K_GPS) * P * ((Eigen::Matrix<float,6,6>::Identity() - K_GPS).transpose()) ) + (K_GPS*R_GPS*K_GPS.transpose());

}

void LocalizationKF::updateGPSReference(const float lat, const float lng, const long alt){
    _gpsReferenceECEF = GPStoECEF(lat,lng,alt);
    _gpsReferenceSLat = sin(lat * degtorad);
    _gpsReferenceCLat = cos(lat * degtorad);
    _gpsReferenceSLng = sin(lng * degtorad);
    _gpsReferenceCLng = cos(lng * degtorad);
}

Eigen::Vector3f LocalizationKF::GPStoECEF(const float lat, const float lng,const long alt){
    const float slat = sin(lat * degtorad);
    const float clat = cos(lat * degtorad);
    const float slng = sin(lng * degtorad);
    const float clng = cos(lng * degtorad);

    const float ecefN = earthMajorAxis/sqrt(1-(earthEccentricity2*slat*slat));
    const float h = alt/1000.0f; //conversion from mm to m

    float ecefX = (ecefN + h)*clat*clng;
    float ecefY = (ecefN + h)*clat*slng;
    float ecefZ = (((1-earthEccentricity2)*ecefN) + h)*slat;

    return Eigen::Vector3f{ecefX,ecefY,ecefZ};
}

Eigen::Vector3f LocalizationKF::GPStoNED(const float lat, const float lng, const long alt){
    Eigen::Matrix3f  _tangentPlane{{-_gpsReferenceSLng, _gpsReferenceCLng, 0},
                                   {-_gpsReferenceSLat*_gpsReferenceCLng, -_gpsReferenceSLat*_gpsReferenceSLng, _gpsReferenceCLat},
                                   {_gpsReferenceCLat*_gpsReferenceCLng, _gpsReferenceCLat*_gpsReferenceSLng, _gpsReferenceSLat}};
    Eigen::Vector3f ENU = _tangentPlane * (GPStoECEF(lat,lng,alt) - _gpsReferenceECEF);
    Eigen::Vector3f NED({ENU(1),ENU(0),-ENU(2)});
    return NED;
}


/**
 * \file libs/numeric/ublasx/test/sign.cpp
 *
 * \brief Test suite for the \c sign operation.
 *
 * 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)
 *
 * \author comcon1 based on code of Marco Guazzone
 */

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/sign.hpp>
#include <cmath>
#include <complex>
#include <cstddef>
#include "libs/numeric/ublasx/test/utils.hpp"

namespace ublas = ::boost::numeric::ublas;
namespace ublasx = ::boost::numeric::ublasx;

static const double tol = 1.0e-15;

BOOST_UBLASX_TEST_DEF( test_real_vector )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Vector" );

	typedef double value_type;
	typedef std::size_t size_type;
	typedef ublas::vector<value_type> vector_type;

	const size_type n(4);

	vector_type v(n);

	v(0) =  0.0;
	v(1) = -2.0;
	v(2) = -3.0;
	v(3) =  4.0;

	vector_type res;
	vector_type expect_res(n);

	res = ublasx::sign(v);

	BOOST_UBLASX_DEBUG_TRACE( "v = " << v );
	BOOST_UBLASX_DEBUG_TRACE( "abs(v) = " << res );

	for (size_type i = 0; i < n; ++i)
	{
		expect_res(i) = v(i) >= 0 ? +1.0 : -1.0;
	}

	BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect_res, n, tol );
}

BOOST_UBLASX_TEST_DEF( test_real_matrix )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Matrix" );

	typedef double value_type;
	typedef std::size_t size_type;
	typedef ublas::matrix<value_type> matrix_type;

	const size_type nr(2);
	const size_type nc(3);

	matrix_type A(nr,nc);

	A(0,0) =  0; A(0,1) = -2; A(0,2) = -3;
	A(1,0) = -4; A(1,1) =  5; A(1,2) =  6;

	matrix_type R;
	matrix_type expect_R(nr,nc);

	R = ublasx::sign(A);

	BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
	BOOST_UBLASX_DEBUG_TRACE( "abs(A) = " << R );

	for (size_type r = 0; r < nr; ++r)
	{
		for (size_type c = 0; c < nc; ++c)
		{
			expect_R(r,c) = A(r,c) >= 0 ? +1.0 : -1.0;
		}
	}

	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, nr, nc, tol );
}

int main()
{

	BOOST_UBLASX_DEBUG_TRACE("Test Suite: 'sign' operation");

	BOOST_UBLASX_TEST_BEGIN();

	BOOST_UBLASX_TEST_DO( test_real_vector );
	BOOST_UBLASX_TEST_DO( test_real_matrix );

	BOOST_UBLASX_TEST_END();
}


// spawn robot swarm at customized quantity, positions and orientation using gazebo service
// this node is to be invoked in a launch file for environment setting of gazebo

// parameters from parameter server
    // robot name: /swarm_sim/robot_name
    // robot urdf: /swarm_sim/robot_name_urdf (replace robot_name with the real name)
    // robot quantity: /robot_quantity
    // robot distribution range: /half_range

#include <ros/ros.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <gazebo_msgs/SpawnModel.h>
#include <geometry_msgs/Pose.h>
#include <Eigen/Geometry>

using namespace Eigen;

Matrix<double, Dynamic, 3> randomGenerator(int quantity, double half_range) {
    // input the quantity of swarm robots and position range, output the pose message

    MatrixXd random_matrix;  // output data
    random_matrix.resize(quantity, 3);
    // first colomn for position.x
    // second colomn for position.y
    // third colomn for rotation angle
    random_matrix = MatrixXd::Random(quantity, 3);  // generate random numbers in (-1, 1)

    // map data, 1st & 2nd colomns to (-half_range, half_range), 3rd to (-M_PI, M_PI)
    random_matrix.col(0) = random_matrix.col(0) * half_range;
    random_matrix.col(1) = random_matrix.col(1) * half_range;
    random_matrix.col(2) = random_matrix.col(2) * M_PI;

    return random_matrix;
}

// int to string converter
std::string intToString(int a) {
    std::stringstream ss;
    ss << a;
    return ss.str();
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "swarm_spawner_gazebo_client");
    ros::NodeHandle nh;
    // service client for service /gazebo/spawn_urdf_model
    ros::ServiceClient client = nh.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_urdf_model");
    gazebo_msgs::SpawnModel spawn_model_srv_msg;  // service message
    geometry_msgs::Pose model_pose;  // model pose message for service message

    // make sure /gazebo/spawn_urdf_model service is ready
    bool service_ready = false;
    while (!service_ready) {
        service_ready = ros::service::exists("/gazebo/spawn_urdf_model",true);
        ROS_INFO("waiting for spawn_urdf_model service");
        ros::Duration(0.5).sleep();
    }
    ROS_INFO("spawn_urdf_model service is ready");

    // commenting out the following line also works fine, just feel safe to wait for a while
    ros::Duration(1.0).sleep();  // wait for gazebo to be initialized

    // get initialization information of robot swarm from parameter
    std::string robot_model_name;
    std::string robot_model_path;
    int robot_quantity;
    double half_range;
    bool get_name, get_path, get_quantity, get_range;
    get_name = nh.getParam("/robot_model_name", robot_model_name);
    // absolute path described from the root directory
    get_path = nh.getParam("/robot_model_path", robot_model_path);
    get_quantity = nh.getParam("/robot_quantity", robot_quantity);
    get_range = nh.getParam("/half_range", half_range);
    if (!(get_name && get_path && get_quantity && get_range))
        return 0;  // return if fail to get parameters

    // prepare the xml for service call, read urdf into string
    std::ifstream inXml;
    std::stringstream strStream;
    std::string xmlStr;
    // finds out the path when launching the launch file is "/home/yang/.ros"
    // instead of the path of the terminal when running launch file
    // char the_path[256];
    // getcwd(the_path, 255);
    // ROS_INFO_STREAM(the_path);
    inXml.open(robot_model_path.c_str());  // why is c_str() needed?
    strStream << inXml.rdbuf();
    xmlStr = strStream.str();
    // prepare the service message
    spawn_model_srv_msg.request.model_xml = xmlStr;
    spawn_model_srv_msg.request.initial_pose.position.z = 0.0;
    spawn_model_srv_msg.request.initial_pose.orientation.x = 0.0;
    spawn_model_srv_msg.request.initial_pose.orientation.y = 0.0;
    spawn_model_srv_msg.request.reference_frame = "world";
    // prepare: the random numbers used in the service call, the position and orientation
    MatrixXd randomNumbers;
    randomNumbers = randomGenerator(robot_quantity, half_range);

    // begin spawn robot through gazebo service
    for (int i=0; i<robot_quantity; i++) {
        std::string index_string = intToString(i);
        // prepare service message for each swarm robot
        spawn_model_srv_msg.request.model_name = robot_model_name + "_" + index_string;
        spawn_model_srv_msg.request.robot_namespace = robot_model_name + "_" + index_string;
        spawn_model_srv_msg.request.initial_pose.position.x = randomNumbers(i, 0);
        spawn_model_srv_msg.request.initial_pose.position.y = randomNumbers(i, 1);
        // calculate the quaternion from random orientation angle
        AngleAxisf aaf(randomNumbers(i, 2), Vector3f::UnitZ());
        Quaternionf qf(aaf);  // convert to quaternion
        // only z & w need to be changed, because of rotation along z axis
        spawn_model_srv_msg.request.initial_pose.orientation.z = qf.z();
        spawn_model_srv_msg.request.initial_pose.orientation.w = qf.w();

        ROS_INFO_STREAM("random x position of " << index_string << " is " << randomNumbers(i, 0));
        ROS_INFO_STREAM("random y position of " << index_string << " is " << randomNumbers(i, 1));
        ROS_INFO_STREAM("random rotation angle of " << index_string << " is " << randomNumbers(i, 2));

        // call service and get response
        bool call_service = client.call(spawn_model_srv_msg);  // call the server
        if (call_service) {
            if (spawn_model_srv_msg.response.success) {
                ROS_INFO_STREAM(robot_model_name << "_" << index_string << " has been spawned");
                ROS_INFO_STREAM("");  // make a blank line
                // std::cout << robot_model_name << "_" << index_string << " has been spawned" << std::endl;
            }
            else {
                ROS_INFO_STREAM(robot_model_name << "_" << index_string << " spawn failed");
                ROS_INFO_STREAM("");  // make a blank line
                // std::cout << robot_model_name << "_" << index_string << " spawn failed" << std::endl;
            }
        }
        else {
            ROS_ERROR("Failed to connect with gazebo server");
            return 0;
        }
    }
    return 0;
}


#include <terrain_server/feature/SlopeFeature.h>
#include <Eigen/Dense>

namespace terrain_server
{

namespace feature
{

SlopeFeature::SlopeFeature() : flat_threshold_(1.0 * (M_PI / 180.0)),
		steep_threshold_(70.0 * (M_PI / 180.0))
{
	name_ = "Slope";
}

SlopeFeature::~SlopeFeature()
{

}

void SlopeFeature::computeCost(double& cost_value,
							   const dwl::Terrain& terrain_info)
{
	double slope = fabs(acos((double) terrain_info.surface_normal(2)));

	if (slope < flat_threshold_)
		cost_value = 0.;
	else if (slope < steep_threshold_) {
		cost_value = -log(1 - (slope - flat_threshold_) / (steep_threshold_ - flat_threshold_));
		if (max_cost_ < cost_value)
			cost_value = max_cost_;
	} else
		cost_value = max_cost_;
}

} //@namespace feature
} //@namespace terrain_server


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_LOG10_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_LOG10_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-exponential
    Function object implementing log10 capabilities

    base ten logarithm function. For integer input types log10 return the truncation
    of the real result.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = log10(x);
    @endcode

    is similar to:

    @code
    T r = log(x)/log(10);
    @endcode

    - log10(x) return Nan for negative enties (peculiarly Mzero
    for floating numbers).

    - The call log10(x, assert_) asserts is x is negative (peculiarly
    take care that it asserts for Mzero but not Zero in case of floating numbers)

    @par Decorators

    std_ for floating entries

    @see log, log2, log1p, is_negative, Mzero

  **/
  const boost::dispatch::functor<tag::log10_> log10 = {};
} }
#endif

#include <boost/simd/function/scalar/log10.hpp>
#include <boost/simd/function/simd/log10.hpp>

#endif


#ifndef __IFUNCTION_HXX__
#define __IFUNCTION_HXX__

#include <Eigen/Dense>

/*
 * IFunction is an interface for what a function needs to contain
 * in order to implement SGD, SVRGD, SAGA, and Katyusha
 *
 */

using namespace Eigen;

namespace Optimastic {

template <int n>
struct IFunction {
    static const int Dimension = n;
    typedef Matrix<double, n, 1> Domain;

    // _partial_gradient takes in a gradient vector and accumulates
    // the ith partial derviative into this vector; this is to avoid
    // making more temporaries and copies of said radient
    virtual void accum_partial_gradient(int i, Domain &x, Domain &grad, double step_size) const = 0;
    virtual Domain full_gradient(Domain &x) const = 0;

    virtual double operator() (const Domain &x) const = 0;
};

} // namespace Optimastic

#endif


#include <iostream>
#include <boost/numeric/interval.hpp>

int main()
{
	boost::numeric::interval<int> range1(0, 100);
	boost::numeric::interval<int> range2(30, 120);

	auto new_range1 = boost::numeric::intersect(range1, range2);

	std::cout << new_range1.lower() << " ~ "
		<< new_range1.upper() << std::endl;

	return 0;
}


/*!
*   \file expm.hpp
*
*   Implement matrix exponential using pade approximation.
*
*  Copyright (c) 2007
*  \author Tsai, Dung-Bang
*
*/
//  Department of Physics,
//  National Taiwan University.
//
//  E-Mail : dbtsai [_at_] dbtsai [_dot_] org
//  Begine : 2007/11/20
//  Last modify : 2007/11/26
//  Version : v0.4
//
//  expm_pad computes the matrix exponential exp(H) for general matrixs,
//  including complex and real matrixs using the irreducible (p,p) degree
//  rational Pade approximation to the exponential
//  exp(z) = r(z) = (+/-)( I+2*(Q(z)/P(z))).
//
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt).
//

#ifndef _BOOST_UBLAS_EXPM_
#define _BOOST_UBLAS_EXPM_
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/traits.hpp>

namespace boost { namespace numeric { namespace ublas {

template<typename MATRIX> MATRIX expm_pad(const MATRIX &H, typename type_traits<typename MATRIX::value_type>::real_type t = 1.0, const int p = 6){
	typedef typename MATRIX::value_type value_type;
        typedef typename MATRIX::size_type size_type;
	typedef typename type_traits<value_type>::real_type real_value_type;
	assert(H.size1() == H.size2());
	assert(p >= 1);
	const size_type n = H.size1();
	const identity_matrix<value_type> I(n);
	matrix<value_type> U(n,n),H2(n,n),P(n,n),Q(n,n);
	real_value_type norm = 0.0;
// Calcuate Pade coefficients
	vector<real_value_type> c(p+1);
	c(0)=1;
	for(size_type i = 0; i < (size_type) p; ++i)
		c(i+1) = c(i) * ((p - i)/((i + 1.0) * (2.0 * p - i)));
// Calcuate the infinty norm of H, which is defined as the largest row sum of a matrix
	for(size_type i=0; i<n; ++i) {
		real_value_type temp = 0.0;
		for(size_type j = 0; j < n; j++)
			temp += std::abs(H(i, j));
		norm = t * std::max<real_value_type>(norm, temp);
	}
// If norm = 0, and all H elements are not NaN or infinity but zero,
// then U should be identity.
	if (norm == 0.0) {
		bool all_H_are_zero = true;
		for(size_type i = 0; i < n; i++)
			for(size_type j = 0; j < n; j++)
				if( H(i,j) != value_type(0.0) )
					all_H_are_zero = false;
		if( all_H_are_zero == true ) return I;
// Some error happens, H has elements which are NaN or infinity.
		std::cerr<<"Null input error in the template expm_pad.\n";
		//		std::cout << "Null INPUT : " << H <<"\n";
		exit(0);
	}
// Scaling, seek s such that || H*2^(-s) || < 1/2, and set scale = 2^(-s)
 	int s = 0;
	real_value_type scale = 1.0;
	if(norm > 0.5) {
		s = std::max<int>(0, static_cast<int>((log(norm) / log(2.0) + 2.0)));
		scale /= real_value_type(std::pow(2.0, s));
		U.assign((scale * t) * H); // Here U is used as temp value due to that H is const
	}
	else
		U.assign(H);

// Horner evaluation of the irreducible fraction, see the following ref above.
// Initialise P (numerator) and Q (denominator)
	H2.assign( prod(U, U) );
	Q.assign( c(p)*I );
	P.assign( c(p-1)*I );
	size_type odd = 1;
	for( size_type k = p - 1; k > 0; --k) {
		( odd == 1 ) ?
			( Q = ( prod(Q, H2) + c(k-1) * I ) ) :
			( P = ( prod(P, H2) + c(k-1) * I ) ) ;
		odd = 1 - odd;
	}
	( odd == 1 ) ? ( Q = prod(Q, U) ) : ( P = prod(P, U) );
	Q -= P;
// In origine expokit package, they use lapack ZGESV to obtain inverse matrix,
// and in that ZGESV routine, it uses LU decomposition for obtaing inverse matrix.
// Since in ublas, there is no matrix inversion template, I simply use the build-in
// LU decompostion package in ublas, and back substitute by myself.

// Implement Matrix Inversion
	permutation_matrix<size_type> pm(n);
	int res = lu_factorize(Q, pm);
	if( res != 0) {
		std::cerr << "Matrix inversion error in the template expm_pad.\n";
		exit(0);
	}
// H2 is not needed anymore, so it is temporary used as identity matrix for substituting.
	H2.assign(I);
	lu_substitute(Q, pm, H2);
	(odd == 1) ?
		( U.assign( -(I + real_value_type(2.0) * prod(H2, P))) ):
		( U.assign(   I + real_value_type(2.0) * prod(H2, P) ) );
// Squaring
	for(size_type i = 0; i < (size_type) s; ++i)
		U = (prod(U,U));
	return U;
}

}}}

#endif


/**
 *  This example shows how to use MLPACK neural networks isolatedly from the
 *  hsmm framework.
 */

#include <mlpack/core.hpp>
#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/init_rules/random_init.hpp>
#include <mlpack/methods/ann/loss_functions/mean_squared_error.hpp>
#include <armadillo>
#include <boost/program_options.hpp>
#include <iostream>

using namespace arma;
using namespace mlpack::ann;
using namespace std;
namespace po = boost::program_options;

mat join_mats(vector<mat> v) {
    int total_cols = 0;
    int total_rows = v.at(0).n_rows;
    for(auto& m: v)
        total_cols += m.n_cols;
    mat ret(total_rows, total_cols);
    int idx = 0;
    for(auto& m: v) {
        int d = m.n_cols;
        ret.cols(idx, idx + d - 1) = m;
        idx += d;
    }
    assert(idx == total_cols);
    return ret;
}

int main(int argc, char *argv[]) {
    mlpack::Log::Info.ignoreInput = false;  // Turning mlpack verbose output on.
    po::options_description desc("Options");
    desc.add_options()
        ("help,h", "Produce help message")
        ("input,i", po::value<string>(), "Path to the input obs")
        ("viterbi,v", po::value<string>(), "Path to the input viterbi file")
        ("nstates,s", po::value<int>(), "Number of states (NNs)")
        ("hiddenunits,u", po::value<int>()->default_value(10),
                "Number of hidden units")
        ("nfiles,n", po::value<int>()->default_value(1),
                "Number of input files to process");
    vector<string> required_fields = {"input", "viterbi", "nfiles",
            "nstates"};
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("help")) {
        cout << desc << endl;
        return 0;
    }
    for(auto s: required_fields) {
        if (!vm.count(s)) {
            cerr << "Error: You must provide the argument: " << s << endl;
            return 1;
        }
    }
    string input_filename = vm["input"].as<string>();
    string viterbi_filename = vm["viterbi"].as<string>();
    int nseq = vm["nfiles"].as<int>();
    int nstates = vm["nstates"].as<int>();
    int hidden_units = vm["hiddenunits"].as<int>();
    vector<mat> obs_for_each_state[nstates];
    vector<mat> times_for_each_state[nstates];
    for(int i = 0; i < nseq; i++) {
        string iname = input_filename;
        string vname = viterbi_filename;
        if (nseq != 1) {
            iname += string(".") + to_string(i);
            vname += string(".") + to_string(i);
        }
        mat obs;
        obs.load(iname, raw_ascii);
        imat vit;
        vit.load(vname, raw_ascii);
        ivec hs = vit.col(0);
        ivec dur = vit.col(1);
        int idx = 0;
        for(int j = 0; j < dur.n_rows; j++) {
            mat segment = obs.cols(idx, idx + dur(j) - 1);
            obs_for_each_state[hs(j)].push_back(segment);
            rowvec times = linspace<rowvec>(0, 1.0, dur(j));
            times_for_each_state[hs(j)].push_back(times);
            idx += dur(j);
        }
    }

    // There should be at least one segment for the hidden state 0.
    int njoints = obs_for_each_state[0].at(0).n_rows;
    FFN<MeanSquaredError<>, RandomInitialization> neural_network[nstates];
    for(int i = 0; i < nstates; i++) {
        mat inputs = join_mats(times_for_each_state[i]);
        mat outputs = join_mats(obs_for_each_state[i]);
        assert (outputs.n_rows == njoints);
        assert(outputs.n_cols == inputs.n_cols);

        // Defining the architecture of the NN.
        neural_network[i].Add<Linear<>>(1, hidden_units);
        neural_network[i].Add<SigmoidLayer<>>();
        neural_network[i].Add<Linear<>>(hidden_units, njoints);

        // Training the NN.
        neural_network[i].Train(inputs, outputs);

        // Evaluating the loss.
        mat test_input = linspace<rowvec>(0,1,100);
        mat test_output;
        neural_network[i].Predict(test_input, test_output);
    }

    for(int i = 0; i < nstates; i++) {
        mat input = linspace<rowvec>(0,1,10);
        mat first_output;
        neural_network[i].Predict(input, first_output);
        mat old_params = neural_network[i].Parameters();

        // Changing the params.
        mat new_params(size(old_params), fill::ones);
        neural_network[i].Parameters() = new_params;
        mat second_output;
        neural_network[i].Predict(input, second_output);

        // Changing the params back to the old ones.
        neural_network[i].Parameters() = old_params;
        mat third_output;
        neural_network[i].Forward(input, third_output, 0, 2);

        assert(approx_equal(first_output, third_output, "reldiff", 1e-7));
    }
    return 0;
}


#include <cstdio>
#include <cstdlib>
#define GL_SILENCE_DEPRECATION
#include <GLFW/glfw3.h>
#include <filesystem>
#include <Eigen/Dense>

#define STB_IMAGE_IMPLEMENTATION
#include "stb_image.h"
#include "delfem2/glfw/viewer2.h"

Eigen::Matrix<double,4,4,Eigen::RowMajor> GetHomographicTransformation(
    const double c1[4][2])
{
  const double c0[4][2] = {
      {-0.5,-0.5},
      {+0.5,-0.5},
      {+0.5,+0.5},
      {-0.5,+0.5} };
  Eigen::Matrix<double,4,4,Eigen::RowMajor> m;
  // set identity as default
    m <<
      1, 0, 0, 0,
      0, 1, 0, 0,
      0, 0, 1, 0,
      0, 0, 0, 1;
  // write some code to compute the 4x4 Homographic transformation matrix `m`;
  // `m` should transfer :
  // (c0[0][0],c0[][1],z) -> (c1[0][0],c1[0][1],z)
  // (c0[1][0],c0[][1],z) -> (c1[1][0],c1[1][1],z)
  // (c0[2][0],c0[][1],z) -> (c1[2][0],c1[2][1],z)
  // (c0[3][0],c0[][1],z) -> (c1[3][0],c1[3][1],z)

  return m;
}

int main() {

  std::string path = std::string(SOURCE_DIR) + "/../assets/ada.png";
  std::vector<char> img_data;
  int img_width, img_height, img_channels;
  { // load image data using stb library
    stbi_set_flip_vertically_on_load(true);
    assert( std::filesystem::exists(path.c_str()) );
    unsigned char *img = stbi_load(
        path.c_str(),
        &img_width, &img_height, &img_channels, 0);
    assert(img_width > 0 && img_height > 0);
    std::cout << "image size: " << img_width << " " << img_height << " " << img_channels << std::endl;
    img_data.assign(img,img+img_width*img_height*img_channels);
    stbi_image_free(img);
  }

  if (!glfwInit()) { exit(EXIT_FAILURE); }
  // set OpenGL's version (note: ver. 2.1 is very old, but I chose because it's simple)
  ::glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 2);
  ::glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
  GLFWwindow *window = ::glfwCreateWindow(500, 500, "task01", nullptr, nullptr);
  if (!window) { // exit if failed to create window
    ::glfwTerminate();
    exit(EXIT_FAILURE);
  }
  ::glfwMakeContextCurrent(window); // working on this window below

  // set image to texture calling OpenGL's function
  // if you are interested in OpenGL's texture, look: https://learnopengl.com/Getting-started/Textures
  unsigned int texture;
  glGenTextures(1, &texture);
  glBindTexture(GL_TEXTURE_2D, texture);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
  glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA,
               static_cast<int>(img_width),
               static_cast<int>(img_height),
               0, GL_RGBA, GL_UNSIGNED_BYTE,
               img_data.data());

  ::glClearColor(1, 1, 1, 1);
  ::glEnable(GL_DEPTH_TEST);
  ::glEnable(GL_POLYGON_OFFSET_FILL);
  ::glPolygonOffset(1.1f, 4.0f);
  while (!::glfwWindowShouldClose(window)) {
    double time = glfwGetTime();
    const double corners[4][2] = {
        {-0.5, -0.5},
        {+0.5, -0.5},
        {+0.5 - 0.4*cos(1*time), +0.5 - 0.4*sin(3*time)},
        {-0.5 + 0.4*sin(2*time), +0.5 + 0.4*cos(5*time)} };

    Eigen::Matrix<double,4,4,Eigen::ColMajor> modelview_matrix = GetHomographicTransformation(corners);

    ::glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    // set projection matrix
    ::glMatrixMode(GL_PROJECTION);
    ::glLoadIdentity();

    // draw red points
    ::glDisable(GL_TEXTURE_2D);
    ::glDisable(GL_LIGHTING);
    ::glMatrixMode(GL_MODELVIEW);
    ::glLoadIdentity();
    ::glColor3d(1,0,0);
    ::glPointSize(10);
    ::glBegin(GL_POINTS);
    ::glVertex2dv(corners[0]);
    ::glVertex2dv(corners[1]);
    ::glVertex2dv(corners[2]);
    ::glVertex2dv(corners[3]);
    ::glEnd();

    // set model view matrix
    ::glMatrixMode(GL_MODELVIEW);
    ::glLoadIdentity();
    ::glMultMatrixd(modelview_matrix.data());
    ::glEnable(GL_TEXTURE_2D);
    ::glColor3d(1,1,1);
    ::glBegin(GL_QUADS);
    ::glTexCoord2d(0,0);
    ::glVertex2d(-0.5,-0.5);
    ::glTexCoord2d(1,0);
    ::glVertex2d(+0.5,-0.5);
    ::glTexCoord2d(1,1);
    ::glVertex2d(+0.5,+0.5);
    ::glTexCoord2d(0,1);
    ::glVertex2d(-0.5,+0.5);
    ::glEnd();
    //
    ::glfwSwapBuffers(window);
    ::glfwPollEvents();
  }
  ::glfwDestroyWindow(window);
  ::glfwTerminate();
  exit(EXIT_SUCCESS);
}


////////////////////////////////////////////////////////////////////////////////
#include <BVH.cpp>
#include <Eigen/Dense>
#include <catch.hpp>
////////////////////////////////////////////////////////////////////////////////

TEST_CASE("test_tree", "[tests]")
{
    Eigen::MatrixXd pts(4, 3);
    pts << 0, 0, 0,
        3, 0, 0,
        0, 3, 0,
        0, 3, 3;

    Eigen::MatrixXi tri(4, 3);
    tri << 0, 1, 2,
        0, 1, 3,
        0, 2, 3,
        1, 2, 3;

    BVH::BVH bvh;
    bvh.init(pts, tri, 1e-10);

    std::vector<unsigned int> pairs;
    bvh.intersect_box(pts.colwise().minCoeff(), pts.colwise().maxCoeff(), pairs);

    CHECK(pairs.size() == tri.rows());
}


// two_dim_rand.cpp
//
#include <iostream>

#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/random.hpp>

double on_circle_prob(double x, double y)
{
    static const double k_delta = 0.01;
    if (std::abs(x*x + y*y - 1) <= k_delta)
        return 1.0;
    else
        return 0;
}

int main(int argc, char* argv[])
{
    using boost::lexical_cast;
    using boost::random::uniform_real_distribution;
    using boost::random::discrete_distribution;

    std::size_t samples = argc > 1 ? lexical_cast<std::size_t>(argv[1]) : 100;
    std::size_t n = argc > 2 ? lexical_cast<std::size_t>(argv[2]) : 100;
    double xmin = -1.0;
    double xmax = 1.0;
    double ymin = -1.0;
    double ymax = 1.0;

    boost::mt19937 gen;
    uniform_real_distribution<> x_dist(xmin, xmax);
    for (int i = 0; i < samples; ) {
        double x = x_dist(gen);
        discrete_distribution<> y_dist(n, ymin, ymax, boost::bind(on_circle_prob, x, _1));
        int rand_n = y_dist(gen);
        double y = (rand_n * 1.0 / n) * (ymax - ymin) + ymin;
        if (true || on_circle_prob(x, y) != 0) {
            std::cout << x << ' ' << y << std::endl;
            i++;
        }
    }
}


#include <Eigen/Dense>

#include <ancse/cfl_condition.hpp>
#include <ancse/config.hpp>
#include <ancse/fvm_rate_of_change.hpp>
#include <ancse/snapshot_writer.hpp>
#include <ancse/time_loop.hpp>

template<class F>
Eigen::VectorXd ic(const F &f, const Grid &grid) {
    Eigen::VectorXd u0(grid.n_cells);
    for(int i = 0; i < grid.n_cells; ++i) {
        u0[i] = f(cell_center(grid, i));
    }

    return u0;
}

TimeLoop make_fvm(const Grid &grid) {
    auto config = get_global_config();
    double t_end = config["t_end"];
    double cfl_number = config["cfl_number"];

    auto n_ghost = grid.n_ghost;
    auto n_cells = grid.n_cells;

    auto model = Model{};

    auto simulation_time = std::make_shared<SimulationTime>(t_end);
    auto fvm_rate_of_change = make_fvm_rate_of_change(grid, model, simulation_time);
    auto boundary_condition = make_boundary_condition(n_ghost, config["boundary_condition"]);
    auto time_integrator = make_runge_kutta(fvm_rate_of_change, boundary_condition, n_cells);
    auto cfl_condition = make_cfl_condition(grid, model, cfl_number);
    auto snapshot_writer = std::make_shared<JSONSnapshotWriter>(grid, simulation_time, std::string(config["output"]));

    return TimeLoop(simulation_time, time_integrator, cfl_condition, snapshot_writer);
}

void smooth_sine_test() {
    auto config = get_global_config();

    int n_ghost = config["n_ghost"];
    int n_cells = int(config["n_interior_cells"]) + n_ghost * 2;

    auto grid = Grid({0.0, 1.0}, n_cells, n_ghost);
    auto u0 = ic([](double x) { return std::sin(2.0*M_PI * x);}, grid);

    auto fvm = make_fvm(grid);
    fvm(u0);
}

int main() {
    smooth_sine_test();

    return 0;
}