/***************************************************************************
/* Javier Juan Albarracin - jajuaal1@ibime.upv.es                         */
/* Universidad Politecnica de Valencia, Spain                             */
/*                                                                        */
/* Copyright (C) 2020 Javier Juan Albarracin                              */
/*                                                                        */
/***************************************************************************
* SVFMM <-> Python data type conversions                                   *
***************************************************************************/

#ifndef PYSVFMM_HPP
#define PYSVFMM_HPP

#define cimg_display 0
#include <CImg.h>
#include <Eigen/Dense>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/eigen.h>
#include <SpatiallyVariantFiniteMixtureModel.hpp>
#include <Distributions/MultivariateNormal.hpp>
#include <Distributions/MultivariateTStudent.hpp>
#include <Distributions/Gamma.hpp>
#include <MarkovRandomFields/MarkovRandomField.hpp>
#include <MarkovRandomFields/FrequentistGaussMarkovRandomField.hpp>
#include <MarkovRandomFields/FrequentistTStudentMarkovRandomField.hpp>
#include <MarkovRandomFields/FrequentistNonLocalMarkovRandomField.hpp>
#include <MarkovRandomFields/BayesianGaussMarkovRandomField.hpp>
#include <MarkovRandomFields/BayesianTStudentMarkovRandomField.hpp>
#include <MarkovRandomFields/BayesianNonLocalMarkovRandomField.hpp>
#include <PyCImg.hpp>
#include <PySTL.hpp>
#include <cstdint>
#include <stdexcept>
#include <sstream>
#include <string>
#include <omp.h>

namespace py = pybind11;
using namespace cimg_library;
using namespace Eigen;

template<typename T>
using ndarray = py::array_t<T, py::array::c_style | py::array::forcecast>;

template<typename Distribution, int Dimensions>
ndarray<double> muToNumpy(const SpatiallyVariantFiniteMixtureModel<Distribution, Dimensions> *svfmm)
{
    const int K = svfmm->components();
    const int D = svfmm->spectrum();

    ndarray<double> output({K, D});
    py::buffer_info info = output.request();

    double *ptr = (double*) info.ptr;
    for (int i = 0; i < K; ++i)
    {
        for (int j = 0; j < D; ++j)
        {
            ptr[j * K + i] = (*svfmm)[i].mu()(j);
        }
    }

    return output;
}

template<typename Distribution, int Dimensions>
ndarray<double> sigmaToNumpy(const SpatiallyVariantFiniteMixtureModel<Distribution, Dimensions> *svfmm)
{
    const int K = svfmm->components();
    const int D = svfmm->spectrum();

    ndarray<double> output({D, D, K});
    py::buffer_info info = output.request();

    double *ptr = (double*) info.ptr;
    for (int i = 0; i < K; ++i)
    {
        for (int j = 0; j < D; ++j)
        {
            for (int k = 0; k < D; ++k)
            {
                ptr[(i * D * D) + (k * D) + j] = (*svfmm)[i].sigma()(j, k);
            }
        }
    }
    return output;
}

template<int Dimensions>
ndarray<double> nuToNumpy(const SpatiallyVariantFiniteMixtureModel<MultivariateTStudent, Dimensions> *svfmm)
{
    const int K = svfmm->components();

    ndarray<double> output({K});
    py::buffer_info info = output.request();

    double *ptr = (double*) info.ptr;
    for (int i = 0; i < K; ++i)
    {
        ptr[i] = (*svfmm)[i].nu();
    }

    return output;
}

template<int Dimensions>
ndarray<double> kToNumpy(const SpatiallyVariantFiniteMixtureModel<Gamma, Dimensions> *svfmm)
{
    const int K = svfmm->components();

    ndarray<double> output({K});
    py::buffer_info info = output.request();

    double *ptr = (double*) info.ptr;
    for (int i = 0; i < K; ++i)
    {
        ptr[i] = (*svfmm)[i].k();
    }

    return output;
}

template<int Dimensions>
ndarray<double> thetaToNumpy(const SpatiallyVariantFiniteMixtureModel<Gamma, Dimensions> *svfmm)
{
    const int K = svfmm->components();

    ndarray<double> output({K});
    py::buffer_info info = output.request();

    double *ptr = (double*) info.ptr;
    for (int i = 0; i < K; ++i)
    {
        ptr[i] = (*svfmm)[i].theta();
    }

    return output;
}

/***************************** Python SVFMM Base *****************************/

class PySVFMMBase
{
public:
    enum Mode { SEGMENTATION, MIXTURE, COMPLETE };
};

/***************************** Python SVFMM *****************************/

template<typename Distribution, int Dimensions>
class PySVFMM : public PySVFMMBase
{
public:
    static py::dict toPython(const SpatiallyVariantFiniteMixtureModel<Distribution, Dimensions> *svfmm, const CImg<bool> &mask, const PySVFMMBase::Mode mode);
};

template<typename Distribution, int Dimensions>
py::dict PySVFMM<Distribution, Dimensions>::toPython(const SpatiallyVariantFiniteMixtureModel<Distribution, Dimensions> *svfmm, const CImg<bool> &mask, const PySVFMMBase::Mode mode)
{
    std::stringstream s;
    s << "In function " << __PRETTY_FUNCTION__ << " ==> Function must be specialized for each distribution type." << std::endl;
    throw std::runtime_error(s.str());

    return py::dict();
}

/***************************** Template Specialization Multivariate Normal *****************************/

template<int Dimensions>
class PySVFMM<MultivariateNormal, Dimensions> : public PySVFMMBase
{
public:
    static py::dict toPython(const SpatiallyVariantFiniteMixtureModel<MultivariateNormal, Dimensions> *svfmm, const CImg<bool> &mask, const PySVFMMBase::Mode mode);
};

template<int Dimensions>
py::dict PySVFMM<MultivariateNormal, Dimensions>::toPython(const SpatiallyVariantFiniteMixtureModel<MultivariateNormal, Dimensions> *svfmm, const CImg<bool> &mask, const PySVFMMBase::Mode mode)
{
    py::dict output = py::dict();

    if (mode == SEGMENTATION || mode == MIXTURE || mode == COMPLETE)
    {
        output["segmentation"] = PyCImg::toNumpy(EigenCImg::toCImg(svfmm->labels().array() + 1, mask));
    }

    if (mode == MIXTURE || mode == COMPLETE)
    {
        output["mu"] = muToNumpy(svfmm);
        output["sigma"] = sigmaToNumpy(svfmm);
        output["priors"] = PyCImg::toNumpy(EigenCImg::toCImg(svfmm->coefficients(), mask));
        output["posteriors"] = PyCImg::toNumpy(EigenCImg::toCImg(svfmm->posteriorProbabilities(), mask));
        output["loglikelihood"] = PySTL::toNumpy(svfmm->logLikelihoodHistory());

    }

    if (mode == COMPLETE)
    {
        py::dict mrf = py::dict();

        const MarkovRandomFieldBase::Model model = svfmm->spatialCoefficients().get()->model();
        const MarkovRandomFieldBase::Tropism tropism = svfmm->spatialCoefficients().get()->tropism();
        const MarkovRandomFieldBase::Topology topology = svfmm->spatialCoefficients().get()->topology();
        const MarkovRandomFieldBase::Estimation estimation = svfmm->spatialCoefficients().get()->estimation();

        mrf["cliques"] = svfmm->spatialCoefficients().get()->cliques();
        mrf["connectivity"] = svfmm->spatialCoefficients().get()->connectivity();
        mrf["nodes"] = svfmm->spatialCoefficients().get()->nodes();
        mrf["classes"] = svfmm->spatialCoefficients().get()->classes();
        mrf["model"] = model == MarkovRandomFieldBase::GAUSS ? "gaussian" : (MarkovRandomFieldBase::TSTUDENT ? "tstudent" : "nonlocal");
        mrf["tropism"] = tropism == MarkovRandomFieldBase::ISOTROPIC ? "isotropic" : "anisotropic";
        mrf["topology"] = topology == MarkovRandomFieldBase::ORTHOGONAL ? "orthogonal" : "complete";
        mrf["estimation"] = estimation == MarkovRandomFieldBase::FREQUENTIST ? "frequentist" : "bayesian";

        const MarkovRandomField<Dimensions> *ptr_ = svfmm->spatialCoefficients().get();

        // Gauss
        if (model == MarkovRandomFieldBase::GAUSS && tropism == MarkovRandomFieldBase::ISOTROPIC && estimation == MarkovRandomFieldBase::BAYESIAN)
        {
            const BayesianIsotropicGaussMarkovRandomField<Dimensions> *ptr = static_cast<const BayesianIsotropicGaussMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
        }
        if (model == MarkovRandomFieldBase::GAUSS && tropism == MarkovRandomFieldBase::ISOTROPIC && estimation == MarkovRandomFieldBase::FREQUENTIST)
        {
            const FrequentistIsotropicGaussMarkovRandomField<Dimensions> *ptr = static_cast<const FrequentistIsotropicGaussMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
        }
        if (model == MarkovRandomFieldBase::GAUSS && tropism == MarkovRandomFieldBase::ANISOTROPIC && estimation == MarkovRandomFieldBase::BAYESIAN)
        {
            const BayesianAnisotropicGaussMarkovRandomField<Dimensions> *ptr = static_cast<const BayesianAnisotropicGaussMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
        }
        if (model == MarkovRandomFieldBase::GAUSS && tropism == MarkovRandomFieldBase::ANISOTROPIC && estimation == MarkovRandomFieldBase::FREQUENTIST)
        {
            const FrequentistAnisotropicGaussMarkovRandomField<Dimensions> *ptr = static_cast<const FrequentistAnisotropicGaussMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
        }
        // T-student
        if (model == MarkovRandomFieldBase::TSTUDENT && tropism == MarkovRandomFieldBase::ISOTROPIC && estimation == MarkovRandomFieldBase::BAYESIAN)
        {
            const BayesianIsotropicTStudentMarkovRandomField<Dimensions> *ptr = static_cast<const BayesianIsotropicTStudentMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
        }
        if (model == MarkovRandomFieldBase::TSTUDENT && tropism == MarkovRandomFieldBase::ISOTROPIC && estimation == MarkovRandomFieldBase::FREQUENTIST)
        {
            const FrequentistIsotropicTStudentMarkovRandomField<Dimensions> *ptr = static_cast<const FrequentistIsotropicTStudentMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
        }
        if (model == MarkovRandomFieldBase::TSTUDENT && tropism == MarkovRandomFieldBase::ANISOTROPIC && estimation == MarkovRandomFieldBase::BAYESIAN)
        {
            const BayesianAnisotropicTStudentMarkovRandomField<Dimensions> *ptr = static_cast<const BayesianAnisotropicTStudentMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
        }
        if (model == MarkovRandomFieldBase::TSTUDENT && tropism == MarkovRandomFieldBase::ANISOTROPIC && estimation == MarkovRandomFieldBase::FREQUENTIST)
        {
            const FrequentistAnisotropicTStudentMarkovRandomField<Dimensions> *ptr = static_cast<const FrequentistAnisotropicTStudentMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
        }
        // Non-Local
        if (model == MarkovRandomFieldBase::NONLOCAL && tropism == MarkovRandomFieldBase::ISOTROPIC && estimation == MarkovRandomFieldBase::BAYESIAN)
        {
            const BayesianIsotropicNonLocalMarkovRandomField<Dimensions> *ptr = static_cast<const BayesianIsotropicNonLocalMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
            mrf["patch_size"] = ptr->patchSize();
            mrf["chi2_sigma"] = ptr->chi2Sigma();
        }
        if (model == MarkovRandomFieldBase::NONLOCAL && tropism == MarkovRandomFieldBase::ISOTROPIC && estimation == MarkovRandomFieldBase::FREQUENTIST)
        {
            const FrequentistIsotropicNonLocalMarkovRandomField<Dimensions> *ptr = static_cast<const FrequentistIsotropicNonLocalMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
            mrf["patch_size"] = ptr->patchSize();
            mrf["chi2_sigma"] = ptr->chi2Sigma();
        }
        if (model == MarkovRandomFieldBase::NONLOCAL && tropism == MarkovRandomFieldBase::ANISOTROPIC && estimation == MarkovRandomFieldBase::BAYESIAN)
        {
            const BayesianAnisotropicNonLocalMarkovRandomField<Dimensions> *ptr = static_cast<const BayesianAnisotropicNonLocalMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
            mrf["patch_size"] = ptr->patchSize();
            mrf["chi2_sigma"] = ptr->chi2Sigma();
        }
        if (model == MarkovRandomFieldBase::NONLOCAL && tropism == MarkovRandomFieldBase::ANISOTROPIC && estimation == MarkovRandomFieldBase::FREQUENTIST)
        {
            const FrequentistAnisotropicNonLocalMarkovRandomField<Dimensions> *ptr = static_cast<const FrequentistAnisotropicNonLocalMarkovRandomField<Dimensions>*>(ptr_);
            mrf["sigma"] = ptr->sigma();
            mrf["nu"] = ptr->nu();
            mrf["patch_size"] = ptr->patchSize();
            mrf["chi2_sigma"] = ptr->chi2Sigma();
        }

        output["mrf"] = mrf;
    }

    return output;
}

