#include <gauss_msgs/NewDeconfliction.h>
#include <gauss_msgs/NewThreats.h>
#include <gauss_msgs/ReadIcao.h>
#include <gauss_msgs/ReadOperation.h>
#include <gauss_msgs/ReadGeofences.h>
#include <gauss_msgs/Waypoint.h>
#include <geometry_msgs/Vector3.h>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <Eigen/Eigen>

bool in_range(double x, double min_x, double max_x) {
    return (x > min_x) && (x < max_x);
}

double clamp(double x, double min_x, double max_x) {
    if (min_x > max_x) {
        ROS_ERROR("[Monitoring] min_x[%lf] > max_x[%lf], swapping!", min_x, max_x);
        std::swap(min_x, max_x);
    }
    return std::max(std::min(x, max_x), min_x);
}

double dot(const geometry_msgs::Vector3& u, const geometry_msgs::Vector3& v) {
    return u.x * v.x + u.y * v.y + u.z * v.z;
}

double length(const geometry_msgs::Vector3& u) {
    return sqrt(u.x * u.x + u.y * u.y + u.z * u.z);
}

// TODO: Use geometry_msgs/Point instead of Waypoint?
geometry_msgs::Vector3 vector_from_point_to_point(const gauss_msgs::Waypoint& A, const gauss_msgs::Waypoint& B) {
    geometry_msgs::Vector3 AB;
    AB.x = B.x - A.x;
    AB.y = B.y - A.y;
    AB.z = B.z - A.z;
    return AB;
}

// Signed Distance Fucntion from P to:
// a sphere centered in C with radius r
double sdSphere(const gauss_msgs::Waypoint& P, const gauss_msgs::Waypoint& C, double r) {
    return length(vector_from_point_to_point(C, P)) - r;
}

// Signed Distance Fucntion from P to:
// a segment (A,B) with some radius r
double sdSegment(const gauss_msgs::Waypoint& P, const gauss_msgs::Waypoint& A, const gauss_msgs::Waypoint& B, double r = 0) {
    geometry_msgs::Vector3 AP = vector_from_point_to_point(A, P);
    geometry_msgs::Vector3 AB = vector_from_point_to_point(A, B);
    double h = clamp(dot(AP, AB) / dot(AB, AB), 0.0, 1.0);
    geometry_msgs::Vector3 aux;
    aux.x = AP.x - AB.x * h;  // TODO: Use eigen?
    aux.y = AP.y - AB.y * h;
    aux.z = AP.z - AB.z * h;
    return length(aux) - r;
}

geometry_msgs::Point translateToPoint(const gauss_msgs::Waypoint& WP) {
    geometry_msgs::Point P;
    P.x = WP.x;
    P.y = WP.y;
    P.z = WP.z;
    return P;
}

struct Segment {
    Segment() = default;
    Segment(gauss_msgs::Waypoint A, gauss_msgs::Waypoint B) {
        // if (A == B) { ROS_WARN("A == B == [%lf, %lf, %lf, %lf]", A.x, A.y, A.z, A.stamp.toSec()); }  // TODO: compare function
        point_A = A;
        point_B = B;
        t_A = A.stamp.toSec();
        t_B = B.stamp.toSec();
        if (t_A >= t_B) {
            // ROS_WARN("t_A[%lf] >= t_B[%lf]", t_A, t_B);
        }
    }

    gauss_msgs::Waypoint point_at_time(double t) const {
        if (t < t_A) {
            // ROS_WARN("t[%lf] < t_A[%lf]", t, t_A);
            return point_A;
        }
        if (t > t_B) {
            // ROS_WARN("t[%lf] > t_B[%lf]", t, t_B);
            return point_B;
        }
        if (t_A == t_B) {
            // ROS_WARN("t_A == t_B == %lf", t_A);
            return point_A;
        }
        if (std::isnan(t)) {
            // ROS_WARN("t is NaN");
            return point_A;
        }

        double m = (t - t_A) / (t_B - t_A);
        gauss_msgs::Waypoint point;
        point.x = point_A.x + m * (point_B.x - point_A.x);
        point.y = point_A.y + m * (point_B.y - point_A.y);
        point.z = point_A.z + m * (point_B.z - point_A.z);
        point.stamp.fromSec(t);
        return point;
    }

    gauss_msgs::Waypoint point_at_param(double m) const {
        gauss_msgs::Waypoint point;
        point.x = point_A.x + m * (point_B.x - point_A.x);
        point.y = point_A.y + m * (point_B.y - point_A.y);
        point.z = point_A.z + m * (point_B.z - point_A.z);
        double t =  t_A + m * (t_B - t_A);
        point.stamp.fromSec(t);
        return point;
    }

    visualization_msgs::Marker translateToMarker(int id, std_msgs::ColorRGBA color) {
        visualization_msgs::Marker marker;
        marker.header.stamp = ros::Time::now();
        marker.header.frame_id = "map";  // TODO: other?
        marker.ns = "segments";
        marker.id = id;
        marker.type = visualization_msgs::Marker::ARROW;
        marker.action = visualization_msgs::Marker::ADD;
        marker.pose.orientation.w = 1;
        marker.scale.x = 5.0;  // shaft diameter
        marker.scale.y = 1.5;  // head diameter
        marker.scale.z = 1.0;  // head length (if not zero)
        marker.color = color;
        marker.lifetime = ros::Duration(1.0);  // TODO: pair with frequency
        marker.points.push_back(translateToPoint(point_A));
        marker.points.push_back(translateToPoint(point_B));
        return marker;
    }

    friend std::ostream& operator<<(std::ostream& out, const Segment& s);
    gauss_msgs::Waypoint point_A;
    gauss_msgs::Waypoint point_B;
    double t_A = 0;
    double t_B = 0;
};

std::ostream& operator<<(std::ostream& out, const Segment& s) {
    out << "[(" << s.point_A << "); (" << s.point_B << ")]";
    return out;
}

std::pair<geometry_msgs::Vector3, geometry_msgs::Vector3> delta(const Segment& first, const Segment& second) {
    geometry_msgs::Vector3 delta_alpha;
    delta_alpha.x = second.point_A.x - first.point_A.x;
    delta_alpha.y = second.point_A.y - first.point_A.y;
    delta_alpha.z = second.point_A.z - first.point_A.z;
    geometry_msgs::Vector3 delta_beta;
    delta_beta.x = second.point_B.x - first.point_B.x;
    delta_beta.y = second.point_B.y - first.point_B.y;
    delta_beta.z = second.point_B.z - first.point_B.z;
    return std::make_pair(delta_alpha, delta_beta);
}

double sq_distance(const Segment& first, const Segment& second, double mu) {
    if (mu < 0) {
        // ROS_WARN("mu[%lf] < 0, clamping!", mu);
        mu = 0;
    }

    if (mu > 1) {
        // ROS_WARN("mu[%lf] > 1, clamping!", mu);
        mu = 1;
    }

    auto d = delta(first, second);
    double delta_x = d.first.x + mu * (d.second.x - d.first.x);
    double delta_y = d.first.y + mu * (d.second.y - d.first.y);
    double delta_z = d.first.z + mu * (d.second.z - d.first.z);
    return pow(delta_x, 2) + pow(delta_y, 2) + pow(delta_z, 2);
}

std::pair<double, double> quadratic_roots(double a, double b, double c) {
    if ((a == 0) && (b == 0) && (c == 0)) {
        // ROS_WARN("a = b = c = 0, any number is a solution!");
        return std::make_pair(std::nan(""), std::nan(""));
    }
    if ((a == 0) && (b == 0) && (c != 0)) {
        // ROS_WARN("a = b = 0, there is no solution!");
        return std::make_pair(std::nan(""), std::nan(""));
    }
    if ((a == 0) && (b != 0)) {
        // ROS_WARN("a = 0, non quadratic!");
        return std::make_pair(-c / b, -c / b);
    }

    float d = b * b - 4 * a * c;
    if (d < 0) {
        // ROS_WARN("d = [%lf], complex solutions!", d);
        return std::make_pair(std::nan(""), std::nan(""));
    }
    double e = sqrt(d);
    return std::make_pair((-b - e) / (2 * a), (-b + e) / (2 * a));
}

std::vector<Segment> getFirstSetOfContiguousSegments(const std::vector<Segment>& input) {
    std::vector<Segment> output;
    if (input.size() < 1) {
        ROS_ERROR("[Monitoring] input.size() < 1");
        return output;
    }

    output.push_back(input[0]);
    float t_gap_threshold = 1.0;  // [s]  TODO: as a parameter?
    for (int i = 1; i < input.size(); i++) {
        double t_gap = fabs(input[i].t_A - input[i - 1].t_B);
        if (t_gap > t_gap_threshold) {
            // i-th Segment is not contiguous
            break;
        }
        output.push_back(input[i]);
    }

    return output;
}

geometry_msgs::Vector3 getUnitOutwardVector(const gauss_msgs::Circle& circle, const gauss_msgs::Waypoint wp) {
    geometry_msgs::Vector3 out;
    out.x = wp.x - circle.x_center;
    out.y = wp.y - circle.y_center;
    auto len = sqrt(pow(out.x, 2) + pow(out.y, 2));
    out.x /= len;
    out.y /= len;
    return out;
}

struct LossConflictiveSegments {
    LossConflictiveSegments(const Segment& first, const Segment& second) : first(first), second(second) {}

    friend std::ostream& operator<<(std::ostream& out, const LossConflictiveSegments& r);
    Segment first;
    Segment second;
    double t_min = std::nan("");
    double s_min = std::nan("");
    double t_crossing_0 = std::nan("");
    double t_crossing_1 = std::nan("");
    bool threshold_is_violated = false;
};

std::ostream& operator<<(std::ostream& out, const LossConflictiveSegments& r) {
    out << "first = " << r.first << '\n';
    out << "second = " << r.second << '\n';
    out << "t_min[s] = " << r.t_min << '\n';
    out << "s_min[m2] = " << r.s_min << '\n';
    out << "t_crossing_0[s] = " << r.t_crossing_0 << '\n';
    out << "t_crossing_1[s] = " << r.t_crossing_1 << '\n';
    out << "threshold_is_violated = " << r.threshold_is_violated << '\n';
    return out;
}

