#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
#include "UKF/Types.h"
#include "UKF/Integrator.h"
#include "UKF/StateVector.h"
#include "UKF/MeasurementVector.h"
#include "UKF/Core.h"
#include "filter.h"

static double fov = 90;
static int width = 672;
static int height = 672;

enum StateFields {
    CameraPosition,
    CameraQuat,
    CameraVelocity,
    CameraAngVelocity,
    CameraAcceleration,
    CameraAngAcceleration,
    ActorPosition,
    ActorYaw,
    ActorVelocity,
    ActorYawVelocity,
    ActorAcceleration,
    ActorYawAcceleration
};

using TrajectoryStateVector = UKF::StateVector<
    UKF::Field<CameraPosition, UKF::Vector<3>>,
    UKF::Field<CameraQuat, UKF::Vector<4>>,
    UKF::Field<CameraVelocity, UKF::Vector<3>>,
    UKF::Field<CameraAngVelocity, UKF::Vector<3>>,
    UKF::Field<CameraAcceleration, UKF::Vector<3>>,
    UKF::Field<CameraAngAcceleration, UKF::Vector<3>>,
    UKF::Field<ActorPosition, UKF::Vector<3>>,
    UKF::Field<ActorYaw, real_t>,
    UKF::Field<ActorVelocity, UKF::Vector<3>>,
    UKF::Field<ActorYawVelocity, real_t>,
    UKF::Field<ActorAcceleration, UKF::Vector<3>>,
    UKF::Field<ActorYawAcceleration, real_t>
>;

namespace UKF {
    template <> template<>
    TrajectoryStateVector TrajectoryStateVector::derivative<>() const {
        TrajectoryStateVector temp;

        /* Position derivative */
        temp.set_field<CameraPosition>(get_field<CameraVelocity>());
        UKF::Vector<3> ang_vel = get_field<CameraAngVelocity>();
        float cr = std::cos(ang_vel[0] * 0.5);
        float sr = std::sin(ang_vel[0] * 0.5);
        float cp = std::cos(ang_vel[1] * 0.5);
        float sp = std::sin(ang_vel[1] * 0.5);
        float cy = std::cos(ang_vel[2] * 0.5);
        float sy = std::sin(ang_vel[2] * 0.5);
        temp.set_field<CameraQuat>(
            UKF::Vector<4>(cr * cp * cy + sr * sp * sy, //w
                            sr * cp * cy - cr * sp * sy, //x
                            cr * sp * cy + sr * cp * sy, //y
                            cr * cp * sy - sr * sp * cy  //z
                            ));

        /* Velocity derivative */
        temp.set_field<CameraVelocity>(get_field<CameraAcceleration>());
        temp.set_field<CameraAngVelocity>(get_field<CameraAngAcceleration>());

        /* Acceleration derivative */
        temp.set_field<CameraAcceleration>(Vector<3>(0,0,0));
        temp.set_field<CameraAngAcceleration>(0);

        /* Position derivative */
        temp.set_field<ActorPosition>(get_field<ActorVelocity>());
        temp.set_field<ActorYaw>(get_field<ActorYawVelocity>());

        /* Velocity derivative */
        temp.set_field<ActorVelocity>(get_field<ActorAcceleration>());
        temp.set_field<ActorYawVelocity>(get_field<ActorYawAcceleration>());

        /* Acceleration derivative */
        temp.set_field<ActorAcceleration>(Vector<3>(0,0,0));
        temp.set_field<ActorYawAcceleration>(0);

        return temp;
    }
}

enum MeasurementFields {
    BoundingBox,
    Depth,
    DronePosition,
    DroneYaw,
    DronePitch,
    HDE
};

using MeasurementVector = UKF::DynamicMeasurementVector<
    UKF::Field<BoundingBox, UKF::Vector<2>>,
    UKF::Field<Depth, real_t>,
    UKF::Field<DronePosition, UKF::Vector<3>>,
    UKF::Field<DroneYaw, real_t>,
    UKF::Field<DronePitch, real_t>,
    UKF::Field<HDE, real_t>
>;

using MotionForecastingCore = UKF::Core<
    TrajectoryStateVector,
    MeasurementVector,
    UKF::IntegratorRK4
>;

UKF::Vector<4> flatten(UKF::Vector<4> quat) {
    double length = sqrt(quat[0] * quat[0] + quat[3] * quat[3]);
    return UKF::Vector<4>(quat[0] / length, 0, 0, quat[3] / length);
}

float getYaw(UKF::Vector<4> q) {
    q = flatten(q);
    double siny_cosp = 2 * (q[0] * q[3] + q[1] * q[2]);
    double cosy_cosp = 1 - 2 * (q[2] * q[2] + q[3] * q[3]);
    return std::atan2(siny_cosp, cosy_cosp);
}

float getPitch(UKF::Vector<4> q) {
    UKF::Vector<4> f = flatten(q);

    float dot_product = f[0] * q[0] + f[1] * q[1] + f[2] * q[2] + f[3] * q[3];
    float angle = acos(2*dot_product*dot_product - 1);
    return angle;
}

namespace UKF {
    template <> template <>
    UKF::Vector<2> MeasurementVector::expected_measurement
    <TrajectoryStateVector, BoundingBox>(
            const TrajectoryStateVector& state) {
        double fx = width/fov;
        double fy = height/fov;
        int cu = width/2;
        int cv = height/2;

        UKF::Vector<3> diff = state.get_field<ActorPosition>() - state.get_field<CameraPosition>();

        float camera_yaw = getYaw(state.get_field<CameraQuat>());
        float camera_pitch = getPitch(state.get_field<CameraQuat>());

        float actor_yaw = std::atan2(diff[0], diff[1]);
        float actor_pitch = std::atan2(diff[2], std::sqrt(diff[1]*diff[1] + diff[0]*diff[0]));

        int u = fx*(actor_yaw - camera_yaw) + cu;
        int v = fy*(actor_pitch - camera_pitch) + cv;

        return UKF::Vector<2>(u, v);
    }
    template <> template <>
    real_t MeasurementVector::expected_measurement
    <TrajectoryStateVector, Depth>(
            const TrajectoryStateVector& state) {
        // Calculate vector r_q/c between drone and actor, and get the magnitude of that
        UKF::Vector<3> diff = state.get_field<CameraPosition>() - state.get_field<ActorPosition>();
        return diff.norm();
    }
    template <> template <>
    UKF::Vector<3> MeasurementVector::expected_measurement
    <TrajectoryStateVector, DronePosition>(
            const TrajectoryStateVector& state) {
        return state.get_field<CameraPosition>();
    }
    template <> template <>
    real_t MeasurementVector::expected_measurement
    <TrajectoryStateVector, DroneYaw>(
            const TrajectoryStateVector& state) {
        UKF::Vector<4> q = state.get_field<CameraQuat>();
        return getYaw(q);
    }
    template <> template <>
    real_t MeasurementVector::expected_measurement
    <TrajectoryStateVector, DronePitch>(
            const TrajectoryStateVector& state) {
        UKF::Vector<4> q = state.get_field<CameraQuat>();
        return getPitch(q);
    }
    template <> template <>
    real_t MeasurementVector::expected_measurement
    <TrajectoryStateVector, HDE>(
            const TrajectoryStateVector& state) {
        float yaw_actor = state.get_field<ActorYaw>();
        float yaw_camera = getYaw(state.get_field<CameraQuat>());
        return yaw_actor - yaw_camera;
    }
}

static MotionForecastingCore filter;
static MeasurementVector meas;

void ukf_init(float x, float y, float z, float yaw) {
    filter.state.set_field<CameraPosition>(UKF::Vector<3>(0,0,0));
    filter.state.set_field<CameraQuat>(UKF::Vector<4>(cos(yaw/2), 0, 0, sin(yaw/2)));
    filter.state.set_field<CameraVelocity>(UKF::Vector<3>(0,0,0));
    filter.state.set_field<CameraAngVelocity>(UKF::Vector<3>(0,0,0));
    filter.state.set_field<CameraAcceleration>(UKF::Vector<3>(0,0,0));
    filter.state.set_field<CameraAngAcceleration>(UKF::Vector<3>(0,0,0));
    filter.state.set_field<ActorPosition>(UKF::Vector<3>(0,0,0));
    filter.state.set_field<ActorYaw>(0);
    filter.state.set_field<ActorVelocity>(UKF::Vector<3>(0,0,0));
    filter.state.set_field<ActorYawVelocity>(0);
    filter.state.set_field<ActorAcceleration>(UKF::Vector<3>(0,0,0));
    filter.state.set_field<ActorYawAcceleration>(0);
    filter.covariance = TrajectoryStateVector::CovarianceMatrix::Identity();

    filter.process_noise_covariance = TrajectoryStateVector::CovarianceMatrix::Identity() * 0.1;
    filter.measurement_covariance << 1e-2, 1e-2, 0.1, 0.2, 0.2, 0.2, 0.1, 0.1, 1e-2;
}

cinematography_msgs::msg::MultiDOF get_state(rclcpp::Duration duration) {
    cinematography_msgs::msg::MultiDOF point;
    point.duration = duration.seconds();
    point.x = filter.state.get_field<ActorPosition>()[0];
    point.y = filter.state.get_field<ActorPosition>()[1];
    point.z = filter.state.get_field<ActorPosition>()[2];
    point.yaw = filter.state.get_field<ActorYaw>();
    point.vx = filter.state.get_field<ActorVelocity>()[0];
    point.vy = filter.state.get_field<ActorVelocity>()[1];
    point.vz = filter.state.get_field<ActorVelocity>()[2];
    point.ax = filter.state.get_field<ActorAcceleration>()[0];
    point.ay = filter.state.get_field<ActorAcceleration>()[1];
    point.az = filter.state.get_field<ActorAcceleration>()[2];

    return point;
}

cinematography_msgs::msg::MultiDOF get_state(rclcpp::Duration duration, MotionForecastingCore filter) {
    cinematography_msgs::msg::MultiDOF point;
    point.duration = duration.seconds();
    point.x = filter.state.get_field<ActorPosition>()[0];
    point.y = filter.state.get_field<ActorPosition>()[1];
    point.z = filter.state.get_field<ActorPosition>()[2];
    point.yaw = filter.state.get_field<ActorYaw>();
    point.vx = filter.state.get_field<ActorVelocity>()[0];
    point.vy = filter.state.get_field<ActorVelocity>()[1];
    point.vz = filter.state.get_field<ActorVelocity>()[2];
    point.ax = filter.state.get_field<ActorAcceleration>()[0];
    point.ay = filter.state.get_field<ActorAcceleration>()[1];
    point.az = filter.state.get_field<ActorAcceleration>()[2];

    return point;
}

// forecast length must be at least 2 (now and next point)
std::vector<cinematography_msgs::msg::MultiDOF> ukf_iterate(rclcpp::Duration point_duration, int forecast_length) {
    if (forecast_length < 2) {
        return std::vector<cinematography_msgs::msg::MultiDOF>();
    }

    std::vector<cinematography_msgs::msg::MultiDOF> path;
    path.reserve(forecast_length);

    path[0] = get_state(point_duration);

    filter.step(point_duration.seconds(), meas);
    path[1] = get_state(point_duration);

    MotionForecastingCore forecaster = filter;
    for(int i = 2; i < forecast_length; i++) {
        forecaster.step(point_duration.seconds(), MeasurementVector());
        path[i] = get_state(point_duration, forecaster);
    }

    return path;
}

void ukf_meas_clear() {
    meas = MeasurementVector();
}

void ukf_set_fov(float f) {
    fov = f;
}

void ukf_set_bb(float width, float height) {
    meas.set_field<BoundingBox>(UKF::Vector<2>(width, height));
}

void ukf_set_depth(float depth) {
    meas.set_field<Depth>(depth);
}

void ukf_set_position(float x, float y, float z) {
    meas.set_field<DronePosition>(UKF::Vector<3>(x, y, z));
}

void ukf_set_yaw(float yaw) {
    meas.set_field<DroneYaw>(yaw);
}

void ukf_set_pitch(float pitch) {
    meas.set_field<DronePitch>(pitch);
}

void ukf_set_hde(float hde) {
    meas.set_field<HDE>(hde);
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_SINC_HPP_INCLUDED
#define NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_SINC_HPP_INCLUDED
#include <nt2/toolbox/trigonometric/functions/sinc.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/include/functions/simd/sin.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/is_inf.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <boost/simd/sdk/config.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::sinc_, boost::simd::tag::simd_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                            )
  {

    typedef typename meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      return nt2::sinc(tofloat(a0));
    }
  };
} }

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::sinc_, boost::simd::tag::simd_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      result_type r1 = nt2::if_else(nt2::lt(nt2::abs(a0), nt2::Eps<A0>()),
                                    nt2::One<A0>(),
                                    nt2::sin(a0)/a0);
      #ifdef BOOST_SIMD_NO_INFINITIES
      return r1;
      #else
      return nt2::if_else(nt2::is_inf(a0), nt2::Zero<A0>(), r1);
      #endif
    }
  };
} }

