#include <iostream>
#include <stdio.h>
#include <fstream>
#include <string>
#include <signal.h>
#include <math.h>
#include <ctime>
#include <sstream>

#include <ros/ros.h>
#include <ros/package.h>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>

#include "core_msgs/PathArray.h"
#include "core_msgs/VehicleState.h"
#include "core_msgs/Control.h"
#include "core_msgs/CenPoint.h"
#include "core_msgs/Estop.h"
#include "core_msgs/ParkPoints.h"
#include "core_util/zdebug.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Float32.h"
#include "geometry_msgs/Pose2D.h"
#include "geometry_msgs/Vector3.h"
#include "sensor_msgs/CompressedImage.h"
#include "sensor_msgs/Image.h"
#include "opencv2/opencv.hpp"
#include <boost/thread.hpp>

bool MONITOR_DEBUG=false;
bool USE_ROSBAG = false;
bool IS_RECORD = false;

struct vehicle_state {
  bool is_auto;
  bool estop;
  int flag_obstacle;
  int gear;
  int brake;
  float speed;
  float steer;
  int control_mode;
};

std::string config_path;

cv::VideoWriter outputMonitorVideo;

vehicle_state vState;
vehicle_state vControl;

image_transport::Publisher publishMonitorImg;
sensor_msgs::ImagePtr msgMonitorImg;
int map_width, map_height;
float map_res;
int x_waypoint, y_waypoint;
cv::Mat occupancy_map;
double occupancy_map_timestamp;
cv::Mat lane_topview;
double lane_topview_timestamp;

cv::Mat state_monitor;
cv::Mat monitor_img;
std::vector<geometry_msgs::Vector3> path_points;
std::vector<geometry_msgs::Vector3> path_points_update;
core_msgs::ParkPoints parkpoints;

double pathtracking_timestamp;

boost::mutex map_mutex_;
int e_stop = 0;
int spath_n = 0;
float uturn_angle =0.0;

void callbackLaneCam(const sensor_msgs::ImageConstPtr& msg_lane_cam)
{
  cv_bridge::CvImageConstPtr cv_ptr;
  try
  {
    cv_ptr = cv_bridge::toCvCopy(msg_lane_cam, sensor_msgs::image_encodings::RGB8);
  }
  catch (cv_bridge::Exception& e)
  {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
  }
  cv::Mat temp = cv_ptr->image.clone();
  cv::Mat temp_resized;
  cv::resize(temp, temp_resized, cv::Size(map_height*2,map_width*2),0,0);

  cv::transpose(temp_resized, temp_resized);
  cv::Mat rot_matrix = getRotationMatrix2D(cv::Point(map_height, map_width), 180, 1.0);
  cv::warpAffine(temp_resized, lane_topview, rot_matrix, temp_resized.size());

  // lane_topview = cv_ptr->image.clone();
  lane_topview_timestamp = msg_lane_cam->header.stamp.toSec();
}
void callbackOccupancyRaw(const sensor_msgs::ImageConstPtr& msg_occupancy_raw)
{
  cv_bridge::CvImageConstPtr cv_ptr;
  try
  {
    cv_ptr = cv_bridge::toCvCopy(msg_occupancy_raw, sensor_msgs::image_encodings::RGB8);
  }
  catch (cv_bridge::Exception& e)
  {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
  }
  state_monitor = cv_ptr->image.clone();
}

void callbackOccupancyMap(const sensor_msgs::ImageConstPtr& msg_occupancy_map)
{
  //if(Z_DEBUG) std::cout<<"occupanc map callback!"<<std::endl;
  cv_bridge::CvImageConstPtr cv_ptr;
  try
  {
    cv_ptr = cv_bridge::toCvCopy(msg_occupancy_map, sensor_msgs::image_encodings::RGB8);
  }
  catch (cv_bridge::Exception& e)
  {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
  }
  occupancy_map = cv_ptr->image.clone();
  occupancy_map_timestamp = msg_occupancy_map->header.stamp.toSec();
}

void callbackWaypoint(const core_msgs::CenPointConstPtr& msg_waypoint) {
  x_waypoint = (float)(msg_waypoint->x_waypoint)/map_res;
  y_waypoint = (float)(msg_waypoint->y_waypoint)/map_res;
}

void callbackFlagObstacle(const std_msgs::Int32::ConstPtr & msg_flag_obstacle) {
  vState.flag_obstacle = msg_flag_obstacle->data;
}

void callbackEstop(const core_msgs::Estop::ConstPtr & msg_estop) {
  e_stop = msg_estop->estop;
}

void callbackSpathn(const std_msgs::Int32::ConstPtr & msg_lookahead_n) {
  spath_n = msg_lookahead_n->data;
}

void callbackUturn(const std_msgs::Float32::ConstPtr & msg_uturn_angle) {
  uturn_angle = msg_uturn_angle->data;
}

void callbackPath(const core_msgs::PathArrayConstPtr& msg_path_tracking)
{
  path_points = msg_path_tracking->pathpoints;
  pathtracking_timestamp = msg_path_tracking->header.stamp.toSec();
  // if(Z_DEBUG) std::cout<<"path callback timestamp:"<<pathtracking_timestamp<<std::endl;
}
void callbackPathUpdate(const core_msgs::PathArrayConstPtr& msg_path_tracking)
{
  path_points_update = msg_path_tracking->pathpoints;
}

void callbackState(const core_msgs::VehicleStateConstPtr& msg_state) {
  vState.is_auto = msg_state->is_auto;
  vState.estop = msg_state->estop;
  vState.gear = msg_state->gear;
  vState.brake = msg_state->brake;
  vState.steer = -msg_state->steer;
  vState.speed = msg_state->speed;
}

void callbackControl(const core_msgs::ControlConstPtr& msg_control) {
  vControl.is_auto = msg_control->is_auto;
  vControl.estop = msg_control->estop;
  vControl.gear = msg_control->gear;
  vControl.brake = msg_control->brake;
  vControl.steer = -msg_control->steer;
  vControl.speed = msg_control->speed;
  vControl.control_mode = msg_control->control_mode;
}

void callbackPark(const core_msgs::ParkPointsConstPtr & msg_park) {
  parkpoints.initpoints = msg_park->initpoints;
  parkpoints.goal_point = msg_park->goal_point;
}

void callbackTerminate(const std_msgs::Int32Ptr& record){
  if(IS_RECORD) {
    outputMonitorVideo.release();
    ROS_INFO("Monitor Video recording safely terminated");
  }
  ros::shutdown();
  return;
}

void drawPath(cv::Mat &canvas) {
  if(vControl.control_mode == 2) {
    //std::cout<<"drawing path"<<std::endl;
    for(int i= path_points.size()-1; i>=1;i--){
      int start_x = (int)(2*path_points.at(i).x);
      int start_y = (int)(2*path_points.at(i).y);
      int end_x = (int)(2*path_points.at(i-1).x);
      int end_y = (int)(2*path_points.at(i-1).y);
      if(i == path_points.size()-spath_n) {
          cv::line(canvas,cv::Point(start_x, start_y), cv::Point(end_x, end_y), cv::Scalar(255,255,100),2);
      }
      else cv::line(canvas,cv::Point(start_x, start_y), cv::Point(end_x, end_y), cv::Scalar(100,255,100),2);
    }
  }
  if(MONITOR_DEBUG && vControl.control_mode ==2) {
    for(int i= path_points_update.size()-1; i>=1;i--){
      int start_x = (int)(2*path_points_update.at(i).x);
      int start_y = (int)(2*path_points_update.at(i).y);
      int end_x = (int)(2*path_points_update.at(i-1).x);
      int end_y = (int)(2*path_points_update.at(i-1).y);
      cv::line(canvas,cv::Point(start_x, start_y), cv::Point(end_x, end_y), cv::Scalar(100,255,255),2);
    }
  }
  else if(vControl.control_mode == 3) {
    cv::line(canvas,cv::Point(map_width, map_height*2), cv::Point(map_width - 2*y_waypoint, map_height*2-2*x_waypoint), cv::Scalar(255,255,100),2);
  }
  else if(vControl.control_mode ==4) {
    cv::line(canvas,cv::Point(map_width, map_height*2), cv::Point(map_width - 2*y_waypoint, map_height*2-2*x_waypoint), cv::Scalar(170,255,100),2);
  }
}

