#ifndef G2O_TARGET_TYPES_6D_HPP_
#define G2O_TARGET_TYPES_6D_HPP_

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/base_unary_edge.h>
#include <Eigen/Core>

using namespace g2o;

typedef Eigen::Matrix<double,6,1> Vector6d;
typedef Eigen::Matrix<double,6,6> Matrix6d;

// This header file specifies a set of types for the different
// tracking examples; note that

class VertexPosition3D : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  VertexPosition3D()
  {
  }

  virtual void setToOriginImpl() {
    _estimate.setZero();
  }

  virtual void oplusImpl(const double* update)
  {
    _estimate[0] += update[0];
    _estimate[1] += update[1];
    _estimate[2] += update[2];
  }

  virtual bool read(std::istream& /*is*/)
  {
    return false;
  }

  virtual bool write(std::ostream& /*os*/) const
  {
    return false;
  }

};

class PositionVelocity3DEdge
{
};

class VertexPositionVelocity3D : public g2o::BaseVertex<6, Vector6d>
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  VertexPositionVelocity3D()
  {
  }

  virtual void setToOriginImpl() {
    _estimate.setZero();
  }

  virtual void oplusImpl(const double* update)
  {
    for (int k = 0; k < 6; k++)
      _estimate[k] += update[k];
  }

  virtual bool read(std::istream& /*is*/)
  {
    return false;
  }

  virtual bool write(std::ostream& /*os*/) const
  {
    return false;
  }

};

// The odometry which links pairs of nodes together
class TargetOdometry3DEdge : public g2o::BaseBinaryEdge<6, Eigen::Vector3d, VertexPositionVelocity3D, VertexPositionVelocity3D>
{
public:
  TargetOdometry3DEdge(double dt, double noiseSigma)
  {
    _dt = dt;

    double q = noiseSigma * noiseSigma;
    double dt2 = dt * dt;

    // Process noise covariance matrix; this assumes an "impulse"
    // noise model; we add a small stabilising term on the diagonal to make it invertible
    Matrix6d Q=Matrix6d::Zero();
    Q(0, 0) = Q(1,1) = Q(2,2) = dt2*dt2*q/4 + 1e-4;
    Q(0, 3) = Q(1, 4) = Q(2, 5) = dt*dt2*q/2;
    Q(3, 3) = Q(4,4) = Q(5,5) = dt2 * q + 1e-4;
    Q(3, 0) = Q(4, 1) = Q(5, 2) = dt*dt2*q/2;

    setInformation(Q.inverse());
  }

  /** set the estimate of the to vertex, based on the estimate of the from vertex in the edge. */
  virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from, g2o::OptimizableGraph::Vertex* to){
    assert(from.size() == 1);
    const VertexPositionVelocity3D* vi = static_cast<const VertexPositionVelocity3D*>(*from.begin());
    VertexPositionVelocity3D* vj = static_cast<VertexPositionVelocity3D*>(to);
    Vector6d viEst=vi->estimate();
    Vector6d vjEst=viEst;

    for (int m = 0; m < 3; m++)
    {
      vjEst[m] += _dt * (vjEst[m+3] + 0.5 * _dt * _measurement[m]);
    }

    for (int m = 0; m < 3; m++)
    {
      vjEst[m+3] += _dt * _measurement[m];
    }
    vj->setEstimate(vjEst);
  }

  /** override in your class if it's not possible to initialize the vertices in certain combinations */
  virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& from, g2o::OptimizableGraph::Vertex* to) {
    //only works on sequential vertices
    const VertexPositionVelocity3D* vi = static_cast<const VertexPositionVelocity3D*>(*from.begin());
    return (to->id() - vi->id() == 1) ? 1.0 : -1.0;
  }

  void computeError()
  {
    const VertexPositionVelocity3D* vi = static_cast<const VertexPositionVelocity3D*>(_vertices[0]);
    const VertexPositionVelocity3D* vj = static_cast<const VertexPositionVelocity3D*>(_vertices[1]);

    for (int k = 0; k < 3; k++)
      {
        _error[k] = vi->estimate()[k] + _dt * (vi->estimate()[k+3] + 0.5 * _dt * _measurement[k]) - vj->estimate()[k];
      }
    for (int k = 3; k < 6; k++)
      {
        _error[k] = vi->estimate()[k] + _dt * _measurement[k-3]- vj->estimate()[k];
      }
  }

  virtual bool read(std::istream& /*is*/)
  {
    return false;
  }

  virtual bool write(std::ostream& /*os*/) const
  {
    return false;
  }

private:
  double _dt;
};

// The GPS
class GPSObservationEdgePositionVelocity3D : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPositionVelocity3D>
{
public:
  GPSObservationEdgePositionVelocity3D(const Eigen::Vector3d& measurement, double noiseSigma)
  {
    setMeasurement(measurement);
    setInformation(Eigen::Matrix3d::Identity() / (noiseSigma*noiseSigma));
  }

  void computeError()
  {
    const VertexPositionVelocity3D* v = static_cast<const VertexPositionVelocity3D*>(_vertices[0]);
    for (int k = 0; k < 3; k++)
      {
        _error[k] = v->estimate()[k] - _measurement[k];
      }
  }

  virtual bool read(std::istream& /*is*/)
  {
    return false;
  }

  virtual bool write(std::ostream& /*os*/) const
  {
    return false;
  }
};

#endif //  __TARGET_TYPES_6D_HPP__


//
// Created by Amir Masoud Abdol on 2019-04-30
//

#define BOOST_TEST_DYN_LINK

#define BOOST_TEST_MODULE PoliciesTest Tests

#include "nlohmann/json.hpp"
#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>
namespace tt = boost::test_tools;
namespace utf = boost::unit_test;
namespace bdata = boost::unit_test::data;

#include "Policy.h"
#include "Experiment.h"

#include "policies_variables_and_functions.h"

using json = nlohmann::ordered_json;

using namespace arma;
using namespace sam;
using namespace std;

BOOST_AUTO_TEST_SUITE( PolicyTests )

BOOST_FIXTURE_TEST_SUITE( constructor, PoliciesVariablesAndFunctions)

