//####### Test module for Breit-Wheeler tables #################################

//Define Module name
 #define BOOST_TEST_MODULE "phys/breit_wheeler/tables"

//Include Boost unit tests library & library for floating point comparison
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>

#include <picsar_qed/physics/breit_wheeler/breit_wheeler_engine_tables.hpp>

#include <vector>
#include <algorithm>
#include <array>

//Tolerance for double precision calculations
const double double_tolerance = 5.0e-5;
const double double_small = 1e-30;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-3;
const float float_small = 1e-20;

using namespace picsar::multi_physics::phys::breit_wheeler;

//Templated tolerance
template <typename T>
T constexpr tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

template <typename T>
T constexpr small()
{
    if(std::is_same<T,float>::value)
        return float_small;
    else
        return double_small;
}

// ------------- Helper functions --------------

const double chi_min = 0.01;
const double chi_max = 1000;
const int how_many = 256;
const int how_many_frac = 256;

template <typename RealType, typename VectorType>
auto get_fake_dndt_table()
{
    const auto params =
        dndt_lookup_table_params<RealType>{
            static_cast<RealType>(chi_min),
            static_cast<RealType>(chi_max), how_many};

    return dndt_lookup_table<RealType, VectorType>{params};
}

template <typename RealType, typename VectorType>
auto get_fake_pair_table()
{
    const auto params =
        pair_prod_lookup_table_params<RealType>{
            static_cast<RealType>(chi_min),
            static_cast<RealType>(chi_max),
            how_many, how_many_frac};

    return pair_prod_lookup_table<RealType, VectorType>{params};
}

// ------------- Tests --------------

// ***Test Breit Wheeler dndt table

template <typename RealType, typename VectorType>
void check_dndt_table()
{
    auto table = get_fake_dndt_table<RealType, VectorType>();
    BOOST_CHECK_EQUAL(table.is_init(),false);

    VectorType coords = table.get_all_coordinates();

    BOOST_CHECK_EQUAL(coords.size(),how_many);

    const RealType log_chi_min = log(chi_min);
    const RealType log_chi_max = log(chi_max);

     for (int i = 0 ; i < static_cast<int>(coords.size()); ++i){
         auto res = coords[i];
         auto expected = static_cast<RealType>(
             exp(log_chi_min + i*(log_chi_max-log_chi_min)/(how_many-1)));
         BOOST_CHECK_SMALL((res-expected)/expected, tolerance<RealType>());
     }

    auto vals = VectorType{coords};

    const RealType alpha = 3.0;

    std::transform(coords.begin(), coords.end(), vals.begin(),
        [=](RealType x){return alpha*x;});

    bool result = table.set_all_vals(vals);
    BOOST_CHECK_EQUAL(result,true);
    BOOST_CHECK_EQUAL(table.is_init(),true);

    auto table_2 = get_fake_dndt_table<RealType, VectorType>();
    BOOST_CHECK_EQUAL(table_2 == table, false);
    BOOST_CHECK_EQUAL(table == table, true);
    BOOST_CHECK_EQUAL(table_2 == table_2, true);

    const RealType xo0 = chi_min*0.1;
    const RealType xo1 = chi_max*10;
    const RealType x0 = chi_min;
    const RealType x1 = (chi_max+chi_min)*0.5642 + chi_min;
    const RealType x2 = chi_max;

    const RealType ye_app_o0 = dndt_approx_left<RealType>(xo0);
    const RealType ye_app_o1 = dndt_approx_right<RealType>(xo1);
    const RealType ye0 = alpha*x0;
    const RealType ye1 = alpha*x1;
    const RealType ye2 = alpha*x2;

    const auto xxs = std::array<RealType, 5>
        {xo0, x0, x1, x2, xo1};

    const auto exp_app = std::array<RealType, 5>
        {ye_app_o0, ye0, ye1, ye2, ye_app_o1};

    const auto is_out = std::array<bool, 5>
        {true, false, false, false, true};

    for(int i = 0 ; i < static_cast<int>(xxs.size()) ; ++i){
        const RealType res = table.interp(xxs[i]);
        bool flag_out = false;
        const RealType res2 = table.interp(xxs[i], &flag_out);
        BOOST_CHECK_EQUAL(flag_out, is_out[i]);
        BOOST_CHECK_EQUAL(res, res2);

        const RealType expect = exp_app[i];

        if(i != 0)
            BOOST_CHECK_SMALL((res-expect)/expect, tolerance<RealType>());
        else
            BOOST_CHECK_SMALL((res-expect), tolerance<RealType>());
    }

    const auto table_view = table.get_view();

    for(int i = 0 ; i < static_cast<int>(xxs.size()) ; ++i){
        BOOST_CHECK_EQUAL(table_view.interp(xxs[i]), table.interp(xxs[i]));
    }
}

BOOST_AUTO_TEST_CASE( picsar_breit_wheeler_dndt_table)
{
    check_dndt_table<double, std::vector<double>>();
    check_dndt_table<float, std::vector<float>>();
}

// *******************************

// ***Test Breit Wheeler dndt table out range approximation

template <typename RealType>
void check_dndt_table_out_approx()
{
    const auto left_chi = std::array<RealType,4>{0.01,0.05,0.1,0.2};
    const auto left_sol = std::array<RealType,4>{
        2.9069621438923337e-117,
        1.2969667695320204e-24,
        4.944416339700773e-13,
        3.0528686912504136e-07};

    for(int i = 0; i < 4; i++){
        const auto left_res = dndt_approx_left(left_chi[i]);
        if(left_sol[i] < small<RealType>()){
            BOOST_CHECK_SMALL(
                left_res,small<RealType>());
        }
        else{
            BOOST_CHECK_SMALL(
                (left_res - left_sol[i])/left_sol[i],tolerance<RealType>());
        }
    }

}

BOOST_AUTO_TEST_CASE( picsar_breit_wheeler_dndt_table_out_approx)
{
    check_dndt_table_out_approx<double>();
    check_dndt_table_out_approx<float>();
}

// *******************************

// ***Test Breit Wheeler dndt table serialization

template <typename RealType, typename VectorType>
void check_dndt_table_serialization()
{
    const auto params =
        dndt_lookup_table_params<RealType>{
            static_cast<RealType>(0.1),static_cast<RealType>(10.0),6};

    auto table = dndt_lookup_table<
        RealType, VectorType>{params, {1.,2.,3.,4.,5.,6.}};

    auto raw_data = table.serialize();
    auto new_table = dndt_lookup_table<
        RealType, VectorType>{raw_data};

    BOOST_CHECK_EQUAL(new_table.is_init(), true);
    BOOST_CHECK_EQUAL(new_table == table, true);
}

BOOST_AUTO_TEST_CASE( picsar_breit_wheeler_dndt_table_serialization)
{
    check_dndt_table_serialization<double, std::vector<double>>();
    check_dndt_table_serialization<float, std::vector<float>>();
}

// *******************************

// ***Test Breit Wheeler pair production table

template <typename RealType, typename VectorType>
void check_pair_production_table()
{
    auto table = get_fake_pair_table<RealType, VectorType>();
    BOOST_CHECK_EQUAL(table.is_init(),false);

    auto coords = table.get_all_coordinates();

    BOOST_CHECK_EQUAL(coords.size(),how_many*how_many_frac);

    const RealType log_chi_min = log(chi_min);
    const RealType log_chi_max = log(chi_max);

     for (int i = 0 ; i < how_many*how_many_frac; ++i){
         auto res_1 = coords[i][0];
         auto res_2 = coords[i][1];
         const auto ii = i/how_many_frac;
         const auto jj = i%how_many_frac;
         auto expected_1 = static_cast<RealType>(
             exp(log_chi_min +ii*(log_chi_max-log_chi_min)/(how_many-1)));
        auto expected_2 = expected_1*static_cast<RealType>(
             0.0 +jj*0.5/(how_many_frac-1));

         BOOST_CHECK_SMALL((res_1-expected_1)/expected_1, tolerance<RealType>());
         if(expected_2 != static_cast<RealType>(0.0))
            BOOST_CHECK_SMALL((res_2-expected_2)/expected_2, tolerance<RealType>());
        else
            BOOST_CHECK_SMALL((res_2-expected_2), tolerance<RealType>());
     }

    auto vals = VectorType(coords.size());

    auto functor = [=](std::array<RealType,2> x){
        return static_cast<RealType>(0.5*pow(2*(x[1]/x[0]), 8.0 + log(x[0])));};

    auto inverse_functor = [=](std::array<RealType,2> x){
            return static_cast<RealType>(0.5*pow(2*x[1], 1.0/(8.0 + log(x[0]))));};

    std::transform(coords.begin(), coords.end(), vals.begin(),functor);

    bool result = table.set_all_vals(vals);
    BOOST_CHECK_EQUAL(result,true);
    BOOST_CHECK_EQUAL(table.is_init(),true);

    auto table_2 = get_fake_pair_table<RealType, VectorType>();
    BOOST_CHECK_EQUAL(table_2 == table, false);
    BOOST_CHECK_EQUAL(table == table, true);
    BOOST_CHECK_EQUAL(table_2 == table_2, true);

    const RealType xo0 = chi_min*0.1;
    const RealType xo1 = chi_max*10;
    const RealType x0 = chi_min;
    const RealType x1 = (chi_max+chi_min)*0.5642 + chi_min;
    const RealType x2 = chi_max;

    const auto xxs = std::array<RealType, 5>
        {xo0, x0, x1, x2, xo1};

    const auto rrs = std::array<RealType, 11>
            {0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 0.99};

    const auto is_out = std::array<bool, 5>
            {true, false, false, false, true};

    for (int i = 0; i < static_cast<int>(xxs.size()); ++i){
        const auto xx = xxs[i];
        for (const auto rr : rrs){
            auto res = table.interp(xx, rr);
            bool flag_out = false;
            const RealType res2 = table.interp(xx, rr, &flag_out);
            BOOST_CHECK_EQUAL(flag_out, is_out[i]);
            BOOST_CHECK_EQUAL(res, res2);

            auto rxx = xx;
            if(rxx < chi_min) rxx = chi_min;
            if(rxx > chi_max) rxx = chi_max;
            auto eff_rr = (rr > 0.5)?(static_cast<RealType>(1.0) - rr):rr;
            auto expected = inverse_functor(std::array<RealType,2>{rxx, eff_rr})*xx;
            if(rr >= 0.5) expected = xx - expected;

            if(expected != static_cast<RealType>(0.0))
                BOOST_CHECK_SMALL((res-expected)/expected, tolerance<RealType>());
            else
                BOOST_CHECK_SMALL(res, small<RealType>());

        }
    }
    const auto table_view = table.get_view();

    const auto ff = std::array<double,4>{0.0, 0.1, 0.5, 0.99};

    for(int i = 0 ; i < static_cast<int>(xxs.size()) ; ++i){
        for (auto f : ff)
            BOOST_CHECK_EQUAL(table_view.interp(xxs[i],f ), table.interp(xxs[i], f));
    }
}

BOOST_AUTO_TEST_CASE( picsar_breit_wheeler_pair_production_table)
{
    check_pair_production_table<double, std::vector<double>>();
    check_pair_production_table<float, std::vector<float>>();
    check_pair_production_table<double, std::vector<double>>();
    check_pair_production_table<float, std::vector<float>>();
}

// *******************************

// ***Test Breit Wheeler pair production table serialization

template <typename RealType, typename VectorType>
void check_pair_production_table_serialization()
{
    const auto params =
        pair_prod_lookup_table_params<RealType>{
            static_cast<RealType>(0.1),static_cast<RealType>(10.0),
            3, 3};

    auto table = pair_prod_lookup_table<
        RealType, VectorType>{params, {1.,2.,3.,4.,5.,6.,7.,8.,9.}};

    auto raw_data = table.serialize();
    auto new_table =
        pair_prod_lookup_table<RealType, VectorType>{raw_data};

    BOOST_CHECK_EQUAL(new_table.is_init(), true);
    BOOST_CHECK_EQUAL(new_table == table, true);
}

BOOST_AUTO_TEST_CASE( picsar_breit_wheeler_pair_production_table_serialization)
{
    check_pair_production_table_serialization<double, std::vector<double>>();
    check_pair_production_table_serialization<float, std::vector<float>>();
}

// *******************************


#include <Eigen/Core>
#include <Eigen/Sparse>
#include <iostream>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <random>
#include <unordered_set>
#include <vector>

