#include "optimization.h"

#include "angleAxisReprojectionError.hpp"
#include "quaternionReprojectionError.hpp"
#include "matrixReprojectionError.hpp"

#include <ceres/ceres.h>

#include <chrono>
#include <algorithm>

#include <Eigen/Geometry>

using namespace std::chrono;

namespace roto
{
namespace
{
template<typename RotationType> void setupProblem(ceres::Problem& problem, const MeasuredScene<RotationType>& measurements, OptimizedScene<RotationType>& parameters);
template<> void setupProblem(ceres::Problem& problem, const MeasuredScene<AngleAxisRotation>& measurements, OptimizedScene<AngleAxisRotation>& parameters)
{
  addAngleAxisReprojectionError(problem, measurements, parameters);
}
template<> void setupProblem(ceres::Problem& problem, const MeasuredScene<QuaternionRotation>& measurements, OptimizedScene<QuaternionRotation>& parameters)
{
  addQuaternionReprojectionError(problem, measurements, parameters);
}
template<> void setupProblem(ceres::Problem& problem, const MeasuredScene<MatrixRotation>& measurements, OptimizedScene<MatrixRotation>& parameters)
{
  addMatrixReprojectionError(problem, measurements, parameters);
}

template<typename RotationType> void print(const RotationType& expectedRotation, const RotationType& initialRotation, const RotationType& optimizedRotation);
template<> void print(const AngleAxisRotation& expectedRotation, const AngleAxisRotation& initialRotation, const AngleAxisRotation& optimizedRotation)
{
  std::cout << "Initial: " << initialRotation(0) << " -> Expected: " << expectedRotation(0) << " Optimized: " << optimizedRotation(0) << std::endl;
  std::cout << "Initial: " << initialRotation(1) << " -> Expected: " << expectedRotation(1) << " Optimized: " << optimizedRotation(1) << std::endl;
  std::cout << "Initial: " << initialRotation(2) << " -> Expected: " << expectedRotation(2) << " Optimized: " << optimizedRotation(2) << std::endl;
  std::cout << "-----------------------------------------" << std::endl;
}
template<> void print(const QuaternionRotation& expectedRotation, const QuaternionRotation& initialRotation, const QuaternionRotation& optimizedRotation)
{
  std::cout << "Initial: " << initialRotation.x() << " -> Expected: " << expectedRotation.x() << " Optimized: " << optimizedRotation.x() << std::endl;
  std::cout << "Initial: " << initialRotation.y() << " -> Expected: " << expectedRotation.y() << " Optimized: " << optimizedRotation.y() << std::endl;
  std::cout << "Initial: " << initialRotation.z() << " -> Expected: " << expectedRotation.z() << " Optimized: " << optimizedRotation.z() << std::endl;
  std::cout << "Initial: " << initialRotation.w() << " -> Expected: " << expectedRotation.w() << " Optimized: " << optimizedRotation.w() << std::endl;
  std::cout << "-----------------------------------------" << std::endl;
}
template<> void print(const MatrixRotation& expectedRotation, const MatrixRotation& initialRotation, const MatrixRotation& optimizedRotation)
{
  std::cout << "Initial: " << initialRotation.row(0) << " -> Expected: " << expectedRotation.row(0) << " Optimized: " << optimizedRotation.row(0) << std::endl;
  std::cout << "Initial: " << initialRotation.row(1) << " -> Expected: " << expectedRotation.row(1) << " Optimized: " << optimizedRotation.row(1) << std::endl;
  std::cout << "Initial: " << initialRotation.row(2) << " -> Expected: " << expectedRotation.row(2) << " Optimized: " << optimizedRotation.row(2) << std::endl;
  std::cout << "-----------------------------------------" << std::endl;
}

template<typename RotationType> void printErrorStatistics(const std::vector<Camera<RotationType> >& expectedParameters, const std::vector<RotationType>& optimizedRotations);
template<> void printErrorStatistics(const std::vector<Camera<AngleAxisRotation> >& expectedParameters, const std::vector<AngleAxisRotation>& optimizedRotations)
{
  assert(expectedParameters.size() == optimizedRotations.size());

  Eigen::Array3d meanErrorCoeffwise(0., 0., 0.);
  Eigen::Array3d maxErrorCoeffwise(0., 0., 0.);
  for(int i = 0; i < expectedParameters.size(); i++)
  {
    const Eigen::Array3d error = (expectedParameters.at(i).pose.rotation - optimizedRotations.at(i)).array();
    meanErrorCoeffwise += error;
    maxErrorCoeffwise = maxErrorCoeffwise.max(error);
  }
  meanErrorCoeffwise /= expectedParameters.size();

  std::cout << "Parameter error statistics: " << std::endl;
  std::cout << "Mean error:" << meanErrorCoeffwise << std::endl;
  std::cout << "Max error:" << maxErrorCoeffwise << std::endl;
}
template<> void printErrorStatistics(const std::vector<Camera<QuaternionRotation> >& expectedParameters, const std::vector<QuaternionRotation>& optimizedRotations)
{
  assert(expectedParameters.size() == optimizedRotations.size());

  Eigen::Array4d meanErrorCoeffwise(0., 0., 0., 0.);
  Eigen::Array4d maxErrorCoeffwise(0., 0., 0., 0.);
  for(int i = 0; i < expectedParameters.size(); i++)
  {
    const Eigen::Array4d error = (expectedParameters.at(i).pose.rotation.coeffs() - optimizedRotations.at(i).coeffs()).array().abs();
    meanErrorCoeffwise += error;
    maxErrorCoeffwise = maxErrorCoeffwise.max(error);
  }
  meanErrorCoeffwise /= expectedParameters.size();

  std::cout << "Parameter error statistics: " << std::endl;
  std::cout << "Mean error:" << std::endl << meanErrorCoeffwise << std::endl;
  std::cout << "Max error:" << std::endl << maxErrorCoeffwise << std::endl;
}
template<> void printErrorStatistics(const std::vector<Camera<MatrixRotation> >& expectedParameters, const std::vector<MatrixRotation>& optimizedRotations)
{
  assert(expectedParameters.size() == optimizedRotations.size());

  Eigen::Array<double, 3, 3> meanErrorCoeffwise = MatrixRotation::Zero();
  Eigen::Array<double, 3, 3> maxErrorCoeffwise = MatrixRotation::Zero();
  for(int i = 0; i < expectedParameters.size(); i++)
  {
    Eigen::Array<double, 3, 3> error = (expectedParameters.at(i).pose.rotation - optimizedRotations.at(i));
    meanErrorCoeffwise += error.abs();
    maxErrorCoeffwise = maxErrorCoeffwise.max(error.abs());
  }
  meanErrorCoeffwise /= expectedParameters.size();

  std::cout << "Parameter error statistics: " << std::endl;
  std::cout << "Mean error:" << std::endl << meanErrorCoeffwise << std::endl;
  std::cout << "Max error:" << std::endl << maxErrorCoeffwise << std::endl;
}

template<typename RotationType>
double optimizeImpl(const MeasuredScene<RotationType>& measurements, OptimizedScene<RotationType>& parameters)
{
  auto initialParameters = parameters;

  /*
   *  1. Setup problem
   */
  ceres::Problem problem;
  setupProblem(problem, measurements, parameters);

  /*
   *  2. Solve problem
   */
  ceres::Solver::Options options;
  options.minimizer_progress_to_stdout = true;
  options.max_num_iterations = 100;
  options.function_tolerance = 1e-15;
  ceres::Solver::Summary summary;

  steady_clock::time_point t1 = steady_clock::now();
  ceres::Solve(options, &problem, &summary);
  steady_clock::time_point t2 = steady_clock::now();
  duration<double> time_span = duration_cast<duration<double>>(t2 - t1);
  std::cout << "It took me " << time_span.count() << " seconds." << std::endl;

  /*
   *  3. Output report
   */

  std::cout << summary.FullReport() << std::endl;
  std::cout << "First 10 cameras: " << std::endl;
  auto n = std::min(10lu,measurements.cameras.size());
  for(int i = 0; i < n ; i++)
  {
    print(measurements.cameras.at(i).pose.rotation, initialParameters.rotation.at(i), parameters.rotation.at(i));
  }
  printErrorStatistics(measurements.cameras, parameters.rotation);

  return time_span.count();
}
} // namespace

template<> double optimize(const MeasuredScene<AngleAxisRotation>& measurements, OptimizedScene<AngleAxisRotation>& parameters)
{
  return optimizeImpl(measurements, parameters);
}
template<> double optimize(const MeasuredScene<QuaternionRotation>& measurements, OptimizedScene<QuaternionRotation>& parameters)
{
  return optimizeImpl(measurements, parameters);
}
template<> double optimize(const MeasuredScene<MatrixRotation>& measurements, OptimizedScene<MatrixRotation>& parameters)
{
  return optimizeImpl(measurements, parameters);
}
} //namespace roto


#include "DataStructures.h"
#include "IncompressibleFluid.h"
#include "IncompressibleLibrary.h"
#include "math.h"
#include "MatrixMath.h"
#include "PolyMath.h"
#include <Eigen/Core>

