// 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 JHMI_LIVER_WALRAND_TREE_HPP_NRC_2016_02_02
#define JHMI_LIVER_WALRAND_TREE_HPP_NRC_2016_02_02

#include "liver/build_vessel_map.hpp"
#include "liver/distance_vessel.hpp"
#include "messages/vessel_tree.pb.h"
#include "shape/voxelized_shape.hpp"
#include "utility/binary_tree.hpp"
#include "utility/octtree.hpp"
#include "utility/protobuf_zip_ostream.hpp"
#include <boost/filesystem.hpp>
#include <google/protobuf/io/coded_stream.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>

namespace jhmi {
  struct closer_to_root{
    m_sq root_dist_;
    m3 root_;
    closer_to_root(m3 const& root, m_sq root_dist) : root_dist_{root_dist}, root_{root} {}

    boost::optional<m_sq> operator()(distance_vessel const& vessel, m3 const& pt) {
      auto cpt = lerp(vessel.l, nearest_t(vessel.l, pt));
      auto dist = distance_squared(cpt - root_);
      if (dist <= root_dist_)
        return dist;
      return boost::none;
    }
  };
  struct closer_to_p1{
    boost::optional<m_sq> operator()(distance_vessel const& vessel, m3 const& pt) {
      auto ncpt = (vessel.l.p1 + vessel.l.p2 + pt) / 3.;
      auto d1 = distance_squared(vessel.l.p1 - ncpt);
      auto d2 = distance_squared(ncpt - pt);
      if (d1 < d2)
        return d2;
      return boost::none;
    }
  };

  class walrand_tree {
    using distance_node = binary_node_t<distance_vessel>;

    binary_tree<distance_vessel> vessels_;
    vidx_to<distance_node> to_vessels_;
    octtree<distance_vessel> grid_;
    voxelized_shape const& liver_;
    id_generator<vessel_tag> get_vessel_id_;
    id_generator<cell_tag> get_cell_id_;
    m3 root_;

    void add_vessel(distance_node n) {
      grid_.add_item(n.value());
      to_vessels_.insert(std::make_pair(n.value().id, n));
    }
  public:
    walrand_tree(build_tree_tag, std::string const& vessel_file,
        voxelized_shape const& liver)
      : vessels_{}, grid_{extents(liver)}, liver_{liver} {

      auto vg = build_vessel_map(vessel_file);
      get_vessel_id_ = vg.second;
      //Organize the map into a binary tree.
      auto parent_id = jhmi_detail::find_parent_vessel(vg.first);
      vessels_ = binary_tree<distance_vessel>{vg.first.at(parent_id).v};
      add_children_vessels(vessels_.root(), [&](auto n) { add_vessel(n); }, vg.first);

      auto node = vessels_.root();
      while (!node.right_child())
        node = node.left_child();
      root_ = node.value().l.p2;
    }

    void connect_cell(m3 const& pt) {
#if 0
      auto nearest = grid_.find_nearest_item(pt, closer_to_root(root_, distance_squared(pt - root_)));
#else
      auto nearest = grid_.find_nearest_item(pt, closer_to_p1{});
#endif
      if (!nearest)
        throw std::runtime_error("Unexpected unconnctable item");
      assert(nearest);
      auto n = to_vessels_.at(nearest->id);
      auto& v = n.value();

      grid_.remove_item(v);
      auto new_start = (v.l.p1 + v.l.p2 + pt) / 3.;
      auto old_start = v.l.p1;
      v.l.p1 = new_start;
      grid_.add_item(v);

      auto new_parent = n.make_left_child_of(
        distance_vessel{get_vessel_id_(), line<m3>{old_start, new_start}});
      add_vessel(new_parent);
      auto cell_vessel = new_parent.set_right_child(
        distance_vessel{get_vessel_id_(), line<m3>{new_start, pt}});
      cell_vessel.value().cell = get_cell_id_();
      add_vessel(cell_vessel);
    }