BOOST_AUTO_TEST_CASE( simple_constructor ) {

  for (auto &def : all_possible_policies) {
    BOOST_CHECK_NO_THROW(Policy(def, lua));
  }

}

BOOST_AUTO_TEST_SUITE_END() // FIXTURE

BOOST_AUTO_TEST_SUITE( call_operator_test )

    BOOST_DATA_TEST_CASE_F(PoliciesVariablesAndFunctions,
                           binary_operators_on_stats,
                           bdata::random( (bdata::distribution=std::uniform_real_distribution<float>(1, 4))) ^
                           bdata::random( (bdata::distribution=std::uniform_int_distribution<int>(100, 1000))) ^
                           bdata::random( (bdata::distribution=std::bernoulli_distribution(0.5))) ^
                           bdata::xrange(1, 4),
                           mean, nobs, status, i) {

      arma::Row<float> data(nobs);
      data.randn(); data += mean;

      DependentVariable dv{data};
      Submission sub{0, 0, 0, 0, dv};

      std::string policy_def;

      policy_def = "mean < " + std::to_string(data.max());
      Policy lesser_mean{policy_def, lua};
      BOOST_TEST(lesser_mean(sub));
      BOOST_TEST(lesser_mean(dv));

      policy_def = "mean > " + std::to_string(data.min());
      Policy greater_mean{policy_def, lua};
      BOOST_TEST(greater_mean(sub));
      BOOST_TEST(greater_mean(dv));

      policy_def = "mean != " + std::to_string(mean);
      Policy non_equal_mean{policy_def, lua};
      BOOST_TEST(non_equal_mean(sub));
      BOOST_TEST(non_equal_mean(dv));

      policy_def = "nobs < " + std::to_string(nobs);
      Policy lesser_nobs{policy_def, lua};
      BOOST_TEST(not lesser_nobs(sub));
      BOOST_TEST(not lesser_nobs(dv));

      policy_def = "nobs <= " + std::to_string(nobs);
      Policy lesser_eq_nobs{policy_def, lua};
      BOOST_TEST(lesser_eq_nobs(sub));
      BOOST_TEST(lesser_eq_nobs(dv));

      policy_def = "nobs >= " + std::to_string(nobs);
      Policy greater_eq_nobs{policy_def, lua};
      BOOST_TEST(greater_eq_nobs(sub));
      BOOST_TEST(greater_eq_nobs(dv));

      policy_def = "nobs == " + std::to_string(nobs);
      Policy equal_nobs{policy_def, lua};
      BOOST_TEST(equal_nobs(sub));
      BOOST_TEST(equal_nobs(dv));

      // Testing the Status
      // ------------------

      dv.setHackedStatus(status);
      BOOST_TEST(dv.isHacked() == status);

      policy_def = "hacked";
      Policy is_hacked{policy_def, lua};
      BOOST_TEST(is_hacked(dv) == status);

      dv.setCandidateStatus(status);
      BOOST_TEST(dv.isCandidate() == status);

      policy_def = "candidate";
      Policy is_candidate{policy_def, lua};
      BOOST_TEST(is_candidate(dv) == status);

      // On Submission
      Submission s2{0, 1, 2, 3, dv};
      BOOST_TEST(s2.isHacked() == status);
      BOOST_TEST(s2.isCandidate() == status);
      BOOST_TEST(is_hacked(s2) == status);
      BOOST_TEST(is_candidate(s2) == status);

    }

//BOOST_DATA_TEST_CASE_F(PoliciesVariablesAndFunctions,
//                       binary,
//                       (
//                        bdata::random( bdata::distribution=std::uniform_real_distribution<float>(1, 4)) ^
//                        bdata::xrange(10)
//                       )
//                       * (
//                         bdata::random( bdata::distribution=std::uniform_real_distribution<float>(1, 4)) ^
//                         bdata::xrange(5)
//                        ),
//                       a, b, c, d) {
//
//  std::cout << "a: " << a << "b: " << b << "c: " << c << std::endl;
//}

  BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END() // SUITE

BOOST_AUTO_TEST_SUITE( PolicyChainTests )

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE( PolicyChainSetTests )

BOOST_AUTO_TEST_SUITE_END()


#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/foreach.hpp>

#include "BMS_Receiver.h"
#include <algorithm>

vector<BmsParamters>& CBMSReceiver::GetBmsDataContainer()
{
    return m_bmsDataContainer;
}
BmsParamStats& CBMSReceiver::GetParameterStats()
{
    return m_paramstat;
}

bool CBMSReceiver::ParseDataFromConsole(string data, BmsParamters& params)
{
    std::stringstream ss;
    ss.str(data);

    try
    {
        boost::property_tree::ptree pt;
	    boost::property_tree::read_json(ss, pt);
    	params.jsonData = data;
    	params.temperature = pt.get<float>("temperature");
    	params.stateOfCharge = pt.get<float>("soc");
    	params.chargeRate = pt.get<float>("chargingrate");
    }
    catch(std::exception const& e)
    {
        std::cerr << e.what() << std::endl;
        return false;
    }

    return true;
}

void CBMSReceiver::PerformMinMaxCalculation()
{

    auto data = std::minmax_element(begin(m_bmsDataContainer), end(m_bmsDataContainer),
                                    [](BmsParamters& p1, BmsParamters& p2)
                                    {
                                        return p1.temperature < p2.temperature;
                                    });

    m_paramstat.minTemperature = data.first->temperature;
    m_paramstat.maxTemperature = data.second->temperature;

    data = std::minmax_element(begin(m_bmsDataContainer), end(m_bmsDataContainer),
                                    [](BmsParamters& p1, BmsParamters& p2)
                                    {
                                        return p1.stateOfCharge < p2.stateOfCharge;
                                    });

    m_paramstat.minSoc = data.first->stateOfCharge;
    m_paramstat.maxSoc = data.second->stateOfCharge;

    data = std::minmax_element(begin(m_bmsDataContainer), end(m_bmsDataContainer),
                                    [](BmsParamters& p1, BmsParamters& p2)
                                    {
                                        return p1.chargeRate < p2.chargeRate;
                                    });

    m_paramstat.minChargeRate = data.first->chargeRate;
    m_paramstat.maxChargeRate = data.second->chargeRate;
}

