//ros
#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseArray.h>

//ipopt
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>

// kinematic model wheel_velocity = forward_matrix * robot_velocity
Eigen::MatrixXd forward_matrix;
Eigen::MatrixXd inversed_matrix;
Eigen::VectorXd wheel_velocity;
Eigen::Vector3d robot_velocity;

//https://robotics.naist.jp/edu/text/?Robotics%2FEigen#b3b26d13
template <typename t_matrix>
t_matrix PseudoInverse(const t_matrix& m, const double &tolerance=1.e-6)
{
  using namespace Eigen;
  typedef JacobiSVD<t_matrix> TSVD;
  unsigned int svd_opt(ComputeThinU | ComputeThinV);
  if(m.RowsAtCompileTime!=Dynamic || m.ColsAtCompileTime!=Dynamic)
  svd_opt= ComputeFullU | ComputeFullV;
  TSVD svd(m, svd_opt);
  const typename TSVD::SingularValuesType &sigma(svd.singularValues());
  typename TSVD::SingularValuesType sigma_inv(sigma.size());
  for(long i=0; i<sigma.size(); ++i)
  {
    if(sigma(i) > tolerance)
      sigma_inv(i)= 1.0/sigma(i);
    else
      sigma_inv(i)= 0.0;
  }
  return svd.matrixV()*sigma_inv.asDiagonal()*svd.matrixU().transpose();
}

using CppAD::AD;

class MPC{
public:
  MPC();

  // state, ref_x, ref_y, ref_yaw
  std::vector<double> solve(Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd);

};

class FG_eval{
public:
  FG_eval(Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd);

  typedef CPPAD_TESTVECTOR(AD<double>) ADvector;

  void operator()(ADvector&, const ADvector&);

private:
  Eigen::VectorXd ref_x;
  Eigen::VectorXd ref_y;
  Eigen::VectorXd ref_yaw;

};

class MPCPathTracker
{
public:
  MPCPathTracker(void);

  void path_callback(const nav_msgs::PathConstPtr&);

  void process(void);
  void path_to_vector(void);

private:
  ros::NodeHandle nh;
  ros::Publisher velocity_pub;
  ros::Publisher path_pub;
  ros::Subscriber path_sub;
  MPC mpc;
  nav_msgs::Path path;
  tf::TransformListener listener;
  geometry_msgs::PoseStamped current_pose;
  geometry_msgs::PoseStamped previous_pose;
  tf::StampedTransform _transform;
  geometry_msgs::TransformStamped transform;
  Eigen::VectorXd path_x;
  Eigen::VectorXd path_y;
  Eigen::VectorXd path_yaw;
  bool first_transform = true;
  double last_time;
  geometry_msgs::Twist velocity;

};

// ホライゾン長さ
int T = 5;
// 周期
double DT = 0.1;// [s]
const double HZ = 10;
// 目標速度
double VREF = 0.5;// [m/s]
// 最大角速度
double MAX_ANGULAR_VELOCITY = 1.0;// [rad/s]
// ホイール角加速度
double MAX_WHEEL_ANGULAR_ACCELERATION = 60;// [rad/s^2]
// ホイール角速度
double MAX_WHEEL_ANGULAR_VELOCITY = 30;// [rad/s]
// ホイール半径
double WHEEL_RADIUS = 0.1;// [m]
// トレッド
double TREAD = 0.4;// [m]
// ホイールベース
double WHEEL_BASE = 0.4;// [m]
// グリッドマップ分解能
double RESOLUTION = 0.1;// [m]
// ロボットの足まわり半径
double ROBOT_RADIUS;
// 足回り配置
double ROBOT_THETA;
// ステア角度制限
double MAX_STEERING_ANGLE = M_PI * 2. / 3.;// [rad]
// 最高速度
double MAX_VELOCITY = 1.5;// [m/s]

std::string WORLD_FRAME;
std::string ROBOT_FRAME;
std::string VELOCITY_TOPIC_NAME;
std::string INTERMEDIATE_PATH_TOPIC_NAME;

// state
size_t x_start = 0;
size_t y_start = x_start + T;
size_t yaw_start = y_start + T;
size_t vx_start = yaw_start + T;
size_t vy_start = vx_start + T;
size_t omega_start = vy_start + T;
size_t omega_w_fr_start = omega_start + T;
size_t omega_w_fl_start = omega_w_fr_start + T;
size_t omega_w_rr_start = omega_w_fl_start + T;
size_t omega_w_rl_start = omega_w_rr_start + T;
size_t theta_s_fr_start = omega_w_rl_start + T;
size_t theta_s_fl_start = theta_s_fr_start + T;
size_t theta_s_rr_start = theta_s_fl_start + T;
size_t theta_s_rl_start = theta_s_rr_start + T;
// input
size_t domega_w_fr_start = theta_s_rl_start + T;
size_t domega_w_fl_start = domega_w_fr_start + T - 1;
size_t domega_w_rr_start = domega_w_fl_start + T - 1;
size_t domega_w_rl_start = domega_w_rr_start + T - 1;
size_t dtheta_s_fr_start = domega_w_rl_start + T - 1;
size_t dtheta_s_fl_start = dtheta_s_fr_start + T - 1;
size_t dtheta_s_rr_start = dtheta_s_fl_start + T - 1;
size_t dtheta_s_rl_start = dtheta_s_rr_start + T - 1;

// 最適化失敗時は最後の成功データを使う
int failure_count = 0;
std::vector<double> result;
std::vector<double> solution_buffer;

