#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;
}


/*
 *  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(),
};


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

}


/**
 * @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;
}


#include <random>
#include <boost/range/irange.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm/for_each.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp>
#include "kde.hpp"

using std::vector;
using boost::irange;
using boost::lambda::_1;
using boost::lambda::_2;
using std::mt19937;
using std::normal_distribution;

double sample_from_normal(
    double mu = 0.0, /**< The mean of the distribution.*/
    double sd = 1.0  /**< The standard deviation of the distribution.*/
) {
  mt19937 gen = RNG::rng()->get_RNG();
  normal_distribution<> d{mu, sd};
  return d(gen);
}

KDE::KDE(std::vector<double> v) : dataset(v) {
    using boost::adaptors::transformed;
    using boost::lambda::_1;
    using utils::mean;

    // Compute the bandwidth using Silverman's rule
    mu = mean(v);
    auto X = v | transformed(_1 - mu);

    // Compute standard deviation of the sample.
    size_t N = v.size();
    double stdev = sqrt(inner_product(X, X, 0.0) / (N - 1));
    bw = pow(4 * pow(stdev, 5) / (3 * N), 1 / 5);
  }

vector<double> KDE::resample(int n_samples) {
  vector<double> samples;
  for (int i : irange(0, n_samples)) {
    double element = select_random_element(dataset);
    samples.push_back(sample_from_normal(element, bw));
  }
  return samples;
}

double KDE::pdf(double x) {
  using utils::sqr;
  double p = 0.0;
  size_t N = dataset.size();
  for (double elem : dataset) {
    double x1 = exp(-sqr(x - elem) / (2 * sqr(bw)));
    x1 /= N * bw * sqrt(2 * M_PI);
    p += x1;
  }
  return p;
}

vector<double> KDE::pdf(vector<double> v) {
  vector<double> values;
  for (double elem : v) {
    values.push_back(pdf(elem));
  }
  return values;
}

double KDE::logpdf(double x) { return log(pdf(x)); }


/*
 *  Explicit to implicit reconstruction
 *  Date : 20 Feb 2020
 *  Author : Sachin Krishnan T V (sachu92@gmail.com)
 */

#include <iostream>
#include <cstdlib>
#include <fstream>
#include <sstream>
#include <cstring>
#include "reconstruct.hpp"
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main(int argc, char *argv[])
{
  // Input: point cloud filename
  // Input: mesh size, box boundaries

  // Output: the implicit surface representation in a VTP file. (mesh points with SDFs)

  if(argc < 9)
  {
    cout<<"Usage: <pointcloud_file> <mesh_size> <low_x> <high_x> <low_y> <high_y> <low_z> <high_z>";
    return 0;
  }

  cout<<endl;
  cout<<"Reading the input file...";
  // Read input file
  readPointCloud(argv[1]);

  cout<<endl<<"Reading parameters...";
  // Set parameters for the output mesh
  mesh_size = atoi(argv[2]);
  xlo = atof(argv[3]);
  xhi = atof(argv[4]);
  ylo = atof(argv[5]);
  yhi = atof(argv[6]);
  zlo = atof(argv[7]);
  zhi = atof(argv[8]);

  xbinsize = (xhi - xlo) / mesh_size;
  ybinsize = (yhi - ylo) / mesh_size;
  zbinsize = (zhi - zlo) / mesh_size;

  cout<<endl<<"Evaluating the SDF...";
  // Evaluate the signed distance functions
  evaluateSDF();

  cout<<endl<<"Reconstructing mesh...";
  reconstructMesh();
  cout<<endl<<"Writing mesh structure to file...";
  outputMesh();
  cout<<endl<<"Done."<<endl;

  return 0;
}

