/*
 * Copyright (C) 2019  Rhys Mainwaring
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#include "wave_gazebo_plugins/Wavefield.hh"
#include "wave_gazebo_plugins/Geometry.hh"
#include "wave_gazebo_plugins/Physics.hh"
#include "wave_gazebo_plugins/Utilities.hh"

#include <Eigen/Dense>

#include <gazebo/gazebo.hh>
#include <gazebo/common/common.hh>
#include <gazebo/msgs/msgs.hh>

#include <ignition/math/Pose3.hh>
#include <ignition/math/Vector2.hh>
#include <ignition/math/Vector3.hh>

#include <array>
#include <iostream>
#include <cmath>
#include <string>

namespace asv
{
///////////////////////////////////////////////////////////////////////////////
// Utilities

  std::ostream& operator<<(std::ostream& os, const std::vector<double>& _vec)
  {
    for (auto&& v : _vec )
      os << v << ", ";
    return os;
  }

///////////////////////////////////////////////////////////////////////////////
// WaveParametersPrivate

  /// \internal
  /// \brief Private data for the WavefieldParameters.
  class WaveParametersPrivate
  {
    /// \brief Constructor.
    public: WaveParametersPrivate():
      number(1),
      scale(2.0),
      angle(2.0*M_PI/10.0),
      steepness(1.0),
      amplitude(0.0),
      period(1.0),
      phase(0.0),
      direction(1, 0),
      angularFrequency(2.0*M_PI),
      wavelength(2*M_PI/Physics::DeepWaterDispersionToWavenumber(2.0*M_PI)),
      wavenumber(Physics::DeepWaterDispersionToWavenumber(2.0*M_PI))
    {
    }

    /// \brief The number of component waves.
    public: size_t number;

    /// \brief Set the scale of the largest and smallest waves.
    public: double scale;

    /// \brief Set the angle between component waves and the mean direction.
    public: double angle;

    /// \brief Control the wave steepness. 0 is sine waves, 1 is Gerstner waves.
    public: double steepness;

    /// \brief The mean wave amplitude [m].
    public: double amplitude;

    /// \brief The mean wave period [s]
    public: double period;

    /// \brief The mean wve phase (not currently enabled).
    public: double phase;

    /// \brief The mean wave direction.
    public: ignition::math::Vector2d direction;

    /// \brief The mean wave angular frequency (derived).
    public: double angularFrequency;

    /// \brief The mean wavelength (derived).
    public: double wavelength;

    /// \brief The mean wavenumber (derived).
    public: double wavenumber;

    /// \brief The component wave angular frequencies (derived).
    public: std::vector<double> angularFrequencies;

    /// \brief The component wave amplitudes (derived).
    public: std::vector<double> amplitudes;

    /// \brief The component wave phases (derived).
    public: std::vector<double> phases;

    /// \brief The component wave steepness factors (derived).
    public: std::vector<double> steepnesses;

    /// \brief The component wavenumbers (derived).
    public: std::vector<double> wavenumbers;

    /// \brief The component wave dirctions (derived).
    public: std::vector<ignition::math::Vector2d> directions;

    /// \brief Recalculate all derived quantities from inputs.
    public: void Recalculate()
    {
      // Normalize direction
      this->direction = Geometry::Normalize(this->direction);

      // Derived mean values
      this->angularFrequency = 2.0 * M_PI / this->period;
      this->wavenumber = Physics::DeepWaterDispersionToWavenumber(this->angularFrequency);
      this->wavelength = 2.0 * M_PI / this->wavenumber;

      // Update components
      this->angularFrequencies.clear();
      this->amplitudes.clear();
      this->phases.clear();
      this->wavenumbers.clear();
      this->steepnesses.clear();
      this->directions.clear();

      for (size_t i=0; i<this->number; ++i)
      {
        const int n = i - this->number/2;
        const double scaleFactor = std::pow(this->scale, n);
        const double a = scaleFactor * this->amplitude;
        const double k = this->wavenumber / scaleFactor;
        const double omega = Physics::DeepWaterDispersionToOmega(k);
        const double phi = this->phase;
        double q = 0.0;
        if (a != 0)
        {
          q = std::min(1.0, this->steepness / (a * k * this->number));
        }

        this->amplitudes.push_back(a);
        this->angularFrequencies.push_back(omega);
        this->phases.push_back(phi);
        this->steepnesses.push_back(q);
        this->wavenumbers.push_back(k);

        // Direction
        const double c = std::cos(n * this->angle);
        const double s = std::sin(n * this->angle);
        // const TransformMatrix T(
        //   c, -s,
        //   s,  c
        // );
        // const ignition::math::Vector2d d = T(this->direction);
        const ignition::math::Vector2d d(
          c * this->direction.X() - s * this->direction.Y(),
          s * this->direction.X() + c * this->direction.Y()
        );
        directions.push_back(d);
      }
    }
  };

///////////////////////////////////////////////////////////////////////////////
// WaveParameters

  WaveParameters::~WaveParameters()
  {
  }

  WaveParameters::WaveParameters()
    : data(new WaveParametersPrivate())
  {
    this->data->Recalculate();
  }

  void WaveParameters::FillMsg(gazebo::msgs::Param_V& _msg) const
  {
    // Clear
    _msg.mutable_param()->Clear();

    // "number"
    {
      auto nextParam = _msg.add_param();
      nextParam->set_name("number");
      nextParam->mutable_value()->set_type(gazebo::msgs::Any::INT32);
      nextParam->mutable_value()->set_int_value(this->data->number);
    }
    // "scale"
    {
      auto nextParam = _msg.add_param();
      nextParam->set_name("scale");
      nextParam->mutable_value()->set_type(gazebo::msgs::Any::DOUBLE);
      nextParam->mutable_value()->set_double_value(this->data->scale);
    }
    // "angle"
    {
      auto nextParam = _msg.add_param();
      nextParam->set_name("angle");
      nextParam->mutable_value()->set_type(gazebo::msgs::Any::DOUBLE);
      nextParam->mutable_value()->set_double_value(this->data->angle);
    }
    // "steepness"
    {
      auto nextParam = _msg.add_param();
      nextParam->set_name("steepness");
      nextParam->mutable_value()->set_type(gazebo::msgs::Any::DOUBLE);
      nextParam->mutable_value()->set_double_value(this->data->steepness);
    }
    // "amplitude"
    {
      auto nextParam = _msg.add_param();
      nextParam->set_name("amplitude");
      nextParam->mutable_value()->set_type(gazebo::msgs::Any::DOUBLE);
      nextParam->mutable_value()->set_double_value(this->data->amplitude);
    }
    // "period"
    {
      auto nextParam = _msg.add_param();
      nextParam->set_name("period");
      nextParam->mutable_value()->set_type(gazebo::msgs::Any::DOUBLE);
      nextParam->mutable_value()->set_double_value(this->data->period);
    }
    // "direction"
    {
      const auto& direction = this->data->direction;
      auto nextParam = _msg.add_param();
      nextParam->set_name("direction");
      nextParam->mutable_value()->set_type(gazebo::msgs::Any::VECTOR3D);
      nextParam->mutable_value()->mutable_vector3d_value()->set_x(direction.X());
      nextParam->mutable_value()->mutable_vector3d_value()->set_y(direction.Y());
      nextParam->mutable_value()->mutable_vector3d_value()->set_z(0);
    }
  }

  void WaveParameters::SetFromMsg(const gazebo::msgs::Param_V& _msg)
  {
    this->data->number    = Utilities::MsgParamSizeT(_msg,    "number",     this->data->number);
    this->data->amplitude = Utilities::MsgParamDouble(_msg,   "amplitude",  this->data->amplitude);
    this->data->period    = Utilities::MsgParamDouble(_msg,   "period",     this->data->period);
    this->data->phase     = Utilities::MsgParamDouble(_msg,   "phase",      this->data->phase);
    this->data->direction = Utilities::MsgParamVector2(_msg,  "direction",  this->data->direction);
    this->data->scale     = Utilities::MsgParamDouble(_msg,   "scale",      this->data->scale);
    this->data->angle     = Utilities::MsgParamDouble(_msg,   "angle",      this->data->angle);
    this->data->steepness = Utilities::MsgParamDouble(_msg,   "steepness",  this->data->steepness);

    this->data->Recalculate();
  }

  void WaveParameters::SetFromSDF(sdf::Element& _sdf)
  {
    this->data->number    = Utilities::SdfParamSizeT(_sdf,    "number",     this->data->number);
    this->data->amplitude = Utilities::SdfParamDouble(_sdf,   "amplitude",  this->data->amplitude);
    this->data->period    = Utilities::SdfParamDouble(_sdf,   "period",     this->data->period);
    this->data->phase     = Utilities::SdfParamDouble(_sdf,   "phase",      this->data->phase);
    this->data->direction = Utilities::SdfParamVector2(_sdf,  "direction",  this->data->direction);
    this->data->scale     = Utilities::SdfParamDouble(_sdf,   "scale",      this->data->scale);
    this->data->angle     = Utilities::SdfParamDouble(_sdf,   "angle",      this->data->angle);
    this->data->steepness = Utilities::SdfParamDouble(_sdf,   "steepness",  this->data->steepness);

    this->data->Recalculate();
  }

  size_t WaveParameters::Number() const
  {
    return this->data->number;
  }

  double WaveParameters::Angle() const
  {
    return this->data->angle;
  }

  double WaveParameters::Scale() const
  {
    return this->data->scale;
  }

  double WaveParameters::Steepness() const
  {
    return this->data->steepness;
  }

  double WaveParameters::AngularFrequency() const
  {
    return this->data->angularFrequency;
  }

  double WaveParameters::Amplitude() const
  {
    return this->data->amplitude;
  }

  double WaveParameters::Period() const
  {
    return this->data->period;
  }

  double WaveParameters::Phase() const
  {
    return this->data->phase;
  }

  double WaveParameters::Wavelength() const
  {
    return this->data->wavelength;
  }

  double WaveParameters::Wavenumber() const
  {
    return this->data->wavenumber;
  }

  ignition::math::Vector2d WaveParameters::Direction() const
  {
    return this->data->direction;
  }

  void WaveParameters::SetNumber(size_t _number)
  {
    this->data->number = _number;
    this->data->Recalculate();
  }

  void WaveParameters::SetAngle(double _angle)
  {
    this->data->angle = _angle;
    this->data->Recalculate();
  }

  void WaveParameters::SetScale(double _scale)
  {
    this->data->scale = _scale;
    this->data->Recalculate();
  }

  void WaveParameters::SetSteepness(double _steepness)
  {
    this->data->steepness = _steepness;
    this->data->Recalculate();
  }

  void WaveParameters::SetAmplitude(double _amplitude)
  {
    this->data->amplitude = _amplitude;
    this->data->Recalculate();
  }

  void WaveParameters::SetPeriod(double _period)
  {
    this->data->period = _period;
    this->data->Recalculate();
  }

  void WaveParameters::SetPhase(double _phase)
  {
    this->data->phase = _phase;
    this->data->Recalculate();
  }

  void WaveParameters::SetDirection(const ignition::math::Vector2d& _direction)
  {
    this->data->direction = _direction;
    this->data->Recalculate();
  }

  const std::vector<double>& WaveParameters::AngularFrequency_V() const
  {
    return this->data->angularFrequencies;
  }

  const std::vector<double>& WaveParameters::Amplitude_V() const
  {
    return this->data->amplitudes;
  }

  const std::vector<double>& WaveParameters::Phase_V() const
  {
    return this->data->phases;
  }

  const std::vector<double>& WaveParameters::Steepness_V() const
  {
    return this->data->steepnesses;
  }

  const std::vector<double>& WaveParameters::Wavenumber_V() const
  {
    return this->data->wavenumbers;
  }

  const std::vector<ignition::math::Vector2d>& WaveParameters::Direction_V() const
  {
    return this->data->directions;
  }

  void WaveParameters::DebugPrint() const
  {
    gzmsg << "number:     " << this->data->number << std::endl;
    gzmsg << "scale:      " << this->data->scale << std::endl;
    gzmsg << "angle:      " << this->data->angle << std::endl;
    gzmsg << "period:     " << this->data->period << std::endl;
    gzmsg << "amplitude:  " << this->data->amplitudes << std::endl;
    gzmsg << "wavenumber: " << this->data->wavenumbers << std::endl;
    gzmsg << "omega:      " << this->data->angularFrequencies << std::endl;
    gzmsg << "phase:      " << this->data->phases << std::endl;
    gzmsg << "steepness:  " << this->data->steepnesses << std::endl;
    for (auto&& d : this->data->directions)
    {
      gzmsg << "direction:  " << d << std::endl;
    }
  }

///////////////////////////////////////////////////////////////////////////////
// WavefieldSampler

  double WavefieldSampler::ComputeDepthDirectly(
    const WaveParameters& _waveParams,
    const ignition::math::Vector3d& _point,
    double time
  )
  {
    // Struture for passing wave parameters to lambdas
    struct WaveParams
    {
      WaveParams(
        const std::vector<double>& _a,
        const std::vector<double>& _k,
        const std::vector<double>& _omega,
        const std::vector<double>& _phi,
        const std::vector<double>& _q,
        const std::vector<ignition::math::Vector2d>& _dir) :
        a(_a), k(_k), omega(_omega), phi(_phi), q(_q), dir(_dir) {}

      const std::vector<double>& a;
      const std::vector<double>& k;
      const std::vector<double>& omega;
      const std::vector<double>& phi;
      const std::vector<double>& q;
      const std::vector<ignition::math::Vector2d>& dir;
    };

    // Compute the target function and Jacobian. Also calculate pz,
    // the z-componen of the Gerstner wave, which we essentially get for free.
    auto wave_fdf = [=](auto x, auto p, auto t, auto& wp, auto& F, auto& J)
    {
      double pz = 0;
      F(0) = p.x() - x.x();
      F(1) = p.y() - x.y();
      J(0, 0) = -1;
      J(0, 1) =  0;
      J(1, 0) =  0;
      J(1, 1) = -1;
      const size_t n = wp.a.size();
      for (auto&& i=0; i<n; ++i)
      {
        const double dx = wp.dir[i].X();
        const double dy = wp.dir[i].Y();
        const double q = wp.q[i];
        const double a = wp.a[i];
        const double k = wp.k[i];
        const double dot = x.x() * dx + x.y() * dy;
        const double theta = k * dot - wp.omega[i] * t;
        const double s = std::sin(theta);
        const double c = std::cos(theta);
        const double qakc = q * a * k * c;
        const double df1x = qakc * dx * dx;
        const double df1y = qakc * dx * dy;
        const double df2x = df1y;
        const double df2y = qakc * dy * dy;
        pz += a * c;
        F(0) += a * dx * s;
        F(1) += a * dy * s;
        J(0, 0) += df1x;
        J(0, 1) += df1y;
        J(1, 0) += df2x;
        J(1, 1) += df2y;
      }
      return pz;
    };

    // Simple multi-variate Newton solver - this version returns the z-component of the
    // wave field at the desired point p.
    auto solver = [=](auto& fdfunc, auto x0, auto p, auto t, auto& wp, auto tol, auto nmax)
    {
      int n = 0;
      double err = 1;
      double pz = 0;
      auto xn = x0;
      Eigen::Vector2d F;
      Eigen::Matrix2d J;
      while (std::abs(err) > tol && n < nmax)
      {
        pz = fdfunc(x0, p, t, wp, F, J);
        xn = x0 - J.inverse() * F;
        x0 = xn;
        err = F.norm();
        n++;
      }
      return pz;
    };

    // Set up parameter references
    WaveParams wp(
      _waveParams.Amplitude_V(),
      _waveParams.Wavenumber_V(),
      _waveParams.AngularFrequency_V(),
      _waveParams.Phase_V(),
      _waveParams.Steepness_V(),
      _waveParams.Direction_V()
    );

    // Tolerances etc.
    const double tol = 1.0E-10;
    const double nmax = 30;

    // Use the target point as the initial guess (this is within sum{amplitudes} of the solution)
    Eigen::Vector2d p2(_point.X(), _point.Y());
    const double pz = solver(wave_fdf, p2, p2, time, wp, tol, nmax);
    const double h = pz - _point.Z();
    return h;
  }

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

} // namespace asv


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file sviinterpolation.hpp
    \brief Svi interpolation interpolation between discrete points
*/