double min_distance(nav_msgs::Path&, geometry_msgs::PoseStamped&);
double get_distance(geometry_msgs::PoseStamped&, geometry_msgs::PoseStamped&);

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

  /*
  local_nh.getParam("HORIZON_T", T);
  */
  local_nh.getParam("/dynamic_avoidance/VREF", VREF);
  local_nh.getParam("/dynamic_avoidance/MAX_ANGULAR_VELOCITY", MAX_ANGULAR_VELOCITY);
  local_nh.getParam("/dynamic_avoidance/RESOLUTION", RESOLUTION);
  local_nh.getParam("/dynamic_avoidance/ROBOT_FRAME", ROBOT_FRAME);
  local_nh.getParam("/dynamic_avoidance/WORLD_FRAME", WORLD_FRAME);
  local_nh.getParam("/dynamic_avoidance/VELOCITY_TOPIC_NAME", VELOCITY_TOPIC_NAME);
  local_nh.getParam("/dynamic_avoidance/INTERMEDIATE_PATH_TOPIC_NAME", INTERMEDIATE_PATH_TOPIC_NAME);
  local_nh.getParam("/fwdis/WHEEL_RADIUS", WHEEL_RADIUS);
  local_nh.getParam("/fwdis/WHEEL_BASE", WHEEL_BASE);
  local_nh.getParam("/fwdis/TREAD", TREAD);
  local_nh.getParam("/fwdis/MAX_WHEEL_ANGULAR_VELOCITY", MAX_WHEEL_ANGULAR_VELOCITY);
  local_nh.getParam("/fwdis/MAX_WHEEL_ANGULAR_ACCELERATION", MAX_WHEEL_ANGULAR_ACCELERATION);
  local_nh.getParam("/fwdis/MAX_VELOCITY", MAX_VELOCITY);
  local_nh.getParam("/fwdis/MAX_STEERING_ANGLE", MAX_STEERING_ANGLE);

  std::cout << "T: " << T << std::endl;
  std::cout << "VREF: " << VREF << std::endl;
  std::cout << "MAX_ANGULAR_VELOCITY: " << MAX_ANGULAR_VELOCITY << std::endl;
  std::cout << "RESOLUTION: " << RESOLUTION << std::endl;
  std::cout << "ROBOT_FRAME: " <<  ROBOT_FRAME << std::endl;
  std::cout << "WORLD_FRAME: " << WORLD_FRAME << std::endl;
  std::cout << "VELOCITY_TOPIC_NAME: " << VELOCITY_TOPIC_NAME << std::endl;
  std::cout << "INTERMEDIATE_PATH_TOPIC_NAME: " << INTERMEDIATE_PATH_TOPIC_NAME << std::endl;
  std::cout << "WHEEL_RADIUS: " << WHEEL_RADIUS << std::endl;
  std::cout << "WHEEL_BASE: " << WHEEL_BASE << std::endl;
  std::cout << "TREAD: " << TREAD << std::endl;
  std::cout << "MAX_WHEEL_ANGULAR_VELOCITY: " << MAX_WHEEL_ANGULAR_VELOCITY << std::endl;
  std::cout << "MAX_WHEEL_ANGULAR_ACCELERATION: " << MAX_WHEEL_ANGULAR_ACCELERATION << std::endl;
  std::cout << "MAX_VELOCITY: " << MAX_VELOCITY << std::endl;
  std::cout << "MAX_STEERING_ANGLE: " << MAX_STEERING_ANGLE << std::endl;

  ROBOT_RADIUS = sqrt(pow(WHEEL_BASE, 2) + pow(TREAD, 2)) / 2.0;
  ROBOT_THETA = atan(TREAD / WHEEL_BASE);
  forward_matrix.resize(8, 3);
  forward_matrix << 1.0, 0.0,  ROBOT_RADIUS * cos(ROBOT_THETA),
                    0.0, 1.0,  ROBOT_RADIUS * sin(ROBOT_THETA),
                    1.0, 0.0, -ROBOT_RADIUS * cos(ROBOT_THETA),
                    0.0, 1.0,  ROBOT_RADIUS * sin(ROBOT_THETA),
                    1.0, 0.0, -ROBOT_RADIUS * cos(ROBOT_THETA),
                    0.0, 1.0, -ROBOT_RADIUS * sin(ROBOT_THETA),
                    1.0, 0.0,  ROBOT_RADIUS * cos(ROBOT_THETA),
                    0.0, 1.0, -ROBOT_RADIUS * sin(ROBOT_THETA);

  inversed_matrix.resize(3, 8);
  inversed_matrix = PseudoInverse(forward_matrix);

  wheel_velocity.resize(8, 1);

  std::cout << forward_matrix << std::endl;
  std::cout << inversed_matrix << std::endl;

  MPCPathTracker mpc_path_tracker;

  ros::Rate loop_rate(HZ);

  while(ros::ok()){
    mpc_path_tracker.process();

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

MPC::MPC(){}

std::vector<double> MPC::solve(Eigen::VectorXd state, Eigen::VectorXd ref_x, Eigen::VectorXd ref_y, Eigen::VectorXd ref_yaw)
{
  /*
   * state:x, y, yaw, vx, vy, omega, omega_w_fr, omega_w_fl, omega_w_rr, omega_w_rl, theta_s_fr, theta_s_fl, theta_s_rr, theta_s_rl
   */
  bool ok = true;
  size_t i;
  typedef CPPAD_TESTVECTOR(double) Dvector;

  double x = state[0];
  double y = state[1];
  double yaw = state[2];
  double vx = state[3];
  double vy = state[4];
  double omega = state[5];
  double omega_w_fr = state[6];
  double omega_w_fl = state[7];
  double omega_w_rr = state[8];
  double omega_w_rl = state[9];
  double theta_s_fr = state[10];
  double theta_s_fl = state[11];
  double theta_s_rr = state[12];
  double theta_s_rl = state[13];

  /*
  std::cout << "--- state ---" << std::endl;
  std::cout << state << std::endl;
  std::cout << "--- path_x ---" << std::endl;
  std::cout << ref_x << std::endl;
  std::cout << "--- path_y ---" << std::endl;
  std::cout << ref_y << std::endl;
  std::cout << "--- path_yaw ---" << std::endl;
  std::cout << ref_yaw << std::endl;
  */

  // 14(state), 8(input)
  size_t n_variables = 14 * T + 8 * (T - 1);

  size_t n_constraints = 14 * T;

  Dvector vars(n_variables);
  for(int i=0;i<n_variables;i++){
    vars[i] = 0.0;
  }

  vars[x_start] = x;
  vars[y_start] = y;
  vars[yaw_start] = yaw;
  vars[vx_start] = vx;
  vars[vy_start] = vy;
  vars[omega_start] = omega;
  vars[omega_w_fr_start] = omega_w_fr;
  vars[omega_w_fl_start] = omega_w_fl;
  vars[omega_w_rr_start] = omega_w_rr;
  vars[omega_w_rl_start] = omega_w_rl;
  vars[theta_s_fr_start] = theta_s_fr;
  vars[theta_s_fl_start] = theta_s_fl;
  vars[theta_s_rr_start] = theta_s_rr;
  vars[theta_s_rl_start] = theta_s_rl;

  Dvector vars_lower_bound(n_variables);
  Dvector vars_upper_bound(n_variables);

  for(int i=0;i<yaw_start;i++){
    // x, y
    vars_lower_bound[i] = -1.0e19;
    vars_upper_bound[i] = 1.0e19;
  }
  for(int i=yaw_start;i<vx_start;i++){
    // yaw
    vars_lower_bound[i] = -1.0e19;
    vars_upper_bound[i] = 1.0e19;
  }
  for(int i=vx_start;i<vy_start;i++){
    // vx
    vars_lower_bound[i] = 0;
    vars_upper_bound[i] = MAX_VELOCITY;
  }
  for(int i=vy_start;i<omega_start;i++){
    // vy
    vars_lower_bound[i] = -MAX_VELOCITY;
    vars_upper_bound[i] = MAX_VELOCITY;
  }
  for(int i=omega_start;i<omega_w_fr_start;i++){
    // omega
    vars_lower_bound[i] = -MAX_ANGULAR_VELOCITY;
    vars_upper_bound[i] = MAX_ANGULAR_VELOCITY;
  }
  for(int i=omega_w_fr_start;i<theta_s_fr_start;i++){
    vars_lower_bound[i] = -MAX_WHEEL_ANGULAR_VELOCITY;
    vars_upper_bound[i] = MAX_WHEEL_ANGULAR_VELOCITY;
  }
  for(int i=theta_s_fr_start;i<domega_w_fr_start;i++){
    vars_lower_bound[i] = -MAX_STEERING_ANGLE;
    vars_upper_bound[i] = MAX_STEERING_ANGLE;
  }
  for(int i=domega_w_fr_start;i<dtheta_s_fr_start;i++){
    vars_lower_bound[i] = -MAX_WHEEL_ANGULAR_ACCELERATION;
    vars_upper_bound[i] = MAX_WHEEL_ANGULAR_ACCELERATION;
  }
  for(int i=dtheta_s_fr_start;i<n_variables;i++){
    // 適当
    vars_lower_bound[i] = -MAX_WHEEL_ANGULAR_VELOCITY * 23.1 / 56.1;
    vars_upper_bound[i] = MAX_WHEEL_ANGULAR_VELOCITY * 23.1 / 56.1;
  }

  // 等式制約
  Dvector constraints_lower_bound(n_constraints);
  Dvector constraints_upper_bound(n_constraints);

  for(int i=0;i<n_constraints;i++){
    constraints_lower_bound[i] = 0.0;
    constraints_upper_bound[i] = 0.0;
  }

  // t=0の設定
  constraints_lower_bound[x_start] = x;
  constraints_lower_bound[y_start] = y;
  constraints_lower_bound[yaw_start] = yaw;
  constraints_lower_bound[vx_start] = vx;
  constraints_lower_bound[vy_start] = vy;
  constraints_lower_bound[omega_start] = omega;
  constraints_lower_bound[omega_w_fr_start] = omega_w_fr;
  constraints_lower_bound[omega_w_fl_start] = omega_w_fl;
  constraints_lower_bound[omega_w_rr_start] = omega_w_rr;
  constraints_lower_bound[omega_w_rl_start] = omega_w_rl;
  constraints_lower_bound[theta_s_fr_start] = theta_s_fr;
  constraints_lower_bound[theta_s_fl_start] = theta_s_fl;
  constraints_lower_bound[theta_s_rr_start] = theta_s_rr;
  constraints_lower_bound[theta_s_rl_start] = theta_s_rl;

  constraints_upper_bound[x_start] = x;
  constraints_upper_bound[y_start] = y;
  constraints_upper_bound[yaw_start] = yaw;
  constraints_upper_bound[vx_start] = vx;
  constraints_upper_bound[vy_start] = vy;
  constraints_upper_bound[omega_start] = omega;
  constraints_upper_bound[omega_w_fr_start] = omega_w_fr;
  constraints_upper_bound[omega_w_fl_start] = omega_w_fl;
  constraints_upper_bound[omega_w_rr_start] = omega_w_rr;
  constraints_upper_bound[omega_w_rl_start] = omega_w_rl;
  constraints_upper_bound[theta_s_fr_start] = theta_s_fr;
  constraints_upper_bound[theta_s_fl_start] = theta_s_fl;
  constraints_upper_bound[theta_s_rr_start] = theta_s_rr;
  constraints_upper_bound[theta_s_rl_start] = theta_s_rl;

  FG_eval fg_eval(ref_x, ref_y, ref_yaw);

  std::string options;
  options += "Integer print_level  0\n";

  options += "Sparse  true        forward\n";
  options += "Sparse  true        reverse\n";

  options += "Numeric max_cpu_time          0.5\n";

  CppAD::ipopt::solve_result<Dvector> solution;

  std::cout << "optimization start" << std::endl;
  CppAD::ipopt::solve<Dvector, FG_eval>(
      options, vars, vars_lower_bound, vars_upper_bound, constraints_lower_bound,
      constraints_upper_bound, fg_eval, solution);

  std::cout << "optimization end" << std::endl;
  ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
  std::cout << solution.status << std::endl;
  std::cout << ok << std::endl;

  auto cost = solution.obj_value;
  std::cout << "Cost " << cost << std::endl;

  std::cout << "solution.x.size() " << solution.x.size() << std::endl;

  if(ok){
    result.clear();
    failure_count = 0;
    // 何故か0だとうまく行かない
    result.push_back(solution.x[vx_start+1]);
    result.push_back(solution.x[vy_start+1]);
    result.push_back(solution.x[omega_start+1]);
    //予測軌道
    for(int i = 0; i < T-1; i++){
      result.push_back(solution.x[x_start+i+1]);
      result.push_back(solution.x[y_start+i+1]);
      result.push_back(solution.x[yaw_start+i+1]);
    }
    int size = solution.x.size();
    solution_buffer.reserve(size);
    for(int i=0;i<size;i++){
      solution_buffer[i] = solution.x[i];
    }
  }else{
    if(failure_count < T - 1){
      failure_count++;
      std::cout << "fail:" << failure_count << std::endl;
    }
    // いいのか不明
    if(solution_buffer.size() > 0){
      result.push_back(solution_buffer[vx_start+1+failure_count]);
      result.push_back(solution_buffer[vy_start+1+failure_count]);
      result.push_back(solution_buffer[omega_start+1+failure_count]);
      //予測軌道
      for(int i = failure_count; i < T-1; i++){
        result.push_back(solution.x[x_start+i+1]);
        result.push_back(solution.x[y_start+i+1]);
        result.push_back(solution.x[yaw_start+i+1]);
      }
    }else{
      for(int i=0;i<3+3*(T-1);i++){
        result.push_back(0);
      }
    }
    /*
    if(solution.status == CppAD::ipopt::solve_result<Dvector>::unknown){
      result[0] = 0;
      result[1] = 0;
      result[2] = 0;
      std::exit(1);
    }
    std::cout << "cheat" << std::endl;
    */
  }
  std::cout << "--- result ---" << std::endl;
  /*
  for(int i=0;i<result.size();i++){
    std::cout << result[i] << std::endl;
  }
  */
  std::cout << solution_buffer[x_start+1] << std::endl;
  std::cout << solution_buffer[y_start+1] << std::endl;
  std::cout << solution_buffer[yaw_start+1] << std::endl;
  std::cout << solution_buffer[vx_start+1] << std::endl;
  std::cout << solution_buffer[vy_start+1] << std::endl;
  std::cout << solution_buffer[omega_start+1] << std::endl;
  std::cout << solution_buffer[omega_w_fr_start+1] << std::endl;
  std::cout << solution_buffer[omega_w_fl_start+1] << std::endl;
  std::cout << solution_buffer[omega_w_rr_start+1] << std::endl;
  std::cout << solution_buffer[omega_w_rl_start+1] << std::endl;
  std::cout << solution_buffer[theta_s_fr_start+1] << std::endl;
  std::cout << solution_buffer[theta_s_fl_start+1] << std::endl;
  std::cout << solution_buffer[theta_s_rr_start+1] << std::endl;
  std::cout << solution_buffer[theta_s_rl_start+1] << std::endl;
  return result;
}

FG_eval::FG_eval(Eigen::VectorXd ref_x, Eigen::VectorXd ref_y, Eigen::VectorXd ref_yaw)
{
  this->ref_x = ref_x;
  this->ref_y = ref_y;
  this->ref_yaw = ref_yaw;
}

void FG_eval::operator()(ADvector& fg, const ADvector& vars)
{
  std::cout << "FG_eval() start" << std::endl;
  // cost
  fg[0] = 0;
  // state
  for(int i=0;i<T-1;i++){
    // pathとの距離
    fg[0] += 10 * (CppAD::pow(vars[x_start + i] - ref_x[i], 2) + CppAD::pow(vars[y_start + i] - ref_y[i], 2));
    // 向き
    fg[0] += 10 * CppAD::pow(vars[yaw_start + i] - ref_yaw[i], 2);
    // 速度
    fg[0] += 5 * CppAD::pow(CppAD::pow(VREF, 2) - CppAD::pow(vars[vx_start + i], 2) - CppAD::pow(vars[vy_start + i], 2), 2);
    // 角加速度
    fg[0] += 1 * CppAD::pow(vars[omega_start + i] - vars[omega_start + i + 1], 2);
    // 角速度
    fg[0] += 1 * CppAD::pow(vars[omega_start + i], 2);
  }
  // input
  for(int i=0;i<T-2;i++){
    fg[0] += 1e-4 * CppAD::pow(vars[domega_w_fr_start + i], 2);
    fg[0] += 1e-4 * CppAD::pow(vars[domega_w_fl_start + i], 2);
    fg[0] += 1e-4 * CppAD::pow(vars[domega_w_rr_start + i], 2);
    fg[0] += 1e-4 * CppAD::pow(vars[domega_w_rl_start + i], 2);
    fg[0] += 1e-4 * CppAD::pow(vars[dtheta_s_fr_start + i], 2);
    fg[0] += 1e-4 * CppAD::pow(vars[dtheta_s_fl_start + i], 2);
    fg[0] += 1e-4 * CppAD::pow(vars[dtheta_s_rr_start + i], 2);
    fg[0] += 1e-4 * CppAD::pow(vars[dtheta_s_rl_start + i], 2);
  }

  std::cout << "constrains start" << std::endl;
  //constraint
  //初期状態
  fg[1 + x_start] = vars[x_start];
  fg[1 + y_start] = vars[y_start];
  fg[1 + yaw_start] = vars[yaw_start];
  fg[1 + vx_start] = vars[vx_start];
  fg[1 + vy_start] = vars[vy_start];
  fg[1 + omega_start] = vars[omega_start];
  fg[1 + omega_w_fr_start] = vars[omega_w_fr_start];
  fg[1 + omega_w_fl_start] = vars[omega_w_fl_start];
  fg[1 + omega_w_rr_start] = vars[omega_w_rr_start];
  fg[1 + omega_w_rl_start] = vars[omega_w_rl_start];
  fg[1 + theta_s_fr_start] = vars[theta_s_fr_start];
  fg[1 + theta_s_fl_start] = vars[theta_s_fl_start];
  fg[1 + theta_s_rr_start] = vars[theta_s_rr_start];
  fg[1 + theta_s_rl_start] = vars[theta_s_rl_start];

  std::cout << "constraints loop start" << std::endl;

  for(int i=0;i<T-1;i++){
    //t+1
    AD<double> x1 = vars[x_start + i + 1];
    AD<double> y1 = vars[y_start + i + 1];
    AD<double> yaw1 = vars[yaw_start + i + 1];
    AD<double> vx1 = vars[vx_start + i + 1];
    AD<double> vy1 = vars[vy_start + i + 1];
    AD<double> omega1 = vars[omega_start + i + 1];
    AD<double> omega_w_fr1 = vars[omega_w_fr_start + i + 1];
    AD<double> omega_w_fl1 = vars[omega_w_fl_start + i + 1];
    AD<double> omega_w_rr1 = vars[omega_w_rr_start + i + 1];
    AD<double> omega_w_rl1 = vars[omega_w_rl_start + i + 1];
    AD<double> theta_s_fr1 = vars[theta_s_fr_start + i + 1];
    AD<double> theta_s_fl1 = vars[theta_s_fl_start + i + 1];
    AD<double> theta_s_rr1 = vars[theta_s_rr_start + i + 1];
    AD<double> theta_s_rl1 = vars[theta_s_rl_start + i + 1];
    //t
    AD<double> x0 = vars[x_start + i];
    AD<double> y0 = vars[y_start + i];
    AD<double> yaw0 = vars[yaw_start + i];
    AD<double> vx0 = vars[vx_start + i];
    AD<double> vy0 = vars[vy_start + i];
    AD<double> omega0 = vars[omega_start + i];
    AD<double> omega_w_fr0 = vars[omega_w_fr_start + i];
    AD<double> omega_w_fl0 = vars[omega_w_fl_start + i];
    AD<double> omega_w_rr0 = vars[omega_w_rr_start + i];
    AD<double> omega_w_rl0 = vars[omega_w_rl_start + i];
    AD<double> theta_s_fr0 = vars[theta_s_fr_start + i];
    AD<double> theta_s_fl0 = vars[theta_s_fl_start + i];
    AD<double> theta_s_rr0 = vars[theta_s_rr_start + i];
    AD<double> theta_s_rl0 = vars[theta_s_rl_start + i];
    //入力ホライゾンはt+1を考慮しない
    AD<double> domega_w_fr0 = vars[domega_w_fr_start + i];
    AD<double> domega_w_fl0 = vars[domega_w_fl_start + i];
    AD<double> domega_w_rr0 = vars[domega_w_rr_start + i];
    AD<double> domega_w_rl0 = vars[domega_w_rl_start + i];
    AD<double> dtheta_s_fr0 = vars[dtheta_s_fr_start + i];
    AD<double> dtheta_s_fl0 = vars[dtheta_s_fl_start + i];
    AD<double> dtheta_s_rr0 = vars[dtheta_s_rr_start + i];
    AD<double> dtheta_s_rl0 = vars[dtheta_s_rl_start + i];

    //制約
    fg[2 + x_start + i] = x1 - (x0 + (vx0 * CppAD::cos(yaw0) - vy0 * CppAD::sin(yaw0)) * DT);
    fg[2 + y_start + i] = y1 - (y0 + (vx0 * CppAD::sin(yaw0) + vy0 * CppAD::cos(yaw0)) * DT);
    fg[2 + yaw_start + i] = yaw1 - (yaw0 + omega0 * DT);
    fg[2 + omega_w_fr_start + i] = omega_w_fr1 - (omega_w_fr0 + domega_w_fr0 * DT);
    fg[2 + omega_w_fl_start + i] = omega_w_fl1 - (omega_w_fl0 + domega_w_fl0 * DT);
    fg[2 + omega_w_rr_start + i] = omega_w_rr1 - (omega_w_rr0 + domega_w_rr0 * DT);
    fg[2 + omega_w_rl_start + i] = omega_w_rl1 - (omega_w_rl0 + domega_w_rl0 * DT);
    fg[2 + theta_s_fr_start + i] = theta_s_fr1 - (theta_s_fr0 + dtheta_s_fr0 * DT);
    fg[2 + theta_s_fl_start + i] = theta_s_fl1 - (theta_s_fl0 + dtheta_s_fl0 * DT);
    fg[2 + theta_s_rr_start + i] = theta_s_rr1 - (theta_s_rr0 + dtheta_s_rr0 * DT);
    fg[2 + theta_s_rl_start + i] = theta_s_rl1 - (theta_s_rl0 + dtheta_s_rl0 * DT);
    AD<double> v_w_fr1 = WHEEL_RADIUS * omega_w_fr1;
    AD<double> v_w_fl1 = WHEEL_RADIUS * omega_w_fl1;
    AD<double> v_w_rr1 = WHEEL_RADIUS * omega_w_rr1;
    AD<double> v_w_rl1 = WHEEL_RADIUS * omega_w_rl1;
    fg[2 + vx_start + i] = vx1 - (inversed_matrix(0, 0) * v_w_fr1 * CppAD::cos(theta_s_fr1) + inversed_matrix(0, 1) * v_w_fr1 * CppAD::sin(theta_s_fr1) + inversed_matrix(0, 2) * v_w_fl1 * CppAD::cos(theta_s_fl1) + inversed_matrix(0, 3) * v_w_fl1 * CppAD::sin(theta_s_fl1) + inversed_matrix(0, 4) * v_w_rl1 * CppAD::cos(theta_s_rl1) + inversed_matrix(0, 5) * v_w_rl1 * CppAD::sin(theta_s_rl1) + inversed_matrix(0, 6) * v_w_rr1 * CppAD::cos(theta_s_rr1) + inversed_matrix(0, 7) * v_w_rr1 * CppAD::sin(theta_s_rr1));
    fg[2 + vy_start + i] = vy1 - (inversed_matrix(1, 0) * v_w_fr1 * CppAD::cos(theta_s_fr1) + inversed_matrix(1, 1) * v_w_fr1 * CppAD::sin(theta_s_fr1) + inversed_matrix(1, 2) * v_w_fl1 * CppAD::cos(theta_s_fl1) + inversed_matrix(1, 3) * v_w_fl1 * CppAD::sin(theta_s_fl1) + inversed_matrix(1, 4) * v_w_rl1 * CppAD::cos(theta_s_rl1) + inversed_matrix(1, 5) * v_w_rl1 * CppAD::sin(theta_s_rl1) + inversed_matrix(1, 6) * v_w_rr1 * CppAD::cos(theta_s_rr1) + inversed_matrix(1, 7) * v_w_rr1 * CppAD::sin(theta_s_rr1));
    fg[2 + omega_start + i] = omega1 - (inversed_matrix(2, 0) * v_w_fr1 * CppAD::cos(theta_s_fr1) + inversed_matrix(2, 1) * v_w_fr1 * CppAD::sin(theta_s_fr1) + inversed_matrix(2, 2) * v_w_fl1 * CppAD::cos(theta_s_fl1) + inversed_matrix(2, 3) * v_w_fl1 * CppAD::sin(theta_s_fl1) + inversed_matrix(2, 4) * v_w_rl1 * CppAD::cos(theta_s_rl1) + inversed_matrix(2, 5) * v_w_rl1 * CppAD::sin(theta_s_rl1) + inversed_matrix(2, 6) * v_w_rr1 * CppAD::cos(theta_s_rr1) + inversed_matrix(2, 7) * v_w_rr1 * CppAD::sin(theta_s_rr1));
  }
  std::cout << "FG_eval() end" << std::endl;
}

MPCPathTracker::MPCPathTracker(void)
{
  velocity_pub = nh.advertise<geometry_msgs::Twist>(VELOCITY_TOPIC_NAME, 100);
  path_pub = nh.advertise<geometry_msgs::PoseArray>("/mpc_path", 100);
  path_sub = nh.subscribe(INTERMEDIATE_PATH_TOPIC_NAME, 100, &MPCPathTracker::path_callback, this);
  path_x = Eigen::VectorXd::Zero(T);
  path_y = Eigen::VectorXd::Zero(T);
  path_yaw = Eigen::VectorXd::Zero(T);
}

void MPCPathTracker::path_callback(const nav_msgs::PathConstPtr& msg)
{
  path = *msg;
}

void MPCPathTracker::process(void)
{
  std::cout << "=== fwdis mpc ===" << std::endl;
  ros::Time start_time = ros::Time::now();
  bool transformed = false;
  geometry_msgs::PoseStamped pose;
  try{
    listener.lookupTransform(WORLD_FRAME, ROBOT_FRAME, ros::Time(0), _transform);
    tf::transformStampedTFToMsg(_transform, transform);
    current_pose.header = transform.header;
    current_pose.pose.position.x = transform.transform.translation.x;
    current_pose.pose.position.y = transform.transform.translation.y;
    current_pose.pose.orientation = transform.transform.rotation;
    pose.header = current_pose.header;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.orientation = transform.transform.rotation;
    transformed = true;
  }catch(tf::TransformException &ex){
    std::cout << ex.what() << std::endl;
  }

  if(!path.poses.empty() && transformed){
    if(first_transform){
      last_time = ros::Time::now().toSec();
      first_transform = false;
    }else{
      //std::cout << current_pose << std::endl;
      double current_time = ros::Time::now().toSec();
      double dt = current_time - last_time;
      last_time = current_time;
      double dx_map = current_pose.pose.position.x - previous_pose.pose.position.x;
      double dy_map = current_pose.pose.position.y - previous_pose.pose.position.y;
      double dyaw = tf::getYaw(current_pose.pose.orientation) - tf::getYaw(previous_pose.pose.orientation);
      double theta = tf::getYaw(previous_pose.pose.orientation);
      double dx_base = dx_map * cos(-theta) - dy_map * sin(-theta);
      double dy_base = dx_map * sin(-theta) + dy_map * cos(-theta);
      double vx_base = dx_base / dt;
      double vy_base = dy_base / dt;
      double omega = dyaw / dt;
      Eigen::Vector2d base_velocity;
      base_velocity << vx_base, vy_base;
      Eigen::Matrix2d rotation_matrix;
      rotation_matrix << cos(dyaw), -sin(dyaw),
                         sin(dyaw),  cos(dyaw);
      Eigen::Vector2d _robot_velocity = rotation_matrix.inverse() * base_velocity;
      double vx = _robot_velocity(0);
      double vy = _robot_velocity(1);
      Eigen::VectorXd current_wheel_velocity;
      current_wheel_velocity.resize(8, 1);
      Eigen::Vector3d current_velocity;
      current_velocity << vx, vy, omega;
      current_wheel_velocity = forward_matrix * current_velocity;
      double s_fr = atan2(current_wheel_velocity(1), current_wheel_velocity(0));
      double w_fr = sqrt(current_wheel_velocity(0) * current_wheel_velocity(0) + current_wheel_velocity(1) * current_wheel_velocity(1)) / WHEEL_RADIUS;
      if(s_fr > MAX_STEERING_ANGLE){
        s_fr -= M_PI;
        w_fr = -w_fr;
      }else if(s_fr < -MAX_STEERING_ANGLE){
        s_fr += M_PI;
        w_fr = -w_fr;
      }
      double s_fl = atan2(current_wheel_velocity(3), current_wheel_velocity(2));
      double w_fl = sqrt(current_wheel_velocity(2) * current_wheel_velocity(2) + current_wheel_velocity(3) * current_wheel_velocity(3)) / WHEEL_RADIUS;
      if(s_fl > MAX_STEERING_ANGLE){
        s_fl -= M_PI;
        w_fl = -w_fl;
      }else if(s_fl < -MAX_STEERING_ANGLE){
        s_fl += M_PI;
        w_fl = -w_fl;
      }
      double s_rl = atan2(current_wheel_velocity(5), current_wheel_velocity(4));
      double w_rl = sqrt(current_wheel_velocity(4) * current_wheel_velocity(4) + current_wheel_velocity(5) * current_wheel_velocity(5)) / WHEEL_RADIUS;
      if(s_rl > MAX_STEERING_ANGLE){
        s_rl -= M_PI;
        w_rl = -w_rl;
      }else if(s_rl < -MAX_STEERING_ANGLE){
        s_rl += M_PI;
        w_rl = -w_rl;
      }
      double s_rr = atan2(current_wheel_velocity(7), current_wheel_velocity(6));
      double w_rr = sqrt(current_wheel_velocity(6) * current_wheel_velocity(6) + current_wheel_velocity(7) * current_wheel_velocity(7)) / WHEEL_RADIUS;
      if(s_rr > MAX_STEERING_ANGLE){
        s_rr -= M_PI;
        w_rr = -w_rr;
      }else if(s_rr < -MAX_STEERING_ANGLE){
        s_rr += M_PI;
        w_rr = -w_rr;
      }
      //std::cout << "current_wheel_velocity" << std::endl;
      //std::cout << current_wheel_velocity << std::endl;

      Eigen::VectorXd state(14);
      state << pose.pose.position.x, pose.pose.position.y, tf::getYaw(pose.pose.orientation), vx, vy, omega, w_fr, w_fl, w_rr, w_rl, s_fr, s_fl, s_rr, s_rl;
      std::cout << "state" << std::endl;
      std::cout << state << std::endl;
      std::cout << "path to vector" << std::endl;
      path_to_vector();
      /*
      std::cout << "path_x" << std::endl;
      std::cout << path_x << std::endl;
      std::cout << "path_y" << std::endl;
      std::cout << path_y << std::endl;
      std::cout << "path_yaw" << std::endl;
      std::cout << path_yaw << std::endl;
      */
      std::cout << "solving" << std::endl;
      auto result = mpc.solve(state, path_x, path_y, path_yaw);
      std::cout << "solved" << std::endl;
      velocity.linear.x = result[0];
      velocity.linear.y = result[1];
      velocity.angular.z = result[2];
      std::cout << velocity << std::endl;
      velocity_pub.publish(velocity);
      // mpc表示
      geometry_msgs::PoseArray mpc_path;
      mpc_path.header.frame_id = ROBOT_FRAME;
      double yaw0 = tf::getYaw(pose.pose.orientation);
      for(int i=0;i<T-1;i++){
        geometry_msgs::Pose temp;
        //temp.position.x = result[3+3*i] * cos(-yaw0) - result[4+3*i] * sin(-yaw0);
        //temp.position.y = result[3+3*i] * sin(-yaw0) + result[4+3*i] * cos(-yaw0);
        temp.position.x = result[3+3*i];
        temp.position.y = result[4+3*i];
        //temp.orientation = tf::createQuaternionMsgFromYaw(result[5+3*i] - yaw0);
        temp.orientation = tf::createQuaternionMsgFromYaw(result[5+3*i]);
        mpc_path.poses.push_back(temp);
      }
      path_pub.publish(mpc_path);
      // ~mpc表示
      path.poses.erase(path.poses.begin());
    }
  }
  previous_pose = current_pose;
  std::cout << ros::Time::now() - start_time << "[s]" << std::endl;
}

void MPCPathTracker::path_to_vector(void)
{
  int m = VREF * DT / RESOLUTION + 1;// TODO:delete 1
  int index = 0;
  for(int i=0;i<T;i++){
    if(i*m<path.poses.size()){
      index = i*m;
      path_x[i] = path.poses[index].pose.position.x;
      path_y[i] = path.poses[index].pose.position.y;
      path_yaw[i] = tf::getYaw(path.poses[index].pose.orientation);
    }else{
      path_x[i] = path.poses[path.poses.size() - 1].pose.position.x;
      path_y[i] = path.poses[path.poses.size() - 1].pose.position.y;
      path_yaw[i] = tf::getYaw(path.poses[path.poses.size() - 1].pose.orientation);
    }
  }
}

double min_distance(nav_msgs::Path& path, geometry_msgs::PoseStamped& pose)
{
  int length = path.poses.size();
  double min_distance = 100;
  for(int i=0;i<length;i++){
    double distance = get_distance(path.poses[i], pose);
    if(min_distance > distance){
      min_distance = distance;
    }
  }
  return min_distance;
}

double get_distance(geometry_msgs::PoseStamped& pose0, geometry_msgs::PoseStamped& pose1)
{
  return sqrt((pose0.pose.position.x - pose1.pose.position.x) * (pose0.pose.position.x - pose1.pose.position.x) + (pose0.pose.position.y - pose1.pose.position.y) * (pose0.pose.position.y - pose1.pose.position.y));
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_ERF_HPP
#define STAN_MATH_PRIM_SCAL_FUN_ERF_HPP

#include <stan/math/prim/scal/fun/boost_policy.hpp>
#include <stan/math/prim/scal/fun/is_nan.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <limits>

namespace stan {
  namespace math {

    /**
     * Return the error function of the specified value.
     *
     * \f[
     * \mbox{erf}(x) = \frac{2}{\sqrt{\pi}} \int_0^x e^{-t^2} dt
     * \f]
     *
     * @param[in] x Argument.
     * @return Error function of the argument.
     */
    inline double erf(double x) {
      if (is_nan(x))
        return std::numeric_limits<double>::quiet_NaN();
      return boost::math::erf(x, boost_policy_t());
    }

    /**
     * Return the error function of the specified argument.  This
     * version is required to disambiguate erf(int).
     *
     * @param[in] x Argument.
     * @return Error function of the argument.
     */
    inline double erf(int x) {
      return erf(static_cast<double>(x));
    }

  }
}
#endif


/*
 * smaxlayer.cpp
 *
 * This is SOFTMAX layer
 *
 *  Feed-forward:
 *    a(l, i) = softmax(a(l), i) = exp(a(l-1, i) / sum(exp(a(l-1)))
 *
 *  Back propagation:
 *    gradient(C, a(l)) = gradient(C, a(l+1)) * M(a(l + 1))
 *    where M(a(l), i, j) = softmax(a(l + 1), i) * (sigma(i, j) - softmax(a(l + 1), j)) where sigma(i, j) = 1 for i=j and 0 otherwise
 */
#include <boost/assert.hpp>

#include "core/utils.h"
#include "smaxlayer.h"

using namespace std;
using namespace boost;
using namespace yann;

////////////////////////////////////////////////////////////////////////////////////////////////
//
// SoftmaxLayer_Context and SoftmaxLayer_TrainingContext implementations
//
namespace yann {

typedef Layer::Context SoftmaxLayer_Context;

class SoftmaxLayer_TrainingContext :
    public SoftmaxLayer_Context
{
  typedef SoftmaxLayer_Context Base;

  friend class SoftmaxLayer;

public:
  SoftmaxLayer_TrainingContext(const MatrixSize & output_size,
                               const MatrixSize & batch_size,
                               const MatrixSize & input_size) :
    Base(output_size, batch_size),
    _tmp(input_size)
  {
  }

  SoftmaxLayer_TrainingContext(const RefVectorBatch & output,
                               const MatrixSize & input_size) :
    Base(output),
    _tmp(input_size)
  {
  }

protected:
  Vector _tmp;
}; // class SoftmaxLayer_TrainingContext

}; // namespace yann

////////////////////////////////////////////////////////////////////////////////////////////////
//
// yann::SoftmaxLayer implementation
//
void yann::SoftmaxLayer::softmax_plus_equal(const RefConstVector & input, RefVector output, const Value & beta)
{
  YANN_CHECK(is_same_size(input, output));
  YANN_CHECK_EQ(input.rows(), 1); // RowMajor layout, breaks for ColMajor

  Value max = input.maxCoeff(); // adjust the computations to avoid overflowing
  Value sum = exp((input.array() - max) * beta).sum();
  output.array() += (exp((input.array() - max) * beta)) / sum;
}

// gradient_input = softmax_derivative(input) * gradient_output
//
// where
//    softmax_derivative(input, ii, jj) = - softmax(input, ii) * softmax(input, jj) if ii != jj
//    softmax_derivative(input, ii, jj) = softmax(ii) * (1 - softmax(ii)) if ii == jj
//
// gradient_input(ii) = sum(softmax_derivative(input, ii, jj) * gradient_output(jj))
//
// gradient_input(ii) = sum(ii != jj, - softmax(input, ii) * softmax(input, jj) * gradient_output(jj)) +
//                      softmax(ii) * (1 - softmax(ii)) * * gradient_output(ii)
//
// gradient_input(ii) = sum(ii != jj, - softmax(input, ii) * softmax(input, jj) * gradient_output(jj)) +
//                      softmax(ii) * gradient_output(ii) - softmax(ii) * softmax(ii) * gradient_output(ii)
//
// gradient_input(ii) = softmax(input, ii) * (sum(-softmax(input, jj) * gradient_output(jj)) + gradient_output(ii))
//
void yann::SoftmaxLayer::softmax_gradient(
    const RefConstVector & input,
    const RefConstVector & gradient_output,
    RefVector tmp,
    RefVector gradient_input,
    const Value & beta)
{
  YANN_CHECK(is_same_size(input, gradient_output));
  YANN_CHECK(is_same_size(input, tmp));
  YANN_CHECK(is_same_size(input, gradient_input));

  tmp.setZero();
  softmax_plus_equal(input, tmp, beta);

  Value sum = (tmp.array() * gradient_output.array()).sum();
  gradient_input = beta * tmp.array() * (gradient_output.array() - sum);
}

yann::SoftmaxLayer::SoftmaxLayer(const MatrixSize & size, const Value & beta) :
    _size(size),
    _beta(beta)
{
  YANN_CHECK_GT(size, 0);
}

yann::SoftmaxLayer::~SoftmaxLayer()
{
}

// Layer overwrites
std::string yann::SoftmaxLayer::get_name() const
{
  return "SoftmaxLayer";
}

bool yann::SoftmaxLayer::is_equal(const Layer & other, double tolerance) const
{
  if(!Base::is_equal(other, tolerance)) {
    return false;
  }
  auto the_other = dynamic_cast<const SoftmaxLayer*>(&other);
  if(the_other == nullptr) {
    return false;
  }
  if(_size != the_other->_size) {
    return false;
  }
  return true;
}

MatrixSize yann::SoftmaxLayer::get_input_size() const
{
  return _size;
}

MatrixSize yann::SoftmaxLayer::get_output_size() const
{
  return _size;
}

unique_ptr<Layer::Context> yann::SoftmaxLayer::create_context(const MatrixSize & batch_size) const
{
  YANN_CHECK(is_valid());
  return make_unique<SoftmaxLayer_Context>(get_output_size(), batch_size);
}
unique_ptr<Layer::Context> yann::SoftmaxLayer::create_context(const RefVectorBatch & output) const
{
  YANN_CHECK(is_valid());
  return make_unique<SoftmaxLayer_Context>(output);
}
unique_ptr<Layer::Context> yann::SoftmaxLayer::create_training_context(
    const MatrixSize & batch_size,
    const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(is_valid());
  return make_unique<SoftmaxLayer_TrainingContext>(get_output_size(), batch_size, get_input_size());
}
unique_ptr<Layer::Context> yann::SoftmaxLayer::create_training_context(
    const RefVectorBatch & output,
    const std::unique_ptr<Layer::Updater> & updater) const
{
  YANN_CHECK(is_valid());
  return make_unique<SoftmaxLayer_TrainingContext>(output, get_input_size());
}

void yann::SoftmaxLayer::feedforward(
    const RefConstVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  auto ctx = dynamic_cast<SoftmaxLayer_Context *>(context);
  YANN_CHECK(ctx);
  YANN_CHECK(is_valid());
  YANN_CHECK_GT(get_batch_size(input), 0);
  YANN_CHECK_EQ(get_batch_item_size(input), get_input_size());
  YANN_CHECK_LE(get_batch_size(input), get_batch_size(ctx->get_output()));
  YANN_CHECK_EQ(get_batch_item_size(ctx->get_output()), get_output_size());

  RefVectorBatch output = ctx->get_output();
  switch(mode) {
  case Operation_Assign:
    output.setZero();
    break;
  case Operation_PlusEqual:
    // do nothing
    break;
  }
  const auto batch_size = get_batch_size(input);
  for(MatrixSize ii = 0; ii < batch_size; ++ii) {
    softmax_plus_equal(get_batch(input, ii), get_batch(output, ii), _beta);
  }
}

void yann::SoftmaxLayer::feedforward(
    const RefConstSparseVectorBatch & input,
    Context * context,
    enum OperationMode mode) const
{
  throw runtime_error("SoftmaxLayer::feedforward() is not implemented for sparse vectors");
}

void yann::SoftmaxLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  YANN_CHECK(is_valid());
  YANN_SLOW_CHECK_GT(get_batch_size(gradient_output), 0);
  YANN_SLOW_CHECK_EQ(get_batch_item_size(gradient_output), get_output_size());
  YANN_SLOW_CHECK_EQ(get_batch_size(input), get_batch_size(gradient_output));
  YANN_SLOW_CHECK_EQ(get_batch_item_size(input), get_input_size());

  auto ctx = static_cast<SoftmaxLayer_TrainingContext*>(context);
  YANN_CHECK(ctx);

  // nothing to do for the softmax layer itself

  // we don't need to calculate the gradient(C, a(l)) for the "first" layer (actual inputs)
  if(gradient_input) {
    YANN_SLOW_CHECK_EQ(get_batch_item_size(input), get_batch_item_size(*gradient_input));
    YANN_SLOW_CHECK_EQ(get_batch_size(input), get_batch_size(*gradient_input));

    const auto batch_size = get_batch_size(input);
    for(MatrixSize ii = 0; ii < batch_size; ++ii) {
      softmax_gradient(
          get_batch(input, ii),
          get_batch(gradient_output, ii),
          ctx->_tmp,
          get_batch(*gradient_input, ii),
          _beta);
    }
  }
}

void yann::SoftmaxLayer::backprop(
    const RefConstVectorBatch & gradient_output,
    const RefConstSparseVectorBatch & input,
    optional<RefVectorBatch> gradient_input,
    Context * context) const
{
  throw runtime_error("SoftmaxLayer::backprop() is not implemented for sparse vectors");
}

void yann::SoftmaxLayer::init(enum InitMode mode, boost::optional<InitContext> init_context)
{
  // nothing to do
}

void yann::SoftmaxLayer::update(Context * context, const size_t & tests_num)
{
  // auto ctx = dynamic_cast<SoftmaxLayer_Context *>(context);
  // YANN_CHECK(ctx);
  // nothing to do
}

void yann::SoftmaxLayer::read(std::istream & is)
{
  Base::read(is);

  // nothing to do
}

void yann::SoftmaxLayer::write(std::ostream & os) const
{
  Base::write(os);
  // nothing to do
}


#include "system/CameraModel.h"
#include "system/DsoSystem.h"
#include "util/geometry.h"
#include "util/types.h"
#include <Eigen/Core>
#include <gtest/gtest.h>
#include <tuple>

using namespace fishdso;

TEST(GeometryTest, IntersectOnSphereTest) {
  std::vector<std::tuple<double, Vec3, Vec3>> tests{
      {M_PI_2, Vec3(0, 0, 1), Vec3(1, 0, -1).normalized()},
      {M_PI_2, Vec3(1, 0, -1).normalized(), Vec3(0, 0, 1)},
      {M_PI_2, Vec3(1, 1, 0).normalized(), Vec3(1, -1, 0).normalized()},
      {M_PI_2, Vec3(1, 0, 1).normalized(), Vec3(-1, -1, -1).normalized()},
      {M_PI / 3, Vec3(0, 1, 0), Vec3(0, 0, 1)}};

  std::vector<std::pair<Vec3, Vec3>> answers{
      {Vec3(0, 0, 1), Vec3(1, 0, 0)},
      {Vec3(1, 0, 0), Vec3(0, 0, 1)},
      {Vec3(1, 1, 0).normalized(), Vec3(1, -1, 0).normalized()},
      {Vec3(1, 0, 1).normalized(), Vec3(0, -1, 0)},
      {Vec3(0, std::sqrt(3) / 2, 0.5), Vec3(0, 0, 1)}};

  std::vector<std::tuple<double, Vec3, Vec3>> testsFalse{
      {M_PI_2, Vec3(0, 0, -1), Vec3(-1, 0, -1).normalized()},
      {M_PI_2, Vec3(-1, 0, -1).normalized(), Vec3(0, -1, -1).normalized()},
      {M_PI * 0.75, Vec3(-1, 0, -100).normalized(),
       Vec3(0, -1, -100).normalized()}};

  const double eps = 1e-9;
  for (int i = 0; i < tests.size(); ++i) {
    ASSERT_TRUE(intersectOnSphere(std::get<0>(tests[i]), std::get<1>(tests[i]),
                                  std::get<2>(tests[i])))
        << "test #" << i << " failed: returned false" << std::endl;
    double err =
        std::sqrt((std::get<1>(tests[i]) - answers[i].first).squaredNorm() +
                  (std::get<2>(tests[i]) - answers[i].second).squaredNorm());
    ASSERT_LT(err, eps) << "test #" << i << " failed: error = " << err
                        << std::endl;
  }

  for (int i = 0; i < testsFalse.size(); ++i)
    ASSERT_FALSE(intersectOnSphere(std::get<0>(testsFalse[i]),
                                   std::get<1>(testsFalse[i]),
                                   std::get<2>(testsFalse[i])))
        << "false-test #" << i << " failed: returned true" << std::endl;
}

TEST(GeometryTest, TriangulateTest) {
  const int testCount = 1000;
  const double border = 1e2;
  const double eps = 1e-8;

  std::mt19937 mt;
  std::uniform_real_distribution<double> coord(-border, border);
  for (int it = 0; it < testCount; ++it) {
    Vec3 a(coord(mt), coord(mt), coord(mt));
    Vec3 b(coord(mt), coord(mt), coord(mt));
    Vec3 c(coord(mt), coord(mt), coord(mt));
    SO3 rot = SO3::sampleUniform(mt);

    double depth1 = (c - a).norm();
    double depth2 = (c - b).norm();

    Vec3 t = -(rot * (b - a));
    SE3 aToB(rot, t);

    Vec3 cDirInACoord = (c - a).normalized();
    Vec3 cDirInBCoord = (aToB * (c - a)).normalized();

    Vec2 result = triangulate(aToB, cDirInACoord, cDirInBCoord);
    Vec2 expected(depth1, depth2);
    double err = (result - expected).norm();
    ASSERT_LT(err, eps) << "test failed:\n"
                        << "a = " << a.transpose() << "\n"
                        << "b = " << b.transpose() << "\n"
                        << "c = " << c.transpose() << "\n"
                        << "rot = "
                        << rot.unit_quaternion().coeffs().transpose() << "\n"
                        << "err = " << err << "\n"
                        << "expected = " << expected.transpose() << "\n"
                        << "result   = " << result.transpose() << std::endl;
  }
}

TEST(GeometryTest, IsSameSideTest) {
  StdVector<std::array<Vec2, 4>> testsTrue{
      {Vec2(0, 0), Vec2(1, 0), Vec2(1, 1), Vec2(0, 1)},
      {Vec2(0, 0), Vec2(1, 1), Vec2(2, 2), Vec2(0, 1)},
      {Vec2(0, 0), Vec2(1, 1), Vec2(2, 2), Vec2(0, -1)},
      {Vec2(1, 1), Vec2(2, 1), Vec2(2, 1), Vec2(3, 1)}};
  StdVector<std::array<Vec2, 4>> testsFalse{
      {Vec2(0, 0), Vec2(1, 0), Vec2(1, 1), Vec2(1, -1)},
      {Vec2(1, 1), Vec2(1, -1), Vec2(-1, -1), Vec2(2, 2)},
      {Vec2(0, 0), Vec2(1, 0), Vec2(1e6, 1), Vec2(1e8, -1)},
      {Vec2(-1, -2), Vec2(0, 2), Vec2(-1, -1), Vec2(0, 0)}};

  for (int i = 0; i < testsTrue.size(); ++i)
    ASSERT_TRUE(isSameSide(testsTrue[i][0], testsTrue[i][1], testsTrue[i][2],
                           testsTrue[i][3]))
        << "test #" << i << " failed: returned false" << std::endl;
  for (int i = 0; i < testsFalse.size(); ++i)
    ASSERT_FALSE(isSameSide(testsFalse[i][0], testsFalse[i][1],
                            testsFalse[i][2], testsFalse[i][3]))
        << "test #" << i << " failed: returned true" << std::endl;
}

TEST(GeometryTest, InInsideTriangleTest) {
  StdVector<std::array<Vec2, 4>> testsTrue{
      {Vec2(0, 0), Vec2(3, 0), Vec2(0, 3), Vec2(1, 1)},
      {Vec2(0, 0), Vec2(1e4, 1), Vec2(2, 2), Vec2(1e4 - 1, 1)},
      {Vec2(0, 0), Vec2(2, 1), Vec2(0, 2), Vec2(0, 1)},
      {Vec2(0, 0), Vec2(2, 0), Vec2(2, 2), Vec2(2, 2)},
      {Vec2(0, 0), Vec2(1, 0), Vec2(2, 0), Vec2(1, 1)},
  };
  StdVector<std::array<Vec2, 4>> testsFalse{
      {Vec2(0, 0), Vec2(1, 0), Vec2(0, 1), Vec2(2, 0)},
      {Vec2(-1, -1), Vec2(0, 2), Vec2(2, 0), Vec2(-1, 1)},
      {Vec2(0, 0), Vec2(1e5, 1), Vec2(0, 1), Vec2(1, 0)}};

  for (int i = 0; i < testsTrue.size(); ++i)
    ASSERT_TRUE(isInsideTriangle(testsTrue[i][0], testsTrue[i][1],
                                 testsTrue[i][2], testsTrue[i][3]))
        << "test #" << i << " failed: returned false" << std::endl;
  for (int i = 0; i < testsFalse.size(); ++i)
    ASSERT_FALSE(isInsideTriangle(testsFalse[i][0], testsFalse[i][1],
                                  testsFalse[i][2], testsFalse[i][3]))
        << "test #" << i << " failed: returned true" << std::endl;
}

TEST(GeometryTest, IsABCDConvexTest) {
  StdVector<std::array<Vec2, 4>> testsTrue{
      {Vec2(0, 0), Vec2(1, 0), Vec2(1, 1), Vec2(0, 1)},
      {Vec2(-3, 0), Vec2(-2, 2), Vec2(3, 0), Vec2(2, -2)},
      {Vec2(0, 0), Vec2(0, 1), Vec2(0, 2), Vec2(1, 1)},
      {Vec2(-1e5, -1), Vec2(0, 0), Vec2(1e4, 1), Vec2(0, 1)}};
  StdVector<std::array<Vec2, 4>> testsFalse{
      {Vec2(0, 0), Vec2(1, 1), Vec2(2, 0), Vec2(1, 3)},
      {Vec2(-1e4, -1), Vec2(0, 0), Vec2(1e5, 1), Vec2(0, 1)},
      {Vec2(0, 0), Vec2(1, 1), Vec2(1, 0), Vec2(0, 1)}};

  for (int i = 0; i < testsTrue.size(); ++i)
    ASSERT_TRUE(isABCDConvex(testsTrue[i][0], testsTrue[i][1], testsTrue[i][2],
                             testsTrue[i][3]))
        << "test #" << i << " failed: returned false" << std::endl;
  for (int i = 0; i < testsFalse.size(); ++i)
    ASSERT_FALSE(isABCDConvex(testsFalse[i][0], testsFalse[i][1],
                              testsFalse[i][2], testsFalse[i][3]))
        << "test #" << i << " failed: returned true" << std::endl;
}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.

/*
 * MIT License
 *
 * Copyright (c) 2017 Zhenqiang.Ying
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include "precomp.hpp"

#ifdef HAVE_EIGEN
#include <Eigen/Sparse>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc.hpp>
#endif

namespace cv {
namespace intensity_transform {

#ifdef HAVE_EIGEN
static void diff(const Mat_<float>& src, Mat_<float>& srcVDiff, Mat_<float>& srcHDiff)
{
    srcVDiff = Mat_<float>(src.size());
    for (int i = 0; i < src.rows; i++)
    {
        if (i < src.rows-1)
        {
            for (int j = 0; j < src.cols; j++)
            {
                srcVDiff(i,j) = src(i+1,j) - src(i,j);
            }
        }
        else
        {
            for (int j = 0; j < src.cols; j++)
            {
                srcVDiff(i,j) = src(0,j) - src(i,j);
            }
        }
    }

    srcHDiff = Mat_<float>(src.size());
    for (int j = 0; j < src.cols-1; j++)
    {
        for (int i = 0; i < src.rows; i++)
        {
            srcHDiff(i,j) = src(i,j+1) - src(i,j);
        }
    }
    for (int i = 0; i < src.rows; i++)
    {
        srcHDiff(i,src.cols-1) = src(i,0) - src(i,src.cols-1);
    }
}

static void computeTextureWeights(const Mat_<float>& x, float sigma, float sharpness, Mat_<float>& W_h, Mat_<float>& W_v)
{
    Mat_<float> dt0_v, dt0_h;
    diff(x, dt0_v, dt0_h);

    Mat_<float> gauker_h;
    Mat_<float> kernel_h = Mat_<float>::ones(1, static_cast<int>(sigma));
    filter2D(dt0_h, gauker_h, -1, kernel_h, Point(-1,-1), 0, BORDER_CONSTANT);

    Mat_<float> gauker_v;
    Mat_<float> kernel_v = Mat_<float>::ones(static_cast<int>(sigma), 1);
    filter2D(dt0_v, gauker_v, -1, kernel_v, Point(-1,-1), 0, BORDER_CONSTANT);

    W_h = Mat_<float>(gauker_h.size());
    W_v = Mat_<float>(gauker_v.size());

    for (int i = 0; i < gauker_h.rows; i++)
    {
        for (int j = 0; j < gauker_h.cols; j++)
        {
            W_h(i,j) = 1 / (std::abs(gauker_h(i,j)) * std::abs(dt0_h(i,j)) + sharpness);
            W_v(i,j) = 1 / (std::abs(gauker_v(i,j)) * std::abs(dt0_v(i,j)) + sharpness);
        }
    }
}

template <class numeric_t>
static Eigen::SparseMatrix<numeric_t> spdiags(const Eigen::Matrix<numeric_t,-1,-1> &B,
                                              const Eigen::VectorXi &d, int m, int n) {
    typedef Eigen::Triplet<numeric_t> triplet_t;
    std::vector<triplet_t> triplets;
    triplets.reserve(static_cast<size_t>(std::min(m,n)*d.size()));

    for (int k = 0; k < d.size(); ++k) {
        int diag = d(k);  // get diagonal
        int i_start = std::max(-diag, 0); // get row of 1st element
        int i_end = std::min(m, m-diag-(m-n)); // get row of last element
        int j = -std::min(0, -diag); // get col of 1st element
        int B_i; // start index i in matrix B
        if (m < n) {
            B_i = std::max(-diag,0); // m < n
        } else {
            B_i = std::max(0,diag); // m >= n
        }
        for (int i = i_start; i < i_end; ++i, ++j, ++B_i) {
            triplets.push_back( {i, j,  B(B_i,k)} );
        }
    }
    Eigen::SparseMatrix<numeric_t> A(m,n);
    A.setFromTriplets(triplets.begin(), triplets.end());
    return A;
}

static Mat solveLinearEquation(const Mat_<float>& img, Mat_<float>& W_h_, Mat_<float>& W_v_, float lambda)
{
    Eigen::MatrixXf W_h;
    cv2eigen(W_h_, W_h);
    Eigen::MatrixXf tempx(W_h.rows(), W_h.cols());
    tempx.block(0, 1, tempx.rows(), tempx.cols()-1) = W_h.block(0, 0, W_h.rows(), W_h.cols()-1);
    for (Eigen::Index i = 0; i < tempx.rows(); i++)
    {
        tempx(i,0) = W_h(i, W_h.cols()-1);
    }

    Eigen::MatrixXf W_v;
    cv2eigen(W_v_, W_v);
    Eigen::MatrixXf tempy(W_v.rows(), W_v.cols());
    tempy.block(1, 0, tempx.rows()-1, tempx.cols()) = W_v.block(0, 0, W_v.rows()-1, W_v.cols());
    for (Eigen::Index j = 0; j < tempy.cols(); j++)
    {
        tempy(0,j) = W_v(W_v.rows()-1, j);
    }

    Eigen::VectorXf dx(W_h.rows()*W_h.cols());
    Eigen::VectorXf dy(W_v.rows()*W_v.cols());

    Eigen::VectorXf dxa(tempx.rows()*tempx.cols());
    Eigen::VectorXf dya(tempy.rows()*tempy.cols());

    //Flatten in a col-major order
    for (Eigen::Index j = 0; j < W_h.cols(); j++)
    {
        for (Eigen::Index i = 0; i < W_h.rows(); i++)
        {
            dx(j*W_h.rows() + i) = -lambda*W_h(i,j);
            dy(j*W_h.rows() + i) = -lambda*W_v(i,j);

            dxa(j*W_h.rows() + i) = -lambda*tempx(i,j);
            dya(j*W_h.rows() + i) = -lambda*tempy(i,j);
        }
    }

    tempx.setZero();
    tempx.col(0) = W_h.col(W_h.cols()-1);

    tempy.setZero();
    tempy.row(0) = W_v.row(W_v.rows()-1);

    W_h.col(W_h.cols()-1).setZero();
    W_v.row(W_v.rows()-1).setZero();

    Eigen::VectorXf dxd1(tempx.rows()*tempx.cols());
    Eigen::VectorXf dyd1(tempy.rows()*tempy.cols());
    Eigen::VectorXf dxd2(W_h.rows()*W_h.cols());
    Eigen::VectorXf dyd2(W_v.rows()*W_v.cols());

    //Flatten in a col-major order
    for (Eigen::Index j = 0; j < tempx.cols(); j++)
    {
        for (Eigen::Index i = 0; i < tempx.rows(); i++)
        {
            dxd1(j*tempx.rows() + i) = -lambda*tempx(i,j);
            dyd1(j*tempx.rows() + i) = -lambda*tempy(i,j);

            dxd2(j*tempx.rows() + i) = -lambda*W_h(i,j);
            dyd2(j*tempx.rows() + i) = -lambda*W_v(i,j);
        }
    }

    Eigen::MatrixXf dxd(dxd1.rows(), dxd1.cols()+dxd2.cols());
    dxd << dxd1, dxd2;

    Eigen::MatrixXf dyd(dyd1.rows(), dyd1.cols()+dyd2.cols());
    dyd << dyd1, dyd2;

    const int k = img.rows*img.cols;
    const int r = img.rows;
    Eigen::Matrix<int, 2, 1> diagx_idx;
    diagx_idx << -k+r, -r;
    Eigen::SparseMatrix<float> Ax = spdiags(dxd, diagx_idx, k, k);

    Eigen::Matrix<int, 2, 1> diagy_idx;
    diagy_idx << -r+1, -1;
    Eigen::SparseMatrix<float> Ay = spdiags(dyd, diagy_idx, k, k);

    Eigen::MatrixXf D = (dx + dy + dxa + dya);
    D = Eigen::MatrixXf::Ones(D.rows(), D.cols()) - D;

    Eigen::Matrix<int, 1, 1> diag_idx_zero;
    diag_idx_zero << 0;
    Eigen::SparseMatrix<float> A = (Ax + Ay) + Eigen::SparseMatrix<float>((Ax + Ay).transpose()) + spdiags(D, diag_idx_zero, k, k);

    //CG solver of Eigen
    Eigen::ConjugateGradient<Eigen::SparseMatrix<float>, Eigen::Lower|Eigen::Upper, Eigen::IncompleteCholesky<float> > cg;
    cg.setTolerance(0.1f);
    cg.setMaxIterations(50);
    cg.compute(A);
    Mat_<float> img_t = img.t();
    Eigen::Map<const Eigen::VectorXf> tin(img_t.ptr<float>(), img_t.rows*img_t.cols);
    Eigen::VectorXf x = cg.solve(tin);

    Mat_<float> tout(img.rows, img.cols);
    tout.forEach(
        [&](float &pixel, const int * position) -> void
        {
            pixel = x(position[1]*img.rows + position[0]);
        }
    );

    return tout;
}

static Mat_<float> tsmooth(const Mat_<float>& src, float lambda=0.01f, float sigma=3.0f, float sharpness=0.001f)
{
    Mat_<float> W_h, W_v;
    computeTextureWeights(src, sigma, sharpness, W_h, W_v);

    Mat_<float> S = solveLinearEquation(src, W_h, W_v, lambda);

    return S;
}

static Mat_<float> rgb2gm(const Mat_<Vec3f>& I)
{
    Mat_<float> gm(I.rows, I.cols);
    gm.forEach(
        [&](float &pixel, const int * position) -> void
        {
            pixel = std::pow(I(position[0], position[1])[0]*I(position[0], position[1])[1]*I(position[0], position[1])[2], 1/3.0f);
        }
    );

    return gm;
}

static Mat_<float> applyK(const Mat_<float>& I, float k, float a=-0.3293f, float b=1.1258f) {
    float beta = std::exp((1 - std::pow(k, a)) * b);
    float gamma = std::pow(k, a);

    Mat_<float> J(I.size());
    pow(I, gamma, J);
    J = J*beta;

    return J;
}

static Mat_<Vec3f> applyK(const Mat_<Vec3f>& I, float k, float a=-0.3293f, float b=1.1258f, float offset=0) {
    float beta = std::exp((1 - std::pow(k, a)) * b);
    float gamma = std::pow(k, a);

    Mat_<Vec3f> J(I.size());
    pow(I, gamma, J);

    return J * beta + Scalar::all(offset);
}

static float entropy(const Mat_<float>& I)
{
    Mat_<uchar> I_uchar;
    I.convertTo(I_uchar, CV_8U, 255);

    std::vector<Mat> planes;
    planes.push_back(I_uchar);
    Mat_<float> hist;
    const int histSize = 256;
    float range[] = { 0, 256 };
    const float* histRange = { range };
    calcHist(&I_uchar, 1, NULL, Mat(), hist, 1, &histSize, &histRange);

    Mat_<float> hist_norm = hist / cv::sum(hist)[0];

    float E = 0;
    for (int i = 0; i < hist_norm.rows; i++)
    {
        if (hist_norm(i,0) > 0)
        {
            E += hist_norm(i,0) * std::log2(hist_norm(i,0));
        }
    }

    return -E;
}

template <typename T> static int sgn(T val)
{
    return (T(0) < val) - (val < T(0));
}

static double minimize_scalar_bounded(const Mat_<float>& I, double begin, double end,
                               double xatol=1e-4, int maxiter=500)
{
// From scipy: https://github.com/scipy/scipy/blob/v1.4.1/scipy/optimize/optimize.py#L1753-L1894
//    """
//    Options
//    -------
//    maxiter : int
//        Maximum number of iterations to perform.
//    disp: int, optional
//        If non-zero, print messages.
//            0 : no message printing.
//            1 : non-convergence notification messages only.
//            2 : print a message on convergence too.
//            3 : print iteration results.
//    xatol : float
//        Absolute error in solution `xopt` acceptable for convergence.
//    """
    double x1 = begin, x2 = end;

    if (x1 > x2) {
        throw std::runtime_error("The lower bound exceeds the upper bound.");
    }

    double sqrt_eps = std::sqrt(2.2e-16);
    double golden_mean = 0.5 * (3.0 - std::sqrt(5.0));
    double a = x1, b = x2;
    double fulc = a + golden_mean * (b - a);
    double nfc = fulc, xf = fulc;
    double rat = 0.0, e = 0.0;
    double x = xf;
    double fx = -entropy(applyK(I, static_cast<float>(x)));
    int num = 1;
    double fu = std::numeric_limits<double>::infinity();

    double ffulc = fx, fnfc = fx;
    double xm = 0.5 * (a + b);
    double tol1 = sqrt_eps * std::abs(xf) + xatol / 3.0;
    double tol2 = 2.0 * tol1;

    for (int iter = 0; iter < maxiter && std::abs(xf - xm) > (tol2 - 0.5 * (b - a)); iter++)
    {
        int golden = 1;
        // Check for parabolic fit
        if (std::abs(e) > tol1) {
            golden = 0;
            double r = (xf - nfc) * (-entropy(applyK(I, static_cast<float>(x))) - ffulc);
            double q = (xf - fulc) * (-entropy(applyK(I, static_cast<float>(x))) - fnfc);
            double p = (xf - fulc) * q - (xf - nfc) * r;
            q = 2.0 * (q - r);

            if (q > 0.0) {
                p = -p;
            }
            q = std::abs(q);
            r = e;
            e = rat;

            // Check for acceptability of parabola
            if (((std::abs(p) < std::abs(0.5*q*r)) && (p > q*(a - xf)) &
                    (p < q * (b - xf)))) {
                rat = (p + 0.0) / q;
                x = xf + rat;

                if (((x - a) < tol2) || ((b - x) < tol2)) {
                    double si = sgn(xm - xf) + ((xm - xf) == 0);
                    rat = tol1 * si;
                }
            } else {      // do a golden-section step
                golden = 1;
            }
        }

        if (golden) {  // do a golden-section step
            if (xf >= xm) {
                e = a - xf;
            } else {
                e = b - xf;
            }
            rat = golden_mean*e;
        }

        double si = sgn(rat) + (rat == 0);
        x = xf + si * std::max(std::abs(rat), tol1);
        fu = -entropy(applyK(I, static_cast<float>(x)));
        num += 1;

        if (fu <= fx) {
            if (x >= xf) {
                a = xf;
            } else {
                b = xf;
            }

            fulc = nfc;
            ffulc = fnfc;
            nfc = xf;
            fnfc = fx;
            xf = x;
            fx = fu;
        } else {
            if (x < xf) {
                a = x;
            } else {
                b = x;
            }

            if ((fu <= fnfc) || (nfc == xf)) {
                fulc = nfc;
                ffulc = fnfc;
                nfc = x;
                fnfc = fu;
            } else if ((fu <= ffulc) || (fulc == xf) || (fulc == nfc)) {
                fulc = x;
                ffulc = fu;
            }
        }

        xm = 0.5 * (a + b);
        tol1 = sqrt_eps * std::abs(xf) + xatol / 3.0;
        tol2 = 2.0 * tol1;
    }

    return xf;
}

static Mat_<Vec3f> maxEntropyEnhance(const Mat_<Vec3f>& I, const Mat_<uchar>& isBad, float a, float b)
{
    Mat_<Vec3f> input;
    resize(I, input, Size(50,50));

    Mat_<float> Y = rgb2gm(input);

    Mat_<uchar> isBad_resize;
    resize(isBad, isBad_resize, Size(50,50));

    std::vector<float> Y_vec;
    for (int i = 0; i < isBad_resize.rows; i++)
    {
        for (int j = 0; j < isBad_resize.cols; j++)
        {
            if (isBad_resize(i,j) >= 0.5)
            {
                Y_vec.push_back(Y(i,j));
            }
        }
    }

    if (Y_vec.empty())
    {
        return I;
    }

    Mat_<float> Y_mat(static_cast<int>(Y_vec.size()), 1, Y_vec.data());
    float opt_k = static_cast<float>(minimize_scalar_bounded(Y_mat, 1, 7));

    return applyK(I, opt_k, a, b, -0.01f);
}

static void BIMEF_impl(InputArray input_, OutputArray output_, float mu, float *k, float a, float b)
{
    CV_INSTRUMENT_REGION()

    Mat input = input_.getMat();
    if (input.empty())
    {
        return;
    }
    CV_CheckTypeEQ(input.type(), CV_8UC3, "Input image must be 8-bits color image (CV_8UC3).");

    Mat_<Vec3f> imgDouble;
    input.convertTo(imgDouble, CV_32F, 1/255.0);

    // t: scene illumination map
    Mat_<float> t_b(imgDouble.size());
    t_b.forEach(
        [&](float &pixel, const int * position) -> void
        {
            pixel = std::max(std::max(imgDouble(position[0], position[1])[0],
                                      imgDouble(position[0], position[1])[1]),
                            imgDouble(position[0], position[1])[2]);
        }
    );

    const float lambda = 0.5;
    const float sigma = 5;

    Mat_<float> t_b_resize;
    resize(t_b, t_b_resize, Size(), 0.5, 0.5);

    Mat_<float> t_our = tsmooth(t_b_resize, lambda, sigma);
    resize(t_our, t_our, t_b.size());

    // k: exposure ratio
    Mat_<Vec3f> J;
    if (k == NULL)
    {
        Mat_<uchar> isBad(t_our.size());
        isBad.forEach(
            [&](uchar &pixel, const int * position) -> void
            {
                pixel = t_our(position[0], position[1]) < 0.5 ? 1 : 0;
            }
        );

        J = maxEntropyEnhance(imgDouble, isBad, a, b);
    }
    else
    {
        J = applyK(imgDouble, *k, a, b);

        // fix overflow
        J.forEach(
            [](Vec3f &pixel, const int * /*position*/) -> void
            {
                pixel(0) = std::min(1.0f, pixel(0));
                pixel(1) = std::min(1.0f, pixel(1));
                pixel(2) = std::min(1.0f, pixel(2));
            }
        );
    }

    // W: Weight Matrix
    Mat_<float> W(t_our.size());
    pow(t_our, mu, W);

    output_.create(input.size(), CV_8UC3);
    Mat output = output_.getMat();
    output.forEach<Vec3b>(
        [&](Vec3b &pixel, const int * position) -> void
        {
            float w = W(position[0], position[1]);
            pixel(0) = saturate_cast<uchar>((imgDouble(position[0], position[1])[0] * w + J(position[0], position[1])[0] * (1 - w)) * 255);
            pixel(1) = saturate_cast<uchar>((imgDouble(position[0], position[1])[1] * w + J(position[0], position[1])[1] * (1 - w)) * 255);
            pixel(2) = saturate_cast<uchar>((imgDouble(position[0], position[1])[2] * w + J(position[0], position[1])[2] * (1 - w)) * 255);
        }
    );
}
#else
static void BIMEF_impl(InputArray, OutputArray, float, float *, float, float)
{
    CV_Error(Error::StsNotImplemented, "This algorithm requires OpenCV built with the Eigen library.");
}
#endif