#endif


//
// Created by michel on 16-10-19.
//

#include "test-helper.h"
#include <simple-dsp/butterworth.h>
#include <vector>

#include <boost/test/data/test_case.hpp>
#include <boost/test/unit_test.hpp>

namespace {

using namespace std;
using Coefficients = simpledsp::IIRCoefficients<float>;
using Filter = simpledsp::Butterworth;

struct Scenario {
  static constexpr float epsilon = 1e-4;

  static bool same(float expected, float actual) {
    if (expected == 0) {
      return fabs(actual) <= epsilon;
    }
    return fabs(expected - actual) / fabs(expected) <= epsilon;
  }

  Filter designer;
  Coefficients expectedCoefficients;

  Scenario(const Filter &filter, const Coefficients &expected)
      : designer(filter), expectedCoefficients(expected) {}

  void test() const;
  bool verifyEqual(const char coefficient[2], size_t index, float expected,
                   float actual) const;
};

} // namespace

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wunused-function"
namespace std {
ostream &operator<<(ostream &stream, const Filter &s) {
  stream << "Butterworth{order=" << s.getOrder()
         << "; frequency=" << s.getFrequency()
         << "; sample rate=" << s.getSampleRate()
         << "; type=" << Filter::getTypeName(s.getType()) << "}";
  return stream;
}

ostream &operator<<(ostream &stream, const Scenario &s) {
  stream << "Scenario={" << s.designer << "}";
  return stream;
}
} // namespace std
#pragma clang diagnostic pop

void Scenario::test() const {
  Coefficients actualCoefficients(expectedCoefficients.getOrder());

  simpledsp::IIRCoefficients<float>::Setter setter =
      actualCoefficients.setter();
  designer.generate(setter);
  BOOST_CHECK_MESSAGE(actualCoefficients.getOrder() ==
                          expectedCoefficients.getOrder(),
                      "Order of coefficient not as expected");
  bool same = true;
  for (size_t i = 0; i <= expectedCoefficients.getOrder(); i++) {
    same &= Scenario::same(expectedCoefficients.getC(i),
                           actualCoefficients.getC(i));
    same &= Scenario::same(expectedCoefficients.getD(i),
                           actualCoefficients.getD(i));
  }
  if (!same) {
    BOOST_CHECK(false);
    for (size_t i = 0; i <= expectedCoefficients.getOrder(); i++) {
      if (!Scenario::same(expectedCoefficients.getC(i),
                          actualCoefficients.getC(i))) {
        cout << "\tC[" << i << "] expected " << expectedCoefficients.getC(i)
             << " actual " << actualCoefficients.getC(i) << endl;
      }
      if (!Scenario::same(expectedCoefficients.getD(i),
                          actualCoefficients.getD(i))) {
        cout << "\tD[" << i << "] expected " << expectedCoefficients.getD(i)
             << " actual " << actualCoefficients.getD(i) << endl;
      }
    }
  }
}
bool Scenario::verifyEqual(const char *coefficient, size_t index,
                           float expected, float actual) const {
  bool equal = Scenario::same(expected, actual);
  if (!equal) {
    BOOST_CHECK_EQUAL(expected, actual);
    cout << "\tCoefficient " << coefficient << "[" << index << "]" << endl;
    return false;
  }
  return true;
}

vector<Scenario> &scenarios() {
  static vector<Scenario> result;
  Coefficients coefficients(8u);
  auto setter = coefficients.setter();

  setter.setOrder(1)
      .setX(0, 0.57919)
      .setX(1, 0.57919)
      .setY(0, -1.00000, simpledsp::iir::CoefficientConvention::POSITIVE_Y)
      .setY(1, -0.15838, simpledsp::iir::CoefficientConvention::POSITIVE_Y);
  result.push_back(
      {Filter::relative(Filter::Type::LOW_PASS).setOrder(1).setCenter(0.3),
       coefficients});

  setter.setOrder(1)
      .setX(0, 0.42081)
      .setX(1, -0.42081)
      .setY(0, -1.00000, simpledsp::iir::CoefficientConvention::POSITIVE_Y)
      .setY(1, -0.15838, simpledsp::iir::CoefficientConvention::POSITIVE_Y);
  result.push_back(
      {Filter::relative(Filter::Type::HIGH_PASS).setOrder(1).setCenter(0.3),
       coefficients});

  setter.setOrder(2)
      .setX(0, 0.39134)
      .setX(1, 0.78267)
      .setX(2, 0.39134)
      .setY(0, -1.00000, simpledsp::iir::CoefficientConvention::POSITIVE_Y)
      .setY(1, -0.36953, simpledsp::iir::CoefficientConvention::POSITIVE_Y)
      .setY(2, -0.19582, simpledsp::iir::CoefficientConvention::POSITIVE_Y);
  result.push_back(
      {Filter::relative(Filter::Type::LOW_PASS).setOrder(2).setCenter(0.3),
       coefficients});

  setter.setOrder(2)
      .setX(0, 0.20657)
      .setX(1, -0.41314)
      .setX(2, 0.20657)
      .setY(0, -1.00000, simpledsp::iir::CoefficientConvention::POSITIVE_Y)
      .setY(1, -0.36953, simpledsp::iir::CoefficientConvention::POSITIVE_Y)
      .setY(2, -0.19582, simpledsp::iir::CoefficientConvention::POSITIVE_Y);
  result.push_back(
      {Filter::relative(Filter::Type::HIGH_PASS).setOrder(2).setCenter(0.3),
       coefficients});

  return result;
}

BOOST_AUTO_TEST_SUITE(iirIntegrationTest)

BOOST_DATA_TEST_CASE(sample, scenarios()) { sample.test(); }

BOOST_AUTO_TEST_SUITE_END()


#include <catch2/catch.hpp>
#include <Eigen/Dense>

#include <ssme/utils.h>

#define PREC .0001

TEST_CASE("data_reader_test", "[read_in_data]"){

    // you have to run the tests from within the ssme/tests/ directory otherwise it won't find the file below

    // cannot have header!
    // need to know num cols
    std::vector<Eigen::Matrix<double,2,1>> data = utils::read_data<2,double>("test_data.csv");
    REQUIRE(data.size() == 1);
    REQUIRE( std::abs(1.23 - data[0](0)) <  PREC);
    REQUIRE( std::abs(4.56 - data[0](1))< PREC);
}


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     kld_sample_size_calculator.cpp
 * \author   Collin Johnson
 *
 * Definition of KLDSampleSizeCalculator.
 */

#include "hssh/metrical/localization/kld_sample_size_calculator.h"
#include "core/angle_functions.h"
#include <boost/algorithm/clamp.hpp>
#include <iostream>

// #define DEBUG_KLD

namespace vulcan
{
namespace hssh
{

int calculate_samples_needed(int numFilledBins, float z, float epsilon);

inline bool valid_bin(int binIndex, int numBins)
{
    return (binIndex >= 0) && (binIndex < numBins);
}

KLDSampleSizeCalculator::KLDSampleSizeCalculator(const kld_sampling_params_t& params)
: sampleBins(0)
, calculationID(0)
, params(params)
{
    sampleBins = new char[params.numXBins * params.numYBins * params.numThetaBins];
}

KLDSampleSizeCalculator::~KLDSampleSizeCalculator(void)
{
    delete[] sampleBins;
}

void KLDSampleSizeCalculator::startNewCalculation(const particle_t& meanSample)
{
    centerSample = meanSample;
    numFilledBins = 0;
    samplesNeeded = 0;
    ++calculationID;
}

void KLDSampleSizeCalculator::addSample(const particle_t& sample)
{
    if (addSampleToBins(sample)) {
        ++numFilledBins;
        samplesNeeded = calculate_samples_needed(numFilledBins, params.z, params.epsilon);

#ifdef DEBUG_KLD
        std::cout << "INFO: KLDSampleSizeCalculator: k=" << numFilledBins << " samples=" << samplesNeeded << '\n';
#endif
    }
}

std::size_t KLDSampleSizeCalculator::currentNumberOfSamplesNeeded(void) const
{
    return (samplesNeeded < params.minSamples) ? params.minSamples : samplesNeeded;
}

bool KLDSampleSizeCalculator::haveDrawnEnoughSamples(std::size_t numSamples) const
{
    return (numSamples >= samplesNeeded) && (numSamples > params.minSamples);
}

bool KLDSampleSizeCalculator::addSampleToBins(const particle_t& sample)
{
    int binX = (sample.pose.x - centerSample.pose.x) / params.xBinWidth + params.numXBins / 2;
    int binY = (sample.pose.y - centerSample.pose.y) / params.yBinWidth + params.numYBins / 2;
    int binTheta =
      (angle_diff(sample.pose.theta, centerSample.pose.theta) / params.thetaBinWidth) + params.numThetaBins / 2;

    bool binWasEmpty = false;

    binX = boost::algorithm::clamp(binX, 0, params.numXBins - 1);
    binY = boost::algorithm::clamp(binY, 0, params.numYBins - 1);
    binTheta = boost::algorithm::clamp(binTheta, 0, params.numThetaBins - 1);

    int index = binIndex(binX, binY, binTheta);
    binWasEmpty = sampleBins[index] != calculationID;
    sampleBins[index] = calculationID;

    return binWasEmpty;
}

int calculate_samples_needed(int numFilledBins, float z, float epsilon)
{
    /*
     * KL-distance = (k-1)/(2*epsilon) * {1 - 2/(9(k-1)) + z*sqrt(2/(9(k-1)))}^3
     *
     * k == numFilledBins
     */

    int samplesNeeded = 0;

    if (numFilledBins > 1) {
        float coefficient = (numFilledBins - 1) / (2 * epsilon);
        float insidePow = 1.0f - 2 / (9 * (numFilledBins - 1)) + z * sqrt(2 / (9 * (numFilledBins - 1)));

        samplesNeeded = static_cast<std::size_t>(coefficient * pow(insidePow, 3));
    }

    return samplesNeeded;
}

}   // namespace hssh
}   // namespace vulcan


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

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

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

//[assign_point_to_index
//` Shows how to assign the lower-left or upper-right point from a box

#include <iostream>

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

using namespace boost::geometry;

int main()
{
    typedef model::d2::point_xy<int> point;
    typedef model::box<point> box;

    point lower_left(0, 0), upper_right(2, 2);

    box b;
    assign_point_to_index<0>(lower_left, b);
    assign_point_to_index<1>(upper_right, b);
    std::cout << "box: " << dsv(b) << std::endl;

    return 0;
}

//]

