/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/exercise.hpp>
#include <ql/pricingengines/blackformula.hpp>
#include <ql/experimental/models/fxoptionhelper.hpp>
#include <ql/time/calendars/nullcalendar.hpp>

#include <boost/make_shared.hpp>

namespace QuantLib {

FxOptionHelper::FxOptionHelper(
    const Period &maturity, const Calendar &calendar, const Real strike,
    const Handle<Quote> fxSpot, const Handle<Quote> volatility,
    const Handle<YieldTermStructure> &domesticYield,
    const Handle<YieldTermStructure> &foreignYield,
    CalibrationHelper::CalibrationErrorType errorType)
    : CalibrationHelper(volatility, domesticYield, errorType),
      hasMaturity_(true), maturity_(maturity), calendar_(calendar),
      strike_(strike), fxSpot_(fxSpot), foreignYield_(foreignYield) {
    registerWith(fxSpot_);
    registerWith(foreignYield_);
}

FxOptionHelper::FxOptionHelper(
    const Date &exerciseDate, const Real strike, const Handle<Quote> fxSpot,
    const Handle<Quote> volatility,
    const Handle<YieldTermStructure> &domesticYield,
    const Handle<YieldTermStructure> &foreignYield,
    CalibrationHelper::CalibrationErrorType errorType)
    : CalibrationHelper(volatility, domesticYield, errorType),
      hasMaturity_(false), exerciseDate_(exerciseDate), strike_(strike),
      fxSpot_(fxSpot), foreignYield_(foreignYield) {
    registerWith(fxSpot_);
    registerWith(foreignYield_);
}

void FxOptionHelper::performCalculations() const {
    if (hasMaturity_)
        exerciseDate_ =
            calendar_.advance(termStructure_->referenceDate(), maturity_);
    tau_ = termStructure_->timeFromReference(exerciseDate_);
    atm_ = fxSpot_->value() * foreignYield_->discount(tau_) /
           termStructure_->discount(tau_);
    effStrike_ = strike_;
    if(effStrike_ == Null<Real>())
        effStrike_ = atm_;
    type_ = effStrike_ >= atm_ ? Option::Call : Option::Put;
    boost::shared_ptr<StrikedTypePayoff> payoff(
        new PlainVanillaPayoff(type_, effStrike_));
    boost::shared_ptr<Exercise> exercise =
        boost::make_shared<EuropeanExercise>(exerciseDate_);
    option_ =
        boost::shared_ptr<VanillaOption>(new VanillaOption(payoff, exercise));
    CalibrationHelper::performCalculations();
}

Real FxOptionHelper::modelValue() const {
    calculate();
    option_->setPricingEngine(engine_);
    return option_->NPV();
}

Real FxOptionHelper::blackPrice(Real volatility) const {
    calculate();
    const Real stdDev = volatility * std::sqrt(tau_);
    return blackFormula(type_, strike_, atm_, stdDev, termStructure_->discount(tau_));
}

} // namespace QuantLib


#include <SBGATPolyhedronGravityModel.hpp>
#include <SBGATPolyhedronGravityModelUQ.hpp>
#include <SBGATObjWriter.hpp>

#include <vtkCleanPolyData.h>
#include <vtkOBJReader.h>

#include <json.hpp>
#include <boost/progress.hpp>

#include <armadillo>

int main(){

	std::ifstream i("input_file.json");
	nlohmann::json input_data;
	i >> input_data;

	arma::arma_rng::set_seed(0);

	std::string PATH_SHAPE = input_data["PATH_SHAPE"];
	double CORRELATION_DISTANCE =  input_data["CORRELATION_DISTANCE"];

	double ERROR_STANDARD_DEV  = input_data["ERROR_STANDARD_DEV"];
	double DENSITY  = input_data["DENSITY"];
	double PERIOD_SD  = input_data["PERIOD_SD"];
	double PERIOD  = input_data["PERIOD"];

	bool UNIT_IN_METERS  = input_data["UNIT_IN_METERS"];
	bool HOLD_MASS_CONSTANT  = input_data["HOLD_MASS_CONSTANT"];

	int N_MONTE_CARLO = input_data["N_MONTE_CARLO"];
	std::vector<int> COV_REGION_CENTERS = input_data["COV_REGION_CENTERS"];

	std::string OUTPUT_DIR = input_data["OUTPUT_DIR"];
	std::string UNCERTAINTY_TYPE = input_data["UNCERTAINTY_TYPE"];

	std::vector<unsigned int > FACETS_TO_INVESTIGATE = input_data["FACETS_TO_INVESTIGATE"];

	std::cout << "- Path to shape: " << PATH_SHAPE << std::endl;
	std::cout << "- Uncertainty type: " << UNCERTAINTY_TYPE << std::endl;
	std::cout << "- Standard deviation on point coordinates (m) : " << ERROR_STANDARD_DEV << std::endl;
	std::cout << "- Correlation distance (m) : " << CORRELATION_DISTANCE << std::endl;
	std::cout << "- Standard deviation on rotation period (s) : " << PERIOD_SD << std::endl;
	std::cout << "- Density (kg/m^3) : " << DENSITY << std::endl;
	std::cout << "- Rotation period (s) : " << PERIOD << std::endl;
	std::cout << "- Covariance region centers:\n" ;
	for(auto center : COV_REGION_CENTERS){
		std::cout << "\t" << center << std::endl;
	}
	std::cout << "- Facets to investigate:\n" ;
	for(auto facet : FACETS_TO_INVESTIGATE){
		std::cout << "\t" << facet << std::endl;
	}

	// Reading
	vtkSmartPointer<vtkOBJReader> reader = vtkSmartPointer<vtkOBJReader>::New();
	reader -> SetFileName(PATH_SHAPE.c_str());
	reader -> Update();

	// An instance of SBGATPolyhedronGravityModel is created to evaluate the PGM of
	// the considered polytdata
	vtkSmartPointer<SBGATPolyhedronGravityModel> pgm_filter = vtkSmartPointer<SBGATPolyhedronGravityModel>::New();
	pgm_filter -> SetInputConnection(reader -> GetOutputPort());
	pgm_filter -> SetDensity(DENSITY);

	if(UNIT_IN_METERS){
		pgm_filter -> SetScaleMeters();
	} else{
		pgm_filter -> SetScaleKiloMeters();
	}

	std::cout << "Building pgm ...\n";
	pgm_filter -> SetOmega(2 * arma::datum::pi / PERIOD * arma::vec({0,0,1}));
	pgm_filter -> Update();

	// An instance of SBGATPolyhedronGravityModelUQ is created to perform
	// uncertainty quantification from the PGM associated to the shape
	SBGATPolyhedronGravityModelUQ pgm_uq;
	pgm_uq.SetModel(pgm_filter);
	pgm_uq.SetPeriodErrorStandardDeviation(PERIOD_SD);
	pgm_uq.PrecomputeMassPropertiesPartials();

	// Saving baseline slices
	pgm_uq.TakeAndSaveSlice(0,OUTPUT_DIR + "baseline_slice_x.txt",0);
	pgm_uq.TakeAndSaveSlice(1,OUTPUT_DIR + "baseline_slice_y.txt",0);
	pgm_uq.TakeAndSaveSlice(2,OUTPUT_DIR + "baseline_slice_z.txt",0);

	std::cout << "Populating shape covariance ...\n";

	// Populate the shape vertices covariance

	if (UNCERTAINTY_TYPE == "radial"){
		for (auto region_center : COV_REGION_CENTERS){
			pgm_uq.AddRadialUncertaintyRegionToCovariance(region_center,ERROR_STANDARD_DEV,CORRELATION_DISTANCE);

		}
	}
	else if (UNCERTAINTY_TYPE == "normal"){
		for (auto region_center : COV_REGION_CENTERS){

			pgm_uq.AddRadialUncertaintyRegionToCovariance(region_center,ERROR_STANDARD_DEV,CORRELATION_DISTANCE);

		}
	}
	else if (UNCERTAINTY_TYPE == "global"){
		pgm_uq.ComputeVerticesCovarianceGlobal(ERROR_STANDARD_DEV,CORRELATION_DISTANCE);
	}
	else{
		throw(std::runtime_error("Got unknown uncertainty direction type: " + UNCERTAINTY_TYPE));
	}

	arma::mat P_CC = pgm_uq.GetVerticesCovariance();
	arma::mat C_CC = pgm_uq.GetCovarianceSquareRoot();

	P_CC.save(OUTPUT_DIR + "full_covariance.txt",arma::raw_ascii);
	C_CC.save(OUTPUT_DIR + "full_covariance_sqrt.txt",arma::raw_ascii);

	// Regularizing the covariance
	int regularized_eigen_values = pgm_uq.RegularizeCovariance();

	std::cout << regularized_eigen_values << " eigenvalues were regularized\n";
	C_CC = pgm_uq.GetCovarianceSquareRoot();

	P_CC = pgm_uq.GetVerticesCovariance();
	P_CC.save(OUTPUT_DIR + "full_covariance_regularized.txt",arma::raw_ascii);
	C_CC.save(OUTPUT_DIR + "full_covariance_sqrt_regularized.txt",arma::raw_ascii);

	regularized_eigen_values = pgm_uq.RegularizeCovariance();

	std::cout << regularized_eigen_values << " eigenvalues were regularized\n";

	std::cout << "Maximum absolute error in covariance square root: " << arma::abs(P_CC - C_CC * C_CC.t()).max() << std::endl;

	std::cout << "Saving non-zero partition of shape covariance ...\n";

	// Analytical UQ
	std::vector<double> analytical_variances_slopes;

	std::cout << "Computing analytical uncertainties ... ";
	auto start = std::chrono::system_clock::now();

	pgm_uq.GetVarianceSlopes(analytical_variances_slopes,FACETS_TO_INVESTIGATE,HOLD_MASS_CONSTANT);

	auto end = std::chrono::system_clock::now();

	std::chrono::duration<double> elapsed_seconds = end-start;
	std::cout << "Done computing analytical uncertainties in " << elapsed_seconds.count() << " s\n";

	// Running a Monte Carlo to compare against
	std::vector<arma::vec> deviations;
	std::vector<double> period_errors;
	std::vector<double> densities;

	std::vector < std::vector<double> > all_slopes;

	std::cout << "Running MC ... ";

	start = std::chrono::system_clock::now();
	SBGATPolyhedronGravityModelUQ::RunMCUQSlopes(PATH_SHAPE,
		DENSITY,
		pgm_filter -> GetOmega() ,
		UNIT_IN_METERS,
		HOLD_MASS_CONSTANT,
		C_CC,
		PERIOD_SD,
		N_MONTE_CARLO,
		FACETS_TO_INVESTIGATE,
		OUTPUT_DIR,
		std::min(30,N_MONTE_CARLO),
		deviations,
		densities,
		period_errors,
		all_slopes);

	end = std::chrono::system_clock::now();

	elapsed_seconds = end-start;

	std::cout << "Done running MC in " << elapsed_seconds.count() << " s\n";

	std::vector<double> mc_variances_slopes(FACETS_TO_INVESTIGATE.size());

	std::cout << "Computing MC dispersions...\n";

	for (int e = 0; e < mc_variances_slopes.size(); ++e){
		arma::vec slopes_mc(N_MONTE_CARLO);
		for (int sample = 0; sample < N_MONTE_CARLO; ++sample){
			slopes_mc(sample) = all_slopes[sample][e];
		}
		mc_variances_slopes[e] = arma::var(slopes_mc);

		slopes_mc.save(OUTPUT_DIR + "/slope_distribution_facet_" + std::to_string(FACETS_TO_INVESTIGATE[e]) + ".txt",arma::raw_ascii);
	}

	std::cout << "\t After " << N_MONTE_CARLO << " MC outcomes:\n";

	for (int e = 0; e < FACETS_TO_INVESTIGATE.size(); ++e){
		std::cout << "\tAt facet " << FACETS_TO_INVESTIGATE[e] << "\n";
		std::cout << "\t\tSlope (rad): " << pgm_filter -> GetSlope(FACETS_TO_INVESTIGATE[e]) << std::endl;
		std::cout << "\t\tMC variance in slope: " << mc_variances_slopes[e] << std::endl;
		std::cout << "\t\tAnalytical variance in slope: " << analytical_variances_slopes[e] << std::endl;
		std::cout << "\t\tError (%): " << (mc_variances_slopes[e] - analytical_variances_slopes[e])/analytical_variances_slopes[e] * 100 << std::endl;

	}

	return 0;
}


