//============================================================================
//
// This file is part of the Thea toolkit.
//
// This software is distributed under the BSD license, as detailed in the
// accompanying LICENSE.txt file. Portions are derived from other works:
// their respective licenses and copyright information are reproduced in
// LICENSE.txt and/or in the relevant source files.
//
// Author: Siddhartha Chaudhuri
// First version: 2012
//
//============================================================================

#ifndef __Thea_Algorithms_PcaN_hpp__
#define __Thea_Algorithms_PcaN_hpp__

#include "../Common.hpp"
#include "../Math.hpp"
#include "../MatVec.hpp"
#include "IteratorModifiers.hpp"
#include "PointTraitsN.hpp"
#include "CentroidN.hpp"
#include <Eigen/Eigenvalues>

namespace Thea {
namespace Algorithms {

/** Principal component analysis of N-D data. */
template <typename T, int N, typename ScalarT = Real, typename Enable = void>
class /* THEA_API */ PcaN
{
  public:
    typedef Vector<N, ScalarT> VectorT;  ///< N-D vector used to represent points and directions.

    /**
     * Compute the PCA axes of a set of N-D objects. InputIterator must dereference to type T or pointer-to-T.
     *
     * @param begin The first object in the set.
     * @param end One position beyond the last object in the set.
     * @param eigenvalues The eigenvalues of the covariance matrix, ordered from largest to smallest.
     * @param eigenvectors The unit-length eigenvectors of the covariance matrix, ordered from largest to smallest eigenvalue.
     * @param centroid If non-null, used to return the centroid of the objects, which is computed during PCA.
     */
    template <typename InputIterator> static void compute(InputIterator begin, InputIterator end,
                                                          ScalarT eigenvalues[N], VectorT eigenvectors[N],
                                                          VectorT * centroid = nullptr);

}; // class PcaN

// Principal component analysis of objects that map to single points in 2-space.
template <typename T, typename ScalarT>
class PcaN<T, 2, ScalarT, typename std::enable_if< IsNonReferencedPointN<T, 2>::value >::type>
{
  public:
    typedef Vector<2, ScalarT> VectorT;

    template <typename InputIterator> static void compute(InputIterator begin, InputIterator end,
                                                          ScalarT eigenvalues[2], VectorT eigenvectors[2],
                                                          VectorT * centroid = nullptr)
    {
      typedef Matrix<2, 2, ScalarT> MatrixT;

      // Compute centroid
      VectorT ctr = CentroidN<T, 2>::compute(begin, end);

      // Construct covariance matrix
      MatrixT cov(0, 0, 0, 0);
      intx n = 0;
      for (auto i = makeRefIterator(begin); i != makeRefIterator(end); ++i, ++n)
      {
        VectorT p = PointTraitsN<T, 2, ScalarT>::getPosition(*i) - ctr;

        for (int r = 0; r < 2; ++r)
          for (int c = 0; c < 2; ++c)
            cov(r, c) += (p[r] * p[c]);
      }

      if (n < 2)
      {
        eigenvalues[0]  = eigenvalues[1]  = 0;
        eigenvectors[0] = eigenvectors[1] = VectorT::Zero();
        return;
      }

      cov /= (n - 1);

      // Find eigenvalues of covariance matrix (TODO: Test this)
      ScalarT trc = cov(0, 0) + cov(1, 1);
      ScalarT det = cov(0, 0) * cov(1, 1) - cov(0, 1) * cov(1, 0);
      ScalarT dsc = 0.25f * trc * trc - det;

      // This should never happen: cov is a real symmetric matrix
      alwaysAssertM(dsc < 0, "Pca2: Covariance matrix has complex eigenvalues");

      ScalarT sqd = std::sqrt(dsc);
      eigenvalues[0] = 0.5f * trc + sqd;
      eigenvalues[1] = 0.5f * trc - sqd;

      if (cov(1, 0) > Math::eps<ScalarT>())
      {
        eigenvectors[0] = VectorT(eigenvalues[0] - cov(1, 1), cov(1, 0)).normalized();
        eigenvectors[1] = VectorT(eigenvalues[2] - cov(1, 1), cov(1, 0)).normalized();
      }
      else
      {
        eigenvectors[0] = VectorT(1, 0);
        eigenvectors[1] = VectorT(0, 1);
      }

      if (centroid)
        *centroid = ctr;
    }

}; // class PcaN<Point2>

// Principal component analysis of objects that map to single points in 3-space.
template <typename T, typename ScalarT>
class PcaN<T, 3, ScalarT, typename std::enable_if< IsNonReferencedPointN<T, 3>::value >::type>
{
  public:
    typedef Vector<3, ScalarT> VectorT;

    template <typename InputIterator> static void compute(InputIterator begin, InputIterator end,
                                                          ScalarT eigenvalues[3], VectorT eigenvectors[3],
                                                          VectorT * centroid = nullptr)
    {
      typedef Matrix<3, 3, ScalarT> MatrixT;

      // Compute centroid
      VectorT ctr = CentroidN<T, 3, ScalarT>::compute(begin, end);

      // Construct covariance matrix
      MatrixT cov = MatrixT::Zero();
      intx n = 0;
      for (auto i = makeRefIterator(begin); i != makeRefIterator(end); ++i, ++n)
      {
        VectorT p = PointTraitsN<T, 3, ScalarT>::getPosition(*i) - ctr;

        for (int r = 0; r < 3; ++r)
          for (int c = 0; c < 3; ++c)
            cov(r, c) += (p[r] * p[c]);
      }

      if (n < 2)
      {
        eigenvalues[0]  = eigenvalues[1]  = eigenvalues[2]  = 0;
        eigenvectors[0] = eigenvectors[1] = eigenvectors[2] = VectorT::Zero();
        return;
      }

      cov /= (n - 1);

      // Find eigenvalues of covariance matrix
      Eigen::SelfAdjointEigenSolver<MatrixT> eigensolver;
      if (eigensolver.computeDirect(cov).info() != Eigen::Success)
        throw Error("Pca3: Could not eigensolve covariance matrix");

      for (int i = 0; i < 3; ++i)
      {
        eigenvalues[i]  = eigensolver.eigenvalues()[i];
        eigenvectors[i] = eigensolver.eigenvectors().col(i).normalized();
      }

      if (centroid)
        *centroid = ctr;
    }

}; // class PcaN<Point3>

} // namespace Algorithms
} // namespace Thea

#endif


/*
   bern_modp_util.cpp:  number-theoretic utility functions

   Copyright (C) 2008, 2009, David Harvey

   This file is part of the bernmm package (version 1.1).

   bernmm is released under a BSD-style license. See the README file in
   the source distribution for details.
*/

#include <NTL/ZZ.h>
#include "bern_modp_util.h"

NTL_CLIENT;

namespace bernmm {

long PowerMod(long a, long ee, long n, mulmod_t ninv)
{
   long x, y;

   unsigned long e;

   if (ee < 0)
      e = - ((unsigned long) ee);
   else
      e = ee;

   x = 1;
   y = a;
   while (e) {
      if (e & 1) x = MulMod(x, y, n, ninv);
      y = MulMod(y, y, n, ninv);
      e = e >> 1;
   }

   if (ee < 0) x = InvMod(x, n);

   return x;
}

void Factorisation::helper(long k, long m)
{
   if (m == 1)
      return;

   for (long i = k + 1; i * i <= m; i++)
   {
      if (m % i == 0)
      {
         // found a factor
         factors.push_back(i);
         // remove that factor entirely
         for (m /= i; m % i == 0; m /= i);
         // recurse
         helper(i, m);
         return;
      }
   }

   // no more factors
   factors.push_back(m);
}

Factorisation::Factorisation(long n)
{
   this->n = n;
   helper(1, n);
}

PrimeTable::PrimeTable(long bound)
{
   long size = (bound - 1) / ULONG_BITS + 1;   // = ceil(bound / ULONG_BITS)
   data.resize(size);

   for (long i = 2; i * i < bound; i++)
      if (is_prime(i))
         for (long j = 2*i; j < bound; j += i)
            set(j);
}

long order(long x, long p, mulmod_t pinv, const Factorisation& F)
{
   // in the loop below, m is always some multiple of the order of x
   long m = p - 1;

   // try to remove factors from m until we can't remove any more
   for (size_t i = 0; i < F.factors.size(); i++)
   {
      long q = F.factors[i];

      while (m % q == 0)
      {
         long mm = m / q;
         if (PowerMod(x, mm, p, pinv) != 1)
            break;
         m = mm;
      }
   }

   return m;
}

long primitive_root(long p, mulmod_t pinv, const Factorisation& F)
{
   if (p == 2)
      return 1;

   long g = 2;
   for (; g < p; g++)
      if (order(g, p, pinv, F) == p - 1)
         return g;

   // no generator exists!?
   abort();
}

};    // end namespace

// end of file ================================================================


#pragma once

// C++ standard library
#include <vector>

// Armadillo
#include <armadillo>

namespace mant {
  arma::mat::fixed<2, 2> rotationMatrix2d(
      const double angle);

  arma::mat::fixed<3, 3> rotationMatrix3d(
      const double rollAngle,
      const double pitchAngle,
      const double yawAngle);

  std::vector<arma::vec::fixed<2>> circleCircleIntersections(
      const arma::vec::fixed<2>& firstCenter,
      const double firstRadius,
      const arma::vec::fixed<2>& secondCenter,
      const double secondRadius);

  std::vector<arma::vec::fixed<3>> circleSphereIntersections(
      const arma::vec::fixed<3>& circleCenter,
      const double circleRadius,
      const arma::vec::fixed<3>& circleNormal,
      const arma::vec::fixed<3>& sphereCenter,
      const double sphereRadius);
}


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2021, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include "GeometricBasics.h"

#include <Eigen/Dense>

#include "BaseLib/Logging.h"
#include "Point3d.h"

