#include <voxelized_geometry_tools/opencl_pointcloud_voxelization.hpp>

#include <atomic>
#include <cmath>
#include <cstdint>
#include <map>
#include <memory>
#include <stdexcept>
#include <string>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/print.hpp>
#include <common_robotics_utilities/voxel_grid.hpp>
#include <voxelized_geometry_tools/collision_map.hpp>
#include <voxelized_geometry_tools/opencl_voxelization_helpers.h>
#include <voxelized_geometry_tools/pointcloud_voxelization_interface.hpp>

namespace voxelized_geometry_tools
{
namespace pointcloud_voxelization
{
OpenCLPointCloudVoxelizer::OpenCLPointCloudVoxelizer(
    const std::map<std::string, int32_t>& options)
    : interface_(opencl_helpers::MakeHelperInterface(options))
{
  if (!interface_->IsAvailable())
  {
    throw std::runtime_error("OpenCLPointCloudVoxelizer not available");
  }
}

VoxelizerRuntime OpenCLPointCloudVoxelizer::DoVoxelizePointClouds(
    const CollisionMap& static_environment, const double step_size_multiplier,
    const PointCloudVoxelizationFilterOptions& filter_options,
    const std::vector<PointCloudWrapperPtr>& pointclouds,
    CollisionMap& output_environment) const
{
  if (interface_->IsAvailable())
  {
    const std::chrono::time_point<std::chrono::steady_clock> start_time =
        std::chrono::steady_clock::now();
    // Allocate device-side memory for tracking grids
    const std::vector<int64_t> device_tracking_grid_offsets =
        interface_->PrepareTrackingGrids(
            static_environment.GetTotalCells(),
            static_cast<int32_t>(pointclouds.size()));
    if (device_tracking_grid_offsets.size() != pointclouds.size())
    {
      throw std::runtime_error("Failed to allocate device tracking grid");
    }
    // Prepare grid data
    const Eigen::Isometry3f inverse_grid_origin_transform_float =
        static_environment.GetInverseOriginTransform().cast<float>();
    const float inverse_step_size =
        static_cast<float>(1.0 /
            (static_environment.GetResolution() * step_size_multiplier));
    const float inverse_cell_size =
        static_cast<float>(static_environment.GetGridSizes().InvCellXSize());
    const int32_t num_x_cells =
        static_cast<int32_t>(static_environment.GetNumXCells());
    const int32_t num_y_cells =
        static_cast<int32_t>(static_environment.GetNumYCells());
    const int32_t num_z_cells =
        static_cast<int32_t>(static_environment.GetNumZCells());
    // Do raycasting of the pointclouds
#pragma omp parallel for
    for (size_t idx = 0; idx < pointclouds.size(); idx++)
    {
      const PointCloudWrapperPtr& pointcloud = pointclouds.at(idx);
      const Eigen::Isometry3f pointcloud_origin_transform_float =
          pointcloud->GetPointCloudOriginTransform().cast<float>();
      const float max_range = static_cast<float>(pointcloud->MaxRange());
      // Copy pointcloud
      std::vector<float> raw_points(pointcloud->Size() * 3, 0.0);
      for (int64_t point = 0; point < pointcloud->Size(); point++)
      {
        pointcloud->CopyPointLocationIntoVectorFloat(
            point, raw_points, point * 3);
      }
      // Raycast
      interface_->RaycastPoints(
          raw_points, pointcloud_origin_transform_float, max_range,
          inverse_grid_origin_transform_float, inverse_step_size,
          inverse_cell_size, num_x_cells, num_y_cells, num_z_cells,
          device_tracking_grid_offsets.at(idx));
    }
    const std::chrono::time_point<std::chrono::steady_clock> raycasted_time =
        std::chrono::steady_clock::now();
    // Filter
    const float percent_seen_free =
        static_cast<float>(filter_options.PercentSeenFree());
    const int32_t outlier_points_threshold =
        filter_options.OutlierPointsThreshold();
    const int32_t num_cameras_seen_free =
        filter_options.NumCamerasSeenFree();
    interface_->PrepareFilterGrid(
        static_environment.GetTotalCells(),
        static_environment.GetImmutableRawData().data());
    interface_->FilterTrackingGrids(
        static_environment.GetTotalCells(),
        static_cast<int32_t>(pointclouds.size()), percent_seen_free,
        outlier_points_threshold, num_cameras_seen_free);
    // Retrieve & return
    interface_->RetrieveFilteredGrid(
        static_environment.GetTotalCells(),
        output_environment.GetMutableRawData().data());
    // Cleanup device memory
    interface_->CleanupAllocatedMemory();
    const std::chrono::time_point<std::chrono::steady_clock> done_time =
        std::chrono::steady_clock::now();
    return VoxelizerRuntime(
        std::chrono::duration<double>(raycasted_time - start_time).count(),
        std::chrono::duration<double>(done_time - raycasted_time).count());
  }
  else
  {
    throw std::runtime_error("OpenCLPointCloudVoxelizer not available");
  }
}
}  // namespace pointcloud_voxelization
}  // namespace voxelized_geometry_tools


#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <std_msgs/Empty.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Path.h>
#include <octomap/math/Vector3.h>
#include <unordered_set>
#include <visualization_msgs/Marker.h>

#include <utility>
#include <sstream>
#include <fstream>
#include <string>
#include <chrono>
#include <boost/filesystem.hpp>

#include <architecture_math.h>
#include <goal_state_machine.h>
#include <exploration_state_machine.h>
#include <observation_maneuver.h>
#include <marker_publishing_utils.h>

#include <architecture_msgs/PositionRequest.h>
#include <architecture_msgs/PositionMiddleMan.h>
#include <architecture_msgs/FindNextGoal.h>
#include <architecture_msgs/DeclareUnobservable.h>

#include <frontiers_msgs/CheckIsFrontier.h>
#include <frontiers_msgs/CheckIsExplored.h>
#include <frontiers_msgs/FrontierNodeStatus.h>
#include <frontiers_msgs/VoxelMsg.h>

#include <lazy_theta_star_msgs/LTStarReply.h>
#include <lazy_theta_star_msgs/LTStarRequest.h>
#include <lazy_theta_star_msgs/LTStarNodeStatus.h>

#define SAVE_CSV 1
#define SAVE_LOG 1

namespace state_manager_node
{
    std::stringstream aux_envvar_home (std::getenv("HOME"));
    std::string folder_name = aux_envvar_home.str() + "/Flying_Octomap_code/src/data";

    ros::Publisher frontier_request_pub;
    ros::Publisher ltstar_request_pub;
    ros::Publisher marker_pub;
    ros::Publisher flight_plan_pub;
    ros::ServiceClient ltstar_status_cliente;
    ros::ServiceClient ask_for_goal_client;

    ros::Timer timer;
    std::chrono::high_resolution_clock::time_point start;
    bool is_successfull_exploration = false;
    std::ofstream log_file;
    #ifdef SAVE_CSV
    std::ofstream csv_file, csv_file_success;
    std::chrono::high_resolution_clock::time_point operation_start, timeline_start;
    #endif

    double px4_loiter_radius;
    double odometry_error;
    double sensing_distance = 3;
    double error_margin;
    double distance_inFront, distance_behind;
    int circle_divisions = 12;
    int uav_id_= 1;
    int max_time_secs =5000;
    double ltstar_safety_margin;
    octomath::Vector3 geofence_min (-5, -5, 1);
    octomath::Vector3 geofence_max (5, 5, 10);
    struct StateData {
        int frontier_request_id;    // id for the request in use
        int frontier_request_count; // generate id for new frontier requests
        int ltstar_request_id;
        bool new_map;
        architecture_msgs::FindNextGoal::Response next_goal_msg;
        lazy_theta_star_msgs::LTStarRequest ltstar_request;
        lazy_theta_star_msgs::LTStarReply ltstar_reply;
        exploration_sm::ExplorationStateMachine exploration_state;
    };
    state_manager_node::StateData state_data;

    std::pair<double, double> calculateTime()
    {
        auto end_millis         = std::chrono::high_resolution_clock::now();

        auto time_span          = std::chrono::duration_cast<std::chrono::duration<double>>(end_millis - operation_start);
        double operation_millis = std::chrono::duration_cast<std::chrono::milliseconds>(time_span).count();

        time_span               = std::chrono::duration_cast<std::chrono::duration<double>>(end_millis - timeline_start);
        double timeline_millis  = std::chrono::duration_cast<std::chrono::milliseconds>(time_span).count();
        operation_start         = std::chrono::high_resolution_clock::now();
        return std::make_pair (timeline_millis, operation_millis);
    }

    void askForObstacleAvoidingPath()
    {
        lazy_theta_star_msgs::LTStarNodeStatus srv;
        if(ltstar_status_cliente.call(srv))
        {
            if((bool)srv.response.is_accepting_requests)
            {
                lazy_theta_star_msgs::LTStarRequest request;
                state_data.ltstar_request_id++;
                request.request_id = state_data.ltstar_request_id;
                request.header.frame_id = "world";
                request.start = state_data.ltstar_reply.waypoints[(state_data.ltstar_reply.waypoints.size()-1)].position;
                request.goal  = state_data.next_goal_msg.start_flyby;
                request.safety_margin = ltstar_safety_margin;
                if(state_data.next_goal_msg.global)
                {
                    request.max_time_secs = max_time_secs;
                }
                else
                {
                    request.max_time_secs = max_time_secs/4;
                }
                #ifdef SAVE_LOG
                log_file << "[State manager] Requesting path " << request << std::endl;
                #endif
                ROS_INFO_STREAM ("[State manager] Requesting path from " << request.start << " to " << request.goal);
                ltstar_request_pub.publish(request);
                state_data.ltstar_request = request;
                state_data.exploration_state.switchState(exploration_sm::waiting_path_response);
            }
        }
        else
        {
            ROS_WARN("[State manager] Lazy Theta Star node not accepting requests.");
        }
    }

    void publishGeofence()
    {
        visualization_msgs::MarkerArray marker_array;
        rviz_interface::publish_geofence(geofence_min, geofence_max, marker_array);
        marker_pub.publish(marker_array);
    }

    bool askForGoalServiceCall()
    {
        publishGeofence();
        architecture_msgs::FindNextGoal find_next_goal;
        find_next_goal.request.new_map = state_data.new_map;
        if(ask_for_goal_client.call(find_next_goal))
        {
            state_data.next_goal_msg = find_next_goal.response;
            state_data.new_map = false;
            return true;
        }
        else
        {
            ROS_WARN("[State manager] Goal SM node not accepting next goal requests.");
            return false;
        }
    }

    nav_msgs::Path generateFlightPlanRequest()
    {
        nav_msgs::Path flight_plan_request;
        geometry_msgs::PoseStamped pose_s;
        for (std::vector<geometry_msgs::Pose>::iterator i = state_data.ltstar_reply.waypoints.begin(); i != state_data.ltstar_reply.waypoints.end(); ++i)
        {
            pose_s.pose = *i;
            flight_plan_request.poses.push_back(pose_s);
        }
        pose_s.pose.position = state_data.next_goal_msg.end_flyby;
        flight_plan_request.poses.push_back(pose_s);

        std::ofstream pathWaypoints;
        pathWaypoints.open (folder_name + "/current/final_path_state_manager.txt", std::ofstream::out | std::ofstream::app);
        for (std::vector<geometry_msgs::PoseStamped>::iterator i = flight_plan_request.poses.begin(); i != flight_plan_request.poses.end(); ++i)
        {
            pathWaypoints << std::setprecision(5) << i->pose.position.x << ", " << i->pose.position.y << ", " << i->pose.position.z << std::endl;

        }
        pathWaypoints.close();
        return flight_plan_request;
    }