template <typename Real> struct SGDMF {
  using SparseMatrix = Eigen::SparseMatrix<Real, Eigen::RowMajor>;
  using DenseMatrix =
      Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
  using DenseVector = Eigen::Matrix<Real, Eigen::Dynamic, 1>;

  using Index = typename DenseMatrix::StorageIndex;
  using Sample = std::tuple<Index, Index, int64_t>;

  inline SGDMF(const SparseMatrix &X, int dim, int random_seed, Real lr,
               Real lambda, Real std, size_t n_negative)
      : X_(X), dim(dim), rng(random_seed), lr(lr), lambda(lambda),
        n_negative(n_negative) {
    X_.makeCompressed();
    P.resize(X_.rows(), dim);
    Q.resize(X_.cols(), dim);
    P_cache.resize(dim);
    Q_cache.resize(dim);
    P_b = DenseVector::Zero(X_.rows());
    Q_b = DenseVector::Zero(X_.cols());
    auto fill_normal = [this, std](DenseMatrix &U) {
      int rows = U.rows();
      int cols = U.cols();
      std::normal_distribution<Real> dist(0, std);
      for (int i = 0; i < rows; i++) {
        for (int j = 0; j < cols; j++) {
          U(i, j) = dist(this->rng);
        }
      }
    };
    fill_normal(P);
    fill_normal(Q);

    size_t dsize = X_.nonZeros() + X_.rows() * n_negative;
    dataset.resize(dsize);
  }

  inline void start_epoch() {
    size_t cursor = 0;
    std::uniform_int_distribution<> dist(0, X_.cols() - 1);
    for (int u = 0; u < X_.rows(); u++) {
      for (typename SparseMatrix::InnerIterator iter(X_, u); iter; ++iter) {
        int j = iter.col();
        Sample q(u, j, 1);
        dataset[cursor++] = std::move(q);
      }
      for (size_t m_ = 0; m_ < n_negative; m_++) {
        dataset[cursor++] = {u, dist(rng), 0};
      }
    }
    if (static_cast<size_t>(X_.nonZeros() + X_.rows() * n_negative) != cursor) {
      throw std::runtime_error("somethong nasty");
    }
    std::shuffle(dataset.begin(), dataset.end(), rng);
  }

  inline Real run_epoch() {
    start_epoch();
    Real mean_loss = 0;
    for (auto &s : dataset) {
      mean_loss += sgd(s);
    }
    return mean_loss / dataset.size();
  }

  inline Real sgd(const Sample &s) {
    const Index &u = std::get<0>(s);
    const Index &i = std::get<1>(s);
    const int64_t &y = std::get<2>(s);
    P_cache.noalias() = P.row(u).transpose();
    Q_cache.noalias() = Q.row(i).transpose();
    Real score = (P_cache.transpose() * Q_cache) + bias + P_b(u) + Q_b(i);
    Real sigma_score;
    Real loss;
    if (score > 0) {
      sigma_score = 1 / (1 + std::exp(-score));
      loss = -std::log(sigma_score) + (1 - y) * score;
    } else {
      Real exp_score = std::exp(score);
      sigma_score = exp_score / (1 + exp_score);
      loss = -y * score + std::log(1 + exp_score);
    }

    Real grad = (y - sigma_score);

    P.row(u).noalias() += lr * (grad * Q_cache - lambda * P_cache).transpose();
    Q.row(i).noalias() += lr * (grad * P_cache - lambda * Q_cache).transpose();
    P_b(u) += lr * (grad - lambda * P_b(u));
    Q_b(i) += lr * (grad - lambda * Q_b(i));
    bias += lr * (grad - lambda * bias);
    return loss;
  }

  SparseMatrix X_;

  Real bias;
  DenseMatrix P, Q;
  DenseVector P_b, Q_b;
  DenseVector P_cache, Q_cache;
  int dim;

  std::vector<Sample> dataset;

private:
  std::mt19937 rng;

  Real lr, lambda;
  size_t n_negative;
};

namespace py = pybind11;
using std::vector;

PYBIND11_MODULE(_sgd_mf, m) {
  using Real = double;
  using MF = SGDMF<Real>;
  py::class_<MF>(m, "_MF")
      .def(py::init<const typename MF::SparseMatrix &, int, int, Real, Real,
                    Real, size_t>())
      .def("step", &MF::run_epoch)
      .def_readonly("dataset", &MF::dataset)
      .def_readwrite("P", &MF::P)
      .def_readwrite("Q", &MF::Q)
      .def_readwrite("P_b", &MF::P_b)
      .def_readwrite("Q_b", &MF::Q_b)
      .def_readwrite("bias", &MF::bias);
}


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

#include "required_defines.hpp"

#include "performance_measure.hpp"

#include <boost/math/special_functions/gamma.hpp>
#include <boost/array.hpp>

#define T double
#include "../test/test_gamma_data.ipp"