#endif


#pragma once

#include <Eigen/Dense>
#include <boost/assert.hpp>
#include <type_traits>

#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"
#include "spectral/polar_to_hermite.hpp"

namespace boltzmann {

class MQEval;  // forward declaration

namespace detail_ {
/**
 * @helper to evaluate macroscopic quantities
 *
 * @param mq
 *
 * @return
 */
struct MQEval_helper
{
 public:
  typedef Eigen::Vector3d vector_t;
  typedef Eigen::Matrix3d tensor_t;
  typedef double scalar_t;

 public:
  MQEval_helper(const MQEval &mq)
      : mq_(mq)
  { /* empty */
  }

  void operator()(const double *ptr_c, unsigned int N);

  template <typename DERIVED>
  void operator()(const Eigen::DenseBase<DERIVED> &c);

 public:
  double m;    // mass
  double e;    // energy
  vector_t v;  // velocity
  vector_t r;  // energy flow
  tensor_t P;  // pressure
  vector_t q;  // heat flow
  tensor_t M;  // momentum flow

 private:
  const MQEval &mq_;
};

}  // detail_

/**
 * @brief macroscopic quantities evaluator
 *
 */
class MQEval
{
 public:
  typedef detail_::MQEval_helper evaluator_t;

 public:
  MQEval() { /*  default constructor */}

  template <typename BASIS>
  MQEval(const BASIS &basis)
  {
    init(basis);
  }

  /**
   *  @brief evaluator to do actual compuations
   *
   *  Returns a struct storing the moments, it has a const reference to *this
   * (where the
   *  coefficients are stored).
   *
   *  Attention: MQEval_helper depends on MQEval by a reference, make sure it
   * does not run out of
   * scope as
   *  long as the MQEval_helper is in use.
   *
   */
  detail_::MQEval_helper evaluator() const { return detail_::MQEval_helper(*this); }

  /**
   *
   * @param basis  Polar-Laguerre basis
   */
  template <typename BASIS>
  void init(const BASIS &basis);

 private:
  typedef Eigen::VectorXd vec_t;
  typedef Eigen::ArrayXd array_t;
  typedef Eigen::MatrixXd mat_t;

 private:
  /// basis size
  unsigned int N_ = 0;
  const double tol_ = 1e-10;

  array_t mass_;
  array_t energy_;
  array_t ux_;
  array_t uy_;
  /// momentum flow
  array_t uxx_;
  array_t uyy_;
  array_t uxy_;
  /// energy flow
  array_t rx_;
  array_t ry_;

 public:
  //@{
  /// coefficients in Polar-Laguerre basis
  const array_t &cmass() const { return mass_; }
  const array_t &cenergy() const { return energy_; }
  const array_t &cux() const { return ux_; }
  const array_t &cuy() const { return uy_; }
  const array_t &cuxx() const { return uxx_; }
  const array_t &cuyy() const { return uyy_; }
  const array_t &cuxy() const { return uxy_; }
  const array_t &crx() const { return rx_; }
  const array_t &cry() const { return ry_; }
  //@}

  unsigned int N() const { return N_; }

 private:
  /**
   * @brief trim non zero coefficients starting from the end
   */
  template <typename DERIVED>
  void trim_coeff(Eigen::DenseBase<DERIVED> &coeffs);
};

// ---------------------------------------------------------------------------
template <typename BASIS>
void
MQEval::init(const BASIS &basis)
{
  typedef BASIS polar_basis_t;
  // compute coefficients

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

  typedef SpectralBasisFactoryHN::basis_type hermite_basis_t;
  hermite_basis_t hermite_basis;

  SpectralBasisFactoryHN::create(hermite_basis, K, 2);
  Polar2Hermite<polar_basis_t, hermite_basis_t> P2H(basis, hermite_basis);
  typedef typename hermite_basis_t::elem_t hermite_elem_t;

  typedef typename boost::mpl::at_c<typename hermite_elem_t::types_t, 0>::type hx_t;
  typedef typename boost::mpl::at_c<typename hermite_elem_t::types_t, 1>::type hy_t;

  typename hermite_elem_t::Acc::template get<hx_t> get_hx;
  typename hermite_elem_t::Acc::template get<hy_t> get_hy;

  // Hermite quadrature
  QHermiteW quad(0.5, K);
  // Hermite polynomials
  HermiteNW<double> hermw(K);
  hermw.compute(quad.pts());

  // test for mass
  vec_t herm_coeffs(N);
  // apply "quadrature":
  Eigen::Map<const array_t> x(quad.points_data(), K);
  Eigen::Map<const array_t> w(quad.weights_data(), K);

  // ------------------------------
  // MASS
  for (int i = 0; i < N; ++i) {
    int kx = get_hx(hermite_basis.get_elem(i)).get_id().k;
    int ky = get_hy(hermite_basis.get_elem(i)).get_id().k;
    auto hx = hermw.get_array(kx);
    auto hy = hermw.get_array(ky);

    herm_coeffs[i] = (hx * w).sum() * (hy * w).sum();
  }
  mass_.resize(N);
  P2H.to_hermite_T(mass_, herm_coeffs);

  // ------------------------------
  // ENERGY
  for (int i = 0; i < N; ++i) {
    int kx = get_hx(hermite_basis.get_elem(i)).get_id().k;
    int ky = get_hy(hermite_basis.get_elem(i)).get_id().k;

    auto hx = hermw.get_array(kx);
    auto hy = hermw.get_array(ky);

    herm_coeffs[i] =
        (x * x * hx * w).sum() * (hy * w).sum() + (x * x * hy * w).sum() * (hx * w).sum();
  }
  energy_.resize(N);
  P2H.to_hermite_T(energy_, herm_coeffs);

  // ------------------------------
  // MOMENTUM
  vec_t herm_coeffs2(N);
  for (int i = 0; i < N; ++i) {
    int kx = get_hx(hermite_basis.get_elem(i)).get_id().k;
    int ky = get_hy(hermite_basis.get_elem(i)).get_id().k;
    auto hx = hermw.get_array(kx);
    auto hy = hermw.get_array(ky);
    double sumx = (x * hx * w).sum() * (hy * w).sum();
    double sumy = (x * hy * w).sum() * (hx * w).sum();
    herm_coeffs[i] = sumx;
    herm_coeffs2[i] = sumy;
  }

  ux_.resize(N);
  P2H.to_hermite_T(ux_, herm_coeffs);
  uy_.resize(N);
  P2H.to_hermite_T(uy_, herm_coeffs2);

  // resize to non-zero contribution
  trim_coeff(mass_);
  trim_coeff(energy_);
  trim_coeff(ux_);
  trim_coeff(uy_);

  uxx_.resize(N);
  uxy_.resize(N);
  uyy_.resize(N);
  // ------------------------------
  // Momentum flow
  for (int i = 0; i < N; ++i) {
    int kx = get_hx(hermite_basis.get_elem(i)).get_id().k;
    int ky = get_hy(hermite_basis.get_elem(i)).get_id().k;
    auto hx = hermw.get_array(kx);
    auto hy = hermw.get_array(ky);
    const double tx = (x * hx * w).sum();
    const double ty = (x * hy * w).sum();
    uxx_[i] = (x * x * hx * w).sum() * (hy * w).sum();
    uxy_[i] = (x * hx * w).sum() * (x * hy * w).sum();
    uyy_[i] = (x * x * hy * w).sum() * (hx * w).sum();
  }
  herm_coeffs = uxx_;
  P2H.to_hermite_T(uxx_, herm_coeffs);
  herm_coeffs = uxy_;
  P2H.to_hermite_T(uxy_, herm_coeffs);
  herm_coeffs = uyy_;
  P2H.to_hermite_T(uyy_, herm_coeffs);

  // ------------------------------
  // Momentum flow
  uxx_.resize(N);
  uxy_.resize(N);
  uyy_.resize(N);
  for (int i = 0; i < N; ++i) {
    int kx = get_hx(hermite_basis.get_elem(i)).get_id().k;
    int ky = get_hy(hermite_basis.get_elem(i)).get_id().k;
    auto hx = hermw.get_array(kx);
    auto hy = hermw.get_array(ky);

    uxx_[i] = (x * x * hx * w).sum() * (hy * w).sum();
    uxy_[i] = (x * hx * w).sum() * (x * hy * w).sum();
    uyy_[i] = (x * x * hy * w).sum() * (hx * w).sum();
  }
  herm_coeffs = uxx_;
  P2H.to_hermite_T(uxx_, herm_coeffs);
  herm_coeffs = uxy_;
  P2H.to_hermite_T(uxy_, herm_coeffs);
  herm_coeffs = uyy_;
  P2H.to_hermite_T(uyy_, herm_coeffs);

  // ------------------------------
  // Energy flow
  rx_.resize(N);
  ry_.resize(N);
  for (int i = 0; i < N; ++i) {
    int kx = get_hx(hermite_basis.get_elem(i)).get_id().k;
    int ky = get_hy(hermite_basis.get_elem(i)).get_id().k;
    auto hx = hermw.get_array(kx);
    auto hy = hermw.get_array(ky);

    rx_[i] =
        (x * x * x * hx * w).sum() * (hy * w).sum() + (x * hx * w).sum() * (x * x * hy * w).sum();
    ry_[i] =
        (x * x * x * hy * w).sum() * (hx * w).sum() + (x * hy * w).sum() * (x * x * hx * w).sum();
  }
  herm_coeffs = rx_;
  P2H.to_hermite_T(rx_, herm_coeffs);
  herm_coeffs = ry_;
  P2H.to_hermite_T(ry_, herm_coeffs);

  trim_coeff(uxx_);
  trim_coeff(uxy_);
  trim_coeff(uyy_);

  trim_coeff(rx_);
  trim_coeff(ry_);
}

template <typename DERIVED>
void
MQEval::trim_coeff(Eigen::DenseBase<DERIVED> &coeffs)
{
  for (unsigned int i = N_ - 1; i >= 0; --i) {
    if (std::abs(coeffs[i]) > tol_) {
      DERIVED tmp = coeffs.segment(0, i + 1);
      coeffs.derived().resize(i + 1);
      coeffs = tmp;
      break;
    }
  }
}

namespace detail_ {
template <typename DERIVED>
void
MQEval_helper::operator()(const Eigen::DenseBase<DERIVED> &c)
{
  static_assert(DERIVED::RowsAtCompileTime == 1 || DERIVED::ColsAtCompileTime == 1,
                "Shape mismatch");

  BOOST_ASSERT(c.cols() * c.rows() == mq_.N());
  auto &cmass = mq_.cmass();
  auto &cux = mq_.cux();
  auto &cuy = mq_.cuy();
  auto &ce = mq_.cenergy();
  auto &cuxx = mq_.cuxx();
  auto &cuxy = mq_.cuxy();
  auto &cuyy = mq_.cuyy();
  auto &crx = mq_.crx();
  auto &cry = mq_.cry();

  auto ca = c.derived().array();

  double rho = (ca.segment(0, cmass.size()) * cmass).sum();
  m = rho;
  double rho_vx = (ca.segment(0, cux.size()) * cux).sum();
  double rho_vy = (ca.segment(0, cuy.size()) * cuy).sum();
  // total energy
  double rho_e = (ca.segment(0, ce.size()) * ce).sum();
  e = rho_e / rho;
  // energy density per unit volume
  double w = 0.5 * rho_e;
  double mxx = (ca.segment(0, cuxx.size()) * cuxx).sum();
  double mxy = (ca.segment(0, cuxy.size()) * cuxy).sum();
  double myy = (ca.segment(0, cuyy.size()) * cuyy).sum();
  M << mxx, mxy, 0, mxy, myy, 0, 0, 0, 0;
  v(0) = rho_vx / rho;
  v(1) = rho_vy / rho;
  v(2) = 0;
  double pxx = mxx - rho * v(0) * v(0);
  double pxy = mxy - rho * v(1) * v(0);
  double pyy = myy - rho * v(1) * v(1);
  P << pxx, pxy, 0, pxy, pyy, 0, 0, 0, 0;
  // energy flow
  r(0) = (ca.segment(0, crx.size()) * crx).sum();
  r(1) = (ca.segment(0, cry.size()) * cry).sum();
  r(2) = 0;
  double v2 = v.squaredNorm();
  // heat flow
  q(0) = 0.5 * (r(0) - 2 * (v * M.row(0)).sum() + rho * v2 + 2 * v(0) * w + v2 * v(0) * rho);
  q(1) = 0.5 * (r(1) - 2 * (v * M.row(1)).sum() + rho * v2 + 2 * v(1) * w + v2 * v(1) * rho);
  q(2) = 0;
}

inline void
MQEval_helper::operator()(const double *ptr_c, unsigned int N)
{
  assert(N == mq_.N());
  Eigen::Map<const Eigen::ArrayXd> c(ptr_c, N);
  this->operator()(c);
}

}  // detail_

}  // end namespace boltzmann


