// Copyright 2020 Tier IV, 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.
//
//
// Author: v1.0 Yukihiro Saito
//

#include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp"

#include "multi_object_tracker/utils/utils.hpp"

#include <autoware_utils/autoware_utils.hpp>

#include <bits/stdc++.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>

PedestrianTracker::PedestrianTracker(
  const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object)
: Tracker(time, object.classification),
  logger_(rclcpp::get_logger("PedestrianTracker")),
  last_update_time_(time),
  z_(object.kinematics.pose_with_covariance.pose.position.z)
{
  object_ = object;

  // initialize params
  ekf_params_.use_measurement_covariance = false;
  float q_stddev_x = 0.0;                               // [m/s]
  float q_stddev_y = 0.0;                               // [m/s]
  float q_stddev_yaw = autoware_utils::deg2rad(20);     // [rad/s]
  float q_stddev_vx = autoware_utils::kmph2mps(5);      // [m/(s*s)]
  float q_stddev_wz = autoware_utils::deg2rad(20);      // [rad/(s*s)]
  float r_stddev_x = 0.4;                               // [m]
  float r_stddev_y = 0.4;                               // [m]
  float r_stddev_yaw = autoware_utils::deg2rad(30);     // [rad]
  float p0_stddev_x = 1.0;                              // [m/s]
  float p0_stddev_y = 1.0;                              // [m/s]
  float p0_stddev_yaw = autoware_utils::deg2rad(1000);  // [rad/s]
  float p0_stddev_vx = autoware_utils::kmph2mps(5);     // [m/(s*s)]
  float p0_stddev_wz = autoware_utils::deg2rad(10);     // [rad/(s*s)]
  ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0);
  ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0);
  ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0);
  ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0);
  ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0);
  ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
  ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
  ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);
  ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0);
  ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0);
  ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0);
  ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0);
  ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0);
  max_vx_ = autoware_utils::kmph2mps(10);  // [m/s]
  max_wz_ = autoware_utils::deg2rad(30);   // [rad/s]

  // initialize X matrix
  Eigen::MatrixXd X(ekf_params_.dim_x, 1);
  X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x;
  X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y;
  X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
  if (object.kinematics.has_twist) {
    X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x;
    X(IDX::WZ) = object.kinematics.twist_with_covariance.twist.angular.z;
  } else {
    X(IDX::VX) = 0.0;
    X(IDX::WZ) = 0.0;
  }

  // initialize P matrix
  Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
  if (
    !ekf_params_.use_measurement_covariance ||
    object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 ||
    object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] == 0.0 ||
    object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] == 0.0) {
    const double cos_yaw = std::cos(X(IDX::YAW));
    const double sin_yaw = std::sin(X(IDX::YAW));
    const double sin_2yaw = std::sin(2.0f * X(IDX::YAW));
    // Rotate the covariance matrix according to the vehicle yaw
    // because p0_cov_x and y are in the vehicle coordinate system.
    P(IDX::X, IDX::X) =
      ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw;
    P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw;
    P(IDX::Y, IDX::Y) =
      ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw;
    P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y);
    P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw;
    P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx;
    P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz;
  } else {
    P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
    P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y];
    P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
    P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X];
    P(IDX::YAW, IDX::YAW) =
      object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW];
    if (object.kinematics.has_twist_covariance) {
      P(IDX::VX, IDX::VX) =
        object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
      P(IDX::WZ, IDX::WZ) =
        object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW];
    } else {
      P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx;
      P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz;
    }
  }

  bounding_box_ = {0.5, 0.5, 1.7};
  cylinder_ = {0.3, 1.7};
  if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
    bounding_box_ = {
      object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z};
  } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) {
    cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z};
  }

  ekf_.init(X, P);
}

bool PedestrianTracker::predict(const rclcpp::Time & time)
{
  const double dt = (time - last_update_time_).seconds();
  bool ret = predict(dt, ekf_);
  if (ret) {
    last_update_time_ = time;
  }
  return ret;
}

bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const
{
  /*  == Nonlinear model ==
   *
   * x_{k+1}   = x_k + vx_k * cos(yaw_k) * dt
   * y_{k+1}   = y_k + vx_k * sin(yaw_k) * dt
   * yaw_{k+1} = yaw_k + (wz_k) * dt
   * vx_{k+1}  = vx_k
   * wz_{k+1}  = wz_k
   *
   */

  /*  == Linearized model ==
   *
   * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt,  0]
   *     [ 0, 1,  vx*cos(yaw)*dt, sin(yaw)*dt,  0]
   *     [ 0, 0,               1,           0, dt]
   *     [ 0, 0,               0,           1,  0]
   *     [ 0, 0,               0,           0,  1]
   */

  // X t
  Eigen::MatrixXd X_t(ekf_params_.dim_x, 1);  // predicted state
  ekf.getX(X_t);
  const double cos_yaw = std::cos(X_t(IDX::YAW));
  const double sin_yaw = std::sin(X_t(IDX::YAW));
  const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW));

  // X t+1
  Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1);                // predicted state
  X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt;  // dx = v * cos(yaw)
  X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt;  // dy = v * sin(yaw)
  X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt;      // dyaw = omega
  X_next_t(IDX::VX) = X_t(IDX::VX);
  X_next_t(IDX::WZ) = X_t(IDX::WZ);

  // A
  Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x);
  A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt;
  A(IDX::X, IDX::VX) = cos_yaw * dt;
  A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt;
  A(IDX::Y, IDX::VX) = sin_yaw * dt;
  A(IDX::YAW, IDX::WZ) = dt;

  // Q
  Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
  // Rotate the covariance matrix according to the vehicle yaw
  // because q_cov_x and y are in the vehicle coordinate system.
  Q(IDX::X, IDX::X) =
    (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt;
  Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt;
  Q(IDX::Y, IDX::Y) =
    (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt;
  Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y);
  Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt;
  Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt;
  Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz * dt * dt;
  Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
  Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1);

  if (!ekf.predict(X_next_t, A, Q)) {
    RCLCPP_WARN(logger_, "Cannot predict");
  }

  return true;
}

bool PedestrianTracker::measureWithPose(
  const autoware_auto_perception_msgs::msg::DetectedObject & object)
{
  constexpr int dim_y = 2;  // pos x, pos y depending on Pose output
  // double measurement_yaw =
  //   autoware_utils::normalizeRadian(tf2::getYaw(object.state.pose_covariance.pose.orientation));
  // {
  //   Eigen::MatrixXd X_t(ekf_params_.dim_x, 1);
  //   ekf_.getX(X_t);
  //   while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) {
  //     measurement_yaw = measurement_yaw + M_PI;
  //   }
  //   while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) {
  //     measurement_yaw = measurement_yaw - M_PI;
  //   }
  //   float theta = std::acos(
  //     std::cos(X_t(IDX::YAW)) * std::cos(measurement_yaw) +
  //     std::sin(X_t(IDX::YAW)) * std::sin(measurement_yaw));
  //   if (autoware_utils::deg2rad(60) < std::fabs(theta)) return false;
  // }

  /* Set measurement matrix */
  Eigen::MatrixXd Y(dim_y, 1);
  Y << object.kinematics.pose_with_covariance.pose.position.x,
    object.kinematics.pose_with_covariance.pose.position.y;

  /* Set measurement matrix */
  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x);
  C(0, IDX::X) = 1.0;  // for pos x
  C(1, IDX::Y) = 1.0;  // for pos y
  // C(2, IDX::YAW) = 1.0;  // for yaw

  /* Set measurement noise covariance */
  Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
  if (
    !ekf_params_.use_measurement_covariance ||
    object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 ||
    object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] == 0.0) {
    R(0, 0) = ekf_params_.r_cov_x;  // x - x
    R(0, 1) = 0.0;                  // x - y
    R(1, 1) = ekf_params_.r_cov_y;  // y - y
    R(1, 0) = R(0, 1);              // y - x
    // R(2, 2) = ekf_params_.r_cov_yaw;                        // yaw - yaw
  } else {
    R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
    R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y];
    // R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW];
    R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X];
    R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y];
    // R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW];
    // R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X];
    // R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y];
    // R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW];
  }
  if (!ekf_.update(Y, C, R)) {
    RCLCPP_WARN(logger_, "Cannot update");
  }

  // normalize yaw and limit vx, wz
  {
    Eigen::MatrixXd X_t(ekf_params_.dim_x, 1);
    Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x);
    ekf_.getX(X_t);
    ekf_.getP(P_t);
    X_t(IDX::YAW) = autoware_utils::normalizeRadian(X_t(IDX::YAW));
    if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) {
      X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_;
    }
    if (!(-max_wz_ <= X_t(IDX::WZ) && X_t(IDX::WZ) <= max_wz_)) {
      X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -max_wz_ : max_wz_;
    }
    ekf_.init(X_t, P_t);
  }

  // position z
  constexpr float gain = 0.9;
  z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z;

  return true;
}

bool PedestrianTracker::measureWithShape(
  const autoware_auto_perception_msgs::msg::DetectedObject & object)
{
  constexpr float gain = 0.9;
  if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
    bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.x;
    bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.y;
    bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z;
  } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) {
    cylinder_.width = gain * cylinder_.width + (1.0 - gain) * object.shape.dimensions.x;
    cylinder_.height = gain * cylinder_.height + (1.0 - gain) * object.shape.dimensions.z;
  } else {
    return false;
  }

  return true;
}

bool PedestrianTracker::measure(
  const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time)
{
  object_ = object;

  if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) {
    RCLCPP_WARN(
      logger_, "There is a large gap between predicted time and measurement time. (%f)",
      (time - last_update_time_).seconds());
  }

  measureWithPose(object);
  measureWithShape(object);

  return true;
}

bool PedestrianTracker::getTrackedObject(
  const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const
{
  object = utils::toTrackedObject(object_);
  object.object_id = getUUID();
  object.classification = getClassification();

  // predict kinematics
  KalmanFilter tmp_ekf_for_no_update = ekf_;
  const double dt = (time - last_update_time_).seconds();
  if (0.001 /*1msec*/ < dt) {
    predict(dt, tmp_ekf_for_no_update);
  }
  Eigen::MatrixXd X_t(ekf_params_.dim_x, 1);                // predicted state
  Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x);  // predicted state
  tmp_ekf_for_no_update.getX(X_t);
  tmp_ekf_for_no_update.getP(P);

  // position
  object.kinematics.pose_with_covariance.pose.position.x = X_t(IDX::X);
  object.kinematics.pose_with_covariance.pose.position.y = X_t(IDX::Y);
  object.kinematics.pose_with_covariance.pose.position.z = z_;
  // quaternion
  {
    double roll, pitch, yaw;
    tf2::Quaternion original_quaternion;
    tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion);
    tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw);
    tf2::Quaternion filtered_quaternion;
    filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW));
    object.kinematics.pose_with_covariance.pose.orientation.x = filtered_quaternion.x();
    object.kinematics.pose_with_covariance.pose.orientation.y = filtered_quaternion.y();
    object.kinematics.pose_with_covariance.pose.orientation.z = filtered_quaternion.z();
    object.kinematics.pose_with_covariance.pose.orientation.w = filtered_quaternion.w();
    object.kinematics.orientation_availability =
      autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN;
  }
  // position covariance
  constexpr double z_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
  constexpr double r_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
  constexpr double p_cov = 0.1 * 0.1;  // TOTODO(yukkysaito)DO Currently tentative
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X);
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y);
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X);
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y);
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov;
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov;
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov;
  object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] =
    P(IDX::YAW, IDX::YAW);

  // twist
  object.kinematics.twist_with_covariance.twist.linear.x = X_t(IDX::VX);
  object.kinematics.twist_with_covariance.twist.angular.z = X_t(IDX::WZ);
  // twist covariance
  constexpr double vy_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
  constexpr double vz_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
  constexpr double wx_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
  constexpr double wy_cov = 0.1 * 0.1;  // TODO(yukkysaito) Currently tentative
  object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX);
  object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov;
  object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov;
  object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW] =
    P(IDX::VX, IDX::WZ);
  object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X] =
    P(IDX::WZ, IDX::VX);
  object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov;
  object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov;
  object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] =
    P(IDX::WZ, IDX::WZ);

  // set shape
  if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
    object.shape.dimensions.x = bounding_box_.width;
    object.shape.dimensions.y = bounding_box_.length;
    object.shape.dimensions.z = bounding_box_.height;
  } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) {
    object.shape.dimensions.x = cylinder_.width;
    object.shape.dimensions.y = cylinder_.width;
    object.shape.dimensions.z = cylinder_.height;
  }

  return true;
}