template <std::size_t N>
double gamma_evaluate2(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::tgamma(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(gamma_test, "gamma")
{
   double result = gamma_evaluate2(factorials);
   result += gamma_evaluate2(near_1);
   result += gamma_evaluate2(near_2);
   result += gamma_evaluate2(near_0);
   result += gamma_evaluate2(near_m10);
   result += gamma_evaluate2(near_m55);

   consume_result(result);
   set_call_count(
      (sizeof(factorials)
      + sizeof(near_1)
      + sizeof(near_2)
      + sizeof(near_0)
      + sizeof(near_m10)
      + sizeof(near_m55)) / sizeof(factorials[0]));
}

template <std::size_t N>
double lgamma_evaluate2(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::lgamma(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(lgamma_test, "lgamma")
{
   double result = lgamma_evaluate2(factorials);
   result += lgamma_evaluate2(near_1);
   result += lgamma_evaluate2(near_2);
   result += lgamma_evaluate2(near_0);
   result += lgamma_evaluate2(near_m10);
   result += lgamma_evaluate2(near_m55);

   consume_result(result);
   set_call_count(
      (sizeof(factorials)
      + sizeof(near_1)
      + sizeof(near_2)
      + sizeof(near_0)
      + sizeof(near_m10)
      + sizeof(near_m55)) / sizeof(factorials[0]));
}

template <std::size_t N>
double tgamma1pm1_evaluate2(const boost::array<boost::array<T, 2>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::tgamma1pm1(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(gamma1pm1_test, "tgamma1pm1")
{
   double result = tgamma1pm1_evaluate2(gammap1m1_data);

   consume_result(result);
   set_call_count(
      sizeof(gammap1m1_data) / sizeof(gammap1m1_data[0]));
}

#ifdef TEST_CEPHES

extern "C" {

double gamma(double);
double lgam(double);

}

template <std::size_t N>
double gamma_evaluate_cephes(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += gamma(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(gamma_test, "gamma-cephes")
{
   double result = gamma_evaluate_cephes(factorials);
   result += gamma_evaluate_cephes(near_1);
   result += gamma_evaluate_cephes(near_2);
   result += gamma_evaluate_cephes(near_0);
   result += gamma_evaluate_cephes(near_m10);
   result += gamma_evaluate_cephes(near_m55);

   consume_result(result);
   set_call_count(
      (sizeof(factorials)
      + sizeof(near_1)
      + sizeof(near_2)
      + sizeof(near_0)
      + sizeof(near_m10)
      + sizeof(near_m55)) / sizeof(factorials[0]));
}

template <std::size_t N>
double lgamma_evaluate_cephes(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += lgam(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(lgamma_test, "lgamma-cephes")
{
   double result = lgamma_evaluate_cephes(factorials);
   result += lgamma_evaluate_cephes(near_1);
   result += lgamma_evaluate_cephes(near_2);
   result += lgamma_evaluate_cephes(near_0);
   result += lgamma_evaluate_cephes(near_m10);
   result += lgamma_evaluate_cephes(near_m55);

   consume_result(result);
   set_call_count(
      (sizeof(factorials)
      + sizeof(near_1)
      + sizeof(near_2)
      + sizeof(near_0)
      + sizeof(near_m10)
      + sizeof(near_m55)) / sizeof(factorials[0]));
}

#endif

#ifdef TEST_GSL

#include <gsl/gsl_sf_gamma.h>

template <std::size_t N>
double gamma_evaluate_gsl(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += gsl_sf_gamma(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(gamma_test, "gamma-gsl")
{
   double result = gamma_evaluate_gsl(factorials);
   result += gamma_evaluate_gsl(near_1);
   result += gamma_evaluate_gsl(near_2);
   result += gamma_evaluate_gsl(near_0);
   result += gamma_evaluate_gsl(near_m10);
   result += gamma_evaluate_gsl(near_m55);

   consume_result(result);
   set_call_count(
      (sizeof(factorials)
      + sizeof(near_1)
      + sizeof(near_2)
      + sizeof(near_0)
      + sizeof(near_m10)
      + sizeof(near_m55)) / sizeof(factorials[0]));
}

template <std::size_t N>
double lgamma_evaluate_gsl(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += gsl_sf_lngamma(data[i][0]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(lgamma_test, "lgamma-gsl")
{
   double result = lgamma_evaluate_gsl(factorials);
   result += lgamma_evaluate_gsl(near_1);
   result += lgamma_evaluate_gsl(near_2);
   result += lgamma_evaluate_gsl(near_0);
   result += lgamma_evaluate_gsl(near_m10);
   result += lgamma_evaluate_gsl(near_m55);

   consume_result(result);
   set_call_count(
      (sizeof(factorials)
      + sizeof(near_1)
      + sizeof(near_2)
      + sizeof(near_0)
      + sizeof(near_m10)
      + sizeof(near_m55)) / sizeof(factorials[0]));
}

#endif

#ifdef TEST_DCDFLIB
#include <dcdflib.h>

template <std::size_t N>
double gamma_evaluate2_dcd(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
   {
      double x = data[i][0];
      result += gamma_x(&x);
   }
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(gamma_test_dcd, "gamma-dcd")
{
   double result = gamma_evaluate2_dcd(factorials);
   result += gamma_evaluate2_dcd(near_1);
   result += gamma_evaluate2_dcd(near_2);
   result += gamma_evaluate2_dcd(near_0);
   result += gamma_evaluate2_dcd(near_m10);
   result += gamma_evaluate2_dcd(near_m55);

   consume_result(result);
   set_call_count(
      (sizeof(factorials)
      + sizeof(near_1)
      + sizeof(near_2)
      + sizeof(near_0)
      + sizeof(near_m10)
      + sizeof(near_m55)) / sizeof(factorials[0]));
}

template <std::size_t N>
double lgamma_evaluate2_dcd(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
   {
      double x = data[i][0];
      result += gamma_log(&x);
   }
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(lgamma_test_dcd, "lgamma-dcd")
{
   double result = lgamma_evaluate2_dcd(factorials);
   result += lgamma_evaluate2_dcd(near_1);
   result += lgamma_evaluate2_dcd(near_2);
   result += lgamma_evaluate2_dcd(near_0);
   result += lgamma_evaluate2_dcd(near_m10);
   result += lgamma_evaluate2_dcd(near_m55);

   consume_result(result);
   set_call_count(
      (sizeof(factorials)
      + sizeof(near_1)
      + sizeof(near_2)
      + sizeof(near_0)
      + sizeof(near_m10)
      + sizeof(near_m55)) / sizeof(factorials[0]));
}

#endif


/*

MIT License

Copyright (c) 2020, R. Gregor Weiß, Benjamin Ries

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE
*/

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/beta.hpp>

#include "geometry.h"

namespace Geometry {

    double regularized_intersection_volume(const double dist,
                                           const double cut,
                                           const unsigned int n) {
        double nd(static_cast<double>(n));
        double nhalfplusonehalf(0.5 * (nd + 1.0));
        double sin2phi(-1.0 * 0.25 * dist * dist);
        sin2phi /= (cut * cut);
        sin2phi += 1.0;
        double incomplete_beta(boost::math::ibeta<double, double, double>(nhalfplusonehalf, 0.5, sin2phi));

        return incomplete_beta;
    }

}


#include "utils.hpp"

#include "edlib/Basis/Basis1DZ2.hpp"
#include "edlib/Basis/ToOriginalBasis.hpp"
#include "edlib/Hamiltonians/TIXXZ.hpp"
#include "edlib/Op/NodeMV.hpp"

#include "edlib/EDP/ConstructSparseMat.hpp"
#include "edlib/EDP/LocalHamiltonian.hpp"

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Sparse>

#include <unsupported/Eigen/KroneckerProduct>

#include <Spectra/MatOp/SparseSymMatProd.h>
#include <Spectra/SymEigsSolver.h>

#include <catch2/catch.hpp>

#include <algorithm>
#include <cassert>
#include <iostream>
#include <random>

using namespace edlib;

template<typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
twoQubitOp(uint32_t N, uint32_t pos1, uint32_t pos2, // NOLINT
           const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& v1,
           const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& v2)
{
    using namespace Eigen;
    // const uint32_t dim = (1U << N);

    assert(pos1 < pos2);

    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> res(1, 1);
    res(0, 0) = 1.0;

    for(uint32_t i = 0; i < pos1; i++)
    {
        res = Eigen::kroneckerProduct(MatrixXd::Identity(2, 2), res).eval();
    }
    res = Eigen::kroneckerProduct(v1, res).eval();
    for(uint32_t i = pos1 + 1; i < pos2; i++)
    {
        res = Eigen::kroneckerProduct(MatrixXd::Identity(2, 2), res).eval();
    }
    res = Eigen::kroneckerProduct(v2, res).eval();
    for(uint32_t i = pos2 + 1; i < N; i++)
    {
        res = Eigen::kroneckerProduct(MatrixXd::Identity(2, 2), res).eval();
    }
    return res;
}

template<typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
singleQubitOp(int N, int pos, const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& v) // NOLINT
{
    using namespace Eigen;
    // const uint32_t dim = (1u << N);

    using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
    MatrixT res(1, 1);
    res(0, 0) = 1.0;

    for(int i = 0; i < pos; i++)
    {
        res = Eigen::kroneckerProduct(MatrixT::Identity(2, 2), res).eval();
    }

    res = Eigen::kroneckerProduct(v, res).eval();
    for(int i = pos + 1; i < N; i++)
    {
        res = Eigen::kroneckerProduct(MatrixT::Identity(2, 2), res).eval();
    }
    return res;
}

template<uint32_t N> class CompareXXZ
{
private:
    double delta_;
    Basis1DZ2<uint32_t> basis_;

    Eigen::MatrixXd hamFull_;
    double gsEnergy_;
    Eigen::VectorXd gsVec_;

public:
    constexpr static int k = (N / 2) * ((N / 2) % 2);
    constexpr static int parity = 1 - 2 * static_cast<int>((N / 2) % 2);

    explicit CompareXXZ(double delta) : delta_{delta}, basis_{N, k, parity, true}
    {
        using namespace Eigen;
        static_assert(N % 2 == 0, "N must be even");

        edp::LocalHamiltonian<double> lh(N, 2);
        for(uint32_t i = 0; i < N; i++)
        {
            lh.addTwoSiteTerm({i, (i + 1) % N}, getSXXYY() + delta_ * getSZZ());
        }
        hamFull_ = MatrixXd(edp::constructSparseMat<double>(1U << N, lh));
        SelfAdjointEigenSolver<MatrixXd> es;
        es.compute(hamFull_);

        gsEnergy_ = es.eigenvalues()[0];
        gsVec_ = es.eigenvectors().col(0);
    }

    void Test()
    {
        using namespace Eigen;
        using Spectra::CompInfo;
        using Spectra::SortRule;
        constexpr size_t max_iter = 1000;
        constexpr double tol = 1e-10;

        TIXXZ<uint32_t> ham(basis_, 1.0, delta_);
        const int dim = basis_.getDim();

        NodeMV mv(dim, 0, dim, ham);

        Spectra::SymEigsSolver<NodeMV> eigs(mv, 2, 6);
        eigs.init();
        eigs.compute(SortRule::SmallestAlge, max_iter, tol, SortRule::SmallestAlge);
        if(eigs.info() != CompInfo::Successful)
        {
            REQUIRE(false);
        }
        const double gsEnergy1 = eigs.eigenvalues()[0];

        REQUIRE(gsEnergy_ == Approx(gsEnergy1).margin(1e-4));

        const VectorXd subspaceGs = eigs.eigenvectors().col(0);
        const VectorXd gsVec1 = [&]() -> VectorXd {
            auto v = toOriginalVectorLM(basis_, subspaceGs.data());
            return Map<VectorXd>(v.data(), 1U << N);
        }();

        const double gsEnergy2
            = double(gsVec1.transpose() * hamFull_ * gsVec1) / double(gsVec1.transpose() * gsVec1);

        REQUIRE(gsEnergy1 == Approx(gsEnergy2).margin(1e-6));
        REQUIRE(std::abs(gsVec_.transpose() * gsVec1) == Approx(1.0).margin(1e-6));
    }
};

TEST_CASE("Compare GS of XXZ using LocalHamiltonian and TIBasis", "[XXZGS]")
{
    SECTION("TIBasis Z2 XXZ N=8")
    {
        CompareXXZ<8> test(1.0);
        test.Test();
    }
    SECTION("TIBasis Z2 XXZ N=10")
    {
        CompareXXZ<10> test(1.0);
        test.Test();
    }
    SECTION("TIBasis Z2 XXZ N=12")
    {
        CompareXXZ<12> test(1.0);
        test.Test();
    }
}


#include "MexPackUnpack.h"
#include "mex.h"
#include <Eigen>

using namespace MexPackUnpackTypes;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

  //argument 1 : real double precision scalar
  //argument 2 : Single precision complex scalar
  //argument 3 : Double precision complex matrix represented as pair of real matrices corresponding to real/imaginary component
  //argument 4 : Double precision complex matrix represented as a pair of pointers, number of rows, and number of columns
  //This will only work with Octave or matlab before 2017b (or newer MATLABs compiled without the -R2018a flag)
  //Note: on MATLAB 2018b an newer (with the -R2018a compilation flag) you would replace
  //CDSP with CDIP and EDCSM with EDCIM
  MexUnpacker<double, std::complex<float>, EDCSM,  CDSP> my_unpack(nrhs, prhs);

  try {

    auto [a, b, c, d] = my_unpack.unpackMex();
    auto [cr, ci] = c; //cr and ci and Eigen::Map<MatrixXd> corresponding to real and imaginary parts of matrix
    auto [d_p, d_M, d_N] = d; //dp is std::pair<double*,double*> (pointers to real and imginary part), d_M is number of rows, d_N is number of columns

    Eigen::MatrixXcd c_comp(cr.rows(), cr.cols());
    c_comp.real() = cr;
    c_comp.imag() = ci;
    c_comp *= a;
    b*=a;
    MexPacker<std::complex<double>, int, Eigen::MatrixXcd, CDSP> my_pack(nlhs, plhs);
    my_pack.PackMex(b, 2, c_comp, d);

  } catch (std::string s) {
    mexPrintf(s.data());
  }

}


#pragma once

#include <Eigen/Dense>
#include <cmath>
#include <robotics/common.hpp>
#include <robotics/system/nonlinear_system.hpp>
#include <vector>

namespace Robotics::Estimation {

    /**
     * @brief A class for implemeting an Extended Kalman Filter
     */
    template <int StateSize, int InputSize, int OutputSize>
    class EKF {
        static_assert(StateSize > 0);
        static_assert(InputSize > 0);
        static_assert(OutputSize > 0);

        using State = ColumnVector<StateSize>;
        using Input = ColumnVector<InputSize>;
        using Measurement = ColumnVector<OutputSize>;

        using NonlinearSystem = Robotics::Model::NonlinearSystem<StateSize, InputSize, OutputSize>;

      public:
        /**
         * @brief Creates a new Extended Kalman Filter
         * @param system nonlinear model of the system
         * @param Q state covariance matrix
         * @param R output covariance matrix
         */
        EKF(NonlinearSystem system, SquareMatrix<StateSize> Q, SquareMatrix<OutputSize> R)
            : system(system), Q(Q), R(R)
        {
        }

        /**
         * @brief Updates the state estimate
         * @param previous_estimate last estimated state
         * @param z latest measurement
         * @param u control input
         * @return the updated state estimate
         */
        State Update(State, Measurement z, Input u, double dt)
        {
            // Predicted state estimate
            system.PropagateDynamics(u, dt);
            x_predicted = system.GetState();

            // Predicted covariance estimate
            SquareMatrix<StateSize> J_F = system.GetStateJacobian(u, dt);
            P_predicted = J_F * P_estimate * J_F.transpose() + Q;

            // Update
            z_predicted = system.GetOutputMatrix() * x_predicted;
            residual = z - z_predicted;

            const Matrix<OutputSize, StateSize> J_H = system.GetOutputJacobian(u, dt);
            S = J_H * P_predicted * J_H.transpose() + R;
            K = P_predicted * J_H.transpose() * S.inverse();
            x_estimate = x_predicted + K * residual;
            P_estimate = (SquareMatrix<StateSize>::Identity() - K * J_H) * P_predicted;

            return x_estimate;
        }

      private:
        NonlinearSystem system;

        SquareMatrix<StateSize> P_predicted, P_estimate;
        SquareMatrix<OutputSize> S;
        Robotics::Matrix<StateSize, OutputSize> K;

        State x_predicted, x_estimate;
        Measurement z_predicted, residual;

        // State covariance
        const SquareMatrix<StateSize> Q;

        // Observation covariance
        const SquareMatrix<OutputSize> R;
    };

}  // namespace Robotics::Estimation


#ifndef FX_DECON_M_HPP
#define FX_DECON_M_HPP

#include <armadillo>
#include "mconvert.h"
#include <cmath>
using namespace arma ;

mat fx_decon(mat DATA, double dt, int lf, double mu, double flow, int fhigh) ;
void ar_modeling(cx_vec x, int lf, double mu, cx_vec& yf, cx_vec& yb) ;

mat fx_decon(mat DATA, double dt, int lf, double mu, double flow, int fhigh)
{
  cx_mat DATA_FX, DATA_FX_b, DATA_FX_f ;
  cx_vec aux_in, aux_out_b, aux_out_f ;
  int ihigh, ilow, k, nf, nt, ntraces ;
  mat DATA_b, DATA_f ;
  nt = DATA.n_rows;
  ntraces = DATA.n_cols;

  nf = pow(2, m2cpp::nextpow2(nt)) ;
  DATA_FX_f = arma::zeros<cx_mat>(nf, ntraces) ;
  DATA_FX_b = arma::zeros<cx_mat>(nf, ntraces) ;
  ilow = std::floor(flow*dt*nf)+1 ;
  if (ilow<1)
  {
    ilow = 1 ;
  }
  ihigh = std::floor(fhigh*dt*nf)+1 ;
  if (ihigh>std::floor(nf/2.0)+1)
  {
    ihigh = std::floor(nf/2.0)+1 ;
  }
  DATA_FX = m2cpp::fft<mat>(DATA, nf, 1) ;
  for (k=ilow; k<=ihigh; k++)
  {
    aux_in = arma::trans(DATA_FX.row(k-1)) ;
    ar_modeling(aux_in, lf, mu, aux_out_f, aux_out_b) ;
    DATA_FX_f.row(k-1) = arma::trans(aux_out_f) ;
    DATA_FX_b.row(k-1) = arma::trans(aux_out_b) ;
  }
  for (k=nf/2.0+2; k<=nf; k++)
  {
    DATA_FX_f.row(k-1) = arma::conj(DATA_FX_f.row(nf-k+1)) ;
    DATA_FX_b.row(k-1) = arma::conj(DATA_FX_b.row(nf-k+1)) ;
  }
  DATA_f = arma::real(m2cpp::ifft<cx_mat>(DATA_FX_f, 1)) ;
  DATA_f = DATA_f.rows(arma::span(0, nt-1)) ;
  DATA_b = arma::real(m2cpp::ifft<cx_mat>(DATA_FX_b, 1)) ;
  DATA_b = DATA_b.rows(arma::span(0, nt-1)) ;
  DATA_f = (DATA_f+DATA_b) ;
  DATA_f.cols(arma::span(lf, ntraces-lf-1)) = DATA_f.cols(arma::span(lf, ntraces-lf-1))/2.0 ;
  return DATA_f ;
}

void ar_modeling(cx_vec x, int lf, double mu, cx_vec& yf, cx_vec& yb)
{
  cx_double beta ;
  cx_mat B, M, temp ;
  cx_vec C, R, ab, af, y ;
  uword nx ;
  nx = m2cpp::length(x) ;
  y = x(arma::span(0, nx-lf-1)) ;
  C = x(arma::span(1, nx-lf)) ;
  R = x(arma::span(nx-lf, nx-1)) ;
  M = m2cpp::hankel(C, R) ;
  B = arma::trans(M)*M ;
  beta = B(0, 0)*(cx_double) mu/100.0 ;
  ab = arma::solve((B+beta*arma::eye<cx_mat>(lf, lf)), arma::trans(M), solve_opts::fast)*y ;
  temp = M*ab ;
  temp = arma::join_cols(temp, arma::zeros<cx_mat>(lf, 1)) ;
  yb = temp ;
  y = x(arma::span(lf, nx-1)) ;
  C = x(arma::span(lf-1, nx-2)) ;
  R = arma::flipud(x(arma::span(0, lf-1))) ;
  M = toeplitz(C, R) ;
  B = arma::trans(M)*M ;
  beta = B(0, 0)*(cx_double) mu/100.0 ;
  af = arma::solve((B+beta*arma::eye<cx_mat>(lf, lf)), arma::trans(M), solve_opts::fast)*y ;
  temp = M*af ;
  temp = arma::join_cols(arma::zeros<cx_mat>(lf, 1), temp) ;
  yf = temp ;
  return ;
}
#endif


#include <iostream>
#include <flower/net.h>
#include <flower/layer/tanh.h>
#include <flower/layer/sigmoid.h>
#include <flower/layer/relu.h>
#include <flower/layer/elu.h>
#include <flower/layer/fully_connected.h>
#include <flower/layer/dropout.h>
#include <flower/gradient_descent.h>
#include <flower/optimizer/momentum.h>
#include <Eigen/CXX11/Tensor>

using namespace std;

int main()
{
    flower::Net net = flower::Net();

    net.add(flower::FullyConnected(3, 3));
    net.add(flower::Elu());
    net.add(flower::FullyConnected(3, 3));
    net.add(flower::Relu());
    net.add(flower::FullyConnected(3, 3));
    net.add(flower::Sigmoid());

    flower::GradientDescent trainer(&net, flower::Momentum());

    Eigen::Tensor<double, 2> t_data(2, 3);
    t_data.setValues({{0.05, 0.1, -0.5}, {0.1, -0.3, 0.4}});

    Eigen::Tensor<double, 2> t_target(2, 3);
    t_target.setValues({{0.01, 0.99, 1.0}, {0.9, 0.3, 0.2}});

    std::cout << "\n";

    for (int i = 0; i < 50; ++i)
    {
        std::cout << "epoch : "
                  << i
                  << " error: "
                  << trainer.feed(t_data, t_target)
                  << std::endl;
    }

    return 0;
}


#pragma once

#include <complex>
#include <functional>
#include <Eigen/Dense>
#include <iostream>
#include <memory>
#include <set>
#include <vector>

namespace prediction
{
enum State {X, Y, YAW, VX, STATES};
enum Obs {X_OBS, Y_OBS, YAW_OBS, VX_OBS, OBSS};
enum IMU {VX_IN, YAWRATE_IN, INPUTS};

class EKF {
 public:
  EKF();
  void initializeKF();
  void initializeSim();
  void estimate();
  void motionUpdate(const double& dt, //current_time,
                 Eigen::Vector4d& x, const Eigen::Vector2d& cont);
  void predictionUpdate(const double& time, Eigen::Vector2d u);
  void observationUpdate(Eigen::Vector4d z, Eigen::Matrix4d R);
  Eigen::Vector4d getObservation();
  Eigen::Vector4d getImu();

  // simulation methods
  void simulate(const double& time, const double& dt);

 private:
  /*  Kalman Filter parameters */
  // ground true states [x y yaw v]
  Eigen::Vector4d xTruth{0, 0, 0, 0};

  // estimated states by KF
  Eigen::Vector4d xEst{xTruth};

  // covariance matrix for process noise of KF
  Eigen::Matrix<double, 4, 4> Q;

  // intial covariance of estimation of KF
  Eigen::Matrix<double, 4, 4> Pest;

  // cov matrix for observation noise of KF
  //Eigen::Vector4d vR{2.25, 2.25, 0.0027, 0.0025};
  //Eigen::Matrix<double, 4, 4> R = vR.array().matrix().asDiagonal();

  Eigen::Vector2d u;

};

}  // namespace EKF


// Copyright (c) 2018-2021 FRC Team 3512. All Rights Reserved.

#pragma once

#include <Eigen/Core>
#include <frc/controller/LinearQuadraticRegulator.h>
#include <frc/estimator/KalmanFilter.h>
#include <frc/logging/CSVLogFile.h>
#include <frc/system/LinearSystem.h>
#include <frc/system/LinearSystemLoop.h>
#include <frc/system/plant/DCMotor.h>
#include <frc/system/plant/LinearSystemId.h>
#include <frc/trajectory/TrapezoidProfile.h>
#include <units/angle.h>
#include <units/angular_velocity.h>

#include "Constants.hpp"

namespace frc3512 {

class FourBarLiftController {
public:
    // State tolerances in radians and radians/sec respectively.
    static constexpr double kAngleTolerance = 0.05;
    static constexpr double kAngularVelocityTolerance = 2.0;

    FourBarLiftController();

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

    /**
     * Enables the control loop.
     */
    void Enable();

    /**
     * Disables the control loop.
     */
    void Disable();

    /**
     * Sets the end goal of the controller profile.
     *
     * @param goal Position in meters to set the goal to.
     */
    void SetGoal(double goal);

    /**
     * Sets the references.
     *
     * @param angle  Angle of the carriage in radians.
     * @param angularVelocity  Angular velocity of the carriage in radians per
     *                         second.
     */
    void SetReferences(units::radian_t angle,
                       units::radians_per_second_t angularVelocity);

    /**
     * Returns whether or not position and velocity are tracking the profile.
     */
    bool AtReferences() const;

    /**
     * Returns whether or not the goal has been reached.
     */
    bool AtGoal() const;

    /**
     * Sets the current encoder measurement.
     *
     * @param measuredAngle Angle of the carriage in radians.
     */
    void SetMeasuredAngle(double measuredAngle);

    /**
     * Returns the control loop calculated voltage.
     */
    double ControllerVoltage() const;

    /**
     * Informs the controller if to use the climbing feedforward.
     *
     * @param climbing Whether or not to use the climbing feedforward.
     */
    void SetClimbing(bool climbing);

    /**
     * Returns the estimated angle.
     */
    double EstimatedAngle() const;

    /**
     * Returns the estimated angular velocity.
     */
    double EstimatedAngularVelocity() const;

    /**
     * Returns the error between the angle reference and the angle
     * estimate.
     */
    double AngleError() const;

    /**
     * Returns the error between the angular velocity reference and the angular
     * velocity estimate.
     */
    double AngularVelocityError() const;

    /**
     * Returns the current angle reference.
     */
    double AngleReference();

    /**
     * Returns the current angular velocity reference.
     */
    double AngularVelocityReference();

    /**
     * Executes the control loop for a cycle.
     */
    void Update();

    /**
     * Resets any internal state.
     */
    void Reset();

private:
    // The current sensor measurement.
    Eigen::Matrix<double, 1, 1> m_y;
    frc::TrapezoidProfile<units::radians>::State m_goal;

    frc::TrapezoidProfile<units::radians>::Constraints constraints{
        Constants::FourBarLift::kMaxV, Constants::FourBarLift::kMaxA};
    frc::TrapezoidProfile<units::radians> m_angleProfile{constraints,
                                                         {0_rad, 0_rad_per_s}};

    frc::TrapezoidProfile<units::radians>::State m_profiledReference;

    frc::LinearSystem<2, 1, 1> m_plant = [=] {
        constexpr auto motor = frc::DCMotor::NEO();

        // Arm moment of inertia
        constexpr auto J = 0.6975_kg_sq_m;

        // Gear ratio
        constexpr double G = 302.22;

        return frc::LinearSystemId::SingleJointedArmSystem(motor, J, G);
    }();
    frc::LinearQuadraticRegulator<2, 1> m_controller{
        m_plant, {0.01245, 0.109726}, {9.0}, Constants::kDt};
    frc::KalmanFilter<2, 1, 1> m_observer{
        m_plant, {0.21745, 0.28726}, {0.01}, Constants::kDt};
    frc::LinearSystemLoop<2, 1, 1> m_loop{m_plant, m_controller, m_observer,
                                          12_V, Constants::kDt};

    bool m_atReferences = false;
    bool m_isEnabled = false;
    bool m_climbing = false;

    frc::CSVLogFile elevatorLogger{"FourBarLift",    "EstPos (rad)",
                                   "RefPos (rad)",   "Voltage (V)",
                                   "EstVel (rad/s)", "RefVel (rad/s)"};
};

}  // namespace frc3512


/*
 * EKF.hpp
 *
 *  Created on: 27.07.2018
 *      Author: tomlucas
 */

#ifndef ESTIMATORS_UKF_HPP_
#define ESTIMATORS_UKF_HPP_

#define FILL_LATER 0

#include "Eigen/Geometry"
#include <Eigen/Cholesky>
#include <OSG_Utils.hpp>

#include "../Plugins/sensor_plugin.hpp"
namespace zavi {
namespace estimator {
/**
 * Base class for all Extended Kalman Filters
 *
 * state_dim Dimension of the State
 * input_dim Dimension of the input Vector
 */
template<typename model>
class UKF: public Estimator {
protected:
	UKF(std::shared_ptr<plugin::SensorPlugin> sensor) :
			box_model(sensor), state_count(0), alignment(alignment.Identity()) {
	}
public:

	typedef model MODEL_TYPE;     //model type
	typedef typename MODEL_TYPE::template OUTER_T<double> STATE_TYPE;
	typedef typename MODEL_TYPE::template INNER_T<double> SIGMA_TYPE;
	typedef Eigen::Matrix<double, MODEL_TYPE::inner_size, MODEL_TYPE::inner_size> STATE_COV_TYPE;     //cov matrix
	typedef Eigen::Matrix<double, MODEL_TYPE::outer_size, MODEL_TYPE::inner_size * 2 + 1> SIGMA_POINTS_TYPE;     //< sigma points matrix type
	typedef Eigen::Matrix<double, MODEL_TYPE::inner_size, MODEL_TYPE::inner_size * 2 + 1> SIGMA_SIGMA_POINTS_TYPE;     //< occurs when the difference of SIGMA_POINTS with the state is drawn

	virtual ~UKF() {
		//smoothAllEstimates();

	}

	STATE_TYPE boxPlus(const STATE_TYPE & state, const SIGMA_TYPE & delta) {
		return box_model.template boxPlus<double>(state, delta);
	}

	SIGMA_TYPE boxMinus(const STATE_TYPE & a, const STATE_TYPE & b) {
		return box_model.template boxMinus<double>(a, b);
	}

	STATE_TYPE stateTransitionFunction(const STATE_TYPE & state, const double time_diff) {
		return box_model.template stateTransitionFunction<double>(state, time_diff);
	}

	/**
	 * Returns the sigma points of the state
	 *
	 *
	 * @param state the state to get the sigma points off
	 * @param cov the covariance of the state
	 * @return a eigen matrix with the sigma points
	 */

	SIGMA_POINTS_TYPE getSigmaPoints(const STATE_TYPE & state, const STATE_COV_TYPE & cov) {
		STATE_COV_TYPE cholesky = cov.llt().matrixL();
		SIGMA_POINTS_TYPE sigma_points = SIGMA_POINTS_TYPE::Zero();
		STATE_COV_TYPE neg_cholesky = STATE_COV_TYPE::Zero() - cholesky;
		Eigen::Matrix<double, MODEL_TYPE::outer_size, MODEL_TYPE::inner_size> negResult, posResult;
		negResult = negResult.Zero();
		posResult = posResult.Zero();
		for (int i = MODEL_TYPE::inner_size - 1; i >= 0; --i) {
			posResult.col(i) = boxPlus(state, cholesky.col(i));
		}
		for (int i = MODEL_TYPE::inner_size - 1; i >= 0; --i) {
			negResult.col(i) = boxPlus(state, neg_cholesky.col(i));
		}
		sigma_points << state, posResult, negResult;

		return sigma_points;
	}

	/**
	 * Simple wrapper to call getSigmaPoints without arguments
	 * @return sigma points of state_vector and covariance
	 */
	virtual SIGMA_POINTS_TYPE getSigmaPoints() {
		return getSigmaPoints(state_vector, covariance);
	}

	/**
	 * Calculates the mean of sigma points
	 * @param sigma_points  the matrix with all sigma points
	 * @param epsilon the stopping criteria
	 * @param max_iterations max iterations of convergence
	 * @return the mean of sigma points
	 */
	STATE_TYPE meanOfSigmaPoints(const SIGMA_POINTS_TYPE &sigma_points, double epsilon = 1e-8,
			int max_iterations = 30) {
		STATE_TYPE mean = sigma_points.col(0);
		STATE_TYPE old_mean = sigma_points.col(0);
		SIGMA_TYPE diff_sum = SIGMA_TYPE::Zero();
		int iterations = 0;
		do {
			iterations++;
			old_mean = mean;
			diff_sum = diff_sum.Zero();
			for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
				diff_sum += boxMinus(mean, sigma_points.col(i));
			}
			diff_sum /= 2. * MODEL_TYPE::inner_size + 1.;
			mean = boxPlus(mean, diff_sum);
		} while (iterations <= max_iterations && boxMinus(mean, old_mean).norm() > epsilon);
		if (iterations > max_iterations)
			printf("Warning: stopped due to excess of iterations");
		return mean;
	}

	/**
	 * Calculates the mean of sigma points
	 * @param sigma_points  the matrix with all sigma points
	 * @param epsilon the stopping criteria
	 * @param max_iterations max iterations of convergence
	 * @return the mean of sigma points
	 */
	template<int measure_dim, int measure_inner_dim>
	Eigen::Matrix<double, measure_dim, 1> meanOfSigmaPoints(
			const Eigen::Matrix<double, measure_dim, MODEL_TYPE::inner_size * 2 + 1> &sigma_points,
			Eigen::Matrix<double, measure_dim, 1> (*boxplus_m)(const Eigen::Matrix<double, measure_dim, 1> & state,
					const Eigen::Matrix<double, measure_inner_dim, 1> & delta),
			Eigen::Matrix<double, measure_inner_dim, 1> (*boxminus_m)(const Eigen::Matrix<double, measure_dim, 1> & a,
					const Eigen::Matrix<double, measure_dim, 1> & b), double epsilon = 1e-6, int max_iterations = 30) {

		Eigen::Matrix<double, measure_dim, 1> mean = sigma_points.col(0);
		Eigen::Matrix<double, measure_dim, 1> old_mean = sigma_points.col(0);
		Eigen::Matrix<double, measure_inner_dim, 1> diff_sum = Eigen::Matrix<double, measure_inner_dim, 1>::Zero();
		int iterations = 0;
		do {
			iterations++;
			old_mean = mean;
			diff_sum = diff_sum.Zero();
			for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
				diff_sum += boxminus_m(mean, sigma_points.col(i));
			}
			diff_sum /= 2. * MODEL_TYPE::inner_size + 1.;
			mean = boxplus_m(mean, diff_sum);
		} while (iterations <= max_iterations && boxminus_m(mean, old_mean).norm() > epsilon);
		if (iterations > max_iterations)
			printf("Warning: stopped due to excess of iterations");
		return mean;
	}
	/**
	 * Saves all releveant states for smoothing
	 * @param input the input u
	 * @param time_diff time since last call
	 */
	void saveStatesForSmoothing(double time_diff) {
		state_count++;
		past_states.push_back(state_vector);
		past_states_smoothed.push_back(state_vector);
		past_covs.push_back(covariance);
		past_timediffs.push_back(time_diff);
	}
	/**
	 * Does  dynamic step in EKF
	 *
	 * @param input the input u
	 * @param time_diff time since last call
	 */
	void dynamicStep(double time_diff) {
		SIGMA_POINTS_TYPE sigma_points = getSigmaPoints();
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			sigma_points.col(i) = stateTransitionFunction(sigma_points.col(i), time_diff);
		}
		state_vector = meanOfSigmaPoints(sigma_points);
		//printf(state_vector);
		//printf(" ");
		SIGMA_SIGMA_POINTS_TYPE result = SIGMA_SIGMA_POINTS_TYPE::Zero();
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			result.col(i) = boxMinus(state_vector, sigma_points.col(i));
		}
		covariance = 0.5 * (result * result.transpose()) + processNoise(time_diff);
		//printf("dynamic");
		//printf(state_vector.block(3, 0, 3, 1).norm());
	}
	/**
	 * Does  a  measurement update in UKF
	 * @param measurement the measurement
	 * @param time_diff time since last call
	 * @param measure_function function to map a state to a predicted measurement
	 * @param noise_function gives the measurement noise with time_diff
	 * @param boxplus_m boxplus for measurement
	 * @param boxminus_m boxminus for measurement
	 * measure dim is the dimension of the measurement vector
	 */
	template<int measure_inner_dim, typename functor, int measure_dim>
	void measurementStepManifold(const Eigen::Matrix<double, measure_dim, 1> & measurement, double time_diff,
			const functor & measure_function, const Eigen::Matrix<double, measure_inner_dim, measure_inner_dim> & noise,
			Eigen::Matrix<double, measure_dim, 1> (*boxplus_m)(const Eigen::Matrix<double, measure_dim, 1> & state,
					const Eigen::Matrix<double, measure_inner_dim, 1> & delta),
			Eigen::Matrix<double, measure_inner_dim, 1> (*boxminus_m)(const Eigen::Matrix<double, measure_dim, 1> & a,
					const Eigen::Matrix<double, measure_dim, 1> & b), void *prior = NULL) {
		SIGMA_POINTS_TYPE sigma_points = getSigmaPoints();
		Eigen::Matrix<double, measure_dim, MODEL_TYPE::inner_size * 2 + 1> expected_zs;
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			expected_zs.col(i) = measure_function(STATE_TYPE(sigma_points.col(i)), alignment, prior);
		}
		Eigen::Matrix<double, measure_dim, 1> mean_z = meanOfSigmaPoints(expected_zs, boxplus_m, boxminus_m);     //< expected measurement mean
		Eigen::Matrix<double, measure_inner_dim, MODEL_TYPE::inner_size * 2 + 1> diff_z;
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			diff_z.col(i) = boxminus_m(mean_z, expected_zs.col(i));
		}
		Eigen::Matrix<double, measure_inner_dim, measure_inner_dim> sigma_z = 0.5 * (diff_z * diff_z.transpose())
				+ noise;
		SIGMA_SIGMA_POINTS_TYPE result = SIGMA_SIGMA_POINTS_TYPE::Zero();
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			result.col(i) = boxMinus(state_vector, sigma_points.col(i));
		}
		Eigen::Matrix<double, MODEL_TYPE::inner_size, measure_inner_dim> sigma_xz = 0.5 * (result * diff_z.transpose());
		Eigen::Matrix<double, MODEL_TYPE::inner_size, measure_inner_dim> kalman_gain = sigma_xz * sigma_z.inverse();
		Eigen::Matrix<double, MODEL_TYPE::inner_size, 1> delta = kalman_gain * boxminus_m(mean_z, measurement);
		STATE_COV_TYPE sigma_t = covariance - (kalman_gain * sigma_z * kalman_gain.transpose());
		SIGMA_POINTS_TYPE sigma_points_second = SIGMA_POINTS_TYPE::Zero();
		STATE_COV_TYPE cholesky = sigma_t.llt().matrixL();
		Eigen::Matrix<double, MODEL_TYPE::outer_size, MODEL_TYPE::inner_size> negResult, posResult;
		negResult = negResult.Zero();
		posResult = posResult.Zero();
		STATE_COV_TYPE neg_cholesky = STATE_COV_TYPE::Zero() - cholesky;
		for (int i = MODEL_TYPE::inner_size - 1; i >= 0; --i) {
			posResult.col(i) = boxPlus(state_vector,
					box_model.template boxPlusInnerSpace<double>(delta, cholesky.col(i)));
		}
		for (int i = MODEL_TYPE::inner_size - 1; i >= 0; --i) {
			negResult.col(i) = boxPlus(state_vector,
					box_model.template boxPlusInnerSpace<double>(delta, neg_cholesky.col(i)));
		}
		sigma_points_second << boxPlus(state_vector, delta), posResult, negResult;

		state_vector = meanOfSigmaPoints(sigma_points_second);
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			result.col(i) = boxMinus(state_vector, sigma_points_second.col(i));
		}
		covariance = 0.5 * (result * result.transpose());

	}
	/**
	 * Does  a  measurement update in UKF
	 * @param measurement the measurement
	 * @param time_diff time since last call
	 * @param measure_function function to map a state to a predicted measurement
	 * @param noise_function gives the measurement noise with time_diff
	 * measure dim is the dimension of the measurement vector
	 */
	template<typename functor, int measure_dim>
	void measurementStep(const Eigen::Matrix<double, measure_dim, 1> & measurement, double time_diff,
			const functor & measure_function, const Eigen::Matrix<double, measure_dim, measure_dim> & noise,
			void * prior = NULL) {

		SIGMA_POINTS_TYPE sigma_points = getSigmaPoints();
		Eigen::Matrix<double, measure_dim, MODEL_TYPE::inner_size * 2 + 1> expected_zs;
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			expected_zs.col(i) = measure_function(STATE_TYPE(sigma_points.col(i)), alignment, prior);
		}
		Eigen::Matrix<double, measure_dim, 1> mean_z = expected_zs.rowwise().mean();     //< expected measurement mean
		Eigen::Matrix<double, measure_dim, MODEL_TYPE::inner_size * 2 + 1> diff_z = expected_zs.colwise() - mean_z;
		Eigen::Matrix<double, measure_dim, measure_dim> sigma_z = 0.5 * (diff_z * diff_z.transpose()) + noise;

		SIGMA_SIGMA_POINTS_TYPE result = SIGMA_SIGMA_POINTS_TYPE::Zero();
		for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
			result.col(i) = boxMinus(state_vector, sigma_points.col(i));
		}
		Eigen::Matrix<double, MODEL_TYPE::inner_size, measure_dim> sigma_xz = 0.5 * (result * diff_z.transpose());

		Eigen::Matrix<double, MODEL_TYPE::inner_size, measure_dim> kalman_gain = sigma_xz * sigma_z.inverse();

		state_vector = boxPlus(state_vector, kalman_gain * (measurement - mean_z));

		covariance = covariance - kalman_gain * sigma_xz.transpose();
	}
	/**
	 * wrapper to call measurementstep with different arguments
	 */
	template<int measure_dim>
	struct MeasurementWrapper {
		Eigen::Matrix<double, measure_dim, 1> (*function)(const STATE_TYPE &, void *);
		MeasurementWrapper(Eigen::Matrix<double, measure_dim, 1> (*function)(const STATE_TYPE &, void *)) :
				function(function) {

		}
		Eigen::Matrix<double, measure_dim, 1> operator()(const STATE_TYPE & state,
				const Eigen::Matrix<double, 4, 4> & alignment, void *prior) const {
			return function(state, prior);
		}

	};
	template<int measure_dim>
	void measurementStep(const Eigen::Matrix<double, measure_dim, 1> & measurement, double time_diff,
			Eigen::Matrix<double, measure_dim, 1> (*measure_function)(const STATE_TYPE &, void * prior),
			const Eigen::Matrix<double, measure_dim, measure_dim> & noise, void * prior = NULL) {
		measurementStep(measurement, time_diff, MeasurementWrapper<measure_dim>(measure_function), noise, prior);
	}

	/**
	 * Smooth previous estimates with new knowledge
	 *
	 * This is taken from:
	 *
	 * Unscented Rauch–Tung–Striebel Smoother by Simo S"arkk"a
	 *
	 * @param k_length the amount of steps to smooth back
	 * @param k_start the starting index for the smoothing
	 */
	void smoothEstimates(unsigned int k_length, unsigned int k_start) {
		if (k_start > state_count - 1) {
			LOG(ERROR)<<"Trying to smooth from a non existent state";
			return;
		}

		if (k_length > k_start) {
			LOG(ERROR)<< "Trying to smooth more states than are before k_start";
			return;
		}
		for (unsigned int k = k_start; k > k_start - k_length; k--) {
			SIGMA_POINTS_TYPE sigma_points = getSigmaPoints(past_states_smoothed[k], past_covs[k]);
			SIGMA_POINTS_TYPE sigma_points_plus;
			for (int i = 0; i < MODEL_TYPE::inner_size * 2 + 1; i++) {
				sigma_points_plus.col(i) = stateTransitionFunction(sigma_points.col(i), past_timediffs[k]);
			}

			STATE_TYPE mean = meanOfSigmaPoints(sigma_points_plus);
			SIGMA_SIGMA_POINTS_TYPE result_plus;
			for (int i = 0; i < MODEL_TYPE::inner_size * 2 + 1; i++) {
				result_plus.col(i) = boxMinus(mean, sigma_points_plus.col(i));
			}

			SIGMA_SIGMA_POINTS_TYPE result;
			STATE_COV_TYPE cov = 0.5*(result_plus * result_plus.transpose())+ processNoise(past_timediffs[k]);
			for (int i = 0; i < MODEL_TYPE::inner_size * 2 + 1; i++) {
				result.col(i) = boxMinus(past_states_smoothed[k], sigma_points.col(i));
			}
			STATE_COV_TYPE c_k_plus =0.5* result * result_plus.transpose();
			STATE_COV_TYPE d_k = c_k_plus * cov.inverse();
			//past_states_smoothed[k] = boxPlus(past_states_smoothed[k],
			//		d_k * boxMinus(mean, past_states_smoothed[k + 1]));
			//past_covs[k] = past_covs[k] + d_k * (past_covs[k + 1] - cov) * d_k.transpose();

			//from here the second sigma propagation applies
			SIGMA_TYPE delta=d_k * boxMinus(mean, past_states_smoothed[k + 1]);
			STATE_COV_TYPE pst_cov_k=past_covs[k] + d_k * (past_covs[k + 1] - cov) * d_k.transpose();

			SIGMA_POINTS_TYPE sigma_points_second = SIGMA_POINTS_TYPE::Zero();
			STATE_COV_TYPE cholesky = pst_cov_k.llt().matrixL();
			Eigen::Matrix<double, MODEL_TYPE::outer_size, MODEL_TYPE::inner_size> negResult, posResult;
			negResult = negResult.Zero();
			posResult = posResult.Zero();
			STATE_COV_TYPE neg_cholesky = STATE_COV_TYPE::Zero() - cholesky;
			for (int i = MODEL_TYPE::inner_size - 1; i >= 0; --i) {
				posResult.col(i) = boxPlus(past_states_smoothed[k],
						box_model.template boxPlusInnerSpace<double>(delta, cholesky.col(i)));
			}
			for (int i = MODEL_TYPE::inner_size - 1; i >= 0; --i) {
				negResult.col(i) = boxPlus(past_states_smoothed[k],
						box_model.template boxPlusInnerSpace<double>(delta, neg_cholesky.col(i)));
			}
			sigma_points_second << boxPlus(past_states_smoothed[k], delta), posResult, negResult;

			past_states_smoothed[k] = meanOfSigmaPoints(sigma_points_second);
			for (int i = MODEL_TYPE::inner_size * 2; i >= 0; --i) {
				result.col(i) = boxMinus(past_states_smoothed[k], sigma_points_second.col(i));
			}
			past_covs[k] = 0.5 * (result * result.transpose());
		}
	}
	/**
	 * Perform smoothing from the newest estimate
	 * @param k_length the amount of estimates to smooth
	 */
	void smoothEstimates(unsigned int k_length) {

		smoothEstimates(k_length, state_count - 2);

	}
	/**
	 * Perform smoothing from the newest estimate for all estimates
	 */
	void smoothAllEstimates() {
		smoothEstimates(state_count - 2);
	}

	static void smoothCallback(plugin::SensorPlugin * plug, void* estimator, double time) {
		UKF * esti = static_cast<UKF *>(estimator);
		esti->smoothAllEstimates();
	}

	/**
	 * Sets the start estimat \hat(x) (0)
	 * @param start_state the starting state vector
	 * @param start_cov  the starting covariance
	 */
	virtual inline void setStart(const STATE_TYPE & start_state, const STATE_COV_TYPE &start_cov) {
		state_vector = start_state;
		covariance = start_cov;
	}

	/**
	 * Gives the dimension of the state
	 * @return Dimension of the state vector
	 */
	static constexpr int getStateDim() {
		return MODEL_TYPE::outer_size;
	}

	virtual inline STATE_COV_TYPE processNoise(const double time_diff) {
		STATE_COV_TYPE matrix = box_model.getStateSTD(time_diff).asDiagonal();

		//zavi::printf(matrix);
		return matrix;
	}

	inline MODEL_TYPE getBoxModel() {
		return box_model;
	}
	inline STATE_TYPE getStateVector() {
		return state_vector;
	}
	inline STATE_COV_TYPE getCov() {
		return covariance;
	}

	std::vector<STATE_TYPE> & getSmoothedStates() {
		return past_states_smoothed;
	}

	void setAlignment(const Eigen::Matrix4d & alignment) {
		this->alignment=alignment;
	}

protected:
	MODEL_TYPE box_model;     // The Model type
	STATE_TYPE state_vector;//< the current estimated state x
	STATE_COV_TYPE covariance;//< the estimated cov(x)
	std::vector<STATE_TYPE> past_states;//< all past states
	std::vector<STATE_TYPE> past_states_smoothed;//< all past states
	//std::vector<INPUT_TYPE> past_inputs;//< all past inputs
	std::vector<double> past_timediffs;//< all past time_diffs

	std::vector<STATE_COV_TYPE> past_covs;// < all past covariance matrices
	unsigned int state_count;//< the current state index
	Eigen::Matrix<double, 4, 4> alignment;
};

}
/* namespace estimator */
}
/* namespace zavi */

#endif /* ESTIMATORS_UKF_HPP_ */


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Alpha_shape_2.h>
#include <CGAL/Alpha_shape_vertex_base_2.h>
#include <CGAL/Alpha_shape_face_base_2.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/algorithm.h>
#include <CGAL/assertions.h>
#include <fstream>
#include <iostream>
#include <list>
#include <vector>
#include <algorithm>
#include <unordered_set>
#include <boost/program_options.hpp>
#include <boost/functional/hash.hpp>

namespace po = boost::program_options;

typedef CGAL::Exact_predicates_inexact_constructions_kernel  K;
typedef K::FT                                                FT;
typedef K::Point_2                                           Point;
typedef K::Segment_2                                         Segment;
typedef CGAL::Alpha_shape_vertex_base_2<K>                   Vb;
typedef CGAL::Alpha_shape_face_base_2<K>                     Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb>          Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds>                Triangulation_2;
using Edge = Triangulation_2::Edge;
using Vertex = Triangulation_2::Vertex_handle;
typedef CGAL::Alpha_shape_2<Triangulation_2>                 Alpha_shape_2;
typedef Alpha_shape_2::Alpha_shape_edges_iterator            Alpha_shape_edges_iterator;

template <class OutputIterator>
void alpha_edges( const Alpha_shape_2& A, OutputIterator out)
{
  Alpha_shape_edges_iterator it = A.alpha_shape_edges_begin(),
                             end = A.alpha_shape_edges_end();
  for( ; it!=end; ++it) {
    Edge e = *it;
    auto r0 = A.classify(e.first);
    auto r = A.classify(e);

    *out++ = A.segment(*it);
  }

  auto vit = A.alpha_shape_vertices_begin(),
       vend = A.alpha_shape_vertices_end();
  for( ; vit!=vend; ++vit) {
    Vertex v = *vit;
    auto r = A.classify(v);
  }
}

template <class OutputIterator>
void alpha_verts( const Alpha_shape_2& A, OutputIterator out)
{
  auto vit = A.alpha_shape_vertices_begin(),
       vend = A.alpha_shape_vertices_end();
  for( ; vit!=vend; ++vit) {
    Vertex v = *vit;
    *(++out) = v->point();
  }
}

template <class OutputIterator>
bool file_input(const std::string& in, OutputIterator out)
{
  std::ifstream is(in, std::ios::in);
  if(is.fail())
  {
    std::cerr << "unable to open file for input" << std::endl;
    return false;
  }
  int n;
  is >> n;
  std::cout << "Reading " << n << " points from file" << std::endl;
  CGAL::copy_n(std::istream_iterator<Point>(is), n, out);
  return true;
}

bool save_output(const std::string& out,
                 const std::vector<Segment>& segments,
                 const std::vector<Point>& verts)
{
  std::ofstream os(out, std::ios::out);
  if(os.fail())
  {
    std::cerr << "unable to open file for output" << std::endl;
    return false;
  }

  os << segments.size() << std::endl;
  for (const auto& seg: segments) {
      os << std::fixed << std::setprecision(6) << seg << std::endl;
  }

  os << verts.size() << std::endl;
  for (const auto& v: verts) {
      os << std::fixed << std::setprecision(6) << v << std::endl;
  }

  return true;
}

struct Input {
    std::string in;
    std::string out;
    double alpha;
};

Input parse_input(int argc, char * argv[]) {
    po::options_description desc("Allowed options");
    desc.add_options()
        ("in", po::value<std::string>()->required(), "input points")
        ("out", po::value<std::string>()->required(), "output edges and verts")
        ("alpha", po::value<double>()->required(), "alpha");

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    Input input;
    input.in = vm["in"].as<std::string>();
    input.out = vm["out"].as<std::string>();
    input.alpha = vm["alpha"].as<double>();

    if (input.alpha <= 0)
        throw std::invalid_argument("alpha should be > 0");
    return input;
}

std::vector<Point> filter_verts(const std::vector<Segment>& segments,
                                std::vector<Point>&& verts) {
    struct HashVert {
        size_t operator()(const Point& p) const {
            size_t res = 0;
            boost::hash_combine(res, p.x());
            boost::hash_combine(res, p.y());
            return res;
        }
    };

    std::unordered_set<Point, HashVert> seg_points;
    for (const auto& seg: segments) {
        seg_points.insert(seg.source());
        seg_points.insert(seg.target());
    }

    verts.erase(
        std::remove_if(verts.begin(), verts.end(), [&seg_points](const auto& p) {
            return seg_points.count(p) > 0;
        }),
        verts.end()
    );

    return std::move(verts);
}

// Reads a list of points and returns a list of segments
// corresponding to the Alpha shape.
int main(int argc, char * argv[])
{
  const auto input = parse_input(argc, argv);
  std::list<Point> points;
  if(! file_input(input.in, std::back_inserter(points)))
    return -1;
  Alpha_shape_2 A(points.begin(), points.end(),
                  FT(input.alpha),
                  Alpha_shape_2::GENERAL);

  std::cout<< " Components for alpha " << input.alpha << " " << A.number_of_solid_components() << std::endl;

  std::vector<Segment> segments;
  std::vector<Point> res_points;
  alpha_edges(A, std::back_inserter(segments));
  alpha_verts(A, std::back_inserter(res_points));
  auto filtered_points = filter_verts(segments, std::move(res_points));
  save_output(input.out, segments, filtered_points);

  return 0;
}


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

    This is an example illustrating the use of the support vector machine
    utilities from the dlib C++ Library.  In particular, we show how to use the
    C parametrization of the SVM in this example.

    This example creates a simple set of data to train on and then shows
    you how to use the cross validation and svm training functions
    to find a good decision function that can classify examples in our
    data set.

    The data used in this example will be 2 dimensional data and will
    come from a distribution where points with a distance less than 10
    from the origin are labeled +1 and all other points are labeled
    as -1.

*/

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

using namespace std;
using namespace dlib;

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

int main(int argc, const char** argv)
{
    // The svm functions use column vectors to contain a lot of the data on
    // which they operate. So the first thing we do here is declare a convenient
    // typedef.

    // This typedef declares a matrix with 2 rows and 1 column.  It will be the
    // object that contains each of our 2 dimensional samples.   (Note that if
    // you wanted more than 2 features in this vector you can simply change the
    // 2 to something else.  Or if you don't know how many features you want
    // until runtime then you can put a 0 here and use the matrix.set_size()
    // member function)
    typedef matrix<double, 2, 1> sample_type;

    // This is a typedef for the type of kernel we are going to use in this
    // example.  In this case I have selected the radial basis kernel that can
    // operate on our 2D sample_type objects.  You can use your own custom
    // kernels with these tools as well, see custom_trainer_ex.cpp for an
    // example.
    typedef radial_basis_kernel<sample_type> kernel_type;

    // Now we make objects to contain our samples and their respective labels.
    std::vector<sample_type> samples;
    std::vector<double> labels;

    // Now let's put some data into our samples and labels objects.  We do this
    // by looping over a bunch of points and labeling them according to their
    // distance from the origin.
    for (int r = -20; r <= 20; ++r)
    {
        for (int c = -20; c <= 20; ++c)
        {
            sample_type samp;
            samp(0) = r;
            samp(1) = c;
            samples.push_back(samp);

            // if this point is less than 10 from the origin
            if (sqrt((double)r*r + c*c) <= 10)
                labels.push_back(+1);
            else
                labels.push_back(-1);

        }
    }

    // Here we normalize all the samples by subtracting their mean and dividing
    // by their standard deviation.  This is generally a good idea since it
    // often heads off numerical stability problems and also prevents one large
    // feature from smothering others.  Doing this doesn't matter much in this
    // example so I'm just doing this here so you can see an easy way to
    // accomplish it.
    vector_normalizer<sample_type> normalizer;
    // Let the normalizer learn the mean and standard deviation of the samples.
    normalizer.train(samples);
    // now normalize each sample
    for (unsigned long i = 0; i < samples.size(); ++i)
        samples[i] = normalizer(samples[i]);

    // Now that we have some data we want to train on it.  However, there are
    // two parameters to the training.  These are the C and gamma parameters.
    // Our choice for these parameters will influence how good the resulting
    // decision function is.  To test how good a particular choice of these
    // parameters are we can use the cross_validate_trainer() function to perform
    // n-fold cross validation on our training data.  However, there is a
    // problem with the way we have sampled our distribution above.  The problem
    // is that there is a definite ordering to the samples.  That is, the first
    // half of the samples look like they are from a different distribution than
    // the second half.  This would screw up the cross validation process but we
    // can fix it by randomizing the order of the samples with the following
    // function call.
    randomize_samples(samples, labels);

    // here we make an instance of the svm_c_trainer object that uses our kernel
    // type.
    svm_c_trainer<kernel_type> trainer;

    // Now we loop over some different C and gamma values to see how good they
    // are.  Note that this is a very simple way to try out a few possible
    // parameter choices.  You should look at the model_selection_ex.cpp program
    // for examples of more sophisticated strategies for determining good
    // parameter choices.
    cout << "doing cross validation" << endl;
    for (double gamma = 0.00001; gamma <= 1; gamma *= 5)
    {
        for (double C = 1; C < 100000; C *= 5)
        {
            // tell the trainer the parameters we want to use
            trainer.set_kernel(kernel_type(gamma));
            trainer.set_c(C);

            cout << "gamma: " << gamma << "    C: " << C;
            // Print out the cross validation accuracy for 3-fold cross validation using
            // the current gamma and C.  cross_validate_trainer() returns a row vector.
            // The first element of the vector is the fraction of +1 training examples
            // correctly classified and the second number is the fraction of -1 training
            // examples correctly classified.
            cout << "     cross validation accuracy: "
                 << cross_validate_trainer(trainer, samples, labels, 3);
        }
    }

    // From looking at the output of the above loop it turns out that good
    // values for C and gamma for this problem are 5 and 0.15625 respectively.
    // So that is what we will use.

    // Now we train on the full set of data and obtain the resulting decision
    // function.  The decision function will return values >= 0 for samples it
    // predicts are in the +1 class and numbers < 0 for samples it predicts to
    // be in the -1 class.
    trainer.set_kernel(kernel_type(0.15625));
    trainer.set_c(5);
    typedef decision_function<kernel_type> dec_funct_type;
    typedef normalized_function<dec_funct_type> funct_type;

    // Here we are making an instance of the normalized_function object.  This
    // object provides a convenient way to store the vector normalization
    // information along with the decision function we are going to learn.
    funct_type learned_function;
    learned_function.normalizer = normalizer;  // save normalization information
    learned_function.function = trainer.train(samples, labels); // perform the actual SVM training and save the results

    // print out the number of support vectors in the resulting decision function
    cout << "\nnumber of support vectors in our learned_function is "
         << learned_function.function.basis_vectors.size() << endl;

    // Now let's try this decision_function on some samples we haven't seen before.
    sample_type sample;

    sample(0) = 3.123;
    sample(1) = 2;
    cout << "This is a +1 class example, the classifier output is " << learned_function(sample) << endl;

    sample(0) = 3.123;
    sample(1) = 9.3545;
    cout << "This is a +1 class example, the classifier output is " << learned_function(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 9.3545;
    cout << "This is a -1 class example, the classifier output is " << learned_function(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 0;
    cout << "This is a -1 class example, the classifier output is " << learned_function(sample) << endl;

    // We can also train a decision function that reports a well conditioned
    // probability instead of just a number > 0 for the +1 class and < 0 for the
    // -1 class.  An example of doing that follows:
    typedef probabilistic_decision_function<kernel_type> probabilistic_funct_type;
    typedef normalized_function<probabilistic_funct_type> pfunct_type;

    pfunct_type learned_pfunct;
    learned_pfunct.normalizer = normalizer;
    learned_pfunct.function = train_probabilistic_decision_function(trainer, samples, labels, 3);
    // Now we have a function that returns the probability that a given sample is of the +1 class.

    // print out the number of support vectors in the resulting decision function.
    // (it should be the same as in the one above)
    cout << "\nnumber of support vectors in our learned_pfunct is "
         << learned_pfunct.function.decision_funct.basis_vectors.size() << endl;

    sample(0) = 3.123;
    sample(1) = 2;
    cout << "This +1 class example should have high probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

    sample(0) = 3.123;
    sample(1) = 9.3545;
    cout << "This +1 class example should have high probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 9.3545;
    cout << "This -1 class example should have low probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

    sample(0) = 13.123;
    sample(1) = 0;
    cout << "This -1 class example should have low probability.  Its probability is: "
         << learned_pfunct(sample) << endl;

    // Another thing that is worth knowing is that just about everything in dlib
    // is serializable.  So for example, you can save the learned_pfunct object
    // to disk and recall it later like so:
    serialize("saved_function.dat") << learned_pfunct;

    // Now let's open that file back up and load the function object it contains.
    deserialize("saved_function.dat") >> learned_pfunct;

    // Note that there is also an example program that comes with dlib called
    // the file_to_code_ex.cpp example.  It is a simple program that takes a
    // file and outputs a piece of C++ code that is able to fully reproduce the
    // file's contents in the form of a std::string object.  So you can use that
    // along with the std::istringstream to save learned decision functions
    // inside your actual C++ code files if you want.

    // Lastly, note that the decision functions we trained above involved well
    // over 200 basis vectors.  Support vector machines in general tend to find
    // decision functions that involve a lot of basis vectors.  This is
    // significant because the more basis vectors in a decision function, the
    // longer it takes to classify new examples.  So dlib provides the ability
    // to find an approximation to the normal output of a trainer using fewer
    // basis vectors.

    // Here we determine the cross validation accuracy when we approximate the
    // output using only 10 basis vectors.  To do this we use the reduced2()
    // function.  It takes a trainer object and the number of basis vectors to
    // use and returns a new trainer object that applies the necessary post
    // processing during the creation of decision function objects.
    cout << "\ncross validation accuracy with only 10 support vectors: "
         << cross_validate_trainer(reduced2(trainer,10), samples, labels, 3);

    // Let's print out the original cross validation score too for comparison.
    cout << "cross validation accuracy with all the original support vectors: "
         << cross_validate_trainer(trainer, samples, labels, 3);

    // When you run this program you should see that, for this problem, you can
    // reduce the number of basis vectors down to 10 without hurting the cross
    // validation accuracy.

    // To get the reduced decision function out we would just do this:
    learned_function.function = reduced2(trainer,10).train(samples, labels);
    // And similarly for the probabilistic_decision_function:
    learned_pfunct.function = train_probabilistic_decision_function(reduced2(trainer,10), samples, labels, 3);
}


/*! \file mesh_generator3D.hpp
\brief Set of functions to generate points.
\author Elad Steinberg
*/
#ifndef MESHGENERATOR3D_HPP
#define MESHGENERATOR3D_HPP 1

#ifdef _MSC_VER
#define _USE_MATH_DEFINES
#endif // _MSC_VER
#include <vector>
#include <cmath>
#include "../3D/GeometryCommon/Voronoi3D.hpp"
#include <algorithm>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

/*! \brief Generates a cartesian mesh
\param nx Number of points along the x axis
\param ny Number of points along the y axis
\param nz Number of points along the z axis
\param lower_left Lower left point
\param upper_right Upper right point
\return Set of three dimensional points
*/
vector<Vector3D> CartesianMesh(std::size_t nx, std::size_t ny, std::size_t nz, Vector3D const& lower_left,
	Vector3D const& upper_right);

/*!
\brief Generates a random grid with uniform point density and a constant seed
\param PointNum The number of points.
\param ll The lower left point of the domain
\param ur The upper right point of the domain
\return List of three dimensional points
*/
vector<Vector3D> RandRectangular(std::size_t PointNum, Vector3D const& ll, Vector3D const& ur,Voronoi3D const* tproc = 0);

vector<Vector3D> RandRectangular(std::size_t PointNum, Vector3D const& ll, Vector3D const& ur, boost::mt19937_64 &gen);

vector<Vector3D> RandSphereR(std::size_t PointNum, Vector3D const& ll, Vector3D const& ur, double Rmin, double Rmax,
	Vector3D center = Vector3D(),Voronoi3D const* tproc = 0);

vector<Vector3D> RandSphereR2(std::size_t PointNum, Vector3D const& ll, Vector3D const& ur,double Rmin,double Rmax
	, Vector3D center = Vector3D(), Voronoi3D const* tproc = 0);

vector<Vector3D> RandSphereR1(std::size_t PointNum, Vector3D const& ll, Vector3D const& ur, double Rmin, double Rmax,
	Vector3D center = Vector3D(),Voronoi3D const* tproc = 0);

vector<Vector3D> RandSphereRa(std::size_t PointNum, Vector3D const& ll, Vector3D const& ur, double Rmin, double Rmax,double a, Vector3D const& center,
	Voronoi3D const* tproc = 0);

#ifdef RICH_MPI
/*!
\brief Generates a random grid with uniform point density and a constant seed
\param PointNum The total number of points to be in all cpus combined.
\param tproc The tessellation of the processors
\return List of three dimensional points
*/
vector<Vector3D> RandPointsMPI(Voronoi3D const& tproc, size_t PointNum);
#endif

#endif //MESHGENERATOR3D_HPP


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

//[ guide_histogram_operators

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

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

  // make two histograms
  auto h1 = make_histogram(axis::regular<>(2, -1.0, 1.0));
  auto h2 = make_histogram(axis::regular<>(2, -1.0, 1.0));

  h1(-0.5); // counts are: 1 0
  h2(0.5);  // counts are: 0 1

  // add them
  auto h3 = h1;
  h3 += h2; // counts are: 1 1

  // adding multiple histograms at once is likely to be optimized by the compiler so that
  // superfluous temporaries avoided, but no guarantees are given; use this equivalent
  // code when you want to make sure: h4 = h1; h4 += h2; h4 += h3;
  auto h4 = h1 + h2 + h3; // counts are: 2 2

  assert(h4.at(0) == 2 && h4.at(1) == 2);

  // multiply by number; h4 *= 2 is not allowed, because the result of a multiplication is
  // not a histogram anymore, it has the type boost::histogram::grid<...>)
  auto g4 = h4 * 2; // counts are: 4 4

  // divide by number; g4 /= 4 also works
  auto g5 = g4 / 4; // counts are: 1 1

  assert(g5.at(0) == 1 && g5.at(1) == 1);
  assert(g4 != g5 && g4 == 4 * g5);

  // note the special effect of multiplication on weight_storage
  auto h = make_histogram_with(weight_storage(), axis::regular<>(2, -1.0, 1.0));
  h(-0.5);

  // counts are: 1 0
  assert(h.at(0).value() == 1 && h.at(1).value() == 0);

  auto h_sum = h + h;
  auto g_mul = 2 * h;

  // values are the same as expected...
  assert(h_sum.at(0).value() == g_mul.at(0).value());
  // ... but variances differ
  assert(h_sum.at(0).variance() == 2 && g_mul.at(0).variance() == 4);

  // equality operator checks variances, so histograms are not equal
  assert(h_sum != g_mul);
}

//]


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <math.h>
#include <limits>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include "modules/world/opendrive/plan_view.hpp"
#include "modules/world/opendrive/lane.hpp"
#include "modules/world/opendrive/odrSpiral.hpp"

namespace modules {
namespace world {
namespace opendrive {

namespace bg = boost::geometry;

bool PlanView::add_line(geometry::Point2d start_point, float heading, float length) {
  //! straight line
  reference_line_.add_point(start_point);
  geometry::Point2d end_point(bg::get<0>(start_point) + length * cos(heading), bg::get<1>(start_point) + length * sin(heading));
  reference_line_.add_point(end_point);

  //! calculate overall length
  length_ = bg::length(reference_line_.obj_);
  return true;
}

bool PlanView::add_spiral(geometry::Point2d start_point, float heading, float length, float curvature_start, float curvature_end, float s_inc) {
  double x = bg::get<0>(start_point), y = bg::get<1>(start_point), t = heading, cDot = (curvature_end - curvature_start) / length;
  double x_old = bg::get<0>(start_point), y_old = bg::get<1>(start_point);

  double s = 0.0;
  for (; s < length; s += s_inc) {
    odrSpiral(s, x_old, y_old, cDot, curvature_start, heading, &x, &y, &t);
    reference_line_.add_point(geometry::Point2d(x, y));
  }

  // fill last point if increment does not match
  double delta_s = fabs(length - s);
  if (delta_s > 0.0) {
    odrSpiral(length, x_old, y_old, cDot, curvature_start, heading, &x, &y, &t);
    reference_line_.add_point(geometry::Point2d(x, y));
  }

  length_ = bg::length(reference_line_.obj_);
  return true;
}

void PlanView::calc_arc_position(const float s, float initial_heading, float curvature, float &dx, float &dy) {
  initial_heading = fmod(initial_heading, 2 * M_PI);
  float hdg = initial_heading - M_PI / 2;

  float a = 2 / curvature * sin(s * curvature / 2);
  float alpha = (M_PI - s * curvature) / 2 - hdg;

  dx = -1 * a * cos(alpha);
  dy = a * sin(alpha);

  // tangent = initial_heading + s * initial_curvature;
}

bool PlanView::add_arc(geometry::Point2d start_point, float heading, float length, float curvature, float s_inc) {
  // add_spiral(start_point, heading, length, curvature, curvature, s_inc);

  float dx, dy;
  double x_old = bg::get<0>(start_point), y_old = bg::get<1>(start_point);
  double s = 0.0;
  for (; s < length; s += s_inc) {
    calc_arc_position(s, heading, curvature, dx, dy);
    reference_line_.add_point(geometry::Point2d(x_old + dx, y_old + dy));
  }

  // fill last point if increment does not match
  double delta_s = fabs(length - s);
  if (delta_s >= 0.0){
    calc_arc_position(length, heading, curvature, dx, dy);
    reference_line_.add_point(geometry::Point2d(x_old + dx, y_old + dy));
  }

  return true;
}

geometry::Line PlanView::create_line(int id, LaneWidth lane_width, float s_inc) {
  float s_start = lane_width.s_start;
  float s_end = lane_width.s_end;
  LaneOffset off = lane_width.off;

  float s = s_start;
  float scale = 0.0f;
  geometry::Line tmp_line;
  geometry::Point2d normal(0.0f, 0.0f);
  int sign = id > 0 ? -1 : 1;

  // TODO(fortiss): check if sampling does work with relative s, probably not
  if (off.b != 0.0f || off.c != 0.0f || off.d != 0.0f || (lane_width.s_end - lane_width.s_start) != 1.0) {
    for (; s < s_end; s += s_inc) {
      geometry::Point2d point = get_point_at_s(reference_line_, s);
      normal = get_normal_at_s(reference_line_, s);
      scale = -sign * polynom(s, off.a, off.b, off.c, off.d);
      tmp_line.add_point(geometry::Point2d(bg::get<0>(point) + scale * bg::get<0>(normal),
                                  bg::get<1>(point) + scale * bg::get<1>(normal)));
    }

    // fill last point if increment does not match
    double delta_s = fabs(s_end-s);
    if(delta_s>0.0){
      geometry::Point2d point = get_point_at_s(reference_line_, s_end);
      normal = get_normal_at_s(reference_line_, s_end);
      scale = -sign * polynom(s_end, off.a, off.b, off.c, off.d);
      tmp_line.add_point(geometry::Point2d(bg::get<0>(point) + scale * bg::get<0>(normal),
                                  bg::get<1>(point) + scale * bg::get<1>(normal)));
    }
  } else {
      for (uint32_t i = 0; i < reference_line_.obj_.size() - 1; i++) {
        normal = get_normal_at_s(reference_line_, reference_line_.s_[i]);
        scale = -sign * polynom(s, off.a, off.b, off.c, off.d);
        tmp_line.add_point(geometry::Point2d(bg::get<0>(reference_line_.obj_[i]) + scale * bg::get<0>(normal),
                                  bg::get<1>(reference_line_.obj_[i]) + scale * bg::get<1>(normal)));
        s += geometry::distance(reference_line_.obj_[i + 1], reference_line_.obj_[i]);
      }
      // add last point
      normal = get_normal_at_s(reference_line_, reference_line_.s_[reference_line_.obj_.size() - 1]);
      int size = reference_line_.obj_.size() - 1;
      scale = -sign * polynom(length_, off.a, off.b, off.c, off.d);
      tmp_line.add_point(geometry::Point2d(bg::get<0>(reference_line_.obj_[size]) + scale * bg::get<0>(normal),
                                bg::get<1>(reference_line_.obj_[size]) + scale * bg::get<1>(normal)));
  }

  return tmp_line;
}

//! TODO: this function needs to resive a vector of Struct {s_start, s_end, off}
LanePtr PlanView::create_lane(LanePosition lane_position, LaneWidths lane_widths, float s_inc) {
  std::shared_ptr<Lane> ret_lane(new Lane(lane_position));
  if (lane_widths.size() > 1) {
    assert("Not supported");
  }
  for (LaneWidth lane_width : lane_widths) {
    geometry::Line tmp_line = create_line(lane_position, lane_width, s_inc);
    ret_lane->set_line(tmp_line);
  }

  // ret_lane->ComputeCenterLine();
  return ret_lane;
}

}  // namespace opendrive
}  // namespace world
}  // namespace modules


#include <iostream>
#include <unistd.h>
#include <Eigen/Dense>
#include "OrthonormalHermite.h"
#include "NelderMead.h"
#include "VariableProjection.h"

using namespace std;

int main()
{
    APPRSDK::VariableProjection<double> approximator;
    APPRSDK::OrthonormalHermite<double> hermiteSys(100, 10);

    Eigen::RowVectorXd inputParameters;
    inputParameters.resize(2);
    inputParameters(0) = 0.7;
    inputParameters(1) = 50;

    Eigen::RowVectorXd lb;
    lb.resize(2);
    lb(0) = 0.01;
    lb(1) = -1000;

    Eigen::RowVectorXd ub;
    ub.resize(2);
    ub(0) = 1000;
    ub(1) = 1000;

    APPRSDK::AvailableOptimizers optId = APPRSDK::AvailableOptimizers::NM;

    approximator.SetNonLinParams(inputParameters);
    approximator.SetMaxErrorForOptimisation(0.01);
    approximator.SetMaxIterationForOptimisation(100);
    approximator.SetFunctionSystem(&hermiteSys);
    approximator.SelectOptimiser(optId, true);
	approximator.SetBoundaries(lb, ub);
    approximator.SetSignal(hermiteSys.GetFunctionSystem().col(4).transpose());

    approximator.Varpro();

	cout<<"Signal: "<<approximator.GetSignal().transpose()<<endl;
	cout<<"Approximaton: "<<approximator.GetApproximation().transpose()<<endl;
	cout<<"Coefficients: "<<approximator.GetLinearParameters().transpose()<<endl;
	cout<<"Dilatation & Translation: "<<approximator.GetNonLinearParameters()<<endl;
    cout<<"Iterations: "<<approximator.GetIterations()<<endl;
    cout<<"Final error: "<<approximator.GetError()<<endl;

    return 0;
}


/* This file is part of PyMesh. Copyright (c) 2017 by Qingnan Zhou */
#include "VoxelDihedralAngleAttribute.h"

#include <cmath>
#include <Eigen/Core>

#include <Mesh.h>
#include <Core/Exception.h>

using namespace PyMesh;

namespace VoxelDihedralAngleAttributeHelper {
    Float angle(const Eigen::Ref<Vector3F>& n1,
            const Eigen::Ref<Vector3F>& n2) {
        return atan2(n1.cross(n2).norm(), n1.dot(n2));
    }

    Vector3F compute_normal(
            const Vector3F& v1,
            const Vector3F& v2,
            const Vector3F& v3) {
        return (v2 - v1).cross(v3 - v1);
    }
}

using namespace VoxelDihedralAngleAttributeHelper;

void VoxelDihedralAngleAttribute::compute_from_mesh(Mesh& mesh) {
    const size_t dim = mesh.get_dim();
    const size_t num_voxels = mesh.get_num_voxels();
    const size_t vertex_per_voxel = mesh.get_vertex_per_voxel();
    if (dim != 3) {
        throw RuntimeError("Voxel dihedral anlge computation is for 3D only.");
    }
    if (num_voxels > 0 && vertex_per_voxel != 4) {
        throw NotImplementedError(
                "Voxel dihedral angle computation only support tet for now.");
    }

    const auto& vertices = mesh.get_vertices();
    const auto& voxels = mesh.get_voxels();
    VectorF& dihedral_angles = m_values;
    dihedral_angles.resize(num_voxels * 6);

    for (size_t i=0; i<num_voxels; i++) {
        Vector4I v = voxels.segment<4>(i*4);
        Vector3F v0 = vertices.segment<3>(v[0]*3);
        Vector3F v1 = vertices.segment<3>(v[1]*3);
        Vector3F v2 = vertices.segment<3>(v[2]*3);
        Vector3F v3 = vertices.segment<3>(v[3]*3);

        Vector3F n0 = compute_normal(v1, v2, v3);
        Vector3F n1 = compute_normal(v0, v3, v2);
        Vector3F n2 = compute_normal(v0, v1, v3);
        Vector3F n3 = compute_normal(v0, v2, v1);

        dihedral_angles[i*6  ] = M_PI - angle(n2, n3);
        dihedral_angles[i*6+1] = M_PI - angle(n0, n3);
        dihedral_angles[i*6+2] = M_PI - angle(n1, n3);
        dihedral_angles[i*6+3] = M_PI - angle(n1, n2);
        dihedral_angles[i*6+4] = M_PI - angle(n0, n1);
        dihedral_angles[i*6+5] = M_PI - angle(n0, n2);
    }
}


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>
#include <boost/graph/make_connected.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/planar_face_traversal.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/make_maximal_planar.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <vector>
#include <fstream>
#include <iostream>
using namespace std;
using namespace boost;

typedef adjacency_list<vecS, vecS, undirectedS, property<vertex_index_t, int>, property<edge_index_t, int>> Graph;
typedef vector<vector<graph_traits<Graph>::edge_descriptor>>                                                embedding_storage_t;
typedef iterator_property_map<embedding_storage_t::iterator, property_map<Graph, vertex_index_t>::type>     embedding_t;

struct face_counter : planar_face_traversal_visitor
{
        face_counter() : count(0) {}
        void begin_face() { ++count; }
        uint count;
};

struct coord_t
{
        size_t x, y;
};

void make_max_planar(Graph& g)
{
        auto e_index = get(edge_index, g);
        graph_traits<Graph>::edges_size_type edge_count = 0;
        graph_traits<Graph>::edge_iterator ei, ei_end;
        for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) put(e_index, *ei, edge_count++);

        typedef vector<graph_traits<Graph>::edge_descriptor> vec_t;
        vector<vec_t> embedding(num_vertices(g));
        boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g, boyer_myrvold_params::embedding = &embedding[0]);

        make_biconnected_planar(g, &embedding[0]);

        edge_count = 0;
        for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) put(e_index, *ei, edge_count++);

        boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g, boyer_myrvold_params::embedding = &embedding[0]);

        make_maximal_planar(g, &embedding[0]);

        edge_count = 0;
        for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) put(e_index, *ei, edge_count++);

        boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g, boyer_myrvold_params::embedding = &embedding[0]);

        face_counter count_visitor;
        planar_face_traversal(g, &embedding[0], count_visitor);
}

