/* KMeansRexCore.cpp
A fast, easy-to-read implementation of the K-Means clustering algorithm.
  allowing customized initialization (random samples or plus plus)
  and vectorized execution via the Eigen matrix template library.

Intended to be compiled as a shared library libkmeansrex.so
 which can then be utilized from high-level interactive environments,
  such as Matlab or Python.

Contains:
  Utility Fcns:
    discrete_rand : sampling discrete r.v.
    select_without_replacement : sample discrete w/out replacement

  Cluster Location Mu Initialization:
    sampleRowsRandom : sample at random (w/out replacement)
    sampleRowsPlusPlus : sample via K-Means++ (Arthur et al)
              see http://en.wikipedia.org/wiki/K-means%2B%2B

  K-Means Algorithm (aka Lloyd's Algorithm)
    run_lloyd : executes lloyd for spec'd number of iterations

  External "C" function interfaces (for calling from Python)
    NOTE: These take only pointers to float arrays, not Eigen array types

    RunKMeans          : compute cluster centers and assignments via lloyd
    SampleRowsPlusPlus : get just a plusplus initialization

Dependencies:
  mersenneTwister2002.c : random number generator

Author: Mike Hughes (www.michaelchughes.com)
Date:   2 April 2015
*/
/*
extern "C" {
  void RunKMeans(double *X_IN,  int N,  int D, int K, int Niter, int seed, char* initname, double *Mu_OUT, double *Z_OUT);
  void SampleRowsPlusPlus(double *X_IN,  int N,  int D, int K, int seed, double *Mu_OUT);
}
*/
#include <iostream>
#include "mersenneTwister2002.c"
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

/*  DEFINE Custom Type Names to make code more readable
    ExtMat :  2-dim matrix/array externally defined (e.g. in Matlab or Python)
*/
typedef Map<ArrayXXd> ExtMat;
typedef ArrayXXd Mat;
typedef ArrayXd Vec;

// ====================================================== Utility Functions
void set_seed( int seed ) {
  init_genrand( seed );
}

int discrete_rand( Vec &p ) {
    double total = p.sum();
    int K = (int) p.size();

    double r = total*genrand_double();
    double cursum = p(0);
    int newk = 0;
    while ( r >= cursum && newk < K-1) {
        newk++;
        cursum += p[newk];
    }
    if ( newk < 0 || newk >= K ) {
        cerr << "Badness. Chose illegal discrete value." << endl;
        return -1;
    }
    return newk;
}

void select_without_replacement( int N, int K, Vec &chosenIDs) {
    Vec p = Vec::Ones(N);
    for (int kk =0; kk<K; kk++) {
      int choice;
      int doKeep = false;
      while ( doKeep==false) {

        doKeep=true;
        choice = discrete_rand( p );

        for (int previd=0; previd<kk; previd++) {
          if (chosenIDs[previd] == choice ) {
            doKeep = false;
            break;
          }
        }
      }
      chosenIDs[kk] = choice;
    }
}

// ======================================================= Init Cluster Locs Mu

void sampleRowsRandom( ExtMat &X, ExtMat &Mu ) {
    int N = X.rows();
    int K = Mu.rows();
    Vec ChosenIDs = Vec::Zero(K);
    select_without_replacement( N, K, ChosenIDs );
		for (int kk=0; kk<K; kk++) {
		  Mu.row( kk ) = X.row( ChosenIDs[kk] );
		}
}

void sampleRowsPlusPlus( ExtMat &X, ExtMat &Mu ) {
    int N = X.rows();
    int K = Mu.rows();
    Vec ChosenIDs = Vec::Ones(K);
    int choice = discrete_rand( ChosenIDs );
    Mu.row(0) = X.row( choice );
    ChosenIDs[0] = choice;
    Vec minDist(N);
    Vec curDist(N);
    for (int kk=1; kk<K; kk++) {
      curDist = ( X.rowwise() - Mu.row(kk-1) ).square().rowwise().sum().sqrt();
      if (kk==1) {
        minDist = curDist;
      } else {
        minDist = curDist.min( minDist );
      }
      choice = discrete_rand( minDist );
      ChosenIDs[kk] = choice;
      Mu.row(kk) = X.row( choice );
    }
}

void init_Mu( ExtMat &X, ExtMat &Mu, const char* initname ) {
	  if ( string( initname ) == "random" ) {
    		sampleRowsRandom( X, Mu );
	  } else if ( string( initname ) == "plusplus" ) {
  			sampleRowsPlusPlus( X, Mu );
	  }
}

// ======================================================= Update Cluster Assignments Z
void pairwise_distance( ExtMat &X, ExtMat &Mu, Mat &Dist ) {

  int N = X.rows();
  int D = X.cols();
  int K = Mu.rows();

  // For small dims D, for loop is noticeably faster than fully vectorized.
  // Odd but true.  So we do fastest thing
  if ( D <= 16 )
  {
    for (int kk=0; kk<K; kk++) {
      Dist.col(kk) = ( X.rowwise() - Mu.row(kk) ).square().rowwise().sum();
    }
  }
  else
  {
	Dist.matrix() = -2*( X.matrix() * Mu.transpose().matrix() );
    Dist.rowwise() += Mu.square().rowwise().sum().transpose().row(0);
  }
}

double assignClosest( ExtMat &X, ExtMat &Mu, ExtMat &Z, Mat &Dist) {
  double totalDist = 0;
  int minRowID;

  pairwise_distance( X, Mu, Dist );

  for (int nn=0; nn<X.rows(); nn++) {
    totalDist += Dist.row(nn).minCoeff( &minRowID );
    Z(nn,0) = minRowID;
  }
  return totalDist;
}

// ======================================================= Update Cluster Locations Mu
void calc_Mu( ExtMat &X, ExtMat &Mu, ExtMat &Z) {
  Mu = Mat::Zero( Mu.rows(), Mu.cols() );
  Vec NperCluster = Vec::Zero( Mu.rows() );

  for (int nn=0; nn<X.rows(); nn++) {
    Mu.row( (int) Z(nn,0) ) += X.row( nn );
    NperCluster[ (int) Z(nn,0)] += 1;
  }
  Mu.colwise() /= NperCluster;
}

// ======================================================= Overall Lloyd Algorithm
void run_lloyd( ExtMat &X, ExtMat &Mu, ExtMat &Z, int Niter )  {
  double prevDist=INT_MAX,totalDist = 0;

  Mat Dist = Mat::Zero( X.rows(), Mu.rows() );

  for (int iter=0; iter<Niter; iter++) {

    totalDist = assignClosest( X, Mu, Z, Dist );
    calc_Mu( X, Mu, Z );
    if ( prevDist == totalDist ) {
      break;
    }
    prevDist = totalDist;
  }
}

// =================================================================================
// =================================================================================
// ===========================  EXTERNALLY CALLABLE FUNCTIONS ======================
// =================================================================================
// =================================================================================

void RunKMeans(double *X_IN,  int N,  int D, int K, int Niter,
               int seed, const char* initname, double *Mu_OUT, double *Z_OUT) {
  set_seed( seed );

  ExtMat X  ( X_IN, N, D);
  ExtMat Mu ( Mu_OUT, K, D);
  ExtMat Z  ( Z_OUT, N, 1);

  init_Mu( X, Mu, initname);
  run_lloyd( X, Mu, Z, Niter );
}

void SampleRowsPlusPlus(double *X_IN,  int N,  int D, int K, int seed, double *Mu_OUT) {
  set_seed( seed );

  ExtMat X  ( X_IN, N, D);
  ExtMat Mu ( Mu_OUT, K, D);

  sampleRowsPlusPlus( X, Mu);
}


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

// Copyright Paul A. Bristow 2015 - 2016.
// Copyright Christopher Kormanyos 2015 - 2016.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// This file also includes Doxygen-style documentation about the function of the code.
// See http://www.doxygen.org for details.

//! \file

//! \brief Example program showing use of math constants.

// Below are snippets of code that are included into Quickbook file fixed_point.qbk.

#include <cmath>
#include <limits>
#include <iomanip>
#include <iostream>

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
// http://www.boost.org/doc/libs/release/libs/multiprecision/doc/html/boost_multiprecision/tut/floats/cpp_bin_float.html

int main()
{
//[bin_float_50_pi
  // Construct a 50 decimal digit multiprecision floating-point version of pi for reference.
  using boost::multiprecision::cpp_bin_float_50;  // 50 decimal digits precision.

  std::cout << std::setprecision(std::numeric_limits<cpp_bin_float_50>::digits10)
    << std::fixed
    << boost::math::constants::pi<cpp_bin_float_50>()
    // 3.14159265358979323846264338327950288419716939937510
    << std::endl;
//] //[/bin_float_50_pi]

  // Use a rather precice fixed_point type that uses a 64-bit integer as its underlying representation.
//[fixed_point__constant
  typedef boost::fixed_point::negatable<3, -60> fixed_point_type;

  std::cout << std::setprecision(std::numeric_limits<fixed_point_type>::digits10)
    << std::fixed
    << boost::math::constants::pi<fixed_point_type>()
    // 3.141592653589793238
    << std::endl;
//] [/fixed_point__constant]

  // Use a small (and so very imprecise) fixed_point type that will fit into a single byte.
//[fixed_point_imprecise_constant
  typedef boost::fixed_point::negatable<2, -5> tiny_fixed_point_type;

  std::cout << std::setprecision(std::numeric_limits<tiny_fixed_point_type>::digits10)
    << std::fixed
    << boost::math::constants::pi<tiny_fixed_point_type>()
    // 3.1
    << std::endl;
//] [/fixed_point_imprecise_constant]

  // Use a precise fixed_point type that will fit into a 128-bit 16 byte item.
//[fixed_point_precise_constant

  typedef boost::fixed_point::negatable<2, -125> precise_fixed_point_type;

  std::cout << std::setprecision(std::numeric_limits<precise_fixed_point_type>::digits10)
    << std::fixed
    << boost::math::constants::pi<precise_fixed_point_type>()
    // 3.1415926535897932384626433832795028842
    << std::endl;
//] [/fixed_point_precise_constant]

} // int main()


#include <armadillo>
#include <gnuplot-iostream.h>

using namespace arma;

int main()
{
    vec x = linspace(0, 2 * M_PI, 50);
    vec y = sin(x);
    mat puntos = join_horiz(x, y);

    Gnuplot gp;

    // Ejemplo básico
    gp << "plot " << gp.file1d(y) << "with lines" << endl;

    cout << "Presionar Enter para continuar\n";
    getchar();

    // "plot" de Matlab
    gp << "set title '\"Plot\" de Matlab' font ',13'\n"
       << "set xlabel 'Tiempo (segundos)'\n"
       << "set ylabel 'Magnitud'\n"
       << "set xrange [0 : 2*pi]\n"
       << "set grid\n"
       << "unset key\n" // Ocultar la leyenda
       << "plot " << gp.file1d(puntos) << "with lines" << endl;

    cout << "Presionar Enter para continuar\n";
    getchar();

    // "stem" de Matlab
    gp << "set title '\"Stem\" de Matlab' font ',13'\n"
       << "plot " << gp.file1d(puntos) << "with impulses, "
       << gp.file1d(puntos) << "with points pt 7 lt 1" << endl;

    cout << "Presionar Enter para continuar\n";
    getchar();

    // "subplot" de Matlab
    gp << "set terminal qt size 600,650\n"
       << "set multiplot layout 2, 1 title '\"Subplot\" de Matlab' font ',14'\n"
       << "set title '\"Plot\" de Matlab' font ',13'\n"
       << "plot " << gp.file1d(puntos) << "with lines\n"
       << "set title '\"Stem\" de Matlab' font ',13'\n"
       << "plot " << gp.file1d(puntos) << "with impulses, "
       << gp.file1d(puntos) << "with points pt 7 lt 1\n"
       << "unset multiplot" << endl;

    cout << "Presionar Enter para continuar\n";
    getchar();

    return 0;
}


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2010

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

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

#include <iostream>
using std::cout; using std::endl;

//[policy_ref_snip9

#include <boost/math/special_functions/gamma.hpp>
using boost::math::tgamma;
using boost::math::policies::policy;
using boost::math::policies::digits10;

typedef policy<digits10<5> > my_pol_5; // Define a new, non-default, policy
// to calculate tgamma to accuracy of approximately 5 decimal digits.
//]

