static char help[] = "Solve small CMEs to benchmark intranode performance.\n\n";

#include<iomanip>
#include <petscmat.h>
#include <petscvec.h>
#include <petscviewer.h>
#include <Sys.h>
#include <armadillo>
#include <cmath>
#include <sys/stat.h>
#include "FspSolverMultiSinks.h"

namespace six_species_cme {
// stoichiometric matrix of the toggle switch model
arma::Mat<PetscInt> SM{{1, -1, 0, 0, 0, 0, 0, 0, -2, 2},
                       {0, 0, 0, 0, -1, 1, -1, 1, 1, -1},
                       {0, 0, 0, 0, -1, 1, 0, 0, 0, 0},
                       {0, 0, 0, 0, 1, -1, -1, 1, 0, 0},
                       {0, 0, 0, 0, 0, 0, 1, -1, 0, 0},
                       {0, 0, 1, -1, 0, 0, 0, 0, 0, 0}};

// reaction parameters
const PetscReal Avo = 6.022140857e23, c0 = 0.043, c1 = 0.0007, c2 = 0.078, c3 = 0.0039, c4 =
    0.012e09 / (Avo), c5 = 0.4791, c6 = 0.00012e09 / (Avo), c7 = 0.8765e-11, c8 =
    0.05e09 / (Avo), c9 = 0.5, avg_cell_cyc_time = 35 * 60.0;

// propensity function
inline PetscReal transreg_propensity(const PetscInt *X, const PetscInt k) {
  switch (k) {
    case 0:return c0 * PetscReal(X[5]);
    case 1:return c1 * PetscReal(X[0]);
    case 2:return c2 * PetscReal(X[3]);
    case 3:return c3 * PetscReal(X[5]);
    case 4:return PetscReal(X[1]) * PetscReal(X[2]);
    case 5:return c5 * PetscReal(X[3]);
    case 6:return PetscReal(X[3]) * PetscReal(X[1]);
    case 7:return c7 * PetscReal(X[4]);
    case 8:return 0.5 * PetscReal(X[0]) * PetscReal(X[0] - 1);
    case 9:return c9 * PetscReal(X[1]);
    default:return 0.0;
  }
}
int propensity(const int reaction,
               const int num_species,
               const int num_states,
               const int *states,
               PetscReal *outputs,
               void *args) {
  int (*X)[6] = ( int (*)[6] ) states;
  for (int i = 0; i < num_states; ++i) {
    outputs[i] = transreg_propensity(X[i], reaction);
  }
  return 0;
}

// Function to constraint the shape of the Fsp
void lhs_constr(PetscInt num_species, PetscInt num_constrs, PetscInt num_states, PetscInt *states, int *vals,
                void *args) {
  for (int j{0}; j < num_states; ++j) {
    for (int i{0}; i < 6; ++i) {
      vals[j * num_constrs + i] = states[num_species * j + i];
    }
  }
}

arma::Row<int> rhs_constr{10, 6, 1, 2, 1, 1};
arma::Row<double> expansion_factors{0.5, 0.5, 0.5, 0.5, 0.5, 0.5};
arma::Row<int> rhs_constr_hyperrec{10, 6, 1, 2, 1, 1};
arma::Row<double> expansion_factors_hyperrec{0.2, 0.2, 0.2, 0.2, 0.2, 0.2};

// function to compute the time-dependent coefficients of the propensity functions
int t_fun(PetscReal t, int nc, double *vals, void *args) {
  arma::Row<PetscReal> u(vals, 10, false, true);
  u.fill(1.0);

  PetscReal AV = 6.022140857 * 1.0e8 * pow(2.0, t / avg_cell_cyc_time); // cell's volume
  u(4) = 0.012e09 / AV;
  u(6) = 0.00012e09 / AV;
  u(8) = 0.05e09 / AV;
  return 0;
}
}

using arma::dvec;
using arma::Col;
using arma::Row;

using std::cout;
using std::endl;

using namespace six_species_cme;
using namespace pacmensl;

void output_marginals(MPI_Comm comm, std::string model_name, PartitioningType fsp_par_type,
                      PartitioningApproach fsp_repart_approach, std::string constraint_type,
                      DiscreteDistribution &solution, arma::Row<int> constraints);

void output_performance(MPI_Comm comm,std::string &model_name,PartitioningType fsp_par_type,
                        PartitioningApproach fsp_repart_approach,std::string constraint_type,
                        ODESolverType ode_type,
                        FspSolverMultiSinks &fsp_solver);

int ParseOptions(MPI_Comm comm, PartitioningType &fsp_par_type, PartitioningApproach &fsp_repart_approach,
                 PetscBool &output_marginal, PetscBool &fsp_log_events);

int main(int argc, char *argv[]) {
  Environment my_env(&argc, &argv, help);
  PetscMPIInt ierr, myRank, num_procs;
  PetscErrorCode petsc_err;

  MPI_Comm comm;
  MPI_Comm_dup(PETSC_COMM_WORLD, &comm);
  MPI_Comm_size(comm, &num_procs);

  // Register PETSc stages for profiling
  PetscLogStage stages[2];
  petsc_err = PetscLogStageRegister("Solve with adaptive default state set shape", &stages[0]);
  CHKERRQ(petsc_err);
  petsc_err = PetscLogStageRegister("Solve with fixed default state set shape", &stages[1]);
  CHKERRQ(petsc_err);

  std::string part_type;
  std::string part_approach;

  std::string model_name = "transcr_reg_6d";
  Model model(SM, t_fun, propensity, nullptr, nullptr, {4, 6, 8});

  PetscReal t_final = 60.00 * 5;
  PetscReal fsp_tol = 1.0e-4;
  arma::Mat<PetscInt> X0 = {2, 6, 0, 2, 0, 0};
  X0 = X0.t();
  arma::Col<PetscReal> p0 = {1.0};
  arma::Mat<PetscInt> stoich_mat = SM;

  // Default options
  PartitioningType fsp_par_type = PartitioningType::GRAPH;
  PartitioningApproach fsp_repart_approach = PartitioningApproach::REPARTITION;
  ODESolverType fsp_odes_type = CVODE;
  PetscBool output_marginal = PETSC_FALSE;
  PetscBool fsp_log_events = PETSC_FALSE;

  ierr = ParseOptions(comm, fsp_par_type, fsp_repart_approach, output_marginal, fsp_log_events);
  CHKERRQ(ierr);

  FspSolverMultiSinks fsp_solver(comm, fsp_par_type, CVODE);
  fsp_solver.SetFromOptions();
  fsp_solver.SetInitialDistribution(X0, p0);
  fsp_solver.SetModel(model);
  DiscreteDistribution solution;

  petsc_err = PetscLogStagePush(stages[0]);
  CHKERRQ(petsc_err);
  // Solve using adaptive default constraints
  fsp_solver.SetInitialBounds(rhs_constr);
  fsp_solver.SetInitialBounds(rhs_constr_hyperrec);
  fsp_solver.SetExpansionFactors(expansion_factors_hyperrec);
  fsp_solver.SetFromOptions();
  fsp_solver.SetUp();
  solution = fsp_solver.Solve(t_final, fsp_tol, 0);
  std::shared_ptr<const StateSetConstrained> fss = std::static_pointer_cast<const StateSetConstrained>(fsp_solver.GetStateSet());
  arma::Row<int> final_hyperrec_constr = fss->GetShapeBounds();
  if (fsp_log_events) {
    output_performance(PETSC_COMM_WORLD, model_name, fsp_par_type, fsp_repart_approach,
                       std::string("adaptive_default"), fsp_odes_type, fsp_solver);
  }
  if (output_marginal) {
    output_marginals(PETSC_COMM_WORLD, model_name, fsp_par_type, fsp_repart_approach,
                     std::string("adaptive_default"), solution, final_hyperrec_constr);
  }
  fsp_solver.ClearState();
  PetscPrintf(comm, "\n ================ \n");

  petsc_err = PetscLogStagePop();
  CHKERRQ(petsc_err);
  petsc_err = PetscLogStagePush(stages[1]);
  CHKERRQ(petsc_err);
  // Solve using fixed default constraints
  fsp_solver.SetInitialBounds(final_hyperrec_constr);
  fsp_solver.SetUp();
  solution = fsp_solver.Solve(t_final, fsp_tol, 0);
  if (fsp_log_events) {
    output_performance(PETSC_COMM_WORLD, model_name, fsp_par_type, fsp_repart_approach,
                       std::string("fixed_default"), fsp_odes_type, fsp_solver);
  }
  if (output_marginal) {
    output_marginals(PETSC_COMM_WORLD, model_name, fsp_par_type, fsp_repart_approach,
                     std::string("fixed_default"), solution, final_hyperrec_constr);
  }
  fsp_solver.ClearState();
  PetscPrintf(comm, "\n ================ \n");

  return ierr;
}

int ParseOptions(MPI_Comm comm, PartitioningType &fsp_par_type, PartitioningApproach &fsp_repart_approach,
                 PetscBool &output_marginal, PetscBool &fsp_log_events) {
  std::string part_type;
  std::string part_approach;
  part_type = part2str(fsp_par_type);
  part_approach = partapproach2str(fsp_repart_approach);

  // Read options for fsp
  char opt[100];
  PetscBool opt_set;
  int ierr;
  ierr = PetscOptionsGetString(NULL, PETSC_NULL, "-fsp_partitioning_type", opt, 100, &opt_set);
  CHKERRQ(ierr);
  if (opt_set) {
    fsp_par_type = str2part(std::string(opt));
  }

  ierr = PetscOptionsGetString(NULL, PETSC_NULL, "-fsp_repart_approach", opt, 100, &opt_set);
  CHKERRQ(ierr);
  if (opt_set) {
    fsp_repart_approach = str2partapproach(std::string(opt));
  }

  ierr = PetscOptionsGetString(NULL, PETSC_NULL, "-fsp_output_marginal", opt, 100, &opt_set);
  CHKERRQ(ierr);
  if (opt_set) {
    if (strcmp(opt, "1") == 0 || strcmp(opt, "true") == 0) {
      output_marginal = PETSC_TRUE;
    }
  }

  ierr = PetscOptionsGetString(NULL, PETSC_NULL, "-fsp_log_events", opt, 100, &opt_set);
  CHKERRQ(ierr);
  if (opt_set) {
    if (strcmp(opt, "1") == 0 || strcmp(opt, "true") == 0) {
      fsp_log_events = PETSC_TRUE;
    }
  }
  PetscPrintf(comm, "Partitiniong option %s \n", part2str(fsp_par_type).c_str());
  PetscPrintf(comm, "Repartitoning option %s \n", partapproach2str(fsp_repart_approach).c_str());
  return 0;
}