#ifndef quantlib_svi_interpolation_hpp
#define quantlib_svi_interpolation_hpp

#include <ql/experimental/volatility/svismilesection.hpp>
#include <ql/math/interpolations/xabrinterpolation.hpp>
#include <boost/assign/list_of.hpp>
#include <utility>

namespace QuantLib {

namespace detail {

inline void checkSviParameters(const Real a, const Real b, const Real sigma,
                               const Real rho, const Real m) {
    QL_REQUIRE(b >= 0.0, "b (" << b << ") must be non negative");
    QL_REQUIRE(std::fabs(rho) < 1.0, "rho (" << rho << ") must be in (-1,1)");
    QL_REQUIRE(sigma > 0.0, "sigma (" << sigma << ") must be positive");
    QL_REQUIRE(a + b * sigma * std::sqrt(1.0 - rho * rho) >= 0.0,
               "a + b sigma sqrt(1-rho^2) (a=" << a << ", b=" << b << ", sigma="
                                               << sigma << ", rho=" << rho
                                               << ") must be non negative");
    QL_REQUIRE(b * (1.0 + std::fabs(rho)) < 4.0,
               "b(1+|rho|) must be less than 4");
}

inline Real sviTotalVariance(const Real a, const Real b, const Real sigma,
                             const Real rho, const Real m, const Real k) {
    return a +
           b * (rho * (k - m) + std::sqrt((k - m) * (k - m) + sigma * sigma));
}

typedef SviSmileSection SviWrapper;

struct SviSpecs {
    Size dimension() { return 5; }
    void defaultValues(std::vector<Real> &params,
                       std::vector<bool> &paramIsFixed, const Real &forward,
                       const Real expiryTime,
                       const std::vector<Real> &addParams) {
        if (params[2] == Null<Real>())
            params[2] = 0.1;
        if (params[3] == Null<Real>())
            params[3] = -0.4;
        if (params[4] == Null<Real>())
            params[4] = 0.0;
        if (params[1] == Null<Real>())
            params[1] = 2.0 / (1.0 + std::fabs(params[3]));
        if (params[0] == Null<Real>()) {
            params[0] = std::max(
                0.20 * 0.20 * expiryTime -
                    params[1] * (params[3] * (-params[4]) +
                                 std::sqrt((-params[4]) * (-params[4]) +
                                           params[2] * params[2])),
                -params[1] * params[2] *
                std::sqrt(1.0 - params[3] * params[3]) + eps1());
        }
    }
    void guess(Array &values, const std::vector<bool> &paramIsFixed,
               const Real &forward, const Real expiryTime,
               const std::vector<Real> &r, const std::vector<Real> &addParams) {
        Size j = 0;
        if (!paramIsFixed[2])
            values[2] = r[j++] + eps1();
        if (!paramIsFixed[3])
            values[3] = (2.0 * r[j++] - 1.0) * eps2();
        if (!paramIsFixed[4])
            values[4] = (2.0 * r[j++] - 1.0);
        if (!paramIsFixed[1])
            values[1] = r[j++] * 4.0 / (1.0 + std::fabs(values[3])) * eps2();
        if (!paramIsFixed[0])
            values[0] = r[j++] * expiryTime -
                        eps2() * (values[1] * values[2] *
                                  std::sqrt(1.0 - values[3] * values[3]));
    }
    Array inverse(const Array &y, const std::vector<bool> &,
                  const std::vector<Real> &, const Real) {
        Array x(5);
        x[2] = std::sqrt(y[2] - eps1());
        x[3] = std::asin(y[3] / eps2());
        x[4] = y[4];
        x[1] = std::tan(y[1] / 4.0 * (1.0 + std::fabs(y[3])) / eps2() * M_PI -
                        M_PI / 2.0);
        x[0] = std::sqrt(y[0] - eps1() +
                         y[1] * y[2] * std::sqrt(1.0 - y[3] * y[3]));
        return x;
    }
    Real eps1() { return 0.000001; }
    Real eps2() { return 0.999999; }
    Array direct(const Array &x, const std::vector<bool> &paramIsFixed,
                 const std::vector<Real> &params, const Real forward) {
        Array y(5);
        y[2] = x[2] * x[2] + eps1();
        y[3] = std::sin(x[3]) * eps2();
        y[4] = x[4];
        if (paramIsFixed[1])
            y[1] = params[1];
        else
            y[1] = (std::atan(x[1]) + M_PI / 2.0) / M_PI * eps2() * 4.0 /
                   (1.0 + std::fabs(y[3]));
        if (paramIsFixed[0])
            y[0] = params[0];
        else
            y[0] = eps1() + x[0] * x[0] -
                   y[1] * y[2] * std::sqrt(1.0 - y[3] * y[3]);
        return y;
    }
    Real weight(const Real strike, const Real forward, const Real stdDev,
                const std::vector<Real> &addParams) {
        return blackFormulaStdDevDerivative(strike, forward, stdDev, 1.0);
    }
    typedef SviWrapper type;
    ext::shared_ptr<type> instance(const Time t, const Real &forward,
                                     const std::vector<Real> &params,
                                     const std::vector<Real> &addParams) {
        return ext::make_shared<type>(t, forward, params);
    }
};
}

//! %Svi smile interpolation between discrete volatility points.
class SviInterpolation : public Interpolation {
  public:
    template <class I1, class I2>
    SviInterpolation(const I1 &xBegin, // x = strikes
                     const I1 &xEnd,
                     const I2 &yBegin, // y = volatilities
                     Time t,           // option expiry
                     const Real &forward, Real a, Real b, Real sigma, Real rho,
                     Real m, bool aIsFixed, bool bIsFixed, bool sigmaIsFixed,
                     bool rhoIsFixed, bool mIsFixed, bool vegaWeighted = true,
                     const ext::shared_ptr<EndCriteria> &endCriteria =
                         ext::shared_ptr<EndCriteria>(),
                     const ext::shared_ptr<OptimizationMethod> &optMethod =
                         ext::shared_ptr<OptimizationMethod>(),
                     const Real errorAccept = 0.0020,
                     const bool useMaxError = false,
                     const Size maxGuesses = 50) {

        impl_ = ext::shared_ptr<Interpolation::Impl>(
            new detail::XABRInterpolationImpl<I1, I2, detail::SviSpecs>(
                xBegin, xEnd, yBegin, t, forward,
                boost::assign::list_of(a)(b)(sigma)(rho)(m),
                boost::assign::list_of(aIsFixed)(bIsFixed)(sigmaIsFixed)(
                    rhoIsFixed)(mIsFixed),
                vegaWeighted, endCriteria, optMethod, errorAccept, useMaxError,
                maxGuesses));
        coeffs_ = ext::dynamic_pointer_cast<
            detail::XABRCoeffHolder<detail::SviSpecs> >(impl_);
    }
    Real expiry() const { return coeffs_->t_; }
    Real forward() const { return coeffs_->forward_; }
    Real a() const { return coeffs_->params_[0]; }
    Real b() const { return coeffs_->params_[1]; }
    Real sigma() const { return coeffs_->params_[2]; }
    Real rho() const { return coeffs_->params_[3]; }
    Real m() const { return coeffs_->params_[4]; }
    Real rmsError() const { return coeffs_->error_; }
    Real maxError() const { return coeffs_->maxError_; }
    const std::vector<Real> &interpolationWeights() const {
        return coeffs_->weights_;
    }
    EndCriteria::Type endCriteria() { return coeffs_->XABREndCriteria_; }

  private:
    ext::shared_ptr<detail::XABRCoeffHolder<detail::SviSpecs> > coeffs_;
};

//! %Svi interpolation factory and traits
class Svi {
  public:
    Svi(Time t,
        Real forward,
        Real a,
        Real b,
        Real sigma,
        Real rho,
        Real m,
        bool aIsFixed,
        bool bIsFixed,
        bool sigmaIsFixed,
        bool rhoIsFixed,
        bool mIsFixed,
        bool vegaWeighted = false,
        ext::shared_ptr<EndCriteria> endCriteria = ext::shared_ptr<EndCriteria>(),
        ext::shared_ptr<OptimizationMethod> optMethod = ext::shared_ptr<OptimizationMethod>(),
        const Real errorAccept = 0.0020,
        const bool useMaxError = false,
        const Size maxGuesses = 50)
    : t_(t), forward_(forward), a_(a), b_(b), sigma_(sigma), rho_(rho), m_(m), aIsFixed_(aIsFixed),
      bIsFixed_(bIsFixed), sigmaIsFixed_(sigmaIsFixed), rhoIsFixed_(rhoIsFixed),
      mIsFixed_(mIsFixed), vegaWeighted_(vegaWeighted), endCriteria_(std::move(endCriteria)),
      optMethod_(std::move(optMethod)), errorAccept_(errorAccept), useMaxError_(useMaxError),
      maxGuesses_(maxGuesses) {}
    template <class I1, class I2>
    Interpolation interpolate(const I1 &xBegin, const I1 &xEnd,
                              const I2 &yBegin) const {
        return SviInterpolation(xBegin, xEnd, yBegin, t_, forward_, a_, b_,
                                 sigma_, rho_, m_, aIsFixed_, bIsFixed_,
                                 sigmaIsFixed_, rhoIsFixed_, mIsFixed_,
                                 vegaWeighted_, endCriteria_, optMethod_,
                                 errorAccept_, useMaxError_, maxGuesses_);
    }
    static const bool global = true;