    std::string write(boost::filesystem::path const& p) const {
      jhmi_message::VesselTree vt;
      RANGES_FOR(auto&& v, vessels_ | view::node_level_order) {
        auto p = v.parent();
        auto r = v.right_child();
        auto l = v.left_child();
        auto vtv = vt.add_vessels();
        vtv->set_id(v.value().id.value());
        vtv->set_parent((p ? p.value().id : vessel_id::invalid()).value());
        vtv->set_left(  (l ? l.value().id : vessel_id::invalid()).value());
        vtv->set_right( (r ? r.value().id : vessel_id::invalid()).value());
        vtv->set_radius(30e-6);
        vtv->set_cell(v.value().cell.value());
        vtv->set_flow(.1);
        vtv->set_entry_pressure(10);
        vtv->set_exit_pressure(10);
        vtv->set_sx(v.value().l.p1.x.value());
        vtv->set_sy(v.value().l.p1.y.value());
        vtv->set_sz(v.value().l.p1.z.value());
        vtv->set_ex(v.value().l.p2.x.value());
        vtv->set_ey(v.value().l.p2.y.value());
        vtv->set_ez(v.value().l.p2.z.value());
        vtv->set_is_const(false);
      }
      std::string filename = "vessel_tree.pbz";
      protobuf_zip_ostream out_stream{p / filename};
      if (!vt.SerializeToZeroCopyStream(out_stream.get()))
        throw std::runtime_error("Failed to write simple vessel tree.");
      return filename;
    }
  };
}//jhmi
#endif


#include <boost/qvm/math.hpp>


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/clamp.hpp>
#include <boost/simd/constant/nan.hpp>
#include <scalar_test.hpp>

STF_CASE_TPL(" clamp",  STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  using bs::clamp;

  STF_EXPR_IS(clamp(T(),T(),T()), T);

  STF_EQUAL(clamp(T(1),T(2),T(4)) ,T(2));
  STF_EQUAL(clamp(T(2),T(2),T(4)), T(2));
  STF_EQUAL(clamp(T(3),T(2),T(4)), T(3));
  STF_EQUAL(clamp(T(4),T(2),T(4)), T(4));
  STF_EQUAL(clamp(T(5),T(2),T(4)), T(4));
}

STF_CASE_TPL(" clamp pedantic on nan",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::clamp;

  auto nan =  bs::Nan<T>();
  STF_IEEE_EQUAL(bs::pedantic_(clamp)(T(nan),T(2),T(4)) , nan);
}


/*
 * Copyright 2020 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
 */

/*
* \author Jordan McManus
*/

#ifndef MAIN_CPP
#define MAIN_CPP

#pragma comment(lib, "Ws2_32.lib")

#include <Eigen/Dense>
#include <fstream>
#include <iostream>
#include <string>
#include "../georeferencing/DatagramRayTracer.hpp"
#include "../svp/CarisSvpFile.hpp"
#include "../datagrams/DatagramParserFactory.hpp"
#include "../svp/SvpSelectionStrategy.hpp"
#include "../svp/SvpNearestByTime.hpp"
#include "../svp/SvpNearestByLocation.hpp"
#include "../math/Boresight.hpp"

void printUsage(){
	std::cerr << "\n\
NAME\n\n\
	raytracer - Produces raytraced vectors for each beam \n\n\
SYNOPSIS\n \
	raytracer [-x lever_arm_x] [-y lever_arm_y] [-z lever_arm_z] [-r roll_angle] [-p pitch_angle] [-h heading_angle] [-d transducer_depth] [-s svp_file] [-S svpStrategy] file\n\n\
DESCRIPTION\n \
    This application will output the raytraced vector for each beam in the MBES file\n\n \
        -S choose one: nearestTime or nearestLocation\n\n \
Copyright 2017-2020 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés" << std::endl;

        exit(1);
}

