#include <cassert>
#include <cmath>
#include <numeric> // accumulate
#include <ostream>
#include <utility>

#include <boost/variant/static_visitor.hpp>

#include "Geometry.h"

using namespace std;

namespace geometry {

Scalar size(Vector const &vec) { return std::hypot(vec.x, vec.y); }

// Dot product
Scalar dot(Vector const &v1, Vector const &v2) {
  return v1.x * v2.x + v1.y * v2.y;
}

Scalar cos(Vector const &a, Vector const &b) {
  return dot(a, b) / (size(a) * size(b));
}

Vector projection(Vector const &v, Vector const &s) {
  return ((v * s) / (s * s)) * s;
}

Scalar projection(Point const &p, Point const &A, Point const &B) {
  // http://stackoverflow.com/a/15187473/6209703
  Vector const direction = B - A;
  Scalar const t = ((p.x - A.x) * (B.x - A.x) + (p.y - A.y) * (B.y - A.y)) /
                   (direction.x * direction.x + direction.y * direction.y);
  // t=[(Cx-Ax)(Bx-Ax)+(Cy-Ay)(By-Ay)]/[(Bx-Ax)^2+(By-Ay)^2]
  return t;
}

Vector unit(Vector const &v) { return v / size(v); }

Scalar distance(Point const &p1, Point const &p2) {
  auto d = p2 - p1;
  return Scalar(std::hypot(d.x, d.y));
}

std::ostream &operator<<(std::ostream &o, Point const &point) {
  o << "[" << point.x << "," << point.y << "]";
  return o;
}

std::ostream &operator<<(std::ostream &o, Vector const &point) {
  o << "(" << point.x << "," << point.y << ")";
  return o;
}

std::ostream &operator<<(std::ostream &o, CircleShape const &shape) {
  o << "{ radius: " << shape.radius << " }";
  return o;
}

std::ostream &operator<<(std::ostream &o, Circle const &c) {
  o << "{ circ at " << c.center << ", r: " << c.radius << " }";
  return o;
}

PolygonalShape &operator+=(PolygonalShape &shape, Vector const &vec) {
  for (auto &v : shape)
    v += vec;
  return shape;
}

PolygonalShape &operator*=(PolygonalShape &shape, Scalar scale) {
  for (auto &v : shape)
    v *= scale;
  return shape;
}

PolygonalShape &operator-=(PolygonalShape &shape, Vector const &vec) {
  for (auto &v : shape)
    v -= vec;
  return shape;
}

std::ostream &operator<<(std::ostream &o, PolygonalShape const &shape) {
  o << "[";
  for (auto const &vec : shape)
    o << " " << vec;
  o << " ]";
  return o;
}

std::ostream &operator<<(std::ostream &o, Polygon const &p) {
  o << "[";
  for (auto const &pt : p)
    o << " " << pt;
  o << " ]";
  return o;
}

std::ostream &operator<<(std::ostream &o, RectangularArea const &area) {
  o << "{ area from " << area.topLeft() << " to " << area.bottomRight() << " }";
  return o;
}

bool in_triangle(Point const &p, Point const &p1, Point const &p2,
                 Point const &p3) {
  auto alpha = ((p2.y - p3.y) * (p.x - p3.x) + (p3.x - p2.x) * (p.y - p3.y)) /
               ((p2.y - p3.y) * (p1.x - p3.x) + (p3.x - p2.x) * (p1.y - p3.y));
  auto beta = ((p3.y - p1.y) * (p.x - p3.x) + (p1.x - p3.x) * (p.y - p3.y)) /
              ((p2.y - p3.y) * (p1.x - p3.x) + (p3.x - p2.x) * (p1.y - p3.y));
  auto gamma = 1.0f - alpha - beta;

  return alpha >= 0 && beta >= 0 && gamma >= 0;
}

Vector perpendicularDistance(Point const &p, Point const &A, Point const &B) {
  Scalar const t = projection(p, A, B);
  Point const projectedCenter = A + t * (B - A);
  return p - projectedCenter;
}

bool onLeftOf(Point const &point, Point const &A, Point const &B) {
  return ((A.x - point.x) * (B.y - point.y) -
          (A.y - point.y) * (B.y - point.x)) > 0;
}

// http://stackoverflow.com/a/21096006/6209703
bool in_polygon(Point const &point, Polygon const &polygon_old) {
  Polygon polygon = polygon_old;

  assert(polygon.size() >= 3);

  while (polygon.size() > 3) {
    // TODO detect automatically polygons rotation/order of verteces
    Polygon newPolygon;
    if (onLeftOf(point, polygon[0], polygon[polygon.size() / 2])) {
      for (size_t i = 0; i <= polygon.size() / 2; i++)
        newPolygon.push_back(polygon[i]);
    } else {
      for (size_t i = polygon.size() / 2; i < polygon.size(); i++)
        newPolygon.push_back(polygon[i]);
      newPolygon.push_back(polygon[0]);
    }

    assert(newPolygon.size() < polygon.size());
    using std::swap;
    swap(newPolygon, polygon);
  }

  return in_triangle(point, polygon[0], polygon[1], polygon[2]);
}

bool in_polygon(Circle const &circle, Polygon const &polygon) {
  for (Point const &p : polygon) {
    Vector const direction = unit(p - circle.center);
    Point const outer = circle.center + circle.radius * direction;
    if (!in_polygon(outer, polygon))
      return false;
  }
  return true;
}

bool in_polygon(Polygon const &polygon1, Polygon const &polygon2) {
  for (Point const &p : polygon1) {
    if (!in_polygon(p, polygon2))
      return false;
  }
  return true;
}

bool in_circle(Point const &point, Circle const &circle) {
  auto vec = point - circle.center;
  return std::hypot(vec.x, vec.y) <= circle.radius;
}

bool in_circle(Circle const & /*circle1*/, Circle const & /*circle2*/) {
  return false;
}

bool in_circle(Polygon const &polygon, Circle const &circle) {
  for (Point const &p : polygon) {
    if (!in_circle(p, circle))
      return false;
  }
  return true;
}

bool in(Point const &point, RectangularArea const &area) {
  if (point.x <= area.topLeft().x)
    return false;
  if (point.x >= area.bottomRight().x)
    return false;
  if (point.y <= area.topLeft().y)
    return false;
  if (point.y >= area.bottomRight().y)
    return false;
  return true;
}

bool in(Circle const &circle, RectangularArea const &area) {
  return in(circle.center, CircleShape{circle.radius}, area);
}

bool in(Point const &point, CircleShape const &shape,
        RectangularArea const &area) {
  if (point.x - shape.radius <= area.topLeft().x)
    return false;
  if (point.x + shape.radius >= area.bottomRight().x)
    return false;
  if (point.y - shape.radius <= area.topLeft().y)
    return false;
  if (point.y + shape.radius >= area.bottomRight().y)
    return false;
  return true;
}

bool in(Polygon const &polygon, RectangularArea const &area) {
  // TODO use accumulate with ranges
  for (auto const &p : polygon) {
    if (!in(p, area))
      return false;
  }

  return true;
}

namespace {

template <typename Object1>
class InVisitor1 : public boost::static_visitor<bool> {
  Object1 const &object1;

public:
  explicit InVisitor1(Object1 const &object1) : object1(object1) {}

  bool operator()(Polygon const &object2) {
    return in_polygon(object1, object2);
  }

  bool operator()(Circle const &object2) { return in_circle(object1, object2); }
};

class InVisitor2 : public boost::static_visitor<bool> {
  Object2D const &object2;

public:
  explicit InVisitor2(Object2D const &object2) : object2(object2) {}

  bool operator()(Polygon const &object1) {
    InVisitor1<Polygon> visitor{object1};
    return boost::apply_visitor(visitor, object2);
  }

  bool operator()(Circle const &object1) {
    InVisitor1<Circle> visitor{object1};
    return boost::apply_visitor(visitor, object2);
  }
};

} // anonymous namespace

bool in(Object2D const &object1, Object2D const &object2) {
  InVisitor2 visitor{object2};
  return boost::apply_visitor(visitor, object1);
}

bool in(Object2D const &object, RectangularArea const &area) {
  class Visitor : public boost::static_visitor<bool> {
  private:
    RectangularArea const &area;

  public:
    Visitor(RectangularArea const &area) : area(area) {}

    bool operator()(Polygon const &polygon) { return in(polygon, area); }

    bool operator()(Circle const &circle) { return in(circle, area); }
  };

  Visitor visitor{area};
  return boost::apply_visitor(visitor, object);
}

Scalar deg2rad(Scalar degrees) { return degrees * 3.141592653589793 / 180.0; }

Vector rotate(Vector const &vec, Angle angle) {
  auto const dangle = deg2rad(angle);
  auto const cs = Scalar(std::cos(dangle));
  auto const sn = Scalar(std::sin(dangle));

  return vec.x * Vector{cs, sn} + vec.y * Vector{Scalar(-sn), cs};
}

Point rotate_around(Point const &center, Point const &point, Angle angle) {
  return center + rotate(point - center, angle);
}

} // namespace geometry


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

   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.
*/

// Free energy, energy, and specific heat of square lattice Ising model

#pragma once

#include <cmath>
#include <stdexcept>
#include <boost/math/constants/constants.hpp>
#include <boost/math/differentiation/autodiff.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>
#include <standards/simpson.hpp>
#include <lattice/graph.hpp>
#include "common.hpp"