static float MovingAvg(float* ptrArrNum, float* ptrSum, int pos, int len, float nextNum)
{
    //Subtract the oldest number from the prev sum, add the new number
    *ptrSum = *ptrSum - ptrArrNum[pos] + nextNum;
    //Assign the nextNum to the position in the array
    ptrArrNum[pos] = nextNum;
    //return the average
    return *ptrSum / len;
}

void CBMSReceiver::PerformSimpleMovingAverage()
{
    int pos = 0;
    float sum, tTemp, tSoc, tCr;

    tTemp = tSoc = tCr = 0;
    // Moving average of temperature, SoC, CR
    for (size_t i = 0; i < m_bmsDataContainer.size(); ++i)
    {
        sum = tTemp;
        m_paramstat.avgTemperature = MovingAvg(m_temperature, &sum, pos, SMA_MAX_LEN, m_bmsDataContainer.at(i).temperature);
        tTemp = sum;

        sum = tSoc;
        m_paramstat.avgSoc = MovingAvg(m_stateOfCharge, &sum, pos, SMA_MAX_LEN, m_bmsDataContainer.at(i).stateOfCharge);
        tSoc = sum;

        sum = tCr;
        m_paramstat.avgChargeRate = MovingAvg(m_chargeRate, &sum, pos, SMA_MAX_LEN, m_bmsDataContainer.at(i).chargeRate);
        tCr = sum;

        pos++;
        if (pos >= SMA_MAX_LEN)
        {
            pos = 0;
        }
    }
}

void CBMSReceiver::CalculateBmsParamStatistics()
{
    PerformMinMaxCalculation();
    PerformSimpleMovingAverage();
}

void CBMSReceiver::PrintDataToConsole()
{
    cout << "________________BMS Statistrics________________" << endl;

    cout << "Minimum temperature        : " << m_paramstat.minTemperature << endl;
    cout << "Maximum temperature        : " << m_paramstat.maxTemperature << endl;
    cout << "Minimum state of charge    : " << m_paramstat.minSoc << endl;
    cout << "Maximum State of charge    : " << m_paramstat.maxSoc << endl;
    cout << "Minimum charge rate        : " << m_paramstat.minChargeRate << endl;
    cout << "Maximum charge rate        : " << m_paramstat.maxChargeRate << endl;
    cout << "Moving average temperature : " << m_paramstat.avgTemperature << endl;
    cout << "Moving average SOC         : " << m_paramstat.avgSoc << endl;
    cout << "Moving average charge rate : " << m_paramstat.avgChargeRate << endl;
 }

void CBMSReceiver::GetDataFromConsole()
{
    std::string data;
    for (int i = 0; i < 100; ++i)
    {
        BmsParamters param;
        std::getline(std::cin, data);
        if (ParseDataFromConsole(data, param))
	    {
            m_bmsDataContainer.push_back(param);
	    }
    }
    CalculateBmsParamStatistics();
    PrintDataToConsole();
}


#ifndef SKYLARK_RANDOM_MATRICES_HPP
#define SKYLARK_RANDOM_MATRICES_HPP

#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_01.hpp>

#include "../utility/typer.hpp"

namespace skylark { namespace base {

/**
 * Generate random matrix using specificed distribution
 * (i.i.d samples).
 *
 * Implementation for local matrices.
 *
 * \param A Output matrix
 * \param m,n Number of rows and colunt
 * \param dist Distribution object
 * \param context Skylark context.
 */
template<typename T, template<typename> class DistributionType>
void RandomMatrix(El::Matrix<T> &A, El::Int m, El::Int n,
    DistributionType<T> &dist, context_t &context) {

    random_samples_array_t< DistributionType<T> > entries =
        context.allocate_random_samples_array(m * n, dist);

    A.Resize(m, n);
    T *data = A.Buffer();

#ifdef SKYLARK_HAVE_OPENMP
#pragma omp parallel for collapse(2)
#endif
    for(size_t j = 0; j < n; j++)
        for(size_t i = 0; i < m; i++)
            data[j * m + i] = entries[j * m + i];
}

template<typename T, template<typename, typename> class DistributionType>
void RandomMatrix(El::Matrix<T> &A, El::Int m, El::Int n,
    DistributionType<T, T> &dist, context_t &context) {

    random_samples_array_t< DistributionType<T, T> > entries =
        context.allocate_random_samples_array(m * n, dist);

    A.Resize(m, n);
    T *data = A.Buffer();

#ifdef SKYLARK_HAVE_OPENMP
#pragma omp parallel for collapse(2)
#endif
    for(size_t j = 0; j < n; j++)
        for(size_t i = 0; i < m; i++)
            data[j * m + i] = entries[j * m + i];
}

/**
 * Generate random matrix using specificed distribution
 * (i.i.d samples).
 *
 * Implementation for distributed matrices.
 *
 * \param A Output matrix
 * \param m,n Number of rows and colunt
 * \param dist Distribution object
 * \param context Skylark context.
 */
template<typename T, El::Distribution CD, El::Distribution RD,
         template<typename> class DistributionType>
void RandomMatrix(El::DistMatrix<T, CD, RD> &A, El::Int m, El::Int n,
    DistributionType<T> &dist, context_t &context) {

    random_samples_array_t< DistributionType<T> > entries =
        context.allocate_random_samples_array(m * n, dist);

    A.Resize(m, n);

    size_t m0 = A.LocalHeight();
    size_t n0 = A.LocalWidth();
    T *data = A.Buffer();

#ifdef SKYLARK_HAVE_OPENMP
#pragma omp parallel for collapse(2)
#endif
    for(size_t j = 0; j < n0; j++)
        for(size_t i = 0; i < m0; i++)
            data[j * m0 + i] = entries[A.GlobalCol(j) * m + A.GlobalRow(i)];
}

/**
 * Generate random matrix using specificed distribution
 * (i.i.d samples).
 *
 * Implementation for distributed matrices.
 *
 * \param A Output matrix
 * \param m,n Number of rows and colunt
 * \param dist Distribution object
 * \param context Skylark context.
 */
template<typename T, El::Distribution CD, El::Distribution RD,
         template<typename, typename> class DistributionType>
void RandomMatrix(El::DistMatrix<T, CD, RD> &A, El::Int m, El::Int n,
    DistributionType<T, T> &dist, context_t &context) {

    random_samples_array_t< DistributionType<T, T> > entries =
        context.allocate_random_samples_array(m * n, dist);

    A.Resize(m, n);

    size_t m0 = A.LocalHeight();
    size_t n0 = A.LocalWidth();
    T *data = A.Buffer();

#ifdef SKYLARK_HAVE_OPENMP
#pragma omp parallel for collapse(2)
#endif
    for(size_t j = 0; j < n0; j++)
        for(size_t i = 0; i < m0; i++)
            data[j * m0 + i] = entries[A.GlobalCol(j) * m + A.GlobalRow(i)];
}

/**
 * Generate random matrix with i.i.d standard Gaussian entries.
 *
 * \param A Output matrix.
 * \param m,n Number of rows and columns.
 * \param context Skylark context.
 */
template<typename MatrixType>
void GaussianMatrix(MatrixType &A, El::Int m, El::Int n,
    context_t &context) {
    typedef typename utility::typer_t<MatrixType>::value_type value_type;

    boost::random::normal_distribution<value_type> dist;
    RandomMatrix(A, m, n, dist, context);
}

/**
 * Generate random matrix with i.i.d [0,1) uniform entries.
 *
 * \param A Output matrix.
 * \param m,n Number of rows and columns.
 * \param context Skylark context.
 */
template<typename MatrixType>
void UniformMatrix(MatrixType &A, El::Int m, El::Int n,
    context_t &context) {
    typedef typename utility::typer_t<MatrixType>::value_type value_type;

    boost::random::uniform_01<value_type, value_type> dist;
    RandomMatrix(A, m, n, dist, context);
}

template<typename T>
void UniformMatrix(sparse_matrix_t<T> &A, El::Int m, El::Int n,
    context_t &context) {

    SKYLARK_THROW_EXCEPTION(unsupported_base_operation() <<
       error_msg("Uniform sparse matrix not supported "
           "and does not make sense."));
}

template<typename T>
void UniformMatrix(sparse_vc_star_matrix_t<T> &A, El::Int m, El::Int n,
    context_t &context) {

    SKYLARK_THROW_EXCEPTION(unsupported_base_operation() <<
        error_msg("Uniform sparse matrix not supported "
           "and does not make sense."));
}

} } // namespace skylark::base