/**
* @param argc number of argument
* @param argv value of the arguments
*/
int main (int argc , char ** argv ){

	#ifdef __GNU__
	setenv("TZ", "UTC", 1);
	#endif
	#ifdef _WIN32
	putenv("TZ");
	#endif

	if(argc < 2){
            printUsage();
	}

        std::string fileName(argv[argc-1]);

        //draft
        double transducerDraft = 0.0;

        //Lever arm
        double leverArmX = 0.0;
        double leverArmY = 0.0;
        double leverArmZ = 0.0;

        //Boresight
        double roll     = 0.0;
        double pitch    = 0.0;
        double heading  = 0.0;

        //SVP strategy
        std::string userSelectedStrategy;
        SvpSelectionStrategy * svpStrategy = NULL;

	std::string	     svpFilename;
	CarisSvpFile svps;

        int index;
        while((index=getopt(argc,argv,"x:y:z:r:p:h:d:s:S:LTg"))!=-1)
        {
            switch(index)
            {
                case 'x':
                    if(sscanf(optarg,"%lf", &leverArmX) != 1)
                    {
                        std::cerr << "Invalid lever arm X offset (-x): " << leverArmX << std::endl;
                        printUsage();
                    }
               break;

                case 'y':
                    if (sscanf(optarg,"%lf", &leverArmY) != 1)
                    {
                        std::cerr << "Invalid lever arm Y offset (-y): " << leverArmY << std::endl;
                        printUsage();
                    }
                break;

                case 'z':
                    if (sscanf(optarg,"%lf", &leverArmZ) != 1)
                    {
                        std::cerr << "Invalid lever arm Z offset (-z): " << leverArmZ << std::endl;
                        printUsage();
                    }
                break;

                case 'r':
                    if (sscanf(optarg,"%lf", &roll) != 1)
                    {
                        std::cerr << "Invalid roll angle offset (-r): " << roll << std::endl;
                        printUsage();
                    }
                break;

                case 'h':
                    if (sscanf(optarg,"%lf", &heading) != 1)
                    {
                        std::cerr << "Invalid heading angle offset (-h): " << heading << std::endl;
                        printUsage();
                    }
                break;

                case 'p':
                    if (sscanf(optarg,"%lf", &pitch) != 1)
                    {
                        std::cerr << "Invalid pitch angle offset (-p): " << pitch << std::endl;
                        printUsage();
                    }
                break;

                case 'd':
                    if (sscanf(optarg, "%lf", &transducerDraft) != 1) {
                        std::cerr << "Invalid transducer draft (-d): " << transducerDraft << std::endl;
                        printUsage();
                    } else {
                        std::cerr << "Transducer draft is: " << transducerDraft << std::endl;
                    }
                break;

		case 's':
			svpFilename = optarg;
			if(!svps.readSvpFile(svpFilename)){
				std::cerr << "Invalid SVP file (-s): " << svpFilename << std::endl;
				printUsage();
			}
                        break;

                case 'S':
			userSelectedStrategy = optarg;
                        if(userSelectedStrategy == "nearestLocation") {
                            std::cerr << "[+] Using nearest location sound velocity profile selection strategy" << std::endl;
                            svpStrategy = new SvpNearestByLocation();
                        } else if(userSelectedStrategy == "nearestTime") {
                            std::cerr << "[+] Using nearest location sound velocity profile selection strategy" << std::endl;
                            svpStrategy = new SvpNearestByTime();
                        } else {
                            std::cerr << "Invalid SVP strategy (-S): " << userSelectedStrategy << std::endl;
                            std::cerr << "Possible choices are:" << std::endl;
                            std::cerr << "-S nearestTime" << std::endl;
                            std::cerr << "-S nearestLocation" << std::endl;
                            printUsage();
                        }
                        break;
            }
        }

        if(svpStrategy == NULL){
            std::cerr << "[+] Using nearest in time sound velocity profile selection strategy by default" << std::endl;
            svpStrategy = new SvpNearestByTime();
        }

        DatagramRayTracer handler(*svpStrategy);

        DatagramParser * parser = NULL;

	try{
            std::cerr << "Decoding " << fileName << std::endl;

            parser = DatagramParserFactory::build(fileName,handler);

            parser->parse(fileName, true);

            std::cout << std::setprecision(12);
            std::cout << std::fixed;

            //Lever arm
            Eigen::Vector3d leverArm;
            leverArm << leverArmX,leverArmY,leverArmZ;

            //Boresight
            Attitude boresightAngles(0,roll,pitch,heading);
            Eigen::Matrix3d boresight;
            Boresight::buildMatrix(boresight,boresightAngles);

            //raytracing
            handler.raytrace(leverArm, boresight, svps.getSvps());

	}
	catch(const char * error){
		std::cerr << "Error whille parsing " << fileName << ": " << error << std::endl;
	}

	if(parser) delete parser;
}