/*
 * @file SURF_FlannMatcher
 * @brief SURF detector + descriptor + FLANN Matcher
 * @author A. Huaman
 */

#include <fstream>
#include <iostream>
#include <stdio.h>
#include <iomanip>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>

#include <Eigen/Dense>
#include <opengv/amm.hpp>
#include <opengv/relative_pose/methods.hpp>
#include <opengv/relative_pose/NoncentralRelativeAdapter.hpp>
#include <opengv/optimization_tools/objective_function_tools/GlobalPnPFunctionInfo.hpp>
#include <opengv/optimization_tools/objective_function_tools/SquaredFunctionNoIterationsInfo.hpp>
#include <opengv/optimization_tools/solver_tools/SolverToolsNoncentralRelativePose.hpp>

#include "opencv2/core.hpp"
#include "opencv2/features2d.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/xfeatures2d.hpp"

#include "random_generators.hpp"
#include "experiment_helpers.hpp"
#include "time_measurement.hpp"

using namespace std;
using namespace cv;
using namespace cv::xfeatures2d;
//void readme();
/*
 * @function main
 * @brief Main function
 */
void returnCorrespondences(const Mat & img1, const Mat & img2, std::vector<KeyPoint> & k1, std::vector<KeyPoint> & k2);

int main( int argc, char** argv )
{
  if( argc != 5 )
  {
    std::cout << "argc: " << argc << std::endl;
    //readme();
    return -1;
  }
  Mat img_left_1 = imread( argv[1], IMREAD_GRAYSCALE );
  Mat img_left_2 = imread( argv[2], IMREAD_GRAYSCALE );

  Mat img_right_1 = imread( argv[3], IMREAD_GRAYSCALE );
  Mat img_right_2 = imread( argv[4], IMREAD_GRAYSCALE );
  if( !img_left_1.data || !img_left_2.data || !img_right_1.data || !img_right_2.data)
  {
    std::cout<< " --(!) Error reading images " << std::endl;
    return -1;
  }
  std::vector<KeyPoint> k1_left;
  std::vector<KeyPoint> k2_left;
  returnCorrespondences(img_left_1, img_left_2, k1_left, k2_left);

  std::vector<KeyPoint> k1_right;
  std::vector<KeyPoint> k2_right;
  returnCorrespondences(img_right_1, img_right_2, k1_right, k2_right);

  /*std::cout << "Left: " << std::endl;
  for(int i=0; i < k1_left.size(); ++i){
    std::cout << "(" << k1_left[i].pt.x << ", " << k1_left[i].pt.y << ")  (" << k2_left[i].pt.x << ", " << k2_left[i].pt.y << ")" <<  std::endl;
  }

  std::cout << "Right: " << std::endl;
  for(int i=0; i < k1_right.size(); ++i){
    std::cout << "(" << k1_right[i].pt.x << ", " << k1_right[i].pt.y << ")  (" << k2_right[i].pt.x << ", " << k2_right[i].pt.y << ")" <<  std::endl;
    }*/

  std::cout << "Cal matrices: " << std::endl;
  Eigen::Matrix3d K_left = Eigen::Matrix3d::Identity(3,3);
  K_left(0,0) = 9.597910e+02; K_left(0,1) =  0.000000e+00; K_left(0,2) = 6.960217e+02;
  K_left(1,0) = 0.000000e+00; K_left(1,1) =  9.569251e+02; K_left(1,2) = 2.241806e+02;
  K_left(2,0) = 0.000000e+00; K_left(2,1) =  0.000000e+00; K_left(2,2) = 1.000000e+00;

  Eigen::Matrix3d K_right = Eigen::Matrix3d::Identity(3,3);
  K_right(0,0) = 9.037596e+02; K_right(0,1) =  0.000000e+00; K_right(0,2) = 6.957519e+02;
  K_right(1,0) = 0.000000e+00; K_right(1,1) =  9.019653e+02; K_right(1,2) = 2.242509e+02;
  K_right(2,0) = 0.000000e+00; K_right(2,1) =  0.000000e+00; K_right(2,2) = 1.000000e+00;

  //We have the calibration matrices and the vector points now it is necessary to build the bearing vectors and convert them in the reference frame of the centra

  //Pose for viewpoint left
  opengv::translation_t position_left = Eigen::MatrixXd::Zero(3,1);
  position_left(0,0) = -0.27;
  opengv::rotation_t rotation_left    = Eigen::MatrixXd::Identity(3,3);

  //Pose for viewpoint right
  opengv::translation_t position_right = Eigen::MatrixXd::Zero(3,1);
  position_right(0,0) = 0.27;
  opengv::rotation_t rotation_right    = Eigen::MatrixXd::Identity(3,3);

  //Create a fake central camera

  opengv::bearingVectors_t bearingVectors1;
  opengv::bearingVectors_t bearingVectors2;
  Eigen::MatrixXd aux = Eigen::MatrixXd::Zero(3,1);
  for(int j = 0; j < k1_left.size(); ++j){
    aux(0,0) = k1_left[j].pt.x; aux(1,0) = k1_left[j].pt.y; aux(2,0) = 1;
    aux = K_left.inverse() * aux;
    bearingVectors1.push_back(aux);
  }

  for(int j = 0; j < k1_right.size(); ++j){
    aux(0,0) = k1_right[j].pt.x; aux(1,0) = k1_right[j].pt.y; aux(2,0) = 1;
    aux = K_right.inverse() * aux;
    bearingVectors1.push_back(aux);
  }

  for(int j = 0; j < k2_left.size(); ++j){
    aux(0,0) = k2_left[j].pt.x; aux(1,0) = k2_left[j].pt.y; aux(2,0) = 1;
    aux = K_left.inverse() * aux;
    bearingVectors2.push_back(aux);
  }

  for(int j = 0; j < k2_right.size(); ++j){
    aux(0,0) = k2_right[j].pt.x; aux(1,0) = k2_right[j].pt.y; aux(2,0) = 1;
    aux = K_right.inverse() * aux;
    bearingVectors2.push_back(aux);
  }
  std::cout << "bearing vector1: " << bearingVectors1.size() << std::endl;
  std::cout << "bearing vector2: " << bearingVectors2.size() << std::endl;
  //To use the methods it will be necessary to build the central adapter
  /*opengv::bearingVectors_t v1;
  opengv::bearingVectors_t v2;
  int cols = (int)good_matches.size();
  for( int i = 0; i < cols; i++ )
    {
      std::cout << "Start cycle: " << i << std::endl;
      Eigen::MatrixXd v_left = Eigen::MatrixXd::Zero(3,1);
      v_left(0,0) = k1[i].pt.x;
      v_left(1,0) = k1[i].pt.y;
      v_left(2,0) = 1;
      //std::cout << "v1: " << std::endl << v1 << std::endl;

      Eigen::MatrixXd v_right = Eigen::MatrixXd::Zero(3,1);
      v_right(0,0) = k2[i].pt.x;
      v_right(1,0) = k2[i].pt.y;
      v_right(2,0) = 1;
      //std::cout << "v2: " << std::endl << v2 <<std::endl;

      v_left = K_left.inverse() * v_left;
      double norm_left = v_left.norm();
      v_left = v_left / norm_left;

      v_right = K_right.inverse() * v_right;
      double norm_right = v_right.norm();
      v_right = v_right /norm_right;

      v1.push_back(v_left);
      v2.push_back(v_right);
    }
  opengv::rotations_t camRotations;
  opengv::rotation_t rotation1 = Eigen::MatrixXd::Identity(3,3);
  opengv::rotation_t rotation2 = Eigen::MatrixXd::Identity(3,3);
  opengv::translation_t position1 = Eigen::MatrixXd::Zero(3,1);
  opengv::translation_t position2 = Eigen::MatrixXd::Zero(3,1);
  position2(0,0) = 0.54;
  camRotations.push_back(rotation1);
  camRotations.push_back(rotation2);

  opengv::translations_t camOffsets;
  camOffsets.push_back(position1);
  camOffsets.push_back(position2);
  std::vector<int> camCorrespondences1;
  std::vector<int> camCorrespondences2;
  int camCorrespondence = 0;
  for(int i = 0; i < good_matches.size(); ++i){
    camCorrespondences1.push_back(camCorrespondence);
    camCorrespondences2.push_back(camCorrespondence++);
  }

  //Extract the relative pose
  opengv::translation_t position; opengv::rotation_t rotation;
  opengv::extractRelativePose(position1, position2, rotation1, rotation2, position, rotation, false );

  //create non-central relative adapter
  opengv::relative_pose::NoncentralRelativeAdapter adapter(
						       v1,
						       v2,
						       camCorrespondences1,
						       camCorrespondences2,
						       camOffsets,
						       camRotations,
						       position,
						       rotation);
  return 0;*/
}

/*
 * @function readme
 */
/*void readme(){
  std::cout << " Usage: ./SURF_FlannMatcher <img1> <img2>" << std::endl;
  }*/

void returnCorrespondences(const Mat & img1, const Mat & img2, std::vector<KeyPoint> & k1, std::vector<KeyPoint> & k2){
  //-- Step 1: Detect the keypoints using SURF Detector, compute the descriptors
  int minHessian = 400;
  Ptr<SURF> detector = SURF::create();
  detector->setHessianThreshold(minHessian);

  std::vector<KeyPoint> keypoints_1, keypoints_2;
  Mat descriptors_1, descriptors_2;
  detector->detectAndCompute( img1, Mat(), keypoints_1, descriptors_1 );
  detector->detectAndCompute( img2, Mat(), keypoints_2, descriptors_2 );
  //-- Step 2: Matching descriptor vectors using FLANN matcher
  FlannBasedMatcher matcher;
  std::vector< DMatch > matches;
  matcher.match( descriptors_1, descriptors_2, matches );
  double max_dist = 0; double min_dist = 100;
  //-- Quick calculation of max and min distances between keypoints
  for( int i = 0; i < descriptors_1.rows; i++ )
  {
    double dist = matches[i].distance;
    if( dist < min_dist ) min_dist = dist;
    if( dist > max_dist ) max_dist = dist;
  }
  printf("-- Max dist : %f \n", max_dist );
  printf("-- Min dist : %f \n", min_dist );
  //-- Draw only "good" matches (i.e. whose distance is less than 2*min_dist,
  //-- or a small arbitary value ( 0.02 ) in the event that min_dist is very
  //-- small)
  //-- PS.- radiusMatch can also be used here.
  std::vector< DMatch > good_matches;
  for( int i = 0; i < descriptors_1.rows; i++ )
  {
    if( matches[i].distance <= max(2*min_dist, 0.02) ){
      good_matches.push_back( matches[i]);
    }
  }
  //-- Draw only "good" matches
  k1.clear();
  k2.clear();
  for( int i = 0; i < (int)good_matches.size(); i++ )
  {
    //printf( "-- Good Match [%d] Keypoint 1: %d  -- Keypoint 2: %d  \n", i, good_matches[i].queryIdx, good_matches[i].trainIdx );
    std::cout << ( "-- Good Match [%d] Keypoint 1: %d  -- Keypoint 2: %d  \n", i, good_matches[i].queryIdx, good_matches[i].trainIdx ) << std::endl;
    k1.push_back(keypoints_1[good_matches[i].queryIdx]);
    k2.push_back(keypoints_2[good_matches[i].trainIdx]);
  }

}