/*
 *  fisher.cpp
 *
 *  Created by Ania M. Kedzierska on 11/11/11.
 *  Copyright 2011 Politecnic University of Catalonia, Center for Genomic Regulation.  This is program can be redistributed, modified or else as given by the terms of the GNU General Public License.
 *
 */

#include <cstdlib>
#include <cmath>

#include "fisher.h"
#include "matrix.h"
#include "state.h"
#include "state_list.h"
#include "alignment.h"

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <stdexcept>

namespace ublas = boost::numeric::ublas;

//Please refer to the Empar paper for the description of the procedure.

//  The free parameters (without stochastic condition) are encoded as a linear index.
// There will be np*nedges + rdf+1, where
// np: free parameters of the transition matrices
// rdf: df of the root.
// We put the parameters in a matric, first the transition parameters, then the root parameters at the end.
// Root parameters are present only for SSM and GMM.

// Converts an edge-row-column to a linear index identifying the parameter for the given model.
long erc2param(Model &Mod, long e, long r, long c) {
  long p = Mod.matrix_structure(r, c);
  return e*Mod.np + p;
}

// Converts the root distribution index into a parameter index for the model.
long root2param(Tree &T, Model &Mod, long r) {
  return T.nedges*Mod.np + Mod.root_structure(r);
}

// STEP 2
// We then encode the free parameters taking into account the stochastic condition.
// i.e. we remove the diagonal entry and first parameter of the root.

// Converts an edge-row-column to a linear index identifying the free parameter.
long erc2freeparam(Tree &T, Model &Mod, long e, long r, long c) {
  long p = Mod.matrix_structure(r, c);
  long a,b;

  // find first appearence of parameter p.
  for (a=0; a < T.nalpha; a++) {
    for(b=0; b < T.nalpha; b++) {
      if (Mod.matrix_structure(a,b) == p) {
        if (b > a) return e*Mod.df + p - a - 1;
        else if (b < a) return e*Mod.df + p - a;
        else return -1;

      }
    }
  }

  return -1;
}

// Converts the root distribution index into a free parameter index.
long root2freeparam(Tree &T, Model &Mod, long r) {
  long p = Mod.root_structure(r);

  if (p == 0) return -1;   // p = 0 is the parameter killed.
  else return T.nedges*Mod.df + p - 1;
}

// Computes the state at the endpoints of an edge.
// T: tree
// e: edge
// sth: state on hidden nodes
// stl: state on leaves

// Output:
// a: the state of the source.
// b: the state of the target.

void edgestate(Tree &T, long e, State &sth, State &stl, long &a, long &b) {
  a = sth.s[T.edges[e].s - T.nleaves];      // state on the source node
  if (T.edges[e].t < T.nleaves){            // if the edge connects to a leaf
    b = stl.s[T.edges[e].t];                // state on the target leaf
  } else {
    b = sth.s[T.edges[e].t - T.nleaves];    // state on the target node
  }
}

void edgestate(Tree &T, long e, std::vector<int> &sth, std::vector<int> &stl, long &a, long &b) {
  a = sth[T.edges[e].s - T.nleaves];      // state on the source node
  if (T.edges[e].t < T.nleaves){          // if the edge connects to a leaf
    b = stl[T.edges[e].t];                // state on the target leaf
  } else {
    b = sth[T.edges[e].t - T.nleaves];    // state on the target node
  }
}

// Returns the root state
long rootstate(State &sth) {
  return sth.s[0];
}

long rootstate(std::vector<int> &sth) {
  return sth[0];
}

// Let k be a parameter index (running over all free parameters on tm + root)
// Every value of k describes a set of states compatible with it (see text).
//   * if k corresponds to a transition matrix on edge e, parameter l, the compatible
//     states are the ones with a state (a,b) on edge e, such that the free parameter
//     on the entry (a,b) in the transition matrix is l.
//   * if k corresponds to a root parameter, then the compatible states are the ones
//     with root state matching that parameter.

// The following functions fill three arrays of probabilities.

// pleaf is a vector of joint probabilities on the leaves
void fill_pleaf(Tree &T, StateList &sl, Parameters &Par, Array1 &pleaf) {
  long i, j, l, u, v;
  double p;

  // Initializations
  pleaf.resize(T.nstleaves);
  for (i=0; i < T.nstleaves; i++) {
    pleaf[i] = 0;
  }

  // Loop over all states
  for (i=0; i < T.nstleaves; i++) {
    for(j=0; j < T.nsthidden; j++) {

      // Compute leaf probabilities
      p = Par.r[rootstate(sl.h[j])];
      for(l=0; l < T.nedges; l++) {
        edgestate(T, l, sl.h[j], sl.l[i], u, v);
        p = p*Par.tm[l][u][v];
      }
      pleaf[i] = pleaf[i] + p;
    }
  }
}

// pcond1 is a matrix of probabilities p(I and k), where "I and k" refers to states with
// leaf state I and compatible with k.
// rows in pcond1 are indexed by leaf states. Columns are indexed by a linear index encoding
// a free parameter.

void fill_pcond1(Tree &T, Model &Mod, StateList &sl, Parameters &Par, Array2 &pcond1) {

  long i,j,l,u,v;
  long k,e,a,b;
  double p;
  long npars = T.nedges*Mod.np + Mod.rdf + 1;

  // Initializations
  pcond1.resize(T.nstleaves);
  for (i=0; i < T.nstleaves; i++) {
    pcond1[i].resize(npars);
    for(k=0; k < npars; k++) {
      pcond1[i][k] = 0;
    }
  }

  // Loop over all states
  for (i=0; i < T.nstleaves; i++) {
    for(j=0; j < T.nsthidden; j++) {

      // Loop over edges
      for(e=0; e < T.nedges; e++) {

        // Compute cond1 probabilities
        p = Par.r[rootstate(sl.h[j])];
        for(l=0; l < T.nedges; l++) {
          if (l == e) continue;
          edgestate(T, l, sl.h[j], sl.l[i], u, v);
          p = p*Par.tm[l][u][v];
        }

        edgestate(T, e, sl.h[j], sl.l[i], a, b);
        k = erc2param(Mod, e, a, b);
        pcond1[i][k] = pcond1[i][k] + p;
      }

      // Take care of root parameters in pcond1
      p = 1;
      for(l=0; l < T.nedges; l++) {
        edgestate(T, l, sl.h[j], sl.l[i], u, v);
        p = p*Par.tm[l][u][v];
      }

      k = root2param(T, Mod, rootstate(sl.h[j]));
      pcond1[i][k] = pcond1[i][k] + p;
    }
  }
}

// pcond2 is a 3-dimensional array of probabilities p(I and k and kp) where "I and k and kp" refers
// to states with leaf state I, and compatible with k and kp.

void fill_pcond2(Tree &T, Model &Mod, StateList &sl, Parameters &Par, Array3 &pcond2) {
  long i, j, l, u, v;
  long k, e, a, b;
  long kp, ep, ap, bp;
  double p;

  long npars = T.nedges*Mod.np + Mod.rdf + 1;

  // Initializations
  pcond2.resize(T.nstleaves);
  for (i=0; i < T.nstleaves; i++) {
    pcond2[i].resize(npars);
    for(k=0; k < npars; k++) {
      pcond2[i][k].resize(npars);
      for(l=0; l < npars; l++) {
        pcond2[i][k][l] = 0;
      }
    }
  }

  // Loop over all states
  for (i=0; i < T.nstleaves; i++) {
    for(j=0; j < T.nsthidden; j++) {

      // Loop over pairs of edges for the edge-edge part of pcond2
      for(e=0; e < T.nedges; e++) {
        for(ep=0; ep < T.nedges; ep++) {

          // when e==ep, the corresponding entry must be zero.
          if (e==ep) continue;

          // Compute cond2 probabilities
          p = Par.r[rootstate(sl.h[j])];
          for(l=0; l < T.nedges; l++) {
            if (l == e || l == ep) continue;
            edgestate(T, l, sl.h[j], sl.l[i], u, v);
            p = p*Par.tm[l][u][v];
          }

          edgestate(T, e, sl.h[j], sl.l[i], a, b);
          k = erc2param(Mod, e, a, b);

          edgestate(T, ep, sl.h[j], sl.l[i], ap, bp);
          kp = erc2param(Mod, ep, ap, bp);

          pcond2[i][k][kp] = pcond2[i][k][kp] + p;
        }
      }

      // Loop a single edge for the edge-root part of pcond2
      for(e=0; e < T.nedges; e++) {
        // Take care of root parameters in pcond1
        p = 1;
        for(l=0; l < T.nedges; l++) {
          if (l == e) continue;
          edgestate(T, l, sl.h[j], sl.l[i], u, v);
          p = p*Par.tm[l][u][v];
        }

        edgestate(T, e, sl.h[j], sl.l[i], a, b);
        k = erc2param(Mod, e, a, b);

        kp = root2param(T, Mod, rootstate(sl.h[j]));

        pcond2[i][k][kp] = pcond2[i][k][kp] + p;
        pcond2[i][kp][k] = pcond2[i][kp][k] + p;
      }

      // Recall: In pcond2 with two root parameters k and kp,
      // we always have p(I and k and kp) = 0.
    }
  }
}

// Puts the parameter values in a vector indexed as in the Fisher matrix for the model.
// (4 parameters per edge on K81).
void get_param_vector(Tree &T, Model &Mod, Parameters &Par, std::vector<double> &param) {
  long e, a, b, k;
  long npar = T.nedges * Mod.np + Mod.rdf + 1;
  param.resize(npar);

  for(e=0; e < T.nedges; e++) {
    for(a=0; a < T.nalpha; a++) {
      for(b=0; b < T.nalpha; b++) {
        k = erc2param(Mod, e, a, b);
        param[k] = Par.tm[e][a][b];
      }
    }
  }

  for(a=0; a < T.nalpha; a++) {
    k = root2param(T, Mod, a);
    param[k] = Par.r[a];
  }
}

// Gets the free parameters for a branch
void get_branch_free_param_vector(Tree &T, Model &Mod, Parameters &Par, long br, std::vector<double> &param) {
  long a, b, k;
  param.resize(Mod.df);

  for(a=0; a < T.nalpha; a++) {
    for(b=0; b < T.nalpha; b++) {
      k = erc2freeparam(T, Mod, 0, a, b);
      if(k < 0) continue;
      param[k] = Par.tm[br][a][b];
    }
  }
}

// Puts the parameter values in a vector indexed as in the Fisher matrix for the free parameters.
// (3 parameters per edge on K81).
void get_free_param_vector(Tree &T, Model &Mod, Parameters &Par, std::vector<double> &param) {
  long e, a, b, k;
  long npar = T.nedges * Mod.df + Mod.rdf;
  param.resize(npar);

  for(e=0; e < T.nedges; e++) {
    for(a=0; a < T.nalpha; a++) {
      for(b=0; b < T.nalpha; b++) {
        k = erc2freeparam(T, Mod, e, a, b);
        if(k < 0) continue;
        param[k] = Par.tm[e][a][b];
      }
    }
  }

  for(a=0; a < T.nalpha; a++) {
    k = root2freeparam(T, Mod, a);
    if(k < 0) continue;
    param[k] = Par.r[a];
  }
}

// Computes the observed Fisher information of all Parameters in Par, on the model with
// hidden nodes and stores them in the matrix Imod ( no approximation, so the matrix is not diagonal).
// The rows and columns of Imod are indexed by a number between 0 and np*nedges + rdf + 1, encoding the free parameters
// of the model. The stochastic condition is not taken into account.

// If data is not NULL, computes observed Fisher instead of expected Fisher.

// The formula for the Fisher info of the model parameters (without stochastic constraints) is:
// I(k, k') = sum_{leaf states I} N p(I and k)*p(I and k') / (p_k*p_k'*p(I))
//                            - p(I and k and k') / (p_k*p_k')   <----- This term only when k!=k'
// Here p_k denotes the value of the parameter k.