void output_performance(MPI_Comm comm,std::string &model_name,PartitioningType fsp_par_type,
                        PartitioningApproach fsp_repart_approach,std::string constraint_type,
                        ODESolverType ode_type,
                        FspSolverMultiSinks &fsp_solver)
{
  int myRank,num_procs;
  MPI_Comm_rank(comm,&myRank);
  MPI_Comm_size(comm,&num_procs);

  std::string ode;
  if (ode_type == KRYLOV)
  {
    ode = "krylov";
  } else
  {
    ode = "cvode";
  }

  std::string part_type;
  std::string part_approach;
  part_type     = part2str(fsp_par_type);
  part_approach = partapproach2str(fsp_repart_approach);

  // Output time breakdowns
  FspSolverComponentTiming    sum_times, min_times, max_times;
  sum_times = fsp_solver.ReduceComponentTiming("sum");
  min_times = fsp_solver.ReduceComponentTiming("min");
  max_times = fsp_solver.ReduceComponentTiming("max");

  if (myRank == 0)
  {
    struct stat buffer;
    int fstat;

    std::string   filename =
                      model_name + "_time_breakdown.dat";

    fstat = stat (filename.c_str(), &buffer);

    std::ofstream file;
    file.open(filename,std::ios_base::app);

    if (fstat != 0){
      file << "ncpu, partitioner, fsp_shape, ode_solver, min_cput, max_cput, avg_cput, mat_gen_time, ode_time, state_expand_time, min_flops, max_flops, avg_flops \n";
    }

    file << num_procs << ","
         << part_type << ","
         << constraint_type << ","
         << ode << ","
         << min_times.TotalTime << ","
         << max_times.TotalTime << ","
         << sum_times.TotalTime/num_procs << ","
         << sum_times.MatrixGenerationTime/num_procs << ","
         << sum_times.ODESolveTime/num_procs << ","
         << sum_times.StatePartitioningTime/num_procs
         << min_times.TotalFlops << ","
         << max_times.TotalFlops << ","
         << sum_times.TotalFlops/num_procs << "\n"
        ;
    file.close();
  }

  FiniteProblemSolverPerfInfo perf_info   = fsp_solver.GetSolverPerfInfo();

  if (myRank == 0){
    std::string filename =
                    model_name + "_perf_info_" + std::to_string(num_procs) + "_" + part_type + "_" + part_approach + "_" +
                        constraint_type + ".dat";
    std::ofstream file;
    file.open(filename);
    file << "Model time, ODEs size, Average processor time (sec) \n";
    for (auto i{0}; i < perf_info.n_step; ++i)
    {
      file << perf_info.model_time[i] << "," << perf_info.n_eqs[i] << "," << perf_info.cpu_time[i] << "\n";
    }
    file.close();
  }
}

void output_marginals(MPI_Comm comm, std::string model_name, PartitioningType fsp_par_type,
                      PartitioningApproach fsp_repart_approach, std::string constraint_type,
                      DiscreteDistribution &solution, arma::Row<int> constraints) {
  int myRank, num_procs;
  MPI_Comm_rank(comm, &myRank);
  MPI_Comm_size(comm, &num_procs);

  std::string part_type;
  std::string part_approach;
  part_type = part2str(fsp_par_type);
  part_approach = partapproach2str(fsp_repart_approach);

  /* Compute the marginal distributions */
  std::vector<arma::Col<PetscReal>> marginals(solution.states_.n_rows);
  for (PetscInt i{0}; i < marginals.size(); ++i) {
    marginals[i] = Compute1DMarginal(solution, i);
  }

  MPI_Comm_rank(PETSC_COMM_WORLD, &myRank);
  if (myRank == 0) {
    for (PetscInt i{0}; i < marginals.size(); ++i) {
      std::string filename =
          model_name + "_marginal_" + std::to_string(i) + "_" + std::to_string(num_procs) + "_" +
              part_type + "_" + part_approach + "_" + constraint_type + ".dat";
      marginals[i].save(filename, arma::raw_ascii);
    }
    std::string filename =
        model_name + "_constraint_bounds_" + std::to_string(num_procs) + "_" + part_type + "_" + part_approach +
            "_" + constraint_type + ".dat";
    constraints.save(filename, arma::raw_ascii);
  }
}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [stirling]
#include <boost/simd/eulerian.hpp>
#include <boost/simd/pack.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_fd = bs::pack <double, 4>;

int main() {
  pack_fd pd = {170.0, 2.0, 35.0, 10.0};

  std::cout
    << "---- simd" << '\n'
    << "<- pd =               " << pd << '\n'
    << "-> bs::stirling(pd) = " << bs::stirling(pd) << '\n';

  double xf = 40.0;

  std::cout
    << "---- scalar"  << '\n'
    << "<- xf =               " << xf << '\n'
    << "-> bs::stirling(xf) = " << bs::stirling(xf) << '\n';
  return 0;
}
//! [stirling]


#include <boost/range/adaptors.hpp>
namespace ba = boost::adaptors;

#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
namespace py = pybind11;

#include "filtration.h"
#include "diagram.h"
#include "omni-field-persistence.h"

PyOmniFieldPersistence
omnifield_homology_persistence(const PyFiltration& filtration)
{
    PyOmniFieldPersistence persistence;
    for(auto& s : filtration)
    {
        using SimplexChainEntry = dionysus::ChainEntry<PyOmniFieldPersistence::Field, PySimplex>;
        using ChainEntry        = dionysus::ChainEntry<PyOmniFieldPersistence::Field, PyOmniFieldPersistence::Index>;
        persistence.add(s.boundary(persistence.field()) |
                                                 ba::transformed([&filtration](const SimplexChainEntry& e)
                                                 { return ChainEntry(e.element(), filtration.index(e.index())); }));
    }
    return persistence;
}

std::vector<PyDiagram>
py_init_omni_diagrams(const PyOmniFieldPersistence& persistence, const PyFiltration& f, PyOmniFieldPersistence::BaseElement p)
{
    return init_diagrams(prime_adapter(persistence, p), f,
                         [](const PySimplex& s)                         { return s.data(); },        // value
                         [](PyOmniFieldPersistence::Index i) -> PyIndex { return i; });              // data
}

PYBIND11_MAKE_OPAQUE(PyOmniFieldPersistence::ZpChain);      // persistence.cpp provides a binding for Chain, which is exactly what this is

void init_omnifield_persistence(py::module& m)
{
    using namespace pybind11::literals;
    m.def("omnifield_homology_persistence",   &omnifield_homology_persistence, "filtration"_a,
          "compute homology persistence of the filtration (pair simplices) over all fields at once");

    m.def("init_diagrams",      &py_init_omni_diagrams,  "ofp"_a, "f"_a, "p"_a,  "initialize diagrams for a specific prime from omnifield persistence and filtration");

    using Index         = PyOmniFieldPersistence::Index;
    using BaseElement   = PyOmniFieldPersistence::BaseElement;
    py::class_<PyOmniFieldPersistence>(m, "OmniFieldPersistence", "compact composition of multiple reduced matrices")
        .def("primes",  &PyOmniFieldPersistence::primes,    "primes over which the matrix differs from the rest")
        .def("column",  [](const PyOmniFieldPersistence& ofp, Index i, BaseElement p)
                        {
                            auto it = ofp.zp_chains().find(i);
                            if (it != ofp.zp_chains().end())
                            {
                                auto pit = it->second.find(p);
                                if (pit != it->second.end())
                                    return pit->second;
                            }
                            return ofp.convert(ofp.q_chains()[i], ofp.zp(p));
                        },                                  "get the column over a specific prime")
        .def("special", &PyOmniFieldPersistence::special,   "test whether the column has a special value over the given prime")
        .def("__len__", &PyOmniFieldPersistence::size,      "size of the persistence object")
        .def("__repr__",    [](const PyOmniFieldPersistence& ofp)
                            { std::ostringstream oss; oss << "OmniFieldPersistence with " << ofp.size() << " columns"; return oss.str(); })
    ;
}


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

  @copyright 2012-2015 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate the  logarithm of the Eps constant.

    @par Semantic:

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

    is similar to:

    @code
      r =  log(Eps<T>());
    @endcode

    @return The Logeps constant for the proper type
  **/
  template<typename T> T Logeps();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant logeps.

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

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

#endif