  private:
    Time t_;
    Real forward_;
    Real a_, b_, sigma_, rho_, m_;
    bool aIsFixed_, bIsFixed_, sigmaIsFixed_, rhoIsFixed_, mIsFixed_;
    bool vegaWeighted_;
    const ext::shared_ptr<EndCriteria> endCriteria_;
    const ext::shared_ptr<OptimizationMethod> optMethod_;
    const Real errorAccept_;
    const bool useMaxError_;
    const Size maxGuesses_;
};
}

#endif


/* CHOMP class implementation
 *
 * Copyright (C) 2016 Rafael Valencia. All rights reserved.
 * License (3-Cluase BSD): https://github.com/rafaelvalencia
 *
 * This code uses and is based on code from:
 *   Project: trychomp https://github.com/poftwaresatent/trychomp
 *   Copyright (C) 2014 Roland Philippsen. All rights reserved.
 *   License (3-Clause BSD) : https://github.com/poftwaresatent/trychomp
 * **
 * \file chomp.cpp
 *
 * CHOMP for point vehicles (x,y) moving holonomously in the plane. It will
 * plan a trajectory (xi) connecting start point (qs) to end point (qe) while
 * avoiding obstacles (obs)
 */
#include "path_adaptor.hpp"
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <stdlib.h>
#include <sys/time.h>
#include <err.h>
#include <algorithm>

typedef Eigen::VectorXd Vector;
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Isometry3d Transform;
static size_t const obs_dim(3); 	// obstacle dimensions (x,y,Radius)

using namespace std;

#define INF 1E20
#define epsilon 2.0

static double *dt(double *f, int n) {
  double *d = new double[n];
  int *v = new int[n];
  double *z = new double[n+1];
  int k = 0;
  v[0] = 0;
  z[0] = -INF;
  z[1] = +INF;
  for (int q = 1; q <= n-1; q++) {        // Compute lower envelope
    double s  = ((f[q]+pow(q,2.0))-(f[v[k]]+pow(v[k],2.0)))/(2*q-2*v[k]);
    while (s <= z[k]) {
      k--;
      s  = ((f[q]+pow(q,2.0))-(f[v[k]]+pow(v[k],2.0)))/(2*q-2*v[k]);
    }
    k++;
    v[k] = q;
    z[k] = s;
    z[k+1] = +INF;
  }

  k = 0;
  for (int q = 0; q <= n-1; q++) {
    while (z[k+1] < q)
      k++;
    d[q] = pow(q-v[k],2.0) + f[v[k]];
  }

  delete [] v;
  delete [] z;
  return d;
}

Matrix ComputeDistanceTranform(Matrix cost_init){

	int width = cost_init.rows();
	int height = cost_init.cols();
	double *f = new double[max(width, height)];

	// transfrom over rows
	for (int x=0; x < width; x++){
		for (int y=0; y<height; y++){
			f[y] = cost_init(x,y);
		}
		double *d = dt(f,height);
		for (int y=0; y< height; y++){
			cost_init(x,y) = d[y];
		}
		delete [] d;
	}

	//cout << cost_init << endl;

	// transfrom over columns
	for (int y=0; y < height; y++){
		for (int x=0; x < width; x++ ){
			f[x] = cost_init(x,y);
		}
		double *d = dt(f, width);
		for (int x=0; x < width; x++){
			cost_init(x,y) = d[x];
		}
		delete [] d;
	}

	delete f;

	//cout << cost_init << endl;
	return cost_init;

}

CHOMP::CHOMP(double dt_input, double eta_input, double lambda_input, size_t nq_input, size_t cdim_input, size_t numIt_input, double gain, double gamma_input)
{
	//Sets basic parameters.
	nq_ = nq_input;			// number of poses q in xi
	cdim_ = cdim_input;		// dimension of config space
	xidim_ = nq_ * cdim_; 	// dimension of trajectory, xidim = nq * cdim
	dt_ =  dt_input;	    // time step
	eta_ = eta_input; 		// >= 1, regularization factor for gradient descent
	lambda_ = lambda_input; // weight of smoothness objective
	numIt_  = numIt_input; 	// Number of iterations
	costGain_ = gain;		// Gain inside cost function (usually 10)
	gamma_ = gamma_input;

	res_ = 0.0001;			// Residual from optimization
	cter_ = 0;				// Zero iterations so far

	PATH_INIT_ = false;     // path is not initialized yet

	OBS_ = Matrix::Zero (obs_dim, 1); //initialize obstacle matrix

	cout << "-----------------------------------------------------:"<< endl;
	cout << "CHOMP has the following parameters:"<< endl;
	cout << "-----------------------------------------------------:"<< endl;
	cout << "Time step: " << dt_ << endl;
	cout << "Eta: " << eta_ << endl;
	cout << "Lambda: " << lambda_ << endl;
	cout << "Number of poses in xi: " << nq_ << endl;
	cout << "Dimensions of config. space: " << cdim_ << endl;
	cout << "Number of iterations: " << numIt_ << endl;
	cout << "Cost function gain: " << costGain_ << endl;
	cout << "-----------------------------------------------------:"<< endl;

}

void CHOMP::makeGrid(double xmin, double ymin, double xmax, double ymax)
{
	int xdim = (int) xmax - xmin;
	int ydim = (int) ymax - ymin;
	grid_ = Matrix::Zero (xdim, ydim);
}

double CHOMP::chompIteration(Vector  &xi, Vector &ti)
{

	// Before performing the iteration check if a path has been given

	if (PATH_INIT_==false)
	{
		cout << "A path was not initialized. Leaving CHOMP iteration! " << endl;
		return NAN;
	}

	//////////////////////////////////////////////////
	// beginning of "the" CHOMP iteration

	Vector nabla_smooth (AA_ * xi_ + bb_);
	Vector nabla_smooth_time(gamma_* (BB_ * ti_ + tt_));

	Vector const & xidd (- nabla_smooth); // indeed, it is the same in this formulation...
	Vector const & tidd ( - nabla_smooth_time/gamma_);  // t''

	Vector nabla_obs (Vector::Zero (xidim_));  // xidim_ : dimension of trajectory
	Vector nabla_obs_time (Vector::Zero (nq_));

	for (size_t iq (0); iq < nq_; ++iq)
	{
		Vector const qq (xi_.block (iq * cdim_, 0, cdim_, 1));
		double currentTime = ti_(iq);

		Vector qd;
		double tprime;

		if (0 == iq) {
			//cout << "block:  " << xi_.block ((iq+1) * cdim_, 0, cdim_, 1) << endl;
		  qd = 0.5 * (xi_.block ((iq+1) * cdim_, 0, cdim_, 1) - qs_);
		  tprime =  1/(2*dt_) * (ti_(iq+1) - ts_);
		}
		else if (iq == nq_ - 1) {
		  qd = 0.5 * (qe_ - xi_.block ((iq-1) * cdim_, 0, cdim_, 1));
		  tprime = 1/(2*dt_) * (te_ - ti_(iq-1));
		}
		else {
		  qd = 0.5 * (xi_.block ((iq+1) * cdim_, 0, cdim_, 1) - xi_.block ((iq-1) * cdim_, 0, cdim_, 1));
		  tprime = 1/(2*dt_) * (ti_(iq+1) - ti_(iq-1));
		}

		//cout << "qs_  " << qs_ << endl;
		//cout << "qe_  " << qe_ << endl;
		//cout << "xi_  "  << xi_ << endl;
		//cout << "qd_  " << qd << endl;
		//cout << "xidd_  " << xidd << endl;
		// In this case, C and W are the same, Jacobian is identity.  We
		// still write more or less the full-fledged CHOMP expressions
		// (but we only use one body point) to make subsequent extension
		// easier.
		//
		Vector const & xx (qq);          // currentTime equivalent to this
		Vector const & xd (qd);
		Matrix const JJ (Matrix::Identity (2, 2)); // a little silly here, as noted above.
		double const vel (xd.norm());         // tprime equivalent to this
		if (vel < 1.0e-3 || tprime < 1.0e-3)
		{
			// avoid div by zero further down
			continue;
		}
		Vector const xdn (xd / vel);
		Vector const xdd (JJ * xidd.block (iq * cdim_, 0, cdim_ , 1));
		Matrix const prj (Matrix::Identity (2, 2) - xdn * xdn.transpose()); // hardcoded planar case
		Vector const kappa (prj * xdd / pow (vel, 2.0));
		Matrix delta = Matrix::Zero(2,1);
		double cost;

		//Add obstacles
		for (int ii = 0; ii < OBS_.cols(); ii++)
		{

			/*
			Vector delta(xx - OBS_.block(0, ii, 2, 1));
			double const dist(delta.norm());
			if ((dist >= OBS_(2, ii)) || (dist < 1e-9))   // Maxdist = radius*2
				continue;
			//double const cost(costGain_ * OBS_(2, ii) * pow(1.0 - dist / OBS_(2, ii), 3.0) / 3.0);
			double const cost(OBS_(2, ii) * pow(1.0 - dist / OBS_(2, ii), 3.0) / 3.0);
			delta *= - costGain_ *pow(1.0 - dist / OBS_(2, ii), 2.0) / dist;
			*/

			//cout << "xx: " << xx(0) << " " << xx(1) << endl;

			int a = (int)xx(0);
			int b = (int)xx(1);

			// cout << "a " << a <<  "  "  << "b " << b << endl;

			double distanceField = Dx_(a, b);

			delta(0,0) =  Dx_(a+1,b) - Dx_(a-1,b);
			delta(1,0) =  Dx_(a,b+1) - Dx_(a,b-1) ;

			//cout << distanceField << endl;

			if (distanceField > epsilon)
				continue;

			if (distanceField < 0)
			{
				 cost = -costGain_*distanceField + 0.5*epsilon;
				 delta = delta*-1;
			}

			else if (distanceField <= epsilon)
			{
				cost = 0.5*epsilon*pow(distanceField - epsilon,2);
				delta = delta*(Dx_(a,b) - epsilon)/epsilon;
			}

			//cout << "JJ transpose  " << JJ.transpose() << endl;
			//cout << "velocity " << vel << endl;
			//cout << "projection matrix " << prj << endl;
			//cout << "cost " << cost << endl;
			//cout << "kappa " << kappa << endl;
			//cout << "delta  " << delta << endl;

			nabla_obs.block(iq * cdim_, 0, cdim_, 1) += JJ.transpose() * vel * (prj * delta - cost * kappa);
		}

	}

	Vector dxi (Ainv_ * (nabla_obs + lambda_ * nabla_smooth));

 	//cout << "change " << dxi / eta_ << endl;

	xi_ -= dxi / eta_;

	xi = xi_; //updated path

	res_ = dxi.norm() / eta_;

	return res_;
	// end of "the" CHOMP iteration
	//////////////////////////////////////////////////
}