namespace ising {
namespace free_energy {
namespace square {

namespace {

template<typename T, typename FVAR>
struct functor {
  typedef T real_t;
  typedef FVAR fvar_t;
  functor(real_t Jx, real_t Jy, fvar_t beta) {
    using std::cosh; using std::sinh;
    c_ = 1 / (2 * boost::math::constants::pi<real_t>());
    chab_ = cosh(2 * beta * Jx) * cosh(2 * beta * Jy);
    k_ = 1 / (sinh(2 * beta * Jx) * sinh(2 * beta * Jy));
  }
  fvar_t operator()(real_t t) const {
    using std::cos; using std::log; using std::sqrt;
    return c_ * log(chab_ + sqrt(1 + k_ * k_ - 2 * k_ * cos(2 * t)) / k_);
  }
  real_t c_;
  fvar_t chab_, k_;
};

template<typename T, std::size_t Order>
struct functor<T, boost::math::differentiation::detail::fvar<T, Order>> {
  typedef T real_t;
  typedef boost::math::differentiation::detail::fvar<real_t, Order> fvar_t;
  functor(real_t Jx, real_t Jy, fvar_t beta) {
    using std::cosh; using std::sinh;
    c_ = 1 / (2 * boost::math::constants::pi<real_t>());
    chab_ = cosh(2 * beta * Jx) * cosh(2 * beta * Jy);
    k_ = 1 / (sinh(2 * beta * Jx) * sinh(2 * beta * Jy));
  }
  fvar_t operator()(real_t t) const {
    using std::abs; using std::cos; using std::log; using std::sqrt;
    auto r = sqrt(1 + k_ * k_ - 2 * k_ * cos(2 * t)) / k_;
    if (abs(r.derivative(1)) > std::numeric_limits<real_t>::max()) {
      auto x = boost::math::differentiation::make_fvar<real_t, 2>(real_t(0));
      r = fvar_t(r.derivative(0)) + std::numeric_limits<real_t>::max() * x * x;
    }
    return c_ * log(chab_ + r);
  }
  real_t c_;
  fvar_t chab_, k_;
};

template<typename T, typename FVAR>
functor<T, FVAR> func(T Jx, T Jy, FVAR beta) { return functor<T, FVAR>(Jx, Jy, beta); }

}

template<typename T, typename U>
inline U infinite(T Jx, T Jy, U beta) {
  const unsigned long max_n = 1 << 16;
  typedef T real_t;
  if (Jx <= 0 || Jy <= 0)
    throw(std::invalid_argument("Jx and Jy should be positive"));
  if (beta <= 0)
    throw(std::invalid_argument("beta should be positive"));
  real_t pi = boost::math::constants::pi<real_t>();
  boost::math::quadrature::tanh_sinh<real_t> integrator;
  auto logZ = log(real_t(2)) / 2 + 2 * integrator.integrate(func(Jx, Jy, beta), 0, pi / 2);
  return - logZ / beta;
}

template<typename I, typename T, typename U>
inline U finite(I Lx, I Ly, T Jx, T Jy, U beta) {
  typedef I int_t;
  typedef T real_t;
  typedef U value_t;
  if (Lx <= 0 || Ly <= 0)
    throw(std::invalid_argument("Lx and Ly should be positive"));
  if (Jx <= 0 || Jy <= 0)
    throw(std::invalid_argument("Jx and Jy should be positive"));
  real_t pi = boost::math::constants::pi<real_t>();
  auto a = beta * Jx;
  auto b = beta * Jy;
  value_t lp0(0), lp1(0), lp2(0), lp3(0);
  auto gamma0 = log((1 + cosh(2*a)) / sinh(2*a)) - 2*b;
  for (int_t k = 0; k < 2 * Ly; ++k) {
    auto cosh_g = (cosh(2*a) * cosh(2*b) - cos(pi * k / Ly) * sinh(2*b)) / sinh(2*a);
    auto gamma = (k == 0) ? abs(gamma0) : (log(cosh_g + sqrt(cosh_g * cosh_g - 1)));
    if ((k & 1) == 1) {
      lp0 += (Lx * gamma/2) + log(1 + exp(-(Lx * gamma)));
      lp1 += (Lx * gamma/2) + log(1 - exp(-(Lx * gamma)));
    } else {
      lp2 += (Lx * gamma/2) + log(1 + exp(-(Lx * gamma)));
      lp3 += (Lx * gamma/2) + log(1 - exp(-(Lx * gamma)));
    }
  }
  auto logZ = -log(real_t(2)) / (Lx * Ly) + a + real_t(1)/2 * log(1 - exp(-4*a));
  if (gamma0 > 0) {
    logZ += (lp0 + log(1 + exp(lp1-lp0) + exp(lp2-lp0) - exp(lp3-lp0))) / (Lx * Ly);
  } else if (gamma0 < 0) {
    logZ += (lp0 + log(1 + exp(lp1-lp0) + exp(lp2-lp0) + exp(lp3-lp0))) / (Lx * Ly);
  } else {
    logZ += (lp0 + log(1 + exp(lp1-lp0) + exp(lp2-lp0))) / (Lx * Ly);
  }
  return - logZ / beta;
}

template<typename T, typename I>
inline T finite_tc(I Lx, I Ly) {
  typedef I int_t;
  typedef T real_t;
  if (Lx <= 0 || Ly <= 0)
    throw(std::invalid_argument("Lx and Ly should be positive"));
  real_t pi = boost::math::constants::pi<real_t>();
  real_t beta = log(sqrt(real_t(2)) + 1)/2;
  real_t lp0(0), lp1(0);
  for (int_t k = 1; k < 2 * Ly; k += 2) {
    real_t c = (2 - cos(pi * k / Ly));
    real_t cosh_g = c;
    real_t gamma_abs = log(cosh_g + sqrt(cosh_g * cosh_g - 1));
    lp0 += (Lx * gamma_abs/2) + log(1 + exp(-(Lx * gamma_abs)));
    lp1 += (Lx * gamma_abs/2) + log(1 - exp(-(Lx * gamma_abs)));
  }
  real_t logZ = -log(real_t(2)) / (Lx * Ly) + beta + real_t(1)/2 * log(1 - exp(-4*beta))
    + (lp0 + 2 * lp1) / (Lx * Ly)
    + (lp0 + log(1 + 2 * exp(lp1-lp0))) / (Lx * Ly);
  return - logZ / beta;
}

template<typename I, typename T, typename U, typename W>
inline boost::math::differentiation::promote<U, W> finite_count(I Lx, I Ly, T Jx, T Jy, U beta, W h) {
  typedef I int_t;
  typedef T real_t;
  typedef boost::math::differentiation::promote<U, W> value_t;
  if (Lx <= 0 || Ly <= 0)
    throw(std::invalid_argument("Lx and Ly should be positive"));
  if (Jx <= 0 || Jy <= 0)
    throw(std::invalid_argument("Jx and Jy should be positive"));

  auto basis = lattice::basis::simple(2);
  auto unitcell = lattice::unitcell(2);
  unitcell.add_site(lattice::coordinate(0, 0), 0);
  unitcell.add_bond(0, 0, lattice::offset(1, 0), 0);
  unitcell.add_bond(0, 0, lattice::offset(0, 1), 1);
  auto graph = lattice::graph(basis, unitcell, lattice::extent(Lx, Ly));
  if (graph.num_sites() >= 32) throw(std::invalid_argument("too large system size"));

  std::vector<real_t> J(2);
  J[0] = Jx;
  J[1] = Jy;
  real_t gs_energy = -(Jx + Jy) * (Lx * Ly);
  int_t num_states = 1 << graph.num_sites();
  value_t Z(0);
  for (int_t c = 0; c < num_states; ++c) {
    real_t energy = 0;
    for (int_t b = 0; b < graph.num_bonds(); ++b) {
      real_t ci = real_t(2) * ((c >> graph.source(b)) & 1) - 1;
      real_t cj = real_t(2) * ((c >> graph.target(b)) & 1) - 1;
      energy -= J[graph.bond_type(b)] * ci * cj;
    }
    real_t mag = 0;
    for (int_t s = 0; s < graph.num_sites(); ++s) {
      mag += real_t(2) * ((c >> s) & 1) - 1;
    }
    Z += exp((gs_energy - energy - h * mag) * beta);
  }
  return (-log(Z)/beta + gs_energy) / (Lx * Ly);
}

} // end namespace square
} // end namespace free_energy
} // end namespace ising


#include <vector>
#include <armadillo>
#include "ConvectionDiffusionEulerImplicit.h"

using namespace std;

void ConvectionDiffusionEulerImplicit::initialize() {
	// Initial time conditions
	current_time = 0.0; // time 0 = option expiration - more pos = farther in the past
	current_t_index = 0;

	// Proccessing step
	solution.resize(T);
    for (unsigned long t = 0; t < T; t++) { solution[t].resize(N,0.0); }
	x_values.resize(N,0.0);

	// Initialize the solution set at all x values
	double x;
	for (unsigned long n = 0; n < N; n++) {
		x = static_cast<double>(n) * h;
		solution[0][n] = pde->initial_condition(x);
		x_values[n] = x;
	}
}

// Compute PDE's boundary conditions
void ConvectionDiffusionEulerImplicit::calculate_boundary() {
	// double time = current_time - k;
	solution[current_t_index][0] = pde->left_boundary(current_time, x_values, solution[current_t_index-1],k,h);
	solution[current_t_index][N-1] = pde->right_boundary(current_time, x_values, solution[current_t_index-1],k,h);
}

// Apply FEM to solve system of PDEs with given initial and boundary conditions
void ConvectionDiffusionEulerImplicit::calculate_inner_mesh() {
	double right, center, left, source, x, prev_time = current_time - k;
	double sigma = k / (h * h);
	double lambda = k / h;

	// INIT TRANSITION MATRIX
	arma::mat A = arma::zeros<arma::mat>(N-2,N-2);
	for (unsigned long n = 1; n < N-1; n++) {
		x = x_values[n];

		A(n-1,n-1) = 1.0
		+ (2.0 * sigma * pde->diffusion_param(current_time,x))
		- (k * pde->solution_param(current_time,x));

		if (n < N-2) {
			A(n-1,n) =  - (sigma * pde->diffusion_param(current_time,x))
            - (0.5 * lambda * pde->convection_param(current_time,x));
		}
        if (n > 1) {
			A(n-1,n-2) = - (sigma * pde->diffusion_param(current_time,x))
			+ (0.5 * lambda * pde->convection_param(current_time,x));
		}
	}

	// INIT BOUNDARY VECTOR
	arma::vec b = arma::zeros<arma::vec>(N-2);

	left = - (sigma * pde->diffusion_param(current_time,x_values[0])) + (0.5 * lambda * pde->convection_param(current_time,x_values[0]));
    b(0) = left * pde->left_boundary(current_time, x_values,solution[current_t_index-1],k,h);

	right = - (sigma * pde->diffusion_param(current_time,x_values[N-1])) - (0.5 * lambda * pde->convection_param(current_time,x_values[N-1]));
    b(N-3) = right * pde->right_boundary(current_time, x_values,solution[current_t_index-1],k,h);

	// INIT SOURCE VECTOR
	arma::vec S = arma::zeros<arma::vec>(N-2);
	for (unsigned long n = 1; n < N-1; n++) {
		S(n-1) = k * pde->source_param(current_time,x_values[n]);
	}

	// LAST SOLUTION
	arma::vec u = arma::zeros<arma::vec>(N-2);
	for (unsigned long n = 1; n < N-1; n++) {
        u(n-1) = solution[current_t_index-1][n];
	}

	// SOLVE
    arma::vec sol = arma::solve(A, u + b + S);

	// STORE
	for (unsigned long n = 1; n < N-1; n++) {
		solution[current_t_index][n] = sol(n-1);
	}
}

// Define the solve function for a single PDE
void ConvectionDiffusionEulerImplicit::solve() {
	while (current_time < t_bound) {
		current_time += k;
		current_t_index++;
		calculate_boundary();
		calculate_inner_mesh();
	}
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2016.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)
//

// Example program showing a bare metal real-time performance measurement
// of a shift-and-add variation of sqrt(negatable), 32-bit.

#include <cstdint>
#include <limits>

#include <mcal_benchmark.h>
#include <mcal_cpu.h>
#include <mcal_irq.h>

#define BOOST_FIXED_POINT_DISABLE_IOSTREAM
#define BOOST_FIXED_POINT_DISABLE_MULTIPRECISION

#include <boost/fixed_point/fixed_point.hpp>

typedef mcal::benchmark::benchmark_port_type port_type;

typedef boost::fixed_point::negatable<7, -24> fixed_point_type;

extern const fixed_point_type x;
extern       fixed_point_type y;

namespace shift
{
  fixed_point_type sqrt(const fixed_point_type& x);

  // Computes the fixed-point square root using a shift-and-add algorithm.
  // Adapted from sqrt found here: http://codepad.org/CqvPx5u0
  // And this has beem modified from Ken Turkowski's algorithm.
  // Despite the somewhat confusing description of the result in the white paper,
  // the algorithm builds a M.F fixed-point square root.
  // @see http://www.realitypixels.com/turk/computergraphics/FixedSqrt.pdf
  fixed_point_type sqrt(const fixed_point_type& x)
  {
    typedef fixed_point_type::unsigned_small_type local_unsigned_small_type;
    typedef fixed_point_type::nothing             local_nothing;

    constexpr std::uint_fast8_t total_bits      = static_cast<std::uint_fast8_t>(fixed_point_type::all_bits);
    constexpr std::uint_fast8_t fractional_bits = static_cast<std::uint_fast8_t>(fixed_point_type::radix_split);

    local_unsigned_small_type root  = static_cast<local_unsigned_small_type>(0U);                  // Clear the root.
    local_unsigned_small_type remHi = static_cast<local_unsigned_small_type>(0U);                  // Clear the high part of the partial remainder.
    local_unsigned_small_type remLo = static_cast<local_unsigned_small_type>(x.crepresentation()); // Get the argument into the low part of the partial remainder.

    for(std::uint_fast8_t count = (total_bits >> 1) + (fractional_bits >> 1); count > 0U; --count)
    {
      // Get 2 bits of the argument.
      remHi = (remHi << 2) | (remLo >> (total_bits - 2));
      remLo <<= 2;

      // Get ready for the next bit in the root.
      root <<= 1;

      // Test the radical.
      const local_unsigned_small_type testDiv = (root << 1) + 1;

      if(remHi >= testDiv)
      {
        remHi = remHi - testDiv;

        ++root;
      }
    }

    return fixed_point_type(local_nothing(), root);
  }

} // namespace shift

namespace app
{
  namespace benchmark
  {
    void task_init();
    void task_func();
  }
}

void app::benchmark::task_init()
{
  port_type::set_direction_output();
}

void app::benchmark::task_func()
{
  mcal::irq::disable_all();
  port_type::set_pin_high();

  // Use either one of the following two lines (but not both)
  // to benchmark variations of square root algorithms.

  y = shift::sqrt(x);
  //y = boost::fixed_point::sqrt(x);

  port_type::set_pin_low();

  mcal::irq::enable_all();
}

const fixed_point_type x(fixed_point_type(6U) /  10U);
      fixed_point_type y;


#ifndef HYPSYS1D_MODEL_HPP
#define HYPSYS1D_MODEL_HPP

#include <cmath>
#include <memory>

#include <Eigen/Dense>
#include <ancse/config.hpp>

/// Interface for implementing different models,
/// eg. Euler equations, Shallow-water equations
///
/// Add more functions to this interface if needed.
class Model {
  public:
    virtual ~Model() = default;

    virtual Eigen::VectorXd flux(const Eigen::VectorXd &u) const = 0;
    virtual Eigen::VectorXd eigenvalues(const Eigen::VectorXd &u) const = 0;
    virtual Eigen::MatrixXd eigenvectors(const Eigen::VectorXd &u) const = 0;
    virtual double max_eigenvalue(const Eigen::VectorXd &u) const = 0;

