#include <iostream>
#include <Eigen/Dense>
#include <string>

using Eigen::MatrixXd;
using namespace Eigen;
using namespace std;

int main()
{
  string name = "Juan pablo mallarino \n github.com";
  int n = 0, m = 0;
  // Bloque de codigo que lee el archivo y determina filas y columnas
  MatrixXd A(n,m);
  // Bloque de codigo que carga el archivo a la matriz
  MatrixXd m = MatrixXd::Random(10,10); // # reales PSEUDO-aleatorios entre -1 y 1
  cout << "Checkpoint 1: \n" << m << endl;
  cout << "Determinant of m:\n" << m.determinant() << endl;
  MatrixXd u_m = m.inverse();
  cout << "Inverse of m:\n" << u_m << endl;
  cout << "Test de valicez m^-1*m:\n" << u_m*m << endl;
  // No existe tal cosa como exactitud en computacion -> decimales
  // el cero en precision doble (double) -> 10^-15
  // -> es un reto!
  // LISP + Mathematica
  m = (m + MatrixXd::Constant(10,10,1.2)) * 50; // # reales aleatorios entre 10 y 110
  cout << "Checkpoint 2: m =\n" << m << endl; // endl hace flush...
  VectorXd v(10);
  v << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;
  cout << "Checkpoint 3: v =" << v << endl;
  cout << "m * v =" << endl << m * v << endl;
  cout << "terminando... "<<name<<endl;
  return 0;
}


#include <Eigen/Dense>

#include <iostream>
#include <iomanip>

#include <cmath>

//! \brief One step of autonomous IVP y'' + y' + y = 0, [y(0), y'(0)] = z0 using SDIRK method
//! Use SDIRK method for first order ode z' = f(z). Steps of size h.
//! \tparam StateType type of solution space y and initial data y0
//! \param[in] z0 initial data z(0)
//! \param[in] h size of the step
//! \param[in] gamma parameter
//! \return next step z1
template <class StateType>
StateType sdirkStep(const StateType & z0, double h, double gamma) {
    // TODO: implement one step of SDIRK method
}

//! \brief Solve autonomous IVP y'' + y' + y = 0, [y(0), y'(0)] = z0 using SDIRK method
//! Use SDIRK method for first order ode z' = f(z), with N equidistant steps
//! \tparam StateType type of solution space z = [y,y']! and initial data z0 = [y(0), y'(0)]
//! \param[in] z0 initial data z(0)
//! \param[in] N number of equidistant steps
//! \param[in] T final time of simulation
//! \param[in] gamma parameter
//! \return vector containing each step of z_k (y and y')
template <class StateType>
std::vector<StateType> sdirkSolve(const StateType & z0, unsigned int N, double T, double gamma) {
    // TODO: implement solution with SDIRK method
}

int main() {
    // Initial data z0 = [y(0), y'(0)]
    Eigen::Vector2d z0;
    z0 << 1,0;
    // Final time
    const double T = 10;
    // Parameter
    const double gamma = (3.+std::sqrt(3.)) / 6.;
    // Mesh sizes
    std::vector<int> N = {20, 40, 80, 160, 320, 640, 1280, 2560, 5120, 10240};
    // Exact solution (only y(t)) given z0 = [y(0), y'(0)] and t
    auto yex = [&z0] (double t) {
        return 1./3.*std::exp(-t/2.) * ( 3.*z0(0) * std::cos( std::sqrt(3.)*t/2. ) +
        std::sqrt(3.)*z0(0) * std::sin( std::sqrt(3.)*t/2. ) +
        2.*std::sqrt(3.)*z0(1) * std::sin( std::sqrt(3.)*t/2. ) );
    };

    // TODO: compute solution and output error/approximate order
}

Nummerical Methods for CSE/PS14/templates_ps14/sdirk_template.cpp


#include <unistd.h>
#include <Eigen/Dense>
#include <string>
#include <fstream>
#include "common.h"

#define TINYEXR_IMPLEMENTATION
#include "tinyexr.h"

using namespace std;

float L(float cosTheta, float rough, float f0, int s)
{
	if (rough == 0) return schlick(cosTheta, f0);
	cosTheta = std::max(cosTheta, 0.001f); // avoid issues at exact grazing

	float sinTheta = std::sqrt(1 - sqr(cosTheta));
	Vector3 incoming(sinTheta, 0, cosTheta);
	float alpha = sqr(rough);
	MicrofacetLobe lobe(alpha, f0, incoming);
	float total = 0;

	for (int i = 0; i < s; i++)
		for (int j = 0; j < s; j++)
		{
			// stratified sampling
			float r1 = (randf() + i) / s;
			float r2 = (randf() + j) / s;
			Vector3 dir;
			float weight;
			bool ok = lobe.sample(r1, r2, dir, weight);
			if (!ok) continue;
			total += weight;
		}

	total /= s * s;
	return total;
}

float T(int i, const FloatImage& img)
{
	// computing the hemispherical integral in solid angle measure:
	// I = 1/pi * int[f(cos theta) * cos theta d omega]

	// this is turned into spherical coordinates, resulting in:
	// I = 2 int[f(cos theta) * cos theta * sin theta d theta] from 0 to pi/2

	// substitute t := cos theta, obtaining
	// I = 2 int[f(t) * t dt]  from 0 to 1
	// compute the latter using trapezoid rule

	int n = img.cols();
	float total = 0;

	for (int j = 0; j < n; j++)
	{
		float t = float(j) / (n - 1);
		float w = j == 0 || j == n-1 ? 0.5f : 1;
		total += 2 * t * img(i, j) * w / (n - 1);
	}

	return total;
}

void writeExr(FloatImage& img, string filename)
{
	int m = img.rows(), n = img.cols();
	ColorImage tmp(m, n);
	for (int i = 0; i < m; i++)
		for (int j = 0; j < n; j++)
			tmp(i, j) = Color::Constant(img(i, j));
	SaveEXR((float*) &tmp(0,0), n, m, 3, 1, filename.c_str(), 0);
}

int main(int argc, char** argv)
{
	int s = 100;  // use s^2 stratified samples
	int n = 256;  // result table size n x n
	float f0 = 0; // Schlick Fresnel normal incidence

	int tmp;
	while ((tmp = getopt(argc, argv, "n:s:f:")) != -1)
	{
		switch (tmp)
		{
			case 'n': n = atoi(optarg); break;
			case 's': s = atoi(optarg); break;
			case 'f': f0 = atof(optarg); break;
		}
	}

	FloatImage Limg(n, n);

	#pragma omp parallel for
	for (int i = 0; i < n; i++)
		for (int j = 0; j < n; j++)
		{
			float rough = float(i) / (n - 1);
			float cosTheta = float(j) / (n - 1);
			Limg(i, j) = L(cosTheta, rough, f0, s);
		}

	FloatImage Timg(1, n);
	for (int i = 0; i < n; i++) Timg(0, i) = T(i, Limg);

	writeExr(Limg, "L.exr");
	writeExr(Timg, "T.exr");
	return 0;
}


/*****************************************************************************
*
* Copyright (C) 2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>
*
* 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 <iomanip>
#include <iostream>
#include <cmath>
#include <boost/math/constants/constants.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>

template<class T>
class func {
public:
  typedef T real_t;
  func(real_t t) : t_(t) {}
  real_t operator()(real_t x, real_t xc) const {
    using std::abs; using std::cos; using std::log;
    if (x < t_ && xc > 0) {
      return cos(2 * x) * log(abs(2 * sin(xc / 2) * sin((t_ + x) / 2)));
    } else if (x > t_ && xc < 0) {
      return cos(2 * x) * log(abs(2 * sin(-xc / 2) * sin((t_ + x) / 2)));
    } else {
      real_t s = abs(cos(x) - cos(t_));
      return s > 0 ? cos(2 * x) * log(s) : real_t(0);
    }
  }
  real_t result() const {
    using std::cos;
    return -boost::math::constants::pi<real_t>() * cos(2 * t_) / 2;
  }
private:
  real_t t_;
};

int main() {
  using std::abs; using std::sqrt;
  typedef double real_t;
  const real_t pi = boost::math::constants::pi<real_t>();

  boost::math::quadrature::tanh_sinh<real_t> integrator;
  real_t termination = sqrt(std::numeric_limits<real_t>::epsilon());

  real_t t = pi / 8;
  func<real_t> f(t);
  real_t error, L1;
  size_t levels;
  real_t q1 = integrator.integrate(f, 0, t, termination, &error, &L1, &levels);
  std::cout << std::scientific << std::setprecision(std::numeric_limits<real_t>::digits10)
            << "lower part: " << q1 << std::endl
            << "estimated error: " << error << std::endl
            << "L1 * error: " << L1 * error << std::endl
            << "levels: " << levels << std::endl;
  real_t q2 = integrator.integrate(f, t, pi, termination, &error, &L1, &levels);
  std::cout << "upper part: " << q2 << std::endl
            << "estimated error: " << error << std::endl
            << "L1 * error: " << L1 * error << std::endl
            << "levels: " << levels << std::endl;
  std::cout << "result: " << (q1 + q2) << std::endl
            << "real error: " << abs(q1 + q2 - f.result()) << std::endl;
}


//
//  neuralnetwork.h
//  NeuralNetwork
//
//  Created by Nikhil Joshi on 10/15/14.
//  Copyright (c) 2014 Nikhil Joshi. All rights reserved.
//

#ifndef __NeuralNetwork__neuralnetwork__
#define __NeuralNetwork__neuralnetwork__

#define EIGEN_DEFAULT_TO_ROW_MAJOR
#include <Eigen/Dense>

#include <iostream>
#include <ctime>
#include <vector>
#include <initializer_list>

#include "utility/utility.hpp"
#include "utility/functionalitysuite.hpp"

namespace NNetwork {

    class Network {
    public:
        // constructor
        Network(std::vector<unsigned int>&& topology) {
            _topology =  Eigen::toEigen(topology);
            initialize();
        }

        Network(std::initializer_list<unsigned int>& ls)
        : Network(std::vector<unsigned int>(ls))
        {
        }

        // Destructor
        ~Network(){
            for (int i = 0; i < _numLayers; i++)
                delete _functions[i];
        }

        // member functions
        // Setters/Getters
        // network topology
        void setTopology(const std::vector<unsigned int>& topology);
        void setTopology(const std::initializer_list<unsigned int>& topology);
        std::vector<unsigned int> getTopology(void)  const      { return Eigen::toStdVector(_topology); }
        size_t numInputs(void)   const                          { return _numInputs; }
        size_t numOutputs(void)  const                          { return _numOutputs; }
        size_t numLayers(void)   const                          { return _numLayers; }
        // weights
        std::vector<std::vector<double>> getWeightsAfterLayer(const unsigned int layer) const;
        Eigen::MatrixXd getWeightsAfterLayerEigen(const unsigned int layer) const;
        void printWeights(void) const;
        void setRandomWeights(void);
        void setRandomWeightsForLayer(unsigned int layer);
        void setWeightsAfterLayer(const unsigned int layer, const std::vector<std::vector<double>>& weights);
        void setWeightsAfterLayer(const unsigned int layer, const Eigen::Ref<const Eigen::MatrixXd>& weights);

        // activation function
        void setFunctionalityAtLayer(const unsigned int layer, FunctionalitySuite& suit);
        // make it a softmax output
        void setSoftmaxAtOutputLayer(void);
        bool isSoftmaxAtOutputLayer(void)                      { return _isSoftmax; }

        // Train the network on given examples
        // Data is assumed given in standard format :
        // One row = one (training) example n input/feature columns followed by m output/label columns
        double train(const char* dataFileName, const double regularizer,
                     const bool verbose = true);
        double train(const Eigen::MatrixXd& data, const double regularizer,
                     const bool verbose = true);

        // Predict
        unsigned int predict(const std::vector<double>& input);

        // Test methods
        friend bool isValidNetworkImplementation(Network& testNet);

    private:

        Eigen::VectorXu _topology;
        std::vector<Eigen::MatrixXd> _weights;
        std::vector<FunctionalitySuite*> _functions;
        size_t _numInputs, _numOutputs, _numLayers;
        bool _isSoftmax;

        // Member functions
        // initializers
        void initialize(void);
        void scaleWeightVectors(void);
        void setupDefaultNetwork(void);

        // Process input to produce output
        void processInput(const Eigen::Ref<const Eigen::VectorXd>& input,
                          std::vector<Eigen::VectorXd>& inputs,
                          std::vector<Eigen::VectorXd>& activations);
        // Calculate loss for the given input - label pair
        double calculateLoss(const Eigen::Ref<const Eigen::MatrixXd>& data,
                             const double regularizer = 0);
        double calculateLoss(const Eigen::Ref<const Eigen::VectorXd>& input,
                             const Eigen::Ref<const Eigen::VectorXd>& label,
                             const double regularizer = 0);

        // Calculate Error Delta by back Propagation
        void unregularizedLossAndWeightCorrectionsForExample(const Eigen::Ref<const Eigen::VectorXd>& input,
                                                             const Eigen::Ref<const Eigen::VectorXd>& label,
                                                             double& loss,
                                                             std::vector<Eigen::MatrixXd>& weightCorrections);
        void lossAndWeightCorrections(const Eigen::Ref<const Eigen::MatrixXd>& data,
                                      const double regularizer,
                                      double& loss,
                                      std::vector<Eigen::MatrixXd>& weightCorrections);
    };

    // Network implementation checks
    bool isValidNetworkImplementation(Network& testNet);

    // include implementation file
#include "network.ipp"

}

#endif /* defined(__NeuralNetwork__neuralnetwork__) */