#endif // SKYLARK_RANDOM_MATRICES_HPP


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef MCS_TEST_SYSTEM_HPP
#define MCS_TEST_SYSTEM_HPP

#include <boost/mpl/list.hpp>
#include <boost/mpl/vector.hpp>

#include <boost/units/base_dimension.hpp>
#include <boost/units/derived_dimension.hpp>
#include <boost/units/io.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/static_constant.hpp>
#include <boost/units/unit.hpp>
#include <boost/units/base_unit.hpp>
#include <boost/units/make_system.hpp>

namespace boost {

namespace units {

//[test_system_snippet_1

/// base dimension of length
struct length_base_dimension : base_dimension<length_base_dimension,1> { };
/// base dimension of mass
struct mass_base_dimension : base_dimension<mass_base_dimension,2> { };
/// base dimension of time
struct time_base_dimension : base_dimension<time_base_dimension,3> { };

//]

#if 0
//[test_system_snippet_2

typedef make_dimension_list<
    boost::mpl::list< dim< length_base_dimension,static_rational<1> > >
>::type   length_dimension;

typedef make_dimension_list<
    boost::mpl::list< dim< mass_base_dimension,static_rational<1> > >
>::type     mass_dimension;

typedef make_dimension_list<
    boost::mpl::list< dim< time_base_dimension,static_rational<1> > >
>::type     time_dimension;

//]
#endif

//[test_system_snippet_3
typedef length_base_dimension::dimension_type    length_dimension;
typedef mass_base_dimension::dimension_type      mass_dimension;
typedef time_base_dimension::dimension_type      time_dimension;
//]

#if 0
//[test_system_snippet_4

typedef make_dimension_list<
    boost::mpl::list< dim< length_base_dimension,static_rational<2> > >
>::type   area_dimension;

typedef make_dimension_list<
    boost::mpl::list< dim< mass_base_dimension,static_rational<1> >,
                      dim< length_base_dimension,static_rational<2> >,
                      dim< time_base_dimension,static_rational<-2> > >
>::type    energy_dimension;

//]
#endif

//[test_system_snippet_5
typedef derived_dimension<length_base_dimension,2>::type  area_dimension;
typedef derived_dimension<mass_base_dimension,1,
                          length_base_dimension,2,
                          time_base_dimension,-2>::type   energy_dimension;
//]

namespace test {

//[test_system_snippet_6

struct meter_base_unit : base_unit<meter_base_unit, length_dimension, 1> { };
struct kilogram_base_unit : base_unit<kilogram_base_unit, mass_dimension, 2> { };
struct second_base_unit : base_unit<second_base_unit, time_dimension, 3> { };

typedef make_system<
    meter_base_unit,
    kilogram_base_unit,
    second_base_unit>::type mks_system;

/// unit typedefs
typedef unit<dimensionless_type,mks_system>      dimensionless;

typedef unit<length_dimension,mks_system>        length;
typedef unit<mass_dimension,mks_system>          mass;
typedef unit<time_dimension,mks_system>          time;

typedef unit<area_dimension,mks_system>          area;
typedef unit<energy_dimension,mks_system>        energy;
//]

//[test_system_snippet_7
/// unit constants
BOOST_UNITS_STATIC_CONSTANT(meter,length);
BOOST_UNITS_STATIC_CONSTANT(meters,length);
BOOST_UNITS_STATIC_CONSTANT(kilogram,mass);
BOOST_UNITS_STATIC_CONSTANT(kilograms,mass);
BOOST_UNITS_STATIC_CONSTANT(second,time);
BOOST_UNITS_STATIC_CONSTANT(seconds,time);

BOOST_UNITS_STATIC_CONSTANT(square_meter,area);
BOOST_UNITS_STATIC_CONSTANT(square_meters,area);
BOOST_UNITS_STATIC_CONSTANT(joule,energy);
BOOST_UNITS_STATIC_CONSTANT(joules,energy);
//]

} // namespace test

//[test_system_snippet_8
template<> struct base_unit_info<test::meter_base_unit>
{
    static std::string name()               { return "meter"; }
    static std::string symbol()             { return "m"; }
};
//]

template<> struct base_unit_info<test::kilogram_base_unit>
{
    static std::string name()               { return "kilogram"; }
    static std::string symbol()             { return "kg"; }
};

template<> struct base_unit_info<test::second_base_unit>
{
    static std::string name()               { return "second"; }
    static std::string symbol()             { return "s"; }
};

} // namespace units

} // namespace boost