    virtual Eigen::VectorXd cons_to_prim(const Eigen::VectorXd &u) const = 0;
    virtual Eigen::VectorXd prim_to_cons(const Eigen::VectorXd &u) const = 0;

    virtual int get_nvars() const = 0;
    virtual std::string get_name() const = 0;
};

class Burgers : public Model {
  public:

    Eigen::VectorXd flux(const Eigen::VectorXd &u) const override
    {
        Eigen::VectorXd f(n_vars);
        f(0) = 0.5*u(0)*u(0);

        return f;
    }

    Eigen::VectorXd eigenvalues(const Eigen::VectorXd &u) const override
    {
        Eigen::VectorXd eigvals(n_vars);
        eigvals(0) = u(0);

        return eigvals;
    }

    Eigen::MatrixXd eigenvectors(const Eigen::VectorXd &) const override
    {
        Eigen::MatrixXd eigvecs(n_vars, n_vars);
        eigvecs (0,0) = 1;

        return eigvecs;
    }

    double max_eigenvalue(const Eigen::VectorXd &u) const override {
        return (eigenvalues(u).cwiseAbs()).maxCoeff();
    }

    Eigen::VectorXd cons_to_prim(const Eigen::VectorXd &u) const override {
        return u;
    }

    Eigen::VectorXd prim_to_cons(const Eigen::VectorXd &u) const override {
        return u;
    }

    int get_nvars() const override
    {
        return n_vars;
    }

    std::string get_name() const override
    {
        return "burgers";
    }

  private:
    int n_vars = 1;
};

/// Euler equations
class Euler : public Model {
  public:

    Eigen::VectorXd flux(const Eigen::VectorXd &u) const override;

    Eigen::VectorXd eigenvalues(const Eigen::VectorXd &u) const override;

    Eigen::MatrixXd eigenvectors(const Eigen::VectorXd &u) const override;

    double max_eigenvalue(const Eigen::VectorXd &u) const override;

    Eigen::VectorXd cons_to_prim(const Eigen::VectorXd &u_cons) const override;

    Eigen::VectorXd prim_to_cons(const Eigen::VectorXd &u_prim) const override;

    void set_gamma(double gamma_)
    {
        gamma = gamma_;
    }

    double get_gamma() const
    {
        return gamma;
    }

    int get_nvars() const override
    {
        return n_vars;
    }

    std::string get_name() const override
    {
        return "euler";
    }

  private:
    int n_vars = 3;
    double gamma = 5./3.;
};

std::shared_ptr<Model> make_model (const nlohmann::json &config);

#endif // HYPSYS1D_MODEL_HPP


#ifndef HELPERS_H
#define HELPERS_H

#include <Eigen/Core>

using Eigen::VectorXd;

//
// Helper functions to fit and evaluate polynomials.
//

// Evaluate a polynomial.
double polyeval(const VectorXd &coeffs, double x) {
  double result = 0.0;
  for (int i = 0; i < coeffs.size(); ++i) {
    result += coeffs[i] * pow(x, i);
  }
  return result;
}

// Fit a polynomial.
// Adapted from:
// https://github.com/JuliaMath/Polynomials.jl/blob/master/src/Polynomials.jl#L676-L716
VectorXd polyfit(const VectorXd &xvals, const VectorXd &yvals, int order) {
  assert(xvals.size() == yvals.size());
  assert(order >= 1 && order <= xvals.size() - 1);

  Eigen::MatrixXd A(xvals.size(), order + 1);

  for (int i = 0; i < xvals.size(); ++i) {
    A(i, 0) = 1.0;
  }

  for (int j = 0; j < xvals.size(); ++j) {
    for (int i = 0; i < order; ++i) {
      A(j, i + 1) = A(j, i) * xvals(j);
    }
  }

  auto Q = A.householderQr();
  auto result = Q.solve(yvals);

  return result;
}

#endif  // HELPERS_H


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

using namespace Eigen;

int main()
{
  MatrixXd m(2,2);
  m(0,0) = 3;
  m(1,0) = 2.5;
  m(0,1) = -1;
  m(1,1) = m(1,0) + m(0,1);
  std::cout << "Here is the matrix m:\n" << m << std::endl;
  VectorXd v(2);
  v(0) = 4;
  v(1) = v(0) - 1;
  std::cout << "Here is the vector v:\n" << v << std::endl;

}


#include <sm/kinematics/quaternion_algebra.hpp>
#include <sm/kinematics/rotations.hpp>
#include <sm/assert_macros.hpp>
#include <Eigen/Geometry>
#include <limits>
#include <cmath>

namespace sm { namespace kinematics {
        template <typename Scalar_ = double>
        inline bool isLessThenEpsilons4thRoot(Scalar_ x){
          static const Scalar_ epsilon4thRoot = pow(std::numeric_limits<Scalar_>::epsilon(), 1.0/4.0);
          return x < epsilon4thRoot;
        }

        // quaternion rotation.
        Eigen::Vector4d r2quat(Eigen::Matrix3d const & R){

            const double & c1 = R(0,0);
            const double & c2 = R(1,0);
            const double & c3 = R(2,0);
            const double & c4 = R(0,1);
            const double & c5 = R(1,1);
            const double & c6 = R(2,1);
            const double & c7 = R(0,2);
            const double & c8 = R(1,2);
            const double & c9 = R(2,2);

            Eigen::Vector4d dc(fabs(1.0+c1-c5-c9),
                               fabs(1.0-c1+c5-c9),
                               fabs(1.0-c1-c5+c9),
                               fabs(1.0+c1+c5+c9));

            unsigned maxq = 0;
            double maxqval = dc(0);

            for(unsigned i = 1; i < 4; i++) {
                if(dc(i) > maxqval) {
                    maxq = i;
                    maxqval = dc(i);
                }
            }

            double c;
            Eigen::Vector4d q;
            if(maxq == 0){
                q(0)=0.5*sqrt(dc(0));
                c = 0.25/q(0);
                q(1)=c*(c4+c2);
                q(2)=c*(c7+c3);
                q(3)=c*(c8-c6);
            } else if(maxq == 1){
                q(1)=0.5*sqrt(dc(1));
                c = 0.25/q(1);
                q(0)=c*(c4+c2);
                q(2)=c*(c6+c8);
                q(3)=c*(c3-c7);
            } else if(maxq == 2){
                q(2)=0.5*sqrt(dc(2));
                c = 0.25/q(2);
                q(0)=c*(c3+c7);
                q(1)=c*(c6+c8);
                q(3)=c*(c4-c2);
            } else {
                q(3)=0.5*sqrt(dc(3));
                c = 0.25/q(3);
                q(0)=c*(c8-c6);
                q(1)=c*(c3-c7);
                q(2)=c*(c4-c2);
            }

            if(q(3) < 0)
                q = -q;

            return q;
        }

        Eigen::Matrix3d quat2r(Eigen::Vector4d const & q){

            SM_ASSERT_NEAR_DBG(std::runtime_error,q.norm(),1.f,1e-4, "The quaternion must be a unit vector to represent a rotation");
            //double n = q(3);
            //Eigen::Vector3d e = makeV3(q(0),q(1),q(2));
            //R = (n^2 - e'*e) * eye(3) + 2 * e * e' + 2 * n * crossMx(e);
            //Eigen::Matrix3d R = (n*n - e.dot(e))*Eigen::Matrix3d::Identity() + 2 * e * e.transpose() + 2 * n * crossMx(e);

            Eigen::Matrix3d R;

            // [ q0^2 - q1^2 - q2^2 + q3^2,           2*q0*q1 + 2*q2*q3,           2*q0*q2 - 2*q1*q3]
            // [         2*q0*q1 - 2*q2*q3, - q0^2 + q1^2 - q2^2 + q3^2,           2*q0*q3 + 2*q1*q2]
            // [         2*q0*q2 + 2*q1*q3,           2*q1*q2 - 2*q0*q3, - q0^2 - q1^2 + q2^2 + q3^2]
            R(0,0) = q[0]*q[0]-q[1]*q[1]-q[2]*q[2]+q[3]*q[3];
            R(0,1) = q[0]*q[1]*2.0+q[2]*q[3]*2.0;
            R(0,2) = q[0]*q[2]*2.0-q[1]*q[3]*2.0;
            R(1,0) = q[0]*q[1]*2.0-q[2]*q[3]*2.0;
            R(1,1) = -q[0]*q[0]+q[1]*q[1]-q[2]*q[2]+q[3]*q[3];
            R(1,2) = q[0]*q[3]*2.0+q[1]*q[2]*2.0;
            R(2,0) = q[0]*q[2]*2.0+q[1]*q[3]*2.0;
            R(2,1) = q[0]*q[3]*(-2.0)+q[1]*q[2]*2.0;
            R(2,2) = -q[0]*q[0]-q[1]*q[1]+q[2]*q[2]+q[3]*q[3];

            return R;
        }

        Eigen::Matrix4d quatPlus(Eigen::Vector4d const & q)
        {
            // [  q3,  q2, -q1, q0]
            // [ -q2,  q3,  q0, q1]
            // [  q1, -q0,  q3, q2]
            // [ -q0, -q1, -q2, q3]
            Eigen::Matrix4d Q;
            Q(0,0) =  q[3]; Q(0,1) =  q[2]; Q(0,2) = -q[1]; Q(0,3) =  q[0];
            Q(1,0) = -q[2]; Q(1,1) =  q[3]; Q(1,2) =  q[0]; Q(1,3) =  q[1];
            Q(2,0) =  q[1]; Q(2,1) = -q[0]; Q(2,2) =  q[3]; Q(2,3) =  q[2];
            Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) =  q[3];

            return Q;
        }

        Eigen::Matrix4d quatOPlus(Eigen::Vector4d const & q)
        {
            // [  q3, -q2,  q1, q0]
            // [  q2,  q3, -q0, q1]
            // [ -q1,  q0,  q3, q2]
            // [ -q0, -q1, -q2, q3]

            Eigen::Matrix4d Q;
            Q(0,0) =  q[3]; Q(0,1) = -q[2]; Q(0,2) =  q[1]; Q(0,3) =  q[0];
            Q(1,0) =  q[2]; Q(1,1) =  q[3]; Q(1,2) = -q[0]; Q(1,3) =  q[1];
            Q(2,0) = -q[1]; Q(2,1) =  q[0]; Q(2,2) =  q[3]; Q(2,3) =  q[2];
            Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) =  q[3];