namespace MathLib
{
double orientation3d(MathLib::Point3d const& p,
                     MathLib::Point3d const& a,
                     MathLib::Point3d const& b,
                     MathLib::Point3d const& c)
{
    auto const pp = Eigen::Map<Eigen::Vector3d const>(p.getCoords());
    auto const pa = Eigen::Map<Eigen::Vector3d const>(a.getCoords());
    auto const pb = Eigen::Map<Eigen::Vector3d const>(b.getCoords());
    auto const pc = Eigen::Map<Eigen::Vector3d const>(c.getCoords());

    Eigen::Vector3d const u = pp - pa;
    Eigen::Vector3d const v = pp - pb;
    Eigen::Vector3d const w = pp - pc;
    return u.cross(v).dot(w);
}

double calcTetrahedronVolume(MathLib::Point3d const& a,
                             MathLib::Point3d const& b,
                             MathLib::Point3d const& c,
                             MathLib::Point3d const& d)
{
    auto const va = Eigen::Map<Eigen::Vector3d const>(a.getCoords());
    auto const vb = Eigen::Map<Eigen::Vector3d const>(b.getCoords());
    auto const vc = Eigen::Map<Eigen::Vector3d const>(c.getCoords());
    auto const vd = Eigen::Map<Eigen::Vector3d const>(d.getCoords());
    Eigen::Vector3d const w = vb - va;
    Eigen::Vector3d const u = vc - va;
    Eigen::Vector3d const v = vd - va;
    return std::abs(u.cross(v).dot(w)) / 6.0;
}

double calcTriangleArea(MathLib::Point3d const& a, MathLib::Point3d const& b,
                        MathLib::Point3d const& c)
{
    auto const va = Eigen::Map<Eigen::Vector3d const>(a.getCoords());
    auto const vb = Eigen::Map<Eigen::Vector3d const>(b.getCoords());
    auto const vc = Eigen::Map<Eigen::Vector3d const>(c.getCoords());
    Eigen::Vector3d const u = vc - va;
    Eigen::Vector3d const v = vb - va;
    Eigen::Vector3d const w = u.cross(v);
    return 0.5 * w.norm();
}

bool isPointInTetrahedron(MathLib::Point3d const& p, MathLib::Point3d const& a,
                          MathLib::Point3d const& b, MathLib::Point3d const& c,
                          MathLib::Point3d const& d, double eps)
{
    double const d0(MathLib::orientation3d(d, a, b, c));
    // if tetrahedron is not coplanar
    if (std::abs(d0) > std::numeric_limits<double>::epsilon())
    {
        bool const d0_sign(d0 > 0);
        // if p is on the same side of bcd as a
        double const d1(MathLib::orientation3d(d, p, b, c));
        if (!(d0_sign == (d1 >= 0) || std::abs(d1) < eps))
        {
            return false;
        }
        // if p is on the same side of acd as b
        double const d2(MathLib::orientation3d(d, a, p, c));
        if (!(d0_sign == (d2 >= 0) || std::abs(d2) < eps))
        {
            return false;
        }
        // if p is on the same side of abd as c
        double const d3(MathLib::orientation3d(d, a, b, p));
        if (!(d0_sign == (d3 >= 0) || std::abs(d3) < eps))
        {
            return false;
        }
        // if p is on the same side of abc as d
        double const d4(MathLib::orientation3d(p, a, b, c));
        return d0_sign == (d4 >= 0) || std::abs(d4) < eps;
    }
    return false;
}

bool isPointInTriangle(MathLib::Point3d const& p,
                       MathLib::Point3d const& a,
                       MathLib::Point3d const& b,
                       MathLib::Point3d const& c,
                       double eps_pnt_out_of_plane,
                       double eps_pnt_out_of_tri,
                       MathLib::TriangleTest algorithm)
{
    switch (algorithm)
    {
        case MathLib::GAUSS:
            return gaussPointInTriangle(p, a, b, c, eps_pnt_out_of_plane,
                                        eps_pnt_out_of_tri);
        case MathLib::BARYCENTRIC:
            return barycentricPointInTriangle(p, a, b, c, eps_pnt_out_of_plane,
                                              eps_pnt_out_of_tri);
        default:
            ERR("Selected algorithm for point in triangle testing not found, "
                "falling back on default.");
    }
    return gaussPointInTriangle(p, a, b, c, eps_pnt_out_of_plane,
                                eps_pnt_out_of_tri);
}

bool gaussPointInTriangle(MathLib::Point3d const& q,
                          MathLib::Point3d const& a,
                          MathLib::Point3d const& b,
                          MathLib::Point3d const& c,
                          double eps_pnt_out_of_plane,
                          double eps_pnt_out_of_tri)
{
    auto const pa = Eigen::Map<Eigen::Vector3d const>(a.getCoords());
    auto const pb = Eigen::Map<Eigen::Vector3d const>(b.getCoords());
    auto const pc = Eigen::Map<Eigen::Vector3d const>(c.getCoords());
    Eigen::Vector3d const v = pb - pa;
    Eigen::Vector3d const w = pc - pa;

    Eigen::Matrix2d mat;
    mat(0, 0) = v.squaredNorm();
    mat(0, 1) = v[0] * w[0] + v[1] * w[1] + v[2] * w[2];
    mat(1, 0) = mat(0, 1);
    mat(1, 1) = w.squaredNorm();
    Eigen::Vector2d y(
        v[0] * (q[0] - a[0]) + v[1] * (q[1] - a[1]) + v[2] * (q[2] - a[2]),
        w[0] * (q[0] - a[0]) + w[1] * (q[1] - a[1]) + w[2] * (q[2] - a[2]));

    y = mat.partialPivLu().solve(y);

    const double lower(eps_pnt_out_of_tri);
    const double upper(1 + lower);

    if (-lower <= y[0] && y[0] <= upper && -lower <= y[1] && y[1] <= upper &&
        y[0] + y[1] <= upper)
    {
        MathLib::Point3d const q_projected(std::array<double, 3>{
            {a[0] + y[0] * v[0] + y[1] * w[0], a[1] + y[0] * v[1] + y[1] * w[1],
             a[2] + y[0] * v[2] + y[1] * w[2]}});
        if (MathLib::sqrDist(q, q_projected) <= eps_pnt_out_of_plane)
        {
            return true;
        }
    }

    return false;
}

bool barycentricPointInTriangle(MathLib::Point3d const& p,
                                MathLib::Point3d const& a,
                                MathLib::Point3d const& b,
                                MathLib::Point3d const& c,
                                double eps_pnt_out_of_plane,
                                double eps_pnt_out_of_tri)
{
    if (std::abs(MathLib::orientation3d(p, a, b, c)) > eps_pnt_out_of_plane)
    {
        return false;
    }

    auto const vp = Eigen::Map<Eigen::Vector3d const>(p.getCoords());
    auto const va = Eigen::Map<Eigen::Vector3d const>(a.getCoords());
    auto const vb = Eigen::Map<Eigen::Vector3d const>(b.getCoords());
    auto const vc = Eigen::Map<Eigen::Vector3d const>(c.getCoords());
    Eigen::Vector3d const pa = va - vp;
    Eigen::Vector3d const pb = vb - vp;
    Eigen::Vector3d const pc = vc - vp;
    double const area_x_2(calcTriangleArea(a, b, c) * 2);

    double const alpha((pb.cross(pc).norm()) / area_x_2);
    if (alpha < -eps_pnt_out_of_tri || alpha > 1 + eps_pnt_out_of_tri)
    {
        return false;
    }
    double const beta((pc.cross(pa).norm()) / area_x_2);
    if (beta < -eps_pnt_out_of_tri || beta > 1 + eps_pnt_out_of_tri)
    {
        return false;
    }
    double const gamma(1 - alpha - beta);
    return !(gamma < -eps_pnt_out_of_tri || gamma > 1 + eps_pnt_out_of_tri);
}

bool isPointInTriangleXY(MathLib::Point3d const& p,
                         MathLib::Point3d const& a,
                         MathLib::Point3d const& b,
                         MathLib::Point3d const& c)
{
    // criterion: p-a = u0 * (b-a) + u1 * (c-a); 0 <= u0, u1 <= 1, u0+u1 <= 1
    Eigen::Matrix2d mat;
    mat(0, 0) = b[0] - a[0];
    mat(0, 1) = c[0] - a[0];
    mat(1, 0) = b[1] - a[1];
    mat(1, 1) = c[1] - a[1];
    Eigen::Vector2d y;
    y << p[0] - a[0], p[1] - a[1];

    y = mat.partialPivLu().solve(y);

    // check if u0 and u1 fulfills the condition
    return 0 <= y[0] && y[0] <= 1 && 0 <= y[1] && y[1] <= 1 && y[0] + y[1] <= 1;
}

bool dividedByPlane(const MathLib::Point3d& a, const MathLib::Point3d& b,
                    const MathLib::Point3d& c, const MathLib::Point3d& d)
{
    for (unsigned x = 0; x < 3; ++x)
    {
        const unsigned y = (x + 1) % 3;
        const double abc =
            (b[x] - a[x]) * (c[y] - a[y]) - (b[y] - a[y]) * (c[x] - a[x]);
        const double abd =
            (b[x] - a[x]) * (d[y] - a[y]) - (b[y] - a[y]) * (d[x] - a[x]);

        if ((abc > 0 && abd < 0) || (abc < 0 && abd > 0))
        {
            return true;
        }
    }
    return false;
}

bool isCoplanar(const MathLib::Point3d& a, const MathLib::Point3d& b,
                const MathLib::Point3d& c, const MathLib::Point3d& d)
{
    auto const pa = Eigen::Map<Eigen::Vector3d const>(a.getCoords());
    auto const pb = Eigen::Map<Eigen::Vector3d const>(b.getCoords());
    auto const pc = Eigen::Map<Eigen::Vector3d const>(c.getCoords());
    auto const pd = Eigen::Map<Eigen::Vector3d const>(d.getCoords());

    Eigen::Vector3d const ab = pb - pa;
    Eigen::Vector3d const ac = pc - pa;
    Eigen::Vector3d const ad = pd - pa;

    auto const eps_squared =
        std::pow(std::numeric_limits<double>::epsilon(), 2);
    if (ab.squaredNorm() < eps_squared || ac.squaredNorm() < eps_squared ||
        ad.squaredNorm() < eps_squared)
    {
        return true;
    }

    // In exact arithmetic <ac*ad^T, ab> should be zero
    // if all four points are coplanar.
    const double sqr_scalar_triple(std::pow(ac.cross(ad).dot(ab), 2));
    // Due to evaluating the above numerically some cancellation or rounding
    // can occur. For this reason a normalisation factor is introduced.
    const double normalisation_factor =
        (ab.squaredNorm() * ac.squaredNorm() * ad.squaredNorm());

    // tolerance 1e-11 is chosen such that
    // a = (0,0,0), b=(1,0,0), c=(0,1,0) and d=(1,1,1e-6) are considered as
    // coplanar
    // a = (0,0,0), b=(1,0,0), c=(0,1,0) and d=(1,1,1e-5) are considered as not
    // coplanar
    return (sqr_scalar_triple / normalisation_factor < 1e-11);
}

}  // end namespace MathLib


#ifndef CANNON_MATH_BARYCENTRIC_COORDS_H
#define CANNON_MATH_BARYCENTRIC_COORDS_H

/*!
 * \file cannon/math/barycentric_coords.hpp
 * \brief File containing free functions relating to barycentric coordinate
 * calculation.
 */

/*!
 * \namespace cannon::math
 * \brief Namespace containing mathematical utilities.
 */

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace math {

    /*!
     * \brief Function to compute barycentric coordinates for a query point on
     * triangle defined by its three vertices. See
     * https://en.wikipedia.org/wiki/Barycentric_coordinate_system
     *
     * \param v0 First vertex
     * \param v1 Second vertex
     * \param v2 Third vertex
     * \param p Query point
     *
     * \returns Barycentric coordinates for the query point.
     */
    Vector3d compute_barycentric_coords(const Vector3d& v0, const Vector3d& v1,
        const Vector3d& v2, const Vector3d& p);

  } // namespace math
} // namespace cannon
#endif /* ifndef CANNON_MATH_BARYCENTRIC_COORDS_H */


//! [mandelbrot-all]
#include <chrono>
#include <iostream>
#include <numeric>
#include <vector>

#include <boost/simd/pack.hpp>
#include <boost/simd/memory/allocator.hpp>

#include <boost/simd/function/aligned_load.hpp>
#include <boost/simd/function/aligned_store.hpp>
#include <boost/simd/function/if_inc.hpp>
#include <boost/simd/function/enumerate.hpp>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

typedef float T;
namespace bs = boost::simd;
using pack_t = bs::pack<T>;
using pack_i = bs::pack<int>;
using pack_l = bs::as_logical_t<pack_t>;

struct mandelbrot{
  int size, max_iter;
  T x_min, x_max, y_min, y_max, x_range, y_range;
  std::vector<int, bs::allocator<int>> iterations;

  mandelbrot(int size_, int max_iter_) : size(size_), max_iter(max_iter_){
    x_min = -2.5;
    x_max = 1;
    y_min = -1;
    y_max = 1;
    x_range = x_max - x_min;
    y_range = y_max - y_min;
    iterations.resize(size * size);
  }

  void evaluate_scalar(){
    for(int i = 0; i < size; ++i){
      T x0 = T(i) / T(size) * x_range + x_min;
      for(int j = 0; j < size; ++j){
	int iteration  = 0;
	T y0 = T(j) / T(size) * y_range + y_min;
	T x = 0;
	T y = 0;
	while(x * x + y * y < 4 && iteration < max_iter){
	  T x_temp = x * x - y * y + x0;
	  y = 2 * x * y + y0;
	  x = x_temp;
	  ++iteration ;
	}
	iterations[j + i * size] = iteration;
      }
    }
  }

