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


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <util/test_macros.hpp>
#include <stdlib.h>
#include <vector>
#include <string>
#include <functional>
#include <random>

#include <unity/lib/gl_sframe.hpp>
#include <unity/lib/unity_sframe.hpp>
#include <unity/lib/variant_deep_serialize.hpp>
#include <unity/toolkits/pattern_mining/rule_mining.hpp>
#include <unity/toolkits/pattern_mining/fp_growth.hpp>

#include <cfenv>

using namespace turi;
using namespace turi::pattern_mining;

/*
 * Note the conviction score tests are commented out. Those cannot be right.
 * Some of the "true values" are NaNs or Infs
 */

/**
 *  Run tests.
*/
struct fp_rule_mining_test  {

    rule_list setupRuleList(void) {
      rule_list my_rules;

      rule rule1;
      rule1.LHS = {1};
      rule1.RHS = {9};
      rule1.LHS_support = 10;
      rule1.RHS_support = 7;
      rule1.total_support = 5;

      rule rule2;
      rule2.LHS = {2};
      rule2.RHS = {9};
      rule2.LHS_support = 10;
      rule2.RHS_support = 5;
      rule2.total_support = 5;

      rule rule3;
      rule3.LHS = {3};
      rule3.RHS = {9};
      rule3.LHS_support = 5;
      rule3.RHS_support = 7;
      rule3.total_support = 5;

      rule rule4;
      rule4.LHS = {4};
      rule4.RHS = {9};
      rule4.LHS_support = 5;
      rule4.RHS_support = 4;
      rule4.total_support = 1;

      my_rules.add_rule(rule1);
      my_rules.add_rule(rule2);
      my_rules.add_rule(rule3);
      my_rules.add_rule(rule4);
      my_rules.num_transactions = 20;
      return my_rules;
    }

  public:

    void testConfidenceScore(void){
      TS_ASSERT_DELTA(confidence_score(10, 7, 5), 5.0 / 10.0, 1e-7);
      TS_ASSERT_DELTA(confidence_score(10, 5, 5), 5.0 / 10.0, 1e-7);
      TS_ASSERT_DELTA(confidence_score(5, 7, 5), 5.0 / 5.0, 1e-7);
      TS_ASSERT_DELTA(confidence_score(5, 4, 1), 1.0 / 5.0, 1e-7);
    }
    void testLiftScore(void){
      TS_ASSERT_DELTA(lift_score(10, 7, 5), 5.0 / 70.0, 1e-7);
      TS_ASSERT_DELTA(lift_score(10, 5, 5), 5.0 / 50.0, 1e-7);
      TS_ASSERT_DELTA(lift_score(5, 7, 5), 5.0 / 35.0, 1e-7);
      TS_ASSERT_DELTA(lift_score(5, 4, 1), 1.0 / 20.0, 1e-7);
    }
//     void testConvictionScore(void){
//       TS_ASSERT_DELTA(conviction_score(10, 7/20.0, 5), (1 - 7.0/20.0) / (1 - 5.0/10.0), 1e-7);
//       TS_ASSERT_DELTA(conviction_score(10, 5/20.0, 5), (1 - 5.0/20.0) / (1 - 5.0/10.0), 1e-7);
//       TS_ASSERT_DELTA(conviction_score(5, 7/20.0, 5), (1 - 7.0/20.0) / (1 - 5.0/5.0), 1e-7);
//       TS_ASSERT_DELTA(conviction_score(5, 4/20.0, 1), (1 - 4.0/20.0) / (1 - 1.0/5.0), 1e-7);
//     }
    void testAllConfidenceScore(void){
      TS_ASSERT_DELTA(all_confidence_score(10, 7, 5), 5.0 / 10.0, 1e-7);
      TS_ASSERT_DELTA(all_confidence_score(10, 5, 5), 5.0 / 10.0, 1e-7);
      TS_ASSERT_DELTA(all_confidence_score(5, 7, 5), 5.0 / 7.0, 1e-7);
      TS_ASSERT_DELTA(all_confidence_score(5, 4, 1), 1.0 / 5.0, 1e-7);
    }
    void testMaxConfidenceScore(void){
      TS_ASSERT_DELTA(max_confidence_score(10, 7, 5), 5.0 / 7.0, 1e-7);
      TS_ASSERT_DELTA(max_confidence_score(10, 5, 5), 5.0 / 5.0, 1e-7);
      TS_ASSERT_DELTA(max_confidence_score(5, 7, 5), 5.0 / 5.0, 1e-7);
      TS_ASSERT_DELTA(max_confidence_score(5, 4, 1), 1.0 / 4.0, 1e-7);
    }
    void testKulcScore(void){
      TS_ASSERT_DELTA(kulc_score(10, 7, 5), 0.5 * ((5.0 / 10.0) + (5.0/7.0)), 1e-7);
      TS_ASSERT_DELTA(kulc_score(10, 5, 5), 0.5 * ((5.0 / 10.0) + (5.0/5.0)), 1e-7);
      TS_ASSERT_DELTA(kulc_score(5, 7, 5), 0.5 * ((5.0 / 5.0) + (5.0/7.0)), 1e-7);
      TS_ASSERT_DELTA(kulc_score(5, 4, 1), 0.5* ((1.0 / 5.0)+ (1.0/4.0)), 1e-7);
    }
    void testCosineScore(void){
      TS_ASSERT_DELTA(cosine_score(10, 7, 5), 5.0 / std::sqrt(70.0), 1e-7);
      TS_ASSERT_DELTA(cosine_score(10, 5, 5), 5.0 / std::sqrt(50.0), 1e-7);
      TS_ASSERT_DELTA(cosine_score(5, 7, 5), 5.0 / std::sqrt(35.0), 1e-7);
      TS_ASSERT_DELTA(cosine_score(5, 4, 1), 1.0 / std::sqrt(20.0), 1e-7);
    }