int main()
{
  cout.precision(5); // To only show 5 (hopefully) accurate decimal digits.
  double t = tgamma(12, my_pol_5()); // Apply the 5 decimal digits accuracy policy to use of tgamma.
  cout << "tgamma(12, my_pol_5() = " << t << endl;
}

/*

Output:
     tgamma(12, my_pol_5() = 3.9917e+007
*/


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <istream>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <complex>

#include "codegen_helper.hpp"

#define PI 3.14159265

// Read a CSV file and dump it to an Eigen Matrix
Eigen::Matrix<double, Eigen::Dynamic, 1> readCSV(std::istream &input, int matSize)
{
    int a = 0;
    int b = 0;

    std::string csvLine;
    Eigen::Matrix<double, Eigen::Dynamic, 1> out;
    out.resize(matSize*matSize);
    // read every line from the stream
    while( std::getline(input, csvLine) )
    {

        std::istringstream csvStream(csvLine);
        std::vector<std::string> csvColumn;
        std::string csvElement;
        // read every element from the line that is seperated by commas
        // and put it into the vector or strings
        while( getline(csvStream, csvElement, ',') )
        {
            csvColumn.push_back(csvElement);
            out[b] = std::stod(csvElement);
            b++;
        }
        a++;
    }
    std::cout << "a : " << a << " b : " << b << std::endl;
    return out;
}

// Build a complex Eigen::Matrix representing the 2D FFT coeffs from 2 files representing the real and imaginary 2D FFT matrices
template <typename Scalar>
Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, 1> CSVtoFTcoeff(std::istream &inputReal, std::istream &inputImag, int matSize)
{
    std::complex<Scalar> consti;
    consti = std::complex<double>{0,1};

    Eigen::Matrix<Scalar, Eigen::Dynamic, 1> outReal;
    Eigen::Matrix<Scalar, Eigen::Dynamic, 1> outImag;
    Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, 1> out;

    outReal = readCSV(inputReal, matSize);
    outImag = readCSV(inputImag, matSize);

    out = outReal + consti*outImag;

    return out;
}

// Evaluate a 2D function from its FT coefficients
template <typename Scalar>
Scalar evaluateFromFT(Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, Eigen::Dynamic> FTcoeffs, Scalar x, Scalar y)
{
    Scalar zero;
    zero = 0;

    std::complex<Scalar> eval;
    eval = std::complex<double>{0,0};

    std::complex<Scalar> consti;
    consti = std::complex<double>{0,1};

    int m = (int)FTcoeffs.rows();
    int n = (int)FTcoeffs.cols();

    Scalar coeffThreshold;
    coeffThreshold = 1e-6;

    for(int i=0; i<n; i++)
    {
        for(int j=0; j<m; j++)
        {
            Scalar coeffReal;
            Scalar coeffImag;

            coeffReal = CppAD::CondExpLt(std::abs(FTcoeffs(i,j)), coeffThreshold, zero, FTcoeffs(i,j).real());
            coeffImag = CppAD::CondExpLt(std::abs(FTcoeffs(i,j)), coeffThreshold, zero, FTcoeffs(i,j).imag());
            std::complex<Scalar> coeff(coeffReal, coeffImag);

            eval += coeff*(cos(2*PI*(y*(i-n/2)/n + x*(j-m/2)/m)) + consti*sin(2*PI*(y*(i-n/2)/n + x*(j-m/2)/m)));
        }
    }
    std::complex<Scalar> scale;
    scale = std::complex<double>{(double)n*m,0};
    eval/=scale;
    return CppAD::sqrt(eval.real()*eval.real() - eval.imag()*eval.imag());
}

// Generates the model for the function f(q, pair) = dist. between frames of given pair
ADFun tapeADShoulderDistanceCheck(Eigen::Matrix<std::complex<ADScalar>, Eigen::Dynamic, Eigen::Dynamic> FTcoeffs)
{
    // Initnialize AD input and output
    Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ad_X;
    Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ad_Y;
    ad_X.resize(2);
    ad_Y.resize(1);
    CppAD::Independent(ad_X);
    // Initialize AD function
    ADFun ad_fun;

    // Tape the function
    ADScalar d = evaluateFromFT<ADScalar>(FTcoeffs, ad_X[0], ad_X[1]);
    ad_Y[0] = d;
    ad_fun.Dependent(ad_X, ad_Y);

    return ad_fun;
}


//
// Copyright 2019 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef BOOST_GIL_IMAGE_PROCESSING_NUMERIC_HPP
#define BOOST_GIL_IMAGE_PROCESSING_NUMERIC_HPP

#include <boost/gil/detail/math.hpp>
#include <cmath>
#include <boost/gil/image_view.hpp>
#include <boost/gil/typedefs.hpp>

namespace boost { namespace gil {

/// \defgroup ImageProcessingMath
/// \brief Math operations for IP algorithms
///
/// This is mostly handful of mathemtical operations that are required by other
/// image processing algorithms
///
/// \brief Normalized cardinal sine
/// \ingroup ImageProcessingMath
///
/// normalized_sinc(x) = sin(pi * x) / (pi * x)
///
inline double normalized_sinc(double x)
{
    return std::sin(x * boost::gil::pi) / (x * boost::gil::pi);
}

/// \brief Lanczos response at point x
/// \ingroup ImageProcessingMath
///
/// Lanczos response is defined as:
/// x == 0: 1
/// -a < x && x < a: 0
/// otherwise: normalized_sinc(x) / normalized_sinc(x / a)
inline double lanczos(double x, std::ptrdiff_t a)
{
    // means == but <= avoids compiler warning
    if (0 <= x && x <= 0)
        return 1;

    if (-a < x && x < a)
        return normalized_sinc(x) / normalized_sinc(x / static_cast<double>(a));

    return 0;
}

inline void compute_tensor_entries(
    boost::gil::gray16_view_t dx,
    boost::gil::gray16_view_t dy,
    boost::gil::gray32f_view_t m11,
    boost::gil::gray32f_view_t m12_21,
    boost::gil::gray32f_view_t m22)
{
    for (std::ptrdiff_t y = 0; y < dx.height(); ++y) {
        for (std::ptrdiff_t x = 0; x < dx.width(); ++x) {
            auto dx_value = dx(x, y);
            auto dy_value = dy(x, y);
            m11(x, y) = dx_value * dx_value;
            m12_21(x, y) = dx_value * dy_value;
            m22(x, y) = dy_value * dy_value;
        }
    }
}

/// \brief Compute xy gradient, and second order x and y gradients
/// \ingroup ImageProcessingMath
///
/// Hessian matrix is defined as a matrix of partial derivates
/// for 2d case, it is [[ddxx, dxdy], [dxdy, ddyy].
/// d stands for derivative, and x or y stand for direction.
/// For example, dx stands for derivative (gradient) in horizontal
/// direction, and ddxx means second order derivative in horizon direction
/// https://en.wikipedia.org/wiki/Hessian_matrix
template <typename GradientView, typename OutputView>
inline void compute_hessian_entries(
    GradientView dx,
    GradientView dy,
    OutputView ddxx,
    OutputView dxdy,
    OutputView ddyy)
{
    using x_coord_t = typename OutputView::x_coord_t;
    using y_coord_t = typename OutputView::y_coord_t;
    using pixel_t = typename std::remove_reference<decltype(ddxx(0, 0))>::type;
    using channel_t = typename std::remove_reference<
                        decltype(
                            std::declval<pixel_t>().at(
                                std::integral_constant<int, 0>{}
                            )
                        )
                       >::type;

    constexpr double x_kernel[3][3] =
    {
        {1, 0, -1},
        {2, 0, -2},
        {1, 0, -1}
    };
    constexpr double y_kernel[3][3] =
    {
        {1, 2, 1},
        {0, 0, 0},
        {-1, -2, -1}
    };
    constexpr auto chosen_channel = std::integral_constant<int, 0>{};
    for (y_coord_t y = 1; y < ddxx.height() - 1; ++y)
    {
        for (x_coord_t x = 1; x < ddxx.width() - 1; ++x)
        {
            pixel_t ddxx_i;
            static_transform(ddxx_i, ddxx_i,
                [](channel_t) { return static_cast<channel_t>(0); });
            pixel_t dxdy_i;
            static_transform(dxdy_i, dxdy_i,
                [](channel_t) { return static_cast<channel_t>(0); });
            pixel_t ddyy_i;
            static_transform(ddyy_i, ddyy_i,
                [](channel_t) { return static_cast<channel_t>(0); });
            for (y_coord_t y_filter = 0; y_filter < 2; ++y_filter)
            {
                for (x_coord_t x_filter = 0; x_filter < 2; ++x_filter)
                {
                    auto adjusted_y = y + y_filter - 1;
                    auto adjusted_x = x + x_filter - 1;
                    ddxx_i.at(std::integral_constant<int, 0>{}) +=
                        dx(adjusted_x, adjusted_y).at(chosen_channel)
                        * x_kernel[y_filter][x_filter];
                    dxdy_i.at(std::integral_constant<int, 0>{}) +=
                        dx(adjusted_x, adjusted_y).at(chosen_channel)
                        * y_kernel[y_filter][x_filter];
                    ddyy_i.at(std::integral_constant<int, 0>{}) +=
                        dy(adjusted_x, adjusted_y).at(chosen_channel)
                        * y_kernel[y_filter][x_filter];
                }
            }
            ddxx(x, y) = ddxx_i;
            dxdy(x, y) = dxdy_i;
            ddyy(x, y) = ddyy_i;
        }
    }
}

/// \brief Generate mean kernel
/// \ingroup ImageProcessingMath
///
/// Fills supplied view with normalized mean
/// in which all entries will be equal to
/// \code 1 / (dst.size()) \endcode
inline void generate_normalized_mean(boost::gil::gray32f_view_t dst)
{
    if (dst.width() != dst.height() || dst.width() % 2 != 1)
        throw std::invalid_argument("kernel dimensions should be odd and equal");
    const float entry = 1.0f / static_cast<float>(dst.size());

    for (auto& pixel: dst) {
        pixel.at(std::integral_constant<int, 0>{}) = entry;
    }
}

/// \brief Generate kernel with all 1s
/// \ingroup ImageProcessingMath
///
/// Fills supplied view with 1s (ones)
inline void generate_unnormalized_mean(boost::gil::gray32f_view_t dst)
{
    if (dst.width() != dst.height() || dst.width() % 2 != 1)
        throw std::invalid_argument("kernel dimensions should be odd and equal");

    for (auto& pixel: dst) {
        pixel.at(std::integral_constant<int, 0>{}) = 1.0f;
    }
}

/// \brief Generate Gaussian kernel
/// \ingroup ImageProcessingMath
///
/// Fills supplied view with values taken from Gaussian distribution. See
/// https://en.wikipedia.org/wiki/Gaussian_blur
inline void generate_gaussian_kernel(boost::gil::gray32f_view_t dst, double sigma)
{
    if (dst.width() != dst.height() || dst.width() % 2 != 1)
        throw std::invalid_argument("kernel dimensions should be odd and equal");

    const double denominator = 2 * boost::gil::pi * sigma * sigma;
    const auto middle = boost::gil::point_t(dst.width() / 2, dst.height() / 2);
    for (boost::gil::gray32f_view_t::coord_t y = 0; y < dst.height(); ++y)
    {
        for (boost::gil::gray32f_view_t::coord_t x = 0; x < dst.width(); ++x)
        {
            const auto delta_x = std::abs(middle.x - x);
            const auto delta_y = std::abs(middle.y - y);
            const double power = (delta_x * delta_x +  delta_y * delta_y) / (2 * sigma * sigma);
            const double nominator = std::exp(-power);
            const float value = nominator / denominator;
            dst(x, y).at(std::integral_constant<int, 0>{}) = value;
        }
    }
}

}} // namespace boost::gil

#endif


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use of the structural SVM solver from the dlib C++
    Library.  Therefore, this example teaches you the central ideas needed to setup a
    structural SVM model for your machine learning problems.  To illustrate the process, we
    use dlib's structural SVM solver to learn the parameters of a simple multi-class
    classifier.  We first discuss the multi-class classifier model and then walk through
    using the structural SVM tools to find the parameters of this classification model.

*/

#include <iostream>
#include <dlib/svm_threaded.h>

using namespace std;
using namespace dlib;