  //! [mandelbrot-simd]
  void evaluate_simd(){
    pack_t step =  bs::enumerate<pack_t>(0);
    for(int i = 0; i < size; ++i){
      pack_t x0 {T(i) / T(size) * x_range + x_min};
      pack_t fac { y_range / T(size) };
      pack_t y_min_t {y_min};
      for(int j = 0; j < size; j += pack_t::static_size){
	int iteration  = 0;

	pack_t y0 = (step + j )* fac + y_min_t;
	pack_t x {0};
	pack_t y {0};
	pack_i iter {0};
	pack_l mask;
	do{
	  pack_t x2 = x * x;
	  pack_t y2 = y * y;

	  y = 2 * x * y + y0;
	  x = x2 - y2 + x0;
	  mask = x2 + y2 < 4;
	  ++iteration;
          iter = bs::if_inc(mask, iter);
	} while(bs::any(mask) && iteration < max_iter);
	bs::aligned_store(iter, &iterations[j + i * size]);
      }
    }
  }
  //! [mandelbrot-simd]

  void display() {
    cv::Mat display;
    cv::Mat A(size, size, CV_32SC1, iterations.data());
    A.convertTo(display, CV_8UC1, 255.0 / 1000.0);
    cv::applyColorMap(display, display, cv::COLORMAP_JET);
    cv::imshow("Mandelbrot", display);
    cv::waitKey(0);
  }
};

int main(int argc, char** argv)
{
  namespace chr = std::chrono;
  using hrc = chr::high_resolution_clock;

  int size = 1024;
  int max_iteration = 1000;
  mandelbrot image(size, max_iteration);
  auto t0 = hrc::now();
  image.evaluate_scalar();
  auto t1 = hrc::now();
  std::cout << " scalar " << chr::duration_cast<chr::milliseconds>(t1 - t0).count() << std::endl;

  t0 = hrc::now();
  image.evaluate_simd();
  t1 = hrc::now();
  std::cout << " simd " << chr::duration_cast<chr::milliseconds>(t1 - t0).count() << std::endl;
  image.display();
}
//! [mandelbrot-all]


// Compiled on https://wandbox.org/
#include "utils.hpp"

#include <iostream>
#include <algorithm>
#include <boost/math/special_functions.hpp>

#define K 10
#define PI (acos(-1) / 2.0)

void bessels()
{
	double v = rand_between<double>(-K, K);
	double x = rand_between<double>(-K, K);
	unsigned int n = rand_int(0, K);

	double i = boost::math::cyl_bessel_i((x < 0) ? floor(v) : v, x);
	double j = boost::math::cyl_bessel_j((x < 0) ? floor(v) : v, x);
	double k = boost::math::cyl_bessel_k(v, abs(x)); // x > 0
	double neumann = boost::math::cyl_neumann(v, abs(x)); // x > 0

	double sph = boost::math::sph_bessel(n, abs(x)); // unsigned int, x > 0
	double sph_nm = boost::math::sph_neumann(n, abs(x)); // unsigned int, x > 0

	print("cyl_bessel_i", (x < 0) ? floor(v) : v, x, i);
	print("cyl_bessel_j", (x < 0) ? floor(v) : v, x, j);
	print("cyl_bessel_k", v, abs(x), k);
	print("cyl_neumann", v, abs(x), neumann);

	print("sph_bessel", n, abs(x), sph);
	print("sph_neumann", n, abs(x), sph_nm);
}

void betas()
{
	double x = rand_between<double>(0, K);
	double y = rand_between<double>(0, K);

	print("beta", x, y, boost::math::beta(x, y));
}

void ellints()
{
	double k = rand_between<double>(-1, 1);
	double phi = rand_between<double>(-PI, PI);
	double v = rand_between<double>(0, 1 / (pow(sin(phi), 2.0)));

	double first = boost::math::ellint_1(k, phi);
	double second = boost::math::ellint_2(k, phi);
	double third = boost::math::ellint_3(k, v, phi);

	print("ellint_1", k, phi, first);
	print("ellint_2", k, phi, second);
	print("ellint_3", k, v, phi, third);
}

void expints()
{
	double x = rand_between<double>(-K, K);

	print("expint", x, boost::math::expint(x));
}

void hermites()
{
	unsigned int n = rand_int(0, K);
	double x = rand_between<double>(-K, K);

	print("hermite", n, x, boost::math::hermite(n, x));
}

void laguerres()
{
	unsigned int n = rand_int(0, K);
	unsigned int m = rand_int(0, K);
	double x = rand_between<double>(-K, K);

	double ret = boost::math::laguerre(n, m, x);
	print("assoc_laguerre", n, m, x, ret);
}

void legendres()
{
	unsigned int n = rand_int(0, K);
	unsigned int m = rand_int(0, K);
	double x = rand_between<double>(-1, 1);

	double ret = boost::math::legendre_p(n, m, x);
	print("assoc_legendre", n, m, x, ret);
}

void zetas()
{
	double x = rand_between<double>(-K, K);

	print("riemann_zeta", x, boost::math::zeta(x));
}

int main()
{
	std::cout << "[" << std::endl;
	{
		repeat(bessels);
		repeat(betas);
		repeat(ellints);
		repeat(expints);
		repeat(hermites);
		repeat(laguerres);
		repeat(legendres);
		repeat(zetas);
		std::cout << "\t[\"clamp\", 1, 6, 5, 5]" << std::endl;
	};
	std::cout << "]" << std::endl;

	system("pause");
	return 0;
}


//
// ExpansionHunter
// Copyright (c) 2020 Illumina, Inc.
//
// Author: Konrad Scheffler <kscheffler@illumina.com>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//

// These utility functions were copied from source code of Strelka Small Variant Caller.

#pragma once

#include <boost/math/special_functions/log1p.hpp>

// returns log(1+x), switches to log1p function when abs(x) is small
static double log1p_switch(const double x)
{
    // TODO Justify this switch point. Related discussion:
    // http://cran.r-project.org/web/packages/Rmpfr/vignettes/log1mexp-note.pdf
    static const double smallx_thresh(0.01);

    if (std::abs(x) < smallx_thresh)
    {
        return boost::math::log1p(x);
    }
    else
    {
        return std::log(1 + x);
    }
}

// Returns the equivalent of log(exp(x1)+exp(x2))
static double getLogSum(double x1, double x2)
{
    if (x1 < x2)
        std::swap(x1, x2);
    return x1 + log1p_switch(std::exp(x2 - x1));
}


#include "iostream"
using namespace std;

#include <ctime>
// include Core and Dense compuation parts from Eigen
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

# define MATRIX_SIZE 50

/**************************************************
 * Exercise 5
 * Suppose there is a large Eigen matrix, we want to know the value in the top
 * left 3 x 3 blocks, and then assign it to Identity 3×3.
 * *************************************************/

int main(int argc, char** argv){
    // the large matrix
    Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);

    // Extract the top-left 3*3
    Matrix3d matrix_33;
    for (int i = 0; i < 3; i++){
        for (int j = 0; j < 3; j++){
            matrix_33(i, j) = matrix_NN(i, j);
        }
    }
    // print it out
    cout << "The top left 3*3 matrix is: " << endl << matrix_33 << endl;

    // assign an Identity to it
    for (int i = 0; i < 3; i++){
        for (int j = 0; j < 3; j++){
            if(i == j){
                matrix_NN(i, j) = 1;
            }
            else{
                matrix_NN(i, j) = 0;
            }
        }
    }

}


#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/affine.hpp>
#include <kv/autodif.hpp>
#include <kv/interval-vector.hpp>

/*
 * comparison of interval, affine, mean value form
 * in evaluation of a 5 variable function.
 * The function is taken from http://ci.nii.ac.jp/naid/110003291707
 */

namespace ub = boost::numeric::ublas;
typedef kv::interval<double> itv;

struct Func {
	template <class T> T operator()(const ub::vector<T>& t) {
		T f1, f2, y;

		f1 = (-10. - 13.2 * t[0] + 0.5 * t[1] - 0.4 * t[2] + 7.3 * t[3] + 4.4 * t[4]);
		f2 = (0.8 + 10.5 * t[0] + 5.9 * t[1] + 7.5 * t[2] - 14.5 * t[3] + 1.3 * t[4]);

		y = f1 * f2 - ((-113.7) * f2 + (-49.8) * f1) + 5660.;
		return y;
	}
};

int main()
{
	int i;
	ub::vector<itv> I;
	Func f;

	std::cout.precision(17);

	I.resize(5);

	I[0] = itv(8.7, 8.8);
	I[1] = itv(-9.4, -9.3);
	I[2] = itv(-4.6, -4.5);
	I[3] = itv(3.5, 3.6);
	I[4] = itv(-2.9, -2.8);

	// interval arithmetic

	std::cout << "interval: " << f(I) << "\n";

	// affine arithmetic

	// ub::vector< kv::affine<double> > a;
	ub::vector< kv::affine<itv::base_type> > a;

	a.resize(5);
	for (i=0; i<5; i++) a(i) = I(i);
	std::cout << "affine: " << to_interval(f(a)) << "\n";

	// mean value form

	ub::vector<itv> c, fdi;
	itv fc, fi;

	c = mid(I);
	fc = f(c);
	kv::autodif<itv>::split(f(kv::autodif<itv>::init(I)), fi, fdi);

	std::cout << "mvf: " << fc + inner_prod(fdi, I - c) << "\n";
}


/*
 * Copyright (c) 2012 Aldebaran Robotics. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the COPYING file.
 */

#include <almath/tools/almath.h>
#include <almath/tools/altrigonometry.h>

#include <cmath>
#include <boost/math/special_functions/pow.hpp>
#include <boost/algorithm/clamp.hpp>

namespace AL
{
  namespace Math
  {

    void modulo2PIInPlace(float& pAngle)
    {
      pAngle = std::fmod(pAngle, AL::Math::_2_PI_);

      if (pAngle>AL::Math::PI)
      {
        pAngle = pAngle - AL::Math::_2_PI_;
      }
      else if (pAngle<-AL::Math::PI)
      {
        pAngle = pAngle + AL::Math::_2_PI_;
      }
    }

    float modulo2PI(float pAngle)
    {
      float result = pAngle;
      modulo2PIInPlace(result);
      return result;
    }

    float meanAngle(const std::vector<float> &pAngles) {
      const std::vector<float> weights(pAngles.size(), 1.f);
      return weightedMeanAngle(pAngles, weights);
    }

    float weightedMeanAngle(const std::vector<float> &pAngles,
                            const std::vector<float> &pWeights) {
      if (pAngles.size() != pWeights.size()) {
        throw std::runtime_error("Angles and weights must have the same size.");
      }
      Math::Position2D sumPosition;
      std::vector<float>::const_iterator a = pAngles.begin();
      std::vector<float>::const_iterator w = pWeights.begin();
      float weightSum = 0.f;
      for (; a != pAngles.end(); ++a, ++w) {
        if (*w <= 0.f) {
          throw std::runtime_error("All weights must be strictly positive.");
        }
        weightSum += *w;
        sumPosition += *w * Math::Position2D(std::cos(*a), std::sin(*a));
      }
      if (sumPosition.norm() < 1e-3f) {
        throw std::runtime_error("No defined mean.");
      }
      return Math::modulo2PI(std::atan2(sumPosition.y, sumPosition.x));
    }

    bool clipData(
      const float& pMin,
      const float& pMax,
      float& pData)
    {
      const float saved = pData;
      pData = boost::algorithm::clamp(saved, pMin, pMax);
      return saved != pData;
    }

    bool clipData(
      const float& pMin,
      const float& pMax,
      std::vector<float>& pData)
    {
      bool isClipped = false;
      for (unsigned int i=0; i<pData.size(); ++i)
      {
        if (clipData(pMin, pMax, pData[i]))
        {
          isClipped = true;
        }
      }
      return isClipped;
    }

    bool clipData(
      const float& pMin,
      const float& pMax,
      std::vector<std::vector<float> >& pData)
    {
      bool isClipped = false;
      for (unsigned int i=0; i<pData.size(); ++i)
      {
        if (clipData(pMin, pMax, pData[i]))
        {
          isClipped = true;
        }
      }
      return isClipped;
    }