//[assign_point_to_index_output
/*`
Output:
[pre
box: ((0, 0), (2, 2))
]
*/
//]


#include <boost/lexical_cast.hpp>
#include <dai/alldai.h>
#include <iostream>
#include <fstream>
#include <string>
#include "dai/emrun.h"
#include <dai/util.h>
#include <string>
#include <sys/stat.h>
#include <time.h>
#include <boost/tokenizer.hpp>
#include <iomanip>      // std::setprecision
#include <sstream>
#include <string>
#include <cstdlib>
#include<math.h>
using namespace std;
using namespace dai;
using namespace boost;
std::string& trim(std::string& s, const char* t = " \t\n\r\f\v")
{
	s.erase(0, s.find_first_not_of(t));
	s.erase(s.find_last_not_of(t) + 1);
	return s;
}
double euclideanDist(double v1[],double v2[],int length)
{
    double distance=0;
    for(int i=0;i<length;i++)
    {
        distance = distance+ pow((v1[i]-v2[i]),2); //Euclidean distance
        //cout<<v1[i]<<" "<<v2[i]<<" "<<distance;
    }
   // cout<<"**** "<<distance<<endl;
    distance = sqrt(distance);
    return distance;
}
double klDist(double v1[],double v2[],int length)
{
    double distance=0;
    for(int i=0;i<length;i++)
    {
        distance = distance + (v1[i]* log0( v1[i]/v2[i])); // KL distance
       // cout<<( v1[i]/v2[i])<<" "<<(v1[i]* std::log( v1[i]/v2[i]))<<endl;
       // cout<<distance<<"+";
    }
    //cout<<"="<<distance<<endl;
    return distance;
}
void calculateDistance(string child, string child1)
{

        const char* childfgfile = &child[0];
        const char* childfgfile1 = &child1[0];
        //cout<<"child file "<< childfgfile<<endl;
        string line1,line2;
        ifstream p1 (childfgfile);
        ifstream c1 (childfgfile1);
        getline(p1,line1);
        getline(c1,line2);

        int factor =0;
        int no_factors = 0;
        int* factor_names;
        int factor_pos=0;
        int* states_arr;
        int no_states = 1;
        int arr_size =1;
        int factor_states =0;
        int tot_arrays =0;
        int arr_set =0;
        int arr_set_states =0;
        int no_arr_arrset =0;
        int stat =0;
        int p_c = 0;
        double pmut=0,pm=0,distval=0;
        while(getline(p1,line1)&&getline(c1,line2))
        {
            line1 = trim(line1);
            line2 = trim(line2);
            //cout<<p_c<<" "<<pmut<<" "<<pm<<" "<<line1<<endl;
            stringstream ss(line1);
            stringstream ss1(line2);
            if(line1.empty())
            {
                p_c = 0;
                stat =0;
                //cout<<"factor variable "<<factor<<endl;
                //seed();
                //pmut = unifRand(); // generates random a value between 0 and 1 for child1
                //cout<<"Mutation probability "<<pmut<<endl;
                no_states = 1;
                arr_size =1;
                factor++;
            }
            if(pmut>pm)
            {
                //c1<<line1<<endl;
                //cout<<"No mutation"<<endl;
                //p_c++;
                //continue;
            }
            if(pmut<=pm) //do mutation handling the constraint that the states of the random variable sum to 1.
            {
                //cout<<"Mutation happening "<<endl;
                double sumo=0,sumo1=0;
                if(p_c == 0)
                {
                    //c1<<line1<<endl;
                }
                if(p_c == 1)
                {
                    ss>>no_factors;
                    //cout<<"no. of factors "<<no_factors<<endl;
                    //c1<<no_factors<<endl;
                }
                if(p_c == 2)
                {
                    factor_names = new int[no_factors];
                    std::vector<std::string> fields;
                    fields = tokenizeString( line1, true,"  ");
                    for( size_t i = 0; i < fields.size(); ++i )
                    {
                        stringstream n;
                        n << fields[i];
                        string s = n.str();
                        if( s.find_first_not_of(" ")!= std::string::npos)
                        {
                            //c1<<s<<" ";
                            n >> factor_names[i];
                            if(factor_names[i] == (factor-1))
                                factor_pos =i;
                        }
                    }
                    //c1<<endl;
                  //  cout<<"factor position "<<factor_pos<<endl;
                }
                if(p_c == 3)
                {
                    states_arr = new int[no_factors];
                    std::vector<std::string> fields1;
                    fields1 = tokenizeString( line1, true,"  ");
                    for( size_t j = 0; j < fields1.size(); ++j )
                    {
                        stringstream n1;
                        n1 << fields1[j];
                        string state = n1.str();
                        if( state.find_first_not_of(" ")!= std::string::npos)
                        {
                            //c1<<state<<" ";
                            n1 >> states_arr[j];
                            no_states = no_states * states_arr[j];
                            if(j < factor_pos)
                                arr_size = arr_size * states_arr[j];
                            if(j == factor_pos)
                            {
                                factor_states = states_arr[j];
                                arr_set_states = arr_size*states_arr[j];
                            }
                        }
                    }
                    //c1<<endl;
                    tot_arrays = no_states/arr_size;
                    arr_set = no_states/arr_set_states;
                    no_arr_arrset = factor_states;
                    //cout<<"array size "<<arr_size<<endl;
                   // cout<<no_states<<"\t"<<"no. of states "<<no_states<<"\t"<<"total arrays "<<tot_arrays<<"\t"<<"array set "<<arr_set<<"\t"<<"no. arrays "<<no_arr_arrset<<"\t"<<"factor states "<<factor_states<<endl;
                    //c1<<no_states<<endl;
                }
                if(p_c >= 5)
                {
                    int k=0;
                    while(k<arr_set)
                    {
                        std::vector< double * > Arrays, Arrays1;
                        for(int l=0; l<no_arr_arrset; l++)
                        {
                            Arrays.push_back( new double[arr_size]);
                            Arrays1.push_back( new double[arr_size]);
                        }
                        vector<double*>::iterator it2,it21;
                        it2=Arrays.begin();
                        it21=Arrays1.begin();
                        //get the current cpt values and copy them in the array
                        int l=0;
                        while(l<no_arr_arrset)
                        {
                            double* arr = *it2;
                            double* arr1 = *it21;
                           // cout<<"new arr"<<endl;
                            int m=0;
                            while(m<arr_size)
                            {
                                line1 = trim(line1);
                                line2 = trim(line2);
                                //cout<<line1 <<" "<<line2<<endl;
                                std::vector<std::string> values,values1;
//                                values = tokenizeString( line1, true," +");
//                                values1 = tokenizeString( line2, true," +");
                                char_separator<char> sep("                 ");
                                tokenizer<char_separator<char>> tokens(line1, sep);
                                for (const auto& t : tokens) {
                                    //cout<<t;
                                    if(t!=" ")
                                    {
                                        values.push_back(t);
                                        // cout<<"pushed "<<t<<endl;
                                    }
                                }
                                //cout<<endl;
                                tokenizer<char_separator<char>> tokens1(line2, sep);
                                for (const auto& t : tokens1) {
                                    //cout<<t;
                                    if(t!=" ")
                                    {
                                        values1.push_back(t);
                                        //cout<<"pushed "<<t<<endl;
                                    }
                                }
                                int ii=0;
                                //cout<<values.size()<<" "<<values1.size()<<endl;
                                for( size_t j = 0; j < values.size(); ++j )
                                {
                                    stringstream n1,n2;
                                    n1 << values[j];
                                    n2 << values1[j];
                                    string state = n1.str();
                                    string state1 = n2.str();
                                    //cout<<state<<" !! "<<state1<<" !! "<<endl;
                                    if( state.find_first_not_of(" ")!= std::string::npos && state1.find_first_not_of(" ")!= std::string::npos)
                                    {
                                        if(ii==1)
                                        {
                                            n1 >> arr[m];
                                            n2 >> arr1[m];
                                            //cout<<p_c<<" "<<line1<<" "<<"arrval = "<<values[0]<<"\tarr[m]"<<arr[m]<<" m = "<<m<<endl;
                                            m++;
                                            if(m<arr_size)
                                            {
                                             getline(p1,line1);
                                             getline(c1,line2);
                                            }
                                            ii=0;
                                        }
                                        else
                                            ii=1;
                                    } //end of if
                                } // end of for
                            } // end of while -m
                            l++;
                            if(l<no_arr_arrset)
                            {
                                getline(p1,line1);
                                getline(c1,line2);
                            }
                            ++it2; ++it21;
                        } //end of while -l
                       // cout<<"new val"<<endl;
                        vector<double*>::iterator it,it1;
                        double sumarr[arr_size];
                        for(int m=0; m<arr_size; m++)
                        {
                            it=Arrays.begin();
                            it1=Arrays1.begin();
                            double valarr[no_arr_arrset],valarr1[no_arr_arrset];
                            for(int l=0; l<no_arr_arrset; l++) // stdnormal rand values are generated and sum is found
                            {
                                double* arr = *it;
                                double* arr1 = *it1;
                                double val = arr[m];
                                double val1 =  arr1[m];
                                valarr[l]=val;
                                valarr1[l]=val1;
                                cout<<val<<" "<<val1<<endl;
                                ++it; ++it1;
                            }

                            sumo+=euclideanDist(valarr,valarr1,no_arr_arrset);
                            sumo1+=klDist(valarr,valarr1,no_arr_arrset);
                           // cout<<endl;
                        }
                        k++;
                        if(k<arr_set)
                         {
                             getline(p1,line1);
                             getline(c1,line2);
                         }
                    }// end of while -k
                    cout<<"******"<<no_states<<" "<<factor<<" "<<sumo<<" "<<sumo1<<" "<<endl;
                    distval = distval+sumo;
                }// end of if p_c>=5

            } // end of if pmut<p_m
            p_c++;
        } // end of while loop
        c1.close();
    p1.close();
    cout<<distval<<endl;
}
// Type for storing a joint state of all variables
int main(int argc, char *argv[]) {
time_t timer1, timer2;
time(&timer1);
clock_t tStart = clock();
    string child,child1;
//    if(argc.length!=2){
//     child =  "/Applications/Utilities/libDAI-0.3.1/alarmnew_factor_graphs/alarm1l.fg";
//     child1 =  "/Applications/Utilities/libDAI-0.3.1/alarmnew_factor_graphs/alarm2l.fg";
//    }
//    else
    {
        stringstream ss2(argv[1]);
        child = ss2.str();
        stringstream ss3(argv[2]);
        child1 = ss3.str();
    }
    calculateDistance(child,child1);

time(&timer2);
double t = difftime(timer2,timer1);
(clock() - tStart)/CLOCKS_PER_SEC;
double t_c = (clock() - tStart)/CLOCKS_PER_SEC;
//cout<<"Time taken: "<<t<<"s"<<endl;
//cout<<" Processor Time taken: "<<t_c<<"s"<<endl;

    return 0;
}


#pragma once

#include <set>

#include <Eigen/Eigenvalues>

#include "CompressedPauliString.hpp"
#include "Operator.hpp"

#include "../utils.hpp"

namespace yavque
{

namespace detail
{

	bool commute(const std::map<uint32_t, Pauli>& p1,
	             const std::map<uint32_t, Pauli>& p2);
	std::string extract_pauli_string(const std::map<uint32_t, Pauli>& pmap);

	class SumPauliStringImpl
	{
	public:
		using PauliString = std::map<uint32_t, Pauli>;

	private:
		const uint32_t num_qubits_;
		std::vector<PauliString> pauli_strings_;
		mutable std::vector<std::shared_ptr<detail::CompressedPauliString>> cps_;

		void update_cps() const
		{
			for(std::size_t n = cps_.size(); n < pauli_strings_.size(); ++n)
			{
				cps_.emplace_back(CPSFactory::get_instance().get_pauli_string_for(
					extract_pauli_string(pauli_strings_[n])));
			}
		}

