/**************************************************************************
Copyright (c) 2012, Julio C. Estrada
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

+ Redistributions of source code must retain the above copyright notice,
  this list of conditions and the following disclaimer.

+ Redistributions in binary form must reproduce the above copyright
  notice, this list of conditions and the following disclaimer in the
  documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**************************************************************************/

#include "utils.h"
#include <cmath>
#include <Eigen/Dense>

#define cimg_display 0
#include <CImg.h>

void gradient(const Eigen::ArrayXXf& I, Eigen::ArrayXXf& dx, Eigen::ArrayXXf& dy)
{
  dx.resize(I.rows(), I.cols());
  dy.resize(I.rows(), I.cols());

  for(int i=0; i<I.rows()-1; i++)
    for(int j=0; j<I.cols()-1; j++){
      dx(i,j) = I(i,j+1)-I(i,j);
      dy(i,j) = I(i+1,j)-I(i,j);
    }
  for(int i=0; i<I.rows(); i++)
    dy(i,I.cols()-1)=dy(i,I.cols()-2);
  for(int i=0; i<I.cols(); i++)
    dx(I.rows()-1,i)=dx(I.rows()-2,i);
}

void parabola(Eigen::ArrayXXf& mat, float A)
{
  const int M=mat.rows(), N=mat.cols();
  const float cm=M/2.0, cn=N/2.0;

  for(int i=0; i<M; i++)
    for(int j=0; j<N; j++)
      mat(i,j)= A*((i-cm)*(i-cm) + (j-cn)*(j-cn));
}

void cosine(const Eigen::ArrayXXf& angle, Eigen::ArrayXXf& cc)
{
  const int M=angle.rows(), N=angle.cols();
  for(int i=0; i<M; i++)
    for(int j=0; j<N; j++)
      cc(i,j)=cos(angle(i,j));
}

void linspaced(Eigen::ArrayXXf& X, Eigen::ArrayXXf& Y, const float bx, float ex,
               const float by, float ey,
               const int M, const int N)
{
  X.resize(M,N);
  Y.resize(M,N);
  const float stepx = (ex-bx)/(float)(N-1);
  const float stepy = (ey-by)/(float)(M-1);
  float *const __restrict__ ptx=(float*)X.data();
  float *const __restrict__ pty=(float*)Y.data();
  size_t idx;

  float x=bx;
  float y=by;
  for(int i=0; i<M; i++){
    idx=N*i;
    x=bx;
    for(int j=0; j<N; j++){
      ptx[idx+j]=x;
      pty[idx+j]=y;
      x+=stepx;
    }
    y+=stepy;
  }
}

Eigen::ArrayXXf peaks(const int M, const int N)
{
  Eigen::ArrayXXf X, Y;
  Eigen::ArrayXXf p(M,N);
  Eigen::ArrayXXf tmp;
  linspaced(X, Y, -3.0, 3.0, -3.0, 3.0, M, N);

  p = 1 - X/2.0;
  tmp = X.pow(5);
  p+= tmp;
  tmp = Y.pow(3);
  p+= tmp;

  tmp = (-X*X-Y*Y).exp();
  p*=tmp;

  return p;
}

Eigen::ArrayXXf ramp(float wx, float wy, const int M, const int N)
{
  Eigen::ArrayXXf X, Y, res;
  linspaced(X, Y, 0, M-1, 0, N-1, M, N);
  res = wx*X + wy*Y;

  return res;
}

Eigen::ArrayXXf speckle_peaks(const int M, const int N, const float magn,
               const int speckle_size)
{
  using namespace cimg_library;
  Eigen::ArrayXXf Ic0;
  Eigen::ArrayXXf Ic1;
  Eigen::ArrayXXf p = peaks(M,N)*magn;
  Eigen::ArrayXXf noise0(M,N);
  Eigen::ArrayXXf noise1(M,N);
  CImg<float> n0(noise0.data(), noise0.cols(), noise0.rows(), 1, 1, true);
  CImg<float> n1(noise1.data(), noise1.cols(), noise1.rows(), 1, 1, true);
  const float pi=M_PI;

  float sigma = speckle_size/6.0;
  n0.rand(-pi, pi);
  n1.rand(-pi, pi);
  n0.blur(sigma);
  n1.blur(sigma);
  noise0 = mapRange(noise0, -pi, pi);
  noise1 = mapRange(noise1, -pi, pi);

  Ic0 = wphase(noise0);
  Ic1 = wphase(noise1 + p + pi/2.);

  return wphase(Ic0-Ic1);
}

Eigen::ArrayXXf wphase(const Eigen::ArrayXXf& p)
{
  Eigen::ArrayXXf wp(p.rows(), p.cols());
  float *const __restrict__ pt_wp = (float*)wp.data();
  float *const __restrict__ pt_p = (float*)p.data();
  size_t idx;

  for(int i=0; i<p.rows(); i++){
    idx=i*p.cols();
    for(int j=0; j<p.cols(); j++)
      pt_wp[idx+j] = atan2(sin(pt_p[idx+j]), cos(pt_p[idx+j]));
  }

  return wp;
}

Eigen::ArrayXXf mapRange(const Eigen::ArrayXXf& mat, float a, float b)
{
  Eigen::ArrayXXf img;
  double min = mat.minCoeff();
  double max = mat.maxCoeff();

  float m = (b-a)/(max-min);

  img = m*(mat-min) + a;
  return img;
}


#pragma once

#include <algorithm> // std::minmax

#include <boost/variant.hpp> // boost::variant

#include "convex_polygon.hpp"
#include "point.hpp"
#include "rectangle.hpp"
#include "segment.hpp"

// TODO uncheck this

namespace geometry {

// ------------------------------------------------------------------------
// Segment
// ------------------------------------------------------------------------

// These could return a point or an interval
template <typename N, Topology Top>
boost::variant<N, Interval<N, Top>> project_impl(const N& first, const N& second) {

    const auto order = first.compare(second);

    if (order < 0) {
        return Interval<N, Top>{first, second};
    }

    if (order > 0) {
        return Interval<N, Top>{second, first};
    }

    // Else they are equal, so just return the point
    return first;
}

template <typename N, Topology Top>
boost::variant<N, Interval<N, Top>> project_x(const Segment<N, Top>& seg) {

    return project_impl<N, Top>(seg.start().x, seg.end().x);
}

template <typename N, Topology Top>
boost::variant<N, Interval<N, Top>> project_y(const Segment<N, Top>& seg) {

    return project_impl<N, Top>(seg.start().y, seg.end().y);
}

template <typename N, Topology Top>
boost::variant<N, Interval<N, Top>> project(const Segment<N, Top>& seg, const Point<N>& axis) {

    const auto start_dot = Point<N>::dot(seg.start(), axis);
    const auto end_dot = Point<N>::dot(seg.end(), axis);

    return project_impl<N, Top>(start_dot, end_dot);
}

// ------------------------------------------------------------------------
// Rectangle
// ------------------------------------------------------------------------

// Rectangles are always non-degenerate, so projection will always return intervals,
// never points.

template <typename N, Topology Top>
Interval<N, Top> project_x(const Rectangle<N, Top>& rect) {
    return rect.interval_x;
}

template <typename N, Topology Top>
Interval<N, Top> project_y(const Rectangle<N, Top>& rect) {
    return rect.interval_y;
}

template <typename N, Topology Top>
Interval<N, Top> project(const Rectangle<N, Top>& rect, const Point<N>& axis) {

    const auto ll_dot = Point<N>::dot(rect.lower_left(), axis);

    const auto ul_dot = Point<N>::dot(rect.upper_left(), axis);

    const auto ur_dot = Point<N>::dot(rect.upper_right(), axis);

    const auto lr_dot = Point<N>::dot(rect.lower_right(), axis);

    const auto minmax = std::minmax({ll_dot, ul_dot, ur_dot, lr_dot});

    return {minmax.first, minmax.second};
}

// ------------------------------------------------------------------------
// Convex Polygon
// ------------------------------------------------------------------------

// Polygons are also always non-degenerate, so they always return intervals,
// never points.

// Find the min and max x values of all the points
template <typename N, Topology Top>
Interval<N, Top> project_x(const ConvexPolygon<N, Top>& poly) {

    const auto& first = poly.vertex(0);

    // Note: the polygon is normalized, so the first point always has the smallest
    // x coordinate.
    const auto min = first.x;
    auto max = first.x;

    for (size_t i = 1; i < poly.size(); ++i) {

        const auto& point = poly.vertex(i);

        if (point.x > max) {
            max = point.x;
        }
    }

    return {min, max};
}

// Find the min and max y values of all the points
template <typename N, Topology Top>
Interval<N, Top> project_y(const ConvexPolygon<N, Top>& poly) {

    const auto& first = poly.vertex(0);

    auto min = first.y;
    auto max = first.y;

    for (size_t i = 1; i < poly.size(); ++i) {

        const auto& point = poly.vertex(i);

        // We know min <= max, so we only need to do the following comparisons

        if (point.y < min) {
            min = point.y;
        } else if (point.y > max) {
            max = point.y;
        }

        // else min <= y <= max, so there are no changes to make
    }

    return {min, max};
}

template <typename N, Topology Top>
Interval<N, Top> project(const ConvexPolygon<N, Top>& poly, const Point<N>& axis) {

    const auto& first = poly.vertex(0);

    const auto first_dot = Point<N>::dot(first, axis);

    auto min = first_dot;
    auto max = first_dot;

    for (size_t i = 1; i < poly.size(); ++i) {

        const auto& point = poly.vertex(i);

        const auto dot = Point<N>::dot(point, axis);

        // We know min <= max, so we only need to do the following comparisons

        if (dot < min) {
            min = dot;
        } else if (dot > max) {
            max = dot;
        }

        // else min <= dot <= max, so there are no changes to make
    }

    return {min, max};
}
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

**/
//==================================================================================================
#ifndef BOOST_SIMD_ARITHMETIC_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-arithmetic Arithmetic functions

    Those functions provides scalar and SIMD algorithms for classical arithmetic operators and
    functions provided by the C and C++ standard library. Other functions are also provided, in particular,
    provision for saturated operations through the use of a @ref decorator.

     - **Possibly saturated operations**

       The functors:

         | name       | name            | name         | name             |
         |:----------:|:---------------:|:------------:|:----------------:|
         | @ref abs   | @ref minus      | @ref oneminus| @ref unary_minus |
         | @ref dec   | @ref multiplies | @ref sqr     | @ref unary_plus  |
         | @ref dist  | @ref plus       | @ref toint   | @ref touint      |
         | @ref inc   |                 |              |                  |

       can be decorated with the  @ref saturated "saturated_" @ref decorator. This decorator has no effect on floating
       calls,  but on integer calls replaces the operation by its saturated equivalent.

       Typically overflows will be replaced by the @ref Valmin/@ref Valmax proper value instead of providing
       undefined behaviour for signed integral types or wrapping modulo Valmax+1 for unsigned ones.

       Peculiarly saturated_(@ref abs) and saturated_(@ref dist) ensure that the result will never be stricly
       negative (which is for instance the case of abs(Valmin<T>()) for T being any signed integral type).

       toint is a rather common operation as it converts floating number to signed integers of the same bit size,
       nevertheless it probably is its saturated version you have to use because it acts properly on large or not finite
       values, this is why an alias for saturated_(toint) is provided as @ref ifix.

     - **Rounding operations**

         | name        | name            | name        | name             |
         |:-----------:|:---------------:|:-----------:|:----------------:|
         | @ref ceil   | @ref round      | @ref ifloor |  @ref inearbyint |
         | @ref floor  | @ref nearbyint  | @ref ifix   |                  |
         | @ref fix    | @ref iceil      | @ref iround |                  |

          -The operations prefixed by 'i' return a value of the integral type associated to the entry type. (If T is
         the entry type it is @ref as_integer_t<T>)

          -The other ones return the same type as the entry.

     - **Division operations**

       @ref divides is the function associated to standard division. There is another one which provides more
       flexibility, namely rounded divisions.

       With two parameters @ref div and @ref divides are equivalent, but @ref div can admit a first option parameter
       that modifies its behaviour.

       The option parameter is described in the following table where a and b are of type T,  fT is a
       supposed floating type associated to T (@ref as_floating_t<T> if it exists) and iT is the integer type associated to T
       (@ref as_integer_t<T>). (fT and iT are here only to support pseudo code description)

         | option             |          call            |      result similar to               |
         |--------------------|--------------------------|--------------------------------------|
         | @ref ceil          |   div(ceil, a, b)        |      T(ceil(fT(a)/fT(b)))            |
         | @ref floor         |   div(floor, a, b)       |      T(floor(fT(a)/fT(b)))           |
         | @ref fix           |   div(fix, a, b)         |      T(fix(fT(a)/fT(b)))             |
         | @ref round         |   div(round, a, b)       |      T(round(fT(a)/fT(b)))           |
         | @ref nearbyint     |   div(nearbyint, a, b)   |      T(nearbyint(fT(a)/fT(b)))       |
         | @ref iceil         |   div(iceil, a, b)       |      iT(iceil(fT(a)/fT(b)))          |
         | @ref ifloor        |   div(ifloor, a, b)      |      iT(ifloor(fT(a)/fT(b)))         |
         | @ref ifix          |   div(ifix, a, b)        |      iT(ifix(fT(a)/fT(b)))           |
         | @ref iround        |   div(iround, a, b)      |      iT(iround(fT(a)/fT(b)))         |
         | @ref inearbyint    |   div(inearbyint, a, b)  |      iT(inearbyint(fT(a)/fT(b)))     |

     - **Remainder operations**

       @ref rem is the remander functor providing same kind of facilities as @ref div

       With two parameters rem(a, b) is equivalent to  rem(fix, a, b), but rem can admit
       a first optional parameter that modifies its behaviour and moreover can use the fast_ decorator if
       limiting case values are not a problem.

       The option parameter can be chosen between @ref ceil, @ref floor, @ref fix, @ref round, @ref nearbyint and if opt is the option,
       the call:

         rem(opt, a, b) is equivalent to a-b*div(opt, a, b)

       For floating entries the underlisted corner cases are handled in the following way (unless the @ref fast "fast_"
       decorator is used, the result in this case being undefined)
        -  if x is \f$\pm\infty\f$ , @ref Nan is returned
        -  if x is \f$\pm0\f$ and y is not 0 x is returned
        -  If y is \f$\pm0\f$, @ref Nan is returned
        -  If either argument is a nan, @ref a nan is returned

     - **complex operations**

       Boost.SIMD  does not provides complex number operations yet, but it will soon. So the following functors that
       have a meaning as a restriction to real number of complex functions, can be seen as a prequel:

        | name        | name            | name        | name             | name             |
        |:-----------:|:---------------:|:-----------:|:----------------:|:----------------:|
        | @ref arg    | @ref conj       | @ref imag   | @ref real        | @ref sqr_abs     |

        For real entries conj and real are identity,  imag always 0, sqr_abs coincide with sqr
        and arg results are always in the set \f$\{0, \pi,  Nan\}\f$

     - **Fused multiply-add operations**

        | name            | name            | name         | name             |
        |:---------------:|:---------------:|:------------:|:----------------:|
        | @ref fma        | @ref fms        | @ref fnma    | @ref fnms        |
        | @ref correct_fma| @ref two_add,   | @ref two_prod| @ref two_split   |

      Correct fused multiply/add implies

      - only one rounding
      - no "intermediate" overflow

      f?m? and fm? family provides this each time it is reasonable
      in terms of performance (mainly if the system has the hard
      wired capability).

      If you need "real" fma capabilities in all circumstances in your own
      code you can use @ref correct_fma (although it can be expansive) or
      (generally still more expansive) std_(fma) by using the decorator.

      @ref two_add, @ref two_prod and @ref two_split are used internally in @ref correct_fma and
      can be useful in searching extra-accuracy in other circumstances as double-double
      computations.

      @ref correct_fma is never used internally by Boost.SIMD

      - **Standard operations**

       The stdlibc++ provides them but only in scalar mode:

         | name       | name            | name         |
         |:----------:|:---------------:|:------------:|
         | @ref abs   | @ref hypot      | @ref remquo  |
         | @ref ceil  | @ref max        | @ref round   |
         | @ref floor | @ref min        | @ref signbit |
         | @ref fma   | @ref rem (%)    | @ref sqrt    |

       Boost.SIMD provides its own scalar and simd versions, but allows
       the use of the @ref std "std_" @ref decorator to call the associated system
       library function if the user needs it.

      - **Other operations**

         | name         | name            | name          |
         |:------------:|:---------------:|:-------------:|
         | @ref average | @ref sqrt       | @ref tenpower |
         | @ref meanof  | @ref sqr        | @ref tofloat  |
         | @ref minmod  | @ref sqrt1pm1   |               |

  **/
} }

#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/arg.hpp>
#include <boost/simd/function/average.hpp>
#include <boost/simd/function/ceil.hpp>
#include <boost/simd/function/conj.hpp>
#include <boost/simd/function/correct_fma.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/dist.hpp>
#include <boost/simd/function/div.hpp>
#include <boost/simd/function/extract.hpp>
#include <boost/simd/function/fix.hpp>
#include <boost/simd/function/floor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/fms.hpp>
#include <boost/simd/function/fnma.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/function/hypot.hpp>
#include <boost/simd/function/iceil.hpp>
#include <boost/simd/function/ifix.hpp>
#include <boost/simd/function/ifloor.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/inearbyint.hpp>
#include <boost/simd/function/iround.hpp>
#include <boost/simd/function/max.hpp>
#include <boost/simd/function/meanof.hpp>
#include <boost/simd/function/min.hpp>
#include <boost/simd/function/minmod.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/rem.hpp>
#include <boost/simd/function/remquo.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/round.hpp>
#include <boost/simd/function/rsqrt.hpp>
#include <boost/simd/function/signbit.hpp>
#include <boost/simd/function/sqr_abs.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/sqrt1pm1.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/function/tenpower.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/function/touint.hpp>
#include <boost/simd/function/trunc.hpp>
#include <boost/simd/function/two_add.hpp>
#include <boost/simd/function/two_prod.hpp>
#include <boost/simd/function/two_split.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <boost/simd/function/unary_plus.hpp>

#endif


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

// See: https://blogs.mathworks.com/cleve/2019/04/29/makima-piecewise-cubic-interpolation/
// And: https://doi.org/10.1145/321607.321609

#ifndef BOOST_MATH_INTERPOLATORS_MAKIMA_HPP
#define BOOST_MATH_INTERPOLATORS_MAKIMA_HPP
#include <memory>
#include <cmath>
#include <boost/math/interpolators/detail/cubic_hermite_detail.hpp>

namespace boost {
namespace math {
namespace interpolators {

template<class RandomAccessContainer>
class makima {
public:
    using Real = typename RandomAccessContainer::value_type;