void BIMEF(InputArray input, OutputArray output, float mu, float a, float b)
{
    BIMEF_impl(input, output, mu, NULL, a, b);
}

void BIMEF(InputArray input, OutputArray output, float k, float mu, float a, float b)
{
    BIMEF_impl(input, output, mu, &k, a, b);
}

}} // cv::intensity_transform::


#include <boost/simd/memory/is_power_of_2.hpp>
#include <boost/assert.hpp>

int main()
{
  BOOST_ASSERT(  boost::simd::is_power_of_2(   2 ) );
  BOOST_ASSERT(  boost::simd::is_power_of_2(  16 ) );
  BOOST_ASSERT( !boost::simd::is_power_of_2(   0 ) );
  BOOST_ASSERT( !boost::simd::is_power_of_2(1337 ) );
}


// randomizer.cpp

// [[Rcpp::plugins(cpp11)]]
// [[Rcpp::plugins(openmp)]]

#include "randomizer.h"
#include <numeric>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/lognormal_distribution.hpp>
#include <boost/random/cauchy_distribution.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/beta_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/bernoulli_distribution.hpp>
#include <boost/random/binomial_distribution.hpp>
#include <boost/random/poisson_distribution.hpp>
#include <boost/random/geometric_distribution.hpp>
using namespace boost::random;