// Before we start, we define three typedefs we will use throughout this program.  The
// first is used to represent the parameter vector the structural SVM is learning, the
// second is used to represent the "sample type".  In this example program it is just a
// vector but in general when using a structural SVM your sample type can be anything you
// want (e.g. a string or an image).  The last typedef is the type used to represent the
// PSI vector which is part of the structural SVM model which we will explain in detail
// later on.  But the important thing to note here is that you can use either a dense
// representation (i.e. a dlib::matrix object) or a sparse representation for the PSI
// vector.  See svm_sparse_ex.cpp for an introduction to sparse vectors in dlib.  Here we
// use the same type for each of these three things to keep the example program simple.
typedef matrix<double,0,1> column_vector;       // Must be a dlib::matrix type.
typedef matrix<double,0,1> sample_type;         // Can be anything you want.
typedef matrix<double,0,1> feature_vector_type; // Must be dlib::matrix or some kind of sparse vector.

// ----------------------------------------------------------------------------------------

int           predict_label                (const column_vector& weights, const sample_type& sample);
column_vector train_three_class_classifier (const std::vector<sample_type>& samples, const std::vector<int>& labels);

// ----------------------------------------------------------------------------------------

#if defined(BUILD_MONOLITHIC)
#define main(cnt, arr)      dlib_svm_struct_ex_main(cnt, arr)
#endif

int main(int argc, const char** argv)
{
    // In this example, we have three types of samples: class 0, 1, or 2.  That is, each of
    // our sample vectors falls into one of three classes.  To keep this example very
    // simple, each sample vector is zero everywhere except at one place.  The non-zero
    // dimension of each vector determines the class of the vector.  So for example, the
    // first element of samples has a class of 1 because samples[0](1) is the only non-zero
    // element of samples[0].
    sample_type samp(3);
    std::vector<sample_type> samples;
    samp = 0,2,0; samples.push_back(samp);
    samp = 1,0,0; samples.push_back(samp);
    samp = 0,4,0; samples.push_back(samp);
    samp = 0,0,3; samples.push_back(samp);
    // Since we want to use a machine learning method to learn a 3-class classifier we need
    // to record the labels of our samples.  Here samples[i] has a class label of labels[i].
    std::vector<int> labels;
    labels.push_back(1);
    labels.push_back(0);
    labels.push_back(1);
    labels.push_back(2);

    // Now that we have some training data we can tell the structural SVM to learn the
    // parameters of our 3-class classifier model.  The details of this will be explained
    // later.  For now, just note that it finds the weights (i.e. a vector of real valued
    // parameters) such that predict_label(weights, sample) always returns the correct
    // label for a sample vector.
    column_vector weights = train_three_class_classifier(samples, labels);

    // Print the weights and then evaluate predict_label() on each of our training samples.
    // Note that the correct label is predicted for each sample.
    cout << weights << endl;
    for (unsigned long i = 0; i < samples.size(); ++i)
        cout << "predicted label for sample["<<i<<"]: " << predict_label(weights, samples[i]) << endl;
}

// ----------------------------------------------------------------------------------------

int predict_label (
    const column_vector& weights,
    const sample_type& sample
)
/*!
    requires
        - weights.size() == 9
        - sample.size() == 3
    ensures
        - Given the 9-dimensional weight vector which defines a 3 class classifier, this
          function predicts the class of the given 3-dimensional sample vector.
          Therefore, the output of this function is either 0, 1, or 2 (i.e. one of the
          three possible labels).
!*/
{
    // Our 3-class classifier model can be thought of as containing 3 separate linear
    // classifiers.  So to predict the class of a sample vector we evaluate each of these
    // three classifiers and then whatever classifier has the largest output "wins" and
    // predicts the label of the sample.  This is the popular one-vs-all multi-class
    // classifier model.
    //
    // Keeping this in mind, the code below simply pulls the three separate weight vectors
    // out of weights and then evaluates each against sample.  The individual classifier
    // scores are stored in scores and the highest scoring index is returned as the label.
    column_vector w0, w1, w2;
    w0 = rowm(weights, range(0,2));
    w1 = rowm(weights, range(3,5));
    w2 = rowm(weights, range(6,8));

    column_vector scores(3);
    scores = dot(w0, sample), dot(w1, sample), dot(w2, sample);

    return index_of_max(scores);
}

// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------

class three_class_classifier_problem : public structural_svm_problem_threaded<column_vector, feature_vector_type>
{
    /*!
        Now we arrive at the meat of this example program.  To use dlib's structural SVM
        solver you need to define an object which tells the structural SVM solver what to
        do for your problem.  In this example, this is done by defining the three_class_classifier_problem
        object which inherits from structural_svm_problem_threaded.  Before we get into the
        details, we first discuss some background information on structural SVMs.

        A structural SVM is a supervised machine learning method for learning to predict
        complex outputs.  This is contrasted with a binary classifier which makes only simple
        yes/no predictions.  A structural SVM, on the other hand, can learn to predict
        complex outputs such as entire parse trees or DNA sequence alignments.  To do this,
        it learns a function F(x,y) which measures how well a particular data sample x
        matches a label y, where a label is potentially a complex thing like a parse tree.
        However, to keep this example program simple we use only a 3 category label output.

        At test time, the best label for a new x is given by the y which maximizes F(x,y).
        To put this into the context of the current example, F(x,y) computes the score for
        a given sample and class label.  The predicted class label is therefore whatever
        value of y which makes F(x,y) the biggest.  This is exactly what predict_label()
        does.  That is, it computes F(x,0), F(x,1), and F(x,2) and then reports which label
        has the biggest value.

        At a high level, a structural SVM can be thought of as searching the parameter space
        of F(x,y) for the set of parameters that make the following inequality true as often
        as possible:
            F(x_i,y_i) > max{over all incorrect labels of x_i} F(x_i, y_incorrect)
        That is, it seeks to find the parameter vector such that F(x,y) always gives the
        highest score to the correct output.  To define the structural SVM optimization
        problem precisely, we first introduce some notation:
            - let PSI(x,y)    == the joint feature vector for input x and a label y.
            - let F(x,y|w)    == dot(w,PSI(x,y)).
              (we use the | notation to emphasize that F() has the parameter vector of
              weights called w)
            - let LOSS(idx,y) == the loss incurred for predicting that the idx-th training
              sample has a label of y.  Note that LOSS() should always be >= 0 and should
              become exactly 0 when y is the correct label for the idx-th sample.  Moreover,
              it should notionally indicate how bad it is to predict y for the idx'th sample.
            - let x_i == the i-th training sample.
            - let y_i == the correct label for the i-th training sample.
            - The number of data samples is N.

        Then the optimization problem solved by dlib's structural SVM solver is the following:
            Minimize: h(w) == 0.5*dot(w,w) + C*R(w)

            Where R(w) == sum from i=1 to N: 1/N * sample_risk(i,w)
            and sample_risk(i,w) == max over all Y: LOSS(i,Y) + F(x_i,Y|w) - F(x_i,y_i|w)
            and C > 0

        You can think of the sample_risk(i,w) as measuring the degree of error you would make
        when predicting the label of the i-th sample using parameters w.  That is, it is zero
        only when the correct label would be predicted and grows larger the more "wrong" the
        predicted output becomes.  Therefore, the objective function is minimizing a balance
        between making the weights small (typically this reduces overfitting) and fitting the
        training data.  The degree to which you try to fit the data is controlled by the C
        parameter.

        For a more detailed introduction to structured support vector machines you should
        consult the following paper:
            Predicting Structured Objects with Support Vector Machines by
            Thorsten Joachims, Thomas Hofmann, Yisong Yue, and Chun-nam Yu

    !*/

public:

    // Finally, we come back to the code.  To use dlib's structural SVM solver you need to
    // provide the things discussed above.  This is the number of training samples, the
    // dimensionality of PSI(), as well as methods for calculating the loss values and
    // PSI() vectors.  You will also need to write code that can compute: max over all Y:
    // LOSS(i,Y) + F(x_i,Y|w).  In particular, the three_class_classifier_problem class is
    // required to implement the following four virtual functions:
    //   - get_num_dimensions()
    //   - get_num_samples()
    //   - get_truth_joint_feature_vector()
    //   - separation_oracle()

    // But first, we declare a constructor so we can populate our three_class_classifier_problem
    // object with the data we need to define our machine learning problem.  All we do here
    // is take in the training samples and their labels as well as a number indicating how
    // many threads the structural SVM solver will use.  You can declare this constructor
    // any way you like since it is not used by any of the dlib tools.
    three_class_classifier_problem (
        const std::vector<sample_type>& samples_,
        const std::vector<int>& labels_,
        const unsigned long num_threads
    ) :
        structural_svm_problem_threaded<column_vector, feature_vector_type>(num_threads),
        samples(samples_),
        labels(labels_)
    {}

    feature_vector_type make_psi (
        const sample_type& x,
        const int label
    ) const
    /*!
        ensures
            - returns the vector PSI(x,label)
    !*/
    {
        // All we are doing here is taking x, which is a 3 dimensional sample vector in this
        // example program, and putting it into one of 3 places in a 9 dimensional PSI
        // vector, which we then return.  So this function returns PSI(x,label).  To see why
        // we setup PSI like this, recall how predict_label() works.  It takes in a 9
        // dimensional weight vector and breaks the vector into 3 pieces.  Each piece then
        // defines a different classifier and we use them in a one-vs-all manner to predict
        // the label.  So now that we are in the structural SVM code we have to define the
        // PSI vector to correspond to this usage.  That is, we need to setup PSI so that
        // argmax_y dot(weights,PSI(x,y)) == predict_label(weights,x).  This is how we tell
        // the structural SVM solver what kind of problem we are trying to solve.
        //
        // It's worth emphasizing that the single biggest step in using a structural SVM is
        // deciding how you want to represent PSI(x,label).  It is always a vector, but
        // deciding what to put into it to solve your problem is often not a trivial task.
        // Part of the difficulty is that you need an efficient method for finding the label
        // that makes dot(w,PSI(x,label)) the biggest.  Sometimes this is easy, but often
        // finding the max scoring label turns into a difficult combinatorial optimization
        // problem.  So you need to pick a PSI that doesn't make the label maximization step
        // intractable but also still well models your problem.
        //
        // Finally, note that make_psi() is a helper routine we define in this example.  In
        // general, you are not required to implement it.  That is, all you must implement
        // are the four virtual functions defined below.

        // So let's make an empty 9-dimensional PSI vector
        feature_vector_type psi(get_num_dimensions());
        psi = 0; // zero initialize it

        // Now put a copy of x into the right place in PSI according to its label.  So for
        // example, if label is 1 then psi would be:  [0 0 0 x(0) x(1) x(2) 0 0 0]
        if (label == 0)
            set_rowm(psi,range(0,2)) = x;
        else if (label == 1)
            set_rowm(psi,range(3,5)) = x;
        else // the label must be 2
            set_rowm(psi,range(6,8)) = x;

        return psi;
    }

    // We need to declare the dimensionality of the PSI vector (this is also the
    // dimensionality of the weight vector we are learning).  Similarly, we need to declare
    // the number of training samples.  We do this by defining the following virtual
    // functions.
    virtual long get_num_dimensions () const { return samples[0].size() * 3; }
    virtual long get_num_samples ()    const { return samples.size(); }

    // In get_truth_joint_feature_vector(), all you have to do is output the PSI() vector
    // for the idx-th training sample when it has its true label.  So here it outputs
    // PSI(samples[idx], labels[idx]).
    virtual void get_truth_joint_feature_vector (
        long idx,
        feature_vector_type& psi
    ) const
    {
        psi = make_psi(samples[idx], labels[idx]);
    }