    makima(RandomAccessContainer && x, RandomAccessContainer && y,
           Real left_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN(),
           Real right_endpoint_derivative = std::numeric_limits<Real>::quiet_NaN())
    {
        using std::isnan;
        using std::abs;
        if (x.size() < 4)
        {
            throw std::domain_error("Must be at least four data points.");
        }
        RandomAccessContainer s(x.size(), std::numeric_limits<Real>::quiet_NaN());
        Real m2 = (y[3]-y[2])/(x[3]-x[2]);
        Real m1 = (y[2]-y[1])/(x[2]-x[1]);
        Real m0 = (y[1]-y[0])/(x[1]-x[0]);
        // Quadratic extrapolation: m_{-1} = 2m_0 - m_1:
        Real mm1 = 2*m0 - m1;
        // Quadratic extrapolation: m_{-2} = 2*m_{-1}-m_0:
        Real mm2 = 2*mm1 - m0;
        Real w1 = abs(m1-m0) + abs(m1+m0)/2;
        Real w2 = abs(mm1-mm2) + abs(mm1+mm2)/2;
        if (isnan(left_endpoint_derivative))
        {
            s[0] = (w1*mm1 + w2*m0)/(w1+w2);
            if (isnan(s[0]))
            {
                s[0] = 0;
            }
        }
        else
        {
            s[0] = left_endpoint_derivative;
        }

        w1 = abs(m2-m1) + abs(m2+m1)/2;
        w2 = abs(m0-mm1) + abs(m0+mm1)/2;
        s[1] = (w1*m0 + w2*m1)/(w1+w2);
        if (isnan(s[1])) {
            s[1] = 0;
        }

        for (decltype(s.size()) i = 2; i < s.size()-2; ++i) {
            Real mim2 = (y[i-1]-y[i-2])/(x[i-1]-x[i-2]);
            Real mim1 = (y[i  ]-y[i-1])/(x[i  ]-x[i-1]);
            Real mi   = (y[i+1]-y[i  ])/(x[i+1]-x[i  ]);
            Real mip1 = (y[i+2]-y[i+1])/(x[i+2]-x[i+1]);
            w1 = abs(mip1-mi) + abs(mip1+mi)/2;
            w2 = abs(mim1-mim2) + abs(mim1+mim2)/2;
            s[i] = (w1*mim1 + w2*mi)/(w1+w2);
            if (isnan(s[i])) {
                s[i] = 0;
            }
        }
        // Quadratic extrapolation at the other end:

        decltype(s.size()) n = s.size();
        Real mnm4 = (y[n-3]-y[n-4])/(x[n-3]-x[n-4]);
        Real mnm3 = (y[n-2]-y[n-3])/(x[n-2]-x[n-3]);
        Real mnm2 = (y[n-1]-y[n-2])/(x[n-1]-x[n-2]);
        Real mnm1 = 2*mnm2 - mnm3;
        Real mn = 2*mnm1 - mnm2;
        w1 = abs(mnm1 - mnm2) + abs(mnm1+mnm2)/2;
        w2 = abs(mnm3 - mnm4) + abs(mnm3+mnm4)/2;

        s[n-2] = (w1*mnm3 + w2*mnm2)/(w1 + w2);
        if (isnan(s[n-2])) {
            s[n-2] = 0;
        }

        w1 = abs(mn - mnm1) + abs(mn+mnm1)/2;
        w2 = abs(mnm2 - mnm3) + abs(mnm2+mnm3)/2;

        if (isnan(right_endpoint_derivative))
        {
            s[n-1] = (w1*mnm2 + w2*mnm1)/(w1+w2);
            if (isnan(s[n-1])) {
                s[n-1] = 0;
            }
        }
        else
        {
            s[n-1] = right_endpoint_derivative;
        }

        impl_ = std::make_shared<detail::cubic_hermite_detail<RandomAccessContainer>>(std::move(x), std::move(y), std::move(s));
    }

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

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

    friend std::ostream& operator<<(std::ostream & os, const makima & m)
    {
        os << *m.impl_;
        return os;
    }

    void push_back(Real x, Real y) {
        using std::abs;
        using std::isnan;
        if (x <= impl_->x_.back()) {
             throw std::domain_error("Calling push_back must preserve the monotonicity of the x's");
        }
        impl_->x_.push_back(x);
        impl_->y_.push_back(y);
        impl_->dydx_.push_back(std::numeric_limits<Real>::quiet_NaN());
        // dydx_[n-2] was computed by extrapolation. Now dydx_[n-2] -> dydx_[n-3], and it can be computed by the same formula.
        decltype(impl_->size()) n = impl_->size();
        auto i = n - 3;
        Real mim2 = (impl_->y_[i-1]-impl_->y_[i-2])/(impl_->x_[i-1]-impl_->x_[i-2]);
        Real mim1 = (impl_->y_[i  ]-impl_->y_[i-1])/(impl_->x_[i  ]-impl_->x_[i-1]);
        Real mi   = (impl_->y_[i+1]-impl_->y_[i  ])/(impl_->x_[i+1]-impl_->x_[i  ]);
        Real mip1 = (impl_->y_[i+2]-impl_->y_[i+1])/(impl_->x_[i+2]-impl_->x_[i+1]);
        Real w1 = abs(mip1-mi) + abs(mip1+mi)/2;
        Real w2 = abs(mim1-mim2) + abs(mim1+mim2)/2;
        impl_->dydx_[i] = (w1*mim1 + w2*mi)/(w1+w2);
        if (isnan(impl_->dydx_[i])) {
            impl_->dydx_[i] = 0;
        }

        Real mnm4 = (impl_->y_[n-3]-impl_->y_[n-4])/(impl_->x_[n-3]-impl_->x_[n-4]);
        Real mnm3 = (impl_->y_[n-2]-impl_->y_[n-3])/(impl_->x_[n-2]-impl_->x_[n-3]);
        Real mnm2 = (impl_->y_[n-1]-impl_->y_[n-2])/(impl_->x_[n-1]-impl_->x_[n-2]);
        Real mnm1 = 2*mnm2 - mnm3;
        Real mn = 2*mnm1 - mnm2;
        w1 = abs(mnm1 - mnm2) + abs(mnm1+mnm2)/2;
        w2 = abs(mnm3 - mnm4) + abs(mnm3+mnm4)/2;

        impl_->dydx_[n-2] = (w1*mnm3 + w2*mnm2)/(w1 + w2);
        if (isnan(impl_->dydx_[n-2])) {
            impl_->dydx_[n-2] = 0;
        }

        w1 = abs(mn - mnm1) + abs(mn+mnm1)/2;
        w2 = abs(mnm2 - mnm3) + abs(mnm2+mnm3)/2;

        impl_->dydx_[n-1] = (w1*mnm2 + w2*mnm1)/(w1+w2);
        if (isnan(impl_->dydx_[n-1])) {
            impl_->dydx_[n-1] = 0;
        }
    }

private:
    std::shared_ptr<detail::cubic_hermite_detail<RandomAccessContainer>> impl_;
};

}
}
}
#endif


#include <deal.II/lac/solver_control.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>

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

#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/tria_boundary_lib.h>
#include <deal.II/grid/grid_tools.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/fe_q.h>

#include <filesystem>

#include "FEM1DWave.h"

namespace fs = std::filesystem;

// Class constructor for a vector field
FEM1DWave::FEM1DWave(unsigned int order, unsigned int problem)
	:
	n(0),
	fe(FE_Q<1>(order), 1),
	dof_handler(triangulation),
	basis(QGauss<1>(order + 1)),
	src(FEM1DGauSrc(Point<1>(0.5), 0.05, 0.05, 0.1 / T))
{
	if (problem == 1 || problem == 2) {
		prob = problem;
	}
	else {
		std::cout << "Error: problem number should be 1 or 2.\n";
		exit(0);
	}

	//Nodal Solution names - this is for writing the output file
	nodal_solution_names.push_back("p");
	nodal_data_component_interpretation.push_back(DataComponentInterpretation::component_is_part_of_vector);
}

//Class destructor
FEM1DWave::~FEM1DWave() {
	dof_handler.clear();
}

//Define the problem domain and generate the mesh
void FEM1DWave::generate_mesh(unsigned int numberOfElements) {

	//Define the limits of your domain
	L = 1; //EDIT
	double x_min = 0.;
	double x_max = L;

	Point<1, double> min(x_min), max(x_max);
	std::vector<unsigned int> meshDimensions(1, numberOfElements);
	GridGenerator::subdivided_hyper_rectangle(triangulation, meshDimensions, min, max);
}

//Setup data structures (sparse matrix, vectors)
void FEM1DWave::setup_system() {
	//Let deal.II organize degrees of freedom
	dof_handler.distribute_dofs(fe);

	//Enter the global node x-coordinates into the vector "nodeLocation"
	std::vector< Point<1, double> > dof_coords(dof_handler.n_dofs());
	nodeLocation.resize(dof_handler.n_dofs());
	DoFTools::map_dofs_to_support_points<1, 1>(mapping, dof_handler, dof_coords);
	for (unsigned int i = 0; i < dof_coords.size(); i++) {
		nodeLocation[i] = dof_coords[i][0];
	}

	//Specify boundary condtions (call the function)
	define_boundary_conds();

	//Define the size of the global matrices and vectors
	sparsity_pattern.reinit(dof_handler.n_dofs(), dof_handler.n_dofs(),
		dof_handler.max_couplings_between_dofs());
	DoFTools::make_sparsity_pattern(dof_handler, sparsity_pattern);
	sparsity_pattern.compress();
	K.reinit(sparsity_pattern);
	M.reinit(sparsity_pattern);
	Au.reinit(sparsity_pattern);
	F.reinit(dof_handler.n_dofs());
	P1.reinit(dof_handler.n_dofs());
	P2.reinit(dof_handler.n_dofs());
	V.reinit(dof_handler.n_dofs());
	RHS.reinit(dof_handler.n_dofs());
	constraints.close();

	//Just some notes...
	std::cout << "   Number of active elems:       " << triangulation.n_active_cells() << std::endl;
	std::cout << "   Number of degrees of freedom: " << dof_handler.n_dofs() << std::endl;

	//make sure solution folder exists
	if (!fs::is_directory("sln") || !fs::exists("sln")) { // Check if src folder exists
		fs::create_directory("sln"); // create src folder
	}
}

//Form elmental vectors and matrices and assemble to the global vector (F) and matrix (K)
void FEM1DWave::assemble_system() {
	MatrixCreator::create_mass_matrix(dof_handler, basis, M);
	MatrixCreator::create_laplace_matrix(dof_handler, basis,K);
	Au.copy_from(M);
	Au.add(T*T*theta*theta, K);

	//zero initial conditions
	P1 = 0;
	P2 = 0;
	V = 0;
	F = 0;
}

void FEM1DWave::solve_p()
{
	SolverControl solver_control(1000, 1e-8*RHS.l2_norm());
	SolverCG<>    cg(solver_control);

	cg.solve(Au, P1, RHS, PreconditionIdentity());
	std::cout << "   u-equation: " << solver_control.last_step() << " CG iterations." << std::endl;
}

void FEM1DWave::solve_v()
{
	SolverControl           solver_control(1000, 1e-8*RHS.l2_norm());
	SolverCG<>              cg(solver_control);
	cg.solve(M, P2, RHS, PreconditionIdentity());
	V += P2;
	std::cout << "   v-equation: " << solver_control.last_step() << " CG iterations." << std::endl;
}