	public:
		explicit SumPauliStringImpl(uint32_t num_qubits) : num_qubits_{num_qubits} { }

		explicit SumPauliStringImpl(uint32_t num_qubits,
		                            std::vector<PauliString> pauli_strings)
			: num_qubits_{num_qubits}, pauli_strings_{std::move(pauli_strings)}
		{
			// check sites pauli strings applied < num_qubits
		}

		explicit SumPauliStringImpl(uint32_t num_qubits,
		                            std::vector<PauliString>&& pauli_strings)
			: num_qubits_{num_qubits}, pauli_strings_{std::move(pauli_strings)}
		{
		}

		uint32_t num_qubits() const { return num_qubits_; }

		void add(const std::map<uint32_t, Pauli>& rhs)
		{
			// check sites pauli strings applied < num_qubits
			pauli_strings_.push_back(rhs);
		}

		void add(std::map<uint32_t, Pauli>&& rhs)
		{
			// check sites pauli strings applied < num_qubits
			pauli_strings_.emplace_back(std::move(rhs));
		}

		bool mutually_commuting() const
		{
			for(uint32_t i = 0; i < pauli_strings_.size() - 1; ++i)
			{
				for(uint32_t j = i + 1; j < pauli_strings_.size(); ++j)
				{
					if(!commute(pauli_strings_[i], pauli_strings_[j]))
					{
						return false;
					}
				}
			}
			return true;
		}

		Eigen::VectorXcd apply(const Eigen::VectorXcd& vec) const
		{
			assert(vec.size() == (1U << num_qubits_));

			if(cps_.size() < pauli_strings_.size())
			{
				update_cps();
			}

			Eigen::VectorXcd res = Eigen::VectorXcd::Zero(vec.size());
			for(std::size_t n = 0; n < pauli_strings_.size(); ++n)
			{
				const auto& pauli_string = pauli_strings_[n];
				std::vector<uint32_t> indices;
				std::transform(pauli_string.cbegin(), pauli_string.cend(),
				               std::back_inserter(indices),
				               [](const auto& p) { return p.first; });
				res += cps_[n]->apply(indices, vec);
			}
			return res;
		}

		/* this function makes sense only when operators are mutually commuting
		 *
		 * */
		Eigen::VectorXcd apply_exp(cx_double t, const Eigen::VectorXcd& vec) const
		{
			assert(vec.size() == (1U << num_qubits_));

			if(cps_.size() < pauli_strings_.size())
			{
				update_cps();
			}

			Eigen::VectorXcd res = vec;
			for(std::size_t n = 0; n < pauli_strings_.size(); ++n)
			{
				const auto& pauli_string = pauli_strings_[n];
				std::vector<uint32_t> indices;
				std::transform(pauli_string.cbegin(), pauli_string.cend(),
				               std::back_inserter(indices),
				               [](const auto& p) { return p.first; });
				res = cps_[n]->apply_exp(t, indices, res);
			}
			return res;
		}
	};
} // namespace detail

class SumPauliString final : public Operator
{
public:
	using PauliString = std::map<uint32_t, Pauli>;

private:
	std::shared_ptr<const detail::SumPauliStringImpl> p_;
	cx_double constant_ = 1.0;

	void dagger_in_place_impl() override { constant_ = std::conj(constant_); }

public:
	explicit SumPauliString(const uint32_t num_qubits, const std::string& name = {})
		: Operator(1U << num_qubits, name),
		  p_{std::make_shared<detail::SumPauliStringImpl>(num_qubits)}
	{
	}

	explicit SumPauliString(const uint32_t num_qubits,
	                        const std::vector<std::map<uint32_t, Pauli>>& pauli_strings,
	                        const std::string& name = {})
		: Operator(1U << num_qubits, name),
		  p_{std::make_shared<detail::SumPauliStringImpl>(num_qubits, pauli_strings)}
	{
	}

	explicit SumPauliString(std::shared_ptr<const detail::SumPauliStringImpl> p,
	                        const std::string& name = {}, cx_double constant = 1.0)
		: Operator(1U << p->num_qubits(), name), p_{std::move(p)}, constant_{constant}
	{
	}

	[[nodiscard]] bool mutually_commuting() const { return p_->mutually_commuting(); }

	[[nodiscard]] std::shared_ptr<const detail::SumPauliStringImpl> get_impl() const
	{
		return p_;
	}
	/* This function might be slow.
	 */
	SumPauliString& operator+=(const PauliString& str)
	{
		auto p = std::make_shared<detail::SumPauliStringImpl>(*p_);
		p->add(str);
		p_ = std::move(p);
		return *this;
	}

	SumPauliString& operator+=(PauliString&& str)
	{
		auto p = std::make_shared<detail::SumPauliStringImpl>(*p_);
		p->add(std::move(str));
		p_ = std::move(p);
		return *this;
	}

	SumPauliString(const SumPauliString&) = default;
	SumPauliString(SumPauliString&&) = default;

	SumPauliString& operator=(const SumPauliString&) = delete;
	SumPauliString& operator=(SumPauliString&&) = delete;

	~SumPauliString() override = default;

	[[nodiscard]] std::unique_ptr<Operator> clone() const override
	{
		auto cloned = std::make_unique<SumPauliString>(*this);
		return cloned;
	}

	[[nodiscard]] Eigen::VectorXcd apply_right(const Eigen::VectorXcd& st) const override
	{
		return constant_ * p_->apply(st);
	}
};

} // namespace yavque


/*
    Copyright 2013 Adobe
    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)
*/
/**************************************************************************************************/

#include <adobe/config.hpp>
#include <adobe/utility/ignore_unused.hpp>
#include <boost/test/unit_test.hpp>

namespace adobe {

/*!
\addtogroup testing
@{
*/

template <typename T, typename Op>
void check_transitive(const T& x, const T& y, const T& z, Op op) {
    ignore_unused(y);
    assert(op(x, y) && op(y, z));
    BOOST_CHECK_MESSAGE(op(x, z), "tranisitive");
}

template <typename T, typename Op>
void check_irreflexive(const T& x, Op op) {
    BOOST_CHECK_MESSAGE(!op(x, x), "irreflexive");
}

template <typename T, typename Op>
void check_antisymmetric(const T& x, const T& y, Op op) {
    BOOST_CHECK_MESSAGE(!(op(x, y) && op(y, x)), "anti-symmetric");
}

template <typename T, typename Op>
void check_less_than_comparable(const T& x, const T& y, const T& z, Op op) {
    check_irreflexive(x, op);
    check_antisymmetric(x, y, op);
    check_transitive(x, y, z, op);
}

//!@}
} // namespace adobe


/// My typedefs
#include <inttypes.h>
#include <boost/cstdint.hpp>
#include <boost/integer_traits.hpp>

typedef int32_t    node_t;
typedef int32_t    edge_t;
typedef int64_t    cost_t;

/// From STL library
#include <vector>
using std::vector;

#include <string>

using std::pair;
using std::make_pair;

/// Label for the labeling and/or dijkstra algorithm
enum Label { UNREACHED, LABELED, SCANNED };

/// Data structure to store Key-Value pairs in a
/// PriorityQueue (as a Fibonacci heap)
struct ValueKey {
   cost_t d;
   node_t u;
   ValueKey(cost_t _d, node_t _u)
      : d(_d), u(_u) {}
   /// The relation establishes the order in the PriorityQueue
   inline bool operator<(ValueKey const & rhs) const { return d < rhs.d; }
};

#include <boost/heap/fibonacci_heap.hpp>
typedef boost::heap::fibonacci_heap<ValueKey>  FibonacciHeap;

#include <boost/heap/d_ary_heap.hpp>
typedef boost::heap::d_ary_heap<ValueKey, boost::heap::arity<2>, boost::heap::mutable_<true> >  BinaryHeap;

#include <boost/heap/d_ary_heap.hpp>
typedef boost::heap::d_ary_heap<ValueKey, boost::heap::arity<3>, boost::heap::mutable_<true> >  TernaryHeap;

#include <boost/heap/skew_heap.hpp>
typedef boost::heap::skew_heap<ValueKey, boost::heap::mutable_<true> >  SkewHeap;

#include <boost/heap/pairing_heap.hpp>
typedef boost::heap::pairing_heap<ValueKey>  PairingHeap;

#include <boost/heap/binomial_heap.hpp>
typedef boost::heap::binomial_heap<ValueKey>  BinomialHeap;

/// Simple Arc class: store tuple (i,j,c)
class Arc {
   public:
      node_t    w;  /// Target node
      cost_t    c;  /// Cost of the arc
      /// Standard constructor
      Arc ( node_t _w, cost_t _c )
         : w(_w), c(_c) {}
};

/// Forward and Backward star: intrusive list
typedef std::vector<Arc>                 FSArcList;
typedef FSArcList::iterator              FSArcIter;

///--------------------------------------------------------------------------------
/// Class of graph to compute RCSP with superadditive cost
class Digraph {
   private:
      node_t  n;
      edge_t  m;

      vector<FSArcList>  Nc;   /// Nodes container

      /// Initialize distance vector with Infinity
      /// Maybe it is better to intialize with an upper bound on the optimal path (optimal rcsp path)
      const cost_t Inf;

   public:
      ///Standard constructor
      Digraph( node_t _n, edge_t _m )
         : n(_n), m(_m), Inf(std::numeric_limits<cost_t>::max())
      {
         assert( n < Inf && m < Inf );
         Nc.reserve(n);
         /// Reserve memory for the set of arcs
         int avg_degree = m/n+1;
         for ( int i = 0; i < n; ++i ) {
            FSArcList tmp;
            tmp.reserve(avg_degree);
            Nc.push_back(tmp);
         }
      }

      void addArc( node_t i, node_t j, cost_t c ) {
         Nc[i].push_back( Arc(j, c) );
      }

      ///--------------------------------------------------
      /// Shortest Path for a graph with positive weights
      /// With a Fibonacci Heap, as given in the book "Algorithms" by Vazirani et all.
      /// NOTE: since 'increase' is O(1), while 'decrease' is O(log n)
      /// We use negative distances in the heap, i.e., we start with distance labels set to -\infinity
      /// However, the distance labels are kept with the correct value
      template <typename PriorityQueue>
      cost_t spp ( node_t S, node_t T, vector<node_t>& P ) {
         typedef typename PriorityQueue::handle_type     handle_t;

         PriorityQueue     H;
         vector<handle_t>  K(n);
         vector<Label>     Q(n,UNREACHED);  /// true if it is in the heap

         /// Initialize the source distance
         //D[S] = 0;
         K[S] = H.push( ValueKey(0,S) );
         while ( !H.empty() ) {
            /// u = deleteMin(H)
            ValueKey p = H.top();
            H.pop();
            node_t u  = p.u;
            Q[u] = SCANNED;
            cost_t Du = -(*K[u]).d;
            if ( u == T ) { break; }
            /// for all edges (u, v) \in E
            for ( FSArcIter it = Nc[u].begin(), it_end = Nc[u].end(); it != it_end; ++it ) {
               node_t v   = it->w;
               if ( Q[v] != SCANNED ) {
                  cost_t Duv = it->c;
                  cost_t Dv  = Du + Duv;
                  if ( Q[v] == UNREACHED ) {
                     P[v] = u;
                     Q[v] = LABELED;
                     K[v] = H.push( ValueKey(-Dv,v) );
                  } else {
                     if ( -(*K[v]).d > Dv ) {
                        P[v] = u;
                        H.increase( K[v], ValueKey(-Dv,v) );
                     }
                  }
               }
            }
         }
         assert( R[T] == SCANNED );
         return -(*K[T]).d;
      }
};

/// Boost Timer
#include <boost/progress.hpp>
using boost::timer;

#include <fstream>

using namespace boost;