namespace CoolProp {

/// A thermophysical property provider for all properties
/**
This fluid instance is populated using an entry from a JSON file and uses
simplified polynomial and exponential functions to calculate thermophysical
and transport properties.
*/
//IncompressibleFluid::IncompressibleFluid();

void IncompressibleFluid::validate() {
    return;
    // TODO: Implement validation function

    // u and s have to be of the polynomial type!
    //throw NotImplementedError("TODO");
}

bool IncompressibleFluid::is_pure() {
    if (density.coeffs.cols() == 1) return true;
    return false;
}

/// Base exponential function
double IncompressibleFluid::baseExponential(IncompressibleData data, double y, double ybase) {
    Eigen::VectorXd coeffs = makeVector(data.coeffs);
    size_t r = coeffs.rows(), c = coeffs.cols();
    if (strict && (r != 3 || c != 1))
        throw ValueError(format("%s (%d): You have to provide a 3,1 matrix of coefficients, not  (%d,%d).", __FILE__, __LINE__, r, c));
    return exp((double)(coeffs[0] / ((y - ybase) + coeffs[1]) - coeffs[2]));
}
/// Base exponential function with logarithmic term
double IncompressibleFluid::baseLogexponential(IncompressibleData data, double y, double ybase) {
    Eigen::VectorXd coeffs = makeVector(data.coeffs);
    size_t r = coeffs.rows(), c = coeffs.cols();
    if (strict && (r != 3 || c != 1))
        throw ValueError(format("%s (%d): You have to provide a 3,1 matrix of coefficients, not  (%d,%d).", __FILE__, __LINE__, r, c));
    return exp(
      (double)(log((double)(1.0 / ((y - ybase) + coeffs[0]) + 1.0 / ((y - ybase) + coeffs[0]) / ((y - ybase) + coeffs[0]))) * coeffs[1] + coeffs[2]));
}

double IncompressibleFluid::basePolyOffset(IncompressibleData data, double y, double z) {
    size_t r = data.coeffs.rows(), c = data.coeffs.cols();
    double offset = 0.0;
    double in = 0.0;
    Eigen::MatrixXd coeffs;
    if (r > 0 && c > 0) {
        offset = data.coeffs(0, 0);
        if (r == 1 && c > 1) {  // row vector -> function of z
            coeffs = Eigen::MatrixXd(data.coeffs.block(0, 1, r, c - 1));
            in = z;
        } else if (r > 1 && c == 1) {  // column vector -> function of y
            coeffs = Eigen::MatrixXd(data.coeffs.block(1, 0, r - 1, c));
            in = y;
        } else {
            throw ValueError(format("%s (%d): You have to provide a vector (1D matrix) of coefficients, not  (%d,%d).", __FILE__, __LINE__, r, c));
        }
        return poly.evaluate(coeffs, in, 0, offset);
    }
    throw ValueError(format("%s (%d): You have to provide a vector (1D matrix) of coefficients, not  (%d,%d).", __FILE__, __LINE__, r, c));
}

/// Density as a function of temperature, pressure and composition.
double IncompressibleFluid::rho(double T, double p, double x) {
    switch (density.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.evaluate(density.coeffs, T, x, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
            return baseExponential(density, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
            return baseLogexponential(density, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
            return exp(poly.evaluate(density.coeffs, T, x, 0, 0, Tbase, xbase));
        case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
            return basePolyOffset(density, T, x);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?", __FILE__,
                                    __LINE__, density.type));
        default:
            throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.", __FILE__, __LINE__, density.type));
    }
}

/// Heat capacities as a function of temperature, pressure and composition.
double IncompressibleFluid::c(double T, double p, double x) {
    switch (specific_heat.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            //throw NotImplementedError("Here you should implement the polynomial.");
            return poly.evaluate(specific_heat.coeffs, T, x, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?", __FILE__,
                                    __LINE__, specific_heat.type));
        default:
            throw ValueError(format("%s (%d): There is no predefined way to use this function type \"[%d]\" for specific heat.", __FILE__, __LINE__,
                                    specific_heat.type));
    }
}

/// Viscosity as a function of temperature, pressure and composition.
double IncompressibleFluid::visc(double T, double p, double x) {
    switch (viscosity.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.evaluate(viscosity.coeffs, T, x, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
            return baseExponential(viscosity, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
            return baseLogexponential(viscosity, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
            return exp(poly.evaluate(viscosity.coeffs, T, x, 0, 0, Tbase, xbase));
        case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
            return basePolyOffset(viscosity, T, x);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?", __FILE__,
                                    __LINE__, viscosity.type));
        default:
            throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.", __FILE__, __LINE__, viscosity.type));
    }
}
/// Thermal conductivity as a function of temperature, pressure and composition.
double IncompressibleFluid::cond(double T, double p, double x) {
    switch (conductivity.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.evaluate(conductivity.coeffs, T, x, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
            return baseExponential(conductivity, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
            return baseLogexponential(conductivity, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
            return exp(poly.evaluate(conductivity.coeffs, T, x, 0, 0, Tbase, xbase));
        case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
            return basePolyOffset(conductivity, T, x);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?", __FILE__,
                                    __LINE__, conductivity.type));
        default:
            throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.", __FILE__, __LINE__, conductivity.type));
    }
}
/// Saturation pressure as a function of temperature and composition.
double IncompressibleFluid::psat(double T, double x) {
    if (T <= this->TminPsat) return 0.0;
    switch (p_sat.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.evaluate(p_sat.coeffs, T, x, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
            return baseExponential(p_sat, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
            return baseLogexponential(p_sat, T, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
            return exp(poly.evaluate(p_sat.coeffs, T, x, 0, 0, Tbase, xbase));
        case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
            return basePolyOffset(p_sat, T, x);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?", __FILE__,
                                    __LINE__, p_sat.type));
        default:
            throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.", __FILE__, __LINE__, p_sat.type));
    }
}
/// Freezing temperature as a function of pressure and composition.
double IncompressibleFluid::Tfreeze(double p, double x) {
    switch (T_freeze.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.evaluate(T_freeze.coeffs, p, x, 0, 0, 0.0, xbase);
        case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
            return baseExponential(T_freeze, x, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
            return baseLogexponential(T_freeze, x, 0.0);
        case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
            return exp(poly.evaluate(T_freeze.coeffs, p, x, 0, 0, 0.0, xbase));
        case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
            return basePolyOffset(T_freeze, p, x);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?", __FILE__,
                                    __LINE__, T_freeze.type));
        default:
            throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.", __FILE__, __LINE__, T_freeze.type));
    }
}

/* Below are direct calculations of the derivatives. Nothing
 * special is going on, we simply use the polynomial class to
 * derive the different functions with respect to temperature.
 */
/// Partial derivative of density with respect to temperature at constant pressure and composition
double IncompressibleFluid::drhodTatPx(double T, double p, double x) {
    switch (density.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.derivative(density.coeffs, T, x, 0, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?", __FILE__,
                                    __LINE__, density.type));
        default:
            throw ValueError(
              format("%s (%d): There is no predefined way to use this function type \"[%d]\" for density.", __FILE__, __LINE__, density.type));
    }
}
/// Partial derivative of entropy
//  with respect to temperature at constant pressure and composition
//  integrated in temperature
double IncompressibleFluid::dsdTatPxdT(double T, double p, double x) {
    switch (specific_heat.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.integral(specific_heat.coeffs, T, x, 0, -1, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?", __FILE__,
                                    __LINE__, specific_heat.type));
        default:
            throw ValueError(
              format("%s (%d): There is no predefined way to use this function type \"[%d]\" for entropy.", __FILE__, __LINE__, specific_heat.type));
    }
}
/// Partial derivative of enthalpy
//  with respect to temperature at constant pressure and composition
//  integrated in temperature
double IncompressibleFluid::dhdTatPxdT(double T, double p, double x) {
    switch (specific_heat.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.integral(specific_heat.coeffs, T, x, 0, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?", __FILE__,
                                    __LINE__, specific_heat.type));
        default:
            throw ValueError(
              format("%s (%d): There is no predefined way to use this function type \"[%d]\" for entropy.", __FILE__, __LINE__, specific_heat.type));
    }
}

/// Mass fraction conversion function
/** If the fluid type is mass-based, it does not do anything. Otherwise,
 *  it converts the mass fraction to the required input. */
double IncompressibleFluid::inputFromMass(double T, double x) {
    if (this->xid == IFRAC_PURE) {
        return _HUGE;
    } else if (this->xid == IFRAC_MASS) {
        return x;
    } else {
        throw NotImplementedError("Mass composition conversion has not been implemented.");
        //switch (mass2input.type) {
        //    case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
        //        return poly.evaluate(mass2input.coeffs, T, x, 0, 0, 0.0, 0.0); // TODO: make sure Tbase and xbase are defined in the correct way
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
        //        return baseExponential(mass2input, x, 0.0);
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
        //        return baseLogexponential(mass2input, x, 0.0);
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
        //        return exp(poly.evaluate(mass2input.coeffs, T, x, 0, 0, 0.0, 0.0)); // TODO: make sure Tbase and xbase are defined in the correct way
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
        //        return basePolyOffset(mass2input, T, x);
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
        //        throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,mass2input.type));
        //        break;
        //    default:
        //        throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.",__FILE__,__LINE__,mass2input.type));
        //        break;
        //}
        //return _HUGE;
    }
}

/// Volume fraction conversion function
/** If the fluid type is volume-based, it does not do anything. Otherwise,
 *  it converts the volume fraction to the required input. */
double IncompressibleFluid::inputFromVolume(double T, double x) {
    if (this->xid == IFRAC_PURE) {
        return _HUGE;
    } else if (this->xid == IFRAC_VOLUME) {
        return x;
    } else {
        throw NotImplementedError("Volume composition conversion has not been implemented.");
        //switch (volume2input.type) {
        //    case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
        //        return poly.evaluate(volume2input.coeffs, T, x, 0, 0, 0.0, 0.0); // TODO: make sure Tbase and xbase are defined in the correct way
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
        //        return baseExponential(volume2input, x, 0.0);
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
        //        return baseLogexponential(volume2input, x, 0.0);
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
        //        return exp(poly.evaluate(volume2input.coeffs, T, x, 0, 0, 0.0, 0.0)); // TODO: make sure Tbase and xbase are defined in the correct way
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
        //        return basePolyOffset(volume2input, T, x);
        //        break;
        //    case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
        //        throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,volume2input.type));
        //        break;
        //    default:
        //        throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.",__FILE__,__LINE__,volume2input.type));
        //        break;
        //}
        //return _HUGE;
    }
}

/// Mole fraction conversion function
/** If the fluid type is mole-based, it does not do anything. Otherwise,
 *  it converts the mole fraction to the required input. */
double IncompressibleFluid::inputFromMole(double T, double x) {
    if (this->xid == IFRAC_PURE) {
        return _HUGE;
    } else if (this->xid == IFRAC_MOLE) {
        return x;
    } else {
        throw NotImplementedError("Mole composition conversion has not been implemented.");
        /*
        switch (mole2input.type) {
            case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
                return poly.evaluate(mole2input.coeffs, T, x, 0, 0, 0.0, 0.0); // TODO: make sure Tbase and xbase are defined in the correct way
                break;
            case IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL:
                return baseExponential(mole2input, x, 0.0);
                break;
            case IncompressibleData::INCOMPRESSIBLE_LOGEXPONENTIAL:
                return baseLogexponential(mole2input, x, 0.0);
                break;
            case IncompressibleData::INCOMPRESSIBLE_EXPPOLYNOMIAL:
                return exp(poly.evaluate(mole2input.coeffs, T, x, 0, 0, 0.0, 0.0)); // TODO: make sure Tbase and xbase are defined in the correct way
                break;
            case IncompressibleData::INCOMPRESSIBLE_POLYOFFSET:
                return basePolyOffset(mole2input, T, x);
                break;
            case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
                throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?",__FILE__,__LINE__,mole2input.type));
                break;
            default:
                throw ValueError(format("%s (%d): Your function type \"[%d]\" is unknown.",__FILE__,__LINE__,mole2input.type));
                break;
        }
        return _HUGE;
        */
    }
}

/* Some functions can be inverted directly, those are listed
 * here. It is also possible to solve for other quantities, but
 * that involves some more sophisticated processing and is not
 * done here, but in the backend, T(h,p) for example.
 */
/// Temperature as a function of density, pressure and composition.
double IncompressibleFluid::T_rho(double Dmass, double p, double x) {
    double d_raw = Dmass;  // No changes needed, no reference values...
    switch (density.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.solve_limits(density.coeffs, x, d_raw, Tmin, Tmax, 0, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?", __FILE__,
                                    __LINE__, specific_heat.type));
        default:
            throw ValueError(format("%s (%d): There is no predefined way to use this function type \"[%d]\" for inverse density.", __FILE__, __LINE__,
                                    specific_heat.type));
    }
}
/// Temperature as a function of heat capacities as a function of temperature, pressure and composition.
double IncompressibleFluid::T_c(double Cmass, double p, double x) {
    double c_raw = Cmass;  // No changes needed, no reference values...
    switch (specific_heat.type) {
        case IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL:
            return poly.solve_limits(specific_heat.coeffs, x, c_raw, Tmin, Tmax, 0, 0, 0, Tbase, xbase);
        case IncompressibleData::INCOMPRESSIBLE_NOT_SET:
            throw ValueError(format("%s (%d): The function type is not specified (\"[%d]\"), are you sure the coefficients have been set?", __FILE__,
                                    __LINE__, specific_heat.type));
        default:
            throw ValueError(format("%s (%d): There is no predefined way to use this function type \"[%d]\" for inverse specific heat.", __FILE__,
                                    __LINE__, specific_heat.type));
    }
}

/*
 * Some more functions to provide a single implementation
 * of important routines.
 * We start with the check functions that can validate input
 * in terms of pressure p, temperature T and composition x.
 */
/// Check validity of temperature input.
/** Compares the given temperature T to the result of a
 *  freezing point calculation. This is not necessarily
 *  defined for all fluids, default values do not cause errors. */