#endif


#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


#include <Eigen/Core>
#include <swift_vio/imu/SimpleImuPropagationJacobian.hpp>
#include <swift_vio/ParallaxAnglePoint.hpp>

namespace swift_vio {
void SimpleImuPropagationJacobian::initialize(
    const okvis::Time startEpoch, const okvis::Time endEpoch,
    const okvis::kinematics::Transformation &endT_WB,
    const Eigen::Matrix<double, 3, 1> &endV_W,
    const Eigen::Matrix<double, 3, 1> &endOmega_B) {
  startEpoch_ = startEpoch;
  endEpoch_ = endEpoch;
  endp_WB_B_ = endT_WB.r();
  endq_WB_ = endT_WB.q();
  endV_WB_W_ = endV_W;
  endOmega_WB_B_ = endOmega_B;
}

SimpleImuPropagationJacobian::SimpleImuPropagationJacobian(
    const okvis::Time startEpoch, const okvis::Time endEpoch,
    const okvis::kinematics::Transformation &endT_WB,
    const Eigen::Matrix<double, 3, 1> &endV_W,
    const Eigen::Matrix<double, 3, 1> &endOmega_B)
    : startEpoch_(startEpoch), endEpoch_(endEpoch), endp_WB_B_(endT_WB.r()),
      endq_WB_(endT_WB.q()), endV_WB_W_(endV_W), endOmega_WB_B_(endOmega_B) {}

Eigen::Matrix3d SimpleImuPropagationJacobian::dp_dv_WB(double deltaTime) {
  Eigen::Matrix3d j = Eigen::Matrix3d::Identity();
  j(0, 0) = deltaTime;
  j(1, 1) = deltaTime;
  j(2, 2) = deltaTime;
  return j;
}

Eigen::Matrix<double, 3, 2> SimpleImuPropagationJacobian::dp_dunitgW(
    double deltaTime, const Eigen::Vector3d &gravityDirection,
    double gravityMagnitude) {
  return 0.5 * deltaTime * deltaTime * gravityMagnitude *
         NormalVectorElement(gravityDirection).getM();
}

Eigen::Matrix<double, 3, 2> SimpleImuPropagationJacobian::dtheta_dunitgW(
    double /*deltaTime*/, const Eigen::Vector3d & /*gravityDirection*/,
    double /*gravityMagnitude*/) {
  return Eigen::Matrix<double, 3, 2>::Zero();
}
} // namespace swift_vio