void readPointCloud(char *filename)
{
  int i;
  int nump = 0;
  bool done_flag = false;
  string line;
  string temp;
  ifstream infile;
  stringstream iss;

  infile.open(filename, ios::in);

  /* Reading the header */
  while(!infile.eof() && !done_flag)
  {
    infile>>line;
    if(strcmp(line.c_str(),"<Piece")==0)
    {
      infile>>line;
      iss.str(line);
      getline(iss,temp,'\"');
      getline(iss,temp,'\"');
      nump = atoi(temp.c_str());
      pc_coord.resize(nump, std::vector<double>(3, 0.0));
      pc_norm.resize(nump, std::vector<double>(3, 0.0));
    }
    else if(strcmp(line.c_str(),"<Points>")==0)
    {
      double rx, ry, rz;
      for(i = 0;i < 5;i++)
        infile>>line;
      for(i = 0;i < nump;i++)
      {
        infile>>rx>>ry>>rz;
        pc_coord[i][0] = rx;
        pc_coord[i][1] = ry;
        pc_coord[i][2] = rz;
      }
    }
    else if(strcmp(line.c_str(), "Name=\"Normals\"")==0)
    {
      double nx, ny, nz;
      // Ignore next string
      infile>>line>>line;
      for(i = 0;i < nump;i++)
      {
        infile>>nx>>ny>>nz;
        pc_norm[i][0] = nx;
        pc_norm[i][1] = ny;
        pc_norm[i][2] = nz;
      }
      done_flag = true;
    }
  }
  infile.close();
  return;
}

double distance(double x1, double y1, double z1, double x2, double y2, double z2)
{
  double d;
  d = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
  return d;
}

double triharmonic_kernel(double x)
{
  return x*x*x;
}

// Calculate the weights
void evaluateSDF()
{
  int i, j;
  int nump = pc_coord.size();
  double x1, y1, z1, x2, y2, z2;

  Matrix<double, Dynamic, Dynamic> K;
  Matrix<double, Dynamic, 1> d;

  K.resize(2*nump, 2*nump);
  d.resize(2*nump, NoChange);

  for(i = 0;i < 2*nump;i++)
  {
    if(i < nump)
    {
      x1 = pc_coord[i][0];
      y1 = pc_coord[i][1];
      z1 = pc_coord[i][2];
      d(i) = 0.0;
    }
    else
    {
      x1 = pc_coord[i-nump][0] + EPS*pc_norm[i-nump][0];
      y1 = pc_coord[i-nump][1] + EPS*pc_norm[i-nump][1];
      z1 = pc_coord[i-nump][2] + EPS*pc_norm[i-nump][2];
      d(i) = EPS;
    }
    for(j = 0;j < 2*nump;j++)
    {
      if(j < nump)
      {
        x2 = pc_coord[j][0];
        y2 = pc_coord[j][1];
        z2 = pc_coord[j][2];
      }
      else
      {
        x2 = pc_coord[j-nump][0] + EPS*pc_norm[j-nump][0];
        y2 = pc_coord[j-nump][1] + EPS*pc_norm[j-nump][1];
        z2 = pc_coord[j-nump][2] + EPS*pc_norm[j-nump][2];
      }
      K(i, j) = triharmonic_kernel(distance(x1, y1, z1, x2, y2, z2));
    }
  }

  Matrix<double, Dynamic, 1> w = K.inverse()*d;
  w.resize(2*nump, NoChange);

  rbf_weight.resize(2*nump);
  for(i = 0;i < 2*nump;i++)
  {
    rbf_weight[i] = w(i);
  }
  return;
}

// Reconstruct the mesh based on the calculated weights
void reconstructMesh()
{
  int i, j, k, l;
  int nump = pc_coord.size();
  long index;
  double mx, my, mz, mphi;

  mesh_ls.resize(mesh_size*mesh_size*mesh_size, 0);

  for(i = 0;i < mesh_size;i++)
  {
    mx = xlo + xbinsize*i;
    for(j = 0;j < mesh_size;j++)
    {
      my = ylo + ybinsize*j;
      for(k = 0;k < mesh_size;k++)
      {
        mz = zlo + zbinsize*k;

        mphi = 0.0;
        for(l = 0;l < nump;l++)
        {
          mphi += rbf_weight[l]*triharmonic_kernel(distance(mx, my, mz, pc_coord[l][0],
                                                            pc_coord[l][1], pc_coord[l][2]));
        }
        for(l = 0;l < nump;l++)
        {
          mphi += rbf_weight[l+nump]*triharmonic_kernel(distance(mx, my, mz,
                                                                 pc_coord[l][0] + EPS*pc_norm[l][0],
                                                                 pc_coord[l][1] + EPS*pc_norm[l][1],
                                                                 pc_coord[l][2] + EPS*pc_norm[l][2]));
        }
        index = k*mesh_size*mesh_size + j*mesh_size + i;
        mesh_ls[index] = mphi;
      }
    }
  }
  return;
}