            return Q;

        }

        Eigen::Vector4d qplus(Eigen::Vector4d const & q, Eigen::Vector4d const & p)
        {
            Eigen::Vector4d qplus_p;
            // p0*q3 + p1*q2 - p2*q1 + p3*q0
            qplus_p[0] = p[0]*q[3] + p[1]*q[2] - p[2]*q[1] + p[3]*q[0];
            // p2*q0 - p0*q2 + p1*q3 + p3*q1
            qplus_p[1] = p[2]*q[0] - p[0]*q[2] + p[1]*q[3] + p[3]*q[1];
            // p0*q1 - p1*q0 + p2*q3 + p3*q2
            qplus_p[2] = p[0]*q[1] - p[1]*q[0] + p[2]*q[3] + p[3]*q[2];
            // p3*q3 - p1*q1 - p2*q2 - p0*q0
            qplus_p[3] = p[3]*q[3] - p[1]*q[1] - p[2]*q[2] - p[0]*q[0];

            return qplus_p;
        }

        Eigen::Vector4d qoplus(Eigen::Vector4d const & q, Eigen::Vector4d const & p)
        {
            Eigen::Vector4d qoplus_p;
            // p0*q3 - p1*q2 + p2*q1 + p3*q0
            qoplus_p[0] = p[0]*q[3] - p[1]*q[2] + p[2]*q[1] + p[3]*q[0];
            // p0*q2 - p2*q0 + p1*q3 + p3*q1
            qoplus_p[1] = p[0]*q[2] - p[2]*q[0] + p[1]*q[3] + p[3]*q[1];
            // p1*q0 - p0*q1 + p2*q3 + p3*q2
            qoplus_p[2] = p[1]*q[0] - p[0]*q[1] + p[2]*q[3] + p[3]*q[2];
            // p3*q3 - p1*q1 - p2*q2 - p0*q0
            qoplus_p[3] = p[3]*q[3] - p[1]*q[1] - p[2]*q[2] - p[0]*q[0];

            return qoplus_p;
        }

        Eigen::Vector4d quatInv(Eigen::Vector4d const & q)
        {
            Eigen::Vector4d qret = q;
            invertQuat(qret);
            return qret;
        }

        void invertQuat(Eigen::Vector4d & q)
        {
            q.head<3>() = -q.head<3>();
        }

        Eigen::Vector3d qeps(Eigen::Vector4d const & q)
        {
            return q.head<3>();
        }

        Eigen::Vector3f qeps(Eigen::Vector4f const & q)
        {
            return q.head<3>();
        }

        double qeta(Eigen::Vector4d const & q)
        {
            return q[3];
        }

        float qeta(Eigen::Vector4f const & q)
        {
            return q[3];
        }

        Eigen::Vector4d axisAngle2quat(Eigen::Vector3d const & a)
        {
            // Method of implementing this function that is accurate to numerical precision from
            // Grassia, F. S. (1998). Practical parameterization of rotations using the exponential map. journal of graphics, gpu, and game tools, 3(3):29–48.

            double theta = a.norm();

            // na is 1/theta sin(theta/2)
            double na;
            if(isLessThenEpsilons4thRoot(theta))
            {
                static const double one_over_48 = 1.0/48.0;
                na = 0.5 + (theta * theta) * one_over_48;
            }
            else
            {
                na = sin(theta*0.5) / theta;
            }
            Eigen::Vector3d axis = a*na;
            double ct = cos(theta*0.5);
            return Eigen::Vector4d(axis[0],axis[1],axis[2],ct);
        }

        /**
         * calculate arcsin(x)/x
         * @param x
         * @return
         */
        template <typename Scalar_>
        inline Scalar_ arcSinXOverX(Scalar_ x) {
          if(isLessThenEpsilons4thRoot(fabs(x))){
            return Scalar_(1.0) + x * x * Scalar_(1/6);
          }
          return asin(x) / x;
        }

        template <typename Scalar_>
        Eigen::Matrix<Scalar_, 3, 1> quat2AxisAngle(Eigen::Matrix<Scalar_, 4, 1> const & q)
        {
          SM_ASSERT_LT_DBG(std::runtime_error, fabs(q.norm() - 1), 8 * std::numeric_limits<Scalar_>::epsilon(), "This function is inteded for unit quternions only.");
          const Eigen::Matrix<Scalar_, 3, 1> a = qeps(q);
          const Scalar_ na = a.norm(), eta = qeta(q);
          Scalar_ scale;
          if(fabs(eta) < na){ // use eta because it is more precise than na to calculate the scale. No singularities here.
            scale = acos(eta) / na;
          } else {
            /*
             * In this case more precision is in na than in eta so lets use na only to calculate the scale:
             *
             * assume first eta > 0 and 1 > na > 0.
             *               u = asin (na) / na  (this implies u in [1, pi/2], because na i in [0, 1]
             *    sin (u * na) = na
             *  sin^2 (u * na) = na^2
             *  cos^2 (u * na) = 1 - na^2
             *                              (1 = ||q|| = eta^2 + na^2)
             *    cos^2 (u * na) = eta^2
             *                              (eta > 0,  u * na = asin(na) in [0, pi/2] => cos(u * na) >= 0 )
             *      cos (u * na) = eta
             *                              (u * na in [ 0, pi/2] )
             *                 u = acos (eta) / na
             *
             * So the for eta > 0 it is acos(eta) / na == asin(na) / na.
             * From some geometric considerations (mirror the setting at the hyper plane q==0) it follows for eta < 0 that (pi - asin(na)) / na = acos(eta) / na.
             */
            if(eta > 0){
              // For asin(na)/ na the singularity na == 0 can be removed. We can ask (e.g. Wolfram alpha) for its series expansion at na = 0. And that is done in the following function.
              scale = arcSinXOverX(na);
            }else{
              // (pi - asin(na))/ na has a pole at na == 0. So we cannot remove this singularity.
              // It is just the cut locus of the unit quaternion manifold at identity and thus the axis angle description becomes necessarily unstable there.
              scale = (M_PI - asin(na)) / na;
            }
          }
          return a * (Scalar_(2) * scale);
        }
        template Eigen::Matrix<double, 3, 1> quat2AxisAngle(Eigen::Matrix<double, 4, 1> const & q);
        template Eigen::Matrix<float, 3, 1> quat2AxisAngle(Eigen::Matrix<float, 4, 1> const & q);

        Eigen::Matrix<double,4,3> quatJacobian(Eigen::Vector4d const & p)
        {
            Eigen::Matrix<double,4,3> J;
            // [  p3, -p2,  p1]
            // [  p2,  p3, -p0]
            // [ -p1,  p0,  p3]
            // [ -p0, -p1, -p2]

            J(0,0) =  p[3];
            J(0,1) = -p[2];
            J(0,2) =  p[1];
            J(1,0) =  p[2];
            J(1,1) =  p[3];
            J(1,2) = -p[0];
            J(2,0) = -p[1];
            J(2,1) =  p[0];
            J(2,2) =  p[3];
            J(3,0) = -p[0];
            J(3,1) = -p[1];
            J(3,2) = -p[2];

            return J*0.5;
        }

        Eigen::Vector4d updateQuat(Eigen::Vector4d const & q, Eigen::Vector3d const & dq)
        {
            // the following code is an optimized version of:
            // Eigen::Vector4d dq4 = axisAngle2quat(dq);
            // Eigen::Vector4d retq = quatPlus(dq4)*q;
            // return retq;

            Eigen::Vector4d dq3 = axisAngle2quat(dq);
            double ca = dq3[3];
            Eigen::Vector4d retq;
            retq[0] = q[0]*ca+dq3[0]*q[3]-dq3[1]*q[2]+dq3[2]*q[1];
            retq[1] = q[1]*ca+dq3[0]*q[2]+dq3[1]*q[3]-dq3[2]*q[0];
            retq[2] = q[2]*ca-dq3[0]*q[1]+dq3[1]*q[0]+dq3[2]*q[3];
            retq[3] = q[3]*ca-dq3[0]*q[0]-dq3[1]*q[1]-dq3[2]*q[2];

            return retq;
        }

        Eigen::Vector3d quatRotate(Eigen::Vector4d const & q_a_b, Eigen::Vector3d const & v_b)
        {
            return v_b + 2.0 * q_a_b.head<3>().cross(q_a_b.head<3>().cross(v_b) - q_a_b[3] * v_b);
        }

        Eigen::Vector4d quatRandom()
        {
            Eigen::Vector4d q_a_b;
            q_a_b.setRandom();
            q_a_b.array() -= 0.5;
            q_a_b /= q_a_b.norm();
            return q_a_b;
        }

        Eigen::Vector4d quatIdentity()
        {
            return Eigen::Vector4d(0,0,0,1);
        }

        Eigen::Matrix<double,3,4> quatS(Eigen::Vector4d q)
        {
            //   [  q3,  q2, -q1, -q0]
            // 2 [ -q2,  q3,  q0, -q1]
            //   [  q1, -q0,  q3, -q2]
            q *= 2.0;

            Eigen::Matrix<double,3,4> S;
            S <<
                q[3],  q[2], -q[1], -q[0],
                -q[2],  q[3],  q[0], -q[1],
                q[1], -q[0],  q[3], -q[2];

            return S;
        }

        Eigen::Matrix<double,4,3> quatInvS(Eigen::Vector4d q)
        {
            q *= 0.5;

            // 1 [  q3, -q2,  q1]
            // - [  q2,  q3, -q0]
            // 2 [ -q1,  q0,  q3]
            //   [ -q0, -q1, -q2]

            Eigen::Matrix<double,4,3> invS;
            invS <<
                q[3], -q[2],  q[1],
                q[2],  q[3], -q[0],
                -q[1],  q[0],  q[3],
                -q[0], -q[1], -q[2];
            return invS;
        }

        Eigen::Vector4d qslerp(const Eigen::Vector4d & q0, const Eigen::Vector4d & q1, double t)
        {
          if(t <= 0.0)
            {
                return q0;
            }
            else if(t >= 1.0)
            {
                return q1;
            }
            else
            {
              if( (q0-q1).squaredNorm() > (q0 + q1).squaredNorm() ) {
                // The quaternions are far away from eachother on the sphere.
                // Flip one around so that this works out.
                return qplus(q0, qexp(t * qlog( qplus(quatInv(q0),-q1))));
              } else {
                return qplus(q0, qexp(t * qlog( qplus(quatInv(q0),q1))));
              }

            }
        }

        /// \brief do linear interpolation between p0 and p1 for times t = [0.0,1.0]
        Eigen::VectorXd lerp(const Eigen::VectorXd & p0, const Eigen::VectorXd & p1, double t)
        {
            SM_ASSERT_EQ(std::runtime_error, p0.size(), p1.size(), "The vectors must be the same size");
            if(t <= 0.0)
            {
                return p0;
            }
            else if(t >= 1.0)
            {
                return p1;
            }
            else
            {
                return (1-t) * p0  +  t * p1;
            }
        }

        Eigen::Matrix<double,3,4> quatLogJacobian(const Eigen::Vector4d& p)
        {
          //      [qx]
          //      [qy]                -2*x
          // p =  [qz],    g(x) = ----------------
          //      [qw]               sqrt(1-qw²)
          //
          //
          //      [2*acos(qw)      0            0            g(qx)]
          // J =  [0            2*acos(qw)      0            g(qy)]
          //      [0            0            2*acos(qw)      g(qz)]

          Eigen::Matrix<double, 3,4> J;
          J.setZero();

          double n = qeps(p).norm();

          double de = n*n*n;//pow(n, 3);
          double u12 = p(1)*p(1) + p(2)*p(2);//pow(p(1), 2) + pow(p(2), 2);
          double u02 = p(0)*p(0) + p(2)*p(2);//pow(p(0), 2) + pow(p(2), 2);
          double u01 = p(0)*p(0) + p(1)*p(1);//pow(p(0), 2) + pow(p(1), 2);
          double a = acos(p(3));
          double uw = sqrt(-(p(3)*p(3) - 1)*n*n);

          J(0,0) = 2*a*u12 / de;
          J(0,1) = -2*a*p(1)*p(0) / de;
          J(0,2) = -2*a*p(2)*p(0) / de;
          J(0,3) = -2*p(0) / uw;
          J(1,0) = -2*a*p(0)*p(1) / de;
          J(1,1) = 2*a*u02 / de;
          J(1,2) = -2*a*p(1)*p(2) / de;
          J(1,3) = -2*p(1) / uw;
          J(2,0) = -2*a*p(0)*p(2) / de;
          J(2,1) = -2*a*p(1)*p(2) / de;
          J(2,2) = 2*a*u01 / de;
          J(2,3) = -2*p(2) / uw;

          return J;
        }

        template <typename Scalar_>
        inline const typename Eigen::Matrix<Scalar_, 4, 3> & quatV(){
          static const Eigen::Matrix<Scalar_, 4, 3> V = 0.5 * Eigen::Matrix<Scalar_, 4, 3>::Identity();
          return V;
        }
        template const Eigen::Matrix<double, 4, 3> & quatV();
        template const Eigen::Matrix<float, 4, 3> & quatV();

        template <typename Scalar_>
        Eigen::Matrix<Scalar_, 3, 3> expDiffMat(const Eigen::Matrix<Scalar_, 3, 1> & vec){
          Scalar_ phi = vec.norm();

          if(phi == 0){
            return Eigen::Matrix<Scalar_, 3, 3>::Identity();
          }

          Eigen::Matrix<Scalar_, 3, 3> vecCross = crossMx(vec);

          Scalar_ phiAbs = fabs(phi);
          Scalar_ phiSquare = phi * phi;

          Scalar_ a;
          Scalar_ b;
          if(!isLessThenEpsilons4thRoot(phiAbs)){
            Scalar_ siPhiHalf = sin(phi / 2);
            a = (2 * siPhiHalf * siPhiHalf / phiSquare);
            b = ((1 - sin(phi) / phi)/phiSquare);
          }
          else{
            a = (1.0/2) * (1 - (1.0 / (24 / 2)) * phiSquare);
            b = (1.0/6) * (1 - (1.0 / (120 / 6)) * phiSquare);
          }

          return Eigen::Matrix<Scalar_, 3, 3>::Identity() - a * vecCross + b * vecCross * vecCross;
        }
        template Eigen::Matrix<double, 3, 3> expDiffMat<double>(const Eigen::Matrix<double, 3, 1>  &);
        template Eigen::Matrix<float, 3, 3> expDiffMat<float>(const Eigen::Matrix<float, 3, 1>  &);

        template <typename Scalar_>
        Eigen::Matrix<Scalar_ , 4, 3> quatExpJacobian(const Eigen::Matrix<Scalar_ , 3, 1>& vec){
          return quatOPlus(axisAngle2quat(vec.template cast<double>())).template cast<Scalar_>() * quatV<Scalar_>() * expDiffMat(vec);
        }
        template Eigen::Matrix<double, 4,3> quatExpJacobian(const Eigen::Matrix<double, 3, 1>& vec);
        template Eigen::Matrix<float, 4,3> quatExpJacobian(const Eigen::Matrix<float, 3, 1>& vec);

        template <typename Scalar_>
        Eigen::Matrix<Scalar_, 3, 3> logDiffMat(const Eigen::Matrix<Scalar_, 3, 1>  & vec){
          Scalar_ phi = vec.norm();
          if(phi == 0){
            return Eigen::Matrix<Scalar_, 3, 3>::Identity();
          }

          Scalar_ phiAbs = fabs(phi);
          Eigen::Matrix<Scalar_, 3, 3> vecCross = crossMx(vec);

          Scalar_ a;
          if(!isLessThenEpsilons4thRoot(phiAbs)){
            Scalar_ phiHalf = 0.5 * phi;
            a = ((1 - phiHalf / tan(phiHalf))/phi/phi);
          }
          else{
            a = 1.0 / 12 * (1 + 1.0 / 60 * phi * phi);
          }
          return Eigen::Matrix<Scalar_, 3, 3>::Identity() + 0.5 * vecCross + a * vecCross * vecCross;
        }
        template Eigen::Matrix<double, 3, 3> logDiffMat<double>(const Eigen::Matrix<double, 3, 1>  &);
        template Eigen::Matrix<float, 3, 3> logDiffMat<float>(const Eigen::Matrix<float, 3, 1>  &);

        template <typename Scalar_>
        Eigen::Matrix<Scalar_ , 3, 4> quatLogJacobian2(const Eigen::Matrix<Scalar_ , 4, 1>& p){
          return logDiffMat(quat2AxisAngle<>(p)) * (quatV<Scalar_>().transpose() * Scalar_(4.0)) * quatOPlus(quatInv(p.template cast<double>())).template cast<Scalar_>();
        }

        template Eigen::Matrix<double, 3, 4> quatLogJacobian2(const Eigen::Matrix<double, 4, 1>&);
        template Eigen::Matrix<float, 3, 4> quatLogJacobian2(const Eigen::Matrix<float, 4, 1>& );
    }} // namespace sm::kinematics