#ifndef STAN_MATH_GPU_MULTIPLY_HPP
#define STAN_MATH_GPU_MULTIPLY_HPP
#ifdef STAN_OPENCL
#include <stan/math/gpu/matrix_gpu.hpp>
#include <stan/math/gpu/kernels/scalar_mul.hpp>
#include <stan/math/gpu/kernels/matrix_multiply.hpp>
#include <Eigen/Dense>

namespace stan {
namespace math {
/**
 * Multiplies the specified matrix on the GPU
 * with the specified scalar.
 *
 * @param A matrix
 * @param scalar scalar
 * @return matrix multipled with scalar
 */
inline matrix_gpu multiply(const matrix_gpu& A, const double scalar) {
  matrix_gpu temp(A.rows(), A.cols());
  if (A.size() == 0)
    return temp;
  try {
    opencl_kernels::scalar_mul(cl::NDRange(A.rows(), A.cols()), temp.buffer(),
                               A.buffer(), scalar, A.rows(), A.cols());
  } catch (const cl::Error& e) {
    check_opencl_error("multiply scalar", e);
  }
  return temp;
}

/**
 * Multiplies the specified matrix on the GPU
 * with the specified scalar.
 *
 * @param scalar scalar
 * @param A matrix
 * @return matrix multipled with scalar
 */
inline auto multiply(const double scalar, const matrix_gpu& A) {
  return multiply(A, scalar);
}

/**
 * Computes the product of the specified GPU matrices.
 *
 * Computes the matrix multiplication C[M, K] = A[M, N] x B[N, K]
 *
 * @param A first matrix
 * @param B second matrix
 * @return the product of the first and second matrix
 *
 * @throw std::invalid_argument if the
 *   number of columns in A and rows in B do not match
 */
inline auto multiply(const matrix_gpu& A, const matrix_gpu& B) {
  check_size_match("multiply (GPU)", "A.cols()", A.cols(), "B.rows()",
                   B.rows());
  matrix_gpu temp(A.rows(), B.cols());
  if (A.size() == 0 || B.size() == 0) {
    temp.zeros();
    return temp;
  }
  int local = opencl_kernels::matrix_multiply.make_functor.get_opts().at(
      "THREAD_BLOCK_SIZE");
  int Mpad = ((A.rows() + local - 1) / local) * local;
  int Npad = ((B.cols() + local - 1) / local) * local;
  int Kpad = ((A.cols() + local - 1) / local) * local;
  // padding the matrices so the dimensions are divisible with local
  // improves performance and readability because we can omit
  // if statements in the
  // multiply kernel
  matrix_gpu tempPad(Mpad, Npad);
  matrix_gpu Apad(Mpad, Kpad);
  matrix_gpu Bpad(Kpad, Npad);
  opencl_kernels::zeros(cl::NDRange(Mpad, Kpad), Apad.buffer(), Mpad, Kpad,
                        TriangularViewGPU::Entire);
  opencl_kernels::zeros(cl::NDRange(Kpad, Npad), Bpad.buffer(), Kpad, Npad,
                        TriangularViewGPU::Entire);
  Apad.sub_block(A, 0, 0, 0, 0, A.rows(), A.cols());
  Bpad.sub_block(B, 0, 0, 0, 0, B.rows(), B.cols());
  int wpt = opencl_kernels::matrix_multiply.make_functor.get_opts().at(
      "WORK_PER_THREAD");
  try {
    opencl_kernels::matrix_multiply(
        cl::NDRange(Mpad, Npad / wpt), cl::NDRange(local, local / wpt),
        Apad.buffer(), Bpad.buffer(), tempPad.buffer(), Apad.rows(),
        Bpad.cols(), Bpad.rows());
  } catch (cl::Error& e) {
    check_opencl_error("multiply", e);
  }
  // unpadding the result matrix
  temp.sub_block(tempPad, 0, 0, 0, 0, temp.rows(), temp.cols());
  return temp;
}

/**
 * Templated product operator for GPU matrices.
 *
 * Computes the matrix multiplication C[M, K] = A[M, N] x B[N, K].
 *
 * @param A A matrix or scalar
 * @param B A matrix or scalar
 * @return the product of the first and second arguments
 *
 * @throw std::invalid_argument if the
 *   number of columns in A and rows in B do not match
 */
inline matrix_gpu operator*(const matrix_gpu& A, const matrix_gpu& B) {
  return multiply(A, B);
}
inline matrix_gpu operator*(const matrix_gpu& B, const double scalar) {
  return multiply(B, scalar);
}
inline matrix_gpu operator*(const double scalar, const matrix_gpu& B) {
  return multiply(scalar, B);
}
}  // namespace math
}  // namespace stan

#endif
#endif


//
//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//

/*
  This file implements the function

  template <class EdgeListGraph, class Size, class P, class T, class R>
  bool bellman_ford_shortest_paths(EdgeListGraph& g, Size N,
     const bgl_named_params<P, T, R>& params)

 */

#ifndef BOOST_GRAPH_BELLMAN_FORD_SHORTEST_PATHS_HPP
#define BOOST_GRAPH_BELLMAN_FORD_SHORTEST_PATHS_HPP

#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/relax.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/concept/assert.hpp>

namespace boost {

  template <class Visitor, class Graph>
  struct BellmanFordVisitorConcept {
    void constraints() {
      BOOST_CONCEPT_ASSERT(( CopyConstructibleConcept<Visitor> ));
      vis.examine_edge(e, g);
      vis.edge_relaxed(e, g);
      vis.edge_not_relaxed(e, g);
      vis.edge_minimized(e, g);
      vis.edge_not_minimized(e, g);
    }
    Visitor vis;
    Graph g;
    typename graph_traits<Graph>::edge_descriptor e;
  };

  template <class Visitors = null_visitor>
  class bellman_visitor {
  public:
    bellman_visitor() { }
    bellman_visitor(Visitors vis) : m_vis(vis) { }

    template <class Edge, class Graph>
    void examine_edge(Edge u, Graph& g) {
      invoke_visitors(m_vis, u, g, on_examine_edge());
    }
    template <class Edge, class Graph>
    void edge_relaxed(Edge u, Graph& g) {
      invoke_visitors(m_vis, u, g, on_edge_relaxed());
    }
    template <class Edge, class Graph>
    void edge_not_relaxed(Edge u, Graph& g) {
      invoke_visitors(m_vis, u, g, on_edge_not_relaxed());
    }
    template <class Edge, class Graph>
    void edge_minimized(Edge u, Graph& g) {
      invoke_visitors(m_vis, u, g, on_edge_minimized());
    }
    template <class Edge, class Graph>
    void edge_not_minimized(Edge u, Graph& g) {
      invoke_visitors(m_vis, u, g, on_edge_not_minimized());
    }
  protected:
    Visitors m_vis;
  };
  template <class Visitors>
  bellman_visitor<Visitors>
  make_bellman_visitor(Visitors vis) {
    return bellman_visitor<Visitors>(vis);
  }
  typedef bellman_visitor<> default_bellman_visitor;

  template <class EdgeListGraph, class Size, class WeightMap,
            class PredecessorMap, class DistanceMap,
            class BinaryFunction, class BinaryPredicate,
            class BellmanFordVisitor>
  bool bellman_ford_shortest_paths(EdgeListGraph& g, Size N,
                         WeightMap weight,
                         PredecessorMap pred,
                         DistanceMap distance,
                         BinaryFunction combine,
                         BinaryPredicate compare,
                         BellmanFordVisitor v)
  {
    BOOST_CONCEPT_ASSERT(( EdgeListGraphConcept<EdgeListGraph> ));
    typedef graph_traits<EdgeListGraph> GTraits;
    typedef typename GTraits::edge_descriptor Edge;
    typedef typename GTraits::vertex_descriptor Vertex;
    BOOST_CONCEPT_ASSERT(( ReadWritePropertyMapConcept<DistanceMap, Vertex> ));
    BOOST_CONCEPT_ASSERT(( ReadablePropertyMapConcept<WeightMap, Edge> ));
    typedef typename property_traits<DistanceMap>::value_type D_value;
    typedef typename property_traits<WeightMap>::value_type W_value;

    typename GTraits::edge_iterator i, end;

    for (Size k = 0; k < N; ++k) {
      bool at_least_one_edge_relaxed = false;
      for (boost::tie(i, end) = edges(g); i != end; ++i) {
        v.examine_edge(*i, g);
        if (relax(*i, g, weight, pred, distance, combine, compare)) {
          at_least_one_edge_relaxed = true;
          v.edge_relaxed(*i, g);
        } else
          v.edge_not_relaxed(*i, g);
      }
      if (!at_least_one_edge_relaxed)
        break;
    }

    for (boost::tie(i, end) = edges(g); i != end; ++i)
      if (compare(combine(get(distance, source(*i, g)), get(weight, *i)),
                  get(distance, target(*i,g))))
      {
        v.edge_not_minimized(*i, g);
        return false;
      } else
        v.edge_minimized(*i, g);

    return true;
  }

  namespace detail {

    template<typename VertexAndEdgeListGraph, typename Size,
             typename WeightMap, typename PredecessorMap, typename DistanceMap,
             typename P, typename T, typename R>
    bool
    bellman_dispatch2
      (VertexAndEdgeListGraph& g,
       typename graph_traits<VertexAndEdgeListGraph>::vertex_descriptor s,
       Size N, WeightMap weight, PredecessorMap pred, DistanceMap distance,
       const bgl_named_params<P, T, R>& params)
    {
      typedef typename property_traits<DistanceMap>::value_type D;
      bellman_visitor<> null_vis;
      typedef typename property_traits<WeightMap>::value_type weight_type;
      typename graph_traits<VertexAndEdgeListGraph>::vertex_iterator v, v_end;
      for (boost::tie(v, v_end) = vertices(g); v != v_end; ++v) {
        put(distance, *v, (std::numeric_limits<weight_type>::max)());
        put(pred, *v, *v);
      }
      put(distance, s, weight_type(0));
      return bellman_ford_shortest_paths
               (g, N, weight, pred, distance,
                choose_param(get_param(params, distance_combine_t()),
                             closed_plus<D>()),
                choose_param(get_param(params, distance_compare_t()),
                             std::less<D>()),
                choose_param(get_param(params, graph_visitor),
                             null_vis)
                );
    }

    template<typename VertexAndEdgeListGraph, typename Size,
             typename WeightMap, typename PredecessorMap, typename DistanceMap,
             typename P, typename T, typename R>
    bool
    bellman_dispatch2
      (VertexAndEdgeListGraph& g,
       detail::error_property_not_found,
       Size N, WeightMap weight, PredecessorMap pred, DistanceMap distance,
       const bgl_named_params<P, T, R>& params)
    {
      typedef typename property_traits<DistanceMap>::value_type D;
      bellman_visitor<> null_vis;
      return bellman_ford_shortest_paths
               (g, N, weight, pred, distance,
                choose_param(get_param(params, distance_combine_t()),
                             closed_plus<D>()),
                choose_param(get_param(params, distance_compare_t()),
                             std::less<D>()),
                choose_param(get_param(params, graph_visitor),
                             null_vis)
                );
    }

    template <class EdgeListGraph, class Size, class WeightMap,
              class DistanceMap, class P, class T, class R>
    bool bellman_dispatch(EdgeListGraph& g, Size N,
                          WeightMap weight, DistanceMap distance,
                          const bgl_named_params<P, T, R>& params)
    {
      dummy_property_map dummy_pred;
      return
        detail::bellman_dispatch2
          (g,
           get_param(params, root_vertex_t()),
           N, weight,
           choose_param(get_param(params, vertex_predecessor), dummy_pred),
           distance,
           params);
    }
  } // namespace detail