    // separation_oracle() is more interesting.  dlib's structural SVM solver will call
    // separation_oracle() many times during the optimization.  Each time it will give it
    // the current value of the parameter weights and separation_oracle() is supposed to
    // find the label that most violates the structural SVM objective function for the
    // idx-th sample.  Then the separation oracle reports the corresponding PSI vector and
    // loss value.  To state this more precisely, the separation_oracle() member function
    // has the following contract:
    //   requires
    //       - 0 <= idx < get_num_samples()
    //       - current_solution.size() == get_num_dimensions()
    //   ensures
    //       - runs the separation oracle on the idx-th sample.  We define this as follows:
    //           - let X           == the idx-th training sample.
    //           - let PSI(X,y)    == the joint feature vector for input X and an arbitrary label y.
    //           - let F(X,y)      == dot(current_solution,PSI(X,y)).
    //           - let LOSS(idx,y) == the loss incurred for predicting that the idx-th sample
    //             has a label of y.  Note that LOSS() should always be >= 0 and should
    //             become exactly 0 when y is the correct label for the idx-th sample.
    //
    //               Then the separation oracle finds a Y such that:
    //                   Y = argmax over all y: LOSS(idx,y) + F(X,y)
    //                   (i.e. It finds the label which maximizes the above expression.)
    //
    //               Finally, we can define the outputs of this function as:
    //               - #loss == LOSS(idx,Y)
    //               - #psi == PSI(X,Y)
    virtual void separation_oracle (
        const long idx,
        const column_vector& current_solution,
        scalar_type& loss,
        feature_vector_type& psi
    ) const
    {
        // Note that the solver will use multiple threads to make concurrent calls to
        // separation_oracle(), therefore, you must implement it in a thread safe manner
        // (or disable threading by inheriting from structural_svm_problem instead of
        // structural_svm_problem_threaded).  However, if your separation oracle is not
        // very fast to execute you can get a very significant speed boost by using the
        // threaded solver.  In general, all you need to do to make your separation oracle
        // thread safe is to make sure it does not modify any global variables or members
        // of three_class_classifier_problem.  So it is usually easy to make thread safe.

        column_vector scores(3);

        // compute scores for each of the three classifiers
        scores = dot(rowm(current_solution, range(0,2)),  samples[idx]),
                 dot(rowm(current_solution, range(3,5)),  samples[idx]),
                 dot(rowm(current_solution, range(6,8)),  samples[idx]);

        // Add in the loss-augmentation.  Recall that we maximize LOSS(idx,y) + F(X,y) in
        // the separate oracle, not just F(X,y) as we normally would in predict_label().
        // Therefore, we must add in this extra amount to account for the loss-augmentation.
        // For our simple multi-class classifier, we incur a loss of 1 if we don't predict
        // the correct label and a loss of 0 if we get the right label.
        if (labels[idx] != 0)
            scores(0) += 1;
        if (labels[idx] != 1)
            scores(1) += 1;
        if (labels[idx] != 2)
            scores(2) += 1;

        // Now figure out which classifier has the largest loss-augmented score.
        const int max_scoring_label = index_of_max(scores);
        // And finally record the loss that was associated with that predicted label.
        // Again, the loss is 1 if the label is incorrect and 0 otherwise.
        if (max_scoring_label == labels[idx])
            loss = 0;
        else
            loss = 1;

        // Finally, compute the PSI vector corresponding to the label we just found and
        // store it into psi for output.
        psi = make_psi(samples[idx], max_scoring_label);
    }

private:

    // Here we hold onto the training data by reference.  You can hold it by value or by
    // any other method you like.
    const std::vector<sample_type>& samples;
    const std::vector<int>& labels;
};

// ----------------------------------------------------------------------------------------

// This function puts it all together.  In here we use the three_class_classifier_problem
// along with dlib's oca cutting plane solver to find the optimal weights given our
// training data.
column_vector train_three_class_classifier (
    const std::vector<sample_type>& samples,
    const std::vector<int>& labels
)
{
    const unsigned long num_threads = 4;
    three_class_classifier_problem problem(samples, labels, num_threads);

    // Before we run the solver we set up some general parameters.  First,
    // you can set the C parameter of the structural SVM by calling set_c().
    problem.set_c(1);

    // The epsilon parameter controls the stopping tolerance.  The optimizer will run until
    // R(w) is within epsilon of its optimal value. If you don't set this then it defaults
    // to 0.001.
    problem.set_epsilon(0.0001);

    // Uncomment this and the optimizer will print its progress to standard out.  You will
    // be able to see things like the current risk gap.  The optimizer continues until the
    // risk gap is below epsilon.
    //problem.be_verbose();

    // The optimizer uses an internal cache to avoid unnecessary calls to your
    // separation_oracle() routine.  This parameter controls the size of that cache.
    // Bigger values use more RAM and might make the optimizer run faster.  You can also
    // disable it by setting it to 0 which is good to do when your separation_oracle is
    // very fast.  If you don't call this function it defaults to a value of 5.
    //problem.set_max_cache_size(20);

    column_vector weights;
    // Finally, we create the solver and then run it.
    oca solver;
    solver(problem, weights);

    // Alternatively, if you wanted to require that the learned weights are all
    // non-negative then you can call the solver as follows and it will put a constraint on
    // the optimization problem which causes all elements of weights to be >= 0.
    //solver(problem, weights, problem.get_num_dimensions());

    return weights;
}

// ----------------------------------------------------------------------------------------


#include <sparse.h>
#include <sparse_fill.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(SPARSE);

BOOST_AUTO_TEST_CASE(product_crm_crm_test)
{
  // scalar case
  sparse::CompressedRowMatrix<sparse::Block<1,1,float> > sA(1,1,1);
  sparse::CompressedRowMatrix<sparse::Block<1,1,float> > sB(1,1,1);
  sparse::CompressedRowMatrix<sparse::Block<1,1,float> > sC(1,1,1);

  sA(0,0)[0] = 5;
  sB(0,0)[0] = 2;
  sparse::prod(sA,sB,sC);
  BOOST_CHECK(sC(0,0)[0] == 5*2);

  sA.clear();
  sB.clear();
  sC.clear();

  sA.resize(4,3,5);
  sB.resize(3,4,3);
  sparse::fill(sA(0,0),1);
  sparse::fill(sA(0,2),2);
  sparse::fill(sA(2,1),3);
  sparse::fill(sA(3,0),4);
  sparse::fill(sA(3,1),5);
  sparse::fill(sB(0,0),6);
  sparse::fill(sB(1,1),7);
  sparse::fill(sB(2,3),8);
  sparse::prod(sA, sB, sC);
  BOOST_CHECK( sC(0,0)[0] == 6  );
  BOOST_CHECK( sC(0,3)[0] == 16 );
  BOOST_CHECK( sC(2,1)[0] == 21 );
  BOOST_CHECK( sC(3,0)[0] == 24 );
  BOOST_CHECK( sC(3,1)[0] == 35 );

  // Observe we do not need to test block case - it has been verified by product_block_test()

}

BOOST_AUTO_TEST_SUITE_END();


#include <iostream>
#include <string>
#include <utility>
#include <stdexcept>
#include <cmath>
#include <cstdlib>
#include <boost/program_options.hpp>
#include "logical_expr.hpp"
#include "quine_mccluskey.hpp"

using namespace std;

template<typename Property>
void print_term_expr(const logical_expr::logical_term<Property> &term,
            char first_char = 'A', char inverter = '~')
{
    for( int i = 0; i < term.size(); ++i ) {
        if( term[i] == false )  cout << inverter;
        if( term[i] != logical_expr::dont_care )
            cout << static_cast<char>(first_char + i);
    }
}

template<typename TermType>
void print_func_expr(
        const logical_expr::logical_function<TermType> &func,
        char first_char = 'A', const string &funcname = "f", char inverter = '~')
{
    cout << funcname << " = ";
    for( auto it = func.begin(); it != func.end(); ++it ) {
        print_term_expr(*it, first_char, inverter);
        if( it + 1 != func.end() )
            cout << " + ";
    }
    cout << endl;
}

template<typename TermType>
void print_truth_table(
        const logical_expr::logical_function<TermType> &f,
        char first_char = 'A', const string &funcname ="f"
    )
{
    cout << "Truth Table: ";
    print_func_expr(f, first_char, funcname);
    for( char c = first_char; c != first_char + f.term_size(); ++c )
        cout << c;
    cout << " | " << funcname << "()" << endl;
    for( int i = 0; i < f.term_size() + 6; ++i )
        cout << ((i == f.term_size() + 1) ? '|' : '-');
    cout << endl;
    logical_expr::arg_generator<> generator(0, std::pow(2, f.term_size()), f.term_size());
    for( auto arg : generator )
        cout << arg << " |  " << f(arg) << endl;
}

int main(int argc, char **argv)
{
    int exit_code = EXIT_SUCCESS;
    try {
        bool print_process = true;
        char first_char = 'A';
        constexpr char inverter = '~';

        //
        // Parse command line options
        //
        using namespace boost::program_options;
        options_description opt("Options");
        opt.add_options()
            ("quiet,q", "never print the information of the process of simplifying")
            ("first-char,c", value<char>(), "specify a character of the first variable used for input expression")
            ("help,h", "display this help and exit");
        variables_map argmap;
        store(parse_command_line(argc, argv, opt), argmap);
        notify(argmap);
        if( argmap.count("help") ) {
            std::cout << opt << endl;
            return EXIT_SUCCESS;
        }
        if( argmap.count("quiet") )
            print_process = false;
        if( argmap.count("first-char") )
            first_char = argmap["first-char"].as<char>();

        // Input a target logical function to be simplfied from stdin
        if( print_process )
            cout << "Logical Function Simplifier (Quine-McCluskey)"   << endl
                 << "[*] Enter a logical function to be simplified"   << endl
                 << "    (ex. \"f(A, B, C) = A + BC + ~A~B + ABC\" )" << endl
                 << "[*] Input: " << flush;
        string line;
        getline(cin, line);

        // Parse input logical expression and return tokenized
        logical_expr::function_parser<inverter, true> parser(line, first_char);
        auto token = parser.parse();
        // Create a logical function with logical_term<term_mark>
        typedef quine_mccluskey::simplifier::property_type PropertyType;
        typedef quine_mccluskey::simplifier::term_type TermType;
        logical_expr::logical_function<TermType> function;
        for( string term : token.second )
            function += logical_expr::parse_logical_term<PropertyType, inverter>(term, token.first.size(), first_char);

        // Create a simplifier using Quine-McCluskey algorithm
        quine_mccluskey::simplifier qm(function);
        if( print_process ) {
            cout << endl << "Sum of products form:" << endl;
            print_truth_table(qm.get_std_spf(), first_char);    // Print the function in sum of products form
            cout << endl << "Compressing ..." << endl;
            qm.compress_table(true);                            // Compress the compression table
            cout << endl << "Prime implicants: " << endl;
            for( const auto &term : qm.get_prime_implicants() ) {      // Print the prime implicants
                print_term_expr(term, first_char);
                cout << "  ";
            }
            cout << endl << endl << "Result of simplifying:" << endl;
        }
        else
            qm.compress_table(false);

        for( const auto &func : qm.simplify() )        // Simplify and print its results
            print_func_expr(func, first_char, parser.function_name() + "\'");
    }
    catch( std::exception &e ) {
        cerr << endl << "[-] Exception: " << e.what() << endl;
        exit_code = EXIT_FAILURE;
    }
    return exit_code;
}


/*
A class to represent the extension field GF(2^M)
*/

#ifndef EXTENSION_FIELD2
#define EXTENSION_FIELD2

#include <boost/operators.hpp>
#include <iostream>
#include <ostream>
#include <vector>
#include <utility>
#include "./helpers.hpp"

using namespace std;

// divide a by b; computer a/b and remainder(a/b)
// return pair with fist: quotient, second: remainder
template<class uint>
pair<uint,uint> divide(const uint a, const uint b){

	uint quot = 0; // quotient
	uint rem = a; // remainder

	if(b > a){ // then a is not divisible by b, and quot = 0
		return pair<uint,uint>(quot,rem);
	}

	while((rem >= b) && (rem != 0)){
		unsigned exp = degree(rem) - degree(b);
		quot |= (1 << exp);
		uint btmp = (b << exp);
		rem ^= btmp;
	}
	return pair<uint,uint>(quot,rem);
};