#ifndef MPI_PI_PI_INCLUDED
#define MPI_PI_PI_INCLUDED

#include "../include/common.hpp"
#include "../include/random.hpp"
#include <boost/mpi.hpp>
#include <boost/multiprecision/number.hpp>

namespace mpi_pi {

namespace area_integral {

template <typename Number>
Number pi(const boost::mpi::communicator &comm, int root, std::size_t terms) {
  common::Division division(comm, terms);
  Number nterms = terms;
  Number delta = Number(1) / nterms;
  Number local_sum = 0;
  Number fxdx = 0;
  for (std::size_t i = division.first; i != division.last; ++i) {
    Number ni = i;
    Number x = ni / nterms;
    Number x1 = (ni + Number(1)) / nterms;
    Number fxdx1 = delta / (x1 * x1 + Number(1));
    if (i == division.first) {
      fxdx = delta / (x * x + Number(1));
    }
    local_sum += (fxdx + fxdx1) / Number(2);
    fxdx = fxdx1;
  }
  local_sum *= Number(4);
  Number result = 0;
  boost::mpi::reduce(comm, local_sum, result, std::plus<Number>(), root);
  return result;
}

} // namespace area_integral

namespace power_series {

template <typename Number>
Number pi(const boost::mpi::communicator &comm, int root, std::size_t terms) {
  common::Division division(comm, terms);
  Number local_sum = 0;
  for (std::size_t i = division.first; i != division.last; ++i) {
    Number ni = i;
    Number term = Number(1) / (Number(2) * ni + Number(1));
    if (i % 2 == 1)
      term = -term;
    local_sum += term;
  }
  local_sum *= Number(4);
  Number result = 0;
  boost::mpi::reduce(comm, local_sum, result, std::plus<Number>(), root);
  return result;
}

} // namespace power_series

namespace improved_power_series {

template <typename Number>
Number pi(const boost::mpi::communicator &comm, int root, std::size_t terms) {
  using boost::multiprecision::pow;
  using std::pow;

  common::Division division(comm, terms);
  Number local_sum = 0;
  for (std::size_t i = division.first; i != division.last; ++i) {
    Number ni = i;
    Number x = ni * Number(2) + Number(1);
    Number term5 = Number(4) / (x * pow(Number(5), x));
    Number term239 = Number(-1) / (x * pow(Number(239), x));
    if (i % 2 == 1) {
      term5 = -term5;
      term239 = -term239;
    }
    local_sum += term5 + term239;
  }
  local_sum *= Number(4);
  Number result = 0;
  boost::mpi::reduce(comm, local_sum, result, std::plus<Number>(), root);
  return result;
}

} // namespace improved_power_series

namespace monte_carlo {

template <typename Number>
Number pi(const boost::mpi::communicator &comm, int root, std::size_t terms) {
  common::Division division(comm, terms);
  random::MinimunStandardEngine random_engine(comm, root);
  std::uniform_real_distribution<double> dist(-1.0, 1.0);
  std::size_t local_hit = 0;
  for (std::size_t i = division.first; i != division.last; ++i) {
    auto x = dist(random_engine);
    auto y = dist(random_engine);
    if ((x * x + y * y) <= 1.0)
      local_hit += 1;
  }
  std::size_t total_hit = 0;
  boost::mpi::reduce(comm, local_hit, total_hit, std::plus<std::size_t>(),
                     root);
  Number result = 0;
  if (comm.rank() == root) {
    result = Number(total_hit) * Number(4) / Number(terms);
  }
  return result;
}

} // namespace monte_carlo

namespace monte_carlo_integral {

template <typename Number>
Number pi(const boost::mpi::communicator &comm, int root, std::size_t terms) {
  common::Division division(comm, terms);
  random::MinimunStandardEngine random_engine(comm, root);
  std::uniform_real_distribution<double> dist(0, 1.0);
  std::size_t local_hit = 0;
  for (std::size_t i = division.first; i != division.last; ++i) {
    auto x = dist(random_engine);
    auto y = dist(random_engine);
    double fx = 1.0 / (1.0 + x * x);
    if (y <= fx)
      local_hit += 1;
  }
  std::size_t total_hit = 0;
  boost::mpi::reduce(comm, local_hit, total_hit, std::plus<std::size_t>(),
                     root);
  Number result = 0;
  if (comm.rank() == root) {
    result = Number(total_hit) * Number(4) / Number(terms);
  }
  return result;
}

} // namespace monte_carlo_integral

namespace random_integral {

template <typename Number>
Number pi(const boost::mpi::communicator &comm, int root, std::size_t terms) {
  common::Division division(comm, terms);
  random::MinimunStandardEngine random_engine(comm, root);
  std::uniform_real_distribution<double> dist(0, 1.0);
  Number nterms = terms;
  Number delta = Number(1) / nterms;
  Number local_sum = 0;
  for (std::size_t i = division.first; i != division.last; ++i) {
    Number x = dist(random_engine);
    Number x1 = x + delta;
    Number fxdx = delta / (x * x + Number(1));
    Number fxdx1 = delta / (x1 * x1 + Number(1));
    local_sum += (fxdx + fxdx1) / Number(2);
  }
  local_sum *= Number(4);
  Number result = 0;
  boost::mpi::reduce(comm, local_sum, result, std::plus<Number>(), root);
  return result;
}

} // namespace random_integral

namespace borwein1987 {

template <typename Number>
Number pi(const boost::mpi::communicator &comm, int root, std::size_t terms) {
  using boost::multiprecision::pow;
  using boost::multiprecision::sqrt;
  using std::pow;
  using std::sqrt;

  if (comm.rank() == root) {
    std::cerr << "borwein1987 is an iterative method, will not run parallelized"
              << std::endl;
  }
  Number x = sqrt(Number(2.0));
  Number p = Number(2) + sqrt(Number(2.0));
  Number y = pow(Number(2.0), Number(1.0) / Number(4.0));
  for (std::size_t i = 0; i != terms; ++i) {
    x = Number(0.5) * (sqrt(x) + Number(1) / sqrt(x));
    p *= (x + Number(1)) / (y + Number(1));
    y = (y * sqrt(x) + Number(1) / sqrt(x)) / (y + Number(1));
  }
  return p;
}

} // namespace borwein1987

namespace yasumasa2002 {

template <typename Number>
Number pi(const boost::mpi::communicator &comm, int root, std::size_t terms) {
  using boost::multiprecision::pow;
  using std::pow;

  common::Division division(comm, terms);
  Number local_sum = 0;
  for (std::size_t i = division.first; i != division.last; ++i) {
    Number ni = i;
    Number x = ni * Number(2) + Number(1);
    Number term49 = Number(12) / (x * pow(Number(49), x));
    Number term57 = Number(32) / (x * pow(Number(57), x));
    Number term239 = Number(-5) / (x * pow(Number(239), x));
    Number term110443 = Number(12) / (x * pow(Number(110443), x));
    if (i % 2 == 1) {
      term49 = -term49;
      term57 = -term57;
      term239 = -term239;
      term110443 = -term110443;
    }
    local_sum += term49 + term57 + term239 + term110443;
  }
  local_sum *= Number(4);
  Number result = 0;
  boost::mpi::reduce(comm, local_sum, result, std::plus<Number>(), root);
  return result;
}

} // namespace yasumasa2002

namespace chudnovsky {

template <typename Number>
Number pi(const boost::mpi::communicator &comm, int root, std::size_t terms) {
  using boost::multiprecision::pow;
  using boost::multiprecision::sqrt;
  using std::pow;
  using std::sqrt;

  if (comm.rank() == root) {
    std::cerr << "this implementation of chudnovsky is iterative, will not run "
                 "parallelized"
              << std::endl;
  }

  Number K = 6;
  Number M = 1;
  Number L = 13591409;
  Number X = 1;
  Number x_multiplier = -pow(Number(640320), 3);
  Number sum = 13591409;
  for (std::size_t k = 0; k != terms; ++k) {
    M = (pow(K, Number(3)) - Number(16) * K) * M /
        pow(k + Number(1), Number(3));
    L += 545140134;
    X *= x_multiplier;
    sum += M * L / X;
    K += 12;
  }
  Number result = Number(426880) * sqrt(Number(10005)) / sum;
  return result;
}

} // namespace chudnovsky

namespace bbp {

template <typename Number>
Number pi(const boost::mpi::communicator &comm, int root, std::size_t terms) {
  using boost::multiprecision::pow;
  using std::pow;

  common::Division division(comm, terms);
  Number local_sum = 0;
  for (std::size_t i = division.first; i != division.last; ++i) {
    Number ni = Number(i);
    Number ni8 = Number(8) * ni;
    Number t1 = Number(4) / (ni8 + Number(1));
    Number t2 = Number(-2) / (ni8 + Number(4));
    Number t3 = Number(-1) / (ni8 + Number(5));
    Number t4 = Number(-1) / (ni8 + Number(6));
    Number term = (t1 + t2 + t3 + t4) / pow(Number(16), ni);
    local_sum += term;
  }
  Number result = 0;
  boost::mpi::reduce(comm, local_sum, result, std::plus<Number>(), root);
  return result;
}

} // namespace bbp

} // namespace mpi_pi

#endif


/**
 * @file sdirk.cc
 * @brief NPDE homework SDIRK code
 * @author Unknown, Oliver Rietmann
 * @date 31.03.2021
 * @copyright Developed at ETH Zurich
 */

#include "sdirk.h"

#include <Eigen/Dense>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>

#include "polyfit.h"