//----------------------------------*-C++-*----------------------------------//
/**
 *  @file   SphericalHarmonics.cc
 *  @author Jeremy Roberts
 *  @date   Jun 30, 2011
 *  @brief  SphericalHarmonics member definitions
 */
//---------------------------------------------------------------------------//

#include "SphericalHarmonics.hh"
#include "utilities/Constants.hh"
#include "utilities/DBC.hh"
#include "utilities/GenException.hh"
#include "utilities/SoftEquivalence.hh"
#include <cmath>
#ifdef DETRAN_ENABLE_BOOST
#include <boost/math/special_functions/spherical_harmonic.hpp>
#include <boost/math/special_functions/factorials.hpp>
#endif

namespace detran_angle
{

// Calculate the spherical harmonic of degree l, order m
// given  cos(polar) and azimuthal angle
double SphericalHarmonics::Y_lm(const int    l,
                                const int    m,
                                const double xi,
                                const double varphi )
{
    Require (xi >= -1.0);
    Require (xi <= 1.0);
    Require (varphi >= 0.0);
    Require (varphi <= detran_utilities::two_pi);

    double sintheta = std::sqrt(1.0 - xi*xi);
    double mu  = sintheta*std::cos(varphi);
    double eta = sintheta*std::sin(varphi);
    return get_Y_lm(l, m, mu, eta, xi);

}

// Calculate the spherical harmonic of degree l, order m
// given the triplet of directional cosines
double SphericalHarmonics::Y_lm(const int    l,
                                const int    m,
                                const double mu,
                                const double eta,
                                const double xi )
{
    return get_Y_lm(l, m, mu, eta, xi);
}

// Calculate the Legendre polynomial of degree l
// given the polar angle cosine
double SphericalHarmonics::Y_lm(const int    l,
                                const double xi )
{
    return get_Y_lm(l, 0, 0, 0, xi);
}

double SphericalHarmonics::get_Y_lm(const int    l,
                                    const int    m,
                                    const double mu,
                                    const double eta,
                                    const double xi )
{
  using std::fabs;
  Require(l >= 0);
  Require(m <= l);
  Require(m >= -l);
  Require(mu >= -1.0);
  Require(mu <= 1.0);
  Require(eta >= -1.0);
  Require(eta <= 1.0);
  Require(xi >= -1.0);
  Require(xi <= 1.0);
  double unity = mu * mu + eta * eta + xi * xi;
  if (!detran_utilities::soft_equiv(1.0, unity, 1.0e-5))
  {
    Require( detran_utilities::soft_equiv( fabs(mu), 0.0 ) &&
             detran_utilities::soft_equiv( fabs(eta), 0.0 ) );
  }

  if ( l == 0 )
    return 1.0;
  else if ( l == 1 )
  {
    if ( m == -1 )
      return eta;
    else if ( m == 0 )
      return xi;
    else if ( m == 1 )
      return mu;
  }
  else if ( l == 2 )
  {
    if ( m == -2 )
      return 1.732050807568877*mu*eta;
    else if ( m == -1 )
      return 1.732050807568877*xi*eta;
    else if ( m == 0 )
      return 1.5*xi*xi-0.5;
    else if ( m == 1 )
      return 1.732050807568877*xi*mu;
    else if ( m == 2 )
      return 0.8660254037844385*(mu*mu-eta*eta);
  }
  else if ( l == 3 )
  {
    if ( m == -3 )
      return 0.7905694150420948*eta*(3*mu*mu-eta*eta);
    else if ( m == -2 )
      return 3.872983346207417*xi*mu*eta;
    else if ( m == -1 )
      return 0.6123724356957945*eta*(5.0*xi*xi-1.0);
    else if ( m == 0 )
      return 2.5*xi*xi*xi - 1.5*xi;
    else if ( m == 1 )
      return 0.6123724356957945*mu*(5.0*xi*xi-1.0);
    else if ( m == 2 )
      return 1.936491673103708*xi*(mu*mu-eta*eta);
    else if ( m == 3 )
      return 0.7905694150420948*mu*(mu*mu-3.0*eta*eta);
  }
  else
  {
#ifdef DETRAN_ENABLE_BOOST
    double phi   = std::acos(mu / std::sqrt(1.0 - xi*xi));
    double theta = std::acos(xi);
    // Normalization so that phi_00 = phi, phi_1m = current components, etc.
    // This leads to a match with definitions given by Hebert.
    double del = (m == 0) ? 1.0 : 2.0;
    double norm = std::pow(-1.0, m) *
                  std::sqrt(del * detran_utilities::four_pi / (2.*l + 1.));
    if (m >= 0)
      return norm * boost::math::spherical_harmonic_r(l, m, theta, phi);
    else
      return norm * boost::math::spherical_harmonic_i(l,-m, theta, phi);
#else
    // Degree not implemented.
    THROW("Maximum Legendre order is 3. For higher orders, enable Boost.");
	return 0;
#endif
  }
  return 0;
}

} // end namespace detran_angle

//---------------------------------------------------------------------------//
//              end of SphericalHarmonics.cc
//---------------------------------------------------------------------------//


// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>

#include <mars/general_functions/utils.h>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <cmath>
#include <iostream>

namespace mars
{
Utils::Utils() = default;

Eigen::MatrixXd Utils::EnforceMatrixSymmetry(const Eigen::Ref<const Eigen::MatrixXd>& mat_in)
{
  Eigen::MatrixXd mat_out = (mat_in + mat_in.transpose()) / 2;
  return mat_out;
}

Eigen::Matrix3d Utils::Skew(const Eigen::Vector3d& v)
{
  Eigen::Matrix3d res;
  res << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;
  return res;
}

Eigen::Matrix4d Utils::MatExp(const Eigen::Matrix4d& A, const int& order)
{
  Eigen::Matrix4d matexp(Eigen::Matrix4d::Identity());  // initial condition with k=0

  int div = 1;
  Eigen::Matrix4d a_loop = A;

  for (int k = 1; k <= order; k++)
  {
    div = div * k;  // factorial(k)
    matexp = matexp + a_loop / div;
    a_loop = a_loop * A;  // adding one exponent each iteration
  }

  return matexp;
}

Eigen::Matrix4d Utils::OmegaMat(const Eigen::Vector3d& v)
{
  Eigen::Matrix4d res;
  res.setZero();

  res.block(0, 1, 1, 3) = -v.transpose();
  res.block(1, 0, 3, 1) = v;
  res.block(1, 1, 3, 3) = -Skew(v);

  return res;
}

Eigen::Quaterniond Utils::QuatFromSmallAngle(const Eigen::Vector3d& d_theta_vec)
{
  const double d_theta_vec_norm = d_theta_vec.norm();
  const double d_theta_squared = d_theta_vec_norm * d_theta_vec_norm;

  if (d_theta_squared / 4.0 < 1.0)
  {
    return Eigen::Quaterniond(sqrt(1 - d_theta_squared / 4.0), d_theta_vec[0] * 0.5, d_theta_vec[1] * 0.5,
                              d_theta_vec[2] * 0.5);
  }
  else
  {
    // In this case the error angle vector is not small at all.
    std::cout << "Warning: The error angle vector is not small" << std::endl;

    double w = 1.0 / sqrt(1 + d_theta_squared / 4.0);
    double f = w * 0.5;
    return Eigen::Quaterniond(w, d_theta_vec[0] * f, d_theta_vec[1] * f, d_theta_vec[2] * f);
  }
}

Eigen::Quaterniond Utils::ApplySmallAngleQuatCorr(const Eigen::Quaterniond& q_prior, const Eigen::Vector3d& correction)
{
  return (q_prior * QuatFromSmallAngle(correction)).normalized();
}

bool Utils::CheckCov(const Eigen::MatrixXd& cov_mat, const std::string& description, const bool& /*check_cond*/)
{
  bool result = true;
  std::string info("[" + description + "]" + ": ");

  bool is_symmetric = Eigen::MatrixXd::Zero(cov_mat.rows(), cov_mat.cols()).isApprox(cov_mat - cov_mat.transpose());
  if (!is_symmetric)
  {
    std::cout << "Warning: " << info << "The covariance matrix is not symmetric" << std::endl;
    result = false;
  }

  double det = cov_mat.determinant();
  if (det <= 0)
  {
    std::cout << "Warning: " << info << "The determinant of the covariance matrix is not positive (" << det << ")"
              << std::endl;
    result = false;
  }

  double min_eigenvalue = Eigen::EigenSolver<Eigen::MatrixXd>(cov_mat).eigenvalues().real().minCoeff();
  if (min_eigenvalue < 0)
  {
    std::cout << "Warning: " << info << "The covariance matrix is not positive semidefinite min: " << min_eigenvalue
              << std::endl;
    result = false;
  }

  return result;
}
}


/**
 * ODE solver - JMU REU 2021
 *
 * @author Mike Lam
 */

// standard headers
#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <string>
#include <cmath>
#include <functional>
using namespace std;

// Boost ODEint headers (individual files compile faster than the catch-all)
#include <boost/numeric/odeint.hpp>
//#include <boost/numeric/odeint/integrate/integrate_const.hpp>
//#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
using namespace boost::numeric::odeint;

// floating-point precision control (control via compiler parameter -Dreal_t)
//typedef double real_t;      // 64 bits
//typedef float real_t;       // 32 bits

// solver state
typedef std::vector<real_t> state_t;

// parameters (most read from command line)
real_t a, b, c, x0;
double t0 = 0.0;
double tn, dt;
const int N = 10;
real_t ac[N+1];

// output stream
ofstream out;

// ODE system
void ode(const state_t &x, state_t &dxdt, const real_t /*t*/)
{
    dxdt[0] = a*pow(x[0],2.0) + b*x[0] + c;
}

// power series solution at x
void calc_coefficients(real_t x)
{
    ac[0] = x;
    ac[1] = a * ac[0]*ac[0] + b * ac[0] + c;

    for (int i = 2; i <= N; i++) {
        ac[i] = 0.0;
        for (int k = 0; k < i; k++) {
            ac[i] += ac[k] * ac[i-1-k];
        }
        ac[i] = (a * ac[i] + b * ac[i-1]) / i;
    }
}

// power series solution using Horner's method
real_t psm(const real_t t)
{
    real_t x = t * ac[N];
    for (int j = N-1; j > 0; j--) {
        x = t * (x + ac[j]);
    }
    x += ac[0];
    return x;
}