    void testConfScoreRules(void){
      rule_list my_rules = setupRuleList();

      std::vector<double> conf_scores = my_rules.score_rules(CONF_SCORE);
      std::vector<double> expected_scores = { 5.0 / 10.0,
                                              5.0 / 10.0,
                                              5.0 / 5.0,
                                              1.0 / 5.0 };
      for (size_t i = 0;i < expected_scores.size(); ++i) {
        TS_ASSERT_DELTA(conf_scores[i], expected_scores[i], 1e-7);
      }
    }
    void testLiftScoreRules(void){
      rule_list my_rules = setupRuleList();

      std::vector<double> lift_scores = my_rules.score_rules(LIFT_SCORE);
      std::vector<double> expected_scores = { 5.0 / 70.0 * 20.0,
                                              5.0 / 50.0 * 20.0,
                                              5.0 / 35.0 * 20.0,
                                              1.0 / 20.0 * 20.0};
      for (size_t i = 0;i < expected_scores.size(); ++i) {
        TS_ASSERT_DELTA(lift_scores[i], expected_scores[i], 1e-7);
      }
    }
//     void testConvictionScoreRules(void){
//       rule_list my_rules = setupRuleList();
//
//       std::vector<double> conviction_scores = my_rules.score_rules(CONVICTION_SCORE);
//       std::vector<double> expected_scores = { (1 - 7.0/20.0) / (1 - 5.0/10.0),
//                                               (1 - 5.0/20.0) / (1 - 5.0/10.0),
//                                               (1 - 7.0/20.0) / (1 - 5.0/5.0),
//                                               (1 - 4.0/20.0) / (1 - 1.0/5.0)};
//       for (size_t i = 0;i < expected_scores.size(); ++i) {
//         TS_ASSERT_DELTA(conviction_scores[i], expected_scores[i], 1e-7);
//       }
//     }
    void testAllConfScoreRules(void){
      rule_list my_rules = setupRuleList();

      std::vector<double> all_conf_scores = my_rules.score_rules(ALL_CONF_SCORE);
      std::vector<double> expected_scores = { 5.0 / 10.0,
                                              5.0 / 10.0,
                                              5.0 / 7.0,
                                              1.0 / 5.0};
      for (size_t i = 0;i < expected_scores.size(); ++i) {
        TS_ASSERT_DELTA(all_conf_scores[i], expected_scores[i], 1e-7);
      }
    }
    void testMaxConfScoreRules(void){
      rule_list my_rules = setupRuleList();

      std::vector<double> max_conf_scores = my_rules.score_rules(MAX_CONF_SCORE);
      std::vector<double> expected_scores = { 5.0 / 7.0,
                                              5.0 / 5.0,
                                              5.0 / 5.0,
                                              1.0 / 4.0};
      for (size_t i = 0;i < expected_scores.size(); ++i) {
        TS_ASSERT_DELTA(max_conf_scores[i], expected_scores[i], 1e-7);
      }
    }
    void testKulcScoreRules(void){
      rule_list my_rules = setupRuleList();

      std::vector<double> kulc_scores = my_rules.score_rules(KULC_SCORE);
      std::vector<double> expected_scores = { 0.5 * ((5.0 / 10.0) + (5.0/7.0)),
                                              0.5 * ((5.0 / 10.0) + (5.0/5.0)),
                                              0.5 * ((5.0 / 5.0) + (5.0/7.0)),
                                              0.5* ((1.0 / 5.0)+ (1.0/4.0))};
      for (size_t i = 0;i < expected_scores.size(); ++i) {
        TS_ASSERT_DELTA(kulc_scores[i], expected_scores[i], 1e-7);
      }
    }
    void testCosineScoreRules(void){
      rule_list my_rules = setupRuleList();

      std::vector<double> cosine_scores = my_rules.score_rules(COSINE_SCORE);
      std::vector<double> expected_scores = { 5.0 / std::sqrt(70.0),
                                              5.0 / std::sqrt(50.0),
                                              5.0 / std::sqrt(35.0),
                                              1.0 / std::sqrt(20.0)};
      for (size_t i = 0;i < expected_scores.size(); ++i) {
        TS_ASSERT_DELTA(cosine_scores[i], expected_scores[i], 1e-7);
      }
    }