bool IncompressibleFluid::checkT(double T, double p, double x) {
    if (Tmin <= 0.) throw ValueError("Please specify the minimum temperature.");
    if (Tmax <= 0.) throw ValueError("Please specify the maximum temperature.");
    if ((Tmin > T) || (T > Tmax)) throw ValueError(format("Your temperature %f is not between %f and %f.", T, Tmin, Tmax));
    double TF = 0.0;
    if (T_freeze.type != IncompressibleData::INCOMPRESSIBLE_NOT_SET) TF = Tfreeze(p, x);
    if (T < TF) throw ValueError(format("Your temperature %f is below the freezing point of %f.", T, TF));
    return true;
}

/// Check validity of pressure input.
/** Compares the given pressure p to the saturation pressure at
 *  temperature T and throws and exception if p is lower than
 *  the saturation conditions.
 *  The default value for psat is -1 yielding true if psat
 *  is not redefined in the subclass.
 *  */
bool IncompressibleFluid::checkP(double T, double p, double x) {
    double ps = 0.0;
    if (p_sat.type != IncompressibleData::INCOMPRESSIBLE_NOT_SET) ps = psat(T, x);
    if (p < 0.0) throw ValueError(format("You cannot use negative pressures: %f < %f. ", p, 0.0));
    if (ps > 0.0 && p < ps) throw ValueError(format("Equations are valid for liquid phase only: %f < %f (psat). ", p, ps));
    return true;
}

/// Check validity of composition input.
/** Compares the given composition x to a stored minimum and
 *  maximum value. Enforces the redefinition of xmin and
 *  xmax since the default values cause an error. */
bool IncompressibleFluid::checkX(double x) {
    if (xmin < 0.0 || xmin > 1.0) throw ValueError("Please specify the minimum concentration between 0 and 1.");
    if (xmax < 0.0 || xmax > 1.0) throw ValueError("Please specify the maximum concentration between 0 and 1.");
    if ((xmin > x) || (x > xmax)) throw ValueError(format("Your composition %f is not between %f and %f.", x, xmin, xmax));
    return true;
}

} /* namespace CoolProp */

// Testing still needs to be enhanced.
/* Below, I try to carry out some basic tests for both 2D and 1D
 * polynomials as well as the exponential functions for vapour
 * pressure etc.
 */
#ifdef ENABLE_CATCH
#    include <math.h>
#    include <iostream>
#    include <catch2/catch_all.hpp>
#    include "TestObjects.h"

Eigen::MatrixXd makeMatrix(const std::vector<double>& coefficients) {
    //IncompressibleClass::checkCoefficients(coefficients,18);
    std::vector<std::vector<double>> matrix;
    std::vector<double> tmpVector;

    tmpVector.clear();
    tmpVector.push_back(coefficients[0]);
    tmpVector.push_back(coefficients[6]);
    tmpVector.push_back(coefficients[11]);
    tmpVector.push_back(coefficients[15]);
    matrix.push_back(tmpVector);

    tmpVector.clear();
    tmpVector.push_back(coefficients[1] * 100.0);
    tmpVector.push_back(coefficients[7] * 100.0);
    tmpVector.push_back(coefficients[12] * 100.0);
    tmpVector.push_back(coefficients[16] * 100.0);
    matrix.push_back(tmpVector);

    tmpVector.clear();
    tmpVector.push_back(coefficients[2] * 100.0 * 100.0);
    tmpVector.push_back(coefficients[8] * 100.0 * 100.0);
    tmpVector.push_back(coefficients[13] * 100.0 * 100.0);
    tmpVector.push_back(coefficients[17] * 100.0 * 100.0);
    matrix.push_back(tmpVector);

    tmpVector.clear();
    tmpVector.push_back(coefficients[3] * 100.0 * 100.0 * 100.0);
    tmpVector.push_back(coefficients[9] * 100.0 * 100.0 * 100.0);
    tmpVector.push_back(coefficients[14] * 100.0 * 100.0 * 100.0);
    tmpVector.push_back(0.0);
    matrix.push_back(tmpVector);

    tmpVector.clear();
    tmpVector.push_back(coefficients[4] * 100.0 * 100.0 * 100.0 * 100.0);
    tmpVector.push_back(coefficients[10] * 100.0 * 100.0 * 100.0 * 100.0);
    tmpVector.push_back(0.0);
    tmpVector.push_back(0.0);
    matrix.push_back(tmpVector);

    tmpVector.clear();
    tmpVector.push_back(coefficients[5] * 100.0 * 100.0 * 100.0 * 100.0 * 100.0);
    tmpVector.push_back(0.0);
    tmpVector.push_back(0.0);
    tmpVector.push_back(0.0);
    matrix.push_back(tmpVector);

    tmpVector.clear();
    return CoolProp::vec_to_eigen(matrix).transpose();
}

TEST_CASE("Internal consistency checks and example use cases for the incompressible fluids", "[IncompressibleFluids]") {
    bool PRINT = false;
    std::string tmpStr;
    std::vector<double> tmpVector;
    std::vector<std::vector<double>> tmpMatrix;

    SECTION("Test case for \"SylthermXLT\" by Dow Chemicals") {

        std::vector<double> cRho;
        cRho.push_back(+1.1563685145E+03);
        cRho.push_back(-1.0269048032E+00);
        cRho.push_back(-9.3506079577E-07);
        cRho.push_back(+1.0368116627E-09);
        CoolProp::IncompressibleData density;
        density.type = CoolProp::IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL;
        density.coeffs = CoolProp::vec_to_eigen(cRho);

        std::vector<double> cHeat;
        cHeat.push_back(+1.1562261074E+03);
        cHeat.push_back(+2.0994549103E+00);
        cHeat.push_back(+7.7175381057E-07);
        cHeat.push_back(-3.7008444051E-20);
        CoolProp::IncompressibleData specific_heat;
        specific_heat.type = CoolProp::IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL;
        specific_heat.coeffs = CoolProp::vec_to_eigen(cHeat);

        std::vector<double> cCond;
        cCond.push_back(+1.6121957379E-01);
        cCond.push_back(-1.3023781944E-04);
        cCond.push_back(-1.4395238766E-07);
        CoolProp::IncompressibleData conductivity;
        conductivity.type = CoolProp::IncompressibleData::INCOMPRESSIBLE_POLYNOMIAL;
        conductivity.coeffs = CoolProp::vec_to_eigen(cCond);

        std::vector<double> cVisc;
        cVisc.push_back(+1.0337654989E+03);
        cVisc.push_back(-4.3322764383E+01);
        cVisc.push_back(+1.0715062356E+01);
        CoolProp::IncompressibleData viscosity;
        viscosity.type = CoolProp::IncompressibleData::INCOMPRESSIBLE_EXPONENTIAL;
        viscosity.coeffs = CoolProp::vec_to_eigen(cVisc);

        CoolProp::IncompressibleFluid XLT;
        XLT.setName("XLT");
        XLT.setDescription("SylthermXLT");
        XLT.setReference("Dow Chemicals data sheet");
        XLT.setTmax(533.15);
        XLT.setTmin(173.15);
        XLT.setxmax(0.0);
        XLT.setxmin(0.0);
        XLT.setTminPsat(533.15);

        XLT.setTbase(0.0);
        XLT.setxbase(0.0);

        /// Setters for the coefficients
        XLT.setDensity(density);
        XLT.setSpecificHeat(specific_heat);
        XLT.setViscosity(viscosity);
        XLT.setConductivity(conductivity);
        //XLT.setPsat(parse_coefficients(fluid_json, "saturation_pressure", false));
        //XLT.setTfreeze(parse_coefficients(fluid_json, "T_freeze", false));
        //XLT.setVolToMass(parse_coefficients(fluid_json, "volume2mass", false));
        //XLT.setMassToMole(parse_coefficients(fluid_json, "mass2mole", false));

        /// A function to check coefficients and equation types.
        //XLT.validate();
        double acc = 0.0001;
        double val = 0;
        double res = 0;

        // Prepare the results and compare them to the calculated values
        double T = 273.15 + 50;
        double p = 10e5;
        double x = 0.0;

        // Compare density
        val = 824.4615702148608;
        res = XLT.rho(T, p, x);
        {
            CAPTURE(T);
            CAPTURE(val);
            CAPTURE(res);
            CHECK(check_abs(val, res, acc));
        }

        // Compare cp
        val = 1834.7455527670554;
        res = XLT.c(T, p, x);
        {
            CAPTURE(T);
            CAPTURE(val);
            CAPTURE(res);
            CHECK(check_abs(val, res, acc));
        }

        // Check property functions
        CHECK_THROWS(XLT.s(T, p, x));
        CHECK_THROWS(XLT.h(T, p, x));
        CHECK_THROWS(XLT.u(T, p, x));

        // Compare v
        val = 0.0008931435169681835;
        res = XLT.visc(T, p, x);
        {
            CAPTURE(T);
            CAPTURE(val);
            CAPTURE(res);
            CHECK(check_abs(val, res, acc));
        }

        // Compare l
        val = 0.10410086156049088;
        res = XLT.cond(T, p, x);
        {
            CAPTURE(T);
            CAPTURE(val);
            CAPTURE(res);
            CHECK(check_abs(val, res, acc));
        }
    }

    SECTION("Test case for Methanol from SecCool") {

        CoolProp::IncompressibleFluid CH3OH = CoolProp::get_incompressible_fluid("CH3OH");

        // Prepare the results and compare them to the calculated values
        double acc = 0.0001;
        double T = 273.15 + 10;
        double p = 10e5;
        double x = 0.25;
        double expected = 0;
        double actual = 0;

        // Compare density
        expected = 963.2886528091547;
        actual = CH3OH.rho(T, p, x);
        {
            CAPTURE(T);
            CAPTURE(p);
            CAPTURE(x);
            CAPTURE(expected);
            CAPTURE(actual);
            CHECK(check_abs(expected, actual, acc));
        }

        // Compare cp
        expected = 3993.9748117022423;
        actual = CH3OH.c(T, p, x);
        {
            CAPTURE(T);
            CAPTURE(p);
            CAPTURE(x);
            CAPTURE(expected);
            CAPTURE(actual);
            CHECK(check_abs(expected, actual, acc));
        }

        // Check property functions
        CHECK_THROWS(CH3OH.s(T, p, x));
        CHECK_THROWS(CH3OH.h(T, p, x));
        CHECK_THROWS(CH3OH.u(T, p, x));

        // Compare v
        expected = 0.0023970245009602097;
        actual = CH3OH.visc(T, p, x) / 1e3;
        {
            CAPTURE(T);
            CAPTURE(p);
            CAPTURE(x);
            CAPTURE(expected);
            CAPTURE(actual);
            std::string errmsg = CoolProp::get_global_param_string("errstring");
            CAPTURE(errmsg);
            CHECK(check_abs(expected, actual, acc));
        }

        // Compare conductivity
        expected = 0.44791148414693727;
        actual = CH3OH.cond(T, p, x);
        {
            CAPTURE(T);
            CAPTURE(p);
            CAPTURE(x);
            CAPTURE(expected);
            CAPTURE(actual);
            std::string errmsg = CoolProp::get_global_param_string("errstring");
            CAPTURE(errmsg);
            CHECK(check_abs(expected, actual, acc));
        }

        // Compare Tfreeze
        expected = -20.02 + 273.15;  // 253.1293105454671;
        actual = CH3OH.Tfreeze(p, x);
        {
            CAPTURE(T);
            CAPTURE(p);
            CAPTURE(x);
            CAPTURE(expected);
            CAPTURE(actual);
            std::string errmsg = CoolProp::get_global_param_string("errstring");
            CAPTURE(errmsg);
            CHECK(check_abs(expected, actual, acc));
        }
    }
}

#endif /* ENABLE_CATCH */


// Copyright(c) 2019-present, Alexander Silva Barbosa & bflib contributors.
// Distributed under the MIT License (http://opensource.org/licenses/MIT)