// The observed Fisher info, is given by the same formula, but inside the sum must add the factor
// x_I / N*p(I).

void Fisher_information_model(Tree &T, Model &Mod, Parameters &Par, long N, Counts *data, Array2 &Imod) {
  long i, j, k;

  double factor;

  std::vector<double> param;

  Array1 pleaf;
  Array2 pcond1;
  Array3 pcond2;

  StateList sl;

  long npars = Mod.np*T.nedges + Mod.rdf + 1;

  Array2 Ineg;
  Ineg.resize(npars);
  Imod.resize(npars);
  for (i=0; i < npars; i++) {
    Ineg[i].resize(npars);
    Imod[i].resize(npars);
    for(j=0; j < npars; j++) {
      Ineg[i][j] = 0;
      Imod[i][j] = 0;
    }
  }

  if (data != NULL && data->nspecies != T.nleaves) {
    throw std::length_error("ERROR: In Fisher_information_model. Counts don't match the tree.");

  }

  sl = create_state_list(T);

  // Fills stuff
  fill_pleaf(T, sl, Par, pleaf);
  fill_pcond1(T, Mod, sl, Par, pcond1);
  fill_pcond2(T, Mod, sl, Par, pcond2);

  get_param_vector(T, Mod, Par, param);

  // Fills entries of Imod one by one.
  for (i=0; i < npars; i++) {
    for(j=0; j < npars; j++) {
      for(k=0; k < T.nstleaves; k++) {
        if (data == NULL) {    // Expected Fisher
          factor = (double) N;
        } else {               // Observed Fisher
          factor = data->c[k] / pleaf[k];
        }

        Imod[i][j] = Imod[i][j] +
          factor * pcond1[k][i] * pcond1[k][j] / pleaf[k];

        Ineg[i][j] = Ineg[i][j] + factor * pcond2[k][i][j];
      }

      // We add up the negatives apart to minimize numerical errors.
      Imod[i][j] = Imod[i][j] - Ineg[i][j];

    }
  }
}

// Takes The Fisher info matrix corresponding to a model Imod, and outputs a Fisher info matrix
// for the free parameters. This means, taking into account the stochastic condition. What we do
// is replace the diagonal parameter by "1 - sum of other parameters"
// Imod has np*nedges + rdf + 1 rows and columns
// Ifree has df*nedges + rdf rows and columns.

// Here np means number of parameters, while df means degrees of freedom. For
// example, K81 has np = 4 and df = 3. SSM has np = 8 and df = 6. The fact that SSM has two
// stochastic constraints instead of one, means that SSM must be treated separately.

void Fisher_information_free(Tree &T, Model &Mod, Array2 &Imod, Array2 &Ifree) {
  long i, j, e, p, l;

  long l1, l2;

  long nfreepar = Mod.df*T.nedges + Mod.rdf;
  long nmodpar = Mod.np*T.nedges + Mod.rdf + 1;

  Ifree.resize(nfreepar);
  for(i=0; i < nfreepar; i++) {
    Ifree[i].resize(nfreepar);
    for(j=0; j < nfreepar; j++) {
      Ifree[i][j] = 0;
    }
  }

  // Counts the number of times a parameter appears in the the transition matrices and root dist.
  std::vector<double> coeff;
  coeff.resize(nmodpar);
  for(i=0; i < nmodpar; i++) {
    coeff[i] = 0;
  }

  for(e=0; e < T.nedges; e++) {
    for(i=0; i < T.nalpha; i++) {
      for(j=0; j < T.nalpha; j++) {
        p = erc2param(Mod, e, i, j);
        coeff[p] = coeff[p] + 1;  // adds one to the corresponding parameter index.
      }
    }
  }
  for (i=0; i < T.nalpha; i++) {
    p = root2param(T, Mod, i);
    coeff[p] = coeff[p] + 1;
  }

  // translation between free parameters and model parameters.
  std::vector<long> modpar;     // corresponding model parameter index
  std::vector<long> modparkill; // corresponding model parameter which is killed.

  modpar.resize(nfreepar);
  modparkill.resize(nfreepar);

  // Transition matrices
  for(e=0; e < T.nedges; e++) {
    for(i=0; i < T.nalpha; i++) {
      for(j=0; j < T.nalpha; j++) {
        l = erc2freeparam(T, Mod, e, i, j);
        if (l < 0) continue;   // negative l means the parameter is killed.
        modpar[l] = erc2param(Mod, e, i, j);
        modparkill[l] = erc2param(Mod, e, i, i);
      }
    }
  }

  // Root
  if (Mod.rdf > 0) {
    for(i=0; i < T.nalpha; i++) {
      l = root2freeparam(T, Mod, i);
      if (l < 0) continue;   // negative l means the parameter is killed.
      modpar[l] = root2param(T, Mod, i);
      modparkill[l] = root2param(T, Mod, 0);
    }
  }

  double c1, c2;
  for(l1=0; l1 < nfreepar ; l1++) {
    c1 = coeff[modpar[l1]]/coeff[modparkill[l1]];
    for(l2=0; l2 < nfreepar; l2++) {
      c2 = coeff[modpar[l2]]/coeff[modparkill[l2]];

      Ifree[l1][l2] = Imod[modpar[l1]][modpar[l2]]
           - c1 * Imod[modparkill[l1]][modpar[l2]]
           - c2 * Imod[modpar[l1]][modparkill[l2]]
           + c1*c2 * Imod[modparkill[l1]][modparkill[l2]];
    }
  }
}

// Computes the fisher information for the model with hidden nodes, and stores the result in I.
// I is a matrix with df*nedges + rdf rows and cols.
// the index for the rows and columns encodes a free parameter.
// 0: first free param on edge 0
// 1: second free param on edge 0
// ...
// df: first free param on edge 1
// ...

void Fisher_information(Tree &T, Model &Mod, Parameters &Par, long N, Array2 &I) {
  Array2 Imod;

  Fisher_information_model(T, Mod, Par, N, NULL, Imod);
  Fisher_information_free(T, Mod, Imod, I);
}

void Observed_Fisher_information(Tree &T, Model &Mod, Parameters &Par, Counts &data, Array2 &I) {
  Array2 Imod;

  Fisher_information_model(T, Mod, Par, data.N, &data, Imod);
  Fisher_information_free(T, Mod, Imod, I);
}

/* Matrix inversion routine.
    Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
template<class T>
bool matrix_inverse (const ublas::matrix<T>& input, ublas::matrix<T>& inverse) {
  using namespace boost::numeric::ublas;
  typedef permutation_matrix<std::size_t> pmatrix;
  // create a working copy of the input
  matrix<T> A(input);
  // create a permutation matrix for the LU-factorization
  pmatrix pm(A.size1());
  // perform LU-factorization
  int res = lu_factorize(A,pm);
  if( res != 0 ) return false;
  // create identity matrix of "inverse"
  inverse.assign(ublas::identity_matrix<T>(A.size1()));
  // backsubstitute to get the inverse
  lu_substitute(A, pm, inverse);
  return true;
}

// Computes the full covariance matrix for the MLE.
void full_MLE_covariance_matrix(Tree &T, Model &Mod, Parameters &Par, long N, Array2 &Cov) {
  long i, j;

  long npars = Mod.df*T.nedges + Mod.rdf;
  ublas::matrix<double> CC(npars, npars);
  ublas::matrix<double> II(npars, npars);
  Array2 I;

  Fisher_information(T, Mod, Par, N, I);

  // Need to convert from Matrix to ublas::matrix<double>
  for(i=0; i < npars; i++) {
    for(j=0; j < npars; j++) {
      II(i, j) = I[i][j];
    }
  }

  bool res = matrix_inverse(II, CC);
  if (!res) {
    throw std::out_of_range( "Could not invert the Fisher information matrix." );
  }

  Cov.resize(npars);
  for(i=0; i < npars; i++) {
    Cov[i].resize(npars);
    for(j=0; j < npars; j++) {
      Cov[i][j] = CC(i,j);
    }
  }
}

// Computes the full covariance matrix for the MLE, using observed Fisher info.
void full_MLE_observed_covariance_matrix(Tree &T, Model &Mod, Parameters &Par, Counts &data, Array2 &Cov) {
  long i, j;

  long npars = Mod.df*T.nedges + Mod.rdf;
  ublas::matrix<double> CC(npars, npars);
  ublas::matrix<double> II(npars, npars);
  Array2 I;

  Observed_Fisher_information(T, Mod, Par, data, I);

  // Need to convert from Matrix to ublas::matrix<double>
  for(i=0; i < npars; i++) {
    for(j=0; j < npars; j++) {
      II(i, j) = I[i][j];
    }
  }

  bool res = matrix_inverse(II, CC);
  if (!res) {
    throw std::out_of_range("Could not invert the Fisher information matrix.");
  }

  Cov.resize(npars);
  for(i=0; i < npars; i++) {
    Cov[i].resize(npars);
    for(j=0; j < npars; j++) {
      Cov[i][j] = CC(i,j);
    }
  }
}

// Extracts the covariance matrix for a given branch from the full covariance matrix.
void branch_covariance_matrix(Model &Mod, Array2 &Covfull, long b, Array2 &Covbr) {
  long i,j;

  Covbr.resize(Mod.df);
  for(i=0; i < Mod.df; i++) {
    Covbr[i].resize(Mod.df);
    for(j=0; j< Mod.df; j++) {
      Covbr[i][j] = Covfull[Mod.df*b + i][Mod.df*b + j];
    }
  }
}

// Extracts the inverted covariance matrix for a given branch from the full covariance matrix.
void branch_inverted_covariance_matrix(Model &Mod, Array2 &Covfull, long b, Array2 &Covbri) {
  long i,j;

  ublas::matrix<double> CC(Mod.df, Mod.df);
  ublas::matrix<double> II(Mod.df, Mod.df);

  for(i=0; i < Mod.df; i++) {
    for(j=0; j< Mod.df; j++) {
      CC(i,j) = Covfull[Mod.df*b + i][Mod.df*b + j];
    }
  }

  bool res = matrix_inverse(CC, II);
  if (!res) {
    throw std::out_of_range( "Could not invert the Covariance matrix." );
  }

  Covbri.resize(Mod.df);
  for(i=0; i < Mod.df; i++) {
    Covbri[i].resize(Mod.df);
    for(j=0; j< Mod.df; j++) {
      Covbri[i][j] = II(i,j);
    }
  }
}


/*--------------------------------------------------------------------------

 File Name:         filter.cpp
 Date Created:      2017/08/07
 Date Modified:     2017/09/08

 Author:            Eric Cristofalo
 Contact:           eric.cristofalo@gmail.com

 Description:       ROS node for estimating realtime pose and odometry via EKF from Optitrack and Ouijibot IMU/wheel encoders

 -------------------------------------------------------------------------*/

#include <iostream>
#include <stdio.h>
#include <math.h>
#include <random>

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
// #include <geometry_msgs/Accel.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

#include <ouijabot/Wheel_Spd.h>

#include <Eigen/Dense>
#include <Eigen/Core>
#include <unsupported/Eigen/MatrixFunctions> // matrix exponential

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

class filterClass {

  ros::NodeHandle nh_;

  ros::Subscriber process_imu_sub_;
  ros::Subscriber process_wheel_speed_sub_;

  ros::Subscriber ground_truth_sub_;
  // ros::Subscriber ground_truth_accel_sub_;

  ros::Publisher state_estimate_pub_;
  tf::TransformBroadcaster state_estimate_broadcaster;
  nav_msgs::Odometry stateEstimateMsg;

  // Initialalize Input Variables
  int displayData, useOptitrack;
  double r_w, r_b;
  VectorXd q_cov_diag, r_cov_diag;