// Write output file in VTK format
void outputMesh()
{
  int i;
  ofstream outfile;

  outfile.open("output.vtk", ios::out);
  outfile<<"# vtk DataFile Version 2.0"<<endl;
  outfile<<"Level set data"<<endl;
  outfile<<"ASCII"<<endl;
  outfile<<"DATASET RECTILINEAR_GRID"<<endl;
  outfile<<"DIMENSIONS "<<mesh_size<<" "<<mesh_size<<" "<<mesh_size<<endl;
  outfile<<"X_COORDINATES "<<mesh_size<<" float"<<endl;
  for(i = 0;i < mesh_size;i++)
  {
    outfile<<i*xbinsize + xlo<<endl;
  }
  outfile<<"Y_COORDINATES "<<mesh_size<<" float"<<endl;
  for(i = 0;i < mesh_size;i++)
  {
    outfile<<i*ybinsize + ylo<<endl;
  }
  outfile<<"Z_COORDINATES "<<mesh_size<<" float"<<endl;
  for(i = 0;i < mesh_size;i++)
  {
    outfile<<i*zbinsize + zlo<<endl;
  }
  outfile<<"POINT_DATA "<<mesh_ls.size()<<endl;
  outfile<<"SCALARS ls_phi float 1"<<endl;
  outfile<<"LOOKUP_TABLE default"<<endl;
  for(i = 0;i < mesh_ls.size();i++)
  {
    outfile<<mesh_ls[i]<<endl;
  }
  outfile.close();
}


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#ifndef __IS_STRAIGHT_LINE_DRAWING_HPP__
#define __IS_STRAIGHT_LINE_DRAWING_HPP__

#include <boost/config.hpp>
#include <boost/utility.hpp> //for next and prior
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_comparison.hpp>
#include <boost/property_map.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/planar_detail/bucket_sort.hpp>

#include <algorithm>
#include <vector>
#include <set>

namespace boost
{

  // Return true exactly when the line segments s1 = ((x1,y1), (x2,y2)) and
  // s2 = ((a1,b1), (a2,b2)) intersect in a point other than the endpoints of
  // the line segments. The one exception to this rule is when s1 = s2, in
  // which case false is returned - this is to accomodate multiple edges
  // between the same pair of vertices, which shouldn't invalidate the straight
  // line embedding. A tolerance variable epsilon can also be used, which
  // defines how far away from the endpoints of s1 and s2 we want to consider
  // an intersection.

  bool intersects(double x1, double y1,
                  double x2, double y2,
                  double a1, double b1,
                  double a2, double b2,
                  double epsilon = 0.000001
                  )
  {

    if (x1 - x2 == 0)
      {
        std::swap(x1,a1);
        std::swap(y1,b1);
        std::swap(x2,a2);
        std::swap(y2,b2);
      }

    if (x1 - x2 == 0)
      {
        BOOST_USING_STD_MAX();
        BOOST_USING_STD_MIN();

        //two vertical line segments
        double min_y = min BOOST_PREVENT_MACRO_SUBSTITUTION(y1,y2);
        double max_y = max BOOST_PREVENT_MACRO_SUBSTITUTION(y1,y2);
        double min_b = min BOOST_PREVENT_MACRO_SUBSTITUTION(b1,b2);
        double max_b = max BOOST_PREVENT_MACRO_SUBSTITUTION(b1,b2);
        if ((max_y > max_b && max_b > min_y) ||
            (max_b > max_y && max_y > min_b)
            )
          return true;
        else
          return false;
      }

    double x_diff = x1 - x2;
    double y_diff = y1 - y2;
    double a_diff = a2 - a1;
    double b_diff = b2 - b1;

    double beta_denominator = b_diff - (y_diff/((double)x_diff)) * a_diff;

    if (beta_denominator == 0)
      {
        //parallel lines
        return false;
      }

    double beta = (b2 - y2 - (y_diff/((double)x_diff)) * (a2 - x2)) /
      beta_denominator;
    double alpha = (a2 - x2 - beta*(a_diff))/x_diff;

    double upper_bound = 1 - epsilon;
    double lower_bound = 0 + epsilon;

    return (beta < upper_bound && beta > lower_bound &&
            alpha < upper_bound && alpha > lower_bound);

  }

