#include "detection_engine.hpp"

#include "legacy_detection.hpp"

#include <algorithm>
#include <chrono>
#include <cmath>
#include <map>

#include "detector/geometry/calibration/dartboard_visualization.hpp"
#include "detector/geometry/detection/dart_processing.hpp"
#include "detector/geometry/detection/motion_processing.hpp"

using json = nlohmann::json;

DetectionEngine::DetectionEngine(int width, int height)
    : width_(width), height_(height)
{
}

std::int64_t DetectionEngine::nowMillis()
{
    return std::chrono::duration_cast<std::chrono::milliseconds>(
               std::chrono::system_clock::now().time_since_epoch())
        .count();
}

bool DetectionEngine::calibrate(const std::vector<cv::Mat> &frames, std::string &error)
{
    std::lock_guard<std::mutex> lock(mutex_);
    calibrated_ = false;
    calibrations_.clear();
    backgrounds_.clear();
    legacy_detection::resetAll();

    if (frames.size() != 3 || std::any_of(frames.begin(), frames.end(), [](const cv::Mat &frame)
                                          { return frame.empty(); }))
    {
        error = "Kalibrering kräver tre kompletta kamerabilder";
        return false;
    }

    calibrations_ = geometry_calibration::calibrateMultipleCameras(frames, false, width_, height_);
    if (calibrations_.size() != 3)
    {
        error = "Geometrikalibreringen gav inte resultat för alla tre kameror";
        calibrations_.clear();
        return false;
    }

    int valid_geometry = 0;
    int oriented_cameras = 0;
    for (const auto &calibration : calibrations_)
    {
        // The legacy scorer only needs the outer double boundary and the 20
        // radial wires. Triple ellipses are useful diagnostics, but rejecting
        // an otherwise good side-camera calibration because contour fitting
        // missed one triple ring makes the three-camera setup unnecessarily
        // brittle.
        if (calibration.ellipses.hasValidDoubles && calibration.ellipses.hasValidBulls &&
            calibration.wires.isValid)
            ++valid_geometry;
        if (calibration.orientation.isStarCamera && calibration.orientation.wedge20WireIndex >= 0)
            ++oriented_cameras;
    }

    if (valid_geometry != 3)
    {
        error = "En eller flera kameror kunde inte hitta tavlans ringar och trådar";
        return false;
    }
    if (oriented_cameras == 0)
    {
        error = "Ingen kamera kunde bestämma var 20-segmentet sitter";
        return false;
    }

    for (const auto &frame : frames)
        backgrounds_.push_back(frame.clone());

    calibrated_ = true;
    legacy_detection::resetAll();
    return true;
}

void DetectionEngine::clearCalibration()
{
    std::lock_guard<std::mutex> lock(mutex_);
    calibrated_ = false;
    calibrations_.clear();
    backgrounds_.clear();
    legacy_detection::resetAll();
}

bool DetectionEngine::calibrated() const
{
    std::lock_guard<std::mutex> lock(mutex_);
    return calibrated_;
}

bool DetectionEngine::isPointInEllipse(cv::Point2f point, const cv::RotatedRect &ellipse)
{
    const cv::Point2f relative = point - ellipse.center;
    const float angle = -ellipse.angle * static_cast<float>(CV_PI) / 180.0F;
    const float cosine = std::cos(angle);
    const float sine = std::sin(angle);
    const cv::Point2f rotated(relative.x * cosine - relative.y * sine,
                              relative.x * sine + relative.y * cosine);
    const float a = ellipse.size.width / 2.0F;
    const float b = ellipse.size.height / 2.0F;
    if (a <= 0.0F || b <= 0.0F)
        return false;
    return (rotated.x * rotated.x) / (a * a) + (rotated.y * rotated.y) / (b * b) <= 1.0F;
}