LossConflictiveSegments checkUnifiedSegmentsLoss(Segment first, Segment second, double s_threshold) {
    // print('checkUnifiedSegmentsLoss:')
    // print(first.point_A)
    // print(first.point_B)
    // print('___________')
    // print(second.point_A)
    // print(second.point_B)
    auto d = delta(first, second);
    // print(d)
    double c_x = pow(d.first.x, 2);
    double c_y = pow(d.first.y, 2);
    double c_z = pow(d.first.z, 2);
    double b_x = 2 * (d.first.x * d.second.x - c_x);
    double b_y = 2 * (d.first.y * d.second.y - c_y);
    double b_z = 2 * (d.first.z * d.second.z - c_z);
    double a_x = pow(d.second.x - d.first.x, 2);
    double a_y = pow(d.second.y - d.first.y, 2);
    double a_z = pow(d.second.z - d.first.z, 2);
    double a = a_x + a_y + a_z;
    double b = b_x + b_y + b_z;
    double c = c_x + c_y + c_z;

    double mu_min, t_min, s_min;
    if (a == 0) {
        // ROS_WARN("a = 0");
        if (b >= 0) {
            mu_min = 0;
            t_min = first.t_A;
            s_min = c;
        } else {
            mu_min = 1;
            t_min = first.t_B;
            s_min = b + c;
        }

    } else {  // a != 0
        double mu_star = -0.5 * b / a;
        double t_star = first.t_A + mu_star * (first.t_B - first.t_A);
        // print(mu_star)
        // print(t_star)
        // print(sq_distance(first, second, mu_star))
        mu_min = clamp(mu_star, 0, 1);
        t_min = first.t_A + mu_min * (first.t_B - first.t_A);
        s_min = sq_distance(first, second, mu_min);
        // print(mu_min)
        // print(t_min)
        // print(s_min)
    }
    auto result = LossConflictiveSegments(first, second);
    result.t_min = t_min;
    result.s_min = s_min;

    if (s_min > s_threshold) {
        // ROS_INFO("s_min[%lf] > s_threshold[%lf]", s_min, s_threshold);
        result.threshold_is_violated = false;
        return result;
    }

    auto mu_bar = quadratic_roots(a, b, c - s_threshold);
    double t_bar_0 = first.t_A + mu_bar.first * (first.t_B - first.t_A);
    double t_bar_1 = first.t_A + mu_bar.second * (first.t_B - first.t_A);
    // print(mu_bar)
    // print(t_bar_0, t_bar_1)
    double mu_crossing_0 = clamp(mu_bar.first, 0, 1);
    double mu_crossing_1 = clamp(mu_bar.second, 0, 1);
    double t_crossing_0 = first.t_A + mu_crossing_0 * (first.t_B - first.t_A);
    double t_crossing_1 = first.t_A + mu_crossing_1 * (first.t_B - first.t_A);
    // print(mu_crossing_0, mu_crossing_1)
    // print(t_crossing_0, t_crossing_1)
    auto first_in_conflict = Segment(first.point_at_time(t_crossing_0), first.point_at_time(t_crossing_1));
    auto second_in_conflict = Segment(second.point_at_time(t_crossing_0), second.point_at_time(t_crossing_1));

    result.first = first_in_conflict;
    result.second = second_in_conflict;
    result.t_crossing_0 = t_crossing_0;
    result.t_crossing_1 = t_crossing_1;
    result.threshold_is_violated = true;
    return result;
}

LossConflictiveSegments checkSegmentsLoss(const std::pair<Segment, Segment>& segments, double s_threshold) {
    // print('checkSegmentsLoss:')
    // print(first.point_A)
    // print(first.point_B)
    // print('___________')
    // print(second.point_A)
    // print(second.point_B)
    double t_A1 = segments.first.t_A;
    double t_B1 = segments.first.t_B;
    double t_A2 = segments.second.t_A;
    double t_B2 = segments.second.t_B;
    double t_alpha = std::max(t_A1, t_A2);
    double t_beta = std::min(t_B1, t_B2);
    if (t_alpha > t_beta) {
        // ROS_INFO("t_alpha[%lf] > t_beta[%lf]", t_alpha, t_beta);
        return LossConflictiveSegments(segments.first, segments.second);
    }

    auto P_alpha1 = segments.first.point_at_time(t_alpha);
    auto P_beta1 = segments.first.point_at_time(t_beta);
    auto P_alpha2 = segments.second.point_at_time(t_alpha);
    auto P_beta2 = segments.second.point_at_time(t_beta);
    return checkUnifiedSegmentsLoss(Segment(P_alpha1, P_beta1), Segment(P_alpha2, P_beta2), s_threshold);
}

std::vector<LossConflictiveSegments> checkTrajectoriesLoss(const std::pair<gauss_msgs::WaypointList, gauss_msgs::WaypointList>& trajectories, double s_threshold) {
    std::vector<LossConflictiveSegments> segment_loss_results;
    if (trajectories.first.waypoints.size() < 2) {
        // TODO: Warn and push the same point twice?
        ROS_ERROR("[Monitoring] Trajectory must contain at least 2 points, [%ld] found in first argument", trajectories.first.waypoints.size());
        return segment_loss_results;
    }
    if (trajectories.second.waypoints.size() < 2) {
        // TODO: Warn and push the same point twice?
        ROS_ERROR("[Monitoring] Trajectory must contain at least 2 points, [%ld] found in second argument", trajectories.second.waypoints.size());
        return segment_loss_results;
    }

    for (int i = 0; i < trajectories.first.waypoints.size() - 1; i++) {
        std::pair<Segment, Segment> segments;
        // printf("First segment, i = %d\n", i);
        segments.first = Segment(trajectories.first.waypoints[i], trajectories.first.waypoints[i + 1]);
        // std::cout << segments.first.point_A << "_____________\n" << segments.first.point_B << '\n';
        for (int j = 0; j < trajectories.second.waypoints.size() - 1; j++) {
            // printf("Second segment, j = %d\n", j);
            segments.second = Segment(trajectories.second.waypoints[j], trajectories.second.waypoints[j + 1]);
            // std::cout << segments.second.point_A << "_____________\n" << segments.second.point_B << '\n';
            auto loss_check = checkSegmentsLoss(segments, s_threshold);
            if (loss_check.threshold_is_violated) {
                // ROS_ERROR("[Monitoring] Loss of separation! [i = %d, j = %d]", i, j);
                // std::cout << loss_check << '\n';
                segment_loss_results.push_back(loss_check);
            }
        }
    }
    return segment_loss_results;
}

struct LossExtreme {
    gauss_msgs::Waypoint in_point;
    gauss_msgs::Waypoint out_point;
};

visualization_msgs::Marker translateToMarker(const LossExtreme& extremes, int id = 0) {
    visualization_msgs::Marker marker;
    std_msgs::ColorRGBA red, green;  // TODO: constants
    red.r = 1.0;
    red.a = 1.0;
    green.g = 1.0;
    green.a = 1.0;

    marker.header.stamp = ros::Time::now();
    marker.header.frame_id = "map";  // TODO: other?
    marker.ns = "extremes";          // TODO: other?
    marker.id = id;
    marker.type = visualization_msgs::Marker::SPHERE_LIST;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.orientation.w = 1;
    marker.scale.x = 5.0;
    marker.scale.y = 5.0;
    marker.scale.z = 5.0;
    marker.lifetime = ros::Duration(1.0);  // TODO: pair with frequency
    marker.points.push_back(translateToPoint(extremes.in_point));
    marker.colors.push_back(red);
    marker.points.push_back(translateToPoint(extremes.out_point));
    marker.colors.push_back(green);
    return marker;
}

gauss_msgs::ConflictiveOperation fillConflictiveOperation(const int& _trajectory_index, const std::map<int, gauss_msgs::Operation>& _index_to_operation_map) {
    gauss_msgs::ConflictiveOperation out_msg;
    out_msg.actual_wp = _index_to_operation_map.at(_trajectory_index).track.waypoints.back();
    out_msg.current_wp = _index_to_operation_map.at(_trajectory_index).current_wp;
    out_msg.estimated_trajectory = _index_to_operation_map.at(_trajectory_index).estimated_trajectory;
    out_msg.flight_plan = _index_to_operation_map.at(_trajectory_index).flight_plan;
    out_msg.flight_plan_updated = _index_to_operation_map.at(_trajectory_index).flight_plan_updated;
    out_msg.landing_spots = _index_to_operation_map.at(_trajectory_index).landing_spots;
    out_msg.operational_volume = _index_to_operation_map.at(_trajectory_index).operational_volume;
    out_msg.uav_id = _index_to_operation_map.at(_trajectory_index).uav_id;

    return out_msg;
}

struct LossResult {
    LossResult(const int i, const int j) : first_trajectory_index(i), second_trajectory_index(j) {}
    friend std::ostream& operator<<(std::ostream& out, const LossResult& r);
    int first_trajectory_index;
    int second_trajectory_index;
    std::vector<LossConflictiveSegments> loss_conflictive_segments;

    bool isEqual(const LossResult& other) {
        const float check_time_margin = 10.0;  // [s]
        return (first_trajectory_index == other.first_trajectory_index && second_trajectory_index == other.second_trajectory_index) &&
               (std::abs(loss_conflictive_segments.front().t_crossing_0 - other.loss_conflictive_segments.front().t_crossing_0) <= check_time_margin ||
                std::abs(loss_conflictive_segments.front().t_crossing_1 - other.loss_conflictive_segments.front().t_crossing_1) <= check_time_margin ||
                std::abs(loss_conflictive_segments.back().t_crossing_0 - other.loss_conflictive_segments.back().t_crossing_0) <= check_time_margin ||
                std::abs(loss_conflictive_segments.back().t_crossing_1 - other.loss_conflictive_segments.back().t_crossing_1) <= check_time_margin);
    }
    gauss_msgs::NewThreat convertToThreat(const std::map<int, gauss_msgs::Operation>& _index_to_operation_map, double& count_id) {
        gauss_msgs::NewThreat out_threat;
        out_threat.threat_id = count_id++;
        out_threat.threat_type = out_threat.LOSS_OF_SEPARATION;
        out_threat.uav_ids.push_back(_index_to_operation_map.at(first_trajectory_index).uav_id);
        out_threat.uav_ids.push_back(_index_to_operation_map.at(second_trajectory_index).uav_id);
        out_threat.priority_ops.push_back(_index_to_operation_map.at(first_trajectory_index).priority);
        out_threat.priority_ops.push_back(_index_to_operation_map.at(second_trajectory_index).priority);
        out_threat.conflictive_operations.push_back(fillConflictiveOperation(first_trajectory_index, _index_to_operation_map));
        out_threat.conflictive_operations.push_back(fillConflictiveOperation(second_trajectory_index, _index_to_operation_map));
        for (auto j : loss_conflictive_segments) {
            out_threat.loss_conflictive_segments.segment_first.push_back(j.first.point_A);
            out_threat.loss_conflictive_segments.segment_first.push_back(j.first.point_B);
            out_threat.loss_conflictive_segments.segment_second.push_back(j.second.point_A);
            out_threat.loss_conflictive_segments.segment_second.push_back(j.second.point_B);
            out_threat.loss_conflictive_segments.t_min = j.t_min;
            out_threat.loss_conflictive_segments.s_min = j.s_min;
            out_threat.loss_conflictive_segments.t_crossing_0 = j.t_crossing_0;
            out_threat.loss_conflictive_segments.t_crossing_1 = j.t_crossing_1;
            out_threat.loss_conflictive_segments.point_at_t_min_segment_first = j.first.point_at_time(j.t_min);
            out_threat.loss_conflictive_segments.point_at_t_min_segment_second = j.second.point_at_time(j.t_min);
        }

        return out_threat;
    }
};

struct GeoConflictiveTrajectory {
    GeoConflictiveTrajectory(int i): trajectory_index(i) {}
    int trajectory_index;
    std::vector<Segment> geofence_conflictive_segments;
    gauss_msgs::Waypoint closest_exit_wp;  // Use the mandatory field as intrusion flag

    bool isEqual(const GeoConflictiveTrajectory& other) {
        const double check_time_margin = 10.0;
        return (trajectory_index == other.trajectory_index &&
                closest_exit_wp.mandatory == other.closest_exit_wp.mandatory &&
               (geofence_conflictive_segments.front().t_A - other.geofence_conflictive_segments.front().t_A <= check_time_margin ||
                geofence_conflictive_segments.back().t_B - other.geofence_conflictive_segments.back().t_B <= check_time_margin));
    }
};