  template <typename Graph,
            typename GridPositionMap,
            typename VertexIndexMap
            >
  bool is_straight_line_drawing(const Graph& g,
                                GridPositionMap drawing,
                                VertexIndexMap vm
                                )
  {

    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator_t;
    typedef typename graph_traits<Graph>::edge_descriptor edge_t;
    typedef typename graph_traits<Graph>::edge_iterator edge_iterator_t;
    typedef typename graph_traits<Graph>::edges_size_type e_size_t;
    typedef typename graph_traits<Graph>::vertices_size_type v_size_t;

    typedef std::size_t x_coord_t;
    typedef std::size_t y_coord_t;
    typedef boost::tuple<edge_t, x_coord_t, y_coord_t> edge_event_t;
    typedef typename std::vector< edge_event_t > edge_event_queue_t;

    typedef tuple<y_coord_t, y_coord_t, x_coord_t, x_coord_t> active_map_key_t;
    typedef edge_t active_map_value_t;
    typedef std::map< active_map_key_t, active_map_value_t > active_map_t;
    typedef typename active_map_t::iterator active_map_iterator_t;

    edge_event_queue_t edge_event_queue;
    active_map_t active_edges;

    edge_iterator_t ei, ei_end;
    for(tie(ei,ei_end) = edges(g); ei != ei_end; ++ei)
      {
        edge_t e(*ei);
        vertex_t s(source(e,g));
        vertex_t t(target(e,g));
        edge_event_queue.push_back
          (make_tuple(e,
                      static_cast<std::size_t>(drawing[s].x),
                      static_cast<std::size_t>(drawing[s].y)
                      )
           );
        edge_event_queue.push_back
          (make_tuple(e,
                      static_cast<std::size_t>(drawing[t].x),
                      static_cast<std::size_t>(drawing[t].y)
                      )
           );
      }

    // Order by edge_event_queue by first, then second coordinate
    // (bucket_sort is a stable sort.)
    bucket_sort(edge_event_queue.begin(), edge_event_queue.end(),
                property_map_tuple_adaptor<edge_event_t, 2>()
                );

    bucket_sort(edge_event_queue.begin(), edge_event_queue.end(),
                property_map_tuple_adaptor<edge_event_t, 1>()
                );

    typedef typename edge_event_queue_t::iterator event_queue_iterator_t;
    event_queue_iterator_t itr_end = edge_event_queue.end();
    for(event_queue_iterator_t itr = edge_event_queue.begin();
        itr != itr_end; ++itr
        )
      {
        edge_t e(get<0>(*itr));
        vertex_t source_v(source(e,g));
        vertex_t target_v(target(e,g));
        if (drawing[source_v].x > drawing[target_v].x)
          std::swap(source_v, target_v);

        active_map_key_t key(get(drawing, source_v).y,
                             get(drawing, target_v).y,
                             get(drawing, source_v).x,
                             get(drawing, target_v).x
                             );

        active_map_iterator_t a_itr = active_edges.find(key);
        if (a_itr == active_edges.end())
          {
            active_edges[key] = e;
          }
        else
          {
            active_map_iterator_t before, after;
            if (a_itr == active_edges.begin())
              before = active_edges.end();
            else
              before = prior(a_itr);
            after = next(a_itr);

            if (after != active_edges.end() || before != active_edges.end())
              {

                edge_t f = after != active_edges.end() ?
                  after->second : before->second;

                vertex_t e_source(source(e,g));
                vertex_t e_target(target(e,g));
                vertex_t f_source(source(f,g));
                vertex_t f_target(target(f,g));

                if (intersects(drawing[e_source].x,
                               drawing[e_source].y,
                               drawing[e_target].x,
                               drawing[e_target].y,
                               drawing[f_source].x,
                               drawing[f_source].y,
                               drawing[f_target].x,
                               drawing[f_target].y
                               )
                    )
                  return false;
              }

            active_edges.erase(a_itr);

          }
      }

    return true;

  }

