// Boost.GIL (Generic Image Library) - tests
//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef BOOST_GIL_IMAGE_PROCESSING_HOUGH_TRANSFORM_HPP
#define BOOST_GIL_IMAGE_PROCESSING_HOUGH_TRANSFORM_HPP

#include <algorithm>
#include <boost/gil/image_processing/hough_parameter.hpp>
#include <boost/gil/rasterization/circle.hpp>
#include <cmath>
#include <cstddef>
#include <iterator>
#include <vector>

namespace boost { namespace gil {
/// \defgroup HoughTransform
/// \brief A family of shape detectors that are specified by equation
///
/// Hough transform is a method of mapping (voting) an object which can be described by
/// equation to single point in accumulator array (also called parameter space).
/// Each set pixel in edge map votes for every shape it can be part of.
/// Circle and ellipse transforms are very costly to brute force, while
/// non-brute-forcing algorithms tend to gamble on probabilities.

/// \ingroup HoughTransform
/// \brief Vote for best fit of a line in parameter space
///
/// The input must be an edge map with grayscale pixels. Be aware of overflow inside
/// accumulator array. The theta parameter is best computed through factory function
/// provided in hough_parameter.hpp
template <typename InputView, typename OutputView>
void hough_line_transform(const InputView& input_view, const OutputView& accumulator_array,
                          const hough_parameter<double>& theta,
                          const hough_parameter<std::ptrdiff_t>& radius)
{
    std::ptrdiff_t r_lower_bound = radius.start_point;
    std::ptrdiff_t r_upper_bound = r_lower_bound + radius.step_size * (radius.step_count - 1);

    for (std::ptrdiff_t y = 0; y < input_view.height(); ++y)
    {
        for (std::ptrdiff_t x = 0; x < input_view.width(); ++x)
        {
            if (!input_view(x, y)[0])
            {
                continue;
            }

            for (std::size_t theta_index = 0; theta_index < theta.step_count; ++theta_index)
            {
                double theta_current =
                    theta.start_point + theta.step_size * static_cast<double>(theta_index);
                std::ptrdiff_t current_r =
                    std::llround(static_cast<double>(x) * std::cos(theta_current) +
                                 static_cast<double>(y) * std::sin(theta_current));
                if (current_r < r_lower_bound || current_r > r_upper_bound)
                {
                    continue;
                }
                std::size_t r_index = static_cast<std::size_t>(
                    std::llround((current_r - radius.start_point) / radius.step_size));
                // one more safety guard to not get out of bounds
                if (r_index < radius.step_count)
                {
                    accumulator_array(theta_index, r_index)[0] += 1;
                }
            }
        }
    }
}

/// \ingroup HoughTransform
/// \brief Vote for best fit of a circle in parameter space according to rasterizer
///
/// The input must be an edge map with grayscale pixels. Be aware of overflow inside
/// accumulator array. Rasterizer is used to rasterize a circle for voting. The circle
/// then is translated for every origin (x, y) in x y parameter space. For available
/// circle rasterizers, please look at rasterization/circle.hpp
template <typename ImageView, typename ForwardIterator, typename Rasterizer>
void hough_circle_transform_brute(const ImageView& input,
                                  const hough_parameter<std::ptrdiff_t> radius_parameter,
                                  const hough_parameter<std::ptrdiff_t> x_parameter,
                                  const hough_parameter<std::ptrdiff_t>& y_parameter,
                                  ForwardIterator d_first, Rasterizer rasterizer)
{
    for (std::size_t radius_index = 0; radius_index < radius_parameter.step_count; ++radius_index)
    {
        const auto radius = radius_parameter.start_point +
                            radius_parameter.step_size * static_cast<std::ptrdiff_t>(radius_index);
        std::vector<point_t> circle_points(rasterizer.point_count(radius));
        rasterizer(radius, {0, 0}, circle_points.begin());
        // sort by scanline to improve cache coherence for row major images
        std::sort(circle_points.begin(), circle_points.end(),
                  [](const point_t& lhs, const point_t& rhs) { return lhs.y < rhs.y; });
        const auto translate = [](std::vector<point_t>& points, point_t offset) {
            std::transform(points.begin(), points.end(), points.begin(), [offset](point_t point) {
                return point_t(point.x + offset.x, point.y + offset.y);
            });
        };

        // in case somebody passes iterator to likes of std::vector<bool>
        typename std::iterator_traits<ForwardIterator>::reference current_image = *d_first;

        // the algorithm has to traverse over parameter space and look at input, instead
        // of vice versa, as otherwise it will call translate too many times, as input
        // is usually bigger than the coordinate portion of parameter space.
        // This might cause extensive cache misses
        for (std::size_t x_index = 0; x_index < x_parameter.step_count; ++x_index)
        {
            for (std::size_t y_index = 0; y_index < y_parameter.step_count; ++y_index)
            {
                const std::ptrdiff_t x = x_parameter.start_point + x_index * x_parameter.step_size;
                const std::ptrdiff_t y = y_parameter.start_point + y_index * y_parameter.step_size;

                auto translated_circle = circle_points;
                translate(translated_circle, {x, y});
                for (const auto& point : translated_circle)
                {
                    if (input(point))
                    {
                        ++current_image(x_index, y_index)[0];
                    }
                }
            }
        }
        ++d_first;
    }
}

}} // namespace boost::gil

#endif


/**
 *  @file    SparseSolver.hpp
 *  @brief   Solves a finite difference problem.
 *  @author  Francois Roy
 *  @date    12/01/2019
 */
#ifndef SPARSESOLVER_H
#define SPARSESOLVER_H

#include <vector>
#include <math.h>
#include <Eigen/SparseCore>
#include<Eigen/SparseCholesky>
#include "spdlog/spdlog.h"
#include "FDProblem.hpp"
#include <iostream>

namespace numerical {

namespace fdm {

/**
 * This class only computes the 1D diffusion problem with Dirichlet/Neumann
 * boundary conditions and heterogenous diffusion coefficient (for now).
 */
template <typename T>
class SparseSolver {
typedef Eigen::SparseMatrix<T> SpMat;
typedef Eigen::Triplet<T> Trip;
typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec;
typedef std::vector<Eigen::Matrix<T, 3, 1>> Coord;
protected:
  SpMat m_A;
  FDProblem<T>* m_problem;
  int m_n, m_n_x, m_n_y, m_n_z, m_n_t, m_dim;
  T m_dt, m_dx, m_dy, m_dz, m_theta;
  Vec m_alpha, m_x, m_y, m_z, m_b, m_u, m_f, m_f_n;
public:
  SparseSolver(FDProblem<T>* problem)
    : m_problem(problem)
    {
        m_n = m_problem->n();
        m_dim = m_problem->dim();
        m_A = SpMat(m_n, m_n);
        m_b = Vec::Zero(m_n);
        m_u = m_problem->u_0();
        T dx, dy, dz, dt, theta;
        theta = m_problem->theta();
        m_n_x = m_problem->n_x();
        m_n_y = m_problem->n_y();
        m_n_z = m_problem->n_z();
        m_n_t = m_problem->n_t();
        m_dt = m_problem->dt();
        m_dx = m_problem->dx()[0];
        m_dy = 1.0;
        m_dz = 1.0;
        if(m_dim != 1){
            m_dy = m_problem->dx()[1];
        }
        if(m_dim == 3){
            m_dz = m_problem->dx()[2];
        }
        m_theta = m_problem->theta();
        m_alpha = Vec::Zero(m_n);
        m_f = Vec::Zero(m_n);
        m_f_n = Vec::Zero(m_n);
        m_x = Vec::Zero(m_n);
        m_y = Vec::Zero(m_n);
        m_z = Vec::Zero(m_n);
        const Coord& coords = m_problem->coordinates();
        // define x, y, z, and alpha
        for(int i=0; i<m_n; i++) {
            m_x[i] = coords[i][0];
            m_y[i] = coords[i][1];
            m_z[i] = coords[i][2];
            m_alpha[i] = m_problem->alpha(coords[i], 0.0);
        }
  }
  ~SparseSolver(){
  }
  /**
  * Assembles the sparse coefficient matrix \f$\mathbf{A}\f$. The matrix is
  * defined by its diagonals. The number of diagonals is related to the
  * number of direct neighbors for interior mesh nodes. In 1D, the matrix has
  * 3 non-zero diagonals, in 2D it has 5, and in 3D, 7.
  *
  * The diagonals of the matrix \f$\mathbf{A}\f$ is are filled by vectorization
  * of the loops for efficiency.
  */
  virtual void assemble_a(){
    std::vector<Trip> trp;
    Vec lower_a, upper_a, lower_b, upper_b;
    Vec diagonal = Vec::Constant(m_n, 1.0);
    Vec lower = Vec::Zero(m_n - 1);
    Vec upper = Vec::Zero(m_n - 1);
    if (m_dim > 1){  // 2D and 3D
        // spdlog::info("2D: n - (nx+1) = {}", m_n - (m_n_x + 1));
        lower_a = Vec::Zero(m_n - (m_n_x + 1));
        upper_a = Vec::Zero(m_n - (m_n_x + 1));
    }
    if (m_dim > 2){  // 3D
        lower_b = Vec::Zero(m_n - (m_n_x + 1)*(m_n_y + 1));
        upper_b = Vec::Zero(m_n - (m_n_x + 1)*(m_n_y + 1));
    }
    // The loops are vectorized for efficiency -- see bench/performances
    if (m_dim == 1){
        // spdlog::info("1D");
        T d = m_dt/m_dx/m_dx*m_theta/2.0;
        spdlog::debug("dx: {}, dt: {}, theta: {}, alpha: {}",
                      m_dx, m_dt, m_theta, m_alpha[0]);
        // Fx must be smaller than 0.5 for explicit and Crank-Nicolson schemes
        spdlog::debug("Fx: {}", m_dt/m_dx/m_dx*m_alpha[0]);
        diagonal[0] = 0.0;
        diagonal[m_n - 1] = 0.0;
        diagonal.segment(1, m_n-2) += d * m_alpha.segment(2, m_n-2);
        diagonal.segment(1, m_n-2) += d * 2.0 * m_alpha.segment(1, m_n-2);
        diagonal.segment(1, m_n-2) += d * m_alpha.segment(0, m_n-2);
        lower.segment(0, (m_n-1)-1) += -d * m_alpha.segment(1, m_n-2);
        lower.segment(0, (m_n-1)-1) += -d * m_alpha.segment(0, m_n-2);
        upper.segment(1, (m_n-1)-1) += -d * m_alpha.segment(2, m_n-2);
        upper.segment(1, (m_n-1)-1) += -d * m_alpha.segment(1, m_n-2);
        // boundary conditions
        // TODO use: m_problem->coeffs_bc(Vec& dia, Vec& lower, Vec& upper,
        //                                Vec& lower_a, Vec& upper_a,
        //                                Vec& lower_b, Vec& upper_b, T t=0.0)
        // instead.
        // std::cout << m_problem->bc_type(0) << std::endl;
        if(m_problem->bc_type(0) == 0){ // left Dirichlet
            diagonal[0] = 1.0;
            upper[0] = 0.0;
          } else{  // left Neumann --> scaled by 1/2
            // here we assume that the diffusion coefficient outside
            // of the boundary is equal to alpha[0]
            diagonal[0] = 0.5 + d*(0.5*m_alpha[0]+m_alpha[0]+0.5*m_alpha[1]);
            upper[0] = -d*(m_alpha[0]+m_alpha[1]);
          }
        if(m_problem->bc_type(1) == 0){ // right Dirichlet
            diagonal[m_n-1] = 1.0;
            lower[(m_n - 1)-1] = 0.0;
          } else {  // right Neumann --> scaled by 1/2
            // here we assume that the diffusion coefficient outside
            // of the boundary is equal to alpha[n-1]
            diagonal[m_n-1] = 0.5 + d*(0.5*m_alpha[m_n-2]+m_alpha[m_n-1]+
                  0.5*m_alpha[m_n-1]);
            lower[(m_n - 1)-1] = -d*(m_alpha[m_n-1]+m_alpha[m_n-2]);
          }

        // insert diagonals in A
        for(int i=0; i<m_n; i++){
            trp.push_back(Trip(i,i,diagonal[i]));
        }
        for(int i=1; i<m_n; i++){
            trp.push_back(Trip(i,i-1,lower[i-1]));
        }
        for(int i=0; i<m_n - 1; i++){
            trp.push_back(Trip(i,i+1,upper[i]));
        }
        // create sparse matrix
        m_A.setFromTriplets(trp.begin(), trp.end());
    } else if (m_dim == 2){
        // spdlog::info("2D");
        // TODO
    } else {  // 3D
        // spdlog::info("3D");
        // TODO
    }
  }
  /**
  * Assembles the RHS vector \f$\mathbf{b}\f$.
  *
  * \f[
  *    b_i = u_i^n + F\left(1-\Theta\right)u_{i+1}^n-2u_i^n+u_{i-1}^n +
  *        \Delta t \Theta f_i^{n+1} + \Delta t \left(1-\Theta\right)f_i^n
  * \f]
  *
  * using vectorization we get:
  *
  * \f[
  *    b[1:n_x-1] = u_n[1:n_x-1] + \left(1-\Theta\right)F
  *        \left(u_n[2:n_x]-2u_n[1:n_x-1]+u_n[0:n_x-2]\right) +
  *        \Theta\Delta t f[1:n_x-1](n+1) +
  *        \left(1-\Theta\right)\Delta t f[1:n_x-1](n)
  * \f]
  *
  */
  virtual void assemble_b(T t, Vec& u_n){
      const Coord& coords = m_problem->coordinates();
      // TODO define the diffusion coefficient and source term only if they
      // depend on time
      for(int i=0; i<m_n; i++) {
          m_alpha[i] = m_problem->alpha(coords[i], t);
          m_f_n[i] = m_problem->source(coords[i], t);
          m_f[i] = m_problem->source(coords[i], t + m_dt);
      }
      if (m_dim == 1){
          // spdlog::info("1D");
          T d = m_dt/m_dx/m_dx*(1.0 - m_theta) / 2.0;
          // spdlog::info("d: {}", d);
          m_b.segment(1, m_n-2) = u_n.segment(1, m_n-2);
          m_b.segment(1, m_n-2) += d * ((m_alpha.segment(2, m_n-2) +
            m_alpha.segment(1, m_n-2)).array() * (u_n.segment(2, m_n-2) -
            u_n.segment(1, m_n-2)).array()).matrix();
          m_b.segment(1, m_n-2) -= d * ((m_alpha.segment(1, m_n-2) +
            m_alpha.segment(0, m_n-2)).array() * (u_n.segment(1, m_n-2) -
            u_n.segment(0, m_n-2)).array()).matrix();
          m_b.segment(1, m_n-2) += m_dt * m_theta * m_f.segment(1, m_n-2);
          m_b.segment(1, m_n-2) += m_dt * (1.0 -m_theta) *
            m_f_n.segment(1, m_n-2);
          // Boundary conditions
          m_problem->rhs_bc(m_b, u_n, m_alpha, m_f_n, m_f, m_dx, m_dy, m_dz,
                            m_dt, m_theta, t);
      } else if (m_dim == 2){
        // spdlog::info("2D");
        // TODO
      } else {  // 3D
        // spdlog::info("3D");
        // TODO
      }
  }
  /*
  * Solve the time dependent problem.
  * TODO: Create a VTKFile class to store the solution in a vtu file at each
  * time steps
  */
  virtual T solve(){
      // Set initial condition
      Vec u_n = m_problem->u_0();
      Vec u = Vec::Zero(m_problem->n());
      int n_t = m_problem->n_t();
      T t, l2norm=0.0;
      Vec t_list = m_problem->t();
      assemble_a();
      // std::cout << m_A << std::endl;
      Eigen::SimplicialLDLT<SpMat> solver;
      // Time loop
      T e=0.0;
      for(int n=0; n<n_t; n++){
        t = t_list[n];
        assemble_b(t, u_n);
        // std::cout << m_b << std::endl;
        // Solve
        solver.compute(m_A);
        if(solver.info()!=Eigen::Success) {
            spdlog::error("decomposition failed");
            spdlog::error("{}", solver.info());
            return 999.0;
        }
        u = solver.solve(m_b);
        if(solver.info()!=Eigen::Success) {
            spdlog::error("solving failed");
            spdlog::error("{}", solver.info());
            return 999.0;
        }
        // spdlog::info("b[1]: {}, b[n-2]: {}", m_b[1], m_b[m_n-2]);
        // TODO Save result in file here.
        const Coord& coords = m_problem->coordinates();
        spdlog::debug("SOLUTION at time {:03.6f}:", t+m_dt);
        for(int i=0;i<m_n;i++){
            e += pow(m_problem->reference(coords[i], t+m_dt)-u[i], 2.0);
            spdlog::debug(
              "coord: ({}, {}, {}), exact: {:03.6f}, computed: {:03.6f}",
              coords[i][0], coords[i][1], coords[i][2],
              m_problem->reference(coords[i], t+m_dt), u[i]);
        }
        u_n = u;
      }
      l2norm += pow(m_dx*m_dt*e, 0.5);
      // spdlog::info("L2-norm: {}", l2norm);
      return l2norm;
  }
};

}  // namespace fdm

}  // namespace numerical