    // Test extract_relevant_rules
    void testExtractRelevantRules(void) {
      std::vector<size_t> id_order = {2, 3, 1, 4, 0};
      gl_sframe closed_itemsets{{"itemsets", {flex_list{2, 1, 4},
                                              flex_list{2, 3},
                                              flex_list{2, 3, 1, 4},
                                              flex_list{3, 1},
                                              flex_list{2},
                                              flex_list{3},
                                              flex_list{1},
                                              flex_list{1, 0},
                                              flex_list{}}},
                              {"support", {20, 24, 12, 20, 30, 27, 23, 13, 40}}};
      fp_results_tree my_results = fp_results_tree(id_order);
      my_results.build_tree(closed_itemsets);

      std::vector<size_t> my_itemset = {1};
      auto my_rules = extract_relevant_rules(my_itemset, my_results);
      // std::cout << my_rules << std::endl;
      TS_ASSERT_EQUALS(my_rules.rules.size(), 7);
      TS_ASSERT_EQUALS(my_rules.get_LHS_supports(), \
          std::vector<size_t>({40, 23, 40, 23, 40, 23, 23}));
      TS_ASSERT_EQUALS(my_rules.get_RHS_supports(), \
          std::vector<size_t>({30, 20, 24, 12, 27, 27, 13}));
      TS_ASSERT_EQUALS(my_rules.get_total_supports(), \
          std::vector<size_t>({30, 20, 24, 12, 27, 20, 13}));
      TS_ASSERT_EQUALS(my_rules.num_transactions, 40); // Support of emptyset

      my_itemset = {4};
      my_rules = extract_relevant_rules(my_itemset, my_results);
      // std::cout << my_rules << std::endl;
      TS_ASSERT_EQUALS(my_rules.rules.size(), 8);
      TS_ASSERT_EQUALS(my_rules.get_LHS_supports(), \
          std::vector<size_t>({40, 20, 40, 20, 40, 40, 40, 40}));
      TS_ASSERT_EQUALS(my_rules.get_RHS_supports(), \
          std::vector<size_t>({30, 20, 24, 12, 27, 20, 23, 13}));
      TS_ASSERT_EQUALS(my_rules.get_total_supports(), \
          std::vector<size_t>({30, 20, 24, 12, 27, 20, 23, 13}));

      my_itemset = {5, 3, 4};
      my_rules = extract_relevant_rules(my_itemset, my_results);
      // std::cout << my_rules << std::endl;
      TS_ASSERT_EQUALS(my_rules.rules.size(), 7);
      TS_ASSERT_EQUALS(my_rules.get_LHS_supports(), \
          std::vector<size_t>({40, 20, 27, 12, 27, 40, 40}));
      TS_ASSERT_EQUALS(my_rules.get_RHS_supports(), \
          std::vector<size_t>({30, 20, 30, 20, 23, 23, 13}));
      TS_ASSERT_EQUALS(my_rules.get_total_supports(), \
          std::vector<size_t>({30, 20, 24, 12, 20, 23, 13}));

    }