#ifndef COMMON_HPP
#define COMMON_HPP

#include <boost/multiprecision/cpp_bin_float.hpp>

namespace ccalc
{
    namespace mp = boost::multiprecision;
    using Float = mp::number<mp::cpp_bin_float<128>>;
} // namespace ccalc

#endif // !COMMON_HPP


#include <iostream>
#include <math.h>
#include <fstream>
#include <time.h>
#include <boost/range/numeric.hpp>
#include <boost/range/adaptor/map.hpp>
#include <unordered_map>
#include <algorithm>
#include <vector>
#include <iterator>
#define h  800
#define w  800
#define output_file "output.raw"

unsigned char* readImage(const char *image, int height, int width, int bpp)
{

	unsigned char* Imagedata = new unsigned char [height*width*bpp];

	FILE *file;
	if (!(file=fopen(image,"rb"))) {
		std::cout << "Cannot open file: " << image << std::endl;
		exit(1);
	}
	fread(Imagedata, sizeof(unsigned char), height*width*bpp, file);
	fclose(file);

	return Imagedata;
}

void saveImage(const char* image, unsigned char* Imagedata, int height, int width, int bpp)
{
	FILE *file;

	if (!(file=fopen(image,"wb"))) {
		std::cout << "Cannot open file: " << image << std::endl;
		exit(1);
	}
	fwrite(Imagedata, sizeof(unsigned char), height*width*bpp, file);
	fclose(file);

	std::cout << "Filed Saved Succesfully" << std::endl;
}

int getEuclideanDist(int pixel, int mean)
{
	return abs(pixel - mean);
}

int getMeanOfCluster(std::unordered_map<int, int> cluster)
{
	std::unordered_map<int, int>::iterator itr;
	int sum(0);

	// for(itr = cluster.begin(); itr != cluster.end(); itr++)
	// {
	// 	sum += itr->second;
	// 	// std::cout << sum << std::endl;
	// }

	sum = boost::accumulate(cluster | boost::adaptors::map_values, 0);

	return sum/cluster.size();
}

unsigned char* getFinalOutput(unsigned char* Image1DOutput, std::unordered_map<int, int> cluster,
	int height, int width, int mean)
{
	std::unordered_map<int, int>::iterator itr;

	for(itr = cluster.begin(); itr != cluster.end(); itr++)
	{
		Image1DOutput[itr->first] = mean;
	}

	return Image1DOutput;
}

unsigned char* getKMeansClustered(unsigned char* Image1D, int height, int width)
{
	int iters(30);
	int mean1(0), mean2(85), mean3(170), mean4(255), pixel;

	unsigned char* Image1DOutput = new unsigned char[height*width];
	std::unordered_map<int, int> cluster1, cluster2, cluster3, cluster4;
	std::vector<int> minDist(4, 0);

	while(iters>0)
	{
		cluster1.clear();
		cluster2.clear();
		cluster3.clear();
		cluster4.clear();
		for (int i = 0; i < height; ++i)
		{
			for (int j = 0; j < width; ++j)
			{
				pixel = Image1D[i*width + j];

				minDist[0] = (int(abs(pixel - mean1)));

				minDist[1] = (int(abs(pixel - mean2)));

				minDist[2] = (int(abs(pixel - mean3)));

				minDist[3] = (int(abs(pixel - mean4)));

				int min_pos = std::distance(minDist.begin(), std::min_element(minDist.begin(),minDist.end()));

				switch(min_pos)
				{
					case 0:
					cluster1[i*width + j] = pixel;
					break;

					case 1:
					cluster2[i*width + j] = pixel;
					break;

					case 2:
					cluster3[i*width + j] = pixel;
					break;

					case 3:
					cluster4[i*width + j] = pixel;
					break;
				}
			}
		}

		iters--;
		mean1 = getMeanOfCluster(cluster1);
		mean2 = getMeanOfCluster(cluster2);
		mean3 = getMeanOfCluster(cluster3);
		mean4 = getMeanOfCluster(cluster4);

		// std::cout << "Means are: " << mean1 << ", " << mean2 << ", " << mean3 << ", " << mean4 << ", " << "; Iter: " << iters << std::endl;
	}

	Image1DOutput = getFinalOutput(Image1DOutput, cluster1, h, w, mean1);
	Image1DOutput = getFinalOutput(Image1DOutput, cluster2, h, w, mean2);
	Image1DOutput = getFinalOutput(Image1DOutput, cluster3, h, w, mean3);
	Image1DOutput = getFinalOutput(Image1DOutput, cluster4, h, w, mean4);

	return Image1DOutput;

}

int main(int argc, char const *argv[])
{
	struct timespec start, stop;
	unsigned char* Image1D = readImage(argv[1], h, w, 1);
	std::cout << "Image read" << std::endl;

	if(clock_gettime(CLOCK_REALTIME, &start) == -1)
		perror("clock gettime");

	unsigned char* Image1DOutput = getKMeansClustered(Image1D, h, w);

	if(clock_gettime( CLOCK_REALTIME, &stop) == -1 )
		perror("clock gettime");

	double time = (stop.tv_sec - start.tv_sec)+ (double)(stop.tv_nsec - start.tv_nsec)/1e9;
	std::cout << "time taken: " << time << std::endl;

	saveImage(output_file, Image1DOutput, h, w, 1);
	delete [] Image1D;
	delete [] Image1DOutput;

	return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_FUNCTIONS_SCALAR_SINC_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_FUNCTIONS_SCALAR_SINC_HPP_INCLUDED
#include <nt2/trigonometric/functions/sinc.hpp>
#include <nt2/include/functions/scalar/sin.hpp>
#include <nt2/include/constants/one.hpp>
#include <boost/simd/sdk/config.hpp>

#if !defined(BOOST_SIMD_NO_INFINITIES)
#include <nt2/include/functions/scalar/is_inf.hpp>
#include <nt2/include/constants/zero.hpp>
#endif

#if !defined(BOOST_SIMD_NO_DENORMALS)
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/constants/eps.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( sinc_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      #if !defined(BOOST_SIMD_NO_INFINITIES)
      if(nt2::is_inf(a0)) return nt2::Zero<result_type>();
      #endif

      #if !defined(BOOST_SIMD_NO_DENORMALS)
      return (nt2::abs(a0) < nt2::Eps<result_type>()) ? nt2::One<result_type>()
                                                      : nt2::sin(a0)/a0;
      #else
      return a0 ? nt2::sin(a0)/a0 : nt2::One<result_type>();
      #endif
    }
  };
} }

#endif


/* ----------------------------------------------------------------------- *//**
 *
 * @file student.hpp
 *
 * @brief Evaluate the Student's t-distribution function.
 * @author Florian Schoppmann
 * @date   November 2010
 *
 *//* -------------------------------------------------------------------- *//**
 *
 * @file student.hpp
 *
 * Emprirical results indicate that the numerical quality of the series
 * expansion from [1] (see notes below) is vastly superior to using continued
 * fractions for computing the cdf via the incomplete beta function.
 *
 * @literature
 *
 * [1] Abramowitz and Stegun, Handbook of Mathematical Functions with Formulas,
 *     Graphs, and Mathematical Tables, 1972
 *     page 948: http://people.math.sfu.ca/~cbm/aands/page_948.htm
 *
 * Further reading (for computing the Student-T cdf via the incomplete beta
 * function):
 *
 * [2] NIST Digital Library of Mathematical Functions, Ch. 8,
 *     Incomplete Gamma and Related Functions,
 *     http://dlmf.nist.gov/8.17
 *
 * [3] Lentz, Generating Bessel functions in Mie scattering calculations using
 *     continued fractions, Applied Optics, Vol. 15, No. 3, 1976
 *
 * [4] Thompson and Barnett, Coulomb and Bessel Functions of Complex Arguments
 *     and Order, Journal of Computational Physics, Vol. 64, 1986
 *
 * [5] Cuyt et al., Handbook of Continued Fractions for Special Functions,
 *     Springer, 2008
 *
 * [6] Gil et al., Numerical Methods for Special Functions, SIAM, 2008
 *
 * [7] Press et al., Numerical Recipes in C++, 3rd edition,
 *     Cambridge Univ. Press, 2007
 *
 * [8] DiDonato, Morris, Jr., Algorithm 708: Significant Digit Computation of
 *     the Incomplete Beta Function Ratios, ACM Transactions on Mathematical
 *     Software, Vol. 18, No. 3, 1992
 *
 * Approximating the Student-T distribution function with the normal
 * distribution:
 *
 * [9]  Gleason, A note on a proposed student t approximation, Computational
 *      Statistics & Data Analysis, Vol. 34, No. 1, 2000
 *
 * [10] Gaver and Kafadar, A Retrievable Recipe for Inverse t, The American
 *      Statistician, Vol. 38, No. 4, 1984
 */

/**
 * @brief Student-t cumulative distribution function
 */
DECLARE_UDF(prob, students_t_cdf)
DECLARE_UDF(prob, students_t_pdf)
DECLARE_UDF(prob, students_t_quantile)

#ifndef MADLIB_MODULES_PROB_STUDENT_T_HPP
#define MADLIB_MODULES_PROB_STUDENT_T_HPP

#include <boost/math/distributions/detail/common_error_handling.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/students_t.hpp>