double CHOMP::chompUpdate(Vector  &xi, double &U_cost, double &curv)
{

	// Before performing the iteration check if a path has been given

	if (PATH_INIT_==false)
	{
		cout << "A path was not initialized. Leaving CHOMP iteration! " << endl;
		return NAN;
	}

	//////////////////////////////////////////////////
	// beginning of "the" CHOMP iteration

	double U(NAN); //cost functional value
	double F_obs(0);//obstacle functional value
	double F_smooth(0);//obstacle functional value
	double curvature(0); //curvature

	Vector nabla_smooth (AA_ * xi_ + bb_);
	Vector const & xidd (nabla_smooth); // indeed, it is the same in this formulation...

	Vector nabla_obs (Vector::Zero (xidim_));
	for (size_t iq (0); iq < nq_; ++iq)
	{
		Vector const qq (xi_.block (iq * cdim_, 0, cdim_, 1));
		Vector qd;
		if (0 == iq) {
		  qd = 0.5 * (xi_.block ((iq+1) * cdim_, 0, cdim_, 1) - qs_);
		}
		else if (iq == nq_ - 1) {
		  qd = 0.5 * (qe_ - xi_.block ((iq-1) * cdim_, 0, cdim_, 1));
		}
		else {
		  qd = 0.5 * (xi_.block ((iq+1) * cdim_, 0, cdim_, 1) - xi_.block ((iq-1) * cdim_, 0, cdim_, 1));;
		}

		// In this case, C and W are the same, Jacobian is identity.  We
		// still write more or less the full-fledged CHOMP expressions
		// (but we only use one body point) to make subsequent extension
		// easier.
		//
		Vector const & xx (qq);
		Vector const & xd (qd);
		Matrix const JJ (Matrix::Identity (2, 2)); // a little silly here, as noted above.
		double const vel (xd.norm());
		if (vel < 1.0e-3)
		{
			// avoid div by zero further down
			continue;
		}
		Vector const xdn (xd / vel);
		Vector const xdd (JJ * xidd.block (iq * cdim_, 0, cdim_ , 1));
		Matrix const prj (Matrix::Identity (2, 2) - xdn * xdn.transpose()); // hardcoded planar case
		Vector const kappa (prj * xdd / pow (vel, 2.0));

		//curvature
		curvature += kappa.norm();

		double acc_cost(0); //sum of cost function values from all obstacles at a given robot pose

		//Add obstacles
		for (int ii = 0; ii < OBS_.cols(); ii++)
		{
			Vector delta(xx - OBS_.block(0, ii, 2, 1));
			double const dist(delta.norm());
			if ((dist >= OBS_(2, ii)) || (dist < 1e-9))
				continue;
			double const cost(costGain_ * OBS_(2, ii) * pow(1.0 - dist / OBS_(2, ii), 3.0) / 3.0);
			delta *= - costGain_ *pow(1.0 - dist / OBS_(2, ii), 2.0) / dist;
			nabla_obs.block(iq * cdim_, 0, cdim_, 1) += JJ.transpose() * vel * (prj * delta - cost * kappa);
			acc_cost += cost;
		}

		//smoothness and obstacle costs
		F_smooth +=    pow(vel / dt_, 2.0);
		F_obs +=    acc_cost * (vel / dt_);
	}

	//compute cost functional (from smoothness and obstacle costs)
	U =  (F_obs + 0.5 * lambda_ * F_smooth ) / (nq_ + 1);
    U_cost = U;

    //normalize curvature to size
    curv =   curvature / (nq_ + 1);

	Vector dxi (Ainv_ * (nabla_obs + lambda_ * nabla_smooth));
	xi_ -= dxi / eta_;

	xi = xi_; //updated path

	res_ = dxi.norm() / eta_;

	return res_;
	// end of "the" CHOMP iteration
	//////////////////////////////////////////////////
}

double CHOMP::chompUpdateWithSearchRegion(Vector  &xi, double theta, double &U_cost, double &curv)
{

	// Before performing the iteration check if a path has been given

	if (PATH_INIT_==false)
	{
		cout << "A path was not initialized. Leaving CHOMP iteration! " << endl;
		return NAN;
	}

	//////////////////////////////////////////////////
	//Compute transformation between global and starting vehicle's reference frames (qs_).
	Eigen::Matrix4d T_global2local, T_local2global;
	T_local2global   <<   cos(theta), -sin(theta), 	0, 	qs_(0),
						  sin(theta),  cos(theta), 	0, 	qs_(1),
						  0, 					0, 	1, 	0,
						  0,					0,	0,	1;
	T_global2local = 	T_local2global.inverse();

	//Slope for lines defining search region
	double m  = 1.; // lines at qs_ with 45 deg of slope

	//////////////////////////////////////////////////
	// beginning of "the" CHOMP iteration

	double U(NAN); //cost functional value
	double F_obs(0);//obstacle functional value
	double F_smooth(0);//obstacle functional value
	double curvature(0); //curvature

	Vector nabla_smooth (AA_ * xi_ + bb_);
	Vector const & xidd (nabla_smooth); // indeed, it is the same in this formulation...

	Vector nabla_obs (Vector::Zero (xidim_));
	for (size_t iq (0); iq < nq_; ++iq)
	{
		Vector const qq (xi_.block (iq * cdim_, 0, cdim_, 1));
		Vector qd;
		if (0 == iq) {
		  qd = 0.5 * (xi_.block ((iq+1) * cdim_, 0, cdim_, 1) - qs_);
		}
		else if (iq == nq_ - 1) {
		  qd = 0.5 * (qe_ - xi_.block ((iq-1) * cdim_, 0, cdim_, 1));
		}
		else {
		  qd = 0.5 * (xi_.block ((iq+1) * cdim_, 0, cdim_, 1) - xi_.block ((iq-1) * cdim_, 0, cdim_, 1));;
		}

		// In this case, C and W are the same, Jacobian is identity.  We
		// still write more or less the full-fledged CHOMP expressions
		// (but we only use one body point) to make subsequent extension
		// easier.
		//
		Vector const & xx (qq);
		Vector const & xd (qd);
		Matrix const JJ (Matrix::Identity (2, 2)); // a little silly here, as noted above.
		double const vel (xd.norm());
		if (vel < 1.0e-3)
		{
			// avoid div by zero further down
			continue;
		}
		Vector const xdn (xd / vel);
		Vector const xdd (JJ * xidd.block (iq * cdim_, 0, cdim_ , 1));
		Matrix const prj (Matrix::Identity (2, 2) - xdn * xdn.transpose()); // hardcoded planar case
		Vector const kappa (prj * xdd / pow (vel, 2.0));
		double gain;

		//curvature
		curvature += kappa.norm();

		double acc_cost(0); //sum of cost function values from all obstacles at a given robot pose
		//Consider obstacles
		for (int ii = 0; ii < OBS_.cols(); ii++)
		{
			//Check search region, if it is outside the search region set a high obs. function gain
			Eigen::Vector4d xx_l, xx_g;
			xx_g << xx(0), xx(1), 0, 1;

			xx_l =  T_global2local *  xx_g;

			//allow only poses infront of initial position (avoids reverse motion paths)
			//if( ( xx_l(0) > 0 ) )
			//no reverse motion paths & inside a region defined by 2 lines centered at qs_
			if( ( xx_l(0) > 0 ) && ( xx_l(1) < m * xx_l(0) ) && (  xx_l(1) > - m * xx_l(0) ) )
			{
				//Inside desired search region
				gain = costGain_;
			}
			else
			{
				//Outside desired search region
				gain = 1000 * costGain_;
				cout << " Outside desired search region!!! " << endl;
			}

			Vector delta(xx - OBS_.block(0, ii, cdim_, 1));
			double const dist(delta.norm());
			if ((dist >= OBS_(2, ii)) || (dist < 1e-9))
				continue;
			double const cost(gain * OBS_(2, ii) * pow(1.0 - dist / OBS_(2, ii), 3.0) / 3.0);
			delta *= - gain *pow(1.0 - dist / OBS_(2, ii), 2.0) / dist;
			nabla_obs.block(iq * cdim_, 0, cdim_, 1) += JJ.transpose() * vel * (prj * delta - cost * kappa);
			acc_cost += cost;
		}

		//smoothness and obstacle costs
		F_smooth +=    pow(vel / dt_, 2.0);
		F_obs +=    acc_cost * (vel / dt_);

	}

	//compute cost functional (from smoothness and obstacle costs)
	U =  (F_obs + 0.5 * lambda_ * F_smooth ) / (nq_ + 1);
    U_cost = U;

    //normalize curvature to size
    curv =   curvature / (nq_ + 1);

	Vector dxi (Ainv_ * (nabla_obs + lambda_ * nabla_smooth));
	xi_ -= dxi / eta_;

	xi = xi_; //updated path

	++cter_; // increase iteration counter

	res_ = dxi.norm() / eta_;
	cout << "It No. " << cter_ << " Res.: " << dxi.norm() / eta_<< " Cost: " << U << " Curvature:" << curv << endl;

	//Detect local minimum
	if(cter_ > 1000 && res_ > 0.1)
	{
		cout << "LOCAL MINIMUM DETECTED! " << endl;
		for (size_t ii (0); ii < xidim_; ++ii)
		{
			 double noise = 0.1 * (- 50 + rand() % 100); //random noise between -5 to 5
			 xi_[ii] = xi_[ii] + noise;
			 cter_ = 0;

		}
	}
	else if (cter_ > 1000 && res_ < 0.01)
	{
		cout << "CONVERGENCE, RESETING COUNTER! " << endl;
		cter_ = 0;
	}

	return res_;

	// end of "the" CHOMP iteration
	//////////////////////////////////////////////////
}

void CHOMP::generatePath(Vector  &xi, Vector &ti)
{
	double err;
	for (size_t ii(0); ii < numIt_; ++ii)
	{
		err = CHOMP::chompIteration(xi, ti);
		cout << "err " << err  << "ii " << ii << endl;
		if (err < 0.01)
		{
			//it converged
			cter_ = 0;
			break;
		}
	}

}

//optimize path using with numIt_ iterations with serch region.
void CHOMP::generatePathWithSearchReg(Vector  &xi, double orientation, double &U_cost, double &curv)
{
	double err;
	for (size_t ii(0); ii < numIt_; ++ii)
	{
		err = CHOMP::chompUpdateWithSearchRegion(xi, orientation,U_cost,curv);
		if (err < 0.01)
		{
			//it converged
			cter_ = 0;
			break;
		}
	}

}

void CHOMP::addObstacle(double px, double py, double radius)
{
	OBS_.conservativeResize(obs_dim, OBS_.cols() + 1);
	OBS_.block(0, OBS_.cols() - 1, obs_dim, 1) << px, py, radius;
}

void CHOMP::setObstacles(Matrix obs)
{
	OBS_.resize (obs_dim, obs.cols());
	OBS_ = obs;

	for (size_t ii(0); ii < OBS_.cols(); ++ii)
	{
		int ox = (int) OBS_(0,ii);
		int oy = (int) OBS_(1,ii);
		int ro = (int) OBS_(2,ii);

		//cout << "ox: " << ox << " " << "oy: " << oy << " " << "ro: " << ro << endl;

		for (int row = (ox - ro); row <= ox + ro; row++)
		{
			for (int col = (oy - ro); col <= oy + ro; col++)
			{
				grid_(row, col) = 1;
			}
		}
	}

	//cout << "grid_  " << grid_ << endl;

	Matrix cost_init = Matrix::Zero(grid_.rows(), grid_.cols());
	for (int i=0; i< grid_.rows(); i++)
	{
		for (int j=0; j < grid_.cols(); j++)
		{
			if (!grid_(i,j))
				cost_init(i,j) = +INF;
		}
	}

	//cout << "cost_init  " << cost_init << endl;
	cost_init = ComputeDistanceTranform(cost_init);

			// Taking square root
	for (int x=0; x < grid_.rows(); x++)
	{
		for (int y=0; y < grid_.cols(); y++)
		{
				cost_init(x,y) = pow(cost_init(x,y), 0.5);
		}
	}

	Matrix posd_x = cost_init;
	//cout << posd_x << endl;

	// EDT of obstacle field Complement
	for (int i=0; i< grid_.rows(); i++)
	{
		for (int j=0; j < grid_.cols(); j++)
		{
			if (grid_(i,j))
				cost_init(i,j) = +INF;
			else
				cost_init(i,j) = 0;
		}
	}

	cost_init = ComputeDistanceTranform(cost_init);

		for (int x=0; x < grid_.rows(); x++)
	{
		for (int y=0; y < grid_.cols(); y++)
		{
				cost_init(x,y) = pow(cost_init(x,y), 0.5);
		}
	}

	Matrix negd_x = cost_init;
	//cout << negd_x << endl;

	Dx_ = posd_x - negd_x; // Discretized Signed Distance Field of obstacle
	//cout << Dx_ << endl;

	//cout << "Final Euclidean Distance Cost " << cost_init << endl;
	//cout << "Current Obstacles: \n" << OBS_ << endl;
}

// Sets an aribitrary value for xi_, qs_, qe_
void CHOMP::setPath(Vector  &qs, Vector &qe, double ts, double te, Vector &xi)
{
	xi_ = xi;
	qs_ = qs;
	qe_ = qe;

	ts_ = ts;
	te_ = te;

	//Sets gradient descent vectors and matrices with the initialized path
	CHOMP::initCHOMP();
}