// x \left( t \right) =1/2\,{\frac {1}{a} \left( \tan \left( 1/2\,t\sqrt
// {4\,ac-{b}^{2}}+\arctan \left( {\frac {2\,{\it x0}\,a+b}{\sqrt {4\,ac-
// {b}^{2}}}} \right)  \right) \sqrt {4\,ac-{b}^{2}}-b \right) }
//
// @author Kevin Rojas
real_t solution(real_t a,real_t b, real_t c, real_t x0, real_t t)
{
    /* Inputs x:
     * a,b,c: Parameters of the ODE
     * x0   : Initial condition
     * x    : Point to evaluate the exact solution on
     * Output:
     * x(t) where x solves x' = ax+bx+c, x(0) = x0
     */

    real_t disc = b*b-4.0*a*c;
    //cout << "discriminant: " << disc << endl;
    real_t val = 0.0;

    if (disc > 0.0) {
       real_t x1 = (-b+sqrt(disc))/(2.0*a);
       real_t x2 = (-b-sqrt(disc))/(2.0*a);
       real_t A = abs((x0-x2)/(x0-x1))*exp((x2-x1)*a*t);
       real_t val1 = (A*x1-x2)/(A-1);

       real_t B = -A;
       real_t val2 = (B*x1-x2)/(B-1);

       if((val1 >x2 && val1> x1) || (val1<x2 && val1<x1)){
           val = val1;
       }
       else{
           val = val2;
       }
       val = val2;
       // TODO: deal with the absolute value?

    } else if (disc < 0.0) {
       real_t A = b/(2.0*sqrt(a));
       real_t D = -disc/(4.0*a*a);
       real_t c0 = atan((x0+A)/sqrt(D));
       val = sqrt(D)*tan(a*sqrt(D)*t+c0)-A;

    } else {
       real_t x1 = -b/(2.0*a);
       real_t c0 = -1.0/(x0-x1);
       val = (x1-1.0/(a*t+c0));
    }
    return val;
}

// debug output helper
void observe(const state_t &x, const real_t t)
{
    real_t sol = solution(a, b, c, x0, t);
    real_t psm_sol = psm(0);

    // re-center power series
    calc_coefficients(psm(dt));

    // write output
    out << setw(8) << t;
    out << setw(16) << sol;
    out << setw(16) << x[0] << setw(16) << fabs(sol-x[0]);
    out << setw(16) << psm_sol << setw(16) << fabs(sol-psm_sol);
    out << endl;
}

int main(int argc, const char* argv[])
{
    // check parameters
    if (argc != 7) {
        cout << "Usage: " << argv[0] << " <a> <b> <c> <x0> <tn> <dt>" << endl;
        return EXIT_FAILURE;
    }

    // parse parameters
    a  = stod(argv[1], NULL);
    b  = stod(argv[2], NULL);
    c  = stod(argv[3], NULL);
    x0 = stod(argv[4], NULL);
    tn = stod(argv[5], NULL);
    dt = stod(argv[6], NULL);
    // TODO: add stepper and t0?

    // initialize state
    state_t x(1);
    x[0] = x0;

    // precalculate coefficients
    calc_coefficients(x0);

    // show floating-point width
    //cout << "sizeof(real_t)=" << sizeof(real_t) << endl;

    // stepper
    runge_kutta4<state_t> stp;

    // integrate w/ debug output
    out.open("out.dat");
    /*size_t steps =*/ integrate_const(stp, ode, x, t0, tn, dt, observe);
    out.close();

    // show final output
    //cout << "steps=" << steps << " x=" << x[0] << endl;

    return EXIT_SUCCESS;
}


/**
 * @file model_predictive_controller.cpp
 * @brief Finite-horizon Model Predictive Controller.
 *
 */

#include <mpc.hpp>
#include <Eigen/Dense>
#include <iostream>

using namespace controller;

MPC::MPC(const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R, const double S) :
    Q(Q), R(R), saturation(S), converged(false)
{
    P_new.setZero();
    Ad.setZero(Q.rows(), Q.cols());
    I.setIdentity(Q.rows(), Q.cols());
    cmd_vel.setZero(1, R.rows()); // [translation rate, rotation rate].
}

Eigen::MatrixXd MPC::computeDiscrete(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, const Eigen::MatrixXd& E, unsigned int numIteration, double tolarance, double dt)
{
    // A Cost-to-go matrix P evolving backwards in time from Q is defined as,
    P = Q;

    // Discretization.
    Ad = (I + A * dt * 0.5) * (I - A * dt * 0.5).inverse();
    Bd = B * dt;

    // Iterating over a finite-horizon Algebraic Riccati Equation to a steady-state solution; that is, P_new - P ~= 0 + tolarance.
    for(unsigned int i = 0; i < numIteration; i++)
    {
        P_new = Ad.transpose() * P * Ad - (Ad.transpose() * P * Bd) * (R + Bd.transpose() * P * Bd).inverse() * (Bd.transpose() * P * Ad) + Q;

        // Check system convergence.
        difference = fabs((P_new - P).maxCoeff());
        if(difference < tolarance)
        {
            // std::cout << "Solution found at i = " << i << std::endl;

            converged = true;
            break;
        }

        // Update Riccati solution.
        P = P_new;
    }

    // Get cmd_vel.
    if(converged)
    {
        K = (R + Bd.transpose() * P * Bd).inverse() * Bd.transpose() * P * Ad;
        controlUpdate = K * E;

        // Compute magnitude of velocity vector.
        cmd_vel <<
            sqrt(pow(controlUpdate(0, 0), 2.0) + pow(controlUpdate(0, 1), 2.0) + pow(controlUpdate(0, 2), 2.0)),
            sqrt(pow(controlUpdate(1, 0), 2.0) + pow(controlUpdate(1, 1), 2.0) + pow(controlUpdate(1, 2), 2.0));

        // Saturate output signal.
        if(cmd_vel(0, 0) > saturation) cmd_vel(0, 0) = saturation;
        if(cmd_vel(0, 0) < -saturation) cmd_vel(0, 0) = -saturation;
        if(cmd_vel(0, 1) > saturation) cmd_vel(0, 1) = saturation;
        if(cmd_vel(0, 1) < -saturation) cmd_vel(0, 1) = -saturation;

        return cmd_vel;
    }
    else
    {
        std::cout << "MPC controller did not converge. Resetting cmd_vel... 沒有解決方案" << std::endl;
        cmd_vel.setZero();

        return cmd_vel;
    }

    converged = false;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2019.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)
//

// chapter10_08-001_pi_millions_with_boost.cpp

// This program can be used to compute millions of digits of pi.
// In fact, it has been used to compute more than one billion
// decimal digits of pi. Boost.Multiprecision is combined with
// GMP (or MPIR) in order to carry out the calculation of pi.

// This program requires inclusion of Boost.Multiprecision
// and linking with GMP (or MPIR on certain targets).

#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <limits>
#include <regex>
#include <sstream>
#include <string>

#include <boost/lexical_cast.hpp>
#include <boost/multiprecision/gmp.hpp>

namespace pi { namespace millions { namespace detail {

// *****************************************************************************
// Function    : template<typename float_type>
//               const float_type& calculate_pi_template(const bool print_progress)
//
// Description : Compute pi using a quadratically convergent Gauss AGM,
//               in the Schoenhage variant. For a description of the algorithm,
//               see Algorithm 16.148, Chapter 16, page 236 in the book
//               J. Arndt and C. Haenel, "Pi Unleashed",
//               (Springer Verlag, Heidelberg, 2001).
//
//               The parameter print_progress_to_cout (= true),
//               will print calculation progress to std::cout.
//
//               Book reference for "Pi Unleashed":
//               https://www.springer.com/de/book/9783642567353
//
// *****************************************************************************
template<typename float_type>
const float_type& pi(const bool print_progress = false)
{
  static bool is_init;

  static float_type val_pi;

  if(!is_init)
  {
    is_init = true;

    const std::regex rx("^[^e]+[e0+-]+([0-9]+)$");

    std::match_results<std::string::const_iterator> mr;

    std::stringstream ss;
    ss.setf(std::ios::scientific);
    ss.precision(static_cast<std::streamsize>(4));

    float_type a (1.0F);
    float_type bB(0.5F);
    float_type s (0.5F);
    float_type t (0.375F);

    // This loop is designed for computing a maximum of a few billion
    // decimal digits of pi. The number of digits roughly doubles
    // with each iteration of the loop. After 20 iterations,
    // the precision is about 2.8 million decimal digits.
    // After 29 iterations, the precision is more than one
    // billion decimal digits.

    for(std::uint_least16_t k = UINT8_C(1); k < UINT8_C(64); ++k)
    {
      using std::sqrt;

      a      += sqrt(bB);
      a      /= 2U;
      val_pi  = a;
      val_pi *= val_pi;
      bB      = (val_pi - t);
      bB     *= 2U;

      const float_type iterate_term((bB - val_pi) * (UINT64_C(1) << k));

      s += iterate_term;

      // Extract the base-10 order of magnitude
      // to estimate the base-10 digits in this
      // iteration.

      // Here, we produce a short printout
      // of the iteration term that is subsequently
      // parsed with a regular expression
      // for extracting the base-10 order.

      // Note: We are only extracting a few digits from iterate_term.
      // So piping it to a stringstream is not exorbitantly costly here.
      ss << iterate_term;

      const std::string str_iterate_term(ss.str());

      const bool is_match =
        std::regex_match(str_iterate_term, mr, rx);

      const std::uint64_t digits10_iterate =
        (is_match ? (std::max)(boost::lexical_cast<std::uint64_t>(mr[1U]), std::uint64_t(0U))
                  : UINT64_C(0));

      if(print_progress)
      {
        std::cout << "Base-10 digits of iteration "
                  << std::right
                  << std::setw(3)
                  << k
                  << ": "
                  << std::right
                  << std::setw(12)
                  << digits10_iterate
                  << '\n';
      }

      // Test the approximate base-10 digits
      // of this iteration term.

      // If we have attained at least half or more
      // of the total desired digits with this
      // iteration, the calculation is finished
      // because the change from the next iteration will be
      // insignificantly small.
      BOOST_CONSTEXPR_OR_CONST std::uint64_t digits10_iterate_goal =
        static_cast<std::uint64_t>((static_cast<std::uint64_t>(std::numeric_limits<float_type>::digits10) + 1LL) / 2LL) + 16LL;

      if(digits10_iterate > digits10_iterate_goal)
      {
        break;
      }

      t  = val_pi;
      t += bB;
      t /= 4U;

      ss.str(std::string());
    }

    if(print_progress)
    {
      std::cout << "Iteration loop done, compute inverse" << '\n';
    }

    val_pi += bB;
    val_pi /= s;

    if(print_progress)
    {
      std::cout << "The pi calculation is done." << '\n';
    }
  }

  return val_pi;
}

template<typename float_type>
std::ostream& report_pi_timing(std::ostream& os, const float elapsed)
{
  return os << "=================================================" << '\n'
            << "Computed "
            << static_cast<std::uint64_t>(std::numeric_limits<float_type>::digits10 - 1)
            << " digits of pi.\n"
            << "Total computation time : "
            << std::fixed
            << std::setprecision(2)
            << elapsed
            << " seconds"
            << '\n'
            << "================================================="
            << '\n';
}

} } } // namespace pi::millions::detail