    // test rule_list::get_top_k_rules()
    void testGetTopKRules(void) {
      std::vector<size_t> id_order = {2, 3, 1, 4, 0};
      gl_sframe closed_itemsets{{"itemsets", {flex_list{2, 1, 4},
                                              flex_list{2, 3},
                                              flex_list{2, 3, 1, 4},
                                              flex_list{3, 1},
                                              flex_list{2},
                                              flex_list{3},
                                              flex_list{1},
                                              flex_list{1, 0},
                                              flex_list{}}},
                              {"support", {20, 24, 12, 20, 30, 27, 23, 13, 40}}};
      fp_results_tree my_results = fp_results_tree(id_order);
      my_results.build_tree(closed_itemsets);

      std::vector<size_t> my_itemset = {2, 0};
      auto my_rules = extract_relevant_rules(my_itemset, my_results);
      // std::cout << my_rules;

      flex_list _conf_rules = my_rules.get_top_k_rules(5, CONF_SCORE);
      std::vector<flex_list> conf_rules;
      for (const auto& cr: _conf_rules) {
        conf_rules.push_back(cr);
      }

      // std::cout << conf_rules;
      TS_ASSERT_EQUALS(conf_rules.size(), 5);
      // First rule is 0 -> 1
      TS_ASSERT_EQUALS(conf_rules[0][3], 13); // Support of 0
      TS_ASSERT_EQUALS(conf_rules[0][4], 23); // Support of 1
      TS_ASSERT_EQUALS(conf_rules[0][5], 13); // Support of 0,1
      TS_ASSERT_DELTA(conf_rules[0][2], 1.0, 1e-6);

      // Second rule is 2 -> 3
      TS_ASSERT_EQUALS(conf_rules[1][3], 30);
      TS_ASSERT_EQUALS(conf_rules[1][4], 27);
      TS_ASSERT_EQUALS(conf_rules[1][5], 24);
      TS_ASSERT_DELTA(conf_rules[1][2], 0.8, 1e-6);

      // Third rule is [] -> 3
      TS_ASSERT_EQUALS(conf_rules[2][3], 40);
      TS_ASSERT_EQUALS(conf_rules[2][4], 27);
      TS_ASSERT_EQUALS(conf_rules[2][5], 27);
      TS_ASSERT_DELTA(conf_rules[2][2], 0.675, 1e-6);

      flex_list _cosine_rules = my_rules.get_top_k_rules(500, COSINE_SCORE);
      std::vector<flex_list> cosine_rules;
      for (const auto& cr: _cosine_rules) {
        cosine_rules.push_back(cr);
      }

      // std::cout << cosine_rules;
      TS_ASSERT_EQUALS(cosine_rules.size(), 7);
      TS_ASSERT_EQUALS(cosine_rules[0][0], flex_list{2});
      TS_ASSERT_EQUALS(cosine_rules[0][1], flex_list{3});
      TS_ASSERT_DELTA(cosine_rules[0][2], 24.0 / std::sqrt(30*27), 1e-3);
      TS_ASSERT_EQUALS(cosine_rules[1][0], flex_list{});
      TS_ASSERT_EQUALS(cosine_rules[1][1], flex_list{3});
      TS_ASSERT_DELTA(cosine_rules[1][2], 27.0 / std::sqrt(27*40), 1e-3);

    }
    // Test extract_top_k_rules()
    void testExtractTopKRules(void) {
      std::vector<size_t> id_order = {2, 3, 1, 4, 0};
      gl_sframe closed_itemsets{{"itemsets", {flex_list{2, 1, 4},
                                              flex_list{2, 3},
                                              flex_list{2, 3, 1, 4},
                                              flex_list{3, 1},
                                              flex_list{2},
                                              flex_list{3},
                                              flex_list{1},
                                              flex_list{1, 0},
                                              flex_list{}}},
                              {"support", {20, 24, 12, 20, 30, 27, 23, 13, 40}}};
      fp_results_tree my_results = fp_results_tree(id_order);
      my_results.build_tree(closed_itemsets);

      std::vector<size_t> my_itemset = {2, 0};

      flex_list _conf_rules = extract_top_k_rules(my_itemset, my_results, 5, CONF_SCORE);
      std::vector<flex_list> conf_rules;
      for (const auto& cr: _conf_rules) {
        conf_rules.push_back(cr);
      }

      // std::cout << conf_rules;
      TS_ASSERT_EQUALS(conf_rules.size(), 5);
      TS_ASSERT_DELTA(conf_rules[0][2], 1.0, 1e-6);
      TS_ASSERT_DELTA(conf_rules[1][2], 0.8, 1e-6);
      TS_ASSERT_DELTA(conf_rules[2][2], 0.675, 1e-6);

      flex_list _cosine_rules = extract_top_k_rules(my_itemset, my_results, 500, COSINE_SCORE);
      std::vector<flex_list> cosine_rules;
      for (const auto& cr: _cosine_rules) {
        cosine_rules.push_back(cr);
      }

      // std::cout << cosine_rules;
      TS_ASSERT_EQUALS(cosine_rules.size(), 7);
      TS_ASSERT_EQUALS(cosine_rules[0][0], flex_list{2});
      TS_ASSERT_EQUALS(cosine_rules[0][1], flex_list{3});
      TS_ASSERT_DELTA(cosine_rules[0][2], 24.0 / std::sqrt(30*27), 1e-3);
      TS_ASSERT_EQUALS(cosine_rules[1][0], flex_list{});
      TS_ASSERT_EQUALS(cosine_rules[1][1], flex_list{3});
      TS_ASSERT_DELTA(cosine_rules[1][2], 27.0 / std::sqrt(27*40), 1e-3);
    }

};