void FEM1DWave::run() {
	Vector<double> tmp(P1.size());
	double t;

	for (; n <= N;)
	{
		++n;
		t = n * T;

		std::cout << "Time step " << n << " at t=" << t	<< std::endl;

		P2 = P1;
		tmp = P2;
		tmp *= -T * T*theta*(1 - theta);
		K.vmult(RHS, tmp);
		tmp = V;
		tmp.sadd(T, P2);
		M.vmult_add(RHS, tmp);

		tmp = F;
		tmp *= (1 - theta);
		src.set_time(t);
		VectorTools::create_right_hand_side(mapping, dof_handler, basis, src, F);
		tmp.add(theta, F);
		RHS.add(T * T * theta, tmp);

		//if dirichlet boundary required... update matrix here
		solve_p();

		RHS = tmp;
		RHS *= T;
		P2 *= -T * (1 - theta);
		P2.add(-T * theta,P1);
		K.vmult_add(RHS, P2);
		solve_v();

		//write output file in vtk format for visualization
		output_results();

		std::cout << "   Total energy: " << (M.matrix_norm_square(V) + K.matrix_norm_square(P1)) / 2 << std::endl;
	}
}

//Specify the Dirichlet boundary conditions
void FEM1DWave::define_boundary_conds() {
	//const unsigned int totalNodes = dof_handler.n_dofs(); //Total number of nodes

	//Identify dirichlet boundary nodes and specify their values.
	//This function is called from within "setup_system"

	/*The vector "nodeLocation" gives the x-coordinate in the real domain of each node,
	  organized by the global node number.*/

	  /*This loops through all nodes in the system and checks to see if they are
		at one of the boundaries. If at a Dirichlet boundary, it stores the node number
		and the applied displacement value in the std::map "boundary_values". Deal.II
		will use this information later to apply Dirichlet boundary conditions.
		Neumann boundary conditions are applied when constructing Flocal in "assembly"*/
		/*
		for(unsigned int globalNode=0; globalNode<totalNodes; globalNode++){
		  if(nodeLocation[globalNode] == 0){
			  boundary_values[globalNode] = g1;
		  }
		  if(nodeLocation[globalNode] == L){
			  if(prob == 1){
			  boundary_values[globalNode] = g2;
			  }
		  }
		}
		*/
}

//Output results
void FEM1DWave::output_results() {
	const std::string tag = "./sln/wave_o" + Utilities::int_to_string(basis.size() - 1) + "_step_" + Utilities::int_to_string(n) + ".vtk";
	//Write results to VTK file
	std::ofstream output1(tag);
	DataOut<1> data_out;
	data_out.attach_dof_handler(dof_handler);

	//Add nodal DOF data
	data_out.add_data_vector(P1, nodal_solution_names, DataOut<1>::type_dof_data, nodal_data_component_interpretation);
	data_out.build_patches();
	data_out.write_vtk(output1);
	output1.close();
}

double FEM1DWave::u_exact(double x) {
	double result = 0;
	//diriclet bc
	if (prob == 2)
	{
	}
	else
	{
	}
	return result;
}

double FEM1DWave::l2norm_of_error() {
	/*
	double l2norm = 0.;

	//Find the l2 norm of the error between the finite element sol'n and the exact sol'n
	const unsigned int   			dofs_per_elem = fe.dofs_per_cell; //This gives you dofs per element
	std::vector<unsigned int> local_dof_indices(dofs_per_elem);
	Vector<double> u_ex; u_ex.reinit(dof_handler.n_dofs());
	double u_h, x, h_e;

	//loop over elements
	for (unsigned int q = 0; q < dof_handler.n_dofs(); q++)
		u_ex[q] = u_exact(nodeLocation[q]);
	u_ex.print(std::cout);
	for (auto i = nodeLocation.begin(); i != nodeLocation.end(); ++i)
		std::cout << *i << ' ';
	std::cout << std::endl;

	typename DoFHandler<1>::active_cell_iterator elem = dof_handler.begin_active(),
		endc = dof_handler.end();
	for (; elem != endc; ++elem) {
		//Retrieve the effective "connectivity matrix" for this element
		elem->get_dof_indices(local_dof_indices);

		//Find the element length
		h_e = nodeLocation[local_dof_indices[1]] - nodeLocation[local_dof_indices[0]];

		double sum = 0.;
		for (unsigned int q = 0; q < quadRule; q++) {
			x = 0.;
			u_h = 0.;
			//Find the values of x and u_h (the finite element solution) at the quadrature points
			for (unsigned int B = 0; B < dofs_per_elem; B++) {
				u_h += D[local_dof_indices[B]] * basis_function(B, quad_points[q]);
				x += nodeLocation[local_dof_indices[B]] * basis_function(B, quad_points[q]);
			}
			//EDIT - Find the l2-norm of the error through numerical integration.
			//This includes evaluating the exact solution at the quadrature points
			sum += pow(u_exact(x) - u_h, 2) * quad_weight[q];
		}
		sum *= h_e / 2;
		l2norm += sum;
	}
	return sqrt(l2norm);
	*/
	return 0.0;
}


#pragma once

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

// offset from center of laser rotation
#define AXIS_OFFSET 0.0 //-0.015

using namespace Eigen;

/**
 * get the 3D minimum distance between 2 lines
 * following http://geomalgorithms.com/a07-_distance.html#Distance-between-Lines
 * @param pos0 origin line0
 * @param dir1 direction line0
 * @param pos1 origin line1
 * @param dir2 direction line1
 * @param tri0 point on line0 closest to line1
 * @param tri1 point on line1 closest to line0
 * @return distance between the lines
 */
double dist3D_Line_to_Line( Vector3d &pos0, Vector3d &dir1,
                            Vector3d &pos1, Vector3d &dir2,
                            Vector3d &tri0, Vector3d &tri1);

/**
    * This function triangulates the position of a sensor using the horizontal and vertical angles from two ligthouses
    * @param angles0 vertical/horizontal angles form first lighthouse
    * @param angles1 vertical/horizontal angles form second lighthouse
    * @param RT_0 pose matrix of first lighthouse
    * @param RT_1 pose matrix of second lighthouses
    * @param triangulated_position the triangulated position
    * @param ray0 ligthhouse ray
    * @param ray1 ligthhouse ray
    */
double triangulateFromLighthouseAngles(Vector2d &angles0, Vector2d &angles1, Matrix4d &RT_0, Matrix4d &RT_1,
                                     Vector3d &triangulated_position, Vector3d &ray0, Vector3d &ray1);

double triangulateFromRays(Vector3d &ray0, Vector3d &ray1, Matrix4d &RT_0, Matrix4d &RT_1, Vector3d &triangulated_position);

void rayFromLighthouseAngles(Vector2d &angles, Vector3d &ray);


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "ConvolutionTools.hpp"
#include "Toeplitz.hpp"
#include "../public/WindowFuncs.hpp"
#include "../../data/FluidIndex.hpp"
#include <Eigen/Eigen>
#include <algorithm>
#include <cmath>
#include <functional>

namespace fluid {
namespace algorithm {

class ARModel
{

  using MatrixXd = Eigen::MatrixXd;
  using ArrayXd = Eigen::ArrayXd;
  using VectorXd = Eigen::VectorXd;

public:
  ARModel(index order) : mParameters(VectorXd::Zero(order)) {}

  const double* getParameters() const { return mParameters.data(); }
  double        variance() const { return mVariance; }
  index         order() const { return mParameters.size(); }

  void estimate(const double* input, index size, index nIterations = 3,
                double robustFactor = 3.0)
  {
    if (nIterations > 0)
      robustEstimate(input, size, nIterations, robustFactor);
    else
      directEstimate(input, size, true);
  }

  double fowardPrediction(const double* input)
  {
    return modelPredict<std::negate<index>>(input);
  }

  double backwardPrediction(const double* input)
  {
    struct Identity
    {
      index operator()(index a) { return a; }
    };
    return modelPredict<Identity>(input);
  }

  double forwardError(const double* input)
  {
    return modelError<&ARModel::fowardPrediction>(input);
  }

  double backwardError(const double* input)
  {
    return modelError<&ARModel::backwardPrediction>(input);
  }

  void forwardErrorArray(double* errors, const double* input, index size)
  {
    modelErrorArray<&ARModel::forwardError>(errors, input, size);
  }

  void backwardErrorArray(double* errors, const double* input, index size)
  {
    modelErrorArray<&ARModel::backwardError>(errors, input, size);
  }

  void setMinVariance(double variance) { mMinVariance = variance; }

private:
  template <typename Op>
  double modelPredict(const double* input)
  {
    double estimate = 0.0;

    for (index i = 0; i < mParameters.size(); i++)
      estimate += mParameters(i) * input[Op()(i + 1)];

    return estimate;
  }

  template <double (ARModel::*Method)(const double*)>
  double modelError(const double* input)
  {
    return input[0] - (this->*Method)(input);
  }

  template <double (ARModel::*Method)(const double*)>
  void modelErrorArray(double* errors, const double* input, index size)
  {
    for (index i = 0; i < size; i++) errors[i] = (this->*Method)(input + i);
  }

  void directEstimate(const double* input, index size, bool updateVariance)
  {
    // copy input to a 32 byte aligned block (otherwise risk segfaults on Linux)
    VectorXd frame = Eigen::Map<const VectorXd>(input, size);

    if (mUseWindow)
    {
      if (mWindow.size() != size)
      {
        mWindow = ArrayXd::Zero(size);
        WindowFuncs::map()[WindowFuncs::WindowTypes::kHann](size, mWindow);
      }

      frame.array() *= mWindow;
    }

    VectorXd autocorrelation(size);
    algorithm::autocorrelateReal(autocorrelation.data(), frame.data(),
                                 asUnsigned(size));

    // Resize to the desired order (only keep coefficients for up to the order
    // we need)
    double pN = mParameters.size() < size ? autocorrelation(mParameters.size())
                                          : autocorrelation(0);
    autocorrelation.conservativeResize(mParameters.size());

    // Form a toeplitz matrix
    MatrixXd mat = toeplitz(autocorrelation);

    // Yule Walker
    autocorrelation(0) = pN;
    std::rotate(autocorrelation.data(), autocorrelation.data() + 1,
                autocorrelation.data() + mParameters.size());
    mParameters = mat.llt().solve(autocorrelation);

    if (updateVariance)
    {
      // Calculate variance
      double variance = mat(0, 0);

      for (index i = 0; i < mParameters.size() - 1; i++)
        variance -= mParameters(i) * mat(0, i + 1);

      setVariance((variance - (mParameters(mParameters.size() - 1) * pN)) /
                  size);
    }
  }