int main(int argc, char** argv)
{
  //argument setting initialization
  if(argc < 4)  {
      std::cout << "usage: rosrun zero_monitor z_monitor debug_mode for_rosbag is_record" << std::endl;
      std::cout << "debug_mode is true for debug" << std::endl;
      std::cout << "for_rosbag is true for monitoring rosbag files" << std::endl;
      std::cout << "is_record is true to record monitoring window as avi file" <<std::endl;
      return -1;
  }

  if (!strcmp(argv[1], "true")) MONITOR_DEBUG = true;
  if (!strcmp(argv[2], "true")) USE_ROSBAG = true;
  if (!strcmp(argv[3], "true")) IS_RECORD = true;

  std::string record_path = ros::package::getPath("core_util");
  //TODO: add date&time to the file name
  record_path += "/data/";

  time_t rawtime;
  struct tm * timeinfo;
  char buffer[80];

  time (&rawtime);
  timeinfo = localtime(&rawtime);
  strftime(buffer,sizeof(buffer),"%y%m%d-%H:%M:%S",timeinfo);
  std::string date_str(buffer);
  record_path += date_str;
  record_path += "_monitor.avi";

  ROS_INFO_STREAM(record_path);

  x_waypoint = 180;
  y_waypoint = 0;

  //TODO: change root path later
  config_path = ros::package::getPath("core_util");
  config_path += "/config/system_config.yaml";
  cv::FileStorage params_config(config_path, cv::FileStorage::READ);
  map_width = params_config["Map.width"];
  map_height = params_config["Map.height"];
  map_res = params_config["Map.resolution"];
  float vehicle_width = params_config["Vehicle.width"];
  float vehicle_length_front = params_config["Vehicle.length_front"];

  bool isVideoOpened = false;
  if(IS_RECORD) {
    isVideoOpened =  outputMonitorVideo.open(record_path, CV_FOURCC('X', 'V', 'I', 'D'), 10, cv::Size(map_width*6, map_height*2+100), true);

    if(isVideoOpened)
      ROS_INFO("monitor video starts recorded!");
  }

  occupancy_map = cv::Mat::zeros(map_height,map_width,CV_8UC3);
  lane_topview = cv::Mat::zeros(map_height,map_width,CV_8UC3);
  state_monitor = cv::Mat::zeros(map_height,map_width,CV_8UC3);

  ros::init(argc, argv, "map_generator");
  ros::start();

  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  if(!USE_ROSBAG) publishMonitorImg = it.advertise("/monitor",1);
  else publishMonitorImg = it.advertise("/monitor_rosbag",1);
  msgMonitorImg.reset(new sensor_msgs::Image);

  ros::Subscriber laneSub, occupancySub, occupancyRawSub;
  if(!USE_ROSBAG) {
    laneSub = nh.subscribe("/warped_image",1,callbackLaneCam);
    occupancySub = nh.subscribe("/occupancy_map",1,callbackOccupancyMap);
    occupancyRawSub = nh.subscribe("/occupancy_map_raw",1,callbackOccupancyRaw);
  }
  else {
    laneSub = nh.subscribe("/warped_image",1,callbackLaneCam);
    occupancySub = nh.subscribe("/occupancy_map/rosbag",1,callbackOccupancyMap);
    occupancyRawSub = nh.subscribe("/occupancy_map_raw",1,callbackOccupancyRaw);
  }

  ros::Subscriber pathSub, pathupdateSub;
  if(MONITOR_DEBUG){
    pathSub = nh.subscribe("/sPath",1,callbackPath);
    pathupdateSub = nh.subscribe("/path_tracking",1,callbackPathUpdate);
  }
  else pathSub = nh.subscribe("/path_tracking",1,callbackPath);

  ros::Subscriber waypointSub, flagobstacleSub, stateSub, controlSub, endSub, estopSub, spathnSub, uturnSub, parkSub;
  waypointSub = nh.subscribe("/updated_cpoint",1,callbackWaypoint);
  flagobstacleSub = nh.subscribe("/flag_obstacle",1,callbackFlagObstacle);
  stateSub = nh.subscribe("/vehicle_state",1,callbackState);
  controlSub = nh.subscribe("/control",1,callbackControl);
  endSub = nh.subscribe("/end_system",1,callbackTerminate);
  estopSub = nh.subscribe("/emergency_stop",1,callbackEstop);
  spathnSub = nh.subscribe("/lookahead_n",1, callbackSpathn);
  uturnSub = nh.subscribe("/uturn_angle",1,callbackUturn);
  //TODO: change this to updated initial_points (from path tracker)
  parkSub = nh.subscribe("/updated_ppoint",1,callbackPark);
  vState.speed = 0;
  vState.steer = 0;
  if(MONITOR_DEBUG) vState.flag_obstacle = 1;
  else vState.flag_obstacle = 0;
  ros::Rate loop_rate(10);
  ros::Time init_time = ros::Time::now();

  int left_wheel_xc, wheel_yc, right_wheel_xc;
  int left_wheel_x1, left_wheel_y1, left_wheel_x2, left_wheel_y2, right_wheel_x1, right_wheel_y1, right_wheel_x2, right_wheel_y2;
  left_wheel_xc = map_width - (int)(vehicle_width/map_res);
  wheel_yc = 2*map_height-1 - (int)(vehicle_length_front/map_res);
  right_wheel_xc = map_width + (int)(vehicle_width/map_res);
  int wheel_radius = 18;

  while(ros::ok()){
    //if(Z_DEBUG) std::cout<<"while loop starts!!"<<std::endl;

    //monitor initialization
    monitor_img = cv::Mat::zeros(map_height*2+100, map_width*6,CV_8UC3);

    cv::Mat occupancy_map_resized = cv::Mat::zeros(map_height*2,map_width*2,CV_8UC3);
    cv::resize(occupancy_map, occupancy_map_resized, cv::Size(map_height*2,map_width*2),0,0);

    cv::Mat lane_topview_resized = cv::Mat::zeros(map_height*2,map_width*2,CV_8UC3);
    cv::resize(lane_topview, lane_topview_resized, cv::Size(map_height*2,map_width*2),0,0);

    //drawing waypoint
    cv::circle(state_monitor, cv::Point(map_width/2 - y_waypoint, map_height-x_waypoint),3,cv::Scalar(255,255,100), -1);
    cv::Mat state_monitor_resized = cv::Mat::zeros(map_height*2,map_width*2,CV_8UC3);
    cv::resize(state_monitor, state_monitor_resized, cv::Size(map_height*2,map_width*2),0,0);
    //if(Z_DEBUG) std::cout<<"state monitor resized!!"<<std::endl;
    drawPath(state_monitor_resized);
    drawPath(lane_topview_resized);
    cv::rectangle(state_monitor_resized, cv::Point(map_width - (int)(vehicle_width/map_res), 2*map_height-1 - (int)(vehicle_length_front/map_res)), cv::Point(map_width+(int)(vehicle_width/map_res), 2*map_height), cv::Scalar(100,100,225), 1);

    //draw parkpoints
    int park_mode;
    nh.getParam("park_mode", park_mode);
    if(parkpoints.initpoints.size() > 0) {
      int init_1_x = map_width/2 - (int)(parkpoints.initpoints.at(0).y/map_res);
      int init_1_y = map_height - (int)(parkpoints.initpoints.at(0).x/map_res);
      int init_2_x = map_width/2 - (int)(parkpoints.initpoints.at(1).y/map_res);
      int init_2_y = map_height - (int)(parkpoints.initpoints.at(1).x/map_res);
      int goal_x = map_width/2 - (int)(parkpoints.goal_point.y/map_res);
      int goal_y = map_height - (int)(parkpoints.goal_point.x/map_res);
      cv::circle(lane_topview_resized, cv::Point(2*init_1_x, 2*init_1_y),5,cv::Scalar(100,100,255), -1);
      cv::circle(lane_topview_resized, cv::Point(2*init_2_x, 2*init_2_y),5,cv::Scalar(100,100,255), -1);
      cv::circle(lane_topview_resized, cv::Point(2*goal_x, 2*goal_y),3,cv::Scalar(100,100,255), -1);
      cv::line(lane_topview_resized, cv::Point(2*init_1_x, 2*init_1_y), cv::Point(2*goal_x, 2*goal_y), cv::Scalar(100,100,255), 2);
      cv::line(lane_topview_resized, cv::Point(2*init_2_x, 2*init_2_y), cv::Point(2*goal_x, 2*goal_y), cv::Scalar(100,100,255), 2);

      cv::circle(state_monitor_resized, cv::Point(2*init_1_x, 2*init_1_y),5,cv::Scalar(100,100,255), -1);
      cv::circle(state_monitor_resized, cv::Point(2*init_2_x, 2*init_2_y),5,cv::Scalar(100,100,255), -1);
      cv::circle(state_monitor_resized, cv::Point(2*goal_x, 2*goal_y),3,cv::Scalar(100,100,255), -1);

      if(Z_DEBUG) std::cout<<"park_points"<<init_1_x<<", "<<init_1_y<<", "<<init_2_x<<", "<<init_2_y<<std::endl;

    }

    int offset_x = (int)((float)wheel_radius * sin(vControl.steer*M_PI/180.f));
    int offset_y = (int)((float)wheel_radius * cos(vControl.steer*M_PI/180.f));
    left_wheel_x1 = left_wheel_xc - offset_x;
    left_wheel_x2 = left_wheel_xc + offset_x;
    left_wheel_y1 = wheel_yc - offset_y;
    left_wheel_y2 = wheel_yc + offset_y;
    right_wheel_x1 = right_wheel_xc - offset_x;
    right_wheel_x2 = right_wheel_xc + offset_x;
    right_wheel_y1 = wheel_yc - offset_y;
    right_wheel_y2 = wheel_yc + offset_y;
    cv::line(state_monitor_resized,cv::Point(left_wheel_x1,left_wheel_y1), cv::Point(left_wheel_x2, left_wheel_y2), cv::Scalar(200,50,50),2);
    cv::line(state_monitor_resized,cv::Point(right_wheel_x1,right_wheel_y1), cv::Point(right_wheel_x2, right_wheel_y2), cv::Scalar(200,50,50),2);
    offset_x = (int)((float)wheel_radius * sin(vState.steer*M_PI/180.f));
    offset_y = (int)((float)wheel_radius * cos(vState.steer*M_PI/180.f));
    left_wheel_x1 = left_wheel_xc - offset_x;
    left_wheel_x2 = left_wheel_xc + offset_x;
    left_wheel_y1 = wheel_yc - offset_y;
    left_wheel_y2 = wheel_yc + offset_y;
    right_wheel_x1 = right_wheel_xc - offset_x;
    right_wheel_x2 = right_wheel_xc + offset_x;
    right_wheel_y1 = wheel_yc - offset_y;
    right_wheel_y2 = wheel_yc + offset_y;
    cv::line(state_monitor_resized,cv::Point(left_wheel_x1,left_wheel_y1), cv::Point(left_wheel_x2, left_wheel_y2), cv::Scalar(200,200,200),2);
    cv::line(state_monitor_resized,cv::Point(right_wheel_x1,right_wheel_y1), cv::Point(right_wheel_x2, right_wheel_y2), cv::Scalar(200,200,200),2);

    //TODO: save path image to other directory (in image files)
    ///add above three images to the monitor_img
    lane_topview_resized.copyTo(monitor_img(cv::Rect(0,0,lane_topview_resized.cols,lane_topview_resized.rows)));
    //if(Z_DEBUG) std::cout<<"lane topview copied!!"<<std::endl;
    state_monitor_resized.copyTo(monitor_img(cv::Rect(lane_topview_resized.cols,0,state_monitor_resized.cols,state_monitor_resized.rows)));
    //if(Z_DEBUG) std::cout<<"state monitor copied!!"<<std::endl;
    occupancy_map_resized.copyTo(monitor_img(cv::Rect(lane_topview_resized.cols+state_monitor_resized.cols,0,occupancy_map_resized.cols,occupancy_map_resized.rows)));
    //if(Z_DEBUG) std::cout<<"occupancy map copied!!"<<std::endl;
    //TODO: add text info to the image
    int state_count;
    nh.getParam("/state_count", state_count);
    std::stringstream stream_state;
    stream_state <<"vehicle state    count: " <<state_count;
    std::string state_text =  stream_state.str();

    cv::putText(monitor_img, state_text, cv::Point(20, map_height*2+5), cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(250,250,250));
    std::string basic_state = "";
    if(vState.is_auto) basic_state +="AUTO   ";
    else basic_state +="MANUAL ";

    if(vState.gear ==0) basic_state +="DRIVE   ";
    else if(vState.gear == 1) basic_state +="NEUTRAL ";
    else if(vState.gear == 2) basic_state +="REAR    ";

    if(vState.estop) basic_state+="!!ESTOP!!";
    if(!vState.estop) cv::putText(monitor_img, basic_state, cv::Point(20, map_height*2+23), cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(250,250,250));
    else cv::putText(monitor_img, basic_state, cv::Point(20, map_height*2+23), cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(255,100,100));

    std::stringstream stream;
    double v_kmph = 3.6*vState.speed;
    stream << "v(km/h): " << std::fixed << std::setprecision(2) << v_kmph << "  steer(deg): "<<vState.steer;
    std::stringstream stream_brake;
    stream_brake << "Brake: " << vState.brake;
    std::string v_state = stream.str();
    std::string b_state = stream_brake.str();
    cv::putText(monitor_img, v_state, cv::Point(20, map_height*2+41), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(250,250,250));
    cv::putText(monitor_img, b_state, cv::Point(20, map_height*2+69), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(250,250,250));

    std::string basic_control = "";
    if(vControl.is_auto) basic_control +="AUTO   ";
    else basic_control +="MANUAL ";

    if(vControl.gear ==0) basic_control +="DRIVE   ";
    else if(vControl.gear == 1) basic_control +="NEUTRAL ";
    else if(vControl.gear == 2) basic_control +="REAR    ";

    if(vControl.estop) basic_control+="!!ESTOP!!";
    if(!vControl.estop) cv::putText(monitor_img, basic_control, cv::Point(20+map_width*4, map_height*2+30), cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(255,255,150));
    else cv::putText(monitor_img, basic_control, cv::Point(20+map_width*4, map_height*2+30), cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(255,100,100));

    int control_count;
    nh.getParam("/control_count", control_count);
    std::stringstream stream_control;
    stream_control <<"control output    count: " <<control_count;
    std::string control_text =  stream_control.str();

    cv::putText(monitor_img, control_text, cv::Point(map_width*4+20, map_height*2+10), cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(250,250,250));
    std::stringstream control;
    control << "v(km/h): " << std::fixed << std::setprecision(2) << 3.6*vControl.speed << "  steer(deg): "<<vControl.steer;
    std::stringstream control_brake;
    control_brake << "Brake: " << vControl.brake;
    std::string v_control = control.str();
    std::string b_control = control_brake.str();
    cv::putText(monitor_img, v_control, cv::Point(map_width*4+20, map_height*2+50), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(255,255,150));
    cv::putText(monitor_img, b_control, cv::Point(map_width*4+20, map_height*2+70), cv::FONT_HERSHEY_PLAIN, 1.2, cv::Scalar(255,255,150));

    std::string control_mode_text = "";
    if(vControl.control_mode == 0) control_mode_text += "NO CONTROL APPLIED";//No Control
    else if(vControl.control_mode == 1) control_mode_text += "Autonomous Emergency Braking";//No Control
    else if(vControl.control_mode == 2) control_mode_text += "OBSTACLE AVOID (NORMAL_S)";//No Control
    else if(vControl.control_mode == 3) control_mode_text += "LANE TRACE (NORMAL_C)";//No Control
    else if(vControl.control_mode == 4) control_mode_text += "OBSTACLE AVOID: NO PATH(SLOW_C)";//No Control
    else if(vControl.control_mode == 5) control_mode_text += "MAKING U TURN";//No Control
    else if(vControl.control_mode == 6) control_mode_text += "GETTING IN THE PARKING LOT";//No Control
    else if(vControl.control_mode == 7) control_mode_text += "GETTING OUT OF THE PARKING LOT";//No Control

    cv::Scalar control_mode_text_color = cv::Scalar(220,220,220);
    if(vControl.control_mode == 1) control_mode_text_color = cv::Scalar(255,200,200);//No Control
    else if(vControl.control_mode == 2) control_mode_text_color = cv::Scalar(100, 255, 100);
    else if(vControl.control_mode == 3) control_mode_text_color = cv::Scalar(255, 255, 100);
    else if(vControl.control_mode == 4) control_mode_text_color = cv::Scalar(170, 255, 100);
    else if(vControl.control_mode == 5 || vControl.control_mode ==6) control_mode_text_color = cv::Scalar(150, 150, 255);
    std::stringstream add_stream;
    if(park_mode ==1) add_stream <<"Starts Parking Mission";
    else if(park_mode ==2) add_stream <<"Starts to Get in the Parking Lot";
    else if(vControl.control_mode == 5) add_stream <<"Uturn angle(deg): "<<std::fixed<< std::setprecision(2)<<uturn_angle;
    else add_stream << "Obstacle Flag: " <<vState.flag_obstacle;
    std::string add_text = add_stream.str();
    cv::putText(monitor_img, add_text, cv::Point(map_width*2+20, map_height*2+60), cv::FONT_HERSHEY_PLAIN, 1.0, control_mode_text_color);
    cv::putText(monitor_img, "Control Mode", cv::Point(map_width*2+20, map_height*2+20), cv::FONT_HERSHEY_PLAIN, 1.0, control_mode_text_color);
    cv::putText(monitor_img, control_mode_text, cv::Point(map_width*2+20, map_height*2+40), cv::FONT_HERSHEY_PLAIN, 1.0, control_mode_text_color);

    std::stringstream aeb_stream;
    aeb_stream<<"E stop Count: "<<e_stop;
    std:: string aeb_text = aeb_stream.str();
    cv::putText(monitor_img, aeb_text, cv::Point(20, map_height*2+85), cv::FONT_HERSHEY_PLAIN, 0.8, control_mode_text_color);
    ros::Time current_time = ros::Time::now();
    ros::Duration to_now(current_time - init_time);
    std::stringstream time_stream;
    time_stream << "execution time(sec): " << std::fixed << std::setprecision(2) << to_now.toSec();
    std::string time_text = time_stream.str();
    cv::putText(monitor_img, time_text, cv::Point(map_width*2+20, map_height*2+80), cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(100,100,255));

    //publishing monitor image
    cv::cvtColor(monitor_img, monitor_img, cv::COLOR_BGR2RGB);
    msgMonitorImg = cv_bridge::CvImage(std_msgs::Header(),"rgb8", monitor_img).toImageMsg();

    publishMonitorImg.publish(msgMonitorImg);

    //adding it to the video record
    if(IS_RECORD)    outputMonitorVideo << monitor_img;

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


#include <boost/test/unit_test.hpp>
#include "Operators.hpp"
#include "Error.hpp"
#include "Value.hpp"

using namespace slim;
BOOST_AUTO_TEST_SUITE(TestOperators)

double to_d(ObjectPtr val)
{
    auto n = dynamic_cast<const Number*>(val.get());
    if (n) return n->get_value();
    BOOST_FAIL("Not a number");
    return 0;
}

BOOST_AUTO_TEST_CASE(equals)
{
    auto a = make_value(55.0);
    auto a2 = make_value(55.0);
    auto b = make_value(60.0);
    auto c = make_value("55");
    auto d = make_value("test");
    auto d2 = make_value("test");
    //Boolean
    BOOST_CHECK(op_eq(TRUE_VALUE.get(), TRUE_VALUE.get())->is_true());
    BOOST_CHECK(!op_eq(TRUE_VALUE.get(), FALSE_VALUE.get())->is_true());
    BOOST_CHECK(!op_eq(FALSE_VALUE.get(), TRUE_VALUE.get())->is_true());

    BOOST_CHECK(!op_ne(TRUE_VALUE.get(), TRUE_VALUE.get())->is_true());
    BOOST_CHECK(op_ne(TRUE_VALUE.get(), FALSE_VALUE.get())->is_true());
    BOOST_CHECK(op_ne(FALSE_VALUE.get(), TRUE_VALUE.get())->is_true());

    //Null
    BOOST_CHECK(op_eq(NIL_VALUE.get(), NIL_VALUE.get())->is_true());
    BOOST_CHECK(!op_ne(NIL_VALUE.get(), NIL_VALUE.get())->is_true());

    //Null and Boolean
    BOOST_CHECK(!op_eq(TRUE_VALUE.get(), NIL_VALUE.get())->is_true());
    BOOST_CHECK(!op_eq(FALSE_VALUE.get(), NIL_VALUE.get())->is_true());

    //Number
    BOOST_CHECK(op_eq(a.get(), a2.get())->is_true());
    BOOST_CHECK(!op_eq(a.get(), b.get())->is_true());
    BOOST_CHECK(!op_ne(a.get(), a2.get())->is_true());
    BOOST_CHECK(op_ne(a.get(), b.get())->is_true());

    //String
    BOOST_CHECK(op_eq(d.get(), d2.get())->is_true());
    BOOST_CHECK(!op_eq(c.get(), d.get())->is_true());
    BOOST_CHECK(!op_ne(a.get(), a2.get())->is_true());
    BOOST_CHECK(op_ne(c.get(), d.get())->is_true());

    //Number and String
    BOOST_CHECK(!op_eq(a.get(), c.get())->is_true());
    BOOST_CHECK(op_ne(a.get(), c.get())->is_true());
}

BOOST_AUTO_TEST_CASE(rel_cmp)
{
    auto a = make_value(55.0);
    auto b = make_value(60.0);
    auto c = make_value("55");
    auto d = make_value("test");
    auto e = make_value("test55");
    auto f = make_value("56");

    //Number
    BOOST_CHECK_EQUAL(0, to_d(op_cmp(a.get(), a.get())));
    BOOST_CHECK_EQUAL(-1, to_d(op_cmp(a.get(), b.get())));
    BOOST_CHECK_EQUAL(1, to_d(op_cmp(b.get(), a.get())));

    BOOST_CHECK(!op_lt(a.get(), a.get())->is_true());
    BOOST_CHECK(op_le(a.get(), a.get())->is_true());
    BOOST_CHECK(!op_gt(a.get(), a.get())->is_true());
    BOOST_CHECK(op_ge(a.get(), a.get())->is_true());

    BOOST_CHECK(op_lt(a.get(), b.get())->is_true());
    BOOST_CHECK(op_le(a.get(), b.get())->is_true());
    BOOST_CHECK(!op_gt(a.get(), b.get())->is_true());
    BOOST_CHECK(!op_ge(a.get(), b.get())->is_true());

    BOOST_CHECK(!op_lt(b.get(), a.get())->is_true());
    BOOST_CHECK(!op_le(b.get(), a.get())->is_true());
    BOOST_CHECK(op_gt(b.get(), a.get())->is_true());
    BOOST_CHECK(op_ge(b.get(), a.get())->is_true());

    //Boolean
    BOOST_CHECK(op_le(TRUE_VALUE.get(), TRUE_VALUE.get())->is_true());
    BOOST_CHECK(op_le(FALSE_VALUE.get(), TRUE_VALUE.get())->is_true());

    BOOST_CHECK(op_lt(FALSE_VALUE.get(), TRUE_VALUE.get())->is_true());
    BOOST_CHECK(!op_lt(TRUE_VALUE.get(), FALSE_VALUE.get())->is_true());

    BOOST_CHECK(op_gt(TRUE_VALUE.get(), FALSE_VALUE.get())->is_true());
    BOOST_CHECK(!op_gt(FALSE_VALUE.get(), TRUE_VALUE.get())->is_true());

    //Null, no-order
    BOOST_CHECK_THROW(op_lt(NIL_VALUE.get(), NIL_VALUE.get()), UnorderableTypeError);

    //Number and Boolean, different types
    BOOST_CHECK_THROW(op_le(FALSE_VALUE.get(), a.get()), UnorderableTypeError);

    //String
    BOOST_CHECK(op_le(c.get(), d.get())->is_true());
    BOOST_CHECK(op_le(c.get(), f.get())->is_true());
    BOOST_CHECK(op_le(d.get(), e.get())->is_true());
}

BOOST_AUTO_TEST_CASE(binary)
{
    auto a = make_value(2.0);
    auto b = make_value(5.0);
    auto c = make_value("55");
    auto d = make_value("test");

    //mul
    BOOST_CHECK_EQUAL(10.0, to_d(a->mul(b.get())));
    BOOST_CHECK_THROW(a->mul(c.get()), TypeError);
    BOOST_CHECK_THROW(c->mul(d.get()), NoMethodError);

    //pow
    BOOST_CHECK_EQUAL(32.0, to_d(a->pow(b.get())));
    BOOST_CHECK_THROW(a->pow(c.get()), TypeError);
    BOOST_CHECK_THROW(c->pow(d.get()), NoMethodError);

    //div
    BOOST_CHECK_EQUAL(2.0 / 5.0, to_d(a->div(b.get())));
    BOOST_CHECK_EQUAL(5.0 / 2.0, to_d(b->div(a.get())));
    BOOST_CHECK_THROW(a->div(c.get()), TypeError);
    BOOST_CHECK_THROW(c->div(d.get()), NoMethodError);

    //mod
    BOOST_CHECK_EQUAL(2.0, to_d(a->mod(b.get())));
    BOOST_CHECK_EQUAL(1.0, to_d(b->mod(a.get())));
    BOOST_CHECK_THROW(a->mod(c.get()), TypeError);
    BOOST_CHECK_THROW(c->mod(d.get()), NoMethodError);

    //add
    BOOST_CHECK_EQUAL(7.0, to_d(a->add(b.get())));
    BOOST_CHECK_EQUAL(7.0, to_d(b->add(a.get())));
    BOOST_CHECK_THROW(a->add(c.get()), TypeError);
    BOOST_CHECK_EQUAL("55test", c->add(d.get())->to_string());
    BOOST_CHECK_EQUAL("test55", d->add(c.get())->to_string());

    //sub
    BOOST_CHECK_EQUAL(-3.0, to_d(a->sub(b.get())));
    BOOST_CHECK_EQUAL(3.0, to_d(b->sub(a.get())));
    BOOST_CHECK_THROW(a->sub(c.get()), TypeError);
    BOOST_CHECK_THROW(c->sub(d.get()), NoMethodError);

    //lshift
    BOOST_CHECK_EQUAL(2 << 5, to_d(a->bit_lshift(b.get())));
    BOOST_CHECK_THROW(a->bit_lshift(c.get()), TypeError);
    BOOST_CHECK_THROW(c->bit_lshift(d.get()), NoMethodError);

    //rshift
    BOOST_CHECK_EQUAL(2 >> 5, to_d(a->bit_rshift(b.get())));
    BOOST_CHECK_THROW(a->bit_rshift(c.get()), TypeError);
    BOOST_CHECK_THROW(c->bit_rshift(d.get()), NoMethodError);

    //and
    BOOST_CHECK_EQUAL(2 & 5, to_d(a->bit_and(b.get())));
    BOOST_CHECK_THROW(a->bit_and(c.get()), TypeError);
    BOOST_CHECK_THROW(c->bit_and(d.get()), NoMethodError);

    //or
    BOOST_CHECK_EQUAL(2 | 5, to_d(a->bit_or(b.get())));
    BOOST_CHECK_THROW(a->bit_or(c.get()), TypeError);
    BOOST_CHECK_THROW(c->bit_or(d.get()), NoMethodError);

    //xor
    BOOST_CHECK_EQUAL(2 ^ 5, to_d(a->bit_xor(b.get())));
    BOOST_CHECK_THROW(a->bit_xor(c.get()), TypeError);
    BOOST_CHECK_THROW(c->bit_xor(d.get()), NoMethodError);
}
BOOST_AUTO_TEST_CASE(unary)
{
    auto a = make_value(2.0);
    auto b = make_value(0.0);
    auto c = make_value("55");
    auto d = make_value("");

    //bit not
    BOOST_CHECK_EQUAL(double(~(int)2), to_d(a->bit_not()));
    BOOST_CHECK_THROW(c->bit_not(), NoMethodError);

    //not
    BOOST_CHECK(!op_not(a.get())->is_true());
    BOOST_CHECK(!op_not(b.get())->is_true());
    BOOST_CHECK(!op_not(c.get())->is_true());
    BOOST_CHECK(!op_not(d.get())->is_true());
    BOOST_CHECK(!op_not(TRUE_VALUE.get())->is_true());
    BOOST_CHECK(op_not(FALSE_VALUE.get())->is_true());

    //negative
    BOOST_CHECK_EQUAL(-2.0, to_d(a->negate()));
    BOOST_CHECK_THROW(c->negate(), NoMethodError);
    BOOST_CHECK_THROW(TRUE_VALUE->negate(), NoMethodError);
    BOOST_CHECK_THROW(NIL_VALUE->negate(), NoMethodError);
}
BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <numeric>
#include <algorithm>
#include <vector>
#include <map>
#include <chrono>
#include <memory>
#include <typeinfo>

#include <hpx/hpx_init.hpp>

#include <boost/serialization/access.hpp>

#include "VFParser.hpp"
#include "BitGraph.hpp"
#include "BitSet.hpp"

//#include "skeletons/Seq.hpp"
#include "skeletons/DepthBounded.hpp"
//#include "skeletons/StackStealing.hpp"

// Number of Words to use in our bitset representation
// Possible to specify at compile to to handler bigger graphs if required
#ifndef NWORDS
#define NWORDS 8
#endif

using Association = std::map<std::pair<unsigned, unsigned>, unsigned>;
using AssociatedEdges = std::vector<std::pair<unsigned, unsigned> >;

template<unsigned n_words_>
std::tuple<BitGraph<n_words_>, std::vector<int>, std::vector<int> >
buildGraph(const std::pair<Association, AssociatedEdges> & g) {
  std::vector<int> order(g.first.size());
  std::vector<int> invorder(g.first.size());
  std::iota(order.begin(), order.end(), 0);

  // Pre-calculate degrees
  std::vector<int> degrees;
  degrees.resize(g.first.size());
  for (auto & f : g.second) {
    ++degrees[f.first];
    ++degrees[f.second];
  }

  std::sort(order.begin(), order.end(),
            [&] (int a, int b) { return true ^ (degrees[a] < degrees[b] || (degrees[a] == degrees[b] && a > b)); });

  // Encode BitGraph
  BitGraph<n_words_> graph;
  graph.resize(g.first.size());

  for (unsigned i = 0 ; i < order.size() ; ++i)
    invorder[order[i]] = i;

  for (auto & f : g.second)
    graph.add_edge(invorder[f.first], invorder[f.second]);

  return std::make_tuple(graph, order, invorder);
}

auto modular_product(const VFGraph & g1, const VFGraph & g2) -> std::pair<Association, AssociatedEdges>
{
  unsigned next_vertex = 0;
  Association association;
  AssociatedEdges edges;

  for (unsigned l = 0 ; l < std::min(g1.vertices_by_label.size(), g2.vertices_by_label.size()) ; ++l)
      for (unsigned v1 : g1.vertices_by_label[l])
          for (unsigned v2 : g2.vertices_by_label[l])
              for (unsigned m = 0 ; m < std::min(g1.vertices_by_label.size(), g2.vertices_by_label.size()) ; ++m)
                  for (unsigned w1 : g1.vertices_by_label[m])
                      for (unsigned w2 : g2.vertices_by_label[m])
                          if (v1 != w1 && v2 != w2
                                  && (g1.edges[v1][v1] == g2.edges[v2][v2])
                                  && (g1.edges[w1][w1] == g2.edges[w2][w2])
                                  && (g1.edges[v1][w1] == g2.edges[v2][w2])
                                  && (g1.edges[w1][v1] == g2.edges[w2][v2])) {

                              if (! association.count({v1, v2}))
                                  association.insert({{v1, v2}, next_vertex++});
                              if (! association.count({w1, w2}))
                                  association.insert({{w1, w2}, next_vertex++});

                              edges.push_back({association.find({v1, v2})->second, association.find({w1, w2})->second});
                          }

  return { association, edges };
}

auto unproduct(const Association & association, unsigned v) -> std::pair<unsigned, unsigned>
{
  for (auto & a : association)
    if (a.second == v)
      return a.first;

  throw "oops";
}

template<unsigned n_words_>
auto colour_class_order(const BitGraph<n_words_> & graph,
                        const BitSet<n_words_> & p,
                        std::array<unsigned, n_words_ * bits_per_word> & p_order,
                        std::array<unsigned, n_words_ * bits_per_word> & p_bounds) -> void {
  BitSet<n_words_> p_left = p; // not coloured yet
  unsigned colour = 0;         // current colour
  unsigned i = 0;              // position in p_bounds

  // while we've things left to colour
  while (! p_left.empty()) {
    // next colour
    ++colour;
    // things that can still be given this colour
    BitSet<n_words_> q = p_left;

    // while we can still give something this colour
    while (! q.empty()) {
      // first thing we can colour
      int v = q.first_set_bit();
      p_left.unset(v);
      q.unset(v);

      // can't give anything adjacent to this the same colour
      graph.intersect_with_row_complement(v, q);

      // record in result
      p_bounds[i] = colour;
      p_order[i] = v;
      ++i;
    }
  }
}

// Main Maxclique B&B Functions
// Probably needs a copy constructor
struct MCSol {
  std::vector<int> members;
  int colours;

  template <class Archive>
  void serialize(Archive & ar, const unsigned int version) {
    ar & members;
    ar & colours;
  }
};

struct MCNode {
  friend class boost::serialization::access;

  MCSol sol;
  int size;
  BitSet<NWORDS> remaining;

  int getObj() const {
    return size;
  }

  template <class Archive>
  void serialize(Archive & ar, const unsigned int version) {
    ar & sol;
    ar & size;
    ar & remaining;
  }

};

struct GenNode : YewPar::NodeGenerator<MCNode, BitGraph<NWORDS> > {
  std::array<unsigned, NWORDS * bits_per_word> p_order;
  std::array<unsigned, NWORDS * bits_per_word> colourClass;

  std::reference_wrapper<const BitGraph<NWORDS> > graph;

  MCSol childSol;
  int childBnd;
  BitSet<NWORDS> p;

  int v;

  GenNode(const BitGraph<NWORDS> & graph, const MCNode & n) : graph(std::cref(graph)) {
    colour_class_order(graph, n.remaining, p_order, colourClass);
    childSol = n.sol;
    childBnd = n.size + 1;
    p = n.remaining;
    numChildren = p.popcount();
    v = numChildren - 1;
  }

  // Get the next value
  MCNode next() override {
    auto sol = childSol;
    sol.members.push_back(p_order[v]);
    sol.colours = colourClass[v] - 1;

    auto cands = p;
    graph.get().intersect_with_row(p_order[v], cands);

    // Side effectful function update
    p.unset(p_order[v]);
    v--;

    return {sol, childBnd, cands};
  }
};

int upperBound(const BitGraph<NWORDS> & space, const MCNode & n) {
  return n.size + n.sol.colours;
}

typedef func<decltype(&upperBound), &upperBound> upperBound_func;

// using ss_skel = YewPar::Skeletons::StackStealing<GenNode,
//                                                  YewPar::Skeletons::API::BnB,
//                                                  YewPar::Skeletons::API::BoundFunction<upperBound_func>,
//                                                  YewPar::Skeletons::API::PruneLevel>;
// using cfunc  = ss_skel::SubTreeTask;
// REGISTER_SEARCHMANAGER(MCNode, cfunc);

int hpx_main(boost::program_options::variables_map & opts) {
  auto patternF = opts["pattern-file"].as<std::string>();
  auto targetF  = opts["target-file"].as<std::string>();

  auto patternG = read_vf(patternF, opts.count("unlabelled"), opts.count("no-edge-labels"), opts.count("undirected"));
  auto targetG = read_vf(targetF, opts.count("unlabelled"), opts.count("no-edge-labels"), opts.count("undirected"));

  auto prod = modular_product(patternG, targetG);

  BitGraph<NWORDS> graph; std::vector<int> order, invorder;
  std::tie(graph, order, invorder) = buildGraph<NWORDS>(prod);

  if (graph.size() > bits_per_word*NWORDS) {
    std::cout << "Binary Cannot Handle Graph of this size. Recompile with a bigger NWORDS" << std::endl;
    hpx::finalize();
    return EXIT_FAILURE;
  }

  auto spawnDepth = opts["spawn-depth"].as<std::uint64_t>();
  auto start_time = std::chrono::steady_clock::now();

  // // Initialise Root Node
  MCSol mcsol;
  mcsol.members.reserve(graph.size());
  mcsol.colours = 0;

  BitSet<NWORDS> cands;
  cands.resize(graph.size());
  cands.set_all();
  MCNode root = {mcsol, 0, cands};

  YewPar::Skeletons::API::Params<int> searchParameters;
  searchParameters.spawnDepth = spawnDepth;
  auto result = YewPar::Skeletons::DepthBounded<GenNode,
                                               YewPar::Skeletons::API::Optimisation,
                                               YewPar::Skeletons::API::BoundFunction<upperBound_func>,
                                               YewPar::Skeletons::API::PruneLevel>
        ::search(graph, root, searchParameters);

  auto overall_time = std::chrono::duration_cast<std::chrono::milliseconds>
    (std::chrono::steady_clock::now() - start_time);

  std::map<int, int> isomorphism;
  auto sol = result.sol;
  for (auto const & v : sol.members) {
    isomorphism.insert(unproduct(prod.first, order[v]));
  }

  // Print Results
  std::cout << std::boolalpha << ! isomorphism.empty() << " " << 0;
  std::cout << " " << isomorphism.size() << std::endl;

  for (auto v : isomorphism)
      std::cout << "(" << v.first << " -> " << v.second << ") ";
  std::cout << std::endl;

  std::cout << overall_time.count() << std::endl;

  auto graphs = std::make_pair(patternG, targetG);
  for (auto & p : isomorphism) {
      if (graphs.first.vertex_labels.at(p.first) != graphs.second.vertex_labels.at(p.second)) {
          std::cerr << "Oops! not an isomorphism due to vertex labels" << std::endl;
          hpx::finalize();
          return EXIT_FAILURE;
      }
  }

  for (auto & p : isomorphism) {
      for (auto & q : isomorphism) {
          if (graphs.first.edges.at(p.first).at(q.first) != graphs.second.edges.at(p.second).at(q.second)) {
              std::cerr << "Oops! not an isomorphism due to edge " << std::to_string(p.first) << "--"
                  << std::to_string(q.first) << " (" << graphs.first.edges[p.first][q.first] << ") vs "
                  << std::to_string(p.second) << "--" << std::to_string(q.second) << " ("
                  << graphs.second.edges[p.second][q.second] << ")" << std::endl;
              hpx::finalize();
              return EXIT_FAILURE;
          }
      }
  }

  return hpx::finalize();
}

int main (int argc, char* argv[]) {
  boost::program_options::options_description
    desc_commandline("Usage: " HPX_APPLICATION_STRING " [options]");

  desc_commandline.add_options()
    ( "spawn-depth,d",
      boost::program_options::value<std::uint64_t>()->default_value(0),
      "Depth in the tree to spawn at"
      )
    ( "pattern-file",
      boost::program_options::value<std::string>(),
      "VF formatted input graph"
      )
    ( "target-file",
      boost::program_options::value<std::string>(),
      "VF formatted input graph"
    )
    ("unlabelled", "Make the graph unlabelled")
    ("no-edge-labels", "Get rid of edge labels, but keep vertex labels")
    ("undirected", "Make the graph undirected");

  return hpx::init(desc_commandline, argc, argv);
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_BITWISE_FUNCTIONS_SCALAR_REVERSEBITS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_BITWISE_FUNCTIONS_SCALAR_REVERSEBITS_HPP_INCLUDED

#include <boost/simd/toolbox/bitwise/functions/reversebits.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::reversebits_, tag::cpu_
                            , (A0)
                            , (scalar_< ints8_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return uint8_t(
        ((uint8_t(a0) * 0x0802LU & 0x22110LU) | (uint8_t(a0) * 0x8020LU & 0x88440LU)) * 0x10101LU >> 16
      );
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::reversebits_, tag::cpu_
                            , (A0)
                            , (scalar_< ints64_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef union { A0 a; boost::simd::uint32_t b[2]; } trick;
      trick z = {a0};
      z.b[0] = reversebits(z.b[0]);
      z.b[1] = reversebits(z.b[1]);
      std::swap(z.b[0], z.b[1]);
      return z.a;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::reversebits_, tag::cpu_
                            , (A0)
                            , (scalar_< ints16_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef union { A0 a; boost::simd::uint8_t b[2]; } trick;
      trick z = {a0};
      z.b[0] = reversebits(z.b[0]);
      z.b[1] = reversebits(z.b[1]);
      std::swap(z.b[0], z.b[1]);
      return  z.a;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::reversebits_, tag::cpu_
                            , (A0)
                            , (scalar_< ints32_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef union { A0 a; boost::simd::uint16_t b[2]; } trick;
      trick z = {a0};
      z.b[0] = reversebits(z.b[0]);
      z.b[1] = reversebits(z.b[1]);
      std::swap(z.b[0], z.b[1]);
      return z.a;
    }
  };
} } }

#endif


#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <random>
#include <Eigen/Cholesky>
#include <fstream>
#include <iomanip>
#include <srrg_system_utils/parse_command_line.h>

#include "srrg_solver/solver_core/instances.h"
#include "srrg_solver/solver_core/factor_graph.h"
#include "srrg_solver/variables_and_factors/types_3d/variable_se3_ad.h"
#include "srrg_solver/variables_and_factors/types_3d/variable_point3_ad.h"
#include "srrg_solver/variables_and_factors/types_3d/se3_pose_pose_geodesic_error_factor.h"
#include "srrg_solver/variables_and_factors/types_3d/se3_pose_point_offset_error_factor.h"

#include "srrg_solver/variables_and_factors/types_2d/variable_se2_ad.h"
#include "srrg_solver/variables_and_factors/types_2d/variable_point2_ad.h"
#include "srrg_solver/variables_and_factors/types_2d/se2_pose_pose_geodesic_error_factor.h"
#include "srrg_solver/variables_and_factors/types_2d/se2_pose_point_error_factor.h"

#include "srrg_solver/variables_and_factors/types_projective/variable_sim3_ad.h"
#include "srrg_solver/variables_and_factors/types_projective/sim3_pose_pose_error_factor_ad.h"
#include "srrg_solver/variables_and_factors/types_projective/se3_pose_point_omni_ba_error_factor.h"

#include "srrg_solver/variables_and_factors/types_3d/instances.h"
#include "srrg_solver/variables_and_factors/types_2d/instances.h"
#include "srrg_solver/variables_and_factors/types_projective/instances.h"
#include "srrg_solver/utils/instances.h"
#include "srrg_solver/solver_incremental/factor_graph_incremental_sorter.h"

#include <Eigen/Cholesky>

using namespace srrg2_core;
using namespace srrg2_solver;
using namespace std;

const std::string exe_name = "solver_app_graph_sorter";
#define LOG std::cerr << exe_name << "|"

const char* banner[] = {
  "fuck yourself",
  0
};

// ia register types
void initTypes() {
  variables_and_factors_2d_registerTypes();
  variables_and_factors_3d_registerTypes();
  variables_and_factors_projective_registerTypes();
  solver_utils_registerTypes();
  solver_registerTypes();
}

// ia THE PROGRAM
int main(int argc, char** argv) {
  initTypes();

  ParseCommandLine cmd_line(argv, banner);
  ArgumentString input_file          (&cmd_line, "i",    "input-file",             "file where to read the input ", "");
  cmd_line.parse();

  if (! input_file.isSet()) {
    cerr << "no input file provided, aborting" << endl;
    return 0;
  }
  std::cerr << "loding file: [" << input_file.value() << "]... " << endl;

  FactorGraphPtr graph(new FactorGraph);
  FactorGraphIncrementalReader reader;
  reader.setGraph(graph);
  reader.setFilePath(input_file.value());
  reader.relaxed().insert(0);
  std::set<VariableBase::Id> new_vars;
  std::set<FactorBase::Id> new_factors;
  while (reader.readEpoch(new_vars, new_factors)) {
    if (new_vars.size()) {
      cerr << "epoch: " << *new_vars.begin() << " vars: " << new_vars.size() << " factors:" << new_factors.size() << endl;
    }
  }
  return 0;
}


// SPDX-License-Identifier: MIT
// SPDX-FileCopyrightText: Copyright 2019-2021 Heal Research

#ifndef OPERON_EVAL_DETAIL
#define OPERON_EVAL_DETAIL

#include "core/node.hpp"
#include "core/types.hpp"
#include "interpreter/functions.hpp"

#include "robin_hood.h"
#include <Eigen/Dense>
#include <fmt/core.h>

#include <tuple>

namespace Operon {

namespace detail {
    // this should be good enough - tests show 512 is about optimal
    template<typename T>
    struct batch_size {
        static const size_t value = 512 / sizeof(T);
    };

    template<typename T>
    using eigen_t = typename Eigen::Array<T, batch_size<T>::value, Eigen::Dynamic, Eigen::ColMajor>;

    template<typename T>
    using eigen_ref = Eigen::Ref<eigen_t<T>, Eigen::Unaligned, Eigen::Stride<batch_size<T>::value, 1>>;

    // dispatching mechanism
    // compared to the simple/naive way of evaluating n-ary symbols, this method has the following advantages:
    // 1) improved performance: the naive method accumulates into the result for each argument, leading to unnecessary assignments
    // 2) minimizing the number of intermediate steps which might improve floating point accuracy of some operations
    //    if arity > 4, one accumulation is performed every 4 args
    template<NodeType Type, typename T>
    inline void dispatch_op_nary(eigen_t<T>& m, Operon::Vector<Node> const& nodes, size_t parentIndex, size_t /* row number - not used */)
    {
        static_assert(Type < NodeType::Aq);
        auto result = m.col(parentIndex);
        const auto f = [](bool cont, decltype(result) res, auto&&... args) {
            if (cont) {
                ContinuedFunction<Type>{}(res, std::forward<decltype(args)>(args)...);
            } else {
                Function<Type>{}(res, std::forward<decltype(args)>(args)...);
            }
        };
        const auto nextArg = [&](size_t i) { return i - (nodes[i].Length + 1); };

        auto arg1 = parentIndex - 1;

        bool continued = false;

        int arity = nodes[parentIndex].Arity;
        while (arity > 0) {
            switch (arity) {
            case 1: {
                f(continued, result, m.col(arg1));
                arity = 0;
                break;
            }
            case 2: {
                auto arg2 = nextArg(arg1);
                f(continued, result, m.col(arg1), m.col(arg2));
                arity = 0;
                break;
            }
            case 3: {
                auto arg2 = nextArg(arg1), arg3 = nextArg(arg2);
                f(continued, result, m.col(arg1), m.col(arg2), m.col(arg3));
                arity = 0;
                break;
            }
            default: {
                auto arg2 = nextArg(arg1), arg3 = nextArg(arg2), arg4 = nextArg(arg3);
                f(continued, result, m.col(arg1), m.col(arg2), m.col(arg3), m.col(arg4));
                arity -= 4;
                arg1 = nextArg(arg4);
                break;
            }
            }
            continued = true;
        }
    }

    template<NodeType Type, typename T>
    inline void dispatch_op_unary(eigen_t<T>& m, Operon::Vector<Node> const&, size_t i, size_t /* row number - not used */)
    {
        static_assert(Type < NodeType::Constant && Type > NodeType::Pow);
        Function<Type>{}(m.col(i), m.col(i - 1));
    }

    template<NodeType Type, typename T>
    inline void dispatch_op_binary(eigen_t<T>& m, Operon::Vector<Node> const& nodes, size_t i, size_t /* row number - not used */)
    {
        static_assert(Type < NodeType::Log && Type > NodeType::Div);
        auto j = i - 1;
        auto k = j - nodes[j].Length - 1;
        Function<Type>{}(m.col(i), m.col(j), m.col(k));
    }

    template<NodeType Type, typename T>
    inline void dispatch_op_simple_unary_or_binary(eigen_t<T>& m, Operon::Vector<Node> const& nodes, size_t parentIndex, size_t /* row number - not used */)
    {
        auto r = m.col(parentIndex);
        size_t i = parentIndex - 1;
        size_t arity = nodes[parentIndex].Arity;

        Function<Type> f{};

        if (arity == 1) {
            f(r, m.col(i));
        } else {
            auto j = i - (nodes[i].Length + 1);
            f(r, m.col(i), m.col(j));
        }
    }

    template<NodeType Type, typename T>
    inline void dispatch_op_simple_nary(eigen_t<T>& m, Operon::Vector<Node> const& nodes, size_t parentIndex, size_t /* row number - not used */)
    {
        auto r = m.col(parentIndex);
        size_t arity = nodes[parentIndex].Arity;

        auto i = parentIndex - 1;

        Function<Type> f{};

        if (arity == 1) {
            f(r, m.col(i));
        } else {
            r = m.col(i);

            for (size_t k = 1; k < arity; ++k) {
                i -= nodes[i].Length + 1;
                f(r, m.col(i));
            }
        }
    }

    struct noop {
        template<typename... Args>
        void operator()(Args&&...) {}
    };

    template<typename X, typename Tuple>
    class tuple_index;

    template<typename X, typename... T>
    class tuple_index<X, std::tuple<T...>> {
        template<std::size_t... idx>
        static constexpr ssize_t find_idx(std::index_sequence<idx...>)
        {
            return -1 + ((std::is_same<X, T>::value ? idx + 1 : 0) + ...);
        }

    public:
        static constexpr ssize_t value = find_idx(std::index_sequence_for<T...>{});
    };

    template<typename T>
    using Callable = typename std::function<void(detail::eigen_t<T>&, Operon::Vector<Node> const&, size_t, size_t)>;

    template<NodeType Type, typename T>
    static constexpr Callable<T> MakeCall()
    {
        if constexpr (Type < NodeType::Aq) { // nary: add, sub, mul, div
            return Callable<T>(detail::dispatch_op_nary<Type, T>);
        } else if constexpr (Type < NodeType::Log) { // binary: aq, pow
            return Callable<T>(detail::dispatch_op_binary<Type, T>);
        } else if constexpr (Type < NodeType::Constant) { // unary: exp, log, sin, cos, tan, tanh, sqrt, cbrt, square, dynamic
            return Callable<T>(detail::dispatch_op_unary<Type, T>);
        }
    }

    template<NodeType Type, typename... Ts, std::enable_if_t<sizeof...(Ts) != 0, bool> = true>
    static constexpr auto MakeTuple()
    {
        return std::tuple(MakeCall<Type, Ts>()...);
    };

    template<typename F, typename... Ts, std::enable_if_t<sizeof...(Ts) != 0 && (std::is_invocable_r_v<void, F, detail::eigen_t<Ts>&, Vector<Node> const&, size_t, size_t> && ...), bool> = true>
    static constexpr auto MakeTuple(F&& f)
    {
        return std::tuple(Callable<Ts>(std::forward<F&&>(f))...);
    }

    template<typename F, typename... Ts, std::enable_if_t<sizeof...(Ts) != 0 && (std::is_invocable_r_v<void, F, detail::eigen_t<Ts>&, Vector<Node> const&, size_t, size_t> && ...), bool> = true>
    static constexpr auto MakeTuple(F const& f)
    {
        return std::tuple(Callable<Ts>(f)...);
    }

    template<NodeType Type>
    static constexpr auto MakeDefaultTuple()
    {
        return MakeTuple<Type, Operon::Scalar, Operon::Dual>();
    }

    template<typename F, std::enable_if_t<
        std::is_invocable_r_v<
            void, F, detail::eigen_t<Operon::Scalar>&, Operon::Vector<Node> const&, size_t, size_t
        > &&
        std::is_invocable_r_v<
            void, F, detail::eigen_t<Operon::Dual>&, Operon::Vector<Node> const&, size_t, size_t
        >, bool> = true>
    static constexpr auto MakeDefaultTuple(F&& f)
    {
        return MakeTuple<F, Operon::Scalar, Operon::Dual>(std::forward<F&&>(f));
    }

} // namespace detail

struct DispatchTable {
    template<typename T>
    using Callable = detail::Callable<T>;

    using Tuple    = std::tuple<Callable<Operon::Scalar>, Callable<Operon::Dual>>;
    using Map      = robin_hood::unordered_flat_map<Operon::Hash, Tuple>;
    using Pair     = robin_hood::pair<Operon::Hash, Tuple>;

    DispatchTable()
    {
        InitializeMap();
    }

    DispatchTable(DispatchTable const& other) : map(other.map) { }
    DispatchTable(DispatchTable &&other) : map(std::move(other.map)) { }

    void InitializeMap()
    {
        const auto hash = [](auto t) { return Node(t).HashValue; };

        map = Map{
            { hash(NodeType::Add), detail::MakeDefaultTuple<NodeType::Add>() },
            { hash(NodeType::Sub), detail::MakeDefaultTuple<NodeType::Sub>() },
            { hash(NodeType::Mul), detail::MakeDefaultTuple<NodeType::Mul>() },
            { hash(NodeType::Sub), detail::MakeDefaultTuple<NodeType::Sub>() },
            { hash(NodeType::Div), detail::MakeDefaultTuple<NodeType::Div>() },
            { hash(NodeType::Aq),  detail::MakeDefaultTuple<NodeType::Aq>() },
            { hash(NodeType::Pow), detail::MakeDefaultTuple<NodeType::Pow>() },
            { hash(NodeType::Log), detail::MakeDefaultTuple<NodeType::Log>() },
            { hash(NodeType::Exp), detail::MakeDefaultTuple<NodeType::Exp>() },
            { hash(NodeType::Sin), detail::MakeDefaultTuple<NodeType::Sin>() },
            { hash(NodeType::Cos), detail::MakeDefaultTuple<NodeType::Cos>() },
            { hash(NodeType::Tan), detail::MakeDefaultTuple<NodeType::Tan>() },
            { hash(NodeType::Tanh), detail::MakeDefaultTuple<NodeType::Tanh>() },
            { hash(NodeType::Sqrt), detail::MakeDefaultTuple<NodeType::Sqrt>() },
            { hash(NodeType::Cbrt), detail::MakeDefaultTuple<NodeType::Cbrt>() },
            { hash(NodeType::Square), detail::MakeDefaultTuple<NodeType::Square>() },
            /* constants and variables not needed here */
        };
    };

    template<typename T>
    inline Callable<T>& Get(Operon::Hash const h)
    {
        constexpr ssize_t idx = detail::tuple_index<Callable<T>, Tuple>::value;
        static_assert(idx >= 0, "Tuple does not contain type T");
        if (auto it = map.find(h); it != map.end()) {
            return std::get<static_cast<size_t>(idx)>(it->second);
        }
        throw std::runtime_error(fmt::format("Hash value {} is not in the map\n", h));
    }

    template<typename T>
    inline Callable<T> const& Get(Operon::Hash const h) const
    {
        constexpr ssize_t idx = detail::tuple_index<Callable<T>, Tuple>::value;
        static_assert(idx >= 0, "Tuple does not contain type T");
        if (auto it = map.find(h); it != map.end()) {
            return std::get<static_cast<size_t>(idx)>(it->second);
        }
        throw std::runtime_error(fmt::format("Hash value {} is not in the map\n", h));
    }

    template<typename F, std::enable_if_t<std::is_invocable_r_v<void, F, detail::eigen_t<Operon::Dual>&, Vector<Node> const&, size_t, size_t>, bool> = true>
    void RegisterCallable(Operon::Hash hash, F const& f) {
        map[hash] = detail::MakeTuple<F, Operon::Scalar, Operon::Dual>(f);
    }

private:
    Map map;
};

} // namespace Operon

#endif


// ======================================================================
/*!
 * \file
 * \brief Implementation of namespace TextGen::CalculatorFactory
 */
// ======================================================================
/*!
 * \namespace TextGen::CalculatorFactory
 *
 * \brief Creating TextGen::Calculator objects
 *
 */
// ======================================================================

#include "CalculatorFactory.h"
#include "ChangeCalculator.h"
#include "CountCalculator.h"
#include "MaximumCalculator.h"
#include "MeanCalculator.h"
#include "MedianCalculator.h"
#include "MinimumCalculator.h"
#include "ModChangeCalculator.h"
#include "ModMeanCalculator.h"
#include "ModStandardDeviationCalculator.h"
#include "ModTrendCalculator.h"
#include "NullCalculator.h"
#include "PercentageCalculator.h"
#include "RangeAcceptor.h"
#include "StandardDeviationCalculator.h"
#include "SumCalculator.h"
#include "TextGenError.h"
#include "TrendCalculator.h"
#include <boost/move/make_unique.hpp>
#include <macgyver/StringConversion.h>
#include <string>

namespace TextGen
{
namespace CalculatorFactory
{
// ----------------------------------------------------------------------
/*!
 * \brief Create an calculator suitable for the given WeatherFunction
 *
 * Throws if there is no suitable data modifier.
 *
 * \param theFunction The weather function
 * \return The data modifier
 */
// ----------------------------------------------------------------------

Calculator* create(WeatherFunction theFunction)
{
  switch (theFunction)
  {
    case Mean:
      return new MeanCalculator;
    case Maximum:
      return new MaximumCalculator;
    case Minimum:
      return new MinimumCalculator;
    case Median:
      return new MedianCalculator;
    case Sum:
      return new SumCalculator;
    case Percentage:
      return new PercentageCalculator;
    case Count:
      return new CountCalculator;
    case Trend:
      return new TrendCalculator;
    case Change:
      return new ChangeCalculator;
    case NullFunction:
      return new NullCalculator;
    case StandardDeviation:
      return new StandardDeviationCalculator;
  }

  throw TextGenError("CalculatorFactory failed to recognize the given function" +
                     Fmi::to_string(static_cast<int>(theFunction)));
}

// ----------------------------------------------------------------------
/*!
 * \brief Create a modular calculator suitable for the given WeatherFunction
 *
 * Throws if there is no suitable data modifier.
 *
 * \param theFunction The weather function
 * \param theModulo The modulo value
 * \return The data modifier
 */
// ----------------------------------------------------------------------

Calculator* create(WeatherFunction theFunction, int theModulo)
{
  switch (theFunction)
  {
    case Mean:
      return new ModMeanCalculator(theModulo);
    case StandardDeviation:
      return new ModStandardDeviationCalculator(theModulo);
    case Change:
      return new ModChangeCalculator(theModulo);
    case Trend:
      return new ModTrendCalculator(theModulo);

    case Percentage:
      return new PercentageCalculator;
    case Count:
      return new CountCalculator;
    case NullFunction:
      return new NullCalculator;

    case Median:
      throw TextGenError("CalculatorFactory cannot create modular Median analyzer");
    case Maximum:
      throw TextGenError("CalculatorFactory cannot create modular Maximum analyzer");
    case Minimum:
      throw TextGenError("CalculatorFactory cannot create modular Minimum analyzer");
    case Sum:
      throw TextGenError("CalculatorFactory cannot create modular Sum analyzer");
  }

  throw TextGenError("CalculatorFactory failed to recognize the given function" +
                     Fmi::to_string(static_cast<int>(theFunction)));
}

// ----------------------------------------------------------------------
/*!
 * \brief Create an calculator suitable for the given WeatherFunction
 *
 * Throws if there is no suitable data modifier.
 *
 * \param theFunction The weather function
 * \param theTester The tester for Percentage calculations
 * \return The data modifier
 */
// ----------------------------------------------------------------------

Calculator* create(WeatherFunction theFunction, const Acceptor& theTester)
{
  switch (theFunction)
  {
    case Mean:
    case Maximum:
    case Minimum:
    case Median:
    case StandardDeviation:
    case Sum:
    case Trend:
    case Change:
    case NullFunction:
      return create(theFunction);
    case Percentage:
    {
      auto tmp = boost::movelib::make_unique<PercentageCalculator>();
      tmp->condition(theTester);
      return tmp.release();
    }
    case Count:
    {
      auto tmp = boost::movelib::make_unique<CountCalculator>();
      tmp->condition(theTester);
      return tmp.release();
    }
  }

  throw TextGenError("CalculatorFactory failed to recognize the given function" +
                     Fmi::to_string(static_cast<int>(theFunction)));
}

// ----------------------------------------------------------------------
/*!
 * \brief Create a modular calculator suitable for the given WeatherFunction
 *
 * Throws if there is no suitable data modifier.
 *
 * \param theFunction The weather function
 * \param theTester The tester for Percentage calculations
 * \param theModulo The modulo value
 * \return The data modifier
 */
// ----------------------------------------------------------------------

Calculator* create(WeatherFunction theFunction, const Acceptor& theTester, int theModulo)
{
  switch (theFunction)
  {
    case Mean:
    case Maximum:
    case Minimum:
    case Median:
    case StandardDeviation:
    case Sum:
    case Trend:
    case Change:
    case NullFunction:
      return create(theFunction, theModulo);
    case Percentage:
    {
      auto tmp = boost::movelib::make_unique<PercentageCalculator>();
      tmp->condition(theTester);
      return tmp.release();
    }
    case Count:
    {
      auto tmp = boost::movelib::make_unique<CountCalculator>();
      tmp->condition(theTester);
      return tmp.release();
    }
  }

  throw TextGenError("CalculatorFactory failed to recognize the given function" +
                     Fmi::to_string(static_cast<int>(theFunction)));
}

}  // namespace CalculatorFactory
}  // namespace TextGen

// ======================================================================


#include "image_geometry/pinhole_camera_model.h"
#include <sensor_msgs/distortion_models.h>
#include <opencv2/calib3d/calib3d.hpp>
#ifdef BOOST_SHARED_PTR_HPP_INCLUDED
#include <boost/make_shared.hpp>
#endif

namespace image_geometry {

enum DistortionState { NONE, CALIBRATED, UNKNOWN };
enum DistortionModel { EQUIDISTANT, PLUMB_BOB_OR_RATIONAL_POLYNOMIAL, UNKNOWN_MODEL };

struct PinholeCameraModel::Cache
{
  DistortionState distortion_state;
  DistortionModel distortion_model;

  cv::Mat_<double> K_binned, P_binned; // Binning applied, but not cropping

  mutable bool full_maps_dirty;
  mutable cv::Mat full_map1, full_map2;

  mutable bool reduced_maps_dirty;
  mutable cv::Mat reduced_map1, reduced_map2;

  mutable bool unrectify_full_maps_dirty;
  mutable cv::Mat unrectify_full_map1, unrectify_full_map2;

  mutable bool unrectify_reduced_maps_dirty;
  mutable cv::Mat unrectify_reduced_map1, unrectify_reduced_map2;

  mutable bool rectified_roi_dirty;
  mutable cv::Rect rectified_roi;

  Cache()
    : distortion_state(UNKNOWN),
      distortion_model(UNKNOWN_MODEL),
      full_maps_dirty(true),
      reduced_maps_dirty(true),
      unrectify_full_maps_dirty(true),
      unrectify_reduced_maps_dirty(true),
      rectified_roi_dirty(true)
  {
  }
};

PinholeCameraModel::PinholeCameraModel()
{
}

PinholeCameraModel& PinholeCameraModel::operator=(const PinholeCameraModel& other)
{
  if (other.initialized())
    this->fromCameraInfo(other.cameraInfo());
  return *this;
}

PinholeCameraModel::PinholeCameraModel(const PinholeCameraModel& other)
{
  if (other.initialized())
    fromCameraInfo(other.cam_info_);
}

// For uint32_t, string, bool...
template<typename T>
bool update(const T& new_val, T& my_val)
{
  if (my_val == new_val)
    return false;
  my_val = new_val;
  return true;
}

// For std::vector
template<typename MatT>
bool updateMat(const MatT& new_mat, MatT& my_mat, cv::Mat_<double>& cv_mat, int rows, int cols)
{
  if ((my_mat == new_mat) && (my_mat.size() == cv_mat.rows*cv_mat.cols))
    return false;
  my_mat = new_mat;
  // D may be empty if camera is uncalibrated or distortion model is non-standard
  cv_mat = (my_mat.size() == 0) ? cv::Mat_<double>() : cv::Mat_<double>(rows, cols, &my_mat[0]);
  return true;
}

template<typename MatT, typename MatU>
bool updateMat(const MatT& new_mat, MatT& my_mat, MatU& cv_mat)
{
  if ((my_mat == new_mat) && (my_mat.size() == cv_mat.rows*cv_mat.cols))
    return false;
  my_mat = new_mat;
  // D may be empty if camera is uncalibrated or distortion model is non-standard
  cv_mat = MatU(&my_mat[0]);
  return true;
}

bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg)
{
  // Create our repository of cached data (rectification maps, etc.)
  if (!cache_)
#ifdef BOOST_SHARED_PTR_HPP_INCLUDED
    cache_ = boost::make_shared<Cache>();
#else
    cache_ = std::make_shared<Cache>();
#endif

  // Binning = 0 is considered the same as binning = 1 (no binning).
  uint32_t binning_x = msg.binning_x ? msg.binning_x : 1;
  uint32_t binning_y = msg.binning_y ? msg.binning_y : 1;

  // ROI all zeros is considered the same as full resolution.
  sensor_msgs::RegionOfInterest roi = msg.roi;
  if (roi.x_offset == 0 && roi.y_offset == 0 && roi.width == 0 && roi.height == 0) {
    roi.width  = msg.width;
    roi.height = msg.height;
  }

  // Update time stamp (and frame_id if that changes for some reason)
  cam_info_.header = msg.header;

  // Update any parameters that have changed. The full rectification maps are
  // invalidated by any change in the calibration parameters OR binning.
  bool full_dirty = false;
  full_dirty |= update(msg.height, cam_info_.height);
  full_dirty |= update(msg.width,  cam_info_.width);
  full_dirty |= update(msg.distortion_model, cam_info_.distortion_model);
  full_dirty |= updateMat(msg.D, cam_info_.D, D_, 1, msg.D.size());
  full_dirty |= updateMat(msg.K, cam_info_.K, K_full_);
  full_dirty |= updateMat(msg.R, cam_info_.R, R_);
  full_dirty |= updateMat(msg.P, cam_info_.P, P_full_);
  full_dirty |= update(binning_x, cam_info_.binning_x);
  full_dirty |= update(binning_y, cam_info_.binning_y);
  cache_->full_maps_dirty |= full_dirty;
  cache_->unrectify_full_maps_dirty |= full_dirty;

  // The reduced rectification maps are invalidated by any of the above or a
  // change in ROI.
  bool reduced_dirty = full_dirty;
  reduced_dirty |= update(roi.x_offset,   cam_info_.roi.x_offset);
  reduced_dirty |= update(roi.y_offset,   cam_info_.roi.y_offset);
  reduced_dirty |= update(roi.height,     cam_info_.roi.height);
  reduced_dirty |= update(roi.width,      cam_info_.roi.width);
  reduced_dirty |= update(roi.do_rectify, cam_info_.roi.do_rectify);
  cache_->reduced_maps_dirty |= reduced_dirty;
  cache_->reduced_maps_dirty |= cache_->full_maps_dirty;
  cache_->unrectify_reduced_maps_dirty |= reduced_dirty;
  cache_->unrectify_reduced_maps_dirty |= cache_->unrectify_full_maps_dirty;

  // As is the rectified ROI
  cache_->rectified_roi_dirty = reduced_dirty;

  // Figure out how to handle the distortion
  if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
      cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL ||
      cam_info_.distortion_model == sensor_msgs::distortion_models::EQUIDISTANT) {
    // If any distortion coefficient is non-zero, then need to apply the distortion
    cache_->distortion_state = NONE;
    for (size_t i = 0; i < cam_info_.D.size(); ++i)
    {
      if (cam_info_.D[i] != 0)
      {
        cache_->distortion_state = CALIBRATED;
        break;
      }
    }
  }
  else
    cache_->distortion_state = UNKNOWN;

  // Get the distortion model, if supported
  if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
      cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL) {
    cache_->distortion_model = PLUMB_BOB_OR_RATIONAL_POLYNOMIAL;
  }
  else if(cam_info_.distortion_model == sensor_msgs::distortion_models::EQUIDISTANT) {
    cache_->distortion_model = EQUIDISTANT;
  }
  else
    cache_->distortion_model = UNKNOWN_MODEL;

  // If necessary, create new K_ and P_ adjusted for binning and ROI
  /// @todo Calculate and use rectified ROI
  bool adjust_binning = (binning_x > 1) || (binning_y > 1);
  bool adjust_roi = (roi.x_offset != 0) || (roi.y_offset != 0);

  if (!adjust_binning && !adjust_roi) {
    K_ = K_full_;
    P_ = P_full_;
  }
  else {
    K_ = K_full_;
    P_ = P_full_;

    // ROI is in full image coordinates, so change it first
    if (adjust_roi) {
      // Move principal point by the offset
      /// @todo Adjust P by rectified ROI instead
      K_(0,2) -= roi.x_offset;
      K_(1,2) -= roi.y_offset;
      P_(0,2) -= roi.x_offset;
      P_(1,2) -= roi.y_offset;
    }

    if (binning_x > 1) {
      double scale_x = 1.0 / binning_x;
      K_(0,0) *= scale_x;
      K_(0,2) *= scale_x;
      P_(0,0) *= scale_x;
      P_(0,2) *= scale_x;
      P_(0,3) *= scale_x;
    }
    if (binning_y > 1) {
      double scale_y = 1.0 / binning_y;
      K_(1,1) *= scale_y;
      K_(1,2) *= scale_y;
      P_(1,1) *= scale_y;
      P_(1,2) *= scale_y;
      P_(1,3) *= scale_y;
    }
  }

  return reduced_dirty;
}

bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg)
{
  return fromCameraInfo(*msg);
}

cv::Size PinholeCameraModel::fullResolution() const
{
  assert( initialized() );
  return cv::Size(cam_info_.width, cam_info_.height);
}

cv::Size PinholeCameraModel::reducedResolution() const
{
  assert( initialized() );

  cv::Rect roi = rectifiedRoi();
  return cv::Size(roi.width / binningX(), roi.height / binningY());
}

cv::Point2d PinholeCameraModel::toFullResolution(const cv::Point2d& uv_reduced) const
{
  cv::Rect roi = rectifiedRoi();
  return cv::Point2d(uv_reduced.x * binningX() + roi.x,
                     uv_reduced.y * binningY() + roi.y);
}

cv::Rect PinholeCameraModel::toFullResolution(const cv::Rect& roi_reduced) const
{
  cv::Rect roi = rectifiedRoi();
  return cv::Rect(roi_reduced.x * binningX() + roi.x,
                  roi_reduced.y * binningY() + roi.y,
                  roi_reduced.width  * binningX(),
                  roi_reduced.height * binningY());
}

cv::Point2d PinholeCameraModel::toReducedResolution(const cv::Point2d& uv_full) const
{
  cv::Rect roi = rectifiedRoi();
  return cv::Point2d((uv_full.x - roi.x) / binningX(),
                     (uv_full.y - roi.y) / binningY());
}