/*
 * lorenz_reconstruct.cpp
 * Date: 2016-02-26
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 * Copyright: Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include "serialize.hpp"

#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/integrate/integrate_const.hpp>

#include <iostream>
#include <fstream>
#include <iterator>

struct lorenz_reconstructed
{
    lorenz_reconstructed( dynsys::individual_type individual , dynsys::norm_type xnorm , dynsys::norm_type ynorm )
    : m_individual( std::move( individual ) ) , m_xnorm( std::move( xnorm ) ) , m_ynorm( std::move( ynorm ) )
    {}

    void operator()( dynsys::state_type x , dynsys::state_type& dxdt , double t ) const
    {
        denormalize( x , m_xnorm );
        dxdt[0] = m_individual[0].root()->eval( x );
        dxdt[1] = m_individual[1].root()->eval( x );
        dxdt[2] = m_individual[2].root()->eval( x );
        denormalize( dxdt , m_ynorm );
    }

    static void denormalize( dynsys::state_type& x , dynsys::norm_type const& norm )
    {
        assert( x.size() == norm.size() );
        for( size_t i=0 ; i<x.size() ; ++i )
            x[i] = ( x[i] - norm[i].first ) / norm[i].second;
    }

    static void normalize( dynsys::state_type& x , dynsys::norm_type const& norm )
    {
        assert( x.size() == norm.size() );
        for( size_t i=0 ; i<x.size() ; ++i )
            x[i] = ( x[i] + norm[i].first ) * norm[i].second;
    }

    dynsys::individual_type m_individual;
    dynsys::norm_type m_xnorm;
    dynsys::norm_type m_ynorm;
};

int main( int argc , char** argv )
{
    if( argc != 2 )
    {
        std::cerr << "usage: " << argv[0] << " winner-file" << "\n";
        return -1;
    }

    std::ifstream fin { argv[1] };

    std::string json_string { std::istream_iterator< char >{ fin } , std::istream_iterator< char >{ } };
    auto winner = dynsys::deserialize_winner( json_string );

    std::cout << "Expressions" << std::endl;
    std::cout << gpcxx::polish_string( winner.trees[0] ) << std::endl;
    std::cout << gpcxx::polish_string( winner.trees[1] ) << std::endl;
    std::cout << gpcxx::polish_string( winner.trees[2] ) << std::endl;
    std::cout << std::endl;

    std::cout << "Normalization" << std::endl;
    for( size_t i=0 ; i<dynsys::dim ; ++i )
        std::cout << winner.xnorm[i].first << " " << winner.xnorm[i].second << " " << winner.ynorm[i].first << " " << winner.ynorm[i].second << "\n";
    std::cout << std::endl;

    auto sys = lorenz_reconstructed { winner.trees , winner.xnorm , winner.ynorm };

    using stepper_type = boost::numeric::odeint::runge_kutta4< dynsys::state_type > ;

    dynsys::state_type x {{ 10.0 , 10.0 , 10.0 }};
    stepper_type stepper;
    std::ofstream fout { "reconstruct.dat" };
    boost::numeric::odeint::integrate_const( stepper , sys , x , 0.0 , 100.0 , 0.1 , [&fout]( auto const& x , auto t ) {
        fout << x[0] << " " << x[1] << " " << x[2] << "\n"; } );

    std::ofstream fout2 { "functions.dat" };
    for( double x = -20.0 ; x <= 20.0 ; x += 2.0 )
    {
        for( double y = -20.0 ; y <= 20.0 ; y += 2.0 )
        {
            for( double z = -20.0 ; z <= 20.0 ; z += 2.0 )
            {
                dynsys::state_type xx {{ x , y , z }};
                dynsys::state_type f;
                sys( xx , f , 0.0 );
                fout2 << x << " " << y << " " << z << " " << f[0] << " " << f[1] << " " << f[2] << "\n";
            }
        }
    }

    return 0;
}


#pragma once

#include <iostream>
#include <cmath>
#include <string>

#include "GNAObject.hh"
#include "TypesFunctions.hh"
#include <Eigen/Dense>

class GeoNeutrinoFluxNormed: public GNASingleObject,
                public TransformationBind<GeoNeutrinoFluxNormed> {
public:
  GeoNeutrinoFluxNormed(double livetime_years): m_livetime_years(livetime_years) {
    variable_(&m_fluxnorm, "FluxNorm");
    transformation_("flux_norm")
      .input("flux")
      .output("normed_flux")
      .types(TypesFunctions::ifSame, TypesFunctions::pass<0>)
      .func(&GeoNeutrinoFluxNormed::CalcNorm);
  }
protected:
  void CalcNorm(FunctionArgs& fargs) {
      auto& args=fargs.args;
      const double* events = args[0].x.data();
      const size_t insize = args[0].type.size();
      size_t first_non_nan_idx{0};
      // find first not nan assuming all other values are not NaN!
      for (; first_non_nan_idx < insize; ++first_non_nan_idx) {
        if (std::isnan(events[first_non_nan_idx])) {
            ++first_non_nan_idx;
        }
        else {
            break;
        }
      }
      const double total_events = std::accumulate(events + first_non_nan_idx, events + insize, 0.);

      fargs.rets[0].x = (m_fluxnorm/total_events) * m_livetime_years * fargs.args[0].x;
  }

  variable<double> m_fluxnorm;

  double m_livetime_years;
};


#include <iostream>
#include <cstdlib>
#include <utility>

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

#include <amgcl/amg.hpp>
#include <amgcl/make_solver.hpp>
#include <amgcl/backend/builtin.hpp>
#include <amgcl/adapter/ublas.hpp>
#include <amgcl/coarsening/smoothed_aggregation.hpp>
#include <amgcl/relaxation/spai0.hpp>
#include <amgcl/solver/bicgstabl.hpp>
#include <amgcl/profiler.hpp>

#include "sample_problem.hpp"

typedef boost::numeric::ublas::compressed_matrix<
    double, boost::numeric::ublas::row_major
    > ublas_matrix;

typedef boost::numeric::ublas::vector<double> ublas_vector;

namespace amgcl {
    profiler<> prof;
}

int main(int argc, char *argv[]) {
    using amgcl::prof;

    std::vector<int>    ptr;
    std::vector<int>    col;
    std::vector<double> val;
    std::vector<double> rhs;

    prof.tic("assemble");
    int m = argc > 1 ? atoi(argv[1]) : 128;
    int n = sample_problem(m, val, col, ptr, rhs);

    // Create ublas matrix with the data.
    ublas_matrix A(n, n);
    A.reserve(ptr[n]);

    for(int i = 0; i < n; ++i)
        for(int j = ptr[i], e = ptr[i+1]; j < e; ++j)
            A.push_back(i, col[j], val[j]);
    prof.toc("assemble");

    prof.tic("build");
    amgcl::make_solver<
        amgcl::amg<
            amgcl::backend::builtin<double>,
            amgcl::coarsening::smoothed_aggregation,
            amgcl::relaxation::spai0
            >,
        amgcl::solver::bicgstabl<
            amgcl::backend::builtin<double>
            >
        > solve( amgcl::backend::map(A) );
    prof.toc("build");

    std::cout << solve.precond() << std::endl;

    ublas_vector x(n, 0);

    prof.tic("solve");
    size_t iters;
    double resid;
    std::tie(iters, resid) = solve(rhs, x);
    prof.toc("solve");

    std::cout << "Iterations: " << iters << std::endl
              << "Error:      " << resid << std::endl
              << std::endl << prof << std::endl;
}


#include "interpolate_bilinearly.h"
#include "matrix.h"
#include "round_point.h"
#include <boost/multi_array.hpp>
#include <iostream>
#include <vector>

Point interpolate_point_bilinearly(Point p1,
                                   const boost::multi_array<double, 2> *xdisp,
                                   const boost::multi_array<double, 2> *ydisp,
                                   const unsigned int lx,
                                   const unsigned int ly)
{
  const double intp_x =
    interpolate_bilinearly(p1.x(), p1.y(), xdisp, 'x', lx, ly);
  const double intp_y =
    interpolate_bilinearly(p1.x(), p1.y(), ydisp, 'y', lx, ly);
  return Point(p1.x() + intp_x, p1.y() + intp_y);
}

void InsetState::project()
{
  // Calculate displacement from proj array
  boost::multi_array<double, 2> xdisp(boost::extents[lx_][ly_]);
  boost::multi_array<double, 2> ydisp(boost::extents[lx_][ly_]);
  for (unsigned int i = 0; i < lx_; ++i) {
    for (unsigned int j=0; j<ly_; ++j) {
      xdisp[i][j] = proj_[i][j].x - i - 0.5;
      ydisp[i][j] = proj_[i][j].y - j - 0.5;
    }
  }

  // Cumulative projection
  for (unsigned int i = 0; i < lx_; ++i) {
    for (unsigned int j = 0; j < ly_; ++j) {

      // TODO: Should the interpolation be made on the basis of triangulation?
      // Calculate displacement for cumulative graticule coordinates
      const double graticule_intp_x = interpolate_bilinearly(
        cum_proj_[i][j].x,
        cum_proj_[i][j].y,
        &xdisp, 'x', lx_, ly_);
      const double graticule_intp_y = interpolate_bilinearly(
        cum_proj_[i][j].x,
        cum_proj_[i][j].y,
        &ydisp, 'y', lx_, ly_);

      // Update cumulative graticule coordinates
      cum_proj_[i][j].x += graticule_intp_x;
      cum_proj_[i][j].y += graticule_intp_y;
    }
  }

  // Specialise/curry interpolate_point_bilinearly such that it only requires
  // one argument (Point p1).
  std::function<Point(Point)> lambda =
    [&xdisp, &ydisp, lx = lx_, ly = ly_](Point p1) {
      return interpolate_point_bilinearly(p1, &xdisp, &ydisp, lx, ly);
    };

  // Apply "lambda" to all points
  transform_points(lambda);
  return;
}

// In chosen_diag() and transformed_triangle(), the input x-coordinates can
// only be 0, lx, or 0.5, 1.5, ..., lx-0.5. A similar rule applies to the
// y-coordinates.
void InsetState::exit_if_not_on_grid_or_edge(const Point p1) const
{
  if ((p1.x() != 0.0 && p1.x() != lx_ && p1.x() - int(p1.x()) != 0.5) ||
      (p1.y() != 0.0 && p1.y() != ly_ && p1.y() - int(p1.y()) != 0.5)) {
    std::cerr << "Error: Invalid input coordinate in triangulation\n"
              << "\tpt = ("
              << p1.x()
              << ", "
              << p1.y()
              << ")"
              << std::endl;
    exit(1);
  }
  return;
}

Point InsetState::projected_point(const Point p1)
{
  exit_if_not_on_grid_or_edge(p1);
  const unsigned int proj_x = std::min(
    static_cast<unsigned int>(lx_) - 1,
    static_cast<unsigned int>(p1.x()));
  const unsigned int proj_y = std::min(
    static_cast<unsigned int>(ly_) - 1,
    static_cast<unsigned int>(p1.y()));
  return Point((p1.x() == 0.0 || p1.x() == lx_) ?
               p1.x() :
               proj_[proj_x][proj_y].x,
               (p1.y() == 0.0 || p1.y() == ly_) ?
               p1.y() :
               proj_[proj_x][proj_y].y);
}

// TODO: chosen_diag() seems to be more naturally thought of as a boolean
// than an integer.

// For a graticule cell with corners stored in the XYPoint array v, determine
// whether the diagonal from v[0] to v[2] is inside the graticule cell. If
// yes, return 0. Otherwise, if the diagonal from v[1] to v[3] is inside the
// graticule cell, return 1. If neither of the two diagonals is inside the
// graticule cell, then the cell's topology is invalid; thus, we exit with an
// error message.
int InsetState::chosen_diag(const Point v[4], unsigned int *num_concave)
{
  // The input v[i].x can only be 0, lx, or 0.5, 1.5, ..., lx-0.5. A similar
  // rule applies to the y-coordinates.
  for (unsigned int i = 0; i < 4; ++i) {
    exit_if_not_on_grid_or_edge(v[i]);
  }

  // Transform the coordinates in v to the corresponding coordinates on the
  // projected grid. If the x-coordinate is 0 or lx, we keep the input. The
  // input v[i].x can only be 0, lx, or 0.5, 1.5, ..., lx-0.5. A similar rule
  // applies to the y-coordinates. This condition is checked in
  // projected_point().
  Point tv[4];
  for (unsigned int i = 0; i < 4; ++i) {
    tv[i] = projected_point(v[i]);
  }

  // Get the two possible midpoints
  const Point midpoint0(
    (tv[0].x() + tv[2].x()) / 2,
    (tv[0].y() + tv[2].y()) / 2);
  const Point midpoint1(
    (tv[1].x() + tv[3].x()) / 2,
    (tv[1].y() + tv[3].y()) / 2);

  // Get the transformed graticule cell as a polygon
  Polygon trans_graticule;
  for (unsigned int i = 0; i < 4; ++i) {
    trans_graticule.push_back(tv[i]);
  }

  // Check if graticule cell is concave
  if (!trans_graticule.is_convex()) {
    *num_concave += 1;
  }
  if (trans_graticule.bounded_side(midpoint0) == CGAL::ON_BOUNDED_SIDE) {
    return 0;
  }
  if (trans_graticule.bounded_side(midpoint1) == CGAL::ON_BOUNDED_SIDE) {
    return 1;
  }
  std::cerr << "Invalid graticule cell! At\n";
  std::cerr << "(" << tv[0].x() << ", " << tv[0].y() << ")\n";
  std::cerr << "(" << tv[1].x() << ", " << tv[1].y() << ")\n";
  std::cerr << "(" << tv[2].x() << ", " << tv[2].y() << ")\n";
  std::cerr << "(" << tv[3].x() << ", " << tv[3].y() << ")\n";
  std::cerr << "Original: \n";
  std::cerr << "(" << v[0].x() << ", " << v[0].y() << ")\n";
  std::cerr << "(" << v[1].x() << ", " << v[1].y() << ")\n";
  std::cerr << "(" << v[2].x() << ", " << v[2].y() << ")\n";
  std::cerr << "(" << v[3].x() << ", " << v[3].y() << ")\n";
  std::cerr << "i: "
            << static_cast<unsigned int>(v[0].x())
            << ", j: "
            << static_cast<unsigned int>(v[0].y())
            << std::endl;
  exit(1);
}

void InsetState::fill_graticule_diagonals()
{
  // Initialize array if running for the first time
  if (graticule_diagonals_.shape()[0] != lx_ ||
      graticule_diagonals_.shape()[1] != ly_) {
    graticule_diagonals_.resize(boost::extents[lx_ - 1][ly_ - 1]);
  }
  unsigned int n_concave = 0;  // Count concave graticule cells
  for (unsigned int i = 0; i < lx_ - 1; ++i) {
    for (unsigned int j = 0; j < ly_ - 1; ++j) {
      Point v[4];
      v[0] = Point(double(i) + 0.5, double(j) + 0.5);
      v[1] = Point(double(i) + 1.5, double(j) + 0.5);
      v[2] = Point(double(i) + 1.5, double(j) + 1.5);
      v[3] = Point(double(i) + 0.5, double(j) + 1.5);
      graticule_diagonals_[i][j] = chosen_diag(v, &n_concave);
    }
  }
  std::cerr << "Number of concave graticule cells: "
            << n_concave
            << std::endl;
  return;
}

std::array<Point, 3> InsetState::transformed_triangle(const std::array<Point, 3>
                                                      tri)
{
  std::array<Point, 3> transf_tri;
  for (unsigned int i = 0; i < 3; ++i) {
    exit_if_not_on_grid_or_edge(tri[i]);
    const auto transf_pt = projected_point(tri[i]);
    transf_tri[i] = transf_pt;
  }
  return transf_tri;
}

// Determine if a point `pt` is on the boundary of a triangle by using cross
// products to find areas spanned by pt and each triangle edge. Idea from:
// https://stackoverflow.com/questions/7050186/find-if-point-lies-on-line-segment
// This function is needed because, sometimes,
// `triangle.bounded_side(Point(x, y)) == CGAL::ON_BOUNDARY` does not return
// `true` even if the point is on the boundary.
bool is_on_triangle_boundary(const Point pt, const Polygon triangle)
{
  for (unsigned int i = 0; i < triangle.size(); ++i) {
    const auto t1 = triangle[i];
    const auto t2 = triangle[(i == triangle.size() - 1) ? 0 : i + 1];
    const double area = (t1.x() - pt.x()) * (t2.y() - pt.y()) -
                        (t2.x() - pt.x()) * (t1.y() - pt.y());
    if (almost_equal(area, 0.0)) {
      return true;
    }
  }
  return false;
}

// Get the untransformed coordinates of the triangle in which the point `pt`
// is located. After transformation, this triangle must be entirely inside
// the transformed graticule cell.
std::array<Point, 3> InsetState::untransformed_triangle(const Point pt)
{
  if (pt.x() < 0 || pt.x() > lx_ || pt.y() < 0 || pt.y() > ly_) {
    CGAL::set_pretty_mode(std::cerr);
    std::cerr << "ERROR: coordinate outside bounding box in "
              << __func__
              << "().\npt = "
              << pt
              << std::endl;
    exit(1);
  }

  // Get original graticule coordinates
  Point v[4];
  v[0] = Point(std::max(0.0, floor(pt.x() + 0.5) - 0.5),
               std::max(0.0, floor(pt.y() + 0.5) - 0.5));
  v[1] = Point(std::min(static_cast<double>(lx_), floor(pt.x() + 0.5) + 0.5),
               v[0].y());
  v[2] = Point(v[1].x(),
               std::min(static_cast<double>(ly_), floor(pt.y() + 0.5) + 0.5));
  v[3] = Point(v[0].x(),
               v[2].y());

  // TODO: diag SEEMS TO BE MORE NATURALLY THOUGHT OF AS bool INSTEAD OF int.
  // Assuming that the transformed graticule does not have self-intersections,
  // at least one of the diagonals must be completely inside the graticule.
  // We use that diagonal to split the graticule into two triangles.
  int diag;
  if (v[0].x() == 0.0 || v[0].y() == 0.0 || v[2].x() == lx_ || v[2].y() == ly_) {

    // Case where the graticule is on the edge of the grid.
    // We calculate the chosen diagonal, as graticule_diagonals does not store
    // the diagonals for edge grid cells.
    unsigned int n_concave = 0;
    diag = chosen_diag(v, &n_concave);
  } else {

    // Case where the graticule is not on the edge of the grid. We can find the
    // already computed chosen diagonal in graticule_diagonals_.
    const unsigned int x = static_cast<unsigned int>(v[0].x());
    const unsigned int y = static_cast<unsigned int>(v[0].y());
    diag = graticule_diagonals_[x][y];
  }

  // Get the two possible triangles
  Polygon triangle1;
  Polygon triangle2;
  if (diag == 0) {
    triangle1.push_back(v[0]);
    triangle1.push_back(v[1]);
    triangle1.push_back(v[2]);
    triangle2.push_back(v[0]);
    triangle2.push_back(v[2]);
    triangle2.push_back(v[3]);
  } else {
    triangle1.push_back(v[0]);
    triangle1.push_back(v[1]);
    triangle1.push_back(v[3]);
    triangle2.push_back(v[1]);
    triangle2.push_back(v[2]);
    triangle2.push_back(v[3]);
  }

  // Determine which untransformed triangle the given point is in. If the
  // point is in neither, an error is raised.
  std::array<Point, 3> triangle_coordinates;
  if ((triangle1.bounded_side(pt) == CGAL::ON_BOUNDED_SIDE) ||
      is_on_triangle_boundary(pt, triangle1)) {
    for (unsigned int i = 0; i < triangle1.size(); ++i) {
      triangle_coordinates[i] = triangle1[i];
    }
  } else if ((triangle2.bounded_side(pt) == CGAL::ON_BOUNDED_SIDE) ||
             is_on_triangle_boundary(pt, triangle2)) {
    for (unsigned int i = 0; i < triangle2.size(); ++i) {
      triangle_coordinates[i] = triangle2[i];
    }
  } else {
    std::cerr << "Point not in graticule cell!\n";
    std::cerr << "Point coordinates:\n";
    std::cerr << "(" << pt.x() << ", " << pt.y() << ")\n";
    std::cerr << "Original graticule cell:\n";
    std::cerr << "(" << v[0].x() << ", " << v[0].y() << ")\n";
    std::cerr << "(" << v[1].x() << ", " << v[1].y() << ")\n";
    std::cerr << "(" << v[2].x() << ", " << v[2].y() << ")\n";
    std::cerr << "(" << v[3].x() << ", " << v[3].y() << ")\n";
    std::cerr << "Chosen diagonal: " << diag << "\n";
    exit(1);
  }
  return triangle_coordinates;
}

Point affine_trans(const std::array<Point, 3> tri,
                   const std::array<Point, 3> org_tri,
                   const Point pt)
{
  // For each point, we make the following transformation. Suppose we find
  // that, before the cartogram transformation, a point (x, y) is in the
  // triangle (a, b, c). We want to find its position in the projected
  // triangle (p, q, r). We locally approximate the cartogram transformation
  // by an affine transformation T such that T(a) = p, T(b) = q and T(c) = r.
  // We can think of T as a 3x3 matrix
  //    -----------
  //   |t11 t12 t13|
  //   |t21 t22 t23|  such that
  //   | 0   0   1 |
  //    -----------
  //    -----------   ----------     ----------
  //   |t11 t12 t13| | a1 b1 c1 |   | p1 q1 r1 |
  //   |t21 t22 t23| | a2 b2 c2 | = | p2 q2 r2 | or TA = P.
  //   | 0   0   1 | | 1  1  1  |   |  1  1  1 |
  //    -----------   ----------     ----------
  // Hence, T = PA^{-1}.
  //                              -----------------------
  //                             |b2-c2 c1-b1 b1*c2-b2*c1|
  // We have A^{-1} = (1/det(A)) |c2-a2 a1-c1 a2*c1-a1*c2|. By multiplying
  //                             |a2-b2 b1-a1 a1*b2-a2*b1|
  //                              -----------------------
  // PA^{-1} we obtain t11, t12, t13, t21, t22, and t23. If the original
  // coordinates are (x, y) on the unprojected map, then the transformed
  // coordinates are:
  // post.x = t11*x + t12*y + t13, post.y = t21*x + t22*y + t23.
  const Point pre(pt.x(),pt.y());

  // Old triangle (a, b, c) expressed as matrix A
  const Matrix abc_mA(org_tri[0], org_tri[1], org_tri[2]);

  // New triangle (p, q, r) expressed as matrix P
  const Matrix pqr_mP(tri[0], tri[1], tri[2]);

  // Transformation matrix T
  const auto mT = pqr_mP.multiplied_with(abc_mA.inverse());

  // Transformed point
  return mT.transformed_point(pre);
}

Point InsetState::projected_point_with_triangulation(const Point pt)
{
  // Get the untransformed triangle the point is in
  const auto old_triangle = untransformed_triangle(pt);

  // Get the coordinates of the transformed triangle
  const auto new_triangle = transformed_triangle(old_triangle);

  // Get the transformed point and return it
  const auto transformed_pt = affine_trans(new_triangle, old_triangle, pt);
  return rounded_point(transformed_pt, lx_, ly_);
}

void InsetState::project_with_triangulation()
{

  // Store reference to current object and call member function
  // projected_point_with_triangulation
  // https://www.nextptr.com/tutorial/ta1430524603/
  // capture-this-in-lambda-expression-timeline-of-change
  std::function<Point(Point)> lambda =
    [&](Point p1) {
      return projected_point_with_triangulation(p1);
    };

  // Transforming all points based on triangulation
  transform_points(lambda);

  // Cumulative projection
  for (unsigned int i = 0; i < lx_; ++i) {
    for (unsigned int j = 0; j < ly_; ++j) {
      const Point old_cum_proj(cum_proj_[i][j].x, cum_proj_[i][j].y);
      const auto new_cum_proj_pt =
        projected_point_with_triangulation(old_cum_proj);
      cum_proj_[i][j].x = new_cum_proj_pt.x();
      cum_proj_[i][j].y = new_cum_proj_pt.y();
    }
  }
  return;
}


#include "learning/learningIO.h"
#include <learning/learningMath.h>
#include <processing/graphCut.h>
#include <util/helper.h>
#include <cstdlib>
#include <ctime>
#include <CGAL/Random.h>
#include <boost/filesystem.hpp>

#include <xtensor/xbuilder.hpp>
#include <xtensor/xmath.hpp>
#include <xtensor-io/xnpz.hpp>

namespace fs = boost::filesystem;

///////////////////////////////////////////////////////////////////////////////////
//////////////////////////////// EXPORT FROM POISSON INPUT ///////////////////////////////
///////////////////////////////////////////////////////////////////////////////////

void cellIndexAndLabel(ofstream& out, Delaunay& Dt, Delaunay::All_cells_iterator& fci, int idx, runningOptions options){

    if(options.ground_truth){
        //    il << "global_index inside_perc outside_perc inside_outside_max gc_label" << endl;
        out << idx << " ";
        out << fci->info().inside_score << " ";
        out << fci->info().outside_score << " ";
        // this means 50/50 cells will be labelled as inside
        int l = fci->info().outside_score > fci->info().inside_score ? 1 : 0;
        out << l << " ";
        out << fci->info().gc_label << " ";
        out << Dt.is_infinite(fci) << endl;
    }
    else
        out << "999 999 999 999 999 " << Dt.is_infinite(fci) << endl;

}
void cellBasedGeometricFeatures(ofstream& out, Delaunay& Dt, Delaunay::All_cells_iterator& fci){

//        cgeom << "radius vol longest_edge shortest_edge" << endl;
    if(Dt.is_infinite(fci))
        out << "0 0 0 0" << endl;
    else{
        auto tet = Dt.tetrahedron(fci);
        tetFeatures tf = calcTetFeatures(tet);
        out << tf.radius << " ";
        out << tf.vol << " ";
        out << tf.longest_edge << " ";
        out << tf.shortest_edge << " ";
        out << endl;
    }
}

void cellBasedVertexFeatures(ofstream& out, Delaunay& Dt, Delaunay::All_cells_iterator& fci){

//    cbvf << "cb_vertex_inside_count cb_vertex_inside_dist_min cb_vertex_inside_dist_max cb_vertex_inside_dist_sum ";
//    cbvf << "cb_vertex_outside_count cb_vertex_outside_dist_min cb_vertex_outside_dist_max cb_vertex_outside_dist_sum ";
//    cbvf << "cb_vertex_last_count cb_vertex_last_dist_min cb_vertex_last_dist_max cb_vertex_last_dist_sum" << endl;

    // inside
    int vdi = fci->info().cb_vertex_inside.size();
    out << vdi << " ";
    if(vdi > 0){
        out << *min_element(fci->info().cb_vertex_inside.begin(), fci->info().cb_vertex_inside.end()) << " ";
        out << *max_element(fci->info().cb_vertex_inside.begin(), fci->info().cb_vertex_inside.end()) << " ";
        out << accumulate(fci->info().cb_vertex_inside.begin(), fci->info().cb_vertex_inside.end(),0.0) << " ";

    }
    else
        out << "0 0 0 ";
    // outside
    int vdo = fci->info().cb_vertex_outside.size();
    out << vdo << " ";
    if(vdo > 0){
        out << *min_element(fci->info().cb_vertex_outside.begin(), fci->info().cb_vertex_outside.end()) << " ";
        out << *max_element(fci->info().cb_vertex_outside.begin(), fci->info().cb_vertex_outside.end()) << " ";
        out << accumulate(fci->info().cb_vertex_outside.begin(), fci->info().cb_vertex_outside.end(),0.0) << " ";
    }
    else
        out << "0 0 0 ";
    // last
    int vdl = fci->info().cb_vertex_last.size();
    out << vdl << " ";
    if(vdl > 0){
        out << *min_element(fci->info().cb_vertex_last.begin(), fci->info().cb_vertex_last.end()) << " ";
        out << *max_element(fci->info().cb_vertex_last.begin(), fci->info().cb_vertex_last.end()) << " ";
        out << accumulate(fci->info().cb_vertex_last.begin(), fci->info().cb_vertex_last.end(),0.0) << " ";
    }
    else
        out << "0 0 0 ";

    out << endl;
}

void cellBasedFacetFeatures(ofstream& out, Delaunay& Dt, Delaunay::All_cells_iterator& fci){

//    cbff << "cb_facet_inside_first_count cb_facet_inside_first_dist_min cb_facet_inside_first_dist_max cb_facet_inside_first_dist_sum ";
//    cbff << "cb_facet_inside_second_count cb_facet_inside_second_dist_min cb_facet_inside_second_dist_max cb_facet_inside_second_dist_sum ";
//    cbff << "cb_facet_outside_first_count cb_facet_outside_first_dist_min cb_facet_outside_first_dist_max cb_facet_outside_first_dist_sum ";
//    cbff << "cb_facet_outside_second_count cb_facet_outside_second_dist_min cb_facet_outside_second_dist_max cb_facet_outside_second_dist_sum ";
//    cbff << "cb_facet_last_first_count cb_facet_last_first_dist_min cb_facet_last_first_dist_max cb_facet_last_first_dist_sum" << endl;
//    cbff << "cb_facet_last_second_count cb_facet_last_second_dist_min cb_facet_last_second_dist_max cb_facet_last_second_dist_sum" << endl;

    // inside first
    int fdif = fci->info().cb_facet_inside_first.size();
    out << fdif << " ";
    if(fdif > 0){
        out << *min_element(fci->info().cb_facet_inside_first.begin(), fci->info().cb_facet_inside_first.end()) << " ";
        out << *max_element(fci->info().cb_facet_inside_first.begin(), fci->info().cb_facet_inside_first.end()) << " ";
        out << accumulate(fci->info().cb_facet_inside_first.begin(), fci->info().cb_facet_inside_first.end(),0.0) << " ";
    }
    else
        out << "0 0 0 ";
    // inside second
    int fdis = fci->info().cb_facet_inside_second.size();
    out << fdis << " ";
    if(fdis > 0){
        out << *min_element(fci->info().cb_facet_inside_second.begin(), fci->info().cb_facet_inside_second.end()) << " ";
        out << *max_element(fci->info().cb_facet_inside_second.begin(), fci->info().cb_facet_inside_second.end()) << " ";
        out << accumulate(fci->info().cb_facet_inside_second.begin(), fci->info().cb_facet_inside_second.end(),0.0) << " ";
    }
    else
        out << "0 0 0 ";
    // outside first
    int fdof = fci->info().cb_facet_outside_first.size();
    out << fdof << " ";
    if(fdof > 0){
        out << *min_element(fci->info().cb_facet_outside_first.begin(), fci->info().cb_facet_outside_first.end()) << " ";
        out << *max_element(fci->info().cb_facet_outside_first.begin(), fci->info().cb_facet_outside_first.end()) << " ";
        out << accumulate(fci->info().cb_facet_outside_first.begin(), fci->info().cb_facet_outside_first.end(),0.0) << " ";
    }
    else
        out << "0 0 0 ";
    // outside second
    int fdos = fci->info().cb_facet_outside_second.size();
    out << fdos << " ";
    if(fdos > 0){
        out << *min_element(fci->info().cb_facet_outside_second.begin(), fci->info().cb_facet_outside_second.end()) << " ";
        out << *max_element(fci->info().cb_facet_outside_second.begin(), fci->info().cb_facet_outside_second.end()) << " ";
        out << accumulate(fci->info().cb_facet_outside_second.begin(), fci->info().cb_facet_outside_second.end(),0.0) << " ";
    }
    else
        out << "0 0 0 ";
    // last first
    int fdlf = fci->info().cb_facet_last_first.size();
    out << fdlf << " ";
    if(fdlf > 0){
        out << *min_element(fci->info().cb_facet_last_first.begin(), fci->info().cb_facet_last_first.end()) << " ";
        out << *max_element(fci->info().cb_facet_last_first.begin(), fci->info().cb_facet_last_first.end()) << " ";
        out << accumulate(fci->info().cb_facet_last_first.begin(), fci->info().cb_facet_last_first.end(),0.0) << " ";
    }
    else
        out << "0 0 0 ";
    // last second
    int fdls = fci->info().cb_facet_last_second.size();
    out << fdls << " ";
    if(fdls > 0){
        out << *min_element(fci->info().cb_facet_last_second.begin(), fci->info().cb_facet_last_second.end()) << " ";
        out << *max_element(fci->info().cb_facet_last_second.begin(), fci->info().cb_facet_last_second.end()) << " ";
        out << accumulate(fci->info().cb_facet_last_second.begin(), fci->info().cb_facet_last_second.end(),0.0) << " ";
    }
    else
        out << "0 0 0 ";

    out << endl;
}

void facetBasedGeometricFeatures(ofstream& out, Delaunay& Dt, Delaunay::All_cells_iterator& fci, int j){

//    fgeom << "area angle beta dist" << endl;
    // export the area and angle off the current facet as edge feature
    auto fac = make_pair(fci,j);
    out << computeFacetArea(Dt,fac) << " ";
    out << computeCosFacetCellAngle(Dt,fac) << " ";
    out << 1-min(computeCosFacetCellAngle(Dt, fac),
                             computeCosFacetCellAngle(Dt, Dt.mirror_facet(fac))) << " ";

    auto neighbor = fci->neighbor(j);
    if(!Dt.is_infinite(neighbor) && !Dt.is_infinite(fci)){
        auto cen1 = CGAL::centroid(fci->vertex(0)->point(),
                                   fci->vertex(1)->point(),
                                   fci->vertex(2)->point(),
                                   fci->vertex(3)->point());
        auto cen2 = CGAL::centroid(neighbor->vertex(0)->point(),
                       neighbor->vertex(1)->point(),
                       neighbor->vertex(2)->point(),
                       neighbor->vertex(3)->point());
        out << CGAL::squared_distance(cen1,cen2);
    }
    else
        out << "-1";
    out << endl;
}
void facetBasedVertexFeatures(ofstream& out, Delaunay& Dt, Delaunay::All_cells_iterator& fci, int j){

//    fbvf << "fb_vertex_inside_count fb_vertex_inside_dist_min fb_vertex_inside_dist_max fb_vertex_inside_dist_sum ";
//    fbvf << "fb_vertex_outside_count fb_vertex_outside_dist_min fb_vertex_outside_dist_max fb_vertex_outside_dist_sum" << endl;
//    fbvf << "fb_vertex_last_count fb_vertex_last_dist_min fb_vertex_last_dist_max fb_vertex_last_dist_sum" << endl;

    // inside
    int vdi = fci->info().fb_vertex_inside[j].size();
    out << vdi << " ";
    if(vdi > 0){
        out << *min_element(fci->info().fb_vertex_inside[j].begin(), fci->info().fb_vertex_inside[j].end()) << " ";
        out << *max_element(fci->info().fb_vertex_inside[j].begin(), fci->info().fb_vertex_inside[j].end()) << " ";
        out << accumulate(fci->info().fb_vertex_inside[j].begin(), fci->info().fb_vertex_inside[j].end(),0.0) << " ";

    }
    else
        out << "0 0 0 ";
    // outside
    int vdo = fci->info().fb_vertex_outside[j].size();
    out << vdo << " ";
    if(vdo > 0){
        out << *min_element(fci->info().fb_vertex_outside[j].begin(), fci->info().fb_vertex_outside[j].end()) << " ";
        out << *max_element(fci->info().fb_vertex_outside[j].begin(), fci->info().fb_vertex_outside[j].end()) << " ";
        out << accumulate(fci->info().fb_vertex_outside[j].begin(), fci->info().fb_vertex_outside[j].end(),0.0) << " ";
    }
    else
        out << "0 0 0 ";
    // last
    int vdl = fci->info().fb_vertex_last[j].size();
    out << vdl << " ";
    if(vdl > 0){
        out << *min_element(fci->info().fb_vertex_last[j].begin(), fci->info().fb_vertex_last[j].end()) << " ";
        out << *max_element(fci->info().fb_vertex_last[j].begin(), fci->info().fb_vertex_last[j].end()) << " ";
        out << accumulate(fci->info().fb_vertex_last[j].begin(), fci->info().fb_vertex_last[j].end(),0.0) << " ";
    }
    else
        out << "0 0 0 ";

    out << endl;

}

void facetBasedFacetFeatures(ofstream& out, Delaunay& Dt, Delaunay::All_cells_iterator& fci, int j){

//    fbff << "fb_facet_inside_count fb_facet_inside_dist_min fb_facet_inside_dist_max fb_facet_inside_dist_sum ";
//    fbff << "fb_facet_outside_count fb_facet_outside_dist_min fb_facet_outside_dist_max fb_facet_outside_dist_sum" << endl;
//    fbff << "fb_facet_last_count fb_facet_last_dist_min fb_facet_last_dist_max fb_facet_last_dist_sum" << endl;

    // inside
    int vdi = fci->info().fb_facet_inside[j].size();
    out << vdi << " ";
    if(vdi > 0){
        out << *min_element(fci->info().fb_facet_inside[j].begin(), fci->info().fb_facet_inside[j].end()) << " ";
        out << *max_element(fci->info().fb_facet_inside[j].begin(), fci->info().fb_facet_inside[j].end()) << " ";
        out << accumulate(fci->info().fb_facet_inside[j].begin(), fci->info().fb_facet_inside[j].end(),0.0) << " ";

    }
    else
        out << "0 0 0 ";
    // outside
    int vdo = fci->info().fb_facet_outside[j].size();
    out << vdo << " ";
    if(vdo > 0){
        out << *min_element(fci->info().fb_facet_outside[j].begin(), fci->info().fb_facet_outside[j].end()) << " ";
        out << *max_element(fci->info().fb_facet_outside[j].begin(), fci->info().fb_facet_outside[j].end()) << " ";
        out << accumulate(fci->info().fb_facet_outside[j].begin(), fci->info().fb_facet_outside[j].end(),0.0) << " ";
    }
    else
        out << "0 0 0 ";
    // last
    int vdl = fci->info().fb_facet_last[j].size();
    out << vdl << " ";
    if(vdl > 0){
        out << *min_element(fci->info().fb_facet_last[j].begin(), fci->info().fb_facet_last[j].end()) << " ";
        out << *max_element(fci->info().fb_facet_last[j].begin(), fci->info().fb_facet_last[j].end()) << " ";
        out << accumulate(fci->info().fb_facet_last[j].begin(), fci->info().fb_facet_last[j].end(),0.0) << " ";
    }
    else
        out << "0 0 0 ";

    out << endl;

}

void exportGraph(dirHolder& dir, runningOptions& options, Delaunay& Dt){

    auto start = std::chrono::high_resolution_clock::now();
    cout << "\nExport graph..." << endl;
    boost::filesystem::path p(dir.write_file);
    string outfile = p.stem().string();
    cout << "\t-to " << "gt/"+outfile+"_X.txt" << endl;
    string rays = (options.export_rays) ? "\t-with rays" : "\t-without rays";
    cout << rays << endl;
    int print_precision = 8;

    ////////////////////////////////////////////
    //////////////    HEADERS    ///////////////
    ////////////////////////////////////////////

    /////////////////////////////////////////////
    //////////////////// CELLS //////////////////
    /////////////////////////////////////////////
    ////// info file
    ofstream info;
    info.open(dir.path+"gt/"+outfile+"_info.txt");
    info << "vertices " << Dt.number_of_vertices() << endl;
    info << "facets " << Dt.number_of_facets() << endl;
    info << "cells " << Dt.number_of_cells() << endl;
    info.close();

    ////// label and index
    // cell index and labels
    ofstream il;
    il.open(dir.path+"gt/"+outfile+"_labels.txt");
    il << "global_index inside_perc outside_perc inside_outside_max gc_label infinite" << endl;

    ////// geometric features
    ofstream cgeom;
    cgeom.open(dir.path+"gt/"+outfile+"_cgeom.txt");
    cgeom << "radius vol longest_edge shortest_edge" << endl;
    cgeom << setprecision(print_precision);

    ////// cell based vertex features
    ofstream cbvf;
    cbvf.open(dir.path+"gt/"+outfile+"_cbvf.txt");
    cbvf << "cb_vertex_inside_count cb_vertex_inside_dist_min cb_vertex_inside_dist_max cb_vertex_inside_dist_sum ";
    cbvf << "cb_vertex_outside_count cb_vertex_outside_dist_min cb_vertex_outside_dist_max cb_vertex_outside_dist_sum ";
    cbvf << "cb_vertex_last_count cb_vertex_last_dist_min cb_vertex_last_dist_max cb_vertex_last_dist_sum" << endl;
    cbvf << setprecision(print_precision);

    ////// cell based facet features
    ofstream cbff;
    cbff.open(dir.path+"gt/"+outfile+"_cbff.txt");
    cbff << "cb_facet_inside_first_count cb_facet_inside_first_dist_min cb_facet_inside_first_dist_max cb_facet_inside_first_dist_sum ";
    cbff << "cb_facet_inside_second_count cb_facet_inside_second_dist_min cb_facet_inside_second_dist_max cb_facet_inside_second_dist_sum ";
    cbff << "cb_facet_outside_first_count cb_facet_outside_first_dist_min cb_facet_outside_first_dist_max cb_facet_outside_first_dist_sum ";
    cbff << "cb_facet_outside_second_count cb_facet_outside_second_dist_min cb_facet_outside_second_dist_max cb_facet_outside_second_dist_sum ";
    cbff << "cb_facet_last_first_count cb_facet_last_first_dist_min cb_facet_last_first_dist_max cb_facet_last_first_dist_sum ";
    cbff << "cb_facet_last_second_count cb_facet_last_second_dist_min cb_facet_last_second_dist_max cb_facet_last_second_dist_sum" << endl;
    cbff << setprecision(print_precision);

    /////////////////////////////////////////////
    //////////////////// EDGES //////////////////
    /////////////////////////////////////////////
    // adjacency files
    ofstream adjacency_ij, adjacency_ji;
    adjacency_ij.open(dir.path+"gt/"+outfile+"_adjacency_ij.txt");
    adjacency_ij << "# list of edges" << endl;
    adjacency_ji.open(dir.path+"gt/"+outfile+"_adjacency_ji.txt");
    adjacency_ji << "# list of edges" << endl;

    // geometric features
    ofstream fgeom;
    fgeom.open(dir.path+"gt/"+outfile+"_fgeom.txt");
    fgeom << "area angle beta dist" << endl;
    // facet based vertex features
    ofstream fbvf;
    fbvf.open(dir.path+"gt/"+outfile+"_fbvf.txt");
    fbvf << "fb_vertex_inside_count fb_vertex_inside_dist_min fb_vertex_inside_dist_max fb_vertex_inside_dist_sum ";
    fbvf << "fb_vertex_outside_count fb_vertex_outside_dist_min fb_vertex_outside_dist_max fb_vertex_outside_dist_sum ";
    fbvf << "fb_vertex_last_count fb_vertex_last_dist_min fb_vertex_last_dist_max fb_vertex_last_dist_sum" << endl;

    // facet based facet features
    ofstream fbff;
    fbff.open(dir.path+"gt/"+outfile+"_fbff.txt");
    fbff << "fb_facet_inside_count fb_facet_inside_dist_min fb_facet_inside_dist_max fb_facet_inside_dist_sum ";
    fbff << "fb_facet_outside_count fb_facet_outside_dist_min fb_facet_outside_dist_max fb_facet_outside_dist_sum ";
    fbff << "fb_facet_last_count fb_facet_last_dist_min fb_facet_last_dist_max fb_facet_last_dist_sum" << endl;

    int edge_count = 0;
    int i, j;

    string npz_out = dir.path+"gt/"+outfile+"_npz.npz";
    Delaunay::All_cells_iterator fci;
    for(fci = Dt.all_cells_begin(); fci != Dt.all_cells_end(); fci++){

        // TODO: there is a problem here, not all cells are exported, running index i < nc at the end;
        i = fci->info().global_idx;

        /////////////////////////////////////////////
        //////////////     LABELS    ////////////////
        /////////////////////////////////////////////
        cellIndexAndLabel(il, Dt, fci, i, options);

        /////////////////////////////////////////////
        ////////////// CELL FEATURES ////////////////
        /////////////////////////////////////////////
        cellBasedGeometricFeatures(cgeom, Dt, fci);
        cellBasedVertexFeatures(cbvf,Dt,fci);
        cellBasedFacetFeatures(cbff,Dt,fci);

//        xt::xarray<double> radius;
//        xt::xarray<double> vol;
//        if(Dt.is_infinite(fci)){
//            radius = {0.0};
//            vol = {0.0};
//        }
//        else{
//            auto tet = Dt.tetrahedron(fci);
//            tetFeatures tf = calcTetFeatures(tet);
//            radius = {tf.radius};
//            vol = {tf.vol};
//        }

//        xt::dump_npz(npz_out,"radius",radius,true,true);
//        xt::dump_npz(npz_out,"volume",vol,true,true);

        /////////////////////////////////////////////
        ////////////// EDGE FEATURES ////////////////
        /////////////////////////////////////////////
        for(int c = 0; c < 4; c++){
            // set the edge to the mirror cell
            j = fci->neighbor(c)->info().global_idx;

            if(j == i) // skip self loops
                continue;
            adjacency_ij << i << " ";
            adjacency_ji << j << " ";

            //// geometric features
            facetBasedGeometricFeatures(fgeom,Dt,fci,c);
            facetBasedVertexFeatures(fbvf,Dt,fci,c);
            facetBasedFacetFeatures(fbff,Dt,fci,c);

            edge_count++;
        }
    }

    il.close();
    cgeom.close();
    cbvf.close();
    cbff.close();

    adjacency_ij.close();
    adjacency_ji.close();
    fgeom.close();
    fbvf.close();
    fbff.close();

    auto stop = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::seconds>(stop - start);
    cout << "\t-Exported " << i << " nodes and " << edge_count << " edges" << endl;
    cout << "\t-in " << duration.count() << "s" << endl;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////// IMPORT ////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////
int loadPrediction(dirHolder dir, dataHolder& data, runningOptions options){

    fs::path path = fs::path(dir.path) / fs::path(dir.prediction_file);
    ifstream file(path.string());

    cout << "\nLoad prediction score..." << endl;
    cout << "\t-from " << path.string() << endl;

    if(!file){
        cout << "\nFILE DOES NOT EXIST OR IS EMPTY!" << endl;
        return 1;
    }
    auto npz_map = xt::load_npz(path.string());

    Delaunay::All_cells_iterator fci;
    int b = data.Dt.number_of_cells();
    auto nc = npz_map["number_of_cells"].cast<long>();
    const int a = nc(0,0);
    // check for same sampling of input and scan
    if(a != b){
        cout << "ERROR: SAMPLING IS NOT THE SAME!" << endl;
        cout << "\t-" << b << " Delaunay cells" << endl;
        cout << "\t-" << a << " imported cells" << endl;
        return 1;
    }

    int i = 0;
    array<size_t, 2> shape = { a, 2 };
    xt::xtensor<float, 2> pred(shape);
    if(options.prediction_type == "lo"){
        pred = npz_map["logits"].cast<float>();
    }
    else if(options.prediction_type == "sm"){
        pred = npz_map["softmax"].cast<float>();
    }
    else if(options.prediction_type == "si"){
        pred = npz_map["sigmoid"].cast<float>();
    }
    else{
        cout << options.prediction_type << " is not a valid prediction type, choose either lo or sm" << endl;
        return 1;
    }
    for(fci = data.Dt.all_cells_begin(); fci != data.Dt.all_cells_end(); fci++){

        if(data.Dt.is_infinite(fci)){
            fci->info().outside_score = 1.0;
            fci->info().inside_score = 0.0;
        }
        else{

            // check if there is a camera inside the cell
            for(auto s = data.sensor_map.begin(); s != data.sensor_map.end(); s++){
                if(data.Dt.tetrahedron(fci).has_on_positive_side(s->second)){
                    fci->info().outside_score = 1000;
                    fci->info().inside_score = -1000;
                    break;
                }
            }

//            xt::xtensor_fixed<double, xt::xshape<a, 2>> pred;
            if(pred.shape(1)==1){
//                cout << pred(i,0) << endl;
                if(pred(i,0)<=0.0){
                    fci->info().outside_score = 0;
                    fci->info().inside_score = 1;
                }
                else{
                    fci->info().outside_score = 1;
                    fci->info().inside_score = 0;
                }
            }
            else{
                fci->info().inside_score = pred(i,0);
                fci->info().outside_score = pred(i,1);
                // TODO: maybe it should be
                // fci->info().inside_score = pred(i,0) - pred(i,1);
                // fci->info().outside_score = pred(i,1) - pred(i,0);
                // because in the case of logits pred(i,0) = 1 - pred(i,1)
                // UPDATE: in fact it doesn't matter! what matters in  the graph cut is the difference between the two labelling options.
            }
        }
        i++;

    }
    cout<< "\t-read " << a << " cell scores. " << endl;

    return 0;
}


// Boost.Geometry

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

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_FORMULAS_MERIDIAN_INVERSE_HPP
#define BOOST_GEOMETRY_FORMULAS_MERIDIAN_INVERSE_HPP

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

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

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

#include <boost/geometry/formulas/flattening.hpp>
#include <boost/geometry/formulas/meridian_segment.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief Compute the arc length of an ellipse.
*/