namespace SDIRK {

/* SAM_LISTING_BEGIN_0 */
Eigen::Vector2d sdirkStep(const Eigen::Vector2d &z0, double h, double gamma) {
  Eigen::Vector2d res;
  // TO DO (13-3.f): compute one timestep of the ODE
#if SOLUTION
  // Matrix A for evaluation of f
  Eigen::Matrix2d A;
  A << 0., 1., -1., -1.;
  // Reuse factorization
  auto A_lu = (Eigen::Matrix2d::Identity() - h * gamma * A).partialPivLu();
  Eigen::Vector2d az = A * z0;

  // Increments
  Eigen::Vector2d k1 = A_lu.solve(az);
  Eigen::Vector2d k2 = A_lu.solve(az + h * (1 - 2 * gamma) * A * k1);

  // Next step
  res = z0 + h * 0.5 * (k1 + k2);
#else
  //====================
  // Your code goes here
  //====================
#endif
  return res;
}
/* SAM_LISTING_END_0 */

/* SAM_LISTING_BEGIN_1 */
std::vector<Eigen::Vector2d> sdirkSolve(const Eigen::Vector2d &z0,
                                        unsigned int N, double T,
                                        double gamma) {
  // Solution vector
  std::vector<Eigen::Vector2d> res(N + 1);
  // TO DO (13-3.g): solve the ODE with uniform timesteps using the SDIRK method
#if SOLUTION
  // Equidistant step size
  const double h = T / N;
  // Push initial data
  res.at(0) = z0;
  // Main loop
  for (unsigned int i = 1; i <= N; ++i) {
    res.at(i) = sdirkStep(res.at(i - 1), h, gamma);
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  return res;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
double cvgSDIRK() {
  double conv_rate;
  // TO DO (13-3.g) study the convergence rate of the method.
#if SOLUTION
  // Initial data z0 = [y(0), y'(0)]
  Eigen::Vector2d z0;
  z0 << 1, 0;
  // Final time
  const double T = 10;
  // Parameter
  const double gamma = (3. + std::sqrt(3.)) / 6.;
  // Mesh sizes
  Eigen::ArrayXd err(10);
  Eigen::ArrayXd N(10);
  N << 20, 40, 80, 160, 320, 640, 1280, 2560, 5120, 10240;

  // Exact solution (only y(t)) given z0 = [y(0), y'(0)] and t
  auto yex = [&z0](double t) {
    return 1. / 3. * std::exp(-t / 2.) *
           (3. * z0(0) * std::cos(std::sqrt(3.) * t / 2.) +
            std::sqrt(3.) * z0(0) * std::sin(std::sqrt(3.) * t / 2.) +
            2. * std::sqrt(3.) * z0(1) * std::sin(std::sqrt(3.) * t / 2.));
  };

  // Store old error for rate computation
  double errold = 0;
  std::cout << std::setw(15) << "n" << std::setw(15) << "maxerr"
            << std::setw(15) << "rate" << std::endl;
  // Loop over all meshes
  for (unsigned int i = 0; i < N.size(); ++i) {
    int n = N(i);
    // Get solution
    auto sol = sdirkSolve(z0, n, T, gamma);
    // Compute error
    err(i) = std::abs(sol.back()(0) - yex(T));

    // Print table
    std::cout << std::setw(15) << n << std::setw(15) << err(i);
    if (i > 0) std::cout << std::setw(15) << std::log2(errold / err(i));
    std::cout << std::endl;

    // Store old error
    errold = err(i);
  }
  // Eigen::VectorXd Neig(N.data());
  // Eigen::VectorXd erreig (err.data());
  Eigen::VectorXd coeffs = polyfit(N.log(), err.log(), 1);
  conv_rate = -coeffs(0);
#else
  //====================
  // Your code goes here
  //====================
#endif
  return conv_rate;
}
/* SAM_LISTING_END_2 */

}  // namespace SDIRK


//
// hough3dlines.cpp
//     Main program implementing the iterative Hough transform
//
// Author:  Tilman Schramke, Christoph Dalitz
// Date:    2020-01-15
// License: see License-BSD2
//

#include "vector3d.h"
#include "pointcloud.h"
#include "hough.h"

#include <cstdlib>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <Eigen/Dense>

using Eigen::MatrixXf;
using namespace std;

// usage message
const char* usage = "Usage:\n"
  "\though3dlines [options] <infile>\n"
  "Options (defaults in brackets):\n"
  "\t-o <outfile>   write results to <outfile> [stdout]\n"
  "\t-dx <dx>       step width in x'y'-plane [0]\n"
  "\t               when <dx>=0, it is set to 1/64 of total width\n"
  "\t-nlines <nl>   maximum number of lines returned [0]\n"
  "\t               when <nl>=0, all lines are returned\n"
  "\t-minvotes <nv> only lines with at least <nv> points are returned [0]\n"
  "\t-gnuplot       print result as a gnuplot command\n"
  "\t-raw           print plot data in easily machine-parsable format\n"
  "\t-delim <char>  use <char> as field delimiter in input file [,]\n"
  "\t-v             be verbose and print Hough space size to stdout\n"
  "\t-vv            be even more verbose and print Hough lines (before LSQ)\n"
  "Version:\n"
  "\t1.2 from 2020-01-15\n";

//--------------------------------------------------------------------
// utility functions
//--------------------------------------------------------------------

//
// orthogonal least squares fit with libeigen
// rc = largest eigenvalue
//
double orthogonal_LSQ(const PointCloud &pc, Vector3d* a, Vector3d* b){
  double rc = 0.0;

  // anchor point is mean value
  *a = pc.meanValue();

  // copy points to libeigen matrix
  Eigen::MatrixXf points = Eigen::MatrixXf::Constant(pc.points.size(), 3, 0);
  for (int i = 0; i < points.rows(); i++) {
    points(i,0) = pc.points.at(i).x;
    points(i,1) = pc.points.at(i).y;
    points(i,2) = pc.points.at(i).z;
  }

  // compute scatter matrix ...
  MatrixXf centered = points.rowwise() - points.colwise().mean();
  MatrixXf scatter = (centered.adjoint() * centered);

  // ... and its eigenvalues and eigenvectors
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eig(scatter);
  Eigen::MatrixXf eigvecs = eig.eigenvectors();

  // we need eigenvector to largest eigenvalue
  // libeigen yields it as LAST column
  b->x = eigvecs(0,2); b->y = eigvecs(1,2); b->z = eigvecs(2,2);
  rc = eig.eigenvalues()(2);

  return (rc);
}

//--------------------------------------------------------------------
// main program
//--------------------------------------------------------------------

int main(int argc, char ** argv) {

  // default values for command line options
  double opt_dx = 0.0;
  int opt_nlines = 0;
  int opt_minvotes = 0;
  enum Outformat { format_normal, format_gnuplot, format_raw };
  Outformat opt_outformat = format_normal;
  char opt_delim = ',';
  int opt_verbose = 0;
  char* infile_name = NULL;
  char* outfile_name = NULL;

  // number of icosahedron subdivisions for direction discretization
  int granularity = 4;
  int num_directions[7] = {12, 21, 81, 321, 1281, 5121, 20481};

  // IO files
  //FILE* infile = NULL;
  FILE* outfile = stdout;

  // bounding box of point cloud
  Vector3d minP, maxP, minPshifted, maxPshifted;
  // diagonal length of point cloud
  double d;

  // parse command line
  for (int i=1; i<argc; i++) {
    if (0 == strcmp(argv[i], "-o")) {
      i++;
      if (i<argc) outfile_name = argv[i];
    }
    else if (0 == strcmp(argv[i], "-dx")) {
      i++;
      if (i<argc) opt_dx = atof(argv[i]);
    }
    else if (0 == strcmp(argv[i], "-nlines")) {
      i++;
      if (i<argc) opt_nlines = atoi(argv[i]);
    }
    else if (0 == strcmp(argv[i], "-minvotes")) {
      i++;
      if (i<argc) opt_minvotes = atoi(argv[i]);
    }
    else if (0 == strcmp(argv[i], "-gnuplot")) {
      opt_outformat = format_gnuplot;
    }
    else if (0 == strcmp(argv[i], "-raw")) {
      opt_outformat = format_raw;
    }
    else if (0 == strcmp(argv[i], "-delim")) {
      i++;
      if (i<argc) opt_delim = argv[i][0];
    }
    else if (0 == strcmp(argv[i], "-v")) {
      opt_verbose = 1;
    }
    else if (0 == strcmp(argv[i], "-vv")) {
      opt_verbose = 2;
    }

    else if (argv[i][0] == '-') {
      fprintf(stderr, "%s", usage);
      return 1;
    }
    else {
      infile_name = argv[i];
    }
  }

  // plausibilty checks
  if (!infile_name) {
    fprintf(stderr, "Error: no infile given!\n%s", usage);
    return 1;
  }
  if (opt_dx < 0){
    fprintf(stderr, "Error: dx < 0!\n%s", usage);
    return 1;
  }
  if (opt_nlines < 0){
    fprintf(stderr, "Error: nlines < 0!\n%s", usage);
    return 1;
  }
  if (opt_minvotes < 0){
    fprintf(stderr, "Error: minvotes < 0!\n%s", usage);
    return 1;
  }
  if (opt_minvotes < 2){
    opt_minvotes = 2;
  }

  // open in/out files
  if (outfile_name) {
    outfile = fopen(outfile_name, "w");
    if (!outfile) {
      fprintf(stderr, "Error: cannot open outfile '%s'!\n", outfile_name);
      return 1;
    }
  }

  // read point cloud from file
  PointCloud X;
  int errorcode = X.readFromFile(infile_name, opt_delim);
  if (2 == errorcode) {
    fprintf(stderr, "Error: wrong file format of infile '%s'!\n", infile_name);
    return 1;
  }
  else if (0 != errorcode) {
    fprintf(stderr, "Error: cannot read infile '%s'!\n", infile_name);
    return 1;
  }
  if (X.points.size() < 2) {
    fprintf(stderr, "Error: point cloud has less than two points\n");
    return 1;
  }

  // center cloud and compute new bounding box
  X.getMinMax3D(&minP, &maxP);
  d = (maxP-minP).norm();
  if (d == 0.0) {
    fprintf(stderr, "Error: all points in point cloud identical\n");
    return 1;
  }
  X.shiftToOrigin();
  X.getMinMax3D(&minPshifted, &maxPshifted);

  // estimate size of Hough space
  if (opt_dx == 0.0) {
    opt_dx = d / 64.0;
  }
  else if (opt_dx >= d) {
    fprintf(stderr, "Error: dx too large\n");
    return 1;
  }
  double num_x = floor(d / opt_dx + 0.5);
  double num_cells = num_x * num_x * num_directions[granularity];
  if (opt_verbose) {
    printf("info: x'y' value range is %f in %.0f steps of width dx=%f\n",
           d, num_x, opt_dx);
    printf("info: Hough space has %.0f cells taking %.2f MB memory space\n",
           num_cells, num_cells * sizeof(unsigned int) / 1000000.0);
  }
#ifdef WEBDEMO
  if (num_cells > 1E8) {
    fprintf(stderr, "Error: program was compiled in WEBDEMO mode, "
            "which does not permit %.0f cells in Hough space\n", num_cells);
    return 2;
  }
#endif

  // first Hough transform
  Hough* hough;
  try {
    hough = new Hough(minPshifted, maxPshifted, opt_dx, granularity);
  } catch (const std::exception &e) {
    fprintf(stderr, "Error: cannot allocate memory for %.0f Hough cells"
            " (%.2f MB)\n", num_cells,
            (double(num_cells) / 1000000.0) * sizeof(unsigned int));
    return 2;
  }
  hough->add(X);

  // print header info if necessary
  if (opt_outformat == format_gnuplot) {
    fprintf(outfile, "set datafile separator '%c'\n"
            "set parametric\n"
            "set xrange [%f:%f]\n"
            "set yrange [%f:%f]\n"
            "set zrange [%f:%f]\n"
            "set urange [%f:%f]\n"
            "splot '%s' using 1:2:3 with points palette",
            opt_delim,
            minP.x, maxP.x, minP.y, maxP.y, minP.z, maxP.z,
            -d, d, infile_name);
  }
  else if (opt_outformat == format_raw) {
    fprintf(outfile, "%f %f %f %f %f %f\n%f %f\n",
            minP.x, maxP.x, minP.y, maxP.y, minP.z, maxP.z, -d, d);
  }

  // iterative Hough transform
  // (Algorithm 1 in IPOL paper)
  PointCloud Y;	// points close to line
  double rc;
  unsigned int nvotes;
  int nlines = 0;
  do {
    Vector3d a; // anchor point of line
    Vector3d b; // direction of line

    hough->subtract(Y); // do it here to save one call

    nvotes = hough->getLine(&a, &b);
    if (opt_verbose > 1) {
      Vector3d p = a + X.shift;
      printf("info: highest number of Hough votes is %i for the following "
             "line:\ninfo: a=(%f,%f,%f), b=(%f,%f,%f)\n",
             nvotes, p.x, p.y, p.z, b.x, b.y, b.z);
    }

    X.pointsCloseToLine(a, b, opt_dx, &Y);

    rc = orthogonal_LSQ(Y, &a, &b);
    if (rc==0.0) break;

    X.pointsCloseToLine(a, b, opt_dx, &Y);
    nvotes = Y.points.size();
    if (nvotes < (unsigned int)opt_minvotes) break;

    rc = orthogonal_LSQ(Y, &a, &b);
    if (rc==0.0) break;

    a = a + X.shift;

    nlines++;
    if (opt_outformat == format_normal) {
      fprintf(outfile, "npoints=%lu, a=(%f,%f,%f), b=(%f,%f,%f)\n",
              Y.points.size(), a.x, a.y, a.z, b.x, b.y, b.z);
    }
    else if (opt_outformat == format_gnuplot) {
      fputs(", \\\n    ", outfile);
      fprintf(outfile, "%f + u * %f, %f + u * %f, %f + u * %f "
              "with lines notitle lc rgb 'black'",
              a.x, b.x, a.y, b.y, a.z, b.z);
    }
    else {
      fprintf(outfile, "%f %f %f %f %f %f %lu\n",
              a.x, a.y, a.z, b.x, b.y, b.z, Y.points.size());
    }

    X.removePoints(Y);

  } while ((X.points.size() > 1) &&
           ((opt_nlines == 0) || (opt_nlines > nlines)));

  // final newline in gnuplot command
  if (opt_outformat == format_gnuplot)
    fputs("\n", outfile);

  // clean up
  delete hough;
  if (outfile_name) fclose(outfile);

  return 0;
}


#include "triangle.hpp"

#include <cmath>
#include <iostream>
#include <stdexcept>

#include <boost/format.hpp>

#include "base-types.hpp"

namespace
{
  bool areValidVertices(const yakovlev::point_t vertices[3]);
}

yakovlev::Triangle::Triangle(
    const point_t & vertice1,
    const point_t & vertice2,
    const point_t & vertice3,
    double rotation
) :
  vertices_{ vertice1, vertice2, vertice3 },
  pos_{ (vertice1.x + vertice2.x + vertice3.x) / 3.0, (vertice1.y + vertice2.y + vertice3.y) / 3.0 }
{
  if (!areValidVertices(vertices_)) {
    throw std::invalid_argument{ "Triangle must not be degenerate" };
  }
  rotate(rotation);
}

bool yakovlev::Triangle::operator==(const Triangle & other) const noexcept
{
  for (const point_t & vert : vertices_) {
    bool hasEqual = false;
    for (const point_t & othervert : other.vertices_) {
      if (vert == othervert) {
        hasEqual = true;
      }
    }
    if (!hasEqual) {
      return false;
    }
  }
  return true;
}

bool yakovlev::Triangle::operator!=(const Triangle & other) const noexcept
{
  return !(*this == other);
}

yakovlev::point_t yakovlev::Triangle::getCenter() const noexcept
{
  return pos_;
}

double yakovlev::Triangle::getArea() const noexcept
{
  return std::abs(vertices_[0].x * (vertices_[1].y - vertices_[2].y)
        + vertices_[1].x * (vertices_[2].y - vertices_[0].y)
        + vertices_[2].x * (vertices_[0].y - vertices_[1].y))
      / 2.0;
}

yakovlev::rectangle_t yakovlev::Triangle::getFrameRect() const noexcept
{
  double maxX = vertices_[0].x, maxY = vertices_[0].y;
  double minX = vertices_[0].x, minY = vertices_[0].y;
  for (int i = 1; i < 3; ++i) {
    minX = std::min(vertices_[i].x, minX);
    maxX = std::max(vertices_[i].x, maxX);
    minY = std::min(vertices_[i].y, minY);
    maxY = std::max(vertices_[i].y, maxY);
  }

  double width = maxX - minX, height = maxY - minY;
  return { width, height, { minX + (width / 2.0), minY + (height / 2.0) } };
}

void yakovlev::Triangle::move(const point_t & pos) noexcept
{
  move(pos.x - pos_.x, pos.y - pos_.y);
}

void yakovlev::Triangle::move(double x, double y) noexcept
{
  for (point_t &vert : vertices_) {
    vert.x += x;
    vert.y += y;
  }
  pos_.x += x;
  pos_.y += y;
}

void yakovlev::Triangle::rotate(double rotation) noexcept
{
  double rad = rotation * M_PI / 180.0;
  double rotSin = sin(rad);
  double rotCos = cos(rad);
  for (point_t & vert : vertices_) {
    point_t local = { vert.x - pos_.x, vert.y - pos_.y };
    vert = {
        pos_.x + local.x * rotCos - local.y * rotSin,
        pos_.y + local.x * rotSin + local.y * rotCos
    };
  }
}

void yakovlev::Triangle::scale(double coef)
{
  if (coef <= 0.0) {
    throw std::invalid_argument{ (boost::format("Invalid rectangle scale coefficient %1% - can not be negative or zero") % coef ).str() };
  }
  for (point_t & vert : vertices_) {
    vert = {
        pos_.x + (vert.x - pos_.x) * coef,
        pos_.y + (vert.y - pos_.y) * coef
    };
  }
}

void yakovlev::Triangle::print(std::ostream & os) const
{
  os << "Triangle with verticies on " << vertices_[0] << ", " << vertices_[1]
      << ", " << vertices_[2] << '\n';
}

namespace
{
  bool areValidVertices(const yakovlev::point_t vertices[3])
  {
    double sides[3] = {
        yakovlev::getDistanceBetweenPoints(vertices[0], vertices[1]),
        yakovlev::getDistanceBetweenPoints(vertices[1], vertices[2]),
        yakovlev::getDistanceBetweenPoints(vertices[0], vertices[2])
    };
    return ((sides[0] + sides[1]) > sides[2])
        && ((sides[0] + sides[2]) > sides[1])
        && ((sides[1] + sides[2]) > sides[0]);
  }
}


// ===========================================================================
// Imagine++ Libraries
// Copyright (C) Imagine
// For detailed information: http://imagine.enpc.fr/software
// ===========================================================================

#include "Imagine/LinAlg/MyEigen.h"
#include <Eigen/Dense>
using namespace Eigen;

// Eigen value decomposition of packed symmetric matrix
template <typename T>
void subSymEigenValues(int *n,T *ap,T *w,T *z,int *info)
{
  T *t= new T [(*n)*(*n)];
  symToMatrix(*n,ap,t);
  Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > AMap (t,*n,*n,OuterStride<>(*n));
   Eigen::JacobiSVD<Matrix<T, Dynamic, Dynamic> > svd (AMap, ComputeFullU | ComputeFullV);
   Map<Matrix<T, Dynamic, Dynamic>, 0, OuterStride<> > zMap (z,*n,*n,OuterStride<>(*n));
   Map<Matrix<T, Dynamic, 1>, 0, InnerStride<0> > wMap (w, *n);
   wMap=svd.singularValues();
   zMap=svd.matrixU();
   Matrix<T, Dynamic, Dynamic> v=svd.matrixV();
   *info=1-AMap.isApprox(zMap * wMap.asDiagonal() * v.adjoint());
   for (int i=0;i<*n;i++){
     if (v.col(i).isApprox(-zMap.col(i)))
     {
       wMap(i)=-wMap(i);
     }
   };
   delete[] t;
}

void symEigenValues(int *n, double *ap, double *w, double *z, int* info) { subSymEigenValues<double>(n, ap, w, z, info); }
void symEigenValues(int *n, float *ap, float *w, float *z, int* info) { subSymEigenValues<float>(n, ap, w, z, info); }


/*
 *  trajectory_factory.hpp
 *
 *  Author(s): Tamas D. Nagy
 *	Created on: 2016-10-25
 *
 *  Static class to generate simple trajectories
 *  built of any corresponding data type.
 *
 */

#ifndef DVRK_TRAJECTORY_FACTORY_HPP_
#define DVRK_TRAJECTORY_FACTORY_HPP_

#include <iostream>
#include <vector>
#include <ros/ros.h>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include "irob_utils/trajectory.hpp"
#include "irob_utils/utils.hpp"
#include "irob_utils/tool_pose.hpp"

namespace saf {

class TrajectoryFactory
{

private:
  TrajectoryFactory() {}
public:

  /**
   *	Uniform ramp between x1 and x2.
   */
  static Trajectory<double> uniformRamp(int N,
                                        double x1=0.0, double x2=1.0)
  {
    Trajectory<double> v;
    for (int i = 0; i < N; i++)
    {
      v.addPoint(interpolate(((double)i)/(N-1), x1, x2));
    }
    return v;
  }

  /**
   *	Accelerating ramp between x1 and x2.
   *	For decceleration: acceleratingRamp(N, x2, x1).reverse().
   */
  static Trajectory<double> acceleratingRamp(int N,
                                             double x1=0.0, double x2=1.0)
  {
    Trajectory<double> v, steps;
    double maxStep = (2.0*(x2-x1))/N;
    steps = uniformRamp(N, 0.0, maxStep);

    double a = x1;
    for (int i = 0; i < N; i++)
    {
      a += steps[i];
      v.addPoint(interpolate(a, x1, x2));
    }
    return v;
  }

  /**
   *	Smooth ramp between x1 and x2.
   */
  static Trajectory<double> smoothenedRamp(int Nfull, int Nacc, int Ndecc,
                                           double x1=0.0, double x2=1.0)
  {
    Trajectory<double> v, stepsAcc, stepsDecc;

    if (Nfull < Nacc || Nfull < Ndecc)
      throw std::runtime_error("Trajectory Nacc > Nfull");

    double accRatio = ((double)Nacc) / Nfull;
    double deccRatio = ((double)Ndecc) / Nfull;
    double maxStep = (x2-x1)/(Nfull-((Nacc+Ndecc)/2.0));
    stepsAcc = uniformRamp(Nacc, 0.0, maxStep);
    stepsDecc = uniformRamp(Ndecc, maxStep, 0.0);

    double a = x1;
    for (int i = 0; i < Nacc; i++)
    {
      a += stepsAcc[i];
      v.addPoint(interpolate(a, x1, x2));
    }

    for (int i = 0; i < Nfull-(Nacc + Ndecc); i++)
    {
      a += maxStep;
      v.addPoint(interpolate(a, x1, x2));
    }

    for (int i = 0; i < Ndecc; i++)
    {
      a += stepsDecc[i];
      v.addPoint(interpolate(a, x1, x2));
    }
    return v;
  }

  /**
   * Linear trajectory between start and end, with constant speed.
   */
  template <class P>
  static Trajectory<P> linearTrajectory(P start,P end,
                                        double speed, double dt)
  {
    Trajectory<P> tr(dt);
    double stepSize = speed * dt;
    int N = (int)round(distanceEuler(start, end) / stepSize)+1;
    Trajectory<double> ramp = uniformRamp(N);
    for (int i = 0; i < ramp.size(); i++)
    {
      tr.addPoint(interpolate(ramp[i], start, end));
    }
    return tr;
  }

  /**
   *	Smoothly accelerationg trajectory between 2 points.
   */
  template <class P>
  static Trajectory<P> linearTrajectoryWithSmoothAcceleration(
      P start,
      P end,
      double speed,
      double acc,
      double dt)
  {
    Trajectory<P> tr(dt);
    double s_full = distanceEuler(start, end);
    if (s_full < 0.00001)
      return tr;

    double t_acc = speed / acc;
    double s_acc = 0.5 * acc * t_acc * t_acc;

    double t_full = (2.0 * t_acc) + ((s_full-(2.0 * s_acc)) / speed);

    int N_full = (int)round(t_full / dt)+1;
    int N_acc = (int)round(t_acc / dt)+1;

    Trajectory<double> ramp = smoothenedRamp(N_full, N_acc, N_acc);

    for (int i = 0; i < ramp.size(); i++)
    {
      tr.addPoint(interpolate(ramp[i], start, end));
    }
    return tr;
  }

  /**
     *	Smoothly accelerationg trajectory between 2 points with waypoints.
     */
  template <class P>
  static Trajectory<P> linearTrajectoryWithSmoothAcceleration(
      P start,
      std::vector<P> waypoints,
      P end,
      double speed,
      double acc,
      double dt)
  {
    if (waypoints.empty())
      return linearTrajectoryWithSmoothAcceleration(
            start, end, speed, acc, dt);
    Trajectory<P> tr(dt);

    // To first waypoints
    double s_full = distanceEuler(start,  waypoints[0]);
    double t_acc = speed / acc;
    double s_acc = 0.5 * acc * t_acc * t_acc;
    double t_full = t_acc + ((s_full-s_acc) / speed);

    int N_full = (int)round(t_full / dt)+1;
    int N_acc = (int)round(t_acc / dt)+1;
    Trajectory<double> acc_ramp = smoothenedRamp(N_full, N_acc, 0);

    for (int i = 0; i < acc_ramp.size(); i++)
    {
      tr.addPoint(interpolate(acc_ramp[i], start, waypoints[0]));
    }

    // Between waypoints
    for(typename std::vector<P>::size_type i = 0;
        i < waypoints.size()-1; i++)
    {
      s_full = distanceEuler(waypoints[i],waypoints[i+1]);
      t_full = s_full / speed;

      N_full = (int)round(t_full / dt)+1;
      Trajectory<double> ramp = uniformRamp(N_full);
      for (int j = 0; j < ramp.size(); j++)
      {
        tr.addPoint(interpolate(ramp[j],
                                waypoints[i],
                                waypoints[i+1]));
      }
    }

    // To endpoint
    s_full = distanceEuler(waypoints.back(), end);
    t_acc = speed / acc;
    s_acc = 0.5 * acc * t_acc * t_acc;
    t_full = t_acc + ((s_full-s_acc) / speed);

    N_full = (int)round(t_full / dt)+1;
    N_acc = (int)round(t_acc / dt)+1;

    Trajectory<double> decc_ramp = smoothenedRamp(N_full, 0, N_acc);
    for (int i = 0; i < decc_ramp.size(); i++)
    {
      tr.addPoint(interpolate(decc_ramp[i],
                              waypoints.back(),
                              end));
    }

    return tr;
  }

  /**
   * Horizontal circular trajectory around center.
   */
  static Trajectory<Eigen::Vector3d> circleTrajectoryHorizontal(
      Eigen::Vector3d start,
      double toAngle, Eigen::Vector3d center,
      double T, double dt)
  {
    Trajectory<Eigen::Vector3d> tr(dt);
    int N = (int)round(T / dt)+1;
    Trajectory<double> ramp = uniformRamp(N, 0.0, toAngle);
    for (int i = 0; i < ramp.size(); i++)
    {
      Eigen::Vector3d p = start-center;
      Eigen::Vector3d p1(p.x()*cos(ramp[i])-p.y()*sin(ramp[i]),
                         p.y()*cos(ramp[i])+p.x()*sin(ramp[i]), p.z());
      Eigen::Vector3d p2 = p1+center;
      tr.addPoint(p2);
    }
    return tr;
  }

  /**
   * Horizontal circular trajectory around center.
   */
  static Trajectory<ToolPose> circleTrajectoryHorizontal(
      ToolPose start,
      double toAngle, Eigen::Vector3d center,
      double T, double dt)
  {
    Trajectory<ToolPose> tr(dt);
    int N = (int)round(T / dt)+1;
    Trajectory<double> ramp = uniformRamp(N, 0.0, toAngle);
    for (int i = 0; i < ramp.size(); i++)
    {
      Eigen::Vector3d p = start.transform.translation()-center;
      Eigen::Vector3d p1(p.x()*cos(ramp[i])-p.y()*sin(ramp[i]),
                         p.y()*cos(ramp[i])+p.x()*sin(ramp[i]), p.z());
      Eigen::Vector3d p2 = p1+center;
      ToolPose po2(p2, Eigen::Quaterniond(start.transform.rotation()), start.jaw);
      tr.addPoint(po2);
    }
    return tr;
  }

  /**
   * Vertical circular trajectory around center in Y plane.
   */
  static Trajectory<Eigen::Vector3d> circleTrajectoryVerticalY(
      Eigen::Vector3d start,
      double toAngle, Eigen::Vector3d center,
      double T, double dt)
  {
    Trajectory<Eigen::Vector3d> tr(dt);
    int N = (int)round(T / dt)+1;
    Trajectory<double> ramp = uniformRamp(N, 0.0, toAngle);
    for (int i = 0; i < ramp.size(); i++)
    {
      Eigen::Vector3d p = start-center;
      Eigen::Vector3d p1(p.x(),
                         p.y()*cos(ramp[i])+p.z()*sin(ramp[i]), p.z()*cos(ramp[i])-p.y()*sin(ramp[i]));
      Eigen::Vector3d p2 = p1+center;
      tr.addPoint(p2);
    }
    return tr;
  }

  /**
   * Vertical circular trajectory around center in X plane.
   */
  static Trajectory<Eigen::Vector3d> circleTrajectoryVerticalX(
      Eigen::Vector3d start,
      double toAngle, Eigen::Vector3d center,
      double T, double dt)
  {
    Trajectory<Eigen::Vector3d> tr(dt);
    int N = (int)round(T / dt)+1;
    Trajectory<double> ramp = uniformRamp(N, 0.0, toAngle);
    for (int i = 0; i < ramp.size(); i++)
    {
      Eigen::Vector3d p = start-center;
      Eigen::Vector3d p1(p.x()*cos(ramp[i])+p.z()*sin(ramp[i]),
                         p.y(), p.z()*cos(ramp[i])-p.x()*sin(ramp[i]));
      Eigen::Vector3d p2 = p1+center;
      tr.addPoint(p2);
    }
    return tr;
  }

};

}

#endif


#ifndef DZNL_BFGS_OPTIMIZER_HPP_INCLUDED
#define DZNL_BFGS_OPTIMIZER_HPP_INCLUDED

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

// Eigen linear algebra library headers
#include <Eigen/Core>

// Project-specific headers
#include "QuadraticLineSearcher.hpp"

namespace dznl {

    enum class StepType { NONE, GRAD, BFGS };

    template <typename T>
    class BFGSOptimizer {
    private: // ========================================== INTERNAL TYPE ALIASES
        typedef std::function<T(const T *)> objective_function_t;
        typedef std::function<void(T *, const T *)> objective_gradient_t;

        typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> MatrixXT;
        typedef Eigen::Matrix<T, Eigen::Dynamic, 1> VectorXT;

    private: // =============================================== MEMBER VARIABLES
        const std::size_t n;
        const objective_function_t f;
        const objective_gradient_t g;

        VectorXT x;
        T fx;
        VectorXT gx;
        VectorXT dg;

        T step_size;
        VectorXT step_dir;
        VectorXT grad_dir;
        MatrixXT hess_inv;

        VectorXT temp;
        std::size_t iter_count;
        StepType last_step_type;

    public: // ===================================================== CONSTRUCTOR
        BFGSOptimizer(std::size_t num_dimensions,
                      const objective_function_t &objective_function,
                      const objective_gradient_t &objective_gradient)
                : n(num_dimensions),
                  f(objective_function),
                  g(objective_gradient),
                  x(n),
                  fx(0),
                  gx(n),
                  dg(n),
                  step_size(0),
                  step_dir(n),
                  grad_dir(n),
                  hess_inv(n, n),
                  temp(n),
                  iter_count(0),
                  last_step_type(StepType::NONE) {
            hess_inv.setIdentity();
        }

    public: // ======================================================= ACCESSORS
        const VectorXT &get_current_point() { return x; }

        T get_current_objective_value() { return fx; }

        const VectorXT &get_current_gradient() { return gx; }

        std::size_t get_iteration_count() { return iter_count; }

        T get_last_step_size() { return step_size; }

        StepType get_last_step_type() { return last_step_type; }

    public: // ======================================================== MUTATORS
        void set_current_point(const VectorXT &p) {
            x = p;
            fx = f(p.data());
            g(gx.data(), p.data());
        }

        void set_iteration_count(std::size_t k) { iter_count = k; }

        void set_step_size(const T &h) { step_size = h; }

    private: // ==================================== OPTIMIZATION HELPER METHODS
        void reset_hessian() { hess_inv.setIdentity(); }

        void update_inverse_hessian() {
            temp = hess_inv.template selfadjointView<Eigen::Upper>() * dg;
            const T lambda = step_size * dg.dot(step_dir);
            const T sigma = (lambda + dg.dot(temp)) / (lambda * lambda);
            temp -= (step_size * lambda * sigma / 2) * step_dir;
            hess_inv.template selfadjointView<Eigen::Upper>().rankUpdate(
                    temp, step_dir, -step_size / lambda);
        }

        T competitive_line_search() {
            dznl::QuadraticLineSearcher<T> grad_searcher(f, x, grad_dir);
            grad_searcher.search(step_size);
            dznl::QuadraticLineSearcher<T> bfgs_searcher(f, x, step_dir);
            bfgs_searcher.search(step_size);
            if (grad_searcher.get_best_objective_value() <
                bfgs_searcher.get_best_objective_value()) {
                last_step_type = StepType::GRAD;
                step_dir = grad_dir;
                reset_hessian();
                fx = grad_searcher.get_best_objective_value();
                return grad_searcher.get_best_step_size();
            } else {
                last_step_type = StepType::BFGS;
                fx = bfgs_searcher.get_best_objective_value();
                return bfgs_searcher.get_best_step_size();
            }
        }

    public: // ============================================ OPTIMIZATION METHODS
        bool step() {
            grad_dir = -gx.normalized();
            step_dir =
                    -(hess_inv.template selfadjointView<Eigen::Upper>() * gx);
            step_dir.normalize();
            const T new_step_size = competitive_line_search();
            if (new_step_size == 0) { return false; }
            ++iter_count;
            step_size = new_step_size;
            x += step_size * step_dir;
            g(temp.data(), x.data());
            dg = temp - gx;
            gx.swap(temp);
            update_inverse_hessian();
            return true;
        }

    }; // class BFGSOptimizer

} // namespace dznl

#endif // DZNL_BFGS_OPTIMIZER_HPP_INCLUDED


//-----------------------------------------------------------------------------
// Copyright (c) 2016 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
//-----------------------------------------------------------------------------
#ifndef _mitrax__Eigen__convolution__hpp_INCLUDED_
#define _mitrax__Eigen__convolution__hpp_INCLUDED_

#include <Eigen/Core>

namespace Eigen{

	template < typename DerivedM, typename DerivedK >
	inline auto convolution(
		MatrixBase< DerivedM > const& m,
		MatrixBase< DerivedK > const& k
	){
		using ScalarK = typename DerivedK::Scalar;

		using ResultType = Matrix< ScalarK, Eigen::Dynamic, Eigen::Dynamic >;

		int kr = k.rows();
		int kc = k.cols();

		int res_r = m.rows() - kr + 1;
		int res_c = m.cols() - kc + 1;

		ResultType res(res_r, res_c);

		for(int my = 0; my < res_r; ++my){
			for(int mx = 0; mx < res_c; ++mx){
				ScalarK b = 0;

				for(int ky = 0; ky < kr; ++ky){
					for(int kx = 0; kx < kc; ++kx){
						b += m(my + ky, mx + kx) * k(ky, kx);
					}
				}

				res.coeffRef(my, mx) = b;
			}
		}

		return res;
	}

	template < typename DerivedM, typename DerivedKC, typename DerivedKR >
	inline auto convolution(
		MatrixBase< DerivedM > const& m,
		MatrixBase< DerivedKC > const& vc,
		MatrixBase< DerivedKR > const& vr
	){
		return convolution(convolution(m, vr), vc);
	}

}

#endif


#include <iostream>
#include <cstdlib>

#include <Eigen/Core>
#include <Eigen/Eigenvalues>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv)
{
	int mag_data_counter = 1000;
	FILE * fp;
	double mag_x, mag_y, mag_z;

	MatrixXd mat_D(mag_data_counter, 9);
	MatrixXd mat_DT;
	fp = fopen("magdata", "r");

	for (int i = 0; i < mag_data_counter; i++)
	{

		fscanf(fp, "%lf %lf %lf\n", &mag_x, &mag_y, &mag_z);
		mat_D(i, 0) = mag_x * mag_x;
		mat_D(i, 1) = mag_y * mag_y;
		mat_D(i, 2) = mag_z * mag_z;
		mat_D(i, 3) = 2 * mag_x * mag_y;
		mat_D(i, 4) = 2 * mag_x * mag_z;
		mat_D(i, 5) = 2 * mag_y * mag_z;
		mat_D(i, 6) = 2 * mag_x;
		mat_D(i, 7) = 2 * mag_y;
		mat_D(i, 8) = 2 * mag_z;
	}
	fclose(fp);
	mat_DT = mat_D.transpose();

	MatrixXd mat_Ones = MatrixXd::Ones(mag_data_counter, 1);

	MatrixXd mat_Result =  (mat_DT * mat_D).inverse() * (mat_DT * mat_Ones);

	Matrix<double, 4, 4>  mat_A_4x4;

	mat_A_4x4(0, 0) = mat_Result(0, 0);
	mat_A_4x4(0, 1) = mat_Result(3, 0);
	mat_A_4x4(0, 2) = mat_Result(4, 0);
	mat_A_4x4(0, 3) = mat_Result(6, 0);

	mat_A_4x4(1, 0) = mat_Result(3, 0);
	mat_A_4x4(1, 1) = mat_Result(1, 0);
	mat_A_4x4(1, 2) = mat_Result(5, 0);
	mat_A_4x4(1, 3) = mat_Result(7, 0);

	mat_A_4x4(2, 0) = mat_Result(4, 0);
	mat_A_4x4(2, 1) = mat_Result(5, 0);
	mat_A_4x4(2, 2) = mat_Result(2, 0);
	mat_A_4x4(2, 3) = mat_Result(8, 0);

	mat_A_4x4(3, 0) = mat_Result(6, 0);
	mat_A_4x4(3, 1) = mat_Result(7, 0);
	mat_A_4x4(3, 2) = mat_Result(8, 0);
	mat_A_4x4(3, 3) = -1.0;

	MatrixXd mat_Center = -((mat_A_4x4.block(0, 0, 3, 3)).inverse() * mat_Result.block(6, 0, 3, 1));

	Matrix<double, 4, 4>  mat_T_4x4;
	mat_T_4x4.setIdentity();
	mat_T_4x4.block(3, 0, 1, 3) = mat_Center.transpose();

	MatrixXd mat_R = mat_T_4x4 * mat_A_4x4 * mat_T_4x4.transpose();

	EigenSolver<MatrixXd> eig(mat_R.block(0, 0, 3, 3) / -mat_R(3, 3));
	//mat_T_4x4(3,0)=mat_Center()
	MatrixXd mat_Eigval(3, 1) ;
	MatrixXd mat_Evecs(3, 3) ;
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			mat_Evecs(i, j) = (eig.eigenvectors())(i, j).real();
		}
	}
	mat_Eigval(0, 0) = (eig.eigenvalues())(0, 0).real();
	mat_Eigval(1, 0) = (eig.eigenvalues())(1, 0).real();
	mat_Eigval(2, 0) = (eig.eigenvalues())(2, 0).real();
	MatrixXd mat_Radii = (1.0 / mat_Eigval.array()).cwiseSqrt();
	MatrixXd mat_Scale = MatrixXd::Identity(3, 3) ;
	mat_Scale(0, 0) = mat_Radii(0, 0);
	mat_Scale(1, 1) = mat_Radii(1, 0);
	mat_Scale(2, 2) = mat_Radii(2, 0);
	double min_Radii = mat_Radii.minCoeff();