  // Initialize Filter Variables
  int initCount;
  double count, initTime;
  bool initPoseBool, predictBool, updateBool, gyroBool, wheelSpdBool;
  ros::Time timeCur, timePrev;
  VectorXd state, state_, state_gt, y, gyro, accel, wheel_spd;
  MatrixXd Sigma, Sigma_, Q, R;
  MatrixXd R_rl;

public:
  filterClass(int in_01, int in_02, double in_03, double in_04, vector<double> in_05, vector<double> in_06, vector<double> in_07) {

    // Subscribe to Ouijibot IMU
    process_imu_sub_ = nh_.subscribe("/ouijabot/imu", 10, &filterClass::imuCallback, this);

    // Subscribe to Ouijibot Wheel Encoders
    process_wheel_speed_sub_ = nh_.subscribe("/ouijabot/wheel_spd", 10, &filterClass::wheelSpeedCallback, this);

    // Publish Odometry Message of State Estimate
    state_estimate_pub_ = nh_.advertise<nav_msgs::Odometry>("/robot/state_estimate", 10);

    // Initialize Variables
    displayData = in_01;
    useOptitrack = in_02;
    r_w = in_03;
    r_b = in_04;
    q_cov_diag = VectorXd::Zero(9);
    for ( int i=0; i<9; i++ ) {
      q_cov_diag(i) = in_05[i];
    }
    r_cov_diag = VectorXd::Zero(7);
    for ( int i=0; i<7; i++ ) {
      r_cov_diag(i) = in_06[i];
    }
    MatrixXd R_temp = MatrixXd::Zero(3,3);
    for ( int i=0; i<3; i++ ) {
      for ( int j=0; j<3; j++) {
        R_temp(i,j) = in_07[i*3+j];
      }
    }
    R_rl = MatrixXd(2,2);
    R_rl = R_temp.block(0,0,2,2);

    // Subscribe to Optitrack Ground Truth Topics
    // Required mocap_interface_odom node to be publishing "ground truth" odometry data (position, derived velocity, and derived acceleration)
    initPoseBool = false;
    if ( useOptitrack ) {
      initPoseBool = true; // Initialize the filter with current Optitrack measurement
      ground_truth_sub_ = nh_.subscribe("/robot/pose", 10, &filterClass::odomCallback, this);
      // ground_truth_accel_sub_ = nh_.subscribe("/robot/accel", 10, &filterClass::odomAccelCallback, this);
    }

    // Initialize Filter Variables
    timeCur = ros::Time::now();
    timePrev = ros::Time::now();
    initCount = 0;
    initTime = 0.0;
    predictBool = false;
    updateBool = false;
    gyroBool = false;
    wheelSpdBool = false;

    // Initialize State Variables
    count = 0;
    state = VectorXd::Zero(9); // update: [x,y,theta,v_x,v_y,omega,a_x,a_y,a_omega]^T
    state_ = VectorXd::Zero(9); // prediction: [x,y,theta,v_x,v_y,omega,a_x,a_y,a_omega]^T
    state_gt = VectorXd::Zero(9); // ground truth: [x,y,theta,v_x,v_y,omega,a_x,a_y,a_omega]^T
    y = VectorXd::Zero(7); // measurement: [w1, w2, w3, w4, w_gyro, a_x, a_y]^T
    gyro = VectorXd::Zero(3); // gyro measurement
    accel = VectorXd::Zero(3); // accelerometer measurement
    wheel_spd = VectorXd::Zero(4); // wheel speed measurement
    Q = q_cov_diag.asDiagonal(); // dynamics covariance matrix
    R = r_cov_diag.asDiagonal(); // measurement covariance matrix
    Sigma = 1E6*MatrixXd::Identity(9,9); // update: estimation error covariance matrix
    Sigma_ = 1E6*MatrixXd::Identity(9,9); // update: estimation error covariance matrix

    // double phiCur, theCur, psiCur;
    // psiCur = M_PI/2.0;
    // theCur = 0.0;
    // phiCur = M_PI;
    // MatrixXd R_phi = MatrixXd::Zero(3,3);
    // MatrixXd R_the = MatrixXd::Zero(3,3);
    // MatrixXd R_psi = MatrixXd::Zero(3,3);
    // R_phi <<  1.0    ,   0.0         ,   0.0         ,
    //           0.0    ,   cos(phiCur) ,   -sin(phiCur),
    //           0.0    ,   sin(phiCur) ,   cos(phiCur) ;
    // R_the <<  cos(theCur)    ,   0.0 ,   sin(theCur) ,
    //           0.0            ,   1.0 ,   0.0         ,
    //           -sin(theCur)   ,   0.0 ,   cos(theCur) ;
    // R_psi <<  cos(psiCur)    ,   -sin(psiCur),   0.0,
    //           sin(psiCur)    ,   cos(psiCur) ,   0.0,
    //           0.0            ,   0.0         ,   1.0;
    // // Quad Orientation
    // MatrixXd R_test = MatrixXd::Zero(3,3);
    // R_test = R_psi*R_the*R_phi;
    // cout << "ROTATION TEST:" << endl << R_test;

  }

  void runFilter()
  {
    // Initialize Detection Timer
    //std::clock_t start = std::clock();

    // Time Interval
    timeCur = ros::Time::now();
    if ( count==0 ) {
      timePrev = ros::Time::now();
    }
    double dt = (timeCur - timePrev).toSec();

    // Prediction Step
    // if ( predictBool ) {
    // if ( predictBool && updateBool ) {
    if ( updateBool ) {

      // Define Property Matrices For Dynamics Matrices
      MatrixXd Rwr = MatrixXd::Zero(2,2); // Active rotation watrix from world frame to local robot frame
      Rwr <<  cos(state(2)), -sin(state(2)),
              sin(state(2)),  cos(state(2));
      MatrixXd Omega = MatrixXd::Zero(2,2);
      Omega << 0.0, -state(5), state(5), 0.0;

      // Construct Nonlinear Continuous-Time Dynamics Matrix
      MatrixXd F_nl = MatrixXd::Zero(9,9);
      F_nl.block(0,3,2,2) = Rwr.transpose();
      F_nl(2,5) = 1.0;
      F_nl.block(3,3,2,2) = Omega.transpose()*dt;
      F_nl.block(3,6,3,3) = MatrixXd::Identity(3,3);
      F_nl.block(6,6,2,2) = Omega.transpose()*dt;
      // Discretize Dynamics Matrix
      F_nl = F_nl.exp();
      F_nl.block(0,3,3,3) = F_nl.block(0,3,3,3)*dt;
      F_nl.block(0,6,3,3) = F_nl.block(0,6,3,3)*dt*dt;
      F_nl.block(3,6,3,3) = F_nl.block(3,6,3,3)*dt;

      // State Prediction
      state_ = F_nl*state;

      // Construct Linearized Continuous-Time Dynamics Matrix
      MatrixXd F = MatrixXd::Zero(9,9);
      F(0,2) = ( -sin(state(2))*state(3) - cos(state(2))*state(4) )*dt;
      F(1,2) = (  cos(state(2))*state(3) - cos(state(2))*state(4) )*dt;
      F.block(0,3,2,2) = Rwr.transpose();
      F_nl(2,5) = 1.0;
      F.block(3,3,2,2) = Omega.transpose()*dt;
      F(3,5) = state(4)*dt;
      F(4,5) = -state(3)*dt;
      F.block(3,6,3,3) = MatrixXd::Identity(3,3);
      F(6,5) = state(7)*dt;
      F(7,5) = -state(6)*dt;
      F.block(6,6,2,2) = Omega.transpose()*dt;
      // Discretize Dynamics Matrix
      F = F.exp();
      F.block(0,3,3,3) = F.block(0,3,3,3)*dt;
      F.block(0,6,3,3) = F.block(0,6,3,3)*dt*dt;
      F.block(3,6,3,3) = F.block(3,6,3,3)*dt;

      // EKF Covariance Prediction in Discrete Time
      // MatrixXd Q_tilde = F*Q*F.transpose()*dt;
      MatrixXd Q_tilde = Q;
      Sigma_ = F*Sigma*F.transpose() + Q_tilde;

      // End EKF Prediction
      // predictBool = false; // TEMPORARY COMMENTED OUT
      timePrev = timeCur;

      if ( displayData ) {
        cout << "Prediction" << endl;
        cout << "Time Interval: " << endl << dt << endl;
        // cout << "Q: " << endl << Q << endl;
        // cout << "Q_tilde: " << endl << Q_tilde << endl;
      }

    }
    else {
      state_ = state;
      Sigma_ = Sigma;
    }

    // Update Step
    if ( updateBool ) {
    // if ( predictBool && updateBool ) {

      // Linear Measurement Matrix
      MatrixXd H = MatrixXd::Zero(7,9);
      double s = sqrt(2.0);
      H(0,3) = -s/(2.0*r_w);  H(0,4) = -s/(2.0*r_w);  H(0,5) = r_b/r_w;
      H(1,3) = -s/(2.0*r_w);  H(1,4) =  s/(2.0*r_w);  H(1,5) = r_b/r_w;
      H(2,3) =  s/(2.0*r_w);  H(2,4) =  s/(2.0*r_w);  H(2,5) = r_b/r_w;
      H(3,3) =  s/(2.0*r_w);  H(3,4) = -s/(2.0*r_w);  H(3,5) = r_b/r_w;
      H.block(4,5,3,3) = MatrixXd::Identity(3,3);

      // Kalman Gain in Discrete Time
      // MatrixXd R_tilde = R/dt;
      MatrixXd R_tilde = R;
      MatrixXd S = (H*Sigma_*H.transpose() + R_tilde);
      MatrixXd KalmanK = Sigma_*H.transpose()*S.inverse();

      // EKF State Update
      state = state_ + KalmanK*(y - H*state_);

      // EKF Covariance Update
      Sigma = Sigma_ - KalmanK*H*Sigma_;
      // cout << "Sigma: " << endl << Sigma(0,0) << endl << Sigma(3,3) << endl << "-----------" << endl;

      // End EKF Update
      updateBool = false;
      // predictBool = false; // TEMPORARY HERE

      if ( displayData ) {
        cout << "Update" << endl;
        // cout << "R: " << endl << R << endl;
        // cout << "R_tilde: " << endl << R_tilde << endl;
      }

    }
    else {
      state = state_;
      Sigma = Sigma_;
    }

    // Convert to Global Reference Frame
    // Somewhat guessing on the transformations for the moment (see negative sign in gyro as well)
    VectorXd state_output = VectorXd::Zero(9);
    state_output.segment(0,2) = -R_rl*state.segment(0,2);
    state_output(2) = state(2);
    state_output.segment(3,2) = -R_rl*state.segment(3,2);
    state_output(5) = state(5);
    state_output.segment(6,2) = -R_rl*state.segment(6,2);
    state_output(8) = state(8);

    // Publish State Estimate Message
    ros::Time pubTime = ros::Time::now();
    // Publish Odometry Transform
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(state_output(2));
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = pubTime;
    odom_trans.header.frame_id = "/odom_frame_est";
    odom_trans.child_frame_id = "/base_link_est";
    odom_trans.transform.translation.x = state_output(0);
    odom_trans.transform.translation.y = state_output(1);
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;
    state_estimate_broadcaster.sendTransform(odom_trans);
    // ros::Duration(0.5).sleep(); // sleep for half a second
    // Set Final Pose
    stateEstimateMsg.header.stamp = pubTime;
    stateEstimateMsg.header.frame_id = "/odom_frame_est";
    stateEstimateMsg.pose.pose.position.x = state_output(0);
    stateEstimateMsg.pose.pose.position.y = state_output(1);
    stateEstimateMsg.pose.pose.position.z = 0.0;
    stateEstimateMsg.pose.pose.orientation = odom_quat;
    // Set Final Pose Covariance
    stateEstimateMsg.pose.covariance[0] = Sigma(0,0);
    stateEstimateMsg.pose.covariance[1] = Sigma(1,1);
    stateEstimateMsg.pose.covariance[2] = Sigma(2,2);
    // Set Final Velocity
    stateEstimateMsg.child_frame_id = "/base_link_est";
    stateEstimateMsg.twist.twist.linear.x = state_output(3);
    stateEstimateMsg.twist.twist.linear.y = state_output(4);
    stateEstimateMsg.twist.twist.linear.z = 0.0;
    stateEstimateMsg.twist.twist.angular.x = 0.0;
    stateEstimateMsg.twist.twist.angular.y = 0.0;
    stateEstimateMsg.twist.twist.angular.z = state_output(5);
        // Set Final Velocity Covariance
    stateEstimateMsg.twist.covariance[0] = Sigma(3,3);
    stateEstimateMsg.twist.covariance[1] = Sigma(4,4);
    stateEstimateMsg.twist.covariance[2] = Sigma(5,5);

    // Publish Odometry Message
    state_estimate_pub_.publish(stateEstimateMsg);

    // Display Data in Terminal
    if (displayData) {
      cout << "Prediction: " << endl << state_ << endl;
      // cout << "Covariance: " << endl << Sigma_(0,0) << endl << Sigma_(1,1) << endl << Sigma_(2,2) << endl << Sigma_(3,3) << endl << Sigma_(4,4) << endl << Sigma_(5,5) << endl;
      cout << "Update: " << endl << state << endl;
      // cout << "Covariance: " << endl << Sigma(0,0) << endl << Sigma(1,1) << endl << Sigma(2,2) << endl << Sigma(3,3) << endl << Sigma(4,4) << endl << Sigma(5,5) << endl;
      // cout << "------------------------------" << endl;
      // cout << "Current Ground Truth: " << endl << state_gt << endl;
      // cout << "Current Estimate: " << endl << state << endl;
      cout << "Current Measurement: " << endl << y << endl;
      cout << "------------------------------" << endl;
    }

    // Update Filter Count
    count++;
  }