  template <typename Graph, typename GridPositionMap>
  bool is_straight_line_drawing(const Graph& g, GridPositionMap drawing)
  {
    return is_straight_line_drawing(g, drawing, get(vertex_index,g));
  }

}

#endif // __IS_STRAIGHT_LINE_DRAWING_HPP__


/*
*   Greedy Search
*   by R. Falque
*   29/11/2018
*/

#ifndef DOWNSAMPLING
#define DOWNSAMPLING

#include <Eigen/Core>
#include <vector>
#include <limits>

#include <cfloat>
#include <iostream>
#include <cmath>
#include <algorithm>

#include "getMinMax.hpp"
#include "farther_sampling.hpp"
#include "nanoflannWrapper.hpp"

using namespace std;

class three_d_point{
public:
	double x;
	double y;
	double z;
};

class point_and_occurences{
public:
	double x;
	double y;
	double z;
	int occurence;
};

inline void voxel_grid_downsampling(Eigen::MatrixXd & in_cloud, double leaf_size, Eigen::MatrixXd & out_cloud)
{
	Eigen::Vector3d min_point, max_point;
	getMinMax(in_cloud, min_point, max_point);

	double inv_leaf_size;
	inv_leaf_size = 1.0/leaf_size;

	Eigen::Vector3i min_box, max_box;
	min_box << floor(min_point(0) * inv_leaf_size ), floor(min_point(1) * inv_leaf_size ), floor(min_point(2) * inv_leaf_size);
	max_box << floor(max_point(0) * inv_leaf_size ), floor(max_point(1) * inv_leaf_size ), floor(max_point(2) * inv_leaf_size);

    Eigen::Vector3i divb, divb_mul;
    divb << max_box(0) - min_box(0) + 1, max_box(1) - min_box(1) + 1, max_box(2) - min_box(2) + 1;
    divb_mul << 1, divb(0), divb(0) * divb(1);

	std::vector < std::vector < std::vector < point_and_occurences> > > voxels;

	voxels.resize(divb(0));
	for (int x_index = 0; x_index < voxels.size(); ++x_index)
	{
		voxels[x_index].resize(divb(1));
		for (int y_index = 0; y_index < voxels[0].size(); ++y_index)
		{
			voxels[x_index][y_index].resize(divb(2));
		}
	}

	// plus assign zeros to voxel_count
	for (int i = 0; i < in_cloud.rows(); ++i)
	{
        int x_index = static_cast<int> ( floor(in_cloud(i, 0) * inv_leaf_size) - min_box(0) );
        int y_index = static_cast<int> ( floor(in_cloud(i, 1) * inv_leaf_size) - min_box(1) );
        int z_index = static_cast<int> ( floor(in_cloud(i, 2) * inv_leaf_size) - min_box(2) );

        voxels[x_index][y_index][z_index].x += in_cloud(i,0);
        voxels[x_index][y_index][z_index].y += in_cloud(i,1);
        voxels[x_index][y_index][z_index].z += in_cloud(i,2);
        voxels[x_index][y_index][z_index].occurence ++;
	}

	std::vector< three_d_point> final_cloud;
	three_d_point temp;
	for (int x_index = 0; x_index < voxels.size(); ++x_index)
	{
		for (int y_index = 0; y_index < voxels[0].size(); ++y_index)
		{
			for (int z_index = 0; z_index < voxels[0][0].size(); ++z_index)
			{
				if (voxels[x_index][y_index][z_index].occurence!= 0)
				{
					temp.x = voxels[x_index][y_index][z_index].x / voxels[x_index][y_index][z_index].occurence;
					temp.y = voxels[x_index][y_index][z_index].y / voxels[x_index][y_index][z_index].occurence;
					temp.z = voxels[x_index][y_index][z_index].z / voxels[x_index][y_index][z_index].occurence;
					final_cloud.push_back(temp);
				}
			}
		}
	}

	out_cloud.resize(final_cloud.size(), 3);
	for (int i = 0; i < final_cloud.size(); ++i)
	{
		out_cloud.row(i) << final_cloud[i].x, final_cloud[i].y, final_cloud[i].z;
	}

};