	mat_Scale = mat_Scale.inverse().array() * min_Radii;
	MatrixXd mat_Correct = mat_Evecs * mat_Scale * mat_Evecs.transpose();

	cout << "The Ellipsoid center is:" << endl << mat_Center << endl;
	cout << "The Ellipsoid radii is:" << endl << mat_Radii << endl;
	cout << "The scale matrix  is:" << endl << mat_Scale << endl;
	cout << "The correct matrix  is:" << endl << mat_Correct << endl;

}


#ifndef PROPAGATION_HPP
#define PROPAGATION_HPP

#include <opencv2/core.hpp>

#include <Eigen/Dense>

#include "../utility/activation.hpp"

/*! \file propagation.hpp
    \brief collection of propagation algorithms
*/

/*!
 *  \addtogroup ocv
 *  @{
 */
namespace ocv{

/*!
 *  \addtogroup ml
 *  @{
 */
namespace ml{

/**
 *@brief forward propagation of neural network, this function\n
 *Assume this is a three layers neural network, this function\n
 *will compute the output of the layer 2
 *@param input The input of the neurals(input of layer 2)
 *@param weight The weight of the input
 *@param bias bias of the input
 *@param output The output of the neurals(output of layer 2).\n
 * After computation, output.rows == weight.rows && output.cols == input.cols
 *@param func unary functor which apply the activation on\n
 * the output
 *@pre input, weight and bias cannot be empty, the type of the\n
 * input, weight, bias and output should be double(CV_64F).\n
 * weight.cols == input.rows && bias.rows == weight.rows
 */
template<typename UnaryFunc = sigmoid>
void forward_propagation(cv::Mat const &input,
                         cv::Mat const &weight,
                         cv::Mat const &bias,
                         cv::Mat &output,
                         UnaryFunc func = UnaryFunc())
{
    if(!input.empty() && !weight.empty() &&
            !bias.empty()){
        //output = weight * input;
        cv::gemm(weight, input, 1.0, cv::Mat(),
                 0.0, output);
        for(int i = 0; i != output.cols; ++i){
            output.col(i) += bias;
        }
        func(output);
    }
}

/**
 *@brief forward propagation of neural network, this function\n
 *Assume this is a three layers neural network, this function\n
 *will compute the output of the layer 2
 *@param input The input of the neurals(input of layer 2)
 *@param weight The weight of the input
 *@param bias bias of the input
 *@param output The output of the neurals(output of layer 2).\n
 * After computation, output.rows == weight.rows && output.cols == input.cols
 *@param func unary functor which apply the activation on\n
 * the output
 *@pre input, weight and bias cannot be empty.\n
 * weight.cols == input.rows && bias.rows == weight.rows
 */
template<typename Derived1,
         typename Derived2,
         typename Derived3,
         typename Derived4,
         typename UnaryFunc = sigmoid>
void forward_propagation(Eigen::MatrixBase<Derived1> const &input,
                         Eigen::MatrixBase<Derived2> const &weight,
                         Eigen::MatrixBase<Derived3> const &bias,
                         Eigen::MatrixBase<Derived4> &output,
                         bool no_overlap = true,
                         UnaryFunc func = UnaryFunc())
{
    static_assert(std::is_same<Derived1::Scalar, Derived2::Scalar>::value &&
                  std::is_same<Derived2::Scalar, Derived3::Scalar>::value &&
                  std::is_same<Derived4::Scalar, Derived4::Scalar>::value,
                  "Data type of matrix input, weight, bias and output should be the same");

    if(input.rows() != 0 && weight.rows() != 0 &&
            bias.rows() != 0){
        if(no_overlap){
            output.noalias() = weight * input;
        }else{
            output = weight * input;
        }

        using Scalar = typename Derived3::Scalar;
        using MatType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
        using Mapper = Eigen::Map<const MatType, Eigen::Aligned>;
        Mapper Map(&bias(0, 0), bias.size());
        output.colwise() += Map;
        func(output);
    }
}

} /*! @} End of Doxygen Groups*/

} /*! @} End of Doxygen Groups*/