#endif // MCS_TEST_SYSTEM_HPP


#include "test_context.hpp"
#include <decimal.hpp>
#include <iostream>
#include <boost/algorithm/string.hpp>

namespace gda
{

int test_context::parse_rounding(const std::string& input)
{
    std::string value{input};

    boost::algorithm::trim(value);
    boost::algorithm::to_lower(value);

    if (value == "ceiling" || value == "up") {
        return std::decimal::FE_DEC_UPWARD;
    }

    if (value == "down") {
        return std::decimal::FE_DEC_DOWNWARD;
    }

    if (value == "floor") {
        return std::decimal::FE_DEC_TOWARD_ZERO;
    }
    // if (value == "half_down") {
    //     // return = ;
    // }

    if (value == "half_even") {
        return std::decimal::FE_DEC_TONEAREST;
    }

    if (value == "half_up") {
        return std::decimal::FE_DEC_TONEARESTFROMZERO;
    }
    // else if (value == "up") {
    //     return std::decimal::FE_DEC_UPWARD;
    // }
    // else if (value == "05up") {
    // }

    throw std::runtime_error("unhandled rounding: " + value);
}

int test_context::parse_min_exponent(const std::string& input)
{
    std::string value{input};
    boost::algorithm::trim(value);
    try {
        return std::stoi(value);
    }
    catch (std::exception& ex) {
        throw std::runtime_error("unable to parse minExponent: " + value);
    }
}

unsigned int test_context::parse_max_exponent(const std::string& input)
{
    std::string value{input};
    boost::algorithm::trim(value);
    try {
        return std::stoi(value);
    }
    catch (std::exception& ex) {
        throw std::runtime_error("unable to parse maxExponent: " + value);
    }
}

unsigned int test_context::parse_precision(const std::string& input)
{
    std::string value{input};
    boost::algorithm::trim(value);
    try {
        return std::stoi(value);
    }
    catch (std::exception& ex) {
        throw std::runtime_error("unable to parse minExponent: " + value);
    }
}

bool test_context::parse_clamp(const std::string& input)
{
    std::string value{input};
    boost::algorithm::trim(value);
    try {
        return std::stoi(value) != 0;
    }
    catch (std::exception& ex) {
        throw std::runtime_error("unable to parse clamp: " + value);
    }
}

bool test_context::apply_directive(const std::string& name, const std::string& value)
{
    auto directive{name};
    boost::algorithm::to_lower(directive);

    if (directive == "dectest") {
        return false;
    }

    if (directive == "version") {
        return false;
    }

    if (directive == "extended") {
        return false;
    }

    if (directive == "precision") {
        m_precision = parse_precision(value);
        return true;
    }

    if (directive == "rounding") {
        m_rounding = parse_rounding(value);
        return true;
    }

    if (directive == "maxexponent") {
        m_max_exponent = parse_max_exponent(value);
        return true;
    }

    if (directive == "minexponent") {
        m_min_exponent = parse_min_exponent(value);
        return true;
    }

    if (directive == "clamp") {
        m_clamp = parse_clamp(value);
        return true;
    }

    throw std::runtime_error("unhandled directive: " + name);
}

const std::optional<int>& test_context::rounding() const
{
    return m_rounding;
}

const std::optional<unsigned int>& test_context::precision() const
{
    return m_precision;
}

const std::optional<int>& test_context::min_exponent() const
{
    return m_min_exponent;
}

const std::optional<unsigned int>& test_context::max_exponent() const
{
    return m_max_exponent;
}

bool test_context::clamp() const
{
    return m_clamp;
}

void test_context::apply_rounding() const
{
    if (!rounding()) {
        return;
    }

    if (std::decimal::fe_dec_setround(*rounding()) != 0) {
        throw std::runtime_error("Error setting rounding mode: " + std::to_string(*rounding()));
    }
}

} // namespace gda

std::ostream& operator<<(std::ostream& os, gda::test_context& context)
{
    os  << "context {\n"
        << "    rounding     : " << (context.rounding() ? std::to_string(*context.rounding()) : "") << '\n'
        << "    precision    : " << (context.precision() ? std::to_string(*context.precision()) : "") << '\n'
        << "    min exponent : " << (context.min_exponent() ? std::to_string(*context.min_exponent()) : "") << '\n'
        << "    max exponent : " << (context.max_exponent() ? std::to_string(*context.max_exponent()) : "") << '\n'
        << "    clamp:       : " << context.clamp() << '\n'
        << "}";
    return os;
}


// Copyright (c) 2014 The The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "random.h"
#include "utilstrencodings.h"
#include "test/test_bitcoin.h"
#include "wallet/crypter.h"

#include <vector>

#include <boost/test/unit_test.hpp>
#include <openssl/aes.h>
#include <openssl/evp.h>

BOOST_FIXTURE_TEST_SUITE(wallet_crypto, BasicTestingSetup)

bool OldSetKeyFromPassphrase(const SecureString& strKeyData, const std::vector<unsigned char>& chSalt, const unsigned int nRounds, const unsigned int nDerivationMethod, unsigned char* chKey, unsigned char* chIV)
{
    if (nRounds < 1 || chSalt.size() != WALLET_CRYPTO_SALT_SIZE)
        return false;

    int i = 0;
    if (nDerivationMethod == 0)
        i = EVP_BytesToKey(EVP_aes_256_cbc(), EVP_sha512(), &chSalt[0],
                          (unsigned char *)&strKeyData[0], strKeyData.size(), nRounds, chKey, chIV);

    if (i != (int)WALLET_CRYPTO_KEY_SIZE)
    {
        memory_cleanse(chKey, sizeof(chKey));
        memory_cleanse(chIV, sizeof(chIV));
        return false;
    }
    return true;
}