template <typename CT, unsigned int Order = 1>
class meridian_inverse
{

public :

    struct result
    {
        result()
            : distance(0)
            , meridian(false)
        {}

        CT distance;
        bool meridian;
    };

    template <typename T>
    static bool meridian_not_crossing_pole(T lat1, T lat2, CT diff)
    {
        CT half_pi = math::pi<CT>()/CT(2);
        return math::equals(diff, CT(0)) ||
                    (math::equals(lat2, half_pi) && math::equals(lat1, -half_pi));
    }

    static bool meridian_crossing_pole(CT diff)
    {
        return math::equals(math::abs(diff), math::pi<CT>());
    }

    template <typename T, typename Spheroid>
    static CT meridian_not_crossing_pole_dist(T lat1, T lat2, Spheroid const& spheroid)
    {
        return math::abs(apply(lat2, spheroid) - apply(lat1, spheroid));
    }

    template <typename T, typename Spheroid>
    static CT meridian_crossing_pole_dist(T lat1, T lat2, Spheroid const& spheroid)
    {
        CT c0 = 0;
        CT half_pi = math::pi<CT>()/CT(2);
        CT lat_sign = 1;
        if (lat1+lat2 < c0)
        {
            lat_sign = CT(-1);
        }
        return math::abs(lat_sign * CT(2) * apply(half_pi, spheroid)
                         - apply(lat1, spheroid) - apply(lat2, spheroid));
    }