    void changeReferencePose2D(
        const float&  pTheta,
        const Pose2D& pPosIn,
        Pose2D&       pPosOut)
    {
      const float cs = std::cos(pTheta);
      const float sn = std::sin(pTheta);
      pPosOut.x = cs*pPosIn.x - sn*pPosIn.y;
      pPosOut.y = sn*pPosIn.x + cs*pPosIn.y;
      pPosOut.theta = pPosIn.theta;
    }

    void changeReferencePose2DInPlace(
        const float& pTheta,
        Pose2D&      pPosOut)
    {
      const float cs = std::cos(pTheta);
      const float sn = std::sin(pTheta);
      const float xOld = pPosOut.x;
      const float yOld = pPosOut.y;

      pPosOut.x = cs*xOld - sn*yOld;
      pPosOut.y = sn*xOld + cs*yOld;
    }

    Position3D position3DFromPosition6D(const Position6D& pPosition6D)
    {
      return Position3D(pPosition6D.x, pPosition6D.y, pPosition6D.z);
    }

    Position6D position6DFromVelocity6D(const Velocity6D& pVel)
    {
      return Position6D(
            pVel.xd,
            pVel.yd,
            pVel.zd,
            pVel.wxd,
            pVel.wyd,
            pVel.wzd);
    }

    Position3D operator*(
      const Rotation&   pRot,
      const Position3D& pPos)
    {
      return Position3D(
            pRot.r1_c1*pPos.x + pRot.r1_c2*pPos.y + pRot.r1_c3*pPos.z,
            pRot.r2_c1*pPos.x + pRot.r2_c2*pPos.y + pRot.r2_c3*pPos.z,
            pRot.r3_c1*pPos.x + pRot.r3_c2*pPos.y + pRot.r3_c3*pPos.z);
    }

    Position3D operator*(
        const Quaternion& pQuat,
        const Position3D& pPos)
    {
      Position3D result;
      result.x = pQuat.w * pQuat.w * pPos.x + 2.0f *pQuat.y * pQuat.w * pPos.z;
      result.x += - 2.0f * pQuat.z * pQuat.w * pPos.y + pQuat.x * pQuat.x * pPos.x;
      result.x += 2.0f * pQuat.y * pQuat.x * pPos.y + 2.0f * pQuat.z * pQuat.x * pPos.z;
      result.x += - pQuat.z * pQuat.z * pPos.x - pQuat.y * pQuat.y * pPos.x;

      result.y = 2.0f * pQuat.x * pQuat.y * pPos.x + pQuat.y * pQuat.y * pPos.y;
      result.y += 2.0f * pQuat.z * pQuat.y * pPos.z + 2.0f * pQuat.w * pQuat.z * pPos.x;
      result.y += - pQuat.z * pQuat.z * pPos.y + pQuat.w * pQuat.w * pPos.y;
      result.y += - 2.0f * pQuat.x * pQuat.w * pPos.z - pQuat.x * pQuat.x * pPos.y;

      result.z = 2.0f * pQuat.x * pQuat.z * pPos.x + 2.0f * pQuat.y * pQuat.z * pPos.y;
      result.z += pQuat.z * pQuat.z * pPos.z - 2.0f * pQuat.w * pQuat.y * pPos.x;
      result.z += - pQuat.y * pQuat.y * pPos.z + 2.0f * pQuat.w * pQuat.x * pPos.y;
      result.z += - pQuat.x * pQuat.x * pPos.z + pQuat.w * pQuat.w * pPos.z;
      return result;
    }

    Velocity6D operator*(
      const float       pVal,
      const Position6D& pPos)
    {
      return Velocity6D(
            pVal * pPos.x,
            pVal * pPos.y,
            pVal * pPos.z,
            pVal * pPos.wx,
            pVal * pPos.wy,
            pVal * pPos.wz);
    }

    Velocity3D operator*(
      const float       pVal,
      const Position3D& pPos)
    {
      return Velocity3D(
            pVal * pPos.x,
            pVal * pPos.y,
            pVal * pPos.z);
    }

    Velocity3D operator*(
      const Rotation&   pRot,
      const Velocity3D& pVel)
    {
      return Velocity3D(
            pRot.r1_c1*pVel.xd + pRot.r1_c2*pVel.yd + pRot.r1_c3*pVel.zd,
            pRot.r2_c1*pVel.xd + pRot.r2_c2*pVel.yd + pRot.r2_c3*pVel.zd,
            pRot.r3_c1*pVel.xd + pRot.r3_c2*pVel.yd + pRot.r3_c3*pVel.zd);
    }

    AL::Math::Rotation rotationFromAngleDirection(
      const float&                pTheta,
      const AL::Math::Position3D& pPos)
    {
      return AL::Math::rotationFromAngleDirection(
            pTheta,
            pPos.x,
            pPos.y,
            pPos.z);
    }

    void position2DFromPose2DInPlace(
        const Pose2D& pPose2D,
        Position2D&   pPosition2D)
    {
      pPosition2D.x = pPose2D.x;
      pPosition2D.y = pPose2D.y;
    }

    Position2D position2DFromPose2D(const Pose2D& pPose2D)
    {
      return Position2D(pPose2D.x, pPose2D.y);
    }

    void position6DFromPose2DInPlace(
        const Pose2D& pPose2D,
        Position6D&   pPosition6D)
    {
      pPosition6D.x  = pPose2D.x;
      pPosition6D.y  = pPose2D.y;
      pPosition6D.z  = 0.0f;
      pPosition6D.wx = 0.0f;
      pPosition6D.wy = 0.0f;
      pPosition6D.wz = pPose2D.theta;
    }

    Position6D position6DFromPose2D(const Pose2D& pPose2D)
    {
      return Position6D(
            pPose2D.x,
            pPose2D.y,
            0.0f,
            0.0f,
            0.0f,
            pPose2D.theta);
    }

    void position6DFromPosition3DInPlace(
        const Position3D& pPosition3D,
        Position6D&       pPosition6D)
    {
      pPosition6D.x  = pPosition3D.x;
      pPosition6D.y  = pPosition3D.y;
      pPosition6D.z  = pPosition3D.z;
      pPosition6D.wx = 0.0f;
      pPosition6D.wy = 0.0f;
      pPosition6D.wz = 0.0f;
    }

    Position6D position6DFromPosition3D(const Position3D& pPosition3D)
    {
      return Position6D(pPosition3D.x,
                        pPosition3D.y,
                        pPosition3D.z,
                        0.0f,
                        0.0f,
                        0.0f);
    }

    void pose2DFromPosition6DInPlace(
        const Position6D& pPosition6D,
        Pose2D&           pPose2D)
    {
      pPose2D.x     = pPosition6D.x;
      pPose2D.y     = pPosition6D.y;
      pPose2D.theta = pPosition6D.wz;
    }

    Pose2D pose2DFromPosition6D(const Position6D& pPosition6D)
    {
      return Pose2D(pPosition6D.x, pPosition6D.y, pPosition6D.wz);
    }

    void pose2DFromPosition2DInPlace(
        const Position2D& pPosition2D,
        const float       pAngle,
        Pose2D&           pPose2D)
    {
      pPose2D.x     = pPosition2D.x;
      pPose2D.y     = pPosition2D.y;
      pPose2D.theta = pAngle;
    }

    Pose2D pose2DFromPosition2D(const Position2D& pPosition2D,
                                const float       pAngle)
    {
      return Pose2D(pPosition2D.x, pPosition2D.y, pAngle);
    }

    Position2D operator*(
      const Pose2D&     pVal,
      const Position2D& pPos)
    {
      return Position2D(
            pVal.x + std::cos(pVal.theta)*pPos.x - std::sin(pVal.theta)*pPos.y,
            pVal.y + std::sin(pVal.theta)*pPos.x + std::cos(pVal.theta)*pPos.y);
    }

    void quaternionFromRotation3D(
        const Rotation3D& pRot3D,
        Quaternion& pQuaternion)
    {
      const float cx = std::cos(0.5f*pRot3D.wx);
      const float cy = std::cos(0.5f*pRot3D.wy);
      const float cz = std::cos(0.5f*pRot3D.wz);
      const float sx = std::sin(0.5f*pRot3D.wx);
      const float sy = std::sin(0.5f*pRot3D.wy);
      const float sz = std::sin(0.5f*pRot3D.wz);

      pQuaternion.w = cz * cy * cx + sz * sy * sx;
      pQuaternion.x = cz * cy * sx - sz * sy * cx;
      pQuaternion.y = cz * sy * cx + cy * sz * sx;
      pQuaternion.z = cy * sz * cx - cz * sy * sx;
    }

    Quaternion quaternionFromRotation3D(
        const Rotation3D& pRot3D)
    {
      Quaternion lQuat;
      quaternionFromRotation3D(pRot3D, lQuat);
      return lQuat;
    }

    void rotationFromQuaternion(
        const Quaternion& pQua,
        Rotation& pRot)
    {
      pRot.r1_c1 = 1.0f - 2.0f*(std::pow(pQua.y, 2) + std::pow(pQua.z, 2));
      pRot.r1_c2 = 2.0f*(pQua.x*pQua.y - pQua.z*pQua.w);
      pRot.r1_c3 = 2.0f*(pQua.x*pQua.z + pQua.y*pQua.w);

      pRot.r2_c1 = 2.0f*(pQua.x*pQua.y + pQua.z*pQua.w);
      pRot.r2_c2 = 1.0f - 2.0f*(std::pow(pQua.x, 2) + std::pow(pQua.z, 2));
      pRot.r2_c3 = 2.0f*(pQua.y*pQua.z - pQua.x*pQua.w);

      pRot.r3_c1 = 2.0f*(pQua.x*pQua.z - pQua.y*pQua.w);
      pRot.r3_c2 = 2.0f*(pQua.y*pQua.z + pQua.x*pQua.w);
      pRot.r3_c3 = 1.0f - 2.0f*(std::pow(pQua.x, 2) + std::pow(pQua.y, 2));
    }

    Rotation rotationFromQuaternion(
        const Quaternion& pQua)
    {
      Rotation lRot;
      rotationFromQuaternion(pQua, lRot);
      return lRot;
    }

    void rotation3DFromQuaternion(
        const AL::Math::Quaternion& pQuaternion,
        AL::Math::Rotation3D& pRot3D)
    {
      //rotationFromQuaternion
      AL::Math::Rotation lRot;
      rotationFromQuaternion(pQuaternion, lRot);

      pRot3D.wz = std::atan2(lRot.r2_c1, lRot.r1_c1);
      const float sy = std::sin(pRot3D.wz);
      const float cy = std::cos(pRot3D.wz);
      pRot3D.wy = std::atan2(-lRot.r3_c1, cy*lRot.r1_c1+sy*lRot.r2_c1);
      pRot3D.wx = std::atan2(sy*lRot.r1_c3-cy*lRot.r2_c3, cy*lRot.r2_c2-sy*lRot.r1_c2);
    }

    Rotation3D rotation3DFromQuaternion(
        const Quaternion& pQuaternion)
    {
      Rotation3D lRotation3D;
      rotation3DFromQuaternion(pQuaternion, lRotation3D);
      return lRotation3D;
    }

    void quaternionPosition3DFromPosition6D(
        const Position6D& pPos6D,
        Quaternion& pQua,
        Position3D& pPos3D)
    {
      quaternionFromRotation3D(
            AL::Math::Rotation3D(pPos6D.wx, pPos6D.wy, pPos6D.wz),
            pQua);

      pPos3D.x = pPos6D.x;
      pPos3D.y = pPos6D.y;
      pPos3D.z = pPos6D.z;
    }

    void pointMassRotationalInertia(
        float pMass,
        const Position3D& pPos,
        std::vector<float>& pInertia)
    {
      const float x2 = boost::math::pow<2>(pPos.x);
      const float y2 = boost::math::pow<2>(pPos.y);
      const float z2 = boost::math::pow<2>(pPos.z);
      pInertia.resize(9);
      pInertia[0] = pMass * (y2 + z2);
      pInertia[4] = pMass * (x2 + z2);
      pInertia[8] = pMass * (x2 + y2);
      pInertia[1] = pInertia[3] = - pMass * pPos.x * pPos.y;
      pInertia[2] = pInertia[6] = - pMass * pPos.x * pPos.z;
      pInertia[5] = pInertia[7] = - pMass * pPos.y * pPos.z;
    }
  } // namespace Math
} // namespace AL