/// Read input data, build graph, and run Dijkstra
cost_t runDijkstra( char* argv[] ) {
   /// Read instance from the OR-lib
   std::ifstream infile(argv[1]);
   if (!infile)
      exit ( EXIT_FAILURE );

   int n;     /// Number of variables
   int m;     /// Number of constraints

   // reads file of the form
   // #nodes #edges
   // e_1 = v_i v_j cost[e_m]
   // ..
   // e_m = v_i v_j cost[e_m]

   /// Read the first line
   infile >> n >> m;
   fprintf(stdout,"n %d, m %d\n", n, m);
   /// Build the graph
   Digraph G (n, m);

   int v, w;
   cost_t c;
   for ( int i = 0; i < m; i++ ) {
      infile >> v >> w >> c;
      G.addArc(v-1, w-1, c);
   }

   vector<node_t> P(n);
   cost_t T_dist;

   timer TIMER;
   for ( int i = 0; i < 50; ++i ) {
      double t0 = TIMER.elapsed();
      node_t S = i;
      node_t T = n-1-i;
      T_dist = G.spp<BinaryHeap>(S, T, P);
      fprintf(stdout,"Time %.4f Cost %"PRId64"\n", TIMER.elapsed()-t0, T_dist);
   }
   fprintf(stdout,"Tot %.4f\n", TIMER.elapsed());

   return T_dist;
}

///------------------------------------------------------------------------------------------
/// Main function
int
main (int argc, char **argv)
{
   if ( argc != 2 ) {
      fprintf(stdout, "usage: ./dijkstra <filename>\n");
      exit ( EXIT_FAILURE );
   }
   /// Measure overall time
   timer TIMER;
   /// Invoke the different Dijkstra algorithm implementations
   cost_t T_dist = runDijkstra(argv);
   /// Print basic figures
   fprintf(stdout,"Cost %"PRId64" - Time %.3f\n", T_dist, TIMER.elapsed());

   return 0;
}


/************************************************************
 *
 * Copyright (c) 2021, University of California, Los Angeles
 *
 * Authors: Kenny J. Chen, Brett T. Lopez
 * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu
 *
 ***********************************************************/

/***********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2020, SMRT-AIST
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *************************************************************************/

#ifndef NANO_GICP_LSQ_REGISTRATION_HPP
#define NANO_GICP_LSQ_REGISTRATION_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/registration.h>

namespace nano_gicp {

enum class LSQ_OPTIMIZER_TYPE { GaussNewton, LevenbergMarquardt };

template<typename PointSource, typename PointTarget>
class LsqRegistration : public pcl::Registration<PointSource, PointTarget, float> {
public:
  using Scalar = float;
  using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;

  using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;

  using PointCloudTarget = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;

protected:
  using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::max_iterations_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::final_transformation_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
  using pcl::Registration<PointSource, PointTarget, Scalar>::converged_;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  LsqRegistration();
  virtual ~LsqRegistration();

  void setRotationEpsilon(double eps);
  void setInitialLambdaFactor(double init_lambda_factor);
  void setDebugPrint(bool lm_debug_print);

  const Eigen::Matrix<double, 6, 6>& getFinalHessian() const;

  virtual void swapSourceAndTarget() {}
  virtual void clearSource() {}
  virtual void clearTarget() {}

protected:
  virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;

  bool is_converged(const Eigen::Isometry3d& delta) const;

  virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H = nullptr, Eigen::Matrix<double, 6, 1>* b = nullptr) = 0;
  virtual double compute_error(const Eigen::Isometry3d& trans) = 0;

  bool step_optimize(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta);
  bool step_gn(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta);
  bool step_lm(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta);

protected:
  double rotation_epsilon_;

  LSQ_OPTIMIZER_TYPE lsq_optimizer_type_;
  int lm_max_iterations_;
  double lm_init_lambda_factor_;
  double lm_lambda_;
  bool lm_debug_print_;

  Eigen::Matrix<double, 6, 6> final_hessian_;
};
}  // namespace nano_gicp

#endif


// Copyright 2019 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// clang-format off

//[ guide_axis_growing

#include <boost/histogram.hpp>
#include <cassert>

#include <iostream>

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

  // make a growing regular axis
  // - it grows new bins with its constant bin width until the value is covered
  auto h1 = make_histogram(axis::regular<double,
                                         use_default,
                                         use_default,
                                         axis::option::growth>{2, 0., 1.});
  // nothing special happens here
  h1(0.1);
  h1(0.9);
  // state: [0, 0.5): 1, [0.5, 1.0): 1
  assert(h1.axis().size() == 2);
  assert(h1.axis().bin(0).lower() == 0.0);
  assert(h1.axis().bin(1).upper() == 1.0);

  // value below range: axis grows new bins until value is in range
  h1(-0.3);
  // state: [-0.5, 0.0): 1, [0, 0.5): 1, [0.5, 1.0): 1
  assert(h1.axis().size() == 3);
  assert(h1.axis().bin(0).lower() == -0.5);
  assert(h1.axis().bin(2).upper() == 1.0);

  h1(1.9);
  // state: [-0.5, 0.0): 1, [0, 0.5): 1, [0.5, 1.0): 1, [1.0, 1.5): 0 [1.5, 2.0): 1
  assert(h1.axis().size() == 5);
  assert(h1.axis().bin(0).lower() == -0.5);
  assert(h1.axis().bin(4).upper() == 2.0);

  // make a growing category axis (here strings)
  // - empty axis is allowed: very useful if categories are not known at the beginning
  auto h2 = make_histogram(axis::category<std::string,
                                          use_default,
                                          axis::option::growth>());
  assert(h2.size() == 0); // histogram is empty
  h2("foo"); // new bin foo, index 0
  assert(h2.size() == 1);
  h2("bar"); // new bin bar, index 1
  assert(h2.size() == 2);
  h2("foo");
  assert(h2.size() == 2);
  assert(h2[0] == 2);
  assert(h2[1] == 1);
}

//]


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

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

#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/mindenormal.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/pio_4.hpp>

NT2_TEST_CASE_TPL( sinc_real,  NT2_REAL_TYPES)
{
  using nt2::sinc;
  using nt2::tag::sinc_;

  NT2_TEST_TYPE_IS(typename nt2::meta::call<sinc_(T)>::type,T);
  typedef T wished_r_t;

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

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(sinc(nt2::Inf<T>()), nt2::Zero<T>(), 0.5);
  NT2_TEST_ULP_EQUAL(sinc(nt2::Minf<T>()), nt2::Zero<T>(), 0.5);
  NT2_TEST_ULP_EQUAL(sinc(nt2::Nan<T>()), nt2::Nan<T>(), 0.5);
#endif
  NT2_TEST_ULP_EQUAL(sinc(-nt2::Pio_2<T>()), T(2)/(nt2::Pi<T>()), 0.5);
  NT2_TEST_ULP_EQUAL(sinc(-nt2::Pio_4<T>()), nt2::sin(nt2::Pio_4<T>())/(nt2::Pio_4<T>()), 0.5);
  NT2_TEST_ULP_EQUAL(sinc(nt2::Pio_2<T>()),  T(2)/(nt2::Pi<T>()), 0.5);
  NT2_TEST_ULP_EQUAL(sinc(nt2::Pio_4<T>()), nt2::sin(nt2::Pio_4<T>())/(nt2::Pio_4<T>()), 0.5);
  NT2_TEST_ULP_EQUAL(sinc(nt2::Eps<T>()), nt2::One<T>(), 0.5);
  NT2_TEST_ULP_EQUAL(sinc(nt2::Mindenormal<T>()), nt2::One<T>(), 0.5);
  NT2_TEST_ULP_EQUAL(sinc(nt2::Zero<T>()), nt2::One<T>(), 0.5);
}


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#include <ctime>
#include "bayesopt/bayesopt.h"                 // For the C API
#include "bayesopt/bayesopt.hpp"               // For the C++ API
#include <boost/numeric/ublas/assignment.hpp> // <<= op assigment

/* Function to be used for C-API testing */
double testFunction(unsigned int n, const double *x,
		    double *gradient, /* NULL if not needed */
		    void *func_data)
{
  double f = 10.;
  for (unsigned int i = 0; i < n; ++i)
    {
      f += (x[i] - .53) * (x[i] - .53);
    }
  return f;
}

/* Class to be used for C++-API testing */
class ExampleQuadratic: public bayesopt::ContinuousModel
{
 public:

  ExampleQuadratic(size_t dim,bayesopt::Parameters param):
    ContinuousModel(dim,param) {}

  double evaluateSample( const vectord &Xi )
  {
    double x[100];
    for (size_t i = 0; i < Xi.size(); ++i)
      {
	x[i] = Xi(i);
      }
    return testFunction(Xi.size(),x,NULL,NULL);
  };

  bool checkReachability( const vectord &query )
  { return true; };

};

int main(int nargs, char *args[])
{
  int n = 10;                   // Number of dimensions
  clock_t start, end;
  double diff,diff2;

  // Common configuration
  // See parameters.h for the available options.
  // Some parameters did not need to be changed for default, but we have done it for
  // illustrative purpose.
  bayesopt::Parameters par = initialize_parameters_to_default();

  par.kernel.name = "kSum(kSEISO,kConst)";
  par.kernel.hp_mean <<= 1.0, 1.0;
  par.kernel.hp_std <<= 1.0, 1.0;

  par.mean.name = "mConst";
  par.mean.coef_mean <<= 1.0;
  par.mean.coef_std <<= 1.0;

  par.surr_name = "sStudentTProcessJef";
  par.noise = 1e-10;

  par.sc_type = SC_MAP;
  par.l_type = L_EMPIRICAL;

  par.n_iterations = 100;    // Number of iterations
  par.random_seed = 0;
  par.n_init_samples = 15;
  par.n_iter_relearn = 0;

  /*******************************************/
  std::cout << "Running C++ interface" << std::endl;

  ExampleQuadratic opt(n,par);
  vectord result(n);

  // Run C++ interface
  start = clock();
  opt.optimize(result);
  end = clock();
  diff = (double)(end-start) / (double)CLOCKS_PER_SEC;

  /*******************************************/
  std::cout << "Running C inferface" << std::endl;

  // Prepare C interface
  double low[128], up[128], xmin[128], fmin[128];

  // Lower and upper bounds
  for (int i = 0; i < n; ++i)
    {
      low[i] = 0.;
      up[i] = 1.;
    }

  // Run C interface
  start = clock();
  bayes_optimization(n,&testFunction,NULL,low,up,xmin,fmin,par.generate_bopt_params());
  end = clock();
  diff2 = (double)(end-start) / (double)CLOCKS_PER_SEC;
  /*******************************************/

  // Results
  std::cout << "Final result C++: " << result << std::endl;
  std::cout << "Elapsed time in C++: " << diff << " seconds" << std::endl;

  std::cout << "Final result C: [" << n <<"](" << xmin[0];
  for (int i = 1; i < n; ++i )
    {
      std::cout << "," << xmin[i];
    }
  std::cout << ")" << std::endl;
  std::cout << "Elapsed time in C: " << diff2 << " seconds" << std::endl;

}


#define BOOST_TEST_MODULE Gpufit

#include "Cpufit/cpufit.h"
#include "Gpufit/gpufit.h"
#include "Tests/utils.h"

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

#include <vector>

void generate_input_linear_fit_1d(FitInput & i)
{
	// number fits, points, parameters
	i.n_fits = 1;
	i.n_points = 2;
	i.n_parameters = 2; // LINEAR_1D has two parameters

	// data and weights
	i.data = { 0, 1 };
	i.weights_ = { 1, 1 };

	// model id and estimator id
	i.model_id = LINEAR_1D;
	i.estimator_id = LSE;

	// initial parameters and parameters to fit
	i.initial_parameters = { 0, 0 };
	i.parameters_to_fit = { 1, 1 };

	// tolerance and max_n_iterations
	i.tolerance = 0.001f;
	i.max_n_iterations = 10;

	// user info
	i.user_info_ = { 0.f, 1.f };
}