bool OldEncrypt(const CKeyingMaterial& vchPlaintext, std::vector<unsigned char> &vchCiphertext, const unsigned char chKey[32], const unsigned char chIV[16])
{
    // max ciphertext len for a n bytes of plaintext is
    // n + AES_BLOCK_SIZE - 1 bytes
    int nLen = vchPlaintext.size();
    int nCLen = nLen + AES_BLOCK_SIZE, nFLen = 0;
    vchCiphertext = std::vector<unsigned char> (nCLen);

    EVP_CIPHER_CTX ctx;

    bool fOk = true;

    EVP_CIPHER_CTX_init(&ctx);
    if (fOk) fOk = EVP_EncryptInit_ex(&ctx, EVP_aes_256_cbc(), NULL, chKey, chIV) != 0;
    if (fOk) fOk = EVP_EncryptUpdate(&ctx, &vchCiphertext[0], &nCLen, &vchPlaintext[0], nLen) != 0;
    if (fOk) fOk = EVP_EncryptFinal_ex(&ctx, (&vchCiphertext[0]) + nCLen, &nFLen) != 0;
    EVP_CIPHER_CTX_cleanup(&ctx);

    if (!fOk) return false;

    vchCiphertext.resize(nCLen + nFLen);
    return true;
}

bool OldDecrypt(const std::vector<unsigned char>& vchCiphertext, CKeyingMaterial& vchPlaintext, const unsigned char chKey[32], const unsigned char chIV[16])
{
    // plaintext will always be equal to or lesser than length of ciphertext
    int nLen = vchCiphertext.size();
    int nPLen = nLen, nFLen = 0;

    vchPlaintext = CKeyingMaterial(nPLen);

    EVP_CIPHER_CTX ctx;

    bool fOk = true;

    EVP_CIPHER_CTX_init(&ctx);
    if (fOk) fOk = EVP_DecryptInit_ex(&ctx, EVP_aes_256_cbc(), NULL, chKey, chIV) != 0;
    if (fOk) fOk = EVP_DecryptUpdate(&ctx, &vchPlaintext[0], &nPLen, &vchCiphertext[0], nLen) != 0;
    if (fOk) fOk = EVP_DecryptFinal_ex(&ctx, (&vchPlaintext[0]) + nPLen, &nFLen) != 0;
    EVP_CIPHER_CTX_cleanup(&ctx);

    if (!fOk) return false;

    vchPlaintext.resize(nPLen + nFLen);
    return true;
}

class TestCrypter
{
public:
static void TestPassphraseSingle(const std::vector<unsigned char>& vchSalt, const SecureString& passphrase, uint32_t rounds,
                 const std::vector<unsigned char>& correctKey = std::vector<unsigned char>(),
                 const std::vector<unsigned char>& correctIV=std::vector<unsigned char>())
{
    unsigned char chKey[WALLET_CRYPTO_KEY_SIZE];
    unsigned char chIV[WALLET_CRYPTO_IV_SIZE];

    CCrypter crypt;
    crypt.SetKeyFromPassphrase(passphrase, vchSalt, rounds, 0);

    OldSetKeyFromPassphrase(passphrase, vchSalt, rounds, 0, chKey, chIV);

    BOOST_CHECK_MESSAGE(memcmp(chKey, crypt.chKey, sizeof(chKey)) == 0, \
        HexStr(chKey, chKey+sizeof(chKey)) + std::string(" != ") + HexStr(crypt.chKey, crypt.chKey + (sizeof crypt.chKey)));
    BOOST_CHECK_MESSAGE(memcmp(chIV, crypt.chIV, sizeof(chIV)) == 0, \
        HexStr(chIV, chIV+sizeof(chIV)) + std::string(" != ") + HexStr(crypt.chIV, crypt.chIV + (sizeof crypt.chIV)));

    if(!correctKey.empty())
        BOOST_CHECK_MESSAGE(memcmp(chKey, &correctKey[0], sizeof(chKey)) == 0, \
            HexStr(chKey, chKey+sizeof(chKey)) + std::string(" != ") + HexStr(correctKey.begin(), correctKey.end()));
    if(!correctIV.empty())
        BOOST_CHECK_MESSAGE(memcmp(chIV, &correctIV[0], sizeof(chIV)) == 0,
            HexStr(chIV, chIV+sizeof(chIV)) + std::string(" != ") + HexStr(correctIV.begin(), correctIV.end()));
}

static void TestPassphrase(const std::vector<unsigned char>& vchSalt, const SecureString& passphrase, uint32_t rounds,
                 const std::vector<unsigned char>& correctKey = std::vector<unsigned char>(),
                 const std::vector<unsigned char>& correctIV=std::vector<unsigned char>())
{
    TestPassphraseSingle(vchSalt, passphrase, rounds, correctKey, correctIV);
    for(SecureString::const_iterator i(passphrase.begin()); i != passphrase.end(); ++i)
        TestPassphraseSingle(vchSalt, SecureString(i, passphrase.end()), rounds);
}

static void TestDecrypt(const CCrypter& crypt, const std::vector<unsigned char>& vchCiphertext, \
                        const std::vector<unsigned char>& vchPlaintext = std::vector<unsigned char>())
{
    CKeyingMaterial vchDecrypted1;
    CKeyingMaterial vchDecrypted2;
    int result1, result2;
    result1 = crypt.Decrypt(vchCiphertext, vchDecrypted1);
    result2 = OldDecrypt(vchCiphertext, vchDecrypted2, crypt.chKey, crypt.chIV);
    BOOST_CHECK(result1 == result2);

    // These two should be equal. However, OpenSSL 1.0.1j introduced a change
    // that would zero all padding except for the last byte for failed decrypts.
    // This behavior was reverted for 1.0.1k.
    if (vchDecrypted1 != vchDecrypted2 && vchDecrypted1.size() >= AES_BLOCK_SIZE && SSLeay() == 0x100010afL)
    {
        for(CKeyingMaterial::iterator it = vchDecrypted1.end() - AES_BLOCK_SIZE; it != vchDecrypted1.end() - 1; it++)
            *it = 0;
    }

    BOOST_CHECK_MESSAGE(vchDecrypted1 == vchDecrypted2, HexStr(vchDecrypted1.begin(), vchDecrypted1.end()) + " != " + HexStr(vchDecrypted2.begin(), vchDecrypted2.end()));

    if (vchPlaintext.size())
        BOOST_CHECK(CKeyingMaterial(vchPlaintext.begin(), vchPlaintext.end()) == vchDecrypted2);
}

static void TestEncryptSingle(const CCrypter& crypt, const CKeyingMaterial& vchPlaintext,
                       const std::vector<unsigned char>& vchCiphertextCorrect = std::vector<unsigned char>())
{
    std::vector<unsigned char> vchCiphertext1;
    std::vector<unsigned char> vchCiphertext2;
    int result1 = crypt.Encrypt(vchPlaintext, vchCiphertext1);

    int result2 = OldEncrypt(vchPlaintext, vchCiphertext2, crypt.chKey, crypt.chIV);
    BOOST_CHECK(result1 == result2);
    BOOST_CHECK(vchCiphertext1 == vchCiphertext2);

    if (!vchCiphertextCorrect.empty())
        BOOST_CHECK(vchCiphertext2 == vchCiphertextCorrect);

    const std::vector<unsigned char> vchPlaintext2(vchPlaintext.begin(), vchPlaintext.end());

    if(vchCiphertext1 == vchCiphertext2)
        TestDecrypt(crypt, vchCiphertext1, vchPlaintext2);
}

static void TestEncrypt(const CCrypter& crypt, const std::vector<unsigned char>& vchPlaintextIn, \
                       const std::vector<unsigned char>& vchCiphertextCorrect = std::vector<unsigned char>())
{
    TestEncryptSingle(crypt, CKeyingMaterial(vchPlaintextIn.begin(), vchPlaintextIn.end()), vchCiphertextCorrect);
    for(std::vector<unsigned char>::const_iterator i(vchPlaintextIn.begin()); i != vchPlaintextIn.end(); ++i)
        TestEncryptSingle(crypt, CKeyingMaterial(i, vchPlaintextIn.end()));
}

};