namespace madlib {

namespace modules {

namespace prob {

typedef boost::math::students_t_distribution<double, boost_mathkit_policy>
    students_t;

namespace {

/**
 * @brief Compute one-sided Student's t cumulative distribution function
 *
 * We use the series expansions 26.7.3 and 26.7.4 from [1] and
 * substitute sin(theta) = t/sqrt(n * z), where z = 1 + t^2/nu.
 *
 * This gives:
 * @verbatim
 *                          t
 *   A(t|1)  = 2 arctan( -------- ) ,
 *                       sqrt(nu)
 *
 *                                                    (nu-3)/2
 *             2   [            t              t         --    2 * 4 * ... * (2i)  ]
 *   A(t|nu) = - * [ arctan( -------- ) + ------------ * \  ---------------------- ]
 *             π   [         sqrt(nu)     sqrt(nu) * z   /_ 3 * ... * (2i+1) * z^i ]
 *                                                       i=0
 *           for odd nu > 1, and
 *
 *                         (nu-2)/2
 *                  t         -- 1 * 3 * ... * (2i - 1)
 *   A(t|nu) = ------------ * \  ------------------------ for even nu,
 *             sqrt(nu * z)   /_ 2 * 4 * ... * (2i) * z^i
 *                            i=0
 *
 * where A(t|nu) = Pr[|T| <= t].
 * @endverbatim
 *
 * @param t
 * @param nu Degree of freedom \f$ \nu > 0 \f$
 * @return \f$ \Pr[|T| < t] \f$ where \f$ t \geq 0 \f$, \f$ T \f$ is a Student's
 *     T-distributed random variable with \f$ \nu \f$ degrees of
 *     freedom.
 *
 * Note: The running time of calculating the series is proportional to nu.
 * We therefore use the normal distribution as an approximation for large nu.
 * Another idea for handling this case can be found in reference [8].
 */
template <class RealType>
inline
RealType
oneSidedStudentsT_CDF(const RealType& t,  uint64_t nu) {
    RealType    z,
                t_by_sqrt_nu;
    RealType    A, /* contains A(t|nu) */
                prod = 1.,
                sum = 1.;

    /* Handle main case (nu \in {1, ..., 200}) in the rest of the function. */
    z = 1. + t * t / static_cast<double>(nu);
    t_by_sqrt_nu = std::fabs(t) / std::sqrt(static_cast<double>(nu));

    if (nu == 1)
    {
        A = 2. / M_PI * std::atan(t_by_sqrt_nu);
    }
    else if (nu & 1) /* odd nu > 1 */
    {
        for (uint64_t j = 2; j + 3 <= nu; j += 2)
        {
            prod = prod * static_cast<double>(j)
                 / (static_cast<double>(j + 1) * z);
            sum = sum + prod;
        }
        A = 2 / M_PI * ( std::atan(t_by_sqrt_nu) + t_by_sqrt_nu / z * sum );
    }
    else /* even nu */
    {
        for (uint64_t j = 2; j + 2 <= nu; j += 2)
        {
            prod = prod * static_cast<double>(j - 1)
                 / (static_cast<double>(j) * z);
            sum = sum + prod;
        }
        A = t_by_sqrt_nu / std::sqrt(z) * sum;
    }

    /* A should obviously be within the interval [0,1] plus minus (hopefully
     * small) rounding errors. */
    if (A > 1.)
        A = 1.;
    else if (A < 0.)
        A = 0.;

    return A;
}

/**
 * @brief Compute parameter for normal CDF for approximating the Student's T CDF
 *
 * Gleason suggested a formula for approximating the Student's
 * t-distribution [9], which goes back to an approximation suggested in [10].
 *
 * Compared to the series expansion, this approximation satisfies
 * rel_error < 0.0001 || abs_error < 0.00000001
 * for all nu >= 200. (Tested on Mac OS X 10.6, gcc-4.2.)
 *
 * @param t
 * @param nu Degree of freedom \f$ \nu > 0 \f$
 * @returns A value \f$ z \f$ such that for a Student's t-distributed
 *     random variable \f$ T \f$ with \f$ nu \f$ degrees of freedom and a
 *     standard normally distributed random variable \f$ Z \f$, it holds that
 *     \f$ \Pr[T \leq t] \approx \Pr[Z \leq z] \f$.
 */
template <class RealType>
inline
RealType
GleasonsNormalApproxForStudentsT(const RealType& t, const RealType& nu) {
    double  g = (nu - 1.5) / ((nu - 1) * (nu - 1)),
            z = std::sqrt( std::log(1. + t * t / nu) / g );

    if (t < 0)
        z *= -1.;

    return z;
}

} // anonymous namespace

/**
 * @brief Compute Student's cumulative distribution function
 *
 * For nu >= 1000000, we just use the normal distribution as an approximation.
 * For 1000000 >= nu >= 200, we use a simple approximation from [9].
 * If nu is not within 0.01 of a natural number, we will call the student-t
 * CDF from boost. Otherwise, our approach should be much more precise than
 * using the incomplete beta function as boost does (see the references).
 *
 * We are much more cautious than usual here (it is folklore that the normal
 * distribution is a "good" estimate for Student-T if nu >= 30), but we can
 * afford the extra work as this function is not designed to be called from
 * inner loops. Performance should still be reasonably good, with at most ~100
 * iterations in any case (just one if nu >= 200).
 *
 * For nu < 200, we use the series expansions 26.7.3 and 26.7.4 from [1] and
 * substitute sin(theta) = t/sqrt(n * z), where z = 1 + t^2/nu (using
 * oneSidedStudentsT_CDF()).
 *
 * @param dist A Student's t-distribution object, containing the degree of
 *     freedom \f$ \nu \f$
 * @param t
 * @return \f$ \Pr[T < t] \f$ where \f$ t \geq 0 \f$, \f$ T \f$ is a Student's
 *     T-distributed random variable with \f$ \nu \f$ degrees of
 *     freedom.
 */
template <class RealType, class Policy>
inline
RealType
cdf(const boost::math::students_t_distribution<RealType, Policy>& dist,
    const RealType& t) {

    RealType df = dist.degrees_of_freedom();

    // FIXME: Add some justification/do some tests.
    if (!std::isfinite(df) || std::fabs(df - std::floor(df))/df > 0.01)
        return boost::math::cdf(dist, t);

    static const char* function = "madlib::modules::prob::cdf("
        "const students_t_distribution<%1%>&, %1%)";

    RealType result;
    if (!boost::math::detail::check_df(function, df, &result, Policy()))
        return result;

    if (df >= 200)
        return
            boost::math::cdf(
                boost::math::normal_distribution<RealType, Policy>(),
                df >= 1000000
                    ? t
                    : GleasonsNormalApproxForStudentsT(t, df)
            );

    // We first compute A = Pr[|T| < t]
    RealType A = oneSidedStudentsT_CDF(t, static_cast<uint64_t>(df));

    /* The Student-T distribution is obviously symmetric around t=0... */
    if (t < 0)
        /* FIXME: If A is approximately 1, we will face a loss of significance.
         *  */
        return .5 * (1. - A);
    else
        /* While we only know A in [0,1] here, the end result will be in
         * [0.5, 1]. Hence, there is no problem with adding 1 and A, even if
         * A << 1. */
        return .5 * (1. + A);
}

/**
 * @brief Compute the complement of Student's cumulative distribution function
 */
template <class RealType, class Policy>
inline
RealType
cdf(
    const boost::math::complemented2_type<
        boost::math::students_t_distribution<RealType, Policy>,
        RealType
    >& c
) {
    RealType df = c.dist.degrees_of_freedom();
    if (df >= 200) {
        static const char* function = "madlib::modules::prob::cdf("
            "const complement(students_t_distribution<%1%>&), %1%)";

        RealType result;
        if (!boost::math::detail::check_df(function, df, &result, Policy()))
            return result;

        return
            boost::math::cdf(complement(
                boost::math::normal_distribution<RealType, Policy>(),
                df >= 1000000
                    ? c.param
                    : GleasonsNormalApproxForStudentsT(c.param, df)
            ));
    }

    return prob::cdf(c.dist, -c.param);
}

template <class RealType, class Policy>
inline
RealType
pdf(const boost::math::students_t_distribution<RealType, Policy>& dist,
    const RealType& t) {
    return boost::math::pdf(dist, t);
}

template <class RealType, class Policy>
inline
RealType
pdf(
    const boost::math::complemented2_type<
        boost::math::students_t_distribution<RealType, Policy>,
        RealType
    >& c
) {
    return boost::math::pdf(c);
}

template <class RealType, class Policy>
inline
RealType
quantile(const boost::math::students_t_distribution<RealType, Policy>& dist,
    const RealType& p) {

    using namespace boost::math;

    static const char* function = "madlib::modules::prob::quantile("
        "const students_t_distribution<%1%>&, %1%)";

    // FIXME: Boost bug 6937 prevent proper argument validation.
    // https://svn.boost.org/trac/boost/ticket/6937
    // Until this is fixed upstream, we do the following checks here.
    RealType df = dist.degrees_of_freedom();
    RealType result;
    if (!detail::check_df(function, df, &result, Policy())
        || !detail::check_probability(function, p, &result, Policy()))
        return result;

    return boost::math::quantile(dist, p);
}

template <class RealType, class Policy>
inline
RealType
quantile(
    const boost::math::complemented2_type<
        boost::math::students_t_distribution<RealType, Policy>,
        RealType
    >& c
) {
    return boost::math::quantile(c);
}

} // namespace prob

} // namespace modules

} // namespace madlib

#endif // defined(MADLIB_MODULES_PROB_STUDENT_T_HPP)


/* test_bernoulli_distribution.cpp
 *
 * Copyright Steven Watanabe 2010
 * 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)
 *
 * $Id: test_bernoulli_distribution.cpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 */

#include <boost/random/bernoulli_distribution.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::bernoulli_distribution<>
#define BOOST_RANDOM_ARG1 p
#define BOOST_RANDOM_ARG1_DEFAULT 0.5
#define BOOST_RANDOM_ARG1_VALUE 0.25

#define BOOST_RANDOM_DIST0_MIN false
#define BOOST_RANDOM_DIST0_MAX true
#define BOOST_RANDOM_DIST1_MIN false
#define BOOST_RANDOM_DIST1_MAX true

#define BOOST_RANDOM_TEST1_PARAMS (0.0)
#define BOOST_RANDOM_TEST1_MIN false
#define BOOST_RANDOM_TEST1_MAX false

#define BOOST_RANDOM_TEST2_PARAMS (1.0)
#define BOOST_RANDOM_TEST2_MIN true
#define BOOST_RANDOM_TEST2_MAX true

#include "test_distribution.ipp"


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

#include "../../test/sparse_solver.h"
#include <Eigen/IterativeSolvers>

template<typename T> void test_minres_T()
{
  // Identity preconditioner
  MINRES<SparseMatrix<T>, Lower, IdentityPreconditioner    > minres_colmajor_lower_I;
  MINRES<SparseMatrix<T>, Upper, IdentityPreconditioner    > minres_colmajor_upper_I;

  // Diagonal preconditioner
  MINRES<SparseMatrix<T>, Lower, DiagonalPreconditioner<T> > minres_colmajor_lower_diag;
  MINRES<SparseMatrix<T>, Upper, DiagonalPreconditioner<T> > minres_colmajor_upper_diag;
  MINRES<SparseMatrix<T>, Lower|Upper, DiagonalPreconditioner<T> > minres_colmajor_uplo_diag;

  // call tests for SPD matrix
  CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_lower_I) );
  CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_upper_I) );

  CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_lower_diag)  );
  CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_upper_diag)  );
  CALL_SUBTEST( check_sparse_spd_solving(minres_colmajor_uplo_diag)  );

  // TO DO: symmetric semi-definite matrix
  // TO DO: symmetric indefinite matrix

}

void test_minres()
{
  CALL_SUBTEST_1(test_minres_T<double>());
//  CALL_SUBTEST_2(test_minres_T<std::compex<double> >());

}


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

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

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Constant  \f$\frac{\sqrt2}{\pi}\f$.

    @par Semantic:

    For type T:

    @code
    T r = Sqrt_2opi<T>();
    @endcode

    is similar to:

    @code
    T r = sqrt(Two<T>())/Pi<T>();
    @endcode

    @return a value of type T

**/
  template<typename T> T Sqrt_2opi();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Constant  \f$\frac{\sqrt2}{\pi}\f$.

      Generate the  constant sqrt_2opi.

      @return The Sqrt_2opi constant for the proper type
    **/
    const boost::dispatch::functor<tag::sqrt_2opi_> sqrt_2opi = {};
  }
} }
#endif

#include <boost/simd/constant/definition/sqrt_2opi.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


//  (C) Copyright Gennadiy Rozental 2011-2015.
//  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)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE example
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

BOOST_AUTO_TEST_CASE( test )
{
  double v1 = 1.23456e28;
  double v2 = 1.23457e28;

  BOOST_REQUIRE_CLOSE( v1, v2, 0.001 );
  // Absolute value of difference between these two values is 1e+23.
  // But we are interested only that it does not exeed 0.001% of a values compared
  // And this test will pass.
}
//]


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

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

#include <boost/compute/lambda.hpp>
#include <boost/compute/algorithm/transform_if.hpp>
#include <boost/compute/container/vector.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace compute = boost::compute;