inline void downsampling(Eigen::MatrixXd & in_cloud,
                         Eigen::MatrixXd & out_cloud,
						 std::vector<int> & in_cloud_samples,
						 double grid_resolution,
						 double leaf_size,
						 bool use_farthest_sampling,
						 bool use_relative_grid)
{
	// overwrite the leaf_size
	if (use_relative_grid) {
		double scale;
		getScale(in_cloud, scale);
		leaf_size = scale / grid_resolution;
	}

	// downsampling
	Eigen::MatrixXd downsampled_cloud;
	if (use_farthest_sampling)
	{
		farthest_sampling_by_sphere(in_cloud, leaf_size/100, downsampled_cloud);
	}
	else
	{
		voxel_grid_downsampling(in_cloud, leaf_size, downsampled_cloud);
	}

	out_cloud.resize(downsampled_cloud.rows(), 3);

	nanoflann_wrapper tree(in_cloud);
	for (int i = 0; i < downsampled_cloud.rows(); ++i)
	{
		std::vector< int > closest_point;
		closest_point = tree.return_k_closest_points(downsampled_cloud.row(i), 1);

		out_cloud.row(i) = in_cloud.row( closest_point[0] );
		in_cloud_samples.push_back(closest_point[0]);
	}

};

#endif


#include <iostream>
#include <list>
#include <vector>
#include <algorithm>
#include <utility>
#include <numeric>
#include <string>
#include <fstream>
#include <map>
#include <sstream>
#include <limits>
#include <bitset>
#include <cmath>
#include <unordered_map>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>

class MemoryGame
{
    public:
        MemoryGame(std::vector<long long> start_num) : starting_numbers(start_num) {}
        std::vector<long long> starting_numbers;
        long long play(const long long end_turn)
        {
            // key - number, value - vector of indices
            std::map<long long, std::vector<long long>> map;
            long long last_spoken;
            long long turn = 1LL;
            std::transform(starting_numbers.begin(), starting_numbers.end(), std::inserter(map, map.end()),
            [&](const long long num)
            {
                std::vector<long long> indices {turn};
                last_spoken = num;
                turn++;
                return std::pair<long long, std::vector<long long>>(num, indices);
            });
            while(turn <= end_turn)
            {
                // if size == 1 speak 0 because number was spoken only once, otherwise speak difference between turns
                last_spoken = (map[last_spoken].size() == 1) ? 0 : map[last_spoken][1] - map[last_spoken][0];
                // save current turn in map for spoken number
                if(map[last_spoken].size() == 2)
                {
                    map[last_spoken][0] = map[last_spoken][1]; // shift to first
                    map[last_spoken].pop_back(); // remove second
                }
                map[last_spoken].push_back(turn);
                turn++;
            }
            return last_spoken;
        }
};

void part1(MemoryGame game)
{
    std::cout << "======\nPart 1\n======\n";
    constexpr long long end = 2020LL;
    std::cout << end << "th number spoken = " << game.play(end) << '\n';
}

void part2(MemoryGame game)
{
    std::cout << "======\nPart 2\n======\n";
    constexpr long long end = 30000000LL;
    std::cout << end <<  "th number spoken = " << game.play(end) << '\n';
}

std::vector<long long> get_input(const std::string file_name)
{
    std::ifstream file(file_name);
    std::string line;
    std::vector<std::string> line_elements;
    std::vector<long long> all_elements;
    if(file.is_open())
    {
        while (std::getline(file, line))
        {
            boost::split(line_elements, line, boost::is_any_of(","), boost::token_compress_on);
            std::transform(line_elements.begin(), line_elements.end(), std::back_inserter(all_elements),
               [](const std::string& str) { return std::stoll(str); });
        }
    }
    file.close();
    return all_elements;
}

int main()
{
    const std::string file_name = "/home/daria/Documents/AoC2020/input/day15.txt";
    std::vector<long long> puzzle_input = get_input(file_name);
    MemoryGame game(puzzle_input);
    part1(game);
    std::cout << '\n';
    part2(game);
    std::cout << '\n';
}