#include "exception.hh"
#include "network.hh"
#include "timer.hh"

#include <Eigen/Dense>
#include <iostream>
#include <random>
#include <utility>

#include <sys/resource.h>
#include <sys/time.h>

using namespace std;
using namespace Eigen;

constexpr size_t batch_size = 1;
constexpr size_t input_size = 1;

/* use squared error as loss function */
float loss_function( const float target, const float actual )
{
  return ( target - actual ) * ( target - actual );
}

/* partial derivative of loss with respect to neural network output */
float compute_pd_loss_wrt_output( const float target, const float actual )
{
  return -2 * ( target - actual );
}

/* actual function we want the neural network to learn */
float true_function( const float input )
{
  return 3 * input + 1;
}

constexpr float learning_rate = 0.001;

void program_body()
{
  /* remove limit on stack size */
  const rlimit limits { RLIM_INFINITY, RLIM_INFINITY };
  CheckSystemCall( "setrlimit", setrlimit( RLIMIT_STACK, &limits ) );

  /* seed C RNG for Eigen random weight initialization */
  srand( Timer::timestamp_ns() );

  /* construct neural network on heap */
  auto nn = make_unique<Network<float, batch_size, input_size, 1>>();
  nn->layer0.weights()( 0 ) = 1;
  nn->layer0.biases()( 0 ) = 1;

  double x_value = rand() % 30;
  // while ( true ) {
  for ( auto i = 0; i < 100; i++ ) {
    cout << "Weight = " << nn->layer0.weights()( 0 ) << ", bias = " << nn->layer0.biases()( 0 ) << "\n";

    /* step 1: construct a unique problem instance */
    Matrix<float, batch_size, input_size> input, ground_truth_output;

    input( 0, 0 ) = x_value;
    ground_truth_output( 0, 0 ) = true_function( x_value );

    // x_value++; /* use a different problem instance next time */
    x_value = rand() % 40;

    cout << "problem instance: " << input( 0, 0 ) << " => " << ground_truth_output( 0, 0 ) << "\n";

    /* step 2: forward propagate and calculate loss function */
    nn->apply( input );

    cout << "NN maps " << input( 0, 0 ) << " => " << nn->output()( 0, 0 ) << "\n";

    cout << "loss when " << ground_truth_output( 0, 0 ) << " desired, " << nn->output()( 0, 0 )
         << " produced = " << loss_function( nn->output()( 0, 0 ), ground_truth_output( 0, 0 ) ) << "\n";

    /* step 3: backpropagate error */
    nn->computeDeltas();
    nn->evaluateGradients( input );

    const float pd_loss_wrt_output
      = compute_pd_loss_wrt_output( ground_truth_output( 0, 0 ), nn->output()( 0, 0 ) );

    /* perturb weight */
    nn->layer0.weights()( 0 ) -= learning_rate * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 0 );
    nn->layer0.biases()( 0 ) -= learning_rate * pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 1 );
    // nn -> layer0.weights()(0) -= learning_rate * pd_loss_wrt_output;
    // nn -> layer0.biases()(0) -= learning_rate * pd_loss_wrt_output * nn->layer0.biases()(0);

    cout << "\n";

    cout << "pd_loss_wrt_output = " << pd_loss_wrt_output << "\n";
    cout << "pd_output_wrt_weight = " << nn->getEvaluatedGradient( 0, 0 ) << "\n";
    cout << "pd_output_wrt_bias = " << nn->getEvaluatedGradient( 0, 1 ) << "\n";

    cout << "\n";

    cout << "perturb weight: " << -pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 0 ) << "\n";
    cout << "perturb bias: " << -pd_loss_wrt_output * nn->getEvaluatedGradient( 0, 1 ) << "\n";

    cout << "\n";
  }

#if 0
  /* input and output */
  for ( int i = 0; i < 10; i++ ) {
    Matrix<float, batch_size, input_size> in, out;
    int v = rand() % 100 + 1;
    in( 0 ) = (float)v;
    out( 0 ) = 3 * v + 1 + (float)rand() / ( RAND_MAX );
    input.push_back( in );
    output.push_back( out );
  }
  for ( int round = 0; round < 100; round++ ) {
    for ( int i = 0; i < 10; i++ ) {
      nn->apply( input[i] );
      cout << "right after apply: " << input[i]( 0 ) << "  " << output[i]( 0 ) << endl;

      // cout << "output: " << nn->output() << "\n";

      // const float loss = ( 100 - nn->output()( 0 ) ) * ( 100 - nn->output()( 0 ) );
      const float loss = getLoss( output[i]( 0 ), nn->output()( 0 ) );

      nn->computeDeltas();
      nn->evaluateGradients( input[i] );

      // const float pd_loss_output = -2 * ( 100 - nn->output()( 0 ) );
      const float pd_loss_output = get_pd_loss_output( output[i]( 0 ), nn->output()( 0 ) );

      cout << "partial derivative of output wrt weight = " << nn->getEvaluatedGradient( 0, 0 ) << "\n";
      cout << "partial derivative of output wrt bias = " << nn->getEvaluatedGradient( 0, 1 ) << "\n";

      cout << "loss: " << loss << "\n";

      cout << "partial derivative of loss wrt output = " << pd_loss_output << "\n";

      cout << "partial derivative of loss wrt weight = " << pd_loss_output * nn->getEvaluatedGradient( 0, 0 )
           << "\n";
      cout << "partial derivative of loss wrt bias = " << pd_loss_output * nn->getEvaluatedGradient( 0, 1 ) << "\n";
      cout << "weight = " << nn->layer0.weights()( 0 ) << endl;
      cout << "biase = " << nn->layer0.biases()( 0 ) << endl;

      const float learning_rate = .0001;

      nn->layer0.weights()( 0 ) *= ( 1 - learning_rate * pd_loss_output * nn->getEvaluatedGradient( 0, 0 ) );
      nn->layer0.biases()( 0 ) *= ( 1 - learning_rate * pd_loss_output * nn->getEvaluatedGradient( 0, 1 ) );
    }
  }

  // testing
  cout << "start testing results" << endl;
  Matrix<float, batch_size, input_size> test_input, test_output;
  for ( int i = 0; i < 10; i++ ) {
    int v = rand() % 100 + 1;
    test_input( 0 ) = (float)v;
    test_output( 0 ) = 3 * v + 1;
    cout << "input: " << test_input( 0 ) << ", output: " << test_output( 0 ) << endl;
    nn->apply( test_input );
    auto nnOutput = nn->output();
    cout << "nn output: " << nnOutput << endl;
  }
#endif
}

int main( int argc, char*[] )
{
  try {
    if ( argc <= 0 ) {
      abort();
    }

    program_body();

    return EXIT_SUCCESS;
  } catch ( const exception& e ) {
    cerr << e.what() << "\n";
    return EXIT_FAILURE;
  }
}


//  (C) Copyright John Maddock 2019.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// Contains Quickbook snippets used by boost/libs/multiprecision/doc/multiprecision.qbk,
// section Literal Types and constexpr Support.

#include <iostream>
#include <boost/config.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

//[constexpr_circle
#include <boost/math/constants/constants.hpp> // For constant pi with full precision of type T.
// using  boost::math::constants::pi;

template <class T>
inline constexpr T circumference(T radius)
{
   return 2 * boost::math::constants::pi<T>() * radius;
}

template <class T>
inline constexpr T area(T radius)
{
   return boost::math::constants::pi<T>() * radius * radius;
}
//] [/constexpr_circle]

template <class T, unsigned Order>
struct const_polynomial
{
 public:
   T data[Order + 1];

 public:
   constexpr const_polynomial(T val = 0) : data{val} {}
   constexpr const_polynomial(const std::initializer_list<T>& init) : data{}
   {
      if (init.size() > Order + 1)
         throw std::range_error("Too many initializers in list!");
      for (unsigned i = 0; i < init.size(); ++i)
         data[i] = init.begin()[i];
   }
   constexpr T& operator[](std::size_t N)
   {
      return data[N];
   }
   constexpr const T& operator[](std::size_t N) const
   {
      return data[N];
   }
   template <class U>
   constexpr T operator()(U val)const
   {
      T result = data[Order];
      for (unsigned i = Order; i > 0; --i)
      {
         result *= val;
         result += data[i - 1];
      }
      return result;
   }
   constexpr const_polynomial<T, Order - 1> derivative() const
   {
      const_polynomial<T, Order - 1> result;
      for (unsigned i = 1; i <= Order; ++i)
      {
         result[i - 1] = (*this)[i] * i;
      }
      return result;
   }
   constexpr const_polynomial operator-()
   {
      const_polynomial t(*this);
      for (unsigned i = 0; i <= Order; ++i)
         t[i] = -t[i];
      return t;
   }
   template <class U>
   constexpr const_polynomial& operator*=(U val)
   {
      for (unsigned i = 0; i <= Order; ++i)
         data[i] = data[i] * val;
      return *this;
   }
   template <class U>
   constexpr const_polynomial& operator/=(U val)
   {
      for (unsigned i = 0; i <= Order; ++i)
         data[i] = data[i] / val;
      return *this;
   }
   template <class U>
   constexpr const_polynomial& operator+=(U val)
   {
      data[0] += val;
      return *this;
   }
   template <class U>
   constexpr const_polynomial& operator-=(U val)
   {
      data[0] -= val;
      return *this;
   }
};

template <class T, unsigned Order1, unsigned Order2>
inline constexpr const_polynomial<T, (Order1 > Order2 ? Order1 : Order2)> operator+(const const_polynomial<T, Order1>& a, const const_polynomial<T, Order2>& b)
{
   if
      constexpr(Order1 > Order2)
      {
         const_polynomial<T, Order1> result(a);
         for (unsigned i = 0; i <= Order2; ++i)
            result[i] += b[i];
         return result;
      }
   else
   {
      const_polynomial<T, Order2> result(b);
      for (unsigned i = 0; i <= Order1; ++i)
         result[i] += a[i];
      return result;
   }
}
template <class T, unsigned Order1, unsigned Order2>
inline constexpr const_polynomial<T, (Order1 > Order2 ? Order1 : Order2)> operator-(const const_polynomial<T, Order1>& a, const const_polynomial<T, Order2>& b)
{
   if
      constexpr(Order1 > Order2)
      {
         const_polynomial<T, Order1> result(a);
         for (unsigned i = 0; i <= Order2; ++i)
            result[i] -= b[i];
         return result;
      }
   else
   {
      const_polynomial<T, Order2> result(b);
      for (unsigned i = 0; i <= Order1; ++i)
         result[i] = a[i] - b[i];
      return result;
   }
}
template <class T, unsigned Order1, unsigned Order2>
inline constexpr const_polynomial<T, Order1 + Order2> operator*(const const_polynomial<T, Order1>& a, const const_polynomial<T, Order2>& b)
{
   const_polynomial<T, Order1 + Order2> result;
   for (unsigned i = 0; i <= Order1; ++i)
   {
      for (unsigned j = 0; j <= Order2; ++j)
      {
         result[i + j] += a[i] * b[j];
      }
   }
   return result;
}
template <class T, unsigned Order, class U>
inline constexpr const_polynomial<T, Order> operator*(const const_polynomial<T, Order>& a, const U& b)
{
   const_polynomial<T, Order> result(a);
   for (unsigned i = 0; i <= Order; ++i)
   {
      result[i] *= b;
   }
   return result;
}
template <class U, class T, unsigned Order>
inline constexpr const_polynomial<T, Order> operator*(const U& b, const const_polynomial<T, Order>& a)
{
   const_polynomial<T, Order> result(a);
   for (unsigned i = 0; i <= Order; ++i)
   {
      result[i] *= b;
   }
   return result;
}
template <class T, unsigned Order, class U>
inline constexpr const_polynomial<T, Order> operator/(const const_polynomial<T, Order>& a, const U& b)
{
   const_polynomial<T, Order> result;
   for (unsigned i = 0; i <= Order; ++i)
   {
      result[i] /= b;
   }
   return result;
}