namespace pi { namespace millions {

template<typename float_type>
void print_pi(std::ostream& os)
{
  // Calculate the value of pi. When doing so, print the calculation
  // messages to the console. Use the clock function to obtain the
  // total time of the pi calculation.

  using local_time_point_type =
    std::chrono::high_resolution_clock::time_point;

  const local_time_point_type start = std::chrono::high_resolution_clock::now();
  detail::pi<float_type>(true);
  const local_time_point_type stop  = std::chrono::high_resolution_clock::now();

  // Evaluate the time that was required for the pi calculation.
  const float elapsed =
      static_cast<float>(std::chrono::duration_cast<std::chrono::milliseconds>(stop - start).count())
    / static_cast<float>(1000.0F);

  // Report the time of the pi calculation to the console.
  static_cast<void>(detail::report_pi_timing<float_type>(std::cout, elapsed));

  // Report the time of the pi calculation to the output stream.
  static_cast<void>(detail::report_pi_timing<float_type>(os, elapsed));

  // Report that we are writing the output file.
  std::cout << "Writing the output file." << '\n';

  // Pipe the value of pi into a stringstream object.
  std::stringstream ss;

  // Pipe the value of pi into a stringstream object with full precision.
  ss << std::fixed
     << std::setprecision(std::streamsize(std::numeric_limits<float_type>::digits10) - 1)
     << detail::pi<float_type>();

  // Extract the string value of pi.
  const std::string str_pi(ss.str());

  // Print pi using the following paramater-tunable format.

  // pi = 3.1415926535 8979323846 2643383279 5028841971 6939937510 : 50
  //        5820974944 5923078164 0628620899 8628034825 3421170679 : 100
  //        8214808651 3282306647 0938446095 5058223172 5359408128 : 150
  //        4811174502 8410270193 8521105559 6446229489 5493038196 : 200
  //        ...

  BOOST_CONSTEXPR_OR_CONST char* char_set_separator   = " ";
  BOOST_CONSTEXPR_OR_CONST char* char_group_separator = "\n";

  BOOST_CONSTEXPR_OR_CONST std::size_t digits_per_set   = 10U;
  BOOST_CONSTEXPR_OR_CONST std::size_t digits_per_line  = digits_per_set * 5U;
  BOOST_CONSTEXPR_OR_CONST std::size_t digits_per_group = digits_per_line * 10U;

  // The digits after the decimal point are grouped
  // in sets of digits_per_set with digits_per_line
  // digits per line. The running-digit count is reported
  // at the end of each line.

  // The char_set_separator character string is inserted
  // between sets of digits. Between groups of lines,
  // we insert a char_group_separator character string
  // (which likely might be selected as a newline).

  // For a simple verification of 1,000,000 digits,
  // for example, go to Wolfram Alpha and ask:
  //   1000000th digit of Pi.
  // This prints out 50 digits of pi in the neighborhood
  // of a million digits, with the millionth digit in bold.

  std::string::size_type pos;

  if(   ((pos = str_pi.find(char('3'), 0U)) != std::string::npos)
     && ((pos = str_pi.find(char('.'), 1U)) != std::string::npos)
     && ((pos = str_pi.find(char('1'), 1U)) != std::string::npos))
  {
    ;
  }
  else
  {
    pos = 0U;
  }

  os << "pi = " << str_pi.substr(0U, pos);

  const std::size_t digit_offset = pos;

  // Extract the digits after the decimal point in a loop.
  // Insert spaces, newlines and a running-digit count
  // in order to create a format for comfortable reading.

  bool all_output_streaming_is_finished = false;

  while(all_output_streaming_is_finished == false)
  {
    // Print a set of digits (i.e. having 10 digits per set).
    const std::string str_pi_substring(str_pi.substr(pos, digits_per_set));

    os << str_pi_substring << char_set_separator;

    pos += (std::min)(std::string::size_type(digits_per_set),
                      str_pi_substring.length());

    const std::size_t number_of_digits(pos - digit_offset);

    // Check if all output streaming is finished.
    all_output_streaming_is_finished = (pos >= str_pi.length());

    if(all_output_streaming_is_finished)
    {
      // Write the final digit count.
      // Break from the printing loop.
      // Flush the output stream with std::endl.

      os << ": " << number_of_digits << std::endl;
    }
    else
    {
      const bool this_line_is_finished =
        (std::size_t(number_of_digits % digits_per_line) == std::size_t(0U));

      if(this_line_is_finished)
      {
        // Print the running-digit count and start a new line.
        os << ": " << number_of_digits << std::endl;

        const bool this_group_of_lines_is_finished =
          (std::size_t(number_of_digits % digits_per_group) == std::size_t(0U));

        if(this_group_of_lines_is_finished)
        {
          // Insert a character (which might be a blank line)
          // after a group of lines.
          os << char_group_separator;
        }

        // Insert spaces at the start of the new line.
        os << "       ";
      }
    }
  }
}

} } // namespace pi::millions

int main()
{
  using float_type =
    boost::multiprecision::number<boost::multiprecision::gmp_float<1000001UL>,
                                  boost::multiprecision::et_off>;

  std::ofstream out("pi.out");

  if(out.is_open())
  {
    pi::millions::print_pi<float_type>(out);

    out.close();
  }

  return 0;
}


#pragma once

#include "EM.hpp"
#include <Eigen/Dense>
#include <cmath>

template<class Float, class C>
requires ContainerOf<C, Eigen::Matrix<Float, C::value_type::RowsAtCompileTime, 1> >
Eigen::Matrix<Float, C::value_type::RowsAtCompileTime, 1> weightedSum(
    const std::vector<Float>& a,
    const C& x)
{
    typedef Eigen::Matrix<Float, C::value_type::RowsAtCompileTime, 1> Vec;
    assert( a.size() == x.size() );
    int n = x.begin()->size();
    return std::transform_reduce(a.begin(), a.end(), x.begin(),
                                 (Vec)Vec::Zero(n));
}

template<class Float, int N>
class IndependentGaussian
        : public ProbabilityDistribution<Eigen::Matrix<Float, N, 1>, Float > {
private:
    typedef Eigen::Matrix<Float, N, 1> Vec;
public:
    size_t n;
    Vec deviation;
    Vec mean;

    IndependentGaussian(size_t _n = N) :
        n{_n}
    {
        assert((N==Eigen::Dynamic || n==N) && "Wrong number of dimensions");
        init();
    }

    IndependentGaussian(const Vec& _deviation, const Vec& _mean) :
        n{(size_t)_deviation.size()},
        deviation{_deviation},
        mean{_mean}
    {
        assert((N==Eigen::Dynamic || n==N) && "Wrong number of dimensions");
    }

    Float operator()(const Vec& x) const{
        assert((size_t)x.size() == n);
        Float p = 1.0;
        Float s = 0.0;
        for(size_t j=0; j<n; j++){
            p *= one_two_pi/deviation(j);
            s += - (x[j]-mean[j])*(x[j]-mean[j])/2.0/deviation[j]/deviation[j];
        }
        return p*exp(s);
    }

    template <Container C>
    requires ContainerOf<C, Vec>
    void likelihoodEstimate(const std::vector<Float>& a,
                            const C& x)
    {
        Float sumA = std::reduce(a.begin(), a.end());
        if(sumA == 0.0){
            return;
        }
        mean = weightedSum(a, x)/sumA;

        Vec s2 = std::transform_reduce(
            a.begin(), a.end(), x.begin(),
            (Vec)Vec::Zero(n),
            std::plus<>(),
            [&](Float ai, const Vec& xi) -> Vec {
                return ai*(xi-mean).unaryExpr([&](auto xi_m){ return xi_m*xi_m; });
            });

        deviation = (s2/sumA).cwiseSqrt();
    }
private:
    static constexpr Float one_two_pi = 1.0/sqrt(2*M_PI);
    void init(){
        mean = Vec::Zero(n);
        deviation = Vec::Constant(n, 1.0);
    }
};

template<class Float, int N>
class Gaussian
        : public ProbabilityDistribution<Eigen::Matrix<Float, N, 1>, Float > {
private:
    typedef Eigen::Matrix<Float, N, 1> Vec;
    typedef Eigen::Matrix<Float, N, N> Mat;
    size_t n;
public:
    Vec mean;
    class : public Mat{
    public:
        Float constantFactor{ calculateConstantFactor() };
        void operator=(const Mat& _invCovariance){
            assert(_invCovariance.rows() == _invCovariance.cols() && "The inverse covariance matrix must be square");
            Mat::operator=(_invCovariance);
            constantFactor = calculateConstantFactor();
        }
    private:
        Float calculateConstantFactor(){
            return pow(one_two_pi, this->rows())*sqrt(this->determinant());
        }
    } invCovariance;

    Gaussian(size_t _n=N) :
        n{_n},
        mean{ Vec::Zero(n) },
        invCovariance{ Vec::Constant(n, 1.0).asDiagonal() }
    {
        assert((N==Eigen::Dynamic || n==N) && "Wrong number of dimensions");
    }

    Gaussian(const Mat& _invCovariance, const Vec& _mean) :
        n{(size_t)_mean.size()},
        mean{_mean},
        invCovariance{_invCovariance}
    {
        assert((N==Eigen::Dynamic || n==N) && "Wrong number of dimensions");
    }

    Float operator()(const Vec& x) const{
        assert((size_t)x.size() == n);
        return invCovariance.constantFactor * exp(-0.5*(x-mean).dot(invCovariance*(x-mean)));
    }

    template <Container C>
    requires ContainerOf<C, Vec>
    void likelihoodEstimate(const std::vector<Float>& a,
                            const C& x)
    {
        Float sumA = std::reduce(a.begin(), a.end());
        if(sumA == 0.0){
            return;
        }
        mean = weightedSum(a, x)/sumA;

        Mat Cov = std::transform_reduce(
            a.begin(), a.end(), x.begin(),
            (Mat)Mat::Zero(n, n),
            std::plus<>(),
            [&](Float ai, const Vec& xi) -> Mat {
                return ai*(xi-mean)*(xi-mean).transpose();
            });
        //FIXME: what if there is no inverse?
        invCovariance = (Cov/sumA).inverse();
    }
private:
    static constexpr Float one_two_pi = 1.0/sqrt(2*M_PI);
};


#include "iostream"
#include <cmath>
#include <string>
#include "CoupledPde.hpp"
#include "gnuplot_i.hpp"
#include <omp.h>
// #include <boost/test/unit_test.hpp>

using namespace Eigen;
using namespace std;

double model_prob_1_rhs(double x){return 2.0;}
double model_prob_2_rhs(double x){return 34.0*sin(x);}
double model_prob_3_rhs(double x, double y){return exp(-(x*x+y*y));}
double uj0(double x){return 1*sin(x)+1;}

int main(int argc, char* argv[]) {
    // int nthreads, tid;
    // #pragma omp parallel
    // {
    //     /* Obtain thread number */
    //     tid = omp_get_thread_num();
    //     printf("Hello World from thread = %d\n", tid);

    //     /* Only master thread does this */
    //     if (tid == 0) {
    //         nthreads = omp_get_num_threads();
    //         printf("Number of threads = %d\n", nthreads);
    //     }
    // }

    SecondOrderOde ode_mp1(500, -5.0, 0.0, model_prob_1_rhs, 0.0, 15.0);
    SecondOrderOde ode_mp2(5, -5.0, 0.0, model_prob_1_rhs, 0.0, 15.0);
    BoundaryConditions bc_mp1;
    BoundaryConditions bc_mp2;
    bc_mp1.SetX0DirichletBc1D(0.15);
    bc_mp1.SetXNNeumannBc1D(0);
    bc_mp2.SetX0NeumannBc1D(1.0);
    bc_mp2.SetXNNeumannBc1D(0);
    // bc_mp1.SetX0NeumannBc1D(0);
    // bc_mp1.SetXNDirichletBc1D(0);
    // bc_mp1.SetXNNeumannBc1D(0);
    // bc_mp1.SetXNRobinBc1D(0);
    BvpPde oxygen(&ode_mp1, &bc_mp1,0.001, 1.0, 10.0, 128, uj0);
    BvpPde organic_mater_1(&ode_mp2, &bc_mp2,0.001, 1.0, 10.0, 128, uj0);
    // BvpOde oxygen(&ode_mp1, &bc_mp1, 10);
    oxygen.SetFilename("oxygen_results.dat");
    organic_mater_1.SetFilename("om1_results.dat");
    // oxygen.Solve();
    CoupledPde cpde(&oxygen, &organic_mater_1);
    cpde.SolveSystem();

    Gnuplot g1, g2;
    g1.set_style("lines").plot_xy(oxygen.mpGrid->xGrid,oxygen.solution,"differentiation");
    g2.set_style("lines").plot_xy(organic_mater_1.mpGrid->xGrid,organic_mater_1.solution,"differentiation");
    // g1.set_style("lines").plot_equation("0.5*x*(1-x)","exact solution");

    // Gnuplot g2;
    // SecondOrderOde ode_mp2(10.0, 5.0, 0.0, uj0, 0.0, 20.0);
    // BoundaryConditions bc_mp2;
    // bc_mp2.SetX0NeumannBc1D(10.0);
    // bc_mp2.SetXNDirichletBc1D(0.0);
    // BvpOde organic_mater_1(&ode_mp2, &bc_mp2, 64);
    // organic_mater_1.SetFilename("model_problem_results2.dat");
    // organic_mater_1.Solve();
    // g2.set_style("points").plot_xy(organic_mater_1.mpGrid->xGrid,organic_mater_1.solution);
    // g2.set_style("lines").plot_equation("(4*exp(x) + exp(-4*x) ) / (4*exp(pi)+exp(-4*pi))-5*sin(x)-3*cos(x)","exact solution");

    return 0;
}


/**
 * @file    pose_estimation_3d2d.cpp
 * @version v1.0.0
 * @date    Oct,27 2017
 * @author  jacob.lin
 */

#include <iostream>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

#include <chrono>

using namespace std;
using namespace cv;

/* find the two photo feature matches points */
void find_feature_matches(const cv::Mat&, const cv::Mat&, std::vector<cv::KeyPoint>&, std::vector<cv::KeyPoint>&, std::vector<cv::DMatch>&);