  void robustEstimate(const double* input, index size, index nIterations,
                      double robustFactor)
  {
    std::vector<double> estimates(asUnsigned(size + mParameters.size()));

    // Calculate an initial estimate of parameters
    directEstimate(input, size, true);

    // Initialise Estimates
    for (index i = mParameters.size(); i < mParameters.size() + size; i++)
      estimates[asUnsigned(i)] = input[i - mParameters.size()];

    // Variance
    robustVariance(estimates.data() + mParameters.size(), input, size,
                   robustFactor);

    // Iterate
    for (index iterations = nIterations; iterations--;)
      robustIteration(estimates.data() + mParameters.size(), input, size,
                      robustFactor);
  }

  double robustResidual(double input, double prediction, double cs)
  {
    return cs * psiFunction((input - prediction) / cs);
  }

  void robustVariance(double* estimates, const double* input, index size,
                      double robustFactor)
  {
    double residualSqSum = 0.0;

    // Iterate to find new filtered input
    for (index i = 0; i < size; i++)
    {
      const double residual =
          robustResidual(input[i], fowardPrediction(estimates + i),
                         robustFactor * sqrt(mVariance));
      residualSqSum += residual * residual;
    }

    setVariance(residualSqSum / size);
  }

  void robustIteration(double* estimates, const double* input, index size,
                       double robustFactor)
  {
    // Iterate to find new filtered input
    for (index i = 0; i < size; i++)
    {
      const double prediction = fowardPrediction(estimates);
      estimates[0] =
          prediction +
          robustResidual(input[i], prediction, robustFactor * sqrt(mVariance));
    }

    // New parameters
    directEstimate(estimates, size, false);
    robustVariance(estimates, input, size, robustFactor);
  }

  void setVariance(double variance)
  {
    if (variance > 0) variance = std::max(mMinVariance, variance);
    mVariance = variance;
  }

  // Huber PSI function
  double psiFunction(double x)
  {
    return fabs(x) > 1 ? std::copysign(1.0, x) : x;
  }

  VectorXd mParameters;
  double   mVariance{0.0};
  ArrayXd  mWindow;
  bool     mUseWindow{true};
  double   mMinVariance{0.0};
};

} // namespace algorithm
} // namespace fluid


/*
 * @Description: Kalman Filter utilities.
 * @Author: Ge Yao
 * @Date: 2020-11-12 15:14:07
 */

#include "lidar_localization/models/kalman_filter/kalman_filter.hpp"

// SVD for observability analysis:
#include <string>
#include <vector>

#include <Eigen/SVD>

#include "lidar_localization/tools/CSVWriter.hpp"

#include "lidar_localization/global_defination/global_defination.h"

#include "glog/logging.h"

namespace lidar_localization {

void KalmanFilter::AnalyzeQ(
    const int DIM_STATE,
    const double &time, const Eigen::MatrixXd &Q, const Eigen::VectorXd &Y,
    std::vector<std::vector<double>> &data
) {
    // perform SVD analysis, thin for rectangular matrix:
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Q, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // add record:
    std::vector<double> record;

    // a. record timestamp:
    record.push_back(time);

    // b. record singular values:
    for (int i = 0; i < DIM_STATE; ++i) {
        record.push_back(svd.singularValues()(i, 0));
    }

    // c. record degree of observability:
    Eigen::MatrixXd X = (
        svd.matrixV() *
        svd.singularValues().asDiagonal().inverse()
    ) * (
        svd.matrixU().transpose() * Y
    ).asDiagonal();

    Eigen::VectorXd degree_of_observability = Eigen::VectorXd::Zero(DIM_STATE);
    for (int i = 0; i < DIM_STATE; ++i) {
        // find max. magnitude response in X:
        Eigen::MatrixXd::Index sv_index;
        X.col(i).cwiseAbs().maxCoeff(&sv_index);

        // associate with corresponding singular value:
        degree_of_observability(sv_index) = svd.singularValues()(i);
    }
    // normalize:
    degree_of_observability = 1.0 / svd.singularValues().maxCoeff() * degree_of_observability;

    for (int i = 0; i < DIM_STATE; ++i) {
        record.push_back(degree_of_observability(i));
    }

    // add to data:
    data.push_back(record);
}

void KalmanFilter::WriteAsCSV(
    const int DIM_STATE,
    const std::vector<std::vector<double>> &data,
    const std::string filename
) {
    // init:
    CSVWriter csv(",");
    csv.enableAutoNewRow(1 + 2*DIM_STATE);

    // a. write header:
    csv << "T";
    for (int i = 0; i < DIM_STATE; ++i) {
        csv << ("sv" + std::to_string(i + 1));
    }
    for (int i = 0; i < DIM_STATE; ++i) {
        csv << ("doo" + std::to_string(i + 1));
    }

    // b. write contents:
    for (const auto &record: data) {
        // cast timestamp to int:
        csv << static_cast<int>(record.at(0));

        for (size_t i = 1; i < record.size(); ++i) {
            csv << std::fabs(record.at(i));
        }
    }

    // save to persistent storage:
    csv.writeToFile(filename);
}

} // namespace lidar_localization


#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>

using namespace std;
using boost::multiprecision::cpp_int;

/*
The lattice in this problem is just a very cleverly disguised combinatorics question. So we just need to calculate the factorials.
*/

// Generate the n'th term for the factorial sequence.
cpp_int factorial(int n) {
	if(n > 1) {
		return n * factorial(n - 1);
	} else {
		return 1;
	}
}

cpp_int nCr(int n, int r) {
	return (factorial(n) / (factorial(r) * factorial(n - r)));
}

int main(int argc, char *argv[]) {
	cout << nCr(40, 20) << endl;
	return 0;
}


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

#include <boost/gil/point.hpp>
#include <cmath>
#include <cstddef>

namespace boost
{
namespace gil
{
/// \ingroup HoughTransform
/// \brief A type to encapsulate Hough transform parameter range
///
/// This type provides a way to express value range for a parameter
/// as well as some factory functions to simplify initialization
template <typename T>
struct hough_parameter
{
    T start_point;
    T step_size;
    std::size_t step_count;

    /// \ingroup HoughTransform
    /// \brief Create Hough parameter from value neighborhood and step count
    ///
    /// This function will take start_point as middle point, and in both
    /// directions will try to walk half_step_count times until distance of
    /// neighborhood is reached
    static hough_parameter<T> from_step_count(T start_point, T neighborhood,
                                              std::size_t half_step_count)
    {
        T step_size = neighborhood / half_step_count;
        std::size_t step_count = half_step_count * 2 + 1;
        // explicitly fill out members, as aggregate init will error out with narrowing
        hough_parameter<T> parameter;
        parameter.start_point = start_point - neighborhood;
        parameter.step_size = step_size;
        parameter.step_count = step_count;
        return parameter;
    }

    /// \ingroup HoughTransform
    /// \brief Create Hough parameter from value neighborhood and step size
    ///
    /// This function will take start_point as middle point, and in both
    /// directions will try to walk step_size at a time until distance of
    /// neighborhood is reached
    static hough_parameter<T> from_step_size(T start_point, T neighborhood, T step_size)
    {
        std::size_t step_count =
            2 * static_cast<std::size_t>(std::floor(neighborhood / step_size)) + 1;
        // do not use step_size - neighborhood, as step_size might not allow
        // landing exactly on that value when starting from start_point
        // also use parentheses on step_count / 2 because flooring is exactly
        // what we want

        // explicitly fill out members, as aggregate init will error out with narrowing
        hough_parameter<T> parameter;
        parameter.start_point = start_point - step_size * (step_count / 2);
        parameter.step_size = step_size;
        parameter.step_count = step_count;
        return parameter;
    }
};

/// \ingroup HoughTransform
/// \brief Calculate minimum angle which would be observable if walked on a circle
///
/// When drawing a circle or moving around a point in circular motion, it is
/// important to not do too many steps, but also to not have disconnected
/// trajectory. This function will calculate the minimum angle that is observable
/// when walking on a circle or tilting a line.
/// WARNING: do keep in mind IEEE 754 quirks, e.g. no-associativity,
/// no-commutativity and precision. Do not expect expressions that are
/// mathematically the same to produce the same values
inline double minimum_angle_step(point_t dimensions)
{
    auto longer_dimension = dimensions.x > dimensions.y ? dimensions.x : dimensions.y;
    return std::atan2(1, longer_dimension);
}

/// \ingroup HoughTransform
/// \brief Create a Hough transform parameter with optimal angle step
///
/// Due to computational intensity and noise sensitivity of Hough transform,
/// having any candidates missed or computed again is problematic. This function
/// will properly encapsulate optimal value range around approx_angle with amplitude of
/// neighborhood in each direction.
/// WARNING: do keep in mind IEEE 754 quirks, e.g. no-associativity,
/// no-commutativity and precision. Do not expect expressions that are
/// mathematically the same to produce the same values
inline hough_parameter<double> make_theta_parameter(double approx_angle, double neighborhood,
                                                    point_t dimensions)
{
    auto angle_step = minimum_angle_step(dimensions);

    // std::size_t step_count =
    //     2 * static_cast<std::size_t>(std::floor(neighborhood / angle_step)) + 1;
    // return {approx_angle - angle_step * (step_count / 2), angle_step, step_count};
    return hough_parameter<double>::from_step_size(approx_angle, neighborhood, angle_step);
}
}} // namespace boost::gil
#endif


// Copyright (c) 2020, ETH Zurich, CVG, Mihai Dusmanu (mihai.dusmanu@inf.ethz.ch)

#include <Eigen/Core>

#include "ceres/ceres.h"

class BiquadraticInterpolator {
    public:
        explicit BiquadraticInterpolator(
            const std::vector<double>& data, size_t n_channels
        ) : data_(data), n_channels_(n_channels) {}