    template <typename T, typename Spheroid>
    static result apply(T lon1, T lat1, T lon2, T lat2, Spheroid const& spheroid)
    {
        result res;

        CT diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2);

        if (lat1 > lat2)
        {
            std::swap(lat1, lat2);
        }

        if ( meridian_not_crossing_pole(lat1, lat2, diff) )
        {
            res.distance = meridian_not_crossing_pole_dist(lat1, lat2, spheroid);
            res.meridian = true;
        }
        else if ( meridian_crossing_pole(diff) )
        {
            res.distance = meridian_crossing_pole_dist(lat1, lat2, spheroid);
            res.meridian = true;
        }
        return res;
    }

    // Distance computation on meridians using series approximations
    // to elliptic integrals. Formula to compute distance from lattitude 0 to lat
    // https://en.wikipedia.org/wiki/Meridian_arc
    // latitudes are assumed to be in radians and in [-pi/2,pi/2]
    template <typename T, typename Spheroid>
    static CT apply(T lat, Spheroid const& spheroid)
    {
        CT const a = get_radius<0>(spheroid);
        CT const f = formula::flattening<CT>(spheroid);
        CT n = f / (CT(2) - f);
        CT M = a/(1+n);
        CT C0 = 1;

        if (Order == 0)
        {
           return M * C0 * lat;
        }

        CT C2 = -1.5 * n;

        if (Order == 1)
        {
            return M * (C0 * lat + C2 * sin(2*lat));
        }

        CT n2 = n * n;
        C0 += .25 * n2;
        CT C4 = 0.9375 * n2;

        if (Order == 2)
        {
            return M * (C0 * lat + C2 * sin(2*lat) + C4 * sin(4*lat));
        }

        CT n3 = n2 * n;
        C2 += 0.1875 * n3;
        CT C6 = -0.729166667 * n3;

        if (Order == 3)
        {
            return M * (C0 * lat + C2 * sin(2*lat) + C4 * sin(4*lat)
                      + C6 * sin(6*lat));
        }

        CT n4 = n2 * n2;
        C4 -= 0.234375 * n4;
        CT C8 = 0.615234375 * n4;

        if (Order == 4)
        {
            return M * (C0 * lat + C2 * sin(2*lat) + C4 * sin(4*lat)
                      + C6 * sin(6*lat) + C8 * sin(8*lat));
        }

        CT n5 = n4 * n;
        C6 += 0.227864583 * n5;
        CT C10 = -0.54140625 * n5;

        // Order 5 or higher
        return M * (C0 * lat + C2 * sin(2*lat) + C4 * sin(4*lat)
                  + C6 * sin(6*lat) + C8 * sin(8*lat) + C10 * sin(10*lat));

    }
};

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