/* Converts the pixel coordinate system to the normalized imaging plane coordinate system */
cv::Point2d pixel2cam(const cv::Point2d&, const cv::Mat&);

/*  */
void bundleAdjustment( const std::vector<cv::Point3f>, const std::vector<cv::Point2f>, const cv::Mat&, cv::Mat&, cv::Mat&);

int main(int argc, char** argv)
{
    if (argc != 5) {
        cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2." << endl;
        return 1;
    }

    //-- 1st, read photo
    cv::Mat img_1 = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
    cv::Mat img_2 = cv::imread(argv[2], CV_LOAD_IMAGE_COLOR);

    if (img_1.empty() || img_2.empty()) {
        cout << "img1 or img2 is no find." << endl;
        return 1;
    }

    //-- 2nd, find the two photo feature matches points
    std::vector<cv::KeyPoint> keypoints_1, keypoints_2;
    std::vector<cv::DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout << "feature matches points total is " << matches.size() << endl;

    //-- 3rd, create 3 dimensions point correspondences
    cv::Mat d1 = cv::imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);      // 深度图为16位无符号数，单通道图像
    cv::Mat K = (cv::Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    std::vector<cv::Point3f> pts_3d;
    std::vector<cv::Point2f> pts_2d;
    for (cv::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;
        cv::Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        pts_3d.push_back(cv::Point3d (p1.x*dd, p1.y*dd, dd));
        pts_2d.push_back(keypoints_2[m.trainIdx].pt);
    }
    cout << "3d-2d pairs: " << pts_3d.size() << endl;

    //-- 4th, use PnP or bundle adjustment compute camera pose.
    cv::Mat r, t;
    cv::solvePnP(pts_3d, pts_2d, K, cv::Mat(), r, t, false);    // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    cv::Mat R;
    cv::Rodrigues(r, R);    // r为旋转向量形式，用Rodrigues公式转换为矩阵

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

    cout << "calling bundle adjustment" << endl;

    bundleAdjustment(pts_3d, pts_2d, K, R, t);

    return 0;
}

/**
 * find the two photo feature matches points
 */
void find_feature_matches(const cv::Mat& img_1, const cv::Mat& img_2, std::vector<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2, std::vector<cv::DMatch>& matches)
{
    //-- Initialize
    cv::Mat descriptors_1, descriptors_2;
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

    //-- 1st: detect Oriented FAST KeyPoint
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //-- 2nd: Calculates BRIEF descriptors with KeyPoint's coordinate
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

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

    //-- 4th: 匹配点对筛选
    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 distance : %f \r\n", max_dist);
    printf("-- Min distance : %f \r\n", min_dist);

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

/**
 * Converts the pixel coordinate system to the normalized imaging plane coordinate system
 */
cv::Point2d pixel2cam(const cv::Point2d& p, const cv::Mat& K)
{
    return cv::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 bundleAdjustment(
    const std::vector<cv::Point3f> points_3d,
    const std::vector<cv::Point2f> points_2d,
    const cv::Mat& K,
    cv::Mat& R,
    cv::Mat& t )
{
    // Initialize g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block;   // pose 维度为 6, landmark 维度为 3
    Block::LinearSolverType* linear_solver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block((std::unique_ptr<Block::LinearSolverType>)(linear_solver));
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg((std::unique_ptr<Block>)solver_ptr);
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();    // camera pose
    Eigen::Matrix3d R_mat;
    R_mat << \
        R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), \
        R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), \
        R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2);
    pose->setId(0);
    pose->setEstimate(
        g2o::SE3Quat(
            R_mat,
            Eigen::Vector3d(t.at<double>(0, 0), t.at<double>(1, 0), t.at<double>(2, 0))
        )
    );
    optimizer.addVertex(pose);

    int index = 1;
    for (const cv::Point3f p:points_3d) {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId(index ++);
        point->setEstimate(Eigen::Vector3d(p.x, p.y, p.z));
        point->setMarginalized(true);   // g2o 中必须设置 marg
        optimizer.addVertex(point);
    }

    // parameter : camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters(
        K.at<double>(0, 0), Eigen::Vector2d(K.at<double>(0, 2), K.at<double>(1, 2)), 0
    );
    camera->setId(0);
    optimizer.addParameter(camera);

    // edges
    index = 1;
    for (const cv::Point2f p:points_2d) {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId(index);
        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(index)));
        edge->setVertex(1, pose);
        edge->setMeasurement(Eigen::Vector2d(p.x, p.y));
        edge->setParameterId(0, 0);
        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(100);
    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 << endl << "after optimization: " << endl;
    cout << "T = " << endl << Eigen::Isometry3d(pose->estimate()).matrix() << endl;
}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#ifndef quantext_nadaraya_watson_regression_hpp
#define quantext_nadaraya_watson_regression_hpp

#include <ql/math/comparison.hpp>

#include <boost/make_shared.hpp>

/*! \file qle/math/nadarayawatson.hpp
    \brief Nadaraya-Watson regression
    \ingroup math
*/

namespace QuantExt {
using QuantLib::Real;
using QuantLib::Size;
namespace detail {

//! Regression impl
/*! \ingroup math
 */
class RegressionImpl {
public:
    virtual ~RegressionImpl() {}
    virtual void update() = 0;
    virtual Real value(Real x) const = 0;
    virtual Real standardDeviation(Real x) const = 0;
};

//! Nadaraya Watson impl
/*! \ingroup math
 */
template <class I1, class I2, class Kernel> class NadarayaWatsonImpl : public RegressionImpl {
public:
    /*! \pre the \f$ x \f$ values must be sorted.
        \pre kernel needs a Real operator()(Real x) implementation
    */
    NadarayaWatsonImpl(const I1& xBegin, const I1& xEnd, const I2& yBegin, const Kernel& kernel)
        : xBegin_(xBegin), xEnd_(xEnd), yBegin_(yBegin), kernel_(kernel) {}

    void update() {}

    Real value(Real x) const {

        Real tmp1 = 0.0, tmp2 = 0.0;

        for (Size i = 0; i < static_cast<Size>(xEnd_ - xBegin_); ++i) {
            Real tmp = kernel_(x - xBegin_[i]);
            tmp1 += yBegin_[i] * tmp;
            tmp2 += tmp;
        }

        return QuantLib::close_enough(tmp2, 0.0) ? 0.0 : tmp1 / tmp2;
    }

    Real standardDeviation(Real x) const {

        Real tmp1 = 0.0, tmp1b = 0.0, tmp2 = 0.0;

        for (Size i = 0; i < static_cast<Size>(xEnd_ - xBegin_); ++i) {
            Real tmp = kernel_(x - xBegin_[i]);
            tmp1 += yBegin_[i] * tmp;
            tmp1b += yBegin_[i] * yBegin_[i] * tmp;
            tmp2 += tmp;
        }

        return QuantLib::close_enough(tmp2, 0.0) ? 0.0 : std::sqrt(tmp1b / tmp2 - (tmp1 * tmp1) / (tmp2 * tmp2));
    }

private:
    I1 xBegin_, xEnd_;
    I2 yBegin_;
    Kernel kernel_;
};
} // namespace detail

//! Nadaraya Watson regression
/*! This implements the estimator

    \f[
    m(x) = \frac{\sum_i y_i K(x-x_i)}{\sum_i K(x-x_i)}
    \f]

    \ingroup math
*/
class NadarayaWatson {
public:
    /*! \pre the \f$ x \f$ values must be sorted.
        \pre kernel needs a Real operator()(Real x) implementation
    */
    template <class I1, class I2, class Kernel>
    NadarayaWatson(const I1& xBegin, const I1& xEnd, const I2& yBegin, const Kernel& kernel) {
        impl_ = boost::make_shared<detail::NadarayaWatsonImpl<I1, I2, Kernel> >(xBegin, xEnd, yBegin, kernel);
    }

    Real operator()(Real x) const { return impl_->value(x); }

    Real standardDeviation(Real x) const { return impl_->standardDeviation(x); }

private:
    boost::shared_ptr<detail::RegressionImpl> impl_;
};

} // namespace QuantExt

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_BY_TRIANGLE_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_BY_TRIANGLE_HPP

#include <boost/mpl/if.hpp>
#include <boost/type_traits.hpp>

#include <boost/geometry/arithmetic/determinant.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>
#include <boost/geometry/strategies/side.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Check at which side of a segment a point lies:
    left of segment (> 0), right of segment (< 0), on segment (0)
\ingroup strategies
\tparam CalculationType \tparam_calculation
 */
template <typename CalculationType = void>
class side_by_triangle
{
public :

    // Template member function, because it is not always trivial
    // or convenient to explicitly mention the typenames in the
    // strategy-struct itself.

    // Types can be all three different. Therefore it is
    // not implemented (anymore) as "segment"

    template <typename P1, typename P2, typename P>
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename boost::mpl::if_c
            <
                boost::is_void<CalculationType>::type::value,
                typename select_most_precise
                    <
                        typename select_most_precise
                            <
                                typename coordinate_type<P1>::type,
                                typename coordinate_type<P2>::type
                            >::type,
                        typename coordinate_type<P>::type
                    >::type,
                CalculationType
            >::type coordinate_type;

        coordinate_type const x = get<0>(p);
        coordinate_type const y = get<1>(p);

        coordinate_type const sx1 = get<0>(p1);
        coordinate_type const sy1 = get<1>(p1);
        coordinate_type const sx2 = get<0>(p2);
        coordinate_type const sy2 = get<1>(p2);

        // Promote float->double, small int->int
        typedef typename select_most_precise
            <
                coordinate_type,
                double
            >::type promoted_type;

        promoted_type const dx = sx2 - sx1;
        promoted_type const dy = sy2 - sy1;
        promoted_type const dpx = x - sx1;
        promoted_type const dpy = y - sy1;

        promoted_type const s
            = geometry::detail::determinant<promoted_type>
                (
                    dx, dy,
                    dpx, dpy
                );

        promoted_type const zero = promoted_type();
        return math::equals(s, zero) ? 0
            : s > zero ? 1
            : -1;
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType>
struct default_strategy<cartesian_tag, CalculationType>
{
    typedef side_by_triangle<CalculationType> type;
};

}
#endif

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_SIDE_BY_TRIANGLE_HPP


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

#include <Eigen/SVD>
#include <calibration_algorithms/plane_to_plane_calibration.h>

namespace unipd
{
namespace calib
{

Transform3
PlaneToPlaneCalibration::estimateTransform (const std::vector<std::pair<Plane3, Plane3>> & plane_pair_vector)
{
  const Size1 SIZE = plane_pair_vector.size();
  assert(SIZE >= 10);

  Eigen::Matrix<Scalar, Eigen::Dynamic, 3> normals_1(SIZE, 3);
  Eigen::Matrix<Scalar, Eigen::Dynamic, 3> normals_2(SIZE, 3);
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> distances_1(SIZE);
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> distances_2(SIZE);

  for (int i = 0; i < SIZE; ++i)
  {
    const Plane3 & plane_1 = plane_pair_vector[i].first;
    const Plane3 & plane_2 = plane_pair_vector[i].second;

    if (plane_1.offset() > 0)
    {
      normals_1.row(i) = -plane_1.normal();
      distances_1(i) = plane_1.offset();
    }
    else
    {
      normals_1.row(i) = plane_1.normal();
      distances_1(i) = -plane_1.offset();
    }

    if (plane_2.offset() > 0)
    {
      normals_2.row(i) = -plane_2.normal();
      distances_2(i) = plane_2.offset();
    }
    else
    {
      normals_2.row(i) = plane_2.normal();
      distances_2(i) = -plane_2.offset();
    }
  }

  Transform3 transform;

  Eigen::Matrix<Scalar, 3, 3> USV = normals_2.transpose() * normals_1;
  Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd;
  svd.compute(USV, Eigen::ComputeFullU | Eigen::ComputeFullV);
  transform.linear() = svd.matrixV() * svd.matrixU().transpose();

//  transform.translation() = (normals_1.transpose() * normals_1).inverse() * normals_1.transpose() * (distances_1 - distances_2);

  // Use QR decomposition
  Eigen::ColPivHouseholderQR<Eigen::Matrix<Scalar, Eigen::Dynamic, 3> > qr(normals_1.rows(), 3);
  qr.compute(normals_1);
  Eigen::Matrix<Scalar, Eigen::Dynamic, 3> R = qr.matrixR().template triangularView<Eigen::Upper>();
  transform.translation() = (qr.colsPermutation() * R.transpose() * R * qr.colsPermutation().transpose()).inverse() * normals_1.transpose() * (distances_1 - distances_2);

  return transform;
}

} // namespace calib
} // namespace unipd