#include "bull_processing.hpp"
#include "color_processing.hpp"
#include "utils.hpp"
#include <algorithm>
#include <cmath>

using namespace cv;
using namespace std;

namespace bull_processing
{
    struct CompactBullCandidate
    {
        bool found = false;
        Point center{0, 0};
        double score = 0.0;
        int area = 0;
    };

    CompactBullCandidate findCompactColorBull(const Mat &redGreenFrame)
    {
        CompactBullCandidate best;

        Mat redMask, greenMask, combinedMask;
        inRange(redGreenFrame, Scalar(0, 0, 180), Scalar(40, 40, 255), redMask);
        inRange(redGreenFrame, Scalar(0, 180, 0), Scalar(40, 255, 40), greenMask);
        bitwise_or(redMask, greenMask, combinedMask);

        Mat closeKernel = getStructuringElement(MORPH_ELLIPSE, Size(3, 3));
        morphologyEx(combinedMask, combinedMask, MORPH_CLOSE, closeKernel);

        Mat labels, stats, centroids;
        int nLabels = connectedComponentsWithStats(combinedMask, labels, stats, centroids);
        double frameArea = static_cast<double>(redGreenFrame.cols * redGreenFrame.rows);
        double idealArea = frameArea * 0.0025;

        for (int i = 1; i < nLabels; i++)
        {
            int area = stats.at<int>(i, CC_STAT_AREA);
            int left = stats.at<int>(i, CC_STAT_LEFT);
            int top = stats.at<int>(i, CC_STAT_TOP);
            int width = stats.at<int>(i, CC_STAT_WIDTH);
            int height = stats.at<int>(i, CC_STAT_HEIGHT);

            if (area < max(120.0, frameArea * 0.00012) ||
                area > frameArea * 0.009 ||
                width < 18 || height < 18 ||
                width > redGreenFrame.cols * 0.16 ||
                height > redGreenFrame.rows * 0.20)
            {
                continue;
            }

            double aspectRatio = static_cast<double>(width) / max(1, height);
            if (aspectRatio < 0.45 || aspectRatio > 2.2)
            {
                continue;
            }

            int redPixels = 0;
            int greenPixels = 0;
            for (int y = top; y < top + height; y++)
            {
                for (int x = left; x < left + width; x++)
                {
                    if (labels.at<int>(y, x) != i)
                    {
                        continue;
                    }
                    if (redMask.at<uchar>(y, x) > 0)
                    {
                        redPixels++;
                    }
                    if (greenMask.at<uchar>(y, x) > 0)
                    {
                        greenPixels++;
                    }
                }
            }

            if (redPixels < 20 || greenPixels < 20 ||
                redPixels + greenPixels < area * 0.35)
            {
                continue;
            }

            double fillRatio = area / max(1.0, static_cast<double>(width * height));
            double compactness = min(aspectRatio, 1.0 / aspectRatio);
            double colorBalance = static_cast<double>(min(redPixels, greenPixels)) / max(1, max(redPixels, greenPixels));
            double sizeScore = 1.0 - min(1.0, abs(area - idealArea) / max(1.0, idealArea));
            double score = colorBalance * 0.35 + fillRatio * 0.25 + compactness * 0.25 + sizeScore * 0.15;

            if (score > best.score)
            {
                best.found = true;
                best.center = Point(cvRound(centroids.at<double>(i, 0)), cvRound(centroids.at<double>(i, 1)));
                best.score = score;
                best.area = area;
            }
        }

        return best;
    }