//Sets qs_ and qe_ and initializes all points xi_ to qs_ (stacked to qs_)
void CHOMP::initStackedPath(Vector  &qs, Vector &qe, double ts, double te)
{
	qs_ = qs;
	qe_ = qe;

	ts_ = ts;
	te_ = te;

	xi_ = Vector::Zero (xidim_);
	ti_ = Vector::Zero (nq_);
	for (size_t ii (0); ii < nq_; ++ii)
	{
		xi_.block (cdim_ * ii, 0, cdim_, 1) = qs_ + (ii+1)*(qe_ - qs_)/(nq_ + 1);
		ti_(ii) = ts_ + (ii+1)*(te_ - ts_)/(nq_ + 1);
	}

	//Sets gradient descent vectors and matrices with the initialized path
	CHOMP::initCHOMP();

}

//Initializes path xi as a straight line conecting the starting point qs_ and the ending point qe_
void CHOMP::initStraightLinePath(Vector  &qs, Vector &qe)
{
	//initalize a new trajectory based on a direct line connecting qs to qe
	qs_ = qs;
	qe_ = qe;

	xi_ = Vector::Zero(xidim_);
	Vector dxi(cdim_);
	dxi << (qe_(0) - qs_(0)) / (nq_ - 1), (qe_(0) - qs_(0)) / (nq_ - 1);
	for (size_t ii(0); ii < nq_; ++ii)
	{
		xi_.block(cdim_ * ii, 0, cdim_, 1) = qs_ + ii * dxi;
	}

	//Sets gradient descent vectors and matrices with the initialized path
	CHOMP::initCHOMP();
}

// Gets xi_
void CHOMP::getPath(Vector &xi, Vector &ti)
{
	xi = xi_;
	ti = ti_;
}

void CHOMP::initCHOMP(void)
{

	// Initializes gradient descent vectors and matrices
	AA_ = Matrix::Zero (xidim_, xidim_);

	// For timing of trajectory
	BB_ = Matrix::Zero(nq_, nq_);

	for (size_t ii(0); ii < nq_; ++ii)
	{
		AA_.block (cdim_ * ii, cdim_ * ii, cdim_ , cdim_) = 2.0 * Matrix::Identity (cdim_, cdim_);
		BB_(ii,ii) = 2.0;
		if (ii > 0)
		{
			AA_.block (cdim_ * (ii-1), cdim_ * ii, cdim_ , cdim_) = -1.0 * Matrix::Identity (cdim_, cdim_);
			AA_.block (cdim_ * ii, cdim_ * (ii-1), cdim_ , cdim_) = -1.0 * Matrix::Identity (cdim_, cdim_);
			BB_(ii-1, ii) = -1.0;
			BB_(ii, ii-1) = -1.0;
		}
	}

	AA_ /= dt_ * dt_ * (nq_ + 1);
	BB_ /= dt_ * dt_ * (nq_ + 1);

	bb_ = Vector::Zero (xidim_);
	bb_.block (0,            0, cdim_, 1) = qs_;
	bb_.block (xidim_ - cdim_, 0, cdim_, 1) = qe_;
	bb_ /= - dt_ * dt_ * (nq_ + 1);

	Ainv_ = AA_.inverse();

	tt_ = Vector::Zero (nq_);
	tt_(0) = ts_;
	tt_(nq_-1) = te_;
	tt_ /= -dt_ * dt_ * (nq_ + 1);

	Binv_ = BB_.inverse();

	PATH_INIT_= true;
}


#pragma once
#include <utility>
#include <armadillo>
#include <kahansum.hpp>
#include <utility.hpp>

namespace arm_simu{

	template<typename RPType>
	struct IntegralPointerBuilder{
		typedef void (*Fptr)(RPType y,double t,RPType& dydt);
		typedef Fptr Pointer;
	};

	//These returns matrix and time vector
	static std::pair<arma::vec,arma::vec> EulerIntegrate(
		typename IntegralPointerBuilder<double>::Pointer equation,double x0,arma::vec t_frame
	){
		double dt=t_frame[1]-t_frame[0];
		std::pair<arma::vec,arma::vec> result;
		result.second=t_frame;

		arma::vec _result(t_frame.size());
		KahanSumParam<double> param;
		_result[0]=x0;
		for (int i=1;i<t_frame.size();i++){
			double slope;
			equation(_result[i-1],t_frame[i-1],slope);

			param.sum=_result[i-1];
			_result[i]=KahanSum::StepSummation<double>(dt*slope,param);

			//KahanSum::ResetStepSummation(param);
		}
		result.first=_result;

		return result;
	}

	static std::pair<arma::mat,arma::vec> EulerIntegrate(
		typename IntegralPointerBuilder<arma::rowvec>::Pointer equation,arma::rowvec x0,arma::vec t_frame
	){
		std::pair<arma::mat,arma::vec> result;

		result.second=t_frame;
		double dt=t_frame[1]-t_frame[0];

		arma::mat _result(t_frame.size(),x0.size());

		arm_simu::KahanSumParam<arma::rowvec> param{
			x0,
			arma::rowvec(x0.size()),
			arma::rowvec(x0.size()),
			arma::rowvec(x0.size())
		};
		param.y.fill(0);
		param.t.fill(0);
		param.c.fill(0);
		_result.row(0)=x0;
		arma::rowvec slope(x0.size());
		for (int i=1;i<t_frame.size();i++){

			equation(_result.row(i-1),t_frame[i-1],slope);

			_result.row(i)=_result.row(i-1)+dt*slope;
			param.sum=_result.row(i-1);
		}

		result.first=_result;

		return result;
	}

}


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// OuterExplicit.hpp
// This file is part of the Garamon for e3ga.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file OuterExplicit.hpp
/// \author Stephane Breuils, Vincent Nozick
/// \brief Explicit precomputed per grades outer product.

#ifndef E3GA_OUTER_PRODUCT_EXPLICIT_HPP__
#define E3GA_OUTER_PRODUCT_EXPLICIT_HPP__
#pragma once

#include <Eigen/Core>

#include "e3ga/Mvec.hpp"
#include "e3ga/Outer.hpp"

/*!
 * @namespace e3ga
 */
namespace e3ga {
    template<typename T> class Mvec;

    /// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 0
	template<typename T>
	void outer_0_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(0);
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void outer_0_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void outer_0_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 0) and mv2 (grade 3).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 0 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 3 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void outer_0_3(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1.coeff(0)*mv2;
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 1
	template<typename T>
	void outer_1_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void outer_1_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(1) - mv1.coeff(1)*mv2.coeff(0);
		mv3.coeffRef(1) +=  mv1.coeff(0)*mv2.coeff(2) - mv1.coeff(2)*mv2.coeff(0);
		mv3.coeffRef(2) +=  mv1.coeff(1)*mv2.coeff(2) - mv1.coeff(2)*mv2.coeff(1);
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 1) and mv2 (grade 2).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 1 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 2 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void outer_1_2(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(2) - mv1.coeff(1)*mv2.coeff(1) + mv1.coeff(2)*mv2.coeff(0);
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 2
	template<typename T>
	void outer_2_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 2) and mv2 (grade 1).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 2 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 1 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void outer_2_1(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3.coeffRef(0) +=  mv1.coeff(0)*mv2.coeff(2) - mv1.coeff(1)*mv2.coeff(1) + mv1.coeff(2)*mv2.coeff(0);
	}

	/// \brief Compute the outer product between two homogeneous multivectors mv1 (grade 3) and mv2 (grade 0).
	/// \tparam the type of value that we manipulate, either float or double or something.
	/// \param mv1 - the first homogeneous multivector of grade 3 represented as an Eigen::VectorXd
	/// \param mv2 - the second homogeneous multivector of grade 0 represented as a Eigen::VectorXd
	/// \param mv3 - the result of mv1^mv2, which is also a homogeneous multivector of grade 3
	template<typename T>
	void outer_3_0(const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv1, const Eigen::Matrix<T, Eigen::Dynamic, 1>& mv2, Eigen::Matrix<T, Eigen::Dynamic, 1>& mv3){
		mv3 += mv1*mv2.coeff(0);
	}

    template<typename T>
	std::array<std::array<std::function<void(const Eigen::Matrix<T, Eigen::Dynamic, 1> & , const Eigen::Matrix<T, Eigen::Dynamic, 1> & , Eigen::Matrix<T, Eigen::Dynamic, 1>&)>, 4>, 4> outerFunctionsContainer = {{
		{{outer_0_0<T>,outer_0_1<T>,outer_0_2<T>,outer_0_3<T>}},
		{{outer_1_0<T>,outer_1_1<T>,outer_1_2<T>,{}}},
		{{outer_2_0<T>,outer_2_1<T>,{},{}}},
		{{outer_3_0<T>,{},{},{}}}
	}};

}/// End of Namespace

#endif // E3GA_OUTER_PRODUCT_EXPLICIT_HPP__


// Copyright  (C)  2018  Craig Carignan <craigc at ssl dot umd dot edu>

// Version: 1.0
// Author: Craig Carignan <craigc at ssl dot umd dot edu>
// Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

// Inverse of a positive definite symmetric matrix times a vector
// based on LDL^T Decomposition
#ifndef LDL_SOLVER_EIGEN_HPP
#define LDL_SOLVER_EIGEN_HPP

#include <Eigen/Core>
#include "../solveri.hpp"

namespace KDL
{
    /**
     * \brief Solves the system of equations Aq = v for q via LDL decomposition,
     *        where A is a square positive definite matrix
     *
     * The algorithm factor A into the product of three matrices LDL^T, where L
     * is a lower triangular matrix and D is a diagonal matrix.  This allows q
     * to be computed without explicity inverting A.  Note that the LDL decomposition
     * is a variant of the classical Cholesky Decomposition that does not require
     * the computation of square roots.
     * Input parameters:
     * @param A matrix<double>(nxn)
     * @param v vector<double> n
     * @param vtmp vector<double> n [temp variable]
     * Output parameters:
     * @param L matrix<double>(nxn)
     * @param D vector<double> n
     * @param q vector<double> n
     * @return 0 if successful, E_SIZE_MISMATCH if dimensions do not match
     * References:
     * https://en.wikipedia.org/wiki/Cholesky_decomposition
     */
    int ldl_solver_eigen(const Eigen::MatrixXd& A, const Eigen::VectorXd& v, Eigen::MatrixXd& L, Eigen::VectorXd& D, Eigen::VectorXd& vtmp, Eigen::VectorXd& q);
}
#endif


#include <random>

#include <ros/ros.h>
#include <geometry_msgs/PoseArray.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

#include <Eigen/Dense>

int NUM_OBSTACLE;
double NEIGHBER_RANGE = 5.0;
double LAMBDA = 2.0;
double GAMMA = 0.35;
double N_PRIME = 3.0;
double N = 2.0;
double MAX_VELOCITY = 1.5;
double RELAXATION_TIME = 0.5;
double DESIRED_FORCE_FACTOR;
double SOCIAL_FORCE_FACTOR;
double HZ;
double INITIAL_KEEP_OUT_POSITION_X;
double INITIAL_KEEP_OUT_POSITION_Y;
double INITIAL_KEEP_OUT_RANGE;

std::string ROBOT_FRAME;
std::string WORLD_FRAME;
std::string OBS_FRAME;
double SIMULATION_SQUARE_LENGTH;

class SFMObstacle
{
public:
    SFMObstacle(void);

    unsigned int id;
    Eigen::Vector3d pose;
    Eigen::Vector3d velocity;
    Eigen::Vector3d current_goal;
    double preferred_speed;
    bool dodging_right;
    Eigen::Vector3d last_desired_force;
    Eigen::Vector3d last_social_force;
private:
};

SFMObstacle::SFMObstacle(void)
{
    id = 0;
    pose = Eigen::Vector3d::Zero();
    velocity = Eigen::Vector3d::Zero();
    current_goal = Eigen::Vector3d::Zero();
    preferred_speed = 1.0;
    dodging_right = true;
    last_desired_force = Eigen::Vector3d::Zero();
    last_social_force = Eigen::Vector3d::Zero();
}

std::ostream& operator<<(std::ostream& out, const SFMObstacle& sfmo)
{
    out << "id: " << sfmo.id << "\n"
        << "pose: " << sfmo.pose.transpose() << "\n"
        << "velocity: " << sfmo.velocity.transpose() << "\n"
        << "current speed: " << sfmo.velocity.norm() << "\n"
        << "current goal: " << sfmo.current_goal.transpose() << "\n"
        << "last desired force: " << sfmo.last_desired_force.transpose() << "\n"
        << "last social force: " << sfmo.last_social_force.transpose();
    return out;
}