/*
GF(2^m)
Parameters are:
- m: the degree of the extension
- prim_poly: primitive polynomial defining the extension field
*/
template<class uint_, uint_ m_, uint_ prim_poly_>
class GF2M:
	boost::field_operators< GF2M<uint_,m_,prim_poly_>,
	boost::equality_comparable< GF2M<uint_,m_,prim_poly_>
	> >
{
public:
	//! an element of the extension field is a polynomial, represented by an unsigned integer
	typedef uint_ uint;
	static const unsigned m = m_;
	static const uint prim_poly = prim_poly_;
	uint el;
	bool empty;
	//! constructors
	GF2M(uint x){el = x % 2;empty = false;};
	//GF2M(unsigned x){el = x ;empty = false;};
	GF2M(uint x,unsigned v){el = x ;empty = false;}; // generates instance with el = x

	GF2M(){ el = 0; empty = true;}; // -1 marks empty

	GF2M& operator += (const GF2M& x){
		el = el ^ x.el;
		return *this;
	}

	GF2M& operator -= (const GF2M& x){
		el = el ^ x.el;
		return *this;
	}

	GF2M& operator *= (const GF2M& x){
		uint a = el;
		uint b = x.el;
		el = 0; // product of this and x
		while (a && b) {
				if (b & 1) // if b is odd, add the corresponding a to p
					el ^= a;

				if (a &  ( ((uint)1) << ( (unsigned) m - 1 )) ) // GF modulo: if a >= 2^m, it overflows when shifted left, so reduce
					a = (a << 1) ^ prim_poly; // XOR with the primitive polynomial
				else
					a <<= 1; // a*2 /* equivalent to a*2
				b >>= 1; // b / 2
		}
		return *this;
	}

	GF2M& operator /= (const GF2M& x){
		*this = (x.inverse() * (*this));
		return *this;
	}

	//! compute the multiplicative inverse via the extended Euclidean algorithm
	GF2M inverse() const {

		uint rem1 = prim_poly;
		uint rem2 = el;
		uint aux1 = 0;
		uint aux2 = 1;

		while( rem2 != 0 ){
			// res.first = quotient(rem1/rem2)
			// res.second = remainder(rem1/rem2)

			pair< uint, uint > res = divide(rem1,rem2);

			GF2M aux_new = GF2M(res.first,0)*GF2M(aux2,0) + GF2M(aux1,0);

			// prepare for the next step
			rem1 = rem2;
			rem2 = res.second;
			aux1 = aux2;
			aux2 = aux_new.el;

		}
		//aux1.multiply_monomial(pow(rem1.poly[0],-1), 0);
		return GF2M(aux1,0);
	}

	unsigned order() const {
		// multiplicative neutral element = 1
		//PFE one = PFE(1);
		GF2M one = GF2M(1);
		GF2M tmp = *this;  //EFE(el);
		unsigned ord = 1;
		while(!(tmp == one)){
			ord++;
			tmp *= *this;
		}
		return ord;
	}

	bool iszero() const {
		return (el == 0);
	}

	bool isempty() const {
		return empty;
	}

	bool operator ==(const GF2M& x) const {
		return (el == x.el);
	}

	bool operator < (const GF2M& x) const {
		return (el < x.el);
	}

};

template<class uint, uint m, uint prim_poly>
ostream &operator<<(ostream& stream, const GF2M<uint,m,prim_poly>& x)
    {
	  	stream << bitset<32>(x.el);
      	return stream; // must return stream
	};

template<class uint, uint m, uint prim_poly>
GF2M<uint,m,prim_poly> pow(const GF2M<uint,m,prim_poly>& a, int exp){

	if(a.iszero()) return a;
	if(exp == 0) {return GF2M<uint,m,prim_poly>(1);}

	GF2M<uint,m,prim_poly> tmp;

	if(exp < 0){
		GF2M<uint,m,prim_poly> am = a.inverse();
		tmp = am;
		for(unsigned i =1; i < abs(exp); ++i) tmp *= am;
	} else { // exp > 0
		tmp = a;
		for(unsigned i =1; i < abs(exp); ++i) tmp *= a;
	}

	return tmp;
};

#endif


#include "pcaauto.h"

#include <Eigen/Eigenvalues>

void PcaAuto::setupInertia( Eigen::Matrix3f const& inertia )
{
    inertia_ = inertia;

    auto solver = Eigen::EigenSolver<Eigen::Matrix3f>{inertia_, true};

    eigenVectors_ = solver.eigenvectors();
    eigenValues_  = solver.eigenvalues();
}


// Boost.Geometry

// Copyright (c) 2016-2017 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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_FORMULAS_THOMAS_DIRECT_HPP
#define BOOST_GEOMETRY_FORMULAS_THOMAS_DIRECT_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/formulas/differential_quantities.hpp>
#include <boost/geometry/formulas/flattening.hpp>
#include <boost/geometry/formulas/result_direct.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief The solution of the direct problem of geodesics on latlong coordinates,
       Forsyth-Andoyer-Lambert type approximation with second order terms.
\author See
    - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
      http://www.dtic.mil/docs/citations/AD0627893
    - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
      http://www.dtic.mil/docs/citations/AD0703541

*/
template <
    typename CT,
    bool EnableCoordinates = true,
    bool EnableReverseAzimuth = false,
    bool EnableReducedLength = false,
    bool EnableGeodesicScale = false
>
class thomas_direct
{
    static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
    static const bool CalcCoordinates = EnableCoordinates || CalcQuantities;
    static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcCoordinates || CalcQuantities;

public:
    typedef result_direct<CT> result_type;

    template <typename T, typename Dist, typename Azi, typename Spheroid>
    static inline result_type apply(T const& lo1,
                                    T const& la1,
                                    Dist const& distance,
                                    Azi const& azimuth12,
                                    Spheroid const& spheroid)
    {
        result_type result;

        CT const lon1 = lo1;
        CT const lat1 = la1;

        if ( math::equals(distance, Dist(0)) || distance < Dist(0) )
        {
            result.lon2 = lon1;
            result.lat2 = lat1;
            return result;
        }

        CT const c0 = 0;
        CT const c1 = 1;
        CT const c2 = 2;
        CT const c4 = 4;

        CT const a = CT(get_radius<0>(spheroid));
        CT const b = CT(get_radius<2>(spheroid));
        CT const f = formula::flattening<CT>(spheroid);
        CT const one_minus_f = c1 - f;

        CT const pi = math::pi<CT>();
        CT const pi_half = pi / c2;

        // keep azimuth small - experiments show low accuracy
        // if the azimuth is closer to (+-)180 deg.
        CT azi12_alt = azimuth12;
        CT lat1_alt = lat1;
        bool alter_result = vflip_if_south(lat1, azimuth12, lat1_alt, azi12_alt);

        CT const theta1 = math::equals(lat1_alt, pi_half) ? lat1_alt :
                          math::equals(lat1_alt, -pi_half) ? lat1_alt :
                          atan(one_minus_f * tan(lat1_alt));
        CT const sin_theta1 = sin(theta1);
        CT const cos_theta1 = cos(theta1);

        CT const sin_a12 = sin(azi12_alt);
        CT const cos_a12 = cos(azi12_alt);

        CT const M = cos_theta1 * sin_a12; // cos_theta0
        CT const theta0 = acos(M);
        CT const sin_theta0 = sin(theta0);

        CT const N = cos_theta1 * cos_a12;
        CT const C1 = f * M; // lower-case c1 in the technical report
        CT const C2 = f * (c1 - math::sqr(M)) / c4; // lower-case c2 in the technical report
        CT const D = (c1 - C2) * (c1 - C2 - C1 * M);
        CT const P = C2 * (c1 + C1 * M / c2) / D;

        // special case for equator:
        // sin_theta0 = 0 <=> lat1 = 0 ^ |azimuth12| = pi/2
        // NOTE: in this case it doesn't matter what's the value of cos_sigma1 because
        //       theta1=0, theta0=0, M=1|-1, C2=0 so X=0 and Y=0 so d_sigma=d
        //       cos_a12=0 so N=0, therefore
        //       lat2=0, azi21=pi/2|-pi/2
        //       d_eta = atan2(sin_d_sigma, cos_d_sigma)
        //       H = C1 * d_sigma
        CT const cos_sigma1 = math::equals(sin_theta0, c0)
                                ? c1
                                : normalized1_1(sin_theta1 / sin_theta0);
        CT const sigma1 = acos(cos_sigma1);
        CT const d = distance / (a * D);
        CT const u = 2 * (sigma1 - d);
        CT const cos_d = cos(d);
        CT const sin_d = sin(d);
        CT const cos_u = cos(u);
        CT const sin_u = sin(u);

        CT const W = c1 - c2 * P * cos_u;
        CT const V = cos_u * cos_d - sin_u * sin_d;
        CT const X = math::sqr(C2) * sin_d * cos_d * (2 * math::sqr(V) - c1);
        CT const Y = c2 * P * V * W * sin_d;
        CT const d_sigma = d + X - Y;
        CT const sin_d_sigma = sin(d_sigma);
        CT const cos_d_sigma = cos(d_sigma);

        if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth))
        {
            result.reverse_azimuth = atan2(M, N * cos_d_sigma - sin_theta1 * sin_d_sigma);

            if (alter_result)
            {
                vflip_rev_azi(result.reverse_azimuth, azimuth12);
            }
        }

        if (BOOST_GEOMETRY_CONDITION(CalcCoordinates))
        {
            CT const S_sigma = c2 * sigma1 - d_sigma;
            CT const cos_S_sigma = cos(S_sigma);
            CT const d_eta = atan2(sin_d_sigma * sin_a12, cos_theta1 * cos_d_sigma - sin_theta1 * sin_d_sigma * cos_a12);
            CT const H = C1 * (c1 - C2) * d_sigma - C1 * C2 * sin_d_sigma * cos_S_sigma;
            CT const d_lambda = d_eta - H;

            result.lon2 = lon1 + d_lambda;

            if (! math::equals(M, c0))
            {
                CT const sin_a21 = sin(result.reverse_azimuth);
                CT const tan_theta2 = (sin_theta1 * cos_d_sigma + N * sin_d_sigma) * sin_a21 / M;
                result.lat2 = atan(tan_theta2 / one_minus_f);
            }
            else
            {
                CT const sigma2 = S_sigma - sigma1;
                //theta2 = asin(cos(sigma2)) <=> sin_theta0 = 1
                // NOTE: cos(sigma2) defines the sign of tan_theta2
                CT const tan_theta2 = cos(sigma2) / math::abs(sin(sigma2));
                result.lat2 = atan(tan_theta2 / one_minus_f);
            }

            if (alter_result)
            {
                result.lat2 = -result.lat2;
            }
        }

        if (BOOST_GEOMETRY_CONDITION(CalcQuantities))
        {
            typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 2> quantities;
            quantities::apply(lon1, lat1, result.lon2, result.lat2,
                              azimuth12, result.reverse_azimuth,
                              b, f,
                              result.reduced_length, result.geodesic_scale);
        }

        return result;
    }

private:
    static inline bool vflip_if_south(CT const& lat1, CT const& azi12, CT & lat1_alt, CT & azi12_alt)
    {
        CT const c2 = 2;
        CT const pi = math::pi<CT>();
        CT const pi_half = pi / c2;

        if (azi12 > pi_half)
        {
            azi12_alt = pi - azi12;
            lat1_alt = -lat1;
            return true;
        }
        else if (azi12 < -pi_half)
        {
            azi12_alt = -pi - azi12;
            lat1_alt = -lat1;
            return true;
        }

        return false;
    }

    static inline void vflip_rev_azi(CT & rev_azi, CT const& azimuth12)
    {
        CT const c0 = 0;
        CT const pi = math::pi<CT>();

        if (rev_azi == c0)
        {
            rev_azi = azimuth12 >= 0 ? pi : -pi;
        }
        else if (rev_azi > c0)
        {
            rev_azi = pi - rev_azi;
        }
        else
        {
            rev_azi = -pi - rev_azi;
        }
    }

    static inline CT normalized1_1(CT const& value)
    {
        CT const c1 = 1;
        return value > c1 ? c1 :
               value < -c1 ? -c1 :
               value;
    }
};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_THOMAS_DIRECT_HPP


// main is provided in tests-main.cpp
#include <catch2/catch.hpp>
#include <mitama/dimensional/quantity.hpp>
#include <mitama/dimensional/systems/si/all.hpp>
#include <mitama/dimensional/systems/nonsi/degree_angle.hpp>
#include <mitama/dimensional/systems/si/prefix.hpp>
#include <random>
#include <test_util.hpp>
#include <mitama/dimensional/arithmetic.hpp>

using namespace mitama;
namespace si = mitama::systems::si;
namespace nsi = mitama::systems::nonsi;
TEST_CASE("degree angle and radian",
          "[quantity][systems/nonsi][degree_angle]")
{
    using namespace Catch::literals;
    quantity_t<nsi::degree_angle_t> s1 = 90;
    quantity_t<si::radian_t> c = s1;
    REQUIRE(c.value() == 1.570796_a);
    quantity_t<nsi::degree_angle_t> s2 = c;
    REQUIRE(s2.value() == 90._a);
}