int main(int argc, char** argv)
{
        if( argc < 2 ){
                cerr << "Usage: straightline [filename]\n";
                return 1;
        }

        string fname(argv[1]);
        ifstream f(fname);
        if( !f ){
                cerr << "file " << fname << " not found!\n";
                return 1;
        }

        string str;
        vector<pair<uint, uint>> edges;
        uint n = 0;
        while( getline(f, str) ){
                uint   colon = str.find(",");
                string stra  = str.substr(0, colon); trim(stra);
                string strb  = str.substr(colon+1 ); trim(strb);
                uint   a     = lexical_cast<uint>(stra);
                uint   b     = lexical_cast<uint>(strb);
                n = max(max(n, a), b);
                edges.push_back(make_pair(a, b));
        }

        Graph g(n);
        for( auto& e : edges ) add_edge(e.first, e.second, g);
        make_max_planar(g);

        embedding_storage_t embedding_storage(num_vertices(g));
        embedding_t         embedding        (embedding_storage.begin(), get(vertex_index,g));

        boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g, boyer_myrvold_params::embedding = embedding);

        vector<graph_traits<Graph>::vertex_descriptor> ordering;
        planar_canonical_ordering(g, embedding, back_inserter(ordering));

        typedef vector<coord_t> 											      straight_line_drawing_storage_t;
        typedef iterator_property_map < straight_line_drawing_storage_t::iterator, property_map<Graph, vertex_index_t>::type> straight_line_drawing_t;

        straight_line_drawing_storage_t straight_line_drawing_storage (num_vertices(g));
        straight_line_drawing_t straight_line_drawing (straight_line_drawing_storage.begin(), get(vertex_index,g));

        chrobak_payne_straight_line_drawing(g, embedding, ordering.begin(), ordering.end(), straight_line_drawing);

        graph_traits<Graph>::vertex_iterator vi, vi_end;
        cout << "graph G {\n";
        for( tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi ){
                coord_t coord(get(straight_line_drawing,*vi));
                cout << *vi << "[pos=\"" << coord.x << ',' << coord.y << "!\"];\n";
        }
        for( auto& e : edges ) cout << e.first << "--" << e.second << " ;\n";
        cout << "}\n";
}