#endif  // SPARSESOLVER_H


#ifndef __PROBABILITY_DISTRIBUTIONS__NORMAL_IMPL_HPP__
#define __PROBABILITY_DISTRIBUTIONS__NORMAL_IMPL_HPP__

#include "normal.hpp"

#include "const_slice.hpp"
#include "slice.hpp"

#include <boost/random/normal_distribution.hpp>
#include <cmath>

namespace ProbabilityDistributions {
  template <class D, class W, class T>
  Normal<D,W,T>::Normal(T mu, T sigma):
    fixed_mu_(false),
    fixed_sigma_(false) {
      set_mu(mu);
      set_sigma(sigma);
    }

  template <class D, class W, class T>
  template <class RNG>
  void Normal<D,W,T>::sample(MA::Array<D>& samples, size_t n_samples, RNG& rng)
  const {
    MA::Size::SizeType size(2);
    size[0] = n_samples;
    size[1] = 1;
    samples.resize(size);

    boost::random::normal_distribution<T> dist(mu_, sigma_);

    D* ptr = samples.get_pointer();

    for (size_t j = 0; j < n_samples; j++)
      ptr[j] = dist(rng);
  }

  template <class D, class W, class T>
  T Normal<D,W,T>::log_likelihood(MA::ConstArray<D> const& data,
      MA::ConstArray<W> const& weight) const {
    check_data_and_weight(data, weight);

    D const* ptr = data.get_pointer();

    T ll = 0;
    T sigma_likelihood = std::log(2*M_PI*sigma_*sigma_)/2;

    for (size_t j = 0; j < data.total_size(); j++) {
      T w = weight(j);
      T s = ptr[j];
      T local_likelihood = s - mu_;
      local_likelihood *= local_likelihood;
      local_likelihood *= inv_sigma2_;
      local_likelihood += sigma_likelihood;
      ll -= w * local_likelihood;
    }

    return ll;
  }

  template <class D, class W, class T>
  void Normal<D,W,T>::MLE(MA::ConstArray<D> const& data,
      MA::ConstArray<W> const& weight, std::vector<size_t> const& indexes) {
    check_data_and_weight(data, weight);

    D const* ptr = data.get_pointer();

    T sum_0 = 0, sum_1 = 0, sum_2 = 0;
    for (size_t j = 0; j < data.total_size(); j++) {
      T w = weight(j);
      T s = ptr[j];
      sum_0 += w;
      sum_1 += w*s;
      sum_2 += w*s*s;
    }

    if (!fixed_mu_)
      set_mu(sum_1/sum_0);
    if (!fixed_sigma_)
      set_sigma(std::sqrt((sum_2 - 2*mu_*sum_1 + mu_*mu_*sum_0)/sum_0));
  }

  template <class D, class W, class T>
  void Normal<D,W,T>::check_data_and_weight(MA::ConstArray<D> const& data,
      MA::ConstArray<W> const& weight) const {
    assert(data.size().size() == 2);
    assert(data.size()[0] > 0);
    assert(data.size()[1] == 1);
    assert(weight.size().size() == 1);
    assert(weight.size()[0] == data.size()[0]);
  }
};

#endif


#include <iostream>
#include <vector>
#include <unordered_map>

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

#include <dionysus/simplex.h>
#include <dionysus/fields/zp.h>
#include <dionysus/fields/z2.h>
#include <dionysus/distances.h>
#include <dionysus/rips.h>
#include <dionysus/zigzag-persistence.h>
namespace d = dionysus;

#include <dionysus/dlog/progress.h>

#include <opts/opts.h>

#include <common.h>     // read_points()

typedef         std::vector<float>                                      Point;
typedef         std::vector<Point>                                      PointContainer;

typedef         d::PairwiseDistances<PointContainer,
                                     d::L2Distance<Point>>              PairDistances;
typedef         PairDistances::DistanceType                             DistanceType;
typedef         PairDistances::IndexType                                Vertex;

typedef         d::Rips<PairDistances>                                  Generator;
typedef         Generator::Simplex                                      Simplex;
typedef         std::set<Simplex>                                       SimplexSet;

typedef         std::vector<Vertex>                                     VertexVector;
typedef         std::vector<DistanceType>                               EpsilonVector;
typedef         std::tuple<Vertex,Vertex>                               Edge;
typedef         std::vector<Edge>                                       EdgeVector;

//typedef         d::Z2Field                                              K;
typedef         d::ZpField<>                                            K;
typedef         d::Simplex<>                                            Simplex;
typedef         d::ZigzagPersistence<K>                                 Persistence;
typedef         typename Persistence::Index                             Index;

typedef         std::unordered_map<Simplex, Index>                      Complex;
typedef         d::ChainEntry<K, Simplex>                               SimplexChainEntry;
typedef         d::ChainEntry<K, Index>                                 ChainEntry;

// debug
typedef         std::unordered_map<Index, Simplex>                      RComplex;

// Information we need to know when a class dies
struct      BirthInfo
{
    typedef         short unsigned                                      Dimension;

                    BirthInfo(DistanceType dist = DistanceType(), Dimension dim = Dimension()):
                        distance(dist), dimension(dim)              {}
    DistanceType    distance;
    Dimension       dimension;
};

typedef         std::unordered_map<Index, BirthInfo>                    BirthMap;

