#ifndef NEURAL_NET_HPP_
#define NEURAL_NET_HPP_

#include <armadillo>
#include <config.h>
#include <string>
#include "util.hpp"
#include "mnist.hpp"

#include <algorithm>
#include <random>
#include <vector>
#include <numeric>

// Apply sigmoid to all elements
template<typename T>
void sigmoid(T&& out) {
    out.transform([](typename T::elem_type val) {
        return (1.0 / (1.0 + exp (-1.0 * val)));
    });
}

/**
 * Simple two-layer neural network for the MNIST data set
 *
 * \code
 * auto train_net = neural_net<float>(train_data, 0.1);
 * \endcode
 */
template<typename T = float>
struct neural_net {
    using elem_t = T;
    using mat_t = arma::Mat<elem_t>;
    using vec_t = arma::Col<elem_t>;
    using subview_t = arma::subview<elem_t>;
    using mnist = mnist<elem_t>;

    const mnist input;    //< Input data
    const elem_t lambda;  //< Regularization parameter
    mat_t theta1;         //< Weights for layer 1
    mat_t theta2;         //< Weights for layer 2
    mat_t a1;             //< Input activations
    mat_t a2;             //< Output activations from layer 1
    mat_t a3;             //< Output activations from layer 2
    mat_t yy;             //< One-hot vector for labels
    mat_t d_theta1;       //< Gradient for layer 1 weights
    mat_t d_theta2;       //< Gradient for layer 2 weights
    mat_t delta3;         //< Gradient from output to labels
    mat_t delta2;         //< Gradient from layer 2 to layer 1 activations

    /**
     * Constructor
     *
     * @param input_   MNIST inut data
     * @param lambda_  lambda value for regularization. Set to 0 for no
     *                 regularization
     */
    neural_net(const mnist& input_, elem_t lambda_ = 1)
    : input(input_),
      lambda(lambda_) {
        using rowvec = arma::Row<elem_t>;
        using arma::zeros;
        using arma::ones;
        using arma::randu;

        elem_t epsilon = 0.12;
        theta1 = (randu<mat_t>(64, input.images.n_cols + 1) * 2 - 1) * epsilon;
        theta2 = (randu<mat_t>(10, theta1.n_rows + 1) * 2 - 1) * epsilon;

        // Convert the column vector of y labels to a matrix where each
        // row has a 1 in the column specified by the label.
        yy = zeros<mat_t>(input.labels.n_rows, 11);
        int row = 0;
        input.labels.for_each([&](const elem_t& element) {
            yy(row, element) = 1.0;
            yy(row, 0) = 1.0;
            row++;
        });

        // Allocate space for each layer of activations. Add an additional
        // column for the bias neuron
        a1 = ones<mat_t>(input.images.n_rows, input.images.n_cols + 1);
        a2 = ones<mat_t>(input.images.n_rows, theta1.n_rows + 1);
        a3 = ones<mat_t>(input.images.n_rows, theta2.n_rows + 1);

        // Load the images onto first activation layer, but do not overwrite
        // the bias neuron
        no_bias(a1) = input.images;

        //shuffle_a1_yy();
    }

    /**
     * Use the neural network to predict outcomes
     *
     * @return The percentage of correct predictions

     */
    elem_t predict(void) const {
        feed_forward();

        // Find neuron with maximum confidence, this is our predicted label
        arma::ucolvec predictions = index_max(no_bias(a3), 1);
        predictions = predictions + 1;

        // Display percentage of correct labels
        return elem_t(sum(predictions == input.labels)) / input.labels.n_rows;
    }

    /**
     * Cost function for the neural net
     *
     * @return the cost
     */
    elem_t cost() const {
        // Cost, without regularization
        elem_t cost = sum(sum(
            -1 * no_bias(yy) % log(no_bias(a3)) -
                (1 - no_bias(yy)) % log(1 - no_bias(a3)), 1))
            / input.images.n_rows;

        // Cost, with regularization
        if (std::abs(lambda) > 0) {
            // Square each element. This next operation makes a copy
            mat_t theta1_sq = square(no_bias(theta1));
            mat_t theta2_sq = square(no_bias(theta2));

            // Sum up all elements in each layer
            elem_t reg = (sum(sum(theta1_sq)) + sum(sum(theta2_sq)));

            // Normalize
            reg *= lambda / (2 * input.images.n_rows);

            // Add in regularization term
            cost += reg;
        }

        return cost;
    }

    /**
     * Calculate the gradient for the weights
     */
    void gradient() {
        elem_t m = a1.n_rows;

        // Delta from labeled data to current set of predictions
        delta3 = a3 - yy;
        d_theta2 = (no_bias(delta3).t() * a2) / m;

        // Delta from output layer to hidden layer
        delta2 = no_bias(delta3) * theta2 % (a2 % (1 - a2));

        // Delta from output layer to hidden layer without bias
        d_theta1 = (no_bias(delta2).t() * a1) / m;

        // Regularization
        if (std::abs(lambda) > 0) {
            no_bias(d_theta1) += (lambda / m) * no_bias(theta1);
            no_bias(d_theta2) += (lambda / m) * no_bias(theta2);
        }
    }

    /**
     * A single Forward propogation step for the neural net
     */
    void feed_forward() const {
        // Pass input thru weights to second layer, do not disturb the bias
        // neuron
        no_bias(a2) = a1 * theta1.t();
        sigmoid(no_bias(a2));

        // Pass hidden layer to output layer
        no_bias(a3) = a2 * theta2.t();
        sigmoid(no_bias(a3));
    }

    /**
     * Train the neural network. Pass in a number of steps and a progress
     * function.
     *
     * \code
     * net.train(1000, [&](size_t i, size_t max_itr) -> void {
     *     if (i == 0 || i % 100 == 0 || i == max_itr - 1) {
     *         std::cout << "\r " << i << " j = " << train_net.cost()
     *                   << std::flush;
     *     }
     * });
     * \endcode
     *
     * @param steps    The number of steps to train
     * @param progress Function called after each step.
     */
    void train(size_t steps, std::function<void(size_t, size_t)> progress) {
        for (size_t i = 0; i < steps; i++) {
            feed_forward();
            // Back prop
            gradient();
            theta1 -= d_theta1;
            theta2 -= d_theta2;
            progress(i, steps);
        }
    }

    /**
     * Save the weights to two files
     */
    void save() const {
        std::string theta1_fn = data_dir + "/theta1.bin";
        std::string theta2_fn = data_dir + "/theta2.bin";
        theta1.save(theta1_fn);
        theta2.save(theta2_fn);
    }

    /**
     * Shuffle the rows of the input and label matrices
     */
    void shuffle_a1_yy() {
        using std::iota;
        using std::vector;
        using std::shuffle;
        using std::begin;
        using std::end;

        vector<size_t> indices(a1.n_rows);
        iota(begin(indices), end(indices), 0);

        auto rng = std::default_random_engine {};
        shuffle(begin(indices), end(indices), rng);

        mat_t a1_shfl(a1.n_rows, a1.n_cols);
        mat_t yy_shfl(yy.n_rows, yy.n_cols);

        for (size_t dst_idx = 0; dst_idx < indices.size(); ++dst_idx) {
            size_t src_idx = indices[dst_idx];
            a1_shfl.row(dst_idx) = a1.row(src_idx);
            yy_shfl.row(dst_idx) = yy.row(src_idx);
        }

        a1 = a1_shfl;
        yy = yy_shfl;
    }
};

#endif /* end of include guard: NEURAL_NET_HPP_ */


#include <fc/uint128.hpp>
#include <fc/io/raw_fwd.hpp>
#include <fc/variant.hpp>
#include <fc/crypto/bigint.hpp>
#include <boost/multiprecision/cpp_int.hpp>

#include <stdexcept>
#include "byteswap.hpp"

namespace fc
{
    typedef boost::multiprecision::uint128_t  m128;

    template <typename T>
    static void divide(const T &numerator, const T &denominator, T &quotient, T &remainder)
    {
      static const int bits = sizeof(T) * 8;//CHAR_BIT;

      if(denominator == 0) {
        throw std::domain_error("divide by zero");
      } else {
        T n      = numerator;
        T d      = denominator;
        T x      = 1;
        T answer = 0;

        while((n >= d) && (((d >> (bits - 1)) & 1) == 0)) {
          x <<= 1;
          d <<= 1;
        }

        while(x != 0) {
          if(n >= d) {
            n -= d;
            answer |= x;
          }

          x >>= 1;
          d >>= 1;
        }

        quotient = answer;
        remainder = n;
      }
    }

    uint128::uint128(const std::string &sz)
    :hi(0), lo(0)
    {
      // do we have at least one character?
      if(!sz.empty()) {
        // make some reasonable assumptions
        int radix = 10;
        bool minus = false;

        std::string::const_iterator i = sz.begin();

        // check for minus sign, i suppose technically this should only apply
        // to base 10, but who says that -0x1 should be invalid?
        if(*i == '-') {
          ++i;
          minus = true;
        }

        // check if there is radix changing prefix (0 or 0x)
        if(i != sz.end()) {
          if(*i == '0') {
            radix = 8;
            ++i;
            if(i != sz.end()) {
              if(*i == 'x') {
                radix = 16;
                ++i;
              }
            }
          }

          while(i != sz.end()) {
            unsigned int n = 0;
            const char ch = *i;

            if(ch >= 'A' && ch <= 'Z') {
              if(((ch - 'A') + 10) < radix) {
                n = (ch - 'A') + 10;
              } else {
                break;
              }
            } else if(ch >= 'a' && ch <= 'z') {
              if(((ch - 'a') + 10) < radix) {
                n = (ch - 'a') + 10;
              } else {
                break;
              }
            } else if(ch >= '0' && ch <= '9') {
              if((ch - '0') < radix) {
                n = (ch - '0');
              } else {
                break;
              }
            } else {
              /* completely invalid character */
              break;
            }

            (*this) *= radix;
            (*this) += n;

            ++i;
          }
        }

        // if this was a negative number, do that two's compliment madness :-P
        if(minus) {
          *this = -*this;
        }
      }
    }

    uint128::operator bigint()const
    {
       auto tmp  = uint128( bswap_64( hi ), bswap_64( lo ) );
       bigint bi( (char*)&tmp, sizeof(tmp) );
       return bi;
    }
    uint128::uint128( const fc::bigint& bi )
    {
       *this = uint128( std::string(bi) ); // TODO: optimize this...
    }

    uint128::operator std::string ()const
    {
      if(*this == 0) { return "0"; }

      // at worst it will be size digits (base 2) so make our buffer
      // that plus room for null terminator
      static char sz [128 + 1];
       sz[sizeof(sz) - 1] = '\0';

      uint128 ii(*this);
      int i = 128 - 1;

      while (ii != 0 && i) {

      uint128 remainder;
      divide(ii, uint128(10), ii, remainder);
          sz [--i] = "0123456789abcdefghijklmnopqrstuvwxyz"[remainder.to_integer()];
      }

      return &sz[i];
    }

    uint128& uint128::operator<<=(const uint128& rhs)
    {
        if(rhs >= 128)
        {
          hi = 0;
          lo = 0;
        }
        else
        {
          unsigned int n = rhs.to_integer();
          const unsigned int halfsize = 128 / 2;

            if(n >= halfsize){
                n -= halfsize;
                hi = lo;
                lo = 0;
            }

            if(n != 0) {
            // shift high half
                hi <<= n;

            const uint64_t mask(~(uint64_t(-1) >> n));

            // and add them to high half
                hi |= (lo & mask) >> (halfsize - n);

            // and finally shift also low half
                lo <<= n;
            }
       }

       return *this;
    }

    uint128 & uint128::operator>>=(const uint128& rhs)
    {
       if(rhs >= 128)
       {
         hi = 0;
         lo = 0;
       }
       else
       {
         unsigned int n = rhs.to_integer();
         const unsigned int halfsize = 128 / 2;

           if(n >= halfsize) {
               n -= halfsize;
               lo = hi;
               hi = 0;
           }

           if(n != 0) {
           // shift low half
               lo >>= n;

           // get lower N bits of high half
           const uint64_t mask(~(uint64_t(-1) << n));

           // and add them to low qword
               lo |= (hi & mask) << (halfsize - n);

           // and finally shift also high half
               hi >>= n;
           }
      }
      return *this;
   }

