#include "yolo_seg_geometry.h"

#include <algorithm>
#include <cmath>
#include <limits>
#include <random>

namespace yolo_seg_geometry {
namespace {

constexpr double epsilon = 1e-7;
constexpr double pi = 3.14159265358979323846;

double cross(const Point& origin, const Point& first, const Point& second) {
    return (first.x - origin.x) * (second.y - origin.y) -
           (first.y - origin.y) * (second.x - origin.x);
}

double squared_distance(const Point& first, const Point& second) {
    const double dx = first.x - second.x;
    const double dy = first.y - second.y;
    return dx * dx + dy * dy;
}

std::vector<Point> convex_hull(std::vector<Point> points) {
    std::sort(points.begin(), points.end(), [](const Point& first, const Point& second) {
        return first.x < second.x || (first.x == second.x && first.y < second.y);
    });
    points.erase(std::unique(points.begin(), points.end(), [](const Point& first, const Point& second) {
        return first.x == second.x && first.y == second.y;
    }), points.end());
    if (points.size() <= 2) return points;

    std::vector<Point> hull(points.size() * 2);
    size_t count = 0;
    for (const auto& point : points) {
        while (count >= 2 && cross(hull[count - 2], hull[count - 1], point) <= 0.0) --count;
        hull[count++] = point;
    }
    const size_t lower_count = count;
    for (size_t index = points.size() - 1; index-- > 0;) {
        const auto& point = points[index];
        while (count > lower_count && cross(hull[count - 2], hull[count - 1], point) <= 0.0) --count;
        hull[count++] = point;
    }
    if (count > 1) --count;
    hull.resize(count);
    return hull;
}

bool contains(const Circle& circle, const Point& point) {
    return circle.valid &&
           squared_distance({circle.center_x, circle.center_y}, point) <=
               circle.radius * circle.radius + epsilon * std::max(1.0, circle.radius * circle.radius);
}

Circle diameter_circle(const Point& first, const Point& second) {
    Circle result;
    result.center_x = (first.x + second.x) * 0.5;
    result.center_y = (first.y + second.y) * 0.5;
    result.radius = std::sqrt(squared_distance(first, second)) * 0.5;
    result.valid = true;
    return result;
}

Circle circumcircle(const Point& first, const Point& second, const Point& third) {
    const double determinant = 2.0 * (
        first.x * (second.y - third.y) +
        second.x * (third.y - first.y) +
        third.x * (first.y - second.y));
    if (std::abs(determinant) <= epsilon) {
        Circle best;
        for (const auto& candidate : {
                 diameter_circle(first, second),
                 diameter_circle(first, third),
                 diameter_circle(second, third)}) {
            if (contains(candidate, first) && contains(candidate, second) && contains(candidate, third) &&
                (!best.valid || candidate.radius < best.radius)) {
                best = candidate;
            }
        }
        return best;
    }
    const double first_norm = first.x * first.x + first.y * first.y;
    const double second_norm = second.x * second.x + second.y * second.y;
    const double third_norm = third.x * third.x + third.y * third.y;
    Circle result;
    result.center_x = (
        first_norm * (second.y - third.y) +
        second_norm * (third.y - first.y) +
        third_norm * (first.y - second.y)) / determinant;
    result.center_y = (
        first_norm * (third.x - second.x) +
        second_norm * (first.x - third.x) +
        third_norm * (second.x - first.x)) / determinant;
    result.radius = std::sqrt(squared_distance({result.center_x, result.center_y}, first));
    result.valid = true;
    return result;
}

double normalize_half_turn(double angle) {
    while (angle < -pi * 0.5) angle += pi;
    while (angle >= pi * 0.5) angle -= pi;
    return angle;
}

} // namespace

std::vector<Point> mask_boundary(
    const unsigned char* mask,
    int width,
    int height,
    int stride,
    int origin_x,
    int origin_y) {
    std::vector<Point> points;
    if (!mask || width <= 0 || height <= 0 || stride < width) return points;
    points.reserve(static_cast<size_t>(width + height) * 2);
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            if (mask[static_cast<size_t>(y) * stride + x] == 0) continue;
            const bool boundary =
                x == 0 || x + 1 == width || y == 0 || y + 1 == height ||
                mask[static_cast<size_t>(y) * stride + x - 1] == 0 ||
                mask[static_cast<size_t>(y) * stride + x + 1] == 0 ||
                mask[static_cast<size_t>(y - 1) * stride + x] == 0 ||
                mask[static_cast<size_t>(y + 1) * stride + x] == 0;
            if (boundary) points.push_back({
                static_cast<double>(origin_x + x),
                static_cast<double>(origin_y + y)});
        }
    }
    return points;
}

Circle minimum_enclosing_circle(const std::vector<Point>& input) {
    auto points = convex_hull(input);
    if (points.empty()) return {};
    std::mt19937 generator(0x3588u);
    std::shuffle(points.begin(), points.end(), generator);
    Circle circle;
    for (size_t first = 0; first < points.size(); ++first) {
        if (contains(circle, points[first])) continue;
        circle = {points[first].x, points[first].y, 0.0, true};
        for (size_t second = 0; second < first; ++second) {
            if (contains(circle, points[second])) continue;
            circle = diameter_circle(points[first], points[second]);
            for (size_t third = 0; third < second; ++third) {
                if (contains(circle, points[third])) continue;
                circle = circumcircle(points[first], points[second], points[third]);
            }
        }
    }
    return circle;
}

RotatedRect minimum_area_rect(const std::vector<Point>& input) {
    const auto points = convex_hull(input);
    if (points.empty()) return {};
    if (points.size() == 1) return {points[0].x, points[0].y, 0.0, 0.0, 0.0, true};

    double best_area = std::numeric_limits<double>::infinity();
    RotatedRect best;
    for (size_t index = 0; index < points.size(); ++index) {
        const Point& first = points[index];
        const Point& second = points[(index + 1) % points.size()];
        const double angle = std::atan2(second.y - first.y, second.x - first.x);
        const double cosine = std::cos(angle);
        const double sine = std::sin(angle);
        double min_x = std::numeric_limits<double>::infinity();
        double max_x = -std::numeric_limits<double>::infinity();
        double min_y = std::numeric_limits<double>::infinity();
        double max_y = -std::numeric_limits<double>::infinity();
        for (const auto& point : points) {
            const double rotated_x = point.x * cosine + point.y * sine;
            const double rotated_y = -point.x * sine + point.y * cosine;
            min_x = std::min(min_x, rotated_x);
            max_x = std::max(max_x, rotated_x);
            min_y = std::min(min_y, rotated_y);
            max_y = std::max(max_y, rotated_y);
        }
        const double width = max_x - min_x;
        const double height = max_y - min_y;
        const double area = width * height;
        if (area + epsilon >= best_area) continue;
        const double rotated_center_x = (min_x + max_x) * 0.5;
        const double rotated_center_y = (min_y + max_y) * 0.5;
        best_area = area;
        best.center_x = rotated_center_x * cosine - rotated_center_y * sine;
        best.center_y = rotated_center_x * sine + rotated_center_y * cosine;
        best.width = width;
        best.height = height;
        best.angle_radians = angle;
        if (best.height > best.width) {
            std::swap(best.width, best.height);
            best.angle_radians += pi * 0.5;
        }
        best.angle_radians = normalize_half_turn(best.angle_radians);
        best.valid = true;
    }
    return best;
}

} // namespace yolo_seg_geometry