//[hermite_example
template <class T, unsigned Order>
class hermite_polynomial
{
   const_polynomial<T, Order> m_data;

 public:
   constexpr hermite_polynomial() : m_data(hermite_polynomial<T, Order - 1>().data() * const_polynomial<T, 1>{0, 2} - hermite_polynomial<T, Order - 1>().data().derivative())
   {
   }
   constexpr const const_polynomial<T, Order>& data() const
   {
      return m_data;
   }
   constexpr const T& operator[](std::size_t N)const
   {
      return m_data[N];
   }
   template <class U>
   constexpr T operator()(U val)const
   {
      return m_data(val);
   }
};
//] [/hermite_example]

//[hermite_example2
template <class T>
class hermite_polynomial<T, 0>
{
   const_polynomial<T, 0> m_data;

 public:
   constexpr hermite_polynomial() : m_data{1} {}
   constexpr const const_polynomial<T, 0>& data() const
   {
      return m_data;
   }
   constexpr const T& operator[](std::size_t N) const
   {
      return m_data[N];
   }
   template <class U>
   constexpr T operator()(U val)
   {
      return m_data(val);
   }
};

template <class T>
class hermite_polynomial<T, 1>
{
   const_polynomial<T, 1> m_data;

 public:
   constexpr hermite_polynomial() : m_data{0, 2} {}
   constexpr const const_polynomial<T, 1>& data() const
   {
      return m_data;
   }
   constexpr const T& operator[](std::size_t N) const
   {
      return m_data[N];
   }
   template <class U>
   constexpr T operator()(U val)
   {
      return m_data(val);
   }
};
//] [/hermite_example2]

void test_double()
{
   constexpr double radius = 2.25;
   constexpr double c      = circumference(radius);
   constexpr double a      = area(radius);

   std::cout << "Circumference = " << c << std::endl;
   std::cout << "Area = " << a << std::endl;

   constexpr const_polynomial<double, 2> pa = {3, 4};
   constexpr const_polynomial<double, 2> pb = {5, 6};
   static_assert(pa[0] == 3);
   static_assert(pa[1] == 4);
   constexpr auto pc = pa * 2;
   static_assert(pc[0] == 6);
   static_assert(pc[1] == 8);
   constexpr auto pd = 3 * pa;
   static_assert(pd[0] == 3 * 3);
   static_assert(pd[1] == 4 * 3);
   constexpr auto pe = pa + pb;
   static_assert(pe[0] == 3 + 5);
   static_assert(pe[1] == 4 + 6);
   constexpr auto pf = pa - pb;
   static_assert(pf[0] == 3 - 5);
   static_assert(pf[1] == 4 - 6);
   constexpr auto pg = pa * pb;
   static_assert(pg[0] == 15);
   static_assert(pg[1] == 38);
   static_assert(pg[2] == 24);

   #if defined(__clang__) && (__clang_major__ < 6)
   #else
   constexpr hermite_polynomial<double, 2> h1;
   static_assert(h1[0] == -2);
   static_assert(h1[1] == 0);
   static_assert(h1[2] == 4);

   constexpr hermite_polynomial<double, 3> h3;
   static_assert(h3[0] == 0);
   static_assert(h3[1] == -12);
   static_assert(h3[2] == 0);
   static_assert(h3[3] == 8);

   constexpr hermite_polynomial<double, 9> h9;
   static_assert(h9[0] == 0);
   static_assert(h9[1] == 30240);
   static_assert(h9[2] == 0);
   static_assert(h9[3] == -80640);
   static_assert(h9[4] == 0);
   static_assert(h9[5] == 48384);
   static_assert(h9[6] == 0);
   static_assert(h9[7] == -9216);
   static_assert(h9[8] == 0);
   static_assert(h9[9] == 512);

   static_assert(h9(0.5) == 6481);
   #endif
}

void test_float128()
{
#ifdef BOOST_HAS_FLOAT128
//[constexpr_circle_usage

   using boost::multiprecision::float128;

   constexpr float128 radius = 2.25;
   constexpr float128 c      = circumference(radius);
   constexpr float128 a      = area(radius);

   std::cout << "Circumference = " << c << std::endl;
   std::cout << "Area = " << a << std::endl;

 //]   [/constexpr_circle_usage]

   constexpr hermite_polynomial<float128, 2> h1;
   static_assert(h1[0] == -2);
   static_assert(h1[1] == 0);
   static_assert(h1[2] == 4);

   constexpr hermite_polynomial<float128, 3> h3;
   static_assert(h3[0] == 0);
   static_assert(h3[1] == -12);
   static_assert(h3[2] == 0);
   static_assert(h3[3] == 8);

   //[hermite_example3
   constexpr hermite_polynomial<float128, 9> h9;
   //
   // Verify that the polynomial's coefficients match the known values:
   //
   static_assert(h9[0] == 0);
   static_assert(h9[1] == 30240);
   static_assert(h9[2] == 0);
   static_assert(h9[3] == -80640);
   static_assert(h9[4] == 0);
   static_assert(h9[5] == 48384);
   static_assert(h9[6] == 0);
   static_assert(h9[7] == -9216);
   static_assert(h9[8] == 0);
   static_assert(h9[9] == 512);
   //
   // Define an abscissa value to evaluate at:
   constexpr float128 abscissa(0.5);
   //
   // Evaluate H_9(0.5) using all constexpr arithmetic, and check that it has the expected result:
   static_assert(h9(abscissa) == 6481);
   //]
#endif
}

int main()
{
   test_double();
   test_float128();
   std::cout << "Done!" << std::endl;
}


#ifndef SPHEP_GENERATOR_HH
#define SPHEP_GENERATOR_HH

#include <Eigen/Dense>

namespace sphep
{
  // compute force from b to a
  template <class V>
  V computeForce(const V& a, const V& b)
  {
    // compute direction of force a-b
    auto diff = a - b;
    // scale force by norm(diff)^3
    typename V::Scalar diffsq = diff.squaredNorm();
    return diff / (diffsq * std::sqrt(diffsq));
  }

  // update the veloctiy in an explicit euler fashion
  template <class V>
  void updateVelocity(const std::vector<V>& points, typename V::Scalar dt, std::vector<V>& velocity)
  {
    assert(points.size() == velocity.size());

    for (std::size_t i = 0; i < points.size(); ++i) {
      for (std::size_t j = i + 1; j < points.size(); ++j) {
        // compute force from j to i
        V diff = computeForce(points[i], points[j]);
        // update velocity of i and j with dt*diff (using action = reaction)
        velocity[i] += dt * diff;
        velocity[j] -= dt * diff;
      }
    }
  }

  // redistribute the given points on the unit sphere, such that they are
  // more evenly distributed
  template <class V>
  void optimizePointsOnUnitSphere(std::vector<V>& points, unsigned int iterations,
                                  typename V::Scalar dist, typename V::Scalar dt)
  {
    const std::size_t N = points.size();
    std::vector<V> velocity(points.size(), V::Zero());
    for (unsigned int iter = 0; iter < iterations; ++iter) {
      // update velocity (explicit euler)
      updateVelocity(points, dt, velocity);
      // update points (explicit euler)
      typename V::Scalar globalDist = 0.0;
      for (std::size_t i = 0; i < N; ++i) {
        V oldPoint(points[i]);
        // update point
        points[i] += dt * velocity[i];
        // fetch p_i back onto sphere
        points[i] /= points[i].norm();
        // compute distance between old and newpoint
        globalDist += (oldPoint - points[i]).norm();
      }
      // stop iteration if global update is less than threshold
      if (globalDist < dist) {
        break;
      }
    }
  }

  // non-specialized struct for generating random points on the unit sphere
  template <class Vector>
  struct PointOnUnitSphereGenerator;

  // three dimensional spcialization of the random points on unit sphere
  // generator. Uses sphere coordinates.
  template <class T>
  struct PointOnUnitSphereGenerator<Eigen::Matrix<T, 3, 1> >
  {
    typedef Eigen::Matrix<T, 3, 1> Vector;

    Vector operator()() const
    {
      // generate random sphere coordinates between [0,2*PI]
      T theta = 2.0 * M_PI * static_cast<T>(std::rand()) / RAND_MAX;
      T phi = 2.0 * M_PI * static_cast<T>(std::rand()) / RAND_MAX;
      // transform sphere coordinates to cartesian
      Vector p;
      p << std::sin(theta) * std::cos(phi), std::sin(theta) * std::sin(phi), std::cos(theta);
      return p;
    }
  };

  // perform the operation axpy on a vector
  template <class Vector>
  struct ScaleAndTranslateFunctor
  {
    typedef typename Vector::Scalar Scalar;

    ScaleAndTranslateFunctor(const Scalar& factor_ = Scalar(1.0),
                             const Vector& offset_ = Vector::Zero())
        : factor(factor_)
        , offset(offset_)
    {
    }

    void operator()(Vector& vector) const { vector = vector * factor + offset; }

    Scalar factor;
    Vector offset;
  };

  // generator more or less evenly distributed points on a sphere
  template <class Vector>
  void generatePoints(unsigned int N, std::vector<Vector>& points,
                      const Vector& center = Vector::Zero(), typename Vector::Scalar radius = 1.0,
                      unsigned int iterations = 500, typename Vector::Scalar dist = 1e-3,
                      typename Vector::Scalar dt = 0.1)
  {
    typedef typename Vector::Scalar Scalar;
    // construct vector
    std::vector<Vector> newPoints;
    newPoints.reserve(N);

    // generate N points on unit sphere
    std::generate_n(std::back_inserter(newPoints), N, PointOnUnitSphereGenerator<Vector>());
    // optimize these points on unit sphere
    optimizePointsOnUnitSphere(newPoints, iterations, dist, dt);

    // map from unit sphere to radius-sphere at center
    std::for_each(newPoints.begin(), newPoints.end(),
                  ScaleAndTranslateFunctor<Vector>(radius, center));

    // copy the new vectors into the output
    std::copy(newPoints.begin(), newPoints.end(), std::back_inserter(points));
  }
}

#endif // SPHEP_GENERATOR_HH


#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <cassert>
#include <iostream>

#include <Eigen/Dense>
#include <algorithm>

#include "aux/eigen2hdf.hpp"
#include "quadrature/qhermite.hpp"
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"
#include "spectral/h2n_1d.hpp"
#include "spectral/hermite_to_nodal.hpp"
#include "spectral/polar_to_hermite.hpp"

//#include <deal.II/base/exceptions.h>

using namespace std;
using namespace boltzmann;

namespace po = boost::program_options;

