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

using namespace arma;
using namespace std;

namespace ic {

class SOM {
public:
    // Constructores
    SOM(const mat& patrones, pair<int, int> dimensiones, const vec& salidaDeseada = {});

    // Interfaz
    void entrenar(int nEpocas,
                  double velocidadInicial,
                  double velocidadFinal,
                  int vecindadInicial,
                  int vecindadFinal);
    void etiquetar();
    int clasificar(const rowvec& patron) const;
    vec clasificar(const mat& patrones) const;
    void graficar(Gnuplot& gp, bool graficarVecindades = true) const;

    // Acceso a miembros
    const field<rowvec>& mapa() const { return m_mapa; };
    const mat& etiquetas() const { return m_etiquetas; };
    const mat& patrones() const { return m_patrones; };

private:
    field<rowvec> m_mapa;
    mat m_etiquetas;
    const mat m_patrones;
    const vec m_salidaDeseada;

    pair<pair<int, int>, double> buscarGanadora(const rowvec& patron) const;
};

SOM::SOM(const mat& patrones, pair<int, int> dimensiones, const vec& salidaDeseada)
    : m_patrones{patrones}
    , m_salidaDeseada{salidaDeseada}
{
    m_mapa = field<rowvec>(dimensiones.first, dimensiones.second);

    // Inicializar mapa
    for (unsigned int i = 0; i < m_mapa.n_rows; ++i) {
        for (unsigned int j = 0; j < m_mapa.n_cols; ++j) {
            m_mapa(i, j) = randu<rowvec>(patrones.n_cols) - 0.5;
        }
    }
}

pair<pair<int, int>, double> SOM::buscarGanadora(const rowvec& patron) const
{
    pair<int, int> coordGanadora;
    double distanciaGanadora = numeric_limits<double>::max();

    for (unsigned int j = 0; j < m_mapa.n_rows; ++j) {
        for (unsigned int k = 0; k < m_mapa.n_cols; ++k) {
            const double distancia = norm(patron - m_mapa(j, k));

            if (distancia < distanciaGanadora) {
                distanciaGanadora = distancia;
                coordGanadora = {j, k};
            }
        }
    }

    return {coordGanadora, distanciaGanadora};
}

void SOM::entrenar(int nEpocas,
                   double velocidadInicial,
                   double velocidadFinal,
                   int vecindadInicial,
                   int vecindadFinal)
{
    const vec velocidad = linspace(velocidadInicial, velocidadFinal, nEpocas);
    // Redondeamos la vecindad porque necesitamos que tenga valores enteros
    const vec vecindad = round(linspace(vecindadInicial, vecindadFinal, nEpocas));

    for (int epoca = 0; epoca < nEpocas; ++epoca) {
        for (unsigned int n = 0; n < m_patrones.n_rows; ++n) {

            // Buscamos la neurona ganadora
            pair<int, int> coordGanadora;
            double distanciaGanadora;
            tie(coordGanadora, distanciaGanadora) = buscarGanadora(m_patrones.row(n));

            // Adaptación de pesos
            const int maxX = int(m_mapa.n_rows - 1);
            const int maxY = int(m_mapa.n_cols - 1);
            const int xInicial = (coordGanadora.first - vecindad(epoca) < 0) ? 0 : (coordGanadora.first - vecindad(epoca));
            const int xFinal = (coordGanadora.first + vecindad(epoca) > maxX) ? maxX : (coordGanadora.first + vecindad(epoca));
            const int yInicial = (coordGanadora.second - vecindad(epoca) < 0) ? 0 : (coordGanadora.second - vecindad(epoca));
            const int yFinal = (coordGanadora.second + vecindad(epoca) > maxY) ? maxY : (coordGanadora.second + vecindad(epoca));

            for (int x = xInicial; x <= xFinal; ++x) {
                for (int y = yInicial; y <= yFinal; ++y) {
                    m_mapa(x, y) += velocidad(epoca) * (m_patrones.row(n) - m_mapa(x, y));
                }
            }
        }
    }
}

void SOM::etiquetar()
{
    if (m_salidaDeseada.empty())
        throw runtime_error("Este SOM no posee una salida deseada asociada a los patrones");

    field<ivec> mapaContador(m_mapa.n_rows, m_mapa.n_cols);

    // Inicializar los contadores de clases
    for (unsigned int x = 0; x < m_mapa.n_rows; ++x) {
        for (unsigned int y = 0; y < m_mapa.n_cols; ++y) {
            mapaContador(x, y) = zeros<ivec>(2);
        }
    }

    // Contamos, para cada neurona, qué cantidad de veces gana para cada clase
    for (unsigned int n = 0; n < m_patrones.n_rows; ++n) {
        pair<int, int> ganadora;
        tie(ganadora, ignore) = buscarGanadora(m_patrones.row(n));

        if (m_salidaDeseada(n) == 0)
            mapaContador(ganadora.first, ganadora.second).at(0) += 1;
        else
            mapaContador(ganadora.first, ganadora.second).at(1) += 1;
    }

    m_etiquetas = mat(m_mapa.n_rows, m_mapa.n_cols);

    // Se asignan las etiquetas de clase
    for (unsigned int x = 0; x < m_mapa.n_rows; ++x) {
        for (unsigned int y = 0; y < m_mapa.n_cols; ++y) {
            if (mapaContador(x, y)(0) == mapaContador(x, y)(1))
                // Si la neurona ganó la misma cantidad de veces para ambas clases,
                // se le asigna la clase al azar.
                m_etiquetas(x, y) = as_scalar(randi(1, distr_param(0, 1)));
            else
                m_etiquetas(x, y) = index_max(mapaContador(x, y));
        }
    }
}

int SOM::clasificar(const rowvec& patron) const
{
    pair<int, int> ganadora;
    tie(ganadora, ignore) = buscarGanadora(patron);

    return m_etiquetas(ganadora.first, ganadora.second);
}

vec SOM::clasificar(const mat& patrones) const
{
    vec result = zeros(patrones.n_rows);

    for (unsigned int n = 0; n < patrones.n_rows; ++n) {
        result(n) = clasificar(rowvec{patrones.row(n)});
    }

    return result;
}

void SOM::graficar(Gnuplot& gp, bool graficarVecindades) const
{
    gp << "set key box opaque width 3" << endl
       << "set xlabel 'x_1' font ',11'" << endl
       << "set ylabel 'x_2' font ',11'" << endl

       // Graficar patrones
       << "plot " << gp.file1d(m_patrones) << "title 'Patrones' with points pt 2 ps 1 lt rgb 'blue', ";

    // Graficar neuronas del mapa y las conexiones
    for (unsigned int x = 0; x < m_mapa.n_rows; ++x) {
        for (unsigned int y = 0; y < m_mapa.n_cols; ++y) {
            // Graficar la neurona
            gp << gp.file1d(m_mapa(x, y).eval()) << "notitle with points ps 2 pt 1 lt -1 lw 3, ";

            if (graficarVecindades) {
                // Graficar conexiones con las vecinas horizontales y verticales
                if (x != 0)
                    gp << gp.file1d(join_vert(m_mapa(x, y), m_mapa(x - 1, y)).eval()) << "notitle with lines lt -1, ";
                if (x != m_mapa.n_rows - 1)
                    gp << gp.file1d(join_vert(m_mapa(x, y), m_mapa(x + 1, y)).eval()) << "notitle with lines lt -1, ";
                if (y != 0)
                    gp << gp.file1d(join_vert(m_mapa(x, y), m_mapa(x, y - 1)).eval()) << "notitle with lines lt -1, ";
                if (y != m_mapa.n_cols - 1)
                    gp << gp.file1d(join_vert(m_mapa(x, y), m_mapa(x, y + 1)).eval()) << "notitle with lines lt -1, ";
            }
        }
    }

    // Título de los centroides para la leyenda
    gp << "NaN title 'Neuronas' with points ps 2 pt 1 lt -1 lw 3" << endl;
}
}


//
// Copyright 2017 Will Mitchell
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "stats/Confidence.h"

#include <boost/math/distributions/students_t.hpp>
namespace bm = boost::math;

double stats::confidence_bound(double sample_size, double variance,
                               double confidence) {
  // Can't have a confidence with fewer than 2 samples
  if (sample_size < 2)
    return 0;

  bm::students_t dist(sample_size - 1);
  double T = bm::quantile(bm::complement(dist, (1 - confidence) / 2));
  return T * sqrt(variance) / sqrt(sample_size);
}


#include <Eigen/Core>
#include <Eigen/LU>
#include <elasty/alembic-manager.hpp>
#include <elasty/fem.hpp>
#include <mathtoolbox/l-bfgs.hpp>
#include <timer.hpp>
#include <vector>

namespace
{
    constexpr std::size_t k_num_dims = 2;

    constexpr double k_youngs_modulus = 200.0;
    constexpr double k_poisson_ratio  = 0.45;

    constexpr double k_first_lame  = elasty::fem::calcFirstLame(k_youngs_modulus, k_poisson_ratio);
    constexpr double k_second_lame = elasty::fem::calcSecondLame(k_youngs_modulus, k_poisson_ratio);

    constexpr unsigned k_num_substeps = 5;
    constexpr double   k_delta_time   = 1.0 / 60.0;

    constexpr double k_damping_factor = 0.0;

    constexpr double k_spring_stiffness = 10000.0;

    enum class Model
    {
        CoRotational,
        StVenantKirchhoff
    };

    constexpr Model k_model = Model::CoRotational;
} // namespace

template <int N> struct Mesh
{
    using ElemList = Eigen::Matrix<std::int32_t, N + 1, Eigen::Dynamic>;

    ElemList elems;

    Eigen::VectorXd x_rest;
    Eigen::VectorXd x;
    Eigen::VectorXd v;
    Eigen::VectorXd f;

    double mass = 1.0;

    /// \details Should be precomputed
    Eigen::VectorXd lumped_mass;

    /// \details Should be precomputed
    std::vector<double> volume_array;

    /// \details Should be precomputed
    std::vector<Eigen::Matrix<double, N, N>> rest_shape_mat_inv_array;

    /// \details Should be precomputed
    std::vector<Eigen::Matrix<double, N * N, N*(N + 1)>> vec_PFPx_array;
};

using TriangleMesh = Mesh<2>;

struct Constraint
{
    std::size_t                              vert_index;
    std::function<Eigen::Vector2d(double t)> motion;
    double                                   stiffness;
};

template <typename Derived> typename Derived::Scalar calcEnergyDensity(const Eigen::MatrixBase<Derived>& deform_grad)
{
    switch (k_model)
    {
        case Model::StVenantKirchhoff:
            return elasty::fem::calcStVenantKirchhoffEnergyDensity(deform_grad, k_first_lame, k_second_lame);
        case Model::CoRotational:
            return elasty::fem::calcCoRotationalEnergyDensity(deform_grad, k_first_lame, k_second_lame);
    }
}

template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, 2, 2> calcPiolaStress(const Eigen::MatrixBase<Derived>& deform_grad)
{
    switch (k_model)
    {
        case Model::StVenantKirchhoff:
            return elasty::fem::calcStVenantKirchhoffPiolaStress(deform_grad, k_first_lame, k_second_lame);
        case Model::CoRotational:
            return elasty::fem::calcCoRotationalPiolaStress(deform_grad, k_first_lame, k_second_lame);
    }
}

class VariationalImplicit2dEngine
{
public:
    VariationalImplicit2dEngine() {}