struct GeofenceResult {
    GeofenceResult(int i): geofence_id(i) {}
    int geofence_id;
    std::vector<GeoConflictiveTrajectory> geo_conflictive_trajectories;

    bool isEqual(const GeofenceResult& other) {
        const float check_time_margin = 10.0;  // [s]
        if (geofence_id != other.geofence_id) {
            return false;
        } else {
            for (auto geo_trajectory : geo_conflictive_trajectories) {
                std::vector<GeoConflictiveTrajectory>::const_iterator traj_it = std::find_if(other.geo_conflictive_trajectories.begin(), other.geo_conflictive_trajectories.end(),
                                                                                [geo_trajectory](GeoConflictiveTrajectory _traj) { return _traj.isEqual(geo_trajectory);});
                return (traj_it != other.geo_conflictive_trajectories.end());
            }
        }
    }
    std::vector<gauss_msgs::NewThreat> convertToThreat(const std::map<int, gauss_msgs::Operation>& _index_to_operation_map, const std::map<int, gauss_msgs::Geofence>& _index_to_geofence_map, double& count_id) {
        std::vector<gauss_msgs::NewThreat> out_threats;
        for (auto geo_conflictive_trajectory : geo_conflictive_trajectories) {
            gauss_msgs::NewThreat aux_threat;
            aux_threat.threat_id = count_id++;
            aux_threat.geofence_ids.push_back(geofence_id);
            aux_threat.uav_ids.push_back(_index_to_operation_map.at(geo_conflictive_trajectory.trajectory_index).uav_id);
            aux_threat.conflictive_geofences.push_back(_index_to_geofence_map.at(geofence_id));
            aux_threat.conflictive_operations.push_back(fillConflictiveOperation(geo_conflictive_trajectory.trajectory_index, _index_to_operation_map));
            std::vector<Segment> aux_vec = getFirstSetOfContiguousSegments(geo_conflictive_trajectory.geofence_conflictive_segments);
            for (auto segment : aux_vec) {
                aux_threat.geofence_conflictive_segments.first_contiguous_segment.push_back(segment.point_A);
                aux_threat.geofence_conflictive_segments.first_contiguous_segment.push_back(segment.point_B);
            }
            for (auto segment : geo_conflictive_trajectory.geofence_conflictive_segments) {
                aux_threat.geofence_conflictive_segments.all_segments.push_back(segment.point_A);
                aux_threat.geofence_conflictive_segments.all_segments.push_back(segment.point_B);
            }

            auto geo_circle = _index_to_geofence_map.at(geofence_id).circle;
            auto crossing_0 = aux_threat.geofence_conflictive_segments.first_contiguous_segment.front();
            auto crossing_1 = aux_threat.geofence_conflictive_segments.first_contiguous_segment.back();
            aux_threat.geofence_conflictive_segments.crossing_0_out_vector = getUnitOutwardVector(geo_circle, crossing_0);
            aux_threat.geofence_conflictive_segments.crossing_1_out_vector = getUnitOutwardVector(geo_circle, crossing_1);
            if (geo_conflictive_trajectory.closest_exit_wp.mandatory) {
                aux_threat.threat_type = aux_threat.GEOFENCE_INTRUSION;
                auto closest_exit = geo_conflictive_trajectory.closest_exit_wp;
                aux_threat.geofence_conflictive_segments.closest_exit_wp = closest_exit;
                aux_threat.geofence_conflictive_segments.closest_exit_out_vector = getUnitOutwardVector(geo_circle, closest_exit);
            } else {
                aux_threat.threat_type = aux_threat.GEOFENCE_CONFLICT;
            }
            out_threats.push_back(aux_threat);
        }

        return out_threats;
    }
};

std::vector<LossResult> getContiguousResults(const LossResult& input) {
    std::vector<LossResult> output;
    if (input.loss_conflictive_segments.size() < 1) {
        ROS_ERROR("[Monitoring] input.loss_conflictive_segments.size() < 1");
        return output;
    }

    float t_gap_threshold = 1.0;  // [s]  TODO: as a parameter?
    auto current_loss_result = input;
    current_loss_result.loss_conflictive_segments.clear();
    current_loss_result.loss_conflictive_segments.push_back(input.loss_conflictive_segments[0]);
    for (int i = 1; i < input.loss_conflictive_segments.size(); i++) {
        double t_gap_first = fabs(input.loss_conflictive_segments[i].first.t_A - input.loss_conflictive_segments[i - 1].first.t_B);
        double t_gap_second = fabs(input.loss_conflictive_segments[i].second.t_A - input.loss_conflictive_segments[i - 1].second.t_B);
        if ((t_gap_first > t_gap_threshold) || (t_gap_second > t_gap_threshold)) {
            // i-th element is not contiguous
            output.push_back(current_loss_result);
            current_loss_result.loss_conflictive_segments.clear();
        }
        current_loss_result.loss_conflictive_segments.push_back(input.loss_conflictive_segments[i]);
    }
    output.push_back(current_loss_result);

    return output;
}

std::pair<LossExtreme, LossExtreme> calculateExtremes(const LossResult& result) {
    std::pair<LossExtreme, LossExtreme> extremes;
    if (result.loss_conflictive_segments.size() < 1) {
        ROS_ERROR("[Monitoring] result.loss_conflictive_segments.size() < 1");
        return extremes;
    }

    // In points are for sure A's from segment 0
    extremes.first.in_point = result.loss_conflictive_segments[0].first.point_A;
    extremes.second.in_point = result.loss_conflictive_segments[0].second.point_A;

    // Initialize out points as B's from segment 0...
    extremes.first.out_point = result.loss_conflictive_segments[0].first.point_B;
    extremes.second.out_point = result.loss_conflictive_segments[0].second.point_B;
    //  ...but update if more contiguous segments are available
    float t_gap_threshold = 1.0;  // [s]
    // Check for first
    for (int i = 1; i < result.loss_conflictive_segments.size(); i++) {
        double t_gap = fabs(result.loss_conflictive_segments[i].first.t_A - result.loss_conflictive_segments[i - 1].first.t_B);
        if (t_gap > t_gap_threshold) {
            break;
        }
        extremes.first.out_point = result.loss_conflictive_segments[i].first.point_B;
    }
    // Check for second
    for (int i = 1; i < result.loss_conflictive_segments.size(); i++) {
        double t_gap = fabs(result.loss_conflictive_segments[i].second.t_A - result.loss_conflictive_segments[i - 1].second.t_B);
        if (t_gap > t_gap_threshold) {
            break;
        }
        extremes.second.out_point = result.loss_conflictive_segments[i].second.point_B;
    }

    return extremes;
}

visualization_msgs::Marker translateToMarker(const LossResult& result) {
    visualization_msgs::Marker marker;
    marker.header.stamp = ros::Time::now();
    marker.header.frame_id = "map";  // TODO: other?
    marker.ns = "loss_" + std::to_string(result.first_trajectory_index) + "_" + std::to_string(result.second_trajectory_index);
    marker.type = visualization_msgs::Marker::LINE_LIST;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.orientation.w = 1;
    marker.scale.x = 1.0;
    marker.color.r = 1.0;  // TODO: color?
    marker.color.a = 1.0;
    marker.lifetime = ros::Duration(1.0);  // TODO: pair with frequency
    for (auto segments_loss : result.loss_conflictive_segments) {
        marker.points.push_back(translateToPoint(segments_loss.first.point_A));
        marker.points.push_back(translateToPoint(segments_loss.first.point_B));
        marker.points.push_back(translateToPoint(segments_loss.second.point_A));
        marker.points.push_back(translateToPoint(segments_loss.second.point_B));
    }
    /*
  double t_min = std::nan("");
  double s_min = std::nan("");
  double t_crossing_0 = std::nan("");
  double t_crossing_1 = std::nan("");
  bool threshold_is_violated = false;
*/
    return marker;
}

std::ostream& operator<<(std::ostream& out, const LossResult& r) {
    out << "first_trajectory_index = " << r.first_trajectory_index << '\n';
    out << "second_trajectory_index = " << r.second_trajectory_index << '\n';
    out << "loss_conflictive_segments = [" << r.second_trajectory_index << '\n';
    for (int i = 0; i < r.loss_conflictive_segments.size(); i++) {
        out << r.loss_conflictive_segments[i] << '\n';
    }
    out << "]\n";
    return out;
}

bool happensBefore(const LossResult& a, const LossResult& b) {
    if (a.loss_conflictive_segments.size() < 1) {
        ROS_ERROR("[Monitoring] a.loss_conflictive_segments.size() < 1");
        return false;
    }
    if (b.loss_conflictive_segments.size() < 1) {
        ROS_ERROR("[Monitoring] b.loss_conflictive_segments.size() < 1");
        return true;
    }

    return a.loss_conflictive_segments[0].t_crossing_0 < b.loss_conflictive_segments[0].t_crossing_0;
}

void cleanStoredList(std::vector<LossResult>& _stored_loss_result_list, std::vector<GeofenceResult>& _stored_geofence_result_list, const std::vector<LossResult>& _actual_loss_result_list, const std::vector<GeofenceResult>& _actual_geofence_result_list, const double& _check_time_margin) {
    if (_actual_loss_result_list.size() == 0) {
        _stored_loss_result_list.clear();
    } else {
        for (auto _stored_result = _stored_loss_result_list.begin(); _stored_result != _stored_loss_result_list.end();) {
            std::vector<LossResult>::const_iterator actual_it = std::find_if(_actual_loss_result_list.begin(), _actual_loss_result_list.end(),
                                                                             [_stored_result](LossResult _actual_result) { return _actual_result.isEqual(*_stored_result); });
            bool do_erase = (actual_it == _actual_loss_result_list.end());
            if (do_erase) {
                _stored_result = _stored_loss_result_list.erase(_stored_result);
            } else {
                _stored_result++;
            }
        }
    }
}