cv::Rect PinholeCameraModel::toReducedResolution(const cv::Rect& roi_full) const
{
  cv::Rect roi = rectifiedRoi();
  return cv::Rect((roi_full.x - roi.x) / binningX(),
                  (roi_full.y - roi.y) / binningY(),
                  roi_full.width  / binningX(),
                  roi_full.height / binningY());
}

cv::Rect PinholeCameraModel::rawRoi() const
{
  assert( initialized() );

  return cv::Rect(cam_info_.roi.x_offset, cam_info_.roi.y_offset,
                  cam_info_.roi.width, cam_info_.roi.height);
}

cv::Rect PinholeCameraModel::rectifiedRoi() const
{
  assert( initialized() );

  if (cache_->rectified_roi_dirty)
  {
    if (!cam_info_.roi.do_rectify)
      cache_->rectified_roi = rawRoi();
    else
      cache_->rectified_roi = rectifyRoi(rawRoi());
    cache_->rectified_roi_dirty = false;
  }
  return cache_->rectified_roi;
}

cv::Point2d PinholeCameraModel::project3dToPixel(const cv::Point3d& xyz) const
{
  assert( initialized() );
  assert(P_(2, 3) == 0.0); // Calibrated stereo cameras should be in the same plane

  // [U V W]^T = P * [X Y Z 1]^T
  // u = U/W
  // v = V/W
  cv::Point2d uv_rect;
  uv_rect.x = (fx()*xyz.x + Tx()) / xyz.z + cx();
  uv_rect.y = (fy()*xyz.y + Ty()) / xyz.z + cy();
  return uv_rect;
}