TEST_CASE("degree amgle and radian generate tests",
          "[quantity][systems/nonsi][degree_angle]")
{
    using namespace Catch::literals;

    REQUIRE(
        test_util::RandomGenerator<double>::uniform( -360, 360 )
            .take(1000)
            .required([](auto value){
                quantity_t<nsi::degree_angle_t> c = value;
                quantity_t<si::radian_t> s = c;
                return c.value() / s.value() == 57.295779_a;
            }));
    REQUIRE(
        test_util::RandomGenerator<double>::uniform( -6.283185, 6.283185)
            .take(1)
            .required([](auto value){
                quantity_t<si::radian_t> s = value;
                quantity_t<nsi::degree_angle_t> c = s;
                return c.value() / s.value() == 57.295779_a;
            }));
}

#include <mitama/dimensional/systems/information/byte.hpp>
#include <mitama/dimensional/systems/information/prefix.hpp>
#include <mitama/dimensional/systems/information/shannon.hpp>
#include <mitama/dimensional/systems/information/nat.hpp>
#include <mitama/dimensional/systems/information/hartley.hpp>
#include <mitama/dimensional/systems/information/conversions.hpp>
#include <boost/format.hpp>

TEST_CASE("information format", "[info]") {
    using namespace mitama::systems::information;
    auto fmt = [](auto x){ return (boost::format("%1%") % x).str(); };
    REQUIRE( fmt(1|bits) == "1 [b]" );
    REQUIRE( fmt(1|bytes) == "1 [B]" );
    REQUIRE( fmt(1|shannon) == "1 [Sh]" );
    REQUIRE( fmt(1|nat) == "1 [nat]" );
    REQUIRE( fmt(1|hartley) == "1 [Hart]" );
}

TEST_CASE("information prefix format", "[info]")
{
    using namespace mitama::systems::information;
    auto fmt = [](auto x){ return (boost::format("%1%") % x).str(); };
    REQUIRE( fmt(1| kibi * bytes) == "1 [KiB]" );
    REQUIRE( fmt(1| mebi * bytes) == "1 [MiB]" );
    REQUIRE( fmt(1| gibi * bytes) == "1 [GiB]" );
    REQUIRE( fmt(1| tebi * bytes) == "1 [TiB]" );
    REQUIRE( fmt(1| pebi * bytes) == "1 [PiB]" );
}


#define BOOST_TEST_MODULE "Test Sorensen class"

#include <boost/test/unit_test.hpp>

#include "distance/Sorensen.hpp"

using namespace genex;

#define TOLERANCE 1e-9

struct MockData
{
  data_t dat_1[5] = {3, 1, 2, 5, 4};
  data_t dat_2[5] = {3.4, 4, 1.2, 2, 3};
};

BOOST_AUTO_TEST_CASE( sorensen_test, *boost::unit_test::tolerance(TOLERANCE)  )
{
  MockData data;
  TimeSeries ts_1(data.dat_1, 5);
  TimeSeries ts_2(data.dat_2, 5);
  Sorensen dist;

  data_t* total = dist.init();

  for (int i = 0; i < ts_1.getLength(); i++) {
    total = dist.reduce(total, total, ts_1[i], ts_2[i]);
  }

  BOOST_TEST( dist.norm(total, ts_1, ts_2), 0.3216 );

  delete total;
}


#include <tiny_math_types.h>
#include <tiny_eigen3x3.h>
#include <tiny_polar_decomposition3x3.h>
#include <tiny_matrix_functions.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <cmath>

BOOST_AUTO_TEST_SUITE(tiny_polar_decomposition);

  BOOST_AUTO_TEST_CASE(eigen_method)
  {
    typedef tiny::MathTypes<double>                   math_types;
    typedef math_types::matrix3x3_type                matrix3x3_type;
    typedef math_types::real_type                     real_type;
    typedef math_types::value_traits                  value_traits;

    real_type epsilon = 10e-7;
    matrix3x3_type A,R,S,D;

    for(size_t i=0;i<10;++i)
    {
      S = matrix3x3_type::random();
      S = tiny::trans(S)*S;
      A = matrix3x3_type::random();
      R = tiny::ortonormalize( A );
      A = R*S;
      R = matrix3x3_type::make_diag(1.0);
      S = matrix3x3_type::make_diag(1.0);
      bool success = tiny::polar_decomposition_eigen(A,R,S);
      if(success)
      {
        bool right_handed = tiny::det(R) > value_traits::zero();
        BOOST_CHECK( right_handed );

        D = A - R*S;
        real_type maximum_deviation =  tiny::max(  abs(D) );

        //BOOST_CHECK_CLOSE( maximum_deviation, value_traits::zero(), tol);
        BOOST_CHECK( maximum_deviation<epsilon );
      }
    }
  }

BOOST_AUTO_TEST_SUITE_END();


#ifndef _FM_HPP
#define _FM_HPP
#include <map>
#include <cmath>
#include <boost/math/special_functions/bessel.hpp>

using namespace std;
using namespace boost::math;

static const int SAMPLE_RATE {44100};

class Param
{
private:
    const float ratio {0.99};
    float target_value;
    float intermediate_value;
public:
    Param(float v) : target_value{v}, intermediate_value{v}
    {
    }

    void set(float v)
    {
        target_value = v;
    }

    float get()
    {
        if ( intermediate_value != target_value )
            intermediate_value = ratio*intermediate_value + (1.0-ratio)*target_value;
        return intermediate_value;
    }

    float abs()
    {
        return target_value;
    }
};

class FMSynth
{
private:
  float carrier_phase {0.0};
  float modulating_phase {0.0};
public:
  float carrier_frequency {200.0};
  float modulating_frequency {100.0};
  Param modulation_index {0.0};

  struct Band
  {
    float amplitude {0.0};
    enum {NEGATIVE, CENTER, POSITIVE, MIXED} type;
  };

  float process()
  {
    const float value {sin(carrier_phase + modulation_index.get()*sin(modulating_phase))};

    carrier_phase += carrier_frequency * 2.0 * M_PI / SAMPLE_RATE;
    if ( carrier_phase > 2*M_PI )
        carrier_phase -= 2*M_PI;
    modulating_phase += modulating_frequency * 2.0 * M_PI / SAMPLE_RATE;
    if ( modulating_phase > 2*M_PI )
        modulating_phase -= 2*M_PI;

    return value;
  }

  float peak_deviation()
  {
      return modulation_index.abs() * modulating_frequency;
  }

  map<float, Band> spectra()
  {
    const float bandwidth {2.0f * (peak_deviation() + modulating_frequency)};
    const int n {static_cast<int>(ceil(bandwidth / modulating_frequency / 2.0)) + 1};
    map<float, Band> bands;

    // initial band
    bands[carrier_frequency].amplitude += cyl_bessel_j(0, modulation_index.abs());
    bands[carrier_frequency].type = Band::CENTER;

    // side bands
    for (int i = 1; i <= n; i ++)
    {
      const double amplitude_pos {cyl_bessel_j(i, modulation_index.abs())};
      double amplitude_neg {amplitude_pos * pow(-1.0, i)};
      const float frequency_pos {carrier_frequency + modulating_frequency*i};
      float frequency_neg {carrier_frequency - modulating_frequency*i};

      // reflect
      if (frequency_neg < 0)
      {
        frequency_neg *= -1;
        amplitude_neg *= -1;
      }

      if ( bands.count(frequency_pos) )
          bands[frequency_pos].type = Band::MIXED;
      else
          bands[frequency_pos].type = Band::POSITIVE;
      bands[frequency_pos].amplitude += amplitude_pos;

      if ( bands.count(frequency_neg) )
          bands[frequency_neg].type = Band::MIXED;
      else
          bands[frequency_neg].type = Band::NEGATIVE;
      bands[frequency_neg].amplitude += amplitude_neg;
    }

    return bands;
  }
};

#endif


#include <iostream>
#include <sstream>
#include <fstream>
#include <unordered_map>
#include <Eigen/Core>
#include <Eigen/Geometry>

const float fx = 2892.843;
const float fy = 2882.249;
const int w = 1600;
const int h = 1200;
const float cx = 824.4251;
const float cy = 605.18715;

void dtu_to_cameras_txt(const std:: string & out_folder) {

  std::cout<<"Creating DTU intrinsics file: "<<out_folder<<"/cameras.txt "<<std::flush;
  std::ofstream cameras_txt (out_folder+"/cameras.txt");
  //intrinsics calibration fixed

  cameras_txt << "0 PINHOLE " << w << " " << h << " " << fx << " " << fy << " " << cx << " " << cy<<std::endl;
  cameras_txt.close();
  std::cout<<"   --->DONE"<<std::endl;
}

void dtu_to_images_txt(const std::string & pathImages,const std:: string & pathCalib, const std:: string & out_folder) {

  std::cout<<"Creating DTU images file: "<<out_folder<<"/images.txt "<<std::endl;
  std::cout<<"pathImages : "<<pathImages<<" "<<std::endl;
  std::cout<<"pathCalib : "<<pathCalib<<" "<<std::endl;

  std::ofstream images_txt (out_folder+"/images.txt");

  //Path Prefixes/postfixes
  const std::string image_prefix = "rect_";
  const std::string image_postfix = "_3_r5000.png";
  const std::string pose_prefix = "pos_";
  const std::string pose_postfix = ".txt";
  const int n_zeros = 3;

  Eigen::Matrix3f K = Eigen::Matrix3f::Zero();
  K(0,0) = fx;
  K(1,1) = fy;
  K(0,2) = cx;
  K(1,2) = cy;
  K(2,2) = 1.0;

  Eigen::Matrix3f K_inv = K.inverse(); //TODO check

  for (int i = 1; i <= 64; ++i){
    std::string number = std::string(n_zeros - std::to_string(i).length(), '0') + std::to_string(i);

    std::ifstream camFile(pathCalib  + "/" + pose_prefix  + number + pose_postfix);
    std::ifstream imgFile(pathImages + "/" + image_prefix + number + image_postfix);

    Eigen::Matrix<float,3,4> cameraMatrix;
    if(camFile.is_open() && imgFile.is_open()){
    //Reading the transpose rotation  since the file stores the inverse of the Hartley Zisserman convention
      camFile >> cameraMatrix(0,0) >> cameraMatrix(0,1) >> cameraMatrix(0,2) >> cameraMatrix(0,3);
      camFile >> cameraMatrix(1,0) >> cameraMatrix(1,1) >> cameraMatrix(1,2) >> cameraMatrix(1,3);
      camFile >> cameraMatrix(2,0) >> cameraMatrix(2,1) >> cameraMatrix(2,2) >> cameraMatrix(2,3);

      Eigen::Matrix<float,3,4> extrinsics;
      extrinsics = K_inv * cameraMatrix;

      Eigen::Matrix3f rotation;
      Eigen::Vector3f traslation;
      for (int curR = 0; curR < 3; ++curR) {
        for (int curC = 0; curC < 3; ++curC) {
          rotation(curR,curC) = extrinsics(curR,curC);
        }
      }
      for (int curR = 0; curR < 3; ++curR) {
        traslation(curR) = extrinsics(curR,3);
      }

      Eigen::Quaternionf q(rotation);

      images_txt << i << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z ()
          << " " << traslation(0) << " " << traslation(1) << " " << traslation(2)
          << " 0 " << image_prefix + number + image_postfix << std::endl;
      images_txt << std::endl;
    }
  }

  images_txt.close();
  std::cout<<"   --->DONE"<<std::endl;
}

void dtu_to_points_txt(const std:: string & out_folder){

  std::cout<<"Creating DTU 3D Points file: "<<out_folder<<"/points3D.txt "<<std::flush;

  std::ofstream points_txt (out_folder+"/points3D.txt");
  points_txt.close();
  std::cout<<"   --->DONE"<<std::endl;
}

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

  if(argc==4){
    dtu_to_cameras_txt(std::string(argv[3]));
    dtu_to_images_txt(std::string(argv[1]), std::string(argv[2]), std::string(argv[3]));
    dtu_to_points_txt(std::string(argv[3]));

  }else{
    std::cout<< "Wrong Arguments" << std::endl;
    std::cout<< "Usage:" << std::endl;
    std::cout<< "dtu_to_colmap pathImages pathcalib outputfolder" << std::endl;
  }

  return 0;
}


//
//  cal_T_abc.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/7/18.
//
//

#ifndef cal_T_abc_hpp
#define cal_T_abc_hpp

#include <stdio.h>
#include <Eigen/Eigen>

using namespace Eigen;

void cal_T_abc(MatrixXd coord ,double density, MatrixXd &T_abc, double v_p, double v_s);

#endif /* cal_T_abc_hpp */