    uint128& uint128::operator/=(const uint128 &b)
    {
        auto self = (m128(hi) << 64) + m128(lo);
        auto other = (m128(b.hi) << 64) + m128(b.lo);
        self /= other;
        hi = static_cast<uint64_t>(self >> 64);
        lo = static_cast<uint64_t>((self << 64 ) >> 64);

        /*
        uint128 remainder;
        divide(*this, b, *this, remainder ); //, *this);
        if( tmp.hi != hi || tmp.lo != lo ) {
           std::cerr << tmp.hi << "  " << hi <<"\n";
           std::cerr << tmp.lo << "  " << lo << "\n";
           exit(1);
        }
        */

        /*
        const auto&  b128 = std::reinterpret_cast<const m128&>(b);
        auto&     this128 = std::reinterpret_cast<m128&>(*this);
        this128 /= b128;
        */
        return *this;
    }

    uint128& uint128::operator%=(const uint128 &b)
    {
        uint128 quotient;
        divide(*this, b, quotient, *this);
        return *this;
    }

    uint128& uint128::operator*=(const uint128 &b)
    {
        uint64_t a0 = (uint32_t) (this->lo        );
        uint64_t a1 = (uint32_t) (this->lo >> 0x20);
        uint64_t a2 = (uint32_t) (this->hi        );
        uint64_t a3 = (uint32_t) (this->hi >> 0x20);

        uint64_t b0 = (uint32_t) (b.lo        );
        uint64_t b1 = (uint32_t) (b.lo >> 0x20);
        uint64_t b2 = (uint32_t) (b.hi        );
        uint64_t b3 = (uint32_t) (b.hi >> 0x20);

        // (a0 + (a1 << 0x20) + (a2 << 0x40) + (a3 << 0x60)) *
        // (b0 + (b1 << 0x20) + (b2 << 0x40) + (b3 << 0x60)) =
        //  a0 * b0
        //
        // (a1 * b0 + a0 * b1) << 0x20
        // (a2 * b0 + a1 * b1 + a0 * b2) << 0x40
        // (a3 * b0 + a2 * b1 + a1 * b2 + a0 * b3) << 0x60
        //
        // all other cross terms are << 0x80 or higher, thus do not appear in result

        this->hi = 0;
        this->lo = a3*b0;
        (*this) += a2*b1;
        (*this) += a1*b2;
        (*this) += a0*b3;
        (*this) <<= 0x20;
        (*this) += a2*b0;
        (*this) += a1*b1;
        (*this) += a0*b2;
        (*this) <<= 0x20;
        (*this) += a1*b0;
        (*this) += a0*b1;
        (*this) <<= 0x20;
        (*this) += a0*b0;

        return *this;
   }

   void uint128::full_product( const uint128& a, const uint128& b, uint128& result_hi, uint128& result_lo )
   {
       //   (ah * 2**64 + al) * (bh * 2**64 + bl)
       // = (ah * bh * 2**128 + al * bh * 2**64 + ah * bl * 2**64 + al * bl
       // =  P * 2**128 + (Q + R) * 2**64 + S
       // = Ph * 2**192 + Pl * 2**128
       // + Qh * 2**128 + Ql * 2**64
       // + Rh * 2**128 + Rl * 2**64
       // + Sh * 2**64  + Sl
       //

       uint64_t ah = a.hi;
       uint64_t al = a.lo;
       uint64_t bh = b.hi;
       uint64_t bl = b.lo;

       uint128 s = al;
       s *= bl;
       uint128 r = ah;
       r *= bl;
       uint128 q = al;
       q *= bh;
       uint128 p = ah;
       p *= bh;

       uint64_t sl = s.lo;
       uint64_t sh = s.hi;
       uint64_t rl = r.lo;
       uint64_t rh = r.hi;
       uint64_t ql = q.lo;
       uint64_t qh = q.hi;
       uint64_t pl = p.lo;
       uint64_t ph = p.hi;

       uint64_t y[4];    // final result
       y[0] = sl;

       uint128_t acc = sh;
       acc += ql;
       acc += rl;
       y[1] = acc.lo;
       acc = acc.hi;
       acc += qh;
       acc += rh;
       acc += pl;
       y[2] = acc.lo;
       y[3] = acc.hi + ph;

       result_hi = uint128( y[3], y[2] );
       result_lo = uint128( y[1], y[0] );

       return;
   }

   static uint8_t _popcount_64( uint64_t x )
   {
      static const uint64_t m[] = {
         0x5555555555555555ULL,
         0x3333333333333333ULL,
         0x0F0F0F0F0F0F0F0FULL,
         0x00FF00FF00FF00FFULL,
         0x0000FFFF0000FFFFULL,
         0x00000000FFFFFFFFULL
      };
      // TODO future optimization:  replace slow, portable version
      // with fast, non-portable __builtin_popcountll intrinsic
      // (when available)

      for( int i=0, w=1; i<6; i++, w+=w )
      {
         x = (x & m[i]) + ((x >> w) & m[i]);
      }
      return uint8_t(x);
   }

   uint8_t uint128::popcount()const
   {
      return _popcount_64( lo ) + _popcount_64( hi );
   }

   void to_variant( const uint128& var,  variant& vo )  { vo = std::string(var);         }
   void from_variant( const variant& var,  uint128& vo ){ vo = uint128(var.as_string()); }

} // namespace fc

/*
 * Portions of the above code were adapted from the work of Evan Teran.
 *
 * Copyright (c) 2008
 * Evan Teran
 *
 * Permission to use, copy, modify, and distribute this software and its
 * documentation for any purpose and without fee is hereby granted, provided
 * that the above copyright notice appears in all copies and that both the
 * copyright notice and this permission notice appear in supporting
 * documentation, and that the same name not be used in advertising or
 * publicity pertaining to distribution of the software without specific,
 * written prior permission. We make no representations about the
 * suitability this software for any purpose. It is provided "as is"
 * without express or implied warranty.
 */