std::string DetectionEngine::scoreAtPoint(cv::Point2f pixel, const DartboardCalibration &calibration)
{
    if (!calibration.ellipses.hasValidDoubles || !calibration.wires.isValid)
        return "MISS";

    if (isPointInEllipse(pixel, calibration.ellipses.innerBullEllipse))
        return "BULL";
    if (isPointInEllipse(pixel, calibration.ellipses.outerBullEllipse))
        return "OUTER";

    const bool inner_triple = isPointInEllipse(pixel, calibration.ellipses.innerTripleEllipse);
    const bool outer_triple = isPointInEllipse(pixel, calibration.ellipses.outerTripleEllipse);
    const bool inner_double = isPointInEllipse(pixel, calibration.ellipses.innerDoubleEllipse);
    const bool outer_double = isPointInEllipse(pixel, calibration.ellipses.outerDoubleEllipse);

    std::string prefix;
    if (outer_double && !inner_double)
        prefix = "D";
    else if (outer_triple && !inner_triple)
        prefix = "T";
    else if (outer_double)
        prefix = "S";
    else
        return "MISS";

    if (!calibration.orientation.isStarCamera || calibration.orientation.wedge20WireIndex < 0)
        return "MISS";

    const cv::Point2f center(calibration.bullCenter);
    const cv::Point2f direction = pixel - center;
    float point_angle = std::atan2(direction.y, direction.x);
    if (point_angle < 0.0F)
        point_angle += 2.0F * static_cast<float>(CV_PI);

    static const std::array<int, 20> numbers = {20, 1, 18, 4, 13, 6, 10, 15, 2, 17, 3, 19, 7, 16, 8, 11, 14, 9, 12, 5};
    const int wire20 = calibration.orientation.wedge20WireIndex;

    for (int index = 0; index < 20; ++index)
    {
        const cv::Point2f wire1 = calibration.wires.wireEndpoints[(wire20 + index) % 20];
        const cv::Point2f wire2 = calibration.wires.wireEndpoints[(wire20 + index + 1) % 20];
        float angle1 = std::atan2(wire1.y - center.y, wire1.x - center.x);
        float angle2 = std::atan2(wire2.y - center.y, wire2.x - center.x);
        if (angle1 < 0.0F)
            angle1 += 2.0F * static_cast<float>(CV_PI);
        if (angle2 < 0.0F)
            angle2 += 2.0F * static_cast<float>(CV_PI);
        if (angle2 < angle1)
            angle2 += 2.0F * static_cast<float>(CV_PI);

        float test_angle = point_angle;
        if (test_angle < angle1)
            test_angle += 2.0F * static_cast<float>(CV_PI);
        if (test_angle >= angle1 && test_angle <= angle2)
            return prefix + std::to_string(numbers[index]);
    }
    return "MISS";
}

std::optional<DetectionEngine::Event> DetectionEngine::process(const std::vector<cv::Mat> &frames)
{
    const auto started = std::chrono::steady_clock::now();
    std::lock_guard<std::mutex> lock(mutex_);
    if (!calibrated_ || frames.size() != 3 || backgrounds_.size() != 3)
        return std::nullopt;
    if (std::any_of(frames.begin(), frames.end(), [](const cv::Mat &frame)
                    { return frame.empty(); }))
        return std::nullopt;

    const auto motion = motion_processing::processMotion(frames, backgrounds_, false);
    const auto darts = dart_processing::processDartState(frames, backgrounds_, motion.motion_finished, false);

    if (darts.previous_state == darts.current_state)
        return std::nullopt;

    Event event;
    event.timestamp = nowMillis();
    if (darts.current_state == dart_processing::DartBoardState::CLEAN)
    {
        event.score = "END";
        event.confidence = 1.0F;
    }
    else
    {
        std::map<std::string, std::vector<int>> scores;
        for (std::size_t index = 0; index < darts.camera_results.size() && index < calibrations_.size(); ++index)
        {
            const auto &camera_result = darts.camera_results[index];
            const auto &calibration = calibrations_[index];
            if (!camera_result.tip_found || !calibration.orientation.isStarCamera)
                continue;
            const std::string score = scoreAtPoint(camera_result.tip_position, calibration);
            if (score != "MISS")
                scores[score].push_back(static_cast<int>(index));
        }

        if (scores.empty())
        {
            event.score = "MISS";
            event.confidence = 0.4F;
        }
        else
        {
            const auto best = std::max_element(scores.begin(), scores.end(), [](const auto &left, const auto &right)
                                               { return left.second.size() < right.second.size(); });
            event.score = best->first;
            event.camera_index = best->second.front();
            event.position = darts.camera_results[event.camera_index].tip_position;
            event.confidence = best->second.size() >= 2 ? 0.95F : 0.78F;
        }
    }

    event.processing_time_ms = static_cast<int>(std::chrono::duration_cast<std::chrono::milliseconds>(
                                                    std::chrono::steady_clock::now() - started)
                                                    .count());
    return event;
}

cv::Mat DetectionEngine::drawOverlay(std::size_t camera_index, const cv::Mat &frame) const
{
    std::lock_guard<std::mutex> lock(mutex_);
    if (!calibrated_ || camera_index >= calibrations_.size() || frame.empty())
        return frame.clone();
    return dartboard_visualization::drawCalibrationOverlay(frame, calibrations_[camera_index], true);
}

json DetectionEngine::calibrationInfo() const
{
    std::lock_guard<std::mutex> lock(mutex_);
    json cameras = json::array();
    for (const auto &calibration : calibrations_)
    {
        cameras.push_back({
            {"index", calibration.camera_index},
            {"valid", calibration.ellipses.hasValidDoubles && calibration.ellipses.hasValidBulls && calibration.wires.isValid},
            {"rings", {
                 {"doubles", calibration.ellipses.hasValidDoubles},
                 {"triples", calibration.ellipses.hasValidTriples},
                 {"bulls", calibration.ellipses.hasValidBulls},
             }},
            {"wires", calibration.wires.isValid ? 20 : 0},
            {"oriented", calibration.orientation.isStarCamera && calibration.orientation.wedge20WireIndex >= 0},
            {"position", orientation_processing::cameraPositionToString(calibration.orientation.cameraPosition)},
            {"bull", {{"x", calibration.bullCenter.x}, {"y", calibration.bullCenter.y}}},
        });
    }
    return {{"ready", calibrated_}, {"cameras", cameras}};
}