// Copyright University of Warwick 2014
// 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)

// Authors:
// John Back

/*! \file LpcFunctions.hh
    \brief File containing the declaration of the LpcFunctions class
*/

/*! \class LpcFunctions
    \brief Class containing various functions used by other classes
*/

#ifndef LPC_FUNCTIONS_HH
#define LPC_FUNCTIONS_HH

#include <Eigen/Dense>

class LpcFunctions {

public:

    //! Default constructor
    LpcFunctions();

    //! Destructor
    virtual ~LpcFunctions();

    //! Weighted mean (centroid) of the hits using Eigen matrices
    /*!
      \param [in] Xi The matrix of hit positions (row = hit, col = x,y,z)
      \param [in] weights The vector of the energy or charge weights of the hits
      \returns The weighted mean (centroid) of the hits as an Eigen vector.
    */
    Eigen::VectorXd getWeightedMean(const Eigen::MatrixXd& Xi,
				    const Eigen::VectorXd& weights) const;

    //! Non-weighted centroid of the hits using Eigen matrices
    /*!
      \param [in] Xi The matrix of hit positions (row = hit, col = x,y,z)
      \returns The non-weighted centroid of the hits as an Eigen vector.
    */
    Eigen::VectorXd getMean(const Eigen::MatrixXd& Xi) const;

    //! Subtract the Eigen matrix of hit positions with a constant vector offset
    /*!
      \param [in] Xi The matrix of hit positions (row = hit, col = x,y,z)
      \param [in] offset The constant vector position offset
      \returns The updated offset hit positions (row = hit, col = x,y,z)
    */
    Eigen::MatrixXd offsetPositions(const Eigen::MatrixXd& Xi,
				    const Eigen::VectorXd& offset) const;

    //! The kernel function for each hit point
    /*!
      \param [in] hitPoint the data hit point
      \param [in] localPoint the local neighbourhood point u
      \param [in] factor the kernel denominator factor
      \returns the kernel function value
    */
    double kernelFunction(const Eigen::VectorXd& hitPoint,
			  const Eigen::VectorXd& localPoint,
			  double factor) const;

    //! The kernel part used by the kernel function
    /*!
      \param [in] x The 1D hit co-ordinate variable
      \param [in] u The 1D co-ordinate of the local neighbourhood point
      \param [in] factor the kernel denominator factor
    */
    double kernelPart(double x, double u, double factor) const;

    //! A function used by the LpcBranchAlgorithm
    /*!
      \param [in] data the matrix of zero mean data (row = hit, col = x,y,z)
      \param [in] u the local neighbourhood point
      \param [in] factor the kernel denominator factor
    */
    double kdex(const Eigen::MatrixXd& data,
		const Eigen::VectorXd& u, double factor) const;

    //! Covariance matrix from zero mean matrix data and vector of weights
    /*!
      \param [in] meanData the matrix of zero mean data (row = hit, col = x,y,z)
      \param [in] weights the vector of hit weights
      \returns the symmetric convariance matrix of the data
    */
    Eigen::MatrixXd formCovarianceMatrix(const Eigen::MatrixXd& meanData,
					 const Eigen::VectorXd& weights) const;

    //! Unweighted ovariance matrix from zero mean matrix data
    /*!
      \param [in] meanData the matrix of zero mean data (row = hit, col = x,y,z)
      \returns the symmetric convariance matrix of the unweighted data
    */
    Eigen::MatrixXd formCovarianceMatrix(const Eigen::MatrixXd& meanData) const;

    //! Covariance matrix from a matrix of data points and vector of weights
    /*!
      \param [in] data the matrix of the data (row = hit, col = x,y,z)
      \param [in] weights the vector of hit weights
      \returns the symmetric convariance matrix of the data
    */
    Eigen::MatrixXd getCovarianceMatrix(const Eigen::MatrixXd& data,
					const Eigen::VectorXd& weights) const;

    //! Covariance matrix from a matrix of data points
    /*!
      \param [in] data the matrix of the data (row = hit, col = x,y,z)
      \returns the symmetric convariance matrix of the data
    */
    Eigen::MatrixXd getCovarianceMatrix(const Eigen::MatrixXd& data) const;

    //! Find the eigenvalues and normalised eigenvectors of the covariance matrix
    /*!
      \param [in] covMatrix The covariance matrix
      \returns a pair of the descending eigenvalues with the normalised eigenvectors (rows)
    */
    std::pair<Eigen::VectorXd, Eigen::MatrixXd>
    findNormEigenVectors(const Eigen::MatrixXd& covMatrix) const;

    //! Calculate the mean and rms of the set of data recursively
    /*!
      \param [in] data The Eigen VectorXd of data points
      \returns the (mean, rms) pair for the set of data
    */
    std::pair<double, double> getMeanAndRms(const Eigen::VectorXd& data) const;

    //! Calculate the perpendicular distance of a point from a line
    /*!
      \param [in] point The point that we want to find the distance for
      \param [in] centroid A point on the line, defined as the centroid
      \param [in] direction The unit direction vector of the line
    */
    double getPerpLineDist(const Eigen::VectorXd& point,
			   const Eigen::VectorXd& centroid,
			   const Eigen::VectorXd& direction) const;

    //! Calculate the perpendicular distance of a point from a line
    //! for points that lie towards the end-point of the line V
    /*!
      \param [in] point The point that we want to find the distance for
      \param [in] centroid A point on the line, defined as the centroid
      \param [in] direction The unit direction vector of the line
      \param [in] dL The distance along the line between the centroid and end-point
    */
    double getPerpLineDist(const Eigen::VectorXd& point,
			   const Eigen::VectorXd& centroid,
			   const Eigen::VectorXd& direction,
			   double dL) const;

protected:

private:

};

#endif


/*
Simulate outbreak of infectious disease.

Infected individuals infect others from an infinite pool. The model keeps track of
who infected whom and when. Infected individuals are initially in a latent phase i.e.
they show no symptoms nor can infect others. The illness then progresses according to
stochastic processes.

Follows the model description in:

Tom Britton and Gianpaolo Scalia Tomba (2018)
Estimation in emerging epidemics: biases and remedies, arXiv:1803.01688v1.
*/

#include <iostream>
#include <iomanip>
#include <math.h>
#include <random>
#include <vector>
#include <cstdlib>
#include <cmath>
#include <chrono>
#include <Eigen/Core>

#include "infectee.hpp"

class Outbreak
{
  public:
    std::vector<Infectee *> infected; // infected individuals (present and past)
    Eigen::MatrixXi counters;         // counts of each infection state per output interval
    std::mt19937_64 prng;             // pseudo random-number generator
    params_struct params;             // user-given parameters (defaults in infectee.hpp)

    Outbreak(std::mt19937_64 &prng, const params_struct &params = params_struct()) : prng(prng), params(params)

    {
        uint n_output = lrint(1. * params.max_time / params.output_interval);
        this->counters = Eigen::MatrixXi::Zero(n_output, N_STATES);

        std::vector<Infectee *> new_infected, new_infected1;
        this->infected.push_back(new Infectee(NULL, 0, prng, params));
        uint output_counter = 0;

        double time = params.timestep;
        while (time <= params.max_time)
        {
            bool is_output_step = std::fmod(time + 1e-9, params.output_interval) < params.timestep;

            // iterate over all infected individuals
            for (std::vector<Infectee *>::iterator it = this->infected.begin(); it != this->infected.end(); ++it)
            {
                new_infected1 = (*it)->update(time, prng, params);

                if (!new_infected1.empty()) // append new infectees by single infector
                {
                    new_infected.reserve(new_infected.size() + new_infected1.size());
                    new_infected.insert(new_infected.end(), new_infected1.begin(), new_infected1.end());
                }

                if (is_output_step)
                    this->counters(output_counter, (*it)->istatus())++;
            }

            if (!new_infected.empty()) // append all new infectees from time step
            {
                this->infected.reserve(this->infected.size() + new_infected.size());
                this->infected.insert(this->infected.end(), new_infected.begin(), new_infected.end());
                // std::cout << "t=" << time << ": New infected " << new_infected.size() << ", total " << infected.size() << std::endl;
                new_infected.clear();
            }

            if (is_output_step)
            {
                if (params.verbose)
                    std::cout << "t=" << time << ": " << this->counters.row(output_counter) << std::endl;
                output_counter++;
            }

            if (this->infected.size() > params.max_infected)
            {
                if (params.verbose)
                    std::cout << "Max number of infected individuals reached. Stopping." << std::endl;
                break;
            }
            time += params.timestep;
        }
    }

    ~Outbreak()
    {
        for (std::vector<Infectee *>::iterator it = this->infected.begin(); it != this->infected.end(); ++it)
            delete *it;  // need to release these manually as allocated dynamically
    }

    Eigen::MatrixXi getCounters()
    {
        return this->counters;
    }

    std::vector<Infectee*> getInfected()
    {
        return this->infected;
    }

    float getR0()
    {
        // Estimate the basic reproduction number (R0) by considering
        // reported cases due to infectors now past the infectious period.
        int n_infected = 0;
        int n_infectors = 0;

        for (std::vector<Infectee *>::iterator it = this->infected.begin(); it != this->infected.end(); ++it)
        {
            if ((*it)->istatus() > 3)
            {
                n_infectors++;
                for (std::vector<Infectee *>::iterator it2 = (*it)->infected.begin(); it2 != (*it)->infected.end(); ++it2)
                {
                    if ((*it2)->is_reported())
                        n_infected++;
                }
            }
        }
        // std::cout << "N_infected: " << n_infected << " n_infectors: " << n_infectors << std::endl;

        return (float) n_infected / n_infectors;
    }

    // Print various statistics for debugging.
    void printStats()
    {
        const uint N_GROUPS = 4;
        Eigen::ArrayXd end_time_sums = Eigen::ArrayXd::Zero(N_GROUPS);
        Eigen::ArrayXi status_sums = Eigen::ArrayXi::Zero(N_GROUPS);
        double offset;

        for (std::vector<Infectee *>::iterator it = this->infected.begin(); it != this->infected.end(); ++it)
        {
            // handle latent period
            if ((*it)->status_trajectory[1] == 1)
                offset = (*it)->end_times[0];
            else
                offset = (*it)->end_times[2];
            end_time_sums[0] += offset - (*it)->infection_time;
            status_sums[0]++;

            // infectious period
            end_time_sums[1] += (*it)->end_times[3] - offset;
            offset = (*it)->end_times[3];
            status_sums[1]++;

            // recovering period
            if ((*it)->status_trajectory[3] == 4)
            {
                end_time_sums[2] += (*it)->end_times[4] - offset;
                status_sums[2]++;
            }
            else // dying period
            {
                end_time_sums[3] += (*it)->end_times[5] - offset;
                status_sums[3]++;
            }
        }

        std::cout.precision(5);
        std::cout << std::setw(20) << "Means:" << std::setw(20) << "Latent period"
                  << std::setw(20) << "Infectious period" << std::setw(20) << "Recovering period"
                  << std::setw(20) << "Dying period" << std::endl;
        std::cout << std::setw(20) << (end_time_sums / status_sums.cast<double>()).transpose() << std::endl;
        std::cout << std::setw(20) << "Expected:" << std::setw(20) << params.latent_period_scale * params.latent_period_shape
                  << std::setw(20) << params.infect_period_scale * params.infect_period_shape
                  << std::setw(20) << params.recover_period_scale * params.recover_period_shape
                  << std::setw(20) << params.dying_period_scale * params.dying_period_shape << std::endl;
        std::cout << "Pr(recovery): " << (1. * status_sums[2]) / (status_sums[2] + status_sums[3])
                  << " Expected " << params.p_recovery << std::endl;
    }
};

int main(int argc, char *argv[])
{
    params_struct params;
    params.verbose = true;
    uint seed;
    double R0;

    if (argc > 1)
    {
        R0 = std::atof(argv[1]);
    }
    else
    {
        R0 = 1.7;
    }
    if (argc > 2)
    {
        seed = std::atoi(argv[2]);
    }
    else
    {
        seed = static_cast<uint>(std::chrono::system_clock::now().time_since_epoch().count());
        std::cout << "Using seed = " << seed << std::endl;
    }
    std::mt19937_64 prng(seed);
    params.infect_delta = params.infect_period_shape * params.infect_period_scale / R0;

    Outbreak ob(prng, params);

    std::cout << "Estimated R0: " << ob.getR0() << std::endl;

    // Eigen::MatrixXi c = ob.getCounters();
    // std::cout << c << std::endl;

    std::vector<Infectee*> inf = ob.getInfected();
    if (inf.size() > 3)
    {
        std::cout << *(inf[0]) << std::endl;
        std::cout << *(inf[1]) << std::endl;
        std::cout << *(inf[2]) << std::endl;
        std::cout << *(inf[(int) (inf.size()/4)]) << std::endl;
    }

    ob.printStats();
    return 0;
}