  template <class EdgeListGraph, class Size, class P, class T, class R>
  bool bellman_ford_shortest_paths
    (EdgeListGraph& g, Size N,
     const bgl_named_params<P, T, R>& params)
  {
    return detail::bellman_dispatch
      (g, N,
       choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
       choose_pmap(get_param(params, vertex_distance), g, vertex_distance),
       params);
  }

  template <class EdgeListGraph, class Size>
  bool bellman_ford_shortest_paths(EdgeListGraph& g, Size N)
  {
    bgl_named_params<int,int> params(0);
    return bellman_ford_shortest_paths(g, N, params);
  }

  template <class VertexAndEdgeListGraph, class P, class T, class R>
  bool bellman_ford_shortest_paths
    (VertexAndEdgeListGraph& g,
     const bgl_named_params<P, T, R>& params)
  {
    BOOST_CONCEPT_ASSERT(( VertexListGraphConcept<VertexAndEdgeListGraph> ));
    return detail::bellman_dispatch
      (g, num_vertices(g),
       choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
       choose_pmap(get_param(params, vertex_distance), g, vertex_distance),
       params);
  }
} // namespace boost

#endif // BOOST_GRAPH_BELLMAN_FORD_SHORTEST_PATHS_HPP


#include "adafParameters.h"
#include "adafFunctions.h"
#include <math.h>
#include <iostream>
#include <fstream>
#include <fmath/physics.h>
#include <fparameters/parameters.h>
#include <boost/property_tree/ptree.hpp>
#include <fparameters/SpaceIterator.h>
#include "State.h"

using namespace std;

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

void adafParameters()
{
	ifstream adafFile, adafParams;

	adafFile.open("adafFile.txt"); adafParams.open("adafParameters.txt");
	adafFile >> nRaux;
	adafParams >> blackHoleMass >> accRateOut >> s >> magFieldPar >> alpha >> jAngMom >> delta >> rTr >> powerIndex;
	adafParams.close();

	logr.resize(nRaux,0.0);
	logTi.resize(nRaux,0.0);
	logTe.resize(nRaux,0.0);
	logv.resize(nRaux,0.0);

	for (size_t i=0;i<nRaux;i++) {
		adafFile >> logr[i] >> logTi[i] >> logTe[i] >> logv[i];
	}
	adafFile.close();

	blackHoleMass *= solarMass;
	schwRadius = 2.0*gravitationalConstant*blackHoleMass / cLight2;
	accRateOut = accRateOut * 1.39e18 * blackHoleMass/solarMass;
	cout << "Total outer accretion power = " << accRateOut*cLight2 << " erg s^{-1}" << endl;
	cout << "Total inner accretion power = " << accRateOut*pow(1.0/exp(logr.back()), s)*cLight2
		 << " erg s^{-1}" << endl;

	rTr = rTr * schwRadius;
	rOutCD = GlobalConfig.get<double>("rOutCD") * schwRadius;
	eMeanMolecularWeight = GlobalConfig.get<double>("mu_e");
	iMeanMolecularWeight = GlobalConfig.get<double>("mu_i");

	nR = GlobalConfig.get<int>("model.particle.default.dim.radius.samples");
	nE = GlobalConfig.get<int>("model.particle.photon.dim.energy.samples");
	nRcd = GlobalConfig.get<int>("model.particle.default.dim.radius_cd.samples");

    paso_r = pow(exp(logr.back())/exp(logr.front()),1.0/(nR));
	paso_rCD = pow(rOutCD/rTr,1.0/(nRcd));

	logMinEnergy = GlobalConfig.get<double>("model.particle.photon.dim.energy.min");
	logMaxEnergy = GlobalConfig.get<double>("model.particle.photon.dim.energy.max");

	inclination = GlobalConfig.get<double>("inclination");

    calculateComptonScatt = GlobalConfig.get<int>("calculateComptonScatt");
	height_method = GlobalConfig.get<int>("height_method");

    calculateThermal = GlobalConfig.get<int>("calculateThermal");
	numProcesses = GlobalConfig.get<int>("thermal.numProcesses");
    if (calculateThermal) {
        calculateComptonRedMatrix = GlobalConfig.get<int>("thermal.compton.calculateRedMatrix");
        comptonMethod = GlobalConfig.get<int>("thermal.compton.method");
        if (1) {

            nGammaCompton = GlobalConfig.get<size_t>("thermal.compton.redMatrixParams.nGammaCompton");
            nTempCompton = GlobalConfig.get<size_t>("thermal.compton.redMatrixParams.nTempCompton");
            nNuPrimCompton = GlobalConfig.get<size_t>("thermal.compton.redMatrixParams.nNuPrimCompton");
            nNuCompton = GlobalConfig.get<size_t>("thermal.compton.redMatrixParams.nNuCompton");
            gammaMinCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.gammaMinCompton");
            gammaMaxCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.gammaMaxCompton");
            tempMinCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.tempMinCompton");
            tempMaxCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.tempMaxCompton");
            nuPrimMinCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.nuPrimMinCompton");
            nuPrimMaxCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.nuPrimMaxCompton");
            nuMinCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.nuMinCompton");
            nuMaxCompton = GlobalConfig.get<double>("thermal.compton.redMatrixParams.nuMaxCompton");

            ofstream fileSizes;
            fileSizes.open("comptonRedMatrix/sizesVecCompton.dat",ios::out);
            fileSizes << nGammaCompton << "\t" << gammaMinCompton << "\t" << gammaMaxCompton << endl
                      << nTempCompton << "\t" << tempMinCompton << "\t" << tempMaxCompton << endl
                      << nNuPrimCompton << "\t" << nuPrimMinCompton << "\t" << nuPrimMaxCompton << endl
                      << nNuCompton << "\t" << nuMinCompton << "\t" << nuMaxCompton << endl;
            fileSizes.close();
        } else {
            ifstream fileSizes;
            fileSizes.open("comptonRedMatrix/sizesVecCompton.dat",ios::in);
            fileSizes >> nGammaCompton >> gammaMinCompton >> gammaMaxCompton
                      >> nTempCompton >> tempMinCompton >> tempMaxCompton
                      >> nNuPrimCompton >> nuPrimMinCompton >> nuPrimMaxCompton
                      >> nNuCompton >> nuMinCompton >> nuMaxCompton;
            fileSizes.close();
        }
        calculatePhotonDensityGap = GlobalConfig.get<int>("thermal.calculatePhotonDensityGap");
    }
	calculateJetEmission = GlobalConfig.get<int>("nonThermal.calculateJetEmission");

    calculateNonThermal = GlobalConfig.get<int>("calculateNonThermal");
    if (calculateNonThermal) {
		accMethod = GlobalConfig.get<double>("nonThermal.acc_method");
		calculateNTprotons = GlobalConfig.get<int>("nonThermal.protons");
		calculateNTelectrons = GlobalConfig.get<int>("nonThermal.electrons");
        calculateLosses = GlobalConfig.get<int>("nonThermal.calculateLosses");
		calculateFlare = GlobalConfig.get<int>("nonThermal.calculateFlare");
        calculateNTdistributions = GlobalConfig.get<int>("nonThermal.calculateDistributions");
        calculateNonThermalLum = GlobalConfig.get<int>("nonThermal.calculateLuminosities");
		calculateNonThermalHE = GlobalConfig.get<int>("nonThermal.calculateHighEnergyProcesses");
        calculateNeutronInj = GlobalConfig.get<int>("nonThermal.neutrons.calculateInjection");
		calculateNeutronDis = GlobalConfig.get<int>("nonThermal.neutrons.calculatePropagation");
		calculateJetDecay = GlobalConfig.get<int>("nonThermal.neutrons.calculateJetDecay");
		calculateSecondaries = GlobalConfig.get<int>("nonThermal.calculateSecondaries");
		calculateNeutrinos = GlobalConfig.get<int>("nonThermal.calculateNeutrinos");
	}
}