int main(int argc, char** argv)
{
    using opts::Options;
    using opts::Option;
    using opts::PosOption;

    short unsigned          skeleton = 2;
    DistanceType            multiplier = 6;
    short unsigned          p = 11;
    std::string             infilename, diagram_name;
    bool                    help;

    Options ops;
    ops
        >> Option('s', "skeleton",      skeleton,           "dimension of the Rips complex we want to compute")
        >> Option('m', "multiplier",    multiplier,         "multiplier for epsilon (distance to the next maxmin point)")
        >> Option('p', "prime",         p,                  "prime for arithmetic")
        >> Option('h', "help",          help,               "show help message")
    ;

    if (!ops.parse(argc,argv) || !(ops >> PosOption(infilename)) || !(ops >> PosOption(diagram_name)))
    {
        std::cout << "Usage: " << argv[0] << " input-points diagram.out" << std::endl;
        std::cout << ops;
        return 1;
    }

    PointContainer          points;
    read_points(infilename, points);

    std::ofstream   dgm_out(diagram_name);
    std::ostream&   out = dgm_out;

    // Construct distances and Rips generator
    PairDistances           distances(points);
    Generator               rips(distances);
    Generator::Evaluator    size(distances);

    // Order vertices and epsilons (in maxmin fashion)
    VertexVector        vertices;
    EpsilonVector       epsilons;
    EdgeVector          edges;
    DistanceType        inf     = std::numeric_limits<DistanceType>::infinity();

    {
        EpsilonVector   dist(distances.size(), inf);

        vertices.push_back(distances.begin());
        //epsilons.push_back(inf);
        while (vertices.size() < distances.size())
        {
            for (Vertex v = distances.begin(); v != distances.end(); ++v)
                dist[v] = std::min(dist[v], distances(v, vertices.back()));
            auto max = std::max_element(dist.begin(), dist.end());
            vertices.push_back(max - dist.begin());
            epsilons.push_back(*max);
        }
        epsilons.push_back(0);
    }

    // Generate and sort all the edges
    for (unsigned i = 0; i != vertices.size(); ++i)
        for (unsigned j = i+1; j != vertices.size(); ++j)
        {
            Vertex u = vertices[i];
            Vertex v = vertices[j];
            if (distances(u,v) <= multiplier*epsilons[j-1])
                edges.emplace_back(u,v);
        }
    std::sort(edges.begin(), edges.end(),
              [&distances](const Edge& e1, const Edge& e2)
              { return distances(std::get<0>(e1), std::get<1>(e1)) < distances(std::get<0>(e2), std::get<1>(e2)); });

    // Construct zigzag
    //K               k;
    K               k(p);
    Persistence     persistence(k);
    Complex         simplices;
#ifdef DIONYSUS_ZIGZAG_DEBUG
    RComplex        rsimplices;
#endif

    // Insert vertices
    Index       op   = 0;
    Index       cell = 0;
    BirthMap    births;
    for (auto v : vertices)
    {
        // Add a vertex
        Simplex s = {v};

        // We don't actually need to transform the boundary here,
        // since it's empty anyway, but we keep it for the sake of completeness
        Index pair = persistence.add(s.boundary(persistence.field()) |
                                                ba::transformed([&simplices](const SimplexChainEntry& e)
                                                { return ChainEntry(e.element(), simplices.find(e.index())->second); }));

#ifdef DIONYSUS_ZIGZAG_DEBUG
        rsimplices.emplace(cell, s);
        persistence.check_boundaries([&simplices](const Simplex& s) { return simplices[s]; },
                                     [&rsimplices](Index i)         { return rsimplices.find(i)->second; });
#endif

        births[op++] = BirthInfo(0,0);                  // record the birth
        simplices.emplace(std::move(s), cell++);        // record the cell id
    }

    // Process vertices
    dlog::progress progress(vertices.size());
    unsigned    ce = 0;         // index of the current one past last edge in the complex
    SimplexSet  cofaces;        // record the cofaces of all the simplices that need to be removed and reinserted
    for (unsigned stage = 0; stage != vertices.size() - 1; ++stage)
    {
        unsigned i = vertices.size() - 1 - stage;

        /* Increase epsilon */
        cofaces.clear();

        // Add anything else that needs to be inserted into the complex
        while (ce < edges.size())
        {
            Vertex u,v;
            std::tie(u,v) = edges[ce];
            if (distances(u,v) <= multiplier*epsilons[i-1])
                ++ce;
            else
                break;
            //std::cout << "Adding cofaces of " << u << ' ' << v << std::endl;
            rips.edge_cofaces(u, v,
                              skeleton,
                              multiplier*epsilons[i-1],
                              [&cofaces](Simplex&& s) { cofaces.insert(s); },
                              vertices.begin(),
                              vertices.begin() + i + 1);
        }

        // Insert all the cofaces
        for (auto& s : cofaces)
        {
            //std::cout << "Inserting: " << s << std::endl;

            Index pair = persistence.add(s.boundary(persistence.field()) |
                                                    ba::transformed([&simplices](const SimplexChainEntry& e)
                                                    { return ChainEntry(e.element(), simplices.find(e.index())->second); }));
            simplices.emplace(std::move(s), cell);      // record the cell id
#ifdef DIONYSUS_ZIGZAG_DEBUG
            rsimplices.emplace(cell, s);
            persistence.check_boundaries([&simplices](const Simplex& s) { return simplices[s]; },
                                         [&rsimplices](Index i)         { return rsimplices.find(i)->second; });
#endif
            ++cell;

            if (pair == Persistence::unpaired())
                births[op++] = BirthInfo(epsilons[i-1],s.dimension());              // record the birth
            else
            {
                const BirthInfo& birth = births[pair];
                if ((birth.distance - epsilons[i-1]) != 0 && birth.dimension < skeleton)
                    out << birth.dimension << " " << birth.distance << " " << epsilons[i-1] << std::endl;
                births.erase(pair);
                ++op;
            }
        }

        /* Remove the vertex */
        //std::cout << "Removing vertex: " << vertices[i] << std::endl;
        cofaces.clear();
        rips.vertex_cofaces(vertices[i],
                            skeleton,
                            multiplier*epsilons[i-1],
                            [&cofaces](Simplex&& s) { cofaces.insert(s); },
                            vertices.begin(),
                            vertices.begin() + i + 1);
        //std::cout << "Total cofaces: " << cofaces.size() << std::endl;

        for (auto& s : cofaces | ba::reversed)
        {
            //std::cout << "Removing: " << s << std::endl;
            Complex::const_iterator  it    = simplices.find(s);
            Index                    c     = it->second;
            simplices.erase(it);

            Index pair  = persistence.remove(c);
#ifdef DIONYSUS_ZIGZAG_DEBUG
            rsimplices.erase(c);
            persistence.check_boundaries([&simplices](const Simplex& s) { return simplices[s]; },
                                         [&rsimplices](Index i)         { return rsimplices.find(i)->second; });
#endif

            if (pair == Persistence::unpaired())
                births[op++] = BirthInfo(epsilons[i-1],s.dimension() - 1);          // record the birth
            else
            {
                const BirthInfo& birth = births[pair];
                if ((birth.distance - epsilons[i-1]) != 0 && birth.dimension < skeleton)
                    out << birth.dimension << " " << birth.distance << " " << epsilons[i-1] << std::endl;
                births.erase(pair);
                ++op;
            }
        }

        ++progress;
    }

    // Remove the last vertex
    Index pair = persistence.remove(0);
    simplices.erase((Complex::const_iterator) simplices.begin());     // TODO: add an assertion that the complex has only 1 simplex
#ifdef DIONYSUS_ZIGZAG_DEBUG
    rsimplices.erase(0);
    persistence.check_boundaries([&simplices](const Simplex& s) { return simplices[s]; },
                                 [&rsimplices](Index i)         { return rsimplices.find(i)->second; });
#endif

    const BirthInfo& birth = births[pair];
    out << birth.dimension << " " << birth.distance << " " << epsilons[0] << std::endl;
    ++progress;

    std::cout << "Finished" << std::endl;
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_UNITS_STATIC_RATIONAL_HPP
#define BOOST_UNITS_STATIC_RATIONAL_HPP

#include <boost/integer/common_factor_ct.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/arithmetic.hpp>

#ifdef __BORLANDC__
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/identity.hpp>
#endif

#include <boost/units/config.hpp>
#include <boost/units/operators.hpp>

/// \file
/// \brief Compile-time rational numbers and operators.

namespace boost {

namespace units {

namespace detail {

struct static_rational_tag {};

}

typedef long   integer_type;

/// Compile time absolute value.
template<integer_type Value>
struct static_abs
{
    BOOST_STATIC_CONSTANT(integer_type,value = Value < 0 ? -Value : Value);
};

// Compile time rational number.
/**
This is an implementation of a compile time rational number, where @c static_rational<N,D> represents
a rational number with numerator @c N and denominator @c D. Because of the potential for ambiguity arising
from multiple equivalent values of @c static_rational (e.g. @c static_rational<6,2>==static_rational<3>),
static rationals should always be accessed through @c static_rational<N,D>::type. Template specialization
prevents instantiation of zero denominators (i.e. @c static_rational<N,0>). The following compile-time
arithmetic operators are provided for static_rational variables only (no operators are defined between
long and static_rational):
    - @c mpl::negate
    - @c mpl::plus
    - @c mpl::minus
    - @c mpl::times
    - @c mpl::divides

Neither @c static_power nor @c static_root are defined for @c static_rational. This is because template types
may not be floating point values, while powers and roots of rational numbers can produce floating point
values.
*/
#ifdef __BORLANDC__

template<integer_type X>
struct make_integral_c {
    typedef boost::mpl::integral_c<integer_type, X> type;
};

template<integer_type N,integer_type D = 1>
class static_rational
{
    public:

        typedef static_rational this_type;

        typedef boost::mpl::integral_c<integer_type, N> N_type;
        typedef boost::mpl::integral_c<integer_type, D> D_type;

        typedef typename make_integral_c<
            (::boost::integer::static_gcd<
                ::boost::units::static_abs<N>::value,
                ::boost::units::static_abs<D>::value
            >::value)>::type gcd_type;
        typedef typename boost::mpl::eval_if<
            boost::mpl::less<
                D_type,
                boost::mpl::integral_c<integer_type, 0>
            >,
            boost::mpl::negate<gcd_type>,
            gcd_type
        >::type den_type;

    public:
        // for mpl arithmetic support
        typedef detail::static_rational_tag tag;

        BOOST_STATIC_CONSTANT(integer_type, Numerator =
            (::boost::mpl::divides<N_type, den_type>::value));
        BOOST_STATIC_CONSTANT(integer_type, Denominator =
            (::boost::mpl::divides<D_type, den_type>::value));

        /// INTERNAL ONLY
        typedef static_rational<N,D>    this_type;

        /// static_rational<N,D> reduced by GCD
        typedef static_rational<
            (::boost::mpl::divides<N_type, den_type>::value),
            (::boost::mpl::divides<D_type, den_type>::value)
        >  type;

        static BOOST_CONSTEXPR integer_type numerator()     { return Numerator; }
        static BOOST_CONSTEXPR integer_type denominator()   { return Denominator; }

        // INTERNAL ONLY
        BOOST_CONSTEXPR static_rational() { }
        //~static_rational() { }
};
#else
template<integer_type N,integer_type D = 1>
class static_rational
{
    private:

        BOOST_STATIC_CONSTEXPR integer_type nabs = static_abs<N>::value,
                                            dabs = static_abs<D>::value;

        /// greatest common divisor of N and D
        // need cast to signed because static_gcd returns unsigned long
        BOOST_STATIC_CONSTEXPR integer_type den =
            static_cast<integer_type>(boost::integer::static_gcd<nabs,dabs>::value) * ((D < 0) ? -1 : 1);

    public:
        // for mpl arithmetic support
        typedef detail::static_rational_tag tag;

        BOOST_STATIC_CONSTEXPR integer_type Numerator = N/den,
            Denominator = D/den;

        /// INTERNAL ONLY
        typedef static_rational<N,D>    this_type;

        /// static_rational<N,D> reduced by GCD
        typedef static_rational<Numerator,Denominator>  type;

        static BOOST_CONSTEXPR integer_type numerator()     { return Numerator; }
        static BOOST_CONSTEXPR integer_type denominator()   { return Denominator; }

        // INTERNAL ONLY
        BOOST_CONSTEXPR static_rational() { }
        //~static_rational() { }
};
#endif

}

}

#if BOOST_UNITS_HAS_BOOST_TYPEOF

#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()

BOOST_TYPEOF_REGISTER_TEMPLATE(boost::units::static_rational, (long)(long))

#endif

namespace boost {

namespace units {

// prohibit zero denominator
template<integer_type N> class static_rational<N,0>;

/// get decimal value of @c static_rational
template<class T,integer_type N,integer_type D>
inline BOOST_CONSTEXPR typename divide_typeof_helper<T,T>::type
value(const static_rational<N,D>&)
{
    return T(N)/T(D);
}

} // namespace units

#ifndef BOOST_UNITS_DOXYGEN

namespace mpl {

#ifdef __BORLANDC__

template<>
struct plus_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            ::boost::mpl::plus<
                boost::mpl::times<typename T0::N_type, typename T1::D_type>,
                boost::mpl::times<typename T1::N_type, typename T0::D_type>
            >::value,
            ::boost::mpl::times<typename T0::D_type, typename T1::D_type>::value
        >::type type;
    };
};

template<>
struct minus_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            ::boost::mpl::minus<
                boost::mpl::times<typename T0::N_type, typename T1::D_type>,
                boost::mpl::times<typename T1::N_type, typename T0::D_type>
            >::value,
            ::boost::mpl::times<typename T0::D_type, typename T1::D_type>::value
        >::type type;
    };
};