/**
 * @author Alexander Silva Barbosa <alexander.ti.ufv@gmail.com>
 * @date 2019
 * Extended Kalman Filter
 */

#pragma once

#include <Eigen/Dense>
#include <random>
#include <chrono>
#include <thread>
#include <vector>

using namespace Eigen;

template <typename dataType, int states, int inputs, int outputs, int dataConverter = -1>
class EKF
{
    private:
        typedef Matrix<dataType, states, 1> MatNx1;
        typedef Matrix<dataType, states, states> MatNxN;
        typedef Matrix<dataType, states, inputs> MatNxM;
        typedef Matrix<dataType, states, outputs> MatNxP;
        typedef Matrix<dataType, outputs, states> MatPxN;
        typedef Matrix<dataType, outputs, outputs> MatPxP;
        typedef Matrix<dataType, inputs, 1> MatMx1;
        typedef Matrix<dataType, outputs, 1> MatPx1;
        typedef Matrix<dataType, dataConverter, 1> MatDx1;
        typedef Matrix<dataType, 3, 1> Mat3x1;

    public:
        typedef MatNx1 State;
        typedef MatMx1 Input;
        typedef MatPx1 Output;
        typedef Input Control;
        typedef Output Sensor;
        typedef MatNxN ModelCovariance;
        typedef MatPxP SensorCovariance;
        typedef MatNxN StateMatrix;
        typedef MatNxM InputMatrix;
        typedef MatPxN OutputMatrix;
        typedef MatNxN ModelJacobian;
        typedef MatPxN SensorJacobian;
        typedef MatDx1 Data;
        typedef Data Landmark;
        typedef Mat3x1 Uncertainty;

    private:
        typedef void (*ModelFunction)(State &x, Input &u, double dt);
        typedef void (*SensorFunction)(Output &y, State &x, Data &d, double dt);
        typedef void (*ModelJacobianFunction)(ModelJacobian &F, State &x, Input &u, double dt);
        typedef void (*SensorJacobianFunction)(SensorJacobian &H, State &x, Data &d, double dt);

        std::default_random_engine gen;
        std::normal_distribution<double> distr{0.0, 1.0};
        std::chrono::time_point<std::chrono::high_resolution_clock> start;

        State x, x_1;
        ModelCovariance Q;
        SensorCovariance R;
        Output z;

        ModelJacobian F;
        SensorJacobian H;

        std::vector<Data> dataPoints;

        MatNx1 randX;
        MatPx1 randY;

        MatNxN P;
        MatNxN Qsqrt;
        MatPxP Rsqrt;

        MatPxP S;
        MatNxP K;
        MatNxN I;

        ModelFunction modelFn;
        SensorFunction sensorFn;
        ModelJacobianFunction modelJFn;
        SensorJacobianFunction sensorJFn;

        void init()
        {
            modelFn = NULL;
            sensorFn = NULL;
            modelJFn = NULL;
            sensorJFn = NULL;

            P = Q;
            I.setIdentity();

            Qsqrt = Q.cwiseSqrt();
            Rsqrt = R.cwiseSqrt();

            start = std::chrono::high_resolution_clock::now();
        }
    public:

        EKF()
        {
            Q.setIdentity();
            R.setIdentity();
            x.setZero();
            init();
        }

        EKF(State X) : x(X)
        {
            Q.setIdentity();
            R.setIdentity();
            init();
        }

        EKF(ModelCovariance Q, SensorCovariance R) : Q(Q), R(R)
        {
            x.setZero();
            init();
        }

        EKF(State X, ModelCovariance Q, SensorCovariance R) : x(X), Q(Q), R(R)
        {
            init();
        }

        virtual ~EKF()
        {

        }

        long long seed()
        {
            long long s = std::chrono::system_clock::now().time_since_epoch().count();
            return seed(s);
        }

        long long seed(long long s)
        {
            gen = std::default_random_engine(s);
            return s;
        }

        State state()
        {
            MatNx1 x;
            x.setZero();
            return x;
        }

        Input input()
        {
            MatMx1 u;
            u.setZero();
            return u;
        }

        Output output()
        {
            MatPx1 y;
            y.setZero();
            return y;
        }

        ModelCovariance createQ()
        {
            ModelCovariance Q;
            Q.setZero();
            return Q;
        }

        SensorCovariance createR()
        {
            SensorCovariance R;
            R.setZero();
            return R;
        }

        Data createData()
        {
            Data D;
            D.setZero();
            return D;
        }

        ModelCovariance getP()
        {
            return P;
        }

        Uncertainty getUncertainty(unsigned int x1, unsigned int x2)
        {
            Uncertainty C;
            C.setZero();
            if(x1 >= states || x2 >= states)
                return C;

            Matrix<dataType, 2, 2> p;
            p(0, 0) = P(x1, x1);
            p(0, 1) = P(x1, x2);
            p(1, 0) = P(x2, x1);
            p(1, 1) = P(x2, x2);

            EigenSolver< Matrix<dataType, 2, 2> > es(p);
            Matrix<dataType, 2, 2> eValue = es.pseudoEigenvalueMatrix();
            Matrix<dataType, 2, 2> eVector = es.pseudoEigenvectors();

            C[0] = eValue(0,0);
            C[1] = eValue(1,1);
            C[2] = std::atan2(eVector(0, 1), eVector(0, 0));

            return C;
        }

        void setQ(ModelCovariance Q)
        {
            this->Q = Q;
            P = Q;
            Qsqrt = Q.cwiseSqrt();
        }

        void setR(SensorCovariance R)
        {
            this->R = R;
            Rsqrt = R.cwiseSqrt();
        }

        double time()
        {
            auto end = std::chrono::high_resolution_clock::now();
            std::chrono::duration<double> diff = end - start;
            start = std::chrono::high_resolution_clock::now();
            return diff.count();
        }

        double delay(double s)
        {
            double ellapsed = time();
            double remain = s - ellapsed;
            if(remain < 0)
                return ellapsed;
            std::this_thread::sleep_for(std::chrono::nanoseconds((long long)(remain * 1e9)));
            ellapsed += time();
            return ellapsed;
        }

        void addData(Data &data)
        {
            dataPoints.push_back(data);
        }

        void fillData(std::vector<Data> &data)
        {
            dataPoints = data;
        }

        std::vector<Data>& data()
        {
            return dataPoints;
        }

        void setModel(ModelFunction fn)
        {
            modelFn = fn;
        }

        void setSensor(SensorFunction fn)
        {
            sensorFn = fn;
        }

        void setModelJacobian(ModelJacobianFunction fn)
        {
            modelJFn = fn;
        }

        void setSensorJacobian(SensorJacobianFunction fn)
        {
            sensorJFn = fn;
        }

        virtual void model(State &x, Input &u, double dt)
        {

        }

        virtual void sensor(Output &z, State &x, Data &d, double dt)
        {

        }

        virtual void modelJacobian(ModelJacobian &F, State &x, Input &u, double dt)
        {

        }

        virtual void sensorJacobian(SensorJacobian &H, State &x, Data &d, double dt)
        {

        }

        void simulate(State &x, Output &y, Input &u, double dt)
        {
            Data data;
            randn(data);

            if(dataPoints.size() > 0)
                data = dataPoints[0];

            doModel(x, u, dt);
            randn(randX);
            x = x + Qsqrt * randX;

            doSensor(y, x, data, dt);
            randn(randY);
            y = y + Rsqrt * randY;
        }

        void simulate(State &x, std::vector<Output> &y, Input &u, double dt)
        {
            doModel(x, u, dt);
            randn(randX);
            x = x + Qsqrt * randX;

            int j = 0, index = 0;
            if(dataPoints.size() == 0)
                j = -1;

            MatDx1 data;
            for(int i = 0; i < y.size(); i++)
            {
                if(j == -1)
                {
                    randn(data);
                    doSensor(y[i], x, data, dt);
                }
                else
                {
                    doSensor(y[i], x, dataPoints[j], dt);
                    j = rand() % dataPoints.size();
                }
                randn(randY);
                y[i] = y[i] + Rsqrt * randY;
            }
        }

        void run(State &xK, Output &y, Input &u, double dt)
        {
            predict(u, dt);

            std::vector<MatDx1> data(1);
            std::vector<MatPx1> ys(1);
            ys[0] = y;
            dataAssoc(ys, data, dt);

            update(y, data[0], dt);
            xK = x;
        }

        void run(State &xK, std::vector<Output> &y, Input &u, double dt)
        {
            predict(u, dt);

            if(y.size() > 0)
            {
                std::vector<MatDx1> data(y.size());
                dataAssoc(y, data, dt);

                for(int i = 0; i < y.size(); i++)
                    update(y[i], data[i], dt);
            }
            xK = x;
        }

    private:
        void predict(Input &u, double dt)
        {
            x_1 = x;
            doModel(x, u, dt);
            doModelJ(F, x_1, u, dt);
            P = F * P * F.transpose() + Q;
        }

        void update(Output &y, Data &d, double dt)
        {
            doSensorJ(H, x, d, dt);

            doSensor(z, x, d, dt);
            S = ( H * P * H.transpose() ) + R;
            K = P * H.transpose() * S.inverse();
            x = x + K * (y - z);
            P = (I - K * H) * P;
        }

        void dataAssoc(std::vector<Output> &y, std::vector<Data> &data, double dt)
        {
            if(dataPoints.size() == 0)
                return;
            MatPx1 v;
            dataType minX, X;
            int minJ;
            Matrix<dataType, 1, 1> Xsq;
            Data d;

            for(int i = 0; i < y.size(); i++)
            {
                minX = 0;
                minJ = -1;
                for (int j = 0; j < dataPoints.size(); j++)
                {
                    d = dataPoints[j];
                    doSensor(z, x, d, dt);
                    v = z - y[i];
                    doSensorJ(H, x, d, dt);
                    S = H * P * H.transpose() + R;
                    Xsq = v.transpose() * S.inverse() * v;
                    X = sqrt(Xsq(0));
                    if(minJ < 0)
                    {
                        minJ = j;
                        minX = X;
                    }
                    else if(X < minX)
                    {
                        minJ = j;
                        minX = X;
                    }
                }
                data[i] = dataPoints[minJ];
            }
        }

        void doModel(State &x, Input &u, double dt)
        {
            if(modelFn != NULL)
                modelFn(x, u, dt);
            else
                model(x, u, dt);
        }

        void doSensor(Output &z, State &x, Data &d, double dt)
        {
            if(sensorFn != NULL)
                sensorFn(z, x, d, dt);
            else
                sensor(z, x, d, dt);
        }

        void doModelJ(ModelJacobian &F, State &x, Input &u, double dt)
        {
            if(modelJFn != NULL)
                modelJFn(F, x, u, dt);
            else
                modelJacobian(F, x, u, dt);
        }

        void doSensorJ(SensorJacobian &H, State &x, Data &d, double dt)
        {
            if(sensorJFn != NULL)
                sensorJFn(H, x, d, dt);
            else
                sensorJacobian(H, x, d, dt);
        }

        template<class T>
        void randn(T &mat)
        {
            for (size_t i = 0; i < mat.rows(); i++)
            {
                for (size_t j = 0; j < mat.cols(); j++)
                {
                    mat(i, j) = distr(gen);
                }
            }
        }

};


#include <freefloating_gazebo/hydro_link.h>
#include <Eigen/Geometry>