int main(int argc, char* argv[])
{
  typedef Eigen::VectorXd vec_t;

  typedef typename SpectralBasisFactoryKS::basis_type polar_basis_t;
  polar_basis_t polar_basis;

  string fname = "spectral_basis.desc";
  cout << "reading `" << fname << "` from disk.\n";
  SpectralBasisFactoryKS::create(polar_basis, fname);

  int K = spectral::get_max_k(polar_basis) + 1;
  int N = polar_basis.n_dofs();

  // create basis files
  typedef typename SpectralBasisFactoryHN::basis_type hermite_basis_t;
  hermite_basis_t hermite_basis;
  SpectralBasisFactoryHN::create(hermite_basis, K, 2);
  cout << "Hermite basis size: " << hermite_basis.n_dofs() << endl;
  cout << "Polar basis size: " << polar_basis.n_dofs() << endl;

  // polar 2 hermite
  Polar2Hermite<polar_basis_t, hermite_basis_t> P2H(polar_basis, hermite_basis);

  typedef Eigen::MatrixXd mat_t;
  // hermite to nodal
  Hermite2Nodal<hermite_basis_t> h2n(
      hermite_basis, K, [K](mat_t& m1, mat_t& m2) { H2N_1d<>::create(m1, m2, K); });

  // debug
  auto& M = h2n.get_h2n();
  cout << "M := H2N-matrix\n";
  cout << "M.T * M\n";
  Eigen::MatrixXd O = M.transpose() * M;

  double MtM_diff = (O - Eigen::MatrixXd::Identity(O.rows(), O.cols())).cwiseAbs().sum();
  if (MtM_diff > 1e-13 * O.rows() * O.cols()) {
    cout << "diff: " << MtM_diff << "\n";
    O = (O.array().cwiseAbs() > 1e-15).select(O, Eigen::ArrayXXd::Zero(O.rows(), O.cols()));
    cout << O << endl;
  }

  cout << "---------- Test: P->H->N->H->P  ----------"
       << "\n";
  fname = "coefficients.h5";
  vec_t cp(N);  // polar coefficients
  if (!boost::filesystem::exists(fname)) {
    cp.setOnes();
  } else {
    hid_t h5_init = H5Fopen(fname.c_str(), H5F_ACC_RDONLY, H5P_DEFAULT);
    eigen2hdf::load(h5_init, "coeffs", cp);
    H5Fclose(h5_init);
  }

  assert(polar_basis.n_dofs() == cp.size());

  // Transform Polar to Hermite basis
  vec_t ch = cp;
  P2H.to_hermite(ch, cp);

  // Transform Hermite to Nodal basis
  Eigen::MatrixXd cn;
  h2n.to_nodal(cn, ch);
  // Transform Nodal to Hermite basis
  h2n.to_hermite(ch.data(), cn);
  // Tranform Hermite to Polar basis
  vec_t cp2(N);
  P2H.to_polar(cp2, ch);

  /* hid_t h5f = H5Fcreate("out.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT); */
  /* eigen2hdf::save(h5f, "coeffs_nodal", cn); */
  /* eigen2hdf::save(h5f, "coeffs_hermite", ch); */
  /* eigen2hdf::save(h5f, "coeffs", cp2); */
  /* H5Fclose(h5f); */

  vec_t diff = (cp2 - cp).rowwise().squaredNorm();
  if (diff.sum() > 1e-10) {
    cout << "diff\n";
    cout << diff << endl;
  } else {
    cout << "sum(diff) " << diff.sum() << endl;
  }

  // int i,j;
  // cout << "max diff: " << diff.maxCoeff(&i, &j) << " at " << i << ", " << j << endl;

  return 0;
}


// This is C++ code for discretizing AR(1) process by using Rouwenhorst method (1995)
// written on 2017/10/17
// z' = rho*z+e e~N(0,(sig_e)^2)
//
// INPUTS (through Terminal)
// N:     Number of grids
// rho:   Coeffecient of AR(1) process z'=mew+rho*z+e, e~N(0,sigmasq)
// sig_e: Std of innovation, e, in AR(1) process
//
// OUTPUT
// vZ:    grids
// mPI:   transition prob. matrix

#include <iostream>
#include <iomanip>
#include <cmath>
#include <random>
#include <armadillo>  // YOU SHOULD INSTALL "armadillo" BEFORE USING THIS FILE

//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//                          MAIN PART
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////

int main(int argc, char* argv[])
{
  ///////////////////////////////////////////////////////////
  // 1. Basic Parameters
  ///////////////////////////////////////////////////////////

  // Ex. Standard quarterly calibration for pruductivity
  // process of the US economy

  int     N = 5;          // Number of grids, N
  double  rho = 0.95;     // AR(1) coefficient
  double  sig_e = 0.007;  // std of innovation

  ///////////////////////////////////////////////////////////
  // 2. Generating grids && Parameterization
  ///////////////////////////////////////////////////////////

  double  sig_z = sqrt(pow(sig_e,2)/(1-pow(rho,2)));  // std of AR(1) process r.v.
  double  z_max = sig_z*sqrt(N-1);                    // Max grid
  double  z_min = -z_max;                             // Min grid
  double  p = (1+rho)/2, q = p;                       // prob. parameters

  arma::vec vZ = arma::linspace<arma::vec>(z_min,z_max,N); // Grid vector 1*N

  ////////////////////////////////////////////////////////////
  // 3. Transition matrix
  ////////////////////////////////////////////////////////////

  arma::mat mPI_old = {{p,1-p},{1-q,q}}; // Initial PI

  // for recursive computation
  arma::mat mPI_new(3,3);
  arma::mat mPI_new1(3,3);
  arma::mat mPI_new2(3,3);
  arma::mat mPI_new3(3,3);
  arma::mat mPI_new4(3,3);

  for (int i=3;i<N+1; i++)
  {

    mPI_new.zeros(i,i);
    mPI_new1.zeros(i,i);
    mPI_new2.zeros(i,i);
    mPI_new3.zeros(i,i);
    mPI_new4.zeros(i,i);

    mPI_new1.submat(0,0,i-2,i-2) = mPI_old;
    mPI_new2.submat(0,1,i-2,i-1) = mPI_old;
    mPI_new3.submat(1,0,i-1,i-2) = mPI_old;
    mPI_new4.submat(1,1,i-1,i-1) = mPI_old;

    mPI_new = p*mPI_new1 + (1-p)*mPI_new2 + (1-q)*mPI_new3 + q*mPI_new4;
    mPI_new.submat(1,0,i-2,i-1) = 0.5 * mPI_new.submat(1,0,i-2,i-1); // divide by 2

    mPI_old.resize(i,i);
    mPI_old = mPI_new;

  }

  ////////////////////////////////////////////////////////////
  // 4. Display result with 4 decimals
  ////////////////////////////////////////////////////////////

  std::cout << "Grids are\n";
  for (int i=0; i<N; i++)
  {
    std::cout << std::fixed << std::setprecision(4) << vZ(i) << " ";
  }

  std::cout << "\n \nTransition Prob Matrix is \n";
  for (int i = 0; i < N; i++)
  {
    for (int j = 0; j < N; j++)
    {
      if (j == N-1)
      {
        std::cout << std::fixed << std::setprecision(4) << mPI_new(i,j) << '\n';
      }
      else
      {
        std::cout << std::fixed << std::setprecision(4) << mPI_new(i,j) << ' ';
      }
    }
  }
  return 0;
}


#include <iostream>
#include <string>
#include <armadillo>
#include <time.h>
#include "ga.h"
#include "simpleNN.h"

using namespace std;
using namespace arma;

//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------
vec sigmoid(vec inp){
    return 1/(1 + exp(-inp));
}
//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------
vec myTanh(vec inp){
    return tanh(inp);
}
//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------
rowvec sigmoidR(rowvec inp){
    return 1/(1 + exp(-inp));
}
//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------
rowvec myTanhR(rowvec inp){
    return tanh(inp);
}
//------------------------------------------------------------------------------------------------------------------------
mat simpleNNmat(mat X, vec inp, mat inpWeights , cube weights, mat outWeights ,vec inpBiases, mat biases, vec outBiases){

    vec buffVec;
    buffVec = sigmoid(inpWeights*inp + inpBiases);

    int largeI = 0;

    mat Y(X.n_rows, outBiases.n_elem);

// Adding Data from values from x to each type of weights and biases.
    for(int i = 0; i < X.n_rows; i++){

        for(int dummyX = 0; dummyX < inpWeights.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < inpWeights.n_cols; dummyY++){
                inpWeights.at(dummyX,dummyY) = X.at(i,largeI);
                largeI++;
            }
        }

        for(int dummyX = 0; dummyX < weights.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < weights.n_cols; dummyY++){
                for(int dummyZ = 0; dummyZ < weights.n_slices; dummyZ++){
                    weights.at(dummyX,dummyY,dummyZ) = X.at(i,largeI);
                    largeI++;
                }
            }
        }

        for(int dummyX = 0; dummyX < outWeights.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < outWeights.n_cols; dummyY++){
                outWeights.at(dummyX,dummyY) = X.at(i,largeI);
                largeI++;
            }
        }

        for(int dummyX = 0; dummyX < inpBiases.n_rows; dummyX++){
            inpBiases.at(dummyX) = X.at(i,largeI);
            largeI++;
        }

        for(int dummyX = 0; dummyX < biases.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < biases.n_cols; dummyY++){
                biases.at(dummyX,dummyY) = X.at(i,largeI);
                largeI++;
            }
        }

        for(int dummyX = 0; dummyX < outBiases.n_rows; dummyX++){
            outBiases.at(dummyX) = X.at(i,largeI);
            largeI++;
        }

// Feed Forward into hidden.
        buffVec = myTanh(inpWeights*inp + inpBiases);
// Feed Forward each hidden Layer.
        for(int l = 0; l < weights.n_slices; l++){
            buffVec = myTanh(weights.slice(l)*buffVec + biases.col(l));
          }
// Feed Forward each hidden Layer.
          Y.row(i) = sigmoidR((outWeights*buffVec).t() + outBiases.t());
   largeI = 0;
   }

      return Y;
}
//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------
vec simpleNNvec(vec x, vec inp, mat inpWeights , cube weights, mat outWeights ,vec inpBiases, mat biases, vec outBiases){

    vec buffVec;
    buffVec = sigmoid(inpWeights*inp + inpBiases);

    int largeI = 0;

    vec y(outBiases.n_elem);

// Adding Data from values from x to each type of weights and biases.
        for(int dummyX = 0; dummyX < inpWeights.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < inpWeights.n_cols; dummyY++){
                inpWeights.at(dummyX,dummyY) = x.at(largeI);
                largeI++;
            }
        }

        for(int dummyX = 0; dummyX < weights.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < weights.n_cols; dummyY++){
                for(int dummyZ = 0; dummyZ < weights.n_slices; dummyZ++){

                    weights.at(dummyX,dummyY,dummyZ) = x.at(largeI);
                    largeI++;
                }
            }
        }

        for(int dummyX = 0; dummyX < outWeights.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < outWeights.n_cols; dummyY++){
                outWeights.at(dummyX,dummyY) = x.at(largeI);
                largeI++;
            }
        }

        for(int dummyX = 0; dummyX < inpBiases.n_rows; dummyX++){
            inpBiases.at(dummyX) = x.at(largeI);
            largeI++;
        }

        for(int dummyX = 0; dummyX < biases.n_rows; dummyX++){
            for(int dummyY = 0; dummyY < biases.n_cols; dummyY++){
                biases.at(dummyX,dummyY) = x.at(largeI);
                largeI++;
            }
        }

        for(int dummyX = 0; dummyX < outBiases.n_rows; dummyX++){
            outBiases.at(dummyX) = x.at(largeI);
            largeI++;
        }

// Feed Forward into hidden.
        buffVec = myTanh(inpWeights*inp + inpBiases);
// Feed Forward each hidden Layer.
        for(int l = 0; l < weights.n_slices; l++){
            buffVec = myTanh(weights.slice(l)*buffVec + biases.col(l));
          }
// Feed Forward  final Layer.
          y = sigmoid((outWeights*buffVec)  + outBiases);
      return y;
}
//------------------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------


// system includes ------------------------------------------------------------
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/MPRealSupport>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <iomanip>
#include <iostream>
//#include <quadmath.h>

// own includes ---------------------------------------------------------------
#include "base/numbers.hpp"
#include "maxwell_quadrature.hpp"
#include "spectral/mpfr/import_std_math.hpp"

namespace mp = boost::multiprecision;  // Reduce the typing a bit later...

using namespace mpfr;

typedef mp::mpfr_float_backend<100000> mfloat_t;
typedef mp::number<mfloat_t> mpfr_float_t;

using namespace std;

const mpfr_float_t PI = boost::math::constants::pi<mpfr_float_t>();
const mpfr_float_t rPI = boost::math::constants::root_pi<mpfr_float_t>();

template <typename T>
class Beta
{
 public:
  Beta(int p = 1)
      : p_(p)
      , beta_n(1)
  {
    beta_n[0] = 0;
    navail = 0;
  }