    Point processBull(const Mat &redGreenFrame, const Point &frameCenter, int camera_idx, bool debug_mode, const BullParams &params)
    {
        log_debug("Bull detection camera " + log_string(camera_idx) + " starting...");

        // Step 1: Create blur mask
        Mat blurredFrame;
        GaussianBlur(redGreenFrame, blurredFrame, Size(7, 7), 2.0);

        // Step 2: Convert to black/white - anything not black becomes white
        Mat grayMask;
        cvtColor(blurredFrame, grayMask, COLOR_BGR2GRAY);
        Mat binaryMask;
        threshold(grayMask, binaryMask, 1, 255, THRESH_BINARY); // Any non-zero pixel becomes white

        // Debug: Save masks
        if (debug_mode)
        {
            system("mkdir -p debug_frames/bull_processing");
            imwrite("debug_frames/bull_processing/blurred_frame_" + to_string(camera_idx) + ".jpg", blurredFrame);
            imwrite("debug_frames/bull_processing/binary_mask_" + to_string(camera_idx) + ".jpg", binaryMask);
        }

        CompactBullCandidate compactBull = findCompactColorBull(redGreenFrame);
        if (compactBull.found && compactBull.score > 0.45)
        {
            log_debug("Compact color bull candidate selected - center=(" +
                      log_string(compactBull.center.x) + "," + log_string(compactBull.center.y) +
                      "), area=" + log_string(compactBull.area) +
                      ", score=" + log_string(compactBull.score));

            if (debug_mode)
            {
                Mat bullDebug = redGreenFrame.clone();
                circle(bullDebug, compactBull.center, 8, Scalar(255, 255, 255), 2);
                circle(bullDebug, compactBull.center, 3, Scalar(255, 255, 255), -1);
                imwrite("debug_frames/bull_processing/compact_bull_" + to_string(camera_idx) + ".jpg", bullDebug);
            }

            return compactBull.center;
        }

        // Step 3: Find contours
        vector<vector<Point>> contours;
        vector<Vec4i> hierarchy;
        findContours(binaryMask, contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE);

        log_debug("Found " + log_string(contours.size()) + " contours total");

        if (contours.empty())
        {
            log_warning("No contours found");
            return frameCenter;
        }

        // Step 4: Analyze all contours with detailed info
        Point bestBullCenter = frameCenter;
        double bestScore = 0;
        int bestContourIndex = -1;

        for (size_t i = 0; i < contours.size(); i++)
        {
            const auto &contour = contours[i];
            double area = contourArea(contour);

            // Skip tiny contours - increased threshold
            if (area < 200)
                continue;

            // Calculate circularity
            double perimeter = arcLength(contour, true);
            if (perimeter == 0)
                continue;

            double circularity = (4 * CV_PI * area) / (perimeter * perimeter);

            // Get contour center
            Point2f center2f;
            float radius;
            minEnclosingCircle(contour, center2f, radius);
            Point center(center2f);

            // Check if this is inner or outer contour
            bool isInner = (hierarchy[i][2] != -1); // Has children (inner contour)
            bool isOuter = (hierarchy[i][3] == -1); // No parent (outer contour)

            // Simple scoring for bull detection
            double score = circularity * 0.8 + (1.0 / (1.0 + area / 200.0)) * 0.2;

            log_debug("Contour " + log_string(i) +
                      ": area=" + log_string((int)area) +
                      ", circ=" + log_string(circularity).substr(0, 4) +
                      ", center=(" + log_string(center.x) + "," + log_string(center.y) + ")" +
                      ", inner=" + log_string(isInner) +
                      ", outer=" + log_string(isOuter) +
                      ", score=" + log_string(score).substr(0, 4));

            if (score > bestScore && circularity > 0.3)
            {
                bestScore = score;
                bestBullCenter = center;
                bestContourIndex = i;
            }
        }

        if (bestContourIndex == -1)
        {
            log_warning("No good contours found, using frame center");
            bestBullCenter = frameCenter;
        }

        log_debug("Bull detection complete - center=(" + log_string(bestBullCenter.x) +
                  "," + log_string(bestBullCenter.y) + "), score=" + log_string(bestScore));

        // Enhanced debug visualization
        if (debug_mode)
        {
            Mat bullDebug = redGreenFrame.clone();

            // Dim the background to 20% to make outlines pop
            bullDebug = bullDebug * 0.2;

            // Collect contour info for top display
            vector<string> contourInfo;
            vector<Scalar> contourColors;
            int validContourCount = 0;

            // Draw ALL contours with new color scheme and collect info
            for (size_t i = 0; i < contours.size(); i++)
            {
                double area = contourArea(contours[i]);
                if (area < 200)
                    continue; // Skip tiny ones in visualization

                double perimeter = arcLength(contours[i], true);
                double circularity = (perimeter > 0) ? (4 * CV_PI * area) / (perimeter * perimeter) : 0;

                Point2f center2f;
                float radius;
                minEnclosingCircle(contours[i], center2f, radius);
                Point center(center2f);

                bool isInner = (hierarchy[i][2] != -1);
                bool isOuter = (hierarchy[i][3] == -1);

                // New color scheme - avoiding green/red
                Scalar color;
                string status;
                if (i == bestContourIndex)
                {
                    color = Scalar(255, 255, 0); // Bright CYAN for best
                    status = "BEST";
                }
                else if (isInner)
                {
                    color = Scalar(0, 255, 255); // YELLOW for inner
                    status = "INNER";
                }
                else if (isOuter)
                {
                    color = Scalar(255, 0, 255); // MAGENTA for outer
                    status = "OUTER";
                }
                else
                {
                    color = Scalar(255, 255, 255); // WHITE for poor
                    status = "POOR";
                }

                drawContours(bullDebug, contours, i, color, 3);

                // Find leftmost point of the contour
                Point leftmostPoint = contours[i][0];
                for (const Point &pt : contours[i])
                {
                    if (pt.x < leftmostPoint.x)
                    {
                        leftmostPoint = pt;
                    }
                }

                // Small number overlay at leftmost point (white text with black outline)
                string numberLabel = to_string(validContourCount);
                Point labelPos = leftmostPoint + Point(-15, 5); // Slightly left of leftmost point

                putText(bullDebug, numberLabel, labelPos,
                        FONT_HERSHEY_SIMPLEX, 0.7, Scalar(0, 0, 0), 3); // Black outline
                putText(bullDebug, numberLabel, labelPos,
                        FONT_HERSHEY_SIMPLEX, 0.7, Scalar(255, 255, 255), 2); // White text

                // Collect info for top display with color
                string info = "Contour " + to_string(validContourCount) + ": A=" + to_string(int(area)) +
                              " C=" + to_string(circularity).substr(0, 4) + " (" + status + ")";
                contourInfo.push_back(info);
                contourColors.push_back(color);

                validContourCount++;
            }

            // Draw final bull center with bright white
            circle(bullDebug, bestBullCenter, 4, Scalar(255, 255, 255), -1);

            // Display contour info at top with colored backgrounds
            int yPos = 30;
            for (size_t i = 0; i < contourInfo.size(); i++)
            {
                const string &info = contourInfo[i];
                Scalar bgColor = contourColors[i];

                // Get text size for background rectangle
                Size textSize = getTextSize(info, FONT_HERSHEY_SIMPLEX, 0.5, 1, nullptr);

                // Draw colored background (dimmed version of contour color)
                Scalar dimmedColor = bgColor * 0.3; // 30% of original color
                rectangle(bullDebug, Point(10, yPos - 20), Point(15 + textSize.width, yPos + 5),
                          dimmedColor, -1);

                // Draw text in white for readability
                putText(bullDebug, info, Point(15, yPos),
                        FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 255, 255), 1);
                yPos += 25;
            }

            imwrite("debug_frames/bull_processing/bull_detection_" + to_string(camera_idx) + ".jpg", bullDebug);
        }

        return bestBullCenter;
    }

} // namespace bull_processing