BOOST_AUTO_TEST_CASE(passphrase) {
    // These are expensive.

    TestCrypter::TestPassphrase(ParseHex("0000deadbeef0000"), "test", 25000, \
                                ParseHex("fc7aba077ad5f4c3a0988d8daa4810d0d4a0e3bcb53af662998898f33df0556a"), \
                                ParseHex("cf2f2691526dd1aa220896fb8bf7c369"));

    std::string hash(GetRandHash().ToString());
    std::vector<unsigned char> vchSalt(8);
    GetRandBytes(&vchSalt[0], vchSalt.size());
    uint32_t rounds = insecure_rand();
    if (rounds > 30000)
        rounds = 30000;
    TestCrypter::TestPassphrase(vchSalt, SecureString(hash.begin(), hash.end()), rounds);
}

BOOST_AUTO_TEST_CASE(encrypt) {
    std::vector<unsigned char> vchSalt = ParseHex("0000deadbeef0000");
    BOOST_CHECK(vchSalt.size() == WALLET_CRYPTO_SALT_SIZE);
    CCrypter crypt;
    crypt.SetKeyFromPassphrase("passphrase", vchSalt, 25000, 0);
    TestCrypter::TestEncrypt(crypt, ParseHex("22bcade09ac03ff6386914359cfe885cfeb5f77ff0d670f102f619687453b29d"));

    for (int i = 0; i != 100; i++)
    {
        uint256 hash(GetRandHash());
        TestCrypter::TestEncrypt(crypt, std::vector<unsigned char>(hash.begin(), hash.end()));
    }

}

BOOST_AUTO_TEST_CASE(decrypt) {
    std::vector<unsigned char> vchSalt = ParseHex("0000deadbeef0000");
    BOOST_CHECK(vchSalt.size() == WALLET_CRYPTO_SALT_SIZE);
    CCrypter crypt;
    crypt.SetKeyFromPassphrase("passphrase", vchSalt, 25000, 0);

    // Some corner cases the came up while testing
    TestCrypter::TestDecrypt(crypt,ParseHex("795643ce39d736088367822cdc50535ec6f103715e3e48f4f3b1a60a08ef59ca"));
    TestCrypter::TestDecrypt(crypt,ParseHex("de096f4a8f9bd97db012aa9d90d74de8cdea779c3ee8bc7633d8b5d6da703486"));
    TestCrypter::TestDecrypt(crypt,ParseHex("32d0a8974e3afd9c6c3ebf4d66aa4e6419f8c173de25947f98cf8b7ace49449c"));
    TestCrypter::TestDecrypt(crypt,ParseHex("e7c055cca2faa78cb9ac22c9357a90b4778ded9b2cc220a14cea49f931e596ea"));
    TestCrypter::TestDecrypt(crypt,ParseHex("b88efddd668a6801d19516d6830da4ae9811988ccbaf40df8fbb72f3f4d335fd"));
    TestCrypter::TestDecrypt(crypt,ParseHex("8cae76aa6a43694e961ebcb28c8ca8f8540b84153d72865e8561ddd93fa7bfa9"));

    for (int i = 0; i != 100; i++)
    {
        uint256 hash(GetRandHash());
        TestCrypter::TestDecrypt(crypt, std::vector<unsigned char>(hash.begin(), hash.end()));
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/math/tools/rational.hpp>


#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <std_msgs/Bool.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

class LaserNoise
{
public:
    LaserNoise()
    {
        ros::NodeHandle private_nh("laser_noise");
        // Initialise the noise period and the rectacular area of noise bounded by the rectacle of which the 4 points are the corners.
        private_nh.param("noise_period", noise_period_, 30.0);
        private_nh.param("x_max", x_max_, 532.61);
        private_nh.param("x_min", x_min_, 531.10);
        private_nh.param("y_max", y_max_, 48.41);
        private_nh.param("y_min", y_min_, 41.24);

        //randomGen_.seed(time(NULL)); // seed the generator
        laser_sub_ = n_.subscribe<sensor_msgs::LaserScan>("scan", 20, &LaserNoise::laserReadCallBAck, this);
        pose_sub_ = n_.subscribe<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 20, &LaserNoise::poseCallback, this);
        reset_node_sub_ = n_.subscribe<std_msgs::Bool>("/noise_reset", 20, &LaserNoise::resetCallBack, this);

        scan_pub_ = n_.advertise<sensor_msgs::LaserScan>("scan_with_noise", 20);
        noise_active_pub_ = n_.advertise<std_msgs::Bool>("noise_active", 20);

        timerNoise_ = n_.createTimer(ros::Duration(noise_period_) , &LaserNoise::timerNoiseCallback, this, false, false);

        areaTriger_ = 0, timerTriger_ =0, timerActivated_= 0;
    }

private:

    //  boost::mt19937 randomGen_;

    ros::NodeHandle n_;
    ros::Subscriber laser_sub_ , pose_sub_ , reset_node_sub_;
    ros::Publisher scan_pub_ , noise_active_pub_;
    sensor_msgs::LaserScan addedNoiseScan_;
    ros::Timer timerNoise_ ;

    void laserReadCallBAck(const sensor_msgs::LaserScan::ConstPtr& msg);
    double GaussianKernel(double mu,double sigma), uniformNoise_;
    void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg);
    void timerNoiseCallback(const ros::TimerEvent&);
    void resetCallBack(const std_msgs::Bool::ConstPtr& msg);

    bool areaTriger_ , timerTriger_, timerActivated_;
    double noise_period_, x_max_, x_min_, y_max_, y_min_ ;

};

