//$Id: utils.h 611 2012-07-18 14:14:00Z koskos $

#ifndef _UTILS_H
#define _UTILS_H

#define IA 16807
#define IM 2147483647
#define AM (1.0/IM)
#define IQ 127773
#define IR 2836
#define NTAB 32
#define NDIV (1+(IM-1)/NTAB)
#define EPS 1.2e-7
#define RNMX (1.0-EPS)
#define PI 3.14159265358979323846

#define LOW_POS_DOUBLE 1e-300
#define BIG_POS_DOUBLE 1e300
#define LOW_NEG_DOUBLE -1e-300
#define BIG_NEG_DOUBLE -1e300
#define LOW_POS_FLOAT 1e-8
#define BIG_POS_FLOAT 1e8
#define LOW_NEG_FLOAT -1e-8
#define BIG_NEG_FLOAT -1e8
#define BIG_POS_INT 1000000000
#define BIG_NEG_INT -1000000000

#include <string>
#include <vector>
#include <queue>
#include <map>
#include <bitset>
#include <list>
#include <tr1/unordered_map>
#include <bitset>
#include <cmath>
#include <algorithm>
#include <iostream>
#include <sstream>
#include <fstream>
#include <iomanip>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <time.h>
#include <sys/time.h>
#include <pthread.h>
#include <exception>
#include <boost/algorithm/string.hpp>
#include <boost/program_options.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/iostreams/filter/bzip2.hpp>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <boost/lexical_cast.hpp>

using namespace std;
namespace bio = boost::iostreams;
namespace bpo = boost::program_options;
namespace bid = boost::uuids;

/******************************************************/
/*                  UTILS STATISTICS                  */
/******************************************************/
namespace putils {
	void initRandom(long s);
	double getRandom();
	string getRandomID();
	int getRandom(int);
	long getSeed();
	void normalise(vector < double > & v);
	int sample(vector< double > & v, double sum);
	double entropy(vector < double > & v);
	double KLdistance(vector < double > & P, vector < double > & Q);
};

/******************************************************/
/*                  UTILS ALGORITHM                   */
/******************************************************/
namespace autils {
	int max(vector < double > & v);
	int max(vector < int > & v);
    void findUniqueSet(vector < bool > & B, vector < int > & U); // ?
    void decompose(int min, vector < vector < int > > & B, vector < vector < vector < int > > > & BB);//?
	int checkDuo (int pa1, int pa2, int ca1, int ca2);
	int checkTrio (int fa1, int fa2, int ma1, int ma2, int ca1, int ca2);
};

/******************************************************/
/*                  UTILS STRING                      */
/******************************************************/
namespace sutils {
	int tokenize(string &, vector < string > &);
	int tokenize(string &, vector < string > &, int);
	string uint2str(unsigned int n);
	string int2str(int n);
	string int2str(vector < int > & v);
	string long2str(long int n);
	string double2str(double n, int prc = 4);
	string double2str(vector < double > &v, int prc = 4);
	string bool2str(vector<bool> & v);
	string date2str(time_t * t, string format);
};

/******************************************************/
/*                  UTILS FILE                        */
/******************************************************/
namespace futils {
	bool isFile(string f);
	bool createFile(string f);
	string extensionFile(string & filename);
	void bool2binary(vector < bool > & V, ostream &fd);
	bool binary2bool(vector < bool > & V, istream & fd);
};

/******************************************************/
/*                  EXCEPTIONS                        */
/******************************************************/
class myException : public exception {
public:
   explicit myException(std::string msg) : msg_(msg) {}

   virtual ~myException() throw() {}

   virtual const char* what() const throw() {
      return msg_.c_str();
   }

private:
   std::string msg_;
};

/******************************************************/
/*                  INPUT FILE                        */
/******************************************************/
class ifile : public bio::filtering_istream {
private:
	string file;
	ifstream fd;
        bool m_isGood = false;

public:
	ifile();
	ifile(string filename , bool binary = false);
	~ifile();
	string name();
	bool open(string filename, bool binary = false);
	bool readString(string &);
	void close();
        bool isGood(){return m_isGood;};
};

/******************************************************/
/*                  OUTPUT FILE                       */
/******************************************************/
class ofile : public bio::filtering_ostream {
private:
	string file;
	ofstream fd;

public:
	ofile();
	ofile(string filename , bool binary = false);
	~ofile();
	string name();
	bool open(string filename, bool binary = false);
	void writeString(string &);
	void close();
};

/******************************************************/
/*                  LOG FILE                          */
/******************************************************/
class lfile {
private:
	string file;
	ofstream fd;
	bool verboseC;
	bool verboseL;

public:
	lfile();
	~lfile();
	string name();
	bool open(string filename = "file.log");
	void close();
	string getPrefix();
	void muteL();
	void unmuteL();
	void muteC();
	void unmuteC();
	void print(string s);
	void printC(string s);
	void printL(string s);
	void println(string s);
	void printlnC(string s);
	void printlnL(string s);
	void warning(string s);
	void error(string s);
};

#endif


/*    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
 *      NASA/JPL HORIZONS Web-Interface, http://ssd.jpl.nasa.gov/horizons.cgi [1].
 *
 */

#define BOOST_TEST_MAIN

#include <stdexcept>
#include <string>
#include <utility>

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

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

#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"
#include "Tudat/Astrodynamics/Ephemerides/keplerStateExtractor.h"
#include "Tudat/InputOutput/parsedDataVectorUtilities.h"
#include "Tudat/Basics/basicTypedefs.h"