    void findTarget()
    {
        #ifdef SAVE_LOG
            log_file << "[State manager][Exploration] exploration_start. Asked for next goal." << std::endl;
        #endif

        while(!askForGoalServiceCall()){}

        if(!state_data.next_goal_msg.success)
        {
            ROS_INFO_STREAM("[State manager][Exploration] finished_exploring - no frontiers reported.");
            log_file << "[State manager][Exploration] finished_exploring - no frontiers reported." << std::endl;
            is_successfull_exploration = true;
            state_data.exploration_state.switchState(exploration_sm::finished_exploring);
        }
        else
        {
            state_data.exploration_state.switchState(exploration_sm::generating_path);
            askForObstacleAvoidingPath();
        }
    }

    void ltstar_cb(const lazy_theta_star_msgs::LTStarReply::ConstPtr& msg)
    {
        if(state_data.exploration_state.getState() != exploration_sm::waiting_path_response)
        {
            #ifdef SAVE_LOG
            log_file << "[State manager] Received path from Lazy Theta Star but the state is not waiting_path_response. Discarding." << std::endl;
            #endif
        }
        else if(msg->request_id != state_data.ltstar_request_id)
        {
            #ifdef SAVE_LOG
            log_file << "[State manager] Received path from Lazy Theta Star with request id " << msg->request_id << " but the current request_id is " << state_data.ltstar_request_id << ". Discarding." << std::endl;
            #endif
        }
        else
        {
            if(msg->success)
            {
                state_data.ltstar_reply = *msg;
                nav_msgs::Path flight_plan_request = generateFlightPlanRequest();
                state_data.new_map = true;
                state_data.exploration_state.switchState(exploration_sm::visit_waypoints);
                flight_plan_pub.publish(flight_plan_request);
                #ifdef SAVE_LOG
                    log_file << "[State manager] Path reply " << *msg << std::endl;
                    log_file << "[State manager] Flight plan request " << flight_plan_request << std::endl;
                #endif
            }
            else
            {
                ROS_WARN_STREAM (     "[State manager] Path reply failed!");
                state_data.exploration_state.switchState(exploration_sm::exploration_start);
                #ifdef SAVE_LOG
                    log_file << "[State manager] Path reply failed!" << std::endl;
                #endif
                findTarget();

            }
            #ifdef SAVE_CSV
                std::pair <double, double> millis_count = calculateTime();
                operation_start = std::chrono::high_resolution_clock::now();
                int success = 2;
                if (msg->success) success = 1;
                else success = 0;
                csv_file_success << millis_count.first << "," << success << "," << std::endl;
            #endif
        }
    }

    void flighPlan_cb(const std_msgs::Empty::ConstPtr& msg)
    {
        state_data.exploration_state.switchState(exploration_sm::exploration_start);
        findTarget();
    }

    void init_state_variables(state_manager_node::StateData& state_data, ros::NodeHandle& nh)
    {
        state_data.new_map = true;
        state_data.ltstar_request_id = 0;
        state_data.frontier_request_count = 0;
        geometry_msgs::Point start_position;
        start_position.x = 0;
        start_position.y = -6;
        start_position.z = 3;
        geometry_msgs::Pose start_pose;
        start_pose.position = start_position;
        std::vector<geometry_msgs::Pose> initial (1, start_pose);
        state_data.ltstar_reply.waypoints = initial;
    }

    void init_param_variables(ros::NodeHandle& nh)
    {
        double temp;
        nh.getParam("px4_loiter_radius", px4_loiter_radius);
        nh.getParam("odometry_error", odometry_error);
        error_margin = std::max(px4_loiter_radius, odometry_error);
        nh.getParam("path/max_time_secs", max_time_secs);
        nh.getParam("path/safety_margin", ltstar_safety_margin);

        float x, y, z;
        nh.getParam("geofence_min/x", x);
        nh.getParam("geofence_min/y", y);
        nh.getParam("geofence_min/z", z);
        geofence_min = octomath::Vector3  (x, y, z);

        nh.getParam("geofence_max/x", x);
        nh.getParam("geofence_max/y", y);
        nh.getParam("geofence_max/z", z);
        geofence_max = octomath::Vector3  (x, y, z);

        nh.getParam("uav_id", uav_id_);

        // Goal state machine
        nh.getParam("oppairs/sensing_distance", sensing_distance);
        nh.getParam("oppairs/distance_inFront", distance_inFront);
        nh.getParam("oppairs/distance_behind",  distance_behind);
        nh.getParam("oppairs/circle_divisions",  circle_divisions);
    }
}

int main(int argc, char **argv)
{
    auto timestamp_chrono = std::chrono::high_resolution_clock::now();
    std::time_t now_c = std::chrono::system_clock::to_time_t(timestamp_chrono - std::chrono::hours(24));
    // std::string timestamp (std::put_time(std::localtime(&now_c), "%F %T") );
    std::stringstream folder_name_stream;
    folder_name_stream << state_manager_node::folder_name+"/" << (std::put_time(std::localtime(&now_c), "%F %T") );
    std::string sym_link_name = state_manager_node::folder_name+"/current";

    boost::filesystem::create_directories(folder_name_stream.str());
    boost::filesystem::create_directory_symlink(folder_name_stream.str(), sym_link_name);

    ros::init(argc, argv, "state_manager");
    ros::NodeHandle nh;
    state_manager_node::init_param_variables(nh);
    // Service client
    state_manager_node::ltstar_status_cliente      = nh.serviceClient<lazy_theta_star_msgs::LTStarNodeStatus>   ("ltstar_status");
    state_manager_node::ask_for_goal_client        = nh.serviceClient<architecture_msgs::FindNextGoal>          ("find_next_goal");
    // Topic subscribers
    ros::Subscriber ltstar_reply_sub = nh.subscribe<lazy_theta_star_msgs::LTStarReply>("ltstar_reply", 5, state_manager_node::ltstar_cb);
    ros::Subscriber flighPlan_notifications_sub = nh.subscribe<std_msgs::Empty>("/uav_" + std::to_string(state_manager_node::uav_id_) + "/flight_plan_notifications", 5, state_manager_node::flighPlan_cb);
    // Topic publishers
    state_manager_node::ltstar_request_pub = nh.advertise<lazy_theta_star_msgs::LTStarRequest>("ltstar_request", 10);
    state_manager_node::marker_pub = nh.advertise<visualization_msgs::MarkerArray>("state_manager_viz", 1);
    state_manager_node::flight_plan_pub = nh.advertise<nav_msgs::Path>("/uav_" + std::to_string(state_manager_node::uav_id_) + "/flight_plan_requests", 1);

    #ifdef SAVE_LOG
    state_manager_node::log_file.open (state_manager_node::folder_name+"/current/state_manager.log", std::ofstream::app);
    #endif
    state_manager_node::init_state_variables(state_manager_node::state_data, nh);

    rviz_interface::PublishingInput pi(state_manager_node::marker_pub, true, "oppairs" );

    geometry_msgs::Point geofence_min_point, geofence_max_point;
    geofence_min_point.x = state_manager_node::geofence_min.x();
    geofence_min_point.y = state_manager_node::geofence_min.y();
    geofence_min_point.z = state_manager_node::geofence_min.z();
    geofence_max_point.x = state_manager_node::geofence_max.x();
    geofence_max_point.y = state_manager_node::geofence_max.y();
    geofence_max_point.z = state_manager_node::geofence_max.z();

    #ifdef SAVE_CSV
    state_manager_node::csv_file_success.open (state_manager_node::folder_name+"/current/success_rate.csv", std::ofstream::app);
    state_manager_node::csv_file_success << "timeline,path_planner,sampling" << std::endl;
    state_manager_node::operation_start = std::chrono::high_resolution_clock::now();
    state_manager_node::timeline_start = std::chrono::high_resolution_clock::now();
    #endif
    // state_manager_node::timer = nh.createTimer(ros::Duration(30), state_manager_node::main_loop);
    ros::spin();
    return 0;
}


#include <iostream>
#include <string>
#include <sstream>
#include <stdexcept>
#include <vector>
#include <map>
#include <stack>
#include <cmath>
#include <boost/regex.hpp>
#include <assert.h>
#include <random>
using namespace std;
//using namespace boost;
//Requires "-lboost_system -lboost_system" in compiler
//OLD
struct battle_data{
    //brook format: int move_Type, bool hostility, double hitchance, int damgage, int damage_type
    //move_type formating: Attack==0, Spell=1,Other=2; damage type formating: Pure: 0, Physical: 1, Magic: 2
    stringstream brook;
    vector<string> effects;
    string source, target;
    battle_data() : brook(), effects(), source(), target() {}
    battle_data(const battle_data &pain)
    : brook(pain.brook.str()), effects(pain.effects), source(pain.source), target(pain.target) {}
    battle_data(string trout, vector<string> fx, string origin, string down)
    : brook(trout), effects(fx), source(origin), target(down) {}
};

struct action{
    bool internal;
    string message, data;
    int speed;
    int priority;
    int id;
    //for internal only
    battle_data river;
    action(int spd, int prior, int idn, string msg, string info)
    : internal(false), speed(spd), priority(prior), id(idn), message(msg), data(info) {}
    action(const battle_data &plan, int idn) : river(plan), id(idn) {}
};
class stak : public std::stack<action>{
public:
    action loot(){
        action temp = this->top();
        this->pop();
        return temp;
    }
};
//TODO:Move above to own header
/*bool hit_check(const &chance){
    random_device rd;  //Will be used to obtain a seed for the random number engine
    mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
    uniform_real_distribution<> distr(0.0, 1.0);
    return (chance>=distr(gen));
}*/