Eigen::Vector3d get_next_goal(const SFMObstacle& agent)
{
    Eigen::Vector3d new_goal = Eigen::Vector3d::Zero();
    while(1){
        new_goal = Eigen::Vector3d::Random() * SIMULATION_SQUARE_LENGTH * 0.5;
        double distance_from_agent = (new_goal - agent.pose).norm();
        if(distance_from_agent > SIMULATION_SQUARE_LENGTH * 0.5){
            return new_goal;
        }
    }
}

Eigen::Vector3d get_social_force(const SFMObstacle& agent, const std::vector<SFMObstacle>& obstacles)
{
    Eigen::Vector3d force = Eigen::Vector3d::Zero();

    for(auto obstacle : obstacles){
        if(agent.id == obstacle.id){
            continue;
        }

        Eigen::Vector3d diff_vector = obstacle.pose - agent.pose;
        double distance = diff_vector.norm();
        if (distance > NEIGHBER_RANGE){
            continue;
        }
        // e_{ij}
        Eigen::Vector3d diff_direction = diff_vector.normalized();

        double other_angle = atan2(diff_direction(1), diff_direction(0));
        double agent_angle = atan2(agent.velocity(1), agent.velocity(0));
        double angle_fov = other_angle - agent_angle;
        angle_fov = atan2(sin(angle_fov), cos(angle_fov));
        if(fabs(angle_fov) > M_PI * 5.0 / 6.0){
            continue;
        }

        Eigen::Vector3d velocity_diff = agent.velocity - obstacle.velocity;

        // D_{ij}
        Eigen::Vector3d interaction_vector = LAMBDA * velocity_diff + diff_direction;
        // t_{ij}
        Eigen::Vector3d interaction_direction = interaction_vector.normalized();

        double interaction_angle = atan2(interaction_direction(1), interaction_direction(0));
        double theta_angle = other_angle - interaction_angle;
        theta_angle = atan2(sin(theta_angle), cos(theta_angle));

        double sign_of_theta = (fabs(theta_angle) == 0.00) ? 0.0 : theta_angle / fabs(theta_angle);

        double b = GAMMA * interaction_vector.norm();

        double force_velocity_amount = -std::exp(-diff_vector.norm() / b - (N_PRIME * b * theta_angle) * (N_PRIME * b * theta_angle));
        double force_angle_amount = -sign_of_theta * std::exp(-diff_vector.norm() / b - (N * b * theta_angle) * (N * b * theta_angle));

        Eigen::Vector3d force_velocity = force_velocity_amount * interaction_direction;

        Eigen::Vector3d interaction_direction_normal;
        if(agent.dodging_right){
            interaction_direction_normal << -interaction_direction(1), interaction_direction(0), interaction_direction(2);
        }else{
            interaction_direction_normal << interaction_direction(1), -interaction_direction(0), interaction_direction(2);
        }

        Eigen::Vector3d force_angle = force_angle_amount * interaction_direction_normal;

        force += force_velocity + force_angle;
    }
    return force;
}

Eigen::Vector3d get_desired_force(const SFMObstacle& agent)
{
    Eigen::Vector3d force = Eigen::Vector3d::Zero();
    // force = (agent.current_goal - agent.pose).normalized();
    Eigen::Vector3d desired_vector = (agent.current_goal - agent.pose).normalized();
    force = (agent.preferred_speed * desired_vector - agent.velocity) / RELAXATION_TIME;
    return force;
}

void set_obs_list(const std::vector<SFMObstacle>& obstacles, std::vector<geometry_msgs::TransformStamped>& obs_list)
{
    obs_list.clear();
    for(auto& obs : obstacles){
        geometry_msgs::TransformStamped tfs;
        tfs.header.stamp = ros::Time::now();
        tfs.header.frame_id = WORLD_FRAME;
        tfs.child_frame_id = OBS_FRAME + std::to_string(obs.id);
        tfs.transform.translation.x = obs.pose(0);
        tfs.transform.translation.y = obs.pose(1);
        tfs.transform.rotation = tf::createQuaternionMsgFromYaw(atan2(obs.velocity(1), obs.velocity(0)));
        obs_list.push_back(tfs);
    }
}