template<>
struct times_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            ::boost::mpl::times<typename T0::N_type, typename T1::N_type>::value,
            ::boost::mpl::times<typename T0::D_type, typename T1::D_type>::value
        >::type type;
    };
};

template<>
struct divides_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            ::boost::mpl::times<typename T0::N_type, typename T1::D_type>::value,
            ::boost::mpl::times<typename T0::D_type, typename T1::N_type>::value
        >::type type;
    };
};

template<>
struct negate_impl<boost::units::detail::static_rational_tag>
{
    template<class T0>
    struct apply {
        typedef typename boost::units::static_rational<
            ::boost::mpl::negate<typename T0::N_type>::value,
            ::boost::mpl::identity<T0>::type::Denominator
        >::type type;
    };
};

template<>
struct less_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply
    {
        typedef mpl::bool_<((mpl::minus<T0, T1>::type::Numerator) < 0)> type;
    };
};

#else

template<>
struct plus_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            T0::Numerator*T1::Denominator+T1::Numerator*T0::Denominator,
            T0::Denominator*T1::Denominator
        >::type type;
    };
};

template<>
struct minus_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            T0::Numerator*T1::Denominator-T1::Numerator*T0::Denominator,
            T0::Denominator*T1::Denominator
        >::type type;
    };
};

template<>
struct times_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            T0::Numerator*T1::Numerator,
            T0::Denominator*T1::Denominator
        >::type type;
    };
};

template<>
struct divides_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply {
        typedef typename boost::units::static_rational<
            T0::Numerator*T1::Denominator,
            T0::Denominator*T1::Numerator
        >::type type;
    };
};

template<>
struct negate_impl<boost::units::detail::static_rational_tag>
{
    template<class T0>
    struct apply {
        typedef typename boost::units::static_rational<-T0::Numerator,T0::Denominator>::type type;
    };
};

template<>
struct less_impl<boost::units::detail::static_rational_tag, boost::units::detail::static_rational_tag>
{
    template<class T0, class T1>
    struct apply
    {
        typedef mpl::bool_<((mpl::minus<T0, T1>::type::Numerator) < 0)> type;
    };
};

#endif

}

#endif

} // namespace boost

#endif // BOOST_UNITS_STATIC_RATIONAL_HPP


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include "DavidsonOperator.hpp"
#include "MatrixFreeOperator.hpp"

// constructors
DavidsonOperator::DavidsonOperator(int size, double eps, bool odiag, bool reorder)
{
    _size = size;
    _odiag = odiag;
    _sparsity = eps;
    _reorder = reorder;

    diag_el = Eigen::VectorXd(_size);
    for (int i=0; i<_size;i++){
        if (_odiag) diag_el(i) = static_cast<double> (i+1);
        else diag_el(i) = static_cast<double> (1. + (std::rand() %1000 ) / 10.);
    }

    if (_reorder)
        _order_index = DavidsonOperator::_sort_index(diag_el);
}

Eigen::ArrayXd DavidsonOperator::_sort_index(Eigen::VectorXd& V) const
{
    Eigen::ArrayXd idx = Eigen::ArrayXd::LinSpaced(V.rows(),0,V.rows()-1);
    std::sort(idx.data(),idx.data()+idx.size(),
              [&](int i1, int i2){return V[i1]<V[i2];});
    return idx;
}

Eigen::VectorXd DavidsonOperator::reorder_col(Eigen::VectorXd& col) const
{
    Eigen::VectorXd out = Eigen::VectorXd::Zero(_size,1);
    for (int j=0; j < _size; j++)
        out(j) = col(_order_index(j));
    return out;
}

//  get a col of the operator
Eigen::VectorXd DavidsonOperator::col(int index_orig) const
{
    int index = index_orig;
    if (_reorder)
        index = _order_index(index_orig);
    Eigen::VectorXd col_out = Eigen::VectorXd::Zero(_size,1);
    for (int j=0; j < _size; j++)
    {
        if (j==index) {
            col_out(j) =  diag_el(j);
        }
        else{
            col_out(j) = _sparsity / std::pow( static_cast<double>(j-index),2) ;
        }
    }

    if (_reorder)
        col_out = DavidsonOperator::reorder_col(col_out);

    return col_out;

}


#ifndef DZNL_QUADRATIC_LINE_SEARCHER_HPP_INCLUDED
#define DZNL_QUADRATIC_LINE_SEARCHER_HPP_INCLUDED

// C++ standard library headers
#include <cstddef>    // for std::size_t
#include <functional> // for std::function
#include <stdexcept>  // for std::invalid_argument

// Eigen linear algebra library headers
#include <Eigen/Core> // for Eigen::Matrix

namespace dznl {

    template <typename T>
    class QuadraticLineSearcher {
    private: // ========================================== INTERNAL TYPE ALIASES
        typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXT;

    private: // =============================================== MEMBER VARIABLES
        const std::size_t n;
        const std::function<T(const T *)> f;

        const VectorXT x0;
        VectorXT xt;
        const VectorXT dx;

        const T f0;

        T best_objective_value;
        T best_step_size;

    public: // ==================================================== CONSTRUCTORS
        explicit QuadraticLineSearcher(
                const std::function<T(const T *)> &objective_function,
                const VectorXT &initial_point,
                const VectorXT &step_direction)
                : n(static_cast<std::size_t>(initial_point.size())),
                  f(objective_function),
                  x0(initial_point),
                  xt(n),
                  dx(step_direction),
                  f0(objective_function(initial_point.data())),
                  best_objective_value(f0),
                  best_step_size(0) {
            if (initial_point.size() != step_direction.size()) {
                throw std::invalid_argument(
                        "dznl::QuadraticLineSearcher constructor received "
                        "initial point and step direction vectors of "
                        "different sizes");
            }
        }

    public: // ======================================================= ACCESSORS
        T get_best_objective_value() { return best_objective_value; }

        T get_best_step_size() { return best_step_size; }

    private: // ===================================== LINE SEARCH HELPER METHODS
        T evaluate_objective_function(const T &step_size,
                                      bool *changed = nullptr) {
            xt = x0 + step_size * dx;
            if (x0 == xt) {
                if (changed != nullptr) { *changed = false; }
                return f0;
            } else {
                if (changed != nullptr) { *changed = true; }
            }
            const T objective_value = f(xt.data());
            if (objective_value < best_objective_value) {
                best_objective_value = objective_value;
                best_step_size = step_size;
            }
            return objective_value;
        }

    public: // ============================================= LINE SEARCH METHODS
        void search(T step_size, std::size_t max_increases = 4) {
            T f1 = evaluate_objective_function(step_size);
            T f2;
            if (f1 < f0) {
                std::size_t num_increases = 0;
                while (true) {
                    const T double_step_size = step_size + step_size;
                    f2 = evaluate_objective_function(double_step_size);
                    if (f2 >= f1) {
                        break;
                    } else {
                        step_size = double_step_size;
                        f1 = f2;
                        if (++num_increases >= max_increases) { return; }
                    }
                }
                const T numer = 4 * f1 - f2 - 3 * f0;
                const T denom = f1 + f1 - f2 - f0;
                const T optimal_step_size = step_size * numer / (denom + denom);
                evaluate_objective_function(optimal_step_size);
            } else {
                while (true) {
                    const T half_step_size = step_size / 2;
                    bool changed;
                    f2 = evaluate_objective_function(half_step_size, &changed);
                    if (!changed) { return; }
                    if (f2 < f0) {
                        break;
                    } else {
                        step_size = half_step_size;
                        if (step_size == 0) { return; }
                        f1 = f2;
                    }
                }
                const T numer = f1 - 4 * f2 + 3 * f0;
                const T denom = f1 - (f2 + f2) + f0;
                const T optimal_step_size = step_size * numer / (4 * denom);
                evaluate_objective_function(optimal_step_size);
            }
        }

    }; // class QuadraticLineSearcher

} // namespace dznl

#endif // DZNL_QUADRATIC_LINE_SEARCHER_HPP_INCLUDED


/*
 *  Copyright (c) 2008--2011, Universitaet Bremen
 *  All rights reserved.
 *
 *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Universitaet Bremen nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file mtk/mean_and_covar.hpp
 * @brief Functions to estimate mean value and covariance on manifolds.
 */

#ifndef MEAN_AND_COVAR_HPP_
#define MEAN_AND_COVAR_HPP_

#include <Eigen/Core>

namespace MTK {

/**
 * \defgroup MeanCov Mean and Covariance Calculation
 * @todo provide functions, which only calculate either mean or covariance
 */
//@{

/**
 * Estimate mean value and covariance of a set of manifold values.
 *
 * @tparam M    Manifold type. Must implement boxminus, boxplus and have
 *              @c typedef scalar and @c enum DOF.
 * @tparam Cont Container Type. Elements must be convertible to @c M.
 *
 * @param mean   reference to mean value (output)
 * @param cov    reference to covariance matrix (output)
 * @param values const reference to input container
 * @param max_it maximum number of iterations (optional).
 *
 * Mean value and covariance are estimated using algorithm described in
 * @cite{Hertzberg2011}
 */
template<class M, class Cont>
double mean_and_covariance(M& mean, Eigen::Matrix<typename M::scalar, M::DOF, M::DOF> &cov,
                           const Cont &values, int max_it = 16)
{
	enum {DOF = M::DOF};
	typedef typename M::scalar scalar;
	mean = values[0];
	double res;
	int i=0;
	do {
		Eigen::Matrix<scalar, DOF, 1> mean_delta, delta;
		mean_delta.setZero();
		for (typename Cont::const_iterator Xi = values.begin(); Xi != values.end(); ++Xi)
		{
			Xi->boxminus(delta.data(), mean);
			mean_delta += delta;
		}
		mean_delta /= values.size();
		res = mean_delta.norm();
		mean.boxplus(mean_delta.data());
	} while (res > 1e-6 && ++i < max_it);

	cov.setZero();
	for (typename Cont::const_iterator Xi = values.begin(); Xi != values.end(); ++Xi)
	{
		Eigen::Matrix<scalar, DOF, 1> delta;
		Xi->boxminus(delta.data(), mean);
		cov += delta * delta.transpose();
	}
	cov *= 0.5;

	return res;
}

//@}

}  // namespace MTK

#endif /* MEAN_AND_COVAR_HPP_ */


#pragma once

#include "EllipsoidalCalibration.hpp"

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Cholesky>
#include <iostream>
namespace icarus
{
    template<typename T>
    struct EllipsoidalCalibrator
    {
        EllipsoidalCalibrator(size_t sampleCount) :
            mSamples(10, sampleCount),
            mIndex(0)
        {}

        void addSample(Eigen::Matrix<T, 3, 1> const & p);

        EllipsoidalCalibration<T> computeCalibration(T norm) const;
    private:
        static Eigen::Matrix<T, 6, 1> computePositiveEigenVector(Eigen::Matrix<T, 6, 6> const & SS);
        static Eigen::Matrix<T, 10, 1> computeEllipsoidEquation(Eigen::Matrix<T, 10, 10> const & S);
        static Eigen::Matrix<T, 3, 4> computeEllipsoidTransformation(Eigen::Matrix<T, 10, 1> const & v, T norm);

        Eigen::Matrix<T, 10, Eigen::Dynamic> mSamples;
        size_t mIndex;
    };