// Copyright (c) 2014 The Bitcoin Core developers
// Copyright (c) 2014-2015 The Dash developers
// Copyright (c) 2015-2017 The PIVX developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "primitives/transaction.h"
#include "main.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(main_tests)

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    CAmount nSum = 0;
    for (int nHeight = 0; nHeight < 1; nHeight += 1) {
        /* premine in block 1 (60001 WISL) */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == (400000 * COIN));
        nSum += nSubsidy;
    }

    for (int nHeight = 1; nHeight < 86401; nHeight += 1) {
        /* 1-86400 */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == (7233 * COIN));
        nSum += nSubsidy;
    }

    for (int nHeight = 86401; nHeight < 172801; nHeight += 1) {
        /* 86400-151199 */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == 1000 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 172801; nHeight < 259201; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == (900 * COIN));
        BOOST_CHECK(MoneyRange(nSubsidy));
        nSum += nSubsidy;
    }

    for (int nHeight = 259201; nHeight < 302401; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == (800 * COIN));
        BOOST_CHECK(MoneyRange(nSubsidy));
        nSum += nSubsidy;

    }
    for (int nHeight = 302401; nHeight < 345601; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == (700 * COIN));
        BOOST_CHECK(MoneyRange(nSubsidy));
        nSum += nSubsidy;
    }
    for (int nHeight = 345601; nHeight < 388801; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == (600 * COIN));
        BOOST_CHECK(MoneyRange(nSubsidy));
        nSum += nSubsidy;
    }
    for (int nHeight = 388801; nHeight < 432001; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == (500 * COIN));
        BOOST_CHECK(MoneyRange(nSubsidy));
        nSum += nSubsidy;
    }
    for (int nHeight = 432001; nHeight < 475201; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == (400 * COIN));
        BOOST_CHECK(MoneyRange(nSubsidy));
        nSum += nSubsidy;
    }
    for (int nHeight = 475201; nHeight < 518401; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == (300 * COIN));
        BOOST_CHECK(MoneyRange(nSubsidy));
        nSum += nSubsidy;
    }
    for (int nHeight = 518401; nHeight < 561601; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == (200 * COIN));
        BOOST_CHECK(MoneyRange(nSubsidy));
        nSum += nSubsidy;
    }
    BOOST_CHECK(nSum == 940691200 * COIN);
    for (int nHeight = 561601; nHeight < 604801; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == (50 * COIN));
        BOOST_CHECK(MoneyRange(nSubsidy));
        nSum += nSubsidy;
    }
    for (int nHeight = 561601; nHeight < 604801; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == (50 * COIN));
        BOOST_CHECK(MoneyRange(nSubsidy));
        nSum += nSubsidy;
    }
    for (int nHeight = 5170400; nHeight < 5270401; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == (30 * COIN));
        BOOST_CHECK(MoneyRange(nSubsidy));
        nSum += nSubsidy;
    }
    for (int nHeight = 5270401; nHeight < 5270600; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy == (8 * COIN));
        BOOST_CHECK(MoneyRange(nSubsidy));
        nSum += nSubsidy;
    }
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (C) 2012-2013 Simon Lynen, ASL, ETH Zurich, Switzerland
 * You can contact the author at <slynen at ethz dot ch>
 *
 * 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.
 */
#ifndef MSF_STATEDEF_HPP_
#define MSF_STATEDEF_HPP_

#include <Eigen/Dense>
#include <msf_core/msf_fwds.h>
#include <boost/fusion/container.hpp>

namespace msf_updates {
/*
 * This file contains the state definition of the EKF as defined for a given set
 * of sensors / states to estimate.
 */
enum StateDefinition {  // Must not manually set the enum values!
  p,
  v,
  q,
  b_w,
  b_a,
  L,
  q_wv,
  p_wv,
  q_ic,
  p_ic,
  b_p/*,
   q_int_*/
};

namespace {
/***
 * Setup core state, then auxiliary state.
 */
typedef boost::fusion::vector<
    // States varying during propagation - must not change the ordering here for
    // now, CalcQ has the ordering hardcoded.
    msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, p,
        msf_core::CoreStateWithPropagation>,  ///< Translation from the world frame to the IMU frame expressed in the world frame.
    msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, v,
        msf_core::CoreStateWithPropagation>,  ///< Velocity of the IMU frame expressed in the world frame.
    msf_core::StateVar_T<Eigen::Quaternion<double>, q,
        msf_core::CoreStateWithPropagation>,  ///< Rotation from the world frame to the IMU frame expressed in the world frame.
    msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, b_w,
        msf_core::CoreStateWithoutPropagation>,  ///< Gyro biases.
    msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, b_a,
        msf_core::CoreStateWithoutPropagation>,  ///< Acceleration biases.