namespace ffg
{

Eigen::Matrix3d skew(const Eigen::Vector3d &v)
{
  Eigen::Matrix3d out;
  out << 0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0;
  return out;
}

Eigen::Vector3d HydroLink::buoyancyForce(double surface_distance)
{
  Eigen::Vector3d force(0,0,buoyancy_force);

  if(surface_distance > -buoyancy_limit)
  {
    if(surface_distance > buoyancy_limit)
      force *= 0;
    else
      force *= cos(M_PI/4.*(surface_distance/buoyancy_limit + 1));
  }
  return force;
}

Eigen::Vector6d HydroLink::hydroDynamicForce(Eigen::Vector6d &vel)
{
  Eigen::Vector6d force(Eigen::Vector6d::Zero());

  // filter velocity
  if(has_added_mass)
    vel_filter.filter(vel);

  // damping part
  if(has_lin_damping)
    force = -lin_damping.cwiseProduct(vel);
  if(has_quad_damping)
    force -= quad_damping.cwiseProduct(vel.cwiseProduct(vel.cwiseAbs()));

  // added mass part
  if(has_added_mass)
  {
    const Eigen::Vector6d acc = (vel - vel_prev)/dt;
    force -= added_mass * acc;

    // added Coriolis
    Eigen::Matrix6d Cor;
    const Eigen::Vector6d A = added_mass * vel;
    const Eigen::Matrix3d Sa = -1 * skew(A.head<3>());
    Cor << Eigen::Matrix3d::Zero(), Sa, Sa, skew(-A.tail<3>());
    force -= Cor * vel;

    vel_prev = vel;
  }
  return force;
}

}


// BSD 3-Clause License
// Copyright (c) 2021 The Trustees of the University of Pennsylvania. All Rights Reserved
// Authors:
// J. Diego Caporale <jdcap@seas.upenn.edu>

/* Basic PD Control example script demonstrating how to use the mjbots_control_loop to 3 motors. The functions to implement are
 * CalcTorques and PrepareLog. In this example we send a PD control torques and log the motor information.
 * Also, an implementation that uses Eigen conversions.
 */

#include "kodlab_mjbots_sdk/mjbots_control_loop.h"
#include "kodlab_mjbots_sdk/joint_moteus.h"
#include "ManyMotorLog.hpp"
#include "kodlab_mjbots_sdk/lcm_subscriber.h"
#include <sys/mman.h>
#include <limits>
#include <Eigen/Core>
#include <Eigen/Dense>

class Joints3DoF : public kodlab::mjbots::MjbotsControlLoop<ManyMotorLog> {
  using MjbotsControlLoop::MjbotsControlLoop;
  void CalcTorques() override {
    std::vector<float> torques(num_motors_, 0);

    if (num_motors_==3){
      Eigen::Vector3f leg_pos_des;
      leg_pos_des<<1-M_PI_2,2,0;
      Eigen::Vector3f positions  = Eigen::Map<Eigen::VectorXf,Eigen::Unaligned> ( robot_->GetJointPositions().data(), num_motors_);
      Eigen::Vector3f velocities = Eigen::Map<Eigen::VectorXf,Eigen::Unaligned> (robot_->GetJointVelocities().data(), num_motors_);
      float kp = 100;
      float kd = 1;
      Eigen::VectorXf tau = kp*(leg_pos_des-positions) + kd*(Eigen::Vector3f::Zero()-velocities);

      Eigen::VectorXf::Map(&torques[0], num_motors_) = tau;
    }
    else{
      std::cout<<"Wrong number of motors"<<std::endl;
    }

    robot_->SetTorques(torques);
  }

  void PrepareLog() override {
    for (int servo = 0; servo < num_motors_; servo++) {
      log_data_.positions[servo]  = robot_->GetJointPositions()[servo];
      log_data_.velocities[servo] = robot_->GetJointVelocities()[servo];
      log_data_.modes[servo] = static_cast<int>(robot_->GetJointModes()[servo]);
      log_data_.torques[servo] = robot_->GetJointTorqueCmd()[servo];
    }
    for (int servo = num_motors_; servo < 13; servo++) {
      log_data_.positions[servo] = 0;
      log_data_.velocities[servo] = 0;
      log_data_.modes[servo] = 0;
      log_data_.torques[servo] = 0;
    }
  }
};

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

  //Setup joints
  std::vector<kodlab::mjbots::JointMoteus> joints;
  joints.emplace_back(100, 4, 1, -1.3635165,    1,  1);
  joints.emplace_back(101, 4,-1,  2.688,  5.0/3.0,  1);
  joints.emplace_back(108, 4, 1, -0.4674585,    1,  1);

  // Define robot options
  kodlab::mjbots::ControlLoopOptions options;
  options.log_channel_name = "motor_data";
  options.frequency = 1000;
  options.realtime_params.main_cpu = 3;
  options.realtime_params.can_cpu  = 2;
  options.parallelize_control_loop = true;

  // Create control loop
  Joints3DoF control_loop(joints, options);

  // Starts the loop, and then join it
  control_loop.Start();
  control_loop.Join();
  return 0;
}


#include "shot_detector.h"

#include <cmath>
#include <string>
#include <opencv/cv.hpp>
#include <boost/log/trivial.hpp>

using namespace std;
using namespace cv;

namespace vrs {
namespace components {

namespace shot_detector {
bool shot_bound(const Mat &curr_frame, const Mat &prev_frame,
		double threshold) {
	//转换为HSV直方图
	Mat hsv1, hsv2;
	bool is_shot_bound = false;
	//如有有必要则进行缩放
	if (curr_frame.cols < FRAME_WIDTH) {
		resize(curr_frame, curr_frame,
				Size(FRAME_WIDTH,
						round(
								(FRAME_WIDTH
										/ static_cast<float>(curr_frame.cols))
										* static_cast<float>(curr_frame.rows))));
	}
	if (prev_frame.cols < FRAME_WIDTH) {
		resize(prev_frame, prev_frame,
				Size(FRAME_WIDTH,
						round(FRAME_WIDTH / static_cast<float>(prev_frame.cols))
								* static_cast<float>(prev_frame.rows)));
	}

	//转换到HSV(HSV 为色相，饱和度，明度) 《图像处理、分析与机器视觉》P27
	cvtColor(curr_frame, hsv1, CV_BGR2HSV);
	cvtColor(prev_frame, hsv2, CV_BGR2HSV);

	//计算hsv直方图
	int hbins = 16, sbins = 16, vbins = 16;
	int hist_size[] = { hbins, sbins, vbins };
	//hue(色调)、saturation(饱和度),value(值)范围变化
	float hranges[] = { 0, 180 }, sranges[] = { 0, 256 },
			vranges[] = { 0, 256 };
	const float *ranges[] = { hranges, sranges, vranges };
	MatND hist1, hist2;
	int channels[] = { 0, 1, 2 };
	bool b_uniform = true, b_accumulate = false;

	calcHist(&hsv1, 1, channels, Mat(), hist1, 3, hist_size, ranges, b_uniform,
			b_accumulate);
	calcHist(&hsv2, 1, channels, Mat(), hist2, 3, hist_size, ranges, b_uniform,
			b_accumulate);

	normalize(hist1, hist1, 1, 0, NORM_L1, -1, Mat());
	normalize(hist2, hist2, 1, 0, NORM_L1, -1, Mat());

	//对比直方图
	double d_corr = compareHist(hist1, hist2, CV_COMP_INTERSECT);
	is_shot_bound = d_corr < threshold;
	BOOST_LOG_TRIVIAL(debug)<< "对比得分=" << d_corr;
	BOOST_LOG_TRIVIAL(debug)<< "是否找到一个镜头?=" << is_shot_bound;
	return is_shot_bound;
}
}

}
}


//
// Expansion Hunter
// Copyright 2016-2019 Illumina, Inc.
// All rights reserved.
//
// Author: Egor Dolzhenko <edolzhenko@illumina.com>,
//         Mitch Bekritsky <mbekritsky@illumina.com>, Richard Shaw
// Concept: Michael Eberle <meberle@illumina.com>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
//

#include "genotyping/RegionLengthEstimation.hh"

#include <boost/lexical_cast.hpp>
#include <boost/math/distributions.hpp>
#include <boost/math/distributions/binomial.hpp>

#include <cmath>
#include <iostream>
#include <random>
#include <vector>

using boost::lexical_cast;
using boost::math::cdf;
using boost::math::poisson_distribution;
using std::round;
using std::vector;

namespace ehunter
{

// Uses the standard Lander-Waterman model to estimate length of a workflow. The confidence interval is computed using a
// generic parametric bootstrap procedure (note that a simpler implementation using Poisson mean CI is possible).
void estimateRegionLength(
    int readCount, int readLength, double depth, int& regionLength, int& lowerBound, int& upperBound)
{
    const double proportionOfReadsStartAtPosition = depth / readLength;
    // The length of sub-workflow where reads can start and still be fully within the workflow
    const int extensionLength = static_cast<int>(round(readCount / proportionOfReadsStartAtPosition));

    const int kSeed = 42;
    std::mt19937 numberGenerator(kSeed);

    // Model for the number of reads that fall within the workflow
    std::poisson_distribution<> poisson(readCount);

    vector<int> bootstrapSamples;
    const int kNumSamples = 10000;
    for (int sampleIndex = 0; sampleIndex < kNumSamples; ++sampleIndex)
    {
        const int sampledReadCount = poisson(numberGenerator);
        const int sampledExtensionLength = static_cast<int>(round(sampledReadCount / proportionOfReadsStartAtPosition));
        const int bootstrapSample = sampledExtensionLength - extensionLength;

        bootstrapSamples.push_back(bootstrapSample);
    }

    // Compute 2.5% and 97.5% quantiles
    std::sort(bootstrapSamples.begin(), bootstrapSamples.end());
    const int lowerQuantile = *(bootstrapSamples.begin() + static_cast<int>(bootstrapSamples.size() * 0.025));
    const int upperQuantile = *(bootstrapSamples.begin() + static_cast<int>(bootstrapSamples.size() * 0.975));

    regionLength = extensionLength + readLength;

    lowerBound = readLength;
    lowerBound += extensionLength - upperQuantile > 0 ? extensionLength - upperQuantile : 0;

    upperBound = readLength;
    upperBound += extensionLength - lowerQuantile > 0 ? extensionLength - lowerQuantile : 0;
}

}


/** \defgroup main Главный модуль программы
    @{
*/

#include <iostream>
#include <cmath>
#include <vector>
#include <algorithm>
#include <chrono>
#include <thread>
#include <sstream>
#include <fstream>
#include <list>
#include <mutex>

#include <boost/asio/thread_pool.hpp>
#include <boost/asio/post.hpp>

//#define debug

using namespace std;
using namespace chrono;

/// Миллисекунды в формате дробного числа.
using d_milliseconds = duration<double, milliseconds::period>;

/// Массив с базовыми простыми числами (результат первого этапа алгоритма).
using FirstStepResult = vector<size_t>;

/// Массив с найденными простыми числами в ходе выполнения второго этапа алгоритма.
using SecondStepResult = vector<uint64_t>;

/// Вывести содержимое контейнера (где есть begin и end).
template <typename C>
string displayContainer(const C& container)
{
    typename C::const_iterator it;

    stringstream ss;

    for (it = container.begin(); it != container.end(); ++it)
    {
        ss << *it << " ";
    }

    ss << endl;

    return ss.str();
}

/**
 * @brief Бенчмарк.
 * @param func Выполняемая функция.
 * @param args Аргументы выполняемой функции.
 * @return Результат вычисления и время выполнения функции.
 */
template <typename Result, typename Function, typename... Args>
pair<double, Result> timeBenchmark(const Function& func, const Args&... args)
{
    Result calc;

    // начинаем считать время
    auto start = steady_clock::now();

    // что-то вычисляем
    calc = func(args...);

    // заканчиваем считать время
    auto end = steady_clock::now();

    auto time = duration_cast<d_milliseconds>(end-start).count();

    return make_pair(time, calc);
}