BOOST_FIXTURE_TEST_SUITE(_fp_rule_mining_test, fp_rule_mining_test)
BOOST_AUTO_TEST_CASE(testConfidenceScore) {
  fp_rule_mining_test::testConfidenceScore();
}
BOOST_AUTO_TEST_CASE(testLiftScore) {
  fp_rule_mining_test::testLiftScore();
}
BOOST_AUTO_TEST_CASE(testAllConfidenceScore) {
  fp_rule_mining_test::testAllConfidenceScore();
}
BOOST_AUTO_TEST_CASE(testMaxConfidenceScore) {
  fp_rule_mining_test::testMaxConfidenceScore();
}
BOOST_AUTO_TEST_CASE(testKulcScore) {
  fp_rule_mining_test::testKulcScore();
}
BOOST_AUTO_TEST_CASE(testCosineScore) {
  fp_rule_mining_test::testCosineScore();
}
BOOST_AUTO_TEST_CASE(testConfScoreRules) {
  fp_rule_mining_test::testConfScoreRules();
}
BOOST_AUTO_TEST_CASE(testLiftScoreRules) {
  fp_rule_mining_test::testLiftScoreRules();
}
BOOST_AUTO_TEST_CASE(testAllConfScoreRules) {
  fp_rule_mining_test::testAllConfScoreRules();
}
BOOST_AUTO_TEST_CASE(testMaxConfScoreRules) {
  fp_rule_mining_test::testMaxConfScoreRules();
}
BOOST_AUTO_TEST_CASE(testKulcScoreRules) {
  fp_rule_mining_test::testKulcScoreRules();
}
BOOST_AUTO_TEST_CASE(testCosineScoreRules) {
  fp_rule_mining_test::testCosineScoreRules();
}
BOOST_AUTO_TEST_CASE(testExtractRelevantRules) {
  fp_rule_mining_test::testExtractRelevantRules();
}
BOOST_AUTO_TEST_CASE(testGetTopKRules) {
  fp_rule_mining_test::testGetTopKRules();
}
BOOST_AUTO_TEST_CASE(testExtractTopKRules) {
  fp_rule_mining_test::testExtractTopKRules();
}
BOOST_AUTO_TEST_SUITE_END()


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is 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.
// __END_LICENSE__

#include <gtest/gtest_VW.h>
#include <boost/random.hpp>
#include <vw/config.h>
#include <vw/Math/Functors.h>

using namespace vw;
using namespace vw::math;

static const double DELTA = 1e-5;

template <class T1, class T2>
static bool is_of_type( T2 ) {
  return boost::is_same<T1,T2>::value;
}

TEST(Functors, Real) {
  ArgRealFunctor f;
  EXPECT_EQ( f(2.0), 2.0 );
  EXPECT_EQ( f(std::complex<double>(2.0,3.0)), 2.0 );
  EXPECT_TRUE( is_of_type<float>( f(float()) ) );
  EXPECT_TRUE( is_of_type<double>( f(double()) ) );
  EXPECT_TRUE( is_of_type<long double>( f((long double)(0)) ) );
  EXPECT_TRUE( is_of_type<int>( f(int()) ) );
  EXPECT_TRUE( is_of_type<float>( f(std::complex<float> ()) ) );
  EXPECT_TRUE( is_of_type<double>( f(std::complex<double> ()) ) );
  EXPECT_TRUE( is_of_type<long double>( f(std::complex<long double> ()) ) );
  EXPECT_TRUE( is_of_type<int>( f(std::complex<int> ()) ) );
}

TEST(Functors, Imag) {
  ArgImagFunctor f;
  EXPECT_EQ( f(2.0), 0.0 );
  EXPECT_EQ( f(std::complex<double>(2.0,3.0)), 3.0 );
  EXPECT_TRUE( is_of_type<float>( f(float()) ) );
  EXPECT_TRUE( is_of_type<double>( f(double()) ) );
  EXPECT_TRUE( is_of_type<long double>( f((long double)(0)) ) );
  EXPECT_TRUE( is_of_type<int>( f(int()) ) );
  EXPECT_TRUE( is_of_type<float>( f(std::complex<float>()) ) );
  EXPECT_TRUE( is_of_type<double>( f(std::complex<double>()) ) );
  EXPECT_TRUE( is_of_type<long double>( f(std::complex<long double>()) ) );
  EXPECT_TRUE( is_of_type<int>( f(std::complex<int>()) ) );
}

TEST(Functors, Abs) {
  ArgAbsFunctor f;
  EXPECT_EQ( f(2.0), 2.0 );
  EXPECT_EQ( f(-2.0), 2.0 );
  EXPECT_DOUBLE_EQ( f(std::complex<double>(3.0,4.0)), 5.0 );
  EXPECT_TRUE( is_of_type<float>( f(float()) ) );
  EXPECT_TRUE( is_of_type<double>( f(double()) ) );
  EXPECT_TRUE( is_of_type<long double>( f((long double)(0)) ) );
  EXPECT_TRUE( is_of_type<int>( f(int()) ) );
  EXPECT_TRUE( is_of_type<float>( f(std::complex<float>()) ) );
  EXPECT_TRUE( is_of_type<double>( f(std::complex<double>()) ) );
  EXPECT_TRUE( is_of_type<long double>( f(std::complex<long double>()) ) );
}