#endif // PROPAGATION_HPP


#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>

using namespace std;

// vertex: 3d vector
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  // override the reset function
  virtual void setToOriginImpl() override {
    _estimate << 0, 0, 0;
  }

  // override the plus operator, just plain vector addition
  virtual void oplusImpl(const double *update) override {
    _estimate += Eigen::Vector3d(update);
  }

  // the dummy read/write function
  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}
};

// edge: 1D error term, connected to exactly one vertex
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}

  // define the error term computation
  virtual void computeError() override {
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
  }

  // the jacobian
  virtual void linearizeOplus() override {
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
    _jacobianOplusXi[0] = -_x * _x * y;
    _jacobianOplusXi[1] = -_x * y;
    _jacobianOplusXi[2] = -y;
  }

  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}

public:
  double _x;  // x data, note y is given in _measurement
};

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // ground−truth values
  double ae = 2.0, be = -1.0, ce = 5.0;        // initial estimation
  int N = 100;                                 // num of data points
  double w_sigma = 1.0;                        // sigma of the noise
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // Random number generator

  vector<double> x_data, y_data;      // the data
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  // set g2o
  // Each error term optimizer has a dimension of 3, and the error value has a dimension of 1.
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // linear solver

  // choose the optimizatoin method from GN, LM, DogLeg
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;   // graph optimizer
  optimizer.setAlgorithm(solver);   // set the algorithm
  optimizer.setVerbose(true);       // print the results

  // add vertex
  CurveFittingVertex *v = new CurveFittingVertex();
  v->setEstimate(Eigen::Vector3d(ae, be, ce));
  v->setId(0);
  optimizer.addVertex(v);

  // add edges
  for (int i = 0; i < N; i++) {
    CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
    edge->setId(i);
    edge->setVertex(0, v);                // connect to the vertex
    edge->setMeasurement(y_data[i]);      // measurement
    edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // set the information matrix
    optimizer.addEdge(edge);
  }

  // carry out the optimization
  cout << "start optimization" << endl;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  // print the results
  Eigen::Vector3d abc_estimate = v->estimate();
  cout << "estimated model: " << abc_estimate.transpose() << endl;

  return 0;
}