//==================================================================================================
/*!
  @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_FUNCTION_CSC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CSC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the cosecante of the input in radian : \f$1/\sin(x)\f$.

    @par Header <boost/simd/function/csc.hpp>

    @par Note

      As most other trigonometric function csc can be called
      with a second optional parameter  which is a tag on
      speed and accuracy (see @ref cos for further details)

    @see cscd, cscpi,

    @par Example:

      @snippet csc.cpp csc

    @par Possible output:

      @snippet csc.txt csc

  **/
  IEEEValue csc(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/csc.hpp>
#include <boost/simd/function/simd/csc.hpp>

#endif


/**
 * \file se3_localization.cpp
 *
 *  Created on: Dec 12, 2018
 *     \author: jsola
 *
 *  ---------------------------------------------------------
 *  This file is:
 *  (c) 2018 Joan Sola @ IRI-CSIC, Barcelona, Catalonia
 *
 *  This file is part of `manif`, a C++ template-only library
 *  for Lie theory targeted at estimation for robotics.
 *  Manif is:
 *  (c) 2018 Jeremie Deray @ IRI-UPC, Barcelona
 *  ---------------------------------------------------------
 *
 *  ---------------------------------------------------------
 *  Demonstration example:
 *
 *  3D Robot localization based on fixed beacons.
 *
 *  See se2_localization.cpp for the 2D equivalent.
 *  See se3_sam.cpp for a more advanced example performing smoothing and mapping.
 *  ---------------------------------------------------------
 *
 *  This demo corresponds to the 3D version of the application
 *  in chapter V, section A, in the paper Sola-18,
 *  [https://arxiv.org/abs/1812.01537].
 *
 *  The following is an abstract of the content of the paper.
 *  Please consult the paper for better reference.
 *
 *
 *  We consider a robot in 3D space surrounded by a small
 *  number of punctual landmarks or _beacons_.
 *  The robot receives control actions in the form of axial
 *  and angular velocities, and is able to measure the location
 *  of the beacons w.r.t its own reference frame.
 *
 *  The robot pose X is in SE(3) and the beacon positions b_k in R^3,
 *
 *      X = |  R   t |              // position and orientation
 *          |  0   1 |
 *
 *      b_k = (bx_k, by_k, bz_k)    // lmk coordinates in world frame
 *
 *  The control signal u is a twist in se(3) comprising longitudinal
 *  velocity vx and angular velocity wz, with no other velocity
 *  components, integrated over the sampling time dt.
 *
 *      u = (vx*dt, 0, 0, 0, 0, w*dt)
 *
 *  The control is corrupted by additive Gaussian noise u_noise,
 *  with covariance
 *
 *    Q = diagonal(sigma_x^2, sigma_y^2, sigma_z^2, sigma_roll^2, sigma_pitch^2, sigma_yaw^2).
 *
 *  This noise accounts for possible lateral and rotational slippage
 *  through a non-zero values of sigma_y, sigma_z, sigma_roll and sigma_pitch.
 *
 *  At the arrival of a control u, the robot pose is updated
 *  with X <-- X * Exp(u) = X + u.
 *
 *  Landmark measurements are of the range and bearing type,
 *  though they are put in Cartesian form for simplicity.
 *  Their noise n is zero mean Gaussian, and is specified
 *  with a covariances matrix R.
 *  We notice the rigid motion action y = h(X,b) = X^-1 * b
 *  (see appendix D),
 *
 *      y_k = (brx_k, bry_k, brz_k)    // lmk coordinates in robot frame
 *
 *  We consider the beacons b_k situated at known positions.
 *  We define the pose to estimate as X in SE(3).
 *  The estimation error dx and its covariance P are expressed
 *  in the tangent space at X.
 *
 *  All these variables are summarized again as follows
 *
 *    X   : robot pose, SE(3)
 *    u   : robot control, (v*dt; 0; 0; 0; 0; w*dt) in se(3)
 *    Q   : control perturbation covariance
 *    b_k : k-th landmark position, R^3
 *    y   : Cartesian landmark measurement in robot frame, R^3
 *    R   : covariance of the measurement noise
 *
 *  The motion and measurement models are
 *
 *    X_(t+1) = f(X_t, u) = X_t * Exp ( w )     // motion equation
 *    y_k     = h(X, b_k) = X^-1 * b_k          // measurement equation
 *
 *  The algorithm below comprises first a simulator to
 *  produce measurements, then uses these measurements
 *  to estimate the state, using a Lie-based error-state Kalman filter.
 *
 *  This file has plain code with only one main() function.
 *  There are no function calls other than those involving `manif`.
 *
 *  Printing simulated state and estimated state together
 *  with an unfiltered state (i.e. without Kalman corrections)
 *  allows for evaluating the quality of the estimates.
 */

#include "manif/SE3.h"

#include <Eigen/Dense>

#include <vector>

#include <iostream>
#include <iomanip>

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

using namespace Eigen;

typedef Array<double, 3, 1> Array3d;
typedef Array<double, 6, 1> Array6d;
typedef Matrix<double, 6, 1> Vector6d;
typedef Matrix<double, 6, 6> Matrix6d;

int main()
{
    // START CONFIGURATION
    //
    //
    const int NUMBER_OF_LMKS_TO_MEASURE = 5;

    // Define the robot pose element and its covariance
    manif::SE3d X, X_simulation, X_unfiltered;
    Matrix6d    P;

    X_simulation.setIdentity();
    X.setIdentity();
    X_unfiltered.setIdentity();
    P.setZero();

    // Define a control vector and its noise and covariance
    manif::SE3Tangentd  u_simu, u_est, u_unfilt;
    Vector6d            u_nom, u_noisy, u_noise;
    Array6d             u_sigmas;
    Matrix6d            U;

    u_nom    << 0.1, 0.0, 0.0, 0.0, 0.0, 0.05;
    u_sigmas << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
    U        = (u_sigmas * u_sigmas).matrix().asDiagonal();

    // Declare the Jacobians of the motion wrt robot and control
    manif::SE3d::Jacobian J_x, J_u;

    // Define five landmarks in R^3
    Vector3d b0, b1, b2, b3, b4, b;
    b0 << 2.0,  0.0,  0.0;
    b1 << 3.0, -1.0, -1.0;
    b2 << 2.0, -1.0,  1.0;
    b3 << 2.0,  1.0,  1.0;
    b4 << 2.0,  1.0, -1.0;
    std::vector<Vector3d> landmarks;
    landmarks.push_back(b0);
    landmarks.push_back(b1);
    landmarks.push_back(b2);
    landmarks.push_back(b3);
    landmarks.push_back(b4);

    // Define the beacon's measurements
    Vector3d                y, y_noise;
    Array3d                 y_sigmas;
    Matrix3d                R;
    std::vector<Vector3d>   measurements(landmarks.size());

    y_sigmas << 0.01, 0.01, 0.01;
    R        = (y_sigmas * y_sigmas).matrix().asDiagonal();

    // Declare the Jacobian of the measurements wrt the robot pose
    Matrix<double, 3, 6>    H;      // H = J_e_x

    // Declare some temporaries
    Vector3d                e, z;   // expectation, innovation
    Matrix3d                E, Z;   // covariances of the above
    Matrix<double, 6, 3>    K;      // Kalman gain
    manif::SE3Tangentd      dx;     // optimal update step, or error-state
    manif::SE3d::Jacobian   J_xi_x; // Jacobian is typedef Matrix
    Matrix<double, 3, 6>    J_e_xi; // Jacobian

    //
    //
    // CONFIGURATION DONE

    // DEBUG
    cout << std::fixed   << std::setprecision(3) << std::showpos << endl;
    cout << "X STATE     :    X      Y      Z    TH_x   TH_y   TH_z " << endl;
    cout << "-------------------------------------------------------" << endl;
    cout << "X initial   : " << X_simulation.log().coeffs().transpose() << endl;
    cout << "-------------------------------------------------------" << endl;
    // END DEBUG

    // START TEMPORAL LOOP
    //
    //

    // Make 10 steps. Measure up to three landmarks each time.
    for (int t = 0; t < 10; t++)
    {
        //// I. Simulation ###############################################################################

        /// simulate noise
        u_noise = u_sigmas * Array6d::Random();             // control noise
        u_noisy = u_nom + u_noise;                          // noisy control

        u_simu   = u_nom;
        u_est    = u_noisy;
        u_unfilt = u_noisy;

        /// first we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -
        X_simulation = X_simulation + u_simu;               // overloaded X.rplus(u) = X * exp(u)

        /// then we measure all landmarks - - - - - - - - - - - - - - - - - - - -
        for (int i = 0; i < landmarks.size(); i++)
        {
            b = landmarks[i];                               // lmk coordinates in world frame

            /// simulate noise
            y_noise = y_sigmas * Array3d::Random();         // measurement noise

            y = X_simulation.inverse().act(b);              // landmark measurement, before adding noise
            y = y + y_noise;                                // landmark measurement, noisy
            measurements[i] = y;                            // store for the estimator just below
        }

        //// II. Estimation ###############################################################################

        /// First we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -

        X = X.plus(u_est, J_x, J_u);                        // X * exp(u), with Jacobians

        P = J_x * P * J_x.transpose() + J_u * U * J_u.transpose();

        /// Then we correct using the measurements of each lmk - - - - - - - - -
        for (int i = 0; i < NUMBER_OF_LMKS_TO_MEASURE; i++)
        {
            // landmark
            b = landmarks[i];                               // lmk coordinates in world frame

           // measurement
            y = measurements[i];                            // lmk measurement, noisy

            // expectation
            e = X.inverse(J_xi_x).act(b, J_e_xi);           // note: e = R.tr * ( b - t ), for X = (R,t).
            H = J_e_xi * J_xi_x;                            // note: H = J_e_x = J_e_xi * J_xi_x
            E = H * P * H.transpose();

            // innovation
            z = y - e;
            Z = E + R;

            // Kalman gain
            K = P * H.transpose() * Z.inverse();            // K = P * H.tr * ( H * P * H.tr + R).inv

            // Correction step
            dx = K * z;                                     // dx is in the tangent space at X

            // Update
            X = X + dx;                                     // overloaded X.rplus(dx) = X * exp(dx)
            P = P - K * Z * K.transpose();
        }

        //// III. Unfiltered ##############################################################################

        // move also an unfiltered version for comparison purposes
        X_unfiltered = X_unfiltered + u_unfilt;

        //// IV. Results ##############################################################################

        // DEBUG
        cout << "X simulated : " << X_simulation.log().coeffs().transpose() << endl;
        cout << "X estimated : " << X.log().coeffs().transpose() << endl;
        cout << "X unfilterd : " << X_unfiltered.log().coeffs().transpose() << endl;
        cout << "-------------------------------------------------------" << endl;
        // END DEBUG

    }

    //
    //
    // END OF TEMPORAL LOOP. DONE.

    return 0;
}


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     univariate_distribution_estimator_impl.cpp
 * \author   Collin Johnson
 *
 * Definition of implementations of the UnivariateDistributionEstimator for the subclasses of UnivariateDistribution:
 *
 *   - UnivariateGaussianDistribution
 *   - GammaDistribution
 *   - BetaDistribution
 *   - ExponentialDistribution
 *   - TruncatedGaussianDistribution
 */

#include "math/univariate_distribution_estimator_impl.h"
#include "math/beta_distribution.h"
#include "math/discrete_gaussian.h"
#include "math/exponential_distribution.h"
#include "math/gamma_distribution.h"
#include "math/statistics.h"
#include "math/truncated_gaussian_distribution.h"
#include "math/univariate_gaussian.h"
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <iostream>

namespace vulcan
{
namespace math
{

using DistPtr = std::unique_ptr<UnivariateDistribution>;

double log_likelihood_gamma_func(double k, double sumXi, double sumLogXi, std::size_t n)
{
    return (k - 1) * sumXi - n * k - n * k * std::log(sumXi / (k * n)) - n * std::log(boost::math::tgamma(k));
}

double log_likelihood_gamma_deriv(double k, double sumXi, double sumLogXi, std::size_t n)
{
    return n * (std::log(k) - boost::math::digamma(k) - std::log(sumXi / n)) + sumLogXi;
}

DistPtr
  UnivariateGaussianDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                                const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{new UnivariateGaussianDistribution(estimate(dataBegin, dataEnd))};
}

UnivariateGaussianDistribution
  UnivariateGaussianDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                    const std::vector<double>::const_iterator dataEnd) const
{
    double var = variance(dataBegin, dataEnd);

    if (var == 0.0) {
        std::cerr << "WARNING: UnivariateGaussianDistributionEstimator: Variance in the data was 0. Setting variance "
                     "to 1e-4.\n";
        var = 1e-4;
    }

    return UnivariateGaussianDistribution(mean(dataBegin, dataEnd), var);
}

DistPtr
  DiscreteGaussianDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                              const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{new DiscreteGaussianDistribution(estimate(dataBegin, dataEnd))};
}

DiscreteGaussianDistribution
  DiscreteGaussianDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                  const std::vector<double>::const_iterator dataEnd) const
{
    double var = variance(dataBegin, dataEnd);

    if (var == 0.0) {
        std::cerr
          << "WARNING: DiscreteGaussianDistributionEstimator: Variance in the data was 0. Setting variance to 1e-4.\n";
        var = 1e-4;
    }

    return DiscreteGaussianDistribution(mean(dataBegin, dataEnd), var);
}

DistPtr GammaDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                         const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{new GammaDistribution(estimate(dataBegin, dataEnd))};
}

GammaDistribution GammaDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                       const std::vector<double>::const_iterator dataEnd) const
{
    std::vector<double> filtered(dataBegin, dataEnd);
    auto validDataEnd = std::remove_if(filtered.begin(), filtered.end(), [](double x) {
        return x <= 0.0;
    });

    int numValidData = std::distance(filtered.begin(), validDataEnd);
    assert(numValidData);

    using namespace std::placeholders;

    double sumXi = std::accumulate(filtered.begin(), validDataEnd, 0.0);
    double sumLogXi = 0.0;
    for (auto val : boost::make_iterator_range(filtered.begin(), validDataEnd)) {
        sumLogXi += std::log(val);
    }

    double s = std::log(sumXi / numValidData) - sumLogXi / numValidData;
    double k0 = (3.0 - s + std::sqrt(std::pow(s - 3.0, 2.0) + 24.0 * s)) / (12.0 * s);

    //     NewtonRaphsonErrorFunc<double> newton(std::bind(log_likelihood_func,  _1, sumXi, sumLogXi, filtered.size()),
    //                                           std::bind(log_likelihood_deriv, _1, sumXi, sumLogXi, filtered.size()));
    //
    //     double k     = find_single_root(newton, k0, 1e-5);
    double k = k0;
    double theta = sumXi / (numValidData * k);
    return GammaDistribution(k, theta);
}

DistPtr BetaDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                        const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{new BetaDistribution(estimate(dataBegin, dataEnd))};
}

BetaDistribution BetaDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                     const std::vector<double>::const_iterator dataEnd) const
{
    double mean = math::mean(dataBegin, dataEnd);
    double variance = math::variance(dataBegin, dataEnd);

    if (variance == 0.0) {
        std::cerr << "WARNING: BetaDistributionEstimator: Variance of data was 0. Setting to 1e-4.\n";
        variance = 1e-4;
    }

    if (variance < mean * (1 - mean) && (variance > 0.0)) {
        double alpha = mean * ((mean * (1.0 - mean) / variance) - 1.0);
        double beta = (1.0 - mean) * ((mean * (1.0 - mean) / variance) - 1.0);
        return BetaDistribution(alpha, beta);
    } else {
        std::cerr << "ERROR: BetaDistributionEstimator: Could not use method-of-moments to find parameters. Mean:"
                  << mean << " Variance:" << variance << '\n'
                  << " Variance should be less than " << (mean * (1 - mean)) << '\n';
        return BetaDistribution();
    }
}

ExponentialDistributionEstimator::ExponentialDistributionEstimator(double maxValue) : max_(maxValue)
{
}

DistPtr ExponentialDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                               const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{new ExponentialDistribution(estimate(dataBegin, dataEnd))};
}

ExponentialDistribution
  ExponentialDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                             const std::vector<double>::const_iterator dataEnd) const
{
    double mean = math::mean(dataBegin, dataEnd);
    if (mean > 0.0) {
        return ExponentialDistribution(1.0 / mean, max_);
    } else {
        std::cerr << "ERROR: ExponentialDistributionEstimator: Invalid mean for the data:" << mean
                  << " Must be greater than 0.\n";
        return ExponentialDistribution(1.0, max_);
    }
}

TruncatedGaussianDistributionEstimator::TruncatedGaussianDistributionEstimator(double lower, double upper)
: lower_(lower)
, upper_(upper)
{
}

DistPtr
  TruncatedGaussianDistributionEstimator::estimateDistribution(const std::vector<double>::const_iterator dataBegin,
                                                               const std::vector<double>::const_iterator dataEnd) const
{
    return DistPtr{new TruncatedGaussianDistribution(estimate(dataBegin, dataEnd))};
}

TruncatedGaussianDistribution
  TruncatedGaussianDistributionEstimator::estimate(const std::vector<double>::const_iterator dataBegin,
                                                   const std::vector<double>::const_iterator dataEnd) const
{
    double var = variance(dataBegin, dataEnd);

    if (var == 0.0) {
        std::cerr
          << "WARNING: TruncatedGaussianDistributionEstimator: Variance in the data was 0. Setting variance to 1e-4.\n";
        var = 1e-4;
    }

    return TruncatedGaussianDistribution(mean(dataBegin, dataEnd), var, lower_, upper_);
}

}   // namespace math
}   // namespace vulcan