cv::Point3d PinholeCameraModel::projectPixelTo3dRay(const cv::Point2d& uv_rect) const
{
  return projectPixelTo3dRay(uv_rect, P_);
}

cv::Point3d PinholeCameraModel::projectPixelTo3dRay(const cv::Point2d& uv_rect, const cv::Matx34d& P) const
{
  assert( initialized() );

  const double& fx = P(0,0);
  const double& fy = P(1,1);
  const double& cx = P(0,2);
  const double& cy = P(1,2);
  const double& Tx = P(0,3);
  const double& Ty = P(1,3);

  cv::Point3d ray;
  ray.x = (uv_rect.x - cx - Tx) / fx;
  ray.y = (uv_rect.y - cy - Ty) / fy;
  ray.z = 1.0;
  return ray;
}

void PinholeCameraModel::rectifyImage(const cv::Mat& raw, cv::Mat& rectified, int interpolation) const
{
  assert( initialized() );

  switch (cache_->distortion_state) {
    case NONE:
      raw.copyTo(rectified);
      break;
    case CALIBRATED:
      initRectificationMaps();
      if (raw.depth() == CV_32F || raw.depth() == CV_64F)
      {
        cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation, cv::BORDER_CONSTANT, std::numeric_limits<float>::quiet_NaN());
      }
      else {
        cv::remap(raw, rectified, cache_->reduced_map1, cache_->reduced_map2, interpolation);
      }
      break;
    default:
      assert(cache_->distortion_state == UNKNOWN);
      throw Exception("Cannot call rectifyImage when distortion is unknown.");
  }
}