void generate_input_gauss_fit_1d(FitInput & i)
{
	// number fits, points, parameters
	i.n_fits = 1;
	i.n_points = 5;
	i.n_parameters = 4; // GAUSS_1D has four parameters

	// data and weights
	clean_resize(i.data, i.n_fits * i.n_points);
	std::vector< float > const true_parameters{ { 4.f, 2.f, 0.5f, 1.f } };
	generate_gauss_1d(i.data, true_parameters);
	i.weights_.clear(); // no weights

	// model id and estimator id
	i.model_id = GAUSS_1D;
	i.estimator_id = LSE;

	// initial parameters and parameters to fit
	i.initial_parameters = { 2.f, 1.5f, 0.3f, 0.f };
	i.parameters_to_fit = { 1, 1, 1, 1 };

	// tolerance and max_n_iterations
	i.tolerance = 0.001f;
	i.max_n_iterations = 10;

	// user info
	i.user_info_.clear(); // no user info
}

void generate_input_gauss_fit_2d(FitInput & i)
{
	// number fits, points, parameters
	i.n_fits = 1;
	i.n_points = 25;
	i.n_parameters = 5; // GAUSS_2D has five parameters

	// data and weights
	clean_resize(i.data, i.n_fits * i.n_points);
	std::vector< float > const true_parameters{ { 4.f, 1.8f, 2.2f, 0.5f, 1.f } };
	generate_gauss_2d(i.data, true_parameters);
	i.weights_.clear(); // no weights

	// model id and estimator id
	i.model_id = GAUSS_2D;
	i.estimator_id = LSE;

	// initial parameters and parameters to fit
	i.initial_parameters = { 2.f, 1.8f, 2.2f, 0.4f, 0.f };
	i.parameters_to_fit = { 1, 1, 1, 1, 1 };

	// tolerance and max_n_iterations
	i.tolerance = 0.0001f;
	i.max_n_iterations = 20;

	// user info
	i.user_info_.clear(); // no user info
}

void generate_input_gauss_fit_2d_elliptic(FitInput & i)
{
    // number fits, points, parameters
    i.n_fits = 1;
    std::size_t const size_x = 5;
    i.n_points = size_x * size_x;
    i.n_parameters = 6; // GAUSS_2D_ELLIPTIC has five parameters

    // data and weights
    clean_resize(i.data, i.n_fits * i.n_points);

    float const center_x = (static_cast<float>(size_x) - 1.f) / 2.f;
    std::vector< float > const true_parameters{ { 4.f, center_x, center_x, 0.4f, 0.6f, 1.f} };
    generate_gauss_2d_elliptic(i.data, true_parameters);
    i.weights_.clear(); // no weights

    // model id and estimator id
    i.model_id = GAUSS_2D_ELLIPTIC;
    i.estimator_id = LSE;

    // initial parameters and parameters to fit
    i.initial_parameters = { 2.f, 1.8f, 2.2f, 0.5f, 0.5f, 0.f };
    i.parameters_to_fit = { 1, 1, 1, 1, 1 };

    // tolerance and max_n_iterations
    i.tolerance = 0.001f;
    i.max_n_iterations = 10;

    // user info
    i.user_info_.clear(); // no user info
}

void perform_cpufit_gpufit_and_check(void (*func)(FitInput &))
{
	// generate the data
	FitInput i;
	func(i);

	// sanity checks (we don't want to introduce faulty data)
	BOOST_CHECK(i.sanity_check());

	// reset output variables
	FitOutput gpu, cpu;
	clean_resize(gpu.parameters, i.n_fits * i.n_parameters);
	clean_resize(gpu.states, i.n_fits);
	clean_resize(gpu.chi_squares, i.n_fits);
	clean_resize(gpu.n_iterations, i.n_fits);

	clean_resize(cpu.parameters, i.n_fits * i.n_parameters);
	clean_resize(cpu.states, i.n_fits);
	clean_resize(cpu.chi_squares, i.n_fits);
	clean_resize(cpu.n_iterations, i.n_fits);

	// call to cpufit, store output
	int const cpu_status
		= cpufit
		(
			i.n_fits,
			i.n_points,
			i.data.data(),
			i.weights(),
			i.model_id,
			i.initial_parameters.data(),
			i.tolerance,
			i.max_n_iterations,
			i.parameters_to_fit.data(),
			i.estimator_id,
			i.user_info_size(),
			i.user_info(),
			cpu.parameters.data(),
			cpu.states.data(),
			cpu.chi_squares.data(),
			cpu.n_iterations.data()
		);

	BOOST_CHECK(cpu_status == 0);

	// call to gpufit, store output
	int const gpu_status
		= gpufit
		(
			i.n_fits,
			i.n_points,
			i.data.data(),
			i.weights(),
			i.model_id,
			i.initial_parameters.data(),
			i.tolerance,
			i.max_n_iterations,
			i.parameters_to_fit.data(),
			i.estimator_id,
			i.user_info_size(),
			i.user_info(),
			gpu.parameters.data(),
			gpu.states.data(),
			gpu.chi_squares.data(),
			gpu.n_iterations.data()
		);

	BOOST_CHECK(gpu_status == 0);

	// check both output for equality
	BOOST_CHECK(cpu.states == gpu.states);
	BOOST_CHECK(cpu.n_iterations == gpu.n_iterations);
	BOOST_CHECK(close_or_equal(cpu.parameters, gpu.parameters));
	BOOST_CHECK(close_or_equal(cpu.chi_squares, gpu.chi_squares));

}

BOOST_AUTO_TEST_CASE( Consistency )
{
	BOOST_TEST_MESSAGE( "linear_fit_1d" );
	perform_cpufit_gpufit_and_check(&generate_input_linear_fit_1d);

	BOOST_TEST_MESSAGE( "gauss_fit_1d" );
	perform_cpufit_gpufit_and_check(&generate_input_gauss_fit_1d);

	BOOST_TEST_MESSAGE( "gauss_fit_2d" );
	perform_cpufit_gpufit_and_check(&generate_input_gauss_fit_2d);

    BOOST_TEST_MESSAGE("gauss_fit_2d_elliptic");
    perform_cpufit_gpufit_and_check(&generate_input_gauss_fit_2d_elliptic);

}


#include <NTL/mat_lzz_pE.h>

#include <NTL/new.h>

NTL_START_IMPL

void add(mat_zz_pE& X, const mat_zz_pE& A, const mat_zz_pE& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix add: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         add(X(i,j), A(i,j), B(i,j));
}

void sub(mat_zz_pE& X, const mat_zz_pE& A, const mat_zz_pE& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      LogicError("matrix sub: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         sub(X(i,j), A(i,j), B(i,j));
}

void negate(mat_zz_pE& X, const mat_zz_pE& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         negate(X(i,j), A(i,j));
}

void mul_aux(mat_zz_pE& X, const mat_zz_pE& A, const mat_zz_pE& B)
{
   long n = A.NumRows();
   long l = A.NumCols();
   long m = B.NumCols();

   if (l != B.NumRows())
      LogicError("matrix mul: dimension mismatch");

   X.SetDims(n, m);

   long i, j, k;
   zz_pX acc, tmp;

   for (i = 1; i <= n; i++) {
      for (j = 1; j <= m; j++) {
         clear(acc);
         for(k = 1; k <= l; k++) {
            mul(tmp, rep(A(i,k)), rep(B(k,j)));
            add(acc, acc, tmp);
         }
         conv(X(i,j), acc);
      }
   }
}

void mul(mat_zz_pE& X, const mat_zz_pE& A, const mat_zz_pE& B)
{
   if (&X == &A || &X == &B) {
      mat_zz_pE tmp;
      mul_aux(tmp, A, B);
      X = tmp;
   }
   else
      mul_aux(X, A, B);
}

static
void mul_aux(vec_zz_pE& x, const mat_zz_pE& A, const vec_zz_pE& b)
{
   long n = A.NumRows();
   long l = A.NumCols();

   if (l != b.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(n);

   long i, k;
   zz_pX acc, tmp;

   for (i = 1; i <= n; i++) {
      clear(acc);
      for (k = 1; k <= l; k++) {
         mul(tmp, rep(A(i,k)), rep(b(k)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

void mul(vec_zz_pE& x, const mat_zz_pE& A, const vec_zz_pE& b)
{
   if (&b == &x || A.position1(x) != -1) {
      vec_zz_pE tmp;
      mul_aux(tmp, A, b);
      x = tmp;
   }
   else
      mul_aux(x, A, b);
}

static
void mul_aux(vec_zz_pE& x, const vec_zz_pE& a, const mat_zz_pE& B)
{
   long n = B.NumRows();
   long l = B.NumCols();

   if (n != a.length())
      LogicError("matrix mul: dimension mismatch");

   x.SetLength(l);

   long i, k;
   zz_pX acc, tmp;

   for (i = 1; i <= l; i++) {
      clear(acc);
      for (k = 1; k <= n; k++) {
         mul(tmp, rep(a(k)), rep(B(k,i)));
         add(acc, acc, tmp);
      }
      conv(x(i), acc);
   }
}

void mul(vec_zz_pE& x, const vec_zz_pE& a, const mat_zz_pE& B)
{
   if (&a == &x) {
      vec_zz_pE tmp;
      mul_aux(tmp, a, B);
      x = tmp;
   }
   else
      mul_aux(x, a, B);

}

void ident(mat_zz_pE& X, long n)
{
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            set(X(i, j));
         else
            clear(X(i, j));
}

void determinant(zz_pE& d, const mat_zz_pE& M_in)
{
   long k, n;
   long i, j;
   long pos;
   zz_pX t1, t2;
   zz_pX *x, *y;

   const zz_pXModulus& p = zz_pE::modulus();

   n = M_in.NumRows();

   if (M_in.NumCols() != n)
      LogicError("determinant: nonsquare matrix");

   if (n == 0) {
      set(d);
      return;
   }

   UniqueArray<vec_zz_pX> M_store;
   M_store.SetLength(n);
   vec_zz_pX *M = M_store.get();

   for (i = 0; i < n; i++) {
      M[i].SetLength(n);
      for (j = 0; j < n; j++) {
         M[i][j].rep.SetMaxLength(2*deg(p)-1);
         M[i][j] = rep(M_in[i][j]);
      }
   }

   zz_pX det;
   set(det);

   for (k = 0; k < n; k++) {
      pos = -1;
      for (i = k; i < n; i++) {
         rem(t1, M[i][k], p);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1))
            pos = i;
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], p);

         // make M[k, k] == -1 mod p, and make row k reduced

         InvMod(t1, M[k][k], p);
         negate(t1, t1);
         for (j = k+1; j < n; j++) {
            rem(t2, M[k][j], p);
            MulMod(M[k][j], t2, t1, p);
         }

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            x = M[i].elts() + (k+1);
            y = M[k].elts() + (k+1);

            for (j = k+1; j < n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }
      }
      else {
         clear(d);
         return;
      }
   }

   conv(d, det);
}

long IsIdent(const mat_zz_pE& A, long n)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (!IsOne(A(i, j))) return 0;
         }

   return 1;
}

void transpose(mat_zz_pE& X, const mat_zz_pE& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   long i, j;

   if (&X == & A) {
      if (n == m)
         for (i = 1; i <= n; i++)
            for (j = i+1; j <= n; j++)
               swap(X(i, j), X(j, i));
      else {
         mat_zz_pE tmp;
         tmp.SetDims(m, n);
         for (i = 1; i <= n; i++)
            for (j = 1; j <= m; j++)
               tmp(j, i) = A(i, j);
         X.kill();
         X = tmp;
      }
   }
   else {
      X.SetDims(m, n);
      for (i = 1; i <= n; i++)
         for (j = 1; j <= m; j++)
            X(j, i) = A(i, j);
   }
}

void solve(zz_pE& d, vec_zz_pE& X,
           const mat_zz_pE& A, const vec_zz_pE& b)

{
   long n = A.NumRows();
   if (A.NumCols() != n)
      LogicError("solve: nonsquare matrix");

   if (b.length() != n)
      LogicError("solve: dimension mismatch");

   if (n == 0) {
      set(d);
      X.SetLength(0);
      return;
   }

   long i, j, k, pos;
   zz_pX t1, t2;
   zz_pX *x, *y;

   const zz_pXModulus& p = zz_pE::modulus();

   UniqueArray<vec_zz_pX> M_store;
   M_store.SetLength(n);
   vec_zz_pX *M = M_store.get();

   for (i = 0; i < n; i++) {
      M[i].SetLength(n+1);
      for (j = 0; j < n; j++) {
         M[i][j].rep.SetMaxLength(2*deg(p)-1);
         M[i][j] = rep(A[j][i]);
      }
      M[i][n].rep.SetMaxLength(2*deg(p)-1);
      M[i][n] = rep(b[i]);
   }

   zz_pX det;
   set(det);

   for (k = 0; k < n; k++) {
      pos = -1;
      for (i = k; i < n; i++) {
         rem(t1, M[i][k], p);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], p);

         // make M[k, k] == -1 mod p, and make row k reduced

         InvMod(t1, M[k][k], p);
         negate(t1, t1);
         for (j = k+1; j <= n; j++) {
            rem(t2, M[k][j], p);
            MulMod(M[k][j], t2, t1, p);
         }

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            x = M[i].elts() + (k+1);
            y = M[k].elts() + (k+1);

            for (j = k+1; j <= n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }
      }
      else {
         clear(d);
         return;
      }
   }

   X.SetLength(n);
   for (i = n-1; i >= 0; i--) {
      clear(t1);
      for (j = i+1; j < n; j++) {
         mul(t2, rep(X[j]), M[i][j]);
         add(t1, t1, t2);
      }
      sub(t1, t1, M[i][n]);
      conv(X[i], t1);
   }

   conv(d, det);
}