Randomizer::Randomizer(unsigned long int s)
 : seed(s)
{
    Reset();
}

void Randomizer::Reset()
{
    if (seed == 0)
        generator.seed();
    else
        generator.seed(seed);
}

double Randomizer::Uniform(double min, double max)
{
    uniform_real_distribution<double> d(min, max);
    return d(generator);
}

double Randomizer::RoundedUniform(double min, double max, double shoulder)
{
    if (min >= max)
        return min;
    double z = Uniform();
    double sd = shoulder * (max - min) / ((1 - shoulder) * 2.50662827463);
    if (z < shoulder / 2)
        return min - abs(Normal(0, sd));
    else if (z < shoulder)
        return max + abs(Normal(0, sd));
    else
        return Uniform(min, max);
}

double Randomizer::Normal(double mean, double sd)
{
    normal_distribution<double> d(mean, sd);
    return d(generator);
}

double Randomizer::Normal(double mean, double sd, double clamp)
{
    double n;
    do n = Normal(mean, sd); while (std::fabs(n - mean) > clamp);
    return n;
}

double Randomizer::LogNormal(double zeta, double sd)
{
    lognormal_distribution<double> d(zeta, sd);
    return d(generator);
}

double Randomizer::Cauchy(double x0, double gamma)
{
    cauchy_distribution<double> d(x0, gamma);
    return d(generator);
}