/**
 * @file kde_model_impl.hpp
 * @author Roberto Hueso
 *
 * Implementation of KDE Model.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_KDE_MODEL_IMPL_HPP
#define MLPACK_METHODS_KDE_MODEL_IMPL_HPP

// In case it hasn't been included yet.
#include "kde_model.hpp"

#include <boost/serialization/variant.hpp>

namespace mlpack {
namespace kde {

//! Initialize the KDEModel with the given parameters.
inline KDEModel::KDEModel(const double bandwidth,
                          const double relError,
                          const double absError,
                          const KernelTypes kernelType,
                          const TreeTypes treeType) :
  bandwidth(bandwidth),
  relError(relError),
  absError(absError),
  kernelType(kernelType),
  treeType(treeType)
{
  // Nothing to do.
}

// Copy constructor.
inline KDEModel::KDEModel(const KDEModel& other) :
  bandwidth(other.bandwidth),
  relError(other.relError),
  absError(other.absError),
  kernelType(other.kernelType),
  treeType(other.treeType)
{
  // Nothing to do.
}

// Move constructor.
inline KDEModel::KDEModel(KDEModel&& other) :
  bandwidth(other.bandwidth),
  relError(other.relError),
  absError(other.absError),
  kernelType(other.kernelType),
  treeType(other.treeType),
  kdeModel(std::move(other.kdeModel))
{
  // Reset other model.
  other.bandwidth = 1.0;
  other.relError = 0.05;
  other.absError = 0;
  other.kernelType = KernelTypes::GAUSSIAN_KERNEL;
  other.treeType = TreeTypes::KD_TREE;
  other.kdeModel = decltype(other.kdeModel)();
}

inline KDEModel& KDEModel::operator=(KDEModel other)
{
  boost::apply_visitor(DeleteVisitor(), kdeModel);
  bandwidth = other.bandwidth;
  relError = other.relError;
  absError = other.absError;
  kernelType = other.kernelType;
  treeType = other.treeType;
  kdeModel = std::move(other.kdeModel);
  return *this;
}

// Clean memory.
inline KDEModel::~KDEModel()
{
  boost::apply_visitor(DeleteVisitor(), kdeModel);
}

inline void KDEModel::BuildModel(arma::mat&& referenceSet)
{
  // Clean memory, if necessary.
  boost::apply_visitor(DeleteVisitor(), kdeModel);

  // Build the actual model.
  if (kernelType == GAUSSIAN_KERNEL && treeType == KD_TREE)
  {
    kdeModel = new KDEType<kernel::GaussianKernel, tree::KDTree>
        (relError, absError, kernel::GaussianKernel(bandwidth));
  }
  else if (kernelType == GAUSSIAN_KERNEL && treeType == BALL_TREE)
  {
    kdeModel = new KDEType<kernel::GaussianKernel, tree::BallTree>
        (relError, absError, kernel::GaussianKernel(bandwidth));
  }
  else if (kernelType == GAUSSIAN_KERNEL && treeType == COVER_TREE)
  {
    kdeModel = new KDEType<kernel::GaussianKernel, tree::StandardCoverTree>
        (relError, absError, kernel::GaussianKernel(bandwidth));
  }
  else if (kernelType == GAUSSIAN_KERNEL && treeType == OCTREE)
  {
    kdeModel = new KDEType<kernel::GaussianKernel, tree::Octree>
        (relError, absError, kernel::GaussianKernel(bandwidth));
  }
  else if (kernelType == GAUSSIAN_KERNEL && treeType == R_TREE)
  {
    kdeModel = new KDEType<kernel::GaussianKernel, tree::RTree>
        (relError, absError, kernel::GaussianKernel(bandwidth));
  }
  else if (kernelType == EPANECHNIKOV_KERNEL && treeType == KD_TREE)
  {
    kdeModel = new KDEType<kernel::EpanechnikovKernel, tree::KDTree>
        (relError, absError, kernel::EpanechnikovKernel(bandwidth));
  }
  else if (kernelType == EPANECHNIKOV_KERNEL && treeType == BALL_TREE)
  {
    kdeModel = new KDEType<kernel::EpanechnikovKernel, tree::BallTree>
        (relError, absError, kernel::EpanechnikovKernel(bandwidth));
  }
  else if (kernelType == EPANECHNIKOV_KERNEL && treeType == COVER_TREE)
  {
    kdeModel = new KDEType<kernel::EpanechnikovKernel, tree::StandardCoverTree>
        (relError, absError, kernel::EpanechnikovKernel(bandwidth));
  }
  else if (kernelType == EPANECHNIKOV_KERNEL && treeType == OCTREE)
  {
    kdeModel = new KDEType<kernel::EpanechnikovKernel, tree::Octree>
        (relError, absError, kernel::EpanechnikovKernel(bandwidth));
  }
  else if (kernelType == EPANECHNIKOV_KERNEL && treeType == R_TREE)
  {
    kdeModel = new KDEType<kernel::EpanechnikovKernel, tree::RTree>
        (relError, absError, kernel::EpanechnikovKernel(bandwidth));
  }
  else if (kernelType == LAPLACIAN_KERNEL && treeType == KD_TREE)
  {
    kdeModel = new KDEType<kernel::LaplacianKernel, tree::KDTree>
        (relError, absError, kernel::LaplacianKernel(bandwidth));
  }
  else if (kernelType == LAPLACIAN_KERNEL && treeType == BALL_TREE)
  {
    kdeModel = new KDEType<kernel::LaplacianKernel, tree::BallTree>
        (relError, absError, kernel::LaplacianKernel(bandwidth));
  }
  else if (kernelType == LAPLACIAN_KERNEL && treeType == COVER_TREE)
  {
    kdeModel = new KDEType<kernel::LaplacianKernel, tree::StandardCoverTree>
        (relError, absError, kernel::LaplacianKernel(bandwidth));
  }
  else if (kernelType == LAPLACIAN_KERNEL && treeType == OCTREE)
  {
    kdeModel = new KDEType<kernel::LaplacianKernel, tree::Octree>
        (relError, absError, kernel::LaplacianKernel(bandwidth));
  }
  else if (kernelType == LAPLACIAN_KERNEL && treeType == R_TREE)
  {
    kdeModel = new KDEType<kernel::LaplacianKernel, tree::RTree>
        (relError, absError, kernel::LaplacianKernel(bandwidth));
  }
  else if (kernelType == SPHERICAL_KERNEL && treeType == KD_TREE)
  {
    kdeModel = new KDEType<kernel::SphericalKernel, tree::KDTree>
        (relError, absError, kernel::SphericalKernel(bandwidth));
  }
  else if (kernelType == SPHERICAL_KERNEL && treeType == BALL_TREE)
  {
    kdeModel = new KDEType<kernel::SphericalKernel, tree::BallTree>
        (relError, absError, kernel::SphericalKernel(bandwidth));
  }
  else if (kernelType == SPHERICAL_KERNEL && treeType == COVER_TREE)
  {
    kdeModel = new KDEType<kernel::SphericalKernel, tree::StandardCoverTree>
        (relError, absError, kernel::SphericalKernel(bandwidth));
  }
  else if (kernelType == SPHERICAL_KERNEL && treeType == OCTREE)
  {
    kdeModel = new KDEType<kernel::SphericalKernel, tree::Octree>
        (relError, absError, kernel::SphericalKernel(bandwidth));
  }
  else if (kernelType == SPHERICAL_KERNEL && treeType == R_TREE)
  {
    kdeModel = new KDEType<kernel::SphericalKernel, tree::RTree>
        (relError, absError, kernel::SphericalKernel(bandwidth));
  }
  else if (kernelType == TRIANGULAR_KERNEL && treeType == KD_TREE)
  {
    kdeModel = new KDEType<kernel::TriangularKernel, tree::KDTree>
        (relError, absError, kernel::TriangularKernel(bandwidth));
  }
  else if (kernelType == TRIANGULAR_KERNEL && treeType == BALL_TREE)
  {
    kdeModel = new KDEType<kernel::TriangularKernel, tree::BallTree>
        (relError, absError, kernel::TriangularKernel(bandwidth));
  }
  else if (kernelType == TRIANGULAR_KERNEL && treeType == COVER_TREE)
  {
    kdeModel = new KDEType<kernel::TriangularKernel, tree::StandardCoverTree>
        (relError, absError, kernel::TriangularKernel(bandwidth));
  }
  else if (kernelType == TRIANGULAR_KERNEL && treeType == OCTREE)
  {
    kdeModel = new KDEType<kernel::TriangularKernel, tree::Octree>
        (relError, absError, kernel::TriangularKernel(bandwidth));
  }
  else if (kernelType == TRIANGULAR_KERNEL && treeType == R_TREE)
  {
    kdeModel = new KDEType<kernel::TriangularKernel, tree::RTree>
        (relError, absError, kernel::TriangularKernel(bandwidth));
  }

  // Train the model.
  TrainVisitor train(std::move(referenceSet));
  boost::apply_visitor(train, kdeModel);
}

// Perform bichromatic evaluation.
inline void KDEModel::Evaluate(arma::mat&& querySet, arma::vec& estimations)
{
  Log::Info << "Evaluating KDE..." << std::endl;
  DualBiKDE eval(std::move(querySet), estimations);
  boost::apply_visitor(eval, kdeModel);
}

// Perform monochromatic evaluation.
inline void KDEModel::Evaluate(arma::vec& estimations)
{
  Log::Info << "Evaluating KDE..." << std::endl;
  DualMonoKDE eval(estimations);
  boost::apply_visitor(eval, kdeModel);
}

// Clean memory.
inline void KDEModel::CleanMemory()
{
  boost::apply_visitor(DeleteVisitor(), kdeModel);
}

// Parameters for KDE evaluation.
DualMonoKDE::DualMonoKDE(arma::vec& estimations):
    estimations(estimations)
{}

// Default KDE evaluation.
template<typename KernelType,
         template<typename TreeMetricType,
                  typename TreeStatType,
                  typename TreeMatType> class TreeType>
void DualMonoKDE::operator()(KDETypeT<KernelType, TreeType>* kde) const
{
  if (kde)
  {
    kde->Evaluate(estimations);
    const size_t dimension = (kde->ReferenceTree())->Dataset().n_rows;
    KernelNormalizer::ApplyNormalizer<KernelType>(kde->Kernel(),
                                                  dimension,
                                                  estimations);
  }
  else
  {
    throw std::runtime_error("no KDE model initialized");
  }
}

// Parameters for KDE evaluation.
DualBiKDE::DualBiKDE(arma::mat&& querySet, arma::vec& estimations):
    dimension(querySet.n_rows),
    querySet(std::move(querySet)),
    estimations(estimations)
{}

// Default KDE evaluation.
template<typename KernelType,
         template<typename TreeMetricType,
                  typename TreeStatType,
                  typename TreeMatType> class TreeType>
void DualBiKDE::operator()(KDETypeT<KernelType, TreeType>* kde) const
{
  if (kde)
  {
    kde->Evaluate(std::move(querySet), estimations);
    KernelNormalizer::ApplyNormalizer<KernelType>(kde->Kernel(),
                                                  dimension,
                                                  estimations);
  }
  else
  {
    throw std::runtime_error("no KDE model initialized");
  }
}

// Parameters for Train.
TrainVisitor::TrainVisitor(arma::mat&& referenceSet) :
    referenceSet(std::move(referenceSet))
{}

// Default Train.
template<typename KernelType,
         template<typename TreeMetricType,
                  typename TreeStatType,
                  typename TreeMatType> class TreeType>
void TrainVisitor::operator()(KDEType<KernelType, TreeType>* kde) const
{
  Log::Info << "Training KDE model..." << std::endl;
  if (kde)
    kde->Train(std::move(referenceSet));
  else
    throw std::runtime_error("no KDE model initialized");
}

// Delete model.
template<typename KDEType>
void DeleteVisitor::operator()(KDEType* kde) const
{
  if (kde)
    delete kde;
}

// Mode of model.
template<typename KDEType>
KDEMode& ModeVisitor::operator()(KDEType* kde) const
{
  if (kde)
    return kde->Mode();
  else
    throw std::runtime_error("no KDE model initialized");
}

// Get mode of model.
KDEMode KDEModel::Mode() const
{
  return boost::apply_visitor(ModeVisitor(), kdeModel);
}

// Modify mode of model.
KDEMode& KDEModel::Mode()
{
  return boost::apply_visitor(ModeVisitor(), kdeModel);
}

// Serialize the model.
template<typename Archive>
void KDEModel::serialize(Archive& ar, const unsigned int /* version */)
{
  ar & BOOST_SERIALIZATION_NVP(bandwidth);
  ar & BOOST_SERIALIZATION_NVP(relError);
  ar & BOOST_SERIALIZATION_NVP(absError);
  ar & BOOST_SERIALIZATION_NVP(kernelType);
  ar & BOOST_SERIALIZATION_NVP(treeType);

  if (Archive::is_loading::value)
    boost::apply_visitor(DeleteVisitor(), kdeModel);

  ar & BOOST_SERIALIZATION_NVP(kdeModel);
}

} // namespace kde
} // namespace mlpack

#endif


/*
// Copyright 2018 University of Liege
//
// 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.
//
// Authors:
// - Adrien Crovato
*/

//// Main
// Link program modules (pre, solver, post)
//
// Variables accessed throughout the code
//
// *Geometry*
// - symY: defines symmetry about Y axis
// - sRef: reference surface of the full wing
//
// *Networks and Field*
// - bPan: (network of) body panels (structure)
// - wPan: (network of) wake panels (structure)
// - fPan: field panels (structure)
// - sp: sub-panels (structure)
//
// *Physics*
// - Minf: freestream Mach number
// - alpha: freestream angle of attack
// - vInf: freestream velocity vector
// - cL: lift coefficient
// - cD: drag coefficient
//
// *Numerics*
// - numC: numerical parameters (structure)

#include <iostream>
#include <Eigen/Dense>
#include <ctime>
#include "pre.h"
#include "solver.h"
#include "post.h"

#define ANSI_COLOR_BLUE    "\x1b[1;34m"
#define ANSI_COLOR_RESET   "\x1b[0m"

using namespace std;
using namespace Eigen;

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

    //// Variable definition
	// Geometry
    bool symY = 0;
    double sRef = 1;
    // Constants
    Numerical_CST numC = {};
	// Freestream
    double Minf = 0;
    double alpha = 0;
    Vector3d vInf(1.0, 0.0, 0.0);
	// Surface and wake panels
    // TODO: if several Networks are considered, use std::vector <network>
	Network bPan = {};
	Network wPan = {};
	// Field cells
	Field fPan = {};
    // Sub-panels
    Subpanel sp = {};
	// Forces
    double cL = 0, cD = 0;

    //// Begin flow
    // Hello World
    time_t now = time(0); // get time now
    char* localNow = ctime(&now);
    cout << ANSI_COLOR_BLUE;
    cout << "***********************************" << endl;
    cout << "**              \\_/              **" << endl;
    cout << "**     \\_______O(_)O_______/     **" << endl;
    cout << "**         _                     **" << endl;
    cout << "**        / \\   __  __  __       **" << endl;
    cout << "**       / _ \\ |__||__||  |      **" << endl;
    cout << "**      /_/ \\_\\|__ |  \\|__|      **" << endl;
    cout << "***********************************";
    cout << ANSI_COLOR_RESET << endl;
    cout << "Hi! My name is Aero v1.0-1809" << endl;
    cout << "Solver started on " << localNow << endl;

    // Check parameters
    if (argc != 3) {
        cout << "Incorrect number of parameters provided!" << endl;
        cout << "Usage: ./aero <pathToConfigFile(.cfg)> <pathToGridgridFile(.pts)>." << endl;
        return 1;
    }

    // Set time counter
    clock_t start = clock();
    clock_t startS = clock();

    // Pre-processing
    pre(argv, numC, symY, sRef, Minf, alpha, vInf, bPan, wPan, fPan, sp);
    clock_t endS = clock();
    cout << "Preprocessing time: " << (endS - startS) / (double) CLOCKS_PER_SEC << "s" << endl << endl;

    // Solver
    startS = clock();
    solver(numC, symY, sRef, alpha, vInf, Minf, bPan, wPan, fPan, sp, cL, cD);
    endS = clock();
    cout << "Solver time: " << (endS - startS) / (double) CLOCKS_PER_SEC << "s" << endl << endl;

    // Post-processing
    startS = clock();
    post(sRef, alpha, Minf, bPan, fPan, cL, cD);
    endS = clock();
    cout << "Postprocessing time: " << (endS - startS) / (double) CLOCKS_PER_SEC << "s" << endl << endl;

    // Total time
    clock_t end = clock();
    cout << "***** Run summary *****" << endl;
    cout << "Total time: " << (end - start) / (double) CLOCKS_PER_SEC << "s" << endl << endl;

    return 0;
}


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#ifndef __multicontact_api_trajectories_cubic_hermite_spline_hpp__
#define __multicontact_api_trajectories_cubic_hermite_spline_hpp__