#ifndef POLYNOMIALS_HPP
#define POLYNOMIALS_HPP

#include <vector>
#include <iostream>
#include <cmath>

#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;

class Polynomial {
public:
    Polynomial() {}
    Polynomial(const Polynomial& orig);
    Polynomial(const vector<double> &coeffs);
    ~Polynomial() {}

    void set_coeffs(const vector<double> &coeffs);
    vector<double> get_coeffs() const;

    void polyfit(const Eigen::VectorXd &xvals,
                 const Eigen::VectorXd &yvals,
                 const int order);
    void polyfit(const vector<double> &xvals,
                 const vector<double> &yvals,
                 const int order);

    double polyeval(const double x) const;
    double polyeval_d(const double x) const;
    double polyeval_dd(const double x) const;
    double polyeval_ddd(const double x) const;
    vector<double> polyeval(const vector<double> &x) const;
    vector<double> polyeval_d(const vector<double> &x) const;
    vector<double> polyeval_dd(const vector<double> &x) const;
    vector<double> polyeval_ddd(const vector<double> &x) const;

private:
    double polyeval(const double x,
                    const vector<double> &coeffs) const;
    vector<double> polyeval(const vector<double> &x,
                            const vector<double> &coeffs) const;
    vector<double> _coeffs;
    vector<double> _coeffs_d;
    vector<double> _coeffs_dd;
    vector<double> _coeffs_ddd;
};

#endif // POLYNOMIALS_HPP


/*
 * oper_matrix.cpp
 *
 *  Created on: 2017. 6. 6.
 *      Author: cho
 */

#include <random>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/io.hpp>
//#include "io.hpp"

using namespace std;
namespace ublas = boost::numeric::ublas;

/// number of data
const unsigned int N = 5;

int main() {
	cout << "==== START " << endl;
	random_device rd;     // seed, rd()
	mt19937_64 gen(4);    // random number generator

	ublas::vector<double> t (N); // time vector, sec
	for (unsigned i = 0; i < t.size(); ++i) t(i) = i+1;
	cout << "t("<<N<<") = " << t << endl;

	uniform_int_distribution<int> dis_y( 0, 10 );
	ublas::vector<double> y (N); // measured value
	for (unsigned i = 0; i < y.size(); ++i) y(i) = dis_y(gen);
	cout << "y("<<N<<") = " << y << endl;

	//const double m_init[N][N] = { {1, 2}, {3, 4} };
	ublas::matrix<double> m( N, N );
	for (unsigned i = 0; i < m.size1 (); ++ i)
		for (unsigned j = 0; j < m.size2 (); ++ j)
			m (i, j) = i*2 + j + 1;
			//m(i,j) = m_init[i][j];
	m(N-1,N-1) = 3;
	cout << "matrix = " << m << endl;

	cout << "\n==== standard operation : +, -, *(scalar), /(scalar) " << endl;
	{
		cout << "t+y = " << t+y << endl;
		cout << "t-y = " << t-y << endl;
		cout << "2*t = " << 2*t << endl;
		cout << "t*3 = " << t*3 << endl;
		cout << "t/2 = " << t/3 << endl;
		cout << "-t  = " << -t << endl;
	}

	cout << "\n==== inner, outer and other products " << endl;
	{
		cout << "inner_prod(t, y) = " << inner_prod( t, y ) << endl;
		cout << "inner_prod(y, t) = " << inner_prod( y, t ) << endl;
		cout << "outer_prod(t, y) = " << outer_prod( t, y ) << endl;
		cout << "outer_prod(y, t) = " << outer_prod( y, t ) << endl;
		cout << "element_prod(t, y) = " << element_prod(t, y) << endl;
		cout << "element_prod(y, t) = " << element_prod(y, t) << endl;
		cout << "element_div(t, y)  = " << element_div(t, y) << endl;
		cout << "element_div(y, t)  = " << element_div(y, t) << endl;
		cout << "prod(m, t)  = " << prod(m, t) << endl;
		cout << "prod(t, m)  = " << prod(t, m) << endl;
		cout << "prec_prod(t, m)  = " << prec_prod(t, m) << endl;
		cout << "prec_prod(m, t)  = " << prec_prod(m, t) << endl;
	}

	cout << "\n==== transformations " << endl;
	{
		cout << "trans(m) = " << ublas::trans(m) << endl;
	}

	cout << "\n==== triangular matrix " << endl;
	{
		auto L  = ublas::triangular_adaptor<ublas::matrix<double>, ublas::lower> (m);
		auto SL = ublas::triangular_adaptor<ublas::matrix<double>, ublas::strict_lower> (m);
		auto UL = ublas::triangular_adaptor<ublas::matrix<double>, ublas::unit_lower> (m);
		cout << "lower        L  = " << L << endl;
		cout << "strict lower SL = " << SL << endl;
		cout << "unit lower   UL = " << UL << endl;

		auto U =  ublas::triangular_adaptor<ublas::matrix<double>, ublas::upper> (m);
		auto SU = ublas::triangular_adaptor<ublas::matrix<double>, ublas::strict_upper> (m);
		auto UU = ublas::triangular_adaptor<ublas::matrix<double>, ublas::unit_upper> (m);
		cout << "upper        U  = " << U << endl;
		cout << "strict upper SU = " << SU << endl;
		cout << "unit upper   UU = " << UU << endl;

		cout << "prod( U, m ) = " << prod(U, m) << endl;

		ublas::vector<double> x(m.size1());
		ublas::vector<double> b(m.size1(), 1);
		cout << "inplace_solve(UL, b, ) : b = " << b << endl;
		inplace_solve( UL, b, ublas::unit_lower_tag() );
		cout << "inplace_solve(UL, b, ) : b = " << b << endl;
		cout << "prod( UL, b ) = " << prod( UL, b ) << endl;
	}

	cout << "\n==== pumutation matrix " << endl;
	{
		ublas::permutation_matrix<size_t> pm(N);
		pm(0) = 1;
		cout << "-- pm = " << pm << endl;
		ublas::swap_rows( pm, m);
		cout << "-- swap_rows( pm, m) = " << m << endl;
		pm(0) = 0; pm(1) = 0;
		cout << "-- pm = " << pm << endl;
		ublas::swap_rows( pm, m);
		cout << "-- swap_rows( pm, m) = " << m << endl;
		pm(0) = 0; pm(1) = 1; pm(2) = 0;
		cout << "-- pm = " << pm << endl;
		ublas::swap_rows( pm, m);
		cout << "-- swap_rows( pm, m) = " << m << endl;
	}

	cout << "\n==== LU factorization" << endl;
	{
		ublas::matrix<double> A(m);
		ublas::permutation_matrix<double> pm(N);
		cout << "-- pm = " << pm << endl;
		bool singular = ublas::lu_factorize( A, pm);
		cout << "-- return = " << singular << endl;
		cout << "-- A  = " << A << endl;
		cout << "-- pm = " << pm << endl;

		auto L = ublas::triangular_adaptor<ublas::matrix<double>, ublas::unit_lower>(A);
		cout << "-- L = " << L << endl;
		auto U = ublas::triangular_adaptor<ublas::matrix<double>, ublas::upper>(A);
		cout << "-- U = " << U << endl;
		cout << "-- LU = " << prod(L,U) << endl;
		ublas::matrix<double> pmA( m ); // pmA means pm*A, at first init as m
		swap_rows(pm, pmA);
		cout << "-- swap_rows(pm,A) = " << pmA << endl;

		cout << "-- solve Lz = b -------------" << endl;
		ublas::vector<double> b( N ); b(0) = 1; b(1) = 2; b(2) = 3;
		ublas::vector<double> z( b );
		cout << "-- L " << L << endl;
		cout << "-- b " << b << endl;
		ublas::inplace_solve( A, z, ublas::unit_lower_tag() );
		cout << "-- z = " << z << endl;
		cout << "-- confirm Lz = " << prod( L, z ) << endl;

		cout << "-- solve Ux = z -------------" << endl;
		ublas::vector<double> x( z );
		cout << "-- U " << U << endl;
		cout << "-- z " << z << endl;
		inplace_solve( A, x, ublas::upper_tag() );
		cout << "-- x = " << x << endl;
		cout << "-- confirm Ux = " << prod( U, x ) << endl;
	}

	cout << "==== END " << endl;
}