#endif // BOOST_GEOMETRY_FORMULAS_MERIDIAN_INVERSE_HPP


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include "toplevelfixture.hpp"
#include <boost/test/unit_test.hpp>
#include <ql/termstructures/yield/piecewiseyieldcurve.hpp>
#include <ql/termstructures/yield/ratehelpers.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/actual360.hpp>
#include <qle/pricingengines/depositengine.hpp>

#include <boost/make_shared.hpp>

using namespace QuantLib;
using namespace QuantExt;
using namespace boost::unit_test_framework;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(DepositTest)

BOOST_AUTO_TEST_CASE(testRepricing) {

    BOOST_TEST_MESSAGE("Testing Repricing of a Deposit on a depo curve...");

    SavedSettings backup;

    Date refDate = Date(8, Dec, 2016);
    Settings::instance().evaluationDate() = refDate;

    std::vector<boost::shared_ptr<RateHelper> > helper;
    helper.push_back(boost::make_shared<DepositRateHelper>(Handle<Quote>(boost::make_shared<SimpleQuote>(0.02)),
                                                           7 * Months, 2, TARGET(), ModifiedFollowing, false,
                                                           Actual360()));

    Handle<YieldTermStructure> curve(
        boost::make_shared<PiecewiseYieldCurve<Discount, LogLinear> >(refDate, helper, Actual365Fixed()));

    boost::shared_ptr<PricingEngine> engine = boost::make_shared<DepositEngine>(curve);

    Deposit depo(100.0, 0.02, 7 * Months, 2, TARGET(), ModifiedFollowing, false, Actual360(), refDate, true, 0 * Days);
    depo.setPricingEngine(engine);

    Real tol = 1.0E-8;
    BOOST_CHECK_MESSAGE(std::abs(depo.NPV()) <= tol,
                        "Deposit NPV(" << depo.NPV() << ") could not be verified, expected 0.0");

    BOOST_CHECK_MESSAGE(std::abs(depo.fairRate() - 0.02) <= tol,
                        "Deposit fair rate (" << depo.fairRate() << ") could not be verified, expected 0.02");
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


//  (C) Copyright Nick Thompson 2020.
//  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 <random>
#include <benchmark/benchmark.h>
#include <boost/math/special_functions/jacobi_theta.hpp>
#include <boost/multiprecision/float128.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

using boost::multiprecision::number;
using boost::multiprecision::mpfr_float_backend;
using boost::multiprecision::float128;
using boost::multiprecision::cpp_bin_float_50;
using boost::multiprecision::cpp_bin_float_100;
using boost::math::jacobi_theta1;
using boost::math::jacobi_theta1tau;

template<class Real>
void JacobiTheta1(benchmark::State& state)
{
    std::random_device rd;
    std::mt19937_64 mt(rd());
    std::uniform_real_distribution<long double> unif(0,0.01);

    Real x = static_cast<Real>(unif(mt));
    Real q = static_cast<Real>(unif(mt));
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(jacobi_theta1(x, q));
        x += std::numeric_limits<Real>::epsilon();
    }
}