    template<typename T>
    void EllipsoidalCalibrator<T>::addSample(Eigen::Matrix<T, 3, 1> const & p)
    {
        mSamples.col(mIndex) <<
            p.x() * p.x(),
            p.y() * p.y(),
            p.z() * p.z(),
            2 * p.y() * p.z(),
            2 * p.x() * p.z(),
            2 * p.x() * p.y(),
            2 * p.x(),
            2 * p.y(),
            2 * p.z(),
            1;

        ++mIndex;
    }

    template<typename T>
    EllipsoidalCalibration<T> EllipsoidalCalibrator<T>::computeCalibration(T norm) const
    {
        Eigen::Matrix<T, 10, 10> S;
        // compute D * D.transpose() but in a way that is almost 3 times faster and uses 4 times less RAM
        S.setZero();
        S.template selfadjointView<Eigen::Lower>().rankUpdate(mSamples);
        S.template triangularView<Eigen::Upper>() = S.transpose();

        auto ellipsoidEquation = computeEllipsoidEquation(S);

        auto ellipsoid = computeEllipsoidTransformation(ellipsoidEquation, norm);

        return EllipsoidalCalibration<T>(ellipsoid);
    }

    template<typename T>
    Eigen::Matrix<T, 6, 1> EllipsoidalCalibrator<T>::computePositiveEigenVector(Eigen::Matrix<T, 6, 6> const & SS)
    {
        Eigen::Matrix<T, 6, 6> C;
        C.setZero();
        C.template block<3, 3>(0, 0).setOnes();
        C.diagonal() << -1, -1, -1, -4, -4, -4;

        Eigen::EigenSolver<Eigen::Matrix<T, 6, 6>> decomposition(C.lu().solve(SS));

        Eigen::Index maxCol;
        decomposition.eigenvalues().real().maxCoeff(&maxCol);
        Eigen::Matrix<T, 6, 1> v1 = decomposition.eigenvectors().col(maxCol).real();
        if (v1(1) < 0) {
            v1 = -v1;
        }
        return v1;
    }

    template<typename T>
    Eigen::Matrix<T, 10, 1> EllipsoidalCalibrator<T>::computeEllipsoidEquation(Eigen::Matrix<T, 10, 10> const & S)
    {
        auto S11 = S.template block<6, 6>(0, 0);
        auto S21 = S.template block<4, 6>(6, 0);
        auto S12 = S.template block<6, 4>(0, 6);//S21.transpose();
        auto S22 = S.template block<4, 4>(6, 6);
        Eigen::Matrix<T, 4, 6> const S22a = S22.inverse() * S21;
        Eigen::Matrix<T, 6, 6> const SS = S11 - S12 * S22a;

        Eigen::Matrix<T, 10, 1> v;
        auto v1 = v.template head<6>();
        auto v2 = v.template tail<4>();

        v1 = computePositiveEigenVector(SS);
        v2 = -S22a * v1;

        return v;
    }

    template<typename T>
    Eigen::Matrix<T, 3, 4> EllipsoidalCalibrator<T>::computeEllipsoidTransformation(Eigen::Matrix<T, 10, 1> const & v, T norm)
    {
        Eigen::Matrix<T, 3, 3> Q;
        Q << v[0], v[5], v[4],
             v[5], v[1], v[3],
             v[4], v[3], v[2];

        Eigen::Matrix<T, 3, 4> ret;
        auto Ainv = ret.template block<3, 3>(0, 0);
        auto B = ret.col(3);

        B = -Q.inverse() * v.template segment<3>(6);

        T scaling = norm / sqrt(B.transpose() * Q * B - v[9]);

        Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T, 3, 3>> Qsolver(Q);
        Ainv.noalias() = scaling * Qsolver.operatorSqrt();
        B = -B;

        std::cout << ret << std::endl;

        return ret;
    }
}


#pragma once

#define _USE_MATH_DEFINES
#include <math.h>
//#include "trkapi.hpp"
#include <toffy/toffy_config.h>
#include <boost/log/trivial.hpp>

#if OCV_VERSION_MAJOR >= 3
#  include <opencv2/calib3d.hpp>
#else
#  include <opencv2/calib3d/calib3d.hpp>
#endif

namespace toffy {
namespace commons {

/** helper functions for camera handling; assumes we have standard optics (90deg hfov).
 */

const int hPix=160;
const int vPix=120;
const float hFov=90.;
const float vFov=65.;
const float pixSinAngle = 0.009817319; // = sin( deg2rad(90/160) );
const float pixCosAngle = 0.999951809; // = cos( deg2rad(90/160) );
const double fl_x_reciprocal = 1.0f / 8.8345892962843834e+01;
const double fl_y_reciprocal = 1.0f / 8.8395902341635306e+01;
const double center_x = 7.9460485484676596e+01;
const double center_y = 5.7816728185872989e+01;

static inline float deg2rad(float a) { return a/180.*M_PI; }

static inline double getPixelSize(double distance )
{
    //float maxFinger = 0.01 / (angl*pdis);  //==pixel width
    return pixSinAngle * distance;
}

static inline void depth2xyz(const cv::Point2f& p, float d, cv::Point3f& xyz)
{
    xyz.x = pixSinAngle*(p.x-hPix/2) *  pixSinAngle*(p.y-vPix/2) * d;
    xyz.y = pixSinAngle*(p.x-hPix/2) *  pixCosAngle*(p.y-vPix/2) * d;
    xyz.z = pixCosAngle*(p.x-hPix/2) * d;

    float rho = deg2rad( (p.x-hPix/2) );
    float tht = deg2rad( (p.y-vPix/2) );

    xyz.x = d * sin (rho) * sin(tht) ;
    xyz.y = d * sin (rho) * cos(tht) ;
    xyz.z = d * cos (rho);
}

static inline void depth2xyz(const cv::Point2f& p, float d, cv::Vec3f& xyz)
{
    xyz[0] = pixSinAngle*(p.x-hPix/2) *  pixSinAngle*(p.y-vPix/2) * d;
    xyz[1] = pixSinAngle*(p.x-hPix/2) *  pixCosAngle*(p.y-vPix/2) * d;
    xyz[2] = pixCosAngle*(p.x-hPix/2) * d;
}

/*static inline void depth2xyz(const cv::Point2f& p, float d, track::vec3f& xyz)
{
    float rho = deg2rad( (p.x-hPix/2) );
    float tht = deg2rad( (p.y-vPix/2) );

    xyz.x = d * sin (rho) * sin(tht) ;
    xyz.y = d * sin (rho) * cos(tht) ;
    xyz.z = d * cos (rho);
}*/

static inline cv::Point3d pointTo3D(cv::Point2d point, float depthValue, cv::Mat cameraMatrix, cv::Size imgSize)
{
	//BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ << point;

	double fl_x_reciprocal, fl_y_reciprocal;
	cv::Point2d center;
	if(cameraMatrix.data) {
	    //Saving parameter from the camera matrix
	    fl_x_reciprocal = 1.0f / cameraMatrix.at<double>(0,0);
	    fl_y_reciprocal = 1.0f / cameraMatrix.at<double>(1,1);
	    //center_x = _cameraMatrix.at<double>(0,2);
	    //center_y = _cameraMatrix.at<double>(1,2);

	    double noV,
		    apertureWidth = (45/1000)*imgSize.width,
		    apertureHeight = (45/1000)*imgSize.height;
	    cv::calibrationMatrixValues(cameraMatrix, imgSize,
				    apertureWidth, apertureHeight,
				    noV, noV, noV, center, noV);
	} else {
	    BOOST_LOG_TRIVIAL(warning) <<"No cameraMatrix data.";
	    return cv::Point3d();
	}

	cv::Point3d out3Dp;

	//Saving parameter from the camera matrix
	out3Dp.x = (static_cast<float> (point.x) - center.x) * depthValue * fl_x_reciprocal; //X
	out3Dp.y = (static_cast<float> (point.y) - center.y) * depthValue * fl_y_reciprocal; //Y
	out3Dp.z = depthValue; //Z

	return out3Dp;
}

static inline cv::Point2i pointTo2D(cv::Point3d point, cv::Mat cameraMatrix, cv::Size imgSize)
{
	//BOOST_LOG_TRIVIAL(debug) << __FUNCTION__;
	double fl_x_reciprocal, fl_y_reciprocal;
	cv::Point2d center;
	if(cameraMatrix.data) {
	    //Saving parameter from the camera matrix
	    fl_x_reciprocal = 1.0f / cameraMatrix.at<double>(0,0);
	    fl_y_reciprocal = 1.0f / cameraMatrix.at<double>(1,1);
	    //center_x = _cameraMatrix.at<double>(0,2);
	    //center_y = _cameraMatrix.at<double>(1,2);

	    double noV,
		    apertureWidth = (45/1000)*imgSize.width,
		    apertureHeight = (45/1000)*imgSize.height;
	    cv::calibrationMatrixValues(cameraMatrix, imgSize,
				    apertureWidth, apertureHeight,
				    noV, noV, noV, center, noV);
	} else {
	    BOOST_LOG_TRIVIAL(warning) <<"No cameraMatrix data.";
	    return cv::Point2i();
	}

	cv::Point2i out2Dp;

	out2Dp.x = (point.x/(point.z*fl_x_reciprocal)) + center.x;
	out2Dp.y = (point.y/(point.z*fl_y_reciprocal)) + center.y;
	// Saving found pixel
	return out2Dp;
}

static inline cv::Point3d pointTo3D(cv::Point point, float depthValue)
{
	BOOST_LOG_TRIVIAL(debug) << __FUNCTION__;

	cv::Point3d out3Dp;

	//Saving parameter from the camera matrix
	out3Dp.x = (static_cast<float> (point.x) - center_x) * depthValue * fl_x_reciprocal; //X
	out3Dp.y = (static_cast<float> (point.y) - center_y) * depthValue * fl_y_reciprocal; //Y
	out3Dp.z = depthValue; //Z

	return out3Dp;
}

static inline cv::Point2i pointTo2D(cv::Point3d point)
{
	BOOST_LOG_TRIVIAL(debug) << __FUNCTION__;

	cv::Point2i out2Dp;

	out2Dp.x = (point.x/(point.z*fl_x_reciprocal)) + center_x;
	out2Dp.y = (point.y/(point.z*fl_y_reciprocal)) + center_y;
	// Saving found pixel
	return out2Dp;
}

}}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/optional.hpp>
#include <cstddef>
#include <limits>

#include "DataStructures/Tensor/TypeAliases.hpp"
#include "Utilities/TypeTraits/RemoveReferenceWrapper.hpp"

/// \cond
namespace PUP {
class er;
}  // namespace PUP
/// \endcond

