using Godot;
using System;
using System.Collections.Generic;
using System.Linq;
using OpenCvSharp;

/// <summary>
/// Detects pupil centers from eye-tracking camera images using OpenCV.
/// Based on threshold-based pupil detection with circularity filtering.
/// </summary>
public partial class PupilDetector : Node
{
    /// <summary>
    /// Represents a detected pupil with its center position and radius.
    /// </summary>
    public struct PupilInfo
    {
        public int X { get; set; }
        public int Y { get; set; }
        public float Radius { get; set; }
        
        public PupilInfo(int x, int y, float radius)
        {
            X = x;
            Y = y;
            Radius = radius;
        }
    }
    
    private const float MinArea = 200f;
    private const float MinRadius = 20f;
    private const float MaxRadius = 96f; // Reduced from 120 to filter false positives
    private const float DarkestPercentile = 15f; // Keep darkest 15% of pixels
    
    /// <summary>
    /// Finds pupil centers in the given image.
    /// </summary>
    /// <param name="img">Input image as OpenCV Mat (BGR or grayscale)</param>
    /// <returns>List of detected pupils (up to 2), sorted left-to-right</returns>
    public static List<PupilInfo> FindPupilCenters(Mat img)
    {
        if (img == null || img.Empty())
        {
            GD.PrintErr("PupilDetector: Input image is null or empty");
            return new List<PupilInfo>();
        }
        
        // Convert to grayscale if needed
        Mat gray = new Mat();
        if (img.Channels() > 1)
        {
            Cv2.CvtColor(img, gray, ColorConversionCodes.BGR2GRAY);
        }
        else
        {
            gray = img.Clone();
        }
        
        // Calculate threshold value for darkest 15% of pixels
        double threshVal = CalculatePercentile(gray, DarkestPercentile);
        
        // Keep only the darkest pixels (robust to uneven lighting)
        Mat mask = new Mat();
        Cv2.Threshold(gray, mask, threshVal, 255, ThresholdTypes.BinaryInv);
        
        // Clean up noise
        Cv2.MedianBlur(mask, mask, 5);
        Mat kernel = Cv2.GetStructuringElement(MorphShapes.Ellipse, new OpenCvSharp.Size(5, 5));
        Cv2.MorphologyEx(mask, mask, MorphTypes.Open, kernel, iterations: 1);
        
        // Find contours - candidates for pupils (dark, roughly circular blobs)
        Cv2.FindContours(mask, out Point[][] contours, out _, RetrievalModes.External, ContourApproximationModes.ApproxSimple);
        
        List<(float score, int x, int y, float radius)> candidates = new List<(float, int, int, float)>();
        
        foreach (var contour in contours)
        {
            double area = Cv2.ContourArea(contour);
            if (area < MinArea)
                continue;
            
            double perimeter = Cv2.ArcLength(contour, true);
            if (perimeter == 0)
                continue;
            
            // Circularity measure: 1.0 = perfect circle
            float circularity = (float)(4 * Math.PI * area / (perimeter * perimeter));
            
            Cv2.MinEnclosingCircle(contour, out Point2f center, out float radius);
            
            if (radius < MinRadius || radius > MaxRadius)
                continue;
            
            // Score: prefer circular and reasonably large
            // Higher circularity closer to 0.8 is better
            float score = -Math.Abs(0.8f - circularity) + 0.0001f * (float)area;
            
            candidates.Add((score, (int)center.X, (int)center.Y, radius));
        }
        
        // Sort by score (best first) and take top 2
        candidates.Sort((a, b) => b.score.CompareTo(a.score));
        var topPupils = candidates.Take(2).Select(c => new PupilInfo(c.x, c.y, c.radius)).ToList();
        
        // Sort left-to-right for consistency
        topPupils.Sort((a, b) => a.X.CompareTo(b.X));
        
        // Cleanup
        gray.Dispose();
        mask.Dispose();
        kernel.Dispose();
        
        return topPupils;
    }
    
    /// <summary>
    /// Calculates the specified percentile value from a grayscale image.
    /// </summary>
    private static double CalculatePercentile(Mat gray, float percentile)
    {
        // Create histogram
        int histSize = 256;
        float[] range = { 0, 256 };
        Rangef[] ranges = { new Rangef(range[0], range[1]) };
        
        Mat hist = new Mat();
        Cv2.CalcHist(new Mat[] { gray }, new int[] { 0 }, null, hist, 1, new int[] { histSize }, ranges);
        
        // Calculate total number of pixels
        long totalPixels = gray.Rows * gray.Cols;
        long targetCount = (long)(totalPixels * (percentile / 100.0));
        
        // Find the threshold value at the percentile
        long cumulative = 0;
        double thresholdValue = 0;
        
        for (int i = 0; i < histSize; i++)
        {
            cumulative += (long)hist.At<float>(i);
            if (cumulative >= targetCount)
            {
                thresholdValue = i;
                break;
            }
        }
        
        hist.Dispose();
        return thresholdValue;
    }
    
    /// <summary>
    /// Visualizes detected pupils on the image for debugging.
    /// </summary>
    public static Mat VisualizePupils(Mat img, List<PupilInfo> pupils)
    {
        Mat vis = img.Clone();
        
        for (int i = 0; i < pupils.Count; i++)
        {
            var pupil = pupils[i];
            
            // Scale down radius since minEnclosingCircle is larger than actual pupil
            int radiusDraw = (int)(pupil.Radius * 0.7f);
            
            // Draw circle around pupil
            Cv2.Circle(vis, new OpenCvSharp.Point(pupil.X, pupil.Y), radiusDraw, new Scalar(0, 255, 0), 2);
            // Draw center point
            Cv2.Circle(vis, new OpenCvSharp.Point(pupil.X, pupil.Y), 3, new Scalar(0, 255, 0), -1);
            
            // Label left/right
            string label = i == 0 ? "L" : "R";
            Cv2.PutText(vis, label, 
                new OpenCvSharp.Point(pupil.X - 10, pupil.Y - radiusDraw - 10),
                HersheyFonts.HersheySimplex, 0.6, new Scalar(0, 255, 0), 2);
        }
        
        return vis;
    }
    
    /// <summary>
    /// Saves a visualization of detected pupils to a file.
    /// </summary>
    public static void SaveVisualization(Mat img, List<PupilInfo> pupils, string outputPath)
    {
        using Mat vis = VisualizePupils(img, pupils);
        Cv2.ImWrite(outputPath, vis);
        GD.Print($"PupilDetector: Saved visualization to {outputPath}");
    }
}