  template <typename A>
  const T& get(unsigned int n, A& alpha);

 private:
  int p_;
  std::vector<T> beta_n;
  unsigned int navail;
};

// ----------------------------------------------------------------------
template <typename T>
template <typename A>
const T& Beta<T>::get(unsigned int n, A& alpha)
{
  if (n < beta_n.size())
    return beta_n[n];
  else {
    // make sure all previous n's exist
    for (unsigned int i = navail + 1; i < n; ++i) {
      this->get(i, alpha);
    }
    // compute
    T am = alpha.get(n - 1, *this);
    T an = alpha.get(n, *this);
    T bm = this->get(n - 1, alpha);
    T denom = 2 + 4 * am * (am + an) + 4 * bm;
    T nom = n * (n + p_);
    T res = nom / denom;
    beta_n.push_back(res);
    navail++;
    assert(navail == n);
    assert(beta_n[n] == res);
    return beta_n[n];
  }
}

// ----------------------------------------------------------------------
template <typename T>
class Alpha
{
 public:
  Alpha(int p = 1)
      : p_(p)
      , alpha_n(2)
  {
    mpfr_float_t one(1);
    mpfr_float_t two(2);
    mpfr_float_t three(3);
    mpfr_float_t four(4);
    mpfr_float_t eight(8);

    if (p == 0) {
      alpha_n[0] = one / rPI;
      alpha_n[1] = two / rPI / (PI - two);
    } else if (p == 1) {
      alpha_n[0] = rPI / two;
      alpha_n[1] = rPI * (PI - two) / two / (four - PI);
    } else if (p == 2) {
      alpha_n[0] = two / rPI;
      alpha_n[1] = four * (four - PI) / rPI / (three * PI - eight);
    } else {
      throw std::runtime_error("invalid p");
    }

    navail = 1;
  }
  template <typename B>
  const T& get(unsigned int n, B& beta);

 private:
  int p_;
  std::vector<T> alpha_n;
  unsigned int navail;
};

// ----------------------------------------------------------------------
template <typename T>
template <typename B>
const T& Alpha<T>::get(unsigned int n, B& beta)
{
  if (n < alpha_n.size())
    return alpha_n[n];
  else {
    // make sure all previous n's exist
    for (unsigned int i = navail + 1; i < n; ++i) {
      this->get(i, beta);
    }
    // use recursion formula (15)
    // compute
    T sum = 0;
    T am = this->get(n - 1, beta);
    T am2 = am * am;
    for (unsigned int k = 0; k < n; ++k) {
      sum += this->get(k, beta);
    }
    sum /= (2 * n - 1 + p_ - 2 * am2 - 2 * beta.get(n - 1, *this));
    sum -= am;
    alpha_n.push_back(sum);
    navail++;
    assert(navail == n);
    assert(alpha_n[n] == sum);
    return alpha_n[n];
  }
}

void MaxwellQuadrature::init(int ndigits)
{
  // recurrence relation using mpfr_float_t
  typedef Alpha<mpfr_float_t> alpha_t;
  typedef Beta<mpfr_float_t> beta_t;

  alpha_t alpha(p);
  beta_t beta(p);

  // eigen with mpreal
  mpreal::set_default_prec(ndigits);
  typedef mpreal mfloat_t;
  //  typedef double mfloat_t;
  typedef Eigen::Matrix<mfloat_t, Eigen::Dynamic, Eigen::Dynamic> MatrixXmp;
  MatrixXmp A(N, N);
  A.fill(0);
  for (int i = 0; i < N; ++i) {
    A(i, i) = mfloat_t(alpha.get(i, beta).backend().data()).setPrecision(ndigits);
    const auto sqrt_beta1 = ::math::sqrt(beta.get(i + 1, alpha));
    const auto sqrt_beta0 = ::math::sqrt(beta.get(i, alpha));

    if (i + 1 < N) A(i, i + 1) = mfloat_t(sqrt_beta1.backend().data()).setPrecision(ndigits);
    if (i > 0) A(i, i - 1) = mfloat_t(sqrt_beta0.backend().data()).setPrecision(ndigits);
  }

  Eigen::SelfAdjointEigenSolver<MatrixXmp> eigensolver;
  eigensolver.compute(A, Eigen::ComputeEigenvectors);

  // compute weights
  const auto V = eigensolver.eigenvectors();
  const auto w = eigensolver.eigenvalues();

  if (V.rows() == 0) {
    cerr << "abort";
    exit(-1);
  }

  const double pi = boltzmann::numbers::PI;
  std::array<double, 3> mp = {std::sqrt(pi) / 2, 0.5, std::sqrt(pi) / 4};

  std::vector<double> weights(N);
  for (int i = 0; i < N; ++i) {
    // eigenvectors are normalized by eigen
    auto v = V(0, i);
    weights[i] = mp[p] * (v * v).toDouble();
  }

  std::vector<double> points(N);
  for (int i = 0; i < N; ++i) {
    points[i] = w(i).toDouble();
  }

  std::vector<std::pair<double, double> > pairs(N);
  for (int i = 0; i < N; ++i) {
    pairs[i] = std::make_pair(points[i], weights[i]);
  }

  std::sort(pairs.begin(), pairs.end());

  for (int i = 0; i < N; ++i) {
    pts_[i] = pairs[i].first;
    wts_[i] = pairs[i].second;
  }
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <sophus/se3.hpp>
#include <chrono>

using namespace std;
using namespace cv;

void find_feature_matches(
  const Mat &img_1, const Mat &img_2,
  std::vector<KeyPoint> &keypoints_1,
  std::vector<KeyPoint> &keypoints_2,
  std::vector<DMatch> &matches);

// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &p, const Mat &K);

// BA by g2o
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

void bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose
);

// BA by gauss-newton
void bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose
);

int main(int argc, char **argv) {
  if (argc != 5) {
    cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2" << endl;
    return 1;
  }
  //-- 读取图像
  Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
  assert(img_1.data && img_2.data && "Can not load images!");

  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
  cout << "一共找到了" << matches.size() << "组匹配点" << endl;

  // 建立3D点
  Mat d1 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);       // 深度图为16位无符号数，单通道图像
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  vector<Point3f> pts_3d;
  vector<Point2f> pts_2d;
  for (DMatch m:matches) {
    ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
    if (d == 0)   // bad depth
      continue;
    float dd = d / 5000.0;
    Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
    pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
    pts_2d.push_back(keypoints_2[m.trainIdx].pt);
  }

  cout << "3d-2d pairs: " << pts_3d.size() << endl;

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  Mat r, t;
  solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
  Mat R;
  cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵
  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 pnp in opencv cost time: " << time_used.count() << " seconds." << endl;

  cout << "R=" << endl << R << endl;
  cout << "t=" << endl << t << endl;

  VecVector3d pts_3d_eigen;
  VecVector2d pts_2d_eigen;
  for (size_t i = 0; i < pts_3d.size(); ++i) {
    pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
    pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
  }

  cout << "calling bundle adjustment by gauss newton" << endl;
  Sophus::SE3d pose_gn;
  t1 = chrono::steady_clock::now();
  bundleAdjustmentGaussNewton(pts_3d_eigen, pts_2d_eigen, K, pose_gn);
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp by gauss newton cost time: " << time_used.count() << " seconds." << endl;

  cout << "calling bundle adjustment by g2o" << endl;
  Sophus::SE3d pose_g2o;
  t1 = chrono::steady_clock::now();
  bundleAdjustmentG2O(pts_3d_eigen, pts_2d_eigen, K, pose_g2o);
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp by g2o cost time: " << time_used.count() << " seconds." << endl;
  return 0;
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  //-- 初始化
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  //-- 第一步:检测 Oriented FAST 角点位置
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  //-- 第二步:根据角点位置计算 BRIEF 描述子
  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
  vector<DMatch> match;
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  //-- 第四步:匹配点对筛选
  double min_dist = 10000, max_dist = 0;

  //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }

  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);

  //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

Point2d pixel2cam(const Point2d &p, const Mat &K) {
  return Point2d
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

void bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose) {
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  const int iterations = 10;
  double cost = 0, lastCost = 0;
  double fx = K.at<double>(0, 0);
  double fy = K.at<double>(1, 1);
  double cx = K.at<double>(0, 2);
  double cy = K.at<double>(1, 2);

  for (int iter = 0; iter < iterations; iter++) {
    Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
    Vector6d b = Vector6d::Zero();

    cost = 0;
    // compute cost
    for (int i = 0; i < points_3d.size(); i++) {
      Eigen::Vector3d pc = pose * points_3d[i];
      double inv_z = 1.0 / pc[2];
      double inv_z2 = inv_z * inv_z;
      Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);

      Eigen::Vector2d e = points_2d[i] - proj;

      cost += e.squaredNorm();
      Eigen::Matrix<double, 2, 6> J;
      J << -fx * inv_z,
        0,
        fx * pc[0] * inv_z2,
        fx * pc[0] * pc[1] * inv_z2,
        -fx - fx * pc[0] * pc[0] * inv_z2,
        fx * pc[1] * inv_z,
        0,
        -fy * inv_z,
        fy * pc[1] * inv_z2,
        fy + fy * pc[1] * pc[1] * inv_z2,
        -fy * pc[0] * pc[1] * inv_z2,
        -fy * pc[0] * inv_z;

      H += J.transpose() * J;
      b += -J.transpose() * e;
    }

    Vector6d dx;
    dx = H.ldlt().solve(b);

    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }

    if (iter > 0 && cost >= lastCost) {
      // cost increase, update is not good
      cout << "cost: " << cost << ", last cost: " << lastCost << endl;
      break;
    }

    // update your estimation
    pose = Sophus::SE3d::exp(dx) * pose;
    lastCost = cost;

    cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;
    if (dx.norm() < 1e-6) {
      // converge
      break;
    }
  }

  cout << "pose by g-n: \n" << pose.matrix() << endl;
}

/// vertex and edges used in g2o ba
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  virtual void setToOriginImpl() override {
    _estimate = Sophus::SE3d();
  }

  /// left multiplication on SE3
  virtual void oplusImpl(const double *update) override {
    Eigen::Matrix<double, 6, 1> update_eigen;
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
  }

  virtual bool read(istream &in) override {}

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

class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}

  virtual void computeError() override {
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    Sophus::SE3d T = v->estimate();
    Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
    pos_pixel /= pos_pixel[2];
    _error = _measurement - pos_pixel.head<2>();
  }

  virtual void linearizeOplus() override {
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
    Sophus::SE3d T = v->estimate();
    Eigen::Vector3d pos_cam = T * _pos3d;
    double fx = _K(0, 0);
    double fy = _K(1, 1);
    double cx = _K(0, 2);
    double cy = _K(1, 2);
    double X = pos_cam[0];
    double Y = pos_cam[1];
    double Z = pos_cam[2];
    double Z2 = Z * Z;
    _jacobianOplusXi
      << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
      0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
  }

  virtual bool read(istream &in) override {}

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

private:
  Eigen::Vector3d _pos3d;
  Eigen::Matrix3d _K;
};

void bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose) {

  // 构建图优化，先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  // pose is 6, landmark is 3
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

  // vertex
  VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose
  vertex_pose->setId(0);
  vertex_pose->setEstimate(Sophus::SE3d());
  optimizer.addVertex(vertex_pose);

  // K
  Eigen::Matrix3d K_eigen;
  K_eigen <<
          K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
    K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
    K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);

  // edges
  int index = 1;
  for (size_t i = 0; i < points_2d.size(); ++i) {
    auto p2d = points_2d[i];
    auto p3d = points_3d[i];
    EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);
    edge->setId(index);
    edge->setVertex(0, vertex_pose);
    edge->setMeasurement(p2d);
    edge->setInformation(Eigen::Matrix2d::Identity());
    optimizer.addEdge(edge);
    index++;
  }

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.setVerbose(true);
  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 << "optimization costs time: " << time_used.count() << " seconds." << endl;
  cout << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;
  pose = vertex_pose->estimate();
}