#include <utils/LQR.hpp>

#include <Eigen/Eigenvalues>
#include <iostream>

#include <utils/utilities.hpp>

using namespace std;

namespace sejong{
    void LQR(const Matrix & A,
             const Matrix & B,
             const Matrix & Q,
             const Matrix & R,
             Matrix & S, bool descrete){
        int n = A.rows();

        Matrix Z(2*n, 2*n);

        if(!descrete){
            // sejong::pretty_print(A, std::cout, "A","");
            // sejong::pretty_print(B, std::cout, "B","");
            // sejong::pretty_print(Q, std::cout, "Q","");
            // sejong::pretty_print(R, std::cout, "R","");

            Z.block(0, 0, n, n) = A;

            Z.block(0, n, n, n) = -B * R.inverse() * B.transpose();

            Z.block(n, 0, n, n) = -Q;

            Z.block(n, n, n, n) = -A.transpose();

        } else {

            Matrix Ainv = A.inverse();
            Z.block(0, 0, n, n) = A + B * R.inverse() * B.transpose() * Ainv.transpose() * Q;
            Z.block(0, n, n, n) = -B * R.inverse() * B.transpose() * Ainv.transpose();
            Z.block(n, 0, n, n) = -Ainv.transpose() * Q;
            Z.block(n, n, n, n) = Ainv.transpose();

        }

        Eigen::EigenSolver<Matrix> es(Z);
        Eigen::MatrixXcd V(2*n, n);
        // cout << "The eigenvalues of Z are:" << endl << es.eigenvalues() << endl;

        int col_num(0);
        for (int i(0); i<2*n; ++i){
            if(!descrete){
                if( (es.eigenvalues()[i]).real() < 0.0){
                    V.col(col_num) = es.eigenvectors().col(i);
                    ++col_num;
                }
            } else {
                if( ((es.eigenvalues()[i]).real())*( (es.eigenvalues()[i]).real()) < 1.0){
                    V.col(col_num) = es.eigenvectors().col(i);
                    ++col_num;
                }
            }
        }
        // cout << "The eigenvalues of Z are:" << endl << es.eigenvalues() << endl;
        // cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl;
        // cout << "Finally, S = " << endl << S << endl;

        Eigen::MatrixXcd Stmp = V.block(n, 0, n, n) * (V.block(0, 0, n, n)).inverse();
        S = Stmp.real();
    }
}


/*
 * problem_adjointness.cpp
 *
 *  Created on: 06.03.2018
 *      Author: thies
 */

#include <base/ConstantMesh.h>
#include <base/DiscretizedFunction.h>
#include <base/MacroFunctionParser.h>
#include <base/Norm.h>
#include <base/SpaceTimeMesh.h>
#include <base/Transformation.h>
#include <base/Tuple.h>
#include <base/Util.h>
#include <deal.II/base/exceptions.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/numbers.h>
#include <deal.II/base/point.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria.h>
#include <forward/WaveEquation.h>
#include <forward/WaveEquationBase.h>
#include <gtest/gtest.h>
#include <measurements/FieldMeasure.h>
#include <measurements/Measure.h>
#include <norms/H1H1.h>
#include <norms/H1L2.h>
#include <norms/L2L2.h>
#include <problems/RhoProblem.h>
#include <problems/CProblem.h>
#include <problems/NuProblem.h>
#include <problems/QProblem.h>
#include <stddef.h>
#include <iostream>
#include <map>
#include <memory>
#include <string>
#include <vector>

namespace {

using namespace dealii;
using namespace wavepi::forward;
using namespace wavepi::base;
using namespace wavepi::problems;
using namespace wavepi::measurements;
using namespace wavepi;

template<int dim>
class TestF: public LightFunction<dim> {
public:
   double evaluate(const Point<dim> &p, const double t) const {
       if (p.norm() < 0.5)
         return std::sin(t * 2 * numbers::PI);
      else
         return 0.0;
   }
};

template<int dim>
class TestG: public LightFunction<dim> {
public:
   double evaluate(const Point<dim> &p, const double t) const {
      Point<dim> pc = Point<dim>::unit_vector(0);
      pc *= 0.5;

      return t * std::sin(p.distance(pc) * 2 * numbers::PI);
   }
};

template<int dim>
class TestH: public LightFunction<dim> {
public:
   double evaluate(const Point<dim> &p, const double t) const {
        return p.norm() * t;
   }
};

template<int dim>
double rho(const Point<dim> &p, double t) {
   return p.norm() + t + 1.0;
}

template<int dim>
double c_squared(const Point<dim> &p, double t) {
   double tmp = p.norm() * t + 1.0;

   return tmp * tmp;
}

template<int dim>
class TestC: public LightFunction<dim> {
public:
   double evaluate(const Point<dim> &p, const double t) const {
     return 1.0 / (rho(p, t) * c_squared(p, t));
   }
};

template<int dim>
class TestRho: public LightFunction<dim> {
public:
   double evaluate(const Point<dim> &p, const double t) const {
     return rho(p, t);
   }
};

template<int dim>
class TestNu: public LightFunction<dim> {
public:
   double evaluate(const Point<dim> &p, const double t) const {
    if (t > 1.0) return 0.0;

      return std::abs(p[0]) * t;
   }
};

template<int dim>
class TestQ: public LightFunction<dim> {
public:
   double evaluate(const Point<dim> &p, const double t) const {
    return p.norm() < 0.5 ? std::sin(t / 2 * 2 * numbers::PI) : 0.0;
   }

   static const Point<dim> q_position;
};

template<int dim>
class TestEstimate: public LightFunction<dim> {
public:
   double evaluate(const Point<dim> &p __attribute__((unused)), const double t __attribute__((unused))) const {
           return 2;
   }
};

template<>
const Point<1> TestQ<1>::q_position = Point<1>(-1.0);
template<>
const Point<2> TestQ<2>::q_position = Point<2>(-1.0, 0.5);
template<>
const Point<3> TestQ<3>::q_position = Point<3>(-1.0, 0.5, 0.0);

template<int dim, typename ProblemType>
void run_adjoint_test(int fe_order, int quad_order, int refines, int n_steps,
      std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm_domain,
      std::shared_ptr<Norm<DiscretizedFunction<dim>>> norm_codomain, double tol) {
   auto triangulation = std::make_shared<Triangulation<dim>>();
   GridGenerator::hyper_cube(*triangulation, -1, 1);
   Util::set_all_boundary_ids(*triangulation, 0);
   triangulation->refine_global(refines);

   double t_start = 0.0, t_end = 2.0, dt = t_end / n_steps;
   std::vector<double> times;

   for (size_t i = 0; t_start + i * dt <= t_end; i++)
      times.push_back(t_start + i * dt);

   std::shared_ptr<SpaceTimeMesh<dim>> mesh = std::make_shared<ConstantMesh<dim>>(times, FE_Q<dim>(fe_order),
         QGauss<dim>(quad_order), triangulation);

   deallog << std::endl << "----------  n_dofs / timestep: " << mesh->get_dof_handler(0)->n_dofs();
   deallog << ", n_steps: " << times.size() << "  ----------" << std::endl;

   WaveEquation<dim> wave_eq(mesh);
   wave_eq.set_param_rho(std::make_shared<TestRho<dim>>());
   wave_eq.set_param_c(std::make_shared<TestC<dim>>());
   wave_eq.set_param_q(std::make_shared<TestQ<dim>>());
   wave_eq.set_param_nu(std::make_shared<TestNu<dim>>());

   TestEstimate<dim> est_cont;
   DiscretizedFunction<dim> estimate(mesh, est_cont);
   estimate.set_norm(norm_domain);

   std::vector<std::shared_ptr<Measure<DiscretizedFunction<dim>, DiscretizedFunction<dim>>>> measures;
   measures.push_back(std::make_shared<FieldMeasure<dim>>(mesh, norm_codomain));

   std::map<std::string, double> consts;
   std::vector<std::shared_ptr<Function<dim>>> pulses;
   pulses.push_back(std::make_shared<MacroFunctionParser<dim>>("if(norm{x|y|z} < 0.2, sin(t), 0.0)", consts));

   ProblemType problem(wave_eq, pulses, measures, std::make_shared<IdentityTransform<dim>>(), 0);
   problem.set_adjoint_solver(WaveEquationBase<dim>::WaveEquationAdjoint);

   problem.set_norm_domain(norm_domain);
   problem.set_norm_codomain(norm_codomain);

   auto data_current = problem.forward(estimate);  // have to run forward at least once
   auto A = problem.derivative(estimate);

   double err_avg = 0.0;
   double err_simple;

   for (size_t i = 0; i < 11; i++) {
      std::shared_ptr<DiscretizedFunction<dim>> f, g;

      if (i == 0) {
         TestF<dim> f_cont;
         f = std::make_shared<DiscretizedFunction<dim>>(mesh, f_cont);

         TestG<dim> g_cont;
         g = std::make_shared<DiscretizedFunction<dim>>(mesh, g_cont);
      } else {
         f = std::make_shared<DiscretizedFunction<dim>>(DiscretizedFunction<dim>::noise(mesh));

         // make it a bit smoother, random noise might be a bit too harsh
         f->set_norm(std::make_shared<norms::H1L2<dim>>(0.5));
         f->dot_transform_inverse();

         g = std::make_shared<DiscretizedFunction<dim>>(DiscretizedFunction<dim>::noise(mesh));

         // make it a bit smoother, random noise might be a bit too harsh
         g->set_norm(std::make_shared<norms::H1L2<dim>>(0.5));
         g->dot_transform_inverse();
      }

      // normalize both f and g (not necessary)
      f->set_norm(norm_domain);
      *f *= 1.0 / f->norm();

      g->set_norm(norm_codomain);
      *g *= 1.0 / g->norm();
      Tuple<DiscretizedFunction<dim>> Tg(*g);

      auto Af = A->forward(*f);
      EXPECT_GT(Af.norm(), 0.0);

      auto Astarg = A->adjoint(Tg);
      EXPECT_GT(Astarg.norm(), 0.0);

      double dot_solf_g = Af * Tg;
      double dot_f_adjg = (*f) * Astarg;
      double fg_err = std::abs(dot_solf_g - dot_f_adjg) / (std::abs(dot_solf_g) + 1e-300);

      if (i == 0) {
         // deallog << "simple f,g: " << std::scientific << "(Lf, g) = " << dot_solf_g << ", (f, L*g) = " << dot_f_adjg
         //         << std::endl;
         err_simple = fg_err;
         deallog << std::scientific << "        relative error for simple f,g = " << fg_err << std::endl;
      } else
         err_avg = ((i - 1) * err_avg + fg_err) / i;

      // deallog << std::scientific << "(Lf, g) = " << dot_solf_g << ", (f, L*g) = " << dot_f_adjg
      //        << ", rel. error = " << fg_err << std::endl;

      // EXPECT_LT(zz_err, tol);
   }

   deallog << std::scientific << "average relative error for random f,g = " << err_avg << std::endl;
   EXPECT_LT(err_simple, tol);
   EXPECT_LT(err_avg, tol);
}
}  // namespace

/* Q */