  void readMeasurement()
  {
    if ( gyroBool && wheelSpdBool && !initPoseBool ) {
      // Fill Measurement Vector
      y.head(4) = wheel_spd;
      y(4) = gyro(2);
      y.tail(2) = accel.head(2);
      // Reset Measurements
      gyroBool = false;
      wheelSpdBool = false;
      // Run Filter Update
      updateBool = true;
      runFilter();
    }
  }

  void imuCallback(const sensor_msgs::Imu& msg)
  {
    // Extract Data
    // double qx, qy, qz, qw;
    // qx = msg.orientation.x;
    // qy = msg.orientation.y;
    // qz = msg.orientation.z;
    // qw = msg.orientation.w;
    // Accelerometer is positioned with a rotation of pi about the z-axis!
    accel(0) = -msg.linear_acceleration.x;
    accel(1) = -msg.linear_acceleration.y;
    accel(2) = msg.linear_acceleration.z;
    // Filter Accelerometer Noise
    if ( abs(accel(0)) < 0.25 ) accel(0) = 0;
    if ( abs(accel(1)) < 0.25 ) accel(1) = 0;
    if ( abs(accel(2)) < 0.25 ) accel(2) = 0;
    // Convert to gyro measurement to radians/second
    gyro(0) = msg.angular_velocity.x*M_PI/180.0;
    gyro(1) = msg.angular_velocity.y*M_PI/180.0;
    gyro(2) = -msg.angular_velocity.z*M_PI/180.0; // need negative sign here with negatives in final output
    // Process EKF Measurement
    gyroBool = true;
    readMeasurement();
  }

  void wheelSpeedCallback(const ouijabot::Wheel_Spd& msg)
  {
    // Extract Data
    // Convert from linear velocity to angular velocity (radians/second) with wheel radius
    wheel_spd(0) = msg.wheel_spd[0]/r_w;
    wheel_spd(1) = msg.wheel_spd[1]/r_w;
    wheel_spd(2) = msg.wheel_spd[2]/r_w;
    wheel_spd(3) = msg.wheel_spd[3]/r_w;
    // Process EKF Measurement
    wheelSpdBool = true;
    readMeasurement();
  }

  void odomCallback(const geometry_msgs::PoseStamped& msg)
  {
    if ( useOptitrack ) {
      // Extract Position
      VectorXd state_temp = VectorXd::Zero(9);
      state_temp(0) = msg.pose.position.x;  // x
      state_temp(1) = msg.pose.position.y;  // x
      double qx = msg.pose.orientation.x;
      double qy = msg.pose.orientation.y;
      double qz = msg.pose.orientation.z;
      double qw = msg.pose.orientation.w;
      double phiCur, theCur, psiCur;
      tf::Quaternion q(qx, qy, qz, qw);
      tf::Matrix3x3 m(q);
      m.getRPY(phiCur, theCur, psiCur);
      state_temp(2) = psiCur;               // theta
      // // Extract Velocity
      // state_temp(3) = msg.pose.orinetation.linear.x;   // v_x
      // state_temp(4) = msg.pose.orinetation.linear.y;   // v_y
      // state_temp(5) = msg.pose.orinetation.angular.z;  // omega
      // Compute Initial Pose
      if ( initPoseBool ) {
        if ( initCount==0) {
          timePrev = ros::Time::now();
        }
        timeCur = ros::Time::now();
        double dt = (timeCur-timePrev).toSec();
        initTime = initTime + dt;
        if ( initTime<10.0 ) {
          initCount++;
          state = state + state_temp;
        }
        else {
          // Average Poses
          state = state/double(initCount);
          // Convert to Local Robot Frame
          // Somewhat guessing on the transformations for the moment (see negative sign in gyro as well)
          VectorXd state_output = VectorXd::Zero(9,1);
          state.segment(0,2) = -R_rl.transpose()*state.segment(0,2);
          state(2) = state(2);
          // Finish Up
          timePrev = ros::Time::now();
          initPoseBool = false;
          predictBool = true;
          cout << "ouijabot_filter: initial mean pose after " << double(initCount) << " iterations and " << initTime << " seconds: " << endl << state << endl;
        }
      }
      else {
        state_gt.head(6) = state_temp.head(6);
      }
    }
  }

  // void odomAccelCallback(const geometry_msgs::Accel& msg) {
  //   if ( useOptitrack ) {
  //     state_gt(6) = msg.linear.x;
  //     state_gt(7) = msg.linear.y;
  //     state_gt(8) = msg.angular.z;
  //   }
  // }

};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "filter");
  ros::NodeHandle nh("~");

  int display_data_flag;
  nh.param<int>("display_data_flag", display_data_flag, 0);
  int use_optitrack_flag;
  nh.param<int>("use_optitrack_flag", use_optitrack_flag, 0);

  double wheel_radius;
  nh.getParam( "wheel_radius", wheel_radius );
  if (!nh.hasParam("wheel_radius")) {
    wheel_radius = 0.025;
  }
  double body_radius;
  nh.getParam( "body_radius", body_radius );
  if (!nh.hasParam("body_radius")) {
    body_radius = 0.1;
  }

  std::vector<double> q_cov_diag;
  nh.getParam( "q_cov_diag", q_cov_diag );
  if (!nh.hasParam("q_cov_diag")) {
    q_cov_diag = {0.0001, 0.0001, 0.0001, 0.001, 0.001, 0.001, 0.01, 0.01, 0.01};
  }
  std::vector<double> r_cov_diag;
  nh.getParam( "r_cov_diag", r_cov_diag );
  if (!nh.hasParam("r_cov_diag")) {
    r_cov_diag = {0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001};
  }

  std::vector<double> rotation_robot_local;
  nh.getParam( "rotation_robot_local", rotation_robot_local );
  if (!nh.hasParam("rotation_robot_local")) {
    rotation_robot_local = {0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0};
  }

  filterClass filter(
    display_data_flag,
    use_optitrack_flag,
    wheel_radius,
    body_radius,
    q_cov_diag,
    r_cov_diag,
    rotation_robot_local
  );

  ros::Rate r(20); // 20 hz
  while (ros::ok()) {
    ros::spinOnce();
    r.sleep();
  }
  ros::shutdown();

  return 0;
}


#pragma once

#include <polyfem/Common.hpp>
#include <polyfem/NLProblem.hpp>
#include <polyfem/MatrixUtils.hpp>
#include <polyfem/State.hpp>

#include <polyfem/Logger.hpp>

#include <cppoptlib/problem.h>
#include <cppoptlib/solver/isolver.h>
#include <cppoptlib/linesearch/armijo.h>
#include <cppoptlib/linesearch/morethuente.h>
#include <Eigen/LU>
#include <iostream>

namespace cppoptlib
{

	template <typename ProblemType>
	class LbfgsSolverL2 : public ISolver<ProblemType, 1>
	{
	public:
		using Superclass = ISolver<ProblemType, 1>;
		using typename Superclass::Scalar;
		using typename Superclass::THessian;
		using typename Superclass::TVector;
		using MatrixType = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;

		enum class LineSearch
		{
			Armijo,
			MoreThuente,
		};

		LineSearch line_search = LineSearch::Armijo;

		void setLineSearch(const std::string &name)
		{
			if (name == "armijo")
			{
				line_search = LineSearch::Armijo;
			}
			else if (name == "more_thuente")
			{
				line_search = LineSearch::MoreThuente;
			}
			else
			{
				throw std::invalid_argument("[SparseNewtonDescentSolver] Unknown line search.");
			}
			polyfem::logger().debug("\tline search {}", name);
		}

		void minimize(ProblemType &objFunc, TVector &x0)
		{
			const size_t m = 10;
			const size_t DIM = x0.rows();
			MatrixType sVector = MatrixType::Zero(DIM, m);
			MatrixType yVector = MatrixType::Zero(DIM, m);
			Eigen::Matrix<Scalar, Eigen::Dynamic, 1> alpha = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>::Zero(m);
			TVector grad(DIM), q(DIM), grad_old(DIM), s(DIM), y(DIM);
			objFunc.gradient(x0, grad);
			TVector x_old = x0;
			TVector x_old2 = x0;

			size_t iter = 0, globIter = 0;
			Scalar H0k = 1;
			this->m_current.reset();
			do
			{
				const Scalar relativeEpsilon = static_cast<Scalar>(0.0001) * std::max(static_cast<Scalar>(1.0), x0.norm());

				if (grad.norm() < relativeEpsilon)
					break;

				//Algorithm 7.4 (L-BFGS two-loop recursion)
				q = grad;
				const int k = std::min(m, iter);

				// for i = k − 1, k − 2, . . . , k − m§
				for (int i = k - 1; i >= 0; i--)
				{
					// alpha_i <- rho_i*s_i^T*q
					const double rho = 1.0 / static_cast<TVector>(sVector.col(i))
												 .dot(static_cast<TVector>(yVector.col(i)));
					alpha(i) = rho * static_cast<TVector>(sVector.col(i)).dot(q);
					// q <- q - alpha_i*y_i
					q = q - alpha(i) * yVector.col(i);
				}
				// r <- H_k^0*q
				q = H0k * q;
				//for i k − m, k − m + 1, . . . , k − 1
				for (int i = 0; i < k; i++)
				{
					// beta <- rho_i * y_i^T * r
					const Scalar rho = 1.0 / static_cast<TVector>(sVector.col(i))
												 .dot(static_cast<TVector>(yVector.col(i)));
					const Scalar beta = rho * static_cast<TVector>(yVector.col(i)).dot(q);
					// r <- r + s_i * ( alpha_i - beta)
					q = q + sVector.col(i) * (alpha(i) - beta);
				}
				// stop with result "H_k*f_f'=q"

				// any issues with the descent direction ?
				Scalar descent = -grad.dot(q);
				Scalar alpha_init = 1.0 / grad.norm();
				if (descent > -0.0001 * relativeEpsilon)
				{
					q = -1 * grad;
					iter = 0;
					alpha_init = 1.0;
				}

				// find steplength
				// const Scalar rate = MoreThuente<ProblemType, 1>::linesearch(x0, -q,  objFunc, alpha_init) ;
				Scalar rate;
				switch (line_search)
				{
				case LineSearch::Armijo:
					rate = Armijo<ProblemType, 1>::linesearch(x0, -q, objFunc);
					break;
				case LineSearch::MoreThuente:
					rate = MoreThuente<ProblemType, 1>::linesearch(x0, -q, objFunc);
					break;
				}
				// update guess
				x0 = x0 - rate * q;

				grad_old = grad;
				objFunc.gradient(x0, grad);

				s = x0 - x_old;
				y = grad - grad_old;

				// update the history
				if (iter < m)
				{
					sVector.col(iter) = s;
					yVector.col(iter) = y;
				}
				else
				{

					sVector.leftCols(m - 1) = sVector.rightCols(m - 1).eval();
					sVector.rightCols(1) = s;
					yVector.leftCols(m - 1) = yVector.rightCols(m - 1).eval();
					yVector.rightCols(1) = y;
				}
				// update the scaling factor
				H0k = y.dot(s) / static_cast<double>(y.dot(y));

				x_old = x0;
				polyfem::logger().debug("\titer: {}, f = {}, ‖g‖_2 = {}", globIter, objFunc.value(x0), grad.norm());

				iter++;
				globIter++;
				++this->m_current.iterations;
				this->m_current.gradNorm = grad.norm(); // template lpNorm<Eigen::Infinity>();
				this->m_status = checkConvergence(this->m_stop, this->m_current);
			} while ((objFunc.callback(this->m_current, x0)) && (this->m_status == Status::Continue));
		}
	};

} // namespace cppoptlib


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

  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_ARCH_COMMON_DETAIL_SCALAR_F_LOG_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_F_LOG_HPP_INCLUDED