    // States not varying during propagation.
    msf_core::StateVar_T<Eigen::Matrix<double, 1, 1>, L>,     ///< Visual scale.
    msf_core::StateVar_T<Eigen::Quaternion<double>, q_wv,
        msf_core::AuxiliaryNonTemporalDrifting>,  ///< Rotation from the world frame to the frame in which the pose is measured expressed in the world frame.
    msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, p_wv>,  ///< Translation from the world frame to the frame in which the pose is measured expressed in the world frame.
    msf_core::StateVar_T<Eigen::Quaternion<double>, q_ic>,  ///< Rotation from the IMU frame to the camera frame expressed in the IMU frame.
    msf_core::StateVar_T<Eigen::Matrix<double, 3, 1>, p_ic>,  ///< Translation from the IMU frame to the camera frame expressed in the IMU frame.
    msf_core::StateVar_T<Eigen::Matrix<double, 1, 1>, b_p>  ///< Pressure sensor bias.

> fullState_T;
}
typedef msf_core::GenericState_T<fullState_T, StateDefinition> EKFState;  ///< The state we want to use in this EKF.
typedef shared_ptr<EKFState> EKFStatePtr;
typedef shared_ptr<const EKFState> EKFStateConstPtr;
}
#include <msf_updates/static_ordering_assertions.h> //DO NOT REMOVE THIS
#endif  // MSF_STATEDEF_HPP_


///////////////////////////////////////////////////////////////////////////////
// ars::function::concept.hpp                                                //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_ARS_FUNCTION_CONCEPT_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_ARS_FUNCTION_CONCEPT_HPP_ER_2009
#include <boost/concept_check.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace ars{
namespace function{

template<typename F,typename T>
class concept{

    BOOST_CONCEPT_USAGE((
        f(x,log_pdf,dlog_pdf)
    ));

    private:
        F f;
        T x;
        T log_pdf;
        T dlog_pdf;

};

}
}// ars
}// detail
}// statistics
}// boost

#endif


#include <iostream>
#include <string>
#include <boost/hana.hpp>

struct Truck
{
    std::string name;
    double bed_length;
};

struct Car
{
    std::string name;
};

struct Motorcycle
{
    std::string name;
};

int main(int argc, char ** argv)
{
    auto vehicles =  boost::hana::make_tuple(Truck {"F-150", 8.5},
                                             Car {"Volt"},
                                             Motorcycle {"Ninja"});

    auto has_bed_length = boost::hana::is_valid([](auto && x) -> decltype((void) x.bed_length)
                                                {
                                                    ;
                                                });

    // Filter out trucks
    auto trucks = boost::hana::filter(vehicles, has_bed_length);
    // Filter out non trucks
    auto non_trucks = boost::hana::filter(vehicles, boost::hana::compose(boost::hana::not_, has_bed_length));

    // Display
    boost::hana::for_each(trucks, [](auto & vehicle)
                                  {
                                      std::cout << "Truck = " << vehicle.name << ", bed length = " << vehicle.bed_length << std::endl;
                                  });

    // Display
    boost::hana::for_each(non_trucks, [](auto & vehicle)
                                      {
                                          std::cout << "Vehicle = " << vehicle.name << std::endl;
                                      });

    return 0;
}


#include <boost/numeric/interval/limits.hpp>


#ifndef _MAX_FLOW_HPP_
#define _MAX_FLOW_HPP_

#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>
using namespace std;
using namespace boost;

class Network {
private:
  typedef adjacency_list_traits < vecS, vecS, directedS > Traits;

public:
  typedef Traits::vertex_descriptor Vertex;
  Network();
  Vertex AddVertex();
  void AddEdge(Vertex &v1, Vertex &v2, const long capacity);
  unsigned long long MaxFlow(Vertex &s, Vertex &t);

private:
  typedef adjacency_list < vecS, vecS, directedS,
    property < vertex_name_t, std::string,
    property < vertex_index_t, long,
    property < vertex_color_t, boost::default_color_type,
    property < vertex_distance_t, long,
    property < vertex_predecessor_t, Traits::edge_descriptor > > > > >,

    property < edge_capacity_t, long long,
    property < edge_residual_capacity_t, long long,
    property < edge_reverse_t, Traits::edge_descriptor > > > > Graph;
  Graph g;
  property_map < Graph, edge_reverse_t >::type rev;
};

#endif


#include <iostream>
#include <fstream>
#include <boost/program_options.hpp>