void ext_parser(action act,stak &stax){
    string data = act.data;
    stringstream creek;
    vector<string> out_targets;
    string temp;
    string outsource;
    boost::smatch lighter;
    int num = 0;
    double odds = 0.0;
    boost::regex current("(?<=Source: )[^<]([^>])+");//Everthing precede by "Source "" any followed by "
    if(boost::regex_search(data, lighter, current)) outsource = lighter.str();
    else{
        cout << "Message: " << act.message << "\nThe data: $ " << data << endl;
        assert(false);
    }
    boost::regex ignorant("(?<=Source:)[^\$]+", boost::regex::icase);//to case that is
    current = "(?<=Type: )[[:alpha:]]+(?=\$)";
    if(boost::regex_search(data, lighter, current)){
        temp = lighter.str();
        ignorant = "attack";
        if(boost::regex_search(data, ignorant)) {creek << "0 "; goto type_end;}
        ignorant = "spell";
        if(boost::regex_search(data, ignorant)) {creek << "1 "; goto type_end;}
        ignorant = "other";
        if(boost::regex_search(data, ignorant)) {creek << "2 "; goto type_end;}
        //throw range_error("In ext_parser lin");
        cout << "Message: " << act.message << "   Temp: " << temp << "\nThe data: $ " << data << endl;
        assert(false);
    }
    else{
        cout << "Message: " << act.message << "\nThe data: $ " << data << endl;
        assert(false);
    }
    type_end:;
    current = "(?<=Hostility: )(true|false)(?=\$)";
    if(boost::regex_search(data, lighter, current)){
        temp = lighter.str();
        creek << temp << " ";
    }
    else{
        cout << "Message: " << act.message << "\nThe data: $ " << data << endl;
        assert(false);
    }
    current = "(?<=Hitchance: )[[:digit:].]+(?=\$)";
    if(boost::regex_search(data, lighter, current)){
        temp = lighter.str();
        odds = stod(temp);
    }
    else odds = 1.0;

    current = "(?<=DMG: )[[:digit:]]+(?=\$)";
    if(boost::regex_search(data, lighter, current)){
        temp = lighter.str();
        num = stoi(temp);
    }
    else num = 0;
    creek << num << " ";
    current = "(?<=Typing: )[^\$]+";
    if(num){
        if(boost::regex_search(data, lighter, current)){
            temp = lighter.str();
            ignorant = "pure";
            if(boost::regex_search(temp, ignorant)) {creek << "0 "; goto dmg_type_end;}
            ignorant = "phys";
            if(boost::regex_search(data, ignorant)) {creek << "1 "; goto dmg_type_end;}
            ignorant = "magic";
            if(boost::regex_search(data, ignorant)) {creek << "2 "; goto dmg_type_end;}
            cout << "Message: " << act.message << "   Temp: " << temp << "\nThe data: $ " << data << endl;
            assert(false);
        }
    }
    dmg_type_end:;
    current = "(?<=Targets: )[^\$]+";
    if(boost::regex_search(data, lighter,current)){
        temp = lighter.str();
        current ="(?<=\<)[^>]+";
        auto words_begin = boost::sregex_iterator(data.begin(), data.end(), current);
        auto words_end = boost::sregex_iterator();
        for (auto it = words_begin; it != words_end; ++it)
            out_targets.push_back(it->str());
    }
    data = creek.str();
    for(string s:out_targets){
        stax.push(action(battle_data(data,{}, outsource, s), act.id));
    }
}

int main(){

}


#include "fieldtrack/BodyFrameTracker.h"
#include "argus_utils/utils/MatrixUtils.h"
#include "argus_utils/geometry/GeometryUtils.h"

#include <boost/foreach.hpp>

using namespace argus_msgs;

namespace argus
{

typedef ConstantVelocityFilterSE3 CVF3;

BodyFrameTracker::BodyFrameTracker( const ros::NodeHandle& nh, const ros::NodeHandle& ph )
: nodeHandle( nh ), privHandle( ph ), targetManager( lookupInterface )
{
	if( !privHandle.getParam( "reference_frame", referenceFrame ) )
	{
		ROS_ERROR_STREAM( "Must specify reference frame." );
		exit( -1 );
	}

	std::string lookupNamespace;
	privHandle.param<std::string>( "lookup_namespace", lookupNamespace, "/lookup" );
	lookupInterface.SetLookupNamespace( lookupNamespace );

	targetPub = nodeHandle.advertise<CompactOdometryArray>( "target_states", 5 );
	refOdomSub = nodeHandle.subscribe( "reference_odometry",
	                                   10,
	                                   &BodyFrameTracker::OdometryCallback,
	                                   this );
	targetPoseSub = nodeHandle.subscribe( "target_pose",
	                                      10,
	                                      &BodyFrameTracker::TargetPoseCallback,
	                                      this );

	double updateRate;
	privHandle.param<double>( "update_rate", updateRate, 10.0 );
	updateTimer = std::make_shared<ros::Timer>
		( nodeHandle.createTimer( ros::Duration( 1.0/updateRate ),
		                          &BodyFrameTracker::TimerCallback,
		                          this ) );
}

void BodyFrameTracker::TimerCallback( const ros::TimerEvent& event )
{
	PoseSE3 displacement;
	PoseSE3::CovarianceMatrix displacementCov;
	if( lastOdometry )
	{
		double dt = ( event.current_real - event.last_real ).toSec();
		ReadLock lock( mutex );
		PoseSE3::TangentVector velocity = MsgToTangent( lastOdometry->twist.twist );
		PoseSE3::CovarianceMatrix velocityCov;
		ParseMatrix( lastOdometry->twist.covariance, velocityCov );
		lock.unlock();
		displacement = PoseSE3::Exp( dt * velocity );
		displacementCov = velocityCov * dt;
	}

	// Publish target states
	CompactOdometryArray::Ptr msg = boost::make_shared<CompactOdometryArray>();
	msg->header.stamp = event.current_real;
	msg->header.frame_id = referenceFrame;
	msg->odometry.reserve( targetRegistry.size() );

	BOOST_FOREACH( TargetRegistry::value_type& item, targetRegistry )
	{
		const std::string& name = item.first;
		TargetRegistration& registration = item.second;
		if( !registration.poseInitialized ) { continue; }

		// Update target pose
		if( lastOdometry )
		{
			// TODO This logic might be broken
			double dt = (event.current_real - registration.filterTime).toSec();
			registration.filter.Predict( registration.Qrate*dt, dt );
			registration.filter.WorldDisplace( displacement.Inverse(), displacementCov );
			registration.filterTime = event.current_real;
		}

		// Publish state
		CompactOdometry odom;
		odom.header.stamp = event.current_real;
		odom.header.frame_id = referenceFrame;
		odom.child_frame_id = name;
		odom.pose.pose = PoseToMsg( registration.filter.Pose() );
		SerializeSymmetricMatrix( registration.filter.PoseCov(),
		                          odom.pose.covariance );
		PoseSE3::TangentVector tangents = registration.filter.Derivs().head<6>();
		odom.twist.twist = TangentToMsg( tangents );
		SerializeSymmetricMatrix( registration.filter.DerivsCov().topLeftCorner<6,6>(),
		                          odom.twist.covariance );

		msg->odometry.push_back( odom );
	}

	targetPub.publish( msg );
}

void BodyFrameTracker::OdometryCallback( const nav_msgs::Odometry::ConstPtr& msg )
{
	WriteLock lock( mutex ); // TODO Technically not necessary since single-threaded
	lastOdometry = msg;
}

void BodyFrameTracker::TargetPoseCallback( const RelativePoseWithCovariance::ConstPtr& msg )
{
	if( msg->relative_pose.observer_name != referenceFrame )
	{
		ROS_WARN_STREAM( "Received relative pose with observer " << msg->relative_pose.observer_name
		                 << " instead of reference frame " << referenceFrame );
		return;
	}

	const std::string& name = msg->relative_pose.target_name;
	if( targetRegistry.count( name ) == 0 ) { RegisterTarget( name ); }

	TargetRegistration& registration = targetRegistry[ name ];
	PoseSE3 pose = MsgToPose( msg->relative_pose.relative_pose );
	PoseSE3::CovarianceMatrix poseCov;
	ParseSymmetricMatrix( msg->covariance, poseCov );

	if( registration.poseInitialized )
	{
		if( msg->header.stamp > registration.filterTime )
		{
			double dt = (msg->header.stamp - registration.filterTime).toSec();
			registration.filter.Predict( registration.Qrate * dt, dt );
			registration.filterTime = msg->header.stamp;
		}
		registration.filter.UpdatePose( pose, poseCov );
	}
	else
	{
		registration.filter.Pose() = pose;
		registration.filter.Derivs() = CVF3::DerivsType::Zero();
		registration.filter.FullCov() = 10 * CVF3::FullCovType::Identity();
		registration.filter.FullCov().topLeftCorner<CVF3::TangentDim,CVF3::TangentDim>()
			= poseCov;
		registration.poseInitialized = true;
		registration.filterTime = msg->header.stamp;
	}
}

void BodyFrameTracker::RegisterTarget( const std::string& name )
{
	if( targetRegistry.count( name ) > 0 ) { return; }

	ROS_INFO_STREAM( "Registering target " << name );

	// TODO Look up target information to set motion model
	targetRegistry[ name ];
}

BodyFrameTracker::TargetRegistration::TargetRegistration()
: poseInitialized( false ), filterTime( ros::Time::now() ),
filter( PoseSE3(), CVF3::DerivsType::Zero(), 10*CVF3::FullCovType::Identity() ),
Qrate( CVF3::FullCovType::Identity() ) {}

}


#include "random.h"
#include <iostream>
#include <boost/thread.hpp>

// random seeding
int64_t cluster_seedgen() {
  int64_t s, seed, pid;
  FILE* f = fopen("/dev/urandom", "rb");
  if (f && fread(&seed, 1, sizeof(seed), f) == sizeof(seed)) {
    fclose(f);
    return seed;
  }
  std::cout << "System entropy source not available, "
               "using fallback algorithm to generate seed instead.";
  if (f) fclose(f);
  pid  = getpid();
  s    = time(NULL);
  seed = std::abs(((s * 181) * ((pid - 83) * 359)) % 104729);
  return seed;
}

// Make sure each thread can have different values.
static boost::thread_specific_ptr<Context> thread_instance_;

Context& Context::Get() {
  if (!thread_instance_.get()) {
    thread_instance_.reset(new Context());
  }
  return *(thread_instance_.get());
}

Context::Context() : random_generator_() {}

Context::~Context() { }

void Context::set_random_seed(const unsigned int seed) {
  Get().random_generator_.reset(new RNG(seed));
}

class Context::RNG::Generator {
 public:
  Generator() : rng_(new rng_t(cluster_seedgen())) {}
  explicit Generator(unsigned int seed) : rng_(new rng_t(seed)) {}
  rng_t* rng() { return rng_.get(); }
 private:
  shared_ptr<rng_t> rng_;
};

Context::RNG::RNG() : generator_(new Generator()) { }

Context::RNG::RNG(unsigned int seed) : generator_(new Generator(seed)) { }

Context::RNG& Context::RNG::operator=(const RNG& other) {
  generator_ = other.generator_;
  return *this;
}

void* Context::RNG::generator() {
  return static_cast<void*>(generator_->rng());
}

#ifdef ENABLE_GPU
#include "gpu_context.h"
gpu_context gpu_ctx;
cublasHandle_t gpu_context::cublas_handle_        = 0;
cusparseHandle_t gpu_context::cusparse_handle_    = 0;
cusparseMatDescr_t gpu_context::cusparse_matdescr_ = 0;
curandGenerator_t gpu_context::curand_generator_   = 0;

gpu_context::gpu_context() {
  CUBLAS_CHECK(cublasCreate(&cublas_handle_));
  CUSPARSE_CHECK(cusparseCreate(&cusparse_handle_));
  CUSPARSE_CHECK(cusparseCreateMatDescr(&cusparse_matdescr_));
  CUSPARSE_CHECK(cusparseSetMatType(cusparse_matdescr_, CUSPARSE_MATRIX_TYPE_GENERAL));
  CUSPARSE_CHECK(cusparseSetMatIndexBase(cusparse_matdescr_, CUSPARSE_INDEX_BASE_ZERO));
  CURAND_CHECK(curandCreateGenerator(&curand_generator_, CURAND_RNG_PSEUDO_DEFAULT));
  //CURAND_CHECK(curandSetPseudoRandomGeneratorSeed(curand_generator_, cluster_seedgen()));
  CURAND_CHECK(curandSetPseudoRandomGeneratorSeed(curand_generator_, 1));
}
#endif


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.
#include "poplin/MultiConvolution.hpp"
#include <boost/assign/list_of.hpp>
#include <boost/optional.hpp>
#include <boost/program_options.hpp>
#include <boost/version.hpp>
#include <iostream>
#include <poplibs_support/TestDevice.hpp>
#include <poplibs_support/VectorUtils.hpp>
#include <poplibs_test/Convolution.hpp>
#include <poplibs_test/Util.hpp>
#include <poplin/codelets.hpp>
#include <popops/codelets.hpp>
#include <sstream>
using namespace poplibs_support;

int main(int argc, char **argv) try {
  namespace po = boost::program_options;

  DeviceType deviceType;
  boost::optional<unsigned> tilesPerIPU;
  std::vector<std::string> convs;
  bool transposeAndFlipWeights;
  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
    ("help,h", "produce help message")
    ("compile-only", "Stop after compilation; don't run the program")
    ("device-type",
      po::value<DeviceType>(&deviceType)->default_value(DeviceType::IpuModel2),
      deviceTypeHelp)
    ("profile", "Output profiling report to standard output")
    ("ignore-data", "Don't upload and download the results from the device. "
     "Note that this means the result is not validated against the model.")
    ("tiles-per-ipu", po::value(&tilesPerIPU), "Number of tiles per IPU")
    ("conv", po::value<std::vector<std::string>>(&convs)
      ->default_value(boost::assign::list_of(""), "")->composing(),
     "parameters for a convolution used in the multiconv")
    ("transpose-flip-weights",
     po::value<bool>(&transposeAndFlipWeights)->default_value(false),
     "whether or not to transpose and flip the weights before each convolution")
    ;
  // clang-format on

  po::variables_map vm;
  try {
    const po::positional_options_description p;
    po::store(
        po::command_line_parser(argc, argv).options(desc).positional(p).run(),
        vm);
    po::notify(vm);

    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 1;
    }
  } catch (const boost::program_options::error &e) {
    std::cerr << e.what() << std::endl;
    return 1;
  }

  const bool profile = deviceType != DeviceType::Cpu && vm.count("profile");
  const bool ignoreData = vm.count("ignore-data");

  std::cout << "got " << convs.size() << " convs" << std::endl;

  const unsigned numIPUs = 1;
  const bool compileIPUCode = true;
  auto device =
      tilesPerIPU
          ? createTestDevice(deviceType, numIPUs, *tilesPerIPU, compileIPUCode)
          : createTestDeviceFullSize(deviceType, numIPUs, compileIPUCode);

  const auto &target = device.getTarget();
  poplar::Graph graph(target);
  poplin::addCodelets(graph);
  popops::addCodelets(graph);

  poplin::PlanningCache cache;
  poplar::program::Sequence uploadProg, prog, downloadProg;

  const poplar::OptionFlags multiConvOptions;
  const std::vector<poplar::OptionFlags> options(convs.size());
  std::vector<poplin::ConvParams> params;
  for (const auto &conv : convs) {
    std::stringstream ss;
    ss << conv;
    poplin::ConvParams p;
    ss >> p;

    params.push_back(std::move(p));
  }

  std::vector<poplin::multiconv::CreateTensorArgs> createInputArgs;
  std::vector<poplin::multiconv::CreateTensorArgs> createWeightsArgs;
  for (unsigned i = 0; i < params.size(); ++i) {
    createInputArgs.push_back(
        {params[i], options[i], "convInput_" + std::to_string(i)});
    createWeightsArgs.push_back(
        {params[i], options[i], "convWeights_" + std::to_string(i)});
  }

  std::vector<poplin::multiconv::ConvolutionArgs> convolutionArgs;
  for (unsigned i = 0; i < params.size(); ++i) {
    auto input = poplin::multiconv::createInput(graph, createInputArgs, i,
                                                multiConvOptions, &cache);
    auto weights = poplin::multiconv::createWeights(graph, createWeightsArgs, i,
                                                    multiConvOptions, &cache);

    convolutionArgs.push_back(
        {std::move(input), std::move(weights), params[i], options[i]});
  }

  const auto outs = poplin::multiconv::convolution(
      graph, convolutionArgs, transposeAndFlipWeights, prog, "multiConv",
      multiConvOptions);

  std::vector<std::pair<std::string, char *>> tmap;
  std::vector<std::unique_ptr<char[]>> rawHostInputs, rawHostWeights,
      rawHostOutputs;
  if (!ignoreData) {
    for (unsigned i = 0; i < convolutionArgs.size(); ++i) {
      auto rawHostInput = poplibs_test::util::allocateHostMemoryForTensor(
          convolutionArgs[i].inputs, createInputArgs[i].name, graph, uploadProg,
          boost::none, tmap);
      rawHostInputs.push_back(std::move(rawHostInput));

      auto rawHostWeight = poplibs_test::util::allocateHostMemoryForTensor(
          convolutionArgs[i].weights, createWeightsArgs[i].name, graph,
          uploadProg, boost::none, tmap);
      rawHostWeights.push_back(std::move(rawHostWeight));

      auto rawHostOutput = poplibs_test::util::allocateHostMemoryForTensor(
          outs[i], "output_" + std::to_string(i), graph, boost::none,
          downloadProg, tmap);
      rawHostOutputs.push_back(std::move(rawHostOutput));
    }
  }

  const poplar::OptionFlags engineOptions;
  poplar::Engine engine(graph, {uploadProg, prog, downloadProg}, engineOptions);

  if (vm.count("compile-only"))
    return 0;

  std::vector<boost::multi_array<double, 3>> hostInputs;
  std::vector<boost::multi_array<double, 4>> hostWeights;
  std::vector<boost::multi_array<double, 3>> modelOutputs;
  if (!ignoreData) {
    poplibs_test::util::attachStreams(engine, tmap);

    std::mt19937 randomEngine;
    for (unsigned i = 0; i < convolutionArgs.size(); ++i) {
      const auto &p = createInputArgs[i].params;
      const auto inChannels = p.inputChannelsPerConvGroup * p.numConvGroups;
      const auto outChannels = p.outputChannelsPerConvGroup * p.numConvGroups;

      hostInputs.emplace_back(
          boost::extents[p.batchSize][inChannels][product(p.inputFieldShape)]);
      poplibs_test::util::writeRandomBinaryValues(
          target, p.inputType, hostInputs.back(), -1.0, 1.0, randomEngine);
      poplibs_test::util::copy(target, hostInputs.back(), p.inputType,
                               rawHostInputs[i].get());

      hostWeights.emplace_back(
          boost::extents[p.numConvGroups][p.outputChannelsPerConvGroup]
                        [p.inputChannelsPerConvGroup][product(p.kernelShape)]);
      poplibs_test::util::writeRandomBinaryValues(
          target, p.inputType, hostWeights.back(), -1.0, 1.0, randomEngine);
      poplibs_test::util::copy(target, hostWeights.back(), p.inputType,
                               rawHostWeights[i].get());

      // build a reference model to validate against
      boost::multi_array<double, 1> biases(boost::extents[outChannels]);
      std::fill(biases.data(), biases.data() + biases.num_elements(), 0.0);

      const auto outFieldShape = p.getOutputFieldShape();
      modelOutputs.emplace_back(
          boost::extents[p.batchSize][outChannels][product(outFieldShape)]);

      poplibs_test::conv::convolution(
          vectorConvert<unsigned>(p.inputFieldShape),
          p.inputTransform.truncationLower, p.inputTransform.truncationUpper,
          p.inputTransform.dilation, p.inputTransform.paddingLower,
          p.inputTransform.paddingUpper, p.inputTransform.flip,
          vectorConvert<unsigned>(p.kernelShape),
          p.kernelTransform.truncationLower, p.kernelTransform.truncationUpper,
          p.kernelTransform.dilation, p.kernelTransform.paddingLower,
          p.kernelTransform.paddingUpper, p.kernelTransform.flip,
          p.outputTransform.truncationLower, p.outputTransform.truncationUpper,
          p.outputTransform.stride, p.outputTransform.paddingLower,
          p.outputTransform.paddingUpper, hostInputs.back(), hostWeights.back(),
          biases, modelOutputs.back());
    }
  }

  device.bind([&](const poplar::Device &d) {
    engine.load(d);
    if (!ignoreData) {
      // upload
      engine.run(0);
    }

    // convolve
    engine.run(1);

    if (!ignoreData) {
      // download
      engine.run(2);
    }
  });

  bool matchesModel = true;
  if (!ignoreData) {
    for (unsigned i = 0; i < convolutionArgs.size(); ++i) {
      const auto &p = convolutionArgs[i].params;
      const auto outFieldShape = p.getOutputFieldShape();
      const auto outChannels = p.outputChannelsPerConvGroup * p.numConvGroups;

      boost::multi_array<double, 3> hostOutput(
          boost::extents[p.batchSize][outChannels][product(outFieldShape)]);
      poplibs_test::util::copy(target, p.outputType, rawHostOutputs[i].get(),
                               hostOutput);

      const auto tolerance = 0.0;
      matchesModel &= poplibs_test::util::checkIsClose(
          "conv_" + std::to_string(i), hostOutput, modelOutputs[i], tolerance,
          tolerance);
    }
  }

  if (profile) {
    engine.printProfileSummary(std::cout, {{"showExecutionSteps", "true"}});
  }

  if (!matchesModel) {
    std::cerr << "Validation failed\n";
    return 1;
  }

  return 0;
} catch (const poplar::graph_memory_allocation_error &e) {
  std::cerr << e.what() << std::endl;

  // this exit code has been marked as a "skip" for ctest.
  return 77;
}


/**
 * ****************************************************************************
 * Copyright (c) 2015, Robert Lukierski.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * Neither the name of the copyright holder nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * ****************************************************************************
 * Gaussian distribution.
 * ****************************************************************************
 */
#ifndef VISIONCORE_TYPES_GAUSSIAN_HPP
#define VISIONCORE_TYPES_GAUSSIAN_HPP

#include <VisionCore/Platform.hpp>

#include <Eigen/Dense>

namespace vc
{
namespace types
{
    template<typename T> class Gaussian;
    template<typename _Scalar, int _Dimension = 1> class MultivariateGaussian;
}
}

namespace Eigen
{
    namespace internal
    {
        // Gaussian of scalars or Eigens
        template<typename T>
        struct traits< vc::types::Gaussian<T> >
        {
            static constexpr int Rows = 1;
            static constexpr int Cols = 1;
            static constexpr int Dimension = Rows * Cols;
            typedef T Scalar;
            typedef Scalar MeanType;
        };

        template<typename _Scalar, int _Rows, int _Cols>
        struct traits<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols> > >
        {
            static constexpr int Rows = _Rows;
            static constexpr int Cols = _Cols;
            static constexpr int Dimension = Rows * Cols;
            typedef _Scalar Scalar;
            typedef Matrix<Scalar,_Rows,_Cols> MeanType;
        };

        template<typename _Scalar, int _Rows, int _Cols, int _Options>
        struct traits<Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols> >, _Options> > :
            traits<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols> > >
        {
            static constexpr int Rows = _Rows;
            static constexpr int Cols = _Cols;
            static constexpr int Dimension = Rows * Cols;
            typedef _Scalar Scalar;
            typedef Map<Matrix<Scalar,_Rows,_Cols>, _Options> MeanType;
        };

        template<typename _Scalar, int _Rows, int _Cols, int _Options>
        struct traits<Map<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols> >, _Options> > :
            traits<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols> > >
        {
            static constexpr int Rows = _Rows;
            static constexpr int Cols = _Cols;
            static constexpr int Dimension = Rows * Cols;
            typedef _Scalar Scalar;
            typedef Map<const Matrix<Scalar,_Rows,_Cols>, _Options> MeanType;
        };

        // Multivariate
        template<typename _Scalar, int _Dimension>
        struct traits<vc::types::MultivariateGaussian<_Scalar,_Dimension> >
        {
            static constexpr int Dimension = _Dimension;
            typedef _Scalar Scalar;
            typedef Matrix<Scalar,_Dimension,1> MeanType;
            typedef Matrix<Scalar,_Dimension,_Dimension> CovarianceType;
        };

        template<typename _Scalar, int _Dimension, int _Options>
        struct traits<Map<vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options> > :
            traits<vc::types::MultivariateGaussian<_Scalar, _Dimension> >
        {
            static constexpr int Dimension = _Dimension;
            typedef _Scalar Scalar;
            typedef Map<Matrix<Scalar,_Dimension,1>, _Options> MeanType;
            typedef Map<Matrix<Scalar,_Dimension,_Dimension>, _Options> CovarianceType;
        };

        template<typename _Scalar, int _Dimension, int _Options>
        struct traits<Map<const vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options> > :
            traits<const vc::types::MultivariateGaussian<_Scalar, _Dimension> >
        {
            static constexpr int Dimension = _Dimension;
            typedef _Scalar Scalar;
            typedef Map<const Matrix<Scalar,_Dimension,1>, _Options> MeanType;
            typedef Map<const Matrix<Scalar,_Dimension,_Dimension>, _Options> CovarianceType;
        };

    }
}

namespace vc
{

namespace types
{

/**
 * Type agnostic Gaussian operations.
 */
template<typename Derived>
class GaussianBase
{
public:
    static constexpr int Dimension = Eigen::internal::traits<Derived>::Dimension;
    static constexpr int Rows = Eigen::internal::traits<Derived>::Rows;
    static constexpr int Cols = Eigen::internal::traits<Derived>::Cols;
    typedef typename Eigen::internal::traits<Derived>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Derived>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Derived>::MeanType ConstMeanType;

    EIGEN_DEVICE_FUNC MeanType& mean()
    {
        return static_cast<Derived*>(this)->mean_nonconst();
    }

    EIGEN_DEVICE_FUNC const MeanType& mean() const
    {
        return static_cast<const Derived*>(this)->mean_const();
    }

    EIGEN_DEVICE_FUNC MeanType& variance()
    {
        return static_cast<Derived*>(this)->variance_nonconst();
    }

    EIGEN_DEVICE_FUNC const MeanType& variance() const
    {
        return static_cast<const Derived*>(this)->variance_const();
    }

    template<typename OtherDerived>
    EIGEN_DEVICE_FUNC inline GaussianBase<Derived>& operator=(const GaussianBase<OtherDerived>& other)
    {
        mean() = other.mean();
        variance() = other.variance();
        return *this;
    }

#if 0
    EIGEN_DEVICE_FUNC inline GaussianBase<Derived> operator+(const GaussianBase<Derived>& other) const
    {
    // TBD
    }

    EIGEN_DEVICE_FUNC inline GaussianBase<Derived> operator-(const GaussianBase<Derived>& other) const
    {
    // TBD
    }

    template<typename OtherScalar>
    EIGEN_DEVICE_FUNC inline GaussianBase<Derived> operator*(const OtherScalar& other) const
    {
    // TBD
    }

    template<typename OtherScalar>
    EIGEN_DEVICE_FUNC inline GaussianBase<Derived> operator/(const OtherScalar& other) const
    {
    // TBD
    }
#endif

#ifdef VISIONCORE_ENABLE_CEREAL
    template<typename Archive>
    void load(Archive & archive, std::uint32_t const version)
    {
        archive(cereal::make_nvp("Mean", mean()));
        archive(cereal::make_nvp("Variance", variance()));
    }

    template<typename Archive>
    void save(Archive & archive, std::uint32_t const version) const
    {
        archive(cereal::make_nvp("Mean", mean()));
        archive(cereal::make_nvp("Variance", variance()));
    }
#endif // VISIONCORE_ENABLE_CEREAL
};

/**
 * Eigen operations for Gaussian.
 */
template<typename Derived, int _Dimension>
class GaussianDispatchingBase : public GaussianBase<Derived>
{
    typedef GaussianBase<Derived> Base;
    friend class GaussianBase<Derived>;
public:
    static constexpr int Dimension = Eigen::internal::traits<Derived>::Dimension;
    static constexpr int Rows = Eigen::internal::traits<Derived>::Rows;
    static constexpr int Cols = Eigen::internal::traits<Derived>::Cols;
    typedef typename Eigen::internal::traits<Derived>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Derived>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Derived>::MeanType ConstMeanType;

    using Base::operator=;

    template<typename _NewScalarType>
    EIGEN_DEVICE_FUNC inline Gaussian<Eigen::Matrix<_NewScalarType, Rows, Cols> >  cast() const
    {
        return Gaussian<Eigen::Matrix<_NewScalarType, Rows, Cols>>(Base::mean().template cast<_NewScalarType>(),
                                                                   Base::variance().template cast<_NewScalarType>());
    }

    EIGEN_DEVICE_FUNC inline MeanType evaluate(const MeanType& x)
    {
        using Eigen::numext::sqrt;
        using Eigen::numext::exp;
        using Eigen::numext::pow;

        const MeanType inv_sqrt_2pi_s2 = Scalar(1.0) / sqrt(Scalar(2.0 * M_PI) * Base::variance().array());
        const MeanType a = (x - Base::mean());
        return inv_sqrt_2pi_s2.array() * exp(-(pow(a.array(), 2))/(Scalar(2.0) * Base::variance().array()));
    }

    EIGEN_DEVICE_FUNC inline void setZero()
    {
        Base::mean() = MeanType::Zero();
        Base::variance() = MeanType::Zero();
    }

    EIGEN_DEVICE_FUNC MeanType stddev() const
    {
        return sqrt(Base::variance().array());
    }
};

/**
 * Scalar operations for Gaussian.
 */
template<typename Derived>
class GaussianDispatchingBase<Derived, 1> : public GaussianBase<Derived>
{
    typedef GaussianBase<Derived> Base;
    friend class GaussianBase<Derived>;
public:
    static constexpr int Dimension = Eigen::internal::traits<Derived>::Dimension;
    static constexpr int Rows = Eigen::internal::traits<Derived>::Rows;
    static constexpr int Cols = Eigen::internal::traits<Derived>::Cols;
    typedef typename Eigen::internal::traits<Derived>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Derived>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Derived>::MeanType ConstMeanType;

    using Base::operator=;

    template<typename NewScalarType>
    EIGEN_DEVICE_FUNC inline Gaussian<NewScalarType> cast() const
    {
        return Gaussian<NewScalarType>((NewScalarType)Base::mean(), (NewScalarType)Base::variance());
    }

    EIGEN_DEVICE_FUNC inline MeanType evaluate(const MeanType& x)
    {
        using Eigen::numext::sqrt;
        using Eigen::numext::exp;

        const Scalar inv_sqrt_2pi_s2 = Scalar(1.0) / sqrt(Scalar(2.0 * M_PI) * Base::variance());
        const Scalar a = (x - Base::mean());
        return inv_sqrt_2pi_s2 * exp(-(a * a)/(Scalar(2.0) * Base::variance()));
    }

    EIGEN_DEVICE_FUNC inline void setZero()
    {
        Base::mean() = Scalar(0.0);
        Base::variance() = Scalar(0.0);
    }

    EIGEN_DEVICE_FUNC MeanType stddev() const
    {
        using Eigen::numext::sqrt;
        return sqrt(Base::variance());
    }
};

/**
 * Multivariate Gaussian Base.
 */
template<typename Derived>
class MultivariateGaussianBase
{
public:
    static constexpr int Dimension = Eigen::internal::traits<Derived>::Dimension;
    typedef typename Eigen::internal::traits<Derived>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Derived>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Derived>::MeanType ConstMeanType;
    typedef typename Eigen::internal::traits<Derived>::CovarianceType CovarianceType;
    typedef const typename Eigen::internal::traits<Derived>::CovarianceType ConstCovarianceType;

    template<typename NewScalarType>
    EIGEN_DEVICE_FUNC inline MultivariateGaussian<NewScalarType,Dimension> cast() const
    {
        return MultivariateGaussian<NewScalarType,Dimension>(mean().template cast<NewScalarType>(),
                                                             covariance().template cast<NewScalarType>());
    }

    EIGEN_DEVICE_FUNC MeanType& mean()
    {
        return static_cast<Derived*>(this)->mean_nonconst();
    }

    EIGEN_DEVICE_FUNC const MeanType& mean() const
    {
        return static_cast<Derived*>(this)->mean_const();
    }

    EIGEN_DEVICE_FUNC CovarianceType& covariance()
    {
        return static_cast<Derived*>(this)->covariance_nonconst();
    }

    EIGEN_DEVICE_FUNC const CovarianceType& covariance() const
    {
        return static_cast<Derived*>(this)->covariance_const();
    }

    template<typename OtherDerived>
    EIGEN_DEVICE_FUNC inline MultivariateGaussianBase<Derived>& operator=(const MultivariateGaussianBase<OtherDerived>& other)
    {
        mean() = other.mean();
        covariance() = other.covariance();
        return *this;
    }

#if 0
EIGEN_DEVICE_FUNC inline MultivariateGaussianBase<Derived> operator+(const MultivariateGaussianBase<Derived>& other) const
    {
        // TBD
    }

    EIGEN_DEVICE_FUNC inline MultivariateGaussianBase<Derived> operator-(const MultivariateGaussianBase<Derived>& other) const
    {
        // TBD
    }

    template<typename OtherScalar>
    EIGEN_DEVICE_FUNC inline MultivariateGaussianBase<Derived> operator*(const OtherScalar& other) const
    {
        // TBD
    }

    template<typename OtherScalar>
    EIGEN_DEVICE_FUNC inline MultivariateGaussianBase<Derived> operator/(const OtherScalar& other) const
    {
        // TBD
    }
#endif

    EIGEN_DEVICE_FUNC inline Scalar evaluate(const MeanType& x)
    {
        using Eigen::numext::sqrt;
        using Eigen::numext::pow;
        using Eigen::numext::exp;

        auto term1 = x - mean();
        auto det = covariance().determinant();
        auto part1 = (Scalar(1.0) / ( sqrt(pow(Scalar(2.0 * M_PI), Scalar(Dimension)) * det) ) );
        auto part2 = exp(Scalar(-0.5) * term1.transpose() * covariance().inverse() * term1);

        return part1 * part2;
    }

    EIGEN_DEVICE_FUNC inline void setZero()
    {
        mean().setZero();
        covariance().setZero();
    }

#ifdef VISIONCORE_ENABLE_CEREAL
    template<typename Archive>
    void load(Archive & archive, std::uint32_t const version)
    {
        archive(cereal::make_nvp("Mean", mean()));
        archive(cereal::make_nvp("Covariance", covariance()));
    }

    template<typename Archive>
    void save(Archive & archive, std::uint32_t const version) const
    {
        archive(cereal::make_nvp("Mean", mean()));
        archive(cereal::make_nvp("Covariance", covariance()));
    }
#endif // VISIONCORE_ENABLE_CEREAL
};

/**
 * Gaussian distribution.
 */
template<typename T>
class Gaussian : public GaussianDispatchingBase<Gaussian<T>, Eigen::internal::traits<Gaussian<T> >::Dimension>
{
    typedef GaussianDispatchingBase<Gaussian<T>, Eigen::internal::traits<Gaussian<T> >::Dimension> Base;
public:
    static constexpr int Dimension = Eigen::internal::traits<Gaussian>::Dimension;
    static constexpr int Rows = Eigen::internal::traits<Gaussian>::Rows;
    static constexpr int Cols = Eigen::internal::traits<Gaussian>::Cols;
    typedef typename Eigen::internal::traits<Gaussian>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Gaussian>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Gaussian>::MeanType ConstMeanType;

    friend class GaussianDispatchingBase<Gaussian<T>, Eigen::internal::traits<Gaussian<T> >::Dimension>;
    friend class GaussianBase<Gaussian<T> >;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    using Base::operator=;
#if 0
    using Base::operator*;
    using Base::operator+;
    using Base::operator-;
    using Base::operator/;
#endif

    EIGEN_DEVICE_FUNC inline Gaussian()
    {
    }

    template<typename OtherDerived>
    EIGEN_DEVICE_FUNC inline Gaussian(const GaussianDispatchingBase<GaussianBase<OtherDerived>,
                                      Eigen::internal::traits<Gaussian>::Dimension>& other)
        : mean_(other.mean()), variance_(other.variance())
    {
    }

    EIGEN_DEVICE_FUNC inline Gaussian(ConstMeanType& mean, ConstMeanType& variance)
        : mean_(mean), variance_(variance)
    {
    }

    EIGEN_DEVICE_FUNC inline ~Gaussian()
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline ConstMeanType& mean_const() const { return mean_; }
    EIGEN_DEVICE_FUNC inline MeanType& mean_nonconst() { return mean_; }

    EIGEN_DEVICE_FUNC inline ConstMeanType& variance_const() const { return variance_; }
    EIGEN_DEVICE_FUNC inline MeanType& variance_nonconst() { return variance_; }

    MeanType mean_;
    MeanType variance_;
};

template<typename T>
inline std::ostream& operator<<(std::ostream& os, const Gaussian<T>& p)
{
    os << "N(" << p.mean() << "," << p.stddev() <<  ")";
    return os;
}

/**
 * Multivariate Gaussian distribution.
 */
template<typename _Scalar, int _Dimension>
class MultivariateGaussian : public MultivariateGaussianBase<MultivariateGaussian<_Scalar,_Dimension>>
{
    typedef MultivariateGaussianBase<MultivariateGaussian<_Scalar,_Dimension>> Base;
public:
    static constexpr int Dimension = Eigen::internal::traits<MultivariateGaussian>::Dimension;
    typedef typename Eigen::internal::traits<MultivariateGaussian>::Scalar Scalar;
    typedef typename Eigen::internal::traits<MultivariateGaussian>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<MultivariateGaussian>::MeanType ConstMeanType;
    typedef typename Eigen::internal::traits<MultivariateGaussian>::CovarianceType CovarianceType;
    typedef const typename Eigen::internal::traits<MultivariateGaussian>::CovarianceType ConstCovarianceType;

    friend class vc::types::MultivariateGaussianBase<MultivariateGaussian<_Scalar,_Dimension>>;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    using Base::operator=;
#if 0
    using Base::operator*;
    using Base::operator+;
    using Base::operator-;
    using Base::operator/;
#endif

    EIGEN_DEVICE_FUNC inline MultivariateGaussian()
    {
    }

    template<typename OtherDerived>
    EIGEN_DEVICE_FUNC inline MultivariateGaussian(const MultivariateGaussianBase<OtherDerived>& other)
        : mean_(other.mean()), covariance_(other.covariance())
    {
    }

    EIGEN_DEVICE_FUNC inline MultivariateGaussian(ConstMeanType& mean, ConstCovarianceType& covariance)
        : mean_(mean), covariance_(covariance)
    {
    }

    EIGEN_DEVICE_FUNC inline ~MultivariateGaussian()
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline ConstMeanType& mean_const() const { return mean_; }
    EIGEN_DEVICE_FUNC inline MeanType& mean_nonconst() { return mean_; }

    EIGEN_DEVICE_FUNC inline ConstCovarianceType& covariance_const() const { return covariance_; }
    EIGEN_DEVICE_FUNC inline CovarianceType& covariance_nonconst() { return covariance_; }

    MeanType mean_;
    CovarianceType covariance_;
};

template<typename _Scalar, int _Dimension>
inline std::ostream& operator<<(std::ostream& os, const MultivariateGaussian<_Scalar,_Dimension>& p)
{
    os << "MultivariateGaussian(" << p.mean() << "," << p.covariance() <<  ")";
    return os;
}

}

}

namespace Eigen
{
/**
 * Specialisation of Eigen::Map for Gaussian.
 */
template<typename _Scalar, int _Rows, int _Cols, int _Options>
class Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>
    : public vc::types::GaussianDispatchingBase<
        Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>,
            Eigen::internal::traits<Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>>::Dimension>
{
    typedef vc::types::GaussianDispatchingBase<
        Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>,
            Eigen::internal::traits<Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>>::Dimension> Base;
public:
    static constexpr int Dimension = Eigen::internal::traits<Map>::Dimension;
    typedef typename Eigen::internal::traits<Map>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Map>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Map>::MeanType ConstMeanType;

    friend class vc::types::GaussianDispatchingBase<
                Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>,
                Eigen::internal::traits<Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>>::Dimension>;
    friend class vc::types::GaussianBase<Map<vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options> >;

    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
#if 0
    using Base::operator*;
    using Base::operator+;
    using Base::operator-;
    using Base::operator/;
#endif

    EIGEN_DEVICE_FUNC inline Map(Scalar* coeffs) : mean_(coeffs), variance_(coeffs + Dimension)
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline ConstMeanType& mean_const() const { return mean_; }
    EIGEN_DEVICE_FUNC inline MeanType& mean_nonconst() { return mean_; }

    EIGEN_DEVICE_FUNC inline ConstMeanType& variance_const() const { return variance_; }
    EIGEN_DEVICE_FUNC inline MeanType& variance_nonconst() { return variance_; }

    MeanType mean_;
    MeanType variance_;
};

/**
 * Specialisation of Eigen::Map for const Gaussian.
 */
template<typename _Scalar, int _Rows, int _Cols, int _Options>
class Map<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>
    : public vc::types::GaussianDispatchingBase<
        Map<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>,
          Eigen::internal::traits<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>>::Dimension>
{
    typedef vc::types::GaussianDispatchingBase<
      Map<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>,
      Eigen::internal::traits<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>>::Dimension>  Base;
public:
    static constexpr int Dimension = Eigen::internal::traits<Map>::Dimension;
    typedef typename Eigen::internal::traits<Map>::Scalar Scalar;
    typedef const typename Eigen::internal::traits<Map>::MeanType ConstMeanType;

    friend class vc::types::GaussianDispatchingBase<
      Map<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options>,
      Eigen::internal::traits<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>>::Dimension>;
    friend class vc::types::GaussianBase<Map<const vc::types::Gaussian<Matrix<_Scalar, _Rows, _Cols>>, _Options> >;

    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
#if 0
    using Base::operator*;
    using Base::operator+;
    using Base::operator-;
    using Base::operator/;
#endif

    EIGEN_DEVICE_FUNC inline Map(const Scalar* coeffs) : mean_(coeffs), variance_(coeffs + Dimension)
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline ConstMeanType& mean_const() const { return mean_; }
    EIGEN_DEVICE_FUNC inline ConstMeanType& variance_const() const { return variance_; }

    ConstMeanType mean_;
    ConstMeanType variance_;
};

/**
 * Specialisation of Eigen::Map for Multivariate Gaussian.
 */
template<typename _Scalar, int _Dimension, int _Options>
class Map<vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options>
  : public vc::types::MultivariateGaussianBase<
    Map<vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options> >
{
    typedef vc::types::MultivariateGaussianBase<Map<vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options>> Base;

public:
    static constexpr int Dimension = Eigen::internal::traits<Map>::Dimension;
    typedef typename Eigen::internal::traits<Map>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Map>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Map>::MeanType ConstMeanType;
    typedef typename Eigen::internal::traits<Map>::CovarianceType CovarianceType;
    typedef const typename Eigen::internal::traits<Map>::CovarianceType ConstCovarianceType;

    friend class vc::types::MultivariateGaussianBase<Map<vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options> >;

    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
#if 0
    using Base::operator*;
    using Base::operator+;
    using Base::operator-;
    using Base::operator/;
#endif

    EIGEN_DEVICE_FUNC inline Map(Scalar* coeffs) : mean_(coeffs), covariance_(coeffs + _Dimension)
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline ConstMeanType& mean_const() const { return mean_; }
    EIGEN_DEVICE_FUNC inline MeanType& mean_nonconst() { return mean_; }

    EIGEN_DEVICE_FUNC inline ConstCovarianceType& covariance_const() const { return covariance_; }
    EIGEN_DEVICE_FUNC inline CovarianceType& covariance_nonconst() { return covariance_; }

    MeanType mean_;
    CovarianceType covariance_;
};

/**
 * Specialisation of Eigen::Map for const Multivariate Gaussian.
 */
template<typename _Scalar, int _Dimension, int _Options>
class Map<const vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options>
  : public vc::types::MultivariateGaussianBase<
    Map<const vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options> >
{
    typedef vc::types::MultivariateGaussianBase<Map<const vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options> > Base;

public:
    static constexpr int Dimension = Eigen::internal::traits<Map>::Dimension;
    typedef typename Eigen::internal::traits<Map>::Scalar Scalar;
    typedef typename Eigen::internal::traits<Map>::MeanType MeanType;
    typedef const typename Eigen::internal::traits<Map>::MeanType ConstMeanType;
    typedef typename Eigen::internal::traits<Map>::CovarianceType CovarianceType;
    typedef const typename Eigen::internal::traits<Map>::CovarianceType ConstCovarianceType;

    friend class vc::types::MultivariateGaussianBase<Map<const vc::types::MultivariateGaussian<_Scalar,_Dimension>, _Options> >;

    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
#if 0
    using Base::operator*;
    using Base::operator+;
    using Base::operator-;
    using Base::operator/;
#endif

    EIGEN_DEVICE_FUNC inline Map(const Scalar* coeffs) : mean_(coeffs), covariance_(coeffs + _Dimension)
    {
    }

protected:
    EIGEN_DEVICE_FUNC inline ConstMeanType& mean_const() const { return mean_; }
    EIGEN_DEVICE_FUNC inline ConstCovarianceType& covariance_const() const { return covariance_; }

    MeanType mean_;
    CovarianceType covariance_;
};

}

#endif // VISIONCORE_TYPES_GAUSSIAN_HPP


#include "assign_bilinear_bases_dist.hxx"
#include "../../SDP.hxx"
#include "set_bases_blocks.hxx"

#include <boost/filesystem.hpp>

#include <iterator>

void read_blocks(const boost::filesystem::path &sdp_path,
                 const El::Grid &grid, const Block_Info &block_info, SDP &sdp);
void read_objectives(const boost::filesystem::path &sdp_path,
                     const El::Grid &grid, El::BigFloat &objective_const,
                     El::DistMatrix<El::BigFloat> &dual_objective_b);

SDP::SDP(const boost::filesystem::path &sdp_path,
         const Block_Info &block_info, const El::Grid &grid)
{
  read_objectives(sdp_path, grid, objective_const, dual_objective_b);
  read_blocks(sdp_path, grid, block_info, *this);
}

SDP::SDP(
  const El::BigFloat &objective_const_input,
  const std::vector<std::vector<El::BigFloat>> &primal_objective_c_input,
  const std::vector<El::Matrix<El::BigFloat>> &free_var_input,
  const El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &yp_to_y_star,
  const El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &dual_objective_b_star,
  const El::BigFloat &primal_c_scale, const Block_Info &block_info,
  const El::Grid &grid)
    : dual_objective_b(dual_objective_b_star.Height(), 1, grid),
      objective_const(objective_const_input)
{
  std::vector<size_t> block_offsets(primal_objective_c_input.size() + 1, 0);
  for(size_t p(0); p < primal_objective_c_input.size(); ++p)
    {
      block_offsets[p + 1]
        = block_offsets[p] + primal_objective_c_input[p].size();
    }

  auto &block_indices(block_info.block_indices);
  std::vector<El::Matrix<El::BigFloat>> bilinear_bases_local(
    2 * block_indices.size());
  for(size_t block(0); block != block_indices.size(); ++block)
    {
      bilinear_bases_local[2 * block].Resize(1, 1);
      bilinear_bases_local[2 * block + 1].Resize(0, 1);
      bilinear_bases_local[2 * block](0, 0) = 1;
    }
  assign_bilinear_bases_dist(bilinear_bases_local, grid, bilinear_bases);
  set_bases_blocks(block_info, bilinear_bases_local, bases_blocks, grid);

  for(size_t block(0); block != block_indices.size(); ++block)
    {
      primal_objective_c.blocks.emplace_back(
        primal_objective_c_input.at(block_indices.at(block)).size(), 1, grid);
      // TODO: copied from sdp_solve/SDP/SDP/read_primal_objective_c.cxx
      auto &primal_block(primal_objective_c.blocks.back());
      const size_t local_height(primal_block.LocalHeight());
      if(primal_block.GlobalCol(0) == 0)
        {
          for(size_t row = 0; row < local_height; ++row)
            {
              const size_t global_row(primal_block.GlobalRow(row));
              primal_block.SetLocal(
                row, 0,
                primal_c_scale
                  * primal_objective_c_input.at(block_indices.at(block))
                      .at(global_row));
            }
        }
    }

  // Setup B
  const int64_t B_Height(block_offsets.back()),
    B_Width(dual_objective_b_star.Height());
  El::Grid global_grid;
  El::DistMatrix<El::BigFloat> B(B_Height, B_Width, global_grid);

  for(int64_t row(0); row != B.LocalHeight(); ++row)
    {
      const size_t global_row(B.GlobalRow(row));
      const auto upper_iterator(std::upper_bound(
        block_offsets.begin(), block_offsets.end(), global_row));
      const size_t block_index(
        std::distance(block_offsets.begin(), upper_iterator) - 1);
      const size_t block_row(global_row - block_offsets[block_index]);
      for(int64_t column(0); column != B.LocalWidth(); ++column)
        {
          const size_t global_column(B.GlobalCol(column));
          B.SetLocal(
            row, column,
            primal_c_scale
              * free_var_input.at(block_index)(block_row, global_column));
        }
    }

  // Transform B into the yp frame.
  El::DistMatrix<El::BigFloat> U(B.Height(), B.Width(), B.Grid());
  El::Zero(U);
  El::DistMatrix<El::BigFloat> yp_to_y(yp_to_y_star.Height(),
                                       yp_to_y_star.Width(), B.Grid());

  for(int64_t row(0); row < yp_to_y.LocalHeight(); ++row)
    {
      const int64_t global_row(yp_to_y.GlobalRow(row));
      for(int64_t column(0); column < yp_to_y.LocalWidth(); ++column)
        {
          const int64_t global_column(yp_to_y.GlobalCol(column));
          yp_to_y.SetLocal(row, column,
                           yp_to_y_star.GetLocal(global_row, global_column));
        }
    }
  El::Gemm(El::Orientation::NORMAL, El::Orientation::NORMAL, El::BigFloat(1.0),
           B, yp_to_y, El::BigFloat(0.0), U);

  El::DistMatrix<El::BigFloat, El::STAR, El::STAR> U_star(U);

  free_var_matrix.blocks.reserve(block_indices.size());
  for(size_t block(0); block != block_indices.size(); ++block)
    {
      const size_t block_index(block_indices.at(block));
      free_var_matrix.blocks.emplace_back(
        free_var_input.at(block_index).Height(),
        free_var_input.at(block_index).Width(), grid);
      auto &free_var_block(free_var_matrix.blocks.back());
      for(int64_t row(0); row != free_var_block.Height(); ++row)
        {
          const int64_t global_row(block_offsets.at(block_index) + row);
          for(int64_t column(0); column != free_var_block.Width(); ++column)
            {
              if(free_var_block.IsLocal(row, column))
                {
                  free_var_block.SetLocal(free_var_block.LocalRow(row),
                                          free_var_block.LocalCol(column),
                                          U_star.GetLocal(global_row, column));
                }
            }
        }
    }

  // Copy over dual_objective_b
  for(int64_t row(0); row < dual_objective_b.LocalHeight(); ++row)
    {
      const int64_t global_row(dual_objective_b.GlobalRow(row));
      for(int64_t column(0); column < dual_objective_b.LocalWidth(); ++column)
        {
          const int64_t global_column(dual_objective_b.GlobalCol(column));
          dual_objective_b.SetLocal(
            row, column,
            dual_objective_b_star.GetLocal(global_row, global_column));
        }
    }
}


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Alexandru-Eugen Ichim
 *                      Willow Garage, Inc
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  $Id: statistical_multiscale_interest_region_extraction.hpp 6728 2012-08-07
 * 05:18:36Z rusu $
 */

#ifndef PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_
#define PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_

#include <pcl/features/statistical_multiscale_interest_region_extraction.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/distances.h>
#include <pcl/features/boost.h>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void pcl::StatisticalMultiscaleInterestRegionExtraction<
    PointT>::generateCloudGraph() {
    // generate a K-NNG (K-nearest neighbors graph)
    pcl::KdTreeFLANN<PointT> kdtree;
    kdtree.setInputCloud(input_);

    using namespace boost;
    typedef property<edge_weight_t, float> Weight;
    typedef adjacency_list<vecS, vecS, undirectedS, no_property, Weight> Graph;
    Graph cloud_graph;

    for (size_t point_i = 0; point_i < input_->points.size(); ++point_i) {
        std::vector<int> k_indices(16);
        std::vector<float> k_distances(16);
        kdtree.nearestKSearch(static_cast<int>(point_i), 16, k_indices,
                              k_distances);

        for (int k_i = 0; k_i < static_cast<int>(k_indices.size()); ++k_i)
            add_edge(point_i, k_indices[k_i], Weight(sqrtf(k_distances[k_i])),
                     cloud_graph);
    }

    const size_t E = num_edges(cloud_graph), V = num_vertices(cloud_graph);
    PCL_INFO("The graph has %lu vertices and %lu edges.\n", V, E);
    geodesic_distances_.clear();
    for (size_t i = 0; i < V; ++i) {
        std::vector<float> aux(V);
        geodesic_distances_.push_back(aux);
    }
    johnson_all_pairs_shortest_paths(cloud_graph, geodesic_distances_);

    PCL_INFO("Done generating the graph\n");
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
bool pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::initCompute() {
    if (!PCLBase<PointT>::initCompute()) {
        PCL_ERROR(
            "[pcl::StatisticalMultiscaleInterestRegionExtraction::initCompute] "
            "PCLBase::initCompute () failed - no input cloud was given.\n");
        return (false);
    }
    if (scale_values_.empty()) {
        PCL_ERROR("[pcl::StatisticalMultiscaleInterestRegionExtraction::"
                  "initCompute] No scale values were given\n");
        return (false);
    }

    return (true);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void pcl::StatisticalMultiscaleInterestRegionExtraction<
    PointT>::geodesicFixedRadiusSearch(size_t &query_index, float &radius,
                                       std::vector<int> &result_indices) {
    for (size_t i = 0; i < geodesic_distances_[query_index].size(); ++i)
        if (i != query_index && geodesic_distances_[query_index][i] < radius)
            result_indices.push_back(static_cast<int>(i));
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void pcl::StatisticalMultiscaleInterestRegionExtraction<
    PointT>::computeRegionsOfInterest(std::list<IndicesPtr> &rois) {
    if (!initCompute()) {
        PCL_ERROR("StatisticalMultiscaleInterestRegionExtraction: not "
                  "completely initialized\n");
        return;
    }

    generateCloudGraph();

    computeF();

    extractExtrema(rois);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::computeF() {
    PCL_INFO("Calculating statistical information\n");

    // declare and initialize data structure
    F_scales_.resize(scale_values_.size());
    std::vector<float> point_density(input_->points.size()),
        F(input_->points.size());
    std::vector<std::vector<float>> phi(input_->points.size());
    std::vector<float> phi_row(input_->points.size());

    for (size_t scale_i = 0; scale_i < scale_values_.size(); ++scale_i) {
        float scale_squared = scale_values_[scale_i] * scale_values_[scale_i];

        // calculate point density for each point x_i
        for (size_t point_i = 0; point_i < input_->points.size(); ++point_i) {
            float point_density_i = 0.0;
            for (size_t point_j = 0; point_j < input_->points.size();
                 ++point_j) {
                float d_g = geodesic_distances_[point_i][point_j];
                float phi_i_j =
                    1.0f /
                    sqrtf(2.0f * static_cast<float>(M_PI) * scale_squared) *
                    expf((-1) * d_g * d_g / (2.0f * scale_squared));

                point_density_i += phi_i_j;
                phi_row[point_j] = phi_i_j;
            }
            point_density[point_i] = point_density_i;
            phi[point_i] = phi_row;
        }

        // compute weights for each pair (x_i, x_j), evaluate the operator A_hat
        for (size_t point_i = 0; point_i < input_->points.size(); ++point_i) {
            float A_hat_normalization = 0.0;
            PointT A_hat;
            A_hat.x = A_hat.y = A_hat.z = 0.0;
            for (size_t point_j = 0; point_j < input_->points.size();
                 ++point_j) {
                float phi_hat_i_j =
                    phi[point_i][point_j] /
                    (point_density[point_i] * point_density[point_j]);
                A_hat_normalization += phi_hat_i_j;

                PointT aux = input_->points[point_j];
                aux.x *= phi_hat_i_j;
                aux.y *= phi_hat_i_j;
                aux.z *= phi_hat_i_j;

                A_hat.x += aux.x;
                A_hat.y += aux.y;
                A_hat.z += aux.z;
            }
            A_hat.x /= A_hat_normalization;
            A_hat.y /= A_hat_normalization;
            A_hat.z /= A_hat_normalization;

            // compute the invariant F
            float aux = 2.0f / scale_values_[scale_i] *
                        euclideanDistance<PointT, PointT>(
                            A_hat, input_->points[point_i]);
            F[point_i] = aux * expf(-aux);
        }

        F_scales_[scale_i] = F;
    }
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
void pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::extractExtrema(
    std::list<IndicesPtr> &rois) {
    std::vector<std::vector<bool>> is_min(scale_values_.size()),
        is_max(scale_values_.size());

    // for each point, check if it is a local extrema on each scale
    for (size_t scale_i = 0; scale_i < scale_values_.size(); ++scale_i) {
        std::vector<bool> is_min_scale(input_->points.size()),
            is_max_scale(input_->points.size());
        for (size_t point_i = 0; point_i < input_->points.size(); ++point_i) {
            std::vector<int> nn_indices;
            geodesicFixedRadiusSearch(point_i, scale_values_[scale_i],
                                      nn_indices);
            bool is_max_point = true, is_min_point = true;
            for (std::vector<int>::iterator nn_it = nn_indices.begin();
                 nn_it != nn_indices.end(); ++nn_it)
                if (F_scales_[scale_i][point_i] < F_scales_[scale_i][*nn_it])
                    is_max_point = false;
                else
                    is_min_point = false;

            is_min_scale[point_i] = is_min_point;
            is_max_scale[point_i] = is_max_point;
        }

        is_min[scale_i] = is_min_scale;
        is_max[scale_i] = is_max_scale;
    }

    // look for points that are min/max over three consecutive scales
    for (size_t scale_i = 1; scale_i < scale_values_.size() - 1; ++scale_i) {
        for (size_t point_i = 0; point_i < input_->points.size(); ++point_i)
            if ((is_min[scale_i - 1][point_i] && is_min[scale_i][point_i] &&
                 is_min[scale_i + 1][point_i]) ||
                (is_max[scale_i - 1][point_i] && is_max[scale_i][point_i] &&
                 is_max[scale_i + 1][point_i])) {
                // add the point to the result vector
                IndicesPtr region(new std::vector<int>);
                region->push_back(static_cast<int>(point_i));

                // and also add its scale-sized geodesic neighborhood
                std::vector<int> nn_indices;
                geodesicFixedRadiusSearch(point_i, scale_values_[scale_i],
                                          nn_indices);
                region->insert(region->end(), nn_indices.begin(),
                               nn_indices.end());
                rois.push_back(region);
            }
    }
}

#define PCL_INSTANTIATE_StatisticalMultiscaleInterestRegionExtraction(T)       \
    template class PCL_EXPORTS                                                 \
        pcl::StatisticalMultiscaleInterestRegionExtraction<T>;

#endif /* PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_ \
        */


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

#include <SFML/Graphics.hpp>

#include <rmf_planner_viz/draw/Graph.hpp>
#include <rmf_planner_viz/draw/Schedule.hpp>

#include <rmf_fleet_adapter/agv/parse_graph.hpp>
#include <rmf_freespace_planner/rmf_probabilistic_road_map.hpp>
#include <rmf_performance_tests/rmf_performance_tests.hpp>
#include <rmf_performance_tests/Scenario.hpp>
#include <rmf_traffic/schedule/Database.hpp>

#include <Eigen/Geometry>

#include <iostream>

#include "imgui-SFML.h"

const std::size_t NotObstacleID = std::numeric_limits<std::size_t>::max();

int main(int argc, char* argv[])
{
  sf::Font font;
  if (!font.loadFromFile("./build/rmf_planner_viz/fonts/OpenSans-Bold.ttf"))
  {
    std::cout <<
      "Failed to load font. Make sure you run the executable from the colcon directory"
              << std::endl;
    return -1;
  }

  if (argc < 2)
  {
    std::cout << "Please provide scenario file name" << std::endl;
    return 0;
  }

  rmf_performance_tests::scenario::Description scenario;
  try
  {
    parse(argv[1], scenario);
  }
  catch (std::runtime_error& e)
  {
    std::cout << e.what() << std::endl;
    return 0;
  }

  using namespace std::chrono_literals;

  const auto& plan_robot = scenario.robots.find(scenario.plan.robot);
  if (plan_robot == scenario.robots.end())
  {
    std::cout << "Plan robot [" << scenario.plan.robot <<
      "]'s traits and profile missing" << std::endl;
    return 0;
  }

  const auto get_wp =
    [&](const rmf_traffic::agv::Graph& graph, const std::string& name)
    {
      return graph.find_waypoint(name)->index();
    };

  const auto start_time = rmf_traffic::Time(rmf_traffic::Duration(0));

  const auto database = std::make_shared<rmf_traffic::schedule::Database>();

  std::vector<rmf_traffic::schedule::Participant> obstacles;

  for (const auto& obstacle : scenario.obstacle_plans)
  {
    const auto& robot = scenario.robots.find(obstacle.robot);

    if (robot == scenario.robots.end())
    {
      std::cout << "Robot [" << obstacle.robot <<
        "] is missing traits and profile. Using traits and profile of plan_robot."
                << std::endl;

      rmf_traffic::agv::Planner planner = rmf_traffic::agv::Planner{
        plan_robot->second,
        {nullptr}
      };

      obstacles.emplace_back(
        rmf_performance_tests::add_obstacle(
          planner, database,
          {
            start_time + std::chrono::seconds(obstacle.initial_time),
            get_wp(plan_robot->second.graph(), obstacle.initial_waypoint),
            obstacle.initial_orientation * M_PI / 180.0
          },
          get_wp(plan_robot->second.graph(), obstacle.goal)
        )
      );
    }
    else
    {
      rmf_traffic::agv::Planner planner = rmf_traffic::agv::Planner{
        robot->second,
        {nullptr}
      };

      obstacles.emplace_back(
        rmf_performance_tests::add_obstacle(
          planner, database,
          {
            start_time + std::chrono::seconds(obstacle.initial_time),
            get_wp(robot->second.graph(), obstacle.initial_waypoint),
            obstacle.initial_orientation * M_PI / 180.0
          },
          get_wp(robot->second.graph(), obstacle.goal)
        )
      );
    }
  }

  std::vector<rmf_freespace_planner::rmf_probabilistic_road_map::ProbabilisticRoadMap::Obstacle>
  static_obstacles;
  for (const auto& obstacle : scenario.obstacle_routes)
  {
    const auto& robot = scenario.robots.at(obstacle.robot);

    obstacles.emplace_back(
      rmf_performance_tests::add_obstacle(
        database,
        robot.vehicle_traits().profile(),
        obstacle.route));

    static_obstacles.push_back(
      {obstacle.route.trajectory().at(0).position().head<2>(),
        robot.vehicle_traits().profile().footprint()->get_characteristic_length()});
  }

  const auto& plan = scenario.plan;

  auto plan_participant = rmf_traffic::schedule::make_participant(
    rmf_traffic::schedule::ParticipantDescription{
      "participant_0",
      "test_trajectory",
      rmf_traffic::schedule::ParticipantDescription::Rx::Responsive,
      plan_robot->second.vehicle_traits().profile()
    },
    database);

  const auto obstacle_validator =
    rmf_traffic::agv::ScheduleRouteValidator::make(
    database, plan_participant.id(),
    plan_robot->second.vehicle_traits().profile());

  rmf_traffic::agv::Planner planner_0(
    plan_robot->second,
    rmf_traffic::agv::Planner::Options(nullptr));

  using namespace std::chrono_literals;

  /// Setup participants

  // set plans for the participants
  using namespace std::chrono_literals;

  std::vector<rmf_traffic::agv::Planner::Start> starts;
  starts.emplace_back(start_time + std::chrono::seconds(
      plan.initial_time),
    get_wp(plan_robot->second.graph(), plan.initial_waypoint),
    plan.initial_orientation);

  rmf_traffic::agv::Planner::Goal goal(get_wp(
      plan_robot->second.graph(), plan.goal));

  const auto& planner_plan = planner_0.plan(starts, goal);

  const auto& routes = planner_plan->get_itinerary();

  const auto& start_timing = std::chrono::steady_clock::now();
  std::vector<rmf_traffic::Route> planned_routes;
  for (const auto& route : routes)
  {
    for (std::size_t i = 0; i < route.trajectory().size() - 1; ++i)
    {
      if ((route.trajectory()[i].position() -
        route.trajectory()[i + 1].position()).norm() < 1)
      {
        continue;
      }
      if (route.trajectory()[i].position().x() ==
        route.trajectory()[i + 1].position().x() &&
        route.trajectory()[i].position().y() ==
        route.trajectory()[i + 1].position().y())
      {
        continue;
      }

      rmf_freespace_planner::rmf_probabilistic_road_map::ProbabilisticRoadMap
        probabilistic_road_map(
        obstacle_validator,
        3,
        1000.0,
        database,
        std::unordered_set<rmf_traffic::schedule::ParticipantId>(
          {plan_participant.id()}));

      auto result = probabilistic_road_map.plan(
        {route.trajectory()[i].position(), route.trajectory()[i].time()},
        {route.trajectory()[i + 1].position()},
        plan_robot->second.vehicle_traits(),
        static_obstacles,
        route.map());
      planned_routes.insert(planned_routes.end(), result.begin(), result.end());
    }
  }

  plan_participant.set(planned_routes);

  auto end_timing = std::chrono::steady_clock::now();

  std::cout << "-------------------------" << std::endl;
  std::cout << "Time taken for PRM " <<
    std::chrono::duration_cast<std::chrono::milliseconds>(
      end_timing - start_timing).count() / 1000.0 << "s" << std::endl;
  std::cout << "-------------------------" << std::endl;

  rmf_planner_viz::draw::Graph graph_0_drawable(
    plan_robot->second.graph(), 0.5, font);
  std::vector<std::string> map_names = graph_0_drawable.get_map_names();
  std::string chosen_map = argv[2];
//  if (graph_0_drawable.current_map())
//    chosen_map = *graph_0_drawable.current_map();

  rmf_planner_viz::draw::Schedule schedule_drawable(
    database, 0.25, chosen_map, start_time + 0s);

  sf::RenderWindow app_window(
    sf::VideoMode(1250, 1028),
    "Test Trajectory",
    sf::Style::Default);

  app_window.resetGLStates();

  const auto bounds = schedule_drawable.bounds();

  rmf_planner_viz::draw::Fit fit({bounds}, 0.02);
  std::cout << "initial bounds:\n"
            << " -- min: " << graph_0_drawable.bounds().min.transpose()
            << "\n -- max: " << graph_0_drawable.bounds().max.transpose() <<
    std::endl;
  ImGui::SFML::Init(app_window);

  auto current_time = start_time;
  current_time += std::chrono::milliseconds(std::stoi(argv[3]));
  sf::Clock deltaClock;

  while (app_window.isOpen())
  {
    current_time += std::chrono::milliseconds(std::stoi(argv[4]));
    sf::Event event;
    while (app_window.pollEvent(event))
    {
      ImGui::SFML::ProcessEvent(event);

      if (event.type == sf::Event::Closed)
      {
        return 0;
      }

      if (event.type == sf::Event::Resized)
      {
        sf::FloatRect visibleArea(0, 0, event.size.width, event.size.height);
        app_window.setView(sf::View(visibleArea));
      }
    }

    ImGui::SFML::Update(app_window, deltaClock.restart());

    graph_0_drawable.choose_map(chosen_map);

    app_window.clear();

    schedule_drawable.timespan(current_time);

    sf::RenderStates states;
    fit.apply_transform(states.transform, app_window.getSize());
    app_window.draw(graph_0_drawable, states);
    app_window.draw(schedule_drawable, states);

    ImGui::SFML::Render();

    app_window.display();
  }
}