#ifndef BOOST_SIMD_NO_NANS
#include <boost/simd/function/scalar/is_nan.hpp>
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#endif
#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/constant/mlog10two2nmb.hpp>
#include <boost/simd/constant/mlog2two2nmb.hpp>
#include <boost/simd/constant/mlogtwo2nmb.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/twotonmb.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#endif
#include <boost/simd/arch/common/detail/generic/f_log_kernel.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/constant/log10_2hi.hpp>
#include <boost/simd/constant/log10_2lo.hpp>
#include <boost/simd/constant/log10_ehi.hpp>
#include <boost/simd/constant/log10_elo.hpp>
#include <boost/simd/constant/log2_em1.hpp>
#include <boost/simd/constant/log_2hi.hpp>
#include <boost/simd/constant/log_2lo.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/scalar/fma.hpp>
#include <boost/simd/function/scalar/is_eqz.hpp>
#include <boost/simd/function/scalar/is_ltz.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd = boost::dispatch;
    template < class A0,
               class Style ,
               class base_A0 = bd::scalar_of_t<A0>
               >
               struct logarithm{};

    template < class A0 >
    struct logarithm< A0, tag::not_simd_type, float>
    {
      using kernel_t = kernel<A0, tag::not_simd_type, float>;

      static BOOST_FORCEINLINE A0 log(A0 const& a0) BOOST_NOEXCEPT
      {
      #ifndef BOOST_SIMD_NO_INFINITIES
        if (BOOST_UNLIKELY(a0 == Inf<A0>())) return a0;
      #endif
      #ifdef BOOST_SIMD_NO_NANS
        if (BOOST_UNLIKELY(is_ltz(a0))) return Nan<A0>();
      #else
        if (BOOST_UNLIKELY(is_nan(a0)||is_ltz(a0))) return Nan<A0>();
      #endif
        if (BOOST_UNLIKELY(is_eqz(a0))) return Minf<A0>();
        A0 z = a0;
      #ifndef BOOST_SIMD_NO_DENORMALS
        A0 t = Zero<A0>();
        if(BOOST_UNLIKELY(abs(z) < Smallestposval<A0>()))
        {
          z *= Twotonmb<A0>();
          t = Mlogtwo2nmb<A0>();
        }
      #endif
        A0 x, fe, x2, y;
        kernel_t::log(z, fe, x, x2, y);
        y = fma(fe, Log_2lo<A0>(), y);
        y = fma(Mhalf<A0>(), x2, y);
      #ifdef BOOST_SIMD_NO_DENORMALS
        return fma(Log_2hi<A0>(), fe, x+y);
      #else
        return fma(Log_2hi<A0>(), fe, x+y+t);
      #endif
      }

      static BOOST_FORCEINLINE  A0 log2(A0 const& a0) BOOST_NOEXCEPT
      {
#ifndef BOOST_SIMD_NO_INFINITIES
        if (BOOST_UNLIKELY(a0 == Inf<A0>())) return a0;
#endif
#ifdef BOOST_SIMD_NO_NANS
        if (BOOST_UNLIKELY(is_ltz(a0))) return Nan<A0>();
#else
        if (BOOST_UNLIKELY(is_nan(a0)||is_ltz(a0))) return Nan<A0>();
#endif
        if (BOOST_UNLIKELY(is_eqz(a0))) return Minf<A0>();
        A0 z = a0;
#ifndef BOOST_SIMD_NO_DENORMALS
        A0 t = Zero<A0>();
        if (BOOST_UNLIKELY(abs(z) < Smallestposval<A0>()))
        {
          z *= Twotonmb<A0>();
          t = Mlog2two2nmb<A0>();
        }
#endif
        A0 x, fe, x2, y;
        kernel_t::log(z, fe, x, x2, y);
        y = fma(Mhalf<A0>(),x2, y);
        z = fma(x,Log2_em1<A0>(),y*Log2_em1<A0>());
#ifdef BOOST_SIMD_NO_DENORMALS
        return ((z+y)+x)+fe;
#else
        return ((z+y)+x)+fe+t;
#endif
      }

      static BOOST_FORCEINLINE  A0 log10(A0 const& a0) BOOST_NOEXCEPT
      {
#ifndef BOOST_SIMD_NO_INFINITIES
        if (BOOST_UNLIKELY(a0 == Inf<A0>())) return a0;
#endif
#ifdef BOOST_SIMD_NO_NANS
        if (BOOST_UNLIKELY(is_ltz(a0))) return Nan<A0>();
#else
        if (BOOST_UNLIKELY(is_nan(a0)||is_ltz(a0))) return Nan<A0>();
#endif
        if (BOOST_UNLIKELY(is_eqz(a0))) return Minf<A0>();
        A0 z = a0;
#ifndef BOOST_SIMD_NO_DENORMALS
        A0 t = Zero<A0>();
        if (BOOST_UNLIKELY(abs(z) < Smallestposval<A0>()))
        {
          z *= Twotonmb<A0>();
          t = Mlog10two2nmb<A0>();
        }
#endif
        A0 x, fe, x2, y;
        kernel_t::log(z, fe, x, x2, y);

        y = fma(Mhalf<A0>(), x2, y);
        z = (x+y)*Log10_elo<A0>();
        z = fma( y, Log10_ehi<A0>(), z);
        z = fma(Log10_ehi<A0>(), y,  z);
        z = fma(Log10_2hi<A0>(), fe, z);
#ifdef BOOST_SIMD_NO_DENORMALS
        return fma(Log10_2lo<A0>(), fe, z);
#else
        return fma(Log10_2lo<A0>(),fe, z+t);
#endif
      }
    };
  }
} }
#endif


#pragma once
#include <boost/geometry.hpp>

#include "math.hpp"

namespace gshhg {

using Cartesian =
    boost::geometry::model::point<double, 3, boost::geometry::cs::cartesian>;
using GeodeticDegree = boost::geometry::model::point<
    double, 3, boost::geometry::cs::geographic<boost::geometry::degree>>;
using GeodeticRadian = boost::geometry::model::point<
    double, 3, boost::geometry::cs::geographic<boost::geometry::radian>>;
using Point =
    boost::geometry::model::point<double, 2, boost::geometry::cs::cartesian>;

using Box = boost::geometry::model::box<Point>;
using Polygon = boost::geometry::model::polygon<Point>;

using Spheroid = boost::geometry::srs::spheroid<double>;

using Andoyer = boost::geometry::strategy::distance::andoyer<Spheroid>;
using Haversine = boost::geometry::strategy::distance::haversine<Spheroid>;
using Thomas = boost::geometry::strategy::distance::thomas<Spheroid>;
using Vincenty = boost::geometry::strategy::distance::vincenty<Spheroid>;

GeodeticRadian cartesian_2_geodetic(const Cartesian& point);
Cartesian geodetic_2_cartesian(const GeodeticRadian& point);

inline GeodeticRadian geodetic_2_radian(const GeodeticDegree& point) {
  return GeodeticRadian(radians(point.get<0>()), radians(point.get<1>()),
                        point.get<2>());
}

inline GeodeticDegree geodetic_2_degree(const GeodeticRadian& point) {
  return GeodeticDegree(degrees(point.get<0>()), degrees(point.get<1>()),
                        point.get<2>());
}

}  // namespace gshhg


//==================================================================================================
/**
  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_ARCH_COMMON_DETAIL_SIMD_F_TRIG_EVALUATION_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_F_TRIG_EVALUATION_HPP_INCLUDED

#include <boost/simd/function/horn.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd { namespace detail
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  template <class A0> struct trig_evaluation < A0,  tag::simd_type, float>
  {
    typedef typename bd::as_integer<A0, signed>::type          iA0;
    typedef typename bd::scalar_of<A0>::type                 stype;

    static BOOST_FORCEINLINE A0 cos_eval(const A0& z)
    {
      const A0 y = bs::horn<A0
        , 0x3d2aaaa5
        , 0xbab60619
        , 0x37ccf5ce > (z);
      return bs::inc(bs::fma(z,bs::Mhalf<A0>(), y*bs::sqr(z)));
    }

    static BOOST_FORCEINLINE A0 sin_eval(const A0& z, const A0& x)
    {
      const A0 y1 =  bs::horn< A0
        , 0xbe2aaaa2
        , 0x3c08839d
        , 0xb94ca1f9 > (z);
      return bs::fma(bs::multiplies(y1,z),x,x);
    }

    static BOOST_FORCEINLINE A0 base_tancot_eval(const A0& z)
    {
      const A0 zz = bs::sqr(z);
      return fma(bs::horn<A0
                , 0x3eaaaa6f
                , 0x3e0896dd
                , 0x3d5ac5c9
                , 0x3cc821b5
                , 0x3b4c779c
                , 0x3c19c53b>(zz), zz*z, z);
    }

    static BOOST_FORCEINLINE A0 tan_eval(const A0& z,  const iA0& n)
    {
      A0 y = base_tancot_eval(z);
      return bs::if_else(bs::is_equal(n, bs::One<iA0>()),y,-bs::rec(y));
    }

    static BOOST_FORCEINLINE A0 cot_eval(const A0& z,  const iA0& n)
    {
      A0 y = base_tancot_eval(z);
      return bs::if_else(bs::is_equal(n, One<iA0>()),bs::rec(y),-y);
    }
  };
} } }

#endif


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

namespace tyco
{
namespace P3
{

template<typename T, typename Cs>
struct row_vector
{
    Eigen::Matrix<T, 1, 4> raw_;
};

template<typename T, typename Cs>
struct column_vector
{
    Eigen::Matrix<T, 4, 1> raw_;
};

template<typename T, typename CsLeft, typename CsRight>
struct homography
{
    Eigen::Matrix<T, 4, 4> raw_;
};

// M^-1
template<typename T, typename CsLeftIn, typename CsRightIn>
homography<T, CsRightIn, CsLeftIn> inverse(const homography<T, CsLeftIn, CsRightIn>& M)
{
    return {M.raw_.inverse()};
}

// M * p
template<typename T, typename CsLeftIn, typename CsRightIn>
column_vector<T, CsLeftIn> operator*(
    const homography<T, CsLeftIn, CsRightIn>& M,
    const column_vector<T, CsRightIn>& p)
{
    return {M.raw_ * p.raw_};
}

// p^T * M
template<typename T, typename CsLeftIn, typename CsRightIn>
row_vector<T, CsRightIn> operator*(
    const row_vector<T, CsLeftIn>& p,
    const homography<T, CsLeftIn, CsRightIn>& M)
{
    return {p.raw_ * M.raw_};
}

// M1 * M2
template<typename T, typename CsLeft, typename CsMiddle, typename CsRight>
const homography<T, CsLeft, CsRight> operator*(
    const homography<T, CsLeft, CsMiddle>& M1,
    const homography<T, CsMiddle, CsRight>& M2)
{
    return {M1.raw_ * M2.raw_};
}

} // namespace P3
} // namespace tyco


/*===========================================================================*\

Author: Matthias W. Smith
Email:  mwsmith2@uw.edu
Date:   11/02/14

Detail: This is a new test program for my Fid libraries

\*===========================================================================*/

//--- std includes ----------------------------------------------------------//
#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
using std::vector;
using std::cout;
using std::endl;

//--- other includes --------------------------------------------------------//
#include <armadillo>

//--- project includes ------------------------------------------------------//
#include "fid.h"
using namespace fid;

int main(int argc, char** argv)
{
  // set precision
  cout.precision(10);
  cout.setf(std::ios::fixed, std:: ios::floatfield);

  // declare variables
  int fid_length = 5000;
  double ti = -1.0;
  double dt = 0.001;
  double ftruth = 23.0;

  vector<double> wf_re;
  vector<double> tm;
  wf_re.reserve(fid_length);
  tm.reserve(fid_length);

  std::ofstream out;
  out.precision(10);

  for (int i = 0; i < fid_length; i++){
    tm.push_back(i * dt + ti);
  }

  for (int i = 0; i < fid_length; ++i) {
    wf_re.push_back(sin(40 * tm[i]));
  }

  auto wf_im = dsp::hilbert(wf_re);
  arma::cx_vec wf(wf_im.size());

  for (int i = 0; i < wf_re.size(); ++i) {
    wf[i] = arma::cx_double(wf_re[i], wf_im[i]);
  }

  auto wf_rc = dsp::rconvolve(wf, 4000);

  out.open("wvd_test_rc_real.txt");
  for (int i = 0; i < wf_rc.n_elem; ++i) {
    out << wf_rc[i].real() << ",";
  }
  out.close();

  out.open("wvd_test_rc_imag.txt");
  for (int i = 0; i < wf_rc.n_elem; ++i) {
    out << wf_rc[i].imag() << ",";
  }
  out.close();

  return 0;
}


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

/*
 * References:
 * Ooura, Takuya, and Masatake Mori. "A robust double exponential formula for Fourier-type integrals." Journal of computational and applied mathematics 112.1-2 (1999): 229-241.
 * http://www.kurims.kyoto-u.ac.jp/~ooura/intde.html
 */
#ifndef BOOST_MATH_QUADRATURE_OOURA_FOURIER_INTEGRALS_HPP
#define BOOST_MATH_QUADRATURE_OOURA_FOURIER_INTEGRALS_HPP
#include <memory>
#include <boost/math/quadrature/detail/ooura_fourier_integrals_detail.hpp>