double Randomizer::Exponential(double rate)
{
    exponential_distribution<double> d(rate);
    return d(generator);
}

double Randomizer::Gamma(double shape, double scale)
{
    gamma_distribution<double> d(shape, scale);
    return d(generator);
}

double Randomizer::Beta(double alpha, double beta)
{
    beta_distribution<double> d(alpha, beta);
    return d(generator);
}

unsigned int Randomizer::Discrete(unsigned int size)
{
    uniform_int_distribution<unsigned int> d(0, size - 1);
    return d(generator);
}

int Randomizer::Discrete(int min, int max)
{
    uniform_int_distribution<int> d(min, max);
    return d(generator);
}

void Randomizer::Multinomial(unsigned int N, std::vector<double>& p, std::vector<unsigned int>& n_out)
{
    unsigned int n = N;
    double p_denom = std::accumulate(p.begin(), p.end(), 0.0);
    for (unsigned int i = 0; i < p.size() - 1; ++i)
    {
        n_out[i] = Binomial(n, p[i] / p_denom);
        n -= n_out[i];
        p_denom -= p[i];
    }
    n_out[p.size() - 1] = n;
}

bool Randomizer::Bernoulli(double p)
{
    if (p <= 0) return false;
    if (p >= 1) return true;
    bernoulli_distribution<double> d(p);
    return d(generator);
}