/*
 * Copyright (c) 2021 Nobuyuki Umetani
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include <random>
#include <vector>
#include <cstdlib>
#include <Eigen/Core>
#if defined(_WIN32) // windows
#  define NOMINMAX   // to remove min,max macro
#  include <windows.h>  // should put before glfw3.h
#endif
#define GL_SILENCE_DEPRECATION
#include <GLFW/glfw3.h>

#include "delfem2/mshprimitive.h"
#include "delfem2/eigen/ls_dense.h"
#include "delfem2/eigen/ls_sparse.h"
#include "delfem2/femsolidlinear.h"
#include "delfem2/lsitrsol.h"
#include "delfem2/mshuni.h"
#include "delfem2/femutil.h"
#include "delfem2/glfw/viewer3.h"
#include "delfem2/glfw/util.h"
#include "delfem2/opengl/old/funcs.h"
#include "delfem2/opengl/old/mshuni.h"

namespace dfm2 = delfem2;

// --------------------------------------------------------------

void Simulation_Mat3(
    std::vector<double> &aDisp,
    delfem2::CMatrixSparseBlock<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> &mA,
    //
    const std::vector<double> &aXYZ0,
    const std::vector<unsigned int> &aHex,
    const std::vector<int> &aBCFlag,
    //
    double mass,
    double myu,
    double lambda,
    const double gravity[3]) {
  const unsigned int np = aXYZ0.size() / 3;
  const unsigned int nDoF = np * 3;
  mA.setZero();
  {
    double ddW[8][8][3][3];
    {
      const double gravity_zero[3] = {0, 0, 0};
      double aP0[8][3], aU[8][3];
      delfem2::FetchData<8, 3>(aP0, aHex.data(), aXYZ0.data());
      delfem2::FetchData<8, 3>(aU, aHex.data(), aDisp.data());
      double dW[8][3];
      std::fill_n(&dW[0][0], 8 * 3, 0.0);
      std::fill_n(&ddW[0][0][0][0], 8 * 8 * 3 * 3, 0.0);
      delfem2::elemMatRes_LinearSolidGravity3_Static_Q1(
          myu, lambda,
          0, gravity_zero,
          aP0, aU, ddW, dW);
    }
    std::vector<unsigned int> tmp_buffer;
    for (unsigned int ih = 0; ih < aHex.size() / 8; ++ih) {
      const unsigned int *aIP = aHex.data() + ih * 8;
      delfem2::Merge<8, 8, 3, 3, double>(mA, aIP, aIP, ddW, tmp_buffer);
    }
  }
  Eigen::VectorXd vec_b(nDoF);
  vec_b.setZero();
  for (unsigned int ip = 0; ip < np; ++ip) {
    vec_b[ip * 3 + 0] += mass * gravity[0];
    vec_b[ip * 3 + 1] += mass * gravity[1];
    vec_b[ip * 3 + 2] += mass * gravity[2];
  }
  { // comput rhs vectors
    const Eigen::VectorXd &vd = Eigen::Map<const Eigen::VectorXd>(aDisp.data(), nDoF);
    AddMatVec(vec_b, 1.0, -1.0, mA, vd);
    std::cout << "energy" << vec_b.dot(vd) << std::endl;
  }
  SetFixedBC_Dia(mA, aBCFlag.data(), 1.f);
  SetFixedBC_Col(mA, aBCFlag.data());
  SetFixedBC_Row(mA, aBCFlag.data());
  delfem2::setZero_Flag(vec_b, aBCFlag, 0);
  // --------------------------------
  Eigen::VectorXd vec_x(vec_b.size());
  {
    double conv_ratio = 1.0e-6;
    int iteration = 1000;
    const std::size_t n = vec_b.size();
    Eigen::VectorXd tmp0(n), tmp1(n);
    std::vector<double> aConv = delfem2::Solve_CG(
        vec_b, vec_x, tmp0, tmp1,
        conv_ratio, iteration, mA);
    std::cout << aConv.size() << std::endl;
  }
  // ------------------------------
  dfm2::XPlusAY(
      aDisp,
      aBCFlag, 1.0, vec_x);
}

void Simulation_Mat4(
    std::vector<double> &aDisp,
    delfem2::CMatrixSparseBlock<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>, 3> &mA,
    //
    const std::vector<double> &aXYZ0,
    const std::vector<unsigned int> &aHex,
    const std::vector<int> &aBCFlag,
    //
    double mass,
    double myu,
    double lambda,
    const double gravity[3]) {
  const unsigned int np = aXYZ0.size() / 3;
  mA.setZero();
  {
    double ddW[8][8][3][3];
    {
      const double gravity_zero[3] = {0, 0, 0};
      double aP0[8][3], aU[8][3];
      delfem2::FetchData<8, 3>(aP0, aHex.data(), aXYZ0.data());
      delfem2::FetchData<8, 3>(aU, aHex.data(), aDisp.data());
      double dW[8][3];
      std::fill_n(&dW[0][0], 8 * 3, 0.0);
      std::fill_n(&ddW[0][0][0][0], 8 * 8 * 3 * 3, 0.0);
      delfem2::elemMatRes_LinearSolidGravity3_Static_Q1(
          myu, lambda,
          0, gravity_zero,
          aP0, aU, ddW, dW);
    }
    std::vector<unsigned int> tmp_buffer;
    for (unsigned int ih = 0; ih < aHex.size() / 8; ++ih) {
      const unsigned int *aIP = aHex.data() + ih * 8;
      delfem2::Merge<8, 8, 3, 3, double>(mA, aIP, aIP, ddW, tmp_buffer);
    }
  }
  Eigen::Matrix<double, -1, 4, Eigen::RowMajor> vec_b(np, 4);
  vec_b.setZero();
  for (unsigned int ip = 0; ip < np; ++ip) {
    vec_b(ip, 0) += mass * gravity[0];
    vec_b(ip, 1) += mass * gravity[1];
    vec_b(ip, 2) += mass * gravity[2];
  }
  { // comput rhs vectors
    Eigen::Matrix<double, -1, 4, Eigen::RowMajor> vd(np, 4);
    for (unsigned int ip = 0; ip < np; ++ip) {
      vd(ip, 0) = aDisp[ip * 3 + 0];
      vd(ip, 1) = aDisp[ip * 3 + 1];
      vd(ip, 2) = aDisp[ip * 3 + 2];
      vd(ip, 3) = 0.0;
    }
    AddMatVec(vec_b, 1.0, -1.0, mA, vd);
    std::cout << "energy" << delfem2::Dot(vec_b, vd) << std::endl;
  }
  SetFixedBC_Dia(mA, aBCFlag.data(), 1.f);
  SetFixedBC_Col(mA, aBCFlag.data());
  SetFixedBC_Row(mA, aBCFlag.data());
  delfem2::setZero_Flag(vec_b, np, aBCFlag, 0);
  // --------------------------------
  Eigen::Matrix<double, -1, 4, Eigen::RowMajor> vec_x(np, 4);
  {
    double conv_ratio = 1.0e-6;
    int iteration = 1000;
    Eigen::Matrix<double, -1, 4, Eigen::RowMajor> tmp0(np, 4), tmp1(np, 4);
    std::vector<double> aConv = delfem2::Solve_CG(
        vec_b, vec_x, tmp0, tmp1,
        conv_ratio, iteration, mA);
    std::cout << aConv.size() << std::endl;
  }
  // ------------------------------
  dfm2::XPlusAY(
      aDisp,
      np, aBCFlag, 1.0, vec_x);
}

int main() {
  std::vector<double> aXYZ0;
  std::vector<unsigned int> aHex;
  dfm2::MeshHex3_Grid(
      aXYZ0, aHex,
      20, 10, 10, 0.1);
  std::vector<double> aMass(aXYZ0.size() / 3);

  delfem2::CMatrixSparseBlock<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> A3;
  delfem2::CMatrixSparseBlock<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>, 3> A4;
  {
    const unsigned int np = aXYZ0.size() / 3;
    std::vector<unsigned int> psup_ind, psup;
    dfm2::JArray_PSuP_MeshElem(
        psup_ind, psup,
        aHex.data(), aHex.size() / 8, 8,
        aXYZ0.size() / 3);
    A3.Initialize(np);
    A3.SetPattern(psup_ind.data(), psup_ind.size(), psup.data(), psup.size());
    A4.Initialize(np);
    A4.SetPattern(psup_ind.data(), psup_ind.size(), psup.data(), psup.size());
  }

  std::vector<double> aDisp(aXYZ0.size(), 0.0);
  std::vector<int> aBCFlag(aXYZ0.size(), 0.0); // 0: free, 1: fix BC
  {
    for (unsigned int ip = 0; ip < aXYZ0.size() / 3; ++ip) {
      double x0 = aXYZ0[ip * 3 + 0];
      if (x0 > 1.0e-10) { continue; }
      aBCFlag[ip * 3 + 0] = 1;
      aBCFlag[ip * 3 + 1] = 1;
      aBCFlag[ip * 3 + 2] = 1;
    }
  }
  const double mass = 0.5;
  const double gravity[3] = {0, 0, -10};
  aDisp.assign(aXYZ0.size(), 0.0);
  for (unsigned int i = 0; i < aDisp.size(); ++i) {
    if (aBCFlag[i] != 0) { continue; }
    aDisp[i] = (i % 10) * 1.0e-4;
  }
  Simulation_Mat3(
      aDisp, A3,
      aXYZ0, aHex, aBCFlag,
      mass, 1.0e+5, 1.e+5, gravity);

  aDisp.assign(aXYZ0.size(), 0.0);
  for (unsigned int i = 0; i < aDisp.size(); ++i) {
    if (aBCFlag[i] != 0) { continue; }
    aDisp[i] = (i % 10) * 1.0e-4;
  }
  Simulation_Mat4(
      aDisp, A4,
      aXYZ0, aHex, aBCFlag,
      mass, 1.0e+5, 1.e+5, gravity);

  // ----------------------
  delfem2::glfw::CViewer3 viewer(1.5);
//  viewer.camera.camera_rot_mode = delfem2::CCam3_OnAxisZplusLookOrigin<double>::CAMERA_ROT_MODE::ZTOP;
//  viewer.camera.theta = 0.1;
//  viewer.camera.psi = 0.1;
  delfem2::glfw::InitGLOld();
  viewer.InitGL();

  delfem2::opengl::setSomeLighting();
  while (!glfwWindowShouldClose(viewer.window)) {
    // -----
    viewer.DrawBegin_oldGL();
    ::glDisable(GL_LIGHTING);
    ::glColor3d(0, 0, 0);
    delfem2::opengl::DrawMeshHex3D_EdgeDisp(
        aXYZ0.data(), aXYZ0.size() / 3,
        aHex.data(), aHex.size() / 8,
        aDisp.data());
    //
    ::glEnable(GL_LIGHTING);
//    dfm2::opengl::DrawMeshHex3D_FaceNorm(aXYZ0.data(), aHex.data(), aHex.size() / 8);
    delfem2::opengl::DrawHex3D_FaceNormDisp(aXYZ0, aHex, aDisp);
    viewer.SwapBuffers();
    glfwPollEvents();
  }
  glfwDestroyWindow(viewer.window);
  glfwTerminate();
  exit(EXIT_SUCCESS);
}


/*
 * Copyright 2018, LAAS-CNRS
 * Author: Steve Tonneau
 */

#ifndef BEZIER_COM_TRAJ_LIB_COMMON_SOLVE_H
#define BEZIER_COM_TRAJ_LIB_COMMON_SOLVE_H

#include <hpp/bezier-com-traj/local_config.hh>
#include <hpp/bezier-com-traj/data.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
#include <hpp/bezier-com-traj/solver/solver-abstract.hpp>

#include <Eigen/Dense>

namespace bezier_com_traj {

/**
 * @brief ComputeDiscretizedWaypoints Given the waypoints defining a bezier curve,
 * computes a discretization of the curve
 * @param wps original waypoints
 * @param bernstein berstein polynoms for
 * @param numSteps desired number of wayoints
 * @return a vector of waypoint representing the discretization of the curve
 */
BEZIER_COM_TRAJ_DLLAPI std::vector<waypoint6_t> ComputeDiscretizedWaypoints(
    const std::vector<waypoint6_t>& wps, const std::vector<ndcurves::Bern<double> >& bernstein, int numSteps);

/**
 * @brief compute6dControlPointInequalities Given linear and angular control waypoints,
 * compute the inequality matrices A and b, A x <= b that constrain the desired control point x.
 * @param cData data for the current contact phase
 * @param wps waypoints or the linear part of the trajectory
 * @param wpL waypoints or the angular part of the trajectory
 * @param useAngMomentum whether the angular momentum is consider or equal to 0
 * @param fail set to true if problem is found infeasible
 * @return
 */
BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(
    const ContactData& cData, const std::vector<waypoint6_t>& wps, const std::vector<waypoint6_t>& wpL,
    const bool useAngMomentum, bool& fail);

/**
 * @brief compute6dControlPointEqualities Given linear and angular control waypoints,
 * compute the equality matrices D and d, D [x; Beta]' = d that constrain the desired control point x and contact
 * forces Beta.
 * @param cData data for the current contact phase
 * @param wps waypoints or the linear part of the trajectory
 * @param wpL waypoints or the angular part of the trajectory
 * @param useAngMomentum whether the angular momentum is consider or equal to 0
 * @param fail set to true if problem is found infeasible
 * @return
 */
BEZIER_COM_TRAJ_DLLAPI std::pair<MatrixXX, VectorX> compute6dControlPointEqualities(
    const ContactData& cData, const std::vector<waypoint6_t>& wps, const std::vector<waypoint6_t>& wpL,
    const bool useAngMomentum, bool& fail);

/**
 * @brief solve x' h x + 2 g' x, subject to A*x <= b using quadprog
 * @param A Inequality matrix
 * @param b Inequality vector
 * @param H Cost matrix
 * @param g cost Vector
 * @param x initGuess initial guess
 * @param minBounds lower bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or
 * a size equal to x. Unbounded elements should be lesser or equal to solvers::UNBOUNDED_UP;
 * @param maxBounds upper bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or
 * a size equal to x Unbounded elements should be higher or lower than solvers::UNBOUNDED_DOWN;
 * @param solver solver used to solve QP or LP. If LGPK is used, Hessian is not considered as an lp is solved
 * @return
 */
BEZIER_COM_TRAJ_DLLAPI ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H, Cref_vectorX g,
                                        Cref_vectorX initGuess, Cref_vectorX minBounds, Cref_vectorX maxBounds,
                                        const solvers::SolverType solver = solvers::SOLVER_QUADPROG);