#include "atom.h"
#include "utils.h"
#include "solution.h"
#include "colours.h"
#include "gutenberg.h"
#include "generalinputs.h"
#include "collectivevariable.h"
#include "reading.h"
#include "parseCLI.h"
#include "customException.hpp"

using namespace std;

int main ( int argc, char **argv ) {
    generalInputs inps;

  try {
    boost::program_options::variables_map clioMap=parseCLiandConfig(argc,argv,&inps);
    if (clioMap.count("help")) return SUCCESS;
    if (clioMap.count("version")) {
      std::cout<<colours::green<<"version: "<<inps.version<<colours::end<<"\n";
      return SUCCESS;
    }

    std::cout<<colours::green<<"Echo the options used\n"<<colours::end;
    std::string debugFile=inps.configFile+".dbg";
    printOptions(&clioMap,&debugFile);
  }
  catch(customException &e) {
    std::cerr<<colours::red<<"Error: "<< e.what() << colours::end<<"\n";
    return e.id;
  }

    if ( inps.seeds.size() ==0 ) inps.seeds.push_back ( inps.seed );
    inps.cut=min<double> ( inps.cut,inps.box/2.0 );
    vector<atom> a;
    inps.log.open ( inps.logf.c_str(),ios::out );
    gutenberg::timeStamp ( "start time:",inps.log );
    for ( int i=0; i<argc; i++ ) {
        utils::Print ( argv[i],inps.log,colours::yellow );
        }
    inps.log<<endl;
    if ( inps.sampler== "VV" ) {
        solution::InitializeSimulation ( a,inps.T0,inps.box,inps.seeds[0], inps.filename );
        gutenberg::logInput ( inps );
        solution::MDDriverVV ( a,inps );
        }
    else if ( inps.sampler== "VEC" ) {
        solution::InitializeSimulation ( a,inps.T0,inps.box,inps.seeds[0], inps.filename,inps.gamma );
        gutenberg::logInput ( inps );
        solution::MDDriverVEC ( a,inps );
        }
    else if ( inps.sampler== "TAMD" ) {
        solution::InitializeSimulation ( a,inps.T0,inps.box,inps.seeds[0], inps.filename,inps.gamma );
        vector<colVar> cv ( 2 );
        cv[0].mu=inps.mu;
        cv[0].gamma=inps.gCV;
        cv[0].k=inps.zk;
        cv[0].i=1;
        cv[0].j=2;
        cv[0].z=cv[0].theta ( a,inps.box );
        cv[0].th=cv[0].z;
        cv[0].v=0.0;
        cv[0].f=0.0;

        cv[1].mu=inps.mu;
        cv[1].gamma=inps.gCV;
        cv[1].k=inps.zk;
        cv[1].i=5;
        cv[1].j=6;
        cv[1].z=cv[1].theta ( a,inps.box );
        cv[1].th=cv[1].z;
        cv[1].v=0.0;
        cv[1].f=0.0;
        gutenberg::logInput ( inps );
        gutenberg::logCV ( cv,inps.log );
        solution::MDDriverTAMD ( a,cv,inps );
        }
    else if ( inps.sampler== "MMC" ) {
        solution::InitializeSimulation ( a,inps.T0,inps.box,inps.seeds[0], inps.filename,inps.gamma );
        // inps.Temperature should contain the temperature of the system... so be sure it is computed if not an input
        gutenberg::logInput ( inps );
//        double vir;
//         int i=105;
//         double en=a[i].myPotentialEnergy ( a,inps.cut,inps.box,inps.shift,vir);
//         utils::Print(en,cout,32,16);
//         cout <<endl;
//         vector<atom> b;
//
//         int l=reading::readXYZ("fort.111",b);
//         for(int k=0;k<b.size();k++) b[k].setID(k);
//         double em=b[i].myPotentialEnergy ( a,inps.cut,inps.box,inps.shift,vir);
//         utils::Print(em,cout,32,16);cout<<endl;
        solution::MMCDriver ( a,inps );
        }
    else if ( inps.sampler== "TAMC" ) {
        solution::InitializeSimulation ( a,inps.T0,inps.box,inps.seeds[0], inps.filename,inps.gamma );
        vector<colVar> cv ( 2 );
        cv[0].mu=inps.mu;
        cv[1].mu=inps.mu;
        cv[0].gamma=inps.gCV;
        cv[1].gamma=inps.gCV;
        cv[0].k=50.0;
        cv[1].k=50.0;
        cv[0].i=1;
        cv[0].j=2;
        cv[1].i=5;
        cv[1].j=6;
        cv[0].z=cv[0].theta ( a,inps.box );
        cv[1].z=cv[1].theta ( a,inps.box );
        cv[0].th=cv[0].z;
        cv[1].th=cv[1].z;
        cv[0].v=0.0;
        cv[1].v=0.0;
        cv[0].f=0.0;
        cv[1].f=0.0;
        gutenberg::logInput ( inps );
        solution::MDDriverTAMC ( a,cv,inps );
        }
    else if ( inps.sampler== "TAHMC" ) {
        solution::InitializeSimulation ( a,inps.T0,inps.box,inps.seeds[0], inps.filename,inps.gamma );
        vector<colVar> cv ( 2 );
        cv[0].mu=inps.mu;
        cv[1].mu=inps.mu;
        cv[0].gamma=inps.gCV;
        cv[1].gamma=inps.gCV;
        cv[0].k=50.0;
        cv[1].k=50.0;
        cv[0].i=1;
        cv[0].j=2;
        cv[1].i=5;
        cv[1].j=6;
        cv[0].z=cv[0].theta ( a,inps.box );
        cv[1].z=cv[1].theta ( a,inps.box );
        cv[0].th=cv[0].z;
        cv[1].th=cv[1].z;
        cv[0].v=0.0;
        cv[1].v=0.0;
        cv[0].f=0.0;
        cv[1].f=0.0;
        gutenberg::logInput ( inps );
        solution::MDDriverTAHMC ( a,cv,inps );
        }
    else {
        solution::GenerateLattice ( a,inps.rho,inps.mass, inps.N,inps.box,inps.el );
        gutenberg::printXYZ ( inps.xyz,utils::stringPlusDouble ( "LJ box ",inps.box ),a,true );

        utils::Print ( "Lattice generated\n", cout,colours::yellow );
        utils::Print ( "use -h for more info on usage\n", cout,colours::red );
        }
    gutenberg::timeStamp ( "end time:",inps.log );
    inps.log.close();
    return 0;
    }