    void proceedFrame()
    {
        const std::size_t num_verts = m_mesh.x_rest.size() / k_num_dims;

        // Reset forces
        m_mesh.f = Eigen::VectorXd::Zero(2 * num_verts);

        // Apply gravity force
        for (std::size_t i = 0; i < num_verts; ++i)
        {
            m_mesh.f[i * 2 + 1] += m_mesh.lumped_mass(i * 2 + 1) * (-9.80665);
        }

        // Calculate the inverse lumped mass matrix
        const auto W = m_mesh.lumped_mass.cwiseInverse().asDiagonal();

        // Calculate the "inertia" position
        const double&         h = m_delta_physics_time;
        const Eigen::VectorXd y = m_mesh.x + h * m_mesh.v + h * h * W * m_mesh.f;

        const auto calcInternalPotential = [&](const Eigen::VectorXd& x)
        {
            double sum = 0.0;

            // Elastic potential
            for (std::size_t i = 0; i < m_mesh.elems.cols(); ++i)
            {
                const auto& indices = m_mesh.elems.col(i);

                // Retrieve precomputed values
                const auto& D_m_inv = m_mesh.rest_shape_mat_inv_array[i];
                const auto& area    = m_mesh.volume_array[i];

                // Calculate the deformation gradient $\mathbf{F}$
                const auto F = elasty::fem::calc2dTriangleDeformGrad(
                    x.segment<2>(2 * indices[0]), x.segment<2>(2 * indices[1]), x.segment<2>(2 * indices[2]), D_m_inv);

                sum += area * calcEnergyDensity(F);
            }

            // Attach-spring potential
            for (const auto& constraint : m_constraints)
            {
                const std::size_t vert_index   = constraint.vert_index;
                const double&     k            = constraint.stiffness;
                const auto        p            = x.segment<2>(vert_index * 2);
                const auto        q            = constraint.motion(m_physics_time + m_delta_physics_time);
                const double      squared_dist = (p - q).squaredNorm();

                sum += 0.5 * k * squared_dist;
            }

            return sum;
        };

        const auto calcInternalPotentialGrad = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd
        {
            Eigen::VectorXd sum = Eigen::VectorXd::Zero(x.size());
            for (std::size_t i = 0; i < m_mesh.elems.cols(); ++i)
            {
                const auto& indices = m_mesh.elems.col(i);

                // Retrieve precomputed values
                const auto& D_m_inv  = m_mesh.rest_shape_mat_inv_array[i];
                const auto& area     = m_mesh.volume_array[i];
                const auto& vec_PFPx = m_mesh.vec_PFPx_array[i];

                // Calculate the deformation gradient $\mathbf{F}$
                const auto F = elasty::fem::calc2dTriangleDeformGrad(
                    x.segment<2>(2 * indices[0]), x.segment<2>(2 * indices[1]), x.segment<2>(2 * indices[2]), D_m_inv);

                // Calculate $\frac{\partial \Phi}{\partial \mathbf{x}}$ and related values
                const auto P      = calcPiolaStress(F);
                const auto vec_P  = Eigen::Map<const Eigen::Vector4d>(P.data(), P.size());
                const auto PPsiPx = vec_PFPx.transpose() * vec_P;

                // Calculate $\frac{\partial E}{\partial \mathbf{x}}$
                const auto PEPx = area * PPsiPx;

                sum.segment<2>(2 * indices[0]) += PEPx.segment<2>(0 * 2);
                sum.segment<2>(2 * indices[1]) += PEPx.segment<2>(1 * 2);
                sum.segment<2>(2 * indices[2]) += PEPx.segment<2>(2 * 2);
            }

            for (const auto& constraint : m_constraints)
            {
                const std::size_t vert_index = constraint.vert_index;
                const double&     k          = constraint.stiffness;
                const auto        p          = x.segment<2>(vert_index * 2);
                const auto        q          = constraint.motion(m_physics_time + m_delta_physics_time);
                const auto        r          = p - q;

                sum.segment<2>(2 * vert_index) += k * r;
            }

            return sum;
        };

        const auto calcMomentumPotential = [&](const Eigen::VectorXd& x)
        {
            return (0.5 / (h * h)) * (x - y).transpose() * m_mesh.lumped_mass.asDiagonal() * (x - y);
        };

        const auto calcMomentumPotentialGrad = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd
        {
            return (1.0 / (h * h)) * m_mesh.lumped_mass.asDiagonal() * (x - y);
        };

        const auto calcObjective = [&](const Eigen::VectorXd& x)
        {
            const double momentum_potential = calcMomentumPotential(x);
            const double internal_potential = calcInternalPotential(x);

            assert(momentum_potential >= 0.0);
            assert(internal_potential >= 0.0);

            return momentum_potential + internal_potential;
        };

        const auto calcObjectiveGrad = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd
        {
            return calcMomentumPotentialGrad(x) + calcInternalPotentialGrad(x);
        };

        // Solve the minimization problem
        unsigned        num_iters;
        Eigen::VectorXd x_opt;
        mathtoolbox::optimization::RunLBfgs(y, calcObjective, calcObjectiveGrad, 1e-06, 100, x_opt, num_iters);

        // Update the internal state
        m_mesh.v = (1.0 / h) * (x_opt - m_mesh.x);
        m_mesh.x = x_opt;

        // Apply naive damping
        m_mesh.v *= std::exp(-k_damping_factor * m_delta_physics_time);

        // Update time counter
        m_physics_time += m_delta_physics_time;
    }

    void initializeScene()
    {
        constexpr std::size_t num_cols  = 20;
        constexpr std::size_t num_rows  = 5;
        constexpr std::size_t num_verts = (num_cols + 1) * (num_rows + 1);
        constexpr std::size_t num_elems = (num_cols * num_rows) * 2;
        constexpr double      size      = 1.0;

        m_mesh.elems.resize(3, num_elems);
        m_mesh.x_rest.resize(num_verts * k_num_dims);

        // Generate a triangle mesh
        for (std::size_t col = 0; col < num_cols; ++col)
        {
            for (std::size_t row = 0; row < num_rows; ++row)
            {
                const auto base = col * (num_rows + 1) + row;

                m_mesh.elems.col(2 * num_rows * col + 2 * row + 0) << 0 + base, 1 + base, (num_rows + 1) + 1 + base;
                m_mesh.elems.col(2 * num_rows * col + 2 * row + 1) << 0 + base, (num_rows + 1) + 1 + base,
                    (num_rows + 1) + base;

                m_mesh.x_rest.segment(k_num_dims * ((num_rows + 1) * col + row), k_num_dims) =
                    Eigen::Vector2d{col * 1.0, -1.0 * row};
            }
            m_mesh.x_rest.segment(k_num_dims * ((num_rows + 1) * col + num_rows), k_num_dims) =
                Eigen::Vector2d{col * 1.0, -1.0 * num_rows};
        }
        for (std::size_t row = 0; row < num_rows; ++row)
        {
            m_mesh.x_rest.segment(k_num_dims * ((num_rows + 1) * num_cols + row), k_num_dims) =
                Eigen::Vector2d{num_cols * 1.0, -1.0 * row};
        }
        m_mesh.x_rest.segment(k_num_dims * ((num_rows + 1) * num_cols + num_rows), k_num_dims) =
            Eigen::Vector2d{num_cols * 1.0, -1.0 * num_rows};

        // Set transform
        m_mesh.x_rest *= 1.0 / static_cast<double>(num_rows);
        for (std::size_t vert = 0; vert < num_verts; ++vert)
        {
            m_mesh.x_rest[2 * vert + 1] += 0.5;
        }
        m_mesh.x_rest *= size;

        // Initialize other values
        m_mesh.x = m_mesh.x_rest;
        m_mesh.v = Eigen::VectorXd::Zero(k_num_dims * num_verts);
        m_mesh.f = Eigen::VectorXd::Zero(k_num_dims * num_verts);

        // Set constraints
        for (std::size_t i = 0; i < num_rows + 1; ++i)
        {
            const auto motion = [&, i](double) -> Eigen::Vector2d
            {
                return m_mesh.x_rest.segment<2>(i * 2);
            };

            m_constraints.push_back(Constraint{i, motion, k_spring_stiffness});
        }
        for (std::size_t i = (num_rows + 1) * num_cols; i < (num_rows + 1) * (num_cols + 1); ++i)
        {
            const auto ease = [](double x)
            {
                return -(std::cos(3.14159265358979 * x) - 1.0) * 0.5;
            };

            const auto motion = [&, i](double t) -> Eigen::Vector2d
            {
                const auto   x_init = m_mesh.x_rest.segment<2>(i * 2);
                const auto   dir    = Eigen::Vector2d{3.0, 0.0};
                const double t_0    = 0.8;
                const double t_1    = t_0 + 1.0;
                const double a      = (t < t_0) ? 0.0 : ((t < t_1) ? t - t_0 : t_1 - t_0);
                const double b      = ease(a);

                return x_init + b * dir;
            };

            m_constraints.push_back(Constraint{i, motion, k_spring_stiffness});
        }

        // Perform precomputation
        m_mesh.volume_array.resize(num_elems);
        m_mesh.rest_shape_mat_inv_array.resize(num_elems);
        m_mesh.vec_PFPx_array.resize(num_elems);
        for (std::size_t elem_index = 0; elem_index < num_elems; ++elem_index)
        {
            const auto& indices = m_mesh.elems.col(elem_index);

            m_mesh.volume_array[elem_index] = elasty::fem::calc2dTriangleArea(m_mesh.x_rest.segment<2>(2 * indices[0]),
                                                                              m_mesh.x_rest.segment<2>(2 * indices[1]),
                                                                              m_mesh.x_rest.segment<2>(2 * indices[2]));
            m_mesh.rest_shape_mat_inv_array[elem_index] =
                elasty::fem::calc2dShapeMatrix(m_mesh.x_rest.segment<2>(2 * indices[0]),
                                               m_mesh.x_rest.segment<2>(2 * indices[1]),
                                               m_mesh.x_rest.segment<2>(2 * indices[2]))
                    .inverse();
            m_mesh.vec_PFPx_array[elem_index] =
                elasty::fem::calcVecTrianglePartDeformGradPartPos(m_mesh.rest_shape_mat_inv_array[elem_index]);
        }
        m_mesh.lumped_mass = elasty::fem::calcTriangleMeshLumpedMass(m_mesh.x_rest, m_mesh.elems, m_mesh.mass);
    }

    /// \brief Getter of the delta physics time.
    ///
    /// \details The value equals to the delta frame time devided by the number of substeps.
    double getDeltaPhysicsTime() const { return m_delta_physics_time; }

    void setDeltaPhysicsTime(const double delta_physics_time) { m_delta_physics_time = delta_physics_time; }

    const TriangleMesh* getMesh() const { return &m_mesh; }

private:
    double m_delta_physics_time = 1.0 / 60.0;
    double m_physics_time       = 0.0;

    std::vector<Constraint> m_constraints;

    TriangleMesh m_mesh;
};

int main(int argc, char** argv)
{
    VariationalImplicit2dEngine engine;

    engine.initializeScene();
    engine.setDeltaPhysicsTime(k_delta_time / static_cast<double>(k_num_substeps));

    const auto        mesh      = engine.getMesh();
    const std::size_t num_verts = mesh->x_rest.size() / 2;
    const std::size_t num_elems = mesh->elems.cols();

    auto alembic_manager = elasty::createTriangleMesh2dAlembicManager(
        "./out.abc", k_delta_time, num_verts, num_elems, mesh->x.data(), mesh->elems.data());

    for (unsigned int frame = 0; frame < 240; ++frame)
    {
        timer::Timer t(std::to_string(frame));

        alembic_manager->submitCurrentStatus();

        for (int i = 0; i < k_num_substeps; ++i)
        {
            engine.proceedFrame();
        }
    }

    return 0;
}


#include <algorithm>
#include <boost/lambda/lambda.hpp>
#include <graphic/sampler.h>

namespace graphic {

void Sampler::add_sample(math::scalar time, math::matrix<4,4> const &m)
{
	sample s;
	s.time = time;
	s.transform = m;
	s.translation.set(m.ij[3][0], m.ij[3][1], m.ij[3][2]);
	s.rotation.set_unit(m);

	if (samples_.size()) {
		samples_.back().rotation_interpolator.setup(samples_.back().rotation, s.rotation);
	}

	samples_.push_back(s);
}

math::matrix<4,4> Sampler::get(math::scalar time) const {
	if (time <= samples_.front().time) return samples_.front().transform;
	if (time >= samples_.back().time) return samples_.back().transform;

	using namespace boost::lambda;
	samples_type::const_iterator it = std::lower_bound(samples_.begin(), samples_.end(), sample(time), &_1 ->* &sample::time < &_2 ->* &sample::time);

	math::scalar k = (time - (it - 1)->time) / (it->time - (it - 1)->time);
	math::scalar inv_k = 1.0f - k;

	math::matrix<4,4> m;
	m.rotation((it - 1)->rotation_interpolator.interpolate(k));
	m.translate((it - 1)->translation * inv_k + it->translation * k);
	return m;
}

}


//=======================================================================
// Copyright 2012 David Doria
// Authors: David Doria
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <iostream>
#include <boost/array.hpp>
#include <boost/graph/grid_graph.hpp>

int main(int argc, char* argv[])
{
  // A 2D grid graph
  typedef boost::grid_graph<2> GraphType;

  // Create a 5x5 graph
  const unsigned int dimension = 5;
  boost::array<std::size_t, 2> lengths = { { dimension, dimension } };
  GraphType graph(lengths);

  // Get the index map of the grid graph
  typedef boost::property_map<GraphType, boost::vertex_index_t>::const_type indexMapType;
  indexMapType indexMap(get(boost::vertex_index, graph));

  // Create a float for every node in the graph
  boost::vector_property_map<float, indexMapType> dataMap(num_vertices(graph), indexMap);

  // Associate the value 2.0 with the node at position (0,1) in the grid
  boost::graph_traits<GraphType>::vertex_descriptor v = { { 0, 1 } };
  put(dataMap, v, 2.0f);

  // Get the data at the node at position (0,1) in the grid
  float retrieved = get(dataMap, v);
  std::cout << "Retrieved value: " << retrieved << std::endl;

  return 0;
}


#ifndef _INTERPOLATOR_HPP_
#define _INTERPOLATOR_HPP_

#include <iostream>
#include <iomanip>
#include <complex>
#include <cmath>
#include <Eigen/Dense>
#include <eigen3/Eigen/Dense>

namespace eco_tracker {

	void getInterpFourier(const int& filter_width,
						  const int& filter_height,
						  Eigen::MatrixXcf& interp1_fs,
						  Eigen::MatrixXcf& interp2_fs,
						  float a);

    Eigen::MatrixXcf CubicSplineFourier(Eigen::MatrixXcf f, float a);
} // namespace eco_tracker

#endif


/*
 *  polynomial_two.hpp
 *
 *
 *  Created by Andrea Bedini on 24/Nov/2011.
 *  Copyright (c) 2011-2014, Andrea Bedini <andrea.bedini@gmail.com>.
 *
 *  Distributed under the terms of the Modified BSD License.
 *  The full license is in the file COPYING, distributed as part of
 *  this software.
 *
 */