/* Boost test/mul.cpp
 * test multiplication, division, square and square root on some intervals
 *
 * Copyright Guillaume Melquiond 2002-2003
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation.
 *
 * None of the above authors nor Polytechnic University make any
 * representation about the suitability of this software for any
 * purpose. It is provided "as is" without express or implied warranty.
 *
 * $Id: mul.cpp,v 1.3 2003/02/05 17:34:36 gmelquio Exp $
 */

#include <boost/numeric/interval.hpp>
#include <boost/numeric/interval/io.hpp>
#include <boost/test/minimal.hpp>

typedef boost::numeric::interval<double> I;

static double min(double a, double b, double c, double d) {
  return std::min(std::min(a, b), std::min(c, d));
}

static double max(double a, double b, double c, double d) {
  return std::max(std::max(a, b), std::max(c, d));
}

static bool test_mul(double al, double au, double bl, double bu) {
  I a(al, au), b(bl, bu);
  I c = a * b;
  return c.lower() == min(al*bl, al*bu, au*bl, au*bu)
      && c.upper() == max(al*bl, al*bu, au*bl, au*bu);
}

static bool test_mul1(double ac, double bl, double bu) {
  I a(ac), b(bl, bu);
  I c = ac * b;
  I d = b * ac;
  I e = a * b;
  return equal(c, d) && equal(d, e);
}

static bool test_div(double al, double au, double bl, double bu) {
  I a(al, au), b(bl, bu);
  I c = a / b;
  return c.lower() == min(al/bl, al/bu, au/bl, au/bu)
      && c.upper() == max(al/bl, al/bu, au/bl, au/bu);
}

static bool test_div1(double al, double au, double bc) {
  I a(al, au), b(bc);
  I c = a / bc;
  I d = a / b;
  return equal(c, d);
}

static bool test_div2(double ac, double bl, double bu) {
  I a(ac), b(bl, bu);
  I c = ac / b;
  I d = a / b;
  return equal(c, d);
}

static bool test_square(double al, double au) {
  I a(al, au);
  I b = square(a);
  I c = a * a;
  return b.upper() == c.upper() &&
         (b.lower() == c.lower() || (c.lower() <= 0 && b.lower() == 0));
}

static bool test_sqrt(double al, double au) {
  I a(al, au);
  I b = square(sqrt(a));
  return subset(abs(a), b);
}

int test_main(int, char*[]) {
  BOOST_CHECK(test_mul(2, 3, 5, 7));
  BOOST_CHECK(test_mul(2, 3, -5, 7));
  BOOST_CHECK(test_mul(2, 3, -7, -5));
  BOOST_CHECK(test_mul(-2, 3, 5, 7));
  BOOST_CHECK(test_mul(-2, 3, -5, 7));
  BOOST_CHECK(test_mul(-2, 3, -7, -5));
  BOOST_CHECK(test_mul(-3, -2, 5, 7));
  BOOST_CHECK(test_mul(-3, -2, -5, 7));
  BOOST_CHECK(test_mul(-3, -2, -7, -5));

  BOOST_CHECK(test_mul1(3, 5, 7));
  BOOST_CHECK(test_mul1(3, -5, 7));
  BOOST_CHECK(test_mul1(3, -7, -5));
  BOOST_CHECK(test_mul1(-3, 5, 7));
  BOOST_CHECK(test_mul1(-3, -5, 7));
  BOOST_CHECK(test_mul1(-3, -7, -5));

  BOOST_CHECK(test_div(30, 42, 2, 3));
  BOOST_CHECK(test_div(30, 42, -3, -2));
  BOOST_CHECK(test_div(-30, 42, 2, 3));
  BOOST_CHECK(test_div(-30, 42, -3, -2));
  BOOST_CHECK(test_div(-42, -30, 2, 3));
  BOOST_CHECK(test_div(-42, -30, -3, -2));

  BOOST_CHECK(test_div1(30, 42, 3));
  BOOST_CHECK(test_div1(30, 42, -3));
  BOOST_CHECK(test_div1(-30, 42, 3));
  BOOST_CHECK(test_div1(-30, 42, -3));
  BOOST_CHECK(test_div1(-42, -30, 3));
  BOOST_CHECK(test_div1(-42, -30, -3));

  BOOST_CHECK(test_div2(30, 2, 3));
  BOOST_CHECK(test_div2(30, -3, -2));
  BOOST_CHECK(test_div2(-30, 2, 3));
  BOOST_CHECK(test_div2(-30, -3, -2));

  BOOST_CHECK(test_square(2, 3));
  BOOST_CHECK(test_square(-2, 3));
  BOOST_CHECK(test_square(-3, 2));

  BOOST_CHECK(test_sqrt(2, 3));
  BOOST_CHECK(test_sqrt(5, 7));
  BOOST_CHECK(test_sqrt(-1, 2));

  return 0;
}


/** @file debug_bvn.cpp
 * @author Mark J. Olah (mjo\@cs.unm DOT edu)
 * @date 2017-2018
 * @brief NormalDist class defintion
 *
 */
#include "PriorHessian/mvn_cdf.h"

#include <cmath>
#include <limits>

#include <armadillo>

#include <boost/math/special_functions/erf.hpp>
#include <boost/math/constants/constants.hpp>

using namespace prior_hessian;

void test_bvn()
{
    const int Nsample=100;
    double ak = 0;
    double ah = 0;
    double r=0.2;
    for(int n=0; n<Nsample; n++) {
        ak += 0.1;
        ah += 0.1;
        double b = bvn_integral(ak,ah,r);
        std::cout<<"ak:"<<ak<<" ah:"<<ah<<" r:"<<r<<" b:"<<b<<"\n";
    }
}

int main()
{
    test_bvn();

}


#include <iostream>
#include <string>
#include <acado_toolkit.hpp>
#include <acado_gnuplot.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

#define pi 3.14159

using namespace std;

static boost::random::mt19937 gen;

int gen_data(float bounds[8],int index, char cost_func, int thread);
float rand_gen(float lim[2]);

int gen_data(float bounds[8], int index, char cost_func, int thread)
{
  std::string states_nm, parameters_nm, control_nm;
  bool flag = false;

  USING_NAMESPACE_ACADO

  DifferentialState        q1,q2,qd1,qd2;   // the differential states
  Control                  tau1,tau2;       // the control input u
  Parameter                T;
  DifferentialEquation     f(0.0,0.5);

  //  -------------------------------------
  OCP ocp( 0.0, 0.5 );

  switch(cost_func){
    case 'a': ocp.minimizeMayerTerm(T*T + tau1*qd1*tau1*qd1 + tau2*qd2*tau2*qd2);
              cout << "cost function = sq_power" << endl;
              break;

    default:  //ocp.minimizeMayerTerm(T*T + tau1*tau1 + tau2*tau2);
              ocp.minimizeLagrangeTerm(tau1*tau1);
              ocp.minimizeLagrangeTerm(tau2*tau2);
              cout << "cost function = sq_torque" << endl;
  }

  f << dot(q1) == qd1;
  f << dot(q2) == qd2;
  f << dot(qd1) == -(48*tau1 - 48*tau2 + 24*qd1*qd1*sin(q2) + 24*qd2*qd2*sin(q2) + 18*qd1*qd1*sin(2*q2) - 72*tau2*cos(q2) + 48*qd1*qd2*sin(q2))/(36*cos(q2)*cos(q2) - 64);
  f << dot(qd2) == (48*tau1 - 240*tau2 + 120*qd1*qd1*sin(q2) + 24*qd2*qd2*sin(q2) + 36*qd1*qd1*sin(2*q2) + 18*qd2*qd2*sin(2*q2) + 72*tau1*cos(q2) - 144*tau2*cos(q2) + 48*qd1*qd2*sin(q2) + 36*qd1*qd2*sin(2*q2))/(18*cos(2*q2) - 46);

  ocp.subjectTo(f);
  ocp.subjectTo(AT_START, q1 == bounds[0] );
  ocp.subjectTo(AT_START, q2 == bounds[1]);
  ocp.subjectTo(AT_START, qd1 == bounds[2]);
  ocp.subjectTo(AT_START, qd2 == bounds[3]);

  ocp.subjectTo(AT_END, q1 == bounds[4]);
  ocp.subjectTo(AT_END, q2 == bounds[5]);
  ocp.subjectTo(AT_END, qd1 == bounds[6]);
  ocp.subjectTo(AT_END, qd2 == bounds[7]);

  // ocp.subjectTo(0.0 <= q1 <= 2*pi);
  // ocp.subjectTo(0.0 <= q2 <= 2*pi);
  ocp.subjectTo(-400 <= tau1 <= 400);  // bounds on the control input u,
  ocp.subjectTo(-400 <= tau2 <= 400);

  //  -------------------------------------

  OptimizationAlgorithm algorithm(ocp);     // the optimization algorithm
  algorithm.set( DISCRETIZATION_TYPE , MULTIPLE_SHOOTING);
  algorithm.set( INTEGRATOR_TYPE , INT_BDF);
  algorithm.set( HESSIAN_APPROXIMATION   , BLOCK_BFGS_UPDATE);
  algorithm.set( KKT_TOLERANCE   , 1e-4);
  algorithm.set( ABSOLUTE_TOLERANCE, 1e-4);
  algorithm.set( INTEGRATOR_TOLERANCE, 1e-4);
  algorithm.set( MAX_NUM_ITERATIONS, 1000);
  algorithm.set( MAX_NUM_INTEGRATOR_STEPS, 10000);

  algorithm.set(PRINT_COPYRIGHT,BT_FALSE);
  algorithm.set(PRINTLEVEL,LOW);
  algorithm.set(PRINT_INTEGRATOR_PROFILE,BT_FALSE);
  algorithm.set(PRINT_SCP_METHOD_PROFILE,BT_FALSE);

  algorithm.solve();                        // solves the problem.

  VariablesGrid grid;
  algorithm.getDifferentialStates(grid);
  DVector final_state(4), req_state(4), diff(4);
  final_state = grid.getLastVector();
  req_state(0)=bounds[4];
  req_state(1)=bounds[5];
  req_state(2)=bounds[6];
  req_state(3)=bounds[7];
  diff = final_state - req_state;

  if (diff.getNorm(VN_L2) < 0.001){
    flag = true;
    states_nm = "states_"+to_string(index)+"_"+to_string(thread)+".txt";
    parameters_nm = "parameters_"+to_string(index)+"_"+to_string(thread)+".txt";
    control_nm = "control_"+to_string(index)+"_"+to_string(thread)+".txt";
    algorithm.getDifferentialStates(states_nm.c_str());
    algorithm.getObjectiveValue(parameters_nm.c_str());
    algorithm.getControls(control_nm.c_str());
  }
  clearAllStaticCounters();
  return flag;
}

float rand_gen(float lim[2]) {
    static boost::random::uniform_real_distribution<> dist(0, 1);
    return lim[0] + dist(gen)*(lim[1] - lim[0]);
}

int main(int argc, const char * argv[]){
  // srand (time(NULL));
  int i;
  float q_lims[2] = {0.0,2*pi};
  float qd_lims[2] = {-30.0,30.0};

  if(argc!=4){
    cout << "Incorrect number of arguments." << endl;
  }
  else{
    int num_iter = atoi(argv[2]);
    gen.seed(time(0) + atoi(argv[3]));
    i = 0;
    while(i<num_iter){
      float bounds[8] = {rand_gen(q_lims),rand_gen(q_lims),rand_gen(qd_lims),rand_gen(qd_lims),rand_gen(q_lims),rand_gen(q_lims),rand_gen(qd_lims),rand_gen(qd_lims)};
      cout << i << endl;
      cout << bounds[0] << " " << bounds[1] << " " << bounds[2] << " " << bounds[3] << " " << bounds[4] << " " << bounds[5] << " " << bounds[6] << " " << bounds[7] << endl;
      bool success = gen_data(bounds,i,*argv[1],atoi(argv[3]));
      if(success){
        i++;
      }
    }
  }
  return 0;
}