void simulate_one_step(std::vector<SFMObstacle>& obstacles, double dt)
{
    if(dt < 0.0){
        return;
    }
    // std::cout << "simulate one step" << std::endl;
    for(auto& obs : obstacles){
        // std::cout << obs << std::endl;
        Eigen::Vector3d relative_goal = obs.current_goal - obs.pose;
        if(relative_goal.norm() < 0.5){
            obs.current_goal = Eigen::Vector3d::Random() * SIMULATION_SQUARE_LENGTH * 0.5;
            obs.current_goal = get_next_goal(obs);
            obs.current_goal(2) = 0;
        }
        Eigen::Vector3d desired_force = get_desired_force(obs);
        obs.last_desired_force = desired_force;
        // std::cout << obs.last_desired_force.transpose() << std::endl;
        Eigen::Vector3d social_force = get_social_force(obs, obstacles);
        obs.last_social_force = social_force;
        // std::cout << obs.last_social_force.transpose() << std::endl;
        Eigen::Vector3d force = DESIRED_FORCE_FACTOR * desired_force + SOCIAL_FORCE_FACTOR * social_force;
        obs.velocity += obs.velocity + force * dt;
        // std::cout << obs.velocity.transpose() << std::endl;
        double speed = obs.velocity.norm();
        if(speed > obs.preferred_speed){
            obs.velocity = obs.velocity.normalized() * obs.preferred_speed;
        }
        obs.pose += obs.velocity * dt;
        // std::cout << obs.pose.transpose() << std::endl;
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "sfm_obstacle_simulator");
    std::cout << "=== sfm_obstacle_simulator ===" << std::endl;
    ros::NodeHandle nh;

    ros::NodeHandle local_nh("~");

    local_nh.param<std::string>("/dynamic_avoidance/ROBOT_FRAME", ROBOT_FRAME, {"base_link"});
    local_nh.param<std::string>("/dynamic_avoidance/WORLD_FRAME", WORLD_FRAME, {"map"});
    local_nh.param<std::string>("/dynamic_avoidance/OBSTACLES_FRAME", OBS_FRAME, {"obs"});
    local_nh.param<double>("HZ", HZ, {20.0});
    local_nh.param<int>("NUM_OBSTACLE", NUM_OBSTACLE, {20});
    local_nh.param<double>("DESIRED_FORCE_FACTOR", DESIRED_FORCE_FACTOR, {20.0});
    local_nh.param<double>("SOCIAL_FORCE_FACTOR", SOCIAL_FORCE_FACTOR, {100.0});
    local_nh.param<double>("SIMULATION_SQUARE_LENGTH", SIMULATION_SQUARE_LENGTH, {20.0});
    local_nh.param<double>("INITIAL_KEEP_OUT_POSITION_X", INITIAL_KEEP_OUT_POSITION_X, {-10.0});
    local_nh.param<double>("INITIAL_KEEP_OUT_POSITION_Y", INITIAL_KEEP_OUT_POSITION_Y, {0.0});
    local_nh.param<double>("INITIAL_KEEP_OUT_RANGE", INITIAL_KEEP_OUT_RANGE, {3.0});
    int SEED;
    local_nh.param<int>("SEED", SEED, {-1});

    std::cout << "HZ: " << HZ << std::endl;
    std::cout << "NUM_OBSTACLE: " << NUM_OBSTACLE << std::endl;
    std::cout << "DESIRED_FORCE_FACTOR: " << DESIRED_FORCE_FACTOR << std::endl;
    std::cout << "SOCIAL_FORCE_FACTOR: " << SOCIAL_FORCE_FACTOR << std::endl;
    std::cout << "SIMULATION_SQUARE_LENGTH: " << SIMULATION_SQUARE_LENGTH << std::endl;
    std::cout << "INITIAL_KEEP_OUT_POSITION_X: " << INITIAL_KEEP_OUT_POSITION_X << std::endl;
    std::cout << "INITIAL_KEEP_OUT_POSITION_Y: " << INITIAL_KEEP_OUT_POSITION_Y << std::endl;
    std::cout << "INITIAL_KEEP_OUT_RANGE: " << INITIAL_KEEP_OUT_RANGE << std::endl;
    std::cout << "SEED: " << SEED << std::endl;

    ros::Rate loop_rate(HZ);

    tf::TransformBroadcaster obs_broadcaster;

    // srand((unsigned int)time(0));// for Eigen
    // srand((unsigned int)SEED);// for Eigen
    srand(SEED < 0 ? (unsigned int)time(0) : (unsigned int)SEED);// for Eigen
    std::random_device rnd;
    std::mt19937 mt(SEED < 0 ? rnd() : SEED);
    std::uniform_real_distribution<> dist(1.0, MAX_VELOCITY);
    std::uniform_int_distribution<> dist_bool(0, 1);

    std::vector<SFMObstacle> obstacles;
    obstacles.clear();
    // initialize
    Eigen::Vector3d init_pos(INITIAL_KEEP_OUT_POSITION_X, INITIAL_KEEP_OUT_POSITION_Y, 0.0);
    for(int i=0;i<NUM_OBSTACLE;i++){
        SFMObstacle o;
        o.id = i;
        while(1){
            o.pose = Eigen::Vector3d::Random() * SIMULATION_SQUARE_LENGTH * 0.5;
            o.pose(2) = 0;
            if((o.pose - init_pos).norm() > INITIAL_KEEP_OUT_RANGE){
                break;
            }
        }
        o.velocity = Eigen::Vector3d::Random();
        o.velocity(2) = 0;
        o.current_goal = Eigen::Vector3d::Random() * SIMULATION_SQUARE_LENGTH * 0.5;
        o.current_goal(2) = 0;
        o.preferred_speed = dist(mt);
        o.dodging_right = dist(mt);
        obstacles.push_back(o);
        std::cout << o << std::endl;
    }

    tf::TransformListener listener;

    while(ros::ok()){
        bool robot_added_flag = false;
        try{
            static bool first_tf_flag = true;
            static double last_tf_time = 0;
            static Eigen::Vector3d last_robot_pose = Eigen::Vector3d::Zero();
            Eigen::Vector3d robot_velocity = Eigen::Vector3d::Zero();
            Eigen::Vector3d robot_pose = Eigen::Vector3d::Zero();

            geometry_msgs::PoseStamped robot_p;
            robot_p.header.frame_id = ROBOT_FRAME;
            robot_p.header.stamp = ros::Time(0);
            robot_p.pose.orientation = tf::createQuaternionMsgFromYaw(0);
            listener.transformPose(WORLD_FRAME, robot_p, robot_p);
            robot_pose(0) = robot_p.pose.position.x;
            robot_pose(1) = robot_p.pose.position.y;
            if(!first_tf_flag){
                double dt = robot_p.header.stamp.toSec() - last_tf_time;
                if(dt > 1e-4){
                    robot_velocity = (robot_pose - last_robot_pose) / dt;
                }else{
                    robot_velocity = Eigen::Vector3d::Zero();
                }
            }else{
                first_tf_flag = false;
            }
            SFMObstacle robot_o;
            robot_o.id = obstacles.size();
            robot_o.pose = robot_pose;
            robot_o.velocity = robot_velocity;
            robot_o.current_goal = robot_o.pose + robot_o.velocity * 5.0;// 5[s] after
            obstacles.push_back(robot_o);
            // std::cout << "robot is added" << std::endl;
            // std::cout << robot_o << std::endl;
            // std::cout << robot_p.header.stamp.toSec() - last_tf_time << std::endl;
            // std::cout << last_robot_pose.transpose() << std::endl;

            robot_added_flag = true;
            last_robot_pose = robot_pose;
            last_tf_time = robot_p.header.stamp.toSec();
        }catch(tf::TransformException& ex){
            std::cout << ex.what() << std::endl;
        }

        simulate_one_step(obstacles, 1 / HZ);

        if(robot_added_flag){
            obstacles.pop_back();
        }

        std::vector<geometry_msgs::TransformStamped> obs_list;
        obs_list.clear();
        set_obs_list(obstacles, obs_list);

        std::cout << "===" << std::endl;
        for(const auto obs : obstacles){
            std::cout << obs << std::endl;
        }

        obs_broadcaster.sendTransform(obs_list);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
};


// Copyright 2021 Apex.AI, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef STATE_ESTIMATION__KALMAN_FILTER__KALMAN_FILTER_HPP_
#define STATE_ESTIMATION__KALMAN_FILTER__KALMAN_FILTER_HPP_

#include <helper_functions/float_comparisons.hpp>
#include <motion_model/motion_model_interface.hpp>
#include <motion_model/stationary_motion_model.hpp>
#include <state_estimation/noise_model/noise_interface.hpp>
#include <state_estimation/state_estimation_interface.hpp>
#include <state_estimation/visibility_control.hpp>

#include <Eigen/LU>

#include <limits>
#include <vector>

namespace autoware
{
namespace common
{
namespace state_estimation
{
///
/// @brief      A Kalman filter implementation.
///
/// @tparam     MotionModelT  Type of the motion model.
/// @tparam     NoiseModelT   Type of the noise model.
///
template<typename MotionModelT, typename NoiseModelT>
class STATE_ESTIMATION_PUBLIC KalmanFilter
  : public StateEstimationInterface<KalmanFilter<MotionModelT, NoiseModelT>>
{
  static_assert(
    std::is_base_of<common::motion_model::MotionModelInterface<MotionModelT>, MotionModelT>::value,
    "\n\nMotion model must inherit from MotionModelInterface\n\n");
  static_assert(
    std::is_base_of<NoiseInterface<NoiseModelT>, NoiseModelT>::value,
    "\n\nNoise model must inherit from NoiseInterface\n\n");
  static_assert(
    std::is_same<typename MotionModelT::State, typename NoiseModelT::State>::value,
    "\n\nMotion model and noise model must have the same underlying state\n\n");

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using State = typename MotionModelT::State;
  using StateMatrix = typename State::Matrix;
  using MotionModel = MotionModelT;
  using NoiseModel = NoiseModelT;

  ///
  /// @brief      Constructs a new instance of a Kalman filter.
  ///
  /// @param[in]  motion_model        The motion model to be used to predict the movement.
  /// @param[in]  noise_model         The noise model that models the motion noise.
  /// @param[in]  initial_state       The initial state of the filter.
  /// @param[in]  initial_covariance  The initial state covariance.
  ///
  explicit KalmanFilter(
    MotionModelT motion_model,
    NoiseModelT noise_model,
    const State & initial_state,
    const StateMatrix & initial_covariance)
  : m_motion_model{motion_model},
    m_noise_model{noise_model},
    m_state{initial_state},
    m_covariance{initial_covariance} {}

  ///
  /// @brief      Predict next state.
  ///
  /// @param[in]  dt    Time difference to the time at which prediction is needed.
  ///
  /// @return     Predicted state.
  ///
  State crtp_predict(const std::chrono::nanoseconds & dt)
  {
    m_state = m_motion_model.predict(m_state, dt);
    const auto & motion_jacobian = m_motion_model.jacobian(m_state, dt);
    m_covariance =
      motion_jacobian * m_covariance * motion_jacobian.transpose() + m_noise_model.covariance(dt);
    return m_state;
  }

  ///
  /// @brief      Correct the predicted state given a measurement
  ///
  /// @note       It is expected that a prediction step was done right before the correction.
  ///
  /// @param[in]  measurement   Current measurement.
  ///
  /// @tparam     MeasurementT  Measurement type.
  ///
  /// @return     State corrected with the measurement.
  ///
  template<typename MeasurementT>
  State crtp_correct(const MeasurementT & measurement)
  {
    const auto expected_measurement = measurement.create_new_instance_from(m_state);
    const auto innovation = wrap_all_angles(measurement.state() - expected_measurement);
    const auto mapping_matrix = measurement.mapping_matrix_from(m_state);
    const auto innovation_covariance =
      mapping_matrix * m_covariance * mapping_matrix.transpose() + measurement.covariance();
    const auto kalman_gain =
      m_covariance * mapping_matrix.transpose() * innovation_covariance.inverse();
    m_state += kalman_gain * innovation.vector();
    m_state.wrap_all_angles();
    m_covariance = (State::Matrix::Identity() - kalman_gain * mapping_matrix) * m_covariance;
    return m_state;
  }

  ///
  /// @brief      Reset the state of the filter to a given state and covariance.
  ///
  /// @param[in]  state       The new state that overwrites one stored in the filter.
  /// @param[in]  covariance  The new covariance that overwrites one stored in the filter.
  ///
  void crtp_reset(const State & state, const StateMatrix & covariance)
  {
    m_state = state;
    m_covariance = covariance;
  }

  /// @brief      Get current state.
  auto & crtp_state() {return m_state;}
  /// @brief      Get current state.
  const auto & crtp_state() const {return m_state;}

  /// @brief      Get current covariance.
  auto & crtp_covariance() {return m_covariance;}
  /// @brief      Get current covariance.
  const auto & crtp_covariance() const {return m_covariance;}

private:
  /// Motion model used to predict the state forward.
  MotionModelT m_motion_model{};
  /// Noise model of the movement.
  NoiseModelT m_noise_model{};
  /// State of the tracked object.
  State m_state{};
  /// Covariance of the state of the tracked object.
  StateMatrix m_covariance{StateMatrix::Zero()};
};

///
/// @brief      A utility function that creates a Kalman filter.
///
/// @details    Mostly this is needed to avoid passing the template parameters explicitly and let
///             the compiler infer them from the objects passed into this function.
///
/// @param[in]  motion_model        A motion model.
/// @param[in]  noise_model         A noise model.
/// @param[in]  initial_state       The initial state
/// @param[in]  initial_covariance  The initial covariance
///
/// @tparam     MotionModelT        Type of the motion model.
/// @tparam     NoiseModelT         Type of the noise model.
///
/// @return     Returns a valid KalmanFilter instance.
///
template<typename MotionModelT, typename NoiseModelT>
auto make_kalman_filter(
  const MotionModelT & motion_model,
  const NoiseModelT & noise_model,
  const typename MotionModelT::State & initial_state,
  const typename MotionModelT::State::Matrix & initial_covariance)
{
  return KalmanFilter<MotionModelT, NoiseModelT>{
    motion_model, noise_model, initial_state, initial_covariance};
}

///
/// @brief      A utility function that creates a Kalman filter that is to be used for correction
///             only, i.e., this Kalman filter cannot predict the state forward in time.
///
/// @details    Mostly this is needed to avoid passing the template parameters explicitly and let
///             the compiler infer them from the objects passed into this function.
///
/// @param[in]  initial_state       The initial state
/// @param[in]  initial_covariance  The initial covariance
///
/// @tparam     StateT              { description }
/// @tparam     MotionModelT  Type of the motion model.
/// @tparam     NoiseModelT   Type of the noise model.
///
/// @return     Returns a valid KalmanFilter instance.
///
template<typename StateT>
auto make_correction_only_kalman_filter(
  const StateT & initial_state,
  const typename StateT::Matrix & initial_covariance)
{
  struct DummyNoise : public NoiseInterface<DummyNoise>
  {
    using State = StateT;
    typename State::Matrix crtp_covariance(const std::chrono::nanoseconds &) const
    {
      throw std::runtime_error(
              "Trying to use a correction-only Kalman filter to predict the state.");
    }
  };

  using MotionModel = common::motion_model::StationaryMotionModel<StateT>;

  return make_kalman_filter(
    MotionModel{}, DummyNoise{}, initial_state, initial_covariance);
}

///
/// @brief      A utility function that creates a Kalman filter from a vector of variances.
///
///             Mostly this is needed to avoid passing the template parameters explicitly and let
///             the compiler infer them from the objects passed into this function.
///
/// @param[in]  motion_model       A motion model.
/// @param[in]  noise_model        A noise model.
/// @param[in]  initial_state      Initial state.
/// @param[in]  initial_variances  Initial variances as a vector.
///
/// @tparam     MotionModelT       Type of the motion model.
/// @tparam     NoiseModelT        Type of the noise model.
///
/// @return     Returns a valid KalmanFilter instance.
///
template<typename MotionModelT, typename NoiseModelT>
auto make_kalman_filter(
  const MotionModelT & motion_model,
  const NoiseModelT & noise_model,
  const typename MotionModelT::State & initial_state,
  const std::vector<typename MotionModelT::State::Scalar> & initial_variances)
{
  using State = typename MotionModelT::State;
  if (initial_variances.size() != static_cast<std::size_t>(State::size())) {
    std::runtime_error(
      "Cannot create Kalman filter - dimensions mismatch. Provided " +
      std::to_string(initial_variances.size()) + " variances, but " +
      std::to_string(State::size()) + " required.");
  }
  typename State::Vector variances{State::Vector::Zero()};
  // A small enough epsilon to compare a floating point variance with zero.
  const auto epsilon = 5.0F * std::numeric_limits<common::types::float32_t>::epsilon();
  for (std::uint32_t i = 0; i < initial_variances.size(); ++i) {
    if (common::helper_functions::comparisons::abs_lte(initial_variances[i], 0.0F, epsilon)) {
      throw std::domain_error("Variances must be positive");
    }
    variances[static_cast<std::int32_t>(i)] = initial_variances[i] * initial_variances[i];
  }
  return KalmanFilter<MotionModelT, NoiseModelT>{
    motion_model, noise_model, initial_state, variances.asDiagonal()};
}

}  // namespace state_estimation
}  // namespace common
}  // namespace autoware

#endif  // STATE_ESTIMATION__KALMAN_FILTER__KALMAN_FILTER_HPP_


/*

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_COMPUTER_VISION_HOMOGRAPHY_HPP
#define PIC_COMPUTER_VISION_HOMOGRAPHY_HPP

#include <vector>
#include <random>
#include <stdlib.h>

#include "../base.hpp"

#include "../util/math.hpp"
#include "../util/eigen_util.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
    #include "../externals/Eigen/SVD"
    #include "../externals/Eigen/Geometry"
#else
    #include <Eigen/Dense>
    #include <Eigen/SVD>
    #include <Eigen/Geometry>
#endif

#endif

#include "../computer_vision/nelder_mead_opt_homography.hpp"

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief estimateHomography estimates an homography matrix H between image 1 to image 2
 * @param points0 is an array of points computed from image 1.
 * @param points1 is an array of points computed from image 2.
 * @return It returns the homography matrix H.
 */
PIC_INLINE Eigen::Matrix3d estimateHomography(std::vector< Eigen::Vector2f > &points0,
                                   std::vector< Eigen::Vector2f > &points1)
{
    Eigen::Matrix3d  H;

    if((points0.size() != points1.size()) || (points0.size() < 4)) {
        H.setZero();
        return H;
    }

    Eigen::Vector3f transform_0 = ComputeNormalizationTransform(points0);
    Eigen::Vector3f transform_1 = ComputeNormalizationTransform(points1);

    Eigen::Matrix3d mat_0 = getShiftScaleMatrix(transform_0);
    Eigen::Matrix3d mat_1 = getShiftScaleMatrix(transform_1);

    int n = int(points0.size());
    Eigen::MatrixXd A(n * 2, 9);

    //set up the linear system
    for(int i = 0; i < n; i++) {
        //transform coordinates for increasing stability of the system
        Eigen::Vector2f p0 = points0[i];
        Eigen::Vector2f p1 = points1[i];

        p0[0] = (p0[0] - transform_0[0]) / transform_0[2];
        p0[1] = (p0[1] - transform_0[1]) / transform_0[2];

        p1[0] = (p1[0] - transform_1[0]) / transform_1[2];
        p1[1] = (p1[1] - transform_1[1]) / transform_1[2];

        int j = i * 2;
        A(j, 0) = 0.0;
        A(j, 1) = 0.0;
        A(j, 2) = 0.0;
        A(j, 3) = p0[0];
        A(j, 4) = p0[1];
        A(j, 5) = 1.0;
        A(j, 6) = -p1[1] * p0[0];
        A(j, 7) = -p1[1] * p0[1];
        A(j, 8) = -p1[1];

        j++;

        A(j, 0) = p0[0];
        A(j, 1) = p0[1];
        A(j, 2) = 1.0;
        A(j, 3) = 0.0;
        A(j, 4) = 0.0;
        A(j, 5) = 0.0;
        A(j, 6) = -p1[0] * p0[0];
        A(j, 7) = -p1[0] * p0[1];
        A(j, 8) = -p1[0];
    }

    //solve the linear system
    Eigen::JacobiSVD< Eigen::MatrixXd > svd(A, Eigen::ComputeFullV);
    Eigen::MatrixXd V = svd.matrixV();

    n = int(V.cols()) - 1;

    //assign and transpose
    H(0, 0) = V(0, n);
    H(0, 1) = V(1, n);
    H(0, 2) = V(2, n);

    H(1, 0) = V(3, n);
    H(1, 1) = V(4, n);
    H(1, 2) = V(5, n);

    H(2, 0) = V(6, n);
    H(2, 1) = V(7, n);
    H(2, 2) = V(8, n);

    H = mat_1.inverse() * H * mat_0;
    return H / H(2, 2);
}

/**
 * @brief estimateHomographyRansac computes the homography such that: points1 = H * points0
 * @param points0
 * @param points1
 * @param inliers
 * @param maxIterations
 * @return
 */
PIC_INLINE Eigen::Matrix3d estimateHomographyRansac(std::vector< Eigen::Vector2f > &points0,
                                         std::vector< Eigen::Vector2f > &points1,
                                         std::vector< unsigned int > &inliers,
                                         unsigned int maxIterations = 100,
                                         double threshold = 4.0,
                                         unsigned int seed = 1)
{
    if(points0.size() < 5) {
        return estimateHomography(points0, points1);
    }

    Eigen::Matrix3d H;
    int nSubSet = 4;

    std::mt19937 m(seed);

    unsigned int n = int(points0.size());

    unsigned int *subSet = new unsigned int [nSubSet];

    inliers.clear();

    for(unsigned int i = 0; i < maxIterations; i++) {

        getRandomPermutation(m, subSet, nSubSet, n);

        std::vector< Eigen::Vector2f > sub_points0;
        std::vector< Eigen::Vector2f > sub_points1;

        for(int j = 0; j < nSubSet; j++) {
            sub_points0.push_back(points0[subSet[j]]);
            sub_points1.push_back(points1[subSet[j]]);
        }

        Eigen::Matrix3d tmpH = estimateHomography(sub_points0, sub_points1);

        //is it a good one?
        std::vector< unsigned int > tmp_inliers;

        for(unsigned int j = 0; j < n; j++) {
            Eigen::Vector3d point_hom = Eigen::Vector3d(points0[j][0], points0[j][1], 1.0);
            Eigen::Vector3d pp = tmpH * point_hom;
            pp /= pp[2];

            double dx = points1[j][0] - pp[0];
            double dy = points1[j][1] - pp[1];
            double squared_diff = (dx * dx) + (dy * dy);

            if(squared_diff < threshold) {
                tmp_inliers.push_back(j);
            }
        }

        //get the inliers
        if(tmp_inliers.size() > inliers.size()) {
            H = tmpH;
            inliers.clear();
            inliers.assign(tmp_inliers.begin(), tmp_inliers.end());
        }
    }

    //improve estimate with inliers only
    if(inliers.size() > 3) {
        #ifdef PIC_DEBUG
            printf("Better estimate using inliers only.\n");
        #endif

        std::vector< Eigen::Vector2f > sub_points0;
        std::vector< Eigen::Vector2f > sub_points1;

        for(unsigned int i = 0; i < inliers.size(); i++) {
            sub_points0.push_back(points0[inliers[i]]);
            sub_points1.push_back(points1[inliers[i]]);
        }

        H = estimateHomography(sub_points0, sub_points1);
    }

    return H;
}

/**
* @brief estimateHomographyRansac computes the homography such that: points1 = H * points0
* @param points0
* @param points1
* @param inliers
* @param maxIterations
* @return
*/
PIC_INLINE Eigen::Matrix3d estimateHomographyWithNonLinearRefinement(
                                         std::vector< Eigen::Vector2f > &points0,
                                         std::vector< Eigen::Vector2f > &points1,
                                         std::vector< unsigned int > &inliers,
                                         unsigned int maxIterationsRansac = 10000,
                                         double thresholdRansac = 2.5,
                                         unsigned int seedRansac = 1,
                                         unsigned int maxIterationsNonLinear = 10000,
                                         float thresholdNonLinear = 1e-5f
                                                          ) {

    Eigen::Matrix3d H = estimateHomographyRansac(points0, points1, inliers,
                                                 maxIterationsRansac, thresholdRansac,
                                                 seedRansac);

    NelderMeadOptHomography nmoh(points0, points1, inliers);
    float *H_array = getLinearArrayFromMatrix(H);
    nmoh.run(H_array, 8, thresholdNonLinear, maxIterationsNonLinear, H_array);
    H = getMatrix3dFromLinearArray(H_array);
    return H;
}

#endif // PIC_DISABLE_EIGEN

} // end namespace pic