        void Evaluate(double row, double col, double* f, double* dfdrow, double* dfdcol) const {
            // Clipping.
            const double row_ = row;
            const double col_ = col;
            row = std::max(std::min(row, row_end_), row_start_);
            col = std::max(std::min(col, col_end_), col_start_);

            const double lagrange_row[3] = {2. * row * (row - .5), (-4.) * (row - .5) * (row + .5), 2. * row * (row + 0.5)};
            const double deriv_lagrange_row[3] = {2. * row + 2. * (row - .5), (-4.) * (row - .5) + (-4.) * (row + .5), 2. * row + 2. * (row + 0.5)};
            const double lagrange_col[3] = {2. * col * (col - .5), (-4.) * (col - .5) * (col + .5), 2. * col * (col + 0.5)};
            const double deriv_lagrange_col[3] = {2. * col + 2. * (col - .5), (-4.) * (col - .5) + (-4.) * (col + .5), 2. * col + 2. * (col + 0.5)};

            for (size_t k = 0; k < n_channels_; ++k) {
                f[k] = 0.;
                if (dfdrow != NULL) {
                    dfdrow[k] = 0.;
                    dfdcol[k] = 0.;
                }

                for (size_t i = 0; i < n_rows_; ++i) {
                    for (size_t j = 0; j < n_cols_; ++j) {
                        double data = data_[n_channels_ * (i * n_cols_ + j) + k];
                        f[k] += lagrange_row[i] * lagrange_col[j] * data;

                        if (dfdrow != NULL) {
                            if (row_ == row) {
                                dfdrow[k] += deriv_lagrange_row[i] * lagrange_col[j] * data;
                            }
                            if (col_ == col) {
                                dfdcol[k] += lagrange_row[i] * deriv_lagrange_col[j] * data;
                            }
                        }
                    }
                }
            }
        }

        // The following two Evaluate overloads are needed for interfacing with automatic differentiation.
        // The first is for when a scalar evaluation is done, and the second one is for when Jets are used.
        void Evaluate(const double& r, const double& c, double* f) const {
            Evaluate(r, c, f, NULL, NULL);
        }

        template<typename JetT> void Evaluate(const JetT& x, const JetT& y, JetT* f) const {
            double frc[n_channels_], dfdr[n_channels_], dfdc[n_channels_];
            Evaluate(x.a, y.a, frc, dfdr, dfdc);
            for (size_t k = 0; k < n_channels_; ++k) {
                f[k].a = frc[k];
                f[k].v = dfdr[k] * x.v + dfdc[k] * y.v;
            }
        }

    private:
        const std::vector<double> data_;
        const size_t n_channels_;
        const double row_start_ = -.5; const double col_start_ = -.5;
        const double row_end_ = .5; const double col_end_ = .5;
        const double one_over_row_stride_ = 2.; const double one_over_col_stride_ = 2.;
        const size_t n_rows_ = 3; const size_t n_cols_ = 3;
};

template<typename Interpolator> struct InterpolatedCostFunctor {
    public:
        explicit InterpolatedCostFunctor(Interpolator&& interpolator) : interpolator_(std::move(interpolator)) {}

    template<typename T> bool operator()(const T* x1, const T* x2, T* residuals) const {
        const Eigen::Map<const Eigen::Matrix<T, 2, 1>> x1_vec(x1);
        const Eigen::Map<const Eigen::Matrix<T, 2, 1>> x2_vec(x2);

        Eigen::Matrix<T, 2, 1> disp;
        interpolator_.Evaluate(x1_vec[0], x1_vec[1], disp.data());

        Eigen::Map<Eigen::Matrix<T, 2, 1>> residuals_vec(residuals);

        residuals_vec = x2_vec - x1_vec - disp;

        return true;
    }

    static ceres::CostFunction* Create(Interpolator&& interpolator) {
        return new ceres::AutoDiffCostFunction<InterpolatedCostFunctor, 2, 2, 2>(new InterpolatedCostFunctor(std::move(interpolator)));
    }

    private:
        const Interpolator interpolator_;
};


#include <RcppArmadillo.h>
// [[Rcpp::depends("RcppArmadillo")]]
// [[Rcpp::depends("BH")]]

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/random.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/math/distributions.hpp>

#include <numeric>
#include <algorithm>
#include <map>
#include <string>
#include <iostream>

using namespace Rcpp;
using namespace arma;
using namespace std;
using namespace boost::multiprecision;
using namespace boost::math;

boost::mt19937 rng; // seed for random sampling

double rbeta_cpp(double shape1, double shape2) {
  double u  = rand() / double(RAND_MAX);
  beta_distribution<> beta_dist(shape1, shape2);
  return quantile(beta_dist, u);
}

// [[Rcpp::export]]
arma::rowvec rDirichlet2(arma::rowvec param, int length) {
  vector<double> ret;
  param *= 10000;
  vector<double> param_vec = conv_to<vector<double> >::from(param);
  int len = length - 1;

  double paramSum = std::accumulate(param_vec.begin()+1,param_vec.end(),(double)0);
  ret.push_back(rbeta_cpp(param_vec[0], paramSum));
  for (int i=1; i<len;i++) {
    double paramSum = std::accumulate(param_vec.begin()+i+1,param_vec.end(),(double)0);
    double phi = rbeta_cpp(param_vec[i], paramSum);
    double sumRet = std::accumulate(ret.begin(),ret.end(),(double)0);
    ret.push_back((1-sumRet) * phi);
  }
  double sumRet = std::accumulate(ret.begin(),ret.end(),(double)0);
  ret.push_back(1-sumRet);
  return ret;
}


#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>
#include <iostream>
#include <cmath>
#include <iomanip>
#include <limits>
#include <exception>

const int PRECISION = 100;
typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<PRECISION> > arbFloat;

bool isStringValid(const std::string & str);
bool isNumberValid(const arbFloat & x);
enum returnID {success = 0, invalidString = 1, invalidNumber = 2};
inline std::string resizeArbtoString(const arbFloat & x);

int main() {
    // Input and check string
    std::cout << "0 <= x < inf\nW(x), x = ";
    std::string inputStr;
    std::getline(std::cin, inputStr);

    if(!isStringValid(inputStr)) return returnID::invalidString;

    arbFloat input = static_cast<arbFloat>(inputStr);

    if(!isNumberValid(input)) return returnID::invalidNumber;

    // Starting guess
    arbFloat wnew = (input >= 2) ? log(input) - log(log(input)) + (log(log(input)) / log(input))
            : static_cast<arbFloat>(0);

    std::cout << std::setprecision(PRECISION)
            << "\nInitial Guess: " << wnew << "\nConvergence:\n";

    // Calculations
    int i;
    std::string preComp, postComp;
    for(i = 0; i == 0 || preComp != postComp; i++){
        std::cout << '\t' << wnew << '\n';
        preComp = resizeArbtoString(wnew);
        wnew -= ((wnew * exp(wnew)) - input) /
                (exp(wnew) * (wnew + 1) - ((wnew + 2) * (wnew * exp(wnew) - input) / ((2 * wnew) + 2)));
        postComp = resizeArbtoString(wnew);
    }

    std::cout << "\nW(" + inputStr + ") = " << wnew << "\n(rounded up to "
            << PRECISION << " digits, precise after " << i << " iterations)\n";

    return 0;
}

// Check input
bool isStringValid(const std::string & str){
    // Check if string contains spaces
    if(std::count(str.begin(), str.end(), ' ') > 0){
        std::cout << "\nError: Input contains spaces\n";
        return false;
    }

    // Check if string contains multiple .
    if(std::count(str.begin(), str.end(), '.') > 1){
        std::cout << "\nError: Input contains multiple decimal marks\n";
        return false;
    }

    // Check if NaN or Out of bounds (due to parsing failure)
    try{
        boost::lexical_cast<arbFloat>(str);
    } catch(std::runtime_error){
        std::cout << "\nError: Unable to parse (value too large or incorrect number type)\n";
        return false;
    } catch(...){
        std::cout << "\nError: Input is NaN\n";
        return false;
    }

    // Check if intentional NaN
    if(boost::icontains(str, "nan")){
        std::cout << "\nError: Intentional NaN\n";
        return false;
    }

    return true;
}

// Check number
bool isNumberValid(const arbFloat & x){
    // Check if result is imaginary
    if(x < 0){
        std::cout << "\nError: Complex result\n";
        return false;
    }

    // Check if out of bounds (interpreted as infinity)
    if(x == std::numeric_limits<arbFloat>::infinity()){
        std::cout << "\nError: Input is out of bounds\n";
        return false;
    }

    return true;
}

// Convert to string and resize using correct precision
inline std::string resizeArbtoString(const arbFloat & x){
    std::string resizedStr = static_cast<std::string>(x);
    resizedStr.resize(PRECISION + 2);
    return resizedStr;
}