void LaserNoise::resetCallBack(const std_msgs::Bool::ConstPtr& msg)
{
    if (msg->data == true)
    {
        timerTriger_ = 0;
        timerNoise_.stop(); // needed to make sure the timer is not running in background
    }

}

void LaserNoise::laserReadCallBAck(const sensor_msgs::LaserScan::ConstPtr& msg)

{

    double sigma;
    double oldRange;
    std_msgs::Bool noise;
    addedNoiseScan_ = *msg ;

    // Guassian noise added
    if (areaTriger_ == 1 && timerTriger_ == 0)
    {
        for (int i=0; i < addedNoiseScan_.ranges.size() ; i++)

        {
            sigma = addedNoiseScan_.ranges[i] * 0.2; // Proportional standard deviation
            oldRange = addedNoiseScan_.ranges[i] ;
            addedNoiseScan_.ranges[i] = addedNoiseScan_.ranges[i] + GaussianKernel(0,sigma);

            if (addedNoiseScan_.ranges[i] > addedNoiseScan_.range_max)
            { addedNoiseScan_.ranges[i] = addedNoiseScan_.range_max;}

            else if (addedNoiseScan_.ranges[i] < addedNoiseScan_.range_min)
            { addedNoiseScan_.ranges[i] = oldRange;}
        }

        noise.data = true;
        noise_active_pub_.publish(noise);
    }

    addedNoiseScan_.header.stamp = ros::Time::now();

    scan_pub_.publish(addedNoiseScan_);

}

void LaserNoise::poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)

{

    if ( (msg->pose.pose.position.x < x_max_) && (msg->pose.pose.position.x > x_min_)
         && (msg->pose.pose.position.y < y_max_) && (msg->pose.pose.position.y > y_min_) )

    {
        areaTriger_ =1;
        timerNoise_.start();
    }
    else
    {

        areaTriger_ = 0;
        std_msgs::Bool noise;
        noise.data = false;
        noise_active_pub_.publish(noise);
        //timerTriger_ = 0;
    }
}

void LaserNoise::timerNoiseCallback(const ros::TimerEvent&)
{
    timerTriger_ = 1;
    timerNoise_.stop();

    ROS_INFO("TIMER ACTIVATED");

}

// Utility for adding noise
double LaserNoise::GaussianKernel(double mu,double sigma)
{
    // using Box-Muller transform to generate two independent standard normally disbributed normal variables

    double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
    double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
    double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
    //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
    // we'll just use X
    // scale to our mu and sigma
    X = sigma * X + mu;
    return X;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "laser_noise");
    LaserNoise lasernoise;

    ros::spin();
}


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/LTIMeasurementModel.h>

#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

LTIMeasurementModel::LTIMeasurementModel(const Ref<const MatrixXd>& measurement_matrix, const Ref<const MatrixXd>& noise_covariance_matrix)
{
    if ((measurement_matrix.rows() == 0) || (measurement_matrix.cols() == 0))
        throw std::runtime_error("ERROR::LTIMEASUREMENTMODEL::CTOR\nERROR:\n\tMeasurement matrix dimensions cannot be 0.");
    else if ((noise_covariance_matrix.rows() == 0) || (noise_covariance_matrix.cols() == 0))
        throw std::runtime_error("ERROR::LTIMEASUREMENTMODEL::CTOR\nERROR:\n\tNoise covariance matrix dimensions cannot be 0.");
    else if (noise_covariance_matrix.rows() != noise_covariance_matrix.cols())
        throw std::runtime_error("ERROR::LTIMEASUREMENTMODEL::CTOR\nERROR:\n\tNoise covariance matrix must be a square matrix.");
    else if (measurement_matrix.rows() != noise_covariance_matrix.rows())
        throw std::runtime_error("ERROR::LTIMEASUREMENTMODEL::CTOR\nERROR:\n\tNumber of rows of the measurement matrix must be the same as the size of the noise covariance matrix.");

    H_ = measurement_matrix;
    R_ = noise_covariance_matrix;
}

std::pair<bool, Eigen::MatrixXd> LTIMeasurementModel::getNoiseCovarianceMatrix() const
{
    return std::make_pair(true, R_);
}

Eigen::MatrixXd LTIMeasurementModel::getMeasurementMatrix() const
{
    return H_;
}