void inv(zz_pE& d, mat_zz_pE& X, const mat_zz_pE& A)
{
   long n = A.NumRows();
   if (A.NumCols() != n)
      LogicError("inv: nonsquare matrix");

   if (n == 0) {
      set(d);
      X.SetDims(0, 0);
      return;
   }

   long i, j, k, pos;
   zz_pX t1, t2;
   zz_pX *x, *y;

   const zz_pXModulus& p = zz_pE::modulus();

   UniqueArray<vec_zz_pX> M_store;
   M_store.SetLength(n);
   vec_zz_pX *M = M_store.get();

   for (i = 0; i < n; i++) {
      M[i].SetLength(2*n);
      for (j = 0; j < n; j++) {
         M[i][j].rep.SetMaxLength(2*deg(p)-1);
         M[i][j] = rep(A[i][j]);
         M[i][n+j].rep.SetMaxLength(2*deg(p)-1);
         clear(M[i][n+j]);
      }
      set(M[i][n+i]);
   }

   zz_pX det;
   set(det);

   for (k = 0; k < n; k++) {
      pos = -1;
      for (i = k; i < n; i++) {
         rem(t1, M[i][k], p);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         MulMod(det, det, M[k][k], p);

         // make M[k, k] == -1 mod p, and make row k reduced

         InvMod(t1, M[k][k], p);
         negate(t1, t1);
         for (j = k+1; j < 2*n; j++) {
            rem(t2, M[k][j], p);
            MulMod(M[k][j], t2, t1, p);
         }

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];   // this is already reduced

            x = M[i].elts() + (k+1);
            y = M[k].elts() + (k+1);

            for (j = k+1; j < 2*n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }
      }
      else {
         clear(d);
         return;
      }
   }

   X.SetDims(n, n);
   for (k = 0; k < n; k++) {
      for (i = n-1; i >= 0; i--) {
         clear(t1);
         for (j = i+1; j < n; j++) {
            mul(t2, rep(X[j][k]), M[i][j]);
            add(t1, t1, t2);
         }
         sub(t1, t1, M[i][n+k]);
         conv(X[i][k], t1);
      }
   }

   conv(d, det);
}

long gauss(mat_zz_pE& M_in, long w)
{
   long k, l;
   long i, j;
   long pos;
   zz_pX t1, t2, t3;
   zz_pX *x, *y;

   long n = M_in.NumRows();
   long m = M_in.NumCols();

   if (w < 0 || w > m)
      LogicError("gauss: bad args");

   const zz_pXModulus& p = zz_pE::modulus();

   UniqueArray<vec_zz_pX> M_store;
   M_store.SetLength(n);
   vec_zz_pX *M = M_store.get();

   for (i = 0; i < n; i++) {
      M[i].SetLength(m);
      for (j = 0; j < m; j++) {
         M[i][j].rep.SetMaxLength(2*deg(p)-1);
         M[i][j] = rep(M_in[i][j]);
      }
   }

   l = 0;
   for (k = 0; k < w && l < n; k++) {

      pos = -1;
      for (i = l; i < n; i++) {
         rem(t1, M[i][k], p);
         M[i][k] = t1;
         if (pos == -1 && !IsZero(t1)) {
            pos = i;
         }
      }

      if (pos != -1) {
         swap(M[pos], M[l]);

         InvMod(t3, M[l][k], p);
         negate(t3, t3);

         for (j = k+1; j < m; j++) {
            rem(M[l][j], M[l][j], p);
         }

         for (i = l+1; i < n; i++) {
            // M[i] = M[i] + M[l]*M[i,k]*t3

            MulMod(t1, M[i][k], t3, p);

            clear(M[i][k]);

            x = M[i].elts() + (k+1);
            y = M[l].elts() + (k+1);

            for (j = k+1; j < m; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(t2, t2, *x);
               *x = t2;
            }
         }

         l++;
      }
   }

   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         conv(M_in[i][j], M[i][j]);

   return l;
}

long gauss(mat_zz_pE& M)
{
   return gauss(M, M.NumCols());
}

void image(mat_zz_pE& X, const mat_zz_pE& A)
{
   mat_zz_pE M;
   M = A;
   long r = gauss(M);
   M.SetDims(r, M.NumCols());
   X = M;
}

void kernel(mat_zz_pE& X, const mat_zz_pE& A)
{
   long m = A.NumRows();
   long n = A.NumCols();

   mat_zz_pE M;
   long r;

   transpose(M, A);
   r = gauss(M);

   X.SetDims(m-r, m);

   long i, j, k, s;
   zz_pX t1, t2;

   zz_pE T3;

   vec_long D;
   D.SetLength(m);
   for (j = 0; j < m; j++) D[j] = -1;

   vec_zz_pE inverses;
   inverses.SetLength(m);

   j = -1;
   for (i = 0; i < r; i++) {
      do {
         j++;
      } while (IsZero(M[i][j]));

      D[j] = i;
      inv(inverses[j], M[i][j]);
   }

   for (k = 0; k < m-r; k++) {
      vec_zz_pE& v = X[k];
      long pos = 0;
      for (j = m-1; j >= 0; j--) {
         if (D[j] == -1) {
            if (pos == k)
               set(v[j]);
            else
               clear(v[j]);
            pos++;
         }
         else {
            i = D[j];

            clear(t1);

            for (s = j+1; s < m; s++) {
               mul(t2, rep(v[s]), rep(M[i][s]));
               add(t1, t1, t2);
            }

            conv(T3, t1);
            mul(T3, T3, inverses[j]);
            negate(v[j], T3);
         }
      }
   }
}