// kate: indent-mode cstyle; space-indent on; indent-width 4;


//
//=======================================================================
// 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 <bits/stdc++.h>
#include <armadillo>

using namespace std;

class Probability_table{
public:
    int rows,cols;
    arma::mat table;

    //Member functions
    Probability_table(int rows,int cols);
    void view();
};


/* Greg Anderson
 * Define a neural network.
 */

#ifndef _EIGEN_WRAPPER_H_
#define _EIGEN_WRAPPER_H_

#include <cstdlib>
#include <string>
#include <vector>
#include <memory>

#include <Eigen/Dense>

/* A tensor is represented as a `std::vector<Eigen::MatrixXd>`.
*/
typedef Eigen::VectorXd Vec;
typedef Eigen::MatrixXd Mat;
typedef std::vector<Mat> Tensor;

static std::string vec_to_str(Vec& v) {
    if (v.size()<=0) return "()";
    std::stringstream ss;
    ss << "(" << v(0);
    for (unsigned int i = 1; i < v.size(); i++) {
        ss << " " << v(i);
        // ss << "," << v(i);
    }
    ss << ")";
    return ss.str();
}

static std::string mat_to_str(Mat& m) {
    if (m.cols()*m.rows()<=0)return "[]";
    std::stringstream ss;
    ss << "[";
    for (int i = 0; i < m.rows(); i++) {
        ss << "Row" << i << ":(" << m(i,0);
        for (int j = 1; j < m.cols(); j++) {
            ss << " " << m(i,j);
        }
        ss << ")   ";
    }
    ss << "]";
    return ss.str();
}

#endif