#ifndef MATHTOOLBOX_BACKTRACKING_LINE_SEARCH_HPP
#define MATHTOOLBOX_BACKTRACKING_LINE_SEARCH_HPP

#include <Eigen/Core>
#include <functional>
#include <iostream>

namespace mathtoolbox
{
    namespace optimization
    {
        /// \brief Run the backtracking line search.
        ///
        /// \details The algorithm is described in the book (Algorithm 3.1: Backtracking Line Search).
        ///
        /// This algorithm tries to find an appropriate step size that satisfies the Armijo condition (i.e., the
        /// safficient decreasing condition). This algorithm runs faster than the line search algorithm for the strong
        /// Wolfe conditions, but it does not guarantee the curvature condition, which is required to stabilize the
        /// overall optimization.
        inline double RunBacktrackingLineSearch(const std::function<double(const Eigen::VectorXd&)>& f,
                                                const Eigen::VectorXd&                               grad,
                                                const Eigen::VectorXd&                               x,
                                                const Eigen::VectorXd&                               p,
                                                const double                                         alpha_init,
                                                const double                                         rho,
                                                const double                                         c = 1e-04)
        {
            constexpr unsigned int max_num_iters = 50;

            assert(p.size() == x.size());

            double alpha = alpha_init;

            for (int iter = 0; iter < max_num_iters; ++iter)
            {
                // Equation 3.4
                const bool armijo_condition = f(x + alpha * p) <= f(x) + c * alpha * grad.transpose() * p;

                if (armijo_condition)
                {
                    return alpha;
                }

                alpha *= rho;
            }

            std::cerr << "Warning: The line search did not converge." << std::endl;
            return alpha;
        }

        /// \brief Run the backtracking line search with bound constraints
        ///
        /// \details This function should be slightly slower than the unbounded one since it evaluates bound conditions
        /// every step.
        ///
        /// \param lower_bound Lower bound of the search space. It can also be a zero-sized vector when there is no
        /// lower bound condition.
        ///
        /// \param upper_bound Upper bound of the search space. It can also be a zero-sized vector when there is no
        /// upper bound condition.
        inline double RunBacktrackingBoundedLineSearch(const std::function<double(const Eigen::VectorXd&)>& f,
                                                       const Eigen::VectorXd&                               grad,
                                                       const Eigen::VectorXd&                               x,
                                                       const Eigen::VectorXd&                               p,
                                                       const Eigen::VectorXd&                               lower_bound,
                                                       const Eigen::VectorXd&                               upper_bound,
                                                       const double                                         alpha_init,
                                                       const double                                         rho,
                                                       const double                                         c = 1e-04)
        {
            constexpr unsigned int max_num_iters = 50;

            assert(p.size() == x.size());

            double alpha = alpha_init;

            for (int iter = 0; iter < max_num_iters; ++iter)
            {
                const Eigen::VectorXd x_new = x + alpha * p;

                // Equation 3.4
                const bool armijo_condition = f(x_new) <= f(x) + c * alpha * grad.transpose() * p;

                // Bound conditions
                const bool lower_bound_condition = lower_bound.size() == 0 || (x_new - lower_bound).minCoeff() >= 0.0;
                const bool upper_bound_condition = upper_bound.size() == 0 || (upper_bound - x_new).minCoeff() >= 0.0;

                if (armijo_condition && lower_bound_condition && upper_bound_condition)
                {
                    return alpha;
                }

                alpha *= rho;
            }

            std::cerr << "Warning: The line search did not converge." << std::endl;
            return alpha;
        }
    } // namespace optimization
} // namespace mathtoolbox

#endif // MATHTOOLBOX_BACKTRACKING_LINE_SEARCH_HPP


#include <yaml-cpp/yaml.h>

#include <Eigen/Dense>
#include <Eigen/OrderingMethods>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <chrono>
#include <iostream>
#include <vector>

#include "NumpySaver.hpp"

using namespace Eigen;
using namespace std;

typedef double type;

const char* divider = "#####################################################";

// copied from https://gist.github.com/lorenzoriano/5414671
template <typename T>
std::vector<T> linspace(T start, T end, size_t N) {
  T h = (end - start) / static_cast<T>(N - 1);
  std::vector<T> xs(N);
  typename std::vector<T>::iterator x;
  T val;
  for (x = xs.begin(), val = start; x != xs.end(); ++x, val += h) *x = val;
  return xs;
}

SparseMatrix<type> get_5_point_schroedinger_matrix(
    double xmin, double xmax, unsigned int N,
    function<double(double)> potential) {
  // cout << "Hello from the Beginning of building the matrix" << endl;
  SparseMatrix<type> mat(N, N);
  mat.reserve(5 * N - 6);

  double h = (xmax - xmin) / N;
  double prefac = 1. / 12. / h / h;

  double x = xmin;
  for (unsigned int i = 0; i < N; i++, x += h) {
    if (i >= 2) mat.insert(i, i - 2) = prefac;
    if (i >= 1) mat.insert(i, i - 1) = -prefac * 16;
    mat.insert(i, i) = prefac * 30 + potential(x);
    if (i < N - 1) mat.insert(i, i + 1) = -prefac * 16;
    if (i < N - 2) mat.insert(i, i + 2) = prefac;
  }
  // cout << "Hello from the End of building the matrix" << endl;

  return mat;
}

double harmonic_oszi_potential(double x) { return x * x; }
double disturbed_harmonic_oszi_potential(double x, double c1, double c2) {
  return x * x + c1 * exp(-x * x * c2);
}

function<double(double)> get_potential(YAML::Node potential_conf) {
  auto pot_name = potential_conf["name"].as<string>();

  if (pot_name == "harmonic") {
    return harmonic_oszi_potential;
  } else if (pot_name == "disturbed") {
    double c1 = potential_conf["c1"].as<double>();
    double c2 = potential_conf["c2"].as<double>();
    return bind(disturbed_harmonic_oszi_potential, std::placeholders::_1, c1,
                c2);
  } else {
    throw runtime_error(
        "Potential needs to be either 'harmonic' or 'disturbed'!");
  }
}

template <typename T>
vector<T> get_values(YAML::Node& node) {
  auto type = node.Type();
  // single value
  if (type == YAML::NodeType::Scalar) {
    vector<T> ret = {node.as<T>()};
    return ret;
  } else if (type == YAML::NodeType::Sequence) {
    return node.as<vector<T>>();
  } else if (type == YAML::NodeType::Map) {
    auto start = node["start"].as<T>();
    auto end = node["end"].as<T>();
    auto N = node["N"].as<size_t>();
    return linspace<T>(start, end, N);
  } else {
    throw std::runtime_error(
        "Range node must either be list, scalar or a map defining "
        "'start','end' and 'N'.");
  }
}

tuple<VectorXd, type, unsigned int> eigenvector_inverse_power_iteration(
    SparseMatrix<type>& matrix, VectorXd& start_vector, double shift = 0,
    double tol = 1e-4, unsigned int max_iterations = 10000) {
  // apply shift
  // cout << "Matrix=\n" << matrix;
  SparseMatrix<type> mat(matrix.rows(), matrix.cols());
  mat.setIdentity();
  mat = matrix - shift * mat;
  // cout << "Shifted Matrix\n" << mat;

  // first do the (sparse) LU decomposition
  Eigen::SparseLU<Eigen::SparseMatrix<type>, Eigen::COLAMDOrdering<int>> solver;
  solver.analyzePattern(mat);
  solver.factorize(mat);

  // now the iteration
  VectorXd y_now(start_vector);
  VectorXd y_last;

  // not the best init value, but should work ok
  type lambda_now = numeric_limits<type>::max();
  type lambda_last;

  // keep track of iterations
  unsigned int i = 0;

  do {
    // normalize vector
    y_now.normalize();
    y_last = y_now;

    // iteration
    y_now = solver.solve(y_last);

    // calculate eigenvector
    lambda_last = lambda_now;
    lambda_now = 1 / (y_last.dot(y_now)) + shift;

  } while ((abs(lambda_last - lambda_now) > tol) && i++ < max_iterations);

  return {y_now, lambda_now, i};
}

void test_inverse_power_iteration(YAML::Node& test_configs) {
  auto size = test_configs.size();
  for (std::size_t i = 0; i < size; i++) {
    auto conf = test_configs[i];

    auto N = conf["dim"].as<int>();
    auto max_iteartions = conf["max_iter"].as<unsigned int>();
    auto tol = conf["tol"].as<double>();

    MatrixXd matrix = MatrixXd::Random(N, N);
    matrix = matrix.selfadjointView<Eigen::Lower>();
    VectorXd start_vector = VectorXd::Random(N);
    SparseMatrix<type> sparse_view = matrix.sparseView();

    cout << divider << "# Test Power Iteration " << i + 1 << "/" << size << endl
         << divider;
    cout << "Matrix=\n" << matrix;
    cout << "\nStart vector=\n" << start_vector;
    cout << "\nTolerance=" << tol << endl << endl;

    YAML::Node shift_node = conf["shift"];
    auto shifts = get_values<double>(shift_node);

    // Eigen solution for comparison:
    EigenSolver<MatrixXd> es(matrix);
    cout << "Eigen-Eigenvalues:" << endl << es.eigenvalues() << endl;
    cout << "Eigen-Eigenvectors:" << endl << es.eigenvectors() << endl << endl;

    for (auto& shift : shifts) {
      cout << divider << "Shift=" << shift << endl;

      auto [eigenvector, eigenvalue, iterations] =
          eigenvector_inverse_power_iteration(sparse_view, start_vector, shift,
                                              tol, max_iteartions);
      cout << "Iterations needed to reach tolerance: " << iterations << endl;
      cout << "Inverse power Iteration eigenvector is:\n" << eigenvector;
      cout << "\nInverse power Iteration eigenvalue is:\n" << eigenvalue;
      if (iterations >= max_iteartions)
        cerr
            << "\nWarning! Exceeded maximum number of iterations! Solution did "
               "not converge!"
            << endl;
      cout << endl;
    }
    cout << endl;
  }
}

void test_harmonic(YAML::Node& test_configs) {
  auto size = test_configs.size();
  for (std::size_t i = 0; i < size; i++) {
    cout << divider << "\n# Test Harmonic Oscillator " << i + 1 << "/" << size
         << endl
         << divider << endl
         << endl;
    auto conf = test_configs[i];

    auto N = conf["N"].as<size_t>();
    auto max_iteartions = conf["max_iter"].as<unsigned int>();
    auto tol = conf["tol"].as<double>();
    auto xmin = conf["xmin"].as<double>();
    auto xmax = conf["xmax"].as<double>();

    function<double(double)> pot = get_potential(conf["potential"]);
    auto matrix = get_5_point_schroedinger_matrix(xmin, xmax, N, pot);

    bool has_name = false;
    string file_name;
    if (conf["name"]) {
      has_name = true;
      file_name = "build/output/" + conf["name"].as<string>() + ".npy";
      cout << "Name to save to: " << file_name << endl;
    }

    NumpySaver saver(file_name);

    if (has_name) {
      VectorXd x_space(N);
      VectorXd pot_space(N);
      double h = (xmax - xmin) / N;

      double x = xmin;
      for (size_t i = 0; i < N; i++, x += h) {
        x_space[i] = x;
        pot_space[i] = pot(x);
      }

      saver << "x" << x_space << "pot" << pot_space;
    }

    VectorXd start_vector = VectorXd::Ones(N);

    YAML::Node shift_node = conf["shift"];
    auto shifts = get_values<double>(shift_node);

    cout << "Tolerance=" << tol << endl;

    // Eigen solution for comparison:
    SelfAdjointEigenSolver<SparseMatrix<type>> es(matrix);
    if (N < 10) {
      cout << "Eigen-Eigenvalues:" << endl << es.eigenvalues() << endl;
      cout << "Eigen-Eigenvectors:" << endl
           << es.eigenvectors() << endl
           << endl;
      cout << "Matrix=\n" << matrix << endl;
      cout << "Start vector=\n" << start_vector << endl;
    }

    for (auto& shift : shifts) {
      cout << divider << "Shift=" << shift << endl;

      auto [eigenvector, eigenvalue, iterations] =
          eigenvector_inverse_power_iteration(matrix, start_vector, shift, tol,
                                              max_iteartions);

      if (has_name) saver << "ev" << eigenvector;

      cout << "Iterations=" << iterations << endl;

      if (N < 10)
        cout << "Inverse power Iteration eigenvector = \n"
             << eigenvector << endl;

      cout << "Inverse power Iteration eigenvalue = " << eigenvalue << endl;

      if (iterations >= max_iteartions)
        cerr << "Warning! Exceeded maximum number of iterations! Solution did "
                "not converge!"
             << endl;

      cout << endl;
    }
    cout << endl;
  }
}