void mul(mat_zz_pE& X, const mat_zz_pE& A, const zz_pE& b_in)
{
   zz_pE b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void mul(mat_zz_pE& X, const mat_zz_pE& A, const zz_p& b_in)
{
   NTL_zz_pRegister(b);
   b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void mul(mat_zz_pE& X, const mat_zz_pE& A, long b_in)
{
   NTL_zz_pRegister(b);
   b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void diag(mat_zz_pE& X, long n, const zz_pE& d_in)
{
   zz_pE d = d_in;
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            X(i, j) = d;
         else
            clear(X(i, j));
}

long IsDiag(const mat_zz_pE& A, long n, const zz_pE& d)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (A(i, j) != d) return 0;
         }

   return 1;
}

long IsZero(const mat_zz_pE& a)
{
   long n = a.NumRows();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

void clear(mat_zz_pE& x)
{
   long n = x.NumRows();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

mat_zz_pE operator+(const mat_zz_pE& a, const mat_zz_pE& b)
{
   mat_zz_pE res;
   add(res, a, b);
   NTL_OPT_RETURN(mat_zz_pE, res);
}

mat_zz_pE operator*(const mat_zz_pE& a, const mat_zz_pE& b)
{
   mat_zz_pE res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(mat_zz_pE, res);
}

mat_zz_pE operator-(const mat_zz_pE& a, const mat_zz_pE& b)
{
   mat_zz_pE res;
   sub(res, a, b);
   NTL_OPT_RETURN(mat_zz_pE, res);
}

mat_zz_pE operator-(const mat_zz_pE& a)
{
   mat_zz_pE res;
   negate(res, a);
   NTL_OPT_RETURN(mat_zz_pE, res);
}

vec_zz_pE operator*(const mat_zz_pE& a, const vec_zz_pE& b)
{
   vec_zz_pE res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_zz_pE, res);
}

vec_zz_pE operator*(const vec_zz_pE& a, const mat_zz_pE& b)
{
   vec_zz_pE res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_zz_pE, res);
}

void inv(mat_zz_pE& X, const mat_zz_pE& A)
{
   zz_pE d;
   inv(d, X, A);
   if (d == 0) ArithmeticError("inv: non-invertible matrix");
}

void power(mat_zz_pE& X, const mat_zz_pE& A, const ZZ& e)
{
   if (A.NumRows() != A.NumCols()) LogicError("power: non-square matrix");

   if (e == 0) {
      ident(X, A.NumRows());
      return;
   }

   mat_zz_pE T1, T2;
   long i, k;

   k = NumBits(e);
   T1 = A;

   for (i = k-2; i >= 0; i--) {
      sqr(T2, T1);
      if (bit(e, i))
         mul(T1, T2, A);
      else
         T1 = T2;
   }

   if (e < 0)
      inv(X, T1);
   else
      X = T1;
}

NTL_END_IMPL


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

int main()
{
    int nx_ori=10001;
    int nz_ori=1200;
    int nx_new=400;
    int nz_new=600;

    arma::Mat<float> model_ori(nz_ori,nx_ori,fill::zeros);
    arma::Mat<float> model_new(nz_new,nx_new,fill::zeros);

    model_ori.load("./model_max2000.dat",raw_binary);
    model_ori.reshape(nz_ori,nx_ori);

    int nx_beg=5600;
    int nx_end=5999;
    int nz_beg=0;
    int nz_end=599;
    model_new=model_ori(span(nz_beg,nz_end),span(nx_beg,nx_end));

    model_new.save("./model_max2000_nz600_nx400.dat",raw_binary);

    return 0;
}


/*
 [auto_generated]
 boost/numeric/odeint/stepper/modified_midpoint.hpp

 [begin_description]
 Modified midpoint method for the use in Burlish-Stoer stepper.
 [end_description]

 Copyright 2011-2013 Mario Mulansky
 Copyright 2011-2013 Karsten Ahnert
 Copyright 2012 Christoph Koke

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_MODIFIED_MIDPOINT_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_MODIFIED_MIDPOINT_HPP_INCLUDED

#include <vector>

#include <boost/numeric/odeint/stepper/base/explicit_stepper_base.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>
#include <boost/numeric/odeint/util/copy.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template<
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = typename algebra_dispatcher< State >::algebra_type ,
class Operations = typename operations_dispatcher< State >::operations_type ,
class Resizer = initially_resizer
>
#ifndef DOXYGEN_SKIP
class modified_midpoint
: public explicit_stepper_base<
  modified_midpoint< State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
  2 , State , Value , Deriv , Time , Algebra , Operations , Resizer >
#else
class modified_midpoint : public explicit_stepper_base
#endif
{

public :

    typedef explicit_stepper_base<
    modified_midpoint< State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
    2 , State , Value , Deriv , Time , Algebra , Operations , Resizer > stepper_base_type;

    typedef typename stepper_base_type::state_type state_type;
    typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_base_type::value_type value_type;
    typedef typename stepper_base_type::deriv_type deriv_type;
    typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
    typedef typename stepper_base_type::time_type time_type;
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::operations_type operations_type;
    typedef typename stepper_base_type::resizer_type resizer_type;
    typedef typename stepper_base_type::stepper_type stepper_type;

    modified_midpoint( unsigned short steps = 2 , const algebra_type &algebra = algebra_type() )
    : stepper_base_type( algebra ) , m_steps( steps )
    { }

    template< class System , class StateIn , class DerivIn , class StateOut >
    void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
    {
        static const value_type val1 = static_cast< value_type >( 1 );
        static const value_type val05 = static_cast< value_type >( 1 ) / static_cast< value_type >( 2 );

        m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );

        const time_type h = dt /  static_cast<value_type>( m_steps );
        const time_type h2 = static_cast<value_type>(2) * h;

        typename odeint::unwrap_reference< System >::type &sys = system;

        time_type th = t + h;

        // m_x1 = x + h*dxdt
        stepper_base_type::m_algebra.for_each3( m_x1.m_v , in , dxdt ,
                typename operations_type::template scale_sum2< value_type , time_type >( val1 , h ) );

        sys( m_x1.m_v , m_dxdt.m_v , th );

        boost::numeric::odeint::copy( in , m_x0.m_v );

        unsigned short i = 1;
        while( i != m_steps )
        {
            // general step
            //tmp = m_x1; m_x1 = m_x0 + h2*m_dxdt; m_x0 = tmp
            stepper_base_type::m_algebra.for_each3( m_x1.m_v , m_x0.m_v , m_dxdt.m_v ,
                    typename operations_type::template scale_sum_swap2< value_type , time_type >( val1 , h2 ) );
            th += h;
            sys( m_x1.m_v , m_dxdt.m_v , th);
            i++;
        }

        // last step
        // x = 0.5*( m_x0 + m_x1 + h*m_dxdt )
        stepper_base_type::m_algebra.for_each4( out , m_x0.m_v , m_x1.m_v , m_dxdt.m_v ,
                typename operations_type::template scale_sum3< value_type , value_type , time_type >( val05 , val05 , val05*h ) );
    }

    void set_steps( unsigned short steps )
    {   m_steps = steps; }

    unsigned short steps( void ) const
    {   return m_steps; }

    template< class StateIn >
    void adjust_size( const StateIn &x )
    {
        resize_impl( x );
        stepper_base_type::adjust_size( x );
    }

private:

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized( false );
        resized |= adjust_size_by_resizeability( m_x0 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_x1 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
        return resized;
    }

    unsigned short m_steps;

    resizer_type m_resizer;

    wrapped_state_type m_x0;
    wrapped_state_type m_x1;
    wrapped_deriv_type m_dxdt;

};

/* Modified midpoint which stores derivatives and state at dt/2 in some external storage for later usage in dense output calculation
 * This Stepper is for use in Bulirsch Stoer only. It DOES NOT meet any stepper concept.
 */
template<
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = typename algebra_dispatcher< State >::algebra_type ,
class Operations = typename operations_dispatcher< State >::operations_type ,
class Resizer = initially_resizer
>
class modified_midpoint_dense_out
{

public :

    typedef State state_type;
    typedef Value value_type;
    typedef Deriv deriv_type;
    typedef Time time_type;
    typedef Algebra algebra_type;
    typedef Operations operations_type;
    typedef Resizer resizer_type;
    typedef state_wrapper< state_type > wrapped_state_type;
    typedef state_wrapper< deriv_type > wrapped_deriv_type;

    typedef modified_midpoint_dense_out< State , Value , Deriv , Time , Algebra , Operations , Resizer > stepper_type;
    typedef std::vector< wrapped_deriv_type > deriv_table_type;

    modified_midpoint_dense_out( unsigned short steps = 2 , const algebra_type &algebra = algebra_type() )
    : m_algebra( algebra ) , m_steps( steps )
    { }

    /*
     * performs a modified midpoint step with m_steps intermediate points
     * stores approximation for x(t+dt/2) in x_mp and all evaluated function results in derivs
     *
     */

    template< class System , class StateIn , class DerivIn , class StateOut >
    void do_step( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt ,
            state_type &x_mp , deriv_table_type &derivs )
    {

        static const value_type val1 = static_cast< value_type >( 1 );
        static const value_type val05 = static_cast< value_type >( 1 ) / static_cast< value_type >( 2 );

        m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize< StateIn > , detail::ref( *this ) , detail::_1 ) );

        const time_type h = dt / static_cast<value_type>( m_steps );
        const time_type h2 = static_cast<value_type>( 2 ) * h;

        typename odeint::unwrap_reference< System >::type &sys = system;

        time_type th = t + h;

        // m_x1 = x + h*dxdt
        m_algebra.for_each3( m_x1.m_v , in , dxdt ,
                typename operations_type::template scale_sum2< value_type , time_type >( val1 , h ) );

        if( m_steps == 2 )
            // result of first step already gives approximation at the center of the interval
            boost::numeric::odeint::copy( m_x1.m_v , x_mp );

        sys( m_x1.m_v , derivs[0].m_v , th );

        boost::numeric::odeint::copy( in , m_x0.m_v );

        unsigned short i = 1;
        while( i != m_steps )
        {
            // general step
            //tmp = m_x1; m_x1 = m_x0 + h2*m_dxdt; m_x0 = tmp
            m_algebra.for_each3( m_x1.m_v , m_x0.m_v , derivs[i-1].m_v ,
                    typename operations_type::template scale_sum_swap2< value_type , time_type >( val1 , h2 ) );
            if( i == m_steps/2-1 )
                // save approximation at the center of the interval
                boost::numeric::odeint::copy( m_x1.m_v , x_mp );

            th += h;
            sys( m_x1.m_v , derivs[i].m_v , th);
            i++;
        }

        // last step
        // x = 0.5*( m_x0 + m_x1 + h*m_dxdt )
        m_algebra.for_each4( out , m_x0.m_v , m_x1.m_v , derivs[m_steps-1].m_v ,
                typename operations_type::template scale_sum3< value_type , value_type , time_type >( val05 , val05 , val05*h ) );
    }

    void set_steps( unsigned short steps )
    {   m_steps = steps; }

    unsigned short steps( void ) const
    {   return m_steps; }

    template< class StateIn >
    bool resize( const StateIn &x )
    {
        bool resized( false );
        resized |= adjust_size_by_resizeability( m_x0 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_x1 , x , typename is_resizeable<state_type>::type() );
        return resized;
    }

    template< class StateIn >
    void adjust_size( const StateIn &x )
    {
        resize( x );
    }

private:

    algebra_type m_algebra;

    unsigned short m_steps;

    resizer_type m_resizer;

    wrapped_state_type m_x0;
    wrapped_state_type m_x1;

};

/********** DOXYGEN ***********/

/**
 * \class modified_midpoint
 *
 * Implementation of the modified midpoint method with a configurable
 * number of intermediate steps. This class is used by the Bulirsch-Stoer
 * algorithm and is not meant for direct usage.
 */

/**
 * \class modified_midpoint_dense_out
 *
 * Implementation of the modified midpoint method with a configurable
 * number of intermediate steps. This class is used by the dense output
 * Bulirsch-Stoer algorithm and is not meant for direct usage.
 * \note This stepper is for internal use only and does not meet
 * any stepper concept.
 */

}
}
}

#endif // BOOST_NUMERIC_ODEINT_STEPPER_MODIFIED_MIDPOINT_HPP_INCLUDED


#include <iostream>

#include <Eigen/Dense>

#include "crocoddyl/core/utils/timer.hpp"

#include "multicopter_mpc/path.h"
#include "multicopter_mpc/mpc-main.hpp"

#define SMOOTH(s) for (std::size_t _smooth = 0; _smooth < s; ++_smooth)

#define STDDEV(vec) std::sqrt(((vec - vec.mean())).square().sum() / ((double)vec.size() - 1))
#define AVG(vec) (vec.mean())

void applyRandomNoise(Eigen::VectorXd& vector) {
  Eigen::VectorXd r_vector = Eigen::VectorXd::Random(vector.size());

  vector += r_vector * 0.05;

  Eigen::Quaterniond quat;
  quat.vec() = vector.segment(3, 3);
  quat.w() = vector(6);
  quat.normalize();

  vector.segment(3, 3) = quat.vec();
  vector(6) = quat.w();
}

class Simulator {
 public:
  Simulator(const double& dt) {
    dt_ = dt;

    std::string model_description_path;
    std::string model_yaml_path;
    std::string mission_yaml_path;

    model_description_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/iris_description/robots/iris_simple.urdf";
    model_yaml_path = MULTICOPTER_MPC_MULTIROTOR_DIR "/iris.yaml";
    mission_yaml_path = MULTICOPTER_MPC_MISSION_DIR "/passthrough.yaml";

    pinocchio::Model model;
    pinocchio::urdf::buildModel(model_description_path, pinocchio::JointModelFreeFlyer(), model);
    model_ = boost::make_shared<pinocchio::Model>(model);

    mc_params_ = boost::make_shared<multicopter_mpc::MultiCopterBaseParams>();
    mc_params_->fill(model_yaml_path);

    mission_ = boost::make_shared<multicopter_mpc::Mission>(model_->nq + model_->nv);
    mission_->fillWaypoints(mission_yaml_path);

    robot_state_ = boost::make_shared<crocoddyl::StateMultibody>(model_);
    actuation_model_ = boost::make_shared<crocoddyl::ActuationModelMultiCopterBase>(
        robot_state_, mc_params_->n_rotors_, mc_params_->tau_f_);

    boost::shared_ptr<crocoddyl::CostModelSum> costs =
        boost::make_shared<crocoddyl::CostModelSum>(robot_state_, actuation_model_->get_nu());
    dif_model_ =
        boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(robot_state_, actuation_model_, costs);
    int_model_ = boost::make_shared<crocoddyl::IntegratedActionModelRK4>(dif_model_, dt_);
    int_data_ = boost::static_pointer_cast<crocoddyl::IntegratedActionDataRK4>(int_model_->createData());

    state_next_ = robot_state_->zero();
  }

  const Eigen::VectorXd& simulateStep(const Eigen::Ref<const Eigen::VectorXd>& state,
                                      const Eigen::Ref<const Eigen::VectorXd>& control) {
    state_trajectory_.push_back(state);
    control_trajectory_.push_back(control);
    int_model_->calc(int_data_, state, control);
    return int_data_->xnext;
  }

 private:
  boost::shared_ptr<pinocchio::Model> model_;

  boost::shared_ptr<crocoddyl::StateMultibody> robot_state_;
  boost::shared_ptr<crocoddyl::ActuationModelMultiCopterBase> actuation_model_;
  boost::shared_ptr<crocoddyl::DifferentialActionModelFreeFwdDynamics> dif_model_;
  boost::shared_ptr<crocoddyl::IntegratedActionModelRK4> int_model_;
  boost::shared_ptr<crocoddyl::IntegratedActionDataRK4> int_data_;

  boost::shared_ptr<multicopter_mpc::MultiCopterBaseParams> mc_params_;
  boost::shared_ptr<multicopter_mpc::Mission> mission_;

  double dt_;
  Eigen::VectorXd state_next_;
  std::vector<Eigen::VectorXd> state_trajectory_;
  std::vector<Eigen::VectorXd> control_trajectory_;
};