#include <gauss_msgs/CheckConflicts.h>
#include <gauss_msgs/ConflictiveOperation.h>
#include <gauss_msgs/Deconfliction.h>
#include <gauss_msgs/DeconflictionPlan.h>
#include <gauss_msgs/Threat.h>
#include <gauss_msgs/Waypoint.h>
#include <ros/ros.h>
#include <tactical_deconfliction/path_finder.h>
#include <Eigen/Eigen>

using namespace std;

// Class definition
class ConflictSolver {
   public:
    ConflictSolver();

   private:
    // Topic Callbacks

    // Service Callbacks
    bool deconflictCB(gauss_msgs::Deconfliction::Request &req, gauss_msgs::Deconfliction::Response &res);

    // Auxilary methods
    geometry_msgs::Point findInitAStarPoint(geometry_msgs::Polygon &_polygon, nav_msgs::Path &_path, int &_init_astar_pos);
    geometry_msgs::Point findGoalAStarPoint(geometry_msgs::Polygon &_polygon, nav_msgs::Path &_path, int &_goal_astar_pos);
    int pnpoly(int nvert, std::vector<float> &vertx, std::vector<float> &verty, float testx, float testy);
    std::vector<double> findGridBorders(geometry_msgs::Polygon &_polygon, nav_msgs::Path &_path, geometry_msgs::Point _init_point, geometry_msgs::Point _goal_point);
    geometry_msgs::Polygon circleToPolygon(double &_x, double &_y, double &_radius, double _nVertices = 9);
    gauss_msgs::Waypoint intersectingPoint(gauss_msgs::Waypoint &_p1, gauss_msgs::Waypoint &_p2, geometry_msgs::Polygon &_polygon);
    std::pair<std::vector<double>, double> getCoordinatesAndDistance(double _x0, double _y0, double _x1, double _y1, double _x2, double _y2);
    double pathDistance(gauss_msgs::DeconflictionPlan &_wp_list);
    double pointsDistance(gauss_msgs::Waypoint &_p1, gauss_msgs::Waypoint &_p2);
    double minDistanceToGeofence(std::vector<gauss_msgs::Waypoint> &_wp_list, geometry_msgs::Polygon &_polygon);
    double calculateRiskiness(gauss_msgs::DeconflictionPlan newplan);
    void decreasePolygon(const geometry_msgs::Polygon &p, double thickness, geometry_msgs::Polygon &q);
    double signedArea(const geometry_msgs::Polygon &p);

    // Auxilary variables
    double rate_;
    double minDist_, dT_;
    double minX_, maxX_, minY_, maxY_, minZ_, maxZ_;
    ros::NodeHandle nh_;

    // Subscribers

    // Publisher
    ros::Publisher pub_sol_1_;
    ros::Publisher pub_sol_2_;
    ros::Publisher pub_sol_3_;
    ros::Publisher pub_sol_4_;
    ros::Publisher pub_sol_5_;
    ros::Publisher pub_sol_6_;
    ros::Publisher pub_sol_7_;
    ros::Publisher pub_sol_8_;

    // Timer

    // Server
    ros::ServiceServer deconflict_server_;

    // Clients
    ros::ServiceClient check_client_;
};

// TacticalDeconfliction Constructor
ConflictSolver::ConflictSolver() {
    // Read parameters
    nh_.param("safetyDistance", minDist_, 10.0);
    nh_.param("dT", dT_, 15.0);
    nh_.param("minX", minX_, -400.0);
    nh_.param("minY", minY_, 0.0);
    nh_.param("minZ", minZ_, 0.0);
    nh_.param("maxX", maxX_, 0.0);
    nh_.param("maxY", maxY_, 400.0);
    nh_.param("maxZ", maxZ_, 300.0);

    // Initialization
    // dT_ = 1.0 / rate_;

    // Publish
    pub_sol_1_ = nh_.advertise<nav_msgs::Path>("/sol1", 1);
    pub_sol_2_ = nh_.advertise<nav_msgs::Path>("/sol2", 1);
    pub_sol_3_ = nh_.advertise<nav_msgs::Path>("/sol3", 1);
    pub_sol_4_ = nh_.advertise<nav_msgs::Path>("/sol4", 1);
    pub_sol_5_ = nh_.advertise<nav_msgs::Path>("/sol5", 1);
    pub_sol_6_ = nh_.advertise<nav_msgs::Path>("/sol6", 1);
    pub_sol_7_ = nh_.advertise<nav_msgs::Path>("/sol7", 1);
    pub_sol_8_ = nh_.advertise<nav_msgs::Path>("/sol8", 1);

    // Subscribe

    // Server
    deconflict_server_ = nh_.advertiseService("/gauss/tactical_deconfliction", &ConflictSolver::deconflictCB, this);

    // Cient
    check_client_ = nh_.serviceClient<gauss_msgs::CheckConflicts>("/gauss/check_conflicts");

    ROS_INFO("[Tactical] Started Tactical Deconfliction node!");
}

int ConflictSolver::pnpoly(int nvert, std::vector<float> &vertx, std::vector<float> &verty, float testx, float testy) {
    int i, j, c = 0;
    for (i = 0, j = nvert - 1; i < nvert; j = i++) {
        if (((verty.at(i) > testy) != (verty.at(j) > testy)) &&
            (testx < (vertx.at(j) - vertx.at(i)) * (testy - verty.at(i)) / (verty.at(j) - verty.at(i)) + vertx.at(i)))
            c = !c;
    }
    return c;
}

double ConflictSolver::signedArea(const geometry_msgs::Polygon &p) {
    double A = 0;
    //========================================================//
    // Assumes:                                               //
    //    N+1 vertices:   p[0], p[1], ... , p[N-1], p[N]      //
    //    Closed polygon: p[0] = p[N]                         //
    // Returns:                                               //
    //    Signed area: +ve if anticlockwise, -ve if clockwise //
    //========================================================//
    int N = p.points.size() - 1;
    for (int i = 0; i < N; i++) A += p.points.at(i).x * p.points.at(i + 1).y - p.points.at(i + 1).x * p.points.at(i).y;
    A *= 0.5;
    return A;
}

void ConflictSolver::decreasePolygon(const geometry_msgs::Polygon &p, double thickness, geometry_msgs::Polygon &q) {
    //=====================================================//
    // Assumes:                                            //
    //    N+1 vertices:   p[0], p[1], ... , p[N-1], p[N]   //
    //    Closed polygon: p[0] = p[N]                      //
    //    No zero-length sides                             //
    // Returns (by reference, as a parameter):             //
    //    Internal poly:  q[0], q[1], ... , q[N-1], q[N]   //
    //=====================================================//
    int N = p.points.size() - 1;
    q.points.resize(N + 1);
    double a, b, A, B, d, cross;
    double displacement = thickness;
    if (signedArea(p) < 0) displacement = -displacement;  // Detects clockwise order
    // Unit vector (a,b) along last edge
    a = p.points.at(N).x - p.points.at(N - 1).x;
    b = p.points.at(N).y - p.points.at(N - 1).y;
    d = sqrt(a * a + b * b);
    a /= d;
    b /= d;
    for (int i = 0; i < N; i++) {  // Loop round the polygon, dealing with successive intersections of lines
        // Unit vector (A,B) along previous edge
        A = a;
        B = b;
        // Unit vector (a,b) along next edge
        a = p.points.at(i + 1).x - p.points.at(i).x;
        b = p.points.at(i + 1).y - p.points.at(i).y;
        d = sqrt(a * a + b * b);
        a /= d;
        b /= d;
        // New vertex
        cross = A * b - a * B;
        const double SMALL = 1.0e-10;
        if (abs(cross) < SMALL) {  // Degenerate cases: 0 or 180 degrees at vertex
            q.points.at(i).x = p.points.at(i).x - displacement * b;
            q.points.at(i).y = p.points.at(i).y + displacement * a;
        } else {  // Usual case
            q.points.at(i).x = p.points.at(i).x + displacement * (a - A) / cross;
            q.points.at(i).y = p.points.at(i).y + displacement * (b - B) / cross;
        }
    }
    // Close the inside polygon
    q.points.at(N) = q.points.at(0);
}

geometry_msgs::Point ConflictSolver::findInitAStarPoint(geometry_msgs::Polygon &_polygon, nav_msgs::Path &_path, int &_init_astar_pos) {
    geometry_msgs::Point out_point;
    std::vector<float> vert_x, vert_y;
    for (int i = 0; i < _polygon.points.size(); i++) {
        vert_x.push_back(_polygon.points.at(i).x);
        vert_y.push_back(_polygon.points.at(i).y);
    }
    vert_x.push_back(_polygon.points.front().x);
    vert_y.push_back(_polygon.points.front().y);
    for (int i = 0; i < _path.poses.size(); i++) {
        if (pnpoly(vert_x.size(), vert_x, vert_y, _path.poses.at(i).pose.position.x, _path.poses.at(i).pose.position.y)) {
            out_point.x = _path.poses.at(i - 1).pose.position.x;
            out_point.y = _path.poses.at(i - 1).pose.position.y;
            out_point.z = _path.poses.at(i - 1).pose.position.z;
            _init_astar_pos = i - 1;
            break;
        }
    }
    return out_point;
}

geometry_msgs::Point ConflictSolver::findGoalAStarPoint(geometry_msgs::Polygon &_polygon, nav_msgs::Path &_path, int &_goal_astar_pos) {
    bool flag1 = false;
    bool flag2 = false;
    geometry_msgs::Point out_point;
    std::vector<float> vert_x, vert_y;
    for (int i = 0; i < _polygon.points.size(); i++) {
        vert_x.push_back(_polygon.points.at(i).x);
        vert_y.push_back(_polygon.points.at(i).y);
    }
    vert_x.push_back(_polygon.points.front().x);
    vert_y.push_back(_polygon.points.front().y);
    for (int i = 0; i < _path.poses.size(); i++) {
        bool in_obstacle = pnpoly(vert_x.size(), vert_x, vert_y, _path.poses.at(i).pose.position.x, _path.poses.at(i).pose.position.y);
        if (in_obstacle && !flag1 && !flag2) flag1 = true;
        if (!in_obstacle && flag1 && !flag2) flag2 = true;
        if (!in_obstacle && flag1 && flag2) {
            out_point.x = _path.poses.at(i).pose.position.x;
            out_point.y = _path.poses.at(i).pose.position.y;
            out_point.z = _path.poses.at(i).pose.position.z;
            _goal_astar_pos = i;
            break;
        }
    }

    if (out_point.x == 0.0 && out_point.y == 0.0) {
        out_point.x = _path.poses.back().pose.position.x;
        out_point.y = _path.poses.back().pose.position.y;
        out_point.z = _path.poses.back().pose.position.z;
    }

    return out_point;
}

std::vector<double> ConflictSolver::findGridBorders(geometry_msgs::Polygon &_polygon, nav_msgs::Path &_path, geometry_msgs::Point _init_point, geometry_msgs::Point _goal_point) {
    geometry_msgs::Point obs_min, obs_max, out_point;
    std::vector<float> vert_x, vert_y;
    for (int i = 0; i < _polygon.points.size(); i++) {
        vert_x.push_back(_polygon.points.at(i).x);
        vert_y.push_back(_polygon.points.at(i).y);
    }
    vert_x.push_back(_polygon.points.front().x);
    vert_y.push_back(_polygon.points.front().y);
    obs_min.x = *std::min_element(vert_x.begin(), vert_x.end());
    obs_min.y = *std::min_element(vert_y.begin(), vert_y.end());
    obs_max.x = *std::max_element(vert_x.begin(), vert_x.end());
    obs_max.y = *std::max_element(vert_y.begin(), vert_y.end());

    std::vector<double> out_grid_borders, temp_x, temp_y;
    temp_x.push_back(_init_point.x);
    temp_x.push_back(_goal_point.x);
    temp_y.push_back(_init_point.y);
    temp_y.push_back(_goal_point.y);
    double min_x = *std::min_element(temp_x.begin(), temp_x.end());
    double min_y = *std::min_element(temp_y.begin(), temp_y.end());
    double max_x = *std::max_element(temp_x.begin(), temp_x.end());
    double max_y = *std::max_element(temp_y.begin(), temp_y.end());

    while (min_x >= obs_min.x || min_y >= obs_min.y || max_x <= obs_max.x || max_y <= obs_max.y) {
        if (min_x >= obs_min.x) min_x = min_x - 1.0;
        if (min_y >= obs_min.y) min_y = min_y - 1.0;
        if (max_x <= obs_max.x) max_x = max_x + 1.0;
        if (max_y <= obs_max.y) max_y = max_y + 1.0;
    }

    out_grid_borders.push_back(min_x);
    out_grid_borders.push_back(min_y);
    out_grid_borders.push_back(max_x);
    out_grid_borders.push_back(max_y);

    return out_grid_borders;
}

geometry_msgs::Polygon ConflictSolver::circleToPolygon(double &_x, double &_y, double &_radius, double _nVertices) {
    geometry_msgs::Polygon out_polygon;
    Eigen::Vector2d centerToVertex(_radius, 0.0), centerToVertexTemp;
    for (int i = 0; i < _nVertices; i++) {
        double theta = i * 2 * M_PI / (_nVertices - 1);
        Eigen::Rotation2D<double> rot2d(theta);
        centerToVertexTemp = rot2d.toRotationMatrix() * centerToVertex;
        geometry_msgs::Point32 temp_point;
        temp_point.x = _x + centerToVertexTemp[0];
        temp_point.y = _y + centerToVertexTemp[1];
        out_polygon.points.push_back(temp_point);
    }

    return out_polygon;
}

template <typename KeyType, typename ValueType>
std::pair<KeyType, ValueType> get_min(const std::map<KeyType, ValueType> &x) {
    using pairtype = std::pair<KeyType, ValueType>;
    return *std::min_element(x.begin(), x.end(), [](const pairtype &p1, const pairtype &p2) {
        return p1.second < p2.second;
    });
}

std::pair<std::vector<double>, double> ConflictSolver::getCoordinatesAndDistance(double _x0, double _y0, double _x1, double _y1, double _x2, double _y2) {
    std::vector<double> coordinates;
    double distance = 1000000;
    std::vector<double> point_xs, point_ys;
    point_xs.push_back(_x1);
    point_xs.push_back(_x2);
    point_ys.push_back(_y1);
    point_ys.push_back(_y2);
    // Get abc -> ax + by + c = 0
    double a = _y2 - _y1;
    double b = _x1 - _x2;
    double c = -(a * _x1 + b * _y1);
    // get XY outpoint
    coordinates.push_back((b * (b * _x0 - a * _y0) - a * c) / (a * a + b * b));
    coordinates.push_back((a * (a * _y0 - b * _x0) - b * c) / (a * a + b * b));
    // check if xy is between p1 and p2
    double max_x = *std::max_element(point_xs.begin(), point_xs.end());
    double min_x = *std::min_element(point_xs.begin(), point_xs.end());
    double max_y = *std::max_element(point_ys.begin(), point_ys.end());
    double min_y = *std::min_element(point_ys.begin(), point_ys.end());
    if (max_x >= coordinates.front() && coordinates.front() >= min_x && max_y >= coordinates.back() && coordinates.back() >= min_y) {
        // get distance
        distance = (double)(abs((_y2 - _y1) * _x0 - (_x2 - _x1) * _y0 + _x2 * _y1 - _y2 * _x1) /
                            sqrt(pow(_y2 - _y1, 2) + pow(_x2 - _x1, 2)));
    }
    // if (distance < 1.0) distance = 1000000;
    // std::cout << max_x << ">" << coordinates.front() << ">" << min_x << " | " << max_y << ">" << coordinates.back() << ">" << min_y << " | d = " << distance << std::endl;
    return std::make_pair(coordinates, distance);
}

gauss_msgs::Waypoint ConflictSolver::intersectingPoint(gauss_msgs::Waypoint &_p1, gauss_msgs::Waypoint &_p2, geometry_msgs::Polygon &_polygon) {
    gauss_msgs::Waypoint out_point;
    // std::cout << "p1" << std::endl << _p1 << std::endl << "p2" << std::endl << _p2 << std::endl;
    for (int i = 0; i < _polygon.points.size() - 1; i++) {
        double dx1, dy1, dx2, dy2, m1, c1, m2, c2, temp_x, temp_y;
        dx1 = _p2.x - _p1.x;
        dy1 = _p2.y - _p1.y;
        m1 = dy1 / dx1;
        c1 = _p1.y - m1 * _p1.x;

        dx2 = _polygon.points.at(i + 1).x - _polygon.points.at(i).x;
        dy2 = _polygon.points.at(i + 1).y - _polygon.points.at(i).y;
        m2 = dy2 / dx2;
        c2 = _polygon.points.at(i + 1).y - m2 * _polygon.points.at(i + 1).x;

        std::vector<double> point_xs, point_ys, point_xs1, point_ys1;
        point_xs.push_back(_polygon.points.at(i).x);
        point_xs.push_back(_polygon.points.at(i + 1).x);
        point_ys.push_back(_polygon.points.at(i).y);
        point_ys.push_back(_polygon.points.at(i + 1).y);
        double max_x = *std::max_element(point_xs.begin(), point_xs.end());
        double min_x = *std::min_element(point_xs.begin(), point_xs.end());
        double max_y = *std::max_element(point_ys.begin(), point_ys.end());
        double min_y = *std::min_element(point_ys.begin(), point_ys.end());

        point_xs1.push_back(_p1.x);
        point_xs1.push_back(_p2.x);
        point_ys1.push_back(_p1.y);
        point_ys1.push_back(_p2.y);
        double max_x1 = *std::max_element(point_xs1.begin(), point_xs1.end());
        double min_x1 = *std::min_element(point_xs1.begin(), point_xs1.end());
        double max_y1 = *std::max_element(point_ys1.begin(), point_ys1.end());
        double min_y1 = *std::min_element(point_ys1.begin(), point_ys1.end());

        // std::cout << _polygon.points.at(i) << std::endl << _polygon.points.at(i+1) << std::endl;
        if ((m1 - m2) != 0) {
            temp_x = (c2 - c1) / (m1 - m2);
            temp_y = m1 * temp_x + c1;
            // std::cout << "[1] " << max_x << " > " << temp_x << " > " << min_x << " | " << max_y << " > " << temp_y << " > " << min_y << std::endl;
            if (max_x >= temp_x && temp_x >= min_x && max_y >= temp_y && temp_y >= min_y) {
                // std::cout << "[2] " << max_x1 << " > " << temp_x << " > " << min_x1 << " | " << max_y1 << " > " << temp_y << " > " << min_y1 << std::endl;
                if (max_x1 >= temp_x && temp_x >= min_x1 && max_y1 >= temp_y && temp_y >= min_y1) {
                    out_point.x = temp_x;
                    out_point.y = temp_y;
                    // std::cout << out_point << std::endl;
                    break;
                }
            }
        }
        // std::cout << " -- " << std::endl;
    }
    // std::cout << " -- -- -- " << std::endl;

    return out_point;
}

double ConflictSolver::pathDistance(gauss_msgs::DeconflictionPlan &_wp_list) {
    double distance = 0;
    for (int i = 0; i < _wp_list.waypoint_list.size() - 1; i++) {
        Eigen::Vector3f p1 = Eigen::Vector3f(_wp_list.waypoint_list.at(i).x, _wp_list.waypoint_list.at(i).y, _wp_list.waypoint_list.at(i).z);
        Eigen::Vector3f p2 = Eigen::Vector3f(_wp_list.waypoint_list.at(i + 1).x, _wp_list.waypoint_list.at(i + 1).y, _wp_list.waypoint_list.at(i + 1).z);
        distance = distance + (p2 - p1).norm();
    }

    return distance;
}

double ConflictSolver::pointsDistance(gauss_msgs::Waypoint &_p1, gauss_msgs::Waypoint &_p2) {
    double distance = 0;
    Eigen::Vector2f p1 = Eigen::Vector2f(_p1.x, _p1.y);
    Eigen::Vector2f p2 = Eigen::Vector2f(_p2.x, _p2.y);
    distance = (p2 - p1).norm();

    return distance;
}

double ConflictSolver::minDistanceToGeofence(std::vector<gauss_msgs::Waypoint> &_wp_list, geometry_msgs::Polygon &_polygon) {
    double min_distance = 1000000;
    for (auto wp : _wp_list) {
        Eigen::Vector2f wp_p = Eigen::Vector2f(wp.x, wp.y);
        std::map<std::vector<double>, double> point_and_distance;
        for (int i = 0; i < _polygon.points.size() - 1; i++) {
            Eigen::Vector2f vertex_p = Eigen::Vector2f(_polygon.points.at(i).x, _polygon.points.at(i).y);
            double temp_distance = (vertex_p - wp_p).norm();
            if (temp_distance < min_distance) min_distance = temp_distance;
            point_and_distance.insert(getCoordinatesAndDistance(wp.x, wp.y,
                                                                _polygon.points.at(i).x, _polygon.points.at(i).y,
                                                                _polygon.points.at(i + 1).x, _polygon.points.at(i + 1).y));
        }
        Eigen::Vector2f vertex_p = Eigen::Vector2f(_polygon.points.back().x, _polygon.points.back().y);
        double temp_distance = (vertex_p - wp_p).norm();
        if (temp_distance < min_distance) min_distance = temp_distance;
        point_and_distance.insert(getCoordinatesAndDistance(wp.x, wp.y,
                                                            _polygon.points.front().x, _polygon.points.front().y,
                                                            _polygon.points.back().x, _polygon.points.back().y));

        auto min_distance_coordinate = get_min(point_and_distance);
        // Check != 0 (if 0 the path collide with the geofence)
        if (min_distance_coordinate.second < min_distance && min_distance_coordinate.second != 0) min_distance = min_distance_coordinate.second;
    }

    return min_distance;
}

double ConflictSolver::calculateRiskiness(gauss_msgs::DeconflictionPlan _newplan) {
    nav_msgs::Path temp_path;
    for (auto wp : _newplan.waypoint_list) {
        geometry_msgs::PoseStamped temp_wp_stamped;
        temp_wp_stamped.pose.position.x = wp.x;
        temp_wp_stamped.pose.position.y = wp.y;
        temp_wp_stamped.pose.position.z = wp.z;
        temp_wp_stamped.header.stamp = wp.stamp;
        temp_path.poses.push_back(temp_wp_stamped);
    }

    PathFinder path_finder;
    nav_msgs::Path interp_path = path_finder.generatePath(temp_path, 0.0, 1.0);
    gauss_msgs::CheckConflicts check_conflict;
    check_conflict.request.uav_id = _newplan.uav_id;
    for (auto wp : interp_path.poses) {
        gauss_msgs::Waypoint temp_wp;
        temp_wp.x = wp.pose.position.x;
        temp_wp.y = wp.pose.position.y;
        temp_wp.z = wp.pose.position.z;
        temp_wp.stamp = wp.header.stamp;
        check_conflict.request.deconflicted_wp.push_back(temp_wp);
    }

    if (!check_client_.call(check_conflict) || !check_conflict.response.success)
        ROS_ERROR("[Tactical] Failed checking conflicts");

    return 100 * check_conflict.response.threats.size() / interp_path.poses.size();
}

// deconflictCB callback
bool ConflictSolver::deconflictCB(gauss_msgs::Deconfliction::Request &req, gauss_msgs::Deconfliction::Response &res) {
    ROS_INFO("[Tactical] Threat to solve [%d, %d]", req.threat.threat_id, req.threat.threat_type);
    //Deconfliction
    if (req.tactical) {
        double start_computational_time = ros::Time::now().toSec();
        gauss_msgs::Threat conflict;
        conflict = req.threat;
        std::vector<gauss_msgs::ConflictiveOperation> conflictive_operations;
        for (int i = 0; i < req.threat.uav_ids.size(); i++) {
            for (int j = 0; j < req.operations.size(); j++) {
                if (req.threat.uav_ids.at(i) == req.operations.at(j).uav_id) {
                    conflictive_operations.push_back(req.operations.at(j));
                }
            }
        }

        if (req.threat.threat_type == req.threat.LOSS_OF_SEPARATION) {
            if (req.operations.size() != 2) ROS_ERROR("[Tactical] Loss of separation. Operation size should be equal to 2 not %zd", req.operations.size());
            if (req.threat.times.size() != 2) ROS_ERROR("[Tactical] Loss of separation. Threat times size should be equal to 2 not %zd", req.threat.times.size());
            gauss_msgs::WaypointList traj1 = conflictive_operations.front().estimated_trajectory;
            gauss_msgs::WaypointList traj2 = conflictive_operations.back().estimated_trajectory;
            gauss_msgs::Waypoint wp1, wp2;
            int j = 0;
            while (abs(traj1.waypoints.at(j).stamp.toSec() - conflict.times.front().toSec()) >= dT_ / 2)
                j++;
            wp1 = traj1.waypoints.at(j);
            int k = 0;
            while (abs(traj2.waypoints.at(k).stamp.toSec() - conflict.times.back().toSec()) >= dT_ / 2)
                k++;
            wp2 = traj2.waypoints.at(k);

            double minDistAux = max(minDist_ + conflictive_operations.front().operational_volume + conflictive_operations.back().operational_volume, conflictive_operations.front().operational_volume + conflictive_operations.back().operational_volume);

            double dist_vert = abs(wp2.z - wp1.z);
            double dist_hor = sqrt(pow(wp2.x - wp1.x, 2) + pow(wp2.y - wp1.y, 2));
            double sep_vert = sqrt(pow(minDistAux, 2) - pow(dist_hor, 2));
            double sep_hor = sqrt(pow(minDistAux, 2) - pow(dist_vert, 2));

            gauss_msgs::DeconflictionPlan newplan;
            gauss_msgs::Waypoint newwp;
            newplan.maneuver_type = 8;  // defined in https://docs.google.com/document/d/1R5jWSw4pyPyplHwwrQCDprIUkMimVz-yR8u_t19ooOA/edit

            if (req.threat.priority_ops.front() >= req.threat.priority_ops.back()) {
                newplan.uav_id = req.threat.uav_ids.back();
                //Above
                if (k > 0)
                    newplan.waypoint_list.push_back(traj2.waypoints.at(k - 1));
                newwp = traj2.waypoints.at(k);
                newwp.z = traj1.waypoints.at(j).z + sep_vert;
                if (newwp.z <= maxZ_ && newwp.z >= minZ_) {
                    newplan.waypoint_list.push_back(newwp);
                    if (k < traj2.waypoints.size()){
                        newplan.waypoint_list.push_back(traj2.waypoints.at(k + 1));
                    }
                    newplan.cost = pathDistance(newplan);
                    newplan.riskiness = calculateRiskiness(newplan);
                    res.deconfliction_plans.push_back(newplan);
                }
                //Below
                newplan.waypoint_list.clear();
                if (k > 0)
                    newplan.waypoint_list.push_back(traj2.waypoints.at(k - 1));
                newwp = traj2.waypoints.at(k);
                newwp.z = traj1.waypoints.at(j).z - sep_vert;
                if (newwp.z <= maxZ_ && newwp.z >= minZ_) {
                    newplan.waypoint_list.push_back(newwp);
                    if (k < traj2.waypoints.size()){
                        newplan.waypoint_list.push_back(traj2.waypoints.at(k + 1));
                    }
                    newplan.cost = pathDistance(newplan);
                    newplan.riskiness = calculateRiskiness(newplan);
                    res.deconfliction_plans.push_back(newplan);
                }
                //On the right
                newplan.waypoint_list.clear();
                if (k > 0)
                    newplan.waypoint_list.push_back(traj2.waypoints.at(k - 1));
                newwp = traj2.waypoints.at(k);
                if (dist_hor == 0) {
                    newwp.x = wp1.x + sep_hor;
                    newwp.y = wp1.y + sep_hor;
                } else {
                    newwp.x = wp1.x + sep_hor * (wp2.x - wp1.x) / dist_hor;
                    newwp.y = wp1.y + sep_hor * (wp2.y - wp1.y) / dist_hor;
                }
                if (newwp.x <= maxX_ && newwp.x >= minX_ && newwp.y <= maxY_ && newwp.y >= minY_) {
                    newplan.waypoint_list.push_back(newwp);
                    if (k < traj2.waypoints.size()){
                        newplan.waypoint_list.push_back(traj2.waypoints.at(k + 1));
                    }
                    newplan.cost = pathDistance(newplan);
                    newplan.riskiness = calculateRiskiness(newplan);
                    res.deconfliction_plans.push_back(newplan);
                }
                //On the left
                newplan.waypoint_list.clear();
                if (k > 0)
                    newplan.waypoint_list.push_back(traj2.waypoints.at(k - 1));
                newwp = traj2.waypoints.at(k);
                if (dist_hor == 0) {
                    newwp.x = wp1.x - sep_hor;
                    newwp.y = wp1.y - sep_hor;
                } else {
                    newwp.x = wp1.x - sep_hor * (wp2.x - wp1.x) / dist_hor;
                    newwp.y = wp1.y - sep_hor * (wp2.y - wp1.y) / dist_hor;
                }
                if (newwp.x <= maxX_ && newwp.x >= minX_ && newwp.y <= maxY_ && newwp.y >= minY_) {
                    newplan.waypoint_list.push_back(newwp);
                    if (k < traj2.waypoints.size()){
                        newplan.waypoint_list.push_back(traj2.waypoints.at(k + 1));
                    }
                    newplan.cost = pathDistance(newplan);
                    newplan.riskiness = calculateRiskiness(newplan);
                    res.deconfliction_plans.push_back(newplan);
                }
            }
            if (req.threat.priority_ops.back() >= req.threat.priority_ops.front()) {
                newplan.uav_id = req.threat.uav_ids.front();
                //Above
                newplan.waypoint_list.clear();
                if (j > 0)
                    newplan.waypoint_list.push_back(traj1.waypoints.at(j - 1));
                newwp = traj1.waypoints.at(j);
                newwp.z = traj2.waypoints.at(k).z + sep_vert;
                if (newwp.z <= maxZ_ && newwp.z >= minZ_) {
                    newplan.waypoint_list.push_back(newwp);
                    if (j < traj1.waypoints.size()){
                        newplan.waypoint_list.push_back(traj1.waypoints.at(j + 1));
                    }
                    newplan.cost = pathDistance(newplan);
                    newplan.riskiness = calculateRiskiness(newplan);
                    res.deconfliction_plans.push_back(newplan);
                }
                //Below
                newplan.waypoint_list.clear();
                if (j > 0)
                    newplan.waypoint_list.push_back(traj1.waypoints.at(j - 1));
                newwp = traj1.waypoints.at(j);
                newwp.z = traj2.waypoints.at(k).z - sep_vert;
                if (newwp.z <= maxZ_ && newwp.z >= minZ_) {
                    newplan.waypoint_list.push_back(newwp);
                    if (j < traj1.waypoints.size()){
                        newplan.waypoint_list.push_back(traj1.waypoints.at(j + 1));
                    }
                    newplan.cost = pathDistance(newplan);
                    newplan.riskiness = calculateRiskiness(newplan);
                    res.deconfliction_plans.push_back(newplan);
                }
                //On the right
                newplan.waypoint_list.clear();
                if (j > 0)
                    newplan.waypoint_list.push_back(traj1.waypoints.at(j - 1));
                newwp = traj1.waypoints.at(j);
                if (dist_hor == 0) {
                    newwp.x = wp2.x + sep_hor;
                    newwp.y = wp2.y + sep_hor;
                } else {
                    newwp.x = wp2.x + sep_hor * (wp1.x - wp2.x) / dist_hor;
                    newwp.y = wp2.y + sep_hor * (wp1.y - wp2.y) / dist_hor;
                }
                if (newwp.x <= maxX_ && newwp.x >= minX_ && newwp.y <= maxY_ && newwp.y >= minY_) {
                    newplan.waypoint_list.push_back(newwp);
                    if (j < traj1.waypoints.size()){
                        newplan.waypoint_list.push_back(traj1.waypoints.at(j + 1));
                    }
                    newplan.cost = pathDistance(newplan);
                    newplan.riskiness = calculateRiskiness(newplan);
                    res.deconfliction_plans.push_back(newplan);
                }
                //On the left
                newplan.waypoint_list.clear();
                if (j > 0)
                    newplan.waypoint_list.push_back(traj1.waypoints.at(j - 1));
                newwp = traj1.waypoints.at(j);
                if (dist_hor == 0) {
                    newwp.x = wp2.x - sep_hor;
                    newwp.y = wp2.y - sep_hor;
                } else {
                    newwp.x = wp2.x - sep_hor * (wp1.x - wp2.x) / dist_hor;
                    newwp.y = wp2.y - sep_hor * (wp1.y - wp2.y) / dist_hor;
                }
                if (newwp.x <= maxX_ && newwp.x >= minX_ && newwp.y <= maxY_ && newwp.y >= minY_) {
                    newplan.waypoint_list.push_back(newwp);
                    if (j < traj1.waypoints.size()){
                        newplan.waypoint_list.push_back(traj1.waypoints.at(j + 1));
                    }
                    newplan.cost = pathDistance(newplan);
                    newplan.riskiness = calculateRiskiness(newplan);
                    res.deconfliction_plans.push_back(newplan);
                }
            }
            res.message = "Conflict solved";
            res.success = true;
        } else if (req.threat.threat_type == req.threat.GEOFENCE_CONFLICT) {
            nav_msgs::Path res_path;
            std::vector<double> res_times;
            for (int i = 0; i < conflictive_operations.front().estimated_trajectory.waypoints.size(); i++) {
                geometry_msgs::PoseStamped temp_pose;
                temp_pose.pose.position.x = conflictive_operations.front().estimated_trajectory.waypoints.at(i).x;
                temp_pose.pose.position.y = conflictive_operations.front().estimated_trajectory.waypoints.at(i).y;
                temp_pose.pose.position.z = conflictive_operations.front().estimated_trajectory.waypoints.at(i).z;
                res_path.poses.push_back(temp_pose);
                res_times.push_back(conflictive_operations.front().estimated_trajectory.waypoints.at(i).stamp.toSec());
            }
            std::vector<gauss_msgs::Geofence> geofences;
            for (int i = 0; i < req.threat.geofence_ids.size(); i++) {
                for (int j = 0; j < req.geofences.size(); j++) {
                    if (req.threat.uav_ids.at(i) == req.operations.at(j).uav_id) {
                        geofences.push_back(req.geofences.at(j));
                    }
                }
            }
            geometry_msgs::Polygon res_polygon;
            if (geofences.front().cylinder_shape) {
                res_polygon = circleToPolygon(geofences.front().circle.x_center,
                                              geofences.front().circle.y_center,
                                              geofences.front().circle.radius);
            } else {
                for (int i = 0; i < geofences.front().polygon.x.size(); i++) {
                    geometry_msgs::Point32 temp_points;
                    temp_points.x = geofences.front().polygon.x.at(i);
                    temp_points.y = geofences.front().polygon.y.at(i);
                    res_polygon.points.push_back(temp_points);
                }
            }
            // [1] Ruta a mi destino evitando una geofence
            geometry_msgs::Point init_astar_point, goal_astar_point, min_grid_point, max_grid_point;
            int init_astar_pos, goal_astar_pos;
            geometry_msgs::Polygon polygon_test_input = res_polygon;
            if (!geofences.front().cylinder_shape) {
                polygon_test_input.points.push_back(polygon_test_input.points.front());
            }
            geometry_msgs::Polygon polygon_test_output;
            decreasePolygon(polygon_test_input, -conflictive_operations.front().operational_volume*1.1, polygon_test_output);
            if (!geofences.front().cylinder_shape) {
                polygon_test_output.points.pop_back();
            }
            init_astar_point = findInitAStarPoint(polygon_test_output, res_path, init_astar_pos);
            goal_astar_point = findGoalAStarPoint(polygon_test_output, res_path, goal_astar_pos);
            std::vector<double> grid_borders = findGridBorders(polygon_test_output, res_path, init_astar_point, goal_astar_point);
            min_grid_point.x = minX_;
            min_grid_point.y = minY_;
            max_grid_point.x = maxX_;
            max_grid_point.y = maxY_;
            PathFinder path_finder(res_path, init_astar_point, goal_astar_point, polygon_test_output, min_grid_point, max_grid_point);
            nav_msgs::Path a_star_path_res = path_finder.findNewPath();
            std::vector<double> interp_times, a_star_times_res;
            interp_times.push_back(res_times.at(init_astar_pos));
            interp_times.push_back(res_times.at(goal_astar_pos));
            a_star_times_res = path_finder.interpWaypointList(interp_times, a_star_path_res.poses.size() - 1);
            a_star_times_res.push_back(res_times.at(goal_astar_pos));
            // Solutions of conflict solver are a_star_path_res and a_star_times_res
            gauss_msgs::Waypoint temp_wp;
            gauss_msgs::DeconflictionPlan temp_wp_list;
            for (int i = 0; i < a_star_path_res.poses.size(); i++) {
                temp_wp.x = a_star_path_res.poses.at(i).pose.position.x;
                temp_wp.y = a_star_path_res.poses.at(i).pose.position.y;
                temp_wp.z = a_star_path_res.poses.at(i).pose.position.z;
                temp_wp.stamp = ros::Time(a_star_times_res.at(i));
                temp_wp_list.waypoint_list.push_back(temp_wp);
            }
            temp_wp_list.maneuver_type = 1;
            temp_wp_list.cost = pathDistance(temp_wp_list);
            temp_wp_list.riskiness = minDistanceToGeofence(temp_wp_list.waypoint_list, res_polygon);
            temp_wp_list.uav_id = req.threat.uav_ids.front();
            res.deconfliction_plans.push_back(temp_wp_list);
            // [3] Ruta que me manda devuelta a casa
            // temp_wp_list.waypoint_list.clear();
            // temp_wp.x = conflictive_operations.front().estimated_trajectory.waypoints.front().x;
            // temp_wp.y = conflictive_operations.front().estimated_trajectory.waypoints.front().y;
            // temp_wp.z = conflictive_operations.front().estimated_trajectory.waypoints.front().z;
            // temp_wp.stamp = conflictive_operations.front().estimated_trajectory.waypoints.front().stamp;
            // temp_wp_list.waypoint_list.push_back(temp_wp);
            // temp_wp.x = conflictive_operations.front().flight_plan.waypoints.front().x;
            // temp_wp.y = conflictive_operations.front().flight_plan.waypoints.front().y;
            // temp_wp.z = conflictive_operations.front().flight_plan.waypoints.front().z;
            // temp_wp_list.waypoint_list.push_back(temp_wp);
            // temp_wp_list.maneuver_type = 3;
            // temp_wp_list.cost = pathDistance(temp_wp_list);
            // temp_wp_list.riskiness = minDistanceToGeofence(temp_wp_list.waypoint_list, res_polygon);
            // temp_wp_list.uav_id = req.threat.uav_ids.front();
            // res.deconfliction_plans.push_back(temp_wp_list);

            res.message = "Conflict solved";
            res.success = true;
        } else if (req.threat.threat_type == req.threat.GEOFENCE_INTRUSION) {
            nav_msgs::Path res_path;
            std::vector<double> res_times;
            for (int i = 0; i < conflictive_operations.front().estimated_trajectory.waypoints.size(); i++) {
                geometry_msgs::PoseStamped temp_pose;
                temp_pose.pose.position.x = conflictive_operations.front().estimated_trajectory.waypoints.at(i).x;
                temp_pose.pose.position.y = conflictive_operations.front().estimated_trajectory.waypoints.at(i).y;
                temp_pose.pose.position.z = conflictive_operations.front().estimated_trajectory.waypoints.at(i).z;
                res_path.poses.push_back(temp_pose);
                res_times.push_back(conflictive_operations.front().estimated_trajectory.waypoints.at(i).stamp.toSec());
            }
            std::vector<gauss_msgs::Geofence> geofences;
            for (int i = 0; i < req.threat.geofence_ids.size(); i++) {
                for (int j = 0; j < req.geofences.size(); j++) {
                    if (req.threat.uav_ids.at(i) == req.operations.at(j).uav_id) {
                        geofences.push_back(req.geofences.at(j));
                    }
                }
            }
            geometry_msgs::Polygon res_polygon;
            if (geofences.front().cylinder_shape) {
                res_polygon = circleToPolygon(geofences.front().circle.x_center,
                                              geofences.front().circle.y_center,
                                              geofences.front().circle.radius);
            } else {
                for (int i = 0; i < geofences.front().polygon.x.size(); i++) {
                    geometry_msgs::Point32 temp_points;
                    temp_points.x = geofences.front().polygon.x.at(i);
                    temp_points.y = geofences.front().polygon.y.at(i);
                    res_polygon.points.push_back(temp_points);
                }
            }
            // [6] Ruta a mi destino saliendo lo antes posible de la geofence
            // Get min distance to polygon border
            // gauss_msgs::Waypoint conflict_point;
            // conflict_point.x = conflictive_operations.front().estimated_trajectory.waypoints.front().x;
            // conflict_point.y = conflictive_operations.front().estimated_trajectory.waypoints.front().y;
            // conflict_point.z = conflictive_operations.front().estimated_trajectory.waypoints.front().z;
            // std::map<std::vector<double>, double> point_and_distance;
            // for (auto vertex : res_polygon.points) {
            //     std::vector<double> point;
            //     point.push_back(vertex.x);
            //     point.push_back(vertex.y);
            //     point_and_distance.insert(std::make_pair(point, sqrt(pow(vertex.x - conflict_point.x, 2) +
            //                                                          pow(vertex.y - conflict_point.y, 2))));
            // }
            // for (int i = 0; i < res_polygon.points.size() - 1; i++) {
            //     point_and_distance.insert(getCoordinatesAndDistance(conflict_point.x, conflict_point.y,
            //                                                         res_polygon.points.at(i).x, res_polygon.points.at(i).y,
            //                                                         res_polygon.points.at(i + 1).x, res_polygon.points.at(i + 1).y));
            // }
            // point_and_distance.insert(getCoordinatesAndDistance(conflict_point.x, conflict_point.y,
            //                                                     res_polygon.points.front().x, res_polygon.points.front().y,
            //                                                     res_polygon.points.back().x, res_polygon.points.back().y));
            // auto min_distance_coordinate = get_min(point_and_distance);
            // Eigen::Vector2f p_conflict, p_min_distance, unit_vec, v_out_polygon;
            // p_conflict = Eigen::Vector2f(conflict_point.x, conflict_point.y);
            // p_min_distance = Eigen::Vector2f(min_distance_coordinate.first.front(), min_distance_coordinate.first.back());
            // unit_vec = (p_min_distance - p_conflict) / (p_min_distance - p_conflict).norm();
            // double safety_distance = conflictive_operations.front().operational_volume;
            // v_out_polygon = unit_vec * safety_distance;
            // geometry_msgs::Point init_astar_point, goal_astar_point, min_grid_point, max_grid_point;
            // init_astar_point.x = min_distance_coordinate.first.front() + v_out_polygon(0);
            // init_astar_point.y = min_distance_coordinate.first.back() + v_out_polygon(1);
            // geometry_msgs::Polygon polygon_test_input = res_polygon;
            // if (!geofences.front().cylinder_shape) {
            //     polygon_test_input.points.push_back(polygon_test_input.points.front());
            // }
            // geometry_msgs::Polygon polygon_test_output;
            // decreasePolygon(polygon_test_input, -conflictive_operations.front().operational_volume*1.1, polygon_test_output);
            // if (!geofences.front().cylinder_shape) {
            //     polygon_test_output.points.pop_back();
            // }
            // int init_astar_pos, goal_astar_pos;
            // goal_astar_point = findGoalAStarPoint(polygon_test_output, res_path, goal_astar_pos);
            // std::vector<double> grid_borders = findGridBorders(polygon_test_output, res_path, init_astar_point, goal_astar_point);
            // min_grid_point.x = minX_;
            // min_grid_point.y = minY_;
            // max_grid_point.x = maxX_;
            // max_grid_point.y = maxY_;
            // PathFinder path_finder(res_path, init_astar_point, goal_astar_point, polygon_test_output, min_grid_point, max_grid_point);
            // nav_msgs::Path a_star_path_res = path_finder.findNewPath();
            // std::vector<double> interp_times, a_star_times_res;
            // interp_times.push_back(conflictive_operations.front().estimated_trajectory.waypoints.front().stamp.toSec());  // init astar pos time
            // interp_times.push_back(res_times.at(goal_astar_pos));
            // a_star_times_res = path_finder.interpWaypointList(interp_times, a_star_path_res.poses.size() - 1);
            // a_star_times_res.push_back(res_times.at(goal_astar_pos));
            // // Solutions of conflict solver are a_star_path_res and a_star_times_res
            gauss_msgs::Waypoint temp_wp;
            gauss_msgs::DeconflictionPlan temp_wp_list;
            // temp_wp.x = conflict_point.x;
            // temp_wp.y = conflict_point.y;
            // temp_wp.z = conflict_point.z;
            // temp_wp.stamp = conflictive_operations.front().estimated_trajectory.waypoints.front().stamp;
            // temp_wp_list.waypoint_list.push_back(temp_wp);
            // for (int i = 0; i < a_star_path_res.poses.size(); i++) {
            //     temp_wp.x = a_star_path_res.poses.at(i).pose.position.x;
            //     temp_wp.y = a_star_path_res.poses.at(i).pose.position.y;
            //     temp_wp.z = a_star_path_res.poses.at(i).pose.position.z;
            //     temp_wp.stamp = ros::Time(a_star_times_res.at(i));
            //     temp_wp_list.waypoint_list.push_back(temp_wp);
            // }
            // temp_wp_list.maneuver_type = 6;
            // temp_wp_list.cost = pathDistance(temp_wp_list);
            // gauss_msgs::Waypoint intersect_p = intersectingPoint(temp_wp_list.waypoint_list.front(), temp_wp_list.waypoint_list.at(1), res_polygon); // ! This is making conflicsolver.cpp to explode
            // temp_wp_list.riskiness = pointsDistance(temp_wp_list.waypoint_list.front(), intersect_p);
            // temp_wp_list.uav_id = req.threat.uav_ids.front();
            // res.deconfliction_plans.push_back(temp_wp_list);
            // [2] Ruta a mi destino por el camino mas corto
            // temp_wp_list.waypoint_list.clear();
            // temp_wp.x = conflict_point.x;
            // temp_wp.y = conflict_point.y;
            // temp_wp.z = conflict_point.z;
            // temp_wp.stamp = conflictive_operations.front().estimated_trajectory.waypoints.front().stamp;
            // temp_wp_list.waypoint_list.push_back(temp_wp);
            // temp_wp.x = conflictive_operations.front().flight_plan.waypoints.back().x;
            // temp_wp.y = conflictive_operations.front().flight_plan.waypoints.back().y;
            // temp_wp.z = conflictive_operations.front().flight_plan.waypoints.back().z;
            // temp_wp.stamp = conflictive_operations.front().flight_plan.waypoints.back().stamp;
            // temp_wp_list.waypoint_list.push_back(temp_wp);
            // temp_wp_list.maneuver_type = 2;
            // temp_wp_list.cost = pathDistance(temp_wp_list);
            // intersect_p = intersectingPoint(temp_wp_list.waypoint_list.front(), temp_wp_list.waypoint_list.back(), res_polygon);
            // temp_wp_list.riskiness = pointsDistance(temp_wp_list.waypoint_list.front(), intersect_p);
            // temp_wp_list.uav_id = req.threat.uav_ids.front();
            // res.deconfliction_plans.push_back(temp_wp_list);
            // std::cout << "4\n";
            // [3] Ruta que me manda de vuelta a casa
            // temp_wp_list.waypoint_list.clear();
            // temp_wp.x = conflictive_operations.front().estimated_trajectory.waypoints.front().x;
            // temp_wp.y = conflictive_operations.front().estimated_trajectory.waypoints.front().y;
            // temp_wp.z = conflictive_operations.front().estimated_trajectory.waypoints.front().z;
            // temp_wp.stamp = conflictive_operations.front().estimated_trajectory.waypoints.front().stamp;
            // temp_wp_list.waypoint_list.push_back(temp_wp);
            // temp_wp.x = conflictive_operations.front().flight_plan.waypoints.front().x;
            // temp_wp.y = conflictive_operations.front().flight_plan.waypoints.front().y;
            // temp_wp.z = conflictive_operations.front().flight_plan.waypoints.front().z;
            // temp_wp_list.waypoint_list.push_back(temp_wp);
            // temp_wp_list.maneuver_type = 3;
            // temp_wp_list.cost = pathDistance(temp_wp_list);
            // intersect_p = intersectingPoint(temp_wp_list.waypoint_list.front(), temp_wp_list.waypoint_list.back(), res_polygon);
            // temp_wp_list.riskiness = pointsDistance(temp_wp_list.waypoint_list.front(), intersect_p);
            // temp_wp_list.uav_id = req.threat.uav_ids.front();
            // res.deconfliction_plans.push_back(temp_wp_list);
            // [?] Ruta que me manda a un landing spot
            if (conflictive_operations.front().landing_spots.waypoints.size() > 0) {
                temp_wp_list.waypoint_list.clear();
                temp_wp.x = conflictive_operations.front().estimated_trajectory.waypoints.front().x;
                temp_wp.y = conflictive_operations.front().estimated_trajectory.waypoints.front().y;
                temp_wp.z = conflictive_operations.front().estimated_trajectory.waypoints.front().z;
                temp_wp.stamp = conflictive_operations.front().estimated_trajectory.waypoints.front().stamp;
                temp_wp_list.waypoint_list.push_back(temp_wp);
                temp_wp.x = conflictive_operations.front().landing_spots.waypoints.front().x;
                temp_wp.y = conflictive_operations.front().landing_spots.waypoints.front().y;
                temp_wp.z = conflictive_operations.front().landing_spots.waypoints.front().z;
                temp_wp.stamp = ros::Time(conflictive_operations.front().estimated_trajectory.waypoints.front().stamp.toSec() + 300.0); // * Random addition
                temp_wp_list.waypoint_list.push_back(temp_wp);
                temp_wp_list.maneuver_type = 5;
                // temp_wp_list.cost = pathDistance(temp_wp_list);
                // intersect_p = intersectingPoint(temp_wp_list.waypoint_list.front(), temp_wp_list.waypoint_list.back(), res_polygon);
                // temp_wp_list.riskiness = pointsDistance(temp_wp_list.waypoint_list.front(), intersect_p);
                temp_wp_list.cost = 0.0;        // ! Forcing this solution to be the one of choice
                temp_wp_list.riskiness = 0.0;   // ! Forcing this solution to be the one of choice
                temp_wp_list.uav_id = req.threat.uav_ids.front();
                res.deconfliction_plans.push_back(temp_wp_list);
            }

            res.message = "Conflict solved";
            res.success = true;
        } else if (req.threat.threat_type == req.threat.UAS_OUT_OV) {
            gauss_msgs::Waypoint temp_wp;
            gauss_msgs::DeconflictionPlan temp_wp_list;
            // [9] Ruta para volver lo antes posible al flight geometry y seguir el plan de vuelo.
            std::map<std::vector<double>, double> point_and_distance;
            for (int i = 0; i < conflictive_operations.front().flight_plan.waypoints.size() - 1; i++) {
                point_and_distance.insert(getCoordinatesAndDistance(conflictive_operations.front().estimated_trajectory.waypoints.back().x,
                                                                    conflictive_operations.front().estimated_trajectory.waypoints.back().y,
                                                                    conflictive_operations.front().flight_plan.waypoints.at(i).x,
                                                                    conflictive_operations.front().flight_plan.waypoints.at(i).y,
                                                                    conflictive_operations.front().flight_plan.waypoints.at(i + 1).x,
                                                                    conflictive_operations.front().flight_plan.waypoints.at(i + 1).y));
            }
            auto min_distance_coordinate = get_min(point_and_distance);
            temp_wp.x = min_distance_coordinate.first.front();
            temp_wp.y = min_distance_coordinate.first.back();
            temp_wp.z = conflictive_operations.front().estimated_trajectory.waypoints.back().z;
            temp_wp_list.waypoint_list.push_back(conflictive_operations.front().estimated_trajectory.waypoints.back());
            temp_wp_list.waypoint_list.push_back(temp_wp);
            temp_wp_list.maneuver_type = 9;
            temp_wp_list.cost = pathDistance(temp_wp_list);
            temp_wp_list.riskiness = std::abs(pathDistance(temp_wp_list) - conflictive_operations.front().operational_volume);
            temp_wp_list.uav_id = req.threat.uav_ids.front();
            res.deconfliction_plans.push_back(temp_wp_list);
            double dist_out_ov = temp_wp_list.riskiness;
            double min_dist_to_path = temp_wp_list.cost;
            // [10] Ruta para seguir con el plan de vuelo, da igual que esté más tiempo fuera del Operational Volume.
            temp_wp_list.waypoint_list.clear();
            temp_wp_list.waypoint_list.push_back(conflictive_operations.front().estimated_trajectory.waypoints.back());
            temp_wp_list.waypoint_list.push_back(conflictive_operations.front().flight_plan.waypoints.at(conflictive_operations.front().current_wp + 1));
            temp_wp_list.maneuver_type = 10;
            temp_wp_list.cost = pathDistance(temp_wp_list);
            double alpha = acos(min_dist_to_path / temp_wp_list.cost);
            temp_wp_list.riskiness = std::abs(dist_out_ov / cos(alpha));
            temp_wp_list.uav_id = req.threat.uav_ids.front();
            res.deconfliction_plans.push_back(temp_wp_list);

            res.message = "Conflict solved";
            res.success = true;
        } else if (req.threat.threat_type == req.threat.LACK_OF_BATTERY) {
            for (auto wp_land : conflictive_operations.front().landing_spots.waypoints) {
                gauss_msgs::Waypoint temp_wp;
                gauss_msgs::DeconflictionPlan temp_wp_list;
                temp_wp = conflictive_operations.front().estimated_trajectory.waypoints.front();
                temp_wp_list.waypoint_list.push_back(temp_wp);
                temp_wp_list.maneuver_type = 5;
                double distance = pointsDistance(conflictive_operations.front().estimated_trajectory.waypoints.front(), wp_land);
                temp_wp_list.cost = distance;
                temp_wp_list.waypoint_list.push_back(wp_land);
                temp_wp_list.riskiness = calculateRiskiness(temp_wp_list);
                temp_wp_list.uav_id = req.threat.uav_ids.front();
                res.deconfliction_plans.push_back(temp_wp_list);
            }
            // [5] Ruta que aterrice en un landing spot
            gauss_msgs::Waypoint temp_wp;
            gauss_msgs::DeconflictionPlan temp_wp_list;
            temp_wp_list.maneuver_type = 5;
            temp_wp = conflictive_operations.front().flight_plan.waypoints.front();
            temp_wp_list.waypoint_list.push_back(conflictive_operations.front().estimated_trajectory.waypoints.front());
            double distance = pointsDistance(conflictive_operations.front().estimated_trajectory.waypoints.front(), temp_wp);
            temp_wp_list.cost = distance;
            temp_wp_list.waypoint_list.push_back(temp_wp);
            temp_wp_list.riskiness = calculateRiskiness(temp_wp_list);
            temp_wp_list.uav_id = req.threat.uav_ids.front();
            res.deconfliction_plans.push_back(temp_wp_list);

            res.message = "Conflict solved";
            res.success = true;
        } else if (req.threat.threat_type == req.threat.GNSS_DEGRADATION) {
            for (auto wp_land : conflictive_operations.front().landing_spots.waypoints) {
                gauss_msgs::Waypoint temp_wp;
                gauss_msgs::DeconflictionPlan temp_wp_list;
                temp_wp = conflictive_operations.front().estimated_trajectory.waypoints.front();
                temp_wp_list.waypoint_list.push_back(temp_wp);
                temp_wp_list.maneuver_type = 5;
                double distance = pointsDistance(conflictive_operations.front().estimated_trajectory.waypoints.front(), wp_land);
                temp_wp_list.cost = distance;
                temp_wp_list.waypoint_list.push_back(wp_land);
                temp_wp_list.riskiness = calculateRiskiness(temp_wp_list);
                temp_wp_list.uav_id = req.threat.uav_ids.front();
                res.deconfliction_plans.push_back(temp_wp_list);
            }
            // [5] Ruta que aterrice en un landing spot
            gauss_msgs::Waypoint temp_wp;
            gauss_msgs::DeconflictionPlan temp_wp_list;
            temp_wp_list.maneuver_type = 5;
            temp_wp = conflictive_operations.front().flight_plan.waypoints.front();
            temp_wp_list.waypoint_list.push_back(conflictive_operations.front().estimated_trajectory.waypoints.front());
            double distance = pointsDistance(conflictive_operations.front().estimated_trajectory.waypoints.front(), temp_wp);
            temp_wp_list.cost = distance;
            temp_wp_list.waypoint_list.push_back(temp_wp);
            temp_wp_list.riskiness = calculateRiskiness(temp_wp_list);
            temp_wp_list.uav_id = req.threat.uav_ids.front();
            res.deconfliction_plans.push_back(temp_wp_list);

            res.message = "Conflict solved";
            res.success = true;
        }
        // ROS_INFO("[Tactical] Computational time: %0.4f", ros::Time::now().toSec() - start_computational_time);
    }
    int cont = 1;
    for (auto plan : res.deconfliction_plans){
        nav_msgs::Path wpl;
        wpl.header.frame_id = "map";
        for (int w = 0; w < plan.waypoint_list.size(); w++){
            geometry_msgs::PoseStamped temp_wp;
            temp_wp.pose.position.x = plan.waypoint_list.at(w).x;
            temp_wp.pose.position.y = plan.waypoint_list.at(w).y;
            temp_wp.pose.position.z = plan.waypoint_list.at(w).z;
            wpl.poses.push_back(temp_wp);
        }
        switch (cont)
        {
        case 1:
            pub_sol_1_.publish(wpl);
            break;
        case 2:
            pub_sol_2_.publish(wpl);
            break;
        case 3:
            pub_sol_3_.publish(wpl);
            break;
        case 4:
            pub_sol_4_.publish(wpl);
            break;
        case 5:
            pub_sol_5_.publish(wpl);
            break;
        case 6:
            pub_sol_6_.publish(wpl);
            break;
        case 7:
            pub_sol_7_.publish(wpl);
            break;
        case 8:
            pub_sol_8_.publish(wpl);
            break;
        }
        cont++;
    }

    // for (auto i : res.deconfliction_plans) std::cout << i << "\n ----------- \n";

    return true;
}

// MAIN function
int main(int argc, char *argv[]) {
    ros::init(argc, argv, "conflict_solver");

    // Create a ConflictSolver object
    ConflictSolver *conflict_solver = new ConflictSolver();

    ros::spin();
}


#include <iostream>
#include <fstream>
#include <strstream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <ceres/ceres.h>
#include <igl/point_mesh_squared_distance.h>
#include <igl/copyleft/marching_cubes.h>

typedef double FT;
typedef Eigen::Matrix<FT, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef Eigen::Matrix<FT, Eigen::Dynamic, 1> VectorX;
typedef Eigen::Matrix<FT, 3, 1> Vector3;

class UniformGrid
{
public:
	UniformGrid()
	: N(0)
	{}
	UniformGrid(int _N) {
		N = _N;
		distances.resize(N);
		for (auto& d : distances) {
			d.resize(N);
			for (auto& v : d)
				v.resize(N, 1e30);
		}
	}
	template <class T>
	T distance(const T* const p) const {
		int px = *(double*)&p[0] * N;
		int py = *(double*)&p[1] * N;
		int pz = *(double*)&p[2] * N;
		if (px < 0 || py < 0 || pz < 0 || px >= N - 1 || py >= N - 1 || pz >= N - 1) {
			T l = (T)0;
			if (px < 0)
				l = l + -p[0] * (T)N;
			else if (px >= N)
				l = l + (p[0] * (T)N - (T)(N - 1 - 1e-3));

			if (py < 0)
				l = l + -p[1] * (T)N;
			else if (py >= N)
				l = l + (p[1] * (T)N - (T)(N - 1 - 1e-3));

			if (pz < 0)
				l = l + -p[2] * (T)N;
			else if (pz >= N)
				l = l + (p[2] * (T)N - (T)(N - 1 - 1e-3));

			return l;
		}
		T wx = p[0] * (T)N - (T)px;
		T wy = p[1] * (T)N - (T)py;
		T wz = p[2] * (T)N - (T)pz;
		T w0 = ((T)1 - wx) * ((T)1 - wy) * ((T)1 - wz) * distances[pz    ][py    ][px    ];
		T w1 = wx 		   * ((T)1 - wy) * ((T)1 - wz) * distances[pz    ][py    ][px + 1];
		T w2 = ((T)1 - wx) * wy 		 * ((T)1 - wz) * distances[pz    ][py + 1][px    ];
		T w3 = wx 		   * wy 		 * ((T)1 - wz) * distances[pz    ][py + 1][px + 1];
		T w4 = ((T)1 - wx) * ((T)1 - wy) * wz 		   * distances[pz + 1][py    ][px    ];
		T w5 = wx 		   * ((T)1 - wy) * wz 		   * distances[pz + 1][py    ][px + 1];
		T w6 = ((T)1 - wx) * wy 		 * wz		   * distances[pz + 1][py + 1][px    ];
		T w7 = wx 		   * wy 		 * wz 		   * distances[pz + 1][py + 1][px + 1];
		T res = w0 + w1 + w2 + w3 + w4 + w5 + w6 + w7;
		T thres = (T)0.02;

		//commented out for deform_tune
		if (res > thres)
			res = thres;
		//

		return res;
	}
	int N;
	std::vector<std::vector<std::vector<double> > > distances;
};

struct LengthError {
  LengthError(const Eigen::Vector3d& v_, double lambda_)
  : v(v_), lambda(lambda_) {}

  template <typename T>
  bool operator()(const T* const p1,
                  const T* const p2,
                  T* residuals) const {
  	T px = p1[0] - p2[0];
  	T py = p1[1] - p2[1];
  	T pz = p1[2] - p2[2];
  	residuals[0] = px - v[0];
  	residuals[1] = py - v[1];
  	residuals[2] = pz - v[2];
    return true;
  }

   // Factory to hide the construction of the CostFunction object from
   // the client code.
   static ceres::CostFunction* Create(const Eigen::Vector3d& v, const double lambda_) {
     return (new ceres::AutoDiffCostFunction<LengthError, 3, 3, 3>(
                 new LengthError(v, lambda_)));
   }
   double lambda;
   Eigen::Vector3d v;
};

struct DistanceError {
  DistanceError(UniformGrid* grid_)
  : grid(grid_) {}

  template <typename T>
  bool operator()(const T* const p1,
                  T* residuals) const {
  	residuals[0] = grid->distance(p1);
  	residuals[1] = (T)0;
  	residuals[2] = (T)0;
    return true;
  }

   // Factory to hide the construction of the CostFunction object from
   // the client code.
   static ceres::CostFunction* Create(UniformGrid* grid) {
     return (new ceres::AutoDiffCostFunction<DistanceError, 3, 3>(
                 new DistanceError(grid)));
   }
   UniformGrid* grid;
};

class Mesh
{
public:
	Mesh() : scale(1.0) {}
	std::vector<Eigen::Vector3d> V;
	std::vector<Eigen::Vector3i> F;
	void ReadOBJ(const char* filename) {
		std::ifstream is(filename);
		char buffer[256];
		while (is.getline(buffer, 256)) {
			std::strstream str;
			str << buffer;
			str >> buffer;
			if (strcmp(buffer, "v") == 0) {
				double x, y, z;
				str >> x >> y >> z;
				V.push_back(Eigen::Vector3d(x, y, z));
			}
			else if (strcmp(buffer, "f") == 0) {
				Eigen::Vector3i f;
				for (int j = 0; j < 3; ++j) {
					str >> buffer;
					int id = 0;
					int p = 0;
					while (buffer[p] != '/') {
						id = id * 10 + (buffer[p] - '0');
						p += 1;
					}
					f[j] = id - 1;
				}
				F.push_back(f);
			}
		}
	}

	void ReadOBJ_Manifold(const char* filename) {
		std::ifstream is(filename);
		char buffer[256];
		while (is.getline(buffer, 256)) {
			std::strstream str;
			str << buffer;
			str >> buffer;
			if (strcmp(buffer, "v") == 0) {
				double x, y, z;
				str >> x >> y >> z;
				V.push_back(Eigen::Vector3d(x, y, z));
			}
			else if (strcmp(buffer, "f") == 0) {
				Eigen::Vector3i f;
				int idx_x, idx_y, idx_z;
				str >> idx_x >> idx_y >> idx_z;
				// std::cout<<idx_x <<' '<< idx_y <<' '<< idx_z <<std::endl;
				f = Eigen::Vector3i(idx_x-1, idx_y-1, idx_z-1);
				F.push_back(f);
			}
		}
	}

	void WriteOBJ(const char* filename) {
		std::ofstream os(filename);
		for (int i = 0; i < V.size(); ++i) {
			os << "v " << V[i][0] << " " << V[i][1] << " " << V[i][2] << "\n";
		}
		for (int i = 0; i < F.size(); ++i) {
			os << "f " << F[i][0] + 1 << " " << F[i][1] + 1 << " " << F[i][2] + 1 << "\n";
		}
		os.close();
	}
	double scale;
	Eigen::Vector3d pos;
	void Normalize() {
		double min_p[3], max_p[3];
		for (int j = 0; j < 3; ++j) {
			min_p[j] = 1e30;
			max_p[j] = -1e30;
			for (int i = 0; i < V.size(); ++i) {
				if (V[i][j] < min_p[j])
					min_p[j] = V[i][j];
				if (V[i][j] > max_p[j])
					max_p[j] = V[i][j];
			}
		}
		scale = std::max(max_p[0] - min_p[0], std::max(max_p[1] - min_p[1], max_p[2] - min_p[2])) * 1.1;
		for (int j = 0; j < 3; ++j)
			pos[j] = min_p[j] - 0.05 * scale;
		for (auto& v : V) {
			v = (v - pos) / scale;
		}
		for (int j = 0; j < 3; ++j) {
			min_p[j] = 1e30;
			max_p[j] = -1e30;
			for (int i = 0; i < V.size(); ++i) {
				if (V[i][j] < min_p[j])
					min_p[j] = V[i][j];
				if (V[i][j] > max_p[j])
					max_p[j] = V[i][j];
			}
		}
	}
	void ApplyTransform(Mesh& m) {
		pos = m.pos;
		scale = m.scale;
		for (auto& v : V) {
			v = (v - pos) / scale;
		}
	}
	void ConstructDistanceField(UniformGrid& grid) {
		Eigen::MatrixXd P(grid.N * grid.N * grid.N, 3);
		int offset = 0;
		for (int i = 0; i < grid.N; ++i) {
			for (int j = 0; j < grid.N; ++j) {
				for (int k = 0; k < grid.N; ++k) {
					P.row(offset) = Eigen::Vector3d(double(k) / grid.N, double(j) / grid.N, double(i) / grid.N);
					offset += 1;
				}
			}
		}

		Eigen::MatrixXd V2(V.size(), 3);
		for (int i = 0; i < V.size(); ++i)
			V2.row(i) = V[i];

		Eigen::MatrixXi F2(F.size(), 3);
		for (int i = 0; i < F.size(); ++i)
			F2.row(i) = F[i];

		Eigen::MatrixXd N(F.size(), 3);
		for (int i = 0; i < F.size(); ++i) {
			Eigen::Vector3d x = V[F[i][1]] - V[F[i][0]];
			Eigen::Vector3d y = V[F[i][2]] - V[F[i][0]];
			N.row(i) = x.cross(y).normalized();
		}

		Eigen::VectorXd sqrD;
		Eigen::VectorXi I;
		Eigen::MatrixXd C;
		igl::point_mesh_squared_distance(P,V2,F2,sqrD,I,C);

		offset = 0;

		for (int i = 0; i < grid.N; ++i) {
			for (int j = 0; j < grid.N; ++j) {
				for (int k = 0; k < grid.N; ++k) {
					Eigen::Vector3d n = N.row(I[offset]);
					Eigen::Vector3d off = P.row(offset);
					off -= V[F[I[offset]][0]];
					double d = n.dot(off);
					d = 1;
					if (d > 0)
						grid.distances[i][j][k] = sqrt(sqrD[offset]);
					else
						grid.distances[i][j][k] = sqrt(-sqrD[offset]);
					offset += 1;
				}
			}
		}

	}

	void FromDistanceField(UniformGrid& grid) {
		Eigen::VectorXd S(grid.N * grid.N * grid.N);
		Eigen::MatrixXd GV(grid.N * grid.N * grid.N, 3);
		int offset = 0;
		for (int i = 0; i < grid.N; ++i) {
			for (int j = 0; j < grid.N; ++j) {
				for (int k = 0; k < grid.N; ++k) {
					S[offset] = grid.distances[i][j][k];
					GV.row(offset) = Eigen::Vector3d(k, j, i);
					offset += 1;
				}
			}
		}
		Eigen::MatrixXd SV;
		Eigen::MatrixXi SF;
		igl::copyleft::marching_cubes(S,GV,grid.N,grid.N,grid.N,SV,SF);
		V.resize(SV.rows());
		F.resize(SF.rows());
		for (int i = 0; i < SV.rows(); ++i)
			V[i] = SV.row(i) / (double)grid.N;
		for (int i = 0; i < SF.rows(); ++i)
			F[i] = SF.row(i);
	}

	void Deform(UniformGrid& grid) {
		double lambda = 1e-3;
		ceres::Problem problem;

		//Move vertices
		std::vector<ceres::ResidualBlockId> v_block_ids;
		for (int i = 0; i < V.size(); ++i) {
			ceres::CostFunction* cost_function = DistanceError::Create(&grid);
			ceres::ResidualBlockId block_id = problem.AddResidualBlock(cost_function, 0, V[i].data());
			v_block_ids.push_back(block_id);
		}

		//Enforce rigidity
		std::vector<ceres::ResidualBlockId> edge_block_ids;
		for (int i = 0; i < F.size(); ++i) {
			for (int j = 0; j < 3; ++j) {
				Eigen::Vector3d v = (V[F[i][j]] - V[F[i][(j + 1) % 3]]);
				ceres::CostFunction* cost_function = LengthError::Create(v, lambda);
				ceres::ResidualBlockId block_id = problem.AddResidualBlock(cost_function, 0, V[F[i][j]].data(), V[F[i][(j + 1) % 3]].data());
				edge_block_ids.push_back(block_id);
			}
		}

		ceres::Solver::Options options;
		options.max_num_iterations = 100;
		options.linear_solver_type = ceres::SPARSE_SCHUR;
		options.minimizer_progress_to_stdout = true;
		ceres::Solver::Summary summary;
		ceres::Solve(options, &problem, &summary);
		std::cout << summary.FullReport() << "\n";

		//V error
		ceres::Problem::EvaluateOptions v_options;
		v_options.residual_blocks = v_block_ids;
		double v_cost;
		problem.Evaluate(v_options, &v_cost, NULL, NULL, NULL);
		std::cout<<"Vertices cost: "<<v_cost<<std::endl;

		//E error
		ceres::Problem::EvaluateOptions edge_options;
		edge_options.residual_blocks = edge_block_ids;
		double edge_cost;
		problem.Evaluate(edge_options, &edge_cost, NULL, NULL, NULL);
		std::cout<<"Rigidity cost: "<<edge_cost<<std::endl;

		double final_cost = v_cost + edge_cost;
		std::cout<<"Final cost: "<<final_cost<<std::endl;
	}

	double Get_Final_Cost(Mesh& ref){
		//normalize by number of vertices and faces
		int ref_num_v = ref.V.size();
		int source_num_f = F.size();

		MatrixX SV(V.size(), 3), RV(ref.V.size(), 3);
		Eigen::MatrixXi SF(F.size(), 3);
		for (int i = 0; i < V.size(); ++i)
			SV.row(i) = V[i];
		for (int i = 0; i < ref.V.size(); ++i)
			RV.row(i) = ref.V[i];
		for (int i = 0; i < F.size(); ++i)
			SF.row(i) = F[i];

		VectorX sqrD;
		Eigen::VectorXi I;
		MatrixX C;
		igl::point_mesh_squared_distance(RV, SV, SF,sqrD,I,C);
		FT coverage_cost = sqrD.sum() * 0.5;
		std::cout<<"Coverage cost: "<<coverage_cost << std::endl;

		FT rigidity_cost = 0.0;
		std::cout<<"Rigidity cost: "<<rigidity_cost<<std::endl;

		FT final_cost = coverage_cost/ref_num_v + rigidity_cost/source_num_f;
		std::cout<<"Final cost: "<<final_cost<<std::endl;

		return final_cost;
	}

};

//For deformation
int main(int argc, char** argv) {
	if (argc < 4) {
		printf("./deform source.obj reference.obj output.obj textfile_name\n");
		return 0;
	}
	//Deform source to fit the reference

	Mesh src, ref;
	src.ReadOBJ_Manifold(argv[1]);
	ref.ReadOBJ_Manifold(argv[2]);

	//Get number of vertices and faces
	std::cout<<"Source:\t\t"<<"Num vertices: "<<src.V.size()<<"\tNum faces: "<<src.F.size()<<std::endl;
	std::cout<<"Reference:\t"<<"Num vertices: "<<ref.V.size()<<"\tNum faces: "<<ref.F.size()<<std::endl<<std::endl;

	UniformGrid grid(100);
	ref.Normalize();
	src.ApplyTransform(ref);

	ref.ConstructDistanceField(grid);

	src.Deform(grid);

	double cost;
	cost = src.Get_Final_Cost(ref);

	double threshold = 1e-3;

	//for deform_tune
	// double threshold = 1e-3;
	// double threshold = 5e-4; #does not filter valid deformations

	if (cost > threshold){
		std::cout<<"INVALID"<<std::endl;
		// return 0;
	}

	else {
		std::cout<<"Deformed"<<std::endl;
	}

	std::string text_file_name = argv[4];
	std::ofstream outfile;
	outfile.open(text_file_name, std::fstream::in | std::fstream::out | std::fstream::app);
	if (!outfile){
		outfile.open(text_file_name,  std::fstream::in | std::fstream::out | std::fstream::trunc);
	}
	//output_objectcost
	outfile<<argv[3]<<'\t'<<cost<<std::endl;
	outfile.close();

	std::ifstream is(argv[1]);
	std::ofstream os(argv[3]);
	char buffer[1024];
	int offset = 0;
	while (is.getline(buffer, 1024)) {
		if (buffer[0] == 'v' && buffer[1] == ' ') {
			auto v = src.V[offset++] * src.scale + src.pos;
			os << "v " << v[0] << " " << v[1] << " " << v[2] << "\n";
		} else {
			os << buffer << "\n";
		}
	}

	is.close();
	os.close();
	return 0;
}


#include <iostream>
#include <math.h>
#include <ctime>
#include <string>
#include <sstream>
#include <cstring>
#include <array>
#include <chrono>

#include <openssl/evp.h>
#include <openssl/bn.h>
#include <openssl/rand.h>
#include <openssl/sha.h>

#include "src/pir.hpp"
#include "src/adprp.hpp"
#include "src/client.hpp"
#include "src/server.hpp"

#include <boost/math/distributions/hypergeometric.hpp>
#include <random>
#include "src/serial.hpp"

#define ONESEC 1000000

using namespace std;

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

    /* Make test database and set related parms */

    uint32_t db_size = 7000;
    uint32_t set_size = 80;
    uint32_t nbr_sets = 90*12;
    int opt;

    while ((opt = getopt(argc, argv, "d:s:n:")) != -1) {
        if (opt == 'd') {
            db_size = atoi(optarg);
        }
        else if (opt == 's') {
            set_size = atoi(optarg);
        }
        else if (opt == 'n') {
            nbr_sets = atoi(optarg);
        }
    }

    std::random_device rd;
    Database my_database(db_size);
    for (int i = 0; i < db_size; i++) {
        auto val = generateRandBlock();
        my_database[i] = val;
    }

// Offline phase begin

    /* Setup client */
    PIRClient client;

    /* Client set params */
    client.set_parms(db_size, set_size, nbr_sets);

    /* Client generate set keys (locally stored) */
    client.generate_setkeys();

    /* Client generate offline query for fetching hints (preprocessed info) */
    OfflineQuery offline_qry = client.generate_offline_query();
    assert(equal_offline_query(offline_qry, deserialize_offline_query(serialize_offline_query(offline_qry))));

    cout << "offline query size: "
      << double(serialize_offline_query(offline_qry).size()) / double(1000) << " KB" << endl;

    /* Setup server */
    PIRServer server;

    /* Server setup database */
    server.set_database(db_size, &my_database);
    //std::cout << "server database set." << std::endl;

    /* Offline reply: output hints (preprocessed info) */
    auto s_prep_st = chrono::high_resolution_clock::now();
    OfflineReply offline_reply = server.generate_offline_reply(offline_qry, 1);
    auto s_prep_ed = chrono::high_resolution_clock::now();
    auto s_prep_time = std::chrono::duration_cast<std::chrono::microseconds>
            (s_prep_ed - s_prep_st).count();
    cout << "Prep: server generates offline reply in "
    << double(s_prep_time)/double(ONESEC) << " sec" << endl;

    OfflineReply a = deserialize_offline_reply(serialize_offline_reply(offline_reply));
    assert(equal_offline_reply(a, offline_reply));

    cout << "offline reply size: "
      << double(serialize_offline_reply(offline_reply).size()) / double(1000) << " KB" << endl;

    /* Offline client hint: client locally stores hints */
    client.update_local_hints(offline_reply);

// Offline phase end

// Online phase begin

    /* Online query */
    OnlineQuery online_qry;
    uint32_t qry_idx = rand()%db_size;
    online_qry = client.generate_online_query(qry_idx);

    assert(equal_online_query(online_qry, deserialize_online_query(serialize_online_query(online_qry))));

    cout << "online query size: "
      << double(serialize_online_query(online_qry).size()) / double(1000) << " KB" << endl;

    /* Online reply */
    OnlineReply online_reply = server.generate_online_reply(online_qry, 1);
    assert(equal_online_reply(online_reply, deserialize_online_reply(serialize_online_reply(online_reply))));
    cout << "online reply size: "
      << double(serialize_online_reply(online_reply).size()) / double(1000) << " KB" << endl;

    OnlineReply online_reply1 = deserialize_online_reply(serialize_online_reply(online_reply));

    /* Client recover data block */
    Block blk = client.recover_block(online_reply1);

    if (blk == my_database[qry_idx]) {
        std::cout << "recover success!" << std::endl;
    } else {
        std::cout << "recover fail!" << std::endl;
    }

    /* Client refresh (probabilistic correctness version) */
    OnlineQuery refresh_query = client.generate_refresh_query(qry_idx);

    cout << "refresh query size: "
      << double(serialize_online_query(refresh_query).size()) / double(1000) << " KB" << endl;

    OnlineReply refresh_reply = server.generate_online_reply(refresh_query, 1);

    cout << "refresh reply size: "
      << double(serialize_online_reply(refresh_reply).size()) / double(1000) << " KB" << endl;

    client.sets[client.cur_qry_setno].hint = blk ^ refresh_reply.parity;

    // incorporate Checklist refresh if possible later

// Online phase end

// Batched addition begin (adding a single batch)

    /* Set up data objects to be added */
    int nbr_add = 0.05 * db_size;   // number of additions (i.e., batch size) = 1% of database size

    // assign random value to items to be added
    std::vector<Block> v(nbr_add);
    for (int i = 0; i < nbr_add; i++) {
        v[i] = rand()%100000;
    }

    /* Server adds new data objects */
    server.add_elements(nbr_add, v);

    /* Server notifies client about 'nbr_add' */
    // TODO: we probably can omit this in testing?

    /* Client generates batched addition query */
    OfflineAddQueryShort offline_add_qry = client.batched_addition_query(nbr_add);
    cout << "client add query generated." << endl;

    assert(equal_offline_add_query(offline_add_qry, deserialize_offline_add_query(serialize_offline_add_query(offline_add_qry))));
    cout << "batched addition query size: "
      << double(serialize_offline_add_query(offline_add_qry).size()) / double(1000) << " KB" << endl;

    /* Server generates batched addition reply */
    OfflineAddQueryShort offline_add_qry1 = deserialize_offline_add_query(serialize_offline_add_query(offline_add_qry));
    OfflineReply offline_add_reply = server.batched_addition_reply(offline_add_qry1);

    OfflineReply offline_add_reply1 = deserialize_offline_reply(serialize_offline_reply(offline_add_reply));

    cout << "batched addition reply size: "
      << double(serialize_offline_reply(offline_add_reply).size()) / double(1000) << " KB" << endl;

    /* Client update local hints after one batched addition */
    client.update_local_hints(offline_add_reply1);

// Batched addition end

// Test begin
// In order to see if hints are correctly updated, let client issue online query

    /* Test client online query after one batched addition */
    qry_idx = 7010;
    online_qry = client.generate_online_query(qry_idx);

    /* Server generates online reply */
    online_reply = server.generate_online_reply(online_qry, 1);

    /* Client recovers block */
    blk = client.recover_block(online_reply);
    if (blk == my_database[qry_idx])
        std::cout << "success!" << std::endl;
    else std::cout << "fail!" << std::endl;

    /* Client refresh */
    refresh_query = client.generate_refresh_query(qry_idx);
    refresh_reply = server.generate_online_reply(refresh_query, 1);
    client.sets[client.cur_qry_setno].hint = blk ^ refresh_reply.parity;

// Test end

    return 0;
}