#ifndef POLYNOMIAL_TWO_HPP
#define POLYNOMIAL_TWO_HPP

#include <boost/operators.hpp>
#include <iosfwd>

template<class T>
class polynomial_two
  : boost::ring_operators1< polynomial_two<T>
  , boost::ring_operators2< polynomial_two<T>, T
  , boost::equality_comparable< polynomial_two<T>
  > > >
{
public:
  typedef unsigned short int index;
  struct element {
    index i, j;
    T c;
    bool operator==(element const& rhs) const
    { return i == rhs.i and j == rhs.j and c == rhs.c; }
  };

private:
  typedef std::vector<element> elements_type;
  elements_type elements_;

public:
  typedef typename elements_type::iterator iterator;
  typedef typename elements_type::const_iterator const_iterator;

private:
  void cleanup() {
    iterator i = elements_.begin();
    while (i != elements_.end()) {
      if (i->c == T(0))
        i = elements_.erase(i);
      else
        ++i;
    }
  }

  struct indices_less {
    bool operator()(element const& a, element const& b) const
    {
      return a.i < b.i or (a.i == b.i and a.j < b.j);
    }
  };

  struct indices_equal {
    bool operator()(element const& a, element const& b) const
    {
      return a.i == b.i and a.j == b.j;
    }
  };

  T& coeff(index i, index j)
  {
    const element e{i, j, T(0)};
    auto it = std::lower_bound(elements_.begin(), elements_.end(), e, indices_less());
    if (it == elements_.end() or not indices_equal()(*it, e)) {
      it = elements_.insert(it, e);
    }
    return it->c;
  }

  polynomial_two(elements_type const& e) : elements_(e)
  { }

public:
  // default constructor
  explicit polynomial_two(T const& a = T(0))
    : elements_{{0, 0, a}}
  {
  }

  polynomial_two(std::initializer_list<element> list)
    : elements_(list)
  {
  }

  // copy constructor
  polynomial_two(polynomial_two<T> const& rhs)
    : elements_(rhs.elements_)
  {
  }

  // move constructor
  polynomial_two(polynomial_two<T>&& rhs)
    : elements_(std::move(rhs.elements_))
  {
  }

  // assignemnt
  polynomial_two<T>& operator=(polynomial_two<T> const& rhs)
  {
    elements_ = rhs.elements_;
    return *this;
  }

  polynomial_two<T>& operator=(polynomial_two<T>&& rhs)
  {
    elements_ = std::move(rhs.elements_);
    return *this;
  }

  // conversions
  template<typename T2>
  friend class polynomial_two;

  template<class T2>
  explicit polynomial_two(T2 const& a)
    : elements_{{0, 0, T(a)}}
  {
  }

  template<class T2>
  explicit polynomial_two(polynomial_two<T2> const& rhs)
  {
    for (auto const& e : rhs.elements_) {
      coeff(e.i, e.j) = T(e.c);
    }
  }
  template<class T2>
  polynomial_two<T>& operator=(T2 const& rhs)
  {
    elements_ = {{0, 0, T(rhs)}};
    return *this;
  }

  template<class T2>
  polynomial_two<T>& operator=(polynomial_two<T2> const& rhs)
  {
    elements_.clear();
    for (auto const& e : rhs.elements_) {
      coeff(e.i, e.j) = T(e.c);
    }
    return *this;
  }

  // static constructors

  static polynomial_two<T> Q()
  {
    return {{1, 0, T(1)}};
  }

  static polynomial_two<T> v()
  {
    return {{0, 1, T(1)}};
  }

  // swap
  void swap(polynomial_two<T>& other) throw ()
  {
    elements_.swap(other.elements_);
  }

  // iterators
  iterator begin() { return elements_.begin(); }
  iterator end()   { return elements_.end(); }

  const_iterator begin() const { return elements_.begin(); }
  const_iterator end()   const { return elements_.end(); }

   // comparison
  bool operator==(polynomial_two<T> const& rhs) const
  {
    return std::equal(begin(), end(), rhs.begin());
  }

  // ring operators with T
  polynomial_two<T>& operator+=(T const& rhs)
  {
    coeff(0, 0) += rhs;
    return *this;
  }

  polynomial_two<T>& operator-=(T const& rhs)
  {
    coeff(0, 0) -= rhs;
    return *this;
  }

  polynomial_two<T>& operator*=(T const& rhs)
  {
    iterator it;
    for (auto& e : elements_)
      e.c *= rhs;
    return *this;
  }

  // ring operators with polynomial_two<T>
  polynomial_two<T>& operator+=(polynomial_two<T> const& rhs)
  {
    for (auto const& e : rhs.elements_)
      coeff(e.i, e.j) += e.c;
    cleanup();
    return *this;
  }

  polynomial_two<T>& operator-=(polynomial_two<T> const& rhs)
  {
    for (auto const& e : rhs.elements_)
      coeff(e.i, e.j) -= e.c;
    cleanup();
    return *this;
  }

  polynomial_two<T>& operator*=(polynomial_two<T> const& rhs)
  {
    polynomial_two<T> result;
    for (auto const& e1 : rhs.elements_)
      for (auto const& e2 : elements_)
        result.coeff(e1.i + e2.i, e1.j + e2.j) += e1.c * e2.c;
    swap(result);
    return *this;
  }

  // unary

  const polynomial_two<T> operator-() const
  {
    polynomial_two<T> result;
    for (auto const& e : elements_)
      result.coeff(e.i, e.j) = -e.c;
    return result;
  }

  // member functions

  const polynomial_two<T> times_Q() const
  {
    polynomial_two<T> result;
    for (auto const& e : elements_)
      result.coeff(e.i + 1, e.j) = e.c;
    return result;
  }

  const polynomial_two<T> times_v() const
  {
    polynomial_two<T> result;
    for (auto const& e : elements_)
      result.coeff(e.i, e.j + 1) = e.c;
    return result;
  }

  friend
  std::ostream& operator<<(std::ostream& o, polynomial_two<T> const& p)
  {
    auto it = p.elements_.begin();
    while (it != p.elements_.end()) {
      T c = it->c;
      if (c < 0) {
        o << "- ";
        c = -c;
      } else {
        o << "+ ";
      }
      if (c != 1 or (it->i == 0 and it->j == 0))
        o << c << " ";
      if (it->i == 1)
        o << "Q ";
      if (it->i > 1)
        o << "Q^" << it->i << " ";
      if (it->j == 1)
        o << "v ";
      if (it->j > 1)
        o << "v^" << it->j << " ";
      ++ it;
    }
    return o;
  }
};

template<class T>
void swap(polynomial_two<T>& p1, polynomial_two<T>& p2) throw ()
{
  p1.swap(p2);
}

#endif // POLYNOMIAL_TWO_HPP


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <smooth/derivatives.hpp>
#include <smooth/feedback/ocp.hpp>
#include <smooth/feedback/utils/sparse.hpp>
#include <smooth/se2.hpp>

template<typename T>
using X = smooth::SE2<T>;

template<typename T>
using U = Eigen::Vector<T, 2>;

template<typename T>
using Q = Eigen::Vector<T, 1>;

template<typename T, std::size_t N>
using Vec = Eigen::Vector<T, N>;

static constexpr auto Nx     = smooth::Dof<X<double>>;
static constexpr auto Nq     = smooth::Dof<Q<double>>;
static constexpr auto Nu     = smooth::Dof<U<double>>;
static constexpr auto Ninner = 1 + Nx + smooth::Dof<U<double>>;
static constexpr auto Nouter = 1 + 2 * Nx + smooth::Dof<Q<double>>;

static constexpr auto t_B_inner = 0;
static constexpr auto x_B_inner = t_B_inner + 1;
static constexpr auto u_B_inner = x_B_inner + Nx;

static constexpr auto tf_B_outer = 0;
static constexpr auto x0_B_outer = tf_B_outer + 1;
static constexpr auto xf_B_outer = x0_B_outer + Nx;
static constexpr auto q_B_outer  = xf_B_outer + Nx;

/// @brief Objective function
struct TestOcpObjective
{
  template<typename T>
  T operator()(T, const X<T> &, const X<T> &, const Vec<T, 1> & q) const
  {
    return q.x();
  }

  Eigen::SparseMatrix<double>
  jacobian(double, const X<double> &, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(1, Nouter);
    ret.coeffRef(0, q_B_outer) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double>
  hessian(double, const X<double> &, const X<double> &, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(Nouter, Nouter);
    return ret;
  }
};

struct TestOcpDyn
{
  template<typename T>
  smooth::Tangent<X<T>> operator()(T, const X<T> & x, const U<T> & u) const
  {
    return {
      u.x() - 0.1 * x.r2().x(),
      0,
      u.y(),
    };
  }

  Eigen::SparseMatrix<double> jacobian(double, const X<double> & x, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(Nx, Ninner);
    ret.coeffRef(0, x_B_inner)     = -0.1 * std::cos(x.so2().angle());  // df1 / dx
    ret.coeffRef(0, x_B_inner + 1) = 0.1 * std::sin(x.so2().angle());   // df1 / dy
    ret.coeffRef(0, u_B_inner)     = 1;
    ret.coeffRef(2, u_B_inner + 1) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double> hessian(double, const X<double> & x, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(Ninner, Nx * Ninner);
    ret.coeffRef(x_B_inner + 0, 0 * Nx + x_B_inner + 2) =
      0.1 * std::sin(x.so2().angle());  // d2f1 / dx dth
    ret.coeffRef(x_B_inner + 1, 0 * Nx + x_B_inner + 2) =
      0.1 * std::cos(x.so2().angle());  // d2f1 / dy dth
    return ret;
  }
};

struct TestOcpIntegrand
{
  template<typename T>
  Vec<T, 1> operator()(T, const X<T> & x, const U<T> & u) const
  {
    return 0.5 * Vec<T, 1>{(x - X<T>::Identity()).squaredNorm() + u.squaredNorm()};
  }

  Eigen::SparseMatrix<double> jacobian(double, const X<double> & x, const U<double> & u) const
  {
    const auto a = x - X<double>::Identity();
    Eigen::SparseMatrix<double> ret(1, Ninner);
    smooth::feedback::block_add(ret, 0, x_B_inner, smooth::dr_rminus_squarednorm<X<double>>(a));
    ret.coeffRef(0, u_B_inner)     = u.x();
    ret.coeffRef(0, u_B_inner + 1) = u.y();
    return ret;
  }

  Eigen::SparseMatrix<double> hessian(double, const X<double> & x, const U<double> &) const
  {
    const auto H = smooth::d2r_rminus_squarednorm<X<double>>(x.log());

    Eigen::SparseMatrix<double> ret(Ninner, 1 * Ninner);
    smooth::feedback::block_add(ret, x_B_inner, x_B_inner, H);
    ret.coeffRef(u_B_inner, u_B_inner)         = 1;
    ret.coeffRef(u_B_inner + 1, u_B_inner + 1) = 1;
    return ret;
  }
};

struct TestOcpCr
{
  template<typename T>
  Vec<T, 1> operator()(T, const X<T> &, const U<T> & u) const
  {
    return Vec<T, 1>{u.x()};
  }

  Eigen::SparseMatrix<double> jacobian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(1, Ninner);
    ret.coeffRef(0, u_B_inner) = 1;
    return ret;
  }

  Eigen::SparseMatrix<double> hessian(double, const X<double> &, const U<double> &) const
  {
    Eigen::SparseMatrix<double> ret(Ninner, 1 * Ninner);
    return ret;
  }
};

struct TestOcpCe
{
  static constexpr auto Nce = 1 + 2 * Nx;

  template<typename T>
  Vec<T, Nce> operator()(T tf, const X<T> & x0, const X<T> & xf, const Vec<T, 1> &) const
  {
    Vec<T, Nce> ret;
    ret << tf, x0.log(), xf.log();
    return ret;
  }

  Eigen::SparseMatrix<double>
  jacobian(double, const X<double> & x0, const X<double> & xf, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(Nce, Nouter);
    ret.coeffRef(tf_B_outer, tf_B_outer) = 1;
    smooth::feedback::block_add(
      ret, x0_B_outer, x0_B_outer, smooth::dr_expinv<X<double>>(x0.log()));
    smooth::feedback::block_add(
      ret, xf_B_outer, xf_B_outer, smooth::dr_expinv<X<double>>(xf.log()));
    return ret;
  }