void PinholeCameraModel::unrectifyImage(const cv::Mat& rectified, cv::Mat& raw, int interpolation) const
{
  assert( initialized() );

  switch (cache_->distortion_state) {
    case NONE:
      rectified.copyTo(raw);
      break;
    case CALIBRATED:
      initUnrectificationMaps();
      if (rectified.depth() == CV_32F || rectified.depth() == CV_64F)
      {
        cv::remap(rectified, raw, cache_->unrectify_reduced_map1, cache_->unrectify_reduced_map2, interpolation, cv::BORDER_CONSTANT, std::numeric_limits<float>::quiet_NaN());
      }
      else {
        cv::remap(rectified, raw, cache_->unrectify_reduced_map1, cache_->unrectify_reduced_map2, interpolation);
      }
      break;
    default:
      assert(cache_->distortion_state == UNKNOWN);
      throw Exception("Cannot call rectifyImage when distortion is unknown.");
  }
}

cv::Point2d PinholeCameraModel::rectifyPoint(const cv::Point2d& uv_raw) const
{
  return rectifyPoint(uv_raw, K_, P_);
}

cv::Point2d PinholeCameraModel::rectifyPoint(const cv::Point2d& uv_raw, const cv::Matx33d& K, const cv::Matx34d& P) const
{
  assert( initialized() );

  if (cache_->distortion_state == NONE)
    return uv_raw;
  if (cache_->distortion_state == UNKNOWN)
    throw Exception("Cannot call rectifyPoint when distortion is unknown.");
  assert(cache_->distortion_state == CALIBRATED);

  /// @todo cv::undistortPoints requires the point data to be float, should allow double
  cv::Point2f raw32 = uv_raw, rect32;
  const cv::Mat src_pt(1, 1, CV_32FC2, &raw32.x);
  cv::Mat dst_pt(1, 1, CV_32FC2, &rect32.x);

  switch (cache_->distortion_model) {
    case PLUMB_BOB_OR_RATIONAL_POLYNOMIAL:
      cv::undistortPoints(src_pt, dst_pt, K, D_, R_, P);
      break;
    case EQUIDISTANT:
      cv::fisheye::undistortPoints(src_pt, dst_pt, K, D_, R_, P);
      break;
    default:
      assert(cache_->distortion_model == UNKNOWN_MODEL);
      throw Exception("Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT.");
  }
  return rect32;
}

cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect) const
{
  return unrectifyPoint(uv_rect, K_, P_);
}

cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect, const cv::Matx33d& K, const cv::Matx34d& P) const
{
  assert( initialized() );

  if (cache_->distortion_state == NONE)
    return uv_rect;
  if (cache_->distortion_state == UNKNOWN)
    throw Exception("Cannot call unrectifyPoint when distortion is unknown.");
  assert(cache_->distortion_state == CALIBRATED);

  // Convert to a ray
  cv::Point3d ray = projectPixelTo3dRay(uv_rect, P);

  // Project the ray on the image
  cv::Mat r_vec, t_vec = cv::Mat_<double>::zeros(3, 1);
  cv::Rodrigues(R_.t(), r_vec);
  std::vector<cv::Point2d> image_point;

  switch (cache_->distortion_model) {
    case PLUMB_BOB_OR_RATIONAL_POLYNOMIAL:
      cv::projectPoints(std::vector<cv::Point3d>(1, ray), r_vec, t_vec, K, D_, image_point);
      break;
    case EQUIDISTANT:
      cv::fisheye::projectPoints(std::vector<cv::Point3d>(1, ray), image_point, r_vec, t_vec, K, D_);
      break;
    default:
      assert(cache_->distortion_model == UNKNOWN_MODEL);
      throw Exception("Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT.");
  }

  return image_point[0];
}

cv::Rect PinholeCameraModel::rectifyRoi(const cv::Rect& roi_raw) const
{
  assert( initialized() );

  /// @todo Actually implement "best fit" as described by REP 104.

  // For now, just unrectify the four corners and take the bounding box.
  cv::Point2d rect_tl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y));
  cv::Point2d rect_tr = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, roi_raw.y));
  cv::Point2d rect_br = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width,
                                                 roi_raw.y + roi_raw.height));
  cv::Point2d rect_bl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y + roi_raw.height));

  cv::Point roi_tl(std::ceil (std::min(rect_tl.x, rect_bl.x)),
                   std::ceil (std::min(rect_tl.y, rect_tr.y)));
  cv::Point roi_br(std::floor(std::max(rect_tr.x, rect_br.x)),
                   std::floor(std::max(rect_bl.y, rect_br.y)));

  return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y);
}

cv::Rect PinholeCameraModel::unrectifyRoi(const cv::Rect& roi_rect) const
{
  assert( initialized() );

  /// @todo Actually implement "best fit" as described by REP 104.

  // For now, just unrectify the four corners and take the bounding box.
  cv::Point2d raw_tl = unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y));
  cv::Point2d raw_tr = unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width, roi_rect.y));
  cv::Point2d raw_br = unrectifyPoint(cv::Point2d(roi_rect.x + roi_rect.width,
                                                  roi_rect.y + roi_rect.height));
  cv::Point2d raw_bl = unrectifyPoint(cv::Point2d(roi_rect.x, roi_rect.y + roi_rect.height));

  cv::Point roi_tl(std::floor(std::min(raw_tl.x, raw_bl.x)),
                   std::floor(std::min(raw_tl.y, raw_tr.y)));
  cv::Point roi_br(std::ceil (std::max(raw_tr.x, raw_br.x)),
                   std::ceil (std::max(raw_bl.y, raw_br.y)));

  return cv::Rect(roi_tl.x, roi_tl.y, roi_br.x - roi_tl.x, roi_br.y - roi_tl.y);
}

void PinholeCameraModel::initRectificationMaps() const
{
  /// @todo For large binning settings, can drop extra rows/cols at bottom/right boundary.
  /// Make sure we're handling that 100% correctly.

  if (cache_->full_maps_dirty) {
    // Create the full-size map at the binned resolution
    /// @todo Should binned resolution, K, P be part of public API?
    cv::Size binned_resolution = fullResolution();
    binned_resolution.width  /= binningX();
    binned_resolution.height /= binningY();

    cv::Matx33d K_binned;
    cv::Matx34d P_binned;
    if (binningX() == 1 && binningY() == 1) {
      K_binned = K_full_;
      P_binned = P_full_;
    }
    else {
      K_binned = K_full_;
      P_binned = P_full_;
      if (binningX() > 1) {
        double scale_x = 1.0 / binningX();
        K_binned(0,0) *= scale_x;
        K_binned(0,2) *= scale_x;
        P_binned(0,0) *= scale_x;
        P_binned(0,2) *= scale_x;
        P_binned(0,3) *= scale_x;
      }
      if (binningY() > 1) {
        double scale_y = 1.0 / binningY();
        K_binned(1,1) *= scale_y;
        K_binned(1,2) *= scale_y;
        P_binned(1,1) *= scale_y;
        P_binned(1,2) *= scale_y;
        P_binned(1,3) *= scale_y;
      }
    }

    switch (cache_->distortion_model) {
      case PLUMB_BOB_OR_RATIONAL_POLYNOMIAL:
        // Note: m1type=CV_16SC2 to use fast fixed-point maps (see cv::remap)
        cv::initUndistortRectifyMap(K_binned, D_, R_, P_binned, binned_resolution,
                                    CV_16SC2, cache_->full_map1, cache_->full_map2);
        break;
      case EQUIDISTANT:
        cv::fisheye::initUndistortRectifyMap(K_binned,D_, R_, P_binned, binned_resolution,
                                             CV_16SC2, cache_->full_map1, cache_->full_map2);
        break;
      default:
        assert(cache_->distortion_model == UNKNOWN_MODEL);
        throw Exception("Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT.");
    }
    cache_->full_maps_dirty = false;
  }

  if (cache_->reduced_maps_dirty) {
    /// @todo Use rectified ROI
    cv::Rect roi(cam_info_.roi.x_offset, cam_info_.roi.y_offset,
                 cam_info_.roi.width, cam_info_.roi.height);
    if (roi.x != 0 || roi.y != 0 ||
        (roi.height != 0 && roi.height != (int)cam_info_.height) ||
        (roi.width != 0 && roi.width  != (int)cam_info_.width)) {

      // map1 contains integer (x,y) offsets, which we adjust by the ROI offset
      // map2 contains LUT index for subpixel interpolation, which we can leave as-is
      roi.x /= binningX();
      roi.y /= binningY();
      roi.width  /= binningX();
      roi.height /= binningY();
      cache_->reduced_map1 = cache_->full_map1(roi) - cv::Scalar(roi.x, roi.y);
      cache_->reduced_map2 = cache_->full_map2(roi);
    }
    else {
      // Otherwise we're rectifying the full image
      cache_->reduced_map1 = cache_->full_map1;
      cache_->reduced_map2 = cache_->full_map2;
    }
    cache_->reduced_maps_dirty = false;
  }
}

void PinholeCameraModel::initUnrectificationMaps() const
{
  /// @todo For large binning settings, can drop extra rows/cols at bottom/right boundary.
  /// Make sure we're handling that 100% correctly.

  if (cache_->unrectify_full_maps_dirty) {
    // Create the full-size map at the binned resolution
    /// @todo Should binned resolution, K, P be part of public API?
    cv::Size binned_resolution = fullResolution();
    binned_resolution.width  /= binningX();
    binned_resolution.height /= binningY();

    cv::Matx33d K_binned;
    cv::Matx34d P_binned;
    if (binningX() == 1 && binningY() == 1) {
      K_binned = K_full_;
      P_binned = P_full_;
    }
    else {
      K_binned = K_full_;
      P_binned = P_full_;
      if (binningX() > 1) {
        double scale_x = 1.0 / binningX();
        K_binned(0,0) *= scale_x;
        K_binned(0,2) *= scale_x;
        P_binned(0,0) *= scale_x;
        P_binned(0,2) *= scale_x;
        P_binned(0,3) *= scale_x;
      }
      if (binningY() > 1) {
        double scale_y = 1.0 / binningY();
        K_binned(1,1) *= scale_y;
        K_binned(1,2) *= scale_y;
        P_binned(1,1) *= scale_y;
        P_binned(1,2) *= scale_y;
        P_binned(1,3) *= scale_y;
      }
    }

    cv::Mat float_map_x(binned_resolution.height, binned_resolution.width, CV_32FC1);
    cv::Mat float_map_y(binned_resolution.height, binned_resolution.width, CV_32FC1);
    for (size_t x = 0; x < binned_resolution.width; x++) {
      for (size_t y = 0; y < binned_resolution.height; y++) {
        cv::Point2f uv_raw(x, y), uv_rect;
        uv_rect = rectifyPoint(uv_raw, K_binned, P_binned);
        float_map_x.at<float>(y, x) = uv_rect.x;
        float_map_y.at<float>(y, x) = uv_rect.y;
      }
    }
    // Note: m1type=CV_16SC2 to use fast fixed-point maps (see cv::remap)
    convertMaps(float_map_x, float_map_y, cache_->unrectify_full_map1, cache_->unrectify_full_map2, CV_16SC2);
    cache_->unrectify_full_maps_dirty = false;
  }

  if (cache_->unrectify_reduced_maps_dirty) {
    /// @todo Use rectified ROI
    cv::Rect roi(cam_info_.roi.x_offset, cam_info_.roi.y_offset,
                 cam_info_.roi.width, cam_info_.roi.height);
    if (roi.x != 0 || roi.y != 0 ||
        (roi.height != 0 && roi.height != (int)cam_info_.height) ||
        (roi.width != 0 && roi.width  != (int)cam_info_.width)) {

      // map1 contains integer (x,y) offsets, which we adjust by the ROI offset
      // map2 contains LUT index for subpixel interpolation, which we can leave as-is
      roi.x /= binningX();
      roi.y /= binningY();
      roi.width  /= binningX();
      roi.height /= binningY();
      cache_->unrectify_reduced_map1 = cache_->unrectify_full_map1(roi) - cv::Scalar(roi.x, roi.y);
      cache_->unrectify_reduced_map2 = cache_->unrectify_full_map2(roi);
    }
    else {
      // Otherwise we're rectifying the full image
      cache_->unrectify_reduced_map1 = cache_->unrectify_full_map1;
      cache_->unrectify_reduced_map2 = cache_->unrectify_full_map2;
    }
    cache_->unrectify_reduced_maps_dirty = false;
  }
}

} //namespace image_geometry


#include "problems/mga_1dsm_transx.h"

#include <keplerian_toolbox/core_functions/array3D_operations.h>
#include <keplerian_toolbox/core_functions/fb_vel.h>
#include <keplerian_toolbox/core_functions/propagate_lagrangian.h>
#include <keplerian_toolbox/core_functions/fb_prop.h>
#include <keplerian_toolbox/lambert_problem.h>

#include <string>
#include <cmath>
#include <numeric>
#include <vector>

#include <boost/array.hpp>
#include <boost/math/constants/constants.hpp>