unsigned int Randomizer::Binomial(unsigned int n, double p)
{
    if (p <= 0) return 0;
    binomial_distribution<int, double> d(n, p);
    return d(generator);
}

unsigned int Randomizer::BetaBinomial(unsigned int n, double p, double a_plus_b)
{
    if (a_plus_b > 0) {
        p = Beta(a_plus_b * p, a_plus_b * (1 - p));
    }
    return Binomial(n, p);
}

int Randomizer::Poisson(double mean)
{
    if (mean <= 0) return 0;
    poisson_distribution<unsigned int, double> d(mean);
    return d(generator);
}

int Randomizer::Geometric(double p)
{
    if (p <= 0) return 0;
    geometric_distribution<unsigned int, double> d(p);
    return d(generator);
}

int Randomizer::Round(double x)
{
    int sign = x < 0 ? -1 : 1;
    double intpart, fracpart;
    fracpart = std::modf(std::fabs(x), &intpart);
    return sign * (intpart + Bernoulli(fracpart));
}

unsigned int Randomizer::operator()()
{
    return generator();
}

unsigned int Randomizer::operator()(unsigned int size)
{
    return Discrete(size);
}


#include "benchmark/benchmark.h"
#include <Eigen/Dense>

void BM_DGEMM(benchmark::State& state) {
  // First resume to ensure the timer is running.  Without this benchmark
  // gives garbage results.
  // You do not need to do this with the opensource version of benchmark.
  state.ResumeTiming();
  state.PauseTiming();
  const int size = state.range(0);
  Eigen::MatrixXd a = Eigen::MatrixXd::Random(size, size);
  Eigen::MatrixXd b = Eigen::MatrixXd::Random(size, size);
  Eigen::MatrixXd c = Eigen::MatrixXd::Random(size, size);
  state.ResumeTiming();
  for (auto _ : state) {
    a.noalias() += b * c;
  }
}

BENCHMARK(BM_DGEMM)
    ->Arg(3)
    ->Arg(4)
    ->Arg(5)
    ->Arg(6)
    ->Arg(7)
    ->Arg(8)
    ->Arg(9)
    ->Arg(10)
    ->Arg(15)
    ->Arg(16)
    ->Arg(20)
    ->Arg(24)
    ->Arg(28)
    ->Arg(31)
    ->Arg(32)
    ->Arg(40)
    ->Arg(63)
    ->Arg(64)
    ->Arg(80)
    ->Arg(100)
    ->Arg(128)
    ->Arg(150)
    ->Arg(200)
    ->Arg(256)
    ->Arg(300)
    ->Arg(400)
    ->Arg(500)
    ->Arg(600);
// any larger and it OOMs

BENCHMARK_MAIN();


#include <stan/math/fwd/mat.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/fpclassify.hpp>
#include <limits>

using stan::math::fvar;

TEST(AgradFwdMatrixSquaredDistance, vector_fd_vector_fd) {
  stan::math::vector_fd v1, v2;

  v1.resize(3);
  v2.resize(3);
  v1 << 1, 3, -5;
  v2 << 4, -2, -1;
  v1(0).d_ = 1.0;
  v1(1).d_ = 2.0;
  v1(2).d_ = 3.0;
  v2(0).d_ = 4.0;
  v2(1).d_ = 5.0;
  v2(2).d_ = 6.0;

  stan::math::fvar<double> a = stan::math::squared_distance(v1, v2);

  EXPECT_FLOAT_EQ(50, a.val_);
  EXPECT_FLOAT_EQ(12, a.d_);

  v1.resize(0);
  v2.resize(0);
  EXPECT_FLOAT_EQ(0, stan::math::squared_distance(v1, v2).val_);

  v1.resize(1);
  v2.resize(2);
  v1 << 1;
  v2 << 2, 3;
  EXPECT_THROW(stan::math::squared_distance(v1, v2), std::invalid_argument);
}

TEST(AgradFwdMatrixSquaredDistance, rowvector_fd_vector_fd) {
  stan::math::row_vector_fd rv;
  stan::math::vector_fd v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;

  stan::math::fvar<double> a = stan::math::squared_distance(rv, v);

  EXPECT_FLOAT_EQ(50, a.val_);
  EXPECT_FLOAT_EQ(12, a.d_);

  rv.resize(0);
  v.resize(0);
  EXPECT_FLOAT_EQ(0, stan::math::squared_distance(rv, v).val_);

  rv.resize(1);
  v.resize(2);
  rv << 1;
  v << 2, 3;
  EXPECT_THROW(stan::math::squared_distance(rv, v), std::invalid_argument);
}

TEST(AgradFwdMatrixSquaredDistance, vector_fd_rowvector_fd) {
  stan::math::row_vector_fd rv;
  stan::math::vector_fd v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;

  stan::math::fvar<double> a = stan::math::squared_distance(v, rv);

  EXPECT_FLOAT_EQ(50, a.val_);
  EXPECT_FLOAT_EQ(12, a.d_);

  v.resize(0);
  rv.resize(0);
  EXPECT_FLOAT_EQ(0, stan::math::squared_distance(v, rv).val_);

  v.resize(1);
  rv.resize(2);
  v << 1;
  rv << 2, 3;
  EXPECT_THROW(stan::math::squared_distance(v, rv), std::invalid_argument);
}