gauss_msgs::NewThreats manageResultList(std::vector<LossResult>& _loss_result_list, std::vector<GeofenceResult>& _geofence_result_list, const std::map<int, gauss_msgs::Operation>& _index_to_operation_map, const std::map<int, gauss_msgs::Geofence>& _index_to_geofence_map) {
    double check_time_margin = 10.0;
    static double threat_count_id = 0;
    static double stored_loss_id_count = 0;
    static double stored_geofence_id_count = 0;
    gauss_msgs::NewThreats out_threats;
    static std::vector<LossResult> stored_loss_result_list;
    static std::vector<GeofenceResult> stored_geofence_result_list;

    if (stored_loss_result_list.size() > 0 || stored_geofence_result_list.size() > 0) cleanStoredList(stored_loss_result_list, stored_geofence_result_list, _loss_result_list, _geofence_result_list, check_time_margin);
    if (_loss_result_list.size() > 0) {
        if (stored_loss_result_list.size() == 0) {
            stored_loss_result_list.push_back(_loss_result_list.front());
            out_threats.request.threats.push_back(_loss_result_list[0].convertToThreat(_index_to_operation_map, threat_count_id));
        } else {
            for (auto _loss_result : _loss_result_list) {
                bool save_loss_result = false;
                // Using lambda, check if both trajectory index of _loss_result are in stored_loss_result_list
                std::vector<LossResult>::iterator stored_it = std::find_if(stored_loss_result_list.begin(), stored_loss_result_list.end(),
                                                                           [_loss_result](LossResult stored_loss_result) { return stored_loss_result.isEqual(_loss_result); });
                save_loss_result = (stored_it == stored_loss_result_list.end());
                if (save_loss_result) {
                    stored_loss_result_list.push_back(_loss_result);
                    out_threats.request.threats.push_back(_loss_result.convertToThreat(_index_to_operation_map, threat_count_id));
                }
            }
        }
    }
    if (_geofence_result_list.size() > 0) {
        if (stored_geofence_result_list.size() == 0) {
            stored_geofence_result_list.push_back(_geofence_result_list.front());
            std::vector<gauss_msgs::NewThreat> aux_vec = _geofence_result_list[0].convertToThreat(_index_to_operation_map, _index_to_geofence_map, threat_count_id);
            out_threats.request.threats.insert(out_threats.request.threats.end(), aux_vec.begin(), aux_vec.end());
        } else {
            for (auto _geofence_result : _geofence_result_list) {
                bool save_geofence_result = false;
                // Using lambda, check if both trajectory index of _geofence_result are in stored_geofence_result_list
                std::vector<GeofenceResult>::iterator stored_it = std::find_if(stored_geofence_result_list.begin(), stored_geofence_result_list.end(),
                                                                               [_geofence_result](GeofenceResult stored_loss_result) { return stored_loss_result.isEqual(_geofence_result); });
                save_geofence_result = (stored_it == stored_geofence_result_list.end());
                if (save_geofence_result) {
                    stored_geofence_result_list.push_back(_geofence_result);
                    std::vector<gauss_msgs::NewThreat> aux_vec = _geofence_result.convertToThreat(_index_to_operation_map, _index_to_geofence_map, threat_count_id);
                    out_threats.request.threats.insert(out_threats.request.threats.end(), aux_vec.begin(), aux_vec.end());
                }
            }
        }
    }

    return out_threats;
}

std::pair<double, double> checkGeofence2D(const Segment& segment, const gauss_msgs::Circle& circle) {

    auto translated_segment = segment;
    translated_segment.point_A.x -= circle.x_center;
    translated_segment.point_A.y -= circle.y_center;
    translated_segment.point_B.x -= circle.x_center;
    translated_segment.point_B.y -= circle.y_center;

    float sq_distance_A = pow(translated_segment.point_A.x, 2) + pow(translated_segment.point_A.y, 2);
    float sq_distance_B = pow(translated_segment.point_B.x, 2) + pow(translated_segment.point_B.y, 2);
    float sq_radius = pow(circle.radius, 2);

    bool point_A_is_in = (sq_distance_A < sq_radius);
    bool point_B_is_in = (sq_distance_B < sq_radius);

    // Geofence2DResult result;
    if (point_A_is_in && point_B_is_in) {
        // A and B inside the circle
        // ROS_INFO("A and B inside the circle");
        return std::make_pair(segment.t_A, segment.t_B);
    }

    float a_x = pow(translated_segment.point_B.x - translated_segment.point_A.x, 2);
    float b_x = 2.0 * (translated_segment.point_B.x - translated_segment.point_A.x) * translated_segment.point_A.x;
    float c_x = pow(translated_segment.point_A.x, 2);
    float a_y = pow(translated_segment.point_B.y - translated_segment.point_A.y, 2);
    float b_y = 2.0 * (translated_segment.point_B.y - translated_segment.point_A.y) * translated_segment.point_A.y;
    float c_y = pow(translated_segment.point_A.y, 2);

    auto m_crossing = quadratic_roots(a_x + a_y, b_x + b_y, c_x + c_y - sq_radius);
    // ROS_INFO("Roots: m = [%lf, %lf]", m_crossing.first, m_crossing.second);

    if (std::isnan(m_crossing.first)) {  // m_crossing.second should also be nan
        // There is no intersection at all
        // ROS_INFO("There is no intersection at all");
        return std::make_pair(std::nan(""), std::nan(""));
    }

    if ((m_crossing.first > 1) || (m_crossing.second < 0)) {
        // ROS_INFO("Intersection is out of the segment");
        return std::make_pair(std::nan(""), std::nan(""));
    }

    auto t_crossing_0 = translated_segment.t_A + clamp(m_crossing.first,  0, 1) * (translated_segment.t_B - translated_segment.t_A);
    auto t_crossing_1 = translated_segment.t_A + clamp(m_crossing.second, 0, 1) * (translated_segment.t_B - translated_segment.t_A);
    return std::make_pair(t_crossing_0, t_crossing_1);
}

bool checkOverlappingInTime(std::pair<double, double> time_interval_a, std::pair<double, double> time_interval_b) {
    return (time_interval_a.first <= time_interval_b.second) && (time_interval_a.second >= time_interval_b.first);
}

geometry_msgs::Point calculateClosestExit(const geometry_msgs::Point& current, const gauss_msgs::Circle& circle) {
    geometry_msgs::Point out;
    float delta_x = current.x - circle.x_center;
    float delta_y = current.y - circle.y_center;
    auto distance = sqrt(pow(delta_x, 2) + pow(delta_y, 2));
    if (distance < 1e-3) {
        // At the center of the geofence? Go East!
        out.x = circle.x_center + circle.radius;
        out.y = circle.y_center;
        // out.x = std::nan("");  // TODO: Better NaN and handle later?
        // out.y = std::nan("");
        return out;
    }
    out.x = circle.x_center + (delta_x / distance) * circle.radius;
    out.y = circle.y_center + (delta_y / distance) * circle.radius;
    return out;
}

