#pragma once

// Eigen
#include <Eigen/Dense>
#include <Eigen/Geometry>

// ROS
#include <sensor_msgs/Imu.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <std_msgs/Float64MultiArray.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

// MATPLOTLIB-CPP
#define _USE_MATH_DEFINES
#include <cmath>
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;

// Submodules
#include <rbp_planner.hpp>
#include <rbp_corridor.hpp>
#include <init_traj_planner.hpp>
#include <mission.hpp>
#include <param.hpp>

namespace SwarmPlanning {
    class RBPPublisher {
    public:
        RBPPublisher(ros::NodeHandle _nh,
                     SwarmPlanning::PlanResult _planResult,
                     SwarmPlanning::Mission _mission,
                     SwarmPlanning::Param _param)
                : nh(std::move(_nh)),
                  planResult(std::move(_planResult)),
                  mission(std::move(_mission)),
                  param(std::move(_param)) {
            qn = mission.qn;
            outdim = 3;

            M = planResult.msgs_traj_info.data.size() - 2 - 1;
            T.resize(M + 1);
            for (int m = 0; m < M + 1; m++) {
                T[m] = planResult.msgs_traj_info.data.at(m + 2);
            }

            dt = 0.1;
            t.resize(floor(T.back() / dt));
            for (int i = 0; i < t.size(); i++) {
                t[i] = i * dt;
            }

            pva.resize(qn);
            coef.resize(qn);
            quad_state.resize(qn);
            currentState.resize(qn);
            for (int qi = 0; qi < qn; qi++) {
                float rows = planResult.msgs_traj_coef[qi].layout.dim.at(0).size;
                float cols = planResult.msgs_traj_coef[qi].layout.dim.at(1).size;
                std::vector<double> data = planResult.msgs_traj_coef[qi].data;
                coef[qi] = Eigen::Map<Eigen::MatrixXd>(data.data(), rows, cols);

                currentState[qi].resize(outdim * param.phi);
                quad_state[qi].resize(outdim * param.phi);
                for (int i = 0; i < outdim * param.phi; i++) {
                    quad_state[qi][i].resize(t.size());
                }
            }

            pubs_traj_coef.resize(qn);
            pubs_traj.resize(qn);
            pubs_relBox.resize(qn);
            for (int qi = 0; qi < qn; qi++) {
                std::string mav_name = "/mav" + std::to_string(qi);
                pubs_traj_coef[qi] = nh.advertise<std_msgs::Float64MultiArray>("/traj_coef" + mav_name, 1);
                pubs_traj[qi] = nh.advertise<nav_msgs::Path>("/desired_trajectory" + mav_name, 1);
                pubs_relBox[qi] = nh.advertise<visualization_msgs::MarkerArray>("/relative_box" + mav_name, 1);
            }
            pub_traj_info = nh.advertise<std_msgs::Float64MultiArray>("/traj_info", 1);
            pub_initTraj = nh.advertise<visualization_msgs::MarkerArray>("/initTraj", 1);
            pub_obsBox = nh.advertise<visualization_msgs::MarkerArray>("/obstacle_box", 1);
            pub_feasibleBox = nh.advertise<visualization_msgs::MarkerArray>("/feasible_box", 1);
            pub_colBox = nh.advertise<visualization_msgs::MarkerArray>("/collision_model", 1);
//            pub_minDist = nh.advertise<geometry_msgs::Pose>("/min_dist", 1);

            msgs_traj.resize(qn);
            msgs_relBox.resize(qn);
        }

        void update(double current_time) {
            update_traj(current_time);
            update_initTraj();
            update_obsBox(current_time);
            update_relBox(current_time);
            update_feasibleBox(current_time);
            update_colBox();
//            update_distance_between_agents_realtime(current_time);
        }

        void publish() {
            for (int qi = 0; qi < qn; qi++) {
                pubs_traj_coef[qi].publish(planResult.msgs_traj_coef[qi]);
                pubs_traj[qi].publish(msgs_traj[qi]);
                pubs_relBox[qi].publish(msgs_relBox[qi]);
            }
            pub_traj_info.publish(planResult.msgs_traj_info);
            pub_initTraj.publish(msgs_initTraj);
            pub_obsBox.publish(msgs_obsBox);
            pub_feasibleBox.publish(msgs_feasibleBox);
            pub_colBox.publish(msgs_colBox);
//            pub_minDist.publish(msgs_minDist);
        }

        void plot(bool log) {
            update_quad_state();
            update_safety_margin_ratio();

            if (log) {
                plot_quad_dynamics();
                plot_safety_margin_ratio();
            }
            ROS_INFO_STREAM("Global min_dist between agents: " << safety_margin_ratio);
            ROS_INFO_STREAM("Total flight distance: " << trajectory_length_sum());
        }

//    void plot_real_time(){
//        plot_distance_between_agents_realtime();
//    }

    private:
        ros::NodeHandle nh;
        SwarmPlanning::PlanResult planResult;
        SwarmPlanning::Mission mission;
        SwarmPlanning::Param param;

        int qn, M, outdim;
        double safety_margin_ratio, dt;
        tf::TransformBroadcaster br;
        std::vector<Eigen::MatrixXd> pva;
        std::vector<Eigen::MatrixXd> coef;
        std::vector<std::vector<double>> currentState;
        std::vector<double> T, t, max_ratio, min_ratio;
        std::vector<std::vector<std::vector<double>>> quad_state;
//    std::shared_ptr<plt::Plot> plot_min_dist_obj;

        // ROS publisher
        ros::Publisher pub_traj_info;
        std::vector<ros::Publisher> pubs_traj_coef;
        std::vector<ros::Publisher> pubs_traj;
        ros::Publisher pub_initTraj;
        ros::Publisher pub_obsBox;
        std::vector<ros::Publisher> pubs_relBox;
        ros::Publisher pub_feasibleBox;
        ros::Publisher pub_colBox;
//        ros::Publisher pub_minDist;

        // ROS messages
        std::vector<nav_msgs::Path> msgs_traj;
        visualization_msgs::MarkerArray msgs_initTraj;
        visualization_msgs::MarkerArray msgs_obsBox;
        std::vector<visualization_msgs::MarkerArray> msgs_relBox;
        visualization_msgs::MarkerArray msgs_feasibleBox;
        visualization_msgs::MarkerArray msgs_colBox;
//        geometry_msgs::Pose msgs_minDist;

        void timeMatrix(double current_time, int &index, Eigen::MatrixXd &polyder) {
            double tseg = 0;
            double tcand;

            // find segment start time tseg
            for (int m = 0; m < M; m++) {
                tcand = T[m];
                if (tcand < current_time) {
                    tseg = tcand;
                    index = m;
                } else {
                    break;
                }
            }
            tseg = current_time - tseg;
            polyder.resize(3, param.n + 1);
            for (int i = 0; i < 3; i++) {
                for (int j = 0; j < param.n + 1; j++) {
                    if (i <= j)
                        polyder(i, param.n - j) =
                                ((i == 0) * 1 + (i == 1) * j + (i == 2) * j * (j - 1)) * pow(tseg, j - i);
                    else
                        polyder(i, param.n - j) = 0;
                }
            }
        }

        void update_traj(double current_time) {
            if (current_time > T.back()) {
                return;
            }
            for (int qi = 0; qi < qn; qi++) {
                int index = 0;
                Eigen::MatrixXd polyder;
                timeMatrix(current_time, index, polyder);

                pva[qi] = polyder * coef[qi].block((param.n + 1) * index, 0, (param.n + 1), 3);

                msgs_traj[qi].header.frame_id = "/world";
                msgs_traj[qi].header.stamp.sec = current_time;

                geometry_msgs::PoseStamped pos_des;
                pos_des.header.frame_id = "/world";
                pos_des.pose.position.x = pva[qi](0, 0);
                pos_des.pose.position.y = pva[qi](0, 1);
                pos_des.pose.position.z = pva[qi](0, 2);
                msgs_traj[qi].poses.emplace_back(pos_des);

                currentState[qi][0] = pva[qi](0, 0);
                currentState[qi][1] = pva[qi](0, 1);
                currentState[qi][2] = pva[qi](0, 2);
                currentState[qi][3] = pva[qi](1, 0);
                currentState[qi][4] = pva[qi](1, 1);
                currentState[qi][5] = pva[qi](1, 2);
                currentState[qi][6] = pva[qi](2, 0);
                currentState[qi][7] = pva[qi](2, 1);
                currentState[qi][8] = pva[qi](2, 2);

                tf::Transform transform;
                transform.setOrigin(tf::Vector3(pva[qi](0, 0), pva[qi](0, 1), pva[qi](0, 2)));
                tf::Quaternion q;
                q.setRPY(0, 0, atan2(pva[qi](1, 1), pva[qi](1, 0)));
                transform.setRotation(q);
                br.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
                                                      "world", "/mav" + std::to_string(qi) + "/base_link"));
            }
        }

        void update_initTraj() {
            visualization_msgs::MarkerArray mk_array;
            for (int qi = 0; qi < qn; qi++) {
                for (int m = 0; m < planResult.initTraj[qi].size(); m++) {
                    visualization_msgs::Marker mk;
                    mk.header.frame_id = "world";
                    mk.header.stamp = ros::Time::now();
                    mk.ns = "mav" + std::to_string(qi);
                    mk.type = visualization_msgs::Marker::CUBE;
                    mk.action = visualization_msgs::Marker::ADD;

                    mk.pose.orientation.x = 0.0;
                    mk.pose.orientation.y = 0.0;
                    mk.pose.orientation.z = 0.0;
                    mk.pose.orientation.w = 1.0;

                    mk.color.a = 1.0;
                    mk.color.r = param.color[qi][0];
                    mk.color.g = param.color[qi][1];
                    mk.color.b = param.color[qi][2];

                    mk.id = m;
                    octomap::point3d p_init = planResult.initTraj[qi][m];
                    mk.pose.position.x = p_init.x();
                    mk.pose.position.y = p_init.y();
                    mk.pose.position.z = p_init.z();

                    mk.scale.x = 0.1;
                    mk.scale.y = 0.1;
                    mk.scale.z = 0.1;

                    mk_array.markers.emplace_back(mk);
                }
            }
            msgs_initTraj = mk_array;
        }

        void update_obsBox(double current_time) {
            visualization_msgs::MarkerArray mk_array;
            for (int qi = 0; qi < qn; qi++) {
                // find current obsBox number
                int box_curr = 0;
                while (box_curr < planResult.SFC[qi].size() &&
                        planResult.SFC[qi][box_curr].second < current_time) {
                    box_curr++;
                }
                if (box_curr >= planResult.SFC[qi].size()) {
                    box_curr = planResult.SFC[qi].size() - 1;
                }

                visualization_msgs::Marker mk;
                mk.header.frame_id = "world";
                mk.ns = "mav" + std::to_string(qi);
                mk.type = visualization_msgs::Marker::CUBE;
                mk.action = visualization_msgs::Marker::ADD;

                mk.pose.orientation.x = 0.0;
                mk.pose.orientation.y = 0.0;
                mk.pose.orientation.z = 0.0;
                mk.pose.orientation.w = 1.0;

                for (int bi = 0; bi < planResult.SFC[qi].size(); bi++) {
                    mk.id = bi;
//                mk.header.stamp = ros::Time(obstacle_boxes[qi][bi].second);
                    std::vector<double> obstacle_box = planResult.SFC[qi][bi].first;

                    {
                        double margin = mission.quad_size[qi];
                        obstacle_box[0] -= margin;
                        obstacle_box[1] -= margin;
                        obstacle_box[2] -= margin;
                        obstacle_box[3] += margin;
                        obstacle_box[4] += margin;
                        obstacle_box[5] += margin;
                    }

                    mk.pose.position.x = (obstacle_box[0] + obstacle_box[3]) / 2.0;
                    mk.pose.position.y = (obstacle_box[1] + obstacle_box[4]) / 2.0;
                    mk.pose.position.z = (obstacle_box[2] + obstacle_box[5]) / 2.0;

                    mk.scale.x = obstacle_box[3] - obstacle_box[0];
                    mk.scale.y = obstacle_box[4] - obstacle_box[1];
                    mk.scale.z = obstacle_box[5] - obstacle_box[2];

                    mk.color.a = 0.2;
                    mk.color.r = param.color[qi][0];
                    mk.color.g = param.color[qi][1];
                    mk.color.b = param.color[qi][2];

                    mk_array.markers.emplace_back(mk);
                }
            }
            msgs_obsBox = mk_array;
        }

        void update_relBox(double current_time) {
            for (int qi = 0; qi < qn; qi++) {
                visualization_msgs::MarkerArray mk_array;
                for (int qj = qi + 1; qj < qn; qj++) {
                    int box_curr = 0;
                    while (box_curr < planResult.RSFC[qi][qj].size() &&
                           planResult.RSFC[qi][qj][box_curr].second < current_time) {
                        box_curr++;
                    }
                    if (box_curr >= planResult.RSFC[qi][qj].size()) {
                        box_curr = planResult.RSFC[qi][qj].size() - 1;
                    }

                    visualization_msgs::Marker mk;
                    mk.header.frame_id = "world";
                    mk.ns = "mav" + std::to_string(qj);

                    // inter-collision model
                    double r = mission.quad_size[qi] + mission.quad_size[qj];
                    double h = r * param.downwash;

                    mk.type = visualization_msgs::Marker::SPHERE;
                    mk.action = visualization_msgs::Marker::ADD;
                    mk.id = 0;

                    mk.pose.orientation.x = 0;
                    mk.pose.orientation.y = 0;
                    mk.pose.orientation.z = 0;
                    mk.pose.orientation.w = 1.0;

                    mk.pose.position.x = pva[qi](0, 0);
                    mk.pose.position.y = pva[qi](0, 1);
                    mk.pose.position.z = pva[qi](0, 2);

                    mk.scale.x = 2 * r;
                    mk.scale.y = 2 * r;
                    mk.scale.z = 2 * h;

                    mk.color.a = 0.5;
                    mk.color.r = 1.0;
                    mk.color.g = 0.0;
                    mk.color.b = 0.0;

                    mk_array.markers.emplace_back(mk);

                    // qj position
                    mk.header.frame_id = "world";
                    mk.type = visualization_msgs::Marker::SPHERE;
                    mk.action = visualization_msgs::Marker::ADD;
                    mk.id = 1;

                    mk.pose.orientation.x = 0;
                    mk.pose.orientation.y = 0;
                    mk.pose.orientation.z = 0;
                    mk.pose.orientation.w = 1.0;

                    mk.pose.position.x = pva[qj](0, 0);
                    mk.pose.position.y = pva[qj](0, 1);
                    mk.pose.position.z = pva[qj](0, 2);

//                mk.scale.x = mission.quad_size[qj];
//                mk.scale.y = mission.quad_size[qj];
//                mk.scale.z = mission.quad_size[qj] * param.downwash;
                    mk.scale.x = 0.1;
                    mk.scale.y = 0.1;
                    mk.scale.z = 0.1;

                    mk.color.a = 1.0;
                    mk.color.r = param.color[qj][0];
                    mk.color.g = param.color[qj][1];
                    mk.color.b = param.color[qj][2];

                    mk_array.markers.emplace_back(mk);

                    // relative box
                    mk.header.frame_id = "world";
                    mk.header.stamp = ros::Time::now();
                    mk.type = visualization_msgs::Marker::CUBE;
                    mk.action = visualization_msgs::Marker::ADD;
                    mk.id = qj + 2;

                    mk.color.a = 0.3;
                    mk.color.r = 0.0;
                    mk.color.g = 1.0;
                    mk.color.b = 0.0;

                    mk.scale.x = 40;
                    mk.scale.y = 40;
                    mk.scale.z = 40;

                    octomap::point3d qi_vector;
                    qi_vector.x() = pva[qi](0, 0);
                    qi_vector.y() = pva[qi](0, 1);
                    qi_vector.z() = pva[qi](0, 2);

                    octomap::point3d normal_vector = planResult.RSFC[qi][qj][box_curr].first;
                    Eigen::Vector3d V3d_normal_vector(normal_vector.x(), normal_vector.y(), normal_vector.z());

                    double distance = r / normal_vector.norm() + mk.scale.z / 2;
//                double distance = mission.quad_size[qi] / normal_vector.norm();

                    mk.pose.position.x = pva[qi](0, 0) + normal_vector.normalized().x() * distance;
                    mk.pose.position.y = pva[qi](0, 1) + normal_vector.normalized().y() * distance;
                    mk.pose.position.z = pva[qi](0, 2) + normal_vector.normalized().z() * distance;

                    Eigen::Vector3d z_0 = Eigen::Vector3d::UnitZ();
                    Eigen::Quaterniond q = Eigen::Quaterniond::FromTwoVectors(z_0, V3d_normal_vector);

                    mk.pose.orientation.x = q.x();
                    mk.pose.orientation.y = q.y();
                    mk.pose.orientation.z = q.z();
                    mk.pose.orientation.w = q.w();

                    mk_array.markers.emplace_back(mk);
                }
                msgs_relBox[qi] = mk_array;
            }
        }

        void update_feasibleBox(double current_time) {
            visualization_msgs::MarkerArray mk_array;
            for (int qj = 0; qj < qn; qj++) {
                // current position
                visualization_msgs::Marker mk;
                mk.header.frame_id = "world";
                mk.header.stamp = ros::Time::now();
                mk.ns = "mav" + std::to_string(qj);
                mk.action = visualization_msgs::Marker::ADD;

                // inter-collision model
                for (int qi = 0; qi < qn; qi++) {
                    mk.type = visualization_msgs::Marker::SPHERE;
                    mk.id = qi;
                    mk.pose.position.x = pva[qi](0, 0);
                    mk.pose.position.y = pva[qi](0, 1);
                    mk.pose.position.z = pva[qi](0, 2);

                    mk.scale.x = 2 * mission.quad_size[qi];
                    mk.scale.y = 2 * mission.quad_size[qi];
                    mk.scale.z = 2 * mission.quad_size[qi] * param.downwash;

                    mk.color.a = 0.5;
                    mk.color.r = param.color[qi][0];
                    mk.color.g = param.color[qi][1];
                    mk.color.b = param.color[qi][2];

                    mk_array.markers.emplace_back(mk);
                }

                // feasible box
                for (int qi = 0; qi < qn; qi++) {
                    mk.type = visualization_msgs::Marker::CUBE;
                    mk.id = qn + qi;
                    mk.color.a = 0.3;
                    mk.color.r = param.color[qi][0];
                    mk.color.g = param.color[qi][1];
                    mk.color.b = param.color[qi][2];

                    mk.scale.x = 40;
                    mk.scale.y = 40;
                    mk.scale.z = 40;

                    double r = mission.quad_size[qi];
                    double h = r * param.downwash;

                    octomap::point3d normal_vector;
                    int box_curr = 0;
                    if (qi < qj) { // RSFC
                        while (box_curr < planResult.RSFC[qi][qj].size() &&
                               planResult.RSFC[qi][qj][box_curr].second < current_time) {
                            box_curr++;
                        }
                        if (box_curr >= planResult.RSFC[qi][qj].size()) {
                            box_curr = planResult.RSFC[qi][qj].size() - 1;
                        }

                        normal_vector = planResult.RSFC[qi][qj][box_curr].first;
                    } else if (qi > qj) { // RSFC
                        while (box_curr < planResult.RSFC[qj][qi].size() &&
                               planResult.RSFC[qj][qi][box_curr].second < current_time) {
                            box_curr++;
                        }
                        if (box_curr >= planResult.RSFC[qj][qi].size()) {
                            box_curr = planResult.RSFC[qj][qi].size() - 1;
                        }

                        normal_vector = -planResult.RSFC[qj][qi][box_curr].first;
                    } else { // SFC
                        while (box_curr < planResult.SFC[qi].size() &&
                               planResult.SFC[qi][box_curr].second < current_time) {
                            box_curr++;
                        }
                        if (box_curr >= planResult.SFC[qi].size()) {
                            box_curr = planResult.SFC[qi].size() - 1;
                        }
                        std::vector<double> obstacle_box = planResult.SFC[qi][box_curr].first;
                        for (int iter = 0; iter < 6; iter++) {
                            double margin = mission.quad_size[qi];
                            if (iter == 2 || iter == 5)
                                margin *= param.downwash;
                            if (iter < 3)
                                obstacle_box[iter] = obstacle_box[iter] - margin;
                            else
                                obstacle_box[iter] = obstacle_box[iter] + margin;
                        }

                        mk.type = visualization_msgs::Marker::LINE_LIST;
                        mk.pose.position.x = 0;
                        mk.pose.position.y = 0;
                        mk.pose.position.z = 0;

                        mk.pose.orientation.x = 0;
                        mk.pose.orientation.y = 0;
                        mk.pose.orientation.z = 0;
                        mk.pose.orientation.w = 1.0;

                        mk.scale.x = 0.05;

                        mk.color.a = 1;
                        mk.color.r = param.color[qi][0];
                        mk.color.g = param.color[qi][1];
                        mk.color.b = param.color[qi][2];

                        geometry_msgs::Point pt_i, pt_j;
                        std::vector<int> index = {0, 1, 2,
                                                  3, 4, 2,
                                                  3, 1, 5,
                                                  0, 4, 5};
                        int offset = 0;
                        for (int iter = 0; iter < 4; iter++) {
                            pt_i.x = obstacle_box[index[offset + 0]];
                            pt_i.y = obstacle_box[index[offset + 1]];
                            pt_i.z = obstacle_box[index[offset + 2]];

                            for (int i = 0; i < 3; i++) {
                                pt_j.x = obstacle_box[(index[offset + 0] + 3 * (i == 0)) % 6];
                                pt_j.y = obstacle_box[(index[offset + 1] + 3 * (i == 1)) % 6];
                                pt_j.z = obstacle_box[(index[offset + 2] + 3 * (i == 2)) % 6];

                                mk.points.emplace_back(pt_i);
                                mk.points.emplace_back(pt_j);
                            }
                            offset += 3;
                        }
                        mk_array.markers.emplace_back(mk);
                        mk.points.clear();

                        continue;
                    }

                    double distance = r / normal_vector.norm() - mk.scale.z / 2;
                    mk.pose.position.x = pva[qi](0, 0) + normal_vector.normalized().x() * distance;
                    mk.pose.position.y = pva[qi](0, 1) + normal_vector.normalized().y() * distance;
                    mk.pose.position.z = pva[qi](0, 2) + normal_vector.normalized().z() * distance;

                    Eigen::Vector3d V3d_normal_vector(normal_vector.x(), normal_vector.y(), normal_vector.z());
                    Eigen::Vector3d z_0 = Eigen::Vector3d::UnitZ();
                    Eigen::Quaterniond q = Eigen::Quaterniond::FromTwoVectors(z_0, V3d_normal_vector);

                    mk.pose.orientation.x = q.x();
                    mk.pose.orientation.y = q.y();
                    mk.pose.orientation.z = q.z();
                    mk.pose.orientation.w = q.w();

                    mk_array.markers.emplace_back(mk);
                }
            }
            msgs_feasibleBox = mk_array;
        }

        // obstacle-collision model
//        void update_colBox() {
//            visualization_msgs::MarkerArray mk_array;
//            for (int qi = 0; qi < qn; qi++) {
//                visualization_msgs::Marker mk;
//                mk.header.frame_id = "world";
//                mk.ns = "colBox";
//                mk.type = visualization_msgs::Marker::CUBE;
//                mk.action = visualization_msgs::Marker::ADD;
//
//                mk.pose.orientation.x = 0;
//                mk.pose.orientation.y = 0;
//                mk.pose.orientation.z = 0;
//                mk.pose.orientation.w = 1.0;
//
//                mk.id = qi;
//                mk.pose.position.x = pva[qi](0, 0);
//                mk.pose.position.y = pva[qi](0, 1);
//                mk.pose.position.z = pva[qi](0, 2);
//
//                mk.scale.x = 2 * mission.quad_size[qi];
//                mk.scale.y = 2 * mission.quad_size[qi];
//                mk.scale.z = 2 * mission.quad_size[qi] * param.downwash;
//
//                mk.color.a = 0.7;
//                mk.color.r = param.color[qi][0];
//                mk.color.g = param.color[qi][1];
//                mk.color.b = param.color[qi][2];
//
//                mk_array.markers.emplace_back(mk);
//            }
//            msgs_colBox = mk_array;
//        }

        void update_colBox() {
            visualization_msgs::MarkerArray mk_array;
            for (int qi = 0; qi < qn; qi++) {
                visualization_msgs::Marker mk;
                mk.header.frame_id = "world";
                mk.ns = "colBox";
                mk.type = visualization_msgs::Marker::SPHERE;
                mk.action = visualization_msgs::Marker::ADD;

                mk.pose.orientation.x = 0;
                mk.pose.orientation.y = 0;
                mk.pose.orientation.z = 0;
                mk.pose.orientation.w = 1.0;

                mk.id = qi;
                mk.pose.position.x = pva[qi](0, 0);
                mk.pose.position.y = pva[qi](0, 1);
                mk.pose.position.z = pva[qi](0, 2);

                mk.scale.x = 2 * mission.quad_size[qi];
                mk.scale.y = 2 * mission.quad_size[qi];
                mk.scale.z = 2 * mission.quad_size[qi] * param.downwash;

                mk.color.a = 0.7;
                mk.color.r = param.color[qi][0];
                mk.color.g = param.color[qi][1];
                mk.color.b = param.color[qi][2];

                mk_array.markers.emplace_back(mk);
            }
            msgs_colBox = mk_array;
        }

        void update_quad_state() {
            for (int qi = 0; qi < qn; qi++) {
                for (int j = 0; j < t.size(); j++) {
                    int index = 0;
                    Eigen::MatrixXd state, polyder;
                    timeMatrix(t[j], index, polyder);
                    state = polyder * coef[qi].block((param.n + 1) * index, 0, (param.n + 1), 3);

                    for (int i = 0; i < outdim * param.phi; i++) {
                        quad_state[qi][i][j] = state(i / outdim, i % outdim);
                    }
                }
            }
        }

        double trajectory_length_sum() {
            double length_sum = 0;
            for (int qi = 0; qi < qn; qi++) {
                for (int i = 0; i < t.size() - 1; i++) {
                    length_sum += sqrt(pow(quad_state[qi][0][i + 1] - quad_state[qi][0][i], 2) +
                                       pow(quad_state[qi][1][i + 1] - quad_state[qi][1][i], 2) +
                                       pow(quad_state[qi][2][i + 1] - quad_state[qi][2][i], 2));
                }
            }
            return length_sum;
        }

        void plot_quad_dynamics() {
            plt::figure_size(1280, 960);

            std::vector<double> dynamic_limit;
            dynamic_limit.resize(t.size());

            // Plot Quad Velocity
            plt::subplot(3, 2, 1);
            for (int qi = 0; qi < qn; qi++) {
                plt::named_plot("agent" + std::to_string(qi), t, quad_state[qi][3]);
            }
            std::fill(dynamic_limit.begin(), dynamic_limit.end(), mission.max_vel[0][0]); //TODO: heterogeneous case
            plt::plot(t, dynamic_limit, "r--");
            std::fill(dynamic_limit.begin(), dynamic_limit.end(), -mission.max_vel[0][0]); //TODO: heterogeneous case
            plt::plot(t, dynamic_limit, "r--");
            plt::title("velocity -x axis");

            plt::subplot(3, 2, 3);
            for (int qi = 0; qi < qn; qi++) {
                plt::named_plot("agent" + std::to_string(qi), t, quad_state[qi][4]);
            }
            std::fill(dynamic_limit.begin(), dynamic_limit.end(), mission.max_vel[0][1]); //TODO: heterogeneous case
            plt::plot(t, dynamic_limit, "r--");
            std::fill(dynamic_limit.begin(), dynamic_limit.end(), -mission.max_vel[0][1]); //TODO: heterogeneous case
            plt::plot(t, dynamic_limit, "r--");
            plt::title("velocity -y axis");

            plt::subplot(3, 2, 5);
            for (int qi = 0; qi < qn; qi++) {
                plt::named_plot("agent" + std::to_string(qi), t, quad_state[qi][5]);
            }
            std::fill(dynamic_limit.begin(), dynamic_limit.end(), mission.max_vel[0][2]); //TODO: heterogeneous case
            plt::plot(t, dynamic_limit, "r--");
            std::fill(dynamic_limit.begin(), dynamic_limit.end(), -mission.max_vel[0][2]); //TODO: heterogeneous case
            plt::plot(t, dynamic_limit, "r--");
            plt::title("velocity -z axis");

            // Plot Quad Acceleration
            plt::subplot(3, 2, 2);
            for (int qi = 0; qi < qn; qi++) {
                plt::named_plot("agent" + std::to_string(qi), t, quad_state[qi][6]);
            }
            std::fill(dynamic_limit.begin(), dynamic_limit.end(), mission.max_acc[0][0]); //TODO: heterogeneous case
            plt::plot(t, dynamic_limit, "r--");
            std::fill(dynamic_limit.begin(), dynamic_limit.end(), -mission.max_acc[0][0]); //TODO: heterogeneous case
            plt::plot(t, dynamic_limit, "r--");
            plt::title("acceleration -x axis");

            plt::subplot(3, 2, 4);
            for (int qi = 0; qi < qn; qi++) {
                plt::named_plot("agent" + std::to_string(qi), t, quad_state[qi][7]);
            }
            std::fill(dynamic_limit.begin(), dynamic_limit.end(), mission.max_acc[0][1]); //TODO: heterogeneous case
            plt::plot(t, dynamic_limit, "r--");
            std::fill(dynamic_limit.begin(), dynamic_limit.end(), -mission.max_acc[0][1]); //TODO: heterogeneous case
            plt::plot(t, dynamic_limit, "r--");
            plt::title("acceleration -y axis");

            plt::subplot(3, 2, 6);
            for (int qi = 0; qi < qn; qi++) {
                plt::named_plot("agent" + std::to_string(qi), t, quad_state[qi][8]);
            }
            std::fill(dynamic_limit.begin(), dynamic_limit.end(), mission.max_acc[0][2]); //TODO: heterogeneous case
            plt::plot(t, dynamic_limit, "r--");
            std::fill(dynamic_limit.begin(), dynamic_limit.end(), -mission.max_acc[0][2]); //TODO: heterogeneous case
            plt::plot(t, dynamic_limit, "r--");
            plt::title("acceleration -z axis");

            plt::legend();
            plt::show(false);
        }

        void update_safety_margin_ratio() {
            double max_ratio_, min_ratio_, ratio;
            max_ratio.resize(t.size());
            min_ratio.resize(t.size());

            safety_margin_ratio = SP_INFINITY;
            for (int i = 0; i < t.size(); i++) {
                max_ratio_ = 0;
                min_ratio_ = SP_INFINITY;
                for (int qi = 0; qi < qn; qi++) {
                    for (int qj = qi + 1; qj < qn; qj++) {
                        ratio = sqrt(pow(quad_state[qi][0][i] - quad_state[qj][0][i], 2) +
                                     pow(quad_state[qi][1][i] - quad_state[qj][1][i], 2) +
                                     pow((quad_state[qi][2][i] - quad_state[qj][2][i]) / param.downwash, 2)) / (mission.quad_size[qi] + mission.quad_size[qj]);

                        if (ratio > max_ratio_) {
                            max_ratio_ = ratio;
                        }
                        if (ratio < min_ratio_) {
                            min_ratio_ = ratio;
                        }
                        if (ratio < safety_margin_ratio) {
                            safety_margin_ratio = ratio;
                        }
                    }
                }
                max_ratio[i] = max_ratio_;
                min_ratio[i] = min_ratio_;
            }
        }

        void plot_safety_margin_ratio() {
            plt::figure(1);
            plt::figure_size(480, 270);

            std::vector<double> collision_ratio;
            collision_ratio.resize(t.size());
            for (int i = 0; i < t.size(); i++) {
                collision_ratio[i] = 1;
            }

            plt::plot(t, collision_ratio, "r--");
//            plt::plot(t, max_ratio);
            plt::plot(t, min_ratio);

            plt::title("Safety margin ratio between Quadrotors");

            plt::show(false);
        }

//    void plot_distance_between_agents_realtime(){
//        collision_dist.resize(t.size());
//        for(int i = 0; i < t.size(); i++){
//            collision_dist[i] = 2 * mission.quad_size[0]; //TODO: heterogeous case
//        }
//
//        plt::figure(2);
//        plt::title("Ellipsoidal Distance between Quadrotor");
//        plt::figure_size(1280, 960);
//        plt::ylim(0, 5);
//        plt::named_plot("collision constraint", t, collision_dist);
//        plot_min_dist_obj.reset(new plt::Plot());
//        plt::axis("equal");
//        plt::legend();
//
//        for(int index = 0; index < t.size(); index++) {
//            std::vector<double> t_partial(t.begin(), t.begin() + index);
//            std::vector<double> min_dist_partial(min_dist.begin(), min_dist.begin() + index);
//            plot_min_dist_obj.get()->update(t_partial, min_dist_partial);
//            plt::pause(0.001);
//        }
//    }

//        void update_distance_between_agents_realtime(double current_time) {
//            int index = floor(current_time / dt);
//            double current_min_dist, alpha;
//
//            if (index + 1 >= t.size()) {
//                index = t.size() - 1;
//                current_min_dist = min_dist[index];
//            } else {
//                alpha = (current_time - index * dt) / dt;
//                current_min_dist = (1 - alpha) * min_dist[index] + alpha * min_dist[index+1];
//            }
//
//            msgs_minDist.position.x = current_min_dist;
//            msgs_minDist.position.y = 2 * mission.quad_size[0];
//        }
    };
}


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_TGSYL_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_TGSYL_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/data_order.hpp>
#include <boost/numeric/bindings/detail/array.hpp>
#include <boost/numeric/bindings/is_column_major.hpp>
#include <boost/numeric/bindings/is_complex.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/is_real.hpp>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>
#include <boost/numeric/bindings/trans_tag.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/utility/enable_if.hpp>

//
// The LAPACK-backend for tgsyl is the netlib-compatible backend.
//
#include <boost/numeric/bindings/lapack/detail/lapack.h>
#include <boost/numeric/bindings/lapack/detail/lapack_option.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace lapack {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end LAPACK-routine.
//
namespace detail {

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * float value-type.
//
template< typename Trans >
inline std::ptrdiff_t tgsyl( const Trans, const fortran_int_t ijob,
        const fortran_int_t m, const fortran_int_t n, const float* a,
        const fortran_int_t lda, const float* b, const fortran_int_t ldb,
        float* c, const fortran_int_t ldc, const float* d,
        const fortran_int_t ldd, const float* e, const fortran_int_t lde,
        float* f, const fortran_int_t ldf, float& scale, float& dif,
        float* work, const fortran_int_t lwork, fortran_int_t* iwork ) {
    fortran_int_t info(0);
    LAPACK_STGSYL( &lapack_option< Trans >::value, &ijob, &m, &n, a, &lda, b,
            &ldb, c, &ldc, d, &ldd, e, &lde, f, &ldf, &scale, &dif, work,
            &lwork, iwork, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * double value-type.
//
template< typename Trans >
inline std::ptrdiff_t tgsyl( const Trans, const fortran_int_t ijob,
        const fortran_int_t m, const fortran_int_t n, const double* a,
        const fortran_int_t lda, const double* b, const fortran_int_t ldb,
        double* c, const fortran_int_t ldc, const double* d,
        const fortran_int_t ldd, const double* e, const fortran_int_t lde,
        double* f, const fortran_int_t ldf, double& scale, double& dif,
        double* work, const fortran_int_t lwork, fortran_int_t* iwork ) {
    fortran_int_t info(0);
    LAPACK_DTGSYL( &lapack_option< Trans >::value, &ijob, &m, &n, a, &lda, b,
            &ldb, c, &ldc, d, &ldd, e, &lde, f, &ldf, &scale, &dif, work,
            &lwork, iwork, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<float> value-type.
//
template< typename Trans >
inline std::ptrdiff_t tgsyl( const Trans, const fortran_int_t ijob,
        const fortran_int_t m, const fortran_int_t n,
        const std::complex<float>* a, const fortran_int_t lda,
        const std::complex<float>* b, const fortran_int_t ldb,
        std::complex<float>* c, const fortran_int_t ldc,
        const std::complex<float>* d, const fortran_int_t ldd,
        const std::complex<float>* e, const fortran_int_t lde,
        std::complex<float>* f, const fortran_int_t ldf, float& scale,
        float& dif, std::complex<float>* work, const fortran_int_t lwork,
        fortran_int_t* iwork ) {
    fortran_int_t info(0);
    LAPACK_CTGSYL( &lapack_option< Trans >::value, &ijob, &m, &n, a, &lda, b,
            &ldb, c, &ldc, d, &ldd, e, &lde, f, &ldf, &scale, &dif, work,
            &lwork, iwork, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<double> value-type.
//
template< typename Trans >
inline std::ptrdiff_t tgsyl( const Trans, const fortran_int_t ijob,
        const fortran_int_t m, const fortran_int_t n,
        const std::complex<double>* a, const fortran_int_t lda,
        const std::complex<double>* b, const fortran_int_t ldb,
        std::complex<double>* c, const fortran_int_t ldc,
        const std::complex<double>* d, const fortran_int_t ldd,
        const std::complex<double>* e, const fortran_int_t lde,
        std::complex<double>* f, const fortran_int_t ldf, double& scale,
        double& dif, std::complex<double>* work, const fortran_int_t lwork,
        fortran_int_t* iwork ) {
    fortran_int_t info(0);
    LAPACK_ZTGSYL( &lapack_option< Trans >::value, &ijob, &m, &n, a, &lda, b,
            &ldb, c, &ldc, d, &ldd, e, &lde, f, &ldf, &scale, &dif, work,
            &lwork, iwork, &info );
    return info;
}

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to tgsyl.
//
template< typename Value, typename Enable = void >
struct tgsyl_impl {};

//
// This implementation is enabled if Value is a real type.
//
template< typename Value >
struct tgsyl_impl< Value, typename boost::enable_if< is_real< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixA, typename MatrixB, typename MatrixC,
            typename MatrixD, typename MatrixE, typename MatrixF,
            typename WORK, typename IWORK >
    static std::ptrdiff_t invoke( const fortran_int_t ijob,
            const MatrixA& a, const MatrixB& b, MatrixC& c, const MatrixD& d,
            const MatrixE& e, MatrixF& f, real_type& scale, real_type& dif,
            detail::workspace2< WORK, IWORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        typedef typename result_of::data_order< MatrixB >::type order;
        typedef typename result_of::trans_tag< MatrixA, order >::type trans;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixC >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixD >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixE >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixF >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixB >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixC >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixD >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixE >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixF >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixC >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixF >::value) );
        BOOST_ASSERT( bindings::size(work.select(fortran_int_t())) >=
                min_size_iwork( bindings::size_column_op(a, trans()),
                bindings::size_column(b) ));
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_work( bindings::size_column_op(a, trans()),
                bindings::size_column(b) ));
        BOOST_ASSERT( bindings::size_minor(a) == 1 ||
                bindings::stride_minor(a) == 1 );
        BOOST_ASSERT( bindings::size_minor(b) == 1 ||
                bindings::stride_minor(b) == 1 );
        BOOST_ASSERT( bindings::size_minor(c) == 1 ||
                bindings::stride_minor(c) == 1 );
        BOOST_ASSERT( bindings::size_minor(d) == 1 ||
                bindings::stride_minor(d) == 1 );
        BOOST_ASSERT( bindings::size_minor(e) == 1 ||
                bindings::stride_minor(e) == 1 );
        BOOST_ASSERT( bindings::size_minor(f) == 1 ||
                bindings::stride_minor(f) == 1 );
        BOOST_ASSERT( bindings::stride_major(a) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column_op(a, trans())) );
        BOOST_ASSERT( bindings::stride_major(b) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column(b)) );
        BOOST_ASSERT( bindings::stride_major(c) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column_op(a, trans())) );
        BOOST_ASSERT( bindings::stride_major(d) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column_op(a, trans())) );
        BOOST_ASSERT( bindings::stride_major(e) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column(b)) );
        BOOST_ASSERT( bindings::stride_major(f) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column_op(a, trans())) );
        return detail::tgsyl( trans(), ijob, bindings::size_column_op(a,
                trans()), bindings::size_column(b), bindings::begin_value(a),
                bindings::stride_major(a), bindings::begin_value(b),
                bindings::stride_major(b), bindings::begin_value(c),
                bindings::stride_major(c), bindings::begin_value(d),
                bindings::stride_major(d), bindings::begin_value(e),
                bindings::stride_major(e), bindings::begin_value(f),
                bindings::stride_major(f), scale, dif,
                bindings::begin_value(work.select(real_type())),
                bindings::size(work.select(real_type())),
                bindings::begin_value(work.select(fortran_int_t())) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename MatrixA, typename MatrixB, typename MatrixC,
            typename MatrixD, typename MatrixE, typename MatrixF >
    static std::ptrdiff_t invoke( const fortran_int_t ijob,
            const MatrixA& a, const MatrixB& b, MatrixC& c, const MatrixD& d,
            const MatrixE& e, MatrixF& f, real_type& scale, real_type& dif,
            minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        typedef typename result_of::data_order< MatrixB >::type order;
        typedef typename result_of::trans_tag< MatrixA, order >::type trans;
        bindings::detail::array< real_type > tmp_work( min_size_work(
                bindings::size_column_op(a, trans()),
                bindings::size_column(b) ) );
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( bindings::size_column_op(a, trans()),
                bindings::size_column(b) ) );
        return invoke( ijob, a, b, c, d, e, f, scale, dif,
                workspace( tmp_work, tmp_iwork ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename MatrixA, typename MatrixB, typename MatrixC,
            typename MatrixD, typename MatrixE, typename MatrixF >
    static std::ptrdiff_t invoke( const fortran_int_t ijob,
            const MatrixA& a, const MatrixB& b, MatrixC& c, const MatrixD& d,
            const MatrixE& e, MatrixF& f, real_type& scale, real_type& dif,
            optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        typedef typename result_of::data_order< MatrixB >::type order;
        typedef typename result_of::trans_tag< MatrixA, order >::type trans;
        real_type opt_size_work;
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( bindings::size_column_op(a, trans()),
                bindings::size_column(b) ) );
        detail::tgsyl( trans(), ijob, bindings::size_column_op(a,
                trans()), bindings::size_column(b), bindings::begin_value(a),
                bindings::stride_major(a), bindings::begin_value(b),
                bindings::stride_major(b), bindings::begin_value(c),
                bindings::stride_major(c), bindings::begin_value(d),
                bindings::stride_major(d), bindings::begin_value(e),
                bindings::stride_major(e), bindings::begin_value(f),
                bindings::stride_major(f), scale, dif, &opt_size_work, -1,
                bindings::begin_value(tmp_iwork) );
        bindings::detail::array< real_type > tmp_work(
                traits::detail::to_int( opt_size_work ) );
        return invoke( ijob, a, b, c, d, e, f, scale, dif,
                workspace( tmp_work, tmp_iwork ) );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const std::ptrdiff_t m,
            const std::ptrdiff_t n ) {
        return std::max< std::ptrdiff_t >(1,2*m*n);
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array iwork.
    //
    static std::ptrdiff_t min_size_iwork( const std::ptrdiff_t m,
            const std::ptrdiff_t n ) {
        return m+n+6;
    }
};

//
// This implementation is enabled if Value is a complex type.
//
template< typename Value >
struct tgsyl_impl< Value, typename boost::enable_if< is_complex< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixA, typename MatrixB, typename MatrixC,
            typename MatrixD, typename MatrixE, typename MatrixF,
            typename WORK, typename IWORK >
    static std::ptrdiff_t invoke( const fortran_int_t ijob,
            const MatrixA& a, const MatrixB& b, MatrixC& c, const MatrixD& d,
            const MatrixE& e, MatrixF& f, real_type& scale, real_type& dif,
            detail::workspace2< WORK, IWORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        typedef typename result_of::data_order< MatrixB >::type order;
        typedef typename result_of::trans_tag< MatrixA, order >::type trans;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixC >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixD >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixE >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixF >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixB >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixC >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixD >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixE >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixA >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixF >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixC >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixF >::value) );
        BOOST_ASSERT( bindings::size(work.select(fortran_int_t())) >=
                min_size_iwork( bindings::size_column_op(a, trans()),
                bindings::size_column(b) ));
        BOOST_ASSERT( bindings::size(work.select(value_type())) >=
                min_size_work( bindings::size_column_op(a, trans()),
                bindings::size_column(b) ));
        BOOST_ASSERT( bindings::size_minor(a) == 1 ||
                bindings::stride_minor(a) == 1 );
        BOOST_ASSERT( bindings::size_minor(b) == 1 ||
                bindings::stride_minor(b) == 1 );
        BOOST_ASSERT( bindings::size_minor(c) == 1 ||
                bindings::stride_minor(c) == 1 );
        BOOST_ASSERT( bindings::size_minor(d) == 1 ||
                bindings::stride_minor(d) == 1 );
        BOOST_ASSERT( bindings::size_minor(e) == 1 ||
                bindings::stride_minor(e) == 1 );
        BOOST_ASSERT( bindings::size_minor(f) == 1 ||
                bindings::stride_minor(f) == 1 );
        BOOST_ASSERT( bindings::stride_major(a) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column_op(a, trans())) );
        BOOST_ASSERT( bindings::stride_major(b) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column(b)) );
        BOOST_ASSERT( bindings::stride_major(c) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column_op(a, trans())) );
        BOOST_ASSERT( bindings::stride_major(d) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column_op(a, trans())) );
        BOOST_ASSERT( bindings::stride_major(e) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column(b)) );
        BOOST_ASSERT( bindings::stride_major(f) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column_op(a, trans())) );
        return detail::tgsyl( trans(), ijob, bindings::size_column_op(a,
                trans()), bindings::size_column(b), bindings::begin_value(a),
                bindings::stride_major(a), bindings::begin_value(b),
                bindings::stride_major(b), bindings::begin_value(c),
                bindings::stride_major(c), bindings::begin_value(d),
                bindings::stride_major(d), bindings::begin_value(e),
                bindings::stride_major(e), bindings::begin_value(f),
                bindings::stride_major(f), scale, dif,
                bindings::begin_value(work.select(value_type())),
                bindings::size(work.select(value_type())),
                bindings::begin_value(work.select(fortran_int_t())) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename MatrixA, typename MatrixB, typename MatrixC,
            typename MatrixD, typename MatrixE, typename MatrixF >
    static std::ptrdiff_t invoke( const fortran_int_t ijob,
            const MatrixA& a, const MatrixB& b, MatrixC& c, const MatrixD& d,
            const MatrixE& e, MatrixF& f, real_type& scale, real_type& dif,
            minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        typedef typename result_of::data_order< MatrixB >::type order;
        typedef typename result_of::trans_tag< MatrixA, order >::type trans;
        bindings::detail::array< value_type > tmp_work( min_size_work(
                bindings::size_column_op(a, trans()),
                bindings::size_column(b) ) );
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( bindings::size_column_op(a, trans()),
                bindings::size_column(b) ) );
        return invoke( ijob, a, b, c, d, e, f, scale, dif,
                workspace( tmp_work, tmp_iwork ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename MatrixA, typename MatrixB, typename MatrixC,
            typename MatrixD, typename MatrixE, typename MatrixF >
    static std::ptrdiff_t invoke( const fortran_int_t ijob,
            const MatrixA& a, const MatrixB& b, MatrixC& c, const MatrixD& d,
            const MatrixE& e, MatrixF& f, real_type& scale, real_type& dif,
            optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        typedef typename result_of::data_order< MatrixB >::type order;
        typedef typename result_of::trans_tag< MatrixA, order >::type trans;
        value_type opt_size_work;
        bindings::detail::array< fortran_int_t > tmp_iwork(
                min_size_iwork( bindings::size_column_op(a, trans()),
                bindings::size_column(b) ) );
        detail::tgsyl( trans(), ijob, bindings::size_column_op(a,
                trans()), bindings::size_column(b), bindings::begin_value(a),
                bindings::stride_major(a), bindings::begin_value(b),
                bindings::stride_major(b), bindings::begin_value(c),
                bindings::stride_major(c), bindings::begin_value(d),
                bindings::stride_major(d), bindings::begin_value(e),
                bindings::stride_major(e), bindings::begin_value(f),
                bindings::stride_major(f), scale, dif, &opt_size_work, -1,
                bindings::begin_value(tmp_iwork) );
        bindings::detail::array< value_type > tmp_work(
                traits::detail::to_int( opt_size_work ) );
        return invoke( ijob, a, b, c, d, e, f, scale, dif,
                workspace( tmp_work, tmp_iwork ) );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const std::ptrdiff_t m,
            const std::ptrdiff_t n ) {
        return std::max< std::ptrdiff_t >(1,2*m*n);
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array iwork.
    //
    static std::ptrdiff_t min_size_iwork( const std::ptrdiff_t m,
            const std::ptrdiff_t n ) {
        return m+n+2;
    }
};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. In
// addition, if applicable, they are overloaded for user-defined workspaces.
// Calls to these functions are passed to the tgsyl_impl classes. In the
// documentation, most overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for tgsyl. Its overload differs for
// * User-defined workspace
//
template< typename MatrixA, typename MatrixB, typename MatrixC,
        typename MatrixD, typename MatrixE, typename MatrixF,
        typename Workspace >
inline typename boost::enable_if< detail::is_workspace< Workspace >,
        std::ptrdiff_t >::type
tgsyl( const fortran_int_t ijob, const MatrixA& a, const MatrixB& b,
        MatrixC& c, const MatrixD& d, const MatrixE& e, MatrixF& f,
        typename remove_imaginary< typename bindings::value_type<
        MatrixA >::type >::type& scale, typename remove_imaginary<
        typename bindings::value_type< MatrixA >::type >::type& dif,
        Workspace work ) {
    return tgsyl_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( ijob, a, b, c, d, e, f, scale, dif,
            work );
}

//
// Overloaded function for tgsyl. Its overload differs for
// * Default workspace-type (optimal)
//
template< typename MatrixA, typename MatrixB, typename MatrixC,
        typename MatrixD, typename MatrixE, typename MatrixF >
inline typename boost::disable_if< detail::is_workspace< MatrixF >,
        std::ptrdiff_t >::type
tgsyl( const fortran_int_t ijob, const MatrixA& a, const MatrixB& b,
        MatrixC& c, const MatrixD& d, const MatrixE& e, MatrixF& f,
        typename remove_imaginary< typename bindings::value_type<
        MatrixA >::type >::type& scale, typename remove_imaginary<
        typename bindings::value_type< MatrixA >::type >::type& dif ) {
    return tgsyl_impl< typename bindings::value_type<
            MatrixA >::type >::invoke( ijob, a, b, c, d, e, f, scale, dif,
            optimal_workspace() );
}

} // namespace lapack
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif


/* ------------------------------------------------------------------------- */
/* UMFPACK Version 4.1 (Apr. 30, 2003), Copyright (c) 2003 by Timothy A.     */
/* Davis.  All Rights Reserved.  See ../README for License.                  */
/* email: davis@cise.ufl.edu    CISE Department, Univ. of Florida.           */
/* web: http://www.cise.ufl.edu/research/sparse/umfpack                      */
/* ------------------------------------------------------------------------- */

/***********************************************************************/
/*         UMFPACK Copyright, License and Availability                 */
/***********************************************************************/
/*
 *
 * UMFPACK Version 4.1 (Apr. 30, 2003),  Copyright (c) 2003 by Timothy A.
 * Davis.  All Rights Reserved.
 *
 * UMFPACK License:
 *
 *   Your use or distribution of UMFPACK or any modified version of
 *   UMFPACK implies that you agree to this License.
 *
 *   THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY
 *   EXPRESSED OR IMPLIED.  ANY USE IS AT YOUR OWN RISK.
 *
 *   Permission is hereby granted to use or copy this program, provided
 *   that the Copyright, this License, and the Availability of the original
 *   version is retained on all copies.  User documentation of any code that
 *   uses UMFPACK or any modified version of UMFPACK code must cite the
 *   Copyright, this License, the Availability note, and "Used by permission."
 *   Permission to modify the code and to distribute modified code is granted,
 *   provided the Copyright, this License, and the Availability note are
 *   retained, and a notice that the code was modified is included.  This
 *   software was developed with support from the National Science Foundation,
 *   and is provided to you free of charge.
 *
 * Availability:
 *
 *   http://www.cise.ufl.edu/research/sparse/umfpack
 *
 */

/* Used by permission. */

/* Simple demo program for UMFPACK               */
/* from UMFPACK Version 4.1 Quick Start Guide    */

/* modified by Kresimir Fresl, 2003              */
/* UMFPACK bindings & ublas::compressed_matrix<> */

#if !defined(TEST_MATLIB_UBLAS) && !defined(TEST_MATLIB_GLAS) && !defined(TEST_MATLIB_MTL) && !defined(TEST_MATLIB_EIGEN)
#define TEST_MATLIB_UBLAS
#endif

#include <iostream>
#include <boost/numeric/bindings/umfpack/umfpack.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#if defined(TEST_MATLIB_UBLAS)

#include <boost/numeric/bindings/ublas/matrix_sparse.hpp>
namespace ublas = boost::numeric::ublas;
typedef ublas::compressed_matrix<double, ublas::column_major, 0,
    ublas::unbounded_array<int>, ublas::unbounded_array<double> > m_t;
typedef m_t& mi_t;
#define AA(i,j, val) a(i,j) = val

#elif defined(TEST_MATLIB_GLAS)

#include <boost/numeric/bindings/glas/compressed.hpp>
typedef glas::sparse_matrix< double, glas::compressed_sparse_structure<
    glas::column_orientation, std::ptrdiff_t, std::ptrdiff_t, 0> > m_t;
typedef m_t& mi_t;
#define AA(i,j, val) insert(a,i,j, val)

#elif defined(TEST_MATLIB_MTL)

#include <boost/numeric/bindings/mtl/compressed2D.hpp>
typedef mtl::compressed2D<double, mtl::matrix::parameters<mtl::tag::col_major> > m_t;
typedef mtl::matrix::inserter<m_t> mi_t;
#define AA(i,j, val) a(i,j) = val

#elif defined(TEST_MATLIB_EIGEN)

#include <boost/numeric/bindings/eigen/sparsematrix.hpp>
typedef Eigen::SparseMatrix<double> m_t;
typedef Eigen::RandomSetter<m_t> mi_t;
#define AA(i,j, val) a(i,j) = val

#endif

typedef boost::numeric::ublas::vector<double> v_t;
namespace umf = boost::numeric::bindings::umfpack;

int main() {

  m_t A (5,5
#if !defined(TEST_MATLIB_EIGEN)
        ,12
#endif
        );
  v_t B (5), X (5);

  {
    mi_t a(A);
    AA(0,0, 2.); AA(0,1, 3.);
    AA(1,0, 3.); AA(1,2, 4.); AA(1,4, 6.);
    AA(2,1,-1.); AA(2,2,-3.); AA(2,3, 2.);
    AA(3,2, 1.);
    AA(4,1, 4.); AA(4,2, 2.); AA(4,4, 1.);
  }
  B(0) = 8.; B(1) = 45.; B(2) = -3.; B(3) = 3.; B(4) = 19.;

  umf::symbolic_type<double> Symbolic;
  umf::numeric_type<double> Numeric;

  umf::symbolic (A, Symbolic);
  umf::numeric (A, Symbolic, Numeric);
  umf::solve (A, X, B, Numeric);

  std::cout << X << std::endl;  // output: [5](1,2,3,4,5)
}


#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <iterator>
#include <numeric>
#include <memory>
#include <chrono>

#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "TestSet.hpp"
#include "BVNS.hpp"
#include "VNS.hpp"
#include "Solution.hpp"
#include "Helper.hpp"

using namespace std;
namespace ublas = boost::numeric::ublas;

/*********************************************************************************/
/*                  CONSTRUCTOR AND MEMBER FUNCTIONS OF BVNS                     */
/*********************************************************************************/

// Init BVNS Heuristic
BVNS::BVNS(const string &directory,
	const unsigned int &kMin,
	const unsigned int &kMax,
	const unsigned int &tMax,
	const unsigned int &shaking_mode,
	const unsigned int &local_Search_mode,
	const unsigned int &update_xij_mode,
	const unsigned int &initial_mode,
	const unsigned int &time):
		VNS(directory, kMin, kMax, tMax, shaking_mode,
			local_Search_mode, update_xij_mode, initial_mode, time),
		m_local_solution(m_locationNumber, 1)
{
	chrono::steady_clock::time_point timeStart = chrono::high_resolution_clock::now();
	initialSolution(m_incumbent_solution, m_bi, m_fi, m_dj, m_cij, m_sum_dj, m_initial_mode);

	unsigned int counter = 0, t = 0;
	bool stop = false;

	for (size_t i = 0; i != tMax && !stop; ++i)
	{
		// For all N_k Structures
		m_k = m_kMin - 1;
		while (m_k < m_kMax && !stop)
		{
			// Shake incumbent Solution
			m_perturbed_solution = shaking(m_incumbent_solution,
				m_bi, m_dj, m_cij, m_k, m_fx, m_shaking_mode);
			// Update Neighborhood Structure N_k(S')
			updateNeighborhoods(m_neighborhood_k, m_perturbed_solution, m_bi,
				m_sum_dj, m_shaking_mode, m_k);
			// Generate Local Solution
			m_local_solution = localSearch(m_perturbed_solution,
				m_neighborhood_k, m_k, m_fx, m_local_search_mode);
			// Move or not
			neighborhoodChange(m_incumbent_solution, m_local_solution, m_k, m_fx, m_best_fx, m_kMin);

			// Check if maximum COmputation Time is exeeded
			chrono::steady_clock::time_point bvns_step = chrono::high_resolution_clock::now();
			double time_end = (double)chrono::duration_cast<chrono::microseconds>(bvns_step - timeStart).count();
			if (time_end >= m_timeMax * 1000000.0)
			{
				cout << "Time Limit:" << m_timeMax << " exceeded." << endl;
				stop = true;
			}
		}
	}

	chrono::steady_clock::time_point timeEnd = chrono::high_resolution_clock::now();

	// Compute Optimal MODI Value
	updateXij(m_cij, m_dj, m_bi, m_incumbent_solution, m_update_xij_mode, m_best_fx);

	// Save Solution and Timestamps to File
	printFinalSolution(m_incumbent_solution, m_best_fx, m_kMax, m_tMax, m_shaking_mode,
		m_local_search_mode, m_update_xij_mode, m_initial_mode, m_directory, timeStart, timeEnd);
}

vector<bool> BVNS::localSearch(
	const vector<bool> &perturbed_solution,
	const vector<vector<bool>> &neighborhood_k,
	const unsigned int &k,
	double &fx,
	const unsigned int &local_search_mode)
{
	if (local_search_mode == 0)
		return localSearchFirstImprovment(perturbed_solution, neighborhood_k, k, fx);
	else
		return localSearchBestImprovment(perturbed_solution, neighborhood_k, k, fx);
}


/* vim: set sw=4 sts=4 et foldmethod=syntax : */

#include "dimacs.hh"
#include "graph.hh"
#include "graph_file_error.hh"

#include <boost/regex.hpp>
#include <fstream>

using std::getline;
using std::ifstream;
using std::stoi;
using std::string;

using boost::regex;
using boost::smatch;

auto read_dimacs(const string & filename) -> Graph
{
    Graph result{ 0 };

    ifstream infile{ filename };
    if (! infile)
        throw GraphFileError{ filename, "unable to open file" };

    string line;
    while (getline(infile, line)) {
        if (line.empty())
            continue;

        /* Lines are comments, a problem description (contains the number of
         * vertices), or an edge. */
        static const regex
            comment{ R"(c(\s.*)?)" },
            problem{ R"(p\s+(edge|col)\s+(\d+)\s+(\d+)?\s*)" },
            edge{ R"(e\s+(\d+)\s+(\d+)\s*)" };

        smatch match;
        if (regex_match(line, match, comment)) {
            /* Comment, ignore */
        }
        else if (regex_match(line, match, problem)) {
            /* Problem. Specifies the size of the graph. Must happen exactly
             * once. */
            if (0 != result.size())
                throw GraphFileError{ filename, "multiple 'p' lines encountered" };
            result.resize(stoi(match.str(2)));
        }
        else if (regex_match(line, match, edge)) {
            /* An edge. DIMACS files are 1-indexed. We assume we've already had
             * a problem line (if not our size will be 0, so we'll throw). */
            int a{ stoi(match.str(1)) }, b{ stoi(match.str(2)) };
            if (0 == a || 0 == b || a > result.size() || b > result.size())
                throw GraphFileError{ filename, "line '" + line + "' edge index out of bounds" };
            result.add_edge(a - 1, b - 1);
        }
        else
            throw GraphFileError{ filename, "cannot parse line '" + line + "'" };
    }

    if (! infile.eof())
        throw GraphFileError{ filename, "error reading file" };

    return result;
}


/*=========================================================================
 Program:   ORFEO Toolbox
 Language:  C++
 Date:      $Date$
 Version:   $Revision$
 Created By:Bouceffa Walid
 Email:	    bouceffa.walid@gmail.com

 Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
 See OTBCopyright.txt for details.

 This software is distributed WITHOUT ANY WARRANTY; without even
 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 PURPOSE.  See the above copyright notices for more information.

 =========================================================================*/

#include "otbFeatureSelection.h"

#include <boost/smart_ptr.hpp>
#include <exception>
#include <iostream>
#include <cstdlib>
#include <string>
#include <vector>
#include "error.hpp"
#include "global.hpp"
#include "subset.hpp"

#include "data_intervaller.hpp"
#include "data_splitter.hpp"
#include "data_splitter_5050.hpp"
#include "data_splitter_cv.hpp"
//#include "data_splitter_holdout.hpp"
//#include "data_splitter_leave1out.hpp"
//#include "data_splitter_resub.hpp"
#include "data_splitter_randrand.hpp"
//#include "data_splitter_randfix.hpp"
#include "data_scaler.hpp"
#include "data_scaler_void.hpp"
//#include "data_scaler_to01.hpp"
//#include "data_scaler_white.hpp"

#include "data_accessor_splitting_memOTB.hpp"

#include "criterion_normal_bhattacharyya.hpp"
//#include "criterion_normal_gmahalanobis.hpp"
//#include "criterion_normal_divergence.hpp"
//#include "criterion_multinom_bhattacharyya.hpp"
#include "criterion_wrapper.hpp"
//#include "criterion_wrapper_bias_estimate.hpp"
//#include "criterion_subsetsize.hpp"
//#include "criterion_sumofweights.hpp"
//#include "criterion_negative.hpp"

#include "distance_euclid.hpp"
//#include "distance_L1.hpp"
#include "distance_Lp.hpp"
#include "classifier_knn.hpp"
//#include "classifier_normal_bayes.hpp"
//#include "classifier_multinom_naivebayes.hpp"
#include "classifier_svm.hpp"

//#include "search_bif.hpp"
//#include "search_bif_threaded.hpp"

//#include "search_exhaustive.hpp"
//#include "search_exhaustive_threaded.hpp"
#include "branch_and_bound_predictor_averaging.hpp"
//#include "search_branch_and_bound_basic.hpp"
#include "search_branch_and_bound_improved.hpp"
#include "search_branch_and_bound_partial_prediction.hpp"
#include "search_branch_and_bound_fast.hpp"
#include "seq_step_straight.hpp"
//#include "seq_step_straight_threaded.hpp"
//#include "seq_step_hybrid.hpp"
//#include "seq_step_ensemble.hpp"
//#include "search_seq_sfs.hpp"
#include "search_seq_sffs.hpp"
//#include "search_seq_sfrs.hpp"
//#include "search_seq_os.hpp"
#include "search_seq_dos.hpp"
#include "result_tracker_dupless.hpp"
//#include "result_tracker_regularizer.hpp"
#include "result_tracker_feature_stats.hpp"
//#include "result_tracker_stabileval.hpp"

namespace otb
{
namespace Wrapper
{

typedef double RETURNTYPE;
typedef float DATATYPE;
typedef double REALTYPE;
typedef int IDXTYPE;
typedef unsigned int DIMTYPE;
typedef short BINTYPE;
typedef FST::Subset<BINTYPE, DIMTYPE> SUBSET;

typedef FST::Data_Intervaller<std::vector<FST::Data_Interval<IDXTYPE> >,IDXTYPE> INTERVALLER;
typedef boost::shared_ptr<FST::Data_Splitter<INTERVALLER,IDXTYPE> > PSPLITTER;
typedef FST::Data_Splitter_CV<INTERVALLER,IDXTYPE> SPLITTERCV;
typedef FST::Data_Splitter_5050<INTERVALLER,IDXTYPE> SPLITTER5050;
typedef FST::Data_Splitter_RandomRandom<INTERVALLER,IDXTYPE,BINTYPE> SPLITTERRR;
typedef FST::Data_Accessor_Splitting_MemOTB<DATATYPE,IDXTYPE,INTERVALLER> DATAACCESSOR;
typedef FST::Distance_Euclid<DATATYPE,DIMTYPE,SUBSET> DISTANCE;

typedef FST::Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE> CLASSIFIERKNN;
typedef FST::Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> CLASSIFIERSVM;

typedef FST::Criterion_Wrapper<RETURNTYPE,SUBSET,CLASSIFIERKNN,DATAACCESSOR> WRAPPERKNN;
typedef FST::Criterion_Wrapper<RETURNTYPE,SUBSET,CLASSIFIERSVM,DATAACCESSOR> WRAPPERSVM;

typedef FST::Sequential_Step_Straight<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERKNN> EVALUATOR;
typedef FST::Sequential_Step_Straight<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERSVM> EVALUATOR_SVM;

typedef FST::Result_Tracker_Feature_Stats<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET> TRACKERSTATS;

void FeatureSelection::InitFSTParams()
{

   AddChoice ( "method.sffs", "sffs Wrapper-based feature selection with Floating Search" );
   AddParameter ( ParameterType_Choice, "method.sffs.searchdirection", "Sequential Forward Floating Selection search procedure" );
   AddChoice("method.sffs.searchdirection.forward", "forward search ");
   AddChoice("method.sffs.searchdirection.backward", "backward search ");
   SetParameterString("method.sffs.searchdirection", "forward");

   AddChoice ( "method.dos", "dos Combined feature subset contents, size and SVM parameters optimization.." );
   AddParameter ( ParameterType_Choice, "method.dos.svmkernel", "svm kernel to use " );
   AddChoice("method.dos.svmkernel.linear", "linear kernel");
   AddChoice("method.dos.svmkernel.rbf", "rbf kernel  ");
   AddChoice("method.dos.svmkernel.poly", "poly kernel ");
   SetParameterString("method.dos.svmkernel", "rbf");
}

FeatureSelection::ImportanceVector FeatureSelection::getFeatureImportanceDOS ( ListSampleType::Pointer trainingListSample,
        LabelListSampleType::Pointer trainingLabeledListSample )
{

          std::cout << "Starting (Missing data substitution) Combined feature subset contents, size and SVM parameters optimization..." << std::endl;
        // randomly sample 50% of data for training and randomly sample (disjunct) 40% for independent testing of final classification performance
        PSPLITTER dsp_outer ( new SPLITTERRR ( 1, 50, 40 ) ); // (there will be one outer randomized split only)
        // in the course of search use the first half of data by 3-fold cross-validation in wrapper FS criterion evaluation
        PSPLITTER dsp_inner ( new SPLITTERCV ( 3 ) );
        // do not scale data
        const DATATYPE missing_value_code=5;
        boost::shared_ptr<FST::Data_Scaler<DATATYPE> > dsc ( new FST::Data_Scaler_void<DATATYPE> ( missing_value_code,1/*to choose correct constructor*/ ) );
        // set-up data access
        boost::shared_ptr<std::vector<PSPLITTER> > splitters ( new std::vector<PSPLITTER> );
        splitters->push_back ( dsp_outer );
        splitters->push_back ( dsp_inner );
        boost::shared_ptr<DATAACCESSOR> da ( new DATAACCESSOR ( trainingListSample,trainingLabeledListSample,splitters,dsc ) );
        da->initialize();
        // initiate access to split data parts
        da->setSplittingDepth ( 0 );
        if ( !da->getFirstSplit() ) throw FST::fst_error ( "50/40 random data split failed." );
        da->setSplittingDepth ( 1 );
        if ( !da->getFirstSplit() ) throw FST::fst_error ( "3-fold cross-validation failure." );
        // initiate the storage for subset to-be-selected + another one as temporary storage
        boost::shared_ptr<SUBSET> sub ( new SUBSET ( da->getNoOfFeatures() ) );
        boost::shared_ptr<SUBSET> sub_temp ( new SUBSET ( da->getNoOfFeatures() ) );
        // set-up SVM (interface to external library LibSVM)
        boost::shared_ptr<CLASSIFIERSVM> csvm ( new CLASSIFIERSVM );

        //csvm->set_kernel_type ( RBF ); // (option: LINEAR, RBF, POLY)

        std::string svm_kernel  = GetParameterAsString("method.dos.svmkernel");

	if( svm_kernel == "linear")
	    csvm->set_kernel_type ( LINEAR );

	else if( svm_kernel == "poly")
	   csvm->set_kernel_type ( POLY );

	else
	     csvm->set_kernel_type ( RBF );

        csvm->initialize ( da );
        // wrap the SVM classifier to enable its usage as FS criterion (criterion value will be estimated by 3-fold cross-val.)
        boost::shared_ptr<WRAPPERSVM> wsvm ( new WRAPPERSVM );
        wsvm->initialize ( csvm,da );
        // set-up the standard sequential search step object (option: hybrid, ensemble)
        boost::shared_ptr<EVALUATOR_SVM> eval ( new EVALUATOR_SVM );
        // set-up Dynamic Oscillating Search procedure
        FST::Search_DOS<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERSVM,EVALUATOR_SVM> srch ( eval );
        srch.set_delta ( 3 );
        // run the search
        std::cout << "Feature selection setup:" << std::endl << *da << std::endl << srch << std::endl << *wsvm << std::endl << std::endl;
        RETURNTYPE bestcritval_train, critval_train, critval_test;
        sub->select_all();
        csvm->optimize_parameters ( da,sub );
        double best_svm_param_C=csvm->get_parameter_C();
        double best_svm_param_gamma=csvm->get_parameter_gamma();
        double best_svm_param_coef0=csvm->get_parameter_coef0();
        bool stop=false;
        sub->deselect_all();
        if ( !srch.search ( 0,bestcritval_train,sub,wsvm, std::cout ) ) throw FST::fst_error ( "Search not finished." );

        /**
        		sub_temp->stateless_copy(*sub);
        		while(!stop)
        		{
        			csvm->optimize_parameters(da,sub);
        			if(!srch.search(0,critval_train,sub_temp,wsvm,std::cout)) throw FST::fst_error("Search not finished.");
        			if(critval_train>bestcritval_train)
        			{
        				bestcritval_train=critval_train;
        				sub->stateless_copy(*sub_temp);
        				best_svm_param_C=csvm->get_parameter_C();
        				best_svm_param_gamma=csvm->get_parameter_gamma();
        				best_svm_param_coef0=csvm->get_parameter_coef0();
        			} else stop=true;
        		}
        		std::cout << std::endl << "Search result: " << std::endl << *sub << std::endl << "Criterion value=" << bestcritval_train << std::endl << std::endl;
        **/
        // 	// (optionally) validate result by estimating SVM accuracy on selected feature sub-space on independent test data
// 		da->setSplittingDepth(0);
// 		csvm->set_parameter_C(best_svm_param_C);
// 		csvm->set_parameter_gamma(best_svm_param_gamma);
// 		csvm->set_parameter_coef0(best_svm_param_coef0);
// 		csvm->train(da,sub);
// 		csvm->test(critval_test,da);
// 		std::cout << "Validated SVM accuracy=" << critval_test << std::endl << std::endl;
//

ImportanceVector vImp;
        vImp.reserve ( sub->get_n() );
        for ( DIMTYPE d=0; d<sub->get_n(); d++ )
        {
            if ( sub->selected_raw ( d ) )
                vImp.push_back ( std::make_pair ( d+1, 1 ) );
            else
                vImp.push_back ( std::make_pair ( d+1, 0 ) );
        }

  return vImp;
}

FeatureSelection::ImportanceVector FeatureSelection::getFeatureImportanceSFFS ( ListSampleType::Pointer trainingListSample,
        LabelListSampleType::Pointer trainingLabeledListSample )
{

  // otbAppLogINFO ( "SFFS GetFeatureImporance " );

    std::cout << "Generalized sequential feature subset search..." << std::endl;
        // keep second half of data for independent testing of final classification performance
        PSPLITTER dsp_outer ( new SPLITTER5050() );
        // in the course of search use the first half of data by 3-fold cross-validation in wrapper FS criterion evaluation
        PSPLITTER dsp_inner ( new SPLITTERCV ( 3 ) );
        // do not scale data
        boost::shared_ptr<FST::Data_Scaler<DATATYPE> > dsc ( new FST::Data_Scaler_void<DATATYPE>() );
        // set-up data access
        boost::shared_ptr<std::vector<PSPLITTER> > splitters ( new std::vector<PSPLITTER> );
        splitters->push_back ( dsp_outer );
        splitters->push_back ( dsp_inner );

        //boost::shared_ptr<DATAACCESSOR> da(new DATAACCESSOR("data/speech_15.trn",splitters,dsc));
        boost::shared_ptr<DATAACCESSOR> da ( new DATAACCESSOR ( trainingListSample,trainingLabeledListSample,splitters,dsc ) );

        da->initialize();
        // initiate access to split data parts
        da->setSplittingDepth ( 0 );
        if ( !da->getFirstSplit() ) throw FST::fst_error ( "50/50 data split failed." );
        da->setSplittingDepth ( 1 );
        if ( !da->getFirstSplit() ) throw FST::fst_error ( "3-fold cross-validation failure." );
        // initiate the storage for subset to-be-selected
        boost::shared_ptr<SUBSET> sub ( new SUBSET ( da->getNoOfFeatures() ) );
        sub->deselect_all();
        // set-up 3-Nearest Neighbor classifier based on Euclidean distances
        boost::shared_ptr<CLASSIFIERKNN> cknn ( new CLASSIFIERKNN );
        cknn->set_k ( 5 );
        // wrap the 3-NN classifier to enable its usage as FS criterion (criterion value will be estimated by 3-fold cross-val.)
        boost::shared_ptr<WRAPPERKNN> wknn ( new WRAPPERKNN );
        wknn->initialize ( cknn,da );
        // set-up the standard sequential search step object (option: hybrid, ensemble, etc.)
        boost::shared_ptr<EVALUATOR> eval ( new EVALUATOR );
        // set-up Sequential Forward Floating Selection search procedure
        FST::Search_SFFS<RETURNTYPE,DIMTYPE,SUBSET,WRAPPERKNN,EVALUATOR> srch ( eval );

	//srch.set_search_direction ( FST::FORWARD ); // try FST::BACKWARD
      std::string search_method = GetParameterAsString("method.sffs.searchdirection") ;

      if(search_method == "backward")
	srch.set_search_direction ( FST::BACKWARD );
      else
	srch.set_search_direction ( FST::FORWARD );

      // set the size of feature groups to be evaluated for inclusion/removal in each sequential step (can be applied to SFS, SFFS, OS, DOS, SFRS)
        srch.set_generalization_level ( 2 );
        // run the search
        std::cout << "Feature selection setup:" << std::endl << *da << std::endl << srch << std::endl << *wknn << std::endl << std::endl;
        RETURNTYPE critval_train, critval_test;
        srch.set_output_detail ( FST::NORMAL ); // set FST::SILENT to disable all text output in the course of search (FST::NORMAL is default)
        if ( !srch.search ( 0,critval_train,sub,wknn,std::cout ) ) throw FST::fst_error ( "Search not finished." );
        // (optionally) validate result by estimating kNN accuracy on selected feature sub-space on independent test data
        //da->setSplittingDepth(0);
        //cknn->train(da,sub);
        //cknn->test(critval_test,da);
        //std::cout << "Validated "<<cknn->get_k()<<"-NN accuracy=" << critval_test << std::endl << std::endl;
        // (optionally) list the best known solutions for each cardinality as recorded throughout the course of search
        //std::cout << "Best recorded solution for subset size:" << std::endl;
        //for(DIMTYPE d=1;d<=sub->get_n();d++)
        //if(srch.get_result(d,critval_train,sub)) std::cout << d << ": val="<< critval_train << ", "<<*sub << std::endl;

	ImportanceVector vImp;

        vImp.reserve ( sub->get_n() );
        for ( DIMTYPE d=0; d<sub->get_n(); d++ )
        {
            if ( sub->selected_raw ( d ) )
                vImp.push_back ( std::make_pair ( d+1, 1 ) );
            else
                vImp.push_back ( std::make_pair ( d+1, 0 ) );
        }

    return vImp;
}

} //end namespace wrapper
} //end namespace otb


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
/*!
    @ingroup group-swar
    This function object performs the extraction of the first part of a boost::simd::pack
    containing two channels of data.

    @par Semantic:
    For any type @c T and any integral constant @c N greater than 1, let's consider two values @c x
    and @c y of type @c boost::simd::pack<T,N> , the following code:
    @code
    boost::simd::pack<T,N> r = deinterleave_first(x,y);
    @endcode
    is equivalent to :
    @code
    boost::simd::pack<T,N> r{x[0], x[2], ..., x[N/2], y[0], y[2], ...,y[N/2]};
    @endcode

    @param  v0  First part of the data to deinterleave
    @param  v1  Second part of the data to deinterleave
    @return A boost::simd::pack containing the deinterleaved value of the first data channel.
  **/
  Value deinterleave_first(Value const& v0, Value const& v1);
} }
#endif

#include <boost/simd/function/simd/deinterleave_first.hpp>

#endif


// Copyright (C) 2015 The Regents of the University of California (Regents).
// 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 Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <Eigen/Core>
#include <glog/logging.h>
#include <gflags/gflags.h>
#include <theia/theia.h>

#include <memory>
#include <string>

DEFINE_string(1dsfm_dataset_directory, "",
              "Dataset where the 1dSFM dataset is located. Do not include a "
              "trailing slash.");

using theia::Reconstruction;
using theia::TrackId;
using theia::ViewId;

// Computes the error in the relative rotation based on the ground truth
// rotations rotation1 and rotation2 (which specify world-to-camera
// transformations).
double ComputeRelativeRotationError(const Eigen::Matrix3d& rotation1,
                                    const Eigen::Matrix3d& rotation2,
                                    const Eigen::Vector3d& relative_rotation) {
  Eigen::Matrix3d relative_rotation_matrix;
  ceres::AngleAxisToRotationMatrix(
      relative_rotation.data(),
      ceres::ColumnMajorAdapter3x3(relative_rotation_matrix.data()));
  const Eigen::Matrix3d loop_rotation = relative_rotation_matrix.transpose() *
                                        (rotation2 * rotation1.transpose());
  Eigen::Vector3d loop_rotation_aa;
  ceres::RotationMatrixToAngleAxis(
      ceres::ColumnMajorAdapter3x3(loop_rotation.data()),
      loop_rotation_aa.data());
  return theia::RadToDeg(loop_rotation_aa.norm());
}

double ComputeRelativeTranslationError(
    const Eigen::Vector3d& position1,
    const Eigen::Vector3d& position2,
    const Eigen::Matrix3d& rotation1,
    const Eigen::Vector3d& relative_translation) {
  const Eigen::Vector3d world_translation =
      rotation1 * (position2 - position1).normalized();
  return theia::RadToDeg(acos(
      theia::Clamp(relative_translation.dot(world_translation), -1.0, 1.0)));
}

void EvaluateRelativeError(
    const theia::ViewGraph& view_graph,
    const Reconstruction& reconstruction_1dsfm,
    const Reconstruction& gt_reconstruction) {
  // For each edge, get the rotate translation and check the error.
  std::vector<double> histogram_bins = {2,  5,   10,  15,  25,  50,
                                        90, 135, 180, 225, 270, 315};
  theia::PoseError pose_error(histogram_bins, histogram_bins);

  const auto& edges = view_graph.GetAllEdges();
  for (const auto& edge : edges) {
    // The reconstruction/view graph from 1dSFM may have a different mapping of
    // names to ViewIds than the ground truth reconstruction, so we have to do a
    // name lookup in the ground truth reconstruction to ensure we have the same
    // view.
    const theia::View* view1_1dsfm =
        reconstruction_1dsfm.View(edge.first.first);
    const theia::View* view2_1dsfm =
        reconstruction_1dsfm.View(edge.first.second);
    if (view1_1dsfm == nullptr || view2_1dsfm == nullptr) {
      continue;
    }

    const ViewId view_id1 =
        gt_reconstruction.ViewIdFromName(view1_1dsfm->Name());
    const ViewId view_id2 =
        gt_reconstruction.ViewIdFromName(view2_1dsfm->Name());
    const theia::View* view1 = gt_reconstruction.View(view_id1);
    const theia::View* view2 = gt_reconstruction.View(view_id2);
    if (view1 == nullptr || view2 == nullptr) {
      continue;
    }
    const theia::Camera& camera1 = view1->Camera();
    const theia::Camera& camera2 = view2->Camera();

    const double rotation_angular_error =
        ComputeRelativeRotationError(camera1.GetOrientationAsRotationMatrix(),
                                     camera2.GetOrientationAsRotationMatrix(),
                                     edge.second.rotation_2);

    const double translation_angular_error = ComputeRelativeTranslationError(
        camera1.GetPosition(),
        camera2.GetPosition(),
        camera1.GetOrientationAsRotationMatrix(),
        edge.second.position_2);
    pose_error.AddError(rotation_angular_error, translation_angular_error);
  }

  LOG(INFO) << "Relative pose errors for 1dsfm = \n"
            << pose_error.PrintMeanMedianHistogram();
}

int main(int argc, char* argv[]) {
  google::InitGoogleLogging(argv[0]);
  THEIA_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);

  LOG(INFO) << "Reading the relative poses from the 1dsfm dataset.";
  std::unique_ptr<Reconstruction> reconstruction_1dsfm(new Reconstruction);
  std::unique_ptr<theia::ViewGraph> view_graph_1dsfm(new theia::ViewGraph);
  CHECK(Read1DSFM(FLAGS_1dsfm_dataset_directory,
                  reconstruction_1dsfm.get(),
                  view_graph_1dsfm.get()))
      << "Could not read 1dsfm dataset from " << FLAGS_1dsfm_dataset_directory;

  const std::string lists_file = FLAGS_1dsfm_dataset_directory + "/list.txt";
  const std::string bundle_file =
      FLAGS_1dsfm_dataset_directory + "/gt_bundle.out";
  std::unique_ptr<theia::Reconstruction> gt_reconstruction(
      new theia::Reconstruction());
  LOG(INFO) << "Converting ground truth bundler file to Theia reconstruction.";
  CHECK(
      theia::ReadBundlerFiles(lists_file, bundle_file, gt_reconstruction.get()))
      << "Could not the ground truth Bundler file at " << bundle_file;

  EvaluateRelativeError(*view_graph_1dsfm,
                        *reconstruction_1dsfm,
                        *gt_reconstruction);

  return 0;
}


#include <iostream>
	using namespace std;

//#include <boost/foreach.hpp>

#include <boost/format.hpp>
	using boost::format;

#include <lvv/array.h>
#include <lvv/lvv.h>
	using lvv::array;

int main() {

	typedef array<int,5> a5_t;

	array <int, 5   >       a0 = {{0,1,2,3,4}}; 			// index starts from 0
	array <int, 5, 1>	a1 = {{1,2,3,4,5}}; 			// index starts from 1
	array <int, 5, 1> const b1 = {{10,20,30,40,50}}; 			// index starts from 1
	array <int, 5, -2>      an = *new array <int, 5, -2>;		// test new

	for (int i = a1.ibegin(); i < a1.iend(); ++i)	{           cout << format("i=%d   a1[i]=%d") %i  % (a1[i]) << endl; };
	for (int i = an.ibegin(); i < an.iend(); ++i)	{ an[i]=i;  cout << format("i=%d   an[i]=%d") %i  % (an[i]) << endl; };

	cout << "a0: "       << a0 << endl;
	cout << "a0 += 10: " << a0 << endl;

	cout << "a1: " << a1 << endl;
	cout << "an: " << an << endl;

	cout << "\n array op= scalar : an+10  " << (an += 10) << endl;

	cout << "\n array op= array \n";
	cout << "a1+=b1  " << (a1 += b1)<< endl;

	//cout << "-a1  " << -a1 << endl;

	// vector ops
	array<float,2> c1={{1,2}};
	array<float,2> c2={{2,2}};
	cout << "dot prod:  {1,2} x {2,2} = " << dot_prod(c1,c2) << endl;
	cout << "norm2: : |{2,2}| = " << norm2(c2) << endl;
	cout << "distance_norm2: : |{1,2}-{2,2}| = " << distance_norm2(c1,c2) << endl;

	//#define FOR_EACH(x,A)  for(A::value_type& it=*A.begin();  ....
	//FOR_EACH(a,A) cout << a << endl;

	/*
	cout << "FOR_ARRAY ";
	typedef typeof(an) T;
	for(
		//array<int,5,-2>::iterator a=an.begin();
		T::iterator a=an.begin();
		a != an.end();
		a++
	)
		cout << *a << " ";

	cout << endl;
	*/

	/*
	{
		cout << "\narray input test, type three whole number\n";
		array<int, 3> A;
		cin >> A;
		cout << "got from  input: " << A << endl;
	}
	*/
}