BENCHMARK_TEMPLATE(JacobiTheta1, float);
BENCHMARK_TEMPLATE(JacobiTheta1, double);
BENCHMARK_TEMPLATE(JacobiTheta1, long double);
BENCHMARK_TEMPLATE(JacobiTheta1, float128);
BENCHMARK_TEMPLATE(JacobiTheta1, number<mpfr_float_backend<100>>);
BENCHMARK_TEMPLATE(JacobiTheta1, number<mpfr_float_backend<200>>);
BENCHMARK_TEMPLATE(JacobiTheta1, number<mpfr_float_backend<300>>);
BENCHMARK_TEMPLATE(JacobiTheta1, number<mpfr_float_backend<400>>);
BENCHMARK_TEMPLATE(JacobiTheta1, number<mpfr_float_backend<1000>>);
BENCHMARK_TEMPLATE(JacobiTheta1, cpp_bin_float_50);
BENCHMARK_TEMPLATE(JacobiTheta1, cpp_bin_float_100);

template<class Real>
void JacobiTheta1Tau(benchmark::State& state)
{
    std::random_device rd;
    std::mt19937_64 mt(rd());
    std::uniform_real_distribution<long double> unif(0,0.01);

    Real x = static_cast<Real>(unif(mt));
    Real q = static_cast<Real>(unif(mt));
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(jacobi_theta1tau(x, q));
        x += std::numeric_limits<Real>::epsilon();
    }
}