BOOST_AUTO_TEST_CASE(transform_if_odd)
{
    using boost::compute::abs;
    using boost::compute::lambda::_1;

    int data[] = { -2, -3, -4, -5, -6, -7, -8, -9 };
    compute::vector<int> vector(data, data + 8, queue);

    compute::vector<int>::iterator end = compute::transform_if(
        vector.begin(), vector.end(), vector.begin(), abs<int>(), _1 % 2 != 0, queue
    );
    BOOST_CHECK_EQUAL(std::distance(vector.begin(), end), 4);

    CHECK_RANGE_EQUAL(int, 4, vector, (+3, +5, +7, +9));
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright 2019 Hans Dembinski
//
// 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)

//[ guide_axis_circular

#include <boost/histogram/axis.hpp>
#include <limits>

int main() {
  using namespace boost::histogram;

  // make a circular regular axis ... [0, 180), [180, 360), [0, 180) ....
  using opts = decltype(axis::option::overflow | axis::option::circular);
  auto r = axis::regular<double, use_default, use_default, opts>{2, 0., 360.};
  assert(r.index(-180) == 1);
  assert(r.index(0) == 0);
  assert(r.index(180) == 1);
  assert(r.index(360) == 0);
  assert(r.index(540) == 1);
  assert(r.index(720) == 0);
  // special values are mapped to the overflow bin index
  assert(r.index(std::numeric_limits<double>::infinity()) == 2);
  assert(r.index(-std::numeric_limits<double>::infinity()) == 2);
  assert(r.index(std::numeric_limits<double>::quiet_NaN()) == 2);

  // since the regular axis is the most common circular axis, there exists an alias
  auto c = axis::circular<>{2, 0., 360.};
  assert(r == c);

  // make a circular integer axis
  auto i = axis::integer<int, use_default, axis::option::circular_t>{1, 4};
  assert(i.index(0) == 2);
  assert(i.index(1) == 0);
  assert(i.index(2) == 1);
  assert(i.index(3) == 2);
  assert(i.index(4) == 0);
  assert(i.index(5) == 1);
}

//]


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[rings
/*`
Shows how to access the exterior ring (one)
and interior rings (zero or more) of a polygon.
Also shows the related ring_type and interior_type.
*/

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> point;
    typedef boost::geometry::model::polygon<point> polygon_type;

    polygon_type poly;

    typedef boost::geometry::ring_type<polygon_type>::type ring_type;
    ring_type& ring = boost::geometry::exterior_ring(poly);

    // For a ring of model::polygon, you can call "push_back".
    // (internally, it is done using a traits::push_back class)
    ring.push_back(point(0, 0));
    ring.push_back(point(0, 5));
    ring.push_back(point(5, 4));
    ring.push_back(point(0, 0));

    ring_type inner;
    inner.push_back(point(1, 1));
    inner.push_back(point(2, 1));
    inner.push_back(point(2, 2));
    inner.push_back(point(1, 1));

    typedef boost::geometry::interior_type<polygon_type>::type int_type;
    int_type& interiors = boost::geometry::interior_rings(poly);
    interiors.push_back(inner);

    std::cout << boost::geometry::dsv(poly) << std::endl;

    // So int_type defines a collection of rings,
    // which is a Boost.Range compatible range
    // The type of an element of the collection is the very same ring type again.
    // We show that.
    typedef boost::range_value<int_type>::type int_ring_type;

    std::cout
        << std::boolalpha
        << boost::is_same<ring_type, int_ring_type>::value
        << std::endl;

    return 0;
}

//]

//[rings_output
/*`
Output:
[pre
(((0, 0), (0, 5), (5, 4), (0, 0)), ((1, 1), (2, 1), (2, 2), (1, 1)))
true
]
*/
//]


#include <deque>
#include <vector>
#include <list>
#include <iostream>
#include <boost/graph/vector_as_graph.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/range.hpp>
#include <boost/range/adaptor/indexed.hpp>

int main(){
    using namespace boost;

    const char * tasks[] = {
        "pick up kids from school",
        "buy groceries (and snack)",
        "get cash at ATM",
        "drop off kids at soccer practice",
        "cook dinner",
        "pick up kids from soccer",
        "eat dinner"
    };

    const int n_tasks = sizeof(tasks) /sizeof(char*);

    std::vector<std::list<int>> g(n_tasks);
    g[0].push_back(3);
    g[1].push_back(3);
    g[1].push_back(4);
    g[2].push_back(1);
    g[3].push_back(5);
    g[4].push_back(6);
    g[5].push_back(6);

    std::deque<int> topo_order;

    topological_sort(g, std::front_inserter(topo_order),
            vertex_index_map(identity_property_map()));

    for(const auto& [index, i] : topo_order
            | boost::adaptors::indexed())
    {
        std::cout << tasks[i.head] << std::endl;
    }
}


//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/elliptic/include/functions/scalar/ellipke.hpp>
#include <nt2/elliptic/include/functions/simd/ellipke.hpp>

#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/functions/sin.hpp>
#include <nt2/include/functions/cos.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/fusion/include/vector_tie.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL ( ellipke_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::ellipke;
  using nt2::tag::ellipke_;
  T a[] = {nt2::One<T>(), nt2::Zero<T>(), nt2::Half<T>(), nt2::Two<T>()};
  size_t N =  sizeof(a)/sizeof(T);
  T e1[] = {nt2::Inf<T>(), nt2::Pio_2<T>(), T(1.854074677301372), nt2::Nan<T>()};
  T e2[] = {nt2::One<T>(), nt2::Pio_2<T>(), T(1.350643881047675), nt2::Nan<T>()};

  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<ellipke_(T)>::type)
                  , (std::pair<T,T>)
                  );
  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<ellipke_(T, T)>::type)
                  , (std::pair<T,T>)
                  );
  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<ellipke_(T, T, T&)>::type)
                  , (T)
                  );
  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<ellipke_(T, T, T&, T&)>::type)
                  , (void)
                  );

 {
    T s, c;
    for(size_t i=0; i < N; ++i)
    {
      ellipke(a[i],nt2::Eps<T>(), s, c);
      NT2_TEST_ULP_EQUAL(s, e1[i], 1);
      NT2_TEST_ULP_EQUAL(c, e2[i], 1);
    }
  }

  {
    T s, c;
    for(size_t i=0; i < N; ++i)
    {
      s = ellipke(a[i], nt2::Eps<T>(), c);
      NT2_TEST_ULP_EQUAL(s, e1[i], 1);
      NT2_TEST_ULP_EQUAL(c, e2[i], 1);
    }
  }

  {
    T s, c;
    for(size_t i=0; i < N; ++i)
    {
      boost::fusion::vector_tie(s, c) = ellipke(a[i]);
      NT2_TEST_ULP_EQUAL(s, e1[i], 1);
      NT2_TEST_ULP_EQUAL(c, e2[i], 1);
    }
  }

  {
    for(size_t i=0; i < N; ++i)
    {
      std::pair<T,T> p = ellipke(a[i]);
      NT2_TEST_ULP_EQUAL(p.first,  e1[i], 1);
      NT2_TEST_ULP_EQUAL(p.second, e2[i], 1);
    }
  }
}


//==============================================================================
//         Copyright 2015 J.T. Lapreste
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/exponential/include/functions/significants.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( significants,  NT2_SIMD_REAL_TYPES)
{
  using nt2::significants;
  using nt2::tag::significants_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename nt2::meta::as_integer<vT>::type ivT;

  typedef typename nt2::meta::call<significants_(vT, ivT)>::type r_t;
  typedef vT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(significants(nt2::Inf<vT>(), 1), nt2::Inf<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(significants(nt2::Minf<vT>(), 1), nt2::Minf<r_t>(), 0.5);
  NT2_TEST_ULP_EQUAL(significants(nt2::Nan<vT>(), 1), nt2::Nan<r_t>(), 0.5);
#endif
  NT2_TEST_ULP_EQUAL(significants(nt2::splat<vT>(25.34), nt2::splat<ivT>(1)), nt2::splat<vT>(30), 0.5);
  NT2_TEST_ULP_EQUAL(significants(nt2::splat<vT>(25.34), nt2::splat<ivT>(2)), nt2::splat<vT>(25), 0.5);
  NT2_TEST_ULP_EQUAL(significants(nt2::splat<vT>(25.34), nt2::splat<ivT>(3)), nt2::splat<vT>(25.3), 0.5);
  NT2_TEST_ULP_EQUAL(significants(nt2::splat<vT>(25.34), nt2::splat<ivT>(4)), nt2::splat<vT>(25.34), 0.5);
  NT2_TEST_ULP_EQUAL(significants(nt2::splat<vT>(-25.34), nt2::splat<ivT>(1)), nt2::splat<vT>(-30), 0.5);
  NT2_TEST_ULP_EQUAL(significants(nt2::splat<vT>(-25.34), nt2::splat<ivT>(2)), nt2::splat<vT>(-25), 0.5);
  NT2_TEST_ULP_EQUAL(significants(nt2::splat<vT>(-25.34), nt2::splat<ivT>(3)), nt2::splat<vT>(-25.3), 0.5);
  NT2_TEST_ULP_EQUAL(significants(nt2::splat<vT>(-25.34), nt2::splat<ivT>(4)), nt2::splat<vT>(-25.34), 0.5);
}


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/45/problem45.hpp"

BOOST_AUTO_TEST_SUITE( Problem45 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem45::solve(2);
        BOOST_CHECK_EQUAL(res, 40755);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem45::solve();
        BOOST_CHECK_EQUAL(res, 1533776805);
    }

BOOST_AUTO_TEST_SUITE_END()


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_AREA_HUILLER_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_AREA_HUILLER_HPP

#include <boost/geometry/strategies/spherical/distance_haversine.hpp>

#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace area
{

/*!
\brief Area calculation by spherical excess / Huiller's formula
\ingroup strategies
\tparam PointOfSegment point type of segments of rings/polygons
\tparam CalculationType \tparam_calculation
\author Barend Gehrels. Adapted from:
- http://www.soe.ucsc.edu/~pang/160/f98/Gems/GemsIV/sph_poly.c
- http://williams.best.vwh.net/avform.htm
\note The version in Gems didn't account for polygons crossing the 180 meridian.
\note This version works for convex and non-convex polygons, for 180 meridian
crossing polygons and for polygons with holes. However, some cases (especially
180 meridian cases) must still be checked.
\note The version which sums angles, which is often seen, doesn't handle non-convex
polygons correctly.
\note The version which sums longitudes, see
http://trs-new.jpl.nasa.gov/dspace/bitstream/2014/40409/1/07-03.pdf, is simple
and works well in most cases but not in 180 meridian crossing cases. This probably
could be solved.

\note This version is made for spherical equatorial coordinate systems

\qbk{

[heading Example]
[area_with_strategy]
[area_with_strategy_output]

[heading See also]
[link geometry.reference.algorithms.area.area_2_with_strategy area (with strategy)]
}

*/
template
<
    typename PointOfSegment,
    typename CalculationType = void
>
class huiller
{
typedef typename boost::mpl::if_c
    <
        boost::is_void<CalculationType>::type::value,
        typename select_most_precise
            <
                typename coordinate_type<PointOfSegment>::type,
                double
            >::type,
        CalculationType
    >::type calculation_type;

protected :
    struct excess_sum
    {
        calculation_type sum;

        // Distances are calculated on unit sphere here
        strategy::distance::haversine<PointOfSegment, PointOfSegment>
                distance_over_unit_sphere;

        inline excess_sum()
            : sum(0)
            , distance_over_unit_sphere(1)
        {}
        inline calculation_type area(calculation_type radius) const
        {
            return - sum * radius * radius;
        }
    };

public :
    typedef calculation_type return_type;
    typedef PointOfSegment segment_point_type;
    typedef excess_sum state_type;

    inline huiller(calculation_type radius = 1.0)
        : m_radius(radius)
    {}

    inline void apply(PointOfSegment const& p1,
                PointOfSegment const& p2,
                excess_sum& state) const
    {
        if (! geometry::math::equals(get<0>(p1), get<0>(p2)))
        {
            calculation_type const half = 0.5;
            calculation_type const two = 2.0;
            calculation_type const four = 4.0;
            calculation_type const two_pi = two * geometry::math::pi<calculation_type>();
            calculation_type const half_pi = half * geometry::math::pi<calculation_type>();

            // Distance p1 p2
            calculation_type a = state.distance_over_unit_sphere.apply(p1, p2);

            // Sides on unit sphere to south pole
            calculation_type b = half_pi - geometry::get_as_radian<1>(p2);
            calculation_type c = half_pi - geometry::get_as_radian<1>(p1);

            // Semi parameter
            calculation_type s = half * (a + b + c);

            // E: spherical excess, using l'Huiller's formula
            // [tg(e / 4)]2   =   tg[s / 2]  tg[(s-a) / 2]  tg[(s-b) / 2]  tg[(s-c) / 2]
            calculation_type E = four * atan(sqrt(geometry::math::abs(tan(s / two)
                    * tan((s - a) / two)
                    * tan((s - b) / two)
                    * tan((s - c) / two))));

            E = geometry::math::abs(E);

            // In right direction: positive, add area. In left direction: negative, subtract area.
            // Longitude comparisons are not so obvious. If one is negative, other is positive,
            // we have to take the dateline into account.
            // TODO: check this / enhance this, should be more robust. See also the "grow" for ll
            // TODO: use minmax or "smaller"/"compare" strategy for this
            calculation_type lon1 = geometry::get_as_radian<0>(p1) < 0
                ? geometry::get_as_radian<0>(p1) + two_pi
                : geometry::get_as_radian<0>(p1);

            calculation_type lon2 = geometry::get_as_radian<0>(p2) < 0
                ? geometry::get_as_radian<0>(p2) + two_pi
                : geometry::get_as_radian<0>(p2);

            if (lon2 < lon1)
            {
                E = -E;
            }

            state.sum += E;
        }
    }

    inline return_type result(excess_sum const& state) const
    {
        return state.area(m_radius);
    }

private :
    /// Radius of the sphere
    calculation_type m_radius;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <typename Point>
struct default_strategy<spherical_equatorial_tag, Point>
{
    typedef strategy::area::huiller<Point> type;
};

// Note: spherical polar coordinate system requires "get_as_radian_equatorial"
/***template <typename Point>
struct default_strategy<spherical_polar_tag, Point>
{
    typedef strategy::area::huiller<Point> type;
};***/

} // namespace services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::area

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_AREA_HUILLER_HPP


/**
 * @file maximum_coverage_test.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-02-17
 */

#include "test_utils/logger.hpp"
#include "test_utils/test_result_check.hpp"

#include "paal/utils/functors.hpp"
#include "paal/greedy/set_cover/maximum_coverage.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/range/irange.hpp>

#include <vector>
#include <iterator>
#include <cmath>

BOOST_AUTO_TEST_CASE(MaximumCoverage) {
    LOGLN("MaximumCoverage");
    const int OPTIMAL = 14;
    const int NUMBER_OF_SETS = 2;
    const double APPROXIMATION_RATIO = 1 - 1 / std::exp(1.0);
    std::vector<std::vector<int>> sets_element = { { 1, 2 },
                                                   { 3, 4, 5, 6 },
                                                   { 7, 8, 9, 10, 11, 12, 13,
                                                     0 },
                                                   { 1, 3, 5, 7, 9, 11, 13 },
                                                   { 2, 4, 6, 8, 10, 12, 0 } };
    auto sets = boost::irange(0, 5);
    std::vector<int> result;
    auto element_index = paal::utils::identity_functor{};
    auto cost = paal::greedy::maximum_coverage(
        sets, paal::utils::make_array_to_functor(sets_element),
        std::back_inserter(result), element_index, NUMBER_OF_SETS);
    check_result(cost, OPTIMAL, APPROXIMATION_RATIO,
                 paal::utils::greater_equal());
}


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

    @copyright 2016 NumScale SAS

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

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

  /*!

    @ingroup group-arithmetic
    This function object computes the negated (fused) multiply add of
    its three parameters.

    @par Header <boost/simd/function/fnma.hpp>

    @par Notes
    The call `fnma(x, y, z)` is similar to `-x*y-z`

    But really conformant fused multiply/add also implies

    - only one rounding

    - no "intermediate" overflow

    fnma provides this for all integral types (however, using it on unsigned types
    is not recommanded for obvious reasons)  and also each time it is reasonable
    in terms of performance for floating ones (i.e. if the system has the hard
    wired capability).

    If you need pedantic fnma capabilities in all circumstances in your own
    code you can use the pedantic_ decorator (can be very expensive).

    @par Decorators
    - pedantic_ ensures the fnma properties and allows SIMD acceleration if available.

    @see fms, fma, fnms

    @par Example:

      @snippet fnma.cpp fnma

    @par Possible output:

      @snippet fnma.txt fnma

  **/
  Value fnma(Value const& x, Value const& y, Value const& z);
} }
#endif

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