TEST(AgradFwdMatrixSquaredDistance, special_values_fd) {
  stan::math::vector_fd v1, v2;
  v1.resize(1);
  v2.resize(1);

  v1 << 0;
  v2 << std::numeric_limits<double>::quiet_NaN();
  EXPECT_TRUE(boost::math::isnan(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(boost::math::isnan(stan::math::squared_distance(v2, v1)));

  v1 << 0;
  v2 << std::numeric_limits<double>::infinity();
  EXPECT_TRUE(boost::math::isinf(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(boost::math::isinf(stan::math::squared_distance(v2, v1)));

  v1 << std::numeric_limits<double>::infinity();
  v2 << std::numeric_limits<double>::infinity();
  EXPECT_TRUE(boost::math::isnan(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(boost::math::isnan(stan::math::squared_distance(v2, v1)));

  v1 << -std::numeric_limits<double>::infinity();
  v2 << std::numeric_limits<double>::infinity();
  EXPECT_TRUE(boost::math::isinf(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(boost::math::isinf(stan::math::squared_distance(v2, v1)));
}

TEST(AgradFwdMatrixSquaredDistance, vector_ffd_vector_ffd) {
  stan::math::vector_ffd v1, v2;

  v1.resize(3);
  v2.resize(3);
  v1 << 1, 3, -5;
  v2 << 4, -2, -1;
  v1(0).d_ = 1.0;
  v1(1).d_ = 2.0;
  v1(2).d_ = 3.0;
  v2(0).d_ = 4.0;
  v2(1).d_ = 5.0;
  v2(2).d_ = 6.0;

  stan::math::fvar<fvar<double> > a = stan::math::squared_distance(v1, v2);

  EXPECT_FLOAT_EQ(50, a.val_.val_);
  EXPECT_FLOAT_EQ(12, a.d_.val_);

  v1.resize(0);
  v2.resize(0);
  EXPECT_FLOAT_EQ(0, stan::math::squared_distance(v1, v2).val_.val_);

  v1.resize(1);
  v2.resize(2);
  v1 << 1;
  v2 << 2, 3;
  EXPECT_THROW(stan::math::squared_distance(v1, v2), std::invalid_argument);
}

TEST(AgradFwdMatrixSquaredDistance, rowvector_ffd_vector_ffd) {
  stan::math::row_vector_ffd rv;
  stan::math::vector_ffd v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;

  stan::math::fvar<fvar<double> > a = stan::math::squared_distance(rv, v);

  EXPECT_FLOAT_EQ(50, a.val_.val_);
  EXPECT_FLOAT_EQ(12, a.d_.val_);

  rv.resize(0);
  v.resize(0);
  EXPECT_FLOAT_EQ(0, stan::math::squared_distance(rv, v).val_.val_);

  rv.resize(1);
  v.resize(2);
  rv << 1;
  v << 2, 3;
  EXPECT_THROW(stan::math::squared_distance(rv, v), std::invalid_argument);
}

TEST(AgradFwdMatrixSquaredDistance, vector_ffd_rowvector_ffd) {
  stan::math::row_vector_ffd rv;
  stan::math::vector_ffd v;

  rv.resize(3);
  v.resize(3);
  rv << 1, 3, -5;
  v << 4, -2, -1;
  rv(0).d_ = 1.0;
  rv(1).d_ = 2.0;
  rv(2).d_ = 3.0;
  v(0).d_ = 4.0;
  v(1).d_ = 5.0;
  v(2).d_ = 6.0;

  stan::math::fvar<fvar<double> > a = stan::math::squared_distance(v, rv);

  EXPECT_FLOAT_EQ(50, a.val_.val_);
  EXPECT_FLOAT_EQ(12, a.d_.val_);

  v.resize(0);
  rv.resize(0);
  EXPECT_FLOAT_EQ(0, stan::math::squared_distance(v, rv).val_.val_);

  v.resize(1);
  rv.resize(2);
  v << 1;
  rv << 2, 3;
  EXPECT_THROW(stan::math::squared_distance(v, rv), std::invalid_argument);
}

TEST(AgradFwdMatrixSquaredDistance, special_values_ffd) {
  stan::math::vector_ffd v1, v2;
  v1.resize(1);
  v2.resize(1);

  v1 << 0;
  v2 << std::numeric_limits<double>::quiet_NaN();
  EXPECT_TRUE(boost::math::isnan(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(boost::math::isnan(stan::math::squared_distance(v2, v1)));

  v1 << 0;
  v2 << std::numeric_limits<double>::infinity();
  EXPECT_TRUE(boost::math::isinf(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(boost::math::isinf(stan::math::squared_distance(v2, v1)));

  v1 << std::numeric_limits<double>::infinity();
  v2 << std::numeric_limits<double>::infinity();
  EXPECT_TRUE(boost::math::isnan(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(boost::math::isnan(stan::math::squared_distance(v2, v1)));

  v1 << -std::numeric_limits<double>::infinity();
  v2 << std::numeric_limits<double>::infinity();
  EXPECT_TRUE(boost::math::isinf(stan::math::squared_distance(v1, v2)));
  EXPECT_TRUE(boost::math::isinf(stan::math::squared_distance(v2, v1)));
}


#pragma once

#include "data.hpp"

#include <boost/math/distributions/chi_squared.hpp>

// Returns true if a is independent of b given X according to Pearson's
// chi-squared test applied to given data.
bool pearsonChiSquaredIndTest(const Data& data, int a, Bitset X, int b) {
    CHECK(a >= 0 && a <= (int)data.catCounts.size());
    CHECK(b >= 0 && b <= (int)data.catCounts.size());
    CHECK(a != b);
    CHECK(X.isSubsetOf(Bitset::range((int)data.catCounts.size())));
    CHECK(!X.contains(a));
    CHECK(!X.contains(b));

    vector<int> ord(data.points.size());
    for(int i = 0; i < (int)ord.size(); ++i) {
        ord[i] = i;
    }

    vector<int> splits;
    splits.push_back(0);
    if(!ord.empty()) {
        splits.push_back((int)ord.size());
    }

    vector<int> newSplits;

    double freedom = 1.0;
    vector<vector<int>> bins;

    X.iterate([&](int v) {
        freedom *= data.catCounts[v];

        if((int)bins.size() < data.catCounts[v]) {
            bins.resize(data.catCounts[v]);
        }

        newSplits.clear();
        newSplits.push_back(0);
        for(int s = 0; s < (int)splits.size() - 1; ++s) {
            int x = splits[s];
            int y = splits[s + 1];

            if(y - x == 1) {
                newSplits.push_back(y);
            } else {
                for(int c = 0; c < data.catCounts[v]; ++c) {
                    bins[c].clear();
                }
                for(int i = x; i < y; ++i) {
                    bins[data.points[ord[i]][v]].push_back(ord[i]);
                }
                int i = x;
                for(int c = 0; c < data.catCounts[v]; ++c) {
                    for(int p : bins[c]) {
                        ord[i++] = p;
                    }
                    if(i != newSplits.back()) {
                        newSplits.push_back(i);
                    }
                }
            }
        }
        swap(splits, newSplits);
    });

    int aCatCount = data.catCounts[a];
    int bCatCount = data.catCounts[b];
    vector<double> freqs(aCatCount * bCatCount);
    vector<double> aFreqs(aCatCount);
    vector<double> bFreqs(bCatCount);

    freedom *= (double)aCatCount - 1.0;
    freedom *= (double)bCatCount - 1.0;

    double chisq = 0.0;
    for(int s = 0; s < (int)splits.size() - 1; ++s) {
        fill(freqs.begin(), freqs.end(), 0.0);
        fill(aFreqs.begin(), aFreqs.end(), 0.0);
        fill(bFreqs.begin(), bFreqs.end(), 0.0);

        int x = splits[s];
        int y = splits[s + 1];
        double N = (double)(y - x);
        double unit = 1.0 / N;

        for(int i = x; i < y; ++i) {
            int aVal = data.points[ord[i]][a];
            int bVal = data.points[ord[i]][b];
            freqs[bVal * aCatCount + aVal] += unit;
            aFreqs[aVal] += unit;
            bFreqs[bVal] += unit;
        }

        double term = 0.0;
        for(int aVal = 0; aVal < aCatCount; ++aVal) {
            for(int bVal = 0; bVal < bCatCount; ++bVal) {
                double expected = aFreqs[aVal] * bFreqs[bVal];
                if(expected > 0.0) {
                    double diff = freqs[bVal * aCatCount + aVal] - expected;
                    term += diff * diff / expected;
                }
            }
        }
        term *= N;
        chisq += term;
    }

    boost::math::chi_squared_distribution<> dist(freedom);
    double crit = boost::math::quantile(dist, 0.95);
    return chisq < crit;
}


#ifndef _ISAC_
#define _ISAC_

#include <iostream>
#include <vector>
#include <Eigen/Dense>

using namespace Eigen;

class iSAC{
    public:
    iSAC(int x_dim, int u_dim);

    void Initialize(MatrixXd w1,MatrixXd w2,MatrixXd w3);
    void set_u_limit(double max[], double min[]);

    void Optimize(double t, VectorXd x, VectorXd x_ref);
    VectorXd Control(double t);

    VectorXd (*state_eq)(double t, VectorXd x, VectorXd u); //f
    MatrixXd (*control_func)(double t, VectorXd x); //h
    MatrixXd (*dstate_eq)(double t, VectorXd x, VectorXd u); //df

    VectorXd get_u_A(void);
    double get_tau_A(void);
    double get_duration(void);

    private:
    int x_dimention;
    int u_dimention;

    /*parameter*/
    const double T_S = 0.02; //sampling parameter
    const int T_HOR = 100; //time horizon
    const double INF = 100000000.0;
    const double EPS = 1.0e-3;

    double inc_cost(double t, VectorXd x, VectorXd x_ref);//incremental cost
    VectorXd dinc_cost(double t, VectorXd x, VectorXd x_ref);//differential incremental cost
    double end_cost(double t, VectorXd x, VectorXd x_ref); //end cost
    VectorXd dend_cost(double t, VectorXd x, VectorXd x_ref);//differential end cost

    double calc_J(double t, VectorXd x, VectorXd u, VectorXd x_ref);
    double calc_J_controlled(double t, MatrixXd x, MatrixXd u, VectorXd x_ref);

    VectorXd u_def(double t);

    /*iSAC parameter*/
    MatrixXd u_nom = VectorXd::Zero(2);//nominal control (often u_nom = 0)
    MatrixXd Q = MatrixXd::Identity(4,4);
    MatrixXd P = MatrixXd::Identity(4,4);
    MatrixXd R = MatrixXd::Identity(2,2);
    double alpha_d = -5;
    double default_duration = T_S/100;

    /*limit*/
    double* u_max;
    double* u_min;

    /*variable*/
    VectorXd u_A;
    double tau_A;
    double duration;

    std::vector<VectorXd> u_s;
    std::vector<double> tau_s;
    std::vector<double> duration_s;
};

#endif //_iSAC_


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

#include <iostream>
#include <fstream>
#include <string>
#include <cstdlib>

#include <NTL/ZZ.h>
#include <NTL/RR.h>

using namespace std;
using namespace NTL;

#define M_PIl 	3.141592653589793238462643383279502884L
#define PI		RR(M_PIl)

//=============================================================
//==Functions==================================================
//=============================================================

//--------------------------------------------------------
//--Find an index corresponding to the maximum value------
//--------------------------------------------------------
int max_index(double* array, int length) {
	int max_ind	= 0;
	double max = array[0];

	for(int i=1; i<length; i++) {
		if(array[i] > max) {
			max_ind = i;
			max = array[i];
		}
	}
	return max_ind;
}

//--------------------------------------------------------
//--Long division between polynomials represented---------
//---in Chebyshev basis-----------------------------------
//--------------------------------------------------------
void Long_div(RR* p, int n, RR* q, int m, RR* quot, RR* rem) {
//--------------------------------------------------------
//--p : Dividend-------q : Divisor------------------------
//--n : degree of p----m : degree of q--------------------
//--quot : quotient----rem : remainder--------------------
//--------------------------------------------------------
	if(m>0) {
		for(int i=n; i>m; i--) {
			RR ratio = p[i]/q[m];
			for(int j=0; j<=m; j++) {
				p[i-m+j] -= q[j]*ratio;
				p[abs(i-m-j)] -= q[j]*ratio;
			}
			quot[i-m] = RR(2.0)*ratio;
		}

		RR ratio = p[m]/q[m];
		for(int j=0; j<=m; j++)
			p[j] -= q[j]*ratio;
		quot[0] = ratio;

	} else {
		for(int i=n; i>=0; i--) {
			quot[i] = p[i]/q[0];
			p[i] = RR(0.0);
		}
	}

	for(int i=0; i<m; i++)
		rem[i] = p[i];
}

//=============================================================
//==Main=======================================================
//=============================================================

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

//=============================================================
//==Setting====================================================
//=============================================================

	int K = 12;								// I_i = [i-.25-e, i-.25+e] where |i|< K

	int deg_bdd = atoi(argv[1]) + 1;		// Bound of the degree +1

	int* deg = new int[K];					// deg[i] = The number of nodes in I_i
	for(int i=0; i<K; i++)					// We assume deg[i] = deg[-i]
		deg[i] = 1;							// Initialize all deg[i] to 1
	int tot_deg = 2*K-1;					// Total number of nodes

	int dev = atoi(argv[2]);
	double err = 1.0/(1 << atoi(argv[2]));	// Maximum deviation from each i-.25

	int sc_num = atoi(argv[3]);				// The number of scaling
	RR sc_fac = conv<RR>(ZZ(1) << sc_num);	// Scaling factor

	RR::SetPrecision(1000);

//=============================================================
//==Degree Searching===========================================
//=============================================================
//--------------------------------------------------------
//--Initialize--------------------------------------------
//--------------------------------------------------------

	double* bdd = new double[K];

	double temp = 0;
	for(int i=1; i<=(2*K-1); i++)
		temp -= log2((double)i);
	temp += (2*K-1)*log2(2*M_PI);
	temp += log2(err);

	for(int i=0; i<K; i++) {
		bdd[i] = temp;
		for(int j=1; j<=K-1-i; j++)
			bdd[i] += log2((double)j + err);
		for(int j=1; j<=K-1+i; j++)
			bdd[i] += log2((double)j + err);
	}

//--------------------------------------------------------
//--Algorithm---------------------------------------------
//--1. Find a point that has the largest theoretical error bound.
//--2. Increase degree by one at that point.--------------
//--(If the point is not 0, also increase degree by-------
//---one at negate of that point)-------------------------
//--3. Check whether total degree is greater than the degree bound
//---If so, end the algorithm. If not, go back to 1.------
//--------------------------------------------------------

	int max_iter = 200;	// Bound of the number of iteration
	int iter;

	for(iter=0; iter<max_iter; iter++) {
		if(tot_deg >= deg_bdd)
			break;
		int maxi = max_index(bdd, K);

		if(maxi != 0) {
			if((tot_deg+2) > deg_bdd)
				break;

			for(int i=0; i<K; i++) {
				bdd[i] -= log2(tot_deg+1);
				bdd[i] -= log2(tot_deg+2);
				bdd[i] += 2.0*log2(2.0*M_PI);

				if(i != maxi) {
					bdd[i] += log2(abs((double)(i-maxi)) + err);
					bdd[i] += log2((double)(i+maxi) + err);
				} else { // i = maxi
					bdd[i] += (log2(err)-1.0);
					bdd[i] += log2(2.0*(double)i + err);
				}
			}

			tot_deg += 2;
		} else { // maxi = 0
			bdd[0] -= log2(tot_deg+1);
			bdd[0] += (log2(err)-1.0);
			bdd[0] += log2(2.0*M_PI);
			for(int i=1; i<K; i++) {
				bdd[i] -= log2(tot_deg+1);
				bdd[i] += log2(2.0*M_PI);
				bdd[i] += log2((double)i + err);
			}

			tot_deg += 1;
		}

		deg[maxi] += 1;
	}

	delete[] bdd;

//--------------------------------------------------------
//--Print the Result of Degree Searching------------------
//--------------------------------------------------------

	cout << "==============================================" << endl;
	cout << "==Degree Searching Result=====================" << endl;
	cout << "==============================================" << endl;

	if(iter == max_iter) {
		cout << "More Iteration Needed" << endl;
	} else {
		cout << "Degree of Polynomial : " << tot_deg-1 << endl;
		cout << "Degree : ";
		for(int i=0; i<K; i++)
			cout << deg[i] << "  ";
		cout << endl;
	}
	cout << "==============================================" << endl;

//=============================================================
//==Find an Interpolation Polynomial===========================
//==Goal : Interpolate cos(2PI x)==============================
//=============================================================
//--------------------------------------------------------
//--Node Setting------------------------------------------
//--------------------------------------------------------

	RR inter_size = RR(1.0)/conv<RR>(((ZZ)(1) << dev));
	// Half of the size of each interval

	RR* z = new RR[tot_deg];	// Node positions
	int cnt = 0;
	if((deg[0]%2)!=0)
		z[cnt++] = -RR(0.25);

	for(int i=K-1; i>0; i--) {
		for(int j=1; j<=deg[i]; j++) {
			RR temp = ((RR(2*j-1))*PI)/(RR(2*deg[i]));
			z[cnt++] = RR(i - 0.25) + inter_size*cos(temp);
			z[cnt++] = RR(-i - 0.25) - inter_size*cos(temp);
		}
	}

	for(int j=1; j<=(deg[0]/2); j++) {
		RR temp = ((RR(2*j-1))*PI)/(RR(2*deg[0]));
		z[cnt++] = RR(-0.25) + inter_size*cos(temp);
		z[cnt++] = RR(-0.25) - inter_size*cos(temp);
	}

	for(int i=0; i<tot_deg; i++)
		z[i] /= sc_fac;

	delete[] deg;

//--------------------------------------------------------
//--Algorithm---------------------------------------------
//--------------------------------------------------------

	RR* d = new RR[tot_deg];
	for(int i=0; i<tot_deg; i++)
		d[i] = cos(RR(2.0)*PI*z[i]);

	for(int j=1; j<tot_deg; j++) {
		for(int l=0; l<tot_deg-j; l++)
			d[l] = (d[l+1] - d[l]) / (z[l+j] - z[l]);
	}

//=============================================================
//==Compute Chebyshev Coefficients by Solving Matrix Equation==
//==Result Polynomial :     ===================================
//==	c[0]T_0(x) + ... + c[tot_deg-1]T_{tot_deg-1}(x)   =====
//== where T_i(x) is an adjusted Chebyshev polynomial =========
//=============================================================

	tot_deg += 1;

	RR* x = new RR[tot_deg];
	for(int i=0; i<tot_deg; i++)
		x[i] = RR(K)/sc_fac * cos(RR(i)*PI/RR(tot_deg-1));

	RR* c = new RR[tot_deg];
	RR* p = new RR[tot_deg];
	for(int i=0; i<tot_deg; i++) {
		p[i] = d[0];
		for(int j=1; j<tot_deg-1; j++)
			p[i] = p[i]*(x[i] - z[j]) + d[j];
	}

	delete[] z;

	RR** T = new RR*[tot_deg];
	for(int i=0; i<tot_deg; i++)
		T[i] = new RR[tot_deg];

	for(int i=0; i<tot_deg; i++) {
		T[i][0] = RR(1.0);
		T[i][1] = x[i]/(RR(K)/sc_fac);
		for(int j=2; j<tot_deg; j++)
			T[i][j] = RR(2.0)*(x[i]/(RR(K)/sc_fac))*T[i][j-1] - T[i][j-2];
	}

	for(int i=0; i<tot_deg-1; i++) {
		RR max_abs = abs(T[i][i]);
		int max_index = i;
		for(int j = i+1; j<tot_deg; j++) {
			if(abs(T[j][i]) > max_abs) {
				max_abs = abs(T[j][i]);
				max_index = j;
			}
		}

		if(i != max_index) {
			for(int j=i; j<tot_deg; j++) {
				RR temp = T[max_index][j];
				T[max_index][j] = T[i][j];
				T[i][j] = temp;
			}

			RR temp = p[max_index];
			p[max_index] = p[i];
			p[i] = temp;
		}

		for(int j=i+1; j<tot_deg; j++)
			T[i][j] /= T[i][i];
		p[i] /= T[i][i];
		T[i][i] = RR(1.0);

		for(int j=i+1; j<tot_deg; j++) {
			p[j] -= T[j][i] * p[i];
			for(int l=i+1; l<tot_deg; l++)
				T[j][l] -= T[j][i] * T[i][l];
			T[j][i] = RR(0.0);
		}
	}

	c[tot_deg-1] = p[tot_deg-1];
	for(int i=tot_deg-2; i>=0; i--) {
		c[i] = p[i];
		for(int j=i+1; j<tot_deg; j++)
			c[i] -= T[i][j]*c[j];
	}

	tot_deg -= 1;

	for(int i=0; i<tot_deg; i++)
		delete[] T[i];
	delete[] T;
	delete[] d;
	delete[] x;
	delete[] p;

//=============================================================
//==Baby Step Giant Step Algorithm=============================
//=============================================================
//--------------------------------------------------------
//--Parameter Setting-------------------------------------
//--------------------------------------------------------

	int temp_tot_deg = tot_deg;

	int m = 1;					// m = ceil(log2(tot_deg))
	while (temp_tot_deg > 1) {
		m++;
		temp_tot_deg /= 2;
	}

	int* pow2 = new int[m+1];	// pow2[i] = 2^i (i=0, ...,m)
	pow2[0] = 1;
	for(int i=0; i<m; i++)
		pow2[i+1] = 2*pow2[i];

	int l;						// l ~ m/2
	if( m % 2 == 0)	{
		l = m/2;
	} else {
		int l1 = m/2;
		int l2 = m/2 + 1;

		l = (pow2[l1] + pow2[m-l1] - l1
				<= pow2[l2] + pow2[m-l2] - l2) ? l1 : l2;
		// Choose one that requires less number
		//		of non-scalar multiplications
	}

//--------------------------------------------------------
//--Algorithm---------------------------------------------
//--Details:    ------------------------------------------
//---1. alg_coef[0][0] represents the interpolation poly--
//---2. alg_coef[i][j] represents polynomial p_{i,j}------
//---3. p_{i,j} = p_{i+1,2j} + p_{i+1,2j+1} T_{2^(m-i-1)}-
//--------------------------------------------------------

	RR*** alg_coef = new RR**[m-l+1];
	for(int i=0; i<m-l+1; i++) {
		alg_coef[i] = new RR*[pow2[i]];
		for(int j=0; j<pow2[i]; j++)
			alg_coef[i][j] = new RR[pow2[m-i]];
	}

	for(int i=0; i<tot_deg; i++)
		alg_coef[0][0][i] = c[i];
	for(int i=tot_deg; i<pow2[m]; i++)
		alg_coef[0][0][i] = RR(0.0);

	delete[] c;

	for(int i=0; i<m-l; i++) {
		RR* divisor = new RR[pow2[m-i-1]+1];
		for(int j=0; j<pow2[m-i-1]; j++)
			divisor[j] = RR(0.0);
		divisor[pow2[m-i-1]] = RR(1.0);

		for(int j=0; j<pow2[i]; j++)
			Long_div(alg_coef[i][j], pow2[m-i]-1, divisor, pow2[m-i-1],
				 		alg_coef[i+1][2*j+1], alg_coef[i+1][2*j]);
	}

//--------------------------------------------------------
//--Print Algorithm Coefficients in File------------------
//--------------------------------------------------------

	system("mkdir -p ./result/coef");
	string path_coef = "./result/coef/Deg";
	path_coef += (to_string(tot_deg-1)
			+ "Err" + to_string(dev)
			+ "Scale" + to_string(sc_num) + ".csv");

	ofstream output_coef(path_coef);

	for(int i=0; i<pow2[m-l]; i++) {
		for(int j=0; j<pow2[l]; j++)
			output_coef << alg_coef[m-l][i][j] << ", ";
		output_coef << endl;
	}

	output_coef.close();

//=============================================================
//==Find Maximum Error and Print Errors in File================
//=============================================================
//--------------------------------------------------------
//--File Path Setting-------------------------------------
//--------------------------------------------------------

	system("mkdir -p ./result/error");
	string path_err = "./result/error/Deg";
	path_err += (to_string(tot_deg-1)
			+ "Err" + to_string(dev)
			+ "Scale" + to_string(sc_num) + ".csv");

	ofstream output_err(path_err);

	output_err << "Tested Values" << "," << "Real Values" << ","
			<< "Approximate Values" << "," << "Error" << ","
			<< "log2(Error)" << endl;

//--------------------------------------------------------
//--Test Nodes Setting------------------------------------
//--------------------------------------------------------

	int test_num = 20;		// The number of test points in each interval I_i

	RR** test = new RR*[2*K-1];
	for(int i=0; i<2*K-1; i++)
		test[i] = new RR[test_num+1];

	RR incr = RR(2.0)*inter_size*RR(1.0/test_num);
	test[0][0] = -RR(0.25) - inter_size;
	for(int i=1; i<test_num+1; i++)
		test[0][i] = test[0][i-1] + incr;

	for(int i=1; i<=K-1; i++) {
		test[2*i-1][0] = RR(-i - 0.25) - inter_size;
		for(int j=1; j<test_num+1; j++)
			test[2*i-1][j] = test[2*i-1][j-1] + incr;
		test[2*i][0] = RR(i - 0.25) - inter_size;
		for(int j=1; j<test_num+1; j++)
			test[2*i][j] = test[2*i][j-1] + incr;
	}

//--------------------------------------------------------
//--Computation and Print Errors in File------------------
//--------------------------------------------------------

	RR max = RR(-999.0);
	for(int i=0; i<2*K-1; i++) {
		for(int j=0; j<test_num+1; j++) {

			RR real = cos(RR(2.0) * PI * test[i][j]); 	// Real value of cos(2PI x)
			RR approx = RR(0.0);						// Approximate value of cos(2PI x)

			RR* BS = new RR[pow2[l]]; 		// Baby-step basis  : T_0(x), ... , T_{2^l-1}(x)
			RR* GS = new RR[m-l];			// Giant-step basis : T_{2^l)(x), ... , T_{2^(m-1)}(x)

			BS[0] = RR(1.0);
			BS[1] = (test[i][j]/RR(K));

			for(int k=2; k<pow2[l]; k++)
				BS[k] = RR(2.0)*BS[k/2]*BS[k-k/2] - BS[k-2*(k/2)];

			GS[0] = RR(2.0)*BS[pow2[l-1]]*BS[pow2[l-1]] - RR(1.0);
			for(int k=1; k<m-l; k++)
				GS[k] = RR(2.0)*GS[k-1]*GS[k-1] - RR(1.0);

			RR** alg_value = new RR*[m-l+1]; 	// Recall that alg_coef[i][j] represents polynomial p_{i,j}
			for(int k=0; k<m-l+1; k++) 			// alg_value[i][j] : The value of p_{i,j} at x
				alg_value[k] = new RR[pow2[k]]; // alg_value[0][0] : The value of interpolation poly at x

			for(int k=0; k<pow2[m-l]; k++) {
				RR temp = RR(0.0);
				for(int s=0; s<pow2[l]; s++)
					temp += alg_coef[m-l][k][s]*BS[s];
				alg_value[m-l][k] = temp;
			}

			for(int k=m-l-1; k>=0; k--) {
				for(int s=0; s<pow2[k]; s++)
					alg_value[k][s] = alg_value[k+1][2*s] + GS[m-l-k-1]*alg_value[k+1][2*s+1];
			}
			approx = alg_value[0][0];

			for(int k=0; k<sc_num; k++)
				approx = RR(2.0)*approx*approx - RR(1.0);	// double angle formula

			output_err << test[i][j] << "," << real << ","
						<< approx << "," << approx-real << ",";

			if(approx-real!=0) {
				if(max < log(abs(approx-real))/log(RR(2.0)))
					max = log(abs(approx-real))/log(RR(2.0));

				output_err << log(abs(approx-real))/log(RR(2.0)) << endl;
			} else {
				output_err << "*" << endl;
			}
		}
	}

	output_err.close();

//--------------------------------------------------------
//--Print Maximum Error of the interpolation polynomial---
//--------------------------------------------------------

	cout << "==============================================" << endl;
	cout << "==Baby Step Giant Step Algorithm Result=======" << endl;
	cout << "==============================================" << endl;
	cout << "Max_Error : " << max << endl;
	cout << "==============================================" << endl;

	for(int i=0; i<m-l+1; i++) {
		for(int j=0; j<pow2[i]; j++)
			delete[] alg_coef[i][j];
		delete[] alg_coef[i];
	}
	delete[] alg_coef;

	for(int i=0; i<2*K-1; i++)
		delete[] test[i];
	delete[] test;
}