  Eigen::SparseMatrix<double>
  hessian(double, const X<double> & x0, const X<double> & xf, const Vec<double, 1> &) const
  {
    Eigen::SparseMatrix<double> ret(Nouter, Nce * Nouter);

    const auto d2_logx0 = smooth::d2r_rminus<X<double>>(x0.log());
    const auto d2_logxf = smooth::d2r_rminus<X<double>>(xf.log());

    for (auto i = 0u; i < Nx; ++i) {
      smooth::feedback::block_add(
        ret, x0_B_outer, Nouter * (1 + i) + x0_B_outer, d2_logx0.block(0, i * Nx, Nx, Nx));

      smooth::feedback::block_add(
        ret, xf_B_outer, Nouter * (1 + Nx + i) + xf_B_outer, d2_logxf.block(0, i * Nx, Nx, Nx));
    }

    return ret;
  }
};

using OcpTest = smooth::feedback::
  OCP<X<double>, U<double>, TestOcpObjective, TestOcpDyn, TestOcpIntegrand, TestOcpCr, TestOcpCe>;

inline const OcpTest ocp_test{
  .theta = TestOcpObjective{},
  .f     = TestOcpDyn{},
  .g     = TestOcpIntegrand{},
  .cr    = TestOcpCr{},
  .crl   = Vec<double, 1>{{-1}},
  .cru   = Vec<double, 1>{{1}},
  .ce    = TestOcpCe{},
  .cel   = Vec<double, 1 + 2 * Nx>::Random(),
  .ceu   = Vec<double, 1 + 2 * Nx>::Random(),
};


/*
  Test program for MatPlot

  Dag Lindbo, dag@csc.kth.se
*/

#include "matplot.h"

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

namespace ublas = boost::numeric::ublas;
typedef ublas::vector<double> Vector;
typedef ublas::matrix<double> Matrix;

using namespace std;
using namespace matplot;

// parameters for 2D plot
const int NN = 40;
const double t_low = 0;
const double t_upp = 5;
const double dt = (t_upp-t_low)/(NN-1);

// parameters for 3D stuff
const int Nx = 200;
const int Ny = 250;
const double x_low = -2.5;
const double x_upp = 1.5;
const double y_low = -2.5;
const double y_upp = 2.5;
const double dx = (x_upp-x_low)/(Nx-1);
const double dy = (y_upp-y_low)/(Ny-1);

#define G(x,y) exp(-(x*x+y*y))*sin(x)*cos(y);

// parameters for quiver plot
const int NNx = 20;
const int NNy = 37;
const double xx_low = 0;
const double xx_upp = 1;
const double yy_low = 0;
const double yy_upp = 1.5;
const double dxx = (xx_upp-xx_low)/(NNx-1);
const double dyy = (yy_upp-yy_low)/(NNy-1);

int main(void)
{
  int i, j;

  // EXAMPLE: Plot_2D =======================================
  Vector x1(NN);
  Vector y1(NN);
  Vector x2(NN);
  Vector y2(NN);

  double t = t_low;

  for (i=0; i<NN; i++)
    {
      x1(i) = cos(t)*t;
      y1(i) = sin(t);
      x2(i) = sin(t)*t+1;
      y2(i) = cos(t)*(0.1-t);
      t+=dt;
    }

  Plot2D_VTK p_2d("x(t)", "y(t)", 800, 600);

  double color1[3] =
    { 0.0, 0.0, 1.0 };
  double color2[3] =
    { 1.0, 0.0, 0.0 };

  p_2d.plot(x1, y1, color1, "-");
  p_2d.plot(x2, y2, color2, ".-");
  p_2d.show();
  //p_2d.draw_to_png("plot_2d.png");

  // EXAMPLE: Contour =======================================

  Vector x(Nx);
  Vector y(Ny);
  Matrix z(Nx, Ny);

  for (i=0; i<Nx; i++)
    x(i) = x_low+i*dx;

  for (i=0; i<Ny; i++)
    y(i) = y_low + i*dy;

  for (i=0; i<Nx; i++)
    for (j=0; j<Ny; j++)
      z(i, j) = G(x(i),y(j));

  Contour_VTK p_cont(800, 600);
  p_cont.contour(x, y, z, true, 20);
  //p_cont.contour_to_file(x,y,z,true,20,"contours.png");

  // EXAMPLE: Surf ==========================================

  double cam[3] =
    { -15.0, 10.0, 12.0 };
  double focal[3] =
    { 0, 0, 0 };
  Surf_VTK p_surf(800, 600);
  //p_surf.surf(x,y,z);
  //p_surf.surf(x,y,z,false);
  p_surf.surf(x, y, z, true, cam, focal);
  //p_surf.surf_to_file(x,y,z,true,"surf.png",cam,focal);

  // Example: Quiver ========================================
  Vector xx(NNx);
  Vector yy(NNy);
  Matrix u(NNx, NNy);
  Matrix v(NNx, NNy);

  for (i=0; i<NNx; i++)
    xx(i) = xx_low+i*dxx;

  for (i=0; i<NNy; i++)
    yy(i) = yy_low + i*dyy;

  for (i=0; i<NNx; i++)
    for (j=0; j<NNy; j++)
      {
   	u(i, j) = -yy(j);
   	v(i, j) = xx(i);
      }

  Quiver_VTK p_quiver(800, 600);
  p_quiver.quiver(xx, yy, u, v, 0.1);
  //p_quiver.quiver_to_file(xx,yy,u,v,0.1,"quiver.png");

  return 0;
}


// Geometric Modeling
// Final Project
// 2D Harmonic Coordinates
// Author: Weiqiang Li
// wl1731@nyu.edu

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/triangle/triangulate.h>
#include <igl/project.h>
#include <igl/unproject.h>
#include <igl/readOFF.h>
#include <igl/slice.h>
#include <igl/cotmatrix.h>
#include <igl/boundary_facets.h>
#include <igl/unique.h>
#include <string>
#include <fstream>
#include <sstream>
#include <unordered_map>

// this project is aimed for harmonic coordinates for mesh deformation
// the supposed usage is any of the following:
// ./final_bin
// which will run the algorithm based on default mesh and cage
// ./final_bin <mesh.off>
// which will read the mesh and automatically build a cage based on the boundary of the mesh
// ./final_bin <mesh.off> <cage.cage>
// which will read the mesh and cage from file
// where mesh.off is the mesh information (V and F)
// in mesh.off, the z coordinates are assumed 0 (not used)
// cage.cage is the cage information (V)
// in cage.cage, the z coordinates are assumed 0 (not used)
// the vertices in cage.cage should be either ordered clockwise or counterclockwise
// interaction manual:
// use mouse (click and drag) to change the location of cage vertex
// the new mesh will be calculated automatically
// press R to reset the cage and mesh to original position
// press U to undo the cage vertex change (only once)
// use W/A/S/D to control the cage vertex location after selected by mouse
// the new mesh will be cauculated automatically

using namespace std;
using namespace Eigen;
using Viewer = igl::opengl::glfw::Viewer;

// debug flags
// #define DEBUG_1
// #define DEBUG_2
// #define DEBUG_3

// mesh
MatrixXd V;
MatrixXi F;
// mesh archive
MatrixXd V1;
MatrixXd V2;

// cage
MatrixXd CV;
MatrixXi CE;
// cage archive
MatrixXd CV1;
MatrixXd CV2;

// triangulate helper
MatrixXd VV0;
MatrixXi VF0;

// harmonic helper
SparseMatrix<double> Aff, Afc;
MatrixXd H;

// interactions helper
bool mouse_down = false;
int current_cage_vertex = -1;
double keyboard_stride = 0.0;

// declarations
void plot_mesh_and_cage(igl::opengl::glfw::Viewer &viewer);
bool callback_key_down(Viewer &viewer, unsigned char key, int modifiers);
int find_nearest_cage(RowVector3d loc);
bool callback_mouse_down(Viewer& viewer, int, int);
bool callback_mouse_move(Viewer& viewer, int mouse_x, int mouse_y);
bool callback_mouse_up(Viewer& viewer, int, int);
void solve_prepare();
void solve_harmonic();
bool read_cage_from_file(string cage_filename);
void generate_cage();

// plots mesh (in V and F) and cage (in CV and CE)
// during mouse click and drag, the selected cage vertex will be marked green
void plot_mesh_and_cage(igl::opengl::glfw::Viewer &viewer) {
  viewer.data().clear();
  viewer.data().set_mesh(V, F);

  if (mouse_down) {
    for (unsigned i = 0; i < CV.rows(); i++) {
      if (i == current_cage_vertex) {
        viewer.data().add_points(CV.row(i), RowVector3d(0,1,0));
      } else {
        viewer.data().add_points(CV.row(i), RowVector3d(1,0,0));
      }
    }
  } else {
    viewer.data().add_points(CV, RowVector3d(1,0,0));
  }
  for (unsigned i = 0; i < CE.rows(); i++) {
    viewer.data().add_edges(
      CV.row(CE(i,0)),
      CV.row(CE(i,1)),
      RowVector3d(1,0,0)
    );
  }
}

// key_down callback
// accepts R (reset), U (undo), W/A/S/D (cage vertex move)
bool callback_key_down(Viewer &viewer, unsigned char key, int modifiers) {
  if (key == 'R') {
    CV = CV1;
    V = V1;
    plot_mesh_and_cage(viewer);
    return true;
  }
  else if (key == 'U') {
    CV = CV2;
    V = V2;
    plot_mesh_and_cage(viewer);
    return true;
  }
  else if (key == 'W' || key == 'S' || key == 'A' || key == 'D') {
    if (current_cage_vertex != -1) {
      if (key == 'W') {
        CV(current_cage_vertex, 1) += keyboard_stride;
      }
      else if (key == 'S') {
        CV(current_cage_vertex, 1) -= keyboard_stride;
      }
      else if (key == 'A') {
        CV(current_cage_vertex, 0) -= keyboard_stride;
      }
      else {
        CV(current_cage_vertex, 0) += keyboard_stride;
      }
      CV2 = CV;
      V2 = V;
      solve_harmonic();
      #ifdef DEBUG_3
      cout << V << endl << endl;
      #endif
      plot_mesh_and_cage(viewer);
    }
    return true;
  }
  return false;
}

// find the nearest cage vertex based on the unprojected mouse location
// the location should be close to at least one of the cage vertices
// otherwise will return -1
int find_nearest_cage(RowVector3d loc) {
  int nearest = -1;
  double min_distance = numeric_limits<double>::max();
  for (unsigned i = 0; i < CV.rows(); i++) {
    RowVector3d diff = loc - CV.row(i);
    if (diff.norm() < min_distance) {
      min_distance = diff.norm();
      nearest = i;
    }
  }
  RowVector3d cage_max = CV.colwise().maxCoeff();
  RowVector3d cage_min = CV.colwise().minCoeff();
  #ifdef DEBUG_1
  cout << cage_max << endl;
  cout << cage_min << endl;
  #endif
  double distance_threshold = (cage_max(0) - cage_min(0) + cage_max(1) - cage_min(1)) / 2 * 0.1;
  keyboard_stride = distance_threshold / 2;
  if (min_distance > distance_threshold) {
    nearest = -1;
  }
  return nearest;
}

// mouse_down callback
// unproject the mouse location and find the a cage vertex nearby
// and then prepare for the drag
bool callback_mouse_down(Viewer& viewer, int, int) {
  int mx = viewer.current_mouse_x;
  int my = viewer.core.viewport(3) - viewer.current_mouse_y;
  RowVector3d origin_point;
  origin_point.setZero();
  RowVector3d projected;
  igl::project(origin_point, viewer.core.view, viewer.core.proj, viewer.core.viewport, projected);
  #ifdef DEBUG_3
  cout << "projected: " << projected << endl;
  #endif
  double mz = projected[2];
  RowVector3d target;
  target.setZero();
  igl::unproject(RowVector3d(mx,my,mz), viewer.core.view, viewer.core.proj, viewer.core.viewport, target);
  int cid = find_nearest_cage(target);
  #ifdef DEBUG_3
  cout << "cage selected: " << cid << endl;
  cout << "mouse location: " << mx << ", " << my << "," << mz << endl;
  cout << "target location: " << target << endl;
  #endif
  if (cid == -1) {
    return false;
  }
  current_cage_vertex = cid;
  mouse_down = true;
  CV2 = CV;
  V2 = V;
  plot_mesh_and_cage(viewer);
  return true;
}

// mouse_move callback
// works only when mouse is down (when dragging)
// unproject the mouse location and move the selected cage vertex accordingly
// and then solve the system
bool callback_mouse_move(Viewer& viewer, int mouse_x, int mouse_y) {
  if (mouse_down) {
    int mx = mouse_x;
    int my = viewer.core.viewport(3) - mouse_y;
    RowVector3d origin_point;
    origin_point.setZero();
    RowVector3d projected;
    igl::project(origin_point, viewer.core.view, viewer.core.proj, viewer.core.viewport, projected);
    #ifdef DEBUG_3
    cout << "projected: " << projected << endl;
    #endif
    double mz = projected[2];
    RowVector3d target;
    target.setZero();
    igl::unproject(RowVector3d(mx,my,mz), viewer.core.view, viewer.core.proj, viewer.core.viewport, target);
    #ifdef DEBUG_3
    cout << "mouse location: " << mx << ", " << my << "," << mz << endl;
    cout << "target location: " << target << endl;
    #endif
    CV.row(current_cage_vertex) = target;
    solve_harmonic();
    plot_mesh_and_cage(viewer);
  }
  return true;
}

// mouse_up callback
bool callback_mouse_up(Viewer& viewer, int, int) {
  mouse_down = false;
  plot_mesh_and_cage(viewer);
  return true;
}

// solver prepare
// use Laplace equation to solve the system
// stores the harmonic matrix in H
// consider mesh and cage as a whole new mesh
// and use cage vertices as boundary vertices
// then utilize variable elimination to build the linear system
void solve_prepare() {
  MatrixXd VV(CV.rows()+V.rows(), 2);
  for (unsigned i = 0; i < CV.rows(); i++) {
    VV(i,0) = CV(i,0);
    VV(i,1) = CV(i,1);
  }
  for (unsigned i = 0; i < V.rows(); i++) {
    VV(CV.rows()+i,0) = V(i,0);
    VV(CV.rows()+i,1) = V(i,1);
  }
  MatrixXd H0(0, 2);
  igl::triangle::triangulate(VV,CE,H0,"Q",VV0,VF0);
  VV0.conservativeResize(VV0.rows(),3);
  VV0.col(2).setZero();
  #ifdef DEBUG_1
  cout << "VV = " << endl << VV << endl;
  cout << "VV0 = " << endl << VV0 << endl;
  #endif
  SparseMatrix<double> L;
  igl::cotmatrix(VV0,VF0,L);
  VectorXi all, in, b;
  igl::colon<int>(0,VV0.rows()-1,all);
  igl::colon<int>(CV.rows(),VV0.rows()-1,in);
  igl::colon<int>(0,CV.rows()-1,b);
  SparseMatrix<double> A = L * (-1);
  // SparseMatrix<double> Aff, Afc;
  igl::slice(A,in,in,Aff);
  igl::slice(A,in,b,Afc);

  SimplicialLLT<SparseMatrix<double>> solver;
  solver.compute(Aff);
  H = solver.solve(MatrixXd(Afc)*(-1));

  #ifdef DEBUG_2
  cout << "VV0: " << VV0.rows() << " * " << VV0.cols() << endl;
  cout << "VF0: " << VF0.rows() << " * " << VF0.cols() << endl;
  cout << "L: " << L.rows() << " * " << L.cols() << endl;
  cout << "A: " << A.rows() << " * " << A.cols() << endl;
  cout << "all: " << all.rows() << " * " << all.cols() << endl;
  cout << "in: " << in.rows() << " * " << in.cols() << endl;
  cout << "b: " << b.rows() << " * " << b.cols() << endl;
  cout << "Aff: " << Aff.rows() << " * " << Aff.cols() << endl;
  cout << "Afc: " << Afc.rows() << " * " << Afc.cols() << endl;
  cout << "H: " << H.rows() << " * " << H.cols() << endl;
  #endif
}

// compute the new mesh based on H
void solve_harmonic() {
  /*
  for (unsigned i = 0; i < V.cols(); i++) {
    VectorXd bc = CV.col(i);
    SimplicialLLT<SparseMatrix<double>> solver(Aff);
    VectorXd XX = solver.solve(MatrixXd(Afc) * (-1) * bc);
    V.col(i) = XX;
  }
  */

  MatrixXd NV = H * CV;
  for (unsigned i = 0; i < NV.rows(); i++) {
    V.row(i) = NV.row(i);
  }

}

// read cage information from .cage file
// if failed, the main function will use automatic cage generation instead
bool read_cage_from_file(string cage_filename) {
  ifstream in(cage_filename);
  if (!in.is_open()) {
    cout << "Error: cannot open the cage file! Automatic cage generation will be used." << endl;
    return false;
  }
  CV.resize(8, 3);
  int index = 0;
  string line;
  while (getline(in, line)) {
    index++;
    istringstream iss(line);
    if (index > CV.rows()) {
      CV.conservativeResize(CV.rows()*2, CV.cols());
    }
    for (unsigned i = 0; i < 3; i++) {
      double xyz;
      if (iss >> xyz) {
        CV(index-1,i) = xyz;
      } else {
        cout << "Error: cannot open the cage file! Automatic cage generation will be used." << endl;
        return false;
      }
    }
  }
  CV.conservativeResize(index, CV.cols());
  in.close();
  return true;
}

// automatically generate cage based on mesh boundary
// compute the mesh centroid and move the boundary vertices even further to build the cage
void generate_cage() {
  int vn = V.rows();
  RowVector3d mesh_centriod;
  mesh_centriod.setZero();
  for (unsigned i = 0; i < vn; i++) {
    mesh_centriod += V.row(i);
  }
  mesh_centriod /= vn;
  MatrixXi VE;
  igl::boundary_facets(F,VE);
  #ifdef DEBUG_2
  cout << "VE: " << endl << VE << endl << endl;
  #endif
  VectorXi CC, IA, IC;
  igl::unique(VE,CC,IA,IC);
  #ifdef DEBUG_2
  cout << "CC: " << endl << CC << endl << endl;
  cout << "IA: " << endl << IA << endl << endl;
  cout << "IC: " << endl << IC << endl << endl;
  #endif
  MatrixXd VI;
  igl::slice(V,CC,1,VI);
  CV.resizeLike(VI);
  for (unsigned i = 0; i < VI.rows(); i++) {
    CV.row(i) = (VI.row(i) - mesh_centriod) * 1.5 + mesh_centriod;
  }
  unordered_map<int,int> dict;
  for (unsigned i = 0; i < CC.rows(); i++) {
    dict[CC(i)] = i;
  }
  CE.resizeLike(VE);
  for (unsigned i = 0; i < VE.rows(); i++) {
    CE(i,0) = dict[VE(i,0)];
    CE(i,1) = dict[VE(i,1)];
  }
}

// main function
int main(int argc, char *argv[])
{
  if (argc <= 1) {

    V.resize(4,3);
    V << 0,0,0,
         1,0,0,
         0,1,0,
         1,1,0;
    F.resize(2,3);
    F << 0,1,2,
         1,3,2;
    CV.resize(4,3);
    CV << -0.5,-0.5,0,
          1.5,-0.5,0,
          1.5,1.5,0,
          -0.5,1.5,0;
    CE.resize(4,2);
    CE << 0,1,
          1,2,
          2,3,
          3,0;

  }
  else if (argc == 2) {

    string mesh_file_name = argv[1];
    igl::readOFF(mesh_file_name,V,F);
    generate_cage();

  }
  else if (argc >= 3) {

    string mesh_file_name = argv[1];
    string mesh_cage_name = argv[2];

    igl::readOFF(mesh_file_name,V,F);
    if (!read_cage_from_file(mesh_cage_name)) {
      generate_cage();
    } else {
      CE.resize(CV.rows(),2);
      for (unsigned i = 0; i < CV.rows(); i++) {
        CE(i,0) = i;
        CE(i,1) = (i+1)%CV.rows();
      }
    }

  }
  else {
    cout << "Usage: ./final_bin <mesh.off> <cage.cage>" << endl;
    cout << "Automatic cage generation: " << endl;
    cout << "Usage: ./final_bin <mesh.off>" << endl;
    cout << "Default mesh and cage: " << endl;
    cout << "Usage: ./final_bin" << endl;
    exit(1);
  }

  CV1 = CV2 = CV;
  V1 = V2 = V;

  // Plot the mesh
  igl::opengl::glfw::Viewer viewer;

  viewer.callback_key_down = &callback_key_down;
  viewer.callback_mouse_down = &callback_mouse_down;
  viewer.callback_mouse_move = &callback_mouse_move;
  viewer.callback_mouse_up = &callback_mouse_up;

  solve_prepare();

  plot_mesh_and_cage(viewer);
  viewer.core.align_camera_center(V,F);

  viewer.launch();

}


#include "stdafx.h"
#include <catch.hpp>
#include <boost/spirit/home/x3.hpp>
#include <boost/fusion/adapted/std_pair.hpp>

#include <iostream>
#include <map>

using namespace boost::spirit::x3;

namespace
{

struct hundreds_ : symbols<unsigned>
{
	hundreds_()
	{
		// DSEL w/ operator ()
		add
			("C", 100)
			("CC", 200)
			("CCC", 300)
			("CD", 400)
			("D", 500)
			("DC", 600)
			("DCC", 700)
			("DCCC", 800)
			("CM", 900)
			;
	}

} hundreds;

struct tens_ : symbols<unsigned>
{
	tens_()
	{
		add
		("X", 10)
			("XX", 20)
			("XXX", 30)
			("XL", 40)
			("L", 50)
			("LX", 60)
			("LXX", 70)
			("LXXX", 80)
			("XC", 90)
			;
	}

} tens;

struct ones_ : symbols<unsigned>
{
	ones_()
	{
		add
		("I", 1)
			("II", 2)
			("III", 3)
			("IV", 4)
			("V", 5)
			("VI", 6)
			("VII", 7)
			("VIII", 8)
			("IX", 9)
			;
	}

} ones;

}

namespace roman_parser
{

auto set_zero = [&](auto& ctx) { _val(ctx) = 0; };
auto add1000 = [&](auto& ctx) { _val(ctx) += 1000; };
auto add = [&](auto& ctx) { _val(ctx) += _attr(ctx); };

rule<class roman, unsigned> const roman = "roman";

auto const roman_def =
eps[set_zero]		// eps is epsilon (empty) parser
>>
(
	-(+lit('M')[add1000])
	>> -hundreds[add]
	>> -tens[add]
	>> -ones[add]
	)
	;

BOOST_SPIRIT_DEFINE(roman);
} // parser

TEST_CASE("a roman parser")
{
	SECTION("parse")
	{
		std::string input = "XI";

		unsigned result = 0;

		auto first = input.begin();
		auto last = input.end();

		bool rc = phrase_parse(first, last, roman_parser::roman, space, result);

		CHECK(rc);
		CHECK(result == 11);
	}
}


#include <boost/test/unit_test.hpp>
#include "utils/normalizations.h"
#include "inputs/minmax.h"
#include "inputs/zmaxnorm.h"
#include "inputs/znorm.h"
#include "inputs/column.h"
#include "utils/algorithms.hpp"
#include <boost/range/algorithm/sort.hpp>

BOOST_AUTO_TEST_CASE(test_znorm)
{
    const std::vector<double> test_data(std::begin(data_mapstd), std::end(data_mapstd));
    auto after_norm{utils::z_normalization(test_data)};
    BOOST_TEST_REQUIRE(after_norm.size() == test_data.size());
    BOOST_TEST_REQUIRE(after_norm.size() == utils::size(expected_first_mapstd));
    std::vector<double> expected(std::begin(expected_first_mapstd), std::end(expected_first_mapstd));
    boost::sort(after_norm);
    boost::sort(expected);
    auto ex_begin = std::begin(expected);
    for (auto n_begin = std::begin(after_norm); n_begin != std::end(after_norm); ++n_begin, ++ex_begin) {
        BOOST_TEST(round_cmp(*n_begin, *ex_begin, 0.0001), "value after the norm "<<*n_begin<<" != expected "<<*ex_begin);
    }

}

BOOST_AUTO_TEST_CASE(test_minmax_norm)
{
    const std::vector<double> test_data(std::begin(min_max_data), std::end(min_max_data));
    double min_val = PsY[0];
    double max_val = PsY[1];
    auto after_norm = utils::min_max_normalization(test_data, min_val, max_val);
    auto found = std::none_of(std::begin(after_norm), std::end(after_norm),
                        [min_val, max_val](auto v)
                        {
                            return v < min_val || v > max_val;
                        }
                    );
    BOOST_TEST_REQUIRE(found,
            "we don't expect to have any value after norm that is less than "
            <<min_val<<" or larger than "<<max_val);
    BOOST_TEST_REQUIRE(after_norm.size() == utils::size(expected_first),
            "the size of the norm output "<<after_norm.size()<<" not the same as the size "<<
            utils::size(expected_first)<<" that we are expecting");
    std::vector<double> expected(std::begin(expected_first), std::end(expected_first));
    boost::sort(after_norm);
    boost::sort(expected);
    auto expect_begin = std::begin(expected);
    for (auto norm_begin = std::begin(after_norm); norm_begin != std::end(after_norm); ++norm_begin, ++expect_begin) {
        BOOST_TEST(round_cmp(*norm_begin, *expect_begin, 0.0001), "value for "<<*norm_begin<<" != expected "<<*expect_begin);
    }
}
BOOST_AUTO_TEST_CASE(test_minmax_norm_second)
{
    const std::vector<double> test_data(std::begin(min_max_data), std::end(min_max_data));
    double min_val = PsY2[0];
    double max_val = PsY2[1];
    auto after_norm = utils::min_max_normalization(test_data, min_val, max_val);
    auto found = std::none_of(std::begin(after_norm), std::end(after_norm),
                        [min_val, max_val](auto v)
                        {
                            return v < min_val || v > max_val;
                        }
                    );
    BOOST_TEST_REQUIRE(found,
            "we don't expect to have any value after norm that is less than "
            <<min_val<<" or larger than "<<max_val);
    BOOST_TEST_REQUIRE(after_norm.size() == utils::size(expected_second),
            "the size of the norm output "<<after_norm.size()<<" not the same as the size "<<
            utils::size(expected_second)<<" that we are expecting");
    std::vector<double> expected(std::begin(expected_second), std::end(expected_second));
    boost::sort(after_norm);
    boost::sort(expected);
    auto expect_begin = std::begin(expected);
    for (auto norm_begin = std::begin(after_norm); norm_begin != std::end(after_norm); ++norm_begin, ++expect_begin) {
        BOOST_TEST(round_cmp(*norm_begin, *expect_begin, 0.0001), "value for "<<*norm_begin<<" != expected "<<*expect_begin);
    }
}

BOOST_AUTO_TEST_CASE(test_max_norm)
{
    const std::vector<double> test_data(std::begin(data_zmax), std::end(data_zmax));
    auto after_norm{utils::max_normalization(test_data)};
    BOOST_TEST_REQUIRE(after_norm.size() == test_data.size());
    BOOST_TEST_REQUIRE(after_norm.size() == utils::size(expected_zmax));
    std::vector<double> expected(std::begin(expected_zmax), std::end(expected_zmax));
    auto f = std::all_of(std::begin(after_norm), std::end(after_norm), [](auto val) {
                return val >= 0 && val <= 1;
            }
    );
    BOOST_TEST_REQUIRE(f, "we have value that is bigger than 1 or smaller than 0");
    //return;
    boost::sort(after_norm);
    boost::sort(expected);
    auto ex_begin = std::begin(expected);
    for (auto n_begin = std::begin(after_norm); n_begin != std::end(after_norm); ++n_begin, ++ex_begin) {
        BOOST_TEST(round_cmp(*n_begin, *ex_begin, 0.0001), "value after the norm "<<*n_begin<<" != expected "<<*ex_begin);
    }
}
BOOST_AUTO_TEST_CASE(test_nan_issue)
{
    const std::vector<double> test_data(std::begin(column_values), std::end(column_values));
    auto after_norm{utils::min_max_normalization(test_data, 0, 1)};
    BOOST_TEST(std::none_of(std::begin(after_norm), std::end(after_norm), [](auto val) {
                        if (std::isnan(val) || std::isinf(val)) {
                            BOOST_TEST(false, "value "<<val<<" is not valid");
                            return true;
                        }
                        return false;
                    }
            )
    );
}


//
// Created by keszocze on 10.10.18.
//

#include <error_rate.hpp>

#include <boost/multiprecision/cpp_int.hpp>
#include <catch2/catch.hpp>
#include <cudd/cplusplus/cuddObj.hh>
#include <cudd_helpers.hpp>
#include <simple.hpp>
#include <from_papers.hpp>
#include <worst_case_error.hpp>
#include <average_case_error.hpp>
#include <worst_case_relative_error.hpp>

#include <iostream>

TEST_CASE("Trivial Error Rate Test") {
    Cudd mgr(2);

    mgr.pushVariableName("x");
    mgr.pushVariableName("y");
    mgr.pushVariableName("z");

    std::vector<std::string> varnames = {"x","y","z"};
    std::vector<std::string> funnames = {"and", "and_hat"};

    BDD x = mgr.bddVar(0);
    BDD y = mgr.bddVar(1);
    BDD z = mgr.bddVar(2);

    BDD and_bdd = x*y*z;

    BDD and_approx_bdd = and_bdd.Constrain(y);

    std::vector<BDD> funs{and_bdd, and_approx_bdd};

//    abo::util::dump_dot(mgr,funs[0],varnames,funnames[0]);
//
//    abo::util::dump_dot(mgr,funs, varnames, funnames);

    REQUIRE(1.0 / 8.0 == abo::error_metrics::error_rate(mgr,and_bdd,and_approx_bdd));

    and_approx_bdd = and_approx_bdd.Constrain(x);

    REQUIRE(3.0 / 8.0 == abo::error_metrics::error_rate(mgr,and_bdd,and_approx_bdd));

    and_approx_bdd = and_approx_bdd.Constrain(z);

    REQUIRE(7.0 / 8.0 == abo::error_metrics::error_rate(mgr,and_bdd,and_approx_bdd));

}

/**
 * @brief Hardcoded example from the 2016 ASP-DAC paper by Soeken et al.
 */
TEST_CASE("Example 3 from ASP-DAC 2016 paper") {
    Cudd mgr(2);

    std::vector<BDD> fun = abo::example_bdds::example3(mgr);

//    abo::util::dump_dot(mgr,fun,{"x_1", "x_2"},{"d_0", "d_1", "d_2", "d_3", "d_4"});

    boost::multiprecision::uint256_t max_error = abo::error_metrics::get_max_value(mgr, fun);

    REQUIRE(13 == max_error);

}

TEST_CASE("Trivial worst_case_error") {
    Cudd mgr(2);

    std::vector<BDD> zero_fun{mgr.bddZero(), mgr.bddZero()};
    std::vector<BDD> one_fun{mgr.bddOne(), mgr.bddOne()};

//    std::cout << abo::error_metrics::worst_case_error(mgr, zero_fun, one_fun);

}

TEST_CASE("Generic OR constraint error rate test"){

    constexpr size_t n = 30;

    Cudd mgr(n);

    BDD or_bdd = abo::example_bdds::or_bdd(mgr,n);

    // TODO Methoden für die generische Erzeugung von AND, OR, XOR etc. BDDs erzeugen

    for (size_t i = 0; i < n; i++) {
        or_bdd = or_bdd + mgr.bddVar(i);
    }

    BDD or_pos_approx_bdd = or_bdd;
    BDD or_neg_approx_bdd = or_bdd;

    REQUIRE(0 == abo::error_metrics::error_rate(mgr, or_bdd,or_pos_approx_bdd));
    REQUIRE(0 == abo::error_metrics::error_rate(mgr, or_bdd,or_neg_approx_bdd));

    for (size_t i = 0; i < n; i++) {
        or_pos_approx_bdd = or_pos_approx_bdd.Constrain(mgr.bddVar(i));
        or_neg_approx_bdd = or_neg_approx_bdd.Constrain(!mgr.bddVar(i));

        double expected_error = double((1L << (i+1)) - 1) / double(1L << n);

        auto computed_error = abo::error_metrics::error_rate(mgr, or_bdd,or_neg_approx_bdd);

//        std::cout << "i=" << i << "\texpected error=" << expected_error <<"\tcomputed error=" << computed_error << "\n";
        REQUIRE(expected_error == computed_error);
        REQUIRE(1.0 / double(1L << n) ==  abo::error_metrics::error_rate(mgr, or_bdd,or_pos_approx_bdd));
    }

}

TEST_CASE("Generic AND constraint error rate test"){

    constexpr size_t n = 20;

    Cudd mgr(n);

    BDD and_bdd = abo::example_bdds::and_bdd(mgr,n);

    BDD and_pos_approx_bdd = and_bdd;
    BDD and_neg_approx_bdd = and_bdd;

    REQUIRE(0 == abo::error_metrics::error_rate(mgr, and_bdd,and_pos_approx_bdd));
    REQUIRE(0 == abo::error_metrics::error_rate(mgr, and_bdd,and_neg_approx_bdd));

    for (size_t i = 0; i < n; i++) {
        and_pos_approx_bdd = and_pos_approx_bdd.Constrain(mgr.bddVar(i));
        and_neg_approx_bdd = and_pos_approx_bdd.Constrain(!mgr.bddVar(i));
        double expected_error = double((1L << (i+1)) - 1) / double(1L << n);

        // TODO sinnvolles overloads, so dsas man keine dämlichen ein-bdd vektoren mehr bauen muss

        auto computed_error = abo::error_metrics::error_rate(mgr, and_bdd,and_pos_approx_bdd);

//        std::cout << "i=" << i << "\texpected error=" << expected_error <<"\tcomputed error=" << computed_error << "\n";
        REQUIRE(expected_error == computed_error);
        REQUIRE(expected_error == abo::error_metrics::error_rate(mgr,and_bdd,and_neg_approx_bdd));
    }

}

TEST_CASE("Trivial average case error") {

    const size_t n = 3;
    Cudd mgr(n);

    std::vector<BDD> zero({mgr.bddZero(), mgr.bddZero()});
    std::vector<BDD> one({mgr.bddOne(), mgr.bddOne()});

    REQUIRE(abo::error_metrics::average_case_error(mgr, one, zero) == 3);
}

static void check_wcr_values(Cudd &mgr, const std::vector<BDD> &f,
                             const std::vector<BDD> &g, long desired_wcr) {
    auto f_ = f;
    auto g_ = g;

    // equalize function sizes (is necessary for the error metrics)
    while (f_.size() < g_.size()) f_.push_back(mgr.bddZero());
    while (g_.size() < f_.size()) g_.push_back(mgr.bddZero());

    double bin_search = abo::error_metrics::wcre_search(mgr, f, g);
    REQUIRE(desired_wcr == bin_search);

    auto ran_search = abo::error_metrics::wcre_randomized_search(mgr, f, g);
    REQUIRE(desired_wcr * ran_search.second == ran_search.first);

    auto symbolic = abo::error_metrics::wcre_symbolic_division(mgr, f, g);
    REQUIRE(desired_wcr == symbolic);

    auto add = abo::error_metrics::wcre_symbolic_division(mgr, f, g);
    REQUIRE(desired_wcr == add);
}

TEST_CASE("Simple worst case relative error metrics") {
    Cudd mgr(256);
    auto f1 = abo::util::number_to_bdds(mgr, 1);

    // check some static values
    for (long f2val : {0, 1, 5, 21, 1232345}) {
        auto f2 = abo::util::number_to_bdds(mgr, f2val + 1);
        check_wcr_values(mgr, f1, f2, f2val);
    }

    // check division by zero not occuring
    check_wcr_values(mgr, abo::util::number_to_bdds(mgr, 0), f1, 1);

    // division is performed
    check_wcr_values(mgr, abo::util::number_to_bdds(mgr, 3),
                     abo::util::number_to_bdds(mgr, 18), 5);
}


#include "stdafx.h"
#include "qmath.h"
#include "config.h"

// each test module could contain no more then one 'main' file with init function defined
// alternatively you could define init function yourself
#include <boost/test/unit_test.hpp>

#include <vector>

// These are sample tests that show the different features of the framework

using namespace math;

namespace
{

template<typename T>
void testQuat()
{
	{
		quat<T> a;
		BOOST_CHECK(a.x == 0.f && a.y == 0.f && a.z == 0.f && a.w == 1.f);
	}

	{
		quat<T> a(1, 2, 3, 4);
		BOOST_CHECK(a.x == 1 && a.y == 2 && a.z == 3 && a.w == 4);
	}

	{
		quat<T> a(quat<T>(1, 2, 3, 4));
		BOOST_CHECK(a.x == 1 && a.y == 2 && a.z == 3 && a.w == 4);
	}

	{
		for (uint32_t i = 0; i < 360; i++)
		{
			quat<T> a;
			a.set_from_angle_axis(radians((T)i), vec3<T>(1, 0, 0));
			quat<T> b = quat_axis_x(radians((T)i));
			BOOST_CHECK(equals(a, b));
#ifdef TEST_AGAINST_EIGEN

			Eigen::AngleAxis<T> _aa(radians((T)i), Eigen::Matrix<T, 3, 1>(1, 0, 0));
			Eigen::Quaternion<T> _a(_aa);
			BOOST_CHECK(_a.x() == a.x && _a.y() == a.y && _a.z() == a.z && _a.w() == a.w);

#endif
		}
	}
	{
		for (uint32_t i = 0; i < 360; i++)
		{
			quat<T> a;
			a.set_from_angle_axis(radians((T)i), vec3<T>(0, 1, 0));
			quat<T> b = quat_axis_y(radians((T)i));
			BOOST_CHECK(equals(a, b));
#ifdef TEST_AGAINST_EIGEN

			Eigen::AngleAxis<T> _aa(radians((T)i), Eigen::Matrix<T, 3, 1>(0, 1, 0));
			Eigen::Quaternion<T> _a(_aa);
			BOOST_CHECK(_a.x() == a.x && _a.y() == a.y && _a.z() == a.z && _a.w() == a.w);

#endif
		}
	}
	{
		for (uint32_t i = 0; i < 360; i++)
		{
			quat<T> a;
			a.set_from_angle_axis(radians((T)i), vec3<T>(0, 0, 1));
			quat<T> b = quat_axis_z(radians((T)i));
			BOOST_CHECK(equals(a, b));
#ifdef TEST_AGAINST_EIGEN

			Eigen::AngleAxis<T> _aa(radians((T)i), Eigen::Matrix<T, 3, 1>(0, 0, 1));
			Eigen::Quaternion<T> _a(_aa);
			BOOST_CHECK(_a.x() == a.x && _a.y() == a.y && _a.z() == a.z && _a.w() == a.w);

#endif
		}
	}
	{
		vec3<T> right(1, 0, 0);
		quat<T> a = quat_axis_z(radians(T(90.0)));
		mat3<T> m = a.get_as_mat3();
		vec3<T> up = rotate(a, right);
		BOOST_CHECK(equals(up, vec3<T>(0, 1, 0), T(0.001)));

#ifdef TEST_AGAINST_EIGEN

		Eigen::Matrix<T, 3, 1> _right(1, 0, 0);
		Eigen::AngleAxis<T> _aa(radians(90.f), Eigen::Matrix<T, 3, 1>(0, 0, 1));
		Eigen::Quaternion<T> _q(_aa);
		Eigen::Matrix<T, 3, 3> _m(_q);
		Eigen::Matrix<T, 3, 1> _up = _q * _right;
		BOOST_CHECK(_up.isApprox(Eigen::Matrix<T, 3, 1>(0, 1, 0), T(0.001)));

#endif
	}

	{
		//test Euler constructors
		vec3<T> tv = normalized(vec3<T>(1, 1, 1));

		// ms temporarily disable these long tests
		//return;

		std::vector<quat<T>> quatAxisX(360);
		std::vector<quat<T>> quatAxisY(360);
		std::vector<quat<T>> quatAxisZ(360);
		for (int i = 0; i < 360; i+=5)
		{
			T angle = radians((T)i);
			quatAxisX[i].set_from_angle_axis(angle, vec3<T>(1, 0, 0));
			quatAxisY[i].set_from_angle_axis(angle, vec3<T>(0, 1, 0));
			quatAxisZ[i].set_from_angle_axis(angle, vec3<T>(0, 0, 1));
		}

		//ZXY = Y * X * Z

		quat<T> aX = quatAxisX[32];
		quat<T> aY = quatAxisY[32];
		quat<T> aZ = quatAxisZ[32];

		// 	quat<T> aXYZ = aZ * aY * aX;

		//test Euler setters getters
		for (int anglex = 0; anglex < 360; anglex += 15)
		{
			vec3<T> angles;
			angles.x = radians((T)anglex);
			quat<T> aX = quatAxisX[anglex];

			printf("\ntesting X = %d", anglex);
			for (int angley = 0; angley < 360; angley += 15)
			{
				angles.y = radians((T)angley);
				quat<T> aY = quatAxisY[angley];

				quat<T> aYX = aX * aY;
				quat<T> aXY = aY * aX;

				for (int anglez = 0; anglez < 360; anglez += 15)
				{
					angles.z = radians((T)anglez);
					//printf("\ntesting (%d, %d, %d)", anglex, angley, anglez);
					quat<T> aZ = quatAxisZ[anglez];

					quat<T> aXYZ = aYX*aZ;
					quat<T> aXZY = aX * aZ * aY;
					quat<T> aYXZ = aXY * aZ;
					quat<T> aYZX = aY * aZ * aX;
					quat<T> aZXY = aZ * aYX;
					quat<T> aZYX = aZ * aXY;

					{
						quat<T> a;
						a.set_from_euler_xyz(angles);
						if (!equals(aXYZ, a))
						{
							BOOST_CHECK(is_one(length(a), T(0.00001)));
							vec3<T> v1 = rotate(aXYZ, tv);
							vec3<T> v2 = rotate(a, tv);
							BOOST_CHECK(equals(v1, v2, T(0.001)));
						}
					}

					{
						quat<T> a;
						a.set_from_euler_xzy(angles);
						if (!equals(aXZY, a))
						{
							BOOST_CHECK(is_one(length(a), T(0.00001)));
							vec3<T> v1 = rotate(aXZY, tv);
							vec3<T> v2 = rotate(a, tv);
							BOOST_CHECK(equals(v1, v2, T(0.001)));
						}
					}

					{
						quat<T> a;
						a.set_from_euler_yxz(angles);
						if (!equals(aYXZ, a))
						{
							BOOST_CHECK(is_one(length(a), T(0.00001)));
							vec3<T> v1 = rotate(aYXZ, tv);
							vec3<T> v2 = rotate(a, tv);
							BOOST_CHECK(equals(v1, v2, T(0.001)));
						}
					}

					{
						quat<T> a;
						a.set_from_euler_yzx(angles);
						if (!equals(aYZX, a))
						{
							BOOST_CHECK(is_one(length(a), T(0.00001)));
							vec3<T> v1 = rotate(aYZX, tv);
							vec3<T> v2 = rotate(a, tv);
							BOOST_CHECK(equals(v1, v2, T(0.001)));
						}
					}

					{
						quat<T> a;
						a.set_from_euler_zxy(angles);
						if (!equals(aZXY, a))
						{
							BOOST_CHECK(is_one(length(a), T(0.00001)));
							vec3<T> v1 = rotate(aZXY, tv);
							vec3<T> v2 = rotate(a, tv);
							BOOST_CHECK(equals(v1, v2, T(0.001)));
						}
					}

					{
						quat<T> a;
						a.set_from_euler_zyx(angles);
						if (!equals(aZYX, a))
						{
							BOOST_CHECK(is_one(length(a), T(0.00001)));
							vec3<T> v1 = rotate(aZYX, tv);
							vec3<T> v2 = rotate(a, tv);
							BOOST_CHECK(equals(v1, v2, T(0.001)));
						}
					}
				}
			}
		}

		//test Euler setters getters
		for (int anglex = 0; anglex < 360; anglex += 15)
		{
			vec3<T> angles;
			angles.x = radians((T)anglex);
			quat<T> aX = quatAxisX[anglex];

			printf("\ntesting X = %d", anglex);
			for (int angley = 0; angley < 360; angley += 15)
			{
				angles.y = radians((T)angley);
				quat<T> aY = quatAxisY[angley];

				quat<T> aYX = aX * aY;
				quat<T> aXY = aY * aX;

				for (int anglez = 0; anglez < 360; anglez += 15)
				{
					angles.z = radians((T)anglez);
					//printf("\ntesting (%d, %d, %d)", anglex, angley, anglez);
					quat<T> aZ = quatAxisZ[anglez];

					{
						quat<T> a;
						a.set_from_euler_xyz(angles);
						vec3<T> angles2;
						a.get_as_euler_xyz(angles2);
						if (!equals(angles, angles2))
						{
							quat<T> a2;
							a2.set_from_euler_xyz(angles2);
							vec3<T> v1 = rotate(aYX * aZ, tv);
							vec3<T> v2 = rotate(a, tv);
							vec3<T> v3 = rotate(a2, tv);
							BOOST_CHECK(equals(v1, v2, T(0.001)) && equals(v2, v3, T(0.001)));
						}
					}

					{
						quat<T> a;
						a.set_from_euler_xzy(angles);
						vec3<T> angles2;
						a.get_as_euler_xzy(angles2);
						if (!equals(angles, angles2))
						{
							quat<T> a2;
							a2.set_from_euler_xzy(angles2);
							vec3<T> v1 = rotate(aX * aZ * aY, tv);
							vec3<T> v2 = rotate(a, tv);
							vec3<T> v3 = rotate(a2, tv);
							BOOST_CHECK(equals(v1, v2, T(0.001)) && equals(v2, v3, T(0.001)));
						}
					}

					{
						quat<T> a;
						a.set_from_euler_yxz(angles);
						vec3<T> angles2;
						a.get_as_euler_yxz(angles2);
						if (!equals(angles, angles2))
						{
							quat<T> a2;
							a2.set_from_euler_yxz(angles2);
							vec3<T> v1 = rotate(aXY * aZ, tv);
							vec3<T> v2 = rotate(a, tv);
							vec3<T> v3 = rotate(a2, tv);
							BOOST_CHECK(equals(v1, v2, T(0.001)) && equals(v2, v3, T(0.001)));
						}
					}

					{
						quat<T> a;
						a.set_from_euler_yzx(angles);
						vec3<T> angles2;
						a.get_as_euler_yzx(angles2);
						if (!equals(angles, angles2))
						{
							quat<T> a2;
							a2.set_from_euler_yzx(angles2);
							vec3<T> v1 = rotate(aY * aZ * aX, tv);
							vec3<T> v2 = rotate(a, tv);
							vec3<T> v3 = rotate(a2, tv);
							BOOST_CHECK(equals(v1, v2, T(0.001)) && equals(v2, v3, T(0.001)));
						}
					}

					{
						quat<T> a;
						a.set_from_euler_zxy(angles);
						vec3<T> angles2;
						a.get_as_euler_zxy(angles2);
						if (!equals(angles, angles2))
						{
							quat<T> a2;
							a2.set_from_euler_zxy(angles2);
							vec3<T> v1 = rotate(aZ * aYX, tv);
							vec3<T> v2 = rotate(a, tv);
							vec3<T> v3 = rotate(a2, tv);
							BOOST_CHECK(equals(v1, v2, T(0.001)) && equals(v2, v3, T(0.001)));
						}
					}

					{
						quat<T> a;
						a.set_from_euler_zyx(angles);
						vec3<T> angles2;
						a.get_as_euler_zyx(angles2);
						if (!equals(angles, angles2))
						{
							quat<T> a2;
							a2.set_from_euler_zyx(angles2);
							vec3<T> v1 = rotate(aZ * aXY, tv);
							vec3<T> v2 = rotate(a, tv);
							vec3<T> v3 = rotate(a2, tv);
							BOOST_CHECK(equals(v1, v2, T(0.001)) && equals(v2, v3, T(0.001)));
						}
					}
				}
			}
		}

	}

}

BOOST_AUTO_TEST_CASE(TESTQUAT)
{
	testQuat<float>();
	testQuat<double>();
}

}


#include "quadrature/qhermite.hpp"
#include "spectral/basis/spectral_basis.hpp"
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/spectral_elem.hpp"
#include "spectral/basis/spectral_elem_accessor.hpp"
#include "spectral/basis/spectral_function/hermite_polynomial.hpp"

#include "spectral/polar_to_hermite.hpp"
#include "spectral/shift_hermite_2d.hpp"

#include <Eigen/Sparse>
#include <boost/program_options.hpp>
#include <fstream>
#include <iomanip>
#include <iostream>

#define PI 3.141592653589793238462643383279502884197

using namespace std;
using namespace boltzmann;

namespace po = boost::program_options;

/**
 * @brief Check coefficients P->H, H->P
 *
 * @param polar_basis
 * @param hermite_basis
 */
template <typename H, typename P>
void test1(const P& polar_basis, const H& hermite_basis)
{
  Polar2Hermite<P, H> P2H(polar_basis, hermite_basis);
  P2H.exportmat("p2h.hdf5");
  unsigned int N = polar_basis.n_dofs();

  std::vector<double> C(N, 1);

  //  C[N-1] = 1.0;
  std::vector<double> Ch(N, 0);
  std::vector<double> Cb(N, 0);

  P2H.to_hermite(Ch, C);
  P2H.to_polar(Cb, Ch);

  cout << setw(5) << "index" << setw(20) << "c" << setw(20) << "T(P->H) T(H->P) c" << setw(20)
       << "error" << endl;
  for (unsigned int i = 0; i < N; ++i) {
    cout << setw(5) << i << "\t" << setw(20) << Cb[i] << setw(20) << C[i] << setw(20)
         << setprecision(6) << scientific << std::abs(Cb[i] - C[i]) << endl;
  }
  cout << "finished\n";
}

/**
 * @brief Polar -> Hermite, evaluate at different points
 *
 * @param polar_basis
 * @param hermite_basis
 */
template <typename H, typename P>
void test2(const P& polar_basis, const H& hermite_basis)
{
  Polar2Hermite<P, H> P2H(polar_basis, hermite_basis);

  /*
   *  Initialize polar coefficients
   */
  const unsigned int N = polar_basis.n_dofs();
  // std::vector<double> C(N, 0);
  // auto elem = SpectralBasisFactoryKS::make_elem(0, K-1, TRIG::COS);
  // unsigned int idx = polar_basis.get_dof_index(elem.get_id());
  // exp(-0.5 r^2)
  // C[idx] = 1.0;

  // if (idx  >= 2)
  //   C[idx-2] = 1.0;
  std::vector<double> C(N, 1.0);

  /*
   * Compute hermite coefficients
   */
  std::vector<double> Ch(N);
  P2H.to_hermite(Ch, C);

  // // output
  // std::ofstream fout("transformed-coefficients");
  // for (size_t i = 0; i < Ch.size(); ++i) {
  //   fout << Ch[i] << endl;
  // }
  // fout.close();

  /*
   * CHECK: do the hermite and polar series expansion match at evaluation
   * points?
   */
  {
    auto evalH = [&](const std::vector<double>& c, double x, double y) {
      auto itc = c.begin();
      double val = 0;
      for (auto it = hermite_basis.begin(); it != hermite_basis.end(); ++it, ++itc) {
        val += it->evaluate_weighted(x, y) * (*itc);
      }
      return val;
    };

    auto evalB = [&](const std::vector<double>& c, double phi, double r) {
      auto itc = c.begin();
      double val = 0;
      for (auto it = polar_basis.begin(); it != polar_basis.end(); ++it, ++itc) {
        val += it->evaluate_weighted(phi, r) * (*itc);
      }
      return val;
    };

    double r = 0.1;
    const int n = 40;
    for (int i = 0; i < n; ++i) {
      double phi = 2 * PI * i / (n + 1);
      double x = r * cos(phi);
      double y = r * sin(phi);

      const double fB = evalB(C, phi, r);
      const double fH = evalH(Ch, x, y);
      cout << setprecision(6) << scientific << setw(20) << fB << "\t" << setw(20) << fH << "\t"
           << setw(20) << std::abs(fB - fH) << endl;
    }
    r = 2.5;
    cout << "r = " << r << endl;
    for (int i = 0; i < n; ++i) {
      double phi = 2 * PI * i / (n + 1);
      double x = r * cos(phi);
      double y = r * sin(phi);

      const double fB = evalB(C, phi, r);
      const double fH = evalH(Ch, x, y);
      cout << setprecision(6) << scientific << setw(20) << fB << "\t" << setw(20) << fH << "\t"
           << setw(20) << std::abs(fB - fH) << endl;
    }
  }
}

int main(int argc, char* argv[])
{
  int K;
  po::options_description options("options");
  options.add_options()
      ("help", "show help message")
      ("nK,K", po::value<int>(&K)->default_value(10), "K");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  cout << "using \n\tK: " << K << "\n";

  if (vm.count("help")) {
    cout << options << "\n";
    return 0;
  }

  typedef typename SpectralBasisFactoryKS::basis_type polar_basis_t;
  polar_basis_t polar_basis;
  SpectralBasisFactoryKS::create(polar_basis, K, K, 2, true);
  SpectralBasisFactoryKS::write_basis_descriptor(polar_basis, "spectral_basis.desc");

  typedef typename SpectralBasisFactoryHN::basis_type hermite_basis_t;
  hermite_basis_t hermite_basis;
  SpectralBasisFactoryHN::create(hermite_basis, K, 2);
  SpectralBasisFactoryHN::write_basis_descriptor(hermite_basis, "hermite_basis.desc");

  cout << "size(polar basis) = " << polar_basis.n_dofs() << endl
       << "size(hermite basis) = " << hermite_basis.n_dofs();

  cout << "\n--------------------\n";
  cout << "Test 1: (P->H) -> (H->P) show coefficients\n";
  test1(polar_basis, hermite_basis);

  cout << "\n--------------------\n";
  cout << "Test 2: (P->H) and compare evaluation at point of c_H, c_P\n";
  test2(polar_basis, hermite_basis);

  return 0;
}


/**
 * @file gamma_distribution.hpp
 * @author Yannis Mentekidis
 *
 * Implementation of a Gamma distribution of multidimensional data that fits
 * gamma parameters (alpha, beta) to data.
 * The fitting is done independently for each dataset dimension (row), based on
 * the assumption each dimension is fully indepeendent.
 *
 * Based on "Estimating a Gamma Distribution" by Thomas P. Minka:
 * research.microsoft.com/~minka/papers/minka-gamma.pdf
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef _MLPACK_CORE_DISTRIBUTIONS_GAMMA_DISTRIBUTION_HPP
#define _MLPACK_CORE_DISTRIBUTIONS_GAMMA_DISTRIBUTION_HPP

#include <mlpack/prereqs.hpp>
#include <mlpack/core/math/random.hpp>
#include <boost/program_options.hpp>

namespace mlpack {
namespace distribution {

/**
 * This class represents the Gamma distribution.  It supports training a Gamma
 * distribution on a given dataset and accessing the fitted alpha and beta
 * parameters.
 *
 * This class supports multidimensional Gamma distributions; however, it is
 * assumed that each dimension is independent; therefore, a multidimensional
 * Gamma distribution here may be seen as a set of independent
 * single-dimensional Gamma distributions---and the parameters are estimated
 * under this assumption.
 *
 * The estimation algorithm used can be found in the following paper:
 *
 * @code
 * @techreport{minka2002estimating,
 *   title={Estimating a {G}amma distribution},
 *   author={Minka, Thomas P.},
 *   institution={Microsoft Research},
 *   address={Cambridge, U.K.},
 *   year={2002}
 * }
 * @endcode
 */
class GammaDistribution
{
 public:
    /**
     * Construct the Gamma distribution with the given number of dimensions
     * (default 0); each parameter will be initialized to 0.
     *
     * @param dimensionality Number of dimensions.
     */
    GammaDistribution(const size_t dimensionality = 0);