namespace boost { namespace math { namespace quadrature {

template<class Real>
class ooura_fourier_sin {
public:
    ooura_fourier_sin(const Real relative_error_tolerance = tools::root_epsilon<Real>(), size_t levels = sizeof(Real)) : impl_(std::make_shared<detail::ooura_fourier_sin_detail<Real>>(relative_error_tolerance, levels))
    {}

    template<class F>
    std::pair<Real, Real> integrate(F const & f, Real omega) {
        return impl_->integrate(f, omega);
    }

    // These are just for debugging/unit tests:
    std::vector<std::vector<Real>> const & big_nodes() const {
        return impl_->big_nodes();
    }

    std::vector<std::vector<Real>> const & weights_for_big_nodes() const {
        return impl_->weights_for_big_nodes();
    }

    std::vector<std::vector<Real>> const & little_nodes() const {
        return impl_->little_nodes();
    }

    std::vector<std::vector<Real>> const & weights_for_little_nodes() const {
        return impl_->weights_for_little_nodes();
    }

private:
    std::shared_ptr<detail::ooura_fourier_sin_detail<Real>> impl_;
};

template<class Real>
class ooura_fourier_cos {
public:
    ooura_fourier_cos(const Real relative_error_tolerance = tools::root_epsilon<Real>(), size_t levels = sizeof(Real)) : impl_(std::make_shared<detail::ooura_fourier_cos_detail<Real>>(relative_error_tolerance, levels))
    {}

    template<class F>
    std::pair<Real, Real> integrate(F const & f, Real omega) {
        return impl_->integrate(f, omega);
    }
private:
    std::shared_ptr<detail::ooura_fourier_cos_detail<Real>> impl_;
};

}}}
#endif


#include "domain/finite_element/finite_element_gaussian.hpp"

#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_update_flags.h>

namespace bart::domain::finite_element {

template <int dim>
FiniteElementGaussian<dim>::FiniteElementGaussian(DiscretizationType discretization, int polynomial_degree)
    : polynomial_degree_(polynomial_degree) {
  std::string description{"deal.II Gaussian, " + std::to_string(dim) + "D, "};

  const auto update_flags = dealii::update_values | dealii::update_gradients | dealii::update_quadrature_points |
                            dealii::update_JxW_values;
  const auto face_update_flags = dealii::update_values | dealii::update_gradients | dealii::update_quadrature_points |
      dealii::update_JxW_values | dealii::update_normal_vectors;

  finite_element_ = GetFiniteElement(discretization);
  cell_quadrature_ = std::make_shared<dealii::QGauss<dim>>(polynomial_degree + 1);
  face_quadrature_ = std::make_shared<dealii::QGauss<dim - 1>>(polynomial_degree + 1);
  values_ = std::make_shared<dealii::FEValues<dim>>(*finite_element_, *cell_quadrature_, update_flags);
  face_values_ = std::make_shared<dealii::FEFaceValues<dim>>(*finite_element_, *face_quadrature_, face_update_flags);

  if (discretization == DiscretizationType::kDiscontinuousFEM) {
    neighbor_face_values_ =
        std::make_shared<dealii::FEFaceValues<dim>>(*finite_element_, *face_quadrature_, face_update_flags);
    description += "Discontinuous, ";
  } else {
    description += "Continuous, ";
  }
  description += "Q = " + std::to_string(polynomial_degree);
  this->set_description(description, utility::DefaultImplementation(true));
}

template <int dim>
auto FiniteElementGaussian<dim>::GetFiniteElement(DiscretizationType discretization)
-> std::shared_ptr<dealii::FiniteElement<dim, dim>> {
  switch (discretization) {
    case DiscretizationType::kContinuousFEM: {
      return std::make_shared<dealii::FE_Q<dim>>(polynomial_degree_);
    }
    case DiscretizationType::kDiscontinuousFEM: {
      return std::make_shared<dealii::FE_DGQ<dim>>(polynomial_degree_);
    }
    default: {
      AssertThrow(false, dealii::ExcMessage("Cannot build FiniteElementGaussian object with discretization type None"));
      break;
    }
  }
}

template class FiniteElementGaussian<1>;
template class FiniteElementGaussian<2>;
template class FiniteElementGaussian<3>;

} // namespace bart::domain::finite_element


/*************************************************
 * Copyright (c) 2017 Toru Ito
 * Released under the MIT license
 * http://opensource.org/licenses/mit-license.php
 *************************************************/

#ifndef Delaunay_hpp
#define Delaunay_hpp

#include <stdio.h>
#include <vector>
#include <set>
#include <Eigen/Core>
#include <Eigen/Geometry>

class Delaunay
{
public:
    struct TriangleData
    {
        unsigned long   index1;
        unsigned long   index2;
        unsigned long   index3;
        double          radius;
        double          bounds[4];
        Eigen::Vector2d center;

        TriangleData()
        {
            index1 = index2 = index3 = 0;
            bounds[0] = bounds[1] = bounds[2] = bounds[3] = 0.0;
            radius = 0.0;
            center = Eigen::Vector2d::Zero();
        }

        bool IsInside( Eigen::Vector2d point )
        {
            if( point.x() < bounds[0] )
                return false;

            if( point.x() > bounds[1] )
                return false;

            if( point.y() < bounds[2] )
                return false;

            if( point.y() > bounds[3] )
                return false;

            if( ( point - center ).norm() > radius )
                return false;

            return true;
        }
    };

    struct EdgeData
    {
        unsigned long index1, index2;

        EdgeData()
        {
            index1 = 0;
            index2 = 0;
        };

        EdgeData( unsigned long index1, unsigned long index2 )
        {
            if( index1 < index2 ) {
                this->index1 = index1;
                this->index2 = index2;
            } else {
                this->index1 = index2;
                this->index2 = index1;
            }
        };
    };

public:
    Delaunay();
    ~Delaunay();

    void SetPoint( std::vector< Eigen::Vector2d > *pPointList );
    void GetResult( std::vector< Eigen::Vector2d > *pPointList, std::vector< std::vector< unsigned int > > *pIndexList );

    void Triangulation();

private:
    std::vector< TriangleData >    m_Triangles;
    std::vector< Eigen::Vector2d > m_Points;
    std::vector< unsigned long >   m_InitTrianglePointIndex;

    void CreateInitTriangle();
    void DeleteInitTriangle();

    void AddTriangle( unsigned long index1, unsigned long index2, unsigned long index3 );
};

#endif /* Delaunay_hpp */


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#include <boost/numeric/odeint.hpp>

#include <cbr_math/lie/group_product.hpp>
#include <cbr_math/lie/Tn.hpp>

#include <cbr_control/asif++.hpp>

#include <matplot/matplot.h>

#include <algorithm>
#include <vector>

struct Dynamics
{
  template<typename T>
  using State = cbr::lie::T2<T>;

  template<typename T>
  using Input = Eigen::Matrix<T, 1, 1>;

  template<typename T>
  Eigen::Matrix<T, 2, 1> f(const State<T> & x) const
  {
    return Eigen::Matrix<T, 2, 1>(x.translation()(1), 0);
  }

  template<typename T>
  Eigen::Matrix<T, 2, 1> g(const State<T> & x) const
  {
    return Eigen::Matrix<T, 2, 1>(0, 1);
  }
};

struct SS
{
  template<typename T>
  Eigen::Matrix<T, 2, 1> operator()(const cbr::lie::T2<T> & x) const
  {
    return Eigen::Matrix<T, 2, 1>(
      T(2) - x.translation().x(),
      T(2) - x.translation().y()
    );
  }
};

struct Backup
{
  template<typename T>
  Eigen::Matrix<T, 1, 1> operator()(const Dynamics::State<T> & x) const
  {
    return Eigen::Matrix<T, 1, 1>(-0.6);
  }
};

int main()
{
  cbr::ASIFParams params;
  params.dt = 0.02;
  params.steps = 200;
  params.constr_dist = 10;
  params.relax_cost = 500;
  params.debug = false;

  Dynamics dyn;
  Backup bu;
  SS ss;

  cbr::ASIF asif(dyn, bu, ss, params);
  asif.setBounds(Eigen::Matrix<double, 1, 1>(-0.5), Eigen::Matrix<double, 1, 1>(1.5));

  std::vector<double> sol_t;
  std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> sol_x;
  std::vector<double> sol_u, sol_udes;

  Eigen::Vector2d x(-5, 1);
  double udes = 1;

  double t = 0;
  double dt = 0.01;

  boost::numeric::odeint::runge_kutta4<Eigen::Vector2d, double, Eigen::Vector2d, double,
    boost::numeric::odeint::vector_space_algebra> stepper {};

  for (int i = 0; i != static_cast<int>(10. / dt); ++i) {
    Eigen::Matrix<double, 1, 1> u(udes);
    asif.filter(cbr::lie::T2d(x), u);

    sol_t.push_back(t);
    sol_x.push_back(x);
    sol_udes.push_back(udes);
    sol_u.push_back(u(0));

    stepper.do_step(
      [&](const Eigen::Vector2d & x, Eigen::Vector2d & dx, double t) {dx(0) = x(1); dx(1) = u(0);},
      x, t, dt
    );
    t += dt;
  }

  // helper function to extract stuff from solutions
  auto ex_fn = [](const auto & item, auto ex_fn) {
      std::vector<double> ret;
      std::transform(item.cbegin(), item.cend(), std::back_inserter(ret), ex_fn);
      return ret;
    };

  matplot::figure();
  matplot::hold(matplot::on);
  matplot::plot(sol_t, ex_fn(sol_x, [](auto s) {return s(0);}))->line_width(2);
  matplot::plot(sol_t, ex_fn(sol_x, [](auto s) {return s(1);}))->line_width(2);
  matplot::title("states");
  matplot::legend({"x", "v"});
  matplot::figure();
  matplot::hold(matplot::on);
  matplot::plot(sol_t, sol_udes)->line_width(2);
  matplot::plot(sol_t, sol_u)->line_width(2);
  matplot::title("input");
  matplot::legend({"u_{des}", "u"});

  matplot::show();

  return EXIT_SUCCESS;
}


///////////////////////////////////////////////////////////////////////////////
// ars::example::search_reflection.cpp                                       //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#include <boost/format.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/normal/include.hpp>
#include <boost/ars/search_reflection.hpp>
#include <boost/ars/constant.hpp>

void example_search_reflection(std::ostream& out){
    std::cout << "-> example_search_reflection" << std::endl;
    using namespace boost;
    namespace st = boost::statistics::detail;

    typedef double value_t;
    typedef st::ars::point<value_t> point_t;
    typedef st::ars::constant<value_t> const_;

    struct local{
        static std::ostream& write(
            std::ostream& o,
            const value_t& x_0,
            const value_t& x_1,
            unsigned n,
            const point_t& p_0,
            const point_t& p_1
        ){
            boost::format f("x_0 = %1%, x_1 = %2%, ");
            f % x_0 % x_1;
            o << f.str();
            o << ", n = " << n << ", p_0 : " << p_0 << ", p_1 : " << p_1;
            return o;
        }
    };

    value_t x_min,x_max, x_0, x_1;
    point_t p_0,p_1;
    unsigned n_max, n = 0;

    {
        typedef math::normal_distribution<value_t> mdist_t;
        typedef const mdist_t& param_t;
        x_min = const_::inf_;
        x_max = const_::inf_;
        n_max = 1e2;
        const value_t mu = 0.0;
        const value_t sigma = 2.0;
        mdist_t mdist(mu,sigma);
        out << (boost::format("N(%1%,%2%)")%mu%sigma);

        {
            x_0 = 100.0;
            x_1 = 100.01;
            n = st::ars::search_reflection_dist(
                x_min,
                x_max,
                mdist,
                x_0,
                x_1,
                p_0,
                p_1,
                n_max
            );
            local::write(out,x_0,x_1,n,p_0,p_1);
            out << std::endl;
        }

        {
            x_0 = -100.01;
            x_1 = -100.00;
            n = st::ars::search_reflection_dist(
                x_min,
                x_max,
                mdist,
                x_0,
                x_1,
                p_0,
                p_1,
                n_max
            );
            local::write(out,x_0,x_1,n,p_0,p_1);
            out << std::endl;
        }

        {
            x_0 = -0.02;
            x_1 = -0.01;
            n = st::ars::search_reflection_dist(
                x_min,
                x_max,
                mdist,
                x_0,
                x_1,
                p_0,
                p_1,
                n_max
            );
            local::write(out,x_0,x_1,n,p_0,p_1);
            out << std::endl;
        }

    }
    out << "<-" << std::endl;
}