namespace pagmo { namespace problem {

int dimension_from_params(const std::vector<kep_toolbox::planet::planet_ptr> seq, const std::vector<bool> m_dsm) {
    int dim = 2;
    int n = seq.size();
    bool dsm = m_dsm.size() == 0 || (m_dsm.size() > 0 && m_dsm[0]);
    dim += (dsm ? 4 : 1);
    for (int i = 1; i < seq.size() - 1; ++i) {
        dsm = m_dsm.size() == 0 || (m_dsm.size() > i && m_dsm[i]);
        dim += (dsm ? 4 : 1);
    }
    return dim;
}

bool mga_1dsm_transx::dsm_in_leg_i(int i) const {
    return (m_dsm.size() == 0) || (m_dsm.size() > i && m_dsm[i]);
}

int mga_1dsm_transx::t0_index() const { return 0; }
int mga_1dsm_transx::tof_index() const { return 1; }
int mga_1dsm_transx::vinf_index() const { return 3; } // 3, 4

int mga_1dsm_transx::base_idx(int i) const{
    bool dsm = dsm_in_leg_i(0);
    int base = (tof_index() + 1);
    if (i == 0) return base;

    int idx = (base + (dsm ? 4 : 1));
    for (int j = 1; j < i; ++j) {
        dsm = dsm_in_leg_i(j);
        idx += (dsm ? 4 : 1);
    }
    return idx;
}
int mga_1dsm_transx::T_idx(int i) const {
    int base = base_idx(i);
    if (i == 0) return base;

    return base;
}

int mga_1dsm_transx::Beta_idx(int i) const {
    int base = base_idx(i);
    return base + 1;
}

int mga_1dsm_transx::R_idx(int i) const {
    int base = base_idx(i);
    return base + 2;
}

int mga_1dsm_transx::DSM_idx(int i) const {
    int base = base_idx(i);
    if (i == 0) return base + 4;

    return base + 3;
}

mga_1dsm_transx::mga_1dsm_transx(const std::vector<kep_toolbox::planet::planet_ptr> seq,
                    const std::vector<bool> dsm,
                    const double dep_altitude, const double arr_altitude, const bool circularize,
                    const kep_toolbox::epoch t0_l, const kep_toolbox::epoch t0_u,
                    const double tof_l, const double tof_u,
                    const double vinf_l, const double vinf_u,
                    const bool add_vinf_dep, const bool add_vinf_arr,
                    const bool multi_objective) : transx_problem(seq, dep_altitude, arr_altitude, circularize, dimension_from_params(seq, dsm), 1 + (int)multi_objective), m_add_vinf_dep(add_vinf_dep), m_add_vinf_arr(add_vinf_arr), m_dsm(dsm) {
    size_t dim(get_dimension());
    decision_vector lb(dim, 0.0), ub(dim, 0.0);

      lb[0] = t0_l.mjd2000(); ub[0] = t0_u.mjd2000();
      lb[1] = tof_l * 365.25; ub[1] = tof_u * 365.25;

      lb[2] = 1e-5; ub[2] = 1 - 1e-5;

      if (dsm_in_leg_i(0)) {
        lb[vinf_index()] = lb[vinf_index() + 1] = 0.0; ub[vinf_index()] = ub[vinf_index() + 1] = 1.0;
        lb[vinf_index() + 2] = vinf_l * 1000; ub[vinf_index() + 2] = vinf_u * 1000;
        lb[DSM_idx(0)] = 1e-5; ub[DSM_idx(0)] = 1 - 1e-5;
      }

      for (int i = 0; i < get_n_legs() - 1; ++i) {
        auto planet = seq[i + 1];

        int j = base_idx(i + 1);
        bool dsm = dsm_in_leg_i(i + 1);
        lb[j] = 1e-5;  ub[j] = 1 - 1e-5; // T[i]
        if (dsm) {

            double a = planet->compute_elements()[0];
            double soi = a * pow((planet->get_mu_self() / planet->get_mu_central_body()), 2/5);
            double soiRad = soi / planet->get_radius();

            double safeDistanceRatio = planet->get_safe_radius() / planet->get_radius();

            lb[j + 1] = -2 * boost::math::constants::pi<double>(); ub[j + 1] = 2 * boost::math::constants::pi<double>(); // Beta
            lb[j + 2] = safeDistanceRatio;   ub[j + 2] = soiRad; // Rad
            lb[j + 3] = 1e-5;  ub[j + 3] = 1 - 1e-5;
        }
      }

        for (int i = 1; i < get_n_legs() - 1; ++i) {
            bool dsm = dsm_in_leg_i(i);
            if (dsm) {
                kep_toolbox::planet::planet_ptr pl = get_seq()[i];
                lb[R_idx(i)] = pl->get_safe_radius() / pl->get_radius();
            }
        }

    set_bounds(lb, ub);
}

mga_1dsm_transx::mga_1dsm_transx(const mga_1dsm_transx &p) : transx_problem(p.get_seq(), p.get_dep_altitude(), p.get_arr_altitude(), p.get_circularize(), p.get_dimension(), p.get_f_dimension()), m_dsm(p.m_dsm), m_add_vinf_arr(p.get_add_vinf_arr()), m_add_vinf_dep(p.get_add_vinf_dep()), m_multi_obj(p.get_multi_obj()) {
  set_bounds(p.get_lb(), p.get_ub());
}

base_ptr mga_1dsm_transx::clone() const {
  return base_ptr(new mga_1dsm_transx(*this));
}

void mga_1dsm_transx::calc_objective(fitness_vector &f, const decision_vector &x, bool should_print, TransXSolution * solution) const {

  std::vector<double> T(get_n_legs());
  double alpha_sum = 0;
  for (int i = 0; i < T.size(); ++i) {
    T[i] = - log(x[T_idx(i)]);
    alpha_sum += T[i];
  }

  bool dsm = dsm_in_leg_i(0);

  kep_toolbox::array3D Vinf;
  if (dsm) {
    double theta = 2 * M_PI * x[vinf_index()];
    double phi = acos(2 * x[vinf_index() + 1] - 1) - M_PI / 2;

    double vinf = x[vinf_index() + 2];
    Vinf[0] = vinf * cos(phi) * cos(theta);
    Vinf[1] = vinf * cos(phi) * sin(theta);
    Vinf[2] = vinf * sin(phi);

    }

  for (int i = 0; i < T.size(); ++i) {
    T[i] = x[tof_index()] * T[i] / alpha_sum;
  }

  int n = get_seq().size();
  std::vector<kep_toolbox::epoch> t_P(n);
  std::vector<kep_toolbox::array3D> r_P(n);
  std::vector<kep_toolbox::array3D> v_P(n);
  std::vector<double> DV(n + 1, 0.0);

  for (int i = 0; i < n; ++i) {
    kep_toolbox::planet::planet_ptr planet = get_seq()[i];
    t_P[i] = kep_toolbox::epoch(x[t0_index()] + std::accumulate(T.begin(), T.begin() + i, 0.0));
    planet->eph(t_P[i], r_P[i], v_P[i]);
  }

  if (should_print) {
    transx_time_info(solution->mutable_times(), get_seq(), t_P);
  }

  kep_toolbox::array3D r, v;
  r = r_P[0];
  if (dsm) {
    if (m_add_vinf_dep) {
        DV[0] += burn_cost(get_seq()[0], Vinf, false, true);
    }
    if (should_print) {
        transx_escape(solution->mutable_escape(), get_seq()[0], v_P[0], r_P[0], Vinf, t_P[0].mjd());
    }

    kep_toolbox::array3D v0;
    kep_toolbox::sum(v0, v_P[0], Vinf);
    v = v0;
    kep_toolbox::propagate_lagrangian(r, v, (dsm ? x[DSM_idx(0)] : 0) * T[0] * ASTRO_DAY2SEC, get_common_mu());
  }

  double dt = (1 - (dsm ? x[DSM_idx(0)] : 0)) * T[0] * ASTRO_DAY2SEC;
  kep_toolbox::lambert_problem l(r, r_P[1], dt, get_common_mu());
  kep_toolbox::array3D v_end_l(l.get_v2()[0]);
  kep_toolbox::array3D v_beg_l(l.get_v1()[0]);

  if (!dsm) {
    kep_toolbox::diff(Vinf, v_beg_l, v_P[0]);
    v = Vinf;
    kep_toolbox::sum(v, v_P[0], Vinf);

    if (m_add_vinf_dep) {
        DV[0] += burn_cost(get_seq()[0], Vinf, false, true);
    }
    if (should_print) {
        transx_escape(solution->mutable_escape(), get_seq()[0], v_P[0], r_P[0], Vinf, t_P[0].mjd());
    }
  }

  kep_toolbox::array3D deltaV;
  kep_toolbox::diff(deltaV, v_beg_l, v);
  DV[0] += kep_toolbox::norm(deltaV);

  if (dsm && should_print) {
    transx_dsm(solution->add_dsms(), v, r, deltaV, v_beg_l, t_P[0].mjd() + T[0] - dt / ASTRO_DAY2SEC, 0);
  }

  for (int i = 1; i < n - 1; ++i) {
    dsm = dsm_in_leg_i(i);

    kep_toolbox::array3D v_rel_in, v_rel_out;
    if (dsm) {
        double radius = x[R_idx(i)] * get_seq()[i]->get_radius();
        double beta = x[Beta_idx(i)];
        kep_toolbox::array3D v_out;
        kep_toolbox::fb_prop(v_out, v_end_l, v_P[i], radius, beta, get_seq()[i]->get_mu_self());

        kep_toolbox::diff(v_rel_in, v_end_l, v_P[i]);
        kep_toolbox::diff(v_rel_out, v_out, v_P[i]);

        if (should_print) {
            transx_flyby(solution->add_flybyes(), get_seq()[i], v_P[i], r_P[i], v_rel_in, v_rel_out, t_P[i].mjd());
        }

        r = r_P[i]; v = v_out;

        kep_toolbox::propagate_lagrangian(r, v, (dsm ? x[DSM_idx(i)] : 0) * T[i] * ASTRO_DAY2SEC, get_common_mu());
    } else {
        r = r_P[i]; v = v_end_l;
    }

    dt = (1 - (dsm ? x[DSM_idx(i)] : 0)) * T[i] * ASTRO_DAY2SEC;
    kep_toolbox::lambert_problem l2(r, r_P[i + 1], dt, get_common_mu());
    v_beg_l = l2.get_v1()[0];
    v_end_l = l2.get_v2()[0];

    if (dsm) {
        kep_toolbox::diff(deltaV, v_beg_l, v);
        DV[i] = kep_toolbox::norm(deltaV);

        if (should_print) {
            transx_dsm(solution->add_dsms(), v, r, deltaV, v_beg_l, t_P[i].mjd() + T[i] - dt / ASTRO_DAY2SEC, 1);
        }
    } else {
        kep_toolbox::diff(v_rel_in, v, v_P[i]);
        kep_toolbox::diff(v_rel_out, v_beg_l, v_P[i]);

        kep_toolbox::planet::planet_ptr planet = get_seq()[i];
        kep_toolbox::fb_vel(DV[i], v_rel_in, v_rel_out, *planet);

        double ta  = acos(kep_toolbox::dot(v_rel_in, v_rel_out)/sqrt(kep_toolbox::dot(v_rel_in,v_rel_in))/sqrt(kep_toolbox::dot(v_rel_out,v_rel_out)));
        double alt = (planet->get_mu_self() / kep_toolbox::dot(v_rel_in,v_rel_in)*(1/sin(ta/2)-1) - planet->get_radius())/1000;
        if (alt > planet->get_safe_radius()) {
            f[0] = DBL_MAX;
            return;
        }

        if (should_print) {
            transx_flyby(solution->add_flybyes(), planet, v_P[i], r_P[i], v_rel_in, v_rel_out, t_P[i].mjd());
        }
    }

  }

  kep_toolbox::array3D Vexc_arr(v_end_l);
  kep_toolbox::diff(Vexc_arr, v_end_l, v_P[v_P.size() - 1]);
  if (m_add_vinf_arr) {
    DV[DV.size() - 1] += burn_cost(get_seq()[get_seq().size() - 1], Vexc_arr, true, get_circularize());
  }
  if (should_print) {
    transx_arrival(solution->mutable_arrival(), get_seq()[get_seq().size() - 1], Vexc_arr, t_P[t_P.size() - 1].mjd());
  }

  double sumDeltaV = std::accumulate(DV.begin(), DV.end(), 0.0);
  double sumT = std::accumulate(T.begin(), T.end(), 0.0);

  if (should_print) {
    solution->set_fuel_cost(sumDeltaV);
  }

  f[0] = sumDeltaV;
  if (get_f_dimension() == 2) {
    f[1] = sumT;
  }

}

std::string mga_1dsm_transx::get_name() const {
  return "MGA-1DSM";
}

}} // namespaces

BOOST_CLASS_EXPORT_IMPLEMENT(pagmo::problem::mga_1dsm_transx)


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_DEFINITION_ONEPLUS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DEFINITION_ONEPLUS_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/dispatch/function/make_callable.hpp>
#include <boost/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    BOOST_DISPATCH_MAKE_TAG(ext, oneplus_, boost::dispatch::elementwise_<oneplus_>);
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, oneplus_);
  }

  BOOST_DISPATCH_CALLABLE_DEFINITION(tag::oneplus_,oneplus);

} }

#endif


#include <cstring>
#include <fstream>
#include <unistd.h>

#include <NTL/ZZX.h>
#include <NTL/vector.h>

#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"

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

    long m = 7;
    long r = 1;
    long p = 65537;
    long cleanup = 1;

    string owner = "owner";
    long number = 1;

    amap.arg("o", owner, "owner's address");
    amap.parse(argc, argv);

    // file names
    const string secretKeyBinaryFileName = "data/secretKey/" + owner + ".bin";
    const string candidateFileName = "data/candidate/" + owner + "-" + to_string(number) + ".txt";

    ifstream secretBinFile(secretKeyBinaryFileName.c_str(), ios::binary);
    ifstream candidateFile(candidateFileName.c_str());

    // check open file
    assert(secretBinFile.is_open());
    assert(candidateFile.is_open());

    // Read in context,
    std::unique_ptr<FHEcontext> context = buildContextFromBinary(secretBinFile);
    readContextBinary(secretBinFile, *context);

    // Read in SecKey and PubKey.
    // Got to insert pubKey into seckey obj first.
    std::unique_ptr<FHESecKey> secKey(new FHESecKey(*context));
    FHEPubKey *pubKey = (FHEPubKey *) secKey.get();

    // read publicKey & secretKey
    readPubKeyBinary(secretBinFile, *pubKey);
    readSecKeyBinary(secretBinFile, *secKey);

    secretBinFile.close();

    cout << "read PublicKey & SecretKey successful.\n" << flush;

    // ready to decryption
    Ctxt encryptionText(*pubKey);
    ZZX resultPoly;

    // get Ctxt to file
    candidateFile >> encryptionText;

    // decrypt poly
    secKey->Decrypt(resultPoly, encryptionText);

    cout << "decryption successful.\n\n";

    cout << "result : "<< resultPoly << endl;
}