/**
 * @brief solve x' h x + 2 g' x, subject to A*x <= b and D*x = c using quadprog
 * @param A Inequality matrix
 * @param b Inequality vector
 * @param D Equality matrix
 * @param d Equality vector
 * @param H Cost matrix
 * @param g cost Vector
 * @return
 */
BEZIER_COM_TRAJ_DLLAPI ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX D, Cref_vectorX d,
                                        Cref_matrixXX H, Cref_vectorX g, Cref_vectorX initGuess,
                                        const solvers::SolverType solver = solvers::SOLVER_QUADPROG);

/**
 * @brief solve x' h x + 2 g' x, subject to A*x <= b using quadprog, with x of fixed dimension 3
 * @param Ab Inequality matrix and vector
 * @param Hg Cost matrix and vector
 * @return
 */
BEZIER_COM_TRAJ_DLLAPI ResultData solve(const std::pair<MatrixXX, VectorX>& Ab, const std::pair<MatrixXX, VectorX>& Hg,
                                        const VectorX& init,
                                        const solvers::SolverType solver = solvers::SOLVER_QUADPROG);

/**
 * @brief solve x' h x + 2 g' x, subject to A*x <= b  and D*x = c using quadprog, with x of fixed dimension 3
 * @param Ab Inequality matrix and vector
 * @param Dd Equality matrix and vector
 * @param Hg Cost matrix and vector
 * @param minBounds lower bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or
 * a size equal to x. Unbounded elements should be equal to -std::numeric_limits<double>::infinity();
 * @param maxBounds upper bounds on x values. Can be of size 0 if all elements of x are unbounded in that direction, or
 * a size equal to x Unbounded elements should be equal to  std::numeric_limits<double>::infinity();
 * @param solver solver used to solve QP or LP. If LGPK is used, Hessian is not considered as an lp is solved
 * @return
 */
BEZIER_COM_TRAJ_DLLAPI ResultData solve(const std::pair<MatrixXX, VectorX>& Ab, const std::pair<MatrixXX, VectorX>& Dd,
                                        const std::pair<MatrixXX, VectorX>& Hg, Cref_vectorX minBounds,
                                        Cref_vectorX maxBounds, const VectorX& init,
                                        const solvers::SolverType solver = solvers::SOLVER_QUADPROG);

template <typename Point>
BEZIER_COM_TRAJ_DLLAPI std::vector<std::pair<double, Point> > computeDiscretizedWaypoints(const ProblemData& pData,
                                                                                double T,
                                                                                const T_time& timeArray);

template <typename Point>
BEZIER_COM_TRAJ_DLLAPI std::vector<std::pair<double, Point> > computeDiscretizedAccelerationWaypoints(
    const ProblemData& pData, double T, const T_time& timeArray);

}  // end namespace bezier_com_traj

#include "common_solve_methods.inl"

#endif


#include <iostream>
#include <Eigen/IterativeLinearSolvers>

#include <libv/lma/lm/solver/solver.hpp>
#include <libv/lma/lm/solver/verbose.hpp>

using namespace Eigen;

void eigen_pcg(const Eigen::MatrixXd& m, const Eigen::VectorXd& jte)
{

  std::cout << "\n\n EIGEN PCG " << std::endl;
  size_t n = jte.size();
  VectorXd x(n), b(jte);
//   typedef SparseMatrix<double> Mat;
  typedef Eigen::MatrixXd Mat;
//   Mat A(n,n);
  Mat A = m;
//   for(size_t i = 0 ; i < m.cols() ; ++i)
//     for(size_t j = 0 ; j < m.rows() ; ++j)
//       A.coeff(j,i) = m(j,i);

//   for(size_t i = 0 ; i < n ; ++i)
//     A.coeffRef(i,i) = 1.0;
//   A.setIdentity();
  std::cout << A << std::endl;
  x.setZero();
  std::cout <<"\n X = " <<  x.transpose() << std::endl;
  std::cout <<"\n B = " <<  b.transpose() << std::endl;
  // fill A and b
  ConjugateGradient<Mat> cg;
  cg.compute(A);
  x = cg.solve(b);
  std::cout << "#iterations:     " << cg.iterations() << std::endl;
  std::cout << "estimated error: " << cg.error()      << std::endl;
  // update b, and solve again
//   x = cg.solve(b);
  std::cout << "\n X = " << x.transpose() << std::endl;
}

// typedef Eigen::Matrix<double,2,1> Type0;
// typedef Eigen::Matrix<double,3,1> Type1;
// typedef Eigen::Matrix<double,1,1> Type2;
//
// struct F
// {
//   bool operator()(const Type0& , const Type1& , const Type2&, Eigen::Matrix<double,2,1>&) const
//   {
//     return true;
//   }
// };

int main()
{
  /*
  Type0 t0;
  Type1 t1;
  Type2 t2;
  std::cout << " Test pcg " << std::endl;
  lma::Solver<F> solver(1,1);

//   solver.algo.norm_eq.seuil=0.9999;
//   solver.algo.norm_eq.max_iteration=100000typedef typename SelectAlgo<Container,Container::NbClass,AlgoTag>::type Algorithm;
//       Algorithm algo(config);;

  auto i0 = ttt::Indice<Type0*>(0);
  auto i1 = ttt::Indice<Type1*>(0);
  auto i2 = ttt::Indice<Type2*>(0);
  solver.add(F(),&t0,&t1,&t2);//lma::bf::make_vector(i0,i1,i2),F());
//   solver.solve(lma::enable_verbose_output());
//   solver.algo.compute_b(solver.algo.ba_);
//   solver.algo.compute_delta_a(solver.algo.ba_);
  using namespace lma;
  typedef typename SelectAlgo<lma::Solver<F>::Container,lma::Solver<F>::Container::NbClass,ImplicitSchurTag<1>>::type Algorithm;
  Algorithm algo(ImplicitSchurTag<1>(0.9999,100000));
  algo.init(solver.bundle);

  lma::bf::at_key<lma::bf::pair<Type0*,Type0*>>(algo.ba_.h())(i0,0) <<
    1,0,
    0,2;

  lma::bf::at_key<lma::bf::pair<Type2*,Type2*>>(algo.ba_.h())(i2,0) <<
    1;

  lma::bf::at_key<lma::bf::pair<Type0*,Type1*>>(algo.ba_.h())(i0,0) <<
    3.2,0.5,2,
    0,-2.5,-1.5;

  lma::bf::at_key<lma::bf::pair<Type1*,Type1*>>(algo.ba_.h())(i1,0) <<
    3,0,0,
    0,4,0,
    0,0,5;

  lma::bf::at_key<Type0*>(algo.ba_.jte())(i0) << 1,2;
  lma::bf::at_key<Type1*>(algo.ba_.jte())(i1) << 3,4,5;
  lma::bf::at_key<Type2*>(algo.ba_.jte())(i2) << 1;

  std::cout << std::endl;
  std::cout << " A = \n" << lma::to_mat(algo.ba_.h()) << std::endl;
//   std::cout << " B = " << lma::to_vect(solver.algo.schur_.bs_).transpose() << std::endl;
  std::cout << " B = " << lma::to_vect(algo.ba_.jte()).transpose() << std::endl;

  algo.compute_y(algo.ba_);
  algo.compute_b(algo.ba_);
  algo.compute_delta_a(algo.ba_);

  std::cout << " X = " << lma::to_vect(algo.ba_.delta()).transpose() << std::endl;

  eigen_pcg(lma::to_mat(algo.ba_.h()),lma::to_vect(algo.ba_.jte()));*/
}


/*
 * This file is part of the Interpolated Polyline (https://github.com/fzi-forschungszentrum-informatik/P3IV),
 * copyright by FZI Forschungszentrum Informatik, licensed under the BSD-3 license (see LICENSE file in main directory)
 */

#pragma once
#include <iostream>
#include <Eigen/Core>
#include <glog/logging.h>

namespace util_probability {

template <typename T, int Dim>
struct NormalDistribution {

    using Mean = Eigen::Matrix<T, Dim, 1>;
    using Covariance = Eigen::Matrix<T, Dim, Dim>;

    NormalDistribution() : _mean{Mean::Zero()}, _covariance{Covariance::Zero()} {
    }

    NormalDistribution(const Eigen::Ref<const Mean>& mean_) : _mean{mean_}, _covariance{Covariance::Zero()} {
    }

    NormalDistribution(const Eigen::Ref<const Mean>& mean_, const Eigen::Ref<const Covariance>& covariance_)
            : _mean{mean_}, _covariance{covariance_} {
        LOG_ASSERT(_covariance.rows() == _covariance.cols());
        LOG_ASSERT(_mean.size() == _covariance.rows());
    }

    NormalDistribution(const Eigen::Ref<const Mean>& mean_, Eigen::Matrix<T, Dim, 1>& variance_)
            : _mean{mean_}, _covariance{Covariance::Zero()} {

        for (size_t i = 0; i < variance_.size(); i++) {
            _covariance(i, i) = variance_(i);
        }
    }

    size_t dimension() const {
        return static_cast<size_t>(_mean.size());
    }

    Mean mean() const {
        return _mean;
    }

    T mean(size_t r, size_t c) const {
        return _mean(r, c);
    }

    Covariance covariance() const {
        return _covariance;
    }

    T covariance(size_t r, size_t c) const {
        return _covariance(r, c);
    }

    virtual Eigen::Matrix<T, Dim, 1> variance() const {
        Eigen::Matrix<T, Dim, 1> variance;
        for (size_t i = 0; i < _covariance.rows(); i++) {
            variance(i) = _covariance(i, i);
        }
        return variance;
    }

protected:
    Mean _mean;
    Covariance _covariance;
};

} // namespace util_probability


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_ALGORITHMS_POISSON_SOLVER_HPP
#define PIC_ALGORITHMS_POISSON_SOLVER_HPP

#include "../base.hpp"

#include "../image.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Sparse"
    #include "../externals/Eigen/src/SparseCore/SparseMatrix.h"
#else
    #include <Eigen/Sparse>
    #include <Eigen/src/SparseCore/SparseMatrix.h>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief computePoissonSolver
 * @param f
 * @param ret
 * @return
 */