#endif


///////////////////////////////////////////////////////////////////////////////////////
// survival::modelss::exponential::scalar::meta::is_survival_model.hpp         		 //
//                                                                                //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                        //
//  Software License, Version 1.0. (See accompanying file                            //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)                 //
///////////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_EXPONENTIAL_SCALAR_META_IS_SURVIVAL_MODEL_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_MODELS_EXPONENTIAL_SCALAR_META_IS_SURVIVAL_MODEL_HPP_ER_2009
#include <boost/mpl/bool.hpp>
#include <boost/statistics/detail/distribution/survival/models/common/meta/is_survival_model.hpp>
#include <boost/statistics/detail/distribution/survival/models/exponential/scalar/model.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace distribution{
namespace survival {
namespace meta{

    template<typename T,typename L>
    struct is_survival_model< exponential_model<T,L> >
        : boost::mpl::bool_<true>{};

}// meta
}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


/**
 * @file
 * @license BSD 3-clause
 * @copyright Copyright (c) 2020, New York University and Max Planck
 * Gesellschaft
 *
 * @brief Implements a qp allocating forces to track a desired centroidal wrench.
 *
 */

#pragma once

#include <Eigen/Dense>
#include <eiquadprog/eiquadprog-fast.hpp>

namespace blmc_controllers
{
typedef Eigen::Array<double, 6, 1> Array6d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

/**
 * @brief Impedance controller between any two frames of the robot.
 */
class CentroidalForceQPController
{
public:
    /**
     * @brief Construct a new ImpedanceController object.
     */
    CentroidalForceQPController();

    /**
     * @brief Initialize the internal data. None real-time safe method.
     *
     * @param number_endeffectors Maximum number of endeffectors in the problem.
     * @param friction_coeff Floor friction coefficient to use.
     * @param qp_penalty_lin The penalty weight for the linear wcom violation.
     * @param qp_penalty_ang The penalty weight for the angular wcom violation.
     */
    void initialize(int number_endeffectors, double friction_coeff,
                    double qp_penalty_lin, double qp_penalty_ang);

    /**
     * @brief Computes the centroidal wrench using a PD controller.
     *
     * @param w_com The desired centroidal wrench to track.
     * @param relative_position_endeff The relative position of the endeffectors
     *     with respect to the center of mass.
     */
    void run(Eigen::Ref<const Vector6d> w_com,
             Eigen::Ref<const Eigen::VectorXd> relative_position_endeff,
             Eigen::Ref<const Eigen::VectorXd> cnt_array);

    /**
     * @brief Get the computed desired forces
     *
     * @return Eigen::VectorXd&
     */
    Eigen::VectorXd& get_forces();

private:  // attributes
    /** @brief Output forces */
    Eigen::VectorXd forces_;
    Eigen::VectorXd sol_;

    Eigen::MatrixXd hess_;
    Eigen::MatrixXd ce_;
    Eigen::MatrixXd ce_new_;
    Eigen::MatrixXd ci_;

    Eigen::VectorXd g0_;
    Eigen::VectorXd ci0_;

    int nb_eff_;

    double mu_;

    double qp_penalty_lin_;
    double qp_penalty_ang_;

    eiquadprog::solvers::EiquadprogFast qp_;

};

}  // namespace blmc_controllers