TEST(Functors, Conj) {
  ArgConjFunctor f;
  EXPECT_EQ( f(2.0), 2.0 );
  EXPECT_EQ( f(std::complex<double>(1.0,2.0)), std::complex<double>(1.0,-2.0) );
  EXPECT_TRUE( is_of_type<float>( f(float()) ) );
  EXPECT_TRUE( is_of_type<double>( f(double()) ) );
  EXPECT_TRUE( is_of_type<long double>( f((long double)(0)) ) );
  EXPECT_TRUE( is_of_type<int>( f(int()) ) );
  EXPECT_TRUE( is_of_type<std::complex<float> >( f(std::complex<float>()) ) );
  EXPECT_TRUE( is_of_type<std::complex<double> >( f(std::complex<double>()) ) );
  EXPECT_TRUE( is_of_type<std::complex<long double> >( f(std::complex<long double>()) ) );
  EXPECT_TRUE( is_of_type<std::complex<int> >( f(std::complex<int>()) ) );
}

TEST(Functors, Median){
  MedianAccumulator<double> V;
  V(8);
  V(9);
  V(3);
  V(5);

  // The median better be 6.5
  EXPECT_TRUE( V.value() == 6.5 );
}

TEST(Functors, StdDev){
  StdDevAccumulator<double> V;
  V(8);
  V(9);
  V(3);
  V(5);

  EXPECT_TRUE( V.value() == 2.3848480035423640366 );
}

TEST(Functors, DestructiveMedian){

  std::vector<double> V;
  V.push_back(8);
  V.push_back(9);
  V.push_back(3);
  V.push_back(5);

  // The median better be 6.5
  EXPECT_TRUE( destructive_median(V) == 6.5 );
}

TEST(Functors, DestructiveNmad){

  std::vector<double> V;
  V.push_back(8);
  V.push_back(9);
  V.push_back(3);
  V.push_back(5);

  EXPECT_TRUE( destructive_nmad(V) == 2.9652 );
}

TEST(Functors, DestructivePercentile){

  std::vector<double> V, W;
  V.push_back(8);
  V.push_back(9);
  V.push_back(3);
  V.push_back(5);

  // Start with new W each time, as it will be messed up

  W = V; EXPECT_TRUE( destructive_percentile(V,   0) == 3 );
  W = V; EXPECT_TRUE( destructive_percentile(V,  20) == 3 );
  W = V; EXPECT_TRUE( destructive_percentile(V,  25) == 3 );
  W = V; EXPECT_TRUE( destructive_percentile(V,  26) == 5 );
  W = V; EXPECT_TRUE( destructive_percentile(V,  45) == 5 );
  W = V; EXPECT_TRUE( destructive_percentile(V,  50) == 5 );
  W = V; EXPECT_TRUE( destructive_percentile(V,  51) == 8 );
  W = V; EXPECT_TRUE( destructive_percentile(V,  70) == 8 );
  W = V; EXPECT_TRUE( destructive_percentile(V,  75) == 8 );
  W = V; EXPECT_TRUE( destructive_percentile(V,  76) == 9 );
  W = V; EXPECT_TRUE( destructive_percentile(V, 100) == 9 );

}

#define TEST_UNARY_MATH_FUNCTOR(func,arg,result)                        \
  do {                                                                                \
    Arg##func##Functor f;                                                                                \
    EXPECT_NEAR( (result), f((float)(arg)),       DELTA );                                                        \
    EXPECT_NEAR( (result), f((double)(arg)),      DELTA );                                                       \
    EXPECT_NEAR( (result), f((long double)(arg)), DELTA );                                                  \
    EXPECT_TRUE( is_of_type<float>(f((float)(arg))) );                                                      \
    EXPECT_TRUE( is_of_type<double>(f((double)(arg))) );                                                    \
    EXPECT_TRUE( is_of_type<long double>(f((long double)(arg))) );                                          \
    EXPECT_TRUE( is_of_type<double>(f((int)(arg))) );                                                       \
  } while(false)