TEST(ProblemAdjointness, AdjointQ1DFE1) {
   for (int i = 3; i < 10; i++)
      run_adjoint_test<1, QProblem<1, DiscretizedFunction<1>>>(1, 3, 6, 1 << i, std::make_shared<norms::L2L2<1>>(),
            std::make_shared<norms::L2L2<1>>(), 1e-1);

   for (int refine = 6; refine >= 1; refine--)
      run_adjoint_test<1, QProblem<1, DiscretizedFunction<1>>>(1, 3, refine, 1 << 9, std::make_shared<norms::L2L2<1>>(),
            std::make_shared<norms::L2L2<1>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointQ1DFE2) {
   for (int i = 3; i < 10; i++)
      run_adjoint_test<1, QProblem<1, DiscretizedFunction<1>>>(2, 6, 4, 1 << i, std::make_shared<norms::L2L2<1>>(),
            std::make_shared<norms::L2L2<1>>(), 1e-1);

   for (int refine = 4; refine >= 1; refine--)
      run_adjoint_test<1, QProblem<1, DiscretizedFunction<1>>>(1, 3, refine, 1 << 9, std::make_shared<norms::L2L2<1>>(),
            std::make_shared<norms::L2L2<1>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointQ2DFE1) {
   for (int i = 3; i < 9; i++)
      run_adjoint_test<2, QProblem<2, DiscretizedFunction<2>>>(1, 3, 5, 1 << i, std::make_shared<norms::L2L2<2>>(),
            std::make_shared<norms::L2L2<2>>(), 1e-1);

   for (int refine = 4; refine >= 1; refine--)
      run_adjoint_test<2, QProblem<2, DiscretizedFunction<2>>>(1, 3, refine, 1 << 8, std::make_shared<norms::L2L2<2>>(),
            std::make_shared<norms::L2L2<2>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointQ2DFE1H1H1) {
   for (int i = 3; i < 9; i++)
      run_adjoint_test<2, QProblem<2, DiscretizedFunction<2>>>(1, 3, 5, 1 << i,
            std::make_shared<norms::H1H1<2>>(0.5, 0.5), std::make_shared<norms::L2L2<2>>(), 1e-1);

   for (int refine = 4; refine >= 1; refine--)
      run_adjoint_test<2, QProblem<2, DiscretizedFunction<2>>>(1, 3, refine, 1 << 8,
            std::make_shared<norms::H1H1<2>>(0.5, 0.5), std::make_shared<norms::L2L2<2>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointQ3DFE1) {
   for (int i = 3; i < 5; i++)
      run_adjoint_test<3, QProblem<3, DiscretizedFunction<3>>>(1, 3, 2, 1 << i, std::make_shared<norms::L2L2<3>>(),
            std::make_shared<norms::L2L2<3>>(), 1e-1);
}

/* A */

TEST(ProblemAdjointness, AdjointRho1DFE1) {
   for (int i = 3; i < 10; i++)
      run_adjoint_test<1, RhoProblem<1, DiscretizedFunction<1>>>(1, 3, 6, 1 << i, std::make_shared<norms::L2L2<1>>(),
            std::make_shared<norms::L2L2<1>>(), 1e-1);

   for (int refine = 6; refine >= 1; refine--)
      run_adjoint_test<1, RhoProblem<1, DiscretizedFunction<1>>>(1, 3, refine, 1 << 9, std::make_shared<norms::L2L2<1>>(),
            std::make_shared<norms::L2L2<1>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointRho1DFE2) {
   for (int i = 3; i < 10; i++)
      run_adjoint_test<1, RhoProblem<1, DiscretizedFunction<1>>>(2, 6, 4, 1 << i, std::make_shared<norms::L2L2<1>>(),
            std::make_shared<norms::L2L2<1>>(), 1e-1);

   for (int refine = 4; refine >= 1; refine--)
      run_adjoint_test<1, RhoProblem<1, DiscretizedFunction<1>>>(1, 3, refine, 1 << 9, std::make_shared<norms::L2L2<1>>(),
            std::make_shared<norms::L2L2<1>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointRho2DFE1) {
   for (int i = 3; i < 9; i++)
      run_adjoint_test<2, RhoProblem<2, DiscretizedFunction<2>>>(1, 3, 5, 1 << i, std::make_shared<norms::L2L2<2>>(),
            std::make_shared<norms::L2L2<2>>(), 1e-1);

   for (int refine = 4; refine >= 1; refine--)
      run_adjoint_test<2, RhoProblem<2, DiscretizedFunction<2>>>(1, 3, refine, 1 << 8, std::make_shared<norms::L2L2<2>>(),
            std::make_shared<norms::L2L2<2>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointRho2DFE1H1H1) {
   for (int i = 3; i < 9; i++)
      run_adjoint_test<2, RhoProblem<2, DiscretizedFunction<2>>>(1, 3, 5, 1 << i,
            std::make_shared<norms::H1H1<2>>(0.5, 0.5), std::make_shared<norms::L2L2<2>>(), 1e-1);

   for (int refine = 4; refine >= 1; refine--)
      run_adjoint_test<2, RhoProblem<2, DiscretizedFunction<2>>>(1, 3, refine, 1 << 8,
            std::make_shared<norms::H1H1<2>>(0.5, 0.5), std::make_shared<norms::L2L2<2>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointRho3DFE1) {
   for (int i = 3; i < 5; i++)
      run_adjoint_test<3, RhoProblem<3, DiscretizedFunction<3>>>(1, 3, 2, 1 << i, std::make_shared<norms::L2L2<3>>(),
            std::make_shared<norms::L2L2<3>>(), 1e-1);
}

/* Nu */

TEST(ProblemAdjointness, AdjointNu1DFE1) {
   for (int i = 3; i < 10; i++)
      run_adjoint_test<1, NuProblem<1, DiscretizedFunction<1>>>(1, 3, 6, 1 << i, std::make_shared<norms::L2L2<1>>(),
            std::make_shared<norms::L2L2<1>>(), 1e-1);

   for (int refine = 6; refine >= 1; refine--)
      run_adjoint_test<1, NuProblem<1, DiscretizedFunction<1>>>(1, 3, refine, 1 << 9,
            std::make_shared<norms::L2L2<1>>(), std::make_shared<norms::L2L2<1>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointNu1DFE2) {
   for (int i = 3; i < 10; i++)
      run_adjoint_test<1, NuProblem<1, DiscretizedFunction<1>>>(2, 6, 4, 1 << i, std::make_shared<norms::L2L2<1>>(),
            std::make_shared<norms::L2L2<1>>(), 1e-1);

   for (int refine = 4; refine >= 1; refine--)
      run_adjoint_test<1, NuProblem<1, DiscretizedFunction<1>>>(1, 3, refine, 1 << 9,
            std::make_shared<norms::L2L2<1>>(), std::make_shared<norms::L2L2<1>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointNu2DFE1) {
   for (int i = 3; i < 9; i++)
      run_adjoint_test<2, NuProblem<2, DiscretizedFunction<2>>>(1, 3, 5, 1 << i, std::make_shared<norms::L2L2<2>>(),
            std::make_shared<norms::L2L2<2>>(), 1e-1);

   for (int refine = 4; refine >= 1; refine--)
      run_adjoint_test<2, NuProblem<2, DiscretizedFunction<2>>>(1, 3, refine, 1 << 8,
            std::make_shared<norms::L2L2<2>>(), std::make_shared<norms::L2L2<2>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointNu2DFE1H1H1) {
   for (int i = 3; i < 9; i++)
      run_adjoint_test<2, NuProblem<2, DiscretizedFunction<2>>>(1, 3, 5, 1 << i,
            std::make_shared<norms::H1H1<2>>(0.5, 0.5), std::make_shared<norms::L2L2<2>>(), 1e-1);

   for (int refine = 4; refine >= 1; refine--)
      run_adjoint_test<2, NuProblem<2, DiscretizedFunction<2>>>(1, 3, refine, 1 << 8,
            std::make_shared<norms::H1H1<2>>(0.5, 0.5), std::make_shared<norms::L2L2<2>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointNu3DFE1) {
   for (int i = 3; i < 5; i++)
      run_adjoint_test<3, NuProblem<3, DiscretizedFunction<3>>>(1, 3, 2, 1 << i, std::make_shared<norms::L2L2<3>>(),
            std::make_shared<norms::L2L2<3>>(), 1e-1);
}

/* C */

TEST(ProblemAdjointness, AdjointC1DFE1) {
   for (int i = 3; i < 10; i++)
      run_adjoint_test<1, CProblem<1, DiscretizedFunction<1>>>(1, 3, 6, 1 << i, std::make_shared<norms::L2L2<1>>(),
            std::make_shared<norms::L2L2<1>>(), 1e-1);

   for (int refine = 6; refine >= 1; refine--)
      run_adjoint_test<1, CProblem<1, DiscretizedFunction<1>>>(1, 3, refine, 1 << 9, std::make_shared<norms::L2L2<1>>(),
            std::make_shared<norms::L2L2<1>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointC1DFE2) {
   for (int i = 3; i < 10; i++)
      run_adjoint_test<1, CProblem<1, DiscretizedFunction<1>>>(2, 6, 4, 1 << i, std::make_shared<norms::L2L2<1>>(),
            std::make_shared<norms::L2L2<1>>(), 1e-1);

   for (int refine = 4; refine >= 1; refine--)
      run_adjoint_test<1, CProblem<1, DiscretizedFunction<1>>>(1, 3, refine, 1 << 9, std::make_shared<norms::L2L2<1>>(),
            std::make_shared<norms::L2L2<1>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointC2DFE1) {
   for (int i = 3; i < 9; i++)
      run_adjoint_test<2, CProblem<2, DiscretizedFunction<2>>>(1, 3, 5, 1 << i, std::make_shared<norms::L2L2<2>>(),
            std::make_shared<norms::L2L2<2>>(), 1e-1);

   for (int refine = 4; refine >= 1; refine--)
      run_adjoint_test<2, CProblem<2, DiscretizedFunction<2>>>(1, 3, refine, 1 << 8, std::make_shared<norms::L2L2<2>>(),
            std::make_shared<norms::L2L2<2>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointC2DFE1H1H1) {
   for (int i = 3; i < 9; i++)
      run_adjoint_test<2, CProblem<2, DiscretizedFunction<2>>>(1, 3, 5, 1 << i,
            std::make_shared<norms::H1H1<2>>(0.5, 0.5), std::make_shared<norms::L2L2<2>>(), 1e-1);

   for (int refine = 4; refine >= 1; refine--)
      run_adjoint_test<2, CProblem<2, DiscretizedFunction<2>>>(1, 3, refine, 1 << 8,
            std::make_shared<norms::H1H1<2>>(0.5, 0.5), std::make_shared<norms::L2L2<2>>(), 1e-1);
}

TEST(ProblemAdjointness, AdjointC3DFE1) {
   for (int i = 3; i < 5; i++)
      run_adjoint_test<3, CProblem<3, DiscretizedFunction<3>>>(1, 3, 2, 1 << i, std::make_shared<norms::L2L2<3>>(),
            std::make_shared<norms::L2L2<3>>(), 1e-1);
}


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_MATRIXIO_HPP
#define MCL_MATRIXIO_HPP 1

#include <Eigen/Dense>
#include <fstream>

namespace mcl
{

template <typename Derived>
static inline void write_eigen_matrix(const std::string& fn, const Eigen::MatrixBase<Derived>& A)
{
	std::ofstream file(fn);
	if (file.is_open()) { file << A; }
}

template<typename T, int dim = Eigen::Dynamic>
static inline bool read_eigen_matrix(const std::string &fn, Eigen::Matrix<T,Eigen::Dynamic,dim> &A)
{
	// dim most often -1 (Eigen::Dynamic)

	std::vector<std::vector<T> > vals;
	std::ifstream ifs;
	ifs.open(fn.c_str());
	if (!ifs) { return false; }
	int cols = 0;

	std::string line;
	while(std::getline(ifs, line))
	{
		vals.emplace_back(std::vector<T>());
		std::istringstream ss(line);
		while (ss.good())
		{
			T val; ss >> val;
			vals.back().emplace_back(val);
		}
		cols = std::max(cols, int(vals.back().size()));
	}
	ifs.close();

	int rows = vals.size();
	if(rows==0 || cols==0)
	{
		// File exists, it's just empty
		A = Eigen::Matrix<T,Eigen::Dynamic,dim>();
		return true;
	}
	cols = dim > 0 ? std::min(cols, dim) : cols;
	A = Eigen::Matrix<T,Eigen::Dynamic,dim>::Zero(rows,cols);
	int nx = vals.size();
	for(int i=0; i<nx; ++i)
	{
		for(int j=0; j<cols; ++j)
		{
			A(i,j) = vals[i][j];
		}
	}
	return true;

}; // end read matrix

} // end namespace mcl

#endif


#include <gen/terrain.hpp>

#include <boost/graph/astar_search.hpp>
#include <gen/graphutils.hpp>

#include <gen/poissonsampling.hpp>
#include <math/mathtools.hpp>

#include <armadillo>

namespace eXl
{

  class ArmaLaplaceMatrix : public LaplaceMatrix
  {
  public:
    arma::sp_mat m_Matrix;
  };

  struct found_goal{};

  template <class Vertex>
  class astar_goal_visitor : public boost::default_astar_visitor
  {
  public:
    inline astar_goal_visitor(Vertex goal) : m_goal(goal) {}
    template <class Graph>
    void examine_vertex(Vertex u, Graph& g) {
      if(u == m_goal)
        throw found_goal();
    }
  private:
    Vertex m_goal;
  };

  template<typename PosMap>
  class VoroDiagGraphDistance : public boost::astar_heuristic<VoronoiGraph::CellGraphImpl, float>
  {
  public:
    typedef boost::graph_traits<VoronoiGraph::CellGraphImpl>::vertex_descriptor Vertex;
    VoroDiagGraphDistance(PosMap const& iCenters, Vertex iGoal, TIndexMap<VoronoiGraph::CellGraphImpl> const& iIndex)
      : m_CellCenter(iCenters), m_Goal(iGoal), m_Index(iIndex) {}
    float operator()(Vertex u)
    {
      unsigned int goalIdx = boost::get(m_Index, m_Goal);
      unsigned int idx = boost::get(m_Index, u);

      Vector2f goalPos = m_CellCenter(goalIdx);
      Vector2f curPos = m_CellCenter(idx);

      return (curPos - goalPos).Length();
    }
  private:
    PosMap const& m_CellCenter;
    TIndexMap<VoronoiGraph::CellGraphImpl> const& m_Index;
    Vertex m_Goal;
  };

  template<typename PosMap>
  void FindPath(VoronoiGraph::CellGraph const& iGraph,
    PosMap const& iCellCenters,
    VoronoiGraph::CellGraphImpl::vertex_descriptor start,
    VoronoiGraph::CellGraphImpl::vertex_descriptor goal,
    Vector<VoronoiGraph::CellGraphImpl::vertex_descriptor>& oPath)
  {
    IndexMap<VoronoiGraph::CellGraphImpl> index(iGraph.m_CellGraph);
    Vector<VoronoiGraph::CellGraphImpl::vertex_descriptor> p(boost::num_vertices(iGraph.m_CellGraph), start);
    Vector<float> d(boost::num_vertices(iGraph.m_CellGraph));
    bool found = false;
    try
    {
      // call astar named parameter interface
      boost::astar_search_tree
      (iGraph.m_CellGraph, start,
        VoroDiagGraphDistance<PosMap>(iCellCenters, goal, index),
        //iFactory.Make<boost::filtered_graph<OutGraph, boost::keep_all, TagFilter> >(filteredGr, goal),
        boost::predecessor_map(boost::make_iterator_property_map(p.begin(), index)).
        distance_map(make_iterator_property_map(d.begin(), index)).
        visitor(astar_goal_visitor<VoronoiGraph::CellGraphImpl::vertex_descriptor>(goal)));
    }
    catch(found_goal )
    { // found a path to the goal
      Vector<VoronoiGraph::CellGraphImpl::vertex_descriptor> path;
      for(auto v = goal;; v = p[v])
      {
        path.push_back(v);

        if(p[v] == v)
          break;
      }
      oPath.swap(path);
      found = true;
    }
  }

  struct TerrainPosMap
  {
    TerrainPosMap(Vector<Terrain::CellProperties> const& iCells)
      :m_Cells(iCells){}

    Vector2f const& operator()(unsigned int i) const {return m_Cells[i].position;}

    Vector<Terrain::CellProperties> const& m_Cells;
  };

  Terrain::Terrain()
  {
  }

  void Terrain::MakeGrid(Vector2f const& iSize, Vector2i const& iGridSize)
  {
    m_Size = iSize;

    Vector2f gridStep(iSize.X() / iGridSize.X(), iSize.Y() / iGridSize.Y());

    m_QueryBox = AABB2Df(-gridStep * 0.51, gridStep * 1.02);

    m_Cells.clear();
    m_Graph.m_CellGraph.clear();
    m_Graph.m_Edges.clear();
    m_Graph.m_Vertices.clear();
    m_Neigh.clear();
    m_Index.clear();

    uint32_t totNumCells = iGridSize.X() * iGridSize.Y();

    m_Cells.reserve(totNumCells);
    m_Neigh.reserve(totNumCells * 4);
    for (uint32_t i = 0; i < totNumCells; ++i)
    {
      boost::add_vertex(m_Graph.m_CellGraph);
    }

    for (int32_t y = 0; y < iGridSize.Y() + 1; ++y)
    {
      for (int32_t x = 0; x < iGridSize.X() + 1; ++x)
      {
        VoronoiGraph::Vertex gridVtx;
        gridVtx.m_Position = Vector2f(gridStep.X() * x, gridStep.Y() * y);
        m_Graph.m_Vertices.push_back(gridVtx);
      }
    }

    uint32_t cellOffset = 0;
    Vector<CellLoc> locs;
    locs.reserve(totNumCells);
    for (int32_t y = 0; y < iGridSize.Y(); ++y)
    {
      for (int32_t x = 0; x < iGridSize.X(); ++x)
      {
        uint32_t gridLowLeftCorner = y * (iGridSize.X() + 1) + x;

        CellProperties cell;
        cell.neighCount = 0;
        cell.position = Vector2f(((float)x + 0.5) * gridStep.X(), ((float)y + 0.5) * gridStep.Y());
        cell.neighStart = m_Neigh.size();

        Vector2f const& cellPos = cell.position;
        locs.push_back(std::make_pair(AABB2Df(cellPos - gridStep * 0.5, gridStep), cellOffset));

        if (x > 0)
        {
          cell.neighCount++;
          m_Neigh.push_back(cellOffset - 1);
        //  auto edgeDesc = boost::add_edge(cellOffset, cellOffset - 1, m_Graph.m_CellGraph);
        //  VoronoiGraph::Edge newEdge;
        //  newEdge.m_Pt1 = gridLowLeftCorner;
        //  newEdge.m_Pt2 = gridLowLeftCorner + iGridSize.X() + 1;
        //  m_Graph.m_Edges.insert(std::make_pair(edgeDesc, newEdge));
        }

        if (x < iGridSize.X() - 1)
        {
          cell.neighCount++;
          m_Neigh.push_back(cellOffset + 1);
          auto edgeDesc = boost::add_edge(cellOffset, cellOffset + 1, m_Graph.m_CellGraph).first;
          VoronoiGraph::Edge newEdge;
          newEdge.m_Pt1 = gridLowLeftCorner + 1;
          newEdge.m_Pt2 = gridLowLeftCorner + 1 + iGridSize.X() + 1;
          m_Graph.m_Edges[edgeDesc] = newEdge;
        }

        if (y > 0)
        {
          cell.neighCount++;
          m_Neigh.push_back(cellOffset - iGridSize.X());
        //  auto edgeDesc = boost::add_edge(cellOffset, cellOffset - iGridSize.X(), m_Graph.m_CellGraph);
        //  VoronoiGraph::Edge newEdge;
        //  newEdge.m_Pt1 = gridLowLeftCorner;
        //  newEdge.m_Pt2 = gridLowLeftCorner + 1;
        //  m_Graph.m_Edges.insert(std::make_pair(edgeDesc, newEdge));
        }

        if (y < iGridSize.Y() - 1)
        {
          cell.neighCount++;
          m_Neigh.push_back(cellOffset + iGridSize.X());

          auto edgeDesc = boost::add_edge(cellOffset, cellOffset + iGridSize.X(), m_Graph.m_CellGraph).first;
          VoronoiGraph::Edge newEdge;
          newEdge.m_Pt1 = gridLowLeftCorner + iGridSize.X() + 1;
          newEdge.m_Pt2 = gridLowLeftCorner + 1 + iGridSize.X() + 1;
          m_Graph.m_Edges[edgeDesc] = newEdge;
        }
        m_Cells.push_back(cell);
        ++cellOffset;
      }
    }

    m_Index = CellIndex(locs);
  }

  void Terrain::MakeCirclePacking(Random& iRand, Vector2f const& iSize, float iCellSize)
  {
    m_Size = iSize;

    m_QueryBox = AABB2Df(-iCellSize * 1.1, -iCellSize * 1.1, iCellSize * 1.1, iCellSize * 1.1);

    m_Cells.clear();
    m_Graph.m_CellGraph.clear();
    m_Graph.m_Edges.clear();
    m_Graph.m_Vertices.clear();
    m_Neigh.clear();
    m_Index.clear();

    Polygoni poly(AABB2Di(Vector2i::ZERO, Vector2i(Mathf::Ceil(m_Size.X()), Mathf::Ceil(m_Size.Y()))));
    PoissonDiskSampling sampler(poly, iRand);

    sampler.Sample(iCellSize, iCellSize);

    Vector<Vector2d> samples;
    Vector<Vector2f> cellCenter;
    sampler.GetLayer(0, samples);
    //sampler.GetLayer(1, samples[1]);
    //sampler.GetLayer(2, samples[2]);

    unsigned int const totSamples = samples.size() /*+ samples[1].size() + samples[2].size()*/;

    VoronoiGraph voroGr;

    for(unsigned int j = 0; j<samples.size(); ++j)
    {
      voroGr.AddCircle(iCellSize, MathTools::ToFVec(samples[j]));
      cellCenter.push_back(MathTools::ToFVec(samples[j]));

      CellProperties props;
      props.position = cellCenter.back();
      m_Cells.push_back(props);
    }

    voroGr.BuildGraph(m_Graph, true);

    {
      Vector<CellLoc> locs(cellCenter.size());
      for(unsigned int i = 0; i<cellCenter.size(); ++i)
      {
        Vector2f const& cellPos = cellCenter[i];
        locs.push_back(std::make_pair(AABB2Df(cellPos - Vector2f::ONE * iCellSize, Vector2f::ONE * 2 * iCellSize), i));
      }
      m_Index = CellIndex(locs);
    }

    unsigned int const numCells = m_Cells.size();
    Vector<unsigned int> sortedPoints;

    for (unsigned int i = 0; i < numCells; ++i)
    {
      m_Cells[i].neighCount = 0;
      sortedPoints.clear();
      Vector2f center = m_Cells[i].position;

      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      boost::tie(edgesBegin, edgesEnd) = boost::out_edges(i, m_Graph.m_CellGraph);
      unsigned int numNeigh = std::distance(edgesBegin, edgesEnd);
      if(edgesBegin != edgesEnd)
      {
        unsigned int firstPoint = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        VoronoiGraph::Edge firstEdge = m_Graph.m_Edges.find(*edgesBegin)->second;
        ++edgesBegin;
        while(firstPoint == numCells)
        {
          --numNeigh;
          firstPoint = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
          firstEdge = m_Graph.m_Edges.find(*edgesBegin)->second;
          ++edgesBegin;
        }

        Segmentf::SortByAngle sortMeth(m_Cells[firstPoint].position);
        std::map<Vector2f, unsigned int, Segmentf::SortByAngle > sortedPointsMap(sortMeth);

        for (; edgesBegin != edgesEnd; ++edgesBegin)
        {
          unsigned int target = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
          if(target == numCells)
          {
            --numNeigh;
            continue;
          }
          sortedPointsMap.insert(std::make_pair(m_Cells[target].position, target));
        }
        sortedPoints.push_back(firstPoint);
        for(auto pair : sortedPointsMap)
        {
          sortedPoints.push_back(pair.second);
        }

        m_Cells[i].neighStart = m_Neigh.size();

        for(uint32_t curNeighIdx = 0; curNeighIdx < numNeigh; ++curNeighIdx)
        {
          unsigned int curNeigh = sortedPoints[curNeighIdx];

          m_Neigh.push_back(curNeigh);
        }

        m_Cells[i].neighCount = numNeigh;
      }
    }
  }

  bool Terrain::GetClosestCell(Vector2f const& iPos, uint32_t& oCellIdx)
  {
    AABB2Df queryBox = m_QueryBox;
    queryBox.m_Data[0] += iPos;
    queryBox.m_Data[1] += iPos;

    Vector<CellLoc> results;
    m_Index.query(boost::geometry::index::intersects(queryBox), std::back_inserter(results));

    if (results.empty())
    {
      return false;
    }

    float minDist = FLT_MAX;
    for (auto const& cellLoc : results)
    {
      CellProperties const& cell = m_Cells[cellLoc.second];
      float const distSq = (cell.position - iPos).SquaredLength();
      if (minDist > distSq)
      {
        oCellIdx = cellLoc.second;
        minDist = distSq;
      }
    }

    return true;
  }

  void Terrain::GetLineCells(Segmentf const& iSeg, float iRadius, Vector<unsigned int>& oCells)
  {
    oCells.clear();

    AABB2Df queryBox;
    queryBox.m_Data[0] = iSeg.m_Ext1;
    queryBox.m_Data[1] = iSeg.m_Ext1;
    queryBox.Absorb(iSeg.m_Ext2);
    queryBox.m_Data[0] -= Vector2f::ONE * iRadius;
    queryBox.m_Data[1] += Vector2f::ONE * iRadius;

    Vector<CellLoc> results;
    m_Index.query(boost::geometry::index::intersects(queryBox), std::back_inserter(results));

    for (auto const& cellLoc : results)
    {
      CellProperties const& cell = m_Cells[cellLoc.second];

      Vector2f dir;
      float dist = iSeg.NearestPointSeg(cell.position, dir);
      if (dist < iRadius)
      {
        oCells.push_back(cellLoc.second);
      }
    }
  }

  void Terrain::GetDiskCells(Vector2f const& iCenter, float iRadius, Vector<unsigned int>& oCells)
  {
    oCells.clear();

    AABB2Df queryBox(iCenter - iRadius * Vector2f::ONE, Vector2f::ONE * 2 * iRadius);

    Vector<CellLoc> results;
    m_Index.query(boost::geometry::index::intersects(queryBox), std::back_inserter(results));

    for(auto cell : results)
    {
      if((m_Cells[cell.second].position - iCenter).Length() < iRadius)
      {
        oCells.push_back(cell.second);
      }
    }
  }

  void Terrain::BuildLaplaceMatrix(ConductivityGetter& iConductivity, LaplaceMatrix*& oMat)
  {
    unsigned int const numCells = m_Cells.size();

    if (oMat != nullptr)
    {
      delete oMat;
    }

    ArmaLaplaceMatrix* matrix = eXl_NEW(ArmaLaplaceMatrix);

    oMat = matrix;

    arma::sp_mat& mat = matrix->m_Matrix;

    mat.set_size(numCells, numCells);

    //Vector<unsigned int> sortedPoints;
    //Vector<Vector2f> cellPoints;

    for (unsigned int i = 0; i < numCells; ++i)
    {
      //cellPoints.clear();
      //sortedPoints.clear();
      float sum = 0.0;

      Vector2f center = m_Cells[i].position;

      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      boost::tie(edgesBegin, edgesEnd) = boost::out_edges(i, m_Graph.m_CellGraph);

      for (; edgesBegin != edgesEnd; ++edgesBegin)
      {
        unsigned int target = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        if (target == numCells)
        {
          continue;
        }
        VoronoiGraph::Edge const& curEdge = m_Graph.m_Edges.find(*edgesBegin)->second;

        float edgeLen = (m_Graph.m_Vertices[curEdge.m_Pt2].m_Position - m_Graph.m_Vertices[curEdge.m_Pt1].m_Position).Length();
        float cellDist = (center - m_Cells[target].position).Length();

        float curConnectionValue = iConductivity(i, target, edgeLen, cellDist);
        sum -= curConnectionValue;

        mat.at(i, target) = 0.5 * curConnectionValue;
      }

      mat.at(i, i) = 0.5 * sum;
    }
  }

  void Terrain::BuildSmoothingMatrix(LaplaceMatrix*& oMat)
  {
    unsigned int const numCells = m_Cells.size();

    if (oMat != nullptr)
    {
      delete oMat;
    }

    ArmaLaplaceMatrix* matrix = eXl_NEW(ArmaLaplaceMatrix);

    oMat = matrix;

    arma::sp_mat& mat = matrix->m_Matrix;

    mat.set_size(numCells, numCells);

    Vector<unsigned int> sortedPoints;
    Vector<Vector2f> cellPoints;

    for (unsigned int i = 0; i < numCells; ++i)
    {
      cellPoints.clear();
      sortedPoints.clear();
      Vector2f center = m_Cells[i].position;

      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      boost::tie(edgesBegin, edgesEnd) = boost::out_edges(i, m_Graph.m_CellGraph);
      unsigned int numNeigh = std::distance(edgesBegin, edgesEnd);
      if(edgesBegin != edgesEnd)
      {
        unsigned int firstPoint = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        VoronoiGraph::Edge firstEdge = m_Graph.m_Edges.find(*edgesBegin)->second;
        ++edgesBegin;
        while(firstPoint == numCells)
        {
          --numNeigh;
          firstPoint = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
          firstEdge = m_Graph.m_Edges.find(*edgesBegin)->second;
          ++edgesBegin;
        }

        Segmentf::SortByAngle sortMeth(m_Cells[firstPoint].position);
        std::map<Vector2f, unsigned int, Segmentf::SortByAngle > sortedPointsMap(sortMeth);

        cellPoints.push_back(m_Graph.m_Vertices[firstEdge.m_Pt1].m_Position);
        cellPoints.push_back(m_Graph.m_Vertices[firstEdge.m_Pt2].m_Position);

        for (; edgesBegin != edgesEnd; ++edgesBegin)
        {
          unsigned int target = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
          if(target == numCells)
          {
            --numNeigh;
            continue;
          }
          VoronoiGraph::Edge const& curEdge = m_Graph.m_Edges.find(*edgesBegin)->second;
          cellPoints.push_back(m_Graph.m_Vertices[curEdge.m_Pt1].m_Position);
          cellPoints.push_back(m_Graph.m_Vertices[curEdge.m_Pt2].m_Position);
          sortedPointsMap.insert(std::make_pair(m_Cells[target].position, target));
        }
        sortedPoints.push_back(firstPoint);
        for(auto pair : sortedPointsMap)
        {
          sortedPoints.push_back(pair.second);
        }

        Polygonf cellPoly;
        Polygonf::ConvexHull(cellPoints, cellPoly);

        float area = cellPoly.Area();
        float sum = 0.0;

        for(uint32_t curNeighIdx = 0; curNeighIdx < numNeigh; ++curNeighIdx)
        {
          float alphaI = 0.0;
          float betaI = 0.0;

          unsigned int curNeigh = sortedPoints[curNeighIdx];
          unsigned int prevNeigh = sortedPoints[curNeighIdx == 0 ? numNeigh - 1 : curNeighIdx - 1];
          unsigned int nextNeigh = sortedPoints[curNeighIdx == numNeigh - 1 ? 0 : curNeighIdx + 1];

          Vector2f next1 = m_Cells[curNeigh].position - m_Cells[nextNeigh].position;
          Vector2f next2 = center - m_Cells[nextNeigh].position;

          float crossA = Segmentf::Cross(next1, next2);
          if(Mathf::Abs(crossA) > Mathf::EPSILON)
          {
            alphaI = Mathf::Abs(next1.Dot(next2) / crossA);
          }

          Vector2f prev1 = center - m_Cells[prevNeigh].position;
          Vector2f prev2 = m_Cells[curNeigh].position - m_Cells[prevNeigh].position;

          float crossB = Segmentf::Cross(prev1, prev2);
          if(Mathf::Abs(crossB) > Mathf::EPSILON)
          {
            betaI = Mathf::Abs(prev1.Dot(prev2) / crossB);
          }
          float curConnectionValue = (alphaI + betaI / area);
          sum -= curConnectionValue;

          mat.at(i, curNeigh) = 0.5 * curConnectionValue;
        }

        mat.at(i, i) = 0.5 * sum;
      }
    }
  }

  void Terrain::Diffusion(LaplaceMatrix const* iMatrix, float iHeatCoefficient, Vector<float>& ioValues)
  {
    ArmaLaplaceMatrix const* matrixContainer = static_cast<ArmaLaplaceMatrix const*>(iMatrix);

    unsigned int const numCells = m_Cells.size();

    eXl_ASSERT(numCells == ioValues.size());

    arma::colvec inputTemp(numCells);
    for (unsigned int i = 0; i < numCells; ++i)
    {
      inputTemp.at(i) = ioValues[i];
    }

    arma::colvec tempRes(numCells, arma::fill::zeros);

    arma::sp_mat resEqn(numCells, numCells);
    arma::sp_mat idMat(numCells, numCells);
    for (unsigned int i = 0; i < numCells; ++i)
      idMat.at(i, i) = 1.0;

    resEqn = idMat - matrixContainer->m_Matrix * iHeatCoefficient;

    arma::spsolve(tempRes, resEqn, inputTemp);

    for (unsigned int i = 0; i < numCells; ++i)
    {
      ioValues[i] = tempRes.at(i);
    }
  }

  size_t Terrain::GetNumMeshVertices()
  {
    size_t numVtx = 2 * m_Graph.m_Edges.size();
    for (auto const& cell : m_Cells)
    {
      numVtx += 1 + cell.neighCount;
    }

    return numVtx;
  }

  size_t Terrain::GetNumMeshIndices()
  {
    return GetNumSmoothMeshIndices() /*+ 6 * m_Graph.m_Edges.size()*/;
  }

  size_t Terrain::GetNumSmoothMeshVertices()
  {
    return m_Cells.size() + m_Graph.m_Vertices.size();
  }

  size_t Terrain::GetNumSmoothMeshIndices()
  {
    size_t numIndices = 0;
    for (auto const& cell : m_Cells)
    {
      numIndices += 3 * cell.neighCount;
    }

    return numIndices;
  }

  void Terrain::BuildMesh(Vector3f const& iScale, const Vector<float>& iHeight, OutputBuffer oPositions, OutputBuffer oNormals, OutputBuffer oTexCoords, OutputBuffer oIdx)
  {
    uint32_t maxIndices = GetNumMeshIndices();
    uint32_t counterCheck = 0;

    Vector3f* positionsPtr = reinterpret_cast<Vector3f*>(oPositions.data);
    float* texCoordPtr = reinterpret_cast<float*>(oTexCoords.data);
    Vector3f* normalsPtr = reinterpret_cast<Vector3f*>(oNormals.data);
    uint32_t* indicesPtr = reinterpret_cast<uint32_t*>(oIdx.data);
    uint32_t addVtxCounter = 0;

    UnorderedMap<uint32_t, uint32_t> vtxAlloc;

    for (unsigned int i = 0; i < m_Cells.size(); ++i)
    {
      uint32_t cellCenterIdx = addVtxCounter;
      vtxAlloc.clear();
      Vector2f ptCenter = m_Cells[i].position;

      float height = iHeight[i];

      Vector3f pos(iScale.X() * ptCenter.X(), iScale.Y() * ptCenter.Y(), iScale.Z() * height);

      *positionsPtr = pos;
      texCoordPtr[0] = ptCenter.X() / m_Size.X();
      texCoordPtr[1] = ptCenter.Y() / m_Size.Y();
      *normalsPtr = Vector3f::UNIT_Z;

      oPositions.NextItem(positionsPtr);
      oTexCoords.NextItem(texCoordPtr);
      oNormals.NextItem(normalsPtr);
      ++addVtxCounter;

      VoronoiGraph::CellGraphImpl::vertex_descriptor curCell = i;
      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      unsigned int numPts = 0;
      for (boost::tie(edgesBegin, edgesEnd) = boost::out_edges(curCell, m_Graph.m_CellGraph); edgesBegin != edgesEnd; ++edgesBegin)
      {
        unsigned int target = curCell == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        if (target >= m_Cells.size())
        {
          continue;
        }

        VoronoiGraph::Edge const& curEdge = m_Graph.m_Edges.find(*edgesBegin)->second;

        auto insertRes = vtxAlloc.insert(std::make_pair(curEdge.m_Pt1, addVtxCounter));
        if (insertRes.second)
        {
          *positionsPtr = Vector3f(iScale.X() * m_Graph.m_Vertices[curEdge.m_Pt1].m_Position.X(),
            iScale.Y() * m_Graph.m_Vertices[curEdge.m_Pt1].m_Position.Y(),
            iScale.Z() * height);
          texCoordPtr[0] = m_Graph.m_Vertices[curEdge.m_Pt1].m_Position.X() / m_Size.X();
          texCoordPtr[1] = m_Graph.m_Vertices[curEdge.m_Pt1].m_Position.Y() / m_Size.Y();
          *normalsPtr = Vector3f::UNIT_Z;

          oPositions.NextItem(positionsPtr);
          oTexCoords.NextItem(texCoordPtr);
          oNormals.NextItem(normalsPtr);
          ++addVtxCounter;
        }
        insertRes = vtxAlloc.insert(std::make_pair(curEdge.m_Pt2, addVtxCounter));
        if (insertRes.second)
        {
          *positionsPtr = Vector3f(iScale.X() * m_Graph.m_Vertices[curEdge.m_Pt2].m_Position.X(),
            iScale.Y() * m_Graph.m_Vertices[curEdge.m_Pt2].m_Position.Y(),
            iScale.Z() * height);
          texCoordPtr[0] = m_Graph.m_Vertices[curEdge.m_Pt2].m_Position.X() / m_Size.X();
          texCoordPtr[1] = m_Graph.m_Vertices[curEdge.m_Pt2].m_Position.Y() / m_Size.Y();
          *normalsPtr = Vector3f::UNIT_Z;

          oPositions.NextItem(positionsPtr);
          oTexCoords.NextItem(texCoordPtr);
          oNormals.NextItem(normalsPtr);
          ++addVtxCounter;
        }
      }

      for (boost::tie(edgesBegin, edgesEnd) = boost::out_edges(curCell, m_Graph.m_CellGraph); edgesBegin != edgesEnd; ++edgesBegin)
      {
        unsigned int target = curCell == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        if (target >= m_Cells.size())
        {
          continue;
        }

        VoronoiGraph::Edge const& curEdge = m_Graph.m_Edges.find(*edgesBegin)->second;

        uint32_t pt1Idx = vtxAlloc[curEdge.m_Pt1];
        uint32_t pt2Idx = vtxAlloc[curEdge.m_Pt2];

        Vector3f const& otherPt1 = *oPositions.Item<Vector3f>(pt1Idx);
        Vector3f const& otherPt2 = *oPositions.Item<Vector3f>(pt2Idx);

        Vector3f locNormal = (pos - otherPt1).Cross(pos - otherPt2);

        if (locNormal.Dot(Vector3f::UNIT_Z) < -Mathf::EPSILON)
        {
          std::swap(pt1Idx, pt2Idx);
        }

        eXl_ASSERT(counterCheck < maxIndices);

        *indicesPtr = cellCenterIdx;
        oIdx.NextItem(indicesPtr);
        *indicesPtr = pt1Idx;
        oIdx.NextItem(indicesPtr);
        *indicesPtr = pt2Idx;
        oIdx.NextItem(indicesPtr);

        counterCheck += 3;
      }
    }

    for (unsigned int i = 0; i < m_Graph.m_Edges.size(); ++i)
    {
      //unsigned int numRef = 0;
      //float height = 0.0;
      //for (auto ref : edgePtRef[i])
      //{
      //  height += oPositions.Item<float>(ref)[2];
      //  ++numRef;
      //}
      //
      //if (numRef > 0)
      //  height /= numRef;
      //
      //
      //positionsPtr[0] = iScale.X() * m_Graph.m_Vertices[i].m_Position.X();
      //positionsPtr[1] = iScale.Y() * m_Graph.m_Vertices[i].m_Position.Y();
      //positionsPtr[2] = height;
      //texCoordPtr[0] = m_Graph.m_Vertices[i].m_Position.X() / m_Size.X();
      //texCoordPtr[1] = m_Graph.m_Vertices[i].m_Position.Y() / m_Size.Y();
      //*normalsPtr = Vector3f::ZERO;
      //
      //oPositions.NextItem(positionsPtr);
      //oTexCoords.NextItem(texCoordPtr);
      //oNormals.NextItem(normalsPtr);
    }
  }

  void Terrain::BuildSmoothMesh(Vector3f const& iScale, const Vector<float>& iHeight, OutputBuffer oPositions, OutputBuffer oNormals, OutputBuffer oTexCoords, OutputBuffer oIdx)
  {
    unsigned int const numCells = m_Cells.size();

    Vector<Set<unsigned int> > edgePtRef(m_Graph.m_Vertices.size());

    float* positionsPtr = reinterpret_cast<float*>(oPositions.data);
    float* texCoordPtr = reinterpret_cast<float*>(oTexCoords.data);
    uint32_t* indicesPtr = reinterpret_cast<uint32_t*>(oIdx.data);
    Vector3f* normalsPtr = reinterpret_cast<Vector3f*>(oNormals.data);

    for (unsigned int i = 0; i < numCells; ++i)
    {
      Vector2f ptCenter = m_Cells[i].position;

      float height = iHeight[i];

      positionsPtr[0] = iScale.X() * ptCenter.X();
      positionsPtr[1] = iScale.Y() * ptCenter.Y();
      positionsPtr[2] = iScale.Z() * height;
      texCoordPtr[0] = ptCenter.X() / m_Size.X();
      texCoordPtr[1] = ptCenter.Y() / m_Size.Y();

      VoronoiGraph::CellGraphImpl::vertex_descriptor curCell = i;
      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      unsigned int numPts = 0;
      for (boost::tie(edgesBegin, edgesEnd) = boost::out_edges(curCell, m_Graph.m_CellGraph); edgesBegin != edgesEnd; ++edgesBegin)
      {
        unsigned int target = curCell == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        if (target >= m_Cells.size())
        {
          continue;
        }

        VoronoiGraph::Edge const& curEdge = m_Graph.m_Edges.find(*edgesBegin)->second;

        uint32_t pt1Idx = numCells + curEdge.m_Pt1;
        uint32_t pt2Idx = numCells + curEdge.m_Pt2;

        *indicesPtr = i;
        oIdx.NextItem(indicesPtr);
        *indicesPtr = pt1Idx;
        oIdx.NextItem(indicesPtr);
        *indicesPtr = pt2Idx;
        oIdx.NextItem(indicesPtr);

        edgePtRef[curEdge.m_Pt1].insert(i);
        edgePtRef[curEdge.m_Pt2].insert(i);
      }

      *normalsPtr = Vector3f::ZERO;

      oPositions.NextItem(positionsPtr);
      oTexCoords.NextItem(texCoordPtr);
      oNormals.NextItem(normalsPtr);
    }

    for(unsigned int i = 0; i<edgePtRef.size(); ++i)
    {
      unsigned int numRef = 0;
      float height = 0.0;
      for(auto ref : edgePtRef[i])
      {
        height += oPositions.Item<float>(ref)[2];
        ++numRef;
      }

      if(numRef > 0)
        height /= numRef;

      positionsPtr[0] = iScale.X() * m_Graph.m_Vertices[i].m_Position.X();
      positionsPtr[1] = iScale.Y() * m_Graph.m_Vertices[i].m_Position.Y();
      positionsPtr[2] = height;
      texCoordPtr[0] = m_Graph.m_Vertices[i].m_Position.X() / m_Size.X();
      texCoordPtr[1] = m_Graph.m_Vertices[i].m_Position.Y() / m_Size.Y();
      *normalsPtr = Vector3f::ZERO;

      oPositions.NextItem(positionsPtr);
      oTexCoords.NextItem(texCoordPtr);
      oNormals.NextItem(normalsPtr);
    }

    indicesPtr = reinterpret_cast<uint32_t*>(oIdx.data);
    for (unsigned int i = 0; i < numCells; ++i)
    {
      Vector3f const& pos = *oPositions.Item<Vector3f>(i);

      VoronoiGraph::CellGraphImpl::vertex_descriptor curCell = i;
      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      unsigned int numPts = 0;
      Vector3f normal;
      Set<unsigned int> pts;
      for (boost::tie(edgesBegin, edgesEnd) = boost::out_edges(curCell, m_Graph.m_CellGraph); edgesBegin != edgesEnd; ++edgesBegin)
      {
        unsigned int target = curCell == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        if (target >= m_Cells.size())
        {
          continue;
        }

        VoronoiGraph::Edge const& curEdge = m_Graph.m_Edges.find(*edgesBegin)->second;

        Vector3f const& otherPt1 = *oPositions.Item<Vector3f>(curEdge.m_Pt1 + numCells);
        Vector3f const& otherPt2 = *oPositions.Item<Vector3f>(curEdge.m_Pt2 + numCells);

        Vector3f locNormal = (pos - otherPt1).Cross(pos - otherPt2);

        if (locNormal.Dot(Vector3f::UNIT_Z) < -Mathf::EPSILON)
        {
          locNormal *= -1.0;
          std::swap(indicesPtr[1], indicesPtr[2]);
        }

        *oNormals.Item<Vector3f>(i) += locNormal;
        *oNormals.Item<Vector3f>(curEdge.m_Pt1 + numCells) += locNormal;
        *oNormals.Item<Vector3f>(curEdge.m_Pt2 + numCells) += locNormal;

        oIdx.NextItem(indicesPtr);
        oIdx.NextItem(indicesPtr);
        oIdx.NextItem(indicesPtr);
      }
    }

    normalsPtr = reinterpret_cast<Vector3f*>(oNormals.data);
    for (unsigned int i = 0; i < numCells + m_Graph.m_Vertices.size(); ++i)
    {
      normalsPtr->Normalize();
      oNormals.NextItem(normalsPtr);
    }
  }

  void Terrain::ComputeFlowMap(const Vector<float>& iHeight, Vector<unsigned int>& oSummits, Vector<int>& oNext)
  {
    unsigned int const numCells = m_Cells.size();

    oSummits.clear();
    oNext.resize(numCells, -1);

    Vector<float> filledHeight(numCells, Mathf::MAX_REAL);

    for (unsigned int i = 0; i < numCells; ++i)
    {
      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      boost::tie(edgesBegin, edgesEnd) = boost::out_edges(i, m_Graph.m_CellGraph);
      for(; edgesBegin != edgesEnd; ++edgesBegin)
      {
        unsigned int curNeigh = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;

        if(curNeigh == numCells)
        {
          filledHeight[i] = iHeight[i];
          break;
        }
      }
    }

    bool updated;
    do
    {
      updated = false;

      for (unsigned int i = 0; i < numCells; ++i)
      {
        if(iHeight[i] == filledHeight[i])
          continue;

        float curOrigH = iHeight[i];

        VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
        boost::tie(edgesBegin, edgesEnd) = boost::out_edges(i, m_Graph.m_CellGraph);
        for(; edgesBegin != edgesEnd; ++edgesBegin)
        {
          unsigned int curNeigh = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;

          eXl_ASSERT(curNeigh < numCells);

          float neighH = filledHeight[curNeigh];
          if(curOrigH >= neighH + Mathf::ZERO_TOLERANCE)
          {
            //C'est bon on a un voisin plus bas
            filledHeight[i] = curOrigH;
            updated = true;
          }
          else
          {
            float newH = neighH + Mathf::ZERO_TOLERANCE;
            if(filledHeight[i] > newH && (newH > curOrigH))
            {
              filledHeight[i] = newH;
              updated = true;
            }
          }
        }
      }
    }while(updated);

    for (unsigned int i = 0; i < numCells; ++i)
    {
      int lowerN = -1;
      float curLowerH = Mathf::MAX_REAL;
      float curH = filledHeight[i];
      bool summit = true;

      VoronoiGraph::CellGraphImpl::out_edge_iterator edgesBegin, edgesEnd;
      boost::tie(edgesBegin, edgesEnd) = boost::out_edges(i, m_Graph.m_CellGraph);
      for(; edgesBegin != edgesEnd; ++edgesBegin)
      {
        unsigned int curNeigh = i == edgesBegin->m_target ? edgesBegin->m_source : edgesBegin->m_target;
        if(curNeigh == numCells)
          continue;

        float neighH = filledHeight[curNeigh];
        if(curH > neighH)
        {
          if(neighH < curLowerH)
          {
            lowerN = curNeigh;
            curLowerH = neighH;
          }
        }
        else
          summit = false;
      }

      if(summit)
        oSummits.push_back(i);

      oNext[i] = lowerN;
    }
  }

  //void Terrain::ComputeNormals(Vector3f const& iScale, const Vector<float>& iHeight, Vector<Vector3f>& oNormals)
  //{
  //  unsigned int const numCells = m_Cells.size();
  //  oNormals.resize(numCells);
  //
  //  for (unsigned int i = 0; i < numCells; ++i)
  //  {
  //    Vector3f pos(m_Cells[i].position.X() * iScale.X(), iHeight[i] * iScale.Y(), m_Cells[i].position.Y() * iScale.Z());
  //
  //    unsigned int neighStart = m_Cells[i].neighStart;
  //    unsigned int neighEnd = m_Cells[i].neighCount + neighStart;
  //
  //    Vector3f normal;
  //    if(neighEnd != neighStart)
  //    {
  //      unsigned int prevNeigh = m_Neigh[neighEnd - 1];
  //      Vector3f prevPt(m_Cells[prevNeigh].position.X() * iScale.X(), iHeight[prevNeigh] * iScale.Y(), m_Cells[prevNeigh].position.Y() * iScale.Z());
  //
  //      for (unsigned int neighIdx = neighStart; neighIdx < neighEnd; ++neighIdx)
  //      {
  //        unsigned int curNeigh = m_Neigh[neighIdx];
  //        Vector3f curPt(m_Cells[curNeigh].position.X() * iScale.X(), iHeight[curNeigh] * iScale.Y(), m_Cells[curNeigh].position.Y() * iScale.Z());
  //        Vector3f locNormal = (pos - prevPt).Cross(pos - curPt);
  //
  //        if(locNormal.Dot(Vector3f::UNIT_Y) < -Mathf::EPSILON)
  //          locNormal *= -1.0;
  //
  //        normal += locNormal;
  //        prevPt = curPt;
  //      }
  //    }
  //    normal.Normalize();
  //    oNormals[i] = normal;
  //  }
  //}

}


// Copyright (C) 2018 Thanaphon Chavengsaksongkram <as12production@gmail.com>, He Sun <he.sun@ed.ac.uk>
// This file is subject to the license terms in the LICENSE file
// found in the top-level directory of this distribution.

#ifndef GSPARSE_ER_POLICY_APROXERSLMJACOBICG_HPP
#define GSPARSE_ER_POLICY_APROXERSLMJACOBICG_HPP

#include "../../Config.hpp"
#include "../../Util/JL.hpp"  // Building Random Projection

#include <Eigen/Dense>
#include <Eigen/Sparse>
namespace gSparse
{
    namespace ER
    {
        namespace Policy
        {
            /// \ingroup EffectiveResistance
            ///
            /// This class Approximate Effective Weight Resistance
            /// Adaptation from http://ccom.uprrp.edu/~ikoutis/SpectralAlgorithms.htm.
            /// The algorithm leverages Conjugated Graident with Jacobi preconditioner to solve linear system
            ///
            class AproxERSLMJacobiCG
            {
            protected:
                /// This function calculates Effective Resistance and return computation status.
                /// \param er A row matrix to receive the EffectiveResistance value
                /// \param graph A std::shared_ptr<IGraph> object representing the graph to calculate resistance
                /// \param eps Error tolerance for conjugated gradient. Default is 1.0f.
                /// \param JLTol Tolerance for JL projection Matrix. Default is 0.5f. (See http://ccom.uprrp.edu/~ikoutis/SpectralAlgorithms.htm.)
                /// \param maxIter  Maximum iteration for conjugated gradient. Default is 300 iterations.
                inline gSparse::COMPUTE_INFO _calculateER(
                    gSparse::PrecisionRowMatrix & er,
                    const gSparse::Graph & graph,
                    double eps = 1.0f,
                    double JLTol = 0.5,
                    int maxIter = 300
                    )
                {
                    er = gSparse::PrecisionRowMatrix::Zero(graph->GetEdgeCount(), 1);

                    std::size_t scale = static_cast<size_t>(
                                std::ceil(
                                std::log2(
                                static_cast<double>(graph->GetIncidentMatrix().cols()) / eps)));

                    for (int i = 1; i != scale + 1; ++i)
                    {
                        Eigen::VectorXd x;
                        gSparse::PrecisionMatrix Q =
                        gSparse::Util::randomProjectionMatrix(1,
                                                                graph->GetIncidentMatrix().rows(),
                                                                static_cast<double>(scale),
                                                                JLTol);

                        gSparse::PrecisionMatrix Y = (Q * graph->GetWeightMatrix().cwiseSqrt() * graph->GetIncidentMatrix());

                        // solve Linear system with 300 max iteration
                        Eigen::ConjugateGradient<gSparse::SparsePrecisionMatrix, Eigen::Lower | Eigen::Upper  > cg;
                        cg.setMaxIterations(maxIter);
                        x = cg.compute(graph->GetLaplacianMatrix()).solve(Y.transpose());

                        if (cg.info() != Eigen::Success)
                        {
                            // Does not converge this iteration. Keeps going.
                            continue;
                        }
                        for (std::size_t j = 0; j != graph->GetEdgeCount(); ++j)
                        {
                            er(j) += pow(std::abs(x(graph->GetEdgeList()(j, 0)) - x(graph->GetEdgeList()(j, 1))), 2.0f);
                        }
                    }
                    // Non finite element goes to zero
                    er = er.unaryExpr([](double v) { return std::isfinite(v)? v : 0.0; });
                    if (er.rows() != graph->GetEdgeCount())
                        return gSparse::NOT_CONVERGING;
                    return gSparse::SUCCESSFUL;
                }
            };
        }
    }
}
#endif


#include <stan/math/prim/scal.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>

TEST(ProbNegBinomial2, ccdf_log_matches_lccdf) {
  double y = 0.8;
  double mu = 1.1;
  double phi = 2.3;

  EXPECT_FLOAT_EQ((stan::math::neg_binomial_2_lccdf(y, mu, phi)),
                  (stan::math::neg_binomial_2_ccdf_log(y, mu, phi)));
  EXPECT_FLOAT_EQ(
      (stan::math::neg_binomial_2_lccdf<double, double, double>(y, mu, phi)),
      (stan::math::neg_binomial_2_ccdf_log<double, double, double>(y, mu,
                                                                   phi)));
}


#include <ql/quantlib.hpp>

#include <boost/make_shared.hpp>

// example / tests for multicurrency lgm model

using namespace QuantLib;

void nodelete() {}

int main() {

    try {

        Date referenceDate(30, July, 2015);

        Settings::instance().evaluationDate() = referenceDate;

        // the single currency models
        // they can be calibrated in the usual way

        Handle<YieldTermStructure> eurYts(boost::make_shared<FlatForward>(
            referenceDate, 0.02, Actual365Fixed()));

        Handle<YieldTermStructure> usdYts(boost::make_shared<FlatForward>(
            referenceDate, 0.05, Actual365Fixed()));

        std::vector<Date> volstepdates;
        std::vector<Real> volsteptimes;
        Array volsteptimes_a(0);
        std::vector<Real> eurVols(1, atof(getenv("EURVOL")));
        std::vector<Real> usdVols(1, atof(getenv("USDVOL")));
        std::vector<Real> fxSigmas(1, atof(getenv("FXVOL")));
        Array fxSigmas_a(fxSigmas.begin(), fxSigmas.end());

        boost::shared_ptr<Lgm1> eurLgm = boost::make_shared<Lgm1>(
            eurYts, volstepdates, eurVols, atof(getenv("EURMR")));
        boost::shared_ptr<Lgm1> usdLgm = boost::make_shared<Lgm1>(
            usdYts, volstepdates, usdVols, atof(getenv("USDMR")));

        std::vector<boost::shared_ptr<Lgm1> > singleModels;
        singleModels.push_back(eurLgm);
        singleModels.push_back(usdLgm);

        std::vector<Handle<YieldTermStructure> > curves;
        curves.push_back(eurYts);
        curves.push_back(usdYts);

        // build cc parametrization from scratch

        // lgm parametrizations

        std::vector<boost::shared_ptr<detail::LgmParametrization<
            detail::LgmPiecewiseAlphaConstantKappa> > > lgmParametrizations;

        boost::shared_ptr<
            detail::LgmParametrization<detail::LgmPiecewiseAlphaConstantKappa> >
            eurParam = eurLgm->parametrization();
        boost::shared_ptr<
            detail::LgmParametrization<detail::LgmPiecewiseAlphaConstantKappa> >
            usdParam = usdLgm->parametrization();

        lgmParametrizations.push_back(eurParam);
        lgmParametrizations.push_back(usdParam);

        // fx parametrizations

        std::vector<boost::shared_ptr<detail::LgmFxParametrization<
            detail::LgmFxPiecewiseSigma> > > fxParametrizations;

        boost::shared_ptr<detail::LgmFxParametrization<
            detail::LgmFxPiecewiseSigma> > fxParam =
            boost::make_shared<detail::LgmFxPiecewiseSigma>(volsteptimes_a,
                                                            fxSigmas_a);

        fxParametrizations.push_back(fxParam);

        // the fx vols, correlations and the cclgmm parametrization / process /
        // model

        std::vector<Handle<Quote> > fxSpots;
        fxSpots.push_back(Handle<Quote>(boost::make_shared<SimpleQuote>(
            std::log(0.9090)))); // EUR-USD ~ 1.10

        Matrix c(3, 3);
        //  FX             EUR         USD
        c[0][0] = 1.0; c[0][1] = 0.99; c[0][2] = 0.99; // FX
        c[1][0] = 0.99; c[1][1] = 1.0; c[1][2] = 0.99; // EUR
        c[2][0] = 0.99; c[2][1] = 0.99; c[2][2] = 1.0; // USD

        boost::shared_ptr<detail::CcLgmPiecewise> ccParam =
            boost::make_shared<detail::CcLgmPiecewise>(fxParametrizations,
                                                       lgmParametrizations, c);

        ccParam->update();

        // test parametrization

        std::clog.precision(12);
        // std::clog << "H0(0.0) = " << ccParam->H_i(0,0.0) << std::endl;
        // std::clog << "H0(1.0) = " << ccParam->H_i(0,1.0) << std::endl;
        // std::clog << "H0(2.0) = " << ccParam->H_i(0,2.0) << std::endl;
        // std::clog << "H1(0.0) = " << ccParam->H_i(1,0.0) << std::endl;
        // std::clog << "H1(1.0) = " << ccParam->H_i(1,1.0) << std::endl;
        // std::clog << "H1(2.0) = " << ccParam->H_i(1,2.0) << std::endl;
        // std::clog << "zeta0(0.0) = " << ccParam->zeta_i(0,0.0) << std::endl;
        // std::clog << "zeta0(1.0) = " << ccParam->zeta_i(0,1.0) << std::endl;
        // std::clog << "zeta0(2.0) = " << ccParam->zeta_i(0,2.0) << std::endl;
        // std::clog << "zeta0(3.0) = " << ccParam->zeta_i(0,3.0) << std::endl;
        // std::clog << "zeta1(0.0) = " << ccParam->zeta_i(1,0.0) << std::endl;
        // std::clog << "zeta1(1.0) = " << ccParam->zeta_i(1,1.0) << std::endl;
        // std::clog << "zeta1(2.0) = " << ccParam->zeta_i(1,2.0) << std::endl;
        // std::clog << "zeta1(3.0) = " << ccParam->zeta_i(1,3.0) << std::endl;
        // std::clog << "alphaialphaj(0.0) = " <<
        // ccParam->alpha_i_alpha_j(0,0,0.0) << std::endl;
        // std::clog << "alphaialphaj(1.0) = " <<
        // ccParam->alpha_i_alpha_j(0,0,1.0) << std::endl;
        // std::clog << "alphaialphaj(2.0) = " <<
        // ccParam->alpha_i_alpha_j(0,0,2.0) << std::endl;
        // std::clog << "alphaialphaj(0.0) = " <<
        // ccParam->alpha_i_alpha_j(1,1,0.0) << std::endl;
        // std::clog << "alphaialphaj(1.0) = " <<
        // ccParam->alpha_i_alpha_j(1,1,1.0) << std::endl;
        // std::clog << "alphaialphaj(2.0) = " <<
        // ccParam->alpha_i_alpha_j(1,1,2.0) << std::endl;
        // std::clog << "alphaialphaj(0.0) = " <<
        // ccParam->alpha_i_alpha_j(0,1,0.0) << std::endl;
        // std::clog << "alphaialphaj(1.0) = " <<
        // ccParam->alpha_i_alpha_j(0,1,1.0) << std::endl;
        // std::clog << "alphaialphaj(2.0) = " <<
        // ccParam->alpha_i_alpha_j(0,1,2.0) << std::endl;
        // std::clog << "alphaialphaj(0.0) = " <<
        // ccParam->alpha_i_alpha_j(1,0,0.0) << std::endl;
        // std::clog << "alphaialphaj(1.0) = " <<
        // ccParam->alpha_i_alpha_j(1,0,1.0) << std::endl;
        // std::clog << "alphaialphaj(2.0) = " <<
        // ccParam->alpha_i_alpha_j(1,0,2.0) << std::endl;
        // std::clog << "sigmaisigmaj(0.0) = " <<
        // ccParam->sigma_i_sigma_j(0,0,0.0) << std::endl;
        // std::clog << "sigmaisigmaj(1.0) = " <<
        // ccParam->sigma_i_sigma_j(0,0,1.0) << std::endl;
        // std::clog << "sigmaisigmaj(2.0) = " <<
        // ccParam->sigma_i_sigma_j(0,0,2.0) << std::endl;
        // std::clog << "alphaisigmaj(0.0) = " <<
        // ccParam->alpha_i_sigma_j(0,0,0.0) << std::endl;
        // std::clog << "alphaisigmaj(1.0) = " <<
        // ccParam->alpha_i_sigma_j(0,0,1.0) << std::endl;
        // std::clog << "alphaisigmaj(2.0) = " <<
        // ccParam->alpha_i_sigma_j(0,0,2.0) << std::endl;
        // std::clog << "alphaisigmaj(0.0) = " <<
        // ccParam->alpha_i_sigma_j(1,0,0.0) << std::endl;
        // std::clog << "alphaisigmaj(1.0) = " <<
        // ccParam->alpha_i_sigma_j(1,0,1.0) << std::endl;
        // std::clog << "alphaisigmaj(2.0) = " <<
        // ccParam->alpha_i_sigma_j(1,0,2.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(0,0,0.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(0,0,1.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(0,0,2.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(1,1,0.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(1,1,1.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(1,1,2.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(1,0,0.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(1,0,1.0) << std::endl;
        // std::clog << "HiAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_alpha_j(1,0,2.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(0,0,0.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(0,0,1.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(0,0,2.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(1,1,0.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(1,1,1.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(1,1,2.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(1,0,0.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(1,0,1.0) << std::endl;
        // std::clog << "HiHjAlphaIAlphaJ(0.0) = " <<
        // ccParam->H_i_H_j_alpha_i_alpha_j(1,0,2.0) << std::endl;
        // std::clog << "HiAlphaISigmaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_sigma_j(0,0,0.0) << std::endl;
        // std::clog << "HiAlphaISigmaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_sigma_j(0,0,1.0) << std::endl;
        // std::clog << "HiAlphaISigmaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_sigma_j(0,0,2.0) << std::endl;
        // std::clog << "HiAlphaISigmaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_sigma_j(1,0,0.0) << std::endl;
        // std::clog << "HiAlphaISigmaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_sigma_j(1,0,1.0) << std::endl;
        // std::clog << "HiAlphaISigmaJ(0.0) = " <<
        // ccParam->H_i_alpha_i_sigma_j(1,0,2.0) << std::endl;
        // std::clog << "int_alphaialphaj(0.0) = " <<
        // ccParam->int_alpha_i_alpha_j(0,0,0.0,0.0) << std::endl;
        // std::clog << "int_alphaialphaj(1.0) = " <<
        // ccParam->int_alpha_i_alpha_j(0,0,0.0,1.0) << std::endl;
        // std::clog << "int_alphaialphaj(2.0) = " <<
        // ccParam->int_alpha_i_alpha_j(0,0,0.0,2.0) << std::endl;
        // std::clog << "int_alphaialphaj(0.0) = " <<
        // ccParam->int_alpha_i_alpha_j(1,1,0.0,0.0) << std::endl;
        // std::clog << "int_alphaialphaj(1.0) = " <<
        // ccParam->int_alpha_i_alpha_j(1,1,0.0,1.0) << std::endl;
        // std::clog << "int_alphaialphaj(2.0) = " <<
        // ccParam->int_alpha_i_alpha_j(1,1,0.0,2.0) << std::endl;
        // std::clog << "int_alphaialphaj(0.0) = " <<
        // ccParam->int_alpha_i_alpha_j(0,1,0.0,0.0) << std::endl;
        // std::clog << "int_alphaialphaj(1.0) = " <<
        // ccParam->int_alpha_i_alpha_j(0,1,0.0,1.0) << std::endl;
        // std::clog << "int_alphaialphaj(2.0) = " <<
        // ccParam->int_alpha_i_alpha_j(0,1,0.0,2.0) << std::endl;
        // std::clog << "int_alphaialphaj(0.0) = " <<
        // ccParam->int_alpha_i_alpha_j(1,0,0.0,0.0) << std::endl;
        // std::clog << "int_alphaialphaj(1.0) = " <<
        // ccParam->int_alpha_i_alpha_j(1,0,0.0,1.0) << std::endl;
        // std::clog << "int_alphaialphaj(2.0) = " <<
        // ccParam->int_alpha_i_alpha_j(1,0,0.0,2.0) << std::endl;
        // std::clog << "int_sigmaisigmaj(0.0) = " <<
        // ccParam->int_sigma_i_sigma_j(0,0,0.0,0.0) << std::endl;
        // std::clog << "int_sigmaisigmaj(1.0) = " <<
        // ccParam->int_sigma_i_sigma_j(0,0,0.0,1.0) << std::endl;
        // std::clog << "int_sigmaisigmaj(2.0) = " <<
        // ccParam->int_sigma_i_sigma_j(0,0,0.0,2.0) << std::endl;
        // std::clog << "int_alphaisigmaj(0.0) = " <<
        // ccParam->int_alpha_i_sigma_j(0,0,0.0,0.0) << std::endl;
        // std::clog << "int_alphaisigmaj(1.0) = " <<
        // ccParam->int_alpha_i_sigma_j(0,0,0.0,1.0) << std::endl;
        // std::clog << "int_alphaisigmaj(2.0) = " <<
        // ccParam->int_alpha_i_sigma_j(0,0,0.0,2.0) << std::endl;
        // std::clog << "int_alphaisigmaj(0.0) = " <<
        // ccParam->int_alpha_i_sigma_j(1,0,0.0,0.0) << std::endl;
        // std::clog << "int_alphaisigmaj(1.0) = " <<
        // ccParam->int_alpha_i_sigma_j(1,0,0.0,1.0) << std::endl;
        // std::clog << "int_alphaisigmaj(2.0) = " <<
        // ccParam->int_alpha_i_sigma_j(1,0,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(0,0,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(0,0,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(0,0,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(1,1,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(1,1,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(1,1,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(0,1,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(0,1,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(0,1,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(1,0,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(1,0,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_alpha_i_alpha_j(1,0,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(0,0,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(0,0,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(0,0,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(1,1,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(1,1,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(1,1,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(0,1,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(0,1,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(0,1,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(0.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(1,0,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(1.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(1,0,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_H_j_alphaialphaj(2.0) = " <<
        // ccParam->int_H_i_H_j_alpha_i_alpha_j(1,0,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_alphaisigmaj(0.0) = " <<
        // ccParam->int_H_i_alpha_i_sigma_j(0,0,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_alphaisigmaj(1.0) = " <<
        // ccParam->int_H_i_alpha_i_sigma_j(0,0,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_alphaisigmaj(2.0) = " <<
        // ccParam->int_H_i_alpha_i_sigma_j(0,0,0.0,2.0) << std::endl;
        // std::clog << "int_H_i_alphaisigmaj(0.0) = " <<
        // ccParam->int_H_i_alpha_i_sigma_j(1,0,0.0,0.0) << std::endl;
        // std::clog << "int_H_i_alphaisigmaj(1.0) = " <<
        // ccParam->int_H_i_alpha_i_sigma_j(1,0,0.0,1.0) << std::endl;
        // std::clog << "int_H_i_alphaisigmaj(2.0) = " <<
        // ccParam->int_H_i_alpha_i_sigma_j(1,0,0.0,2.0) << std::endl;

        // std::clog << "rho alpha-alpha 00" << ccParam->rho_alpha_alpha(0,0) <<
        // std::endl;
        // std::clog << "rho alpha-alpha 01" << ccParam->rho_alpha_alpha(0,1) <<
        // std::endl;
        // std::clog << "rho alpha-alpha 10" << ccParam->rho_alpha_alpha(1,0) <<
        // std::endl;
        // std::clog << "rho alpha-alpha 11" << ccParam->rho_alpha_alpha(1,1) <<
        // std::endl;
        // std::clog << "rho alpha-sigma 00" << ccParam->rho_alpha_sigma(0,0) <<
        // std::endl;
        // std::clog << "rho alpha-sigma 10" << ccParam->rho_alpha_sigma(1,0) <<
        // std::endl;
        // std::clog << "rho sigma-sigma 00" << ccParam->rho_sigma_sigma(0,0) <<
        // std::endl;

        // end test parametrization

        boost::shared_ptr<
            CcLgmProcess<detail::CcLgmPiecewise, detail::LgmFxPiecewiseSigma,
                         detail::LgmPiecewiseAlphaConstantKappa> > process =
            boost::make_shared<CcLgmProcess<
                detail::CcLgmPiecewise, detail::LgmFxPiecewiseSigma,
                detail::LgmPiecewiseAlphaConstantKappa> >(ccParam, fxSpots,
                                                          curves);

        // generate paths

        Size n = atoi(getenv("N")); // N paths
        Time T = atof(getenv("T")); // cashflow time
        Size steps = static_cast<Size>(
            T * atof(getenv("STEPS")));   // STEPS steps per year
        Size seed = atoi(getenv("SEED")); // rng seed
        TimeGrid grid(T, steps);

        PseudoRandom::rsg_type sg =
            PseudoRandom::make_sequence_generator(steps * 3, seed);
        MultiPathGenerator<PseudoRandom::rsg_type> pg(process, grid, sg, false);

        PseudoRandom::rsg_type sg2 =
            PseudoRandom::make_sequence_generator(steps, seed);
        PathGenerator<PseudoRandom::rsg_type> pg2(usdLgm->stateProcess(), grid,
                                                  sg2, false);

        std::vector<Sample<MultiPath> > paths;
        for (Size j = 0; j < n; ++j) {
            paths.push_back(pg.next());
        }

        std::vector<Sample<Path> > paths2;
        for (Size j = 0; j < n; ++j) {
            paths2.push_back(pg2.next());
        }

        // output paths for visual inspection in gnuplot

        if (atoi(getenv("OUTPUT"))) {
            // cc model paths
            for (Size i = 0; i < paths[0].value[0].length(); ++i) {
                std::cout << grid[i] << " ";
                for (Size j = 0; j < n; ++j) {
                    std::cout << std::exp(paths[j].value[0][i]) << " "
                              << paths[j].value[1][i] << " "
                              << paths[j].value[2][i] << " "
                              << paths2[j].value[i] << " ";
                }
                std::cout << "\n";
            }
        }

        // test: 1 USD in 1y, priced in domestic measure

        Size l = paths[0].value[0].length() - 1;
        IncrementalStatistics stat, stat2;
        for (Size j = 0; j < n; ++j) {
            Real fx = std::exp(paths[j].value[0][l]);
            Real zeur = paths[j].value[1][l];
            Real zusd = paths[j].value[2][l];
            Real zusd2 = paths2[j].value[l];
            Real stddev = eurLgm->stateProcess()->stdDeviation(0.0, 0.0, T);
            Real stddev2 = usdLgm->stateProcess()->stdDeviation(0.0, 0.0, T);
            Real y = (zeur - eurLgm->stateProcess()->expectation(0.0, 0.0, T)) /
                     (!close_enough(stddev, 0.0) ? stddev : 1.0);
            Real y2 =
                (zusd2 - usdLgm->stateProcess()->expectation(0.0, 0.0, T)) /
                (!close_enough(stddev2, 0.0) ? stddev2 : 1.0);
            stat.add(1.0 * fx / eurLgm->numeraire(T, y));
            stat2.add(1.0 / usdLgm->numeraire(T, y2));
        }
        std::clog << "1 USD @ 1y  = " << stat.mean() << " EUR +/- "
                  << stat.errorEstimate() << std::endl;
        ;
        std::clog << "curve price = " << usdYts->discount(T) << " spot "
                  << std::exp(fxSpots[0]->value()) << " EUR price "
                  << usdYts->discount(T) * std::exp(fxSpots[0]->value())
                  << "\n";

        std::clog << "1 USD @ 1y = " << stat2.mean() << " USD +/-"
                  << stat2.errorEstimate() << std::endl;

        return 0;

    } catch (QuantLib::Error e) {
        std::clog << "ql exception : " << e.what() << "\n";
    } catch (std::exception e) {
        std::clog << "std exception: " << e.what() << "\n";
    }
}


#include "corner_detection.h"
#include <cslibs_boost_geometry/algorithms.h>
#include <boost/geometry/algorithms/distance.hpp>

using namespace cslibs_vectormaps;

CornerDetection::CornerDetection(const CornerDetectionParameter &parameter) :
    parameter_(parameter)
{
}

void CornerDetection::operator () (const Vectors &vectors,
                                   Points &corners,
                                   std::vector<double> &cornerness,
                                   Points &loose_endpoints,
                                   progress_callback progress)
{
    struct Corner {
        Corner(const dxf::DXFMap::Point &point,
               const double cornerness) :
            point(point),
            cornerness(cornerness)
        {
        }

        dxf::DXFMap::Point  point;
        double              cornerness;
    };

    auto capped_abs  = [] (const double x)
    {return (fabs(M_PI - fabs(x))  < 1e-3) ? 0.0 : fabs(x);};
    auto less_corner= [] (const Corner &c1,const Corner &c2)
    {return c1.point.x() < c2.point.x() || c1.point.y() < c2.point.y();};
    auto less_point = [] (const dxf::DXFMap::Point &p1,const dxf::DXFMap::Point &p2)
    {return p1.x() < p2.x() || p1.y() < p2.y();};

    const double mu = parameter_.pref_corner_angle;
    const double sigma = parameter_.pref_corner_angle_std_dev;
    auto cornerness_from_angle = [mu, sigma](const double x)
    {
        return std::exp(-0.5 * (x - mu) * (x - mu) / (sigma * sigma));
    };

    std::size_t count = 0;

    std::set<Corner, decltype(less_corner)> corner_set(less_corner);
    std::set<dxf::DXFMap::Point, decltype(less_point)> loose_endpoint_set(less_point);

    for(const Vector &v1 : vectors) {
        double min_distance_p1 = std::numeric_limits<double>::max();
        double min_distance_p2 = std::numeric_limits<double>::max();
        Vector  closest_p1;
        Vector  closest_p2;

        /// do a probabilistic approach, the wider the angle the more unintresting the point is
        /// mix that with the distance

        /// both ends of the line have to minimized not only one !!! that is the problem why it doesn't work

        for(const Vector &v2 : vectors) {
            double distance_p1 = cslibs_boost_geometry::algorithms::distance<double, Point>(v1.first,  v2);
            double distance_p2 = cslibs_boost_geometry::algorithms::distance<double, Point>(v1.second, v2);

            if(cslibs_boost_geometry::algorithms::equal<Point, double>(v1,v2, 1e-6))
                continue;

            if(distance_p1 <= min_distance_p1) {
                closest_p1 = v2;
                min_distance_p1 = distance_p1;
            }
            if(distance_p2 <= min_distance_p2) {
                closest_p2 = v2;
                min_distance_p2 = distance_p2;
            }
        }

        double angle_p1 = cslibs_boost_geometry::algorithms::angle<double, Point>(v1,closest_p1, 1e-6);
        double angle_p2 = cslibs_boost_geometry::algorithms::angle<double, Point>(v1,closest_p2, 1e-6);

        if(capped_abs(angle_p1) >= parameter_.min_corner_angle &&
                min_distance_p1 <= parameter_.max_corner_point_distance) {

            double c = cornerness_from_angle(angle_p1);
            corner_set.insert(Corner(v1.first, c));

        } else if(min_distance_p1 >= parameter_.min_loose_endpoint_distance) {

            loose_endpoint_set.insert(v1.first);

        }
        if(capped_abs(angle_p2) >= parameter_.min_corner_angle &&
                min_distance_p2 <= parameter_.max_corner_point_distance) {

            double c = cornerness_from_angle(angle_p2);
            corner_set.insert(Corner(v1.second, c));

        } else if(min_distance_p2 >= parameter_.min_loose_endpoint_distance) {

            loose_endpoint_set.insert(v1.second);

        }
        progress(++count / (double) vectors.size() * 100);
    }

    auto it_last = corner_set.begin();
    auto it = corner_set.begin();
    ++it;
    corners.emplace_back(it_last->point);
    cornerness.emplace_back(it_last->cornerness);
    while(it != corner_set.end()) {
        if(!cslibs_boost_geometry::algorithms::equal<Point, double>(it->point,it_last->point, 1e-2)) {
            corners.emplace_back(it->point);
            cornerness.emplace_back(it->cornerness);
        }
        ++it;
        ++it_last;
    }

    loose_endpoints.assign(loose_endpoint_set.begin(), loose_endpoint_set.end());
    progress(100);
}


/* Boost libs/numeric/odeint/examples/point_type.hpp

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

 solar system example for Hamiltonian stepper

 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 POINT_TYPE_HPP_INCLUDED
#define POINT_TYPE_HPP_INCLUDED

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

//[ point_type
/*the point type */
template< class T , size_t Dim >
class point :
    boost::additive1< point< T , Dim > ,
    boost::additive2< point< T , Dim  > , T ,
    boost::multiplicative2< point< T , Dim > , T
    > > >
    {
    public:

        const static size_t dim = Dim;
        typedef T value_type;
        typedef point< value_type , dim > point_type;

        // ...
        // constructors
        //<-
        point( void )
        {
            for( size_t i=0 ; i<dim ; ++i ) m_val[i] = 0.0;
        }

        point( value_type val )
        {
            for( size_t i=0 ; i<dim ; ++i ) m_val[i] = val;
        }

        point( value_type x , value_type y , value_type z = 0.0 )
        {
            if( dim > 0 ) m_val[0] = x;
            if( dim > 1 ) m_val[1] = y;
            if( dim > 2 ) m_val[2] = z;
        }
        //->

        // ...
        // operators
        //<-
        T operator[]( size_t i ) const { return m_val[i]; }
        T& operator[]( size_t i ) { return m_val[i]; }

        point_type& operator+=( const point_type& p )
        {
            for( size_t i=0 ; i<dim ; ++i )
                m_val[i] += p[i];
            return *this;
        }

        point_type& operator-=( const point_type& p )
        {
            for( size_t i=0 ; i<dim ; ++i )
                m_val[i] -= p[i];
            return *this;
        }

        point_type& operator+=( const value_type& val )
        {
            for( size_t i=0 ; i<dim ; ++i )
                m_val[i] += val;
            return *this;
        }

        point_type& operator-=( const value_type& val )
        {
            for( size_t i=0 ; i<dim ; ++i )
                m_val[i] -= val;
            return *this;
        }

        point_type& operator*=( const value_type &val )
        {
            for( size_t i=0 ; i<dim ; ++i )
                m_val[i] *= val;
            return *this;
        }

        point_type& operator/=( const value_type &val )
        {
            for( size_t i=0 ; i<dim ; ++i )
                m_val[i] /= val;
            return *this;
        }

        //->

    private:

        T m_val[dim];
    };

    //...
    // more operators
    //]

    //
    // the - operator
    //
    template< class T , size_t Dim >
    point< T , Dim > operator-( const point< T , Dim > &p )
    {
        point< T , Dim > tmp;
        for( size_t i=0 ; i<Dim ; ++i ) tmp[i] = -p[i];
        return tmp;
    }

    //
    // scalar product
    //
    template< class T , size_t Dim >
    T scalar_prod( const point< T , Dim > &p1 , const point< T , Dim > &p2 )
    {
        T tmp = 0.0;
        for( size_t i=0 ; i<Dim ; ++i ) tmp += p1[i] * p2[i];
        return tmp;
    }

    //
    // norm
    //
    template< class T , size_t Dim >
    T norm( const point< T , Dim > &p1 )
    {
        return scalar_prod( p1 , p1 );
    }

    //
    // absolute value
    //
    template< class T , size_t Dim >
    T abs( const point< T , Dim > &p1 )
    {
        return sqrt( norm( p1 ) );
    }

    //
    // output operator
    //
    template< class T , size_t Dim >
    std::ostream& operator<<( std::ostream &out , const point< T , Dim > &p )
    {
        if( Dim > 0 ) out << p[0];
        for( size_t i=1 ; i<Dim ; ++i ) out << " " << p[i];
        return out;
    }

#endif //POINT_TYPE_HPP_INCLUDED


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//

#include <boost/gil/extension/io/jpeg.hpp>

// Demonstrates how to compute gradients along the x-axis
// This example converts the input image to a greyscale view via color_converted_view,
// and then relies on the function static_transform to apply the operation halfdiff_cast_channels.
// The result is captured in a view, initially blacked out via a call to fill_pixels (defined in
// include/boost/gil/algorithm.hpp)
// static_transform is defined in include/boost/gil/color_based_algorithm.hpp and applies an operation
// to either a single source or two sources and a destination (as is the case here).
// In this example, the gradient is calculated as half the difference between the two pixels surrounding x
// in the loop in x_gradient.

using namespace boost::gil;

template <typename Out>
struct halfdiff_cast_channels {
    template <typename T> Out operator()(const T& in1, const T& in2) const {
        return Out((in2-in1)/2);
    }
};

template <typename SrcView, typename DstView>
void x_gradient(SrcView const& src, DstView const& dst)
{
    using dst_channel_t = typename channel_type<DstView>::type;

    for (int y = 0; y < src.height(); ++y)
    {
        typename SrcView::x_iterator src_it = src.row_begin(y);
        typename DstView::x_iterator dst_it = dst.row_begin(y);

        for (int x = 1; x < src.width() - 1; ++x)
        {
            static_transform(src_it[x - 1], src_it[x + 1], dst_it[x],
                halfdiff_cast_channels<dst_channel_t>());
        }
    }
}

template <typename SrcView, typename DstView>
void x_luminosity_gradient(SrcView const& src, DstView const& dst)
{
    using gray_pixel_t = pixel<typename channel_type<SrcView>::type, gray_layout_t>;
    x_gradient(color_converted_view<gray_pixel_t>(src), dst);
}

int main()
{
    rgb8_image_t img;
    read_image("test.jpg",img, jpeg_tag{});

    gray8s_image_t img_out(img.dimensions());
    fill_pixels(view(img_out),int8_t(0));

    x_luminosity_gradient(const_view(img), view(img_out));
    write_view("out-x_gradient.jpg",color_converted_view<gray8_pixel_t>(const_view(img_out)), jpeg_tag{});

    return 0;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/LU>
#include <algorithm>

template<typename T> std::string type_name() { return "other"; }
template<> std::string type_name<float>() { return "float"; }
template<> std::string type_name<double>() { return "double"; }
template<> std::string type_name<int>() { return "int"; }
template<> std::string type_name<std::complex<float> >() { return "complex<float>"; }
template<> std::string type_name<std::complex<double> >() { return "complex<double>"; }
template<> std::string type_name<std::complex<int> >() { return "complex<int>"; }

#define EIGEN_DEBUG_VAR(x) std::cerr << #x << " = " << x << std::endl;

template<typename T> inline typename NumTraits<T>::Real epsilon()
{
 return std::numeric_limits<typename NumTraits<T>::Real>::epsilon();
}

template<typename MatrixType> void inverse_permutation_4x4()
{
  typedef typename MatrixType::Scalar Scalar;
  typedef typename MatrixType::RealScalar RealScalar;
  Vector4i indices(0,1,2,3);
  for(int i = 0; i < 24; ++i)
  {
    MatrixType m = MatrixType::Zero();
    m(indices(0),0) = 1;
    m(indices(1),1) = 1;
    m(indices(2),2) = 1;
    m(indices(3),3) = 1;
    MatrixType inv = m.inverse();
    double error = double( (m*inv-MatrixType::Identity()).norm() / epsilon<Scalar>() );
    VERIFY(error == 0.0);
    std::next_permutation(indices.data(),indices.data()+4);
  }
}

template<typename MatrixType> void inverse_general_4x4(int repeat)
{
  typedef typename MatrixType::Scalar Scalar;
  typedef typename MatrixType::RealScalar RealScalar;
  double error_sum = 0., error_max = 0.;
  for(int i = 0; i < repeat; ++i)
  {
    MatrixType m;
    RealScalar absdet;
    do {
      m = MatrixType::Random();
      absdet = ei_abs(m.determinant());
    } while(absdet < 10 * epsilon<Scalar>());
    MatrixType inv = m.inverse();
    double error = double( (m*inv-MatrixType::Identity()).norm() * absdet / epsilon<Scalar>() );
    error_sum += error;
    error_max = std::max(error_max, error);
  }
  std::cerr << "inverse_general_4x4, Scalar = " << type_name<Scalar>() << std::endl;
  double error_avg = error_sum / repeat;
  EIGEN_DEBUG_VAR(error_avg);
  EIGEN_DEBUG_VAR(error_max);
  VERIFY(error_avg < (NumTraits<Scalar>::IsComplex ? 8.0 : 1.25));
  VERIFY(error_max < (NumTraits<Scalar>::IsComplex ? 64.0 : 20.0));
}

void test_eigen2_prec_inverse_4x4()
{
  CALL_SUBTEST_1((inverse_permutation_4x4<Matrix4f>()));
  CALL_SUBTEST_1(( inverse_general_4x4<Matrix4f>(200000 * g_repeat) ));

  CALL_SUBTEST_2((inverse_permutation_4x4<Matrix<double,4,4,RowMajor> >()));
  CALL_SUBTEST_2(( inverse_general_4x4<Matrix<double,4,4,RowMajor> >(200000 * g_repeat) ));

  CALL_SUBTEST_3((inverse_permutation_4x4<Matrix4cf>()));
  CALL_SUBTEST_3((inverse_general_4x4<Matrix4cf>(50000 * g_repeat)));
}