    /**
     * Construct the Gamma distribution, training on the given parameters.
     *
     * @param data Data to train the distribution on.
     * @param tol Convergence tolerance. This is *not* an absolute measure:
     *    It will stop the approximation once the *change* in the value is
     *    smaller than tol.
     */
    GammaDistribution(const arma::mat& data, const double tol = 1e-8);

    /**
     * Construct the Gamma distribution given two vectors alpha and beta.
     *
     * @param alpha The vector of alphas, one per dimension.
     * @param beta The vector of betas, one per dimension.
     */
    GammaDistribution(const arma::vec& alpha, const arma::vec& beta);

    /**
     * Destructor.
     */
    ~GammaDistribution() {}

    /**
     * This function trains (fits distribution parameters) to new data or the
     * dataset the object owns.
     *
     * @param rdata Reference data to fit parameters to.
     * @param tol Convergence tolerance. This is *not* an absolute measure:
     *    It will stop the approximation once the *change* in the value is
     *    smaller than tol.
     */
    void Train(const arma::mat& rdata, const double tol = 1e-8);

    /**
     * Fits an alpha and beta parameter according to observation probabilities.
     * This method is not yet implemented.
     *
     * @param observations The reference data, one observation per column
     * @param probabilities The probability of each observation. One value per
     *     column of the observations matrix.
     * @param tol Convergence tolerance. This is *not* an absolute measure:
     *    It will stop the approximation once the *change* in the value is
     *    smaller than tol.
     */
    void Train(const arma::mat& observations,
               const arma::vec& probabilities,
               const double tol = 1e-8);