/// Запись данных в файл.
template <typename T>
void writeToFile(const string& filename, const T& data)
{
    ofstream f(filename);

    if (f)
    {
        for (auto c: data)
        {
            f << c << " ";
        }
    }
}

/**
 * @brief Первый этап модицифированного последовательного поиска простых чисел.
 * @details
 * Классический метод - решето Эратосфена.
 * Поиск в интервале [2; n]
 * @param n Верхняя граница поиска простых чисел.
 * @return Массив с базовыми простыми числами.
 */
FirstStepResult getBasePrimeBySieveOfEratosthenes(const size_t n)
{
    FirstStepResult basePrime;

    vector<uint8_t> range(n+1, 0);
    for (size_t i = 2; i <= n; ++i)
    {
        // если непомеченное простое число
        if (!range[i])
        {
            basePrime.push_back(i);
            // пометим другие числа на расстоянии шага i как составные
            for (size_t j = i + i; j <= n; j += i)
            {
                range[j] = 1;
            }
        }
    }

    return basePrime;
}

/**
 * @brief Второй этап модифицированного последовательного поиска простых чисел.
 * @param i_begin Начальный индекс поиска
 * @param i_end Конечный индекс поиска
 * @param basePrime Массив с базовыми простыми числами
 * @return Массив с простыми числами, найденные в диапазоне от (i_begin; i_end]
 */
SecondStepResult seqModSearch(const uint64_t i_begin,
                              const uint64_t i_end,
                              const FirstStepResult &basePrime
                              )
{
    SecondStepResult primeNumbers;

    for (uint64_t number = i_begin + 1; number <= i_end; ++number)
    {
        auto isDivide = [&](uint64_t basePrimeNumber)
        {
            return number % basePrimeNumber == 0;
        };

        const auto res = find_if(basePrime.begin(), basePrime.end(), isDivide);

        if (res == basePrime.end())
        {
            primeNumbers.push_back(number);
        }
    }

    return primeNumbers;
}

/**
 * @brief Второй этап модифицированного последовательного поиска простых чисел в полном диапазоне.
 * @param[in] i_begin Начальный индекс поиска
 * @param[in] i_end Конечный индекс поиска
 * @param[in] basePrime Массив с базовыми простыми числами
 * @param[in] basePrimeBegin Начальный индекс массива с базовыми простыми числами
 * @param[in] basePrimeEnd Конечный индекс массива с базовыми простыми числами
 * @param[out] fullRange Полный диапазон чисел, где 0 - простое, 1 - составное число.
 */
void seqModSearchInFullRange(const uint64_t i_begin,
                             const uint64_t i_end,
                             const FirstStepResult &basePrime,
                             const ptrdiff_t basePrimeBegin,
                             const ptrdiff_t basePrimeEnd,
                             vector<uint8_t> &fullRange
                             )
{
    uint64_t firstNumber = i_begin + 1;
    for (uint64_t number = firstNumber; number <= i_end; ++number)
    {
        auto isDivide = [&](uint64_t basePrimeNumber)
        {
            return number % basePrimeNumber == 0;
        };

        const auto res = find_if(basePrime.begin() + basePrimeBegin, basePrime.begin() + basePrimeEnd, isDivide);

        if (res != basePrime.begin() + basePrimeEnd)
        {
            fullRange[number - firstNumber] = 1;
        }
    }
}

/**
 * @brief Второй этап модифицированного последовательного поиска простых чисел в полном диапазоне по одному простому числу.
 * @param[in] i_begin Начальный индекс поиска
 * @param[in] i_end Конечный индекс поиска
 * @param[in] prime Простое число
 * @param[out] fullRange Полный диапазон чисел, где 0 - простое, 1 - составное число.
 */
void seqModSearchInFullRangeByOnePrime(const uint64_t i_begin,
                                       const uint64_t i_end,
                                       const size_t prime,
                                       vector<uint8_t> &fullRange
                                       )
{
    uint64_t firstNumber = i_begin + 1;
    for (uint64_t number = firstNumber; number <= i_end; ++number)
    {
        if (number % prime == 0)
        {
            fullRange[number - firstNumber] = 1;
        }
    }
}

/// Параллельная декомпозиция по данным.
SecondStepResult parDecompositionByData(uint64_t begin,
                                        uint64_t n,
                                        const FirstStepResult &basePrime,
                                        uint8_t threadCount
                                        )
{
#ifdef debug
    cout << "> Par decomposition by data [" << int(threadCount) << " threads] start..." << endl;
#endif

    // Результат - массив с простыми числами из диапазона от (begin, n].
    SecondStepResult prime;

    list<SecondStepResult> results;

    double range = double(n - begin) / double(threadCount);

#ifdef debug
    cout << "data range: " << range << endl;
#endif

    list<thread> threads;

    for (size_t i = 0; i < threadCount; ++i)
    {
        const auto i_begin = uint64_t(llround(double(i) * range)) + begin;
        const auto i_end = uint64_t(llround(double(i) * range + range)) + begin;

#ifdef debug
        cout << i << " thread - " << "begin: " << i_begin << " " << "end: " << i_end << endl;
#endif

        const auto threadHandler = [i_begin, i_end, &basePrime, &results]()
        {
            results.push_back(seqModSearch(i_begin, i_end, ref(basePrime)));
        };

        thread t { threadHandler };
        threads.push_back(move(t));
    }

    for (auto &t : threads)
    {
        t.join();
    }

    results.sort([](const SecondStepResult& v1, const SecondStepResult& v2)
    {
        return v1.front() < v2.front();
    });

    for (const auto &res: results)
    {
        prime.insert(prime.end(), res.begin(), res.end());
    }

    return prime;
}

/// Последовательная декомпозиция набора простых чисел.
SecondStepResult seqDecompositionByBasePrime(const uint64_t begin,
                                             const uint64_t n,
                                             const FirstStepResult &basePrime
                                             )
{
    const auto diff = n - begin;
    // диапазон чисел (begin, n]
    // 1  - если составное
    // 0 - если простое
    vector<uint8_t> fullRange(diff, 0);

    // Результат - массив с простыми числами из диапазона от (begin, n].
    SecondStepResult prime;

    seqModSearchInFullRange(begin, n, basePrime, ptrdiff_t(0), ptrdiff_t(basePrime.size()-1), fullRange);

    for (size_t i = 0; i < diff; ++i)
    {
        if (!fullRange[i])
        {
            prime.push_back(i + (begin + 1));
        }
    }

    return prime;
}

/// Параллельная декомпозиция набора простых чисел.
SecondStepResult parDecompositionByBasePrime(const uint64_t begin,
                                             const uint64_t n,
                                             const FirstStepResult &basePrime,
                                             const uint8_t threadCount
                                             )
{

#ifdef debug
    cout << "> Par decomposition by base prime [" << int(threadCount) << " threads] start..." << endl;
#endif

    const auto diff = n - begin;
    // диапазон чисел (begin, n]
    // 1  - если составное
    // 0 - если простое
    vector<uint8_t> fullRange(diff, 0);

    // Результат - массив с простыми числами из диапазона от (begin, n].
    SecondStepResult prime;

    double rangeForThread = double(basePrime.size()) / double(threadCount);

#ifdef debug
    cout << "base prime range for thread: " << rangeForThread << endl;
#endif

    list<thread> threads;

    for (size_t i = 0; i < threadCount; ++i)
    {
        const auto i_begin = ptrdiff_t(llround(double(i) * rangeForThread));
        const auto i_end = ptrdiff_t(llround(double(i) * rangeForThread + rangeForThread));

#ifdef debug
        cout << i << " thread - " << "begin: " << i_begin << " " << "end: " << i_end << endl;
#endif

        if (i_begin == i_end)
        {

#ifdef debug
            cout << "skip thread, because i_begin == i_end" << endl;
#endif
            continue;
        }

        thread t { seqModSearchInFullRange, begin, n, ref(basePrime), i_begin, i_end, ref(fullRange) };
        threads.push_back(move(t));
    }

    for (auto &t : threads)
    {
        t.join();
    }

    for (size_t i = 0; i < diff; ++i)
    {
        if (!fullRange[i])
        {
            prime.push_back(i + (begin + 1));
        }
    }

    return prime;
}

/// Параллельный алгоритм с использованием Thread Pool
SecondStepResult parThreadPool(uint64_t begin,
                               uint64_t n,
                               const FirstStepResult &basePrime,
                               const uint8_t threadCount
                               )
{

#ifdef debug
    cout << "> Par thread pool start..." << endl;
#endif

    const auto diff = n - begin;
    // диапазон чисел (begin, n]
    // 1  - если составное
    // 0 - если простое
    vector<uint8_t> fullRange(diff, 0);

    // Результат - массив с простыми числами из диапазона от (begin, n].
    SecondStepResult prime;

    const auto basePrimeSize = basePrime.size();

    boost::asio::thread_pool pool(threadCount);

    for (size_t i = 0; i < basePrimeSize; ++i)
    {
        const auto prime = basePrime[i];

        boost::asio::post(pool, [prime, begin, n, &fullRange]()
        {
            seqModSearchInFullRangeByOnePrime(begin, n, prime, fullRange);
        });

    }

    // ожидаем все потоки в пуле
    pool.join();

    for (size_t i = 0; i < diff; ++i)
    {
        if (!fullRange[i])
        {
            prime.push_back(i + (begin + 1));
        }
    }

    return prime;
}

/// Параллельный алгоритм с использованием Thread Pool
SecondStepResult parPrimeEnumeration(uint64_t begin,
                                     uint64_t n,
                                     const FirstStepResult &basePrime,
                                     const uint8_t threadCount
                                     )
{

#ifdef debug
    cout << "> Par prime enumeration [" << int(threadCount) << " threads] start..." << endl;
#endif

    const auto diff = n - begin;
    // диапазон чисел (begin, n]
    // 1  - если составное
    // 0 - если простое
    vector<uint8_t> fullRange(diff, 0);

    const auto basePrimeSize = basePrime.size();

    list<thread> threads;

    size_t currentPrimeIndex = 0;
    mutex m;

    for (size_t i = 0; i < threadCount; ++i)
    {
        auto handler = [begin, n, basePrimeSize, &basePrime, &m, &currentPrimeIndex, &fullRange]()
        {
            while (true)
            {
                unique_lock lck {m};

                if (currentPrimeIndex >= basePrimeSize)
                {
                    break;
                }

                auto prime = basePrime[currentPrimeIndex++];

                // разблокируем мьютекс, так как перестали работать
                // с разделяемым ресурсом (индекс)
                lck.unlock();

                seqModSearchInFullRangeByOnePrime(begin, n, prime, fullRange);
            }
        };

        thread t { handler };
        threads.push_back(move(t));
    }

    for (auto &t : threads)
    {
        t.join();
    }

    // Результат - массив с простыми числами из диапазона от (begin, n].
    SecondStepResult prime;

    for (size_t i = 0; i < diff; ++i)
    {
        if (!fullRange[i])
        {
            prime.push_back(i + (begin + 1));
        }
    }

    return prime;
}