PIC_INLINE Image *computePoissonSolver(Image *f, Image *ret = NULL)
{
    if(f == NULL) {
        return NULL;
    }

    //allocate the output
    if(ret == NULL) {
        ret = f->allocateSimilarOne();
    }

    int width = f->width;
    int height = f->height;
    int tot = height * width;

    #ifdef PIC_DEBUG
        printf("Init matrix...");
    #endif

    std::vector< Eigen::Triplet< double > > tL;

    for(int i = 0; i < height; i++) {
        int tmpI = i * width;

        for(int j = 0; j < width; j++) {
            int indI = tmpI + j;

            tL.push_back(Eigen::Triplet< double > (indI, indI, 4.0f));

            if(((indI + 1) < tot) &&
               ((indI % width) != (width - 1))) {

                tL.push_back(Eigen::Triplet< double > (indI, indI + 1, -1.0f));
                tL.push_back(Eigen::Triplet< double > (indI + 1, indI, -1.0f));
            }
        }
    }

    for(int i = 0; i < (tot - width); i++) {
        tL.push_back(Eigen::Triplet< double > (i + width, i         , -1.0f));
        tL.push_back(Eigen::Triplet< double > (i        , i +  width, -1.0f));
    }

    #ifdef PIC_DEBUG
        printf("Ok\n");
    #endif

    //solve the linear system for each color channel
    Eigen::SparseMatrix<double> A = Eigen::SparseMatrix<double>(tot, tot);
    A.setFromTriplets(tL.begin(), tL.end());
    Eigen::SimplicialCholesky<Eigen::SparseMatrix<double> > solver(A);

    for(int k = 0; k < f->channels; k++) {

        Eigen::VectorXd b, x;
        b = Eigen::VectorXd::Zero(tot);

        //copy values from f to b
        for(int i = 0; i < height; i++) {
            int tmpI = i * width;
            for(int j = 0; j < width; j++) {
                int indI = (tmpI + j);
                b[indI] = - f->data[indI * f->channels + k];
            }
        }

        x = solver.solve(b);

        if(solver.info() != Eigen::Success) {
            #ifdef PIC_DEBUG
                printf("SOLVER FAILED!\n");
            #endif

            return ret;
        }

        #ifdef PIC_DEBUG
            printf("SOLVER SUCCESS!\n");
        #endif

        for(int i = 0; i < height; i++) {
            int tmpI = i * width;

            for(int j = 0; j < width; j++) {
                (*ret)(j, i)[k] = float(x(tmpI + j));
            }
        }
    }

    return ret;
}

#endif

/**
 * @brief computePoissonSolverIterative
 * @param img
 * @param laplacian
 * @param coords
 * @param maxSteps
 * @return
 */
PIC_INLINE Image *computePoissonSolverIterative(Image *img, Image *laplacian,
                              std::vector<int> coords,
                              int maxSteps = 100)
{
    #ifdef PIC_DEBUG
        printf("Iterative Poisson solver... ");
    #endif

    if(maxSteps < 1) {
        maxSteps = 100;
    }

    Image *tmpImg = img->clone();
    Image *tmpSwap = NULL;

    int c, x, y;

    for(int i = 0; i < maxSteps; i++) {
        for(unsigned int j = 0; j < coords.size(); j++) {
            int coord = coords[j];
            img->reverseAddress(coord, x, y);

            float workValue = -laplacian->data[coord];

            c = img->getAddress(x + 1, y);
            workValue += img->data[c];

            c = img->getAddress(x - 1, y);
            workValue += img->data[c];

            c = img->getAddress(x, y + 1);
            workValue += img->data[c];

            c = img->getAddress(x, y - 1);
            workValue += img->data[c];

            tmpImg->data[coord] = workValue / 4.0f;
        }

        tmpSwap = img;
        img     = tmpImg;
        tmpImg  = tmpSwap;
    }

    #ifdef PIC_DEBUG
        printf("done.\n");
    #endif

    return img;
}

} // end namespace pic

#endif /* PIC_ALGORITHMS_POISSON_SOLVER_HPP */


//
// Created by tim on 22.02.21.
//
#include <Eigen/StdVector>
#include <Eigen/Geometry>
#include <iostream>
#include "graphSlamSaveStructure.h"