namespace tudat
{
namespace unit_tests
{

// Short-hand notation.
namespace parsed_data_vector_utilities = input_output::parsed_data_vector_utilities;
using Eigen::Vector6d;

BOOST_AUTO_TEST_SUITE( test_keplerStateExtractor )

//! Test the extract function.
BOOST_AUTO_TEST_CASE( keplerStateExtractor_Extract )
{
    // Using declaration.
    using namespace ephemerides;
    using namespace orbital_element_conversions;
    namespace field_types = input_output::field_types;

    // Create parsed data line map pointer.
    // Define a new type: pair of field type and pointer to value.
    typedef std::pair< input_output::FieldType,
            parsed_data_vector_utilities::FieldValuePtr > FieldDataPair;

    // Create test strings, based on Earth orbital elements at JD = 2456074.5 in SI units [1].
    std::string testSemiMajorAxis = "149641767.7265875";
    std::string testEccentricity = "0.01625818315929578";
    std::string testInclination = "0.000038294684687687365297820321195993";
    std::string testLongitudeOfAscendingNode = "3.3677017965694967895481414920601";
    std::string testArgumentOfPeriapsis = "4.7066684303635934184870904755709";
    std::string testTrueAnomaly = "2.5013767559662625175504405244245";

    // Convert strings to doubles.
    const double expectedSemiMajorAxis = std::stod( testSemiMajorAxis );
    const double expectedEccentricity = std::stod( testEccentricity );
    const double expectedInclination = std::stod( testInclination );
    const double expectedLongitudeOfAscendingNode = std::stod(
                testLongitudeOfAscendingNode );
    const double expectedArgumentOfPeriapsis = std::stod(
                testArgumentOfPeriapsis );
    const double expectedTrueAnomaly = std::stod( testTrueAnomaly );

    // Store strings as field values.
    parsed_data_vector_utilities::FieldValuePtr testFieldValueSemiMajorAxis(
                new input_output::FieldValue( field_types::state::semiMajorAxis,
                                              testSemiMajorAxis ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueEccentricity(
                new input_output::FieldValue( field_types::state::eccentricity,
                                              testEccentricity ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueInclination(
                new input_output::FieldValue( field_types::state::inclination,
                                              testInclination ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueLongitudeOfAscendingNode(
                new input_output::FieldValue( field_types::state::longitudeOfAscendingNode,
                                              testLongitudeOfAscendingNode ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueArgumentOfPeriapsis(
                new input_output::FieldValue( field_types::state::argumentOfPeriapsis,
                                              testArgumentOfPeriapsis ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueTrueAnomaly(
                new input_output::FieldValue( field_types::state::trueAnomaly,
                                              testTrueAnomaly ) );

    // Create a new pointer to data map.
    parsed_data_vector_utilities::ParsedDataLineMapPtr testDataMap =
            std::make_shared< parsed_data_vector_utilities::ParsedDataLineMap >(
                std::map< input_output::FieldType,
                parsed_data_vector_utilities::FieldValuePtr >( ) );

    // Store field values in data map.
    testDataMap->insert( FieldDataPair( field_types::state::semiMajorAxis,
                                        testFieldValueSemiMajorAxis ) );
    testDataMap->insert( FieldDataPair( field_types::state::eccentricity,
                                        testFieldValueEccentricity ) );
    testDataMap->insert( FieldDataPair( field_types::state::inclination,
                                        testFieldValueInclination ) );
    testDataMap->insert( FieldDataPair( field_types::state::longitudeOfAscendingNode,
                                        testFieldValueLongitudeOfAscendingNode ) );
    testDataMap->insert( FieldDataPair( field_types::state::argumentOfPeriapsis,
                                        testFieldValueArgumentOfPeriapsis ) );
    testDataMap->insert( FieldDataPair( field_types::state::trueAnomaly,
                                        testFieldValueTrueAnomaly ) );

    // Create Cartesian state extractor.
    KeplerStateExtractor testKeplerStateExtractor;

    // Extract test data map.
    std::shared_ptr< Vector6d > returnedKeplerianElements =
            testKeplerStateExtractor.extract( testDataMap );

    // Verify that the returned values correspond to the expected values.
    BOOST_CHECK_EQUAL( ( *returnedKeplerianElements )( semiMajorAxisIndex ),
                       expectedSemiMajorAxis );
    BOOST_CHECK_EQUAL( ( *returnedKeplerianElements )( eccentricityIndex ),
                       expectedEccentricity );
    BOOST_CHECK_EQUAL( ( *returnedKeplerianElements )( inclinationIndex ),
                       expectedInclination );
    BOOST_CHECK_EQUAL( ( *returnedKeplerianElements )( argumentOfPeriapsisIndex ),
                       expectedArgumentOfPeriapsis );
    BOOST_CHECK_EQUAL( ( *returnedKeplerianElements )( longitudeOfAscendingNodeIndex ),
                       expectedLongitudeOfAscendingNode );
    BOOST_CHECK_EQUAL( ( *returnedKeplerianElements )( trueAnomalyIndex ),
                       expectedTrueAnomaly );
}

//! Test the extract function with mean anomaly as input.
BOOST_AUTO_TEST_CASE( keplerStateExtractor_ExtractWithMeanAnomaly )
{
    // Using declaration.
    using namespace ephemerides;
    using namespace orbital_element_conversions;
    namespace field_types = input_output::field_types;

    // Create parsed data line map pointer.
    // Define a new type: pair of field type and pointer to value.
    typedef std::pair< input_output::FieldType,
            parsed_data_vector_utilities::FieldValuePtr > FieldDataPair;

    // Create test strings, based on Earth orbital elements at JD = 2456074.5 in SI units [1].
    std::string testSemiMajorAxis = "149641767.7265875";
    std::string testEccentricity = "0.01625818315929578";
    std::string testInclination = "0.000038294684687687365297820321195993";
    std::string testLongitudeOfAscendingNode = "3.3677017965694967895481414920601";
    std::string testArgumentOfPeriapsis = "4.7066684303635934184870904755709";
    std::string testMeanAnomaly = "2.4817611918827110773177514320574";

    // Convert strings to doubles.
    double expectedSemiMajorAxis = std::stod( testSemiMajorAxis );
    double expectedEccentricity = std::stod( testEccentricity );
    double expectedInclination = std::stod( testInclination );
    double expectedLongitudeOfAscendingNode = std::stod(
                testLongitudeOfAscendingNode );
    double expectedArgumentOfPeriapsis = std::stod( testArgumentOfPeriapsis );

    // Set expected true anomaly.
    double expectedTrueAnomaly = 2.5013767559662625175504405244245;

    // Store strings as field values.
    parsed_data_vector_utilities::FieldValuePtr testFieldValueSemiMajorAxis(
                new input_output::FieldValue( field_types::state::semiMajorAxis,
                                              testSemiMajorAxis ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueEccentricity(
                new input_output::FieldValue( field_types::state::eccentricity,
                                              testEccentricity ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueInclination(
                new input_output::FieldValue( field_types::state::inclination,
                                              testInclination ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueLongitudeOfAscendingNode(
                new input_output::FieldValue( field_types::state::longitudeOfAscendingNode,
                                              testLongitudeOfAscendingNode ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueArgumentOfPeriapsis(
                new input_output::FieldValue( field_types::state::argumentOfPeriapsis,
                                              testArgumentOfPeriapsis ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueMeanAnomaly(
                new input_output::FieldValue( field_types::state::meanAnomaly,
                                              testMeanAnomaly ) );

    // Create a new pointer to data map.
    parsed_data_vector_utilities::ParsedDataLineMapPtr testDataMap =
            std::make_shared< parsed_data_vector_utilities::ParsedDataLineMap >(
                std::map< input_output::FieldType,
                parsed_data_vector_utilities::FieldValuePtr >( ) );

    // Store field values in data map.
    testDataMap->insert( FieldDataPair( field_types::state::semiMajorAxis,
                                        testFieldValueSemiMajorAxis ) );
    testDataMap->insert( FieldDataPair( field_types::state::eccentricity,
                                        testFieldValueEccentricity ) );
    testDataMap->insert( FieldDataPair( field_types::state::inclination,
                                        testFieldValueInclination ) );
    testDataMap->insert( FieldDataPair( field_types::state::longitudeOfAscendingNode,
                                        testFieldValueLongitudeOfAscendingNode ) );
    testDataMap->insert( FieldDataPair( field_types::state::argumentOfPeriapsis,
                                        testFieldValueArgumentOfPeriapsis ) );
    testDataMap->insert( FieldDataPair( field_types::state::meanAnomaly,
                                        testFieldValueMeanAnomaly ) );

    // Create Cartesian state extractor.
    KeplerStateExtractor testKeplerStateExtractor;

    // Extract test data map.
    std::shared_ptr< Vector6d > returnedKeplerianElements =
            testKeplerStateExtractor.extract( testDataMap );

    // Verify that the returned values correspond to the expected values.
    BOOST_CHECK_EQUAL( ( *returnedKeplerianElements )( semiMajorAxisIndex ),
                       expectedSemiMajorAxis );
    BOOST_CHECK_EQUAL( ( *returnedKeplerianElements )( eccentricityIndex ),
                       expectedEccentricity );
    BOOST_CHECK_EQUAL( ( *returnedKeplerianElements )( inclinationIndex ),
                       expectedInclination );
    BOOST_CHECK_EQUAL( ( *returnedKeplerianElements )( argumentOfPeriapsisIndex ),
                       expectedArgumentOfPeriapsis );
    BOOST_CHECK_EQUAL( ( *returnedKeplerianElements )( longitudeOfAscendingNodeIndex ),
                       expectedLongitudeOfAscendingNode );

    BOOST_CHECK_CLOSE_FRACTION( ( *returnedKeplerianElements )( trueAnomalyIndex ),
                                expectedTrueAnomaly,
                                std::numeric_limits< double >::epsilon( ) );
}

//! Test if the extract function throws the necessary exceptions.
BOOST_AUTO_TEST_CASE( keplerStateExtractor_MissingSemiMajorAxis )
{
    // Using declaration.
    using namespace ephemerides;
    namespace field_types = input_output::field_types;

    // Create parsed data line map pointer.
    // Define a new type: pair of field type and pointer to value.
    typedef std::pair< input_output::FieldType,
            parsed_data_vector_utilities::FieldValuePtr > FieldDataPair;

    // Create test strings, based on Earth orbital elements at JD = 2456074.5 in SI units [1].
    std::string testEccentricity = "0.01625818315929578";
    std::string testInclination = "0.000038294684687687365297820321195993";
    std::string testLongitudeOfAscendingNode = "3.3677017965694967895481414920601";
    std::string testArgumentOfPeriapsis = "4.7066684303635934184870904755709";
    std::string testTrueAnomaly = "2.4817611918827110773177514320574";

    // Store strings as field values.
    parsed_data_vector_utilities::FieldValuePtr testFieldValueEccentricity(
                new input_output::FieldValue( field_types::state::eccentricity,
                                              testEccentricity ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueInclination(
                new input_output::FieldValue( field_types::state::inclination,
                                              testInclination ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueLongitudeOfAscendingNode(
                new input_output::FieldValue( field_types::state::longitudeOfAscendingNode,
                                              testLongitudeOfAscendingNode ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueArgumentOfPeriapsis(
                new input_output::FieldValue( field_types::state::argumentOfPeriapsis,
                                              testArgumentOfPeriapsis ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueTrueAnomaly(
                new input_output::FieldValue( field_types::state::trueAnomaly,
                                              testTrueAnomaly ) );

    // Create a new pointer to data map.
    parsed_data_vector_utilities::ParsedDataLineMapPtr testDataMap =
            std::make_shared< parsed_data_vector_utilities::ParsedDataLineMap >(
                std::map< input_output::FieldType,
                parsed_data_vector_utilities::FieldValuePtr >( ) );

    // Store field values in data map.
    testDataMap->insert( FieldDataPair( field_types::state::eccentricity,
                                        testFieldValueEccentricity ) );
    testDataMap->insert( FieldDataPair( field_types::state::inclination,
                                        testFieldValueInclination ) );
    testDataMap->insert( FieldDataPair( field_types::state::longitudeOfAscendingNode,
                                        testFieldValueLongitudeOfAscendingNode ) );
    testDataMap->insert( FieldDataPair( field_types::state::argumentOfPeriapsis,
                                        testFieldValueArgumentOfPeriapsis ) );
    testDataMap->insert( FieldDataPair( field_types::state::trueAnomaly,
                                        testFieldValueTrueAnomaly ) );

    // Create Cartesian state extractor.
    KeplerStateExtractor testKeplerStateExtractor;

    // Set flag.
    bool isSemiMajorAxisFound = true;

    // Try to extract, which should result in a runtime error.
    try
    {
        // Extract test data map.
        std::shared_ptr< Vector6d > returnedKeplerianElements =
                testKeplerStateExtractor.extract( testDataMap );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch ( std::runtime_error )
    {
        isSemiMajorAxisFound = false;
    }

    // Check value of flag.
    BOOST_CHECK( !isSemiMajorAxisFound );
}

//! Test if the extract function throws the necessary exceptions.
BOOST_AUTO_TEST_CASE( keplerStateExtractor_MissingEccentricity )
{
    // Using declaration.
    using namespace ephemerides;
    namespace field_types = input_output::field_types;

    // Create parsed data line map pointer.
    // Define a new type: pair of field type and pointer to value.
    typedef std::pair< input_output::FieldType,
                       parsed_data_vector_utilities::FieldValuePtr > FieldDataPair;

    // Create test strings, based on Earth orbital elements at JD = 2456074.5 in SI units [1].
    std::string testSemiMajorAxis = "149641767.7265875";
    std::string testInclination = "0.000038294684687687365297820321195993";
    std::string testLongitudeOfAscendingNode = "3.3677017965694967895481414920601";
    std::string testArgumentOfPeriapsis = "4.7066684303635934184870904755709";
    std::string testTrueAnomaly = "2.4817611918827110773177514320574";

    // Store strings as field values.
    parsed_data_vector_utilities::FieldValuePtr testFieldValueSemiMajorAxis(
                new input_output::FieldValue( field_types::state::semiMajorAxis,
                                                     testSemiMajorAxis ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueInclination(
                new input_output::FieldValue( field_types::state::inclination,
                                                     testInclination ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueLongitudeOfAscendingNode(
                new input_output::FieldValue( field_types::state::longitudeOfAscendingNode,
                                                     testLongitudeOfAscendingNode ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueArgumentOfPeriapsis(
                new input_output::FieldValue( field_types::state::argumentOfPeriapsis,
                                                     testArgumentOfPeriapsis ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueTrueAnomaly(
                new input_output::FieldValue( field_types::state::trueAnomaly,
                                                     testTrueAnomaly ) );

    // Create a new pointer to data map.
    parsed_data_vector_utilities::ParsedDataLineMapPtr testDataMap =
            std::make_shared< parsed_data_vector_utilities::ParsedDataLineMap >(
                std::map< input_output::FieldType,
                          parsed_data_vector_utilities::FieldValuePtr >( ) );

    // Store field values in data map.
    testDataMap->insert( FieldDataPair( field_types::state::semiMajorAxis,
                                        testFieldValueSemiMajorAxis ) );
    testDataMap->insert( FieldDataPair( field_types::state::inclination,
                                        testFieldValueInclination ) );
    testDataMap->insert( FieldDataPair( field_types::state::longitudeOfAscendingNode,
                                        testFieldValueLongitudeOfAscendingNode ) );
    testDataMap->insert( FieldDataPair( field_types::state::argumentOfPeriapsis,
                                        testFieldValueArgumentOfPeriapsis ) );
    testDataMap->insert( FieldDataPair( field_types::state::trueAnomaly,
                                        testFieldValueTrueAnomaly ) );

    // Create Cartesian state extractor.
    KeplerStateExtractor testKeplerStateExtractor;

    // Set flag.
    bool isEccentricityFound = true;

    // Try to extract, which should result in a runtime error.
    try
    {
        // Extract test data map.
        std::shared_ptr< Vector6d > returnedKeplerianElements =
                testKeplerStateExtractor.extract( testDataMap );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch ( std::runtime_error )
    {
        isEccentricityFound = false;
    }

    // Check value of flag.
    BOOST_CHECK( !isEccentricityFound );
}

//! Test if the extract function throws the necessary exceptions.
BOOST_AUTO_TEST_CASE( keplerStateExtractor_MissingInclination )
{
    // Using declaration.
    using namespace ephemerides;
    namespace field_types = input_output::field_types;

    // Create parsed data line map pointer.
    // Define a new type: pair of field type and pointer to value.
    typedef std::pair< input_output::FieldType,
                       parsed_data_vector_utilities::FieldValuePtr > FieldDataPair;

    // Create test strings, based on Earth orbital elements at JD = 2456074.5 in SI units [1].
    std::string testSemiMajorAxis = "149641767.7265875";
    std::string testEccentricity = "0.01625818315929578";
    std::string testLongitudeOfAscendingNode = "3.3677017965694967895481414920601";
    std::string testArgumentOfPeriapsis = "4.7066684303635934184870904755709";
    std::string testTrueAnomaly = "2.4817611918827110773177514320574";

    // Store strings as field values.
    parsed_data_vector_utilities::FieldValuePtr testFieldValueSemiMajorAxis(
                new input_output::FieldValue( field_types::state::semiMajorAxis,
                                                     testSemiMajorAxis ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueEccentricity(
                new input_output::FieldValue( field_types::state::eccentricity,
                                                     testEccentricity ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueLongitudeOfAscendingNode(
                new input_output::FieldValue( field_types::state::longitudeOfAscendingNode,
                                                     testLongitudeOfAscendingNode ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueArgumentOfPeriapsis(
                new input_output::FieldValue( field_types::state::argumentOfPeriapsis,
                                                     testArgumentOfPeriapsis ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueTrueAnomaly(
                new input_output::FieldValue( field_types::state::trueAnomaly,
                                                     testTrueAnomaly ) );

    // Create a new pointer to data map.
    parsed_data_vector_utilities::ParsedDataLineMapPtr testDataMap =
            std::make_shared< parsed_data_vector_utilities::ParsedDataLineMap >(
                std::map< input_output::FieldType,
                          parsed_data_vector_utilities::FieldValuePtr >( ) );

    // Store field values in data map.
    testDataMap->insert( FieldDataPair( field_types::state::semiMajorAxis,
                                        testFieldValueSemiMajorAxis ) );
    testDataMap->insert( FieldDataPair( field_types::state::eccentricity,
                                        testFieldValueEccentricity ) );
    testDataMap->insert( FieldDataPair( field_types::state::longitudeOfAscendingNode,
                                        testFieldValueLongitudeOfAscendingNode ) );
    testDataMap->insert( FieldDataPair( field_types::state::argumentOfPeriapsis,
                                        testFieldValueArgumentOfPeriapsis ) );
    testDataMap->insert( FieldDataPair( field_types::state::trueAnomaly,
                                        testFieldValueTrueAnomaly ) );

    // Create Cartesian state extractor.
    KeplerStateExtractor testKeplerStateExtractor;

    // Set flag.
    bool isInclinationFound = true;

    // Try to extract, which should result in a runtime error.
    try
    {
        // Extract test data map.
        std::shared_ptr< Vector6d > returnedKeplerianElements =
                testKeplerStateExtractor.extract( testDataMap );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch ( std::runtime_error )
    {
        isInclinationFound = false;
    }

    // Check value of flag.
    BOOST_CHECK( !isInclinationFound );
}

//! Test if the extract function throws the necessary exceptions.
BOOST_AUTO_TEST_CASE( keplerStateExtractor_MissingLongitudeOfAscendingNode )
{
    // Using declaration.
    using namespace ephemerides;
    namespace field_types = input_output::field_types;

    // Create parsed data line map pointer.
    // Define a new type: pair of field type and pointer to value.
    typedef std::pair< input_output::FieldType,
                       parsed_data_vector_utilities::FieldValuePtr > FieldDataPair;

    // Create test strings, based on Earth orbital elements at JD = 2456074.5 in SI units [1].
    std::string testSemiMajorAxis = "149641767.7265875";
    std::string testEccentricity = "0.01625818315929578";
    std::string testInclination = "0.000038294684687687365297820321195993";
    std::string testArgumentOfPeriapsis = "4.7066684303635934184870904755709";
    std::string testTrueAnomaly = "2.4817611918827110773177514320574";

    // Store strings as field values.
    parsed_data_vector_utilities::FieldValuePtr testFieldValueSemiMajorAxis(
                new input_output::FieldValue( field_types::state::semiMajorAxis,
                                                     testSemiMajorAxis ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueEccentricity(
                new input_output::FieldValue( field_types::state::eccentricity,
                                                     testEccentricity ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueInclination(
                new input_output::FieldValue( field_types::state::inclination,
                                                     testInclination ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueArgumentOfPeriapsis(
                new input_output::FieldValue( field_types::state::argumentOfPeriapsis,
                                                     testArgumentOfPeriapsis ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueTrueAnomaly(
                new input_output::FieldValue( field_types::state::trueAnomaly,
                                                     testTrueAnomaly ) );

    // Create a new pointer to data map.
    parsed_data_vector_utilities::ParsedDataLineMapPtr testDataMap =
            std::make_shared< parsed_data_vector_utilities::ParsedDataLineMap >(
                std::map< input_output::FieldType,
                          parsed_data_vector_utilities::FieldValuePtr >( ) );

    // Store field values in data map.
    testDataMap->insert( FieldDataPair( field_types::state::semiMajorAxis,
                                        testFieldValueSemiMajorAxis ) );
    testDataMap->insert( FieldDataPair( field_types::state::eccentricity,
                                        testFieldValueEccentricity ) );
    testDataMap->insert( FieldDataPair( field_types::state::inclination,
                                        testFieldValueInclination ) );
    testDataMap->insert( FieldDataPair( field_types::state::argumentOfPeriapsis,
                                        testFieldValueArgumentOfPeriapsis ) );
    testDataMap->insert( FieldDataPair( field_types::state::trueAnomaly,
                                        testFieldValueTrueAnomaly ) );

    // Create Cartesian state extractor.
    KeplerStateExtractor testKeplerStateExtractor;

    // Set flag.
    bool isLongitudeOfAscendingNodeFound = true;

    // Try to extract, which should result in a runtime error.
    try
    {
        // Extract test data map.
        std::shared_ptr< Vector6d > returnedKeplerianElements =
                testKeplerStateExtractor.extract( testDataMap );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch ( std::runtime_error )
    {
        isLongitudeOfAscendingNodeFound = false;
    }

    // Check value of flag.
    BOOST_CHECK( !isLongitudeOfAscendingNodeFound );
}

//! Test if the extract function throws the necessary exceptions.
BOOST_AUTO_TEST_CASE( keplerStateExtractor_MissingArgumentOfPeriapsis )
{
    // Using declaration.
    using namespace ephemerides;
    namespace field_types = input_output::field_types;

    // Create parsed data line map pointer.
    // Define a new type: pair of field type and pointer to value.
    typedef std::pair< input_output::FieldType,
                       parsed_data_vector_utilities::FieldValuePtr > FieldDataPair;

    // Create test strings, based on Earth orbital elements at JD = 2456074.5 in SI units [1].
    std::string testSemiMajorAxis = "149641767.7265875";
    std::string testEccentricity = "0.01625818315929578";
    std::string testInclination = "0.000038294684687687365297820321195993";
    std::string testLongitudeOfAscendingNode = "3.3677017965694967895481414920601";
    std::string testTrueAnomaly = "2.4817611918827110773177514320574";

    // Store strings as field values.
    parsed_data_vector_utilities::FieldValuePtr testFieldValueSemiMajorAxis(
                new input_output::FieldValue( field_types::state::semiMajorAxis,
                                                     testSemiMajorAxis ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueEccentricity(
                new input_output::FieldValue( field_types::state::eccentricity,
                                                     testEccentricity ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueInclination(
                new input_output::FieldValue( field_types::state::inclination,
                                                     testInclination ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueLongitudeOfAscendingNode(
                new input_output::FieldValue( field_types::state::longitudeOfAscendingNode,
                                                     testLongitudeOfAscendingNode ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueTrueAnomaly(
                new input_output::FieldValue( field_types::state::trueAnomaly,
                                                     testTrueAnomaly ) );

    // Create a new pointer to data map.
    parsed_data_vector_utilities::ParsedDataLineMapPtr testDataMap =
            std::make_shared< parsed_data_vector_utilities::ParsedDataLineMap >(
                std::map< input_output::FieldType,
                          parsed_data_vector_utilities::FieldValuePtr >( ) );

    // Store field values in data map.
    testDataMap->insert( FieldDataPair( field_types::state::semiMajorAxis,
                                        testFieldValueSemiMajorAxis ) );
    testDataMap->insert( FieldDataPair( field_types::state::eccentricity,
                                        testFieldValueEccentricity ) );
    testDataMap->insert( FieldDataPair( field_types::state::inclination,
                                        testFieldValueInclination ) );
    testDataMap->insert( FieldDataPair( field_types::state::longitudeOfAscendingNode,
                                        testFieldValueLongitudeOfAscendingNode ) );
    testDataMap->insert( FieldDataPair( field_types::state::trueAnomaly,
                                        testFieldValueTrueAnomaly ) );

    // Create Cartesian state extractor.
    KeplerStateExtractor testKeplerStateExtractor;

    // Set flag.
    bool isArgumentOfPeriapsisFound = true;

    // Try to extract, which should result in a runtime error.
    try
    {
        // Extract test data map.
        std::shared_ptr< Vector6d > returnedKeplerianElements =
                testKeplerStateExtractor.extract( testDataMap );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch ( std::runtime_error )
    {
        isArgumentOfPeriapsisFound = false;
    }

    // Check value of flag.
    BOOST_CHECK( !isArgumentOfPeriapsisFound );
}

//! Test if the extract function throws the necessary exceptions.
BOOST_AUTO_TEST_CASE( keplerStateExtractor_MissingTrueOrMeanAnomaly )
{
    // Using declaration.
    using namespace ephemerides;
    namespace field_types = input_output::field_types;

    // Create parsed data line map pointer.
    // Define a new type: pair of field type and pointer to value.
    typedef std::pair< input_output::FieldType,
                       parsed_data_vector_utilities::FieldValuePtr > FieldDataPair;

    // Create test strings, based on Earth orbital elements at JD = 2456074.5 in SI units [1].
    std::string testSemiMajorAxis = "149641767.7265875";
    std::string testEccentricity = "0.01625818315929578";
    std::string testInclination = "0.000038294684687687365297820321195993";
    std::string testLongitudeOfAscendingNode = "3.3677017965694967895481414920601";
    std::string testArgumentOfPeriapsis = "4.7066684303635934184870904755709";

    // Store strings as field values.
    parsed_data_vector_utilities::FieldValuePtr testFieldValueSemiMajorAxis(
                new input_output::FieldValue( field_types::state::semiMajorAxis,
                                                     testSemiMajorAxis ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueEccentricity(
                new input_output::FieldValue( field_types::state::eccentricity,
                                                     testEccentricity ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueInclination(
                new input_output::FieldValue( field_types::state::inclination,
                                                     testInclination ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueLongitudeOfAscendingNode(
                new input_output::FieldValue( field_types::state::longitudeOfAscendingNode,
                                                     testLongitudeOfAscendingNode ) );
    parsed_data_vector_utilities::FieldValuePtr testFieldValueArgumentOfPeriapsis(
                new input_output::FieldValue( field_types::state::argumentOfPeriapsis,
                                                     testArgumentOfPeriapsis ) );

    // Create a new pointer to data map.
    parsed_data_vector_utilities::ParsedDataLineMapPtr testDataMap =
            std::make_shared< parsed_data_vector_utilities::ParsedDataLineMap >(
                std::map< input_output::FieldType,
                          parsed_data_vector_utilities::FieldValuePtr >( ) );

    // Store field values in data map.
    testDataMap->insert( FieldDataPair( field_types::state::semiMajorAxis,
                                        testFieldValueSemiMajorAxis ) );
    testDataMap->insert( FieldDataPair( field_types::state::eccentricity,
                                        testFieldValueEccentricity ) );
    testDataMap->insert( FieldDataPair( field_types::state::inclination,
                                        testFieldValueInclination ) );
    testDataMap->insert( FieldDataPair( field_types::state::longitudeOfAscendingNode,
                                        testFieldValueLongitudeOfAscendingNode ) );
    testDataMap->insert( FieldDataPair( field_types::state::argumentOfPeriapsis,
                                        testFieldValueArgumentOfPeriapsis ) );

    // Create Cartesian state extractor.
    KeplerStateExtractor testKeplerStateExtractor;

    // Set flag.
    bool isTrueAnomalyFound = true;

    // Try to extract, which should result in a runtime error.
    try
    {
        // Extract test data map.
        std::shared_ptr< Vector6d > returnedKeplerianElements =
                testKeplerStateExtractor.extract( testDataMap );
    }

    // Catch the expected runtime error, and set the boolean flag to false.
    catch ( std::runtime_error )
    {
        isTrueAnomalyFound = false;
    }

    // Check value of flag.
    BOOST_CHECK( !isTrueAnomalyFound );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


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

#ifndef VELOCITY_CONTROLLER__SMOOTH_STOP_HPP_
#define VELOCITY_CONTROLLER__SMOOTH_STOP_HPP_

#include <rclcpp/rclcpp.hpp>

#include <boost/optional.hpp>

#include <algorithm>
#include <cmath>
#include <limits>
#include <utility>
#include <vector>

/**
 * @brief Smooth stop class to implement vehicle specific deceleration profiles
 */
class SmoothStop
{
public:
  /**
   * @brief initialize the state of the smooth stop
   * @param [in] pred_vel_in_target predicted ego velocity when the stop command will be executed
   * @param [in] pred_stop_dist predicted stop distance when the stop command will be executed
   */
  void init(const double pred_vel_in_target, const double pred_stop_dist)
  {
    weak_acc_time_ = rclcpp::Clock{RCL_ROS_TIME}.now();

    // when distance to stopline is near the car
    if (pred_stop_dist < std::numeric_limits<double>::epsilon()) {
      strong_acc_ = params_.min_strong_acc;
      return;
    }

    strong_acc_ = -std::pow(pred_vel_in_target, 2) / (2 * pred_stop_dist);
    strong_acc_ = std::max(std::min(strong_acc_, params_.max_strong_acc), params_.min_strong_acc);
  }

  /**
   * @brief set the parameters of this smooth stop
   * @param [in] max_strong_acc maximum strong acceleration value [m/s²]
   * @param [in] min_strong_acc minumum strong acceleration value [m/s²]
   * @param [in] weak_acc weak acceleration value [m/s²]
   * @param [in] weak_stop_acc weak stopping acceleration value [m/s²]
   * @param [in] strong_stop_acc strong stopping acceleration value [m/s²]
   * @param [in] min_fast_vel minumum velocity to consider ego to be running fast [m/s]
   * @param [in] min_running_vel minimum velocity to consider ego to be running [m/s]
   * @param [in] min_running_acc minimum acceleration to consider ego to be running [m/s]
   * @param [in] weak_stop_time time allowed for stopping with a weak acceleration [s]
   * @param [in] weak_stop_dist distance to the stop point bellow which a weak accel is applied [m]
   * @param [in] strong_stop_dist distance to the stop point bellow which a strong accel is applied
   * [m]
   */
  void setParams(
    double max_strong_acc, double min_strong_acc, double weak_acc, double weak_stop_acc,
    double strong_stop_acc, double min_fast_vel, double min_running_vel, double min_running_acc,
    double weak_stop_time, double weak_stop_dist, double strong_stop_dist)
  {
    params_.max_strong_acc = max_strong_acc;
    params_.min_strong_acc = min_strong_acc;
    params_.weak_acc = weak_acc;
    params_.weak_stop_acc = weak_stop_acc;
    params_.strong_stop_acc = strong_stop_acc;

    params_.min_fast_vel = min_fast_vel;
    params_.min_running_vel = min_running_vel;
    params_.min_running_acc = min_running_acc;
    params_.weak_stop_time = weak_stop_time;

    params_.weak_stop_dist = weak_stop_dist;
    params_.strong_stop_dist = strong_stop_dist;
  }

  /**
   * @brief predict time when car stops by fitting some latest observed velocity history
   *        with linear function (v = at + b)
   * @param [in] vel_hist history of previous ego velocities as (rclcpp::Time, double[m/s]) pairs
   */
  boost::optional<double> calcTimeToStop(
    const std::vector<std::pair<rclcpp::Time, double>> & vel_hist) const
  {
    // return when vel_hist is empty
    const size_t vel_hist_size = vel_hist.size();
    if (vel_hist_size == 0) {
      return {};
    }

    // calculate some variables for fitting
    const rclcpp::Time current_ros_time = rclcpp::Clock{RCL_ROS_TIME}.now();
    double mean_t = 0.0;
    double mean_v = 0.0;
    double sum_tv = 0.0;
    double sum_tt = 0.0;
    for (const auto & vel : vel_hist) {
      const double t = (vel.first - current_ros_time).seconds();
      const double v = vel.second;

      mean_t += t / vel_hist_size;
      mean_v += v / vel_hist_size;
      sum_tv += t * v;
      sum_tt += t * t;
    }

    // return when gradient a (of v = at + b) cannot be calculated.
    // See the following calculation of a
    if (
      std::abs(vel_hist_size * mean_t * mean_t - sum_tt) < std::numeric_limits<double>::epsilon()) {
      return {};
    }

    // calculate coefficients of linear function (v = at + b)
    const double a =
      (vel_hist_size * mean_t * mean_v - sum_tv) / (vel_hist_size * mean_t * mean_t - sum_tt);
    const double b = mean_v - a * mean_t;

    // return when v is independent of time (v = b)
    if (std::abs(a) < std::numeric_limits<double>::epsilon()) {
      return {};
    }

    // calculate time to stop by substituting v = 0 for v = at + b
    const double time_to_stop = -b / a;
    if (time_to_stop > 0) {
      return time_to_stop;
    }

    return {};
  }

  /**
   * @brief calculate accel command while stopping
   *        Decrease velocity with strong_acc_,
   *        then loose brake pedal with params_.weak_acc to stop smoothly
   *        If the car is still running, input params_.weak_stop_acc
   *        and then params_.strong_stop_acc in steps not to exceed stopline too much
   * @param [in] stop_dist distance left to travel before stopping [m]
   * @param [in] current_vel current velocity of ego [m/s]
   * @param [in] current_acc current acceleration of ego [m/s²]
   * @param [in] vel_hist history of previous ego velocities as (rclcpp::Time, double[m/s]) pairs
   * @param [in] delay_time assumed time delay when the stop command will actually be executed
   */
  double calculate(
    const double stop_dist, const double current_vel, const double current_acc,
    const std::vector<std::pair<rclcpp::Time, double>> & vel_hist, const double delay_time)
  {
    // predict time to stop
    const auto time_to_stop = calcTimeToStop(vel_hist);

    // calculate some flags
    const bool is_fast_vel = std::abs(current_vel) > params_.min_fast_vel;
    const bool is_running = std::abs(current_vel) > params_.min_running_vel ||
                            std::abs(current_acc) > params_.min_running_acc;

    // when exceeding the stopline (stop_dist is negative in these cases.)
    if (stop_dist < params_.strong_stop_dist) {  // when exceeding the stopline much
      return params_.strong_stop_acc;
    } else if (stop_dist < params_.weak_stop_dist) {  // when exceeding the stopline a bit
      return params_.weak_stop_acc;
    }

    // when the car is running
    if (is_running) {
      // when the car will not stop in a certain time
      if (time_to_stop && *time_to_stop > params_.weak_stop_time + delay_time) {
        return strong_acc_;
      } else if (!time_to_stop && is_fast_vel) {
        return strong_acc_;
      }

      weak_acc_time_ = rclcpp::Clock{RCL_ROS_TIME}.now();
      return params_.weak_acc;
    }

    // for 0.5 seconds after the car stopped
    if ((rclcpp::Clock{RCL_ROS_TIME}.now() - weak_acc_time_).seconds() < 0.5) {
      return params_.weak_acc;
    }

    // when the car is not running
    return params_.strong_stop_acc;
  }

private:
  struct Params
  {
    double max_strong_acc;
    double min_strong_acc;
    double weak_acc;
    double weak_stop_acc;
    double strong_stop_acc;

    double min_fast_vel;
    double min_running_vel;
    double min_running_acc;
    double weak_stop_time;

    double weak_stop_dist;
    double strong_stop_dist;
  };
  Params params_;

  double strong_acc_;
  rclcpp::Time weak_acc_time_;
};

#endif  // VELOCITY_CONTROLLER__SMOOTH_STOP_HPP_