#include "multicontact-api/trajectories/fwd.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"
#include "multicontact-api/serialization/archive.hpp"

#include <Eigen/Dense>

namespace multicontact_api
{
  namespace trajectories
  {
    namespace detail
    {
//      template<typename _Scalar, int _dim>
    }

    template<typename _Scalar, int _dim>
    struct CubicHermiteSplineTpl
    : public serialization::Serializable< CubicHermiteSplineTpl<_Scalar,_dim> >
    {
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      typedef _Scalar Scalar;
      enum { dim = _dim };
      typedef Eigen::Matrix<Scalar,dim,Eigen::Dynamic> MatrixDx;
      typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1> VectorX;
      typedef Eigen::Matrix<Scalar,dim,1> VectorD;
      typedef Eigen::DenseIndex Index;

      CubicHermiteSplineTpl()
      : m_absicca()
      , m_points(_dim,0)
      , m_derivatives(_dim,0)
      , m_ds()
      {}

      CubicHermiteSplineTpl(const Index size)
      : m_absicca(size)
      , m_points(_dim,size)
      , m_derivatives(_dim,size)
      , m_ds((size-1>0)?size-1:0)
      {}

      template<typename VectorDerived, typename MatrixDerived>
      CubicHermiteSplineTpl(const Eigen::MatrixBase<VectorDerived> & absicca,
                            const Eigen::MatrixBase<MatrixDerived> & points,
                            const Eigen::MatrixBase<MatrixDerived> & derivatives
                            )
      : m_absicca(absicca)
      , m_points(points)
      , m_derivatives(derivatives)
      , m_ds(absicca.size()-1)
      {
        //EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(MatrixDerived,MatrixDx);
        //EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(VectorDerived,VectorX);

        assert(points.cols() == derivatives.cols() && "Points and derivatives must have the same number of columns.");
        assert(points.cols() == absicca.size() && "Points and times must have the same dimension.");

        compute();
        assert(check());
      }

      template<typename OtherScalar>
      CubicHermiteSplineTpl(const CubicHermiteSplineTpl<OtherScalar,dim> & other)
      : m_absicca(other.m_absicca)
      , m_points(other.m_points)
      , m_derivatives(other.m_derivatives)
      {
        compute();
      }

      static CubicHermiteSplineTpl Constant(const VectorD & point)
      {
        MatrixDx points(point.size(),2), derivatives(MatrixDx::Zero(point.size(),2));
        points << point, point;

        VectorX absicca(2); absicca << 0., 1.;

        return CubicHermiteSplineTpl(absicca,points,derivatives);
      }

      template<typename OtherScalar>
      bool operator==(const CubicHermiteSplineTpl<OtherScalar,dim> & other) const
      {
        return
        m_absicca == other.m_absicca
        && m_points == other.m_points
        && m_derivatives == other.m_derivatives
        && m_ds == other.m_ds
        ;
      }

      template<typename OtherScalar>
      CubicHermiteSplineTpl operator+(const CubicHermiteSplineTpl<OtherScalar,dim> & other) const
      {
        assert(dimension() == other.dimension());
        assert(m_absicca.isApprox(other.m_absicca));
        return CubicHermiteSplineTpl(m_absicca, m_points+other.m_points,
                                     m_derivatives+other.m_derivatives);
      }

      template<typename OtherScalar>
      CubicHermiteSplineTpl operator-(const CubicHermiteSplineTpl<OtherScalar,dim> & other) const
      {
        assert(dimension() == other.dimension());
        assert(m_absicca.isApprox(other.m_absicca));
        return CubicHermiteSplineTpl(m_absicca, m_points-other.m_points,
                                     m_derivatives-other.m_derivatives);
      }

      template<typename OtherScalar>
      CubicHermiteSplineTpl& operator+=(const CubicHermiteSplineTpl<OtherScalar,dim> & other)
      {
        assert(dimension() == other.dimension());
        assert(m_absicca.isApprox(other.m_absicca));
        m_points += other.m_points;
        m_derivatives += other.m_derivatives;
        return *this;
      }

      template<typename OtherScalar>
      CubicHermiteSplineTpl& operator-=(const CubicHermiteSplineTpl<OtherScalar,dim> & other)
      {
        assert(dimension() == other.dimension());
        assert(m_absicca.isApprox(other.m_absicca));
        m_points-=other.m_points;
        m_derivatives-=other.m_derivatives;
        return *this;
      }

      template<typename OtherScalar>
      bool operator!=(const CubicHermiteSplineTpl<OtherScalar,dim> & other) const
      { return !(*this != other); }

      const VectorX & absicca() const { return m_absicca; }
      void setAbsicca(const VectorX & absicca)
      {
        assert(absicca.size() == size());
        m_absicca = absicca;
        compute();
        assert(check());
      }

      const MatrixDx & points() const { return m_points; }
      MatrixDx & points() { return m_points; }

      const MatrixDx & derivatives() const { return m_derivatives; }
      MatrixDx & derivatives() { return m_derivatives; }

      ///
      /// \brief Returns the number of points contained in the trajectory.
      ///
      Index size() const { return m_points.cols(); }

      ///
      /// \brief Resize the trajectory with the given input size.
      ///
      void resize(const Index size)
      {
        assert(size >= 0);
        m_points.conservativeResize(dimension(),size);
        m_derivatives.conservativeResize(dimension(),size);
        m_absicca.conservativeResize(size);
        m_ds.conservativeResize(size-1);
      }

      ///
      /// \brief Returns the dimension of the trajectory.
      ///
      Index dimension() const { return m_points.rows(); }

      ///
      /// \brief Returns the number of intervals contained in the trajectory.
      ///
      Index numIntervals() const { return size()-1; }

      ///
      /// \brief Eval the spline at value s.
      ///
      template<typename Derived1, typename Derived2>
      void eval(const Scalar t,
                const Eigen::MatrixBase<Derived1> & p,
                const Eigen::MatrixBase<Derived2> & m) const
      {
        //EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived1,VectorD)
        //EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived2,VectorD)

        const Index id = findInterval(t);
        Eigen::MatrixBase<Derived1> & p_ = const_cast<Eigen::MatrixBase<Derived1> &>(p);
        Eigen::MatrixBase<Derived2> & m_ = const_cast<Eigen::MatrixBase<Derived2> &>(m);
        if(id == size()-1)
        {
          p_ = m_points.template rightCols<1>();
          m_ = m_derivatives.template rightCols<1>();
          return;
        }

        const typename MatrixDx::ConstColXpr p0 = m_points.col(id);
        const typename MatrixDx::ConstColXpr p1 = m_points.col(id+1);

        const typename MatrixDx::ConstColXpr m0 = m_derivatives.col(id);
        const typename MatrixDx::ConstColXpr m1 = m_derivatives.col(id+1);

        const Scalar & t0 = m_absicca[id];
        const Scalar & t1 = m_absicca[id+1];

        const Scalar dt = (t1-t0);
        const Scalar alpha = (t - t0)/dt;

        assert(0. <= alpha <= 1. && "alpha must be in [0,1]");

        Scalar h00, h10, h01, h11;
        evalCoeffs(alpha,h00,h10,h01,h11);
        h10 *= dt; h11 *= dt;

        p_ = (h00 * p0 + h10 * m0 + h01 * p1 + h11 * m1);

        m_ = (1.-alpha) * m0 + alpha * m1;
      }

    protected:

      static void evalCoeffs(const Scalar t,
                             Scalar & h00, Scalar & h10, Scalar & h01, Scalar & h11)
      {
        h00 = 1.; h10 = t; h01 = 0.; h11 = 0.;

        Scalar t_pow = t;

        // t^2
        t_pow = t*t;
        h00 -= 3*t_pow;
        h10 -= 2*t_pow;
        h01 += 3*t_pow;
        h11 -= t_pow;

        // t^3
        t_pow *= t;
        h00 += 2*t_pow;
        h10 += t_pow;
        h01 -= 2*t_pow;
        h11 += t_pow;
      }

      ///
      /// \brief Internal check. It checks if the abissica is monotone.
      ///
      bool check() const
      {
        return (m_ds.array() > 0.).all();
      }

      ///
      /// \brief Internal computation
      ///
      void compute()
      {
        assert(numIntervals() >= 1 && "The number of intervals must be greater or equal to 1.");
        m_ds = m_absicca.tail(numIntervals()) - m_absicca.head(numIntervals());
      }

      ///
      /// \brief Returns the index of the interval for the interpolation.
      ///
      Index findInterval(const Scalar t) const
      {
        if(t < m_absicca[0]) return 0;
        if(t > m_absicca[size()-1]) return numIntervals()-1;

        Index left_id = 0;
        Index right_id = size()-1;

        while(left_id <= right_id)
        {
          const Index middle_id = left_id + (right_id - left_id)/2;
          if(m_absicca[middle_id] < t)
            left_id = middle_id+1;
          else if(m_absicca[middle_id] > t)
            right_id = middle_id-1;
          else
            return middle_id;
        }

        return left_id-1;
      }

      VectorX m_absicca;
      MatrixDx m_points;
      MatrixDx m_derivatives;
      VectorX m_ds;

      // Serialization of the class
      friend class boost::serialization::access;

      template<class Archive>
      void save(Archive & ar, const unsigned int /*version*/) const
      {
        const Index m_size = size();
        ar & boost::serialization::make_nvp("size",m_size);
        ar & boost::serialization::make_nvp("absicca",m_absicca);
        ar & boost::serialization::make_nvp("points",m_points);
        ar & boost::serialization::make_nvp("derivatives",m_derivatives);
      }

      template<class Archive>
      void load(Archive & ar, const unsigned int /*version*/)
      {
        Index m_size;
        ar & boost::serialization::make_nvp("size",m_size);
        resize(m_size);

        ar & boost::serialization::make_nvp("absicca",m_absicca);
        ar & boost::serialization::make_nvp("points",m_points);
        ar & boost::serialization::make_nvp("derivatives",m_derivatives);

        compute();
      }

      BOOST_SERIALIZATION_SPLIT_MEMBER()

    };

    template<typename Scalar, int dim> CubicHermiteSplineTpl<Scalar,dim>
    createHermiteSplineAtAbsicca(const CubicHermiteSplineTpl<Scalar,dim>& spline,
                                    const typename
                                    CubicHermiteSplineTpl<Scalar,dim>::VectorX& absicca)
    {
      typedef CubicHermiteSplineTpl<Scalar,dim> SplineType;
      typedef typename SplineType::VectorX VectorX;
      typedef typename SplineType::MatrixDx MatrixDx;
      const VectorX& t0 = spline.absicca();

      //Assert that the final and initial point is the same
      assert(absicca[0] == t0[0]);
      assert(absicca[absicca.size()-1] == t0[t0.size()-1]);
      //Assert that the new resolution is higher.
      //Otherwise, there might be loss of information.
      assert(absicca.size() >= spline.size());

      MatrixDx p_new(dim, absicca.size());
      MatrixDx m_new(dim, absicca.size());

      for (int k=0;k<absicca.size();k++)
      {
        typename MatrixDx::ColXpr p = p_new.col(k);
        typename MatrixDx::ColXpr m = m_new.col(k);
        spline.eval(absicca[k], p, m);
      }

      return SplineType(absicca, p_new, m_new);
    }
  }
}