#define TEST_BINARY_MATH_FUNCTOR(func,arg1,arg2,result)                                                   \
  do {                                                                                \
    ArgArg##func##Functor f;                                                                              \
    EXPECT_NEAR( (result), f((float)(arg1),(float)(arg2)),             DELTA );                                         \
    EXPECT_NEAR( (result), f((double)(arg1),(double)(arg2)),           DELTA );                                       \
    EXPECT_NEAR( (result), f((long double)(arg1),(long double)(arg2)), DELTA );                             \
    EXPECT_TRUE( is_of_type<float>(f((float)(arg1),(float)(arg2))) );                                       \
    EXPECT_TRUE( is_of_type<double>(f((double)(arg1),(double)(arg2))) );                                    \
    EXPECT_TRUE( is_of_type<long double>(f((long double)(arg1),(long double)(arg2))) );                     \
    EXPECT_TRUE( is_of_type<double>( f((float)(arg1),(double)(arg2))) );                                    \
    EXPECT_TRUE( is_of_type<long double>( f((float)(arg1),(long double)(arg2))) );                          \
    EXPECT_TRUE( is_of_type<double>( f((int)(arg1),(int)(arg2))) );                                         \
    EXPECT_TRUE( is_of_type<float>( f((int)(arg1),(float)(arg2))) );                                        \
    EXPECT_NEAR( (result), ArgVal##func##Functor<float>(arg2)((float)(arg1)),             DELTA );                      \
    EXPECT_NEAR( (result), ArgVal##func##Functor<double>(arg2)((double)(arg1)),           DELTA );                    \
    EXPECT_NEAR( (result), ArgVal##func##Functor<long double>(arg2)((long double)(arg1)), DELTA );          \
    EXPECT_TRUE( is_of_type<float>(ArgVal##func##Functor<float>(arg2)((float)(arg1))) );                    \
    EXPECT_TRUE( is_of_type<double>(ArgVal##func##Functor<double>(arg2)((double)(arg1))) );                 \
    EXPECT_TRUE( is_of_type<long double>(ArgVal##func##Functor<long double>(arg2)((long double)(arg1))) );  \
    EXPECT_TRUE( is_of_type<double>( ArgVal##func##Functor<double>(arg2)((float)(arg1))) );                 \
    EXPECT_TRUE( is_of_type<long double>( ArgVal##func##Functor<long double>(arg2)((float)(arg1))) );       \
    EXPECT_TRUE( is_of_type<double>( ArgVal##func##Functor<int>((int)(arg2))((int)(arg1))) );               \
    EXPECT_TRUE( is_of_type<float>( ArgVal##func##Functor<float>(arg2)((int)(arg1))) );                     \
    EXPECT_NEAR( (result), ValArg##func##Functor<float>(arg1)((float)(arg2)),             DELTA );                      \
    EXPECT_NEAR( (result), ValArg##func##Functor<double>(arg1)((double)(arg2)),           DELTA );                    \
    EXPECT_NEAR( (result), ValArg##func##Functor<long double>(arg1)((long double)(arg2)), DELTA );          \
    EXPECT_TRUE( is_of_type<float>(ValArg##func##Functor<float>(arg1)((float)(arg2))) );                    \
    EXPECT_TRUE( is_of_type<double>(ValArg##func##Functor<double>(arg1)((double)(arg2))) );                 \
    EXPECT_TRUE( is_of_type<long double>(ValArg##func##Functor<long double>(arg1)((long double)(arg2))) );  \
    EXPECT_TRUE( is_of_type<double>( ValArg##func##Functor<float>(arg1)((double)(arg2))) );                 \
    EXPECT_TRUE( is_of_type<long double>( ValArg##func##Functor<float>(arg1)((long double)(arg2))) );       \
    EXPECT_TRUE( is_of_type<double>( ValArg##func##Functor<int>((int)(arg1))((int)(arg2))) );               \
    EXPECT_TRUE( is_of_type<float>( ValArg##func##Functor<int>((int)arg1)((float)(arg2))) );                \
  } while(false)

TEST(Functors, Acos)  { TEST_UNARY_MATH_FUNCTOR(Acos,  0.5, 1.04719755);   }
TEST(Functors, Asin)  { TEST_UNARY_MATH_FUNCTOR(Asin,  0.5, 0.523598776); }
TEST(Functors, Atan)  { TEST_UNARY_MATH_FUNCTOR(Atan,  1.0, 0.785398163); }
TEST(Functors, Cos)   { TEST_UNARY_MATH_FUNCTOR(Cos,   1.0, 0.540302306);  }
TEST(Functors, Sin)   { TEST_UNARY_MATH_FUNCTOR(Sin,   1.0, 0.841471);  }
TEST(Functors, Tan)   { TEST_UNARY_MATH_FUNCTOR(Tan,   1.0, 1.55741);   }
TEST(Functors, Cosh)  { TEST_UNARY_MATH_FUNCTOR(Cosh,  1.0, 1.54308);  }
TEST(Functors, Sinh)  { TEST_UNARY_MATH_FUNCTOR(Sinh,  1.0, 1.1752);   }
TEST(Functors, Tanh)  { TEST_UNARY_MATH_FUNCTOR(Tanh,  1.0, 0.761594); }
TEST(Functors, Exp)   { TEST_UNARY_MATH_FUNCTOR(Exp,   1.0, 2.718281);  }
TEST(Functors, Log)   { TEST_UNARY_MATH_FUNCTOR(Log,   2.0, 0.693147);  }
TEST(Functors, Log10) { TEST_UNARY_MATH_FUNCTOR(Log10, 2.0, 0.30103); }
TEST(Functors, Sqrt)  { TEST_UNARY_MATH_FUNCTOR(Sqrt,  2.0, 1.41421);  }
TEST(Functors, Ceil)  { TEST_UNARY_MATH_FUNCTOR(Ceil,  1.5, 2.0);
                        TEST_UNARY_MATH_FUNCTOR(Ceil, -1.5, -1.0);    }
TEST(Functors, Floor) { TEST_UNARY_MATH_FUNCTOR(Floor, 1.5, 1.0);
                        TEST_UNARY_MATH_FUNCTOR(Floor,-1.5, -2.0);   }

TEST(Functors, Atan2) { TEST_BINARY_MATH_FUNCTOR(Atan2,2.0,1.0,1.10715); }
TEST(Functors, Pow)   { TEST_BINARY_MATH_FUNCTOR(Pow,3.0,2.0,9.0);  }

#ifndef WIN32
TEST(Functors, Acosh) { TEST_UNARY_MATH_FUNCTOR(Acosh,1.5,0.962424); }
TEST(Functors, Asinh) { TEST_UNARY_MATH_FUNCTOR(Asinh,1.0,0.881374); }
TEST(Functors, Atanh) { TEST_UNARY_MATH_FUNCTOR(Atanh,0.5,0.549306); }

#ifdef VW_HAVE_EXP2
TEST(Functors, Exp2)  { TEST_UNARY_MATH_FUNCTOR(Exp2,1.0,2.0); }
#else
TEST(Functors, DISABLED_Exp2)  { }
#endif

#ifdef VW_HAVE_LOG2
TEST(Functors, Log2) { TEST_UNARY_MATH_FUNCTOR(Log2,2.0,1.0); }
#else
TEST(Functors, DISABLED_Log2) { }
#endif

#ifdef VW_HAVE_TGAMMA
TEST(Functors, Tgamma) { TEST_UNARY_MATH_FUNCTOR(Tgamma,1.5,0.886227); }
#else
TEST(Functors, DISABLED_Tgamma) { }
#endif

TEST(Functors, Expm1)    { TEST_UNARY_MATH_FUNCTOR(Expm1,1.0,1.718281);  }
TEST(Functors, Log1p)    { TEST_UNARY_MATH_FUNCTOR(Log1p,1.0,0.693147);  }
TEST(Functors, Cbrt)     { TEST_UNARY_MATH_FUNCTOR(Cbrt,2.0,1.25992);    }
TEST(Functors, Erf)      { TEST_UNARY_MATH_FUNCTOR(Erf,1.0,0.842701);    }
TEST(Functors, Erfc)     { TEST_UNARY_MATH_FUNCTOR(Erfc,1.0,0.157299);   }
TEST(Functors, Lgamma)   { TEST_UNARY_MATH_FUNCTOR(Lgamma,2.5,0.284683); }
TEST(Functors, Round)    { TEST_UNARY_MATH_FUNCTOR(Round,1.4,1.0);
                           TEST_UNARY_MATH_FUNCTOR(Round,1.5,2.0);       }
TEST(Functors, Trunc)    { TEST_UNARY_MATH_FUNCTOR(Trunc,1.5,1.0);
                           TEST_UNARY_MATH_FUNCTOR(Trunc,-1.5,-1.0);     }

TEST(Functors, Hypot)    { TEST_BINARY_MATH_FUNCTOR(Hypot,2.0,1.0,2.23607); }
TEST(Functors, Copysign) { TEST_BINARY_MATH_FUNCTOR(Copysign,3.0,-2.0,-3.0);
                           TEST_BINARY_MATH_FUNCTOR(Copysign,3.0,2.0,3.0); }
TEST(Functors, Fdim)     { TEST_BINARY_MATH_FUNCTOR(Fdim,3.0,2.0,1.0);
                           TEST_BINARY_MATH_FUNCTOR(Fdim,2.0,3.0,0.0); }

#endif

TEST(Functiors, Median) {
  MedianAccumulator<double> median;

  boost::mt19937 random_gen(42);
  boost::cauchy_distribution<double> cauchy(35,80);
  boost::variate_generator<boost::mt19937&,
    boost::cauchy_distribution<double> > generator(random_gen, cauchy);

  for ( uint16 i = 0; i < 50000; i++ )
    median( generator() );

  EXPECT_NEAR( median.value(), 35.0, 1.5 );
}


/* -*- 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;
	}

}