int main(int argc, char *argv[])
{
    cout << thread::hardware_concurrency() << endl;

    constexpr size_t default_n = 100;

    auto n = default_n;

    if (argc >= 2)
    {
        n = stoull(argv[1]);
    }

    const auto sqrtN = static_cast<size_t>(sqrt(n));

    // Первый шаг.
    const auto firstStepRes = timeBenchmark<FirstStepResult>(getBasePrimeBySieveOfEratosthenes, sqrtN);
    cout << "> First step: " << firstStepRes.first << " ms" << endl;
    writeToFile("firstStep.txt", firstStepRes.second);

    // Последовательная декомпозиция по данным
    const auto seqDecompositionByDataRes = timeBenchmark<SecondStepResult>(seqModSearch, sqrtN, n, ref(firstStepRes.second));
    cout << "> Seq decomposition by data: " << seqDecompositionByDataRes.first << " ms" << endl;
    writeToFile("SeqDecompositionByData.txt", seqDecompositionByDataRes.second);

    // Параллельная декомпозиция по данным: threadCount потоков
    const auto doParDecompositionByData = [&](uint8_t threadCount)
    {
        const auto parDecompositionByDataRes = timeBenchmark<SecondStepResult>(parDecompositionByData, sqrtN, n, ref(firstStepRes.second), threadCount);
        cout << "> Par decomposition by data [" + to_string(threadCount) + " threads]: " << parDecompositionByDataRes.first << " ms" << endl;
        writeToFile("ParDecompositionByData_" + to_string(threadCount) + "t.txt", parDecompositionByDataRes.second);
    };

    doParDecompositionByData(2);
    doParDecompositionByData(4);
    doParDecompositionByData(8);

    // Последовательная декомпозиция набора простых чисел
    const auto seqDecompositionByBasePrimeRes = timeBenchmark<SecondStepResult>(seqDecompositionByBasePrime, sqrtN, n, ref(firstStepRes.second));
    cout << "> Seq decomposition by base prime set: " << seqDecompositionByBasePrimeRes.first << " ms" << endl;
    writeToFile("SeqDecompositionByBasePrime.txt", seqDecompositionByBasePrimeRes.second);

    // Параллельная декомпозиция набора простых чисел: threadCount потоков
    const auto doParDecompositionByBasePrime = [n, sqrtN, &firstStepRes](uint8_t threadCount)
    {
        const auto parDecompositionByBasePrimeRes = timeBenchmark<SecondStepResult>(parDecompositionByBasePrime, sqrtN, n, ref(firstStepRes.second), threadCount);
        cout << "> Par decomposition by base prime set [" + to_string(threadCount) + " threads]: " << parDecompositionByBasePrimeRes.first << " ms" << endl;
        writeToFile("ParDecompositionByBasePrime_" + to_string(threadCount) + "t.txt", parDecompositionByBasePrimeRes.second);
    };

    doParDecompositionByBasePrime(2);
    doParDecompositionByBasePrime(4);
    doParDecompositionByBasePrime(8);

    // С применением Thread Pool
    const auto doParThreadPool = [n, sqrtN, &firstStepRes]()
    {
        const auto parThreadPoolRes = timeBenchmark<SecondStepResult>(parThreadPool, sqrtN, n, ref(firstStepRes.second), uint8_t(thread::hardware_concurrency()));
        cout << "> Par thread pool [" << thread::hardware_concurrency() << " threads] [" + to_string(firstStepRes.second.size()) + " tasks]: " << parThreadPoolRes.first << " ms" << endl;
        writeToFile("ParThreadPool.txt", parThreadPoolRes.second);
    };

    // Пул потоков
    doParThreadPool();

    // С последовательным перебором простых чисел
    const auto doParPrimeEnumeration = [n, sqrtN, &firstStepRes](uint8_t threadCount)
    {
        const auto parPrimeEnumerationRes = timeBenchmark<SecondStepResult>(parPrimeEnumeration, sqrtN, n, ref(firstStepRes.second), threadCount);
        cout << "> Par prime enumeration [" << int(threadCount) << " threads]: " << parPrimeEnumerationRes.first << " ms" << endl;
        writeToFile("ParPrimeEnumeration_" + to_string(threadCount) + "t.txt", parPrimeEnumerationRes.second);
    };

    doParPrimeEnumeration(2);
    doParPrimeEnumeration(4);
    doParPrimeEnumeration(8);

    system("pause");
    return 0;
}
/** @} */


//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

int main () {
    using namespace boost::numeric::ublas;
    vector<double> v1 (3), v2 (3);
    for (unsigned i = 0; i < (std::min) (v1.size (), v2.size ()); ++ i)
        v1 (i) = v2 (i) = i;

    std::cout << v1 + v2 << std::endl;
    std::cout << v1 - v2 << std::endl;
}


#include <iostream>
#include <cassert>
#include <iomanip>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> GraphTraits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, int,
                                              boost::property<boost::edge_residual_capacity_t, int,
                                                              boost::property<boost::edge_reverse_t, GraphTraits::edge_descriptor,
                                                                              boost::property<boost::edge_weight_t, int>>>>>
    Graph;

const int unreachable_distance = std::numeric_limits<int>::max();

bool check_shelter_reachable_in_t(const std::vector<std::vector<int>> &distance_matrix, const int t)
{
  const int a = distance_matrix.size();
  const int s = distance_matrix.at(0).size(); // HACK This might be double the s in testcase()

  int next_free_node = 0;
  const int node_source = next_free_node++;
  const int node_sink = next_free_node++;
  const auto get_agent_node = [next_free_node, a](int i) {
    assert(i >= 0 && i < a);
    return next_free_node + i;
  };
  next_free_node += a;
  const auto get_shelter_node = [next_free_node, s](int i) {
    assert(i >= 0 && i < s);
    return next_free_node + i;
  };
  next_free_node += s;
  const int num_nodes = next_free_node;
  Graph G(num_nodes);

  const auto add_edge = [&G](int from, int to) {
    DEBUG(5, "add_edge(" << from << ", " << from << ")");
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const Graph::edge_descriptor e = boost::add_edge(from, to, G).first;
    const Graph::edge_descriptor rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = 1;
    c_map[rev_e] = 0;
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  };

  for (int i_a = 0; i_a < a; i_a++)
  {
    add_edge(node_source, get_agent_node(i_a));
  }
  for (int i_s = 0; i_s < s; i_s++)
  {
    add_edge(get_shelter_node(i_s), node_sink);
  }
  for (int i_a = 0; i_a < a; i_a++)
  {
    for (int i_s = 0; i_s < s; i_s++)
    {
      const int dist = distance_matrix.at(i_a).at(i_s);
      if (dist != unreachable_distance && dist <= t)
      {
        add_edge(get_agent_node(i_a), get_shelter_node(i_s));
      }
    }
  }

  const int flow = boost::push_relabel_max_flow(G, node_source, node_sink);
  DEBUG(3, "t " << t << " flow " << flow);
  assert(flow >= 0 && flow <= a && flow <= s);
  return flow == a;
}

int find_lower_bound(std::function<bool(int)> is_match)
{
  if (is_match(0))
  {
    return 0;
  }

  int base = 0;
  while (!is_match(1 << base))
  {
    base++;
  }

  int low = base == 0 ? 0 : 1 << (base - 1);
  int high = 1 << base;
  assert(!is_match(low) && is_match(high));
  while (low < high)
  {
    const int mid = (low + high) / 2;
    if (is_match(mid))
    {
      high = mid;
    }
    else
    {
      low = mid + 1;
    }
  }
  assert(!is_match(low - 1) && is_match(low));
  return low;
}

void testcase()
{
  int n, m, a, s, c, d;
  std::cin >> n >> m >> a >> s >> c >> d;
  assert(n >= 1 && n <= 1e3 && m >= 0 && m <= 5e3);
  assert(a >= 1 && a <= 1e2 && s >= 1 && s <= 1e2);
  assert(c >= 1 && c <= 2);
  assert(d >= 1 && d <= 1e3);

  Graph G(n);
  const auto add_edge = [&G](int from, int to, int weight) {
    auto w_map = boost::get(boost::edge_weight, G);
    const auto e_res = boost::add_edge(from, to, G);
    const Graph::edge_descriptor e = e_res.first;
    const bool is_new = e_res.second;
    w_map[e] = is_new ? weight : std::min(w_map[e], weight);
  };
  for (int i = 0; i < m; i++)
  {
    std::string w_string;
    int x, y, z;
    std::cin >> w_string >> x >> y >> z;
    assert(w_string == "S" || w_string == "L");
    assert(x >= 0 && x < n && y >= 0 && y < n && z >= 1 && z <= 1e4);
    const bool is_lift = w_string == "L";

    add_edge(x, y, z);
    if (is_lift)
    {
      add_edge(y, x, z);
    }
  }

  std::vector<int> nodes_by_agent(a);
  for (int &node : nodes_by_agent)
  {
    std::cin >> node;
    assert(node >= 0 && node < n);
  }

  std::vector<int> nodes_by_shelter(s);
  for (int &node : nodes_by_shelter)
  {
    std::cin >> node;
    assert(node >= 0 && node < n);
  }

  std::vector<std::vector<int>> distance_matrix(a, std::vector<int>(c * s, unreachable_distance));
  std::vector<int> temp_distances(n);
  auto temp_distance_map = boost::make_iterator_property_map(temp_distances.begin(), boost::get(boost::vertex_index, G));
  for (int i_a = 0; i_a < a; i_a++)
  {
    boost::dijkstra_shortest_paths(G, nodes_by_agent.at(i_a), boost::distance_map(temp_distance_map).distance_inf(unreachable_distance));
    for (int i_s = 0; i_s < s; i_s++)
    {
      const int dist = temp_distances.at(nodes_by_shelter.at(i_s));
      if (dist != unreachable_distance)
      {
        for (int i_c = 0; i_c < c; i_c++)
        {
          const int virtual_i_s = i_c * s + i_s;
          distance_matrix.at(i_a).at(virtual_i_s) = dist + i_c * d;
          DEBUG(4, "distance_matrix.at(" << i_a << ").at(" << virtual_i_s << ") = " << distance_matrix.at(i_a).at(virtual_i_s));
        }
      }
    }
  }

  int t = find_lower_bound([&distance_matrix](const int test_t) {
    return check_shelter_reachable_in_t(distance_matrix, test_t);
  });
  std::cout << t + d << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);
  std::cout << std::fixed << std::setprecision(0);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
    DEBUG(1, "");
  }

  return 0;
}