#endif // PIC_COMPUTER_VISION_HOMOGRAPHY_HPP


/*
    utils.hpp
    Desc: Utility functions for mvee library
    @author Chris Larson, Cornell University
    @date (2017)
    @version 1.0
*/

#include <iostream>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include <string>
#include <thread>
#include <future>
#include <exception>
#include <Eigen/Dense>
#include <boost/algorithm/string.hpp>

int findIdx(Eigen::VectorXd& vec, double val)
{
    for (int i=0; i<vec.rows(); i++)
    {
        if (vec(i) == val)
        {
			return i;
        }
    }
    return -1;
}

std::vector<double> toStdVec(Eigen::VectorXd& fromX, double (*f)(double))
{
	std::vector<double> toX(fromX.size(), 0.);
	for (int i=0; i<fromX.size(); i++)
	{
		toX[i] = (*f)(fromX(i));
	}
	return toX;
}

std::vector<std::vector<double>> toStdMat(Eigen::MatrixXd& fromX, double (*f)(double))
{
	std::vector<std::vector<double>>
	toX(fromX.rows(), std::vector<double>(fromX.cols(), 0.));
	for (int i=0; i<fromX.rows(); i++)
	{
		for (int j=0; j<fromX.cols(); j++)
		{
			toX[i][j] = (*f)(fromX(i, j));
		}
	}
	return toX;
}

Eigen::MatrixXd readCSV(std::string file, char delim)
{
	// Get dimensions for Eigen::MatrixXd constructor
	int rows = 0;
	int cols = 0;
	std::ifstream in;
	in.open(file);
	if (in.is_open())
	{
		std::string s;
		while(!in.eof())
		{
			getline(in, s);
			if (s != "")
			{
				rows++;
				if (!cols)
				{
					cols = count(s.begin(), s.end(), delim) + 1;
				}
			}
		}
	}
	else
	{
		throw std::runtime_error("Error: Cannot open " + file + ".");
	}
	in.close();

	// Stream file into Eigen::MatrixXd container
	std::string line;
	Eigen::MatrixXd X = Eigen::MatrixXd(rows, cols);
	int row = 0;
	int col = 0;
	in.open(file);
	while (getline(in, line))
	{
		char* ptr = (char*) line.c_str();
		int len = line.length();
		col = 0;
		char* start = ptr;
		for (int i=0; i<len; i++)
		{
			if (ptr[i] == delim)
			{
				X(row, col++) = atof(start);
				start = ptr + i + 1;
			}
		}
		X(row, col) = atof(start);
		row++;
	}
	in.close();

	return X;
}


#include "optimizer.h"

#include <random>
#include <cassert>
#include <iomanip>
#include <iostream>
#include <sstream>

using namespace LiteMath;

struct OptComplex : public IOptimizer
{
  OptComplex(){}

  void         Init(const TriangleMesh& a_mesh, const Img& a_image) override;
  TriangleMesh Run (size_t a_numIters = 100) override;

  TriangleMesh g_mesh; ///<! global mesh optimized mesh
  Img          g_targetImage;
  size_t       g_iter = 0;
};

IOptimizer* CreateComplexOptimizer() { return new OptComplex; };

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

#include <Eigen/Dense>              // optimization methods
#define OPTIM_ENABLE_EIGEN_WRAPPERS // optimization methods
#include "optim.hpp"                // optimization methods

typedef Eigen::Matrix<double, Eigen::Dynamic, 1> EVector;

EVector VectorFromMesh(const TriangleMesh& a_mesh)
{
  EVector result( a_mesh.vertices.size()*2 + a_mesh.colors.size()*3 );
  size_t currPos = 0;
  for(size_t vertId=0; vertId< a_mesh.vertices.size(); vertId++, currPos+=2)
  {
    result[currPos+0] = a_mesh.vertices[vertId].x;
    result[currPos+1] = a_mesh.vertices[vertId].y;
  }
  for(size_t faceId=0; faceId < a_mesh.colors.size(); faceId++, currPos+=3)
  {
   result[currPos+0] = a_mesh.colors[faceId].x;
   result[currPos+1] = a_mesh.colors[faceId].y;
   result[currPos+2] = a_mesh.colors[faceId].z;
  }
  return result;
}

constexpr float alphaPos   = 0.1f;
constexpr float alphaColor = 0.00001f;

EVector VectorFromDMesh(const DTriangleMesh& a_mesh)
{
  EVector result(a_mesh.totalParams());
  size_t currPos = 0;
  for(int vertId=0; vertId< a_mesh.numVertices(); vertId++, currPos+=2)
  {
    result[currPos+0] = a_mesh.vertices()[vertId].x*alphaPos;
    result[currPos+1] = a_mesh.vertices()[vertId].y*alphaPos;
  }
  for(int faceId=0; faceId < a_mesh.numFaces(); faceId++, currPos+=3)
  {
    result[currPos+0] = a_mesh.faceColors()[faceId].x*alphaColor;
    result[currPos+1] = a_mesh.faceColors()[faceId].y*alphaColor;
    result[currPos+2] = a_mesh.faceColors()[faceId].z*alphaColor;
  }
  return result;
}

TriangleMesh MeshFromVector(const EVector& a_vec, const TriangleMesh& a_mesh)
{
  TriangleMesh result = a_mesh;
  size_t currPos = 0;
  for(size_t vertId=0; vertId< result.vertices.size(); vertId++, currPos+=2)
  {
    result.vertices[vertId].x = a_vec[currPos+0];
    result.vertices[vertId].y = a_vec[currPos+1];
  }
  for(size_t faceId=0; faceId < result.colors.size(); faceId++, currPos+=3)
  {
    result.colors[faceId].x = a_vec[currPos+0];
    result.colors[faceId].y = a_vec[currPos+1];
    result.colors[faceId].z = a_vec[currPos+2];
  }
  return result;
}

float EvalFunction(const EVector& vals_inp, EVector* grad_out, void* opt_data)
{
  OptComplex* pObj = (OptComplex*)opt_data;

  TriangleMesh mesh = MeshFromVector(vals_inp, pObj->g_mesh);

  constexpr int samples_per_pixel = 4;

  Img img(256, 256);
  std::mt19937 rng(1234);
  render(mesh, samples_per_pixel, rng, img);

  std::stringstream strOut;
  strOut  << "rendered_opt/render_" << std::setfill('0') << std::setw(4) << pObj->g_iter << ".bmp";
  save_img(img, strOut.str());

  Img adjoint(img.width, img.height, float3{1, 1, 1});
  float mse = MSEAndDiff(img, pObj->g_targetImage, adjoint);
  Img dx(img.width, img.height), dy(img.width, img.height); // actually not needed here

  DTriangleMesh d_mesh(mesh.vertices.size(), mesh.colors.size());
  d_render(mesh, adjoint, samples_per_pixel, img.width * img.height , rng, dx, dy, d_mesh);

  std::cout << "iter " << pObj->g_iter << ", error = " << mse << std::endl;
  (*grad_out) = VectorFromDMesh(d_mesh); // apply 2.0f*summ(I[x,y] - I_target[x,y]) to get correct gradient for target image
  pObj->g_iter++;
  return mse;
}

void OptComplex::Init(const TriangleMesh& a_mesh, const Img& a_image)
{
  g_mesh        = a_mesh;
  g_targetImage = a_image;
  g_iter        = 0;
}

TriangleMesh OptComplex::Run(size_t a_numIters)
{
  optim::algo_settings_t settings;
  settings.iter_max = a_numIters;
  settings.gd_settings.method = 0; // 0 for simple gradient descend, 6 ADAM
  settings.gd_settings.par_step_size      = 1.0; // initialization for ADAM
  settings.gd_settings.step_decay         = true;
  settings.gd_settings.step_decay_periods = a_numIters/10;
  settings.gd_settings.step_decay_val     = 0.75f;
  settings.opt_error_value                = 20.0f;

  EVector x = VectorFromMesh(g_mesh);
  bool success = optim::gd(x, &EvalFunction, this, settings);
  std::cout << "OptComplex, optimization is FINISHED!" << std::endl;

  return MeshFromVector(x, g_mesh);
}