/**
 * \file dcs/math/stats/distribution/normal.hpp
 *
 * \brief The Normal probability distribution.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_STATS_DISTRIBUTION_NORMAL_HPP
#define DCS_MATH_STATS_DISTRIBUTION_NORMAL_HPP

#include <dcs/detail/config/boost.hpp>

#if !DCS_DETAIL_CONFIG_BOOST_CHECK_VERSION(103500) // 1.35
# 	error "Required Boost library version >= 1.35"
#endif

#include <boost/math/distributions/normal.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cstddef>
#include <dcs/math/policies/policy.hpp>
#include <iostream>
#include <vector>

namespace dcs { namespace math { namespace stats {

using ::std::size_t;

/**
 * The Normal distribution with parameter \f$\mu\f$ (the mean) and \f$\sigma\f$
 * (the standard deviation).
 *
 * \tparam RealT The type used for real numbers.
 * \tparam PolicyT The policy type.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template < typename RealT=double, typename PolicyT=::dcs::math::policies::policy<> >
class normal_distribution
{
	public: typedef RealT support_type;
	public: typedef RealT value_type;
	public: typedef PolicyT policy_type;

	public: explicit normal_distribution(support_type mean=0, support_type stddev=1)
		: dist_(mean,stddev)
//		: mean_(mean),
//		  stddev_(stddev)
	{
		// empty
	}

	// compiler-generated copy ctor and assignment operator are fine

	/**
	 * \brief Generate a vector of random numbers distributed according to this
	 * normal distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \param n The number of random numbers to generate.
	 * \return A random number distributed according to this normal
	 * distribution.
	 *
	 * A \c normal random number distribution produces random numbers * \f$x\f$
	 * distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\mu,\sigma) = \frac{1}{\sigma\sqrt{2\pi}}\exp\left(-\frac{(x-\mu)^2}{2\sigma^2}\right)
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		support_type rand(UniformRandomGeneratorT& rng) const
	{
		typedef ::boost::normal_distribution<support_type> rdist_type;
		typedef ::boost::variate_generator<UniformRandomGeneratorT&, rdist_type> variate_type;

		return variate_type(rng, rdist_type(dist_.mean(), dist_.standard_deviation()))();
	}

	/**
	 * \brief Generate a vector of random numbers distributed according to this
	 * normal distribution.
	 *
	 * \param rng A uniform random number generator.
	 * \param n The number of random numbers to generate.
	 * \return A vector of random numbers distributed according to this
	 * normal distribution.
	 *
	 * A \c normal random number distribution produces random numbers * \f$x\f$
	 * distributed according to the probability density function:
	 * \f[
	 *   \Pr(x|\mu,\sigma) = \frac{1}{\sigma\sqrt{2\pi}}\exp\left(-\frac{(x-\mu)^2}{2\sigma^2}\right)
	 * \f]
	 */
	public: template <typename UniformRandomGeneratorT>
		::std::vector<support_type> rand(UniformRandomGeneratorT& rng, size_t n)
	{
		typedef ::boost::normal_distribution<support_type> rdist_type;
        typedef ::boost::variate_generator<UniformRandomGeneratorT&, rdist_type> variate_type;

		::std::vector<support_type> rnds(n);

        for ( ; n > 0; --n)
		{
			rnds.push_back(variate_type(rng, rdist_type(dist_.mean(), dist_.standard_deviation()))());
		}

		return rnds;
	}

	public: support_type mean() const
	{
		return dist_.mean();
	}

	public: support_type stddev() const
	{
		return dist_.standard_deviation();
	}

	public: support_type location() const
	{
		return dist_.location();
	}

	public: support_type scale() const
	{
		return dist_.scale();
	}

	public: support_type quantile(value_type p) const
	{
		return ::boost::math::quantile(dist_, p);
	}

	//private: support_type mean_;
	//private: support_type stddev_;
	private: ::boost::math::normal_distribution<value_type,policy_type> dist_;
};

template <
	typename CharT,
	typename CharTraitsT,
	typename RealT,
	typename PolicyT
>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, normal_distribution<RealT,PolicyT> const& dist)
{
	return os << "Normal("
			  << "mean=" <<  dist.mean()
			  << ", stddev=" <<  dist.stddev()
			  << ")";
}

}}} // Namespace dcs::math::stats

#endif // DCS_MATH_STATS_DISTRIBUTION_NORMAL_HPP


//
// Created by matt on 20/01/2021.
//

#include <iostream>

#include <Eigen/Core>

#include "simple_kf.h"
#include "standard_kf.h"
#include "extended_kf.h"
#include "unscented_kf.h"
#include "system.h"
#include "numeric_differentiation.h"

int main() {

  const int iterations = 3;
  using SS = systems::SimpleSystem;
  const SS::MeasurementVector z(1.0, 2.0);

  ////////////////////////////////////////////////////////////////////
  std::cout << "Simple kalman filter\n";
  kf0::KalmanFilter skf;
  skf.SetCov(systems::SimpleSystem::StateMatrix::Identity());
  std::cout << "Intial state:\n";
  std::cout << skf.GetState().transpose() << '\n';
  std::cout << skf.GetCov() << '\n';
  for (int i = 0; i < iterations; i++) {
    std::cout << "Iteration: " << i << '\n';
    std::cout << "Predict: " << skf.Predict().transpose() << '\n';
    std::cout << skf.GetCov() << '\n';
    std::cout << "Measurement: " << (z * (double) i).transpose() << "\n";
    std::cout << "Update: " << skf.Update(z * (double) i).transpose() << "\n";
    std::cout << skf.GetCov() << "\n\n";
  }

  ////////////////////////////////////////////////////////////////////
  std::cout << "Templated kalman filter 1\n";

  kf1::KalmanFilter<4> kf1;
  kf1::ProcessModel<4> process = [](const Eigen::Matrix<double, 4, 1>& x) -> Eigen::Matrix<double, 4, 1> {
    Eigen::Matrix<double, 4, 4> F = Eigen::Matrix<double, 4, 4>::Identity();
    F(0, 2) = 0.1; // px' = px + vx*dt
    F(1, 3) = 0.1; // py' = py + vy*dt
    return F * x;
  };
  kf1::MeasurementModel<4, 2> measurement_model = [](
      const Eigen::Matrix<double, 4, 1>& x) -> Eigen::Matrix<double, 2, 1> {
    Eigen::Matrix<double, 2, 4> H = Eigen::Matrix<double, 2, 4>::Zero();
    // Measure position only
    H(0, 0) = 1.0;
    H(1, 1) = 1.0;
    return H * x;
  };

  kf1.SetCov(Eigen::Matrix4d::Identity());
  Eigen::Matrix4d Q = Eigen::Matrix4d::Identity() * 2.0;
  Eigen::Matrix2d R = Eigen::Matrix2d::Identity();

  std::cout << kf1.GetState().transpose() << '\n';
  std::cout << kf1.GetCov() << '\n';
  for (int i = 0; i < iterations; i++) {
    std::cout << "Iteration: " << i << '\n';
    kf1.Predict(process, Q);
    std::cout << "Predict: " << kf1.GetState().transpose() << '\n';
    std::cout << kf1.GetCov() << '\n';
    kf1.Update<2>(measurement_model, z * (double) i, R);
    std::cout << "Measurement: " << (z * (double) i).transpose() << "\n";
    std::cout << "Update: " << kf1.GetState().transpose() << "\n\n";
    std::cout << kf1.GetCov() << '\n';
  }

  ////////////////////////////////////////////////////////////////////

  std::cout << "Templated kalman filter 2\n";

  kf2::KalmanFilter<4, 2>::ProcessModel process_model_2 = [](
      const Eigen::Matrix<double, 4, 1>& x) -> Eigen::Matrix<double, 4, 1> {
    Eigen::Matrix<double, 4, 4> F = Eigen::Matrix<double, 4, 4>::Identity();
    F(0, 2) = 0.1; // px' = px + vx*dt
    F(1, 3) = 0.1; // py' = py + vy*dt
    return F * x;
  };
  kf2::KalmanFilter<4, 2>::MeasurementModel measurement_model_2 = [](
      const Eigen::Matrix<double, 4, 1>& x) -> Eigen::Matrix<double, 2, 1> {
    Eigen::Matrix<double, 2, 4> H = Eigen::Matrix<double, 2, 4>::Zero();
    // Measure position only
    H(0, 0) = 1.0;
    H(1, 1) = 1.0;
    return H * x;
  };

  kf2::KalmanFilter<4, 2> kf2(process_model_2, measurement_model_2);
  kf2.SetCov(Eigen::Matrix4d::Identity());

  for (int i = 0; i < iterations; i++) {
    std::cout << "Iteration: " << i << '\n';
    kf2.Predict(Q);
    std::cout << "Predict: " << kf2.GetState().transpose() << '\n';
    kf2.Update(z * (double) i, R);
    std::cout << "Measurement: " << (z * (double) i).transpose() << "\n";
    std::cout << "Update: " << kf2.GetState().transpose() << "\n\n";
  }

  ////////////////////////////////////////////////////////////////////
  std::cout << "Templated kalman filter 3\n";

  kf3::KalmanFilter<4, 2, SS> kf_3;
  kf_3.SetCov(Eigen::Matrix4d::Identity());

  for (int i = 0; i < iterations; i++) {
    std::cout << "Iteration: " << i << '\n';
    kf_3.Predict(Q);
    std::cout << "Predict: " << kf_3.GetState().transpose() << '\n';
    kf_3.Update(z * (double) i, R);
    std::cout << "Measurement: " << (z * (double) i).transpose() << "\n";
    std::cout << "Update: " << kf_3.GetState().transpose() << "\n\n";
  }

  ////////////////////////////////////////////////////////////////////
  std::cout << "Templated kalman filter 4\n";

  kf4::KalmanFilter<SS> kf_4;
  kf_4.SetCov(Eigen::Matrix4d::Identity());

  for (int i = 0; i < iterations; i++) {
    std::cout << "Iteration: " << i << '\n';
    kf_4.Predict(Q);
    std::cout << "Predict: " << kf_4.GetState().transpose() << '\n';
    kf_4.Update(z * (double) i, R);
    std::cout << "Measurement: " << (z * (double) i).transpose() << "\n";
    std::cout << "Update: " << kf_4.GetState().transpose() << "\n\n";
  }

  ////////////////////////////////////////////////////////////////////
  std::cout << "Another system\n";

  const Eigen::Vector3d z3(1.0, 2.0, 0.5);
  Eigen::Matrix<double, 6, 6> Q3 = Eigen::Matrix<double, 6, 6>::Identity() * 2.0;
  Eigen::Matrix3d R3 = Eigen::Matrix3d::Identity();

  kf4::KalmanFilter<systems::AnotherSystem> kf_5;
  for (int i = 0; i < iterations; i++) {
    std::cout << "Iteration: " << i << '\n';
    kf_5.Predict(Q3);
    std::cout << "Predict: " << kf_5.GetState().transpose() << '\n';
    kf_5.Update(z3 * (double) i, R3);
    std::cout << "Measurement: " << (z3 * (double) i).transpose() << "\n";
    std::cout << "Update: " << kf_5.GetState().transpose() << "\n\n";
  }

  ////////////////////////////////////////////////////////////////////
  std::cout << "Kalman filter using dynamic sized matrices\n";

  Eigen::VectorXd starting_state = Eigen::VectorXd::Zero(4);
  kf5::KalmanFilter<systems_dynamic::SimpleSystem> kf_6(starting_state);
  kf_6.SetCov(Eigen::MatrixXd::Identity(4, 4));

  for (int i = 0; i < iterations; i++) {
    std::cout << "Iteration: " << i << '\n';
    kf_6.Predict(Q);
    std::cout << "Predict: " << kf_6.GetState().transpose() << '\n';
    kf_6.Update(z * (double) i, R);
    std::cout << "Measurement: " << (z * (double) i).transpose() << "\n";
    std::cout << "Update: " << kf_6.GetState().transpose() << "\n\n";
  }

  ////////////////////////////////////////////////////////////////////

  Eigen::Vector4d current_state = Eigen::Vector4d::Zero();

  SS::StateMatrix JF = numeric_differentiation::CalculateJacobian<SS::kStateSize, SS::kStateSize>(current_state, SS::processModel);
  std::cout << JF << "\n\n";
  Eigen::Matrix<double, SS::kMeasurementSize, SS::kStateSize> JH =
      numeric_differentiation::CalculateJacobian<SS::kStateSize, SS::kMeasurementSize>(current_state, SS::measurementModel);
  std::cout << JH << "\n\n";

  ////////////////////////////////////////////////////////////////////
  std::cout << "EKF 1 (with linear system) \n";

  ekf1::ExtendedKalmanFilter<SS> ekf_1;
  ekf_1.SetCov(SS::StateMatrix::Identity());

  for (int i = 0; i < iterations; i++) {
    std::cout << "Iteration: " << i << '\n';
    ekf_1.Predict(Q);
    std::cout << "Predict: " << ekf_1.GetState().transpose() << '\n';
    ekf_1.Update(z * (double) i, R);
    std::cout << "Measurement: " << (z * (double) i).transpose() << "\n";
    std::cout << "Update: " << ekf_1.GetState().transpose() << "\n\n";
  }

  ////////////////////////////////////////////////////////////////////
  std::cout << "EKF 1 (with non-linear system) \n";

  using NLS = systems::NonLinearSystem;
  ekf1::ExtendedKalmanFilter<NLS> ekf_2;
  NLS::ProcessNoiseMatrix Q_nl = NLS::ProcessNoiseMatrix::Identity() * 10.0;
  NLS::MeasurementNoiseMatrix R_nl = NLS::MeasurementNoiseMatrix::Identity() * 1.0;

  ekf_2.SetCov(Q_nl);
  for (int i = 0; i < iterations; i++) {
    std::cout << "Iteration: " << i << '\n';
    ekf_2.Predict(Q_nl);
    std::cout << "Predict: " << ekf_2.GetState().transpose() << '\n';
    ekf_2.Update(z, R_nl);
    std::cout << "Measurement: " << z.transpose() << "\n";
    std::cout << "Update: " << ekf_2.GetState().transpose() << "\n\n";
  }

 // velocity is estimated correctly after a few iterations
 // why doesn't heading work out?

  ////////////////////////////////////////////////////////////////////
  std::cout << "Unscented kalman filter 1\n";

  ukf1::UnscentedKalmanFilter<SS> ukf_1;
  ukf_1.SetCov(SS::StateMatrix::Identity());

  std::cout << ukf_1.GetState().transpose() << '\n';
  std::cout << ukf_1.GetCov() << '\n';
  for (int i = 0; i < iterations; i++) {
    std::cout << "Iteration: " << i << '\n';
    ukf_1.Predict(Q);
    std::cout << "Predict: " << ukf_1.GetState().transpose() << '\n';
    ukf_1.Update(z * (double) i, R);
    std::cout << "Measurement: " << (z * (double) i).transpose() << "\n";
    std::cout << "Update: " << ukf_1.GetState().transpose() << "\n";
  }

  return 0;
}