std::vector<GeoConflictiveTrajectory> checkGeofenceConflict(const std::vector<gauss_msgs::WaypointList>& trajectories, const std::vector<double>& volumes, const gauss_msgs::Geofence& geofence) {
    std::vector<GeoConflictiveTrajectory> result;

    if (!geofence.cylinder_shape) {
        // TODO: implement also for polygons
        ROS_ERROR("[Monitoring] Polygon geofences not implemented yet");
        return result;
        // float64 min_altitude	# meters
        // float64 max_altitude	# meters
        // Polygon polygon
        //     float64[] x
        //     float64[] y
    }

    if (trajectories.size() != volumes.size()) {
        ROS_ERROR("[Monitoring] Sizes do not match: trajectories.size() = %ld, volumes.size() = %ld", trajectories.size(), volumes.size());
        return result;
    }

    for (int i = 0; i < trajectories.size(); i++) {
        // ROS_INFO("Checking trajectory [%d]", i);
        GeoConflictiveTrajectory current_result(i);

        if (trajectories[i].waypoints.size() < 2) {
            // TODO: Warn and push the same point twice?
            ROS_ERROR("[Monitoring]: trajectory must contain at least 2 points, [%ld] found in second argument", trajectories[i].waypoints.size());
            continue;
        }
        auto operational_volume = volumes[i];
        auto rectified_geofence = geofence;
        rectified_geofence.min_altitude -= operational_volume;
        rectified_geofence.max_altitude += operational_volume;
        rectified_geofence.circle.radius += operational_volume;

        for (int j = 0; j < trajectories[i].waypoints.size() - 1; j++) {
            // ROS_INFO("Checking segment [%d, %d]", j, j + 1);
            auto segment = Segment(trajectories[i].waypoints[j], trajectories[i].waypoints[j + 1]);

            // TODO: combine the following two ifs into a single one?
            if ((segment.point_A.z < rectified_geofence.min_altitude) && (segment.point_B.z < rectified_geofence.min_altitude)) {
                // The whole segment lies below the rectified_geofence
                // ROS_INFO("The whole segment lies below the geofence");
                continue;
            }
            if ((segment.point_A.z > rectified_geofence.max_altitude) && (segment.point_B.z > rectified_geofence.max_altitude)) {
                // The whole segment lies above the rectified_geofence
                // ROS_INFO("The whole segment lies above the geofence");
                continue;
            }

            // TODO: enlarge the circle with operational volume (* some security_gain)
            float delta_z = segment.point_B.z - segment.point_A.z;
            // if (fabs(delta_z) < 1e-3) {  // TODO: Early return?
            //     // We can consider the whole segment lies inside the geofence z interval
            //     // TODO: Consider the special case of j == 0 for geofence intrusion!
            //     ROS_INFO("We can consider the whole segment lies inside the geofence z interval");
            // }

            // Get the segment that does lie between min_alt, max_alt
            if (segment.point_A.z < rectified_geofence.min_altitude) {
                // Point A lies below the geofence z interval
                // ROS_INFO("Point A lies below the geofence z interval");
                float m_min = (rectified_geofence.min_altitude - segment.point_A.z) / delta_z;
                segment.point_A = segment.point_at_param(m_min);
                // TODO: Consider the special case of j == 0 for geofence intrusion!
            } else if (segment.point_A.z > rectified_geofence.max_altitude) {
                // Point A lies above the geofence z interval
                // ROS_INFO("Point A lies above the geofence z interval");
                float m_max = (rectified_geofence.max_altitude - segment.point_A.z) / delta_z;
                segment.point_A = segment.point_at_param(m_max);
                // TODO: Consider the special case of j == 0 for geofence intrusion!
            }

            // Same for point B  // TODO: repeated code!
            if (segment.point_B.z < rectified_geofence.min_altitude) {
                // Point B lies below the geofence z interval
                // ROS_INFO("Point B lies below the geofence z interval");
                float m_min = (rectified_geofence.min_altitude - segment.point_B.z) / delta_z;
                segment.point_B = segment.point_at_param(m_min);
            } else if (segment.point_B.z > rectified_geofence.max_altitude) {
                // Point B lies above the geofence z interval
                // ROS_INFO("Point B lies above the geofence z interval");
                float m_max = (rectified_geofence.max_altitude - segment.point_B.z) / delta_z;
                segment.point_B = segment.point_at_param(m_max);
            }

            auto conflict_times = checkGeofence2D(segment, rectified_geofence.circle);
            if (std::isnan(conflict_times.first)) {  // conflict_times.second should be also nan
                // ROS_INFO("No conflicts");
                // return result; // TODO: Only for debug!
                continue;
            }  // TODO: Rename to GeofenceConflict?

            auto current_time = ros::Time::now().toSec();
            if (current_time > conflict_times.second) {  // Should be also > conflict_times.second
                // ROS_INFO("Past conflicts do not count :)");
                // return result; // TODO: Only for debug!
                continue;
            }

            if (checkOverlappingInTime(conflict_times, std::make_pair(rectified_geofence.start_time.toSec(), rectified_geofence.end_time.toSec()))) {
                // ROS_INFO("Conflict!");  // TODO
                auto current_position = trajectories[i].waypoints[j];
                // Check also for intrusion:
                if ((j == 0)
                    && in_range(current_position.z, rectified_geofence.min_altitude, rectified_geofence.max_altitude)
                    && (pow(current_position.x - rectified_geofence.circle.x_center, 2) + pow(current_position.y - rectified_geofence.circle.y_center, 2) < pow(rectified_geofence.circle.radius, 2))
                    ) {
                    // ROS_ERROR("[Monitoring] Geofence intrusion! [i = %d]", current_result.trajectory_index);
                    current_result.closest_exit_wp.mandatory = true;
                    auto exit_circle = geofence.circle;
                    exit_circle.radius += operational_volume * 2.0;
                    auto xy_closest_exit = calculateClosestExit(translateToPoint(current_position), exit_circle);
                    // auto xy_closest_exit = calculateClosestExit(translateToPoint(current_position), rectified_geofence.circle);
                    current_result.closest_exit_wp.x = xy_closest_exit.x;
                    current_result.closest_exit_wp.y = xy_closest_exit.y;
                    current_result.closest_exit_wp.z = current_position.z;  // Suppose we want the closest exit with no changes in altuitude!
                }
                current_result.geofence_conflictive_segments.push_back(Segment(segment.point_at_time(conflict_times.first), segment.point_at_time(conflict_times.second)));
            }
            // result.push_back(current_result);  // TODO: Only for debug!
            // return result;  // TODO: Only for debug!
        }
        if (current_result.geofence_conflictive_segments.size() > 0) {
            result.push_back(current_result);
        }
    }

    return result;
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "continuous_monitoring");

    ros::NodeHandle n;
    // ros::NodeHandle np("~");
    ROS_INFO("[Monitoring] Started monitoring node!");
    double safety_distance;
    bool just_one_threat;
    n.param("safetyDistance", safety_distance, 10.0);
    n.param("just_one_threat", just_one_threat, false);
    double safety_distance_sq = pow(safety_distance, 2);

    auto read_icao_srv_url = "/gauss/read_icao";
    auto read_operation_srv_url = "/gauss/read_operation";
    auto read_geofences_srv_url = "/gauss/read_geofences";
    auto tactical_srv_url = "/gauss/new_tactical_deconfliction";
    auto alternatives_topic_url = "/gauss/possible_alternatives";
    auto new_threats_srv_url = "/gauss/new_threats";
    auto visualization_topic_url = "/gauss/visualize_monitoring";

    ros::ServiceClient icao_client = n.serviceClient<gauss_msgs::ReadIcao>(read_icao_srv_url);
    ros::ServiceClient operation_client = n.serviceClient<gauss_msgs::ReadOperation>(read_operation_srv_url);
    ros::ServiceClient geofences_client = n.serviceClient<gauss_msgs::ReadGeofences>(read_geofences_srv_url);
    ros::ServiceClient tactical_client = n.serviceClient<gauss_msgs::NewDeconfliction>(tactical_srv_url);
    ros::ServiceClient possible_alternatives_client = n.serviceClient<gauss_msgs::NewDeconfliction>(alternatives_topic_url);
    ros::ServiceClient new_threats_client = n.serviceClient<gauss_msgs::NewThreats>(new_threats_srv_url);
    ros::Publisher visualization_pub = n.advertise<visualization_msgs::MarkerArray>(visualization_topic_url, 1);

    ROS_INFO("[Monitoring] Waiting for required services...");
    ros::service::waitForService(read_icao_srv_url, -1);
    ROS_INFO("[Monitoring] %s: ok", read_icao_srv_url);
    ros::service::waitForService(read_operation_srv_url, -1);
    ROS_INFO("[Monitoring] %s: ok", read_operation_srv_url);
    ros::service::waitForService(read_geofences_srv_url, -1);
    ROS_INFO("[Monitoring] %s: ok", read_geofences_srv_url);
    ros::service::waitForService(tactical_srv_url, -1);
    ROS_INFO("[Monitoring] %s: ok", tactical_srv_url);
    ros::service::waitForService(new_threats_srv_url, -1);
    ROS_INFO("[Monitoring] %s: ok", new_threats_srv_url);
    ros::Rate rate(1);  // [Hz]
    while (ros::ok()) {
        gauss_msgs::ReadIcao read_icao;
        if (icao_client.call(read_icao)) {
            // ROS_INFO("[Monitoring] Read icao addresses... ok");
            // std::cout << read_icao.response << '\n';
        } else {
            ROS_ERROR("[Monitoring] Failed to call service: [%s]", read_icao_srv_url);
            return 1;
        }

        gauss_msgs::ReadOperation read_operation;
        read_operation.request.uav_ids = read_icao.response.uav_id;
        if (operation_client.call(read_operation)) {
            // ROS_INFO("[Monitoring] Read operations... ok");
            // std::cout << read_operation.response << '\n';
        } else {
            ROS_ERROR("[Monitoring] Failed to call service: [%s]", read_operation_srv_url);
            return 1;
        }

        gauss_msgs::ReadGeofences read_geofences;
        read_geofences.request.geofences_ids = read_icao.response.geofence_id;
        if (geofences_client.call(read_geofences)) {
            // ROS_INFO("[Monitoring] Read geofences... ok");
            // std::cout << read_geofences.response << '\n';
        } else {
            ROS_ERROR("[Monitoring] Failed to call service: [%s]", read_geofences_srv_url);
            return 1;
        }

        std::map<std::string, int> icao_to_index_map;
        std::map<int, gauss_msgs::Operation> index_to_operation_map;
        std::map<int, gauss_msgs::Geofence> index_to_geofence_map;
        std::vector<gauss_msgs::WaypointList> estimated_trajectories;
        std::vector<double> operational_volumes;
        for (auto operation : read_operation.response.operation) {
            // std::cout << operation << '\n';
            if (operation.is_started) {
                icao_to_index_map[operation.icao_address] = estimated_trajectories.size();
                index_to_operation_map[estimated_trajectories.size()] = operation;
                estimated_trajectories.push_back(operation.estimated_trajectory);
                operational_volumes.push_back(operation.operational_volume);
            }
        }

        std::vector<GeofenceResult> geofence_results_list;
        for (auto geofence : read_geofences.response.geofences) {
            index_to_geofence_map[geofence.id] = geofence;
            // std::cout << geofence << '\n';
            // ROS_INFO("_________________________");
            // ROS_INFO("Checking geofence id [%d]", geofence.id);
            GeofenceResult current_result(geofence.id);
            current_result.geo_conflictive_trajectories = checkGeofenceConflict(estimated_trajectories, operational_volumes, geofence);
            if (current_result.geo_conflictive_trajectories.size() > 0) {
                geofence_results_list.push_back(current_result);
            }
        }
        // Visualize...
        visualization_msgs::MarkerArray marker_array;
        for (int i = 0; i < geofence_results_list.size(); i++) {
            auto geofence_result = geofence_results_list[i];
            // geofence_result.geofence_id
            for (int j = 0; j < geofence_result.geo_conflictive_trajectories.size(); j++) {
                auto trajectory = geofence_result.geo_conflictive_trajectories[j];
                // trajectory.trajectory_index;
                auto all_conflicts = trajectory.geofence_conflictive_segments;
                auto first_conflict = getFirstSetOfContiguousSegments(all_conflicts);
                auto conflicts = first_conflict;
                int segment_id = 1e6 * i + 1e3 * j;
                std_msgs::ColorRGBA segment_color;
                segment_color.a = 1.0;
                segment_color.r = 1.0;
                if (trajectory.closest_exit_wp.mandatory) {
                    // Means it is an intrusion!
                    Segment way_out(trajectory.closest_exit_wp, conflicts.back().point_B);
                    marker_array.markers.push_back(way_out.translateToMarker(segment_id+1e9, segment_color));
                    // continue;
                }
                // else:
                for (int k = 0; k < conflicts.size(); k++) {
                    segment_id += k;
                    segment_color.g = 0.5;
                    marker_array.markers.push_back(conflicts[k].translateToMarker(segment_id, segment_color));
                }
            }
        }

        std::vector<LossResult> loss_results_list;
        if (estimated_trajectories.size() >= 2) {
            for (int i = 0; i < estimated_trajectories.size() - 1; i++) {
                std::pair<gauss_msgs::WaypointList, gauss_msgs::WaypointList> trajectories;
                trajectories.first = estimated_trajectories[i];
                for (int j = i + 1; j < estimated_trajectories.size(); j++) {
                    // ROS_INFO("Checking trajectories: [%d, %d]", i, j);
                    trajectories.second = estimated_trajectories[j];
                    double s_threshold = std::max(safety_distance_sq, pow(operational_volumes[i] + operational_volumes[j], 2));
                    auto loss_conflictive_segments = checkTrajectoriesLoss(trajectories, s_threshold);
                    if (loss_conflictive_segments.size() > 0) {
                        LossResult loss_result(i, j);
                        loss_result.loss_conflictive_segments = loss_conflictive_segments;
                        loss_results_list.push_back(loss_result);
                    }
                }
            }
        }

        std::sort(loss_results_list.begin(), loss_results_list.end(), happensBefore);
        gauss_msgs::NewThreats threats_msg;
        if (just_one_threat && (loss_results_list.size() > 0 || geofence_results_list.size() > 0)) {
            threats_msg = manageResultList(loss_results_list, geofence_results_list, index_to_operation_map, index_to_geofence_map);
            just_one_threat = false;
        }
        if (threats_msg.request.threats.size() > 0) {
            std::string cout_threats;
            for (auto threat : threats_msg.request.threats) {
                cout_threats = cout_threats + " [" + std::to_string(threat.threat_id) + " " + std::to_string(threat.threat_type) + " |";
                for (auto uav_id : threat.uav_ids) cout_threats = cout_threats + " " + std::to_string(uav_id);
                cout_threats = cout_threats + "]";
            }
            ROS_INFO_STREAM("[Monitoring] Threats detected: [id type | uav] " + cout_threats);
            if (new_threats_client.call(threats_msg)) {
                // ROS_INFO("[Monitoring] Call tactical... ok");
            } else {
                ROS_ERROR("[Monitoring] Failed to call service: [%s]", tactical_srv_url);
                return 1;
            }
        }

        for (int i = 0; i < loss_results_list.size(); i++) {
            // std::cout << loss_results_list[i] << '\n';
            marker_array.markers.push_back(translateToMarker(loss_results_list[i]));
            auto extremes = calculateExtremes(loss_results_list[i]);
            marker_array.markers.push_back(translateToMarker(extremes.first, 10 * i));
            marker_array.markers.push_back(translateToMarker(extremes.second, 10 * i + 1));
        }
        visualization_pub.publish(marker_array);

        ros::spinOnce();
        rate.sleep();
    }

    // ros::spin();
    return 0;
}


#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <iostream>
#include <png.h>
#include <TaskGraph>
#include <TaskUserFunctions.h>
#include <boost/static_assert.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include "mandelbrot.hpp"

using namespace tg;

const int IMG_X = 12800;
const int IMG_Y ((IMG_X/(NUM_SPES*4))*3*NUM_SPES); // Approximately 4:3 aspect ratio with number of rows multiple of NUM_SPES

const int MAX_ITERATIONS = 1000;
const char* OUTPUT_FILE = "mandelbrot_set.png";
const float GAMMA_EXPONENT = 3.5f;

const int  UNROLL_FACTOR = 8; // MUST BE A DIVISOR OF IMG_X!
BOOST_STATIC_ASSERT(IMG_X % UNROLL_FACTOR == 0);

typedef TaskGraph<void, int, char[IMG_X]> mandel_tg;