void createExampleVertex(graphSlamSaveStructure &graphSaved) {
    Eigen::Vector3f positionVertex0(0, 0, 0);
    Eigen::AngleAxisf rotation_vector0(0.0f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationVertex0(rotation_vector0);
    float covariance0 = 0.0;
    Eigen::Vector3f covarianceVector(1, 1, 0);
    graphSaved.addVertex(0, positionVertex0, rotationVertex0, covarianceVector * covariance0, covariance0);

    Eigen::Vector3f positionVertex1(1, 1, 0);
    Eigen::AngleAxisf rotation_vector1(-0.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationVertex1(rotation_vector1.toRotationMatrix());
    graphSaved.addVertex(1, positionVertex1, rotationVertex1, covarianceVector * covariance0, covariance0);

    Eigen::Vector3f positionVertex2(1, 1, 0);
    Eigen::AngleAxisf rotation_vector2(-0.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationVertex2(rotation_vector2.toRotationMatrix());
    graphSaved.addVertex(2, positionVertex2, rotationVertex2, covarianceVector * covariance0, covariance0);

    Eigen::Vector3f positionVertex3(1, 1, 0);
    Eigen::AngleAxisf rotation_vector3(0.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationVertex3(rotation_vector3.toRotationMatrix());
    graphSaved.addVertex(3, positionVertex3, rotationVertex3, covarianceVector * covariance0, covariance0);

    Eigen::Vector3f positionVertex4(2, 2, 0);
    graphSaved.addVertex(4, positionVertex4, rotationVertex0, covarianceVector * covariance0, covariance0);

}

void createExampleEdge(graphSlamSaveStructure &graphSaved) {

    Eigen::Vector3f eyeVector(1, 1, 0);
    Eigen::Vector3f positionDifference0(8, 10, 0);
    Eigen::AngleAxisf rotation_vector0(-0.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationDifference0(rotation_vector0.toRotationMatrix());
    float covariancePosition0 = 0.1;
    float covarianceQuaternion0 = 0.1;
    graphSaved.addEdge(0, 1, positionDifference0, rotationDifference0, eyeVector * covariancePosition0,
                       covarianceQuaternion0);

    Eigen::Vector3f positionDifference1(4, 9, 0);
    Eigen::AngleAxisf rotation_vector1(30.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationDifference1(rotation_vector1.toRotationMatrix());
    float covariancePosition1 = 3;
    float covarianceQuaternion1 = 3;
    graphSaved.addEdge(1, 2, positionDifference1, rotationDifference1, eyeVector * covariancePosition1,
                       covarianceQuaternion1);

    Eigen::Vector3f positionDifference2(0, 12.4, 0);
    Eigen::AngleAxisf rotation_vector2(0.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationDifference2(rotation_vector2.toRotationMatrix());
    float covariancePosition2 = 0.1;
    float covarianceQuaternion2 = 0.1;
    graphSaved.addEdge(2, 3, positionDifference2, rotationDifference2, eyeVector * covariancePosition2,
                       covarianceQuaternion2);

    Eigen::Vector3f positionDifference3(2, -9, 0);
    Eigen::AngleAxisf rotation_vector3(0.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationDifference3(rotation_vector3.toRotationMatrix());
    float covariancePosition3 = 0.1;
    float covarianceQuaternion3 = 0.1;
    graphSaved.addEdge(3, 4, positionDifference3, rotationDifference3, eyeVector * covariancePosition3,
                       covarianceQuaternion3);

    Eigen::Vector3f positionDifference4(2, 12.4 - 9, 0);
    Eigen::AngleAxisf rotation_vector4(0.0f / 180.0f * 3.14159f, Eigen::Vector3f(0, 0, 1));
    Eigen::Quaternionf rotationDifference4(rotation_vector4.toRotationMatrix());
    float covariancePosition4 = 0.1;
    float covarianceQuaternion4 = 0.1;
    graphSaved.addEdge(2, 4, positionDifference4, rotationDifference4, eyeVector * covariancePosition4,
                       covarianceQuaternion4);

}

int
main(int argc, char **argv) {
    const int dimension = 3;
    graphSlamSaveStructure graphSaved(dimension);

    createExampleVertex(graphSaved);

    createExampleEdge(graphSaved);

    //graphSaved.optimizeGraphWithSlam();
    graphSaved.printCurrentStateGeneralInformation();
    //graphSaved.printCurrentState();
    //graphSaved.getEdgeBetweenNodes(2,4);
    //std::cout << "now hierachical graph design" << std::endl;
    graphSaved.createHierachicalGraph(1);//this is 1 m
    graphSaved.optimizeGraphWithSlamTopDown(false);
    graphSlamSaveStructure differentGraph = graphSaved;

    differentGraph.optimizeGraphWithSlam(false);

    graphSaved.printCurrentStateGeneralInformation();
    //hierachicalGraph.printCurrentStateGeneralInformation();
    graphSaved.printCurrentStateGeneralInformation();

    return (0);
}


#include <Eigen/Dense>
#include <mutex>
#include <chrono>
#include <atomic>
#include <iostream>

#ifndef UTILS_KALMAN_H
#define UTILS_KALMAN_H

namespace utils {

    //https://stackoverflow.com/a/28876046
    template <typename T>
    struct is_chrono_duration
    {
        static constexpr bool value = false;
    };

    template <typename Rep, typename Period>
    struct is_chrono_duration<std::chrono::duration<Rep, Period>>
    {
        static constexpr bool value = true;
    };

    static void PRINT_MATRIX_SIZES(const Eigen::MatrixXd& P_0, const Eigen::VectorXd& z, const Eigen::VectorXd& u) {
        int m = z.size(), l = u.size(), n = P_0.rows();
        std::cout << "\n\n\n--------------------------" << std::endl;
        std::cout << "m = z.size() = " << m << std::endl;
        std::cout << "l = u.size() = " << l << std::endl;
        std::cout << "n = P.rows() = " << n << std::endl;
        std::cout << "A(nxn) = " << n << "x" << n << std::endl;
        std::cout << "B(nxl) = " << n << "x" << l << std::endl;
        std::cout << "C(mxn) = " << m << "x" << n << std::endl;
        std::cout << "Q(nxn) = " << n << "x" << n << std::endl;
        std::cout << "R(mxm) = " << m << "x" << m << std::endl;
        std::cout << "K(nxm) = " << n << "x" << m << std::endl;
        std::cout << "x(n) = " << n << std::endl;
        std::cout << "--------------------------\n\n\n" << std::endl;
    }

    static void PRINT_MATRIX_SIZES(const Eigen::MatrixXd& P_0, const Eigen::VectorXd& z) {
        int m = z.size(), n = P_0.rows();
        std::cout << "\n\n\n--------------------------" << std::endl;
        std::cout << "m = z.size() = " << m << std::endl;
        std::cout << "n = P.rows() = " << n << std::endl;
        std::cout << "A(nxn) = " << n << "x" << n << std::endl;
        std::cout << "C(mxn) = " << m << "x" << n << std::endl;
        std::cout << "Q(nxn) = " << n << "x" << n << std::endl;
        std::cout << "R(mxm) = " << m << "x" << m << std::endl;
        std::cout << "K(nxm) = " << n << "x" << m << std::endl;
        std::cout << "x(n) = " << n << std::endl;
        std::cout << "--------------------------\n\n\n" << std::endl;
    }
    template <typename T = std::chrono::milliseconds>
    class Kalman {
        static_assert(is_chrono_duration<T>::value, "T not derived from std::chrono::duration");
    private:
        Eigen::VectorXd x_;
        std::mutex mtx_;
        std::chrono::steady_clock::time_point last_call_;
        Eigen::MatrixXd P_, K_, I_;
        std::atomic<long> dt_{ 0 };
        std::atomic_bool first_call_ {true};

        /*
        Method to set dt_ for the integral / derivate term
        */
        void setDt();

        /**
         * Initialize the component
         */
        void init();

        /**
         * Update all matrices and dt
         */
        void preupdate();

        /**
         * Update the matrixes and calculate the values based on new x
         * @param z The measured / calculated system state
         */
        void updateStep(const Eigen::VectorXd& z);

    protected:
        Eigen::MatrixXd A_, Q_, B_, R_, C_;

        /**
         * Method to update the A matrix (state transition model) of the Kalman Filter
         */
        virtual void updateA();

        /**
         * Method to update the B matrix (control-input) of the Kalman Filter
         */
        virtual void updateB();

        /**
         * Method to update the C matrix (uutput matrix) of the Kalman Filter
         */
        virtual void updateC();

        /**
         * Method to update the Q matrix (process noise covariance) of the Kalman Filter
         */
        virtual void updateQ();

        /**
         * Method to update the R matrix (measurement noise covariance) of the Kalman Filter
         */
        virtual void updateR();

    public:
        /**
         * Constructor
         * @param A State transition model
         * @param B Control input matrix
         * @param C Output matrix
         * @param P_0 Initial error covariance estiamtion
         * @param Q Process noise covariance
         * @param R Measurement noise covariance
         * @param x0 Inital state guess
         */
        Kalman(
            const Eigen::MatrixXd& A,
            const Eigen::MatrixXd& B,
            const Eigen::MatrixXd& C,
            const Eigen::MatrixXd& P_0,
            const Eigen::MatrixXd& Q,
            const Eigen::MatrixXd& R,
            const Eigen::VectorXd& x0
        );

        /**
         * Constructor
         * @param A State transition model
         * @param B Control input matrix
         * @param C Output matrix
         * @param P_0 Initial error covariance estiamtion
         * @param Q Process noise covariance
         * @param R Measurement noise covariance
         */
        Kalman(
            const Eigen::MatrixXd& A,
            const Eigen::MatrixXd& B,
            const Eigen::MatrixXd& C,
            const Eigen::MatrixXd& P_0,
            const Eigen::MatrixXd& Q,
            const Eigen::MatrixXd& R
        );

        /**
         * Constructor
         *
         * @param C Output matrix
         * @param P_0 Initial error covariance estiamtion
         * @param R Measurement noise covariance
         * @param x0 Inital state guess
         */
        Kalman(
            const Eigen::MatrixXd& C,
            const Eigen::MatrixXd& P_0,
            const Eigen::MatrixXd& R,
            const Eigen::VectorXd& x0
        );

        /**
         * Constructor
         *
         * @param C Output matrix
         * @param P_0 Initial error covariance estiamtion
         * @param R Measurement noise covariance
         */
        Kalman(
            const Eigen::MatrixXd& C,
            const Eigen::MatrixXd& P_0,
            const Eigen::MatrixXd& R
        );

        /**
         * Predict the state of the system based on observed state
         * @param out Vector to store prediction
         * @param z The observed state
         *
         */
        void predict(Eigen::VectorXd& out, const Eigen::VectorXd& z);

        /**
         * Predict the state of the system based on control input
         * @param out Vector to store prediction
         * @param z The observed state
         * @param u control input
         */
        void predict(Eigen::VectorXd& out, const Eigen::VectorXd& z, const Eigen::VectorXd& u);

        /**
         * Method to get dt value
         * @returns delta t between last steps
         */
        double getDt();
    };
}

#endif


#include "cloud_math.h"
#include <algorithm>

#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Eigen>
#include <Eigen/Sparse>

#define LOG1(message)

namespace Cloud
{

//----( random numbers )------------------------------------------------------

void randomize (Vector<float> x, float sigma)
{
  const size_t x_size = x.size;
  float * restrict x_data = x.data;

  for (size_t i = 0; i < x_size; ++i) {
    x_data[i] = sigma * random_std();
  }
}

size_t random_index (const Vector<float> & likes)
{
  float total = sum(likes);
  ASSERT_LT(0, total);

  while (true) {
    float t = random_unif(0, total);

    for (int i = 0, I = likes.size; i < I; ++i) {

      t -= likes[i];
      if (t < 0) return i;
    }
  }
}

size_t random_index (const VectorXf & likes)
{
  float total = likes.sum();
  ASSERT_LT(0, total);

  while (true) {
    float t = random_unif(0, total);

    for (int i = 0, I = likes.size(); i < I; ++i) {

      t -= likes(i);
      if (t < 0) return i;
    }
  }
}

int random_index (const VectorSf & likes)
{
  float total = likes.sum();
  ASSERT_LT(0, total);

  while (true) {
    float t = random_unif(0, total);

    for (VectorSf::InnerIterator iter(likes); iter; ++iter) {

      t -= iter.value();
      if (t < 0) return iter.index();
    }
  }
}

void generate_noise (Vector<int8_t> & noise, float sigma)
{
  const float quantization_correction = 1 / 6.0f;
  sigma = sqrtf(sqr(sigma) + quantization_correction);

  for (size_t i = 0, I = noise.size; i < I; ++i) {
    noise[i] = roundi(sigma * random_std());
  }
}

//----( normalization )-------------------------------------------------------

void normalize_l1 (VectorXf & x, float tot) { x *= tot / x.sum(); }
void normalize_l1 (VectorSf & x, float tot) { x *= tot / x.sum(); }
void normalize_l1 (MatrixXf & x, float tot) { x *= tot / x.sum(); }
void normalize_l1 (MatrixSf & x, float tot) { x *= tot / x.sum(); }

void normalize_rows_l1 (size_t I, size_t J, Vector<float> & A)
{
  for (size_t i = 0; i < I; ++i) {

    Vector<float> row = A.block(J, i);

    row /= sum(row);
  }
}

void normalize_columns_l1 (MatrixXf & A)
{
  const int I = A.rows();
  const int J = A.cols();

  for (int j = 0; j < J; ++j) {

    Vector<float> col(I, & A.coeffRef(0,j));

    col /= sum(col);
  }
}

void normalize_columns_l1 (MatrixXd & A)
{
  const int I = A.rows();
  const int J = A.cols();

  for (int j = 0; j < J; ++j) {

    Vector<double> col(I, & A.coeffRef(0,j));

    col /= sum(col);
  }
}

void normalize_columns_l1 (MatrixSf & A)
{
  // WARNING this assumes column-major ordering

  for (int i = 0; i < A.outerSize(); ++i) {
    A.col(i) *= 1.0f / A.col(i).sum();
  }
}

//----( sparse tools )--------------------------------------------------------

double density (const VectorXf & x)
{
  const size_t I = x.size();

  double sum_x1 = 0;
  double sum_x2 = 0;

  const float * restrict x_ = x.data();

  for (size_t i = 0; i < I; ++i) {
    double xi = x_[i];

    sum_x1 += max(-xi,xi);
    sum_x2 += xi * xi;
  }

  return sqr(sum_x1) / sum_x2 / I;
}

float density (const VectorSf & x)
{
  float sum_x1 = 0;
  float sum_x2 = 0;

  for (VectorSf::InnerIterator iter(x); iter; ++iter) {
    float xi = iter.value();

    sum_x1 += max(-xi,xi);
    sum_x2 += xi * xi;
  }

  return sqr(sum_x1) / sum_x2 / x.size();
}

double density (const MatrixXf & x)
{
  const size_t I = x.rows() * x.cols();

  double sum_x1 = 0;
  double sum_x2 = 0;

  const float * restrict x_ = x.data();

  for (size_t i = 0; i < I; ++i) {
    double xi = x_[i];

    sum_x1 += max(-xi,xi);
    sum_x2 += xi * xi;
  }

  return sqr(sum_x1) / sum_x2 / I;
}

double density (const MatrixSf & x)
{
  double sum_x1 = 0;
  double sum_x2 = 0;

  for (int i = 0; i < x.outerSize(); ++i) {
    for (MatrixSf::InnerIterator iter(x,i); iter; ++iter) {
      double xi = iter.value();

      sum_x1 += max(-xi,xi);
      sum_x2 += xi * xi;
    }
  }

  return sqr(sum_x1) / sum_x2 / x.size();
}

namespace {
struct SparseVectorEntry
{
  float value;
  int index;

  SparseVectorEntry () {}
  SparseVectorEntry (float v, int i) : value(v), index(i) {}

  bool operator< (const SparseVectorEntry & other) const
  {
    return value > other.value;
  }
};
struct SparseMatrixEntry
{
  float value;
  int row;
  int col;

  SparseMatrixEntry () {}
  SparseMatrixEntry (float v, int i, int j) : value(v), row(i), col(j) {}

  bool operator< (const SparseMatrixEntry & other) const
  {
    return value > other.value;
  }
};
} // anonymous namespace

float sparsify_size (VectorSf & sparse, int size)
{
  ASSERT_LT(0, size);
  ASSERT_LT(size, sparse.nonZeros());

  static std::vector<SparseVectorEntry> entries;

  for (VectorSf::InnerIterator iter(sparse); iter; ++iter) {
    entries.push_back(SparseVectorEntry(iter.value(), iter.index()));
  }

  std::nth_element(entries.begin(), entries.begin() + size, entries.end());

  VectorSf sparser(sparse.size());
  sparser.reserve(size);

  for (int e = 0; e < size; ++e) {
    const SparseVectorEntry & entry = entries[e];
    sparser.insert(entry.index) = entry.value;
  }

  sparser.finalize();
  entries.clear();

  std::swap(sparse, sparser);

  return sparser.sum() - sparse.sum();
}

float sparsify_size (MatrixSf & sparse, int size)
{
  ASSERT_LT(0, size);
  ASSERT_LT(size, sparse.nonZeros());

  LOG("sparsifying " << sparse.rows() << " x " << sparse.cols()
      << " matrix from " << sparse.nonZeros() << " to " << size << " entries");

  static std::vector<SparseMatrixEntry> entries;

  for (int i = 0; i < sparse.outerSize(); ++i) {
    for (MatrixSf::InnerIterator iter(sparse,i); iter; ++iter) {
      entries.push_back(
          SparseMatrixEntry(iter.value(), iter.row(), iter.col()));
    }
  }

  std::nth_element(entries.begin(), entries.begin() + size, entries.end());

  MatrixSf sparser(sparse.rows(), sparse.cols());
  sparser.reserve(size);

  for (int e = 0; e < size; ++e) {
    const SparseMatrixEntry & entry = entries[e];
    sparser.insert(entry.row, entry.col) = entry.value;
  }

  sparser.finalize();
  entries.clear();

  std::swap(sparse, sparser);

  float old_sum = sparser.sum();
  float new_sum = sparse.sum();
  float loss = (old_sum - new_sum) / old_sum;
  float density = float(size) / sparser.nonZeros();
  LOG("sparsifying to density " << density
      << " loses " << (loss*100) << "% of mass");

  return old_sum - new_sum;
}

float sparsify_absolute (
    const VectorXf & dense,
    VectorSf & sparse,
    float thresh)
{
  ASSERT_LE(0, thresh);

  const int I = dense.size();

  sparse.resize(I);

  float loss = 0;

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

    const float dense_i = dense(i);
    const float abs_i = fabsf(dense_i);

    if (abs_i > thresh) sparse.insert(i) = dense_i;
    else loss += abs_i;
  }

  sparse.finalize();

  return loss;
}

float sparsify_absolute (
    const VectorSf & sparse,
    VectorSf & sparser,
    float thresh)
{
  ASSERT_LE(0, thresh);

  sparser.resize(sparse.size());

  float loss = 0;

  for (VectorSf::InnerIterator iter(sparse); iter; ++iter) {

    const float value_i = iter.value();
    const float abs_i = fabsf(value_i);

    if (abs_i > thresh) sparser.insert(iter.index()) = value_i;
    else loss += abs_i;
  }

  sparser.finalize();

  return loss;
}

float sparsify_absolute (
    const VectorXf & dense,
    VectorSf & sparse,
    const Vector<float> & thresh)
{
  ASSERT1_LE(0, min(thresh));

  const int I = dense.size();

  sparse.resize(I);

  float loss = 0;

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

    const float dense_i = dense(i);
    const float abs_i = fabsf(dense_i);

    if (abs_i > thresh[i]) sparse.insert(i) = dense_i;
    else loss += abs_i;
  }

  sparse.finalize();

  return loss;
}

void sparsify_absolute (const MatrixXf & dense, MatrixSf & sparse, float thresh)
{
  ASSERT_LE(0, thresh);

  LOG("sparsifying " << dense.rows() << " x " << dense.cols()
      << " matrix to threshold " << thresh);

  const int I = dense.rows();
  const int J = dense.cols();

  sparse.resize(I,J);

  double sum_dense = 0;
  double sum_sparse = 0;

  for (int j = 0; j < J; ++j) {
    for (int i = 0; i < I; ++i) {

      const float dense_ij = dense(i,j);
      const float abs_ij = fabsf(dense_ij);
      sum_dense += abs_ij;

      if (abs_ij > thresh) {

        sparse.insert(i,j) = dense_ij;
        sum_sparse += abs_ij;
      }
    }
  }

  sparse.finalize();

  float density = sparse.nonZeros() / float(I * J);
  float loss = (sum_dense - sum_sparse) / sum_dense;
  LOG("sparsifying to density " << density
      << " loses " << (100 * loss) << "% of mass");
}

void sparsify_soft_relative_to_row_col_max (
    const MatrixXf & dense,
    MatrixSf & sparse,
    float relthresh,
    bool ignore_diagonal)
{
  ASSERT_LT(0, relthresh);

  LOG("sparsifying " << dense.rows() << " x " << dense.cols()
      << " positive matrix to relative threshold " << relthresh);

  VectorXf row_max;
  VectorXf col_max;

  if (ignore_diagonal) {

    VectorXf diag = dense.diagonal();
    const_cast<MatrixXf &>(dense).diagonal().setZero();

    row_max = dense.rowwise().maxCoeff();
    col_max = dense.colwise().maxCoeff();

    const_cast<MatrixXf &>(dense).diagonal() = diag;

  } else {

    row_max = dense.rowwise().maxCoeff();
    col_max = dense.colwise().maxCoeff();

  }

  const int I = dense.rows();
  const int J = dense.cols();

  sparse.resize(I,J);

  double sum_dense = 0;
  double sum_sparse = 0;

  for (int j = 0; j < J; ++j) {
    for (int i = 0; i < I; ++i) {

      const float dense_ij = dense(i,j);
      sum_dense += dense_ij;

      const float thresh = relthresh * min(row_max(i), col_max(j));
      if (dense_ij > thresh) {

        sparse.insert(i,j) = dense_ij;
        sum_sparse += dense_ij;
      }
    }
  }

  sparse.finalize();

  float density = sparse.nonZeros() / float(I * J);
  float loss = (sum_dense - sum_sparse) / sum_dense;
  LOG("sparsifying to density " << density
      << " loses " << (100 * loss) << "% of mass");
}

float max_entries_heuristic (MatrixXf & dense)
{
  int N = dense.rows() + dense.cols();
  return N * log2f(N);
}

void sparsify_hard_relative_to_row_col_max (
    const MatrixXf & dense,
    MatrixSf & sparse,
    float relthresh,
    int max_entries,
    bool ignore_diagonal)
{
  do {
    sparsify_soft_relative_to_row_col_max(
        dense,
        sparse,
        relthresh,
        ignore_diagonal);
    relthresh *= 1.5f;
  } while (sparse.nonZeros() > max_entries);
}

//----( joint probabilities )-------------------------------------------------

double likelihood_entropy (const VectorXf & likes)
{
  double sum_l = 0;
  double sum_l_log_l = 0;

  for (size_t i = 0, I = likes.size(); i < I; ++i) {

    float li = likes[i];
    if (li > 0) {

      sum_l += li;
      sum_l_log_l += li * log(li);
    }
  }

  return log(sum_l) - sum_l_log_l / sum_l;
}

double likelihood_entropy (const VectorSf & likes)
{
  double sum_l = 0;
  double sum_l_log_l = 0;

  for (VectorSf::InnerIterator iter(likes); iter; ++iter) {

    float li = iter.value();
    if (li > 0) {

      sum_l += li;
      sum_l_log_l += li * log(li);
    }
  }

  return log(sum_l) - sum_l_log_l / sum_l;
}

double likelihood_entropy_rate (const MatrixSf & joint_likes)
{
  double sum_L = 0;
  double sum_L_H = 0;

  for (int i = 0; i < joint_likes.outerSize(); ++i) {

    double sum_l = 0;
    double sum_l_log_l = 0;

    for (MatrixSf::InnerIterator iter(joint_likes,i); iter; ++iter) {

      float li = iter.value();
      if (li > 0) {

        sum_l += li;
        sum_l_log_l += li * log(li);
      }
    }

    sum_L += sum_l;
    sum_L_H += sum_l * log(sum_l) - sum_l_log_l;
  }

  return sum_L_H / sum_L;
}

double likelihood_mutual_info (const MatrixSf & joint_likes)
{
  double sum_lij = 0;
  double sum_lij_log_lij = 0;

  double sum_li = 0;
  double sum_li_log_li = 0;

  Vector<double> likes_j(joint_likes.innerSize());
  likes_j.set(0);

  for (int i = 0; i < joint_likes.outerSize(); ++i) {

    double li = 0;

    for (MatrixSf::InnerIterator iter(joint_likes,i); iter; ++iter) {

      float lij = iter.value();
      if (lij > 0) {

        sum_lij += lij;
        sum_lij_log_lij += lij * log(lij);

        li += lij;
        likes_j[iter.index()] += lij;
      }
    }

    if (li > 0) {
      sum_li += li;
      sum_li_log_li += li * log(li);
    }
  }

  double sum_lj = 0;
  double sum_lj_log_lj = 0;

  for (size_t j = 0, J = likes_j.size; j < J; ++j) {

    double lj = likes_j[j];
    if (lj > 0) {

      sum_lj += lj;
      sum_lj_log_lj += lj * log(lj);
    }
  }

  double hij = log(sum_lij) - sum_lij_log_lij / sum_lij;
  double hi = log(sum_li) - sum_li_log_li / sum_li;
  double hj = log(sum_lj) - sum_lj_log_lj / sum_lj;

  return hi + hj - hij;
}

void constrain_marginals_bp (
    MatrixXf & joint,
    const VectorXf & prior_dom,
    const VectorXf & prior_cod,
    VectorXf & temp_dom,
    VectorXf & temp_cod,
    float tol,
    size_t max_steps,
    bool logging)
{
  // Enforce simultaineous constraints on a joint PMF
  //
  //   /\x. sum y. J(y,x) = p(x)
  //   /\y. sum x. J(y,x) = q(y)

  ASSERT_EQ(prior_dom.size(), joint.cols());
  ASSERT_EQ(prior_cod.size(), joint.rows());
  ASSERT_LT(0, prior_dom.minCoeff());
  ASSERT_LT(0, prior_cod.minCoeff());

  if (logging) LOG("  constraining marginals via full BP");

  const size_t X = joint.cols();
  const size_t Y = joint.rows();

  const Vector<float> p = as_vector(prior_dom);
  const Vector<float> q = as_vector(prior_cod);

  Vector<float> J = as_vector(joint);
  Vector<float> sum_y_J = as_vector(temp_dom);
  Vector<float> sum_x_J = as_vector(temp_cod);

  float stepsize = 0;
  size_t steps = 0;
  while (steps < max_steps) {
    ++steps;
    if (logging) cout << "   step " << steps << "/" << max_steps << flush;

    stepsize = 0;

    // constrain sum y. J(y,x) = 1 first,
    // in case joint is initalized with conditional

    for (size_t x = 0; x < X; ++x) {
      Vector<float> J_x = J.block(Y, x);
      sum_y_J[x] = sum(J_x);
    }
    ASSERT_LT(0, min(sum_y_J)); // XXX error here
    imax(stepsize, sqrtf(max_dist_squared(sum_y_J, p)));
    idiv_store_rhs(p, sum_y_J);
    for (size_t x = 0; x < X; ++x) {
      Vector<float> J_x = J.block(Y, x);
      J_x *= sum_y_J[x];
    }

    sum_x_J.zero();
    for (size_t x = 0; x < X; ++x) {
      Vector<float> J_x = J.block(Y, x);
      sum_x_J += J_x;
    }
    ASSERT_LT(0, min(sum_x_J));
    imax(stepsize, sqrtf(max_dist_squared(sum_x_J, q)));
    idiv_store_rhs(q, sum_x_J);
    for (size_t x = 0; x < X; ++x) {
      Vector<float> J_x = J.block(Y, x);
      J_x *= sum_x_J;
    }

    if (logging) LOG(", stepsize = " << stepsize);
    if (stepsize < tol) break;
  }
}

//----( distances )-----------------------------------------------------------

float squared_distance (const Point & x, const Point & y)
{
  ASSERT_EQ(x.size, y.size);

  typedef int Accum;

  const uint8_t * restrict x_data = x.data;
  const uint8_t * restrict y_data = y.data;

  Accum result = 0;
  for (size_t i = 0, I = x.size; i < I; ++i) {
    result += sqr(Accum(x_data[i]) - Accum(y_data[i]));
  }

  return result;
}

} // namespace Cloud


#include "LITD_VirtualCar.h"
#include "LITD_VirtualPoint.h"
#include "math_utilities.h"
#include <Eigen/Dense>

#include <stdio.h>
#include <unistd.h>
#include <iostream>

LITD_VirtualCar::LITD_VirtualCar()
{
    carPosition.x = 0.0;
    carPosition.y = -0.2;
    carPosition.h = 0.0;
    carSpeed = 0.0;

    stanleyGain = 1.5;
    vKp = 0.1;
    vKi =0.5;

    vpOld.x = carPosition.x;
    vpOld.y = carPosition.y;
    vIntegrate = 0.0;
}

double LITD_VirtualCar::getActSpeed(LITD_VirtualPoint vp, double dtime){
    double distance = (carPosition.getVector2d() - vpOld.getVector2d()).norm();
    std::cout << "Distance : " << distance << std::endl;
    vpOld = carPosition;
    double actSpeed = distance / dtime;
    return actSpeed;
}

void LITD_VirtualCar::speedRegulator(LITD_VirtualPoint vp, double dtime){
    double actSpeed = getActSpeed(vp, dtime);
    double diff = vp.speed - actSpeed;
    vIntegrate += diff;

    carSpeed = vKp * diff + vKi * dtime * vIntegrate;

    std::cout << "-----------------------" << std::endl;
    std::cout << "Vp speed : " << vp.speed << std::endl;
    std::cout << "Act Speed : " << actSpeed << std::endl;
    std::cout << "Speed_diff : " << diff << std::endl;
    std::cout << "Integrate val: " << vIntegrate << std::endl;
    std::cout << "carspeed: " << carSpeed  << std::endl;
    std::cout << "-----------------------" << std::endl;
}

//Params: position of otpimum point, delta time step
//returns the new position of the car after dtime
LITD_VirtualPoint LITD_VirtualCar::updateStep(LITD_VirtualPoint vp, double dtime)
{

    double rad2degree = 180.0 / M_PI;
    //vector between car and virtualpoint
    Vector2d diff = vp.getVector2d() - carPosition.getVector2d();

    //calc sign to steer in direction of road
    int sign = 1;
    double diff_heading_abs = wrapTo2Pi(atan2(diff(1), diff(0)));
    if(wrapTo2Pi(diff_heading_abs - wrapTo2Pi(vp.h))> 4.712 ){
        sign = -1;
    }
    //calc new direction

    //calc normal distance of tangent to car (e)
    e = (vp.getVector2d() - carPosition.getVector2d()).norm() * sign;

    //calc angle between car heading and point tangent
    theta_c =  wrapTo2Pi(vp.h) - wrapTo2Pi(carPosition.h);

    //calc steering-angle with stanley-approach
    carSteeringAngle = theta_c + atan2(stanleyGain*e, carSpeed);

    //Debug Messages
    std::cout << "-----------------------" << std::endl;
    std::cout << "point heading : " << vp.h << "(" << rad2degree * vp.h << "°)" << std::endl;
    std::cout << "car heading: " << carPosition.h << "(" << rad2degree * carPosition.h << "°)" << std::endl;
    std::cout << "diff heading: " << diff_heading_abs << "(" << rad2degree * diff_heading_abs << "°)" << std::endl;
    std::cout << "e: " << e << std::endl;
    std::cout << "Theta_C: " << theta_c << "(" << rad2degree * theta_c << "°)" << std::endl;

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

        std::cout << "h: " << carPosition.h << std::endl;
        carPosition.h += tan(carSteeringAngle)/CAR_AXIS_DIST * carSpeed * dtime/SIM_STEPS;
        carPosition.x += cos(carPosition.h) * carSpeed * dtime/SIM_STEPS;
        carPosition.y += sin(carPosition.h) * carSpeed * dtime/SIM_STEPS;
    }

    std::cout << "new car pos x: " << carPosition.x << std::endl;
    std::cout << "new car pos y: " << carPosition.y << std::endl;
    std::cout << "-----------------------" << std::endl;

 speedRegulator(vp, dtime);
    //calc next point of front axis
    LITD_VirtualPoint newFrontAxisPoint;
    //newFrontAxisPoint.x = carPosition.x+ cos(theta_c) * carSpeed  * dtime;
    //newFrontAxisPoint.y = carPosition.y + sin(theta_c) * carSpeed * dtime;
    //newFrontAxisPoint.h = tan(carSteeringAngle) / (CAR_AXIS_DIST);

    //carPosition = newFrontAxisPoint;

}