    /**
     * This function trains (fits distribution parameters) to a dataset with
     * pre-computed statistics logMeanx, meanLogx, meanx for each dimension.
     *
     * @param logMeanxVec Is each dimension's logarithm of the mean
     *     (log(mean(x))).
     * @param meanLogxVec Is each dimension's mean of logarithms (mean(log(x))).
     * @param meanxVec Is each dimension's mean (mean(x)).
     * @param tol Convergence tolerance. This is *not* an absolute measure:
     *    It will stop the approximation once the *change* in the value is
     *    smaller than tol.
     */
    void Train(const arma::vec& logMeanxVec,
               const arma::vec& meanLogxVec,
               const arma::vec& meanxVec,
               const double tol = 1e-8);

    /**
     * This function returns the probability of a group of observations.
     *
     * The probability of the value x is
     *
     * \frac{x^(\alpha - 1)}{\Gamma(\alpha) * \beta^\alpha} * e ^
     * {-\frac{x}{\beta}}
     *
     * for one dimension. This implementation assumes each dimension is
     * independent, so the product rule is used.
     *
     * @param observations Matrix of observations, one per column.
     * @param probabilities column vector of probabilities, one per observation.
     */
    void Probability(const arma::mat& observations,
                     arma::vec& Probabilities) const;