#pragma once
#include <cstdint>
#include <cassert>
#include <algorithm>
#include <map>
#include <boost/dynamic_bitset.hpp>

#include "../Basis/AbstractBasis1D.hpp"

template<typename UINT>
class TIXXZ
{
private:
	const edlib::AbstractBasis1D<UINT>& basis_;
	double J_;
	double delta_;

public:
	TIXXZ(const edlib::AbstractBasis1D<UINT>& basis, double J, double delta)
		: basis_(basis), J_(J), delta_(delta)
	{

	}

	std::map<int, double> getCol(UINT n) const
	{
		unsigned int N = basis_.getN();

		UINT a = basis_.getNthRep(n);
		const boost::dynamic_bitset<> bs(N, a);

		std::map<int, double> m;
		for(unsigned int i = 0; i < N; i++)
		{
			//Next-nearest
			{
				unsigned int j = (i+1)%N;
				int sgn = (1-2*bs[i])*(1-2*bs[j]);

				m[n] += J_*delta_*sgn;

				UINT s = a;
				s ^= basis_.mask({i,j});

				int bidx;
				double coeff;

				std::tie(bidx, coeff) = basis_.hamiltonianCoeff(s, n);

				if(bidx >= 0)
				{
					m[bidx] += J_*(1.0-sgn)*coeff;
				}
			}
		}
		return m;
	}
};