namespace domain {
namespace CoordinateMaps {

/*!
 * \ingroup CoordinateMapsGroup
 *
 * \brief Redistributes gridpoints within the unit sphere.
 * \image html SpecialMobius.png "A sphere with a `mu` of 0.25."
 *
 * \details A special case of the conformal Mobius transformation that
 * maps the unit ball to itself. This map depends on a single
 * parameter, `mu` \f$ = \mu\f$, which is the x-coordinate of the preimage
 * of the origin under this map. This map has the fixed points \f$x=1\f$ and
 * \f$x=-1\f$. The map is singular for \f$\mu=1\f$ but we have found that this
 * map is accurate up to 12 decimal places for values of \f$\mu\f$ up to 0.96.
 *
 * We define the auxiliary variables
 * \f[ r := \sqrt{x^2 + y^2 +z^2}\f]
 * and
 * \f[ \lambda := \frac{1}{1 - 2 x \mu + \mu^2 r^2}\f]
 *
 * The map corresponding to this transformation in cartesian coordinates
 * is then given by:
 *
 * \f[\vec{x}'(x,y,z) =
 * \lambda\begin{bmatrix}
 * x(1+\mu^2) - \mu(1+r^2)\\
 * y(1-\mu^2)\\
 * z(1-\mu^2)\\
 * \end{bmatrix}\f]
 *
 * The inverse map is the same as the forward map with \f$\mu\f$
 * replaced by \f$-\mu\f$.
 *
 * This map is intended to be used only inside the unit sphere.  A
 * point inside the unit sphere maps to another point inside the unit
 * sphere. The map can have undesirable behavior at certain points
 * outside the unit sphere: The map is singular at
 * \f$(x,y,z) = (1/\mu, 0, 0)\f$ (which is outside the unit sphere
 * since \f$|\mu| < 1\f$). Moreover, a point on the \f$x\f$-axis
 * arbitrarily close to the singularity maps to an arbitrarily large
 * value on the \f$\pm x\f$-axis, where the sign depends on which side
 * of the singularity the point is on.
 *
 * A general Mobius transformation is a function on the complex plane, and
 * takes the form \f$ f(z) = \frac{az+b}{cz+d}\f$, where
 * \f$z, a, b, c, d \in \mathbb{C}\f$, and \f$ad-bc\neq 0\f$.
 *
 * The special case used in this map is the function
 * \f$ f(z) = \frac{z - \mu}{1 - z\mu}\f$. This has the desired properties:
 * - The unit disk in the complex plane is mapped to itself.
 *
 * - The x-axis is mapped to itself.
 *
 * - \f$f(\mu) = 0\f$.
 *
 * The three-dimensional version of this map is obtained by rotating the disk
 * in the plane about the x-axis.
 *
 * This map is useful for performing transformations along the x-axis
 * that preserve the unit disk. A concrete example of this is in the BBH
 * domain, where two BBHs with a center-of-mass at x=\f$\mu\f$ can be shifted
 * such that the new center of mass is now located at x=0. Additionally,
 * the spherical shape of the outer wave-zone is preserved and, as a mobius
 * map, the spherical coordinate shapes of the black holes is also preserved.
 */
class SpecialMobius {
 public:
  static constexpr size_t dim = 3;
  explicit SpecialMobius(double mu) noexcept;
  SpecialMobius() = default;
  ~SpecialMobius() = default;
  SpecialMobius(SpecialMobius&&) = default;
  SpecialMobius(const SpecialMobius&) = default;
  SpecialMobius& operator=(const SpecialMobius&) = default;
  SpecialMobius& operator=(SpecialMobius&&) = default;

  template <typename T>
  std::array<tt::remove_cvref_wrap_t<T>, 3> operator()(
      const std::array<T, 3>& source_coords) const noexcept;

  /// Returns boost::none for target_coords outside the unit sphere.
  boost::optional<std::array<double, 3>> inverse(
      const std::array<double, 3>& target_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame> jacobian(
      const std::array<T, 3>& source_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame> inv_jacobian(
      const std::array<T, 3>& source_coords) const noexcept;

  // clang-tidy: google runtime references
  void pup(PUP::er& p) noexcept;  // NOLINT

  bool is_identity() const noexcept { return is_identity_; }

 private:
  template <typename T>
  std::array<tt::remove_cvref_wrap_t<T>, 3> mobius_distortion(
      const std::array<T, 3>& coords, double mu) const noexcept;
  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame>
  mobius_distortion_jacobian(const std::array<T, 3>& coords, double mu) const
      noexcept;
  friend bool operator==(const SpecialMobius& lhs,
                         const SpecialMobius& rhs) noexcept;

  double mu_{std::numeric_limits<double>::signaling_NaN()};
  bool is_identity_{false};
};
bool operator!=(const SpecialMobius& lhs, const SpecialMobius& rhs) noexcept;
}  // namespace CoordinateMaps
}  // namespace domain


#include "GEFE_utility.h"
#include <armadillo>

// Given the corresponding eigenvalues of H and H with the jth column removed, returns the ith
// row eigenvector. This is denoted as:
// prod(lambda_H_i - lambda_Hj_k) / prod(lambda_H_i - lambda_H_k), when i != k.
double getRowiEigenvector(int i, const arma::mat& H_eigenvalues, const arma::mat& Hj_eigenvalues) {
    const double lambda_H_i = H_eigenvalues[i];
    double v_ij = 1;
    const int jth_column = H_eigenvalues.size() - 1;

    for (int k = 0; k < H_eigenvalues.size(); ++k) {
        double numerator = 1;   // lambda_H_i - lambda_Hj_k
        double denominator = 1; // lambda_H_i - lambda_H_k
        if (k != jth_column) {
            numerator = lambda_H_i - Hj_eigenvalues[k];
        }
        const double lambda_H_k = H_eigenvalues[k];
        if (lambda_H_k != lambda_H_i) {
            denominator = lambda_H_i - lambda_H_k;
        }
        v_ij *= numerator/denominator;
    }
    return v_ij;
}

double getEigenvectorFromEigenvalues(const arma::mat& H, int i, int j) {
    if (!H.is_square()) {
        throw std::invalid_argument("\nH is not square.");
    }
    if (i < 0 || i > H.n_rows - 1) {
        throw std::invalid_argument("\nj must be an integer representing the "
                                    "row of the desired eigenvector values, "
                                    "such that 0 <= i <= N-1.");
    }
    if (j < 0 || j > H.n_cols - 1) {
        throw std::invalid_argument("\nj must be an integer representing the "
                                    "column of the desired eigenvector values, "
                                    "such that 0 <= j <= N-1.");
    }

    const arma::vec H_eigenvalues = eig_sym(H);

    arma::mat Hj_eigenvalues = H;
    Hj_eigenvalues.shed_col(j);
    Hj_eigenvalues.shed_row(j);

    return getRowiEigenvector(i, H_eigenvalues, Hj_eigenvalues);
}

arma::vec getEigenvectorFromEigenvalues(const arma::mat& H, const arma::vec& ii, int j) {
    if (!H.is_square()) {
        throw std::invalid_argument("\nH is not square.");
    }
    for (const double i : ii) {
        if (i < 0 || i > H.n_rows - 1) {
            throw std::invalid_argument("\nEach i in ii must be a row of the matrix H. "
                                        "For each i, 0 <= i <= N-1.");
        }
    }
    if (j < 0 || j > H.n_cols - 1) {
        throw std::invalid_argument("\nj must be an integer representing the "
                                    "column of the desired eigenvector values, "
                                    "such that 0 <= j <= N-1.");
    }

    const arma::vec H_eigenvalues = eig_sym(H);

    arma::mat Hj_eigenvalues = H;
    Hj_eigenvalues.shed_col(j);
    Hj_eigenvalues.shed_row(j);

    arma::vec v_ij(ii.size(), arma::fill::zeros);
    for (const double i : ii) {
        v_ij[i] = getRowiEigenvector(i, H_eigenvalues, Hj_eigenvalues);
    }
    return v_ij;
}


// Copyright (c) 2011 The University of Sydney

#include <cmath>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/foreach.hpp>
#include <Eigen/Core>
#include <opencv2/core/version.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "region_properties.h"
#include <comma/base/exception.h>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS( cs::cartesian )

namespace snark{ namespace imaging {

/// compute the area of a polygon and its convex hull
/// @param points polygon points
/// @param area area of the polygon
/// @param convexArea area of the convex hull
void compute_area( const std::vector< cv::Point >& points, double& area, double& convexArea )
{
    boost::geometry::model::polygon< boost::tuple< double, double > > polygon;
    for( unsigned int i = 0; i < points.size(); i++ )
    {
        boost::geometry::append( polygon, boost::make_tuple( points[i].x, points[i].y ) );
    }
    boost::geometry::append( polygon, boost::make_tuple( points[0].x, points[0].y ) ); // close polygon

    area = boost::geometry::area( polygon );

    boost::geometry::model::polygon< boost::tuple<double, double> > hull;
    boost::geometry::convex_hull( polygon, hull );

    convexArea = boost::geometry::area( hull );
}

/// constructor
/// @param image input image, is considered as a binary image ( all non-zero pixels are 1 )
region_properties::region_properties ( const cv::Mat& image, double minArea ):
    m_minArea( minArea )
{
    cv::Mat binary;
    if( image.channels() == 3 )
    {
        cv::cvtColor( image, binary, cv::COLOR_RGB2GRAY );
    }
    else if( image.channels() == 1 )
    {
        binary = image;
    }
    else
    {
        COMMA_THROW( comma::exception, "incorrect number of channels, should be 1 or 3, not " << image.channels() );
    }
//     cv::Mat closed;
//     cv::morphologyEx( binary, closed, cv::MORPH_CLOSE, cv::Mat::ones( 3, 3, CV_8U) );
    std::vector< std::vector<cv::Point> > contours;
    cv::findContours( binary, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE );

    for( unsigned int i = 0; i < contours.size(); i++ )
    {
        binary = cv::Scalar(0);
        #if defined( CV_VERSION_EPOCH ) && CV_VERSION_EPOCH == 2
            cv::drawContours( binary, contours, i, cv::Scalar(0xFF), 1 );
        #else
            cv::drawContours( binary, contours, i, cv::Scalar(0xFF), cv::FILLED );
        #endif
        cv::Rect rect = cv::boundingRect( cv::Mat( contours[i]) );
        cv::Moments moments = cv::moments( binary( rect ), true );
        double x = moments.m10/moments.m00;
        double y = moments.m01/moments.m00;
        double area = moments.m00; // cv::countNonZero( binary( rect ) )
        if( area > m_minArea )
        {
            // see wikipedia, image moments
            double diff = moments.nu20 - moments.nu02;
            double a = 0.5 * ( moments.nu20 + moments.nu02 );
            double b = 0.5 * std::sqrt( 4 * moments.nu11 * moments.nu11 + diff * diff );
            double minEigenValue = a - b;
            double maxEigenValue = a + b;
    //         std::cerr << " min " << minEigenValue << " max " << maxEigenValue << std::endl;
            double theta = 0.5 * std::atan2( 2 * moments.nu11, diff );
            double eccentricity = 1;
            if( std::fabs( maxEigenValue ) > 1e-15 )
            {
                eccentricity = std::sqrt( 1 - minEigenValue / maxEigenValue );
            }

            double polygonArea;
            double convexArea;
            compute_area( contours[i], polygonArea, convexArea );
    //         std::cerr << " area " << area << " polygon " << polygonArea << " convex " << convexArea << std::endl;

            blob blob;
            blob.majorAxis = 2 * std::sqrt( moments.m00 * maxEigenValue );
            blob.minorAxis = 2 * std::sqrt( moments.m00 * minEigenValue );
            blob.orientation = theta;
            blob.centroid = cv::Point( x + rect.x, y + rect.y );
            blob.area = area;
            blob.eccentricity = eccentricity;
            blob.solidity = 0;
            if( std::fabs( convexArea ) > 1e-15 )
            {
                blob.solidity = polygonArea / convexArea;
            }
            m_blobs.push_back( blob );
        }
    }
}

/// draw debug information on the image
void region_properties::show( cv::Mat& image, bool text )
{
    for( unsigned int i = 0; i < m_blobs.size(); i++ )
    {
        cv::Point centroid = m_blobs[i].centroid;
        cv::circle( image, centroid, 3, cv::Scalar( 0, 0, 255 ), 2 );
        std::stringstream s;
        s << i;
        if( text )
        {
            cv::putText( image, s.str(), centroid + cv::Point( 2, 2 ), cv::FONT_HERSHEY_PLAIN ,1, cv::Scalar( 0, 0, 255 ) );
        }
        cv::ellipse( image, centroid, cv::Size( m_blobs[i].majorAxis, m_blobs[i].minorAxis ), m_blobs[i].orientation * 180.0 / M_PI, 0, 360, cv::Scalar( 0, 255, 0 ) );
//         std::cerr << i << ": area " << m_blobs[i].area << " eccentricity " << m_blobs[i].eccentricity << " solidity " << m_blobs[i].solidity << std::endl;
    }
}

} }


/**
@file SBGATMassPropertiesUQ.hpp
@class  SBGATMassPropertiesUQ
@author Benjamin Bercovici
@date January 2019

@brief  Evaluation of the formal uncertainty in the volume, center of mass, inertia tensor parametrization
from a topologically-closed, constant density polyhedron.

@copyright MIT License, Benjamin Bercovici and Jay McMahon
*/

#ifndef SBGATMassPropertiesUQUQ_hpp
#define SBGATMassPropertiesUQUQ_hpp

#include <armadillo>
#include "SBGATMassProperties.hpp"
#include <SBGATFilterUQ.hpp>

class SBGATMassPropertiesUQ : public SBGATFilterUQ{
public:

  /**
  Sets the model associated to this uncertainty quantification container
  and updates the partials of the mass properties relative to the shape
  @param[in] pgm pointer to valid SBGATFilter
  @param[in]
  */
  virtual void SetModel(vtkSmartPointer<SBGATFilter> model){this -> model = model;}

  /**
  Runs a finite-differencing based test of the implemented PGM partials
  @param input path to obj file used to test the partials
  @param tol relative tolerance
  @param shape_in_meters true if tested shape has its coordinates expressed in meters
  */
  static void TestPartials(std::string input , double tol ,bool shape_in_meters);

/**
Return the partial derivative of the shape's center of mass with respect to the shape's vertices coordinates
@return partial derivative of the shape's center of mass with respect to the shape's vertices
*/
  const arma::mat & GetPartialComPartialC() const {return this -> precomputed_partialGpartialC;}

  /**
  Return the partial derivative of the 6 unique components of the inertia tensor {I(0,0),I(1,1),I(2,2),I(0,1),I(0,2),I(1,2)}
  with respect to the shape coordinates
  @return partial derivative
  */
  const arma::mat & GetPartialIPartialC() const {return this -> precomputed_partialIpartialC;}

  /**
  Return the partial derivative of the MRP orienting the body-frame (B) to principal-frame (P) dcm (PB)
  with respect to the shape vertices coordinates
  @return partial derivative
  */
  const arma::mat & GetPartialSigmaPartialC() const { return this -> precomputed_partialSigmapartialC;}

  /**
  Return the partial derivative of the volume
  with respect to the shape coordinates
  @return partial derivative
  */
  const arma::rowvec & GetPartialVolumePartialC() const {return this -> precomputed_partialVpartialC;}

  /**
  Applies prescribed deviation to all the N_vertices control points and updates model
  @param delta_C deviation (3 * N_vertices x 1)
  */
  virtual void ApplyDeviation(const arma::vec & delta_C);

  /**
  Evaluates the partial of the volume, center of mass and mrp orienting the principal axes
  relative to the vertices coordinates and stores the computed partials in designated containers
  */
  void PrecomputeMassPropertiesPartials();

  /**
  Return the partial derivative of the unit density moments relative to a change in the inertia tensor parametrization
  @return  partial derivative of the unit density moments relative to a change in the inertia tensor parametrization
  */
  const arma::mat & GetPartialUnitDensityMomentsPartialI() const{return this -> precomputed_partialUnitDensityMomentsPartialI;}

  /**
  Return the partial derivative of the MRP orienting the body-frame (B) to principal-frame (P) dcm (PB)
  with respect to the inertia tensor parametrization
  @return partial derivative
  */
  const arma::mat::fixed<3,6> & GetPartialSigmaPartialI() const {return this -> precomputed_partialSigmapartialI;}

/**
  Runs a Monte Carlo on the shape and the volume, center-of-mass and inertia tensor parametrizaton
  @param[in] path_to_shape path to reference shape
  @param[in] density small body density in kg/m^3
  @param[in] shape_in_meters true if reference shape has its units expressed in meters, false otherwise
  @param[in] C_CC square root of the covariance of the shape vertices coordinates. Must be of dimensions (3N_C * 3N_C) where N_C is the
  number of vertices in the reference shape
  @param[in] N_samples number of shape outcomes to draw
  @param[in] output_dir path ending in "/" where to save shape-related monte-carlo data. Only used
  if last argument is larger than 0
  @param[in] N_saved_shapes number of shape outcomes to save. must be lesser or equal than N_samples
  @param[out] deviations holds N_samples 3*N_C column vectors storing the deviation applied to the coordinates of the
  reference shape at every sample
  @param[out] all_volumes holds N_samples of the volume
  @param[out] all_com holds N_samples of the center-of-mass
  @param[out] all_inertia holds N_samples of the inertia tensor parametrization
  */

  static void RunMCUQVolumeCOMInertia(std::string path_to_shape,
    const double & density,
    const bool & shape_in_meters,
    const arma::mat & C_CC,
    const unsigned int & N_samples,
    std::string output_dir,
    int N_saved_shapes,
    arma::mat & deviations,
    arma::vec & all_volumes,
    arma::mat &  all_com,
    arma::mat & all_inertia);

protected:

  /**
  Returns the partial derivative of the MRP orienting the principal axes with respect to the parametrization
  of the unit-density inertia tensor
  @return partial derivative
  */
  arma::mat::fixed<3,6> PartialSigmaPartialI() const;

/**
Returns partial derivative of unit-density inertia moments relative to the parametrization
of the unit-density inertia tensor
*/
  arma::mat::fixed<3,6>  PartialUnitDensityMomentsPartialI() const;

/**
Return the partial derivative of the (q,r) component of the contribution of the f-facet
to the shape's inertia tensor, relative to the f-facet
vertices coordinates
@param f facet index
@param q first index
@param r second index
@param Tf 9x1 vector holding coordinates of vertices in facet EXPRESSED IN METERS
@return partial derivative of the (q,r) component of the contribution of the f-facet
to the shape's inertia tensor
*/
  arma::rowvec::fixed<9> PartialEqDeltaIfErPartialTf(const int & f, const int & q, const int & r,const arma::vec::fixed<9> & Tf) const;

  /**
  Return the partial derivative of the shape's center-of-mass
  with respect to the f-th facet coordinates
  @param f facet index
  @return partial derivative of shape's center-of-mass with respect to facet coordinates
  */
  arma::mat::fixed<3,9> PartialDeltaComPartialTf(const int & f) const;

  /**
  Return the partial derivative of e_q.T * DeltaIOverDeltaVfEr * e_r with respect to the f-facet
  vertices coordinates
  @param e_q first 3x1 vector canonical unit vector
  @param e_r first 3x1 vector canonical unit vector
  @param Tf 9x1 vector holding coordinates of vertices in facet EXPRESSED IN METERS
  @return the partial derivative of e_q.T * DeltaIOverDeltaVfEr * e_r with respect to the f-facet
  vertices coordinates
  */
  arma::rowvec::fixed<9> PartialEqDeltaIOverDeltaVfErPartialTf(const arma::vec::fixed<3> & e_q,const arma::vec::fixed<3> & e_r,
    const arma::vec::fixed<9> & Tf) const;

  /**
  Return the partial derivative of the volume of the tetrahedron subtended by facet f
  with respect to the facet coordinates
  @param f facet index
  @return partial derivative of tetrahedron volume with respect to facet coordinates
  */
  arma::rowvec::fixed<9> PartialDeltaVfPartialTf(const int & f) const;

  /**
  Return the partial derivative of the center of mass of the considered tetrahedron
  with respect to the facet coordinates
  @return partial derivative of center of mass with respect to facet coordinates
  */
  static arma::mat::fixed<3,9> PartialDeltaCMfPartialTf();

  /**
  Return the partial derivative of the tetrahedron's inertia tensor parametrization
  relative to the facet coordinates
  @param f facet index
  @return partial derivative of the tetrahedron's inertia tensor parametrization
  relative to the facet coordinates
  */
  arma::mat::fixed<6,9> PartialDeltaIfPartialTf(const int & f) const;

  /**
  Return the partial derivative of a tetrahedron's  inertia-times-volume tensor parametrization
  with respect to the subtending facet's vertices coordinates
  @param f facet index
  @return partial derivative of the tetrahedron's inertia-times-volume tensor parametrization
  with respect to the subtending facet's vertices coordinates
  */
  arma::mat::fixed<6,9> PartialDeltaIOverDeltaVPartialTf(const int & f) const;

  /**
  Applies deviation to the coordinates of the vertices in the prescribed facet
  and updates the pgm
  @param delta_Tf deviation
  @param f facet index
  */
  virtual void ApplyTfDeviation(arma::vec::fixed<9> delta_Tf,const int & f);

  static void TestPartialDeltaVfPartialTf(std::string input,double tol,bool shape_in_meters);
  static void TestPartialDeltaIOverDeltaVPartialTf(std::string input,double tol,bool shape_in_meters);
  static void TestPartialDeltaIfPartialTf(std::string input,double tol,bool shape_in_meters);
  static void TestPartialDeltaVPartialC(std::string input,double tol,bool shape_in_meters);
  static void TestGetPartialVolumePartialC(std::string input,double tol,bool shape_in_meters);
  static void TestGetPartialComPartialC(std::string input,double tol,bool shape_in_meters);
  static void TestGetPartialIPartialC(std::string input,double tol,bool shape_in_meters);
  static void TestGetPartialAllInertiaPartialC(std::string input,double tol,bool shape_in_meters) ;
  static void TestPartialEqDeltaIfErPartialTf(std::string input,double tol,bool shape_in_meters);
  static void TestGetPartialSigmaPartialC(std::string input,double tol,bool shape_in_meters);

  arma::rowvec precomputed_partialVpartialC;
  arma::mat precomputed_partialGpartialC;
  arma::mat precomputed_partialSigmapartialC;
  arma::mat precomputed_partialIpartialC;
  arma::mat::fixed<3,6> precomputed_partialUnitDensityMomentsPartialI;
  arma::mat::fixed<3,6> precomputed_partialSigmapartialI;

};

#endif


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//! OPENOR_INTERFACE_FILE(openOR_core)
//****************************************************************************
/**
 * @file
 * @ingroup openOR_core
 */

#ifndef openOR_core_math_utilities_hpp
#define openOR_core_math_utilities_hpp

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

namespace openOR {

   namespace Math {

      /**
       * \brief square
       * @ingroup openOR_core
       */
      template<typename T>
      T square(const T& arg) { return arg * arg; }

      /**
       * \brief clamp
       * @ingroup openOR_core
       */
      template<typename T>
      T clamp(const T& arg, const T& min, const T& max) { return std::min<T>(max, std::max<T>(min, arg)); }

      /**
       * \brief rounds a double to the next integer value
       */
      inline double round(const double& d)
      {
         return floor(d + 0.5);
      }

      /**
       * \brief rounds a float to the next integer value
       */
      inline float round(const float& f)
      {
         return floorf(f + 0.5f);
      }

      /**
       * \brief rounds a long double to the next integer value
       */

      inline long double round(const long double& d)
      {
         return floorl(d + 0.5);
      }

      /**
       * \brief nextPowerOfTwo
       * @ingroup openOR_core
       */
      inline static unsigned int nextPowerOfTwo(unsigned int v) {
         --v;
         v |= v >> 1;
         v |= v >> 2;
         v |= v >> 4;
         v |= v >> 8;
         v |= v >> 16;
         ++v;
         return v;
      }

      /**
       * \brief nextPowerOfTwo
       * @ingroup openOR_core
       */
      inline static unsigned short nextPowerOfTwo(unsigned short v) {
         --v;
         v |= v >> 1;
         v |= v >> 2;
         v |= v >> 4;
         v |= v >> 8;
         ++v;
         return v;
      }

      /**
       * \brief isPowerOfTwo
       * @ingroup openOR_core
       */
      inline bool isPowerOfTwo(unsigned int n) { return ((n & (n - 1)) == 0); }

      /**
       * \brief isPowerOfTwo
       * @ingroup openOR_core
       */
      inline bool isPowerOfTwo(unsigned short n) { return ((n & (n - 1)) == 0); }