#endif // ifndef __multicontact_api_trajectories_cubic_hermite_spline_hpp__


#ifndef TVTML_DATA2D_HPP
#define TVTML_DATA2D_HPP

// system includes
#include <cassert>
#include <limits>
#include <cmath>
#include <random>

#include <string>
#include <iostream>
#include <sstream>
#include <fstream>

// OpenCV includes
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#ifdef TV_DATA_DEBUG
    #include <opencv2/highgui/highgui.hpp>
#endif

//Eigen includes
#include <Eigen/Core>
#include <Eigen/Geometry>

// video++ includes
#include <vpp/vpp.hh>
#include <vpp/utils/opencv_bridge.hh>

// own includes
#include "manifold.hpp"

namespace tvmtl{

// Specialization 2D Data
template < typename MANIFOLD >
class Data< MANIFOLD, 2>{

    public:
	static const int img_dim;
	// Manifold typedefs
	typedef typename MANIFOLD::value_type value_type;
	typedef typename MANIFOLD::scalar_type scalar_type;

	// Storage typedefs
	typedef vpp::image2d<value_type> storage_type;

	typedef double weights_type;
	typedef vpp::image2d<weights_type> weights_mat;

	typedef bool inp_type;
	typedef vpp::image2d<inp_type> inp_mat;

	inline bool doInpaint() const { return inpaint_; }

	// Data Init functions
	inline void initEdgeweights();
	inline void initInp();

	// Input functions
	void rgb_imread(std::string filename);
	void rgb_readBrightness(std::string filename);
	void rgb_readChromaticity(std::string filename);

	void readMatrixDataFromCSV(std::string filename, const int nx, const int ny);

	// Noise functions
	void add_gaussian_noise(double stdev);
	//void add_gaussian_noise_spd(double stdev);

	// Random Input functions
	// TODO: - Paramaterize for manifold type
	//       - Use user-defined functor as parameter
	void create_nonsmooth_son(const int ny, const int nx);
	void create_nonsmooth_spd(const int ny, const int nx);

	// EdgeFunctions
	void findEdgeWeights();
	void setEdgeWeights(const weights_mat&);

	// Inpainting Functions
	void findInpWeights(const int channel=2);
	void createRandInpWeights(const double threshold);

	// OutputFunctions
	void rgb_saveimage(std::string fname);
	void rgb_show();

	void output_weights(const weights_mat& mat, std::string filename) const;

	void output_img(std::string filename) const;
	void output_matval_img(std::string filename) const;
	void output_nimg(std::string filename) const;
//    private:
	// Data members
	// TODO Don't forget to initialize with 1px border
	// alignment defaults to 16byte for SSE/SSE2, 32 Byte for AVX
	storage_type img_;
	storage_type noise_img_;
	weights_mat edge_weights_;