/* ----------------------------------------------------------------------------
 * Copyright 2021, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#ifndef SOLVER_IPOPT_HPP
#define SOLVER_IPOPT_HPP

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

#include <iomanip>  //set precision
#include <iostream>

#include "panther_types.hpp"
#include "utils.hpp"
#include <casadi/casadi.hpp>
#include "timer.hpp"
#include "separator.hpp"
#include "octopus_search.hpp"

// For the yaw search:
#include <boost/graph/astar_search.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
#include <boost/random.hpp>

typedef PANTHER_timers::Timer MyTimer;

class SolverIpopt
{
public:
  SolverIpopt(mt::parameters &par, std::shared_ptr<mt::log> log_ptr);

  ~SolverIpopt();

  bool optimize();

  // setters
  void setMaxRuntimeKappaAndMu(double runtime, double kappa, double mu);
  bool setInitStateFinalStateInitTFinalT(mt::state initial_state, mt::state final_state, double t_init,
                                         double &t_final);
  void setHulls(ConvexHullsOfCurves_Std &hulls);
  void setSimpsonFeatureSamples(const std::vector<Eigen::Vector3d> &samples,
                                const std::vector<Eigen::Vector3d> &w_velsampleswrtworld);
  void setFocusOnObstacle(bool focus_on_obstacle)
  {
    focus_on_obstacle_ = focus_on_obstacle;
  }
  mt::trajectory traj_solution_;

  // getters
  void getPlanes(std::vector<Hyperplane3D> &planes);
  int getNumOfLPsRun();
  int getNumOfQCQPsRun();
  void getSolution(mt::PieceWisePol &solution);
  double getTimeNeeded();

  int B_SPLINE = 1;  // B-Spline Basis
  int MINVO = 2;     // Minimum volume basis
  int BEZIER = 3;    // Bezier basis

  bool checkGradientsUsingFiniteDiff();

  mt::parameters par_;

protected:
private:
  // https://stackoverflow.com/a/11498248/6057617
  double wrapFromMPitoPi(double x)
  {
    x = fmod(x + M_PI, 2 * M_PI);
    if (x < 0)
      x += 2 * M_PI;
    return x - M_PI;
  }

  bool getIntersectionWithPlane(const Eigen::Vector3d &P1, const Eigen::Vector3d &P2, const Eigen::Vector4d &coeff,
                                Eigen::Vector3d &intersection);

  void addObjective();
  void addConstraints();

  void saturateQ(std::vector<Eigen::Vector3d> &q);

  // transform functions (with Eigen)
  void transformPosBSpline2otherBasis(const Eigen::Matrix<double, 3, 4> &Qbs, Eigen::Matrix<double, 3, 4> &Qmv,
                                      int interval);
  void transformVelBSpline2otherBasis(const Eigen::Matrix<double, 3, 3> &Qbs, Eigen::Matrix<double, 3, 3> &Qmv,
                                      int interval);

  void generateRandomGuess();
  bool generateAStarGuess();
  void generateStraightLineGuess();

  void printStd(const std::vector<Eigen::Vector3d> &v);
  void printStd(const std::vector<double> &v);
  void generateGuessNDFromQ(const std::vector<Eigen::Vector3d> &q, std::vector<Eigen::Vector3d> &n,
                            std::vector<double> &d);

  void fillPlanesFromNDQ(const std::vector<Eigen::Vector3d> &n, const std::vector<double> &d,
                         const std::vector<Eigen::Vector3d> &q);

  void generateRandomD(std::vector<double> &d);
  void generateRandomN(std::vector<Eigen::Vector3d> &n);
  void generateRandomQ(std::vector<Eigen::Vector3d> &q);

  void printQVA(const std::vector<Eigen::Vector3d> &q);

  void printQND(std::vector<Eigen::Vector3d> &q, std::vector<Eigen::Vector3d> &n, std::vector<double> &d);

  void findCentroidHull(const Polyhedron_Std &hull, Eigen::Vector3d &centroid);

  casadi::DM generateYawGuess(casadi::DM matrix_qp_guess, casadi::DM all_w_fe, double y0, double ydot0, double ydotf,
                              double t0, double tf);

  std::vector<Eigen::Vector3d> n_;  // Each n_[i] has 3 elements (nx,ny,nz)
  std::vector<double> d_;           // d_[i] has 1 element

  mt::PieceWisePol pwp_solution_;

  int basis_ = B_SPLINE;

  int p_ = 5;
  int M_;
  int N_;

  int Ny_;

  double index_instruction_;  // hack

  int num_of_normals_;

  int num_of_obst_;
  int num_of_segments_;

  std::vector<Hyperplane3D> planes_;

  Eigen::RowVectorXd knots_;
  double t_init_;
  double t_final_;
  double deltaT_;

  mt::state initial_state_;
  mt::state final_state_;

  Eigen::Vector3d q0_, q1_, q2_, qNm2_, qNm1_, qN_;

  ConvexHullsOfCurves_Std hulls_;

  MyTimer opt_timer_;

  double max_runtime_ = 2;  //[seconds]

  // Guesses
  std::vector<Eigen::Vector3d> n_guess_;   // Guesses for the normals
  std::vector<double> d_guess_;            // Guesses for the d of the planes
  std::vector<Eigen::Vector3d> qp_guess_;  // Guesses for the position control points
  std::vector<double> qy_guess_;           // Guesses for the yaw control points

  double kappa_ = 0.2;  // kappa_*max_runtime_ is spent on the initial guess
  double mu_ = 0.5;     // mu_*max_runtime_ is spent on the optimization

  int num_of_QCQPs_run_ = 0;

  // transformation between the B-spline control points and other basis
  std::vector<Eigen::Matrix<double, 4, 4>> M_pos_bs2basis_;
  std::vector<Eigen::Matrix<double, 3, 3>> M_vel_bs2basis_;
  std::vector<Eigen::Matrix<double, 4, 4>> A_pos_bs_;

  // double a_star_bias_ = 1.0;

  std::unique_ptr<separator::Separator> separator_solver_ptr_;
  std::unique_ptr<OctopusSearch> octopusSolver_ptr_;

  casadi::Function cf_op_;
  // casadi::Function cf_op_force_final_pos_;
  casadi::Function cf_fixed_pos_op_;
  casadi::Function cf_fit_yaw_;
  casadi::Function cf_visibility_;

  casadi::DM all_w_fe_;
  casadi::DM all_w_velfewrtworld_;
  casadi::DM b_Tmatrixcasadi_c_;

  // auxiliary types
  // struct location
  // {
  //   float y, x;  // lat, long
  // };

  struct data
  {
    float yaw;
    size_t layer;
    size_t circle;

    void print()
    {
      std::cout << "yaw= " << yaw << ", layer= " << layer << std::endl;
    }
  };

  typedef float cost_graph;

  ///////////////////////////////// Things for the yaw search
  // specify some types
  // typedef adjacency_list<listS, vecS, undirectedS, no_property, property<edge_weight_t, cost_graph>> mygraph_t;
  typedef boost::adjacency_list<boost::listS, boost::vecS, boost::directedS, data,
                                boost::property<boost::edge_weight_t, cost_graph>>
      mygraph_t;
  typedef boost::property_map<mygraph_t, boost::edge_weight_t>::type WeightMap;
  typedef mygraph_t::vertex_descriptor vd;
  typedef mygraph_t::edge_descriptor edge_descriptor;
  // typedef std::pair<int, int> edge;

  mygraph_t mygraph_;

  double num_of_yaw_per_layer_;  // = par_.num_of_yaw_per_layer;
  double num_of_layers_;         // = par_.num_samples_simpson;
  // WeightMap weightmap_;
  std::vector<std::vector<vd>> all_vertexes_;
  casadi::DM vector_yaw_samples_;

  std::shared_ptr<mt::log> log_ptr_;

  casadi::DM eigen2casadi(const Eigen::Vector3d &a);

  bool focus_on_obstacle_;

  // std::unique_ptr<mygraph_t> mygraph_ptr;
  //////////////////////////////////

  // PImpl idiom
  // https://www.geeksforgeeks.org/pimpl-idiom-in-c-with-examples/
  // struct PImpl;
  // std::unique_ptr<PImpl> m_casadi_ptr_;  // Opaque pointer

  // double Ra_ = 1e10;
};

// struct SolverIpopt::PImpl  // TODO: Not use PImpl
// {
//   casadi::Function casadi_function_;
//   casadi::DM all_w_fe_;
// };

#endif


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

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integer_list.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTANT_ASSERT(to<List>(integer_list<int, 0, 1, 2>) == list(int_<0>, int_<1>, int_<2>));
    BOOST_HANA_CONSTANT_ASSERT(head(integer_list<int, 0, 1, 2>) == int_<0>);
    //! [main]
}


// ---------------------------------------------------------------------------
//
//  This file is part of PermLib.
//
// Copyright (c) 2009-2011 Thomas Rehn <thomas@carmen76.de>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. The name of the author may not be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
// OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
// IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
// NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// ---------------------------------------------------------------------------

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

#include <iostream>
#include <fstream>

#include <permlib/permutation.h>
#include <permlib/permutationword.h>
#include <permlib/bsgs.h>

using namespace permlib;

template<class PERM>
void testPermutation() {
	const unsigned int n = 10;
	PERM a(n, "1 2 5,3 6 7 8, 9 4 10");
	PERM b(n, "1 2, 5 3 6, 7 8 9 4 10");

	BOOST_CHECK_EQUAL(a / 1, 4);
	BOOST_CHECK_EQUAL(a % 4, 1);
	BOOST_CHECK_EQUAL((~a) / 4, 1);

	BOOST_CHECK_EQUAL(b / 1, 0);
	BOOST_CHECK_EQUAL(b % 0, 1);
	BOOST_CHECK_EQUAL((~b) / 0, 1);

	for (unsigned int i = 0; i < n; ++i) {
		BOOST_CHECK_EQUAL( a / (~a / i), i);
		BOOST_CHECK_EQUAL( a % (~a % i), i);
	}

	PERM c = a * b;
	PERM d(a);
	d *= b;

	BOOST_CHECK(c == d);

	d.invertInplace();

	for (unsigned int i = 0; i < n; ++i) {
		BOOST_CHECK_EQUAL( c / (d / i), i);
		BOOST_CHECK_EQUAL( c % (d % i), i);
	}

	c *= d;

	BOOST_CHECK(c.isIdentity());

	PERM a2(a);
	PERM e(a * a);
	a2 *= a2;
	BOOST_CHECK( a2 == e );
}

BOOST_AUTO_TEST_CASE( test_permutation )
{
    testPermutation<Permutation>();
}

BOOST_AUTO_TEST_CASE( test_permutationword )
{
    testPermutation<PermutationWord>();
}

void testPermutationOrder(const Permutation& p) {
	Permutation q(p);

	for (boost::uint64_t i = 0; i < p.order() - 1; ++i) {
		BOOST_CHECK( ! q.isIdentity() );
		q *= p;
	}
	BOOST_CHECK( q.isIdentity() );
}

BOOST_AUTO_TEST_CASE( test_permutation_order ) {
	const unsigned int n = 10;
	Permutation a(n, "1 2 5,3 6 7 8, 9 4 10");
	Permutation b(n, "1 2, 5 3 6, 7 8 4 10");
	Permutation c(n, "1 2, 5 3 6, 7 8 9 4 10");
	Permutation d(n, "1 2 3, 5 6 7, 8 9 10");

	BOOST_CHECK_EQUAL( a.order(), 12 );
	testPermutationOrder(a);

	BOOST_CHECK_EQUAL( b.order(), 12 );
	testPermutationOrder(b);

	BOOST_CHECK_EQUAL( c.order(), 30 );
	testPermutationOrder(c);

	BOOST_CHECK_EQUAL( d.order(), 3 );
	testPermutationOrder(d);
}

BOOST_AUTO_TEST_CASE( test_permutation_cycles )
{
	const unsigned int n = 10;
	Permutation a(n, "1 2 5,3 6 7 8, 9 4 10");
	Permutation b(n, "1 2, 5 3 6, 7 8 4 10");

	typedef std::pair<dom_int, unsigned int> CyclePair;
	CyclePair c;

	std::list<CyclePair> cyclesA = a.cycles();
	BOOST_REQUIRE_EQUAL(cyclesA.size(), 3);

	c = cyclesA.front();
	cyclesA.pop_front();
	BOOST_CHECK_EQUAL(c.first, 0);
	BOOST_CHECK_EQUAL(c.second, 3);

	c = cyclesA.front();
	cyclesA.pop_front();
	BOOST_CHECK_EQUAL(c.first, 2);
	BOOST_CHECK_EQUAL(c.second, 4);

	c = cyclesA.front();
	cyclesA.pop_front();
	BOOST_CHECK_EQUAL(c.first, 3);
	BOOST_CHECK_EQUAL(c.second, 3);

	// include trivial cycles
	std::list<CyclePair> cyclesB = b.cycles(true);
	BOOST_REQUIRE_EQUAL(cyclesB.size(), 4);

	c = cyclesB.front();
	cyclesB.pop_front();
	BOOST_CHECK_EQUAL(c.first, 0);
	BOOST_CHECK_EQUAL(c.second, 2);

	c = cyclesB.front();
	cyclesB.pop_front();
	BOOST_CHECK_EQUAL(c.first, 2);
	BOOST_CHECK_EQUAL(c.second, 3);

	c = cyclesB.front();
	cyclesB.pop_front();
	BOOST_CHECK_EQUAL(c.first, 3);
	BOOST_CHECK_EQUAL(c.second, 4);

	c = cyclesB.front();
	cyclesB.pop_front();
	BOOST_CHECK_EQUAL(c.first, 8);
	BOOST_CHECK_EQUAL(c.second, 1);

	cyclesB = b.cycles(false);
	BOOST_REQUIRE_EQUAL(cyclesB.size(), 3);

	c = cyclesB.front();
	cyclesB.pop_front();
	BOOST_CHECK_EQUAL(c.first, 0);
	BOOST_CHECK_EQUAL(c.second, 2);

	c = cyclesB.front();
	cyclesB.pop_front();
	BOOST_CHECK_EQUAL(c.first, 2);
	BOOST_CHECK_EQUAL(c.second, 3);

	c = cyclesB.front();
	cyclesB.pop_front();
	BOOST_CHECK_EQUAL(c.first, 3);
	BOOST_CHECK_EQUAL(c.second, 4);
}

BOOST_AUTO_TEST_CASE( test_permutation_projection )
{
	std::string line1("1 2 5 9,3 6 7 8, 4 10");
	const unsigned int n = 10;
	Permutation a(n, line1);

	const dom_int projDomainSize = 4;
	const dom_int projDomain[projDomainSize] = { 2, 5, 6, 7 };
	Permutation* p = a.project(projDomainSize, projDomain, projDomain+projDomainSize);

	BOOST_ASSERT( p );

	BOOST_CHECK( ! p->isIdentity() );

	for (unsigned int i = 0; i < projDomainSize; ++i)
		BOOST_CHECK_EQUAL( *p / i, (i + 1) % projDomainSize );

	delete p;
}

BOOST_AUTO_TEST_CASE( test_exotic_permutations )
{
  Permutation* p = new Permutation(1, "");
  BOOST_ASSERT( p );
  BOOST_CHECK_EQUAL( p->size(), 1 );
  delete p;

  p = new Permutation(0, "");
  BOOST_ASSERT( p );
  BOOST_CHECK_EQUAL( p->size(), 0 );
  delete p;
}

BOOST_AUTO_TEST_CASE( test_identity )
{
	std::string line1("1 2 5 9,3 6 7 8, 4 10");
	const unsigned int n = 10;
	Permutation a(n, line1);

	BOOST_CHECK( ! a.isIdentity() );

	const dom_int lineArray[] = { 1, 4, 5, 9, 8, 6, 7, 2, 0, 3 };
	Permutation b(lineArray, lineArray + n);
	BOOST_CHECK( ! b.isIdentity() );

	Permutation c(a);
	c = ~a * b;
	BOOST_CHECK( c.isIdentity() );
}


//
// Created by Hamza El-Kebir on 4/17/21.
//

#ifndef LODESTAR_ZEROORDERHOLD_HPP
#define LODESTAR_ZEROORDERHOLD_HPP

#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
#include "Lodestar/systems/StateSpace.hpp"
#include "Lodestar/aux/CompileTimeQualifiers.hpp"

namespace ls {
    namespace analysis {
        /**
         * @brief Routines for computing zero-order hold transformation on state
         * space systems.
         *
         * @sa theory.ldstr.dev/discretization
         */
        class ZeroOrderHold {
        public:
            template<typename TScalar = double, int TStateDim = Eigen::Dynamic, int TInputDim = Eigen::Dynamic, int TOutputDim = Eigen::Dynamic>
            struct mallocStruct {
                template<int TTStateDim = TStateDim, int TTInputDim = TInputDim, int TTOutputDim = TOutputDim, typename ::std::enable_if<
                        (TTStateDim >= 0) && (TTInputDim >= 0) && (TTOutputDim >= 0)>::type * = nullptr>
                mallocStruct() : upperXM(decltype(upperXM)::Zero()), lowerXM(decltype(lowerXM)::Zero()),
                                 XM(decltype(XM)::Zero()), XXM(
                                decltype(XXM)::Zero())
                {}

                template<int TTStateDim = TStateDim, int TTInputDim = TInputDim, int TTOutputDim = TOutputDim, typename ::std::enable_if<
                        (TTStateDim < 0) && (TTInputDim < 0) && (TTOutputDim < 0)>::type * = nullptr>
                mallocStruct()
                {}

                Eigen::Matrix<TScalar, TStateDim, LS_STATIC_UNLESS_DYNAMIC(TStateDim + TInputDim)> upperXM;
                Eigen::Matrix<TScalar, TInputDim, LS_STATIC_UNLESS_DYNAMIC(TStateDim + TInputDim)> lowerXM;
                Eigen::Matrix<TScalar, LS_STATIC_UNLESS_DYNAMIC(TStateDim + TInputDim), LS_STATIC_UNLESS_DYNAMIC(
                        TStateDim + TInputDim)> XM;
                Eigen::Matrix<TScalar, TStateDim, LS_STATIC_UNLESS_DYNAMIC(TStateDim + TInputDim)> XXM;
            };

            /**
             * @brief Generates zero-order hold discretization from a
             * continuous-time state space system.
             *
             * @param A State matrix.
             * @param B Input matrix.
             * @param C Output matrix.
             * @param D Feedforward matrix.
             * @param dt Sampling period.
             *
             * @return Zero-order hold discrete-time state space system.
             */
            static systems::StateSpace<>
            c2d(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
                const Eigen::MatrixXd &C, const Eigen::MatrixXd &D, double dt);

            /**
             * @brief Generates zero-order hold discretization from a
             * continuous-time state space system.
             *
             * @param A State matrix.
             * @param B Input matrix.
             * @param C Output matrix.
             * @param D Feedforward matrix.
             * @param dt Sampling period.
             *
             * @return Zero-order hold discrete-time state space system.
             */
            static systems::StateSpace<>
            c2d(Eigen::MatrixXd *A, Eigen::MatrixXd *B, Eigen::MatrixXd *C, Eigen::MatrixXd *D, double dt);

            template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
            static void c2d(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt,
                            systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                            mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                            LS_IS_DYNAMIC_DEFAULT(TStateDim, TInputDim, TOutputDim));

            template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
            static void c2d(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt,
                            systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                            mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                            LS_IS_STATIC_DEFAULT(TStateDim, TInputDim, TOutputDim));

            /**
             * @brief Generates zero-order hold discretization from a
             * continuous-time state space system.
             *
             * @param ss State space system.
             * @param dt Sampling period.
             *
             * @return Zero-order hold discrete-time state space system.
             */
            static systems::StateSpace<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>
            c2d(const systems::StateSpace<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic> &ss, double dt);

            /**
             * @brief Generates zero-order hold discretization from a
             * continuous-time state space system.
             *
             * @param ss Pointer to state space system.
             * @param dt Sampling period.
             *
             * @return Zero-order hold discrete-time state space system.
             */
            static systems::StateSpace<> c2d(const systems::StateSpace<> *ss, double dt);

            /**
             * @brief Reverts a zero-order hold discretization on a
             * discrete-time state space system.
             *
             * @param A State matrix.
             * @param B Input matrix.
             * @param C Output matrix.
             * @param D Feedforward matrix.
             * @param dt Sampling period.
             *
             * @return Zero-order hold continuous-time state space system.
             */
            static systems::StateSpace<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>
            d2c(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
                const Eigen::MatrixXd &C, const Eigen::MatrixXd &D, double dt);

            /**
             * @brief Reverts a zero-order hold discretization on a
             * discrete-time state space system.
             *
             * @param A State matrix.
             * @param B Input matrix.
             * @param C Output matrix.
             * @param D Feedforward matrix.
             * @param dt Sampling period.
             *
             * @return Zero-order hold continuous-time state space system.
             */
            static systems::StateSpace<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>
            d2c(Eigen::MatrixXd *A, Eigen::MatrixXd *B,
                Eigen::MatrixXd *C, Eigen::MatrixXd *D, double dt);

            /**
             * @brief Reverts a zero-order hold discretization on a
             * discrete-time state space system.
             *
             * @param ss State space system.
             * @param dt Sampling period.
             *
             * @return Continuous-time state space system.
             */
            static systems::StateSpace<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic>
            d2c(const systems::StateSpace<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::Dynamic> &ss, double dt);

            /**
             * @brief Reverts a zero-order hold discretization on a
             * discrete-time state space system.
             *
             * @param ss State space system.
             * @param dt Sampling period.
             *
             * @return Continuous-time state space system.
             */
            static systems::StateSpace<>
            d2c(const systems::StateSpace<> *ss, double dt);

            /**
             * @brief Reverts a zero-order hold discretization on a
             * discrete-time state space system.
             *
             * This method retrieves the sampling period from the state space
             * object.
             *
             * @param ss State space system.
             * @param dt Sampling period.
             *
             * @return Continuous-time state space system.
             */
            static systems::StateSpace<>
            d2c(const systems::StateSpace<> *ss);

            /**
             * @brief Reverts a zero-order hold discretization on a
             * discrete-time state space system.
             *
             * This method retrieves the sampling period from the state space
             * object.
             *
             * @param ss State space system.
             * @param dt Sampling period.
             *
             * @return Continuous-time state space system.
             */
            static systems::StateSpace<>
            d2c(const systems::StateSpace<> &ss);

            /**
             * @brief Reverts a zero-order hold discretization on a
             * discrete-time state space system.
             *
             * @param ss State space system.
             * @param dt Sampling period.
             *
             * @return Continuous-time state space system.
             */
            template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
            static void d2c(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt,
                            systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                            mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                            LS_IS_DYNAMIC_DEFAULT(TStateDim, TInputDim, TOutputDim));

            /**
             * @brief Reverts a zero-order hold discretization on a
             * discrete-time state space system.
             *
             * @param ss State space system.
             * @param dt Sampling period.
             *
             * @return Continuous-time state space system.
             */
            template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
            static void d2c(const systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss, double dt,
                            systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                            mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                            LS_IS_STATIC_DEFAULT(TStateDim, TInputDim, TOutputDim));
        };
    }
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::analysis::ZeroOrderHold::c2d(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
                                      double dt,
                                      ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                                      ls::analysis::ZeroOrderHold::mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                                      LS_IS_DYNAMIC(TStateDim, TInputDim, TOutputDim))
{
    dt = abs(dt);

    const long n = ss->stateDim();
    const long m = ss->inputDim();

    memStruct->upperXM.block(0, 0, n, n) << (ss->getA());
    memStruct->upperXM.block(0, n, n, m) << (ss->getB());

    memStruct->lowerXM.setZero();

    memStruct->XM.block(0, 0, n, n + m) << memStruct->upperXM;
    memStruct->XM.block(n, 0, m, n + m) << memStruct->lowerXM;

    memStruct->XXM = (memStruct->XM * dt).exp().block(0, 0, n, n + m);

    out->setA(memStruct->XXM.block(0, 0, n, n));
    out->setB(memStruct->XXM.block(0, n, n, m));
    out->setC(Eigen::MatrixXd::Identity(n, n));
    out->setD(Eigen::MatrixXd::Zero(n, m));
    out->setDiscreteParams(dt, true);

}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::analysis::ZeroOrderHold::c2d(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
                                      double dt,
                                      ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                                      ls::analysis::ZeroOrderHold::mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                                      LS_IS_STATIC(TStateDim, TInputDim, TOutputDim))
{
    dt = abs(dt);

    memStruct->upperXM.template block<TStateDim, TStateDim>(0, 0);
    memStruct->upperXM.template block<TStateDim, TStateDim>(0, 0) << (ss->getA());
    memStruct->upperXM.template block<TStateDim, TInputDim>(0, TStateDim) << (ss->getB());

    memStruct->lowerXM.setZero();

    memStruct->XM.template block<TStateDim, TStateDim + TInputDim>(0, 0) << memStruct->upperXM;
    memStruct->XM.template block<TInputDim, TStateDim + TInputDim>(TStateDim, 0) << memStruct->lowerXM;

    memStruct->XXM = (memStruct->XM * dt).exp().template block<TStateDim, TStateDim + TInputDim>(0, 0);

    out->setA(memStruct->XXM.template block<TStateDim, TStateDim>(0, 0));
    out->setB(memStruct->XXM.template block<TStateDim, TInputDim>(0, TStateDim));
    out->setDiscreteParams(dt, true);
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::analysis::ZeroOrderHold::d2c(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
                                      double dt,
                                      ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                                      ls::analysis::ZeroOrderHold::mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                                      LS_IS_DYNAMIC(TStateDim, TInputDim, TOutputDim))
{
    dt = abs(dt);

    const long n = ss->stateDim();
    const long m = ss->inputDim();

    memStruct->upperXM.block(0, 0, n, n) << (ss->getA());
    memStruct->upperXM.block(0, n, n, m) << (ss->getB());

    memStruct->lowerXM.setZero();
    memStruct->lowerXM.block(0, n, m, m).setIdentity();

    memStruct->XM.block(0, 0, n, n + m) << memStruct->upperXM;
    memStruct->XM.block(n, 0, m, n + m) << memStruct->lowerXM;

    memStruct->XXM = (memStruct->XM).log().block(0, 0, n, n + m) / dt;

    out->setA(memStruct->XXM.block(0, 0, n, n));
    out->setB(memStruct->XXM.block(0, n, n, m));
}

template<typename TScalar, int TStateDim, int TInputDim, int TOutputDim>
void ls::analysis::ZeroOrderHold::d2c(const ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *ss,
                                      double dt,
                                      ls::systems::StateSpace<TScalar, TStateDim, TInputDim, TOutputDim> *out,
                                      ls::analysis::ZeroOrderHold::mallocStruct<TScalar, TStateDim, TInputDim, TOutputDim> *memStruct,
                                      LS_IS_STATIC(TStateDim, TInputDim, TOutputDim))
{
    dt = abs(dt);

    memStruct->upperXM.template block<TStateDim, TStateDim>(0, 0) << (ss->getA());
    memStruct->upperXM.template block<TStateDim, TInputDim>(0, TStateDim) << (ss->getB());

    memStruct->lowerXM.setZero();
    memStruct->lowerXM.template block<TInputDim, TInputDim>(0, TStateDim)
            << Eigen::Matrix<TScalar, TInputDim, TInputDim>::Identity();

    memStruct->XM.template block<TStateDim, TStateDim + TInputDim>(0, 0) << memStruct->upperXM;
    memStruct->XM.template block<TInputDim, TStateDim + TInputDim>(TStateDim, 0) << memStruct->lowerXM;

    memStruct->XXM = (memStruct->XM).log().template block<TStateDim, TStateDim + TInputDim>(0, 0) / dt;

    out->setA(memStruct->XXM.template block<TStateDim, TStateDim>(0, 0));
    out->setB(memStruct->XXM.template block<TStateDim, TInputDim>(0, TStateDim));
}

#endif //LODESTAR_ZEROORDERHOLD_HPP