void run_tests() {
  cout << divider << divider << "## RUNNING TESTS\n"
       << divider << divider << endl
       << endl;

  YAML::Node config = YAML::LoadFile("test.yaml");

  auto config_power_iter = config["inverse_iteration"];
  test_inverse_power_iteration(config_power_iter);

  auto config_harmonic = config["harmonic"];
  test_harmonic(config_harmonic);
}

void run_program() {
  YAML::Node configs = YAML::LoadFile("config.yaml");
  auto size = configs.size();

  for (std::size_t i = 0; i < size; i++) {
    cout << divider << divider << "\n# Running Simulation  " << i + 1 << "/"
         << size << endl
         << divider << divider << endl;
    auto conf = configs[i];

    auto N = conf["N"].as<size_t>();
    auto max_iteartions = conf["max_iter"].as<unsigned int>();
    auto tol = conf["tol"].as<double>();
    auto xmin = conf["xmin"].as<double>();
    auto xmax = conf["xmax"].as<double>();

    function<double(double)> pot = get_potential(conf["potential"]);
    auto matrix = get_5_point_schroedinger_matrix(xmin, xmax, N, pot);
    VectorXd start_vector = VectorXd::Ones(N);

    bool has_name = false;
    string file_name;
    if (conf["name"]) {
      has_name = true;
      file_name = "build/output/" + conf["name"].as<string>() + ".npy";
      cout << "Save to:\t" << file_name << endl;
    }

    NumpySaver saver(file_name);

    if (has_name) {
      VectorXd x_space(N);
      VectorXd pot_space(N);
      double h = (xmax - xmin) / N;

      double x = xmin;
      for (size_t i = 0; i < N; i++, x += h) {
        x_space[i] = x;
        pot_space[i] = pot(x);
      }

      saver << "x" << x_space << "pot" << pot_space;
    }

    YAML::Node shift_node = conf["shift"];
    auto shifts = get_values<double>(shift_node);

    cout << "N=\t\t" << N << endl;
    cout << "Tolerance=\t" << tol << endl;
    cout << "xmin=\t\t" << xmin << endl;
    cout << "xmax=\t\t" << xmax << endl;
    cout << "function=\t" << conf["potential"]["name"] << endl << endl;

    // to time the methods
    chrono::steady_clock::time_point start;
    chrono::duration<double> elapsed_seconds;

    // Eigen solution for comparison:
    if (N <= 1000) {
      start = chrono::steady_clock::now();

      SelfAdjointEigenSolver<SparseMatrix<type>> es(matrix);
      auto ev_ref = es.eigenvalues();

      elapsed_seconds = chrono::steady_clock::now() - start;
      cout << "Eigen::SelfAdjointEigenSolver<SparseMatrix> took "
           << elapsed_seconds.count() * 1e3 << "ms" << endl;

      cout << "eigenvalues=" << ev_ref(seq(0, shifts.size())).transpose()
           << endl
           << endl;

      auto evec_ref = es.eigenvectors();

      if (has_name)
        NumpySaver("build/output/" + conf["name"].as<string>() +
                   "_reference.npy")
            << evec_ref;

      if (N < 10) {
        cout << "Eigen-Eigenvalues:" << endl << ev_ref << endl;
        cout << "Eigen-Eigenvectors:" << endl << evec_ref << endl << endl;
        cout << "Matrix=\n" << matrix << endl;
        cout << "Start vector=\n" << start_vector << endl;
      }
    } else {
      cout << "Not going to test SelfAdjointEigenSolver, since it would take "
              "too long."
           << endl;
    }

    start = std::chrono::steady_clock::now();
    for (auto& shift : shifts) {
      cout << divider << "\nShift=\t\t" << shift << endl;

      auto [eigenvector, eigenvalue, iterations] =
          eigenvector_inverse_power_iteration(matrix, start_vector, shift, tol,
                                              max_iteartions);

      elapsed_seconds = chrono::steady_clock::now() - start;

      if (has_name) saver << "ev" << eigenvector;

      cout << "elapsed time=\t" << elapsed_seconds.count() * 1e3 << "ms"
           << endl;

      cout << "Iterations=\t" << iterations << endl;
      if (N < 10)
        cout << "Inverse power Iteration eigenvector = \n"
             << eigenvector << endl;

      cout << "eigenvalue=\t" << eigenvalue << endl;

      if (iterations >= max_iteartions)
        cerr << "Warning! Exceeded maximum number of iterations! Solution did "
                "not converge!"
             << endl;
    }
    cout << endl;
  }
}

int main(int argc, char const* argv[]) {
  // uncomment this to run the test
  // run_tests();
  run_program();
  return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

//[transform_sqrt_example

#include <vector>
#include <algorithm>

#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/functional/math.hpp>

namespace compute = boost::compute;

int main()
{
    // get default device and setup context
    compute::device device = compute::system::default_device();
    compute::context context(device);
    compute::command_queue queue(context, device);

    // generate random data on the host
    std::vector<float> host_vector(10000);
    std::generate(host_vector.begin(), host_vector.end(), rand);

    // create a vector on the device
    compute::vector<float> device_vector(host_vector.size(), context);

    // transfer data from the host to the device
    compute::copy(
        host_vector.begin(), host_vector.end(), device_vector.begin(), queue
    );

    // calculate the square-root of each element in-place
    compute::transform(
        device_vector.begin(),
        device_vector.end(),
        device_vector.begin(),
        compute::sqrt<float>(),
        queue
    );

    // copy values back to the host
    compute::copy(
        device_vector.begin(), device_vector.end(), host_vector.begin(), queue
    );

    return 0;
}

//]


// TODO : Dimention Agnosticism

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

namespace bg = boost::geometry;
namespace ublas = boost::numeric::ublas;

// Make vector from 2 points
template<typename Point>
ublas::vector<double> make_vector(const Point& a, const Point& b)
{
	ublas::vector<double> v(2);
	v[0] = bg::get<0>(b) - bg::get<0>(a);
	v[1] = bg::get<1>(b) - bg::get<1>(a);
	return v;
}

// Calculate cos from 2 vectors
double cos_from_vecs(const ublas::vector<double>& v1,
					 const ublas::vector<double>& v2) {
	return ublas::inner_prod(v1, v2)/(ublas::norm_2(v1)*ublas::norm_2(v2));
}

// Calculate cos from 3 points
template<typename Point>
double cos_from_dots(const Point& a, const Point& b, const Point& c) {
	return cos_from_vecs(make_vector(b,a), make_vector(b,c));
}

template<typename MultiPoint>
void convex_hull(const MultiPoint& in, MultiPoint& out) {

	typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
	point_t min_coordinate;

	bool flag = false;

	// TODO : Remove points which cannot be a part of a convex hull
	for (const auto &point : in) {
		if (!flag) {
			bg::set<0>(min_coordinate, bg::get<0>(point));
			bg::set<1>(min_coordinate, bg::get<1>(point));
			flag = true;
			continue;
		}
		if (bg::get<0>(point) < bg::get<0>(min_coordinate) ||
				(bg::get<0>(point) == bg::get<0>(min_coordinate) &&
				 bg::get<1>(point) < bg::get<1>(min_coordinate))) {

			bg::set<0>(min_coordinate, bg::get<0>(point));
			bg::set<1>(min_coordinate, bg::get<1>(point));
		}
	}

	point_t B(bg::get<0>(min_coordinate), bg::get<1>(min_coordinate)-1.0);	// Before
	point_t S(bg::get<0>(min_coordinate), bg::get<1>(min_coordinate));	// Start
	point_t T;	// To

	bg::append(out, S);

	while(1) {
		double min_cos_value = 1.0;
		point_t p;
		for (const auto &point : in) {
			bg::set<0>(T, bg::get<0>(point));
			bg::set<1>(T, bg::get<1>(point));

			if (bg::get<0>(S)==bg::get<0>(T) && bg::get<1>(S)==bg::get<1>(T)) {
				continue;
			}

			double cos_value = cos_from_dots(B,S,T);

			if (cos_value < min_cos_value) {
				min_cos_value = cos_value;
				bg::set<0>(p, bg::get<0>(T));
				bg::set<1>(p, bg::get<1>(T));
			}
		}
		bg::append(out, p);
		if (bg::get<0>(p)==bg::get<0>(min_coordinate) &&
			bg::get<1>(p)==bg::get<1>(min_coordinate)) {
			// Break because Pi+1 == P0
			break;
		}

		// B = S
		bg::set<0>(B, bg::get<0>(S));
		bg::set<1>(B, bg::get<1>(S));

		// S = p
		bg::set<0>(S, bg::get<0>(p));
		bg::set<1>(S, bg::get<1>(p));
	}
}


#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>

#include "boundary.hpp"
#include "mass_matrix_assembly.hpp"
#include "neumann_boundary_assemble.hpp"
#include "stiffness_matrix_assembly.hpp"

//! Evolves the solution in time using the explicit Forward Euler scheme
//!
//! @param vertices the list of vertices that build the triangular mesh
//! @param triangles the list of indices that represent the triangular mesh
//! @param u0 the initial data
//! @param gamma the parameter gamma (has to do with the boundary conditions)
//! @param m number of timesteps to perform
//! @returns a pair which contains as first parameter the solution at time = 1,
//!          and second parameter the energy evolving over time.
std::pair<Eigen::VectorXd, std::vector<double>> radiativeTimeEvolutionExplicit(const Eigen::MatrixXd &vertices,
                                                                               const Eigen::MatrixXi &triangles,
                                                                               const Eigen::VectorXd &u0,
                                                                               const double           gamma,
                                                                               const int              m) {
	std::vector<double> energy;
	Eigen::VectorXd     u(u0.size());
	u = u0;

	double dt = 1.0 / m;

	// Get the edges, this is used to compute the neumann boundary matrix
	Eigen::MatrixXi edges = getBoundaryEdges(vertices, triangles);

	// Define the needed matrices and solvers
	SparseMatrix M = assembleMassMatrix(vertices, triangles);
	SparseMatrix A = assembleStiffnessMatrix(vertices, triangles);
	SparseMatrix B = assembleBoundaryMatrix(vertices, edges, gamma);
	A += B;

	Eigen::SimplicialLDLT<SparseMatrix> SolverM;
	SolverM.compute(M);

	if (SolverM.info() != Eigen::Success) {
		throw std::runtime_error("Could not decompose matrix M");
	}

	// (write your solution here)
	for (int timestep = 0; timestep < m; ++timestep) {
		// Step the solution forward in time
		// (write your solution here)
		u += SolverM.solve(-1.0 * dt * A * u).eval();

		energy.push_back(u.sum() / u.rows());
	}

	return std::make_pair(u, energy);
}


/*
 * @Description: utils for ceres residual block analytic Jacobians
 * @Author: Ge Yao
 * @Date: 2020-11-29 15:47:49
 */
#ifndef LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_UTILS_HPP_
#define LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_UTILS_HPP_

#include <Eigen/Eigen>
#include <Eigen/Core>
#include <Eigen/Dense>

#include <sophus/so3.hpp>

namespace sliding_window {

Eigen::Matrix3d JacobianRInv(const Eigen::Vector3d &w) {
    Eigen::Matrix3d J_r_inv = Eigen::Matrix3d::Identity();

    double theta = w.norm();

    if ( theta > 1e-5 ) {
        Eigen::Vector3d k = w.normalized();
        Eigen::Matrix3d K = Sophus::SO3d::hat(k);

        J_r_inv = J_r_inv
                  + 0.5 * K
                  + (1.0 - (1.0 + std::cos(theta)) * theta / (2.0 * std::sin(theta))) * K * K;
    }

    return J_r_inv;
}

} // namespace sliding_window

#endif // LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_UTILS_HPP_