    /*
     * This is a shortcut to the Probability(arma::mat&, arma::vec&) function
     * for when we want to evaluate only the probability of one dimension of the
     * gamma.
     *
     * @param x The 1-dimensional observation.
     * @param dim The dimension for which to calculate the probability
     */
    double Probability(double x, size_t dim) const;

    /**
     * This function returns the logarithm of the probability of a group of
     * observations.
     *
     * The logarithm of the probability of a value x is
     *
     * log(\frac{x^(\alpha - 1)}{\Gamma(\alpha) * \beta^\alpha} * e ^
     * {-\frac{x}{\beta}})
     *
     * for one dimension. This implementation assumes each dimension is
     * independent, so the product rule is used.
     *
     * @param observations Matrix of observations, one per column.
     * @param logProbabilities column vector of log probabilities, one per
     *     observation.
     */
    void LogProbability(const arma::mat& observations,
                        arma::vec& LogProbabilities) const;

    /**
     * This function returns an observation of this distribution
     */
    arma::vec Random() const;

    // Access to Gamma distribution parameters.

    //! Get the alpha parameter of the given dimension.
    double Alpha(const size_t dim) const { return alpha[dim]; }
    //! Modify the alpha parameter of the given dimension.
    double& Alpha(const size_t dim) { return alpha[dim]; }