      /**
       * \brief Solver for linear function A*x=y
       */
      template<typename Type>
      boost::numeric::ublas::vector<Type> solve(const boost::numeric::ublas::matrix<Type>& A, const boost::numeric::ublas::vector<Type>& y)
      {
         //create a permutation matrix for the LU-factorization
         boost::numeric::ublas::matrix<Type> matA(A);
         boost::numeric::ublas::permutation_matrix<std::size_t> pm(matA.size1());
         int res = boost::numeric::ublas::lu_factorize(matA, pm);
         if (res == 0)
            return boost::numeric::ublas::vector<Type>();

         boost::numeric::ublas::vector<Type> vecX(y);
         boost::numeric::ublas::lu_substitute(matA, pm, vecX);
         return vecX;
      }

   }
}
#endif


//============================================================================//
//---------------- pnt_integrity/GeodeticConverter.cpp ---------*- C++ -*-----//
//============================================================================//
// BSD 3-Clause License
//
// Copyright (c) 2017, ETHZ ASL
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice, this
//   list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// * Neither the name of the copyright holder nor the names of its
//   contributors may be used to endorse or promote products derived from
//   this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//----------------------------------------------------------------------------//
/// Third-party.  Downloaded from: https://github.com/ethz-asl/geodetic_utils //
//============================================================================//
#ifndef GEODETIC_CONVERTER_H_
#define GEODETIC_CONVERTER_H_

#include <Eigen/Dense>

namespace geodetic_converter
{
// Geodetic system parameters
/// \brief Equatorial radius (a), in meters
const double kSemimajorAxis = 6378137;
/// \brief Semi-minor radius (b), in meters
const double kSemiminorAxis = 6356752.3142;
/// \brief First eccentricity squared (e2), dimensionless
/// e2 = (a^2 - b^2) / a^2 = f * (2 - f)
const double kFirstEccentricitySquared = 6.69437999014 * 0.001;
/// \brief Second eccentricity squared (e'2), dimensionless
/// e'2 = (a^2 - b^2) / b^2 = e^2 / (1 - e^2) = e2 / (1 - e2)
const double kSecondEccentricitySquared = 6.73949674228 * 0.001;
/// \brief  flattening, dimensionless
const double kFlattening = 1 / 298.257223563;
/// \brief Pi (pi), dimensionless
const double PI = 3.14159265358979323846;

/// \brief Class to implement gedetic conversions for the pnt_integrity library
class GeodeticConverter
{
public:
  /// \brief Constructor for converter object
  ///
  /// Constructor initializes the reference flag to false.
  GeodeticConverter() { haveReference_ = false; }

  /// \brief Destructor for the converter object
  ~GeodeticConverter() {}

  // Default copy constructor and assignment operator are OK.

  /// \brief Returns the reference flag
  ///
  /// Returns a flag to indicate if the converter's reference position has
  /// been set.
  bool isInitialised() { return haveReference_; }

  /// \brief Returns the reference position
  ///
  /// Returns the reference position with the  latitude / longitude in radians
  /// and altitude in meters
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  void getReference(double* latitude, double* longitude, double* altitude)
  {
    *latitude  = initial_latitude_;
    *longitude = initial_longitude_;
    *altitude  = initial_altitude_;
  }

  /// \brief Sets the reference position
  ///
  /// Sets the reference to the provided position (LLA)
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  void initialiseReference(const double latitude,
                           const double longitude,
                           const double altitude)
  {
    // Save NED origin
    initial_latitude_  = latitude;
    initial_longitude_ = longitude;
    initial_altitude_  = altitude;

    // Compute ECEF of NED origin
    geodetic2Ecef(latitude,
                  longitude,
                  altitude,
                  &initial_ecef_x_,
                  &initial_ecef_y_,
                  &initial_ecef_z_);

    // Compute ECEF to NED and NED to ECEF matrices
    double phiP = atan2(
      initial_ecef_z_, sqrt(pow(initial_ecef_x_, 2) + pow(initial_ecef_y_, 2)));

    ecef_to_ned_matrix_ = nRe(phiP, initial_longitude_);
    ned_to_ecef_matrix_ =
      nRe(initial_latitude_, initial_longitude_).transpose();

    haveReference_ = true;
  }

  /// \brief Converts the provided LLA to ECEF
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  /// \param x The ECEF X psoition in meters
  /// \param y The ECEF Y position in meters
  /// \param z The ECEF Z position in meters
  void geodetic2Ecef(const double latitude,
                     const double longitude,
                     const double altitude,
                     double*      x,
                     double*      y,
                     double*      z)
  {
    // Convert geodetic coordinates to ECEF.
    // http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22
    double lat_rad = latitude;
    double lon_rad = longitude;
    double xi =
      sqrt(1 - kFirstEccentricitySquared * sin(lat_rad) * sin(lat_rad));
    *x = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * cos(lon_rad);
    *y = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * sin(lon_rad);
    *z = (kSemimajorAxis / xi * (1 - kFirstEccentricitySquared) + altitude) *
         sin(lat_rad);
  }

  /// \brief Converts the provided ECEF to LLA
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  /// \param x The ECEF X psoition in meters
  /// \param y The ECEF Y position in meters
  /// \param z The ECEF Z position in meters
  void ecef2Geodetic(const double x,
                     const double y,
                     const double z,
                     double*      latitude,
                     double*      longitude,
                     double*      altitude)
  {
    // Convert ECEF coordinates to geodetic coordinates.
    // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
    // to geodetic coordinates," IEEE Transactions on Aerospace and
    // Electronic Systems, vol. 30, pp. 957-961, 1994.

    double r = sqrt(x * x + y * y);
    double Esq =
      kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
    double F = 54 * kSemiminorAxis * kSemiminorAxis * z * z;
    double G = r * r + (1 - kFirstEccentricitySquared) * z * z -
               kFirstEccentricitySquared * Esq;
    double C =
      (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) /
      pow(G, 3);
    double S = cbrt(1 + C + sqrt(C * C + 2 * C));
    double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
    double Q =
      sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
    double r_0 =
      -(P * kFirstEccentricitySquared * r) / (1 + Q) +
      sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q) -
           P * (1 - kFirstEccentricitySquared) * z * z / (Q * (1 + Q)) -
           0.5 * P * r * r);
    double U   = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + z * z);
    double V   = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) +
                    (1 - kFirstEccentricitySquared) * z * z);
    double Z_0 = kSemiminorAxis * kSemiminorAxis * z / (kSemimajorAxis * V);
    *altitude =
      U * (1 - kSemiminorAxis * kSemiminorAxis / (kSemimajorAxis * V));
    *latitude  = atan((z + kSecondEccentricitySquared * Z_0) / r);
    *longitude = atan2(y, x);
  }

  /// \brief Converts the provided ECEF to NED
  ///
  /// \param east NED east in meters
  /// \param north NED north in meters
  /// \param down NED down in meters
  /// \param x The ECEF X psoition in meters
  /// \param y The ECEF Y position in meters
  /// \param z The ECEF Z position in meters
  void ecef2Ned(const double x,
                const double y,
                const double z,
                double*      north,
                double*      east,
                double*      down)
  {
    // Converts ECEF coordinate position into local-tangent-plane NED.
    // Coordinates relative to given ECEF coordinate frame.

    Eigen::Vector3d vect, ret;
    vect(0) = x - initial_ecef_x_;
    vect(1) = y - initial_ecef_y_;
    vect(2) = z - initial_ecef_z_;
    ret     = ecef_to_ned_matrix_ * vect;
    *north  = ret(0);
    *east   = ret(1);
    *down   = -ret(2);
  }

  /// \brief Converts the provided NED to ECEF
  ///
  /// \param east NED east in meters
  /// \param north NED north in meters
  /// \param down NED down in meters
  /// \param x The ECEF X psoition in meters
  /// \param y The ECEF Y position in meters
  /// \param z The ECEF Z position in meters
  void ned2Ecef(const double north,
                const double east,
                const double down,
                double*      x,
                double*      y,
                double*      z)
  {
    // NED (north/east/down) to ECEF coordinates
    Eigen::Vector3d ned, ret;
    ned(0) = north;
    ned(1) = east;
    ned(2) = -down;
    ret    = ned_to_ecef_matrix_ * ned;
    *x     = ret(0) + initial_ecef_x_;
    *y     = ret(1) + initial_ecef_y_;
    *z     = ret(2) + initial_ecef_z_;
  }

  /// \brief Converts the provided LLA to NED
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  /// \param east NED east in meters
  /// \param north NED north in meters
  /// \param down NED down in meters
  void geodetic2Ned(const double latitude,
                    const double longitude,
                    const double altitude,
                    double*      north,
                    double*      east,
                    double*      down)
  {
    // Geodetic position to local NED frame
    double x, y, z;
    geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);
    ecef2Ned(x, y, z, north, east, down);
  }

  /// \brief Converts the provided NED to LLA
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  /// \param east NED east in meters
  /// \param north NED north in meters
  /// \param down NED down in meters
  void ned2Geodetic(const double north,
                    const double east,
                    const double down,
                    double*      latitude,
                    double*      longitude,
                    double*      altitude)
  {
    // Local NED position to geodetic coordinates
    double x, y, z;
    ned2Ecef(north, east, down, &x, &y, &z);
    ecef2Geodetic(x, y, z, latitude, longitude, altitude);
  }

  /// \brief Converts the provided LLA to ENU
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  /// \param east ENU east in meters
  /// \param north ENU north in meters
  /// \param up ENU up in meters
  void geodetic2Enu(const double latitude,
                    const double longitude,
                    const double altitude,
                    double*      east,
                    double*      north,
                    double*      up)
  {
    // Geodetic position to local ENU frame
    double x, y, z;
    geodetic2Ecef(latitude, longitude, altitude, &x, &y, &z);

    double aux_north, aux_east, aux_down;
    ecef2Ned(x, y, z, &aux_north, &aux_east, &aux_down);

    *east  = aux_east;
    *north = aux_north;
    *up    = -aux_down;
  }

  /// \brief Converts the provided ENU to LLA
  ///
  /// \param latitude Latitude in radians
  /// \param longitude Longitude in radians
  /// \param altitude Altitude in meters
  /// \param east ENU east in meters
  /// \param north ENU north in meters
  /// \param up ENU up in meters
  void enu2Geodetic(const double east,
                    const double north,
                    const double up,
                    double*      latitude,
                    double*      longitude,
                    double*      altitude)
  {
    // Local ENU position to geodetic coordinates

    const double aux_north = north;
    const double aux_east  = east;
    const double aux_down  = -up;
    double       x, y, z;
    ned2Ecef(aux_north, aux_east, aux_down, &x, &y, &z);
    ecef2Geodetic(x, y, z, latitude, longitude, altitude);
  }

private:
  inline Eigen::Matrix3d nRe(const double lat_radians, const double lon_radians)
  {
    const double sLat = sin(lat_radians);
    const double sLon = sin(lon_radians);
    const double cLat = cos(lat_radians);
    const double cLon = cos(lon_radians);

    Eigen::Matrix3d ret;
    ret(0, 0) = -sLat * cLon;
    ret(0, 1) = -sLat * sLon;
    ret(0, 2) = cLat;
    ret(1, 0) = -sLon;
    ret(1, 1) = cLon;
    ret(1, 2) = 0.0;
    ret(2, 0) = cLat * cLon;
    ret(2, 1) = cLat * sLon;
    ret(2, 2) = sLat;

    return ret;
  }

  inline double rad2Deg(const double radians) { return (radians / PI) * 180.0; }

  inline double deg2Rad(const double degrees) { return (degrees / 180.0) * PI; }

  double initial_latitude_;
  double initial_longitude_;
  double initial_altitude_;

  double initial_ecef_x_;
  double initial_ecef_y_;
  double initial_ecef_z_;

  Eigen::Matrix3d ecef_to_ned_matrix_;
  Eigen::Matrix3d ned_to_ecef_matrix_;

  bool haveReference_;

};  // class GeodeticConverter
}  // namespace geodetic_converter

#endif  // GEODETIC_CONVERTER_H_