// -----------------------------------------------------------------------------
// Copyright (c) 2022 Mohamed Aladem
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this softwareand associated documentation files(the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and /or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions :
//
// The above copyright noticeand this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
// -----------------------------------------------------------------------------

#include "micro_graph_optimizer.h"

#include <Eigen/Geometry>
#include <string>
#include <iostream>
#include <map>
#include <fstream>

using namespace mgo;

// This example demonstrates optimzing a non-linear 2D SLAM problem read from a g2o file.

double normalize_angle(double theta_rad)
{
  // Normalize the angle to the range [-pi, pi).
  constexpr double kPI = 3.14159265358979323846;
  constexpr double k2PI = 2.0 * kPI;
  return (theta_rad - k2PI * std::floor((theta_rad + kPI) / k2PI));
}

class Pose2d : public Variable
{
public:
  Pose2d(double x, double y, double yaw_rad) :
    m_x(x), m_y(y), m_yaw_rad(yaw_rad)
  {
  }

  virtual int dim()const override { return 3; }
  virtual void plus(const Eigen::VectorXd& delta) override
  {
    m_x += delta[0];
    m_y += delta[1];
    m_yaw_rad = normalize_angle(m_yaw_rad + delta[2]);
  }

  double x()const { return m_x; }
  double y()const { return m_y; }
  double yaw_rad()const { return m_yaw_rad; }

private:
  double m_x, m_y, m_yaw_rad;
};

class Constraint2d : public Factor
{
public:
  Constraint2d(Pose2d* v_a, Pose2d* v_b, double x_ab, double y_ab,
    double yaw_ab_rad, const Eigen::Matrix3d& sqrt_info) :
    m_pos_ab(x_ab, y_ab), m_yaw_ab_rad(yaw_ab_rad), m_sqrt_info(sqrt_info)
  {
    add_variable(v_a);
    add_variable(v_b);
  }

  virtual int dim()const { return 3; }

  virtual Eigen::VectorXd error()const override
  {
    MGO_ASSERT(this->num_variables() == 2);
    const Pose2d* v_a = static_cast<Pose2d*>(this->variable_at(0));
    const Pose2d* v_b = static_cast<Pose2d*>(this->variable_at(1));
    Eigen::Vector3d r;
    Eigen::Vector2d pos_ab_pred = { v_b->x() - v_a->x(), v_b->y() - v_a->y() };
    r.head<2>() = Eigen::Rotation2Dd(v_a->yaw_rad()).toRotationMatrix().transpose() * pos_ab_pred - m_pos_ab;
    r(2) = normalize_angle((v_b->yaw_rad() - v_a->yaw_rad()) - m_yaw_ab_rad);
    return r;
  }

  virtual Eigen::VectorXd subtract_error(const Eigen::VectorXd& e1, const Eigen::VectorXd& e2)const override
  {
    Eigen::Vector3d diff;
    diff << (e1(0) - e2(0)), (e1(1) - e2(1)), normalize_angle(e1(2) - e2(2));
    return diff;
  }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

private:
  Eigen::Vector2d m_pos_ab;
  double m_yaw_ab_rad;
  Eigen::Matrix3d m_sqrt_info;
};

bool read_g2o(const std::string& filename, mgo::FactorGraph* graph)
{
  std::ifstream file(filename);
  if (!file.is_open())
  {
    MGO_LOG("Failed to open file: %s", filename.c_str());
    return false;
  }

  std::string line;
  std::map<int, Pose2d*> id_to_pose;
  while (std::getline(file, line))
  {
    std::stringstream ss(line);
    std::string data_type;
    ss >> data_type;
    if (data_type == "VERTEX_SE2")
    {
      int id;
      double x, y, th;
      ss >> id >> x >> y >> th;
      Pose2d* p = new Pose2d(x, y, normalize_angle(th));
      graph->add_variable(p);
      id_to_pose[id] = p;
    }
    else if (data_type == "EDGE_SE2")
    {
      int id_a, id_b;
      double dx, dy, d_yaw, i11, i12, i13, i22, i23, i33;
      ss >> id_a >> id_b >> dx >> dy >> d_yaw >> i11 >> i12 >> i13 >> i22 >> i23 >> i33;
      Eigen::Matrix3d info_mtrx = (Eigen::Matrix3d() <<
        i11, i12, i13,
        i12, i22, i23,
        i13, i23, i33).finished();
      MGO_ASSERT(id_to_pose.count(id_a) != 0);
      MGO_ASSERT(id_to_pose.count(id_b) != 0);
      graph->add_factor(new Constraint2d(id_to_pose[id_a], id_to_pose[id_b], dx, dy,
        d_yaw, info_mtrx.llt().matrixL()));
    }
    else
    {
      MGO_LOG("Unhandled type: %s", data_type.c_str());
      return false;
    }
  }
  return true;
}

void dump_poses(const std::string& filename, const FactorGraph& graph)
{
  std::ofstream file(filename);
  if (!file.is_open())
  {
    MGO_LOG("Failed to open file: %s", filename.c_str());
    return;
  }

  const std::vector<Variable*>& variables = graph.get_variables();
  for (int i = 0, count = variables.size(); i < count; ++i)
  {
    Pose2d* p = static_cast<Pose2d*>(variables[i]);
    file << i << " " << p->x() << " " << p->y() << " " << p->yaw_rad() << std::endl;
  }
}

int main()
{
  FactorGraph graph;
  // You can get this dataset from: https://lucacarlone.mit.edu/datasets/
  if (!read_g2o("./input_M3500_g2o.g2o", &graph))
  {
    return -1;
  }

  // Fix the first variable.
  graph.get_variables()[0]->fixed = true;

  dump_poses("./original.txt", graph);
  mgo::optimize_gn(&graph);
  dump_poses("./optimized.txt", graph);

  return 0;
}


#ifndef _ROBOLAND_COMMAND_TOOLS_HH_
#define _ROBOLAND_COMMAND_TOOLS_HH_

#include <Eigen/Dense>

#include <array>

using namespace std;

namespace roboland {

namespace tools {

class BSpline
{
public:
  class Exception : public std::exception
  {
  private:
    string msg;
  public:
    Exception(const string msg):
      msg("Exception in spline: " + msg)
    {
    }

    const char *what() const noexcept override
    {
      return msg.data();
    }
  };

  typedef Eigen::Matrix<double, 1, 5> Coeffs;
  typedef Eigen::Matrix<double, 5, 1> CoeffsCompl;

  struct Point
  {
    Point() = default;
    Point(const double x,
          const double y);

    bool operator==(const BSpline::Point &other) const;
    double x;
    double y;
    double getSlope(const BSpline::Point &other) const;
  };

  struct Spline
  {
    Spline() = default;
    Spline(const array<Point, 3> &points,
           const double s_i_1);
    Spline(const array<BSpline::Point, 2> &init_point,
           const double s_i_1);

    double start_x;
    double end_x;
    array<Point, 3> points;
    array<double, 4> control_points;

    int isInRange(const double x) const;
    double scaleX(const double x) const;
    void calculateControlPoints(const double s_i_1);
    double getValue(const double x) const;
    double getDerValue(const double x) const;
    double get2DerValue(const double x) const;
    bool validSpline() const;
    string getRangeStr() const;
  };

  BSpline() = default;
  BSpline(const vector<BSpline::Point> &initial_points,
               const float min_thr=INFINITY,
               const float max_thr=INFINITY,
               const float slope_thr=INFINITY);
  BSpline::Spline createSpline(const array<Point, 4> &points);

  int addPoint(const Point &point);
  double getValue(const double x) const;
  double getDerValue(const double x) const;
  double get2DerValue(const double x) const;
  void erase(const double x);
  void eraseUpper(const double x);
  bool isInRange(const double x) const;
private:
  vector<Spline> splines;
  double min;
  double max;
  double min_thr;
  double max_thr;
  double slope_thr;

  static CoeffsCompl getCoeffsCompl(const double x);
  static CoeffsCompl getDerCoeffsCompl(const double x);
};

double radianRound(double theta);

} // namespace tools

} // namespace roboland

#endif /* _ROBOLAND_COMMAND_TOOLS_HH_ */


#include <boost/property_map/dynamic_property_map.hpp>
#include <boost/config.hpp>
#include <boost/graph/graphml.hpp>
#include "include_and_types.cpp"
#include "my_pearce.cpp"
#include "my_nuutila.cpp"
#include "my_tarjan.cpp"
#include "my_pearce_not_recursive.cpp"
#include <boost/timer/timer.hpp>
#include <iostream>
#include <vector>
#include <stack>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>

#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/random/linear_congruential.hpp>

using namespace boost;

typedef boost::erdos_renyi_iterator<boost::minstd_rand, Graph> ERGen;

template <class Result1, class Result2>
bool compare_results(const Result1 r1, const Result2 r2){
    if (r1.size() != r2.size())
        return false;
    int maxVal=0;
    int minVal=0;
    for(int i=0; i!=r1.size(); i++){
        maxVal = std::max(maxVal, r1[i]);
        minVal = std::min(minVal, r2[i]);
    }
    minVal-=1;
    maxVal+=1;
    int comp[maxVal];
    for (int i=0; i != maxVal; i++)
        comp[i]=minVal;

    for (int i = 0; i != r1.size(); i++){
        if (comp[r1[i]]==minVal) {
            for (int j = 0; j < i; j++) {
                if (r1[i] != r1[j] && r2[i] == r2[j])
                    return false;
            }
            comp[r1[i]] = r2[i];
        }
        if (comp[r1[i]]!=r2[i]){
            return false;
        }
    }
    return true;
}

std::vector<int> correct_nuutila_root(std::vector<int> vec){
    bool ok = false;
    while(!ok){
        ok = true;
        for(int i=0; i<vec.size(); i++){
            if(vec[i] != i && vec[i]!= vec[vec[i]]){
                vec[i]=vec[vec[i]];
                ok=false;
            }
        }
    }
    return vec;
}
int main(int, char*[])
{
    /*
    typedef std::pair<int, int> Edge;
    const int num_nodes = 8;
    enum nodes { A, B, C, D, E, F, G, H, I, L, M, N};
    char name[] = "ABCDEFGHILMN";

    Edge edge_array[] = { Edge(A, B),
                          Edge(B, C), Edge(B, H),
                          Edge(C, D), Edge(C, G),
                          Edge(D, E),
                          Edge(E, F), Edge(E, C),
                          Edge(G, F), Edge(G, D),
                          Edge(H, A), Edge(H, G),
                          Edge(I, L),
                          Edge(M, N), Edge(N, M)
    };
    int num_arcs = sizeof(edge_array) / sizeof(Edge);
    Graph g(edge_array, edge_array + num_arcs, num_nodes);
    std::cout << "A directed graph:" << std::endl;
    print_graph(g,name);
    std::cout << std::endl;
    */

    dynamic_properties dp;
    minstd_rand gen;
    // Create graph with 100 nodes and edges with probability 0.05
    int nodes= 30;
    Graph g(ERGen(gen, nodes, 0.05), ERGen(), nodes);

    write_graphml(std::cout, g, dp, true);

    // Tarjan's algorithm over the given graph
    std::cout << "Tarjan\t\t" << std::flush;
    TarjanClass<typeInt, typeInt, typeInt, typeBool, typeBool, typeInt> tarjan(&g);
    std::vector<int>* component = tarjan.tarjan_scc();

    // Nuutila's algorithm over the given graph
    std::cout << "Nuutila\t\t" << std::flush;
    NuutilaClass<typeInt, typeInt, typeBool> nuutila(&g);
    std::vector<int>* root_nuutila = nuutila.nuutila_scc();

    // Pearce's recursive algorithm over the given graph
    std::cout << "Pearce\t\t" << std::flush;
    PearceClass<typeInt> pearce(&g);
    std::vector<int>* rindex = pearce.pearce_scc();

    // Pearce's non recursive algorithm over the given graph
    std::cout << "PearceNR\t\t" << std::flush;
    PearceNR <typeBool, typeInt> pearcenr(&g);
    std::vector<int>* rindexNR = pearcenr.pearce_not_recursive_scc();

    /*std::cout << "Pearce_NR\t" << std::flush;
    std::vector<int> rindex_not_recursive(num_vertices(g));
    int num_pearce_not_recursive = pearce_not_recursive_scc(g, make_iterator_property_map(rindex_not_recursive.begin(), get(vertex_index, g)));
    */
    std::vector<int> bgl_component(num_vertices(g)), discover_time(num_vertices(g));
    std::vector<default_color_type> bgl_color(num_vertices(g));
    std::vector<Vertex> bgl_root(num_vertices(g));
    int num_bgl = strong_components(g,
                                make_iterator_property_map(bgl_component.begin(), get(vertex_index, g)),
                                root_map(make_iterator_property_map(bgl_root.begin(), get(vertex_index, g))).
                                        color_map(make_iterator_property_map(bgl_color.begin(), get(vertex_index, g))).
                                        discover_time_map(make_iterator_property_map(discover_time.begin(), get(vertex_index, g))));

    std::cout << "Number of components: bgl:"<< num_bgl << std::endl;
    //std::cout << "Total number of components: " << num << std::endl;

    std::cout << "No" << "\t->\tBgl\tTa\tNu\tPe\tPeNr" << std::endl;
    for (int i = 0; i != (*component).size(); ++i){
        std::cout << i << "\t->\t"<< bgl_component[i] << "\t" << (*component)[i] << "\t" << (*root_nuutila)[i] << "\t" << (*rindex)[i] << "\t" << (*rindexNR)[i] << std::endl;
    }
    return 0;
}