#include <iostream>
#include <dlib/matrix.h>

int main()
{
    // declare a matrix
    dlib::matrix<double, 3, 1> y;
    dlib::matrix<double, 3, 3> M;
    // initialize matrices
    M = 54.2, 65.2, 43,
        23.4, 12.3, 55.4,
        11, 34.6, 78.9;
    y = 3.5,
        1.2,
        6.7;
    // solve this linear system
    dlib::matrix<double> x = dlib::inv(M)*y;
    std::cout << "x: \n" << x << "\n";
    // check if the calculation is correct
    std::cout << "Check: M*x- y= \n" << M*x - y << "\n";

    // sum the matrix
    double sumM = 0;
    for (int r = 0; r < M.nr(); ++r)
    {
        for (int c = 0; c < M.nc(); ++c)
        {
            sumM += M(r, c);
        }
    }
    std::cout << "Sum of matrix by looping= " << sumM << "\n";
    std::cout << "Sum of matrix by sum() function= " << dlib::sum(M) << "\n";
    // print using comma separator
    std::cout <<"print matrix M using comma delimiter: \n" <<dlib::csv << M << "\n";

    std::system("pause");
    return 0;
}


/**
 * @file gradientflow.h
 * @brief NPDE homework GradientFlow code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include "gradientflow.h"

#include <Eigen/Core>
#include <cmath>
#include <vector>

namespace GradientFlow {

/* SAM_LISTING_BEGIN_0 */
Eigen::MatrixXd ButcherMatrix() {
  Eigen::MatrixXd A(6, 5);
  // clang-format off
  A <<       0.25,          0.,       0.,       0.,   0.,
    0.5,        0.25,       0.,       0.,   0.,
    17./50.,     -1./25.,     0.25,       0.,   0.,
    371./1360., -137./2720., 15./544.,     0.25,   0.,
    25./24.,    -49./48., 125./16., -85./12., 0.25,
    25./24.,    -49./48., 125./16., -85./12., 0.25;
  // clang-format on
  return A;
}
/* SAM_LISTING_END_0 */

/* SAM_LISTING_BEGIN_1 */
std::vector<Eigen::VectorXd> solveGradientFlow(const Eigen::VectorXd &d,
                                               double lambda,
                                               const Eigen::VectorXd &y,
                                               double T, unsigned int N) {
  std::vector<Eigen::VectorXd> Y(N + 1);
  // TO DO (0-2.h)
#if SOLUTION
  // Define the right hand side of the ODE y' = f(y), and the Jacobian of f.
  auto f = [d, lambda](const Eigen::VectorXd &yt) {
    Eigen::VectorXd val =
        -2. * std::cos(yt.squaredNorm()) * yt - 2. * lambda * d.dot(yt) * d;
    return val;
  };
  auto J = [d, lambda](const Eigen::VectorXd &yt) {
    int dim = yt.size();
    Eigen::MatrixXd term1 =
        4. * std::sin(yt.squaredNorm()) * yt * yt.transpose();
    Eigen::MatrixXd term2 =
        -2. * std::cos(yt.squaredNorm()) * Eigen::MatrixXd::Identity(dim, dim);
    Eigen::MatrixXd term3 = -2. * lambda * d * d.transpose();
    Eigen::MatrixXd Jval = term1 + term2 + term3;
    return Jval;
  };

  // Split the interval [0,T] into N intervals of size h.
  double h = T / N;
  Eigen::VectorXd yt = y;
  Y.at(0) = y;
  // Evolve up to time T:
  for (int i = 1; i <= N; i++) {
    yt = discEvolSDIRK(f, J, yt, h);
    Y.at(i) = yt;
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  return Y;
}
/* SAM_LISTING_END_1 */

}  // namespace GradientFlow


//          Copyright Gunter Winkler 2004 - 2009.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

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

#include <boost/numeric/ublas/io.hpp>

using std::cout;
using std::endl;

namespace ublas = boost::numeric::ublas;

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

  ublas::matrix<double> M (3, 3);
  for (std::size_t i=0; i < M.data().size(); ++i) { M.data()[i] = 1+i ; }

  std::cout << "full         M  = " << M << "\n" ;

  ublas::triangular_matrix<double, ublas::lower> L;
  ublas::triangular_matrix<double, ublas::unit_lower> UL;
  ublas::triangular_matrix<double, ublas::strict_lower> SL;

  L  = ublas::triangular_adaptor<ublas::matrix<double>, ublas::lower> (M);
  SL = ublas::triangular_adaptor<ublas::matrix<double>, ublas::strict_lower> (M);
  UL = ublas::triangular_adaptor<ublas::matrix<double>, ublas::unit_lower> (M);

  std::cout << "lower        L  = " << L << "\n"
            << "strict lower SL = " << SL << "\n"
            << "unit lower   UL = " << UL << "\n" ;

  ublas::triangular_matrix<double, ublas::upper> U;
  ublas::triangular_matrix<double, ublas::unit_upper> UU;
  ublas::triangular_matrix<double, ublas::strict_upper> SU;

  U =  ublas::triangular_adaptor<ublas::matrix<double>, ublas::upper> (M);
  SU = ublas::triangular_adaptor<ublas::matrix<double>, ublas::strict_upper> (M);
  UU = ublas::triangular_adaptor<ublas::matrix<double>, ublas::unit_upper> (M);

  std::cout << "upper        U  = " << U << "\n"
            << "strict upper SU = " << SU << "\n"
            << "unit upper   UU = " << UU << "\n" ;

  std::cout << "M = L + SU ? " << ((norm_inf( M - (L + SU) ) == 0.0)?"ok":"failed") << "\n";
  std::cout << "M = U + SL ? " << ((norm_inf( M - (U + SL) ) == 0.0)?"ok":"failed") << "\n";

}


/**
 * @file matode.cc
 * @brief NPDE homework MatODE code
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Dense>

namespace MatODE {

/* SAM_LISTING_BEGIN_3 */
	Eigen::MatrixXd eeulstep(const Eigen::MatrixXd &A, const Eigen::MatrixXd &Y0,
	                         double h) {
		return Y0 + A * Y0 * h;
	}
/* SAM_LISTING_END_3 */

/* SAM_LISTING_BEGIN_4 */
	Eigen::MatrixXd ieulstep(const Eigen::MatrixXd &A, const Eigen::MatrixXd &Y0,
	                         double h) {

		Eigen::MatrixXd Y1 = (Eigen::MatrixXd::Identity(A.rows(), A.cols()) - h * A).lu().solve(Y0);

		return Y1;
	}
/* SAM_LISTING_END_4 */

/* SAM_LISTING_BEGIN_5 */
	Eigen::MatrixXd impstep(const Eigen::MatrixXd &A, const Eigen::MatrixXd &Y0,
	                        double h) {

		Eigen::MatrixXd I = Eigen::MatrixXd::Identity(A.rows(), A.cols());

		return 	(I - 0.5 * h * A).lu().solve((I + 0.5 * h * A) * Y0);
	}
/* SAM_LISTING_END_5 */

}  // namespace MatODE