    //! Get the beta parameter of the given dimension.
    double Beta(const size_t dim) const { return beta[dim]; }
    //! Modify the beta parameter of the given dimension.
    double& Beta(const size_t dim) { return beta[dim]; }

    //! Get the dimensionality of the distribution.
    size_t Dimensionality() const { return alpha.n_elem; }

 private:
    //! Array of fitted alphas.
    arma::vec alpha;
    //! Array of fitted betas.
    arma::vec beta;

    /**
     * This is a small function that returns true if the update of alpha is
     * smaller than the tolerance ratio.
     *
     * @param aOld old value of parameter we want to estimate (alpha in our
     *      case).
     * @param aNew new value of parameter (the value after 1 iteration from
     *      aOld).
     * @param tol Convergence tolerance. Relative measure (see documentation of
     *      GammaDistribution::Train).
     */
    inline bool Converged(const double aOld,
                          const double aNew,
                          const double tol);
};

} // namespace distribution
} // namespace mlpack

#endif


/*
 * ex10.cpp
 *
 * 	\brief     Tenth exercise
 *  \details   This class reads graph-data and computes the so-called Steiner-tree for the first 100 terminals by multithreading
 *  \author    Julia Baumbach
 *  \date      15.07.2017
 */

#include "GraphReader.h"
#include <iostream>
#include <sstream>
#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>
#include "SteinerSolver.h"
#include "TreeChecker.h"

using namespace std;

/*
 * \fn bool hasDivisor(vector<int>, int)
 * \brief computes if an int has a divisor in a list of ints
 * \return true, if it has a divisor, otherwise false
 */
bool hasDivisor(const vector<int>* result, int j) {
	for (int i = 0; i < result->size(); i++) {
		if ((j % result->at(i)) == 0 || (result->at(i))/2 >= j) {
			return true;
		}
	}
	return false;
}

/*
 * \fn vector<int> computePrimes(int upperBound)
 * \brief computes all primes in range from 2 to upperBound
 * \return vector of all primes
 */
void computePrimes(vector<int>* target, int upperBound){
	target->push_back(2);
	for(int i = 3; i < upperBound; i++){
		if(!hasDivisor(target, i)){
			target->push_back(i);
		}
	}
}

/*
 * \fn int main(int argc, char* argv[])
 * \brief main function. reads in graph data and prints the solution for the steiner tree problem. Run program with ./ex10 NUMBERTHREADS FILENAME
 * \return EXIT_SUCCESS if program exited correctly, otherwise EXIT_FAILURE
 */
int main(int argc, char* argv[]){
	//Initialize timer for cpu time measurement
	boost::timer::cpu_timer cpu_timer;

	int numberOfThreads;
	stringstream ss(argv[1]);
	ss >> numberOfThreads;

	cout << "Read graph... " << endl;
	GraphReader* reader = new GraphReader();
	if(!reader->readDataFromFile(argv[2])){
		cerr << "Error while reading data. Exit program" << endl;
		return EXIT_FAILURE;
	}

	//Get data from graphReader
	unsigned int numberVertices = reader->getNumberOfVertices();
	SortedEdges edges = reader->getSortedEdges();
	WeightMap weights = reader->getWeightMap();

	delete reader;
	//Initialize timer for wallclock time measurement
	boost::timer::cpu_timer wall_timer;

	cout << "Compute primes... " << endl;
	vector<int>* terminals = new vector<int>();
	computePrimes(terminals, numberVertices);
	vector<int>* firstHundredTerminals;
	if(terminals->size() < 100){
		firstHundredTerminals = terminals;
	}else {
		firstHundredTerminals = new vector<int>(terminals->begin(), terminals->begin() + 100);
	}

	//Solve the Steiner Problem for the given graph and given start nodes
	Edges resultEdges[100];
	int resultObjValues[100];

	cout << "Compute Steiner..." << endl;
	#pragma omp parallel for num_threads(numberOfThreads)
	for(int i = 0; i < firstHundredTerminals->size(); i++){
		SteinerSolver* mySteiner = new SteinerSolver(*terminals);
		resultEdges[i] = mySteiner->solveSteiner(edges, numberVertices, firstHundredTerminals->at(i));
		resultObjValues[i] = mySteiner->getObjectiveValue();
		delete mySteiner;
	}
	//Stop wall time
	boost::timer::cpu_times wall_time = wall_timer.elapsed();

	cout << "Search for minimum..." << endl;
	//Search for the minimal steiner tree
	int indexMinNode;
	int minObjValue = INT_MAX;
	for(int i = 0; i < firstHundredTerminals->size(); i++){
		if(resultObjValues[i] < minObjValue){
			indexMinNode = i;
			minObjValue = resultObjValues[i];
		}
	}

	cout << "Check tree..." << endl;
	//Check if the minimal steiner tree is a tree and contains all terminals
	TreeChecker myChecker(resultEdges[indexMinNode], numberVertices);
	if(!myChecker.allNodesContained(firstHundredTerminals)){
		cout << "CONTAINS NOT ALL TERMINALS" << endl;
	}
	if(!myChecker.hasNoCircles()){
		cout << "CONTAINS CIRLCES" << endl;
	}
	if(!myChecker.isConnected()){
		cout << "NOT CONNECTED" << endl;
	}

	//print results
	cout << "TLEN: " << resultObjValues[indexMinNode] << endl;
	Edges result = resultEdges[indexMinNode];
	cout << "TREE: ";
	for(int i = 0; i < result.size(); i++){
		if(i == result.size() -1){
			cout << "(" << result.at(i).first << "," << result.at(i).second << ")" << endl;
		}else {
			cout << "(" << result.at(i).first << "," << result.at(i).second << ") ";
		}
	}

	delete firstHundredTerminals;
	//Print measured time
	boost::timer::cpu_times cpu_time = cpu_timer.elapsed();

	cout << "TIME: " << (cpu_time.system + cpu_time.user) * 1e-9 << " seconds" << endl;
	cout << "WALL: " << wall_time.wall * 1e-9 <<  " seconds" << endl;

	return EXIT_SUCCESS;
}