	bool inpaint_;
	inp_mat inp_;
};

/*----- Implementation 2D Data ------*/
template < typename MANIFOLD >
const int Data<MANIFOLD, 2>::img_dim = 2;

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::setEdgeWeights(const weights_mat& w){
    edge_weights_= vpp::clone(w);
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::initInp(){
    inp_ = inp_mat(noise_img_.domain());
    vpp::fill(inp_, false);
    inpaint_ = false;
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::initEdgeweights(){
    edge_weights_ = weights_mat(noise_img_.domain());
    vpp::fill(edge_weights_, 1.0);
}

// FIXME: This is problematic for greyscale pictures since vuchar3 and vu[i] is hardcoded, works only due to range check of Eigen for []
template < typename MANIFOLD >
void Data<MANIFOLD, 2>::findEdgeWeights(){
    #ifdef TV_DATA_DEBUG
	std::cout << "Find Edges..." << std::endl;
    #endif
    cv::Mat gray, edge;

    { // Local Scope to save memory
	vpp::image2d<vpp::vuchar3> ucharimg(noise_img_.domain());

	// Convert double to uchar function
	auto double2uchar = [] (auto& i, const auto& n) {
	    value_type v = n * static_cast<typename MANIFOLD::scalar_type>(std::numeric_limits<unsigned char>::max());
	    vpp::vuchar3 vu = vpp::vuchar3::Zero();
	    vu[0]=static_cast<unsigned char>(v[2]);
	    vu[1]=static_cast<unsigned char>(v[1]);
	    vu[2]=static_cast<unsigned char>(v[0]);
	    i = vu;
	};

	vpp::pixel_wise(ucharimg, noise_img_) | double2uchar;
	cv::Mat src = vpp::to_opencv(ucharimg);
	cv::cvtColor(src, gray, CV_BGR2GRAY);
    }

    cv::blur(gray, edge, cv::Size(3,3));
    cv::Canny(edge, edge, 50, 150, 3);

    #ifdef TV_DATA_DEBUG
	cv::namedWindow( "Detected Edges", cv::WINDOW_NORMAL );
	cv::imshow("Detected Edges", edge);
	cv::waitKey(0);
    #endif

    vpp::image2d<unsigned char> ucharweights = vpp::from_opencv<unsigned char>(edge);
    vpp::pixel_wise(ucharweights, edge_weights_)() | [] (const unsigned char &uw, weights_type& ew) {
	ew = 1.0 - 0.99 * ( static_cast<weights_type>(uw) / static_cast<weights_type>(std::numeric_limits<unsigned char>::max()) );
    };

    #ifdef TV_DATA_DEBUG
	output_weights(edge_weights_, "wedge.csv");
    #endif
}

//TODO: static assert to avoid data that has not exactly 3 channels
// static assert that channel is element {1,2,3}
// make enum RGB out
template < typename MANIFOLD >
void Data<MANIFOLD, 2>::findInpWeights(const int channel){
    #ifdef TV_DATA_DEBUG
	std::cout << "Find Inpainting Area..." << std::endl;
    #endif
    inp_ = inp_mat(noise_img_.domain());
    vpp::pixel_wise(noise_img_, inp_) | [&] (const value_type& img, inp_type& inp) { inp = static_cast<inp_type>(img[channel-1] > 0.95); };
    //vpp::pixel_wise(noise_img_, inp_) | [&] (const value_type& img, inp_type& inp) { inp = static_cast<inp_type>(img[channel-1] > 0.95); };
    #ifdef TV_DATA_DEBUG
	int nr = inp_.nrows();
	int nc = inp_.ncols();

	std::fstream f;
	f.open("inp.csv", std::fstream::out);

	for (int r=0; r<nr; r++){
	    const inp_type* cur = &inp_(r,0);
	    for (int c=0; c<nc; c++){
		f << cur[c];
		if(c != nc-1) f << ",";
	    }
	    f <<  std::endl;
	}
	f.close();
    #endif
}

//TODO: static assert to avoid data that has not exactly 3 channels
// static assert that channel is element {1,2,3}
// make enum RGB out
template < typename MANIFOLD >
void Data<MANIFOLD, 2>::createRandInpWeights(const double threshold){

    #ifdef TV_DATA_DEBUG
	std::cout << "Create Random Inp Weights..." << std::endl;
    #endif

    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> rand(0.0, 1.0);

    inp_ = inp_mat(noise_img_.domain());
    vpp::pixel_wise(noise_img_, inp_) | [&] (const value_type& img, inp_type& inp) { inp = static_cast<inp_type>(rand(gen) > threshold); };

    #ifdef TV_DATA_DEBUG
	int nr = inp_.nrows();
	int nc = inp_.ncols();

	std::fstream f;
	f.open("inp.csv", std::fstream::out);

	for (int r=0; r<nr; r++){
	    const inp_type* cur = &inp_(r,0);
	    for (int c=0; c<nc; c++){
		f << cur[c];
		if(c != nc-1) f << ",";
	    }
	    f <<  std::endl;
	}
	f.close();
    #endif
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::rgb_imread(std::string filename){
	static_assert(MANIFOLD::value_dim == 3,"ERROR: RGB Input requires a Manifold with embedding dimension N=3!");
	vpp::image2d<vpp::vuchar3> input_image;
	input_image = vpp::clone(vpp::from_opencv<vpp::vuchar3 >(cv::imread(filename)));
	noise_img_ = storage_type(input_image.domain());
	// Convert Picture of uchar to double
	    vpp::pixel_wise(input_image, noise_img_) | [] (auto& i, auto& n) {
	    value_type v = value_type::Zero();
	    vpp::vuchar3 vu = i;
	    // TODO: insert manifold scalar type
	    v[0]=static_cast<double>(vu[2]); //opencv saves as BGR
	    v[1]=static_cast<double>(vu[1]);
	    v[2]=static_cast<double>(vu[0]);
	    n = v / static_cast<double>(std::numeric_limits<unsigned char>::max());
	};
    img_ = vpp::clone(noise_img_, vpp::_border = 1);

    initInp();
    initEdgeweights();
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::rgb_saveimage(std::string fname){
    static_assert(MANIFOLD::value_dim == 3,"ERROR: RGB Ouput requires a Manifold with embedding dimension N=3!");
	// Convert Picture of double to uchar
	vpp::image2d<vpp::vuchar3> vucharimg(img_.domain());
	vpp::pixel_wise(vucharimg, img_) | [] (auto& i, auto& n) {
	    value_type v = n * (double) std::numeric_limits<unsigned char>::max();
	    vpp::vuchar3 vu = vpp::vuchar3::Zero();
	    vu[0]=(unsigned char) v[2];
	    vu[1]=(unsigned char) v[1];
	    vu[2]=(unsigned char) v[0];
	    i = vu;
	};

	cv::imwrite(fname, to_opencv(vucharimg));
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::rgb_show(){
    static_assert(MANIFOLD::value_dim == 3,"ERROR: RGB Ouput requires a Manifold with embedding dimension N=3!");
	// Convert Picture of double to uchar
	vpp::image2d<vpp::vuchar3> vucharimg(img_.domain());
	vpp::pixel_wise(vucharimg, img_) | [] (auto& i, auto& n) {
	    value_type v = n * (double) std::numeric_limits<unsigned char>::max();
	    vpp::vuchar3 vu = vpp::vuchar3::Zero();
	    vu[0]=(unsigned char) v[2];
	    vu[1]=(unsigned char) v[1];
	    vu[2]=(unsigned char) v[0];
	    i = vu;
	};
	cv::namedWindow("Image Viewer", cv::WINDOW_NORMAL );
	cv::imshow( "Image Viewer", vpp::to_opencv(vucharimg));
	cv::waitKey(0);

}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::rgb_readBrightness(std::string filename){
	static_assert(MANIFOLD::value_dim == 1,"ERROR: Brightness Input requires a Manifold with embedding dimension N=1!");
	vpp::image2d<vpp::vuchar3> input_image;
	input_image = vpp::clone(vpp::from_opencv<vpp::vuchar3 >(cv::imread(filename)));
	noise_img_ = storage_type(input_image.domain());
	// Convert Picture of uchar to double
	    vpp::pixel_wise(input_image, noise_img_)(vpp::_no_threads) | [] (auto& i, auto& n) {
	    vpp::vdouble3 v;
	    vpp::vuchar3 vu = i;
	    // TODO: insert manifold scalar type
	    v[0]=static_cast<double>(vu[2]); //opencv saves as BGR
	    v[1]=static_cast<double>(vu[1]);
	    v[2]=static_cast<double>(vu[0]);
	    v = v / static_cast<double>(std::numeric_limits<unsigned char>::max());
	    n.setConstant(v.norm()/std::sqrt(3));
	};
    img_ = vpp::clone(noise_img_, vpp::_border = 1);

    initInp();
    initEdgeweights();
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::rgb_readChromaticity(std::string filename){
	static_assert(MANIFOLD::value_dim == 3,"ERROR: Chromaticity Input requires a Manifold with embedding dimension N=3!");
	vpp::image2d<vpp::vuchar3> input_image;
	input_image = vpp::clone(vpp::from_opencv<vpp::vuchar3 >(cv::imread(filename)));
	noise_img_ = storage_type(input_image.domain());
	// Convert Picture of uchar to double
	    vpp::pixel_wise(input_image, noise_img_) | [] (auto& i, auto& n) {
	    value_type v;
	    // TODO: insert manifold scalar type
	    v[0]=static_cast<double>(i[2])+1.0; //opencv saves as BGR
	    v[1]=static_cast<double>(i[1])+1.0;
	    v[2]=static_cast<double>(i[0])+1.0;
	    v = v / (static_cast<double>(std::numeric_limits<unsigned char>::max())+1.0);

	    double norm = v.norm();
	    if(norm!=0)
		n = v / norm;
	    else
		n = v;

	    #ifdef TV_DATA_DEBUG
		if(!std::isfinite(n(0))){
		    std::cout << "\nvuchar pixel " << i << std::endl;
		    std::cout << "double pixel " << v << std::endl;
		    std::cout << "normalized pixel " << n << std::endl;
		}
	    #endif
	};
    img_ = vpp::clone(noise_img_, vpp::_border = 1);

    initInp();
    initEdgeweights();
}

template <typename MANIFOLD>
void Data<MANIFOLD, 2>::readMatrixDataFromCSV(std::string filename, const int nx, const int ny){
    #ifdef TV_DATA_DEBUG
	std::cout << "ReadMatrixData from CSV File..." << std::endl;
    #endif
    noise_img_ = storage_type(ny, nx);
    vpp::fill(noise_img_, MANIFOLD::value_type::Zero());

    const int N = MANIFOLD::value_type::RowsAtCompileTime;
    const int N2 = MANIFOLD::value_dim;
    int cols, rows = 0;

    std::ifstream infile(filename, std::ifstream::in);

    std::string line = "";
    while (std::getline(infile, line)){
	std::stringstream strstr(line);
	std::string word = "";

	if(cols == 0)
	    while (std::getline(strstr, word, ','))
		cols++;

	rows++;
    }

    assert(N2==cols);
    assert(nx*ny==rows);

    infile.clear();
    infile.seekg(0, std::ios_base::beg);

    Eigen::Matrix<typename MANIFOLD::scalar_type, N2, 1> vectorizedMat;
    vectorizedMat.setZero();

    auto it = noise_img_.begin();
    while (std::getline(infile, line)){
	std::stringstream strstr(line);
	std::string word = "";
	int j=0;
	while (std::getline(strstr, word,',')){
	    typename MANIFOLD::scalar_type entry= static_cast<typename MANIFOLD::scalar_type>(std::stod(word));
	    vectorizedMat(j) = entry;
	    ++j;
	}
	*it = Eigen::Map<typename MANIFOLD::value_type>(vectorizedMat.data());
	it.next();
    }
    img_ = vpp::clone(noise_img_, vpp::_border = 1);
    fill_border_closest(img_);

    initInp();
    initEdgeweights();
}

template <typename MANIFOLD>
void Data<MANIFOLD, 2>::add_gaussian_noise(double stdev){

    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<typename MANIFOLD::scalar_type> rand(0.0, stdev);

    auto generate = [&] (typename MANIFOLD::scalar_type entry){
	return entry + rand(gen);
    };

	if(MANIFOLD::non_isometric_embedding)
	    vpp::pixel_wise(noise_img_) | [&] (value_type& i){ MANIFOLD::interpolation_preprocessing(i); };

    vpp::pixel_wise(noise_img_) | [&] (value_type& i) { i = i.unaryExpr(generate); };

	if(MANIFOLD::non_isometric_embedding)
	    vpp::pixel_wise(noise_img_) | [&] (value_type& i){ MANIFOLD::interpolation_postprocessing(i); };

    vpp::pixel_wise(noise_img_) | [&] (value_type& i) { MANIFOLD::projector(i); };

    img_ = vpp::clone(noise_img_, vpp::_border = 1);
    fill_border_closest(img_);
}

/*
//TODO: Generalize implementation to be compatible with add_gaussian_noise
template <typename MANIFOLD>
void Data<MANIFOLD, 2>::add_gaussian_noise_spd(double stdev){

    static_assert(MANIFOLD::MyType==SPD,"add_gaussian_noise_spd is implemented only for SPD(N)!");

    vpp::pixel_wise(noise_img_) | [&] (value_type& i){ MANIFOLD::interpolation_preprocessing(i); };

    std::random_device rd;
    std::mt19937 gen(rd());
    std::normal_distribution<scalar_type> rand(0.0, stdev);

    auto generate = [&] (scalar_type entry){
	return rand(gen);
    };

    vpp::pixel_wise(noise_img_) | [&] (value_type& i) {
	value_type r = value_type::Zero().unaryExpr(generate);
	i = i + (r + r.transpose()) * 0.5;
    };

    vpp::pixel_wise(noise_img_) | [&] (value_type& i){ MANIFOLD::interpolation_postprocessing(i); };

    img_ = vpp::clone(noise_img_, vpp::_border = 1);
}
*/

// TODO: Generalize to general N
template <typename MANIFOLD>
void Data<MANIFOLD, 2>::create_nonsmooth_son(const int ny,const int nx){
    #ifdef TV_DATA_DEBUG
	std::cout << "Create Nonsmooth SO(3) Picture..." << std::endl;
    #endif

    const int N = MANIFOLD::value_type::RowsAtCompileTime;

    static_assert(MANIFOLD::MyType==SO, "ERROR: Only possible for SO(N) manifolds");
    static_assert(N==3, "ERROR: Only possible for SO(3) manifolds");

    noise_img_ = storage_type(ny, nx);

    auto son_inserter = [&] (typename MANIFOLD::value_type& v, const vpp::vint2& coord){

	typename MANIFOLD::scalar_type x = coord(1)*1.0 / nx;
	typename MANIFOLD::scalar_type y = coord(0)*1.0 / ny;
	Eigen::Matrix<typename MANIFOLD::scalar_type, N, 1> rotation_axis;
	if(x > 0.5)
	    rotation_axis << 2.0 * x, y, 0.0;
	else
	    rotation_axis << 0.0, 2.0 * x, 0.5;

	typename MANIFOLD::scalar_type alpha;
	if(x > y)
	    alpha = x + y;
	else
	    alpha = M_PI * 0.5 + x - y;

	//v = MANIFOLD::value_type::Identity();
	v = Eigen::AngleAxis<typename MANIFOLD::scalar_type>(alpha, rotation_axis.normalized());
    };

    vpp::pixel_wise(noise_img_, noise_img_.domain()) | son_inserter;
    img_ = vpp::clone(noise_img_, vpp::_border = 1);
    fill_border_closest(img_);

    initInp();
    initEdgeweights();
}

// TODO: Generalize to general N
template <typename MANIFOLD>
void Data<MANIFOLD, 2>::create_nonsmooth_spd(const int ny,const int nx){
    #ifdef TV_DATA_DEBUG
	std::cout << "Create Nonsmooth SPD(3) Picture..." << std::endl;
    #endif

    const int N = MANIFOLD::value_type::RowsAtCompileTime;

    static_assert(MANIFOLD::MyType==SPD, "ERROR: Only possible for SPD(N) manifolds");
    static_assert(N==3, "ERROR: Only possible for SPD(3) manifolds");

    noise_img_ = storage_type(ny, nx);

    auto spd_inserter = [&] (typename MANIFOLD::value_type& v, const vpp::vint2& coord){

	typename MANIFOLD::scalar_type x = 1.0 * coord(1) / nx;
	typename MANIFOLD::scalar_type y = 1.0 * coord(0) / ny;

	Eigen::Matrix<typename MANIFOLD::scalar_type, N, N> R;
	Eigen::DiagonalMatrix< typename MANIFOLD::scalar_type, N> D(N);
	Eigen::Matrix<typename MANIFOLD::scalar_type, N, 1> rotation_axis;
	typename MANIFOLD::scalar_type alpha;

	if(x + y < 1.0){
	    rotation_axis << x, y, 2.0;
	    alpha = x + 2.0 * y;
	}
	else{
	    rotation_axis << y, -x, 1.0;
	    alpha = y + 2.0 * x;
	}

	D.diagonal() << x + 0.2, y + 0.2, 0.5;
	R  = Eigen::AngleAxis<typename MANIFOLD::scalar_type>(alpha, rotation_axis.normalized());
	v = R.transpose() * D * R;
    };

    vpp::pixel_wise(noise_img_, noise_img_.domain()) | spd_inserter;
    img_ = vpp::clone(noise_img_, vpp::_border = 1);
    fill_border_closest(img_);

    initInp();
    initEdgeweights();
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::output_weights(const weights_mat& weights, std::string filename) const{
    int nr = weights.nrows();
    int nc = weights.ncols();

    std::fstream f;
    f.open(filename, std::fstream::out);

    for (int r=0; r<nr; r++){
	const typename Data<MANIFOLD,2>::weights_type* cur = &weights(r,0);
	for (int c=0; c<nc; c++){
	    f << cur[c];
	    if(c != nc-1) f << ",";
	}
	f <<  std::endl;
    }
    f.close();
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::output_matval_img(std::string filename) const{
    int nr = img_.nrows();
    int nc = img_.ncols();

    std::fstream f;
    f.open(filename, std::fstream::out);
    Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "\n");
    for (int r=0; r<nr; r++){
	const auto* cur = &img_(r,0);
	for (int c=0; c<nc; c++)
	    f << cur[c].format(CommaInitFmt);
    }
    f.close();
}
template < typename MANIFOLD >
void Data<MANIFOLD, 2>::output_img(std::string filename) const{
    int nr = img_.nrows();
    int nc = img_.ncols();

    std::fstream f;
    f.open(filename, std::fstream::out);
    Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "");
    for (int r=0; r<nr; r++){
	const auto* cur = &img_(r,0);
	for (int c=0; c<nc; c++){
	    f << cur[c].format(CommaInitFmt);
	    if(c != nc-1) f << ",";
	}
	f <<  std::endl;
    }
    f.close();
}

template < typename MANIFOLD >
void Data<MANIFOLD, 2>::output_nimg(std::string filename) const{
    int nr = noise_img_.nrows();
    int nc = noise_img_.ncols();

    std::fstream f;
    f.open(filename, std::fstream::out);
    Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "", "");
    for (int r=0; r<nr; r++){
	const auto* cur = &noise_img_(r,0);
	for (int c=0; c<nc; c++){
	    f << cur[c].format(CommaInitFmt);
	    if(c != nc-1) f << ",";
	}
	f <<  std::endl;
    }
    f.close();
}

}// end namespace tvmtl

#endif


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 Copyright (C) 2017 Aareal Bank AG
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/date_time.hpp>
#include <boost/make_shared.hpp>
#include <ql/cashflows/cashflows.hpp>
#include <ql/cashflows/coupon.hpp>
#include <ql/cashflows/simplecashflow.hpp>
#include <ql/termstructures/credit/flathazardrate.hpp>
#include <ql/termstructures/yield/zerospreadedtermstructure.hpp>
#include <qle/pricingengines/discountingriskybondengine.hpp>

using namespace std;
using namespace QuantLib;

namespace QuantExt {

DiscountingRiskyBondEngine::DiscountingRiskyBondEngine(const Handle<YieldTermStructure>& discountCurve,
                                                       const Handle<DefaultProbabilityTermStructure>& defaultCurve,
                                                       const Handle<Quote>& recoveryRate,
                                                       const Handle<Quote>& securitySpread, Period timestepPeriod,
                                                       boost::optional<bool> includeSettlementDateFlows)
    : defaultCurve_(defaultCurve), recoveryRate_(recoveryRate), securitySpread_(securitySpread),
      timestepPeriod_(timestepPeriod), includeSettlementDateFlows_(includeSettlementDateFlows) {
    discountCurve_ =
        securitySpread_.empty()
            ? discountCurve
            : Handle<YieldTermStructure>(boost::make_shared<ZeroSpreadedTermStructure>(discountCurve, securitySpread));
    registerWith(discountCurve_);
    registerWith(defaultCurve_);
    registerWith(recoveryRate_);
    registerWith(securitySpread_);
}

DiscountingRiskyBondEngine::DiscountingRiskyBondEngine(const Handle<YieldTermStructure>& discountCurve,
                                                       const Handle<Quote>& securitySpread, Period timestepPeriod,
                                                       boost::optional<bool> includeSettlementDateFlows)
    : securitySpread_(securitySpread), timestepPeriod_(timestepPeriod),
      includeSettlementDateFlows_(includeSettlementDateFlows) {
    discountCurve_ =
        securitySpread_.empty()
            ? discountCurve
            : Handle<YieldTermStructure>(boost::make_shared<ZeroSpreadedTermStructure>(discountCurve, securitySpread));
    registerWith(discountCurve_);
    registerWith(securitySpread_);
}

void DiscountingRiskyBondEngine::calculate() const {
    QL_REQUIRE(!discountCurve_.empty(), "discounting term structure handle is empty");

    results_.valuationDate = (*discountCurve_)->referenceDate();
    results_.value = calculateNpv(results_.valuationDate, arguments_.cashflows);

    bool includeRefDateFlows =
        includeSettlementDateFlows_ ? *includeSettlementDateFlows_ : Settings::instance().includeReferenceDateEvents();
    // a bond's cashflow on settlement date is never taken into
    // account, so we might have to play it safe and recalculate
    // same parameters as above, we can avoid another call
    if (!includeRefDateFlows && results_.valuationDate == arguments_.settlementDate) {
        results_.settlementValue = results_.value;
    } else {
        // no such luck
        results_.settlementValue = calculateNpv(arguments_.settlementDate, arguments_.cashflows);
    }
}

Real DiscountingRiskyBondEngine::calculateNpv(Date npvDate, const Leg& cashflows,
                                              const Handle<YieldTermStructure>& incomeCurve,
                                              const bool conditionalOnSurvival) const {
    Real npvValue = 0;

    // handle case where we wish to price simply with benchmark curve and scalar security spread
    // i.e. credit curve term structure (and recovery) have not been specified
    // we set the default probability and recovery rate to zero in this instance (issuer credit worthiness already
    // captured within security spread)
    boost::shared_ptr<DefaultProbabilityTermStructure> creditCurvePtr =
        defaultCurve_.empty()
            ? boost::make_shared<QuantLib::FlatHazardRate>(results_.valuationDate, 0.0, discountCurve_->dayCounter())
            : defaultCurve_.currentLink();
    Rate recoveryVal = recoveryRate_.empty() ? 0.0 : recoveryRate_->value();

    // compounding factors for npv date
    Real dfSettl = incomeCurve.empty() ? discountCurve_->discount(npvDate) : incomeCurve->discount(npvDate);
    Real spSettl = conditionalOnSurvival ? creditCurvePtr->survivalProbability(npvDate) : 1.0;

    Size numCoupons = 0;
    bool hasLiveCashFlow = false;
    for (Size i = 0; i < cashflows.size(); i++) {
        boost::shared_ptr<CashFlow> cf = cashflows[i];
        if (cf->hasOccurred(npvDate, includeSettlementDateFlows_))
            continue;
        hasLiveCashFlow = true;

        // Coupon value is discounted future payment times the survival probability
        Probability S = creditCurvePtr->survivalProbability(cf->date()) / spSettl;
        npvValue += cf->amount() * S * discountCurve_->discount(cf->date()) / dfSettl;

        /* The amount recovered in the case of default is the recoveryrate*Notional*Probability of
           Default; this is added to the NPV value. For coupon bonds the coupon periods are taken
           as the timesteps for integrating over the probability of default.
        */
        boost::shared_ptr<Coupon> coupon = boost::dynamic_pointer_cast<Coupon>(cf);
        if (coupon) {
            numCoupons++;
            Date startDate = coupon->accrualStartDate();
            Date endDate = coupon->accrualEndDate();
            Date effectiveStartDate = (startDate <= npvDate && npvDate <= endDate) ? npvDate : startDate;
            Date defaultDate = effectiveStartDate + (endDate - effectiveStartDate) / 2;
            Probability P = creditCurvePtr->defaultProbability(effectiveStartDate, endDate) / spSettl;

            npvValue += coupon->nominal() * recoveryVal * P * discountCurve_->discount(defaultDate) / dfSettl;
        }
    }

    // the ql instrument might not yet be expired and still have not anything to value if
    // the npvDate > evaluation date
    if (!hasLiveCashFlow)
        return 0.0;

    if (cashflows.size() > 1 && numCoupons == 0) {
        QL_FAIL("DiscountingRiskyBondEngine does not support bonds with multiple cashflows but no coupons");
    }

    /* If there are no coupon, as in a Zero Bond, we must integrate over the entire period from npv date to
       maturity. The timestepPeriod specified is used as provide the steps for the integration. This only applies
       to bonds with 1 cashflow, identified as a final redemption payment.
    */
    if (cashflows.size() == 1) {
        boost::shared_ptr<Redemption> redemption = boost::dynamic_pointer_cast<Redemption>(cashflows[0]);
        if (redemption) {
            Date startDate = npvDate;
            while (startDate < redemption->date()) {
                Date stepDate = startDate + timestepPeriod_;
                Date endDate = (stepDate > redemption->date()) ? redemption->date() : stepDate;
                Date defaultDate = startDate + (endDate - startDate) / 2;
                Probability P = creditCurvePtr->defaultProbability(startDate, endDate) / spSettl;

                npvValue += redemption->amount() * recoveryVal * P * discountCurve_->discount(defaultDate) / dfSettl;
                startDate = stepDate;
            }
        }
    }

    return npvValue;
}
} // namespace QuantExt


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <cmath>
#include <Eigen/Geometry>
#include <boost/format.hpp>
#include <comma/base/exception.h>
#include <comma/math/compare.h>
#include "../angle.h"
#include "coordinates.h"
#include <iomanip>

namespace {

    static const double eighty_degrees = 80 * M_PI / 180.0;

    static const double scaled_cos_eighty_degrees = 2.0 / M_PI * std::cos( eighty_degrees );

    bool precise_is_far( const snark::spherical::coordinates & l, const snark::spherical::coordinates & r, double _epsilon )
    {
        return Eigen::AngleAxis< double >( Eigen::Quaternion< double >::FromTwoVectors( l.to_cartesian(), r.to_cartesian() ) ).angle() > _epsilon;
    }

    bool is_far( const snark::spherical::coordinates & l, const snark::spherical::coordinates & r, double _epsilon )
    {
        // for very short arcs skip shortcuts and enforce thorough comparison because of numerical stability issues in Eigen
        // the actual value is about 6 m / earth radius; enough to make AngleAxis::angle() non-zero
        double threshold = std::max( snark::spherical::coordinates::epsilon, _epsilon );
        // make quick decisions first; for latitude shortcut no explanations needed
        if ( std::abs( l.latitude - r.latitude ) > threshold ) { return true; }
        double delta_longitude = std::abs( l.longitude - r.longitude );
        // nearly 360 delta means very close; take the shorter of the two possible arcs
        delta_longitude = delta_longitude > M_PI ? 2 * M_PI - delta_longitude : delta_longitude;
        // longitude shortcut:
        //     ignore near-Pole regions
        //     take a lower limit on great circle arc length:
        //         the arc at the higher (by modulo) of the two latitudes is shorter than the real arc (lower estimate)
        //             proof not given here; differentiate the exact expression by one of the latitudes; the derivative sign is
        //             defined by the other latitude: negative in the Northern hemisphere, positive in the Southern; therefore,
        //             if the fixed latitude (not the one being differentiated upon) is chosen as the max by modulo, the function
        //             may only increase as the other latitude moves either South or North, respectively
        //         for the same latitudes, use the lower estimate of the great circle arc length:
        //             exact length = 2 * asin( cos(latitude) * sin( delta_longitude / 2 ) )
        //             estimates: asin(x) >= x for any x in [0, 1]
        //                        sin(y) >= 2/pi * y for any y in [0, pi/2]
        //             thus exact length >= 2 cos(latitude) * sin( delta_longitude / 2 ) >= cos(latitude) * 2/pi * delta_longitude >= cos(80 degrees) * 2/pi * delta_longitude
        //     if the lower limit is above the threshold, the real arc length is above the threshold as well
        double max_latitude = std::max( std::abs( l.latitude ), std::abs( r.latitude ) );
        if ( max_latitude < eighty_degrees && delta_longitude * scaled_cos_eighty_degrees > threshold ) { return true; }
        // shortcuts failed, do lengthy computations
        return precise_is_far( l, r, threshold );
    }
}

namespace snark { namespace spherical {

const double coordinates::epsilon = 1e-6;

bool coordinates::operator==( const coordinates& rhs ) const
{
    return !is_far( *this, rhs, epsilon );
}

bool coordinates::is_near( const coordinates& c, double epsilon ) const
{
    return !is_far( *this, c, epsilon );
}

bool coordinates::is_near( const coordinates & l, const coordinates & r, double _epsilon )
{
    return !is_far( l, r, _epsilon );
}

/// limits longitude to [-PI, PI)
static double limit( const double longitude )
{
    // todo: this is just a quick patch, not a real fix; what if longitude equals e.g. -3*M_PI?
    // todo: seriously quick and dirty, to fix anti merridian crossing
    return longitude < -M_PI ? longitude + 2 * M_PI : longitude >= M_PI ? longitude - 2 * M_PI : longitude;
}

coordinates::coordinates( const double latitude, const double longitude ) : latitude( latitude ), longitude( limit( longitude ) )
{
}

coordinates::coordinates( const snark::bearing_elevation& be ) : latitude( be.elevation() ), longitude( limit( be.bearing() ) )
{
}

coordinates::coordinates( const Eigen::Vector3d& xyz )
{
    snark::range_bearing_elevation rbe( xyz );
    latitude = rbe.elevation();
    longitude = limit( rbe.bearing() );
}

Eigen::Vector3d to_navigation_frame( const coordinates& c, const Eigen::Vector3d& v )
{
    const Eigen::Matrix3d& r1 = Eigen::AngleAxis< double >( -c.longitude, Eigen::Vector3d( 0, 0, 1 ) ).toRotationMatrix();
    const Eigen::Matrix3d& r2 = Eigen::AngleAxis< double >( c.latitude + M_PI / 2, Eigen::Vector3d( 0, 1, 0 ) ).toRotationMatrix();
    return r2 * r1 * v;
}

coordinates& coordinates::operator+=( const coordinates& rhs )
{
    double d = latitude + rhs.latitude;
    static const double epsilon = 0.00005;
    if( comma::math::equal( d, M_PI / 2, epsilon ) ) { d = M_PI / 2; }
    else if( comma::math::equal( d, -M_PI / 2, epsilon ) ) { d = -M_PI / 2; }
    else if( d > M_PI / 2 || d < -M_PI / 2 ) { COMMA_THROW( comma::exception, "adding " << ( latitude * 180 / M_PI ) << " and " << ( rhs.latitude * 180 / M_PI ) << " gives invalid latitude of " << ( d * 180 / M_PI ) << " degress" ); }
    latitude = d;
    longitude += rhs.longitude;
    while( longitude < -M_PI ) { longitude += 2 * M_PI; } // brutal, but mod() is slower in most cases, i think
    while( this->longitude >= M_PI ) { longitude -= 2 * M_PI; } // brutal, but mod() is slower in most cases, i think
    return *this;
}

coordinates::operator std::string() const
{
    const std::pair< double, double >& c = to_degrees();
    return (boost::format("%.2f") % c.first ).str() + "," + (boost::format("%.2f") % c.second).str();
}

bool coordinates::is_near( const Eigen::Vector3d& c, double _epsilon ) const
{
    return ( to_cartesian() - c ).lpNorm<Eigen::Infinity>() < _epsilon;
}

coordinates coordinates::from_degrees(double latitude, double longitude)
{
    return coordinates( snark::math::radians( snark::math::degrees( latitude ) ).value,
                        snark::math::radians( snark::math::degrees( longitude ) ).value );
}

std::pair< double, double > coordinates::to_degrees() const
{
    return std::make_pair( snark::math::degrees( snark::math::radians( latitude ) ).value
                         , snark::math::degrees( snark::math::radians( longitude ) ).value );
}

} } // namespace snark { namespace spherical {