BENCHMARK_TEMPLATE(JacobiTheta1Tau, float);
BENCHMARK_TEMPLATE(JacobiTheta1Tau, double);
BENCHMARK_TEMPLATE(JacobiTheta1Tau, long double);
BENCHMARK_TEMPLATE(JacobiTheta1Tau, float128);
BENCHMARK_TEMPLATE(JacobiTheta1Tau, number<mpfr_float_backend<100>>);
BENCHMARK_TEMPLATE(JacobiTheta1Tau, number<mpfr_float_backend<200>>);
BENCHMARK_TEMPLATE(JacobiTheta1Tau, number<mpfr_float_backend<300>>);
BENCHMARK_TEMPLATE(JacobiTheta1Tau, number<mpfr_float_backend<400>>);
BENCHMARK_TEMPLATE(JacobiTheta1Tau, number<mpfr_float_backend<1000>>);
BENCHMARK_TEMPLATE(JacobiTheta1Tau, cpp_bin_float_50);
BENCHMARK_TEMPLATE(JacobiTheta1Tau, cpp_bin_float_100);

BENCHMARK_MAIN();


#include <boost/python/numpy.hpp>
#include <Eigen/Dense>

namespace p = boost::python;
namespace np = boost::python::numpy;

/* python helper */
template <typename T, typename U>
static Eigen::Matrix<T, -1, -1> npToEigen2d(np::ndarray np){
	int r = np.shape(0), c = np.shape(1);
	Eigen::Matrix<T, -1, -1> mat(r, c);
	U *data = reinterpret_cast<U *>(np.get_data());
	for (int i = 0; i < r; i++)
		for (int j = 0; j < c; j++)
			mat(i, j) = data[i * c + j];
	return mat;
}

template <typename T, typename U>
static np::ndarray eigenTonp2d(const Eigen::Matrix<U, -1, -1> &mat){
	int r = mat.rows(), c = mat.cols();
	p::tuple shape = p::make_tuple(r, c);
	np::dtype dtype = np::dtype::get_builtin<T>();
	np::ndarray np = np::empty(shape, dtype);
	for (int i = 0; i < r; i++)
		for (int j = 0; j < c; j++)
			np[i][j] = mat(i, j);
	return np;
}

template <typename T, typename U>
static Eigen::Matrix<T, -1, 1> npToEigen(np::ndarray np){
	int sz = np.shape(0);
	Eigen::Matrix<T, -1, 1> vec(sz);
	U *data = reinterpret_cast<U *>(np.get_data());
	for (int i = 0; i < sz; i++)
		vec[i] = data[i];
	return vec;
}

template <typename T, typename U>
static np::ndarray eigenTonp(const Eigen::Matrix<U, -1, 1> &vec){
	p::tuple shape = p::make_tuple(vec.size());
	np::dtype dtype = np::dtype::get_builtin<T>();
	np::ndarray np = np::empty(shape, dtype);
	for (int i = 0; i < vec.size(); i++)
		np[i] = vec[i];
	return np;
}

boost::python::tuple getGAE(np::ndarray values_np, np::ndarray reward_np, np::ndarray done_np, double gamma, double lamda){
	Eigen::VectorXd values = npToEigen<double, float>(values_np);
	Eigen::VectorXd reward = npToEigen<double, float>(reward_np);
	Eigen::VectorXi done = npToEigen<int, int>(done_np);
	int n = values.size();
	Eigen::VectorXd returns = Eigen::VectorXd::Zero(n);
	Eigen::VectorXd advants = Eigen::VectorXd::Zero(n);

	double prev_return = 0., current_return = 0.;
	double prev_value = 0., current_value = 0.;
	double prev_advant = 0., current_advant = 0.;
	for (int i = n - 1; i >= 0; i--){
		prev_return = done[i] ? 0. : current_return;
		prev_advant = done[i] ? 0. : current_advant;
		prev_value = done[i] ? 0. : current_value;

		current_return = reward[i] + gamma * prev_return;
		current_advant = prev_advant * gamma * lamda + (reward[i] - values[i] + gamma * prev_value);
		current_value = values[i];

		returns[i] = current_return;
		advants[i] = current_advant;
	}
	advants = advants / sqrt(advants.squaredNorm() / n);
	return p::make_tuple(eigenTonp<float>(returns), eigenTonp<float>(advants));
}

BOOST_PYTHON_MODULE(libRLHelper){
	np::initialize();

	p::def("getGAE", getGAE);
}

int main(){}


#include <boost/math/common_factor.hpp>


#ifdef MATLAB_MEX_FILE
#include "mex.h"
#include "posedata.h"
#include "get_floor_fHf.h"
#include <Eigen/Dense>

using namespace Eigen;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	if (nrhs != 4) {
		mexErrMsgIdAndTxt("get_floor_fHf:nrhs", "Four input arguments are required.");
	}
	if (nlhs != 2) {
		mexErrMsgIdAndTxt("get_floor_fHf:nlhs", "Two output arguments are required.");
	}
	if (!mxIsDouble(prhs[0]) || mxIsComplex(prhs[0])) {
		mexErrMsgIdAndTxt("get_floor_fHf:notDouble", "Input data must be type double.");
	}
	if(mxGetNumberOfElements(prhs[0]) != 6 && mxGetNumberOfElements(prhs[1]) != 6 && mxGetNumberOfElements(prhs[2]) != 9 && mxGetNumberOfElements(prhs[3]) != 9) {
		mexErrMsgIdAndTxt("get_floor_fHf:incorrectSize", "Input dimensions incorrect.");
	}
    // Convert to expected input
    VectorXd x1_tmp = Map<VectorXd>(mxGetPr(prhs[0]), 6);
    VectorXd x2_tmp = Map<VectorXd>(mxGetPr(prhs[1]), 6);
    MatrixXd x1 = Map<MatrixXd>(x1_tmp.data(), 2, 3);
    MatrixXd x2 = Map<MatrixXd>(x2_tmp.data(), 2, 3);

    VectorXd R1_tmp = Map<VectorXd>(mxGetPr(prhs[2]), 9);
    VectorXd R2_tmp = Map<VectorXd>(mxGetPr(prhs[3]), 9);
    Matrix3d R1 = Map<Matrix3d>(R1_tmp.data(), 3, 3);
    Matrix3d R2 = Map<Matrix3d>(R2_tmp.data(), 3, 3);

    // Compute output
	PoseData posedata = get_floor_fHf(x1, x2, R1, R2);

    // Wrap it up to Matlab compatible output
	plhs[0] = mxCreateDoubleMatrix(3, 3, mxREAL);
	double* zr = mxGetPr(plhs[0]);
    for (Index i = 0; i < posedata.homography.size(); i++) {
        zr[i] = posedata.homography(i);
    }

    plhs[1] = mxCreateDoubleMatrix(1, 1, mxREAL);
    zr = mxGetPr(plhs[1]);
    zr[0] = posedata.focal_length;
}
#endif


#pragma once

#include <Eigen/Dense>

#include <PnC/WBC/Task.hpp>

class AngularMomentumTask : public Task {
 public:
  AngularMomentumTask(RobotSystem* robot_, const double dt_eps);
  virtual ~AngularMomentumTask();

 private:
  /* Update op_cmd, pos_err, vel_des, acc_des
   *
   * vel_des_ = [w_x, w_y, w_z] // angular momentum
   * acc_des_ = [a_x, a_y, a_z] // angular momentum rate
   *
   */
  virtual bool _UpdateCommand(const Eigen::VectorXd& pos_des,
                              const Eigen::VectorXd& vel_des,
                              const Eigen::VectorXd& acc_des);
  virtual bool _UpdateTaskJacobian();
  virtual bool _UpdateTaskJDotQdot();

  Eigen::MatrixXd Ag_cur_;   // Current centroidal inertia matrix
  Eigen::MatrixXd Ag_prev_;  // Previous centroidal inertia matrix
  Eigen::MatrixXd Agdot_;    // Current Estimate for centroidal inertia matrix
  double dt_eps_;            // time interval to use for approximating Agdot_
  bool first_pass_;          // first time computing Jdotqdot
};