/*! \file demo_1d_autoscaling.cpp
    \brief An example to demonstrate simple 1D settings, including auto-scaling.
    \details
      See auto_1d_containers.cpp for an example autoscaling with multiple data-series.
      See also demo_1d_plot.cpp for a wider range of use.
*/
// Copyright Jacob Voytko 2007
// Copyright Paul A Bristow 2008, 2009, 2012

// 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)

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

//[demo_1d_autoscaling_1

/*`First some includes to use Boost.Plot
(and some others only needed for this example).
*/

#include <boost/svg_plot/svg_1d_plot.hpp>
  //using namespace boost::svg;
  //using boost::svg::svg_1d_plot;

//#include <boost/svg_plot/show_1d_settings.hpp>
// Only needed for showing which settings in use.
// void boost::svg::show_1d_plot_settings(svg_1d_plot&);
 // (Also provides operator<< for std::pair).

#include <iostream> // for debugging.
  //using std::cout;
  //using std::endl;
  //using std::boolalpha;
#include <limits>
//  using std::numeric_limits;
#include <vector>
//  using std::vector;
#include <set>
//  using std::multiset;

#include <utility>
//  using std::pair;

#include <boost/svg_plot/detail/auto_axes.hpp>
  using boost::svg::show; //! A single STL container.
  using boost::svg::show_all; //! Multiple STL containers.
  using boost::svg::range_mx; //! Find min and max of a single STL container.
  using boost::svg::range_all;  //! Find min and max of multiple STL containers.
//] [/demo_1d_autoscaling_1]

constexpr double tol100eps = 1000 * std::numeric_limits<double>::epsilon(); // A suitable value for tight.

int main()
{
//[demo_1d_autoscaling_2
/*`This example uses a few types of containers to demonstrate autoscaling.[br]
Autoscaling must inspect the data in the container in order to find axis ranges that will be suitable.
First we create a container and fill with some fictional data, and then display the values.
*/

  try
  { // Ensure error, warning and information messages are displayed by the catch block below.

    std::vector<double> my_data;
    // Initialize my_data with some entirely fictional data.
    my_data.push_back(0.2); // [0]
    my_data.push_back(1.1); // [1]
    my_data.push_back(4.2); // [2]
    my_data.push_back(3.3); // [3]
    my_data.push_back(5.4); // [4]
    my_data.push_back(6.5); // [5]
    show(my_data); // Show entire container contents,
    // 6 values in container: 0.2 1.1 4.2 3.3 5.4 6.5

    using namespace boost::svg;  // Convenient to access all the many svg_plot features.

    /*`Construct a plot , and add some data to the plot. */

    svg_1d_plot my_1d_plot; // Construct a plot with all the default constructor values.
    my_1d_plot.title("Demo 1D autoscaling").x_label("X values"); // Add a plot title and X axis-label.
    my_1d_plot.plot(my_data, "Auto 1D my_data"); // Add whole data-series from my_data.

    //`Use `x_autoscale` to scale the axis, in this most common and simplest case, using all the values.
    my_1d_plot.x_autoscale(my_data);

    /*`and finally write the SVG XML to a file.*/
    my_1d_plot.write("demo_1d_autoscaling_1.svg"); // Write the plot to file.
//] [/demo_1d_autoscaling_2]

//[demo_1d_autoscaling_3
    /*`In a second example, we use a different type of container, a `std::set`,
    and use autoscale in a more advanced way.*/
    std::multiset<double> my_set;
    // Initialize `my_set` with some entirely fictional data.
    my_set.insert(1.2);
    my_set.insert(2.3);
    my_set.insert(3.4);
    my_set.insert(4.5);
    my_set.insert(5.6);
    my_set.insert(6.7);
    my_set.insert(7.8);
    my_set.insert(8.9);
    // Show the set.
    std::multiset<double>::const_iterator si;
    show(my_set); // for two different types of container.
    // 8 values in container: 1.2 2.3 3.4 4.5 5.6 6.7 7.8 8.9

    svg_1d_plot my_1d_plot_2; // Construct a plot with all the default constructor values.

    /*`and also override the default controls of @c scale_axis function used by autoscaling.*/
    // Set some autoscaling parameters:
    my_1d_plot_2.x_with_zero(true); // Always include zero in the axis range, even if the data values don't.
    my_1d_plot_2.x_min_ticks(10); // Ensure more than the default minimum number of ticks.
    my_1d_plot_2.x_steps(0); // Permits axis ticks in steps of 1,2,4,6,8 or 1, 5, 10, or 1, 2, 5, 10.
    my_1d_plot_2.x_tight(0.01); // Prevent values that are only 1% outside the last tick requiring another tick.

    // Show the flags just set.
    std::cout << (my_1d_plot_2.x_with_zero() ? "x_with_zero, " : "not x_with_zero, ") // x_with_zero
      << my_1d_plot_2.x_min_ticks() << " x_min_ticks, " // 10
      << my_1d_plot_2.x_steps() << " x_steps, "  // 0
      << my_1d_plot_2.x_tight() << " tightness." << std::endl; // 0.001

    /*`Purely to show the possibilities with autoscale, we don't use the whole container,
    but use the two-iterators version of autoscale to *not use the first nor the last values* for autoscaling.
    (Remember values in the constainer `std::set` are sorted).
    */
    my_1d_plot_2.x_autoscale(++my_set.begin(),--my_set.end());
    /*`This also sets `autoscale(true)`, but note that `x_range()` is still not updated.
    If we want, we can display the ranges chosen by autoscale:
    */
    std::cout << " x_auto_min_value " << my_1d_plot_2.x_auto_min_value()
      << ", x_auto_max _value " << my_1d_plot_2.x_auto_max_value()
      << ", x_auto_tick_interval " << my_1d_plot_2.x_auto_tick_interval() << std::endl;

    /*`As before, we add the data set to the plot, and write to SVG XML to file.
    If you inspect the plot, you will see that the
    lowest data value 1.2 and highest data value 8.9 are no longer shown,
    because it is now outside the plot window.
    This is as if we had only written part of the data-series.
    */
     my_1d_plot_2.plot(++my_set.begin(),--my_set.end(), "Auto 1D my_set"); // Add 'top and tailed' data-series from my_set.
   //my_1d_plot.plot(my_set, "Auto 1D my_set"); // Add whole data-series from my_set.
     my_1d_plot_2.write("demo_1d_autoscaling_2.svg"); // Write the plot to file.

    /*`If we want, we can check the autoscale range used, noting that zero *is* included because we demanded it.*/
    using boost::svg::detail::operator<<;
    std::cout << "x_range() " << my_1d_plot_2.x_range() << std::endl; // x_range() 0, 8

    //show_1d_plot_settings(my_1d_plot_2); // If required.
  }
  /*`try'n'catch blocks are very useful to display any plot error messages.
  Otherwise any exceptions thrown will just terminate - silently and most unhelpfully.
  */
  catch(const std::exception& e)
  {
    std::cout <<"\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
 //] [/demo_1d_autoscaling_3]
  return 0;
} // int main()
/*

//[demo_1d_autoscaling_output

demo_1d_autoscaling.cpp

6 values in container: 0.2 1.1 4.2 3.3 5.4 6.5
8 values in container: 1.2 2.3 3.4 4.5 5.6 6.7 7.8 8.9
x_with_zero, 10 x_min_ticks, 0 x_steps, 0.01 tightness.
 x_auto_min_value 0, x_auto_max _value 8, x_auto_tick_interval 0.5
x_range() 0, 8

//] [/demo_1d_autoscaling_output]

*/


//==================================================================================================
/*
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
*/
//==================================================================================================
#include <cmath>
#include <boost/math/special_functions/bessel.hpp>
#include <eve/function/cyl_bessel_j0.hpp>
#include <eve/constant/valmin.hpp>
#include <eve/constant/valmax.hpp>
#include "producers.hpp"
#include <cmath>

TTS_CASE_TPL("wide random check on cyl_bessel_j0", EVE_TYPE)
{
  auto bj0 = [](auto x) -> EVE_VALUE {return boost::math::cyl_bessel_j(EVE_VALUE(0), x); };

  eve::uniform_prng<EVE_VALUE> p(0, 4);
  TTS_RANGE_CHECK(p, bj0, eve::cyl_bessel_j0);
}


/* ScaFES
 * Copyright (c) 2011-2015, ZIH, TU Dresden, Federal Republic of Germany.
 * For details, see the files COPYING and LICENSE in the base directory
 * of the package.
 */

/**
 *  @file EMFDTD.hpp
 *
 *  @brief Implementation of EMFDTD.
 */

#ifndef EMFDTD_HPP_
#define EMFDTD_HPP_

#include <fstream>
#include <string>
#include <vector>

#include <boost/property_tree/ini_parser.hpp>
#include <boost/tokenizer.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/serialization/vector.hpp>

#include <ScaFES.hpp>

#include "physical_constants.hpp"
#include "geometry/IndicatorEllipsoid.hpp"
#include "CPMLParams.hpp"
#include "stencils/CompleteCPMLCurlStencil.hpp"

#include "sources/ModulatedGaussianSource.hpp"
#include "conversion.hpp"

/*******************************************************************************
 ******************************************************************************/
/**
 * \class EMFDTDng
 *  @brief Class for discretized EM problem.
 *
 * \section EMFDTDng3D EMFDTD at ellipsoid.
 *
 * The EM problem is discretized using the FDTD method (Finite Differences
 * at the Time Domain).
 *
 * The workflow for running the simulation is as follows:
 *
 *      - Set up the computational domain within in the source code.
 *      - Recompile source code.
 *      - Set up EM parameters within the ini file.
 *      - Run simulation.
 *
 * Parameters of the PML (perfectly matched layers):
 *
 *      [PML]
        d=10
        m_a=1.
        m=3
        k_max=1.
        s_max=0.01222
        a_max=1.e-3

 * Location and parameters of the source.
 *
        [source]
        ; Location within computational domain
        location=25x25x25
        ; modulation frequency:
        f_mod = 0
        Gauss pulse: time step with maximal stimulation (=1)
        t0 = 40.0
        ; Width
        spread = 8.0

 * Location and target file of the sink.
 *
        [sink]
        ; Location within computational domain
        location=30x30x30
        ; Target file
        data_file = reference.txt

 * Domain initialization
 *
        [initialization]
        ; for ellipsoid: (px,py,pz,rx,ry,rz)
        parameters = 100 100 100 50 50 50
 */

/** data field access by symbolic name. */
enum class DFs: std::vector<std::string>::size_type {
    SIGMA=0,
    Ex, Ey, Ez,
    Hx, Hy, Hz,
    Dx, Dy, Dz,
    Ix, Iy, Iz,
    Psi_ExDy, Psi_ExDz, Psi_EyDx,
    Psi_EyDz, Psi_EzDx, Psi_EzDy,
    Psi_HxDy, Psi_HxDz, Psi_HyDx,
    Psi_HyDz, Psi_HzDx, Psi_HzDy
};

/** when to write which data field. */
typedef ScaFES::WriteHowOften W;
std::vector<W> initWritePolicy()
{
    std::vector<W> v;
    v.push_back(W::AT_START);
    v.push_back(W::LIKE_GIVEN_AT_CL); v.push_back(W::LIKE_GIVEN_AT_CL); v.push_back(W::LIKE_GIVEN_AT_CL);
    v.push_back(W::NEVER); v.push_back(W::NEVER); v.push_back(W::NEVER);
    v.push_back(W::NEVER); v.push_back(W::NEVER); v.push_back(W::NEVER);
    v.push_back(W::NEVER); v.push_back(W::NEVER); v.push_back(W::NEVER);
    v.push_back(W::NEVER); v.push_back(W::NEVER); v.push_back(W::NEVER);
    v.push_back(W::NEVER); v.push_back(W::NEVER); v.push_back(W::NEVER);
    v.push_back(W::NEVER); v.push_back(W::NEVER); v.push_back(W::NEVER);
    v.push_back(W::NEVER); v.push_back(W::NEVER); v.push_back(W::NEVER);
    return v;
}
static const std::vector<W> writePolicy = initWritePolicy();

/** data field names. */
std::vector<std::string> initDFNames()
{
    std::vector<std::string> v;
    v.push_back("SIGMA");
    v.push_back("Ex"); v.push_back("Ey"); v.push_back("Ez");
    v.push_back("Hx"); v.push_back("Hy"); v.push_back("Hz");
    v.push_back("Dx"); v.push_back("Dy"); v.push_back("Dz");
    v.push_back("Ix"); v.push_back("Iy"); v.push_back("Iz");
    v.push_back("Psi_ExDy"); v.push_back("Psi_ExDz"); v.push_back("Psi_EyDx");
    v.push_back("Psi_EYDz"); v.push_back("Psi_EzDx"); v.push_back("Psi_EzDy");
    v.push_back("Psi_HxDy"); v.push_back("Psi_HxDz"); v.push_back("Psi_HyDx");
    v.push_back("Psi_HyDz"); v.push_back("Psi_HzDx"); v.push_back("Psi_HzDy");
    return v;
}
static const std::vector<std::string> DFNames = initDFNames();

/** MPI halos. */
std::vector<int> initHaloWidths()
{
    std::vector<int> v;
    v.push_back(0);
    v.push_back(1); v.push_back(1); v.push_back(1);
    v.push_back(1); v.push_back(1); v.push_back(1);
    v.push_back(0); v.push_back(0); v.push_back(0);
    v.push_back(0); v.push_back(0); v.push_back(0);
    v.push_back(0); v.push_back(0); v.push_back(0);
    v.push_back(0); v.push_back(0); v.push_back(0);
    v.push_back(0); v.push_back(0); v.push_back(0);
    v.push_back(0); v.push_back(0); v.push_back(0);
    return v;
}
static const std::vector<int> HaloWidths = initHaloWidths();

template<typename CT, bool ForwardOnly>
class EMFDTDng: public ScaFES::Problem< EMFDTDng<CT, ForwardOnly>, CT, 3> {
public:
    static const size_t DIM=3;
private:
    typedef ScaFES::Problem< EMFDTDng<CT, ForwardOnly>, CT, 3> Super;
    typedef std::vector<std::string>::size_type size_type;
    typedef ScaFES::Ntuple<int, DIM> Index;
    typedef typename Index::value_type index_type;
    using PTree=boost::property_tree::ptree;

    // Stencils
    typedef CompleteCurlCPMLStencil<CT, Index, 0, 0, 1, 0, 1, 0> HxStencil;
    typedef CompleteCurlCPMLStencil<CT, Index, 1, 0, 0, 0, 0, 1> HyStencil;
    typedef CompleteCurlCPMLStencil<CT, Index, 0, 1, 0, 1, 0, 0> HzStencil;
    typedef CompleteCurlCPMLStencil<CT, Index, 0, -1, 0, 0, 0, -1> DxStencil;
    typedef CompleteCurlCPMLStencil<CT, Index, 0, 0, -1, -1, 0, 0> DyStencil;
    typedef CompleteCurlCPMLStencil<CT, Index, -1, 0, 0, 0, -1, 0> DzStencil;
public:

    EMFDTDng(
        ScaFES::Parameters const& cl,
        ScaFES::GridGlobal<DIM> const& gg,
        const PTree& ptree
        )
    : Super(cl, gg, true
        , DFNames // DF names
        , HaloWidths // MPI halo width
        , std::vector<bool>(DFNames.size(), false) // is known? --> false
        , std::vector<int>(DFNames.size(), 0) // border layers: 0 -> complete volume
        , std::vector<CT>(DFNames.size(), CT(0.)) // initial values for DFs
        , writePolicy
        , std::vector<bool>(DFNames.size(), false) // compute error? --> false
        , parse<CT>(ptree.get<std::string>("initialization.parameters"), " ") // params, vOld in initialize()
        )
    , ptree_(ptree)
    , cfln_(.5)
    , ds_(1.)
    , dt_((cfln_*ds_)/C_0)
    , cpmlp_(dt_,
        ptree_.get<std::size_t>("PML.d"),
        ptree_.get<CT>("PML.m_a"),
        ptree_.get<CT>("PML.m"),
        ptree_.get<CT>("PML.k_max"),
        ptree_.get<CT>("PML.s_max"),
        ptree_.get<CT>("PML.a_max")
        )
    , first_idx_(0, 0, 0)
    , last_idx_(cl.nNodes().at(0)-1, cl.nNodes().at(2)-1, cl.nNodes().at(2)-1)
    , loc_src_(vec2idx(parse<index_type>(ptree_.get<std::string>("source.location"))))
    , loc_sink_(vec2idx(parse<index_type>(ptree_.get<std::string>("sink.location"))))
    , reference_(load_reference(ptree_.get<std::string>("optimization.reference_data_file"), cl.nTimesteps()))
    , src_(dt_,ptree_)
    , hxstencil_(cpmlp_, Index(1, 0, 0), Index(0, -1, -1), last_idx_, cfln_)
    , hystencil_(cpmlp_, Index(0, 1, 0), Index(-1, 0, -1), last_idx_, cfln_)
    , hzstencil_(cpmlp_, Index(0, 0, 1), Index(-1, -1, 0), last_idx_, cfln_)
    , dxstencil_(cpmlp_, Index(0, 0, 1), Index(-1, 0, 0), last_idx_, -cfln_)
    , dystencil_(cpmlp_, Index(1, 0, 1), Index(0, -1, 0), last_idx_, -cfln_)
    , dzstencil_(cpmlp_, Index(1, 1, 0), Index(0, 0, -1), last_idx_, -cfln_)
    {
        boost::mpi::communicator world;

        if (world.rank()==0)
            std::cout<<"Source at "<<loc_src_
            <<"\nSink at "<<loc_sink_
            <<"\nInitialization parameters: "
            <<ptree.get<std::string>("initialization.parameters")
            <<std::endl;

        if (loc_src_==loc_sink_)
            throw std::runtime_error("Source and sink cannot be at the same position.");

        if ((loc_src_<first_idx_)||(loc_src_>last_idx_))
            throw std::runtime_error("Source not within simulation area.");
        if ((loc_sink_<first_idx_)||(loc_sink_>last_idx_))
            throw std::runtime_error("Sink not within simulation area.");

        if (cl.tau()!=1.0)
            throw std::runtime_error("Please choose \"--starttime\" and \"--endtime\" so that tau()=1.0.");
    }

    /**
     * Empty.
     */
    template<typename TT>
    void initInner(std::vector< ScaFES::DataField<TT, DIM> >& /*vNew*/,
        std::vector< ScaFES::DataField<TT, DIM> > const& /*vOld*/,
        Index const& /*idxNode*/,
        int const& /*timestep*/) { }

    /**
     * Empty.
     */
    template<typename TT>
    void initBorder(std::vector< ScaFES::DataField<TT, DIM> >& /*vNew*/,
            std::vector< ScaFES::DataField<TT, DIM> > const& /*vOld*/,
            Index const& /*idxNode*/,
            int const& /*timestep*/) { }

    /**
     * Initialize from p.
     */
    template <typename TT>
    void initInner
    (
     std::vector< ScaFES::DataField<TT, DIM> >& dfs,
     std::vector<TT> const& p,
     Index const& idx,
     int const& /*timestep*/
    )
    {
            typedef ScaFES::Ntuple<TT, DIM> TT3;
            TT3 fidx(TT(idx.elem(0)), TT(idx.elem(1)), TT(idx.elem(2)));

            const TT inside=3.546e7;
            const TT outside=0.;

            IndicatorEllipsoid<TT> e(p.at(0), p.at(1), p.at(2), p.at(3), p.at(4), p.at(5), inside, outside);

            const TT sigma=e(fidx);

            dfs[0](idx)=sigma;
    }

    /**
     * Wire to initInner.
     */
    template<typename TT>
    void initBorder(std::vector< ScaFES::DataField<TT, DIM> >& dfs,
            std::vector<TT> const& p,
            Index const& idx,
            int const& timestep)
    {
        this->initInner(dfs, p, idx, timestep);
    }

    /**
     * Compute H.
     */
    template<typename TT>
    void updateInner(std::vector< ScaFES::DataField<TT, DIM> >& n,
            std::vector< ScaFES::DataField<TT, DIM> > const& o,
            Index const& idx,
            int const& /*timestep*/)
    {
        // start by updating H from E
        n[sc(DFs::Hx)](idx)=hxstencil_(idx, o[sc(DFs::Hx)],
            o[sc(DFs::Ey)], o[sc(DFs::Psi_EyDz)], n[sc(DFs::Psi_EyDz)],
            o[sc(DFs::Ez)], o[sc(DFs::Psi_EzDy)], n[sc(DFs::Psi_EzDy)]);

        n[sc(DFs::Hy)](idx)=hystencil_(idx, o[sc(DFs::Hy)],
            o[sc(DFs::Ez)], o[sc(DFs::Psi_EzDx)], n[sc(DFs::Psi_EzDx)],
            o[sc(DFs::Ex)], o[sc(DFs::Psi_ExDz)], n[sc(DFs::Psi_ExDz)]);

        n[sc(DFs::Hz)](idx)=hzstencil_(idx, o[sc(DFs::Hz)],
            o[sc(DFs::Ex)], n[sc(DFs::Psi_ExDy)], n[sc(DFs::Psi_ExDy)],
            o[sc(DFs::Ey)], n[sc(DFs::Psi_EyDx)], n[sc(DFs::Psi_EyDx)]);
    }

    /**
     * Wire to updateInner.
     */
    template<typename TT>
    void updateBorder(std::vector< ScaFES::DataField<TT, DIM> >& vNew,
            std::vector< ScaFES::DataField<TT, DIM>>const& vOld,
            Index const& idx,
            int const& timestep)
    {
        updateInner(vNew, vOld, idx, timestep);
    }

    /**
     * Compute E.
     */
    template<typename TT>
    void updateInner2(std::vector< ScaFES::DataField<TT, DIM> >& n,
            std::vector< ScaFES::DataField<TT, DIM> > const& o,
            Index const& idx,
            int const& /*timestep*/)
    {
        // update D from H
        // n.b.:
        // 1) although comfortable, DO NOT USE "auto" for type deduction. ADOL-C yiels obscure and hard to debug compilation failures otherwise ("auto"-deduced rhs type is not assignable).
        // 2) although comfortable, NEVER violate the "compute -> use -> persist" order for (AD-)active variables. ADOL-C will simply not work if you persist a datum (i.e., write it to memory) and re-use it from there. The use of temporaries is a workaround.
        TT Dx=dxstencil_(idx, o[sc(DFs::Dx)],
            o[sc(DFs::Hz)], o[sc(DFs::Psi_HzDy)], n[sc(DFs::Psi_HzDy)],
            o[sc(DFs::Hy)], o[sc(DFs::Psi_HyDz)], n[sc(DFs::Psi_HyDz)]);

        TT Dy=dystencil_(idx, o[sc(DFs::Dy)],
            o[sc(DFs::Hx)], o[sc(DFs::Psi_HxDz)], n[sc(DFs::Psi_HxDz)],
            o[sc(DFs::Hz)], o[sc(DFs::Psi_HzDx)], n[sc(DFs::Psi_HzDx)]);

        TT Dz=dzstencil_(idx, o[sc(DFs::Dz)],
            o[sc(DFs::Hy)], o[sc(DFs::Psi_HyDx)], n[sc(DFs::Psi_HyDx)],
            o[sc(DFs::Hx)], o[sc(DFs::Psi_HxDy)], n[sc(DFs::Psi_HxDy)]);

        TT ga=1./(1.+(o[sc(DFs::SIGMA)](idx))*dt_/EPSILON_0);
        TT gb=(o[sc(DFs::SIGMA)](idx))*dt_/EPSILON_0;

        TT Ex=ga*(Dx-o[sc(DFs::Ix)](idx));
        TT Ey=ga*(Dy-o[sc(DFs::Iy)](idx));
        TT Ez=ga*(Dz-o[sc(DFs::Iz)](idx));

        n[sc(DFs::Ix)](idx)=o[sc(DFs::Ix)](idx)+(gb*Ex);
        n[sc(DFs::Iy)](idx)=o[sc(DFs::Iy)](idx)+(gb*Ey);
        n[sc(DFs::Iz)](idx)=o[sc(DFs::Iz)](idx)+(gb*Ez);

        // write temporaries to dependent fields, see above, item 2
        n[sc(DFs::Dx)](idx)=Dx;
        n[sc(DFs::Dy)](idx)=Dy;
        n[sc(DFs::Dz)](idx)=Dz;
        n[sc(DFs::Ex)](idx)=Ex;
        n[sc(DFs::Ey)](idx)=Ey;

        // inject source, use old time
        if (idx==loc_src_)
            n[sc(DFs::Ez)](idx)=src_((o[sc(DFs::Ez)]).time());
        else
            n[sc(DFs::Ez)](idx)=Ez;

        // collect sink
        if (idx==loc_sink_) {
            convert<CT, TT> c;
            CT val=c(n[sc(DFs::Ez)](idx));
            sink_.push_back(val);
        }
    }

    /**
     * Wire to updateInner2.
     */
    template<typename TT>
    void updateBorder2(std::vector< ScaFES::DataField<TT, DIM> >& vNew,
            std::vector< ScaFES::DataField<TT, DIM>>const& vOld,
            Index const& idx,
            int const& timestep)
    {
        updateInner2(vNew, vOld, idx, timestep);
    }

    /**
     * Empty.
     */
    template<typename TT>
    void evalInner(std::vector< ScaFES::DataField<TT, DIM> >& /*vNew*/,
        ScaFES::Ntuple<int, DIM> const& /*idxNode*/,
        int const& /*timestep*/) { }

    /**
     * Empty.
     */
    template<typename TT>
    void evalBorder(std::vector< ScaFES::DataField<TT, DIM> >& /*vNew*/,
            ScaFES::Ntuple<int, DIM> const& /*idxNode*/,
            int const& /*timestep*/) { }

    /**
     * Write sink data to file.
     */
    void writeSinkData()
    {
        boost::mpi::communicator world;

        if (world.size()!=1) {
            if (world.rank()==0) {
                if (sink_.size()==0)
                    world.recv(boost::mpi::any_source, 0, sink_);
            } else {
                if (sink_.size()!=0)
                    world.send(0, 0, sink_);
            }
        }

        if (world.rank()!=0)
            return;

        const std::string fname=ptree_.get<std::string>("sink.data_file");

        if (fname.size()==0)
            throw std::runtime_error("Sink file name empty.");

        std::cout<<"Writing sink data to file \""
            <<fname<<"\"..."
            <<std::flush;

        std::ofstream of(fname.c_str());
        of.setf(of.scientific);
        of.precision(20);

        for (auto it=sink_.begin(); it!=sink_.end(); ++it)
            of<<(*it)<<std::endl;

        std::cout<<"done."<<std::endl;
    }

private:
    /**
     * Layer structure for datafields with holes.
     */
    std::vector<int> layers(const PTree& p)
    {
        std::vector<int> ret(13, 0);
        const int d=p.get<int>("PML.d")+2;

        for (int i=0; i<12; ++i)
            ret.push_back(d);

        return ret;
    };

    /**
     * Parse into vector.
     * @param s
     * @param sep
     * @return
     */
    template <typename T>
    static std::vector<T> parse(const std::string& s, const char* sep="x")
    {
        boost::char_separator<char> csep(sep);
        boost::tokenizer<boost::char_separator<char> > tok(s, csep);

        std::vector<T> ret;

        for (auto ti=tok.begin(); ti!=tok.end(); ++ti) {
            try {
            ret.push_back(boost::lexical_cast<T>(*ti));
            }
            catch (...) {
                std::cout << "While parsing string \"" << s << "\":" << std::endl;
                throw;
            }
        }
        return ret;
    }

    /**
     * Make Index from vector.
     * @param v
     * @return
     */
    static Index vec2idx(const std::vector<typename Index::value_type>& v)
    {
        Index ret;

        for (unsigned long int i=0; i<Index::DIM; ++i)
            ret[i]=v.at(i);

        return ret;
    }

    /**
     * Cast "nice names" into accessible ints
     * @param df
     * @return
     */
    static size_type sc(const DFs & df)
    {
        return static_cast<size_type> (df);
    }

    /**
     * Load reference data from file.
     */
    static std::vector<CT> load_reference(const std::string& fname, const unsigned int& n)
    {
        boost::mpi::communicator world;
        std::vector<CT> ret;

        if (ForwardOnly==true) {
            if (world.rank()==0)
                std::cout<<"NOT reading reference from file. Calling TF evaluation routines WILL FAIL!"<<std::endl;
            return ret;
        }

        /** TODO: This statement is unreachable in case ForwardOnly=true. */
        if (world.rank()==0) {
            std::ifstream ifs(fname.c_str());

            if (!ifs.is_open())
                throw std::runtime_error("Failed opening file \""+fname+"\".");

            while ((ifs.good()) && (ret.size()<n)) {
                std::string line;
                std::getline(ifs, line);
                try {
                    ret.push_back(boost::lexical_cast<CT, std::string>(line));
                } catch (...) {
                    std::cout<<"While parsing reference file \"" << fname << "\": " <<std::endl;
                    throw;
                }
            }

            if (ret.size()<n)
                throw std::runtime_error("Reference file \""+fname+"\" contains too few items.");
        }

        if (world.size()>1)
            boost::mpi::broadcast(world, ret, 0);

        return ret;
    }

    const PTree ptree_;
    const CT cfln_, ds_, dt_;
    const CPMLParams<CT> cpmlp_;
    const Index first_idx_, last_idx_, loc_src_, loc_sink_;
    const std::vector<CT> reference_;
    const ModulatedGaussianSource<CT> src_;
    std::vector<CT> sink_;

    const HxStencil hxstencil_;
    const HyStencil hystencil_;
    const HzStencil hzstencil_;
    const DxStencil dxstencil_;
    const DyStencil dystencil_;
    const DzStencil dzstencil_;
};

#endif // EMFDTD_HPP_


#include "feature_estimator.h"

#include <Eigen/Dense>

namespace {

int calculate_medoid_index(std::vector<Eigen::Vector3d> &points) {
  std::vector<double> distances(points.size(), 0);
  for (auto i = 0u; i < points.size(); ++i) {
    for (auto j = 0u; j < points.size(); ++j) {
      const double distance = (points[i] - points[j]).eval().norm();
      distances[i] += distance;
    }
  }

  int min_idx = 0;
  double min_distance = std::numeric_limits<double>::max();
  for (auto i = 0u; i < distances.size(); ++i) {
    if (distances[i] < min_distance) {
      min_idx = i;
      min_distance = distances[i];
    }
  }

  return min_idx;
}

}  // namespace

namespace pcs {

FeatureEstimator::FeatureEstimator(std::shared_ptr<pcs::PointCloud> point_cloud,
                                   double voxel_size, int num_neighbors,
                                   int num_scales, int batch_size)
    : point_cloud_(point_cloud),
      voxel_size_(voxel_size),
      num_neighbors_(num_neighbors),
      num_scales_(num_scales),
      batch_size_(batch_size),
      tree_(nullptr),
      num_points_(0),
      has_colors_(false),
      pool_(std::thread::hardware_concurrency()) {
  if (point_cloud_ and not point_cloud_->empty()) {
    num_points_ = point_cloud_->points_.size();
    has_colors_ = point_cloud_->has_colors();
    tree_ = std::shared_ptr<KDTree<3>>(new KDTree<3>(
        point_cloud_->points_, std::thread::hardware_concurrency()));

    // Build points pyramid
    point_clouds_.push_back(point_cloud_);
    trees_.push_back(tree_);
    for (auto s = 1u; s < num_scales_; ++s) {
      const auto voxel_scale = std::pow(2, s);
      auto down_scaled_cloud =
          voxel_down_sample(*(point_clouds_[s - 1]), voxel_size_ * voxel_scale);
      point_clouds_.push_back(down_scaled_cloud);

      auto tree = std::shared_ptr<KDTree<3>>(new KDTree<3>(
          point_clouds_[s]->points_, std::thread::hardware_concurrency()));
      trees_.push_back(tree);
    }
  }
}

Eigen::VectorXd FeatureEstimator::get_features_for_point(
    std::size_t point_id) const {
  if (point_id >= num_points_) {
    std::string error_string("given point index { ");
    error_string +=
        std::to_string(point_id) + " } is bigger then point cloud size { ";
    error_string += std::to_string(num_points_) + " }";
    throw std::out_of_range(error_string);
  }

  const std::size_t num_features = feature_size();
  Eigen::VectorXd features(num_features);
  Eigen::Vector3d point = point_cloud_->points_[point_id];
  Eigen::Vector3d neig_color(0., 0., 0.);

  for (auto s = 0u; s < point_clouds_.size(); ++s) {
    const std::size_t scale_offset = s * features_per_scale;
    auto neighbors = trees_[s]->find_nns(point, num_neighbors_);
    if (s == 0 and has_colors_) {
      for (auto k = 0u; k < neighbors.size(); ++k) {
        neig_color += (point_cloud_->colors_[neighbors[k].first]);
      }
    }
    std::vector<Eigen::Vector3d> n_points;
    for (auto j = 0u; j < neighbors.size(); ++j) {
      n_points.push_back(point_clouds_[s]->points_[neighbors[j].first]);
    }

    Eigen::Vector3d medoid = n_points[calculate_medoid_index(n_points)];
    Eigen::Matrix3d cov = Eigen::Matrix3d::Zero();
    Eigen::MatrixXd data = Eigen::MatrixXd::Zero(num_neighbors_, 3);
    for (auto j = 0u; j < num_neighbors_; ++j) {
      data.row(j) = n_points[j].transpose();
    }

    Eigen::MatrixXd centered = data.rowwise() - data.colwise().mean();
    cov = (centered.transpose() * centered) / (num_neighbors_ - 1);

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig(cov);
    Eigen::Matrix3d eigenvectors = eig.eigenvectors();
    Eigen::Vector3d eigenvalues = eig.eigenvalues();
    eigenvalues.normalize();

    // Covariance features
    const double omnivariance =
        std::pow(eigenvalues(0) * eigenvalues(1) * eigenvalues(2), 1.0 / 3.0);

    features[scale_offset + 0] = omnivariance;
    double eigenentropy = 0.0;
    for (int k = 0; k < 3; ++k) {
      eigenentropy -= (eigenvalues(k) * std::log(eigenvalues(k)));
    }

    features[scale_offset + 1] = eigenentropy;

    const double anisotropy =
        (eigenvalues(2) - eigenvalues(0)) / eigenvalues(2);
    features[scale_offset + 2] = anisotropy;

    const double planarity = (eigenvalues(1) - eigenvalues(0)) / eigenvalues(2);
    features[scale_offset + 3] = planarity;

    const double linearity = (eigenvalues(2) - eigenvalues(1)) / eigenvalues(2);
    features[scale_offset + 4] = linearity;

    const double surface_variation = eigenvalues(0);
    features[scale_offset + 5] = surface_variation;

    const double scatter = eigenvalues(0) / eigenvalues(2);
    features[scale_offset + 6] = scatter;

    const double verticality = 1. - std::abs(eigenvectors.col(0)[2]);
    features[scale_offset + 7] = verticality;

    // Moments features
    double first_order_first_axis = 0;
    double first_order_second_axis = 0;
    double second_order_first_axis = 0;
    double second_order_second_axis = 0;
    for (auto &p : n_points) {
      Eigen::Vector3d diff = p - medoid;
      first_order_first_axis += diff.dot(eigenvectors.col(2));
      first_order_second_axis += diff.dot(eigenvectors.col(1));
      second_order_first_axis +=
          first_order_first_axis * first_order_first_axis;
      second_order_second_axis +=
          first_order_second_axis * first_order_second_axis;
    }
    features[scale_offset + 8] = first_order_first_axis;
    features[scale_offset + 9] = first_order_second_axis;
    features[scale_offset + 10] = second_order_first_axis;
    features[scale_offset + 11] = second_order_second_axis;

    // Height features
    double z_min = std::numeric_limits<double>::max();
    double z_max = -std::numeric_limits<double>::max();
    for (auto &p : n_points) {
      const double z = p.z();
      if (z < z_min) {
        z_min = z;
      }
      if (z > z_max) {
        z_max = z;
      }
    }

    const double vertical_range = z_max - z_min;
    features[scale_offset + 12] = vertical_range;

    const double height_below = point.z() - z_min;
    features[scale_offset + 13] = height_below;

    const double height_above = z_max - point.z();
    features[scale_offset + 14] = height_above;
  }

  // Color features calculated if initial cloud has colors
  if (has_colors_) {
    const std::size_t geometric_features_offset =
        num_scales_ * features_per_scale;
    Eigen::Vector3d main_color = point_cloud_->colors_[point_id];
    features[geometric_features_offset + 0] = main_color(0);
    features[geometric_features_offset + 1] = main_color(1);
    features[geometric_features_offset + 2] = main_color(2);

    neig_color /= num_neighbors_;
    features[geometric_features_offset + 3] = neig_color(0);
    features[geometric_features_offset + 4] = neig_color(1);
    features[geometric_features_offset + 5] = neig_color(2);
  }
  features =
      features.unaryExpr([](double v) { return std::isfinite(v) ? v : 0.0; });
  return features;
}

Eigen::MatrixXd FeatureEstimator::get_features_for_batch(
    std::size_t start_id, std::size_t end_id) const {
  if (end_id > point_cloud_->points_.size() or
      start_id >= point_cloud_->points_.size() or end_id <= start_id) {
    std::string error_string("invalid slice { ");
    error_string += std::to_string(start_id) + " : ";
    error_string += std::to_string(end_id) + " } for array with { ";
    error_string += std::to_string(point_cloud_->points_.size()) + " } points";
    throw std::out_of_range(error_string);
  }

  const std::size_t range = end_id - start_id;
  const std::size_t num_features = feature_size();
  Eigen::MatrixXd result(range, num_features);
  const double drange =
      static_cast<double>(range) / static_cast<double>(pool_.num_threads());
  const std::size_t num_points_per_thread =
      std::max<std::size_t>(1u, std::ceil(drange));
  const std::size_t max_threads = std::min(pool_.num_threads(), range);
  for (auto i = 0u; i < max_threads; ++i) {
    const auto s = start_id + i * num_points_per_thread;
    const auto e = std::min(end_id, s + num_points_per_thread);
    pool_.add_task([&result, s, e, start_id, this] {
      for (auto j = s; j < e; ++j) {
        result.row(j - start_id) = get_features_for_point(j);
      }
    });
  }
  pool_.wait();

  return result;
}

Eigen::MatrixXd FeatureEstimator::get_features_for_points(
    const std::vector<int> &idxs) const {
  const std::size_t range = idxs.size();
  const std::size_t num_features = feature_size();
  Eigen::MatrixXd result(range, num_features);
  const double drange =
      static_cast<double>(range) / static_cast<double>(pool_.num_threads());
  const std::size_t num_points_per_thread =
      std::max<std::size_t>(1u, std::ceil(drange));
  const std::size_t max_threads = std::min(pool_.num_threads(), range);
  std::vector<std::future<void>> futures;
  for (auto i = 0u; i < max_threads; ++i) {
    const auto s = i * num_points_per_thread;
    const auto e = std::min(range, s + num_points_per_thread);
    futures.push_back(pool_.add_task([&result, &idxs, s, e, this] {
      for (auto j = s; j < e; ++j) {
        result.row(j) = get_features_for_point(idxs[j]);
      }
    }));
  }
  for (auto &&x : futures) {
    x.get();
  }

  return result;
}

std::size_t FeatureEstimator::num_points() const { return num_points_; }

std::vector<unsigned int> FeatureEstimator::soft_voting_smoothing(
    const Eigen::MatrixXd &probabilities, std::size_t num_neighbors) const {
  std::vector<unsigned int> classes(probabilities.rows());

  if (probabilities.size() == 0) {
    return classes;
  }
  const std::size_t num_classes = probabilities.cols();

  const double drange = static_cast<double>(num_points_) /
                        static_cast<double>(pool_.num_threads());
  const std::size_t num_points_per_thread =
      std::max<std::size_t>(1u, std::ceil(drange));
  const std::size_t max_threads = std::min(pool_.num_threads(), num_points_);
  for (auto i = 0u; i < max_threads; ++i) {
    const auto s = i * num_points_per_thread;
    const auto e = std::min(num_points_, s + num_points_per_thread);
    pool_.add_task(
        [&classes, &probabilities, num_neighbors, num_classes, s, e, this] {
          for (auto point_id = s; point_id < e; ++point_id) {
            Eigen::Vector3d point = point_cloud_->points_[point_id];
            const Eigen::Vector3d main_color = point_cloud_->colors_[point_id];
            const auto neighbors = tree_->find_nns(point, num_neighbors);
            float weight_sum = 0.f;
            Eigen::VectorXd commite_result = Eigen::VectorXd::Zero(num_classes);
            for (const auto &neighbor : neighbors) {
              commite_result += probabilities.row(neighbor.first);
            }
            Eigen::VectorXd::Index index;
            commite_result.maxCoeff(&index);
            classes[point_id] = index;
          }
        });
  }
  pool_.wait();

  return classes;
}

std::size_t FeatureEstimator::num_batches() const {
  return static_cast<std::size_t>(
      std::ceil(static_cast<double>(num_points_) / batch_size_));
}

std::size_t FeatureEstimator::batch_size() const { return batch_size_; }

Eigen::MatrixXd FeatureEstimator::get_features_for_batch(
    std::size_t batch_id) const {
  const std::size_t start_id = batch_id * batch_size_;
  const std::size_t end_id =
      std::min<std::size_t>(start_id + batch_size_, num_points_);
  return get_features_for_batch(start_id, end_id);
}

std::size_t FeatureEstimator::feature_size() const {
  if (has_colors_) {
    return color_features + features_per_scale * num_scales_;
  } else {
    return features_per_scale * num_scales_;
  }
}

std::vector<unsigned int> FeatureEstimator::hard_voting_smoothing(
    const std::vector<unsigned int> &labels, std::size_t num_neighbors) const {
  std::vector<unsigned int> smooth_labels(labels.size(), 0);

  if (labels.empty()) {
    return smooth_labels;
  }

  const double drange = static_cast<double>(num_points_) /
                        static_cast<double>(pool_.num_threads());
  const std::size_t num_points_per_thread =
      std::max<std::size_t>(1u, std::ceil(drange));
  const std::size_t max_threads = std::min(pool_.num_threads(), num_points_);
  for (auto i = 0u; i < max_threads; ++i) {
    const auto s = i * num_points_per_thread;
    const auto e = std::min(num_points_, s + num_points_per_thread);
    pool_.add_task([this, s, e, num_neighbors, &smooth_labels, &labels] {
      for (auto point_id = s; point_id < e; ++point_id) {
        Eigen::Vector3d point = point_cloud_->points_[point_id];
        const auto neighbors = tree_->find_nns(point, num_neighbors);
        std::unordered_map<unsigned int, int> commite_result;
        for (const auto &neighbor : neighbors) {
          commite_result[labels[neighbor.first]]++;
        }
        unsigned int max_class = 0;
        int max_value = -1;
        for (auto &v : commite_result) {
          if (v.second > max_value) {
            max_class = v.first;
            max_value = v.second;
          }
        }
        smooth_labels[point_id] = max_class;
      }
    });
  }
  pool_.wait();

  return smooth_labels;
}

}  // namespace pcs


#include "headers/Object.hpp"
#include "headers/Sphere.hpp"
#include <armadillo>
#include <vector>

using namespace arma;
using namespace std;

Object::Object() {
}

vec Object::cor(const vec &d, const vector<Light> &luz, const vector<Object *> objetos, const Ambient ambiente, const vec &vetor, const unsigned int &indice){

    vec Ca, Co, Cs, V, L, H, cor_final;
    double T2;
    cor_final << 0 << 0 << 0;
    unsigned int ind=0;

    for (unsigned int i=0; i<luz.size(); i++){
        bool sombra=false;

	for (unsigned int j=0; j<objetos.size(); j++){

            if (j!=indice && objetos[j]->colide(d, T2, luz[i].GetPosicao())){
		if ((T2 > 0) && (T2 <= 1)) {
                    sombra=true;
                    ind = j;
                }
            }
	}

	if (!sombra){
            V = luz[i].GetPosicao()-d;
            L = normalise(luz[i].GetPosicao()-d);
            H = (L+V)/2;
            H = H / norm(H,2);

            if (this->getTipo() == 0){

                this->setNormal(d - this->getCentro());
                this->setNormal(normalise(this->getNormal()));
            }

            Co = this->getKd() % luz[i].GetI() * max(0.0, dot(this->getNormal(), L));
            Cs = this->getKs() % luz[i].GetI() * pow(max(0.0, dot(this->getNormal(), H)), this->getP());
            cor_final += Co + Cs;
	}
    }

    Ca = ambiente.GetIa() * ambiente.GetKa();
    cor_final += Ca;

    for (int i=0; i<3; i++){
	if (cor_final(i) > 255){
            cor_final(i) = 255;
        }

	else if (cor_final(i) < 0){
            cor_final(i) = 0;
        }
    }

    return cor_final;

}


#define BOOST_TEST_MODULE test_div
#include <boost/test/included/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <mave/mave.hpp>
#include <tests/generate_random_matrices.hpp>
#include <tests/tolerance.hpp>

typedef boost::mpl::list<
    mave::vector<double, 3>,    mave::vector<float, 3>,
    mave::matrix<double, 3, 3>, mave::matrix<float, 3, 3>
    > test_targets;

constexpr std::size_t N = 12000;

BOOST_AUTO_TEST_CASE_TEMPLATE(division, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto vectors = mave::test::generate_random<T>(N, mt);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);

    for(std::size_t i=0; i<N; ++i)
    {
        const auto& v = vectors.at(i);
        const auto& s = scalars.at(i);

        const auto v_ = v / s;
        for(std::size_t j=0; j<v.size(); ++j)
        {
            BOOST_TEST(v_[j] == v[j] / s,
                       mave::test::tolerance<typename T::value_type>());
        }
        BOOST_TEST(v.diagnosis());
        BOOST_TEST(v_.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(division_2arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=2)
    {
        const auto& s1 = scalars.at(i);
        const auto& s2 = scalars.at(i+1);
        const auto& v11 = vectors.at(i);
        const auto& v12 = vectors.at(i+1);

        const auto v2 = std::tie(v11, v12) / std::make_tuple(s1, s2);

        const auto v21 = std::get<0>(v2);
        const auto v22 = std::get<1>(v2);

        for(std::size_t j=0; j<v21.size(); ++j)
        {
            BOOST_TEST(v21[j] == v11[j] / s1,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v22[j] == v12[j] / s2,
                       mave::test::tolerance<typename T::value_type>());
        }

        BOOST_TEST(v11.diagnosis());
        BOOST_TEST(v12.diagnosis());
        BOOST_TEST(v21.diagnosis());
        BOOST_TEST(v22.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(division_3arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=3)
    {
        const auto& s1  = scalars.at(i);
        const auto& s2  = scalars.at(i+1);
        const auto& s3  = scalars.at(i+2);
        const auto& v11 = vectors.at(i);
        const auto& v12 = vectors.at(i+1);
        const auto& v13 = vectors.at(i+2);

        const auto v2 = std::tie(v11, v12, v13) / std::make_tuple(s1, s2, s3);

        const auto v21 = std::get<0>(v2);
        const auto v22 = std::get<1>(v2);
        const auto v23 = std::get<2>(v2);

        for(std::size_t j=0; j<v21.size(); ++j)
        {
            BOOST_TEST(v21[j] == v11[j] / s1,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v22[j] == v12[j] / s2,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v23[j] == v13[j] / s3,
                       mave::test::tolerance<typename T::value_type>());
        }
        BOOST_TEST(v11.diagnosis());
        BOOST_TEST(v12.diagnosis());
        BOOST_TEST(v13.diagnosis());

        BOOST_TEST(v21.diagnosis());
        BOOST_TEST(v22.diagnosis());
        BOOST_TEST(v23.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(division_4arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=4)
    {
        const auto& s1  = scalars.at(i);
        const auto& s2  = scalars.at(i+1);
        const auto& s3  = scalars.at(i+2);
        const auto& s4  = scalars.at(i+3);
        const auto& v11 = vectors.at(i);
        const auto& v12 = vectors.at(i+1);
        const auto& v13 = vectors.at(i+2);
        const auto& v14 = vectors.at(i+3);

        const auto v2 = std::tie(v11, v12, v13, v14) / std::make_tuple(s1, s2, s3, s4);

        const auto v21 = std::get<0>(v2);
        const auto v22 = std::get<1>(v2);
        const auto v23 = std::get<2>(v2);
        const auto v24 = std::get<3>(v2);

        for(std::size_t j=0; j<v21.size(); ++j)
        {
            BOOST_TEST(v21[j] == v11[j] / s1,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v22[j] == v12[j] / s2,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v23[j] == v13[j] / s3,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v24[j] == v14[j] / s4,
                       mave::test::tolerance<typename T::value_type>());
        }

        BOOST_TEST(v11.diagnosis());
        BOOST_TEST(v12.diagnosis());
        BOOST_TEST(v13.diagnosis());
        BOOST_TEST(v14.diagnosis());

        BOOST_TEST(v21.diagnosis());
        BOOST_TEST(v22.diagnosis());
        BOOST_TEST(v23.diagnosis());
        BOOST_TEST(v24.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(div_assign_1arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; ++i)
    {
        const auto& s  = scalars.at(i);
        const auto& v1 = vectors.at(i);

        auto v2 = v1;
        v2 /= s;
        for(std::size_t j=0; j<v1.size(); ++j)
        {
            BOOST_TEST(v2[j] == v1[j] / s,
                       mave::test::tolerance<typename T::value_type>());
        }
        BOOST_TEST(v1.diagnosis());
        BOOST_TEST(v2.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(div_assign_2arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=2)
    {
        const auto& s1 = scalars.at(i);
        const auto& s2 = scalars.at(i+1);
        const auto& v11 = vectors.at(i);
        const auto& v12 = vectors.at(i+1);

        auto v21(v11);
        auto v22(v12);
        std::tie(v21, v22) /= std::make_tuple(s1, s2);

        for(std::size_t j=0; j<v21.size(); ++j)
        {
            BOOST_TEST(v21[j] == v11[j] / s1,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v22[j] == v12[j] / s2,
                       mave::test::tolerance<typename T::value_type>());
        }

        BOOST_TEST(v11.diagnosis());
        BOOST_TEST(v12.diagnosis());

        BOOST_TEST(v21.diagnosis());
        BOOST_TEST(v22.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(div_assign_3arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=3)
    {
        const auto& s1  = scalars.at(i);
        const auto& s2  = scalars.at(i+1);
        const auto& s3  = scalars.at(i+2);
        const auto& v11 = vectors.at(i);
        const auto& v12 = vectors.at(i+1);
        const auto& v13 = vectors.at(i+2);

        auto v21(v11);
        auto v22(v12);
        auto v23(v13);
        std::tie(v21, v22, v23) /= std::make_tuple(s1, s2, s3);

        for(std::size_t j=0; j<v21.size(); ++j)
        {
            BOOST_TEST(v21[j] == v11[j] / s1,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v22[j] == v12[j] / s2,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v23[j] == v13[j] / s3,
                       mave::test::tolerance<typename T::value_type>());
        }

        BOOST_TEST(v11.diagnosis());
        BOOST_TEST(v12.diagnosis());
        BOOST_TEST(v13.diagnosis());

        BOOST_TEST(v21.diagnosis());
        BOOST_TEST(v22.diagnosis());
        BOOST_TEST(v23.diagnosis());
    }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(div_assign_4arg, T, test_targets)
{
    std::mt19937 mt(123456789);
    const auto scalars = mave::test::generate_random<typename T::value_type>(N, mt);
    const auto vectors = mave::test::generate_random<T>(N, mt);

    for(std::size_t i=0; i<N; i+=4)
    {
        const auto& s1  = scalars.at(i);
        const auto& s2  = scalars.at(i+1);
        const auto& s3  = scalars.at(i+2);
        const auto& s4  = scalars.at(i+3);
        const auto& v11 = vectors.at(i);
        const auto& v12 = vectors.at(i+1);
        const auto& v13 = vectors.at(i+2);
        const auto& v14 = vectors.at(i+3);

        auto v21(v11);
        auto v22(v12);
        auto v23(v13);
        auto v24(v14);
        std::tie(v21, v22, v23, v24) /= std::make_tuple(s1, s2, s3, s4);

        for(std::size_t j=0; j<v21.size(); ++j)
        {
            BOOST_TEST(v21[j] == v11[j] / s1,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v22[j] == v12[j] / s2,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v23[j] == v13[j] / s3,
                       mave::test::tolerance<typename T::value_type>());
            BOOST_TEST(v24[j] == v14[j] / s4,
                       mave::test::tolerance<typename T::value_type>());
        }

        BOOST_TEST(v11.diagnosis());
        BOOST_TEST(v12.diagnosis());
        BOOST_TEST(v13.diagnosis());
        BOOST_TEST(v14.diagnosis());

        BOOST_TEST(v21.diagnosis());
        BOOST_TEST(v22.diagnosis());
        BOOST_TEST(v23.diagnosis());
        BOOST_TEST(v24.diagnosis());
    }
}


// BSD 3-Clause License

// Copyright (c) 2021, Chenyu
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice,
// this
//    list of conditions and the following disclaimer.

// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.

// 3. Neither the name of the copyright holder nor the names of its
//    contributors may be used to endorse or promote products derived from
//    this software without specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

// edited by Steffen Urban (urbste@googlemail.com), August 2021

#include "theia/sfm/global_pose_estimation/l1_rotation_global_estimator.h"

#include <Eigen/SparseCore>
#include <glog/logging.h>

#include "theia/math/l1_solver.h"
#include "theia/math/rotation.h"
#include "theia/sfm/global_pose_estimation/rotation_estimator_util.h"
#include "theia/util/map_util.h"
#include "theia/util/timer.h"

namespace theia {

L1RotationGlobalEstimator::L1RotationGlobalEstimator(
    const int num_orientations,
    const int num_edges,
    const L1RotationOptions& options)
    : options_(options) {
  tangent_space_step_.resize((num_orientations - 1) * 3);
  tangent_space_residual_.resize(num_edges * 3);
}

void L1RotationGlobalEstimator::SetViewIdToIndex(
    const std::unordered_map<ViewId, int>& view_id_to_index) {
  view_id_to_index_ = view_id_to_index;
}

void L1RotationGlobalEstimator::SetSparseMatrix(
    const Eigen::SparseMatrix<double>& sparse_matrix) {
  sparse_matrix_ = sparse_matrix;
}

bool L1RotationGlobalEstimator::SolveL1Regression(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& relative_rotations,
    std::unordered_map<ViewId, Eigen::Vector3d>* global_rotations) {
  const int num_edges = relative_rotations.size();

  CHECK_NOTNULL(global_rotations);
  CHECK_GT(global_rotations->size(), 0);
  CHECK_GT(num_edges, 0);

  if (view_id_to_index_.empty()) {
    ViewIdToAscentIndex(*global_rotations, &view_id_to_index_);
  }

  if (sparse_matrix_.rows() == 0) {
    SetupLinearSystem(relative_rotations,
                      (*global_rotations).size(),
                      view_id_to_index_,
                      &sparse_matrix_);
  }

  L1Solver<Eigen::SparseMatrix<double>>::Options l1_solver_options;
  l1_solver_options.max_num_iterations = 5;
  L1Solver<Eigen::SparseMatrix<double>> l1_solver(l1_solver_options,
                                                  sparse_matrix_);

  tangent_space_step_.setZero();
  ComputeResiduals(relative_rotations, global_rotations);

  Timer timer;
  for (int i = 0; i < options_.max_num_l1_iterations; i++) {
    l1_solver.Solve(tangent_space_residual_, &tangent_space_step_);
    UpdateGlobalRotations(global_rotations);
    ComputeResiduals(relative_rotations, global_rotations);

    double avg_step_size = ComputeAverageStepSize();

    if (avg_step_size <= options_.l1_step_convergence_threshold) {
      break;
    }
    l1_solver_options.max_num_iterations *= 2;
    l1_solver.SetMaxIterations(l1_solver_options.max_num_iterations);
  }

  LOG(INFO) << "Total time [L1Regression]: "
            << timer.ElapsedTimeInSeconds() * 1e3 << " ms.";

  return true;
}

void L1RotationGlobalEstimator::UpdateGlobalRotations(
    std::unordered_map<ViewId, Eigen::Vector3d>* global_rotations) {
  for (auto& rotation : *global_rotations) {
    const int view_index = FindOrDie(view_id_to_index_, rotation.first) - 1;
    if (view_index == kConstantRotationIndex) {
      continue;
    }

    // Apply the rotation change to the global orientation.
    const Eigen::Vector3d& rotation_change =
        tangent_space_step_.segment<3>(3 * view_index);
    rotation.second =
        theia::MultiplyRotations(rotation.second, rotation_change);
  }
}

void L1RotationGlobalEstimator::ComputeResiduals(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& relative_rotations,
    std::unordered_map<ViewId, Eigen::Vector3d>* global_rotations) {
  int rotation_error_index = 0;

  for (const auto& relative_rotation : relative_rotations) {
    const Eigen::Vector3d& relative_rotation_aa =
        relative_rotation.second.rotation_2;
    const Eigen::Vector3d& rotation1 =
        FindOrDie(*global_rotations, relative_rotation.first.first);
    const Eigen::Vector3d& rotation2 =
        FindOrDie(*global_rotations, relative_rotation.first.second);

    // Compute the relative rotation error as:
    //   R_err = R2^t * R_12 * R1.
    tangent_space_residual_.segment<3>(3 * rotation_error_index) =
        theia::MultiplyRotations(
            -rotation2,
            theia::MultiplyRotations(relative_rotation_aa, rotation1));
    ++rotation_error_index;
  }
}

double L1RotationGlobalEstimator::ComputeAverageStepSize() {
  // compute the average step size of the update in tangent_space_step_
  const int num_vertices = tangent_space_step_.size() / 3;
  double delta_V = 0;
  for (int k = 0; k < num_vertices; ++k) {
    delta_V += tangent_space_step_.segment<3>(3 * k).norm();
  }
  return delta_V / num_vertices;
}

}  // namespace theia


#include <limits>
#include <Eigen/Geometry>
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/numpy.h>

#include <igl/point_mesh_squared_distance.h>
#include <igl/AABB.h>

namespace py = pybind11;
using namespace Eigen;

typedef Eigen::Vector3d Vec3d;
typedef Eigen::Vector2d Vec2d;

std::tuple<py::array_t<int>, py::array_t<double>, py::array_t<int>, py::array_t<double>>
rayMeshIntersect(
        py::array_t<double> verts_in,
        py::array_t<int> tris_in,
        py::array_t<double> ray_pts_in,
        py::array_t<double> ray_dirs_in,
        double max_distance,
        double max_angle,
        bool allow_backface_hit)
{
    auto verts = verts_in.unchecked<2>();
    auto tris = tris_in.unchecked<2>();
    auto rayPts = ray_pts_in.unchecked<2>();
    auto rayDirs = ray_dirs_in.unchecked<2>();

    std::vector<int> hitTris;
    std::vector<Vec2d> hitUVs;
    std::vector<Vec3d> hitPoints;
    std::vector<int> hitRayIndices;

    double maxAngleCos = std::cos(max_angle);
    for(int iray = 0; iray < rayPts.shape(0); iray++) {
        Vec3d rayPos(rayPts(iray, 0), rayPts(iray, 1), rayPts(iray, 2));
        Vec3d rayDir(rayDirs(iray, 0), rayDirs(iray, 1), rayDirs(iray, 2));
        Vec3d rayDirNorm = rayDir.normalized();
        Vec3d bestHit;
        float bestU, bestV, bestAngleCos;
        int bestTri = -1;
        float best_t = std::numeric_limits<float>::infinity();
        for(int itri = 0; itri < tris.shape(0); itri++) {
            // put data into Vec3d for easy dot/cross operations
            auto i1 = tris(itri, 0); auto i2 = tris(itri, 1); auto i3 = tris(itri, 2);
            Vec3d v1(verts(i1, 0), verts(i1, 1), verts(i1, 2));
            Vec3d v2(verts(i2, 0), verts(i2, 1), verts(i2, 2));
            Vec3d v3(verts(i3, 0), verts(i3, 1), verts(i3, 2));
            // perform ray-triangle hit test
            Vec3d edge1 = v2 - v1;
            Vec3d edge2 = v3 - v1;
            Vec3d pvec = rayDir.cross(edge2);
            float det = edge1.dot(pvec);
            if(std::abs(det) < std::numeric_limits<double>::epsilon()) {
                continue;
            }
            float invDet = 1.0 / det;
            Vec3d tvec = rayPos - v1;
            float u = tvec.dot(pvec) * invDet;
            if(u < 0.0 || u > 1.0) {
                continue;
            }
            Vec3d qvec = tvec.cross(edge1);
            float v = rayDir.dot(qvec) * invDet;
            if(v < 0.0 || u + v > 1.0) {
                continue;
            }
            float t = edge2.dot(qvec) * invDet;
            Vec3d hitPoint = v1 + edge1*u + edge2*v;
            // check if nearest hit and if it is valid
            if(fabs(t) < fabs(best_t) && (hitPoint - rayPos).norm() < max_distance) {
                bestHit = hitPoint;
                bestU = u; bestV = v;
                bestTri = itri;
                best_t = t;
                Vec3d normal = edge1.normalized().cross(edge2.normalized()).normalized();
                if (allow_backface_hit) {
                    bestAngleCos = fmax(rayDirNorm.dot(normal), rayDirNorm.dot(normal * -1.f));
                }
                else {
                    bestAngleCos = rayDirNorm.dot(normal);
                }
            }
        }
        if(bestTri > -1) {
            // check ray-normal angle
            if(bestAngleCos < maxAngleCos) {
                continue;
            }
            hitUVs.emplace_back(bestU, bestV);
            hitPoints.push_back(bestHit);
            hitRayIndices.push_back(iray);
            hitTris.push_back(bestTri);
        }
    }

    return std::make_tuple(
            py::array_t<int>(hitTris.size(), hitTris.data()),
            py::array_t<double>({(unsigned long)hitUVs.size(), 2ul}, (double*)hitUVs.data()),
            py::array_t<int>(hitRayIndices.size(), hitRayIndices.data()),
            py::array_t<double>({(unsigned long)hitPoints.size(), 3ul}, (double*)hitPoints.data())
    );
}

std::tuple<VectorXi, MatrixX2d, VectorXi>
rayMeshIntersectFast(
        const MatrixXd& verts,
        const MatrixXi& tris,
        const MatrixXd& ray_pts,
        const MatrixXd& ray_dirs)
{
    igl::AABB<MatrixXd, 3> tree;
    tree.init(verts, tris);

    std::vector<std::pair<igl::Hit, int>> hits;
    for (int i = 0; i < ray_pts.rows(); i++) {
        igl::Hit hit;
        if (tree.intersect_ray(verts, tris, ray_pts.row(i), ray_dirs.row(i), hit)) {
            hits.emplace_back(hit, i);
        }
    }

    VectorXi tri_ixs(hits.size());
    MatrixX2d barys(hits.size(), 2);
    VectorXi ray_ixs(hits.size());

    for(int i = 0; i < hits.size(); i++) {
        ray_ixs(i) = hits[i].second;
        const igl::Hit & hit = hits[i].first;
        tri_ixs(i) = hit.id;
        barys.row(i) = Vec2d((double)hit.u, (double)hit.v);
    }

    return std::make_tuple(tri_ixs, barys, ray_ixs);
}

std::tuple<VectorXd, VectorXi, MatrixX3d, MatrixX2d>
closestPointOnMesh(const MatrixXd& P, const MatrixXd& V, const MatrixXi& tris)
{
    VectorXd sq_dists;
    MatrixX3d hit_pts;
    VectorXi tri_ixs;
    igl::point_mesh_squared_distance(P, V, tris, sq_dists, tri_ixs, hit_pts);

    // determine uv coordinates of those closest hits
    MatrixX2d hit_uv(hit_pts.rows(), 2);
    for (int i = 0; i < P.rows(); ++i) {
        Vector3d p = hit_pts.row(i);
        Vector3i tri = tris.row(tri_ixs(i));
        // setup local coordinate frame
        Vector3d e10 = V.row(tri(1)) - V.row(tri(0));
        Vector3d e20 = V.row(tri(2)) - V.row(tri(0));
        Vector3d n = e10.cross(e20); // normal
        Matrix3d F;
        F << e10, e20, n;
        // invert local coordinate frame to get
        // frame-local coordinates
        Vector3d v0 = hit_pts.row(i) - V.row(tri(0));
        Vector3d uvw = F.inverse() * v0;
        hit_uv(i, 0) = uvw(0);
        hit_uv(i, 1) = uvw(1);
    }

    return std::make_tuple(sq_dists, tri_ixs, hit_pts, hit_uv);
};

PYBIND11_PLUGIN(_intersections_ext) {
    using namespace pybind11::literals;

    py::module m("_intersections_ext");
    m.def("ray_mesh_intersect", &rayMeshIntersect,
            "verts"_a, "tris"_a, "ray_pts"_a, "ray_dirs"_a,
            "max_distance"_a = std::numeric_limits<double>::infinity(),
            "max_angle"_a = std::numeric_limits<double>::infinity(),
            "allow_backface_hit"_a = true);

    m.def("ray_mesh_intersect_fast", &rayMeshIntersectFast,
            "verts"_a, "tris"_a, "ray_pts"_a, "ray_dirs"_a);

    m.def("closest_points_on_mesh", &closestPointOnMesh,
          "points"_a, "vertices"_a, "triangles"_a);

    return m.ptr();
}


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <vector>
#include <math.h>

#include "NRGclasses.hpp"
using namespace std;

//////////////
double CNRGarray::EniBlockj(int i, int j){

  // returns the energy of the i-th state in block j

  double aux=0.0;

  if ( (2*j-2)<BlockBegEnd.size() )
    {
      if ( (BlockBegEnd[2*j-2]+(i-1))<dEn.size() )
	{
	  aux=dEn[BlockBegEnd[2*j-2]+(i-1)];
	}
    }

  return(aux);

}
//////////////
// void CNRGarray::SetsizeBlock(){

//   int ii,i1;
//   for (ii=0;ii<BlockBegEnd.size();ii+=2)
//     {
//       sizeBlock.push_back(BlockBegEnd[ii+1]-BlockBegEnd[ii]+1);
//     }

// }
//////////////
void CNRGarray::PrintQNumbers(){

  int ii;
  cout << "Qnumbers : " << endl;
  for (ii=0;ii<QNumbers.size();ii++)
    cout << QNumbers[ii] << "  ";
  cout << endl;
}

//////////////
void CNRGarray::PrintAll(){

  int ii;
  void CNRGarray::PrintQNumbers();

  PrintQNumbers();

  cout << "dEn : " << endl;
  for (ii=0;ii<dEn.size();ii++)
    cout << dEn[ii] << " ";
  cout << endl;

  cout << "BlockBegEnd : " << endl;
  for (ii=0;ii<BlockBegEnd.size();ii+=2)
    cout << BlockBegEnd[ii] << " "<< BlockBegEnd[ii+1] << ", ";
  cout << endl;

  cout << "Size Blocks : " << endl;
  for (ii=0;ii<NumBlocks();ii++)
    cout << GetBlockSize(ii) << " ";
  cout << endl;
}
//////////////

int CNRGarray::NumBlocks(){

  return((int)(QNumbers.size()/NQNumbers));

}
//////////////
int CNRGarray::Nstates(){

  return(BlockBegEnd[NQNumbers*(NumBlocks()-1)+1]-BlockBegEnd[0]+1);

}

//////////////
void CNRGarray::SetDegen(){

  int nst,ii,jj;

  // Needs work

//   iDegen.push_back(1);
//   ii=1;
//   while ( ii<dEn.size() )
//     {
//       if (fabs(dEn[ii]-dEn[ii-1])<1.0E-10) nst=iDegen[ii-1]+1;
//       else nst=1;
//       iDegen.push_back(nst);
//       ii++;
//     }

}

void CNRGarray::FilterQNumbers(){

  int equal=0;
  int ii,i1;

  vector<double>::iterator qnums_iter,qnums_iter2,qnums_ii1,qnums_ii2;

  for (qnums_iter=QNumbers.begin(); qnums_iter<QNumbers.end();
       qnums_iter+=NQNumbers)
    {

      //      cout << "Iter1 = " << (*qnums_iter) << endl;
      ii=1;
      qnums_iter2=qnums_iter+ii*NQNumbers;
      while (qnums_iter2<QNumbers.end())
	{
	  //  cout << "Iter2 = " << (*qnums_iter2) << endl;
	  equal=1;
	  i1=0;
	  for (qnums_ii2=qnums_iter2;
	       qnums_ii2<qnums_iter2+NQNumbers;qnums_ii2++)
	    {
	      qnums_ii1=qnums_iter+i1;
	      if ( (*qnums_ii2==*qnums_ii1)&&(equal==1) ) equal=1;
	      else equal=0;
	      i1++;
	    }
	  if (equal==1)
	    {
	      QNumbers.erase(qnums_iter2,qnums_iter2+NQNumbers);
	    }
	  ii++;
	  qnums_iter2=qnums_iter+ii*NQNumbers;
	}
    }

}

//////////////
void CNRGarray::ClearAll(){

  QNumbers.clear();
  dEn.clear();
  dEigVec.clear();
  iDegen.clear();

  BlockBegEnd.clear();

}

//////////////
double CNRGarray::GetQNumber(int iblock, int whichqn){

  int iq=iblock*NQNumbers+whichqn;

  return(QNumbers[iq]);
}

//////////////
int CNRGarray::GetBlockLimit(int iblock, int whichlimit){

  return(BlockBegEnd[2*iblock+whichlimit]);

}

//////////////
int CNRGarray::GetBlockSize(int iblock){

  return(BlockBegEnd[2*iblock+1]-BlockBegEnd[2*iblock]+1);

}

////////////////////////////////////
// Class CNRGbasisarray functions //
////////////////////////////////////

//////////////
//void CNRGbasisarray::ClearVecsBasis(){
void CNRGbasisarray::ClearAll(){

  CNRGarray::ClearAll();

  iType.clear();
  StCameFrom.clear();

}
//////////////
void CNRGbasisarray::PrintAll(){

  CNRGarray::PrintAll();

  cout << "Type : " << endl;
  for (int ii=0;ii<iType.size();ii++)
    cout << iType[ii] << " ";
  cout << endl;

  cout << "StCameFrom : " << endl;
  for (int ii=0;ii<StCameFrom.size();ii++)
    cout << StCameFrom[ii] << " ";
  cout << endl;

}
///////////////////

////////////////////////////////////
// Class CNRGmatrix functions //
////////////////////////////////////

// vector<double> CNRGmatrix::GetFullMatrix(int Nst){

//   vector<double> Mat((Nst*Nst),0.0); //All zeros

//   int i1;
//   for (int ii=0;ii<vec.size();ii++)
// 	{
// 	  i1=ij2r(Nst,vec[ii].ist,vec[ii].jst);
// 	  if (i1<Mat.size()) Mat[i1]=vec[ii].val;
// 	  i1=ij2r(Nst,vec[ii].jst,vec[ii].ist);
// 	  if (i1<Mat.size()) Mat[i1]=vec[ii].val;
// 	}

//   return(Mat);

// }
//////////////
int CNRGmatrix::GetMatBlockLimit(int iblock1, int iblock2, int whichlimit){

  int ii=0;

  while ( ((iblock1!=MatBlockMap[ii])||(iblock2!=MatBlockMap[ii+1]))
	  &&(ii<MatBlockMap.size()) ) ii+=2;

  return(MatBlockBegEnd[2*ii+whichlimit]);

}

//////////////
int CNRGmatrix::GetMatBlockSize(int iblock1, int iblock2){

  int ii=0;

  while ( ((iblock1!=MatBlockMap[ii])||(iblock2!=MatBlockMap[ii+1]))
	  &&(ii<MatBlockMap.size()) ) ii+=2;

  return(MatBlockBegEnd[2*ii+1]-MatBlockBegEnd[2*ii]+1);

}

void CNRGmatrix::DiagBlock(int iblock,
			   vector<double> &eigvalues,
			   vector<double> &eigvectors){

  // LAPACK variables
  char JOBZ='N',UPLO='U';
  int Nst;
  int INFO=0,LDA,LWORK;

  // LDA=Nst
  // LWORK=3*Nst-1

  double *A;
  double *eigv;
  double *work;

  // Get Block info

  Nst=GetMatBlockSize(iblock,iblock);
//   cout << "Size block                 : " << GetBlockSize(iblock) << endl;
//   cout << "No of independent elements : " << GetMatrixBlockSize(iblock)
//        << endl;

  //Nst=2;
  LDA=Nst;
  LWORK=3*Nst-1;
  eigv = new double [Nst];
  work = new double [LWORK];
  A = new double[Nst*Nst];

  // I'll work on a routine to map upper triangular to r.
  // For now, let's use this:
  double **Aaux;
  Aaux = new double*[Nst];
  for (int ii=0;ii<Nst;ii++) Aaux[ii]=new double[Nst];

  for (int ii=0;ii<MatEl.size();ii++)
    {
//       // This takes a tremendous amount of CPU time! Must do better!
//       if ( (vec[ii].Bli==iblock)&&(vec[ii].Blj==iblock) )
// 	{
// 	  // Awful construcion. An invitation to segmentation faults.
// 	  Aaux[vec[ii].ibl][vec[ii].jbl]=vec[ii].val;
// 	  Aaux[vec[ii].jbl][vec[ii].ibl]=vec[ii].val;
// 	}
    }
  int i1=0;
  for (int ii=0;ii<Nst;ii++)
    {
      for (int jj=0;jj<Nst;jj++)
	{
	  A[i1]=Aaux[ii][jj];
	  i1++;
	}
    }
//   int i1=0;
//   int r1=0;
//   for (int i=GetBlockLimit(iblock,0);
//            i<GetBlockLimit(iblock,1);i++)
//     {
//       for (int j=i;
//            j<GetBlockLimit(iblock,1);j++)
// 	{
// 	  r1=ij2r(Nstates(),i,j);
//       Aaux[]=vec[r1].val;
// 	}
//       i1++;
//     }

//   A[0]=1.0;
//   A[1]=-sqrt(2.0);
//   A[2]=-sqrt(2.0);
//   A[3]=1.0;

  dsyev_(&JOBZ,&UPLO,&Nst,&A[0],&LDA,eigv,work,&LWORK,&INFO);

  for (int ii=0;ii<Nst;ii++)
    {
      eigvalues.push_back(eigv[ii]);
      eigvectors.push_back(eigv[ii]);
      cout << " Block  : " << iblock << endl;
      std::cout << "E = " << eigvalues[ii] << std::endl;
    }

  delete[] eigv;
  delete[] work;
  delete[] A;

  for (int ii=0;ii<Nst;ii++) delete[] Aaux[ii];
  delete[] Aaux;

}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_ONEOSQRTEPS_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_ONEOSQRTEPS_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Oneosqrteps generic tag

     Represents the Oneosqrteps constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER(Oneosqrteps,double,0
                                , 0x453504f3UL
                                , 0x4190000000000000ULL)
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Oneosqrteps, Site> dispatching_Oneosqrteps(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Oneosqrteps, Site>();
   }
   template<class... Args>
   struct impl_Oneosqrteps;
  }
  /*!
    Generates \f$1/\sqrt\epsilon\f$

    @par Semantic:

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

    is similar to:

    @code
    if T is integral
      r = T(0)
    else if T is double
      r =  pow(2.0, 26);
    else if T is float
      r =  pow(2.0f, 11.5);
    @endcode

  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Oneosqrteps, Oneosqrteps)
} }

#include <boost/simd/constant/common.hpp>

#endif


#pragma once

#include <array>
#include <cfloat>
#include <exception>
#include <fstream>
#include <functional>
#include <sstream>
#include <vector>

#include <Eigen/Geometry>

#include "MotionFrameCollection.hpp"
#include "Quaternion.hpp"

#include "geometry.hpp"

class SkeletonTree {
public:
  class Node {
  public:
    typedef std::array<float, 3> Channel;
    typedef std::array<float, 3> Offset;

    Node()
        : nodeLabel_(""), nodeName_(""), childNodes_(std::vector<Node *>(0)) {
      translationChannel_[0] = 0.0f;
      translationChannel_[1] = 0.0f;
      translationChannel_[2] = 0.0f;

      angleChannel_[0] = 0.0f;
      angleChannel_[1] = 0.0f;
      angleChannel_[2] = 0.0f;

      offset_[0] = 0.0f;
      offset_[1] = 0.0f;
      offset_[2] = 0.0f;

      rotationQuaternion_ = Eigen::Quaternion<float>::Identity();
    }

    // CC
    Node(const Node &otherNode)
        : nodeLabel_(""), nodeName_(""), childNodes_(std::vector<Node *>(0)) {
      nodeLabel_ = otherNode.nodeLabel_;
      nodeName_ = otherNode.nodeName_;
      translationChannel_ = otherNode.translationChannel_;
      angleChannel_ = otherNode.angleChannel_;
      offset_ = otherNode.offset_;
      rotationQuaternion_ = otherNode.rotationQuaternion_;

      // recursively copy construct the child nodes on the heap
      for (Node *otherNodeChild : otherNode.childNodes_) {
        childNodes_.push_back(new Node(*otherNodeChild));
      }
    }

    // AO
    Node &operator=(const Node &otherNode) {
      nodeLabel_ = otherNode.nodeLabel_;
      nodeName_ = otherNode.nodeName_;
      translationChannel_ = otherNode.translationChannel_;
      angleChannel_ = otherNode.angleChannel_;
      offset_ = otherNode.offset_;
      rotationQuaternion_ = otherNode.rotationQuaternion_;

      // recursively copy construct the child nodes on the heap
      for (Node *otherNodeChild : otherNode.childNodes_) {
        childNodes_.push_back(new Node(*otherNodeChild));
      }

      return *this;
    }

    ~Node() {
      for (Node *nextChildNode : childNodes_) {
        delete nextChildNode;
      }
    }

    std::string getLabel() const { return nodeLabel_; }

    void setLabel(const std::string &newNodeLabel) {
      nodeLabel_ = newNodeLabel;
    }

    std::string getName() const { return nodeName_; }

    void setName(const std::string &newNodeName) { nodeName_ = newNodeName; }

    Channel getTranslationChannel() const { return translationChannel_; }

    void setTranslationChannel(const Channel &newTranslationChannel) {
      translationChannel_ = newTranslationChannel;
    }

    // todo: rm
    Channel getAngleChannel() const { return angleChannel_; }

    // todo: rm
    void setAngleChannel(const Channel &newAngleChannel) {
      angleChannel_ = newAngleChannel;

      //// update rotation quaternion
      rotationQuaternion_ = Eigen::Quaternion<float>::Identity();

      Eigen::Quaternion<float> zAxisRotation(
          cos(degreesToRadians(angleChannel_[0]) / 2), 0.0f, 0.0f,
          sin(degreesToRadians(angleChannel_[0]) / 2));
      Eigen::Quaternion<float> yAxisRotation(
          cos(degreesToRadians(angleChannel_[1]) / 2), 0.0f,
          sin(degreesToRadians(angleChannel_[1]) / 2), 0.0f);
      Eigen::Quaternion<float> xAxisRotation(
          cos(degreesToRadians(angleChannel_[2]) / 2),
          sin(degreesToRadians(angleChannel_[2]) / 2), 0.0f, 0.0f);

      rotationQuaternion_ =
          zAxisRotation * yAxisRotation * xAxisRotation * rotationQuaternion_;
      rotationQuaternion_.normalize();
    }

    Offset getOffset() const { return offset_; }

    void setOffset(const Offset &newOffset) { offset_ = newOffset; }

    Eigen::Quaternion<float> getRotationQuaternion() const {
      return rotationQuaternion_;
    }

    void setRotationQuaternion(const Eigen::Quaternion<float> &newQuaternion) {
      rotationQuaternion_ = newQuaternion;
      rotationQuaternion_.normalize();
    }

    void addChildNode(const Node &newNode) {
      Node *newChildNode = new Node(newNode);
      childNodes_.push_back(newChildNode);
    }

    std::vector<Node *> getChildNodes() const { return childNodes_; }

  private:
    std::string nodeLabel_, nodeName_;

    Channel translationChannel_; // note: only used in root node
    Channel angleChannel_; // todo: rm? With rotationQuaternion_, this isn't
                           // needed...
    Offset offset_;

    Eigen::Quaternion<float> rotationQuaternion_;

    std::vector<Node *> childNodes_;

    friend class SkeletonTree;
  };

  SkeletonTree() {}

  SkeletonTree(const Node &rootNode) { rootNode_ = rootNode; }

  // CC
  SkeletonTree(const SkeletonTree &otherTree) {
    rootNode_ = otherTree.rootNode_;
  }

  // AO
  SkeletonTree &operator=(const SkeletonTree &otherTree) {
    rootNode_ = otherTree.rootNode_;
    return *this;
  }

  Node *getRootNode() { return (Node *)&rootNode_; }

  Node *addNode(const Node &newNode, Node *parentNode) {
    // look for the node which matches the pointer, add the child using
    // breadth-first search, add new child node to found node
    std::vector<Node *> nextNodes, nextNextNodes;
    nextNodes.push_back((Node *)&rootNode_);
    while (nextNodes.size()) {
      nextNextNodes.clear();

      for (Node *nextNode : nextNodes) {
        if (nextNode == parentNode) {
          nextNode->addChildNode(newNode);
          return nextNode->childNodes_[nextNode->childNodes_.size() - 1];
        }

        for (Node *nextNextNode : nextNode->childNodes_) {
          nextNextNodes.push_back(nextNextNode);
        }
      }

      nextNodes = nextNextNodes;
    }

    throw std::runtime_error("Parent node not found");
  }

  void enumerateDepthFirst(std::function<void(Node *nextNode)> callback) const {
    std::vector<Node *> nextNodes;
    nextNodes.push_back((Node *)&rootNode_);
    while (nextNodes.size()) {
      Node *nextNode = nextNodes.back();
      nextNodes.pop_back();

      callback(nextNode);

      std::vector<Node *> childNodes = nextNode->getChildNodes();
      for (auto reverseIterator = childNodes.rbegin();
           reverseIterator != childNodes.rend(); ++reverseIterator) {
        nextNodes.push_back(*reverseIterator);
      }
    }
  }

  // todo: update; assign quaternions to nodes, instead of euler angles
  void updateChannels(const MotionFrameCollection::Frame &motionFrame) {
    unsigned nextChannelIndex = 0;
    bool isRoot = true;
    enumerateDepthFirst(
        [&motionFrame, &nextChannelIndex, &isRoot](Node *nextNode) {
          // skip endsites
          if (nextNode->getLabel() == "End")
            return;

          if (isRoot) {
            //// set translation channel
            Node::Channel translationChannel;
            for (int i = 0; i < 3; ++i) {
              translationChannel[i] = motionFrame[3 * nextChannelIndex + i];
            }

            ++nextChannelIndex;

            nextNode->setTranslationChannel(translationChannel);

            //// set rotation channel
            Node::Channel rotationChannel;
            for (int i = 0; i < 3; ++i) {
              rotationChannel[i] = motionFrame[3 * nextChannelIndex + i];
            }

            ++nextChannelIndex;

            nextNode->setAngleChannel(rotationChannel);

            isRoot = false;
          } else {
            //// set rotation channel
            Node::Channel rotationChannel;
            for (int i = 0; i < 3; ++i) {
              rotationChannel[i] = motionFrame[3 * nextChannelIndex + i];
            }

            ++nextChannelIndex;

            nextNode->setAngleChannel(rotationChannel);
          }
        });
  }

  // interpolate between two frames, using the provided parameter
  void updateChannels(const MotionFrameCollection::Frame &motionFrame1,
                      const MotionFrameCollection::Frame &motionFrame2,
                      double interpolationParameter) {
    unsigned nextChannelIndex = 0;
    bool isRoot = true;
    enumerateDepthFirst([&motionFrame1, &motionFrame2, &nextChannelIndex,
                         &interpolationParameter, &isRoot](Node *nextNode) {
      // skip endsites
      if (nextNode->getLabel() == "End")
        return;

      if (isRoot) {
        //// set translation channel
        Node::Channel translationChannel;
        for (int i = 0; i < 3; ++i) {
          translationChannel[i] = SkeletonTree::getLinearlyInterpolatedValue(
              motionFrame1[3 * nextChannelIndex + i],
              motionFrame2[3 * nextChannelIndex + i], interpolationParameter);
        }

        ++nextChannelIndex;

        nextNode->setTranslationChannel(translationChannel);

        //// set rotation quaternion
        Node::Channel frame1RotationChannel, frame2RotationChannel;
        for (int i = 0; i < 3; ++i) {
          frame1RotationChannel[i] = motionFrame1[3 * nextChannelIndex + i];
          frame2RotationChannel[i] = motionFrame2[3 * nextChannelIndex + i];
        }

        //// compute interpolated quaternion
        Quaternion frame1Quaternion(frame1RotationChannel);
        Quaternion frame2Quaternion(frame2RotationChannel);

        Eigen::Quaternion<float> interpolatedQuaternion =
            frame1Quaternion.getEigenQuaternion().slerp(
                interpolationParameter, frame2Quaternion.getEigenQuaternion());

        nextNode->setRotationQuaternion(interpolatedQuaternion);

        ++nextChannelIndex;

        isRoot = false;
      } else {
        //// set rotation quaternion
        Node::Channel frame1RotationChannel, frame2RotationChannel;
        for (int i = 0; i < 3; ++i) {
          frame1RotationChannel[i] = motionFrame1[3 * nextChannelIndex + i];
          frame2RotationChannel[i] = motionFrame2[3 * nextChannelIndex + i];
        }

        //// compute interpolated quaternion
        Quaternion frame1Quaternion(frame1RotationChannel);
        Quaternion frame2Quaternion(frame2RotationChannel);

        Eigen::Quaternion<float> interpolatedQuaternion =
            frame1Quaternion.getEigenQuaternion().slerp(
                interpolationParameter, frame2Quaternion.getEigenQuaternion());

        nextNode->setRotationQuaternion(interpolatedQuaternion);

        ++nextChannelIndex;
      }
    });
  }

  void writeToFileStream(std::ofstream &outputFileStream) const {
    if (outputFileStream.is_open()) {
      writeNodeToFile((Node *)&rootNode_, outputFileStream);
    } else {
      throw std::runtime_error("Failed to open skeleton output file");
    }
  }

  // returns the dimension boundary values which contain the skeleton tree in
  // the neutral position, in the format: [min_x, max_x, min_y, max_y, min_z,
  // max_z]
  std::array<float, 6> getSkeletonTreeDimensionBounds() {
    std::array<float, 6> skeletonTreeDimensionBounds;
    skeletonTreeDimensionBounds[0] = FLT_MAX; // initial min_x
    skeletonTreeDimensionBounds[1] = 0.0f;    // initial max_x
    skeletonTreeDimensionBounds[2] = FLT_MAX; // initial min_y
    skeletonTreeDimensionBounds[3] = 0.0f;    // initial max_y
    skeletonTreeDimensionBounds[4] = FLT_MAX; // initial min_z
    skeletonTreeDimensionBounds[5] = 0.0f;    // initial max_z

    std::vector<float> accumulatedOffset(3, 0);

    calculateSkeletonTreeDimesionBounds(
        (Node *)&rootNode_, skeletonTreeDimensionBounds, accumulatedOffset);

    return skeletonTreeDimensionBounds;
  }

private:
  Node rootNode_;

  // recursive function to write nodes
  void writeNodeToFile(Node *node, std::ofstream &outputFileStream,
                       bool isRoot = true, int tabDepth = 0) const {
    // pad output by tab depth
    std::stringstream tabOffsetStream;
    for (int i = 0; i < tabDepth; ++i)
      tabOffsetStream << '\t';

    // write node name
    outputFileStream << tabOffsetStream.str() << node->getLabel() << " "
                     << node->getName() << std::endl;

    outputFileStream << tabOffsetStream.str() << "{" << std::endl;

    std::stringstream indentedTabOffsetStream;
    for (int i = 0; i < tabDepth + 1; ++i)
      indentedTabOffsetStream << '\t';

    // write node offset
    Node::Offset offset = node->getOffset();
    outputFileStream << indentedTabOffsetStream.str() << "OFFSET " << offset[0]
                     << " " << offset[1] << " " << offset[2] << std::endl;

    // write node channels if the node is not an endsite
    if (node->getChildNodes().size()) {
      if (isRoot) {
        outputFileStream
            << indentedTabOffsetStream.str()
            << "CHANNELS 6 Xposition Yposition Zposition Zrotation "
               "Yrotation Xrotation "
            << std::endl;
      } else {
        outputFileStream << indentedTabOffsetStream.str()
                         << "CHANNELS 3 Zrotation Yrotation Xrotation "
                         << std::endl;
      }
    }

    // write node children
    for (Node *nextChildNode : node->getChildNodes()) {
      writeNodeToFile(nextChildNode, outputFileStream, false, tabDepth + 1);
    }

    outputFileStream << tabOffsetStream.str() << "}" << std::endl;
  }

  // recursive method to compute the maximum dimension bounds of the skeleton
  // tree, in the neutral position
  void calculateSkeletonTreeDimesionBounds(
      Node *node, std::array<float, 6> &dimensionBounds,
      std::vector<float> accumulatedOffset) const {
    Node::Offset nodeOffset = node->getOffset();

    std::vector<float> newAccumulatedOffset = accumulatedOffset;
    newAccumulatedOffset[0] += nodeOffset[0];
    newAccumulatedOffset[1] += nodeOffset[1];
    newAccumulatedOffset[2] += nodeOffset[2];

    // compare accumulated offset with dimensions
    dimensionBounds[0] = newAccumulatedOffset[0] < dimensionBounds[0]
                             ? newAccumulatedOffset[0]
                             : dimensionBounds[0];
    dimensionBounds[1] = newAccumulatedOffset[0] > dimensionBounds[1]
                             ? newAccumulatedOffset[0]
                             : dimensionBounds[1];
    dimensionBounds[2] = newAccumulatedOffset[1] < dimensionBounds[2]
                             ? newAccumulatedOffset[1]
                             : dimensionBounds[2];
    dimensionBounds[3] = newAccumulatedOffset[1] > dimensionBounds[3]
                             ? newAccumulatedOffset[1]
                             : dimensionBounds[3];
    dimensionBounds[4] = newAccumulatedOffset[2] < dimensionBounds[4]
                             ? newAccumulatedOffset[2]
                             : dimensionBounds[4];
    dimensionBounds[5] = newAccumulatedOffset[2] > dimensionBounds[5]
                             ? newAccumulatedOffset[2]
                             : dimensionBounds[5];

    for (Node *nextChildNode : node->getChildNodes()) {
      calculateSkeletonTreeDimesionBounds(nextChildNode, dimensionBounds,
                                          newAccumulatedOffset);
    }
  }

  static double getLinearlyInterpolatedValue(double value1, double value2,
                                             double interpolationParameter) {
    return (1 - interpolationParameter) * value1 +
           interpolationParameter * value2;
  }
};


///////////////////////////////////////////////////////////////////////////////
// count.hpp                                                                 //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_EMPIRICAL_DISTRIBUTION_COUNT_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_EMPIRICAL_DISTRIBUTION_COUNT_HPP_ER_2010
#include <map>
#include <functional> // less

#include <boost/type_traits.hpp>

#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>
#include <boost/mpl/bool.hpp>

#include <boost/range.hpp>
#include <boost/numeric/conversion/converter.hpp>

#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/parameters/accumulator.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>

#include <boost/statistics/detail/non_parametric/empirical_distribution/ordered_sample.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace empirical_distribution{
namespace impl{

    // T can be an integer or a float
    template<typename T>
	class count : public boost::accumulators::accumulator_base{
        typedef std::size_t size_;
        typedef boost::accumulators::dont_care dont_care_;

        public:

        typedef size_ result_type;
        typedef T sample_type;

        count(dont_care_){}

        void operator()(dont_care_){}

        template<typename Args>
        result_type result(const Args& args)const{
            namespace ns = empirical_distribution;
            typedef ns::tag::ordered_sample tag_;
            return boost::accumulators::extract_result<tag_>(
                args[ boost::accumulators::accumulator ]
            )[
                args[ boost::accumulators::sample ]
            ];
        }

    };

}// impl
namespace tag
{
    struct count: boost::accumulators::depends_on<
      	boost::statistics::detail
        	::empirical_distribution::tag::ordered_sample
    >
    {
        struct impl{
            template<typename T,typename W>
            struct apply{
                typedef detail::empirical_distribution::impl::count<T> type;
            };
        };
    };
}// tag
namespace result_of{

    template<typename AccSet,typename T>
    struct count{
    	typedef detail::empirical_distribution::tag::count tag_;
        typedef typename
            boost::accumulators::detail::template
            	extractor_result<AccSet,tag_>::type type;
    };

}// result_of
namespace extract
{

    template<typename AccSet,typename T>
    typename detail::empirical_distribution
        ::result_of::template count<AccSet,T>::type
  	count(AccSet const& acc,const T& x)
    {
        namespace ed = detail::empirical_distribution;
    	typedef ed::tag::count tag_;
        return boost::accumulators::extract_result<tag_>(
            acc,
            (boost::accumulators::sample = x)
        );
  	}

}// extract
}// empirical_distribution
}// detail
}// statistics
}// boost

#endif


/// ---------------------------------------------------------------------------
/// @section LICENSE
///
/// Copyright (c) 2016 Georgia Tech Research Institute (GTRI)
///               All Rights Reserved
///
/// 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.
/// ---------------------------------------------------------------------------
/// @file filename.ext
/// @author Kevin DeMarco <kevin.demarco@gtri.gatech.edu>
/// @author Eric Squires <eric.squires@gtri.gatech.edu>
/// @version 1.0
/// ---------------------------------------------------------------------------
/// @brief A brief description.
///
/// @section DESCRIPTION
/// A long description.
/// ---------------------------------------------------------------------------
#include <scrimmage/plugin_manager/RegisterPlugin.h>
#include "RigidBody6DOFControllerPID.h"
#include <boost/algorithm/string.hpp>
#include <iostream>

REGISTER_PLUGIN(scrimmage::Controller,
                RigidBody6DOFControllerPID,
                RigidBody6DOFControllerPID_plugin)

namespace sc = scrimmage;

void set_pid(sc::PID &pid, std::string str, bool is_angle) {
    std::vector<std::string> str_vals;
    boost::split(str_vals, str, boost::is_any_of(","));

    if (str_vals.size() != 4) {
        std::cout << "error parsing in RigidBody6DOFControllerPID" << std::endl;
    } else {
        double p = std::stod(str_vals[0]);
        double i = std::stod(str_vals[1]);
        double d = std::stod(str_vals[2]);
        pid.set_parameters(p, i, d);

        if (is_angle) {
            double i_lim = sc::Angles::deg2rad(std::stod(str_vals[3]));
            pid.set_integral_band(i_lim);
            pid.set_is_angle(true);
        } else {
            double i_lim = std::stod(str_vals[3]);
            pid.set_integral_band(i_lim);
        }
    }
}

void RigidBody6DOFControllerPID::init(std::map<std::string, std::string> &params) {
    set_pid(heading_pid_, params["heading_pid"], true);
    set_pid(alt_pid_, params["alt_pid"], false);
    set_pid(vel_pid_, params["vel_pid"], false);
    u_ = std::make_shared<Eigen::Vector4d>();
}

bool RigidBody6DOFControllerPID::step(double t, double dt) {
    double desired_yaw = desired_state_->quat().yaw();

    heading_pid_.set_setpoint(desired_yaw);
    double u_heading = heading_pid_.step(dt, state_->quat().yaw());
    double roll_error = u_heading + state_->quat().roll();

    alt_pid_.set_setpoint(desired_state_->pos()(2));
    double u_alt = alt_pid_.step(dt, state_->pos()(2));
    double pitch_error = (-u_alt - state_->quat().pitch());

    vel_pid_.set_setpoint(desired_state_->vel()(0));
    double u_thrust = vel_pid_.step(dt, state_->vel().norm());

    (*u_) << u_thrust, roll_error, pitch_error, 0;
    return true;
}