int main(int argc, char* argv[]) {
  int threads;
  const Options options = getOptions("mandelbrot", argc, argv);

  if (options.threads < 1 || options.threads > NUM_SPES)
  {
    std::cerr << "Please select a number of threads between 1 and " << NUM_SPES << " (inclusive)." << std::endl;
    exit(EXIT_FAILURE);
  }
  else if (IMG_Y % options.threads != 0)
  {
    std::cerr << "Please choose a number of threads that is a factor of the image height (" << IMG_Y << ")." << std::endl;
    exit(EXIT_FAILURE);
  }
  else
  {
    threads = options.threads;
  }

  FILE *output_file;
  boost::ptr_vector<mandel_tg> taskGraphs;
  TaskFarm tFarm;

  mandel_tg T;
  taskgraph(mandel_tg, T, tuple2(y_pos, line)) {
    tVar(float, x);
    tVar(float, y);
    tVar(float, x0);
    tVar(float, y0);
    tVar(float, xtemp);
    tVar(int, x_pos);
    tVar(int, iteration);
    tVar(float, pixel);

    y0 = (y_pos * (3.0f / IMG_Y)) - 1.5f;

    // Iterate over the scanline
    tFor (x_pos, 0, IMG_X - 1) {
      x0 = (x_pos * (3.5f / IMG_X)) - 2.5f;

      // Unroll the loop
      for (int i = 0; i < UNROLL_FACTOR; ++i) {
        x = x0;
        y = y0;
        iteration = 0;

        // Test if this location is in the set
        tWhile ((x*x + y*y) < 4 && iteration < MAX_ITERATIONS) {
          xtemp = (x*x) - (y*y) + x0;
          y = 2*x*y + y0;
          x = xtemp;
          iteration+=1;
        }

        // Calculate and set the value of the pixel
        tIf(iteration == MAX_ITERATIONS)
          pixel = 0.0f;
        tElse
          pixel = 8.0f * (iteration + 1.0f - tLogf(tLogf(tSqrtf(x*x + y*y))) / std::log(2.0f)) / MAX_ITERATIONS;

        line[x_pos] = tPowf(pixel, (1.0f / GAMMA_EXPONENT)) * 255.0f;
        x_pos+=1;
      }
      x_pos-=1;
    }
  }

  T.compile(tg::SPU_GCC, false);

  for(int index=0; index<threads; ++index)
  {
    taskGraphs.push_back(new mandel_tg(T.duplicate()));
    tFarm.add(&taskGraphs.back());
  }

  png_structp png_ptr;
  png_infop info_ptr;

  if (!(output_file = fopen(OUTPUT_FILE, "wb"))) {
    std::cerr << "Unable to open " << OUTPUT_FILE << " for writing. Aborting now." << std::endl;
    exit(1);
  }

  if (!(png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL))) {
    std::cerr << "Failed to create png write struct. Aborting now." << std::endl;
    exit(1);
  }

  if (!(info_ptr = png_create_info_struct(png_ptr))) {
    std::cerr << "Failed to create png info struct. Aborting now." << std::endl;
    // Clean up
    png_destroy_write_struct(&png_ptr,static_cast<png_infopp>(NULL));
    exit(1);
  }

  png_init_io(png_ptr, output_file);
  png_set_IHDR(png_ptr, info_ptr, IMG_X, IMG_Y, 8,
               PNG_COLOR_TYPE_PALETTE,
               PNG_INTERLACE_NONE,
               PNG_COMPRESSION_TYPE_DEFAULT,
               PNG_FILTER_TYPE_DEFAULT);
  png_set_gamma(png_ptr, 0.5, 0.45455);

  std::vector<png_color> palette(generateBlueYellowColourMap());
  png_set_PLTE(png_ptr, info_ptr, &palette[0], palette.size());
  png_write_info(png_ptr, info_ptr);

  std::vector<char*> scanlines(threads);

  for (int spe = 0; spe < threads; ++spe)
    scanlines[spe] = static_cast<char*>(spu_malloc(IMG_X));

  for (int line = 0; line < IMG_Y; line+=threads)
  {
    std::vector<int> lineNumbers(threads);

    for(int spe=0; spe<threads; ++spe)
    {
      lineNumbers[spe] = line+spe;
      taskGraphs[spe].setParameters(lineNumbers[spe], scanlines[spe]);
    }

    tFarm.execute();

    for (int spe = 0; spe < threads; ++spe)
    {
      png_bytep png_row_ptr = reinterpret_cast<png_bytep>(scanlines[spe]);
      png_write_row(png_ptr, png_row_ptr);
    }
  }

  for (int spe = 0; spe < threads; ++spe)
    spu_free(scanlines[spe]);

  png_write_end(png_ptr, info_ptr);
  png_destroy_write_struct(&png_ptr, &info_ptr);

  exit(EXIT_SUCCESS);
}


#ifndef UTIL_RANDOM_HPP_
#define UTIL_RANDOM_HPP_

#include <random>
#include <vector>

#include <boost/assert.hpp>

#include "SpinMutex.hpp"
#include "ThreadIndexManager.hpp"

namespace util
{

class Random
{
private:
	static std::vector<Random> random;
	static util::SpinMutex mutex;

public:
	Random() : engine(std::random_device{}()) {}
	std::default_random_engine engine;

	static void seed(unsigned int seed, int thread = util::ThreadIndexManager::getLocalId())
	{
		getEngine(thread).seed(seed);
	}

	static int nextInt(int min, int max, int thread = util::ThreadIndexManager::getLocalId())
	{
		std::uniform_int_distribution<> dist(min, max);
		return dist(getEngine(thread));
	}

	static unsigned long long nextULL(int thread = util::ThreadIndexManager::getLocalId())
	{
		std::uniform_int_distribution<unsigned long long> dist(
			std::numeric_limits<unsigned long long>::min(),
			std::numeric_limits<unsigned long long>::max()
		);
		return dist(getEngine(thread));
	}

	static float nextReal(int thread = util::ThreadIndexManager::getLocalId())
	{
		std::uniform_real_distribution<> dist;
		return dist(getEngine(thread));
	}

	static std::default_random_engine& getEngine(int thread = util::ThreadIndexManager::getLocalId())
	{
		if(random.size() <= thread){
            std::lock_guard<util::SpinMutex> lock(mutex);
			if(random.size() <= thread){
                random.resize(thread + 1);
            }
        }

		return random[thread].engine;
	}
};

} // end of namespace util

#endif


/*
MIT License

Copyright (c) 2018 Bastien Durix

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
 */

#ifndef _VECTOR3_H_
#define _VECTOR3_H_

#include <meta_operations/MetaOperations.hpp>
#include <Eigen/Dense>

/**
 * @brief Class representing a 3D vector
 */
template<typename X0, typename X1, typename X2>
class Vector3
{
    public:
		/**
		 * @brief Evaluation of the vector value
		 */
        template<typename T, typename ...Args>
        static inline Eigen::Vector3d eval(const T &p, Args... args)
		{
			return Eigen::Vector3d(X0::eval(p,args...),X1::eval(p,args...),X2::eval(p,args...));
		};

		/**
		 * @brief Writing of the vector value
		 */
        template<typename ...Args>
        static std::string write(Args... args)
        {
            return "(" + X0::write(args...) + "," + X1::write(args...) + "," + X2::write(args...) + ")";
        };

		/**
		 * @brief Struct recursively applying the modifier F<T>
		 */
		template<template<typename T> typename F>
		struct apply_rec
		{
			using type = Vector3<typename X0::template apply_rec<F>::type,
								 typename X1::template apply_rec<F>::type,
								 typename X2::template apply_rec<F>::type>;
		};

		/**
		 * @brief Importance order
		 */
		static const unsigned int outerOrder = 3;

		/**
		 * @brief Importance order
		 */
		static const unsigned int innerOrder = 0;
};

/**
 * @brief Derivative of the parameter with respect to an argument
 */
template<typename X0, typename X1, typename X2, typename A>
struct Der<Vector3<X0,X1,X2>,A>
{
    using type = Vector3<typename Der<X0,A>::type,typename Der<X1,A>::type,typename Der<X2,A>::type>;
};

/**
 * @brief Simplification of the 3D vector
 */
template<typename X0, typename X1, typename X2>
struct Simp<Vector3<X0,X1,X2> >
{
    using type = Vector3<typename Simp<X0>::type,typename Simp<X1>::type,typename Simp<X2>::type>;
};

#endif //_VECTOR3_H_


//
// Created by Dominik Krupke, http://krupke.cc on 10/7/17.
//

#include <gtest/gtest.h>
#include <boost/graph/adjacency_list.hpp>
#include "min_vertex_cover.h"

TEST(Library, IsValidVertexCover) {
  using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS>;
  Graph graph{6};
  boost::add_edge(0, 3, graph);
  boost::add_edge(0, 4, graph);
  boost::add_edge(0, 5, graph);
  boost::add_edge(1, 3, graph);
  boost::add_edge(1, 4, graph);
  boost::add_edge(1, 5, graph);
  boost::add_edge(2, 3, graph);
  boost::add_edge(2, 4, graph);
  boost::add_edge(2, 5, graph);

  std::vector<boost::graph_traits<Graph>::vertex_descriptor> vertex_cover_1;
  vertex_cover_1.push_back(0);
  vertex_cover_1.push_back(1);
  std::vector<boost::graph_traits<Graph>::vertex_descriptor> vertex_cover_2;
  vertex_cover_2.push_back(0);
  vertex_cover_2.push_back(1);
  vertex_cover_2.push_back(2);

  ASSERT_FALSE(bipartvc::is_valid_vertex_cover(graph, vertex_cover_1));
  ASSERT_TRUE(bipartvc::is_valid_vertex_cover(graph, vertex_cover_2));
}

TEST(Library, MinVertexCover) {
  using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS>;
  Graph graph{7};
  boost::add_edge(0, 3, graph);
  boost::add_edge(0, 4, graph);
  boost::add_edge(0, 5, graph);
  boost::add_edge(1, 3, graph);
  boost::add_edge(1, 4, graph);
  boost::add_edge(1, 5, graph);
  boost::add_edge(2, 3, graph);
  boost::add_edge(2, 4, graph);
  boost::add_edge(2, 5, graph);
  boost::add_edge(2, 6, graph);

  auto partition_classifier = [](boost::graph_traits<Graph>::vertex_descriptor v) -> bipartvc::Partition {
    if (v < 3) {
      return bipartvc::Partition::A;
    } else {
      return bipartvc::Partition::B;
    }
  };
  auto vertex_cover = bipartvc::get_minimal_vertex_cover(graph, partition_classifier);

  ASSERT_EQ(vertex_cover.size(), 3);
  ASSERT_TRUE(bipartvc::is_valid_vertex_cover(graph, vertex_cover));

  //swap partitions (A is treated differently than B by the algorithm so we should check this)
  auto complementary_partition_classifier = [](boost::graph_traits<Graph>::vertex_descriptor v) -> bipartvc::Partition {
    if (v < 3) {
      return bipartvc::Partition::B;
    } else {
      return bipartvc::Partition::A;
    }
  };
  auto vertex_cover_2 = bipartvc::get_minimal_vertex_cover(graph, complementary_partition_classifier);

  ASSERT_EQ(vertex_cover_2.size(), 3);
  ASSERT_TRUE(bipartvc::is_valid_vertex_cover(graph, vertex_cover_2));

  //and with automatic partition detection.
  auto vertex_cover_3 = bipartvc::get_minimal_vertex_cover(graph);
  ASSERT_EQ(vertex_cover_3.size(), 3);
  ASSERT_TRUE(bipartvc::is_valid_vertex_cover(graph, vertex_cover_3));
}

TEST(MinVertexCover, Star) {
  using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS>;
  Graph graph{7};
  boost::add_edge(0, 2, graph);
  boost::add_edge(0, 3, graph);
  boost::add_edge(0, 4, graph);
  boost::add_edge(0, 5, graph);
  boost::add_edge(0, 6, graph);
  boost::add_edge(1, 2, graph);
  boost::add_edge(1, 3, graph);
  boost::add_edge(1, 4, graph);
  boost::add_edge(1, 5, graph);
  boost::add_edge(1, 6, graph);
  auto partition_classifier = [](boost::graph_traits<Graph>::vertex_descriptor v) -> bipartvc::Partition {
    if (v < 2) {
      return bipartvc::Partition::A;
    } else {
      return bipartvc::Partition::B;
    }
  };
  auto vertex_cover = bipartvc::get_minimal_vertex_cover(graph, partition_classifier);

  ASSERT_EQ(vertex_cover.size(), 2);
  ASSERT_TRUE(bipartvc::is_valid_vertex_cover(graph, vertex_cover));

  //swap partitions (A is treated differently than B by the algorithm so we should check this)
  auto complementary_partition_classifier = [](boost::graph_traits<Graph>::vertex_descriptor v) -> bipartvc::Partition {
    if (v < 2) {
      return bipartvc::Partition::B;
    } else {
      return bipartvc::Partition::A;
    }
  };
  auto vertex_cover_2 = bipartvc::get_minimal_vertex_cover(graph, complementary_partition_classifier);

  ASSERT_EQ(vertex_cover_2.size(), 2);
  ASSERT_TRUE(bipartvc::is_valid_vertex_cover(graph, vertex_cover_2));

  //and with automatic partition detection.
  auto vertex_cover_3 = bipartvc::get_minimal_vertex_cover(graph);
  ASSERT_EQ(vertex_cover_3.size(), 2);
  ASSERT_TRUE(bipartvc::is_valid_vertex_cover(graph, vertex_cover_3));

}

TEST(MinVertexCover, ConnectedStar) {
  using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS>;
  Graph graph{12};
  boost::add_edge(0, 2, graph);
  boost::add_edge(0, 3, graph);
  boost::add_edge(0, 4, graph);
  boost::add_edge(0, 5, graph);
  boost::add_edge(0, 6, graph);
  boost::add_edge(1, 7, graph);
  boost::add_edge(1, 8, graph);
  boost::add_edge(1, 9, graph);
  boost::add_edge(1, 10, graph);
  boost::add_edge(1, 11, graph);
  boost::add_edge(0, 1, graph);

  auto vertex_cover_3 = bipartvc::get_minimal_vertex_cover(graph);
  ASSERT_EQ(vertex_cover_3.size(), 2);
  ASSERT_TRUE(bipartvc::is_valid_vertex_cover(graph, vertex_cover_3));

}

TEST(MinVertexCover, EvenCycle) {
  using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS>;
  Graph graph{12};
  boost::add_edge(0, 1, graph);
  boost::add_edge(1, 2, graph);
  boost::add_edge(2, 3, graph);
  boost::add_edge(3, 4, graph);
  boost::add_edge(4, 5, graph);
  boost::add_edge(5, 6, graph);
  boost::add_edge(6, 7, graph);
  boost::add_edge(7, 8, graph);
  boost::add_edge(8, 9, graph);
  boost::add_edge(9, 10, graph);
  boost::add_edge(10, 11, graph);
  boost::add_edge(11, 0, graph);

  auto vertex_cover_3 = bipartvc::get_minimal_vertex_cover(graph);
  ASSERT_EQ(vertex_cover_3.size(), 6);
  ASSERT_TRUE(bipartvc::is_valid_vertex_cover(graph, vertex_cover_3));

}


#pragma once
#include <iostream>
#include <boost/math/common_factor_rt.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <chrono>
#include <vector>

#include "globals.hpp"

//the Test_Suite namespace contains functions that aid in
//verifying the correctness of gcd algorithms as well as
//testing the performance of generic gcd routines
namespace Test_Suite{

	//these functions make sure that the gcd algorithms are correct
	template <typename IntegerType=Default_Test_Element> bool Check_Algorithm_Validity(std::vector<IntegerType>&(*fun_)(std::vector<IntegerType>&));
	template <typename IntegerType=Default_Test_Element> bool Check_Answer_Validity(std::vector<Default_Test_Element> const& vec);

	//utility
	template <typename IntegerType=Default_Test_Element> boost::random::uniform_int_distribution<IntegerType> Get_Specified_Bit_Length_Distribution(int bit_length);

};

//these functions make sure that the gcd algorithms are correct
template <typename IntegerType> bool Test_Suite::Check_Algorithm_Validity(std::vector<IntegerType>&(*fun_)(std::vector<IntegerType>&)){
	int fail_rate=0;

	boost::random::mt19937 gen(std::time(0));
	Default_Test_Element a;
	std::vector<Default_Test_Element> vec;
	auto dist = Test_Suite::Get_Specified_Bit_Length_Distribution(30);

	//fill up the vector
	for (int i = 0; i < 100; ++i){
		a = dist(gen);
		vec.push_back(a);
	}

	//std::cout << "no: "; for (auto it: vec){std::cout << it << ",";}std::cout << std::endl << std::endl;

	fun_(vec);
	fail_rate += Test_Suite::Check_Answer_Validity(vec);

	bool failed = false;
	if (fail_rate > 0){failed = true;}
	return failed;
}
template <typename IntegerType> bool Test_Suite::Check_Answer_Validity(std::vector<Default_Test_Element> const& vec){

	Default_Test_Element min = 0;
	if (!vec.empty()){
		min = vec[0];
	}

	for (auto const& it: vec){
		if (min > it){
			std::cerr << "list not sorted." << std::endl;
			return false;
		}
	}

	//std::cout << "yes: "; for (auto it: vec){std::cout << it << ",";}std::cout << std::endl << std::endl;

	return true;
}

//utility
template <typename IntegerType> boost::random::uniform_int_distribution<IntegerType> Test_Suite::Get_Specified_Bit_Length_Distribution(int bit_length){
	IntegerType min = pow(2,bit_length-1);
	IntegerType max = pow(2,bit_length)-1;
	return boost::random::uniform_int_distribution<IntegerType>(min,max);
}


// Author(s): Jeroen Keiren
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
// / \file bag_test.cpp
// / \brief Basic regression test for bag expressions.

#define BOOST_TEST_MODULE bag_test
#include <boost/test/included/unit_test.hpp>

#include "mcrl2/data/bag.h"
#include "mcrl2/data/parse.h"
#include "mcrl2/data/rewriter.h"

using namespace mcrl2;
using namespace mcrl2::data;

// test whether parsing s returns an expression matching predicate p.
// Furthermore check whether the expression does not satisfy q.
template <typename Predicate, typename NegativePredicate>
void test_data_expression(const std::string& s, const variable_vector& v, Predicate p, NegativePredicate q)
{
  std::cerr << "testing data expression " << s << std::endl;
  data_expression e = parse_data_expression(s, v);
  std::cerr << "parsed expression " << e << "\n";
  BOOST_CHECK(p(e));
  BOOST_CHECK(!q(e));
}

void test_expression(const std::string& evaluate, const std::string& expected, data::rewriter r)
{
  data_expression d1 = parse_data_expression(evaluate);
  data_expression d2 = parse_data_expression(expected);
  if (r(d1)!=r(d2))
  {
    std::cerr << "Evaluating: " << evaluate << "\n";
    std::cerr << "Result: " << d1 << "\n";
    std::cerr << "Expected result: " << expected << "\n";
    BOOST_CHECK(r(d1) == r(d2));
    std::cerr << "------------------------------------------------------\n";
  }
}

void bag_expression_test()
{
  data::data_specification specification;

  specification.add_context_sort(sort_bag::bag(sort_pos::pos()));
  specification.add_context_sort(sort_bag::bag(sort_bool::bool_()));

  data::rewriter normaliser(specification);

  variable_vector v;
  v.push_back(parse_variable("b:Bag(Nat)"));

  BOOST_CHECK(sort_bag::is_bag(sort_bag::bag(sort_nat::nat())));
  BOOST_CHECK(!sort_bag::is_bag(sort_nat::nat()));

  test_data_expression("{x : Nat | x}", v, sort_bag::is_constructor_application, sort_bag::is_in_application);
  test_data_expression("1 in b", v, sort_bag::is_in_application, sort_bag::is_union_application);
  test_data_expression("{:} + b", v, sort_bag::is_union_application, sort_bag::is_intersection_application);
  test_data_expression("(({:} + b) - {20:1}) * {40:5}", v, sort_bag::is_intersection_application, is_less_application<data_expression>);
  test_data_expression("{10:count(20,b)} < b", v, is_less_application<data_expression>, sort_bag::is_bag_comprehension_application);
  test_data_expression("b <= {20:2}", v, is_less_equal_application<data_expression>, sort_bag::is_set2bag_application);
  test_data_expression("Set2Bag({20,30,40})", v, sort_bag::is_set2bag_application, sort_bag::is_union_application);
  test_data_expression("{20:2} + Set2Bag({20,30,40})", v, sort_bag::is_union_application, is_less_equal_application<data_expression>);
  test_data_expression("b <= Set2Bag({20,30,40})", v, is_less_equal_application<data_expression>, sort_bag::is_constructor_application);

  test_data_expression("b <= {20:2} + Set2Bag({20,30,40})", v, is_less_equal_application<data_expression>, sort_bag::is_constructor_application);

  data_expression e = parse_data_expression("{20:1}", v);
  BOOST_CHECK(sort_fbag::is_cons_application(normaliser(e)));

  e = parse_data_expression("{20:4, 30:3, 40:2}", v);
  BOOST_CHECK(sort_fbag::is_cons_application(normaliser(e)));

  e = parse_data_expression("{10:count(20,b)}", v);
  BOOST_CHECK(sort_fbag::is_cinsert_application(normaliser(e)));

  // Chect the operation == on bags
  test_expression("{:} == ({true:2} - {true:2})","true",normaliser);  // {true}-{true} is a trick to type {:} == {:}.
  test_expression("{:} == {false:2}", "false",normaliser);
  test_expression("{:} == {true:2}", "false",normaliser);
  test_expression("{true:2} == {true:3}", "false",normaliser);
  test_expression("{false:2} == {false:2}", "true",normaliser);
  test_expression("{true:2} == {false:2, true:2}", "false",normaliser);
  test_expression("{false:2, true:2} == {false:2}", "false",normaliser);
  test_expression("{false:2, true:2} == {true:2, false:2}", "true",normaliser);

  // Check the operation < on bags.
  test_expression("{:} < ({true:2} - {true:2})","false",normaliser);  // {true}-{true} is a trick to type {:} == {:}.
  test_expression("{true:2} < {false:4}", "false",normaliser);
  test_expression("{true:2} < {false:2}", "false",normaliser);
  test_expression("{false:2} < {true:4}", "false",normaliser);
  test_expression("{true:2} < {true:3}", "true",normaliser);
  test_expression("{false:2} < {false:1}", "false",normaliser);
  test_expression("{true:2} < {false:4, true:2}", "true",normaliser);
  test_expression("{false:2} < {false:1, true:2}", "false",normaliser);
  test_expression("{true:2} < {true:4, false:2}", "true",normaliser);
  test_expression("{false:2} < {true:4, false:2}", "true",normaliser);
  test_expression("{true:2, false:4} < {true:4}", "false",normaliser);
  test_expression("{true:2, false:4} < {false:2}", "false",normaliser);
  test_expression("{false:2, true:4} < {true:2}", "false",normaliser);
  test_expression("{false:2, true:4} < {false:5}", "false",normaliser);
  test_expression("{true:2, false:4} < {false:2, true:2}", "false",normaliser);
  test_expression("{true:2, false:4} < {true:2, false:2}", "false",normaliser);
  test_expression("{false:2, true:1} < {false:2, true:2}", "true",normaliser);
  test_expression("{false:2, true:4} < {true:7, false:2}", "true",normaliser);
  test_expression("{false:1, false:1} < {false:2}", "false",normaliser);

  // Check the operation <= on bags.
  test_expression("{:} <= ({true:2}-{true:2})","true",normaliser);  // {true} - {true} is a trick to type {:} == {:}.
  test_expression("{true:2} <= {false:2}", "false",normaliser);
  test_expression("{false:2} <= {true:2}", "false",normaliser);
  test_expression("{true:2} <= {true:2}", "true",normaliser);
  test_expression("{true:3} <= {true:2}", "false",normaliser);
  test_expression("{false:2} <= {false:1}", "false",normaliser);
  test_expression("{false:2} <= {false:7}", "true",normaliser);
  test_expression("{true:2} <= {false:2, true:2}", "true",normaliser);
  test_expression("{false:2} <= {false:4, true:2}", "true",normaliser);
  test_expression("{true:2} <= {true:1, false:2}", "false",normaliser);
  test_expression("{false:2} <= {true:2, false:2}", "true",normaliser);
  test_expression("{true:2, false:2} <= {true:3}", "false",normaliser);
  test_expression("{true:2, false:2} <= {false:1}", "false",normaliser);
  test_expression("{false:2, true:2} <= {true:2}", "false",normaliser);
  test_expression("{false:2, true:2} <= {false:2}", "false",normaliser);
  test_expression("{true:2, false:2} <= {false:2, true:2}", "true",normaliser);
  test_expression("{true:2, false:2} <= {true:2, false:2}", "true",normaliser);
  test_expression("{false:2, true:2} <= {false:2, true:2}", "true",normaliser);
  test_expression("{false:2, true:2} <= {true:2, false:2}", "true",normaliser);
  test_expression("{false:1, false:2} <= {false:2}", "false", normaliser);

  // Test the operation - on bags.
  test_expression("{true:0} - {:}", "{true:0}", normaliser);
  test_expression("{:} - {true:1}", "{true:0}", normaliser);
  test_expression("{true:1} - {:}", "{true:1}", normaliser);
  test_expression("{:} - {true:2}", "{true:0}", normaliser);
  test_expression("{true:2} - {:}", "{true:2}", normaliser);
  test_expression("{true:1} - {true:1}", "{true:0}", normaliser);
  test_expression("{true:2} - {true:1}", "{true:1}", normaliser);
  test_expression("{true:1} - {true:2}", "{true:0}", normaliser);
  test_expression("{true:1} - {false:1}", "{true:1}", normaliser);
  test_expression("{false:1} - {true:1}", "{false:1}", normaliser);
  test_expression("{true:2} - {false:1}", "{true:2}", normaliser);
  test_expression("{false:2} - {true:1}", "{false:2}", normaliser);
  test_expression("{true:1} - {false:2}", "{true:1}", normaliser);
  test_expression("{false:1} - {true:2}", "{false:1}", normaliser);
  test_expression("{true:1, false:1} - {false:1}", "{true:1}", normaliser);
  test_expression("{true:1, false:1} - {true:1}", "{false:1}", normaliser);
  test_expression("{true:1, false:1} - {true:1, false:1}", "{true:0}",
                  normaliser);
  test_expression("{true:1, false:1} - {false:1, true:1}", "{true:0}",
                  normaliser);
  test_expression("{true:2, false:2} - {false:1}", "{true:2, false:1}",
                  normaliser);
  test_expression("{true:2, false:2} - {true:1}", "{true:1, false:2}",
                  normaliser);
  test_expression("{true:2, false:2} - {true:1,false:1}", "{true:1, false:1}",
                  normaliser);
  test_expression("{true:2, false:2} - {false:1,true:1}", "{true:1, false:1}",
                  normaliser);
  test_expression("{true:2, false:2} - {false:2}", "{true:2}", normaliser);
  test_expression("{true:2, false:2} - {false:2}", "{true:2}", normaliser);
}

BOOST_AUTO_TEST_CASE(test_main)
{
  bag_expression_test();
}


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

//// Compressible module (field solver)
// Compute field variables and field sources
//
// I/O:
// - Minf: freestream Mach number
// - vInf: freestream velocity vector
// - bPan: (network of) body panels (structure)
// - fPan: field panels (structure)
// - sp: sub-panels (structure)
// - b2fAIC: body to field AIC (structure)
// - f2fAIC: field to field AIC (structure)
// - spAIC: body to field AIC for subpanels (structure)

#include <iostream>
#include <Eigen/Dense>
#include "solve_field.h"
#include "compute_fVars.h"

using namespace std;
using  namespace Eigen;

#define GAMMA 1.4
#define CMU 1.0
#define M_C 1.0

void solve_field(double Minf, Vector3d &vInf, Network &bPan, Field &fPan, Subpanel &sp,
                 Body_AIC &b2fAIC, Field2field_AIC &f2fAIC, Subpanel_AIC &spAIC) {

    //// Begin
    cout << "Computing field variables... " << flush;

    //// Field variables
    compute_fVars(Minf, vInf, bPan, fPan, sp, b2fAIC, f2fAIC, spAIC);

    //// Field sources
    // Density gradient
    for (int i = 0; i < fPan.nE; i++) {
        int f = fPan.eIdx(i);
        double dU, dV, dW; // Variables for residual

        // X-derivative
        double fb=0, ff=0, fb2=0, ff2=0;
        if (fPan.fbdMap(f,0)) {
            fb = (fPan.rho(f) - fPan.rho(f - 1)) / fPan.deltaX;
            fb2 = (fPan.U(f,0) - fPan.U(f - 1,0)) / fPan.deltaX;
        }
        if (fPan.fbdMap(f,1)) {
            ff = (fPan.rho(f + 1) - fPan.rho(f)) / fPan.deltaX;
            ff2 = (fPan.U(f + 1,0) - fPan.U(f,0)) / fPan.deltaX;
        }
        if (fPan.fbdMap(f,0) && fPan.fbdMap(f,1)) {
            fPan.dRho(f,0) = 0.5*(fb+ff);
            dU = 0.5*(fb2+ff2);
        }
        else {
            fPan.dRho(f,0) = fb+ff;
            dU = fb2+ff2;
        }

        // Y-derivative
        fb=0, ff=0, fb2=0, ff2=0;
        if (fPan.fbdMap(f,2)) {
            fb = (fPan.rho(f) - fPan.rho(f - fPan.nX*fPan.nZ)) / fPan.deltaY;
            fb2 = (fPan.U(f,1) - fPan.U(f - fPan.nX*fPan.nZ,1)) / fPan.deltaY;
        }
        if (fPan.fbdMap(f,3)) {
            ff = (fPan.rho(f + fPan.nX*fPan.nZ) - fPan.rho(f)) / fPan.deltaY;
            ff2 = (fPan.U(f + fPan.nX*fPan.nZ,1) - fPan.U(f,1)) / fPan.deltaY;
        }
        if (fPan.fbdMap(f,2) && fPan.fbdMap(f,3)) {
            fPan.dRho(f,1) = 0.5*(fb+ff);
            dV = 0.5*(fb2+ff2);
        }
        else {
            fPan.dRho(f,1) = fb+ff;
            dV = fb2+ff2;
        }

        // Z-derivative
        fb=0, ff=0, fb2=0, ff2=0;
        if (fPan.fbdMap(f,4)) {
            fb = (fPan.rho(f) - fPan.rho(f - fPan.nX)) / fPan.deltaZ;
            fb2 = (fPan.U(f,2) - fPan.U(f - fPan.nX,2)) / fPan.deltaZ;
        }
        if (fPan.fbdMap(f,5)) {
            ff = (fPan.rho(f + fPan.nX) - fPan.rho(f)) / fPan.deltaZ;
            ff2 = (fPan.U(f + fPan.nX,2) - fPan.U(f,2)) / fPan.deltaZ;
        }
        if (fPan.fbdMap(f,4) && fPan.fbdMap(f,5)) {
            fPan.dRho(f,2) = 0.5*(fb+ff);
            dW = 0.5*(fb2+ff2);
        }
        else {
            fPan.dRho(f,2) = fb+ff;
            dW = fb2+ff2;
        }

        // Field source
        fPan.sigma(f) = -1 / (fPan.rho(f)) * fPan.U.row(f).dot(fPan.dRho.row(f));
        // Residual
        fPan.epsilon(f) = dU + dV + dW - fPan.sigma(f);
    }

    // TODO 1) Artificial density (pros: physical; cons: does not work on MG or RG/MG)
    // TODO 2) Artificial viscosity (pros: works on RG/MG; cons: cut through surface, not physical)
    // TODO NB) With current form, x-upwinding gives same results as s-upwinding.

    //// Artificial viscosity
    for (int i = 0; i < fPan.nE; i++) {
        int f = fPan.eIdx(i);
        double mu, deltaSigmaX, deltaSigmaY, deltaSigmaZ; // Variables for artificial viscosity

        if (fPan.M(f) > M_C) {
            mu = CMU * (1 - M_C * M_C / (fPan.M(f) * fPan.M(f)));

            // X-contribution
            if (fPan.U(f,0) > 0 && fPan.fbdMap(f,0))
                deltaSigmaX = fPan.sigma(f) - fPan.sigma(f - 1);
            else if (fPan.U(f,0) < 0 && fPan.fbdMap(f,1))
                deltaSigmaX = fPan.sigma(f + 1) - fPan.sigma(f);
            else
                deltaSigmaX = 0;
            // Y-contribution
            if (fPan.U(f,1) > 0 && fPan.fbdMap(f,2))
                deltaSigmaY = fPan.sigma(f) - fPan.sigma(f - fPan.nX*fPan.nZ);
            else if (fPan.U(f,1) < 0 && fPan.fbdMap(f,3))
                deltaSigmaY = fPan.sigma(f + fPan.nX*fPan.nZ) - fPan.sigma(f);
            else
                deltaSigmaY = 0;
            // Z-contribution
            if (fPan.U(f,2) > 0 && fPan.fbdMap(f,4))
                deltaSigmaZ = fPan.sigma(f) - fPan.sigma(f - fPan.nX);
            else if (fPan.U(f,2) < 0 && fPan.fbdMap(f,5))
                deltaSigmaZ = fPan.sigma(f + fPan.nX) - fPan.sigma(f);
            else
                deltaSigmaZ = 0;

            fPan.sigma(f) -= mu / fPan.U.row(f).norm() *
                               (fPan.U(f, 0) * deltaSigmaX + fPan.U(f, 1) * deltaSigmaY + fPan.U(f, 2) * deltaSigmaZ);
        }
    }

    cout << "Done!" << endl;
    cout << "Max. Mach number: " << fPan.M.maxCoeff() << endl;
    cout << "Field sources strength: " << fPan.sigma.norm() << endl << endl;
}