using OpenCvSharp;
using Godot;
using ETPreferences;
using System;

public partial class DebugViewer : EyeTrackingConsumer
{
    // One Euro Filters for smoothing eye tracking data
    private OneEuroFilter _leftEyeXFilter;
    private OneEuroFilter _leftEyeYFilter;
    private OneEuroFilter _rightEyeXFilter;
    private OneEuroFilter _rightEyeYFilter;

    // Filter configuration
    [Export]
    public float MinCutoff { get; set; } = 1.0f; // Minimum cutoff frequency
    [Export]
    public float Beta { get; set; } = ET_Preferences.filter_default; // Speed coefficient
    [Export]
    public float DCutoff { get; set; } = 1.0f; // Derivative cutoff frequency

    /// <summary>
    /// Initializes a new instance of the DebugViewer class
    /// </summary>
    /// <param name="name">The name of this consumer</param>
    public DebugViewer(string name = "Debug Viewer") : base(name)
    {
        InitializeFilters();
    }

    /// <summary>
    /// Initialize filter instances
    /// </summary>
    public void InitializeFilters()
    {
        // Set coefficient from user preference
        if (ET_Preferences.Instance != null && float.TryParse(ET_Preferences.Instance.smoothing_intensity, out float parsed))
        {
            Beta = Math.Clamp(parsed, 0, 1);
        }

        _leftEyeXFilter = new OneEuroFilter(MinCutoff, Beta, DCutoff);
        _leftEyeYFilter = new OneEuroFilter(MinCutoff, Beta, DCutoff);
        _rightEyeXFilter = new OneEuroFilter(MinCutoff, Beta, DCutoff);
        _rightEyeYFilter = new OneEuroFilter(MinCutoff, Beta, DCutoff);
    }


    public override void OnEyeDataUpdate(EyeTrackingData data)
    {
        base.OnEyeDataUpdate(data);

        // Close the viewers if they are active
        if (!Enabled)
        {
            if (Cv2.GetWindowProperty("Left Eye", WindowPropertyFlags.Visible) >= 1)
            {
                Cv2.DestroyWindow("Left Eye");
            }

            if (Cv2.GetWindowProperty("Right Eye", WindowPropertyFlags.Visible) >= 1)
            {
                Cv2.DestroyWindow("Right Eye");
            }
        }
    }

    protected override void ProcessData(EyeTrackingData data)
    {
        // Get current time for filtering
        float currentTime = (float)Time.GetTicksMsec() / 1000.0f;

        // User preference for smoothing filter
        bool filterDisabled = ET_Preferences.Instance != null && !ET_Preferences.Instance.smoothing_enabled;

        BlinkMode blinkMode = EyeTrackingPreferences.GetBlinkMode();
        bool isBlinkFrame = data.HasAnyFlagCondition();
        bool skipFilterForBlink = (blinkMode == BlinkMode.Blinking || blinkMode == BlinkMode.Winking) && isBlinkFrame;

        // Apply one euro filter to smooth eye positions
        float leftX = (filterDisabled || skipFilterForBlink) ? data.LeftEyeX : _leftEyeXFilter.Filter(data.LeftEyeX, currentTime);
        float leftY = (filterDisabled || skipFilterForBlink) ? data.LeftEyeY : _leftEyeYFilter.Filter(data.LeftEyeY, currentTime);
        float rightX = (filterDisabled || skipFilterForBlink) ? data.RightEyeX : _rightEyeXFilter.Filter(data.RightEyeX, currentTime);
        float rightY = (filterDisabled || skipFilterForBlink) ? data.RightEyeY : _rightEyeYFilter.Filter(data.RightEyeY, currentTime);

        if (data.LeftEyeImage != null)
        {
            Mat leftEyeWithOverlay = new Mat();
            Cv2.Flip(data.LeftEyeImage, leftEyeWithOverlay, FlipMode.Y);
            DrawGrid(leftEyeWithOverlay);
            
            // Draw raw eye position (white)
            DrawEyePosition(leftEyeWithOverlay, data.LeftEyeX, data.LeftEyeY, new Scalar(100, 100, 100), "Raw");
            
            // Draw filtered eye position (green)
            DrawEyePosition(leftEyeWithOverlay, leftX, leftY, new Scalar(255, 255, 255), "Filtered", new System.Numerics.Vector2(0, -15));
            
            Cv2.ImShow("Left Eye", leftEyeWithOverlay);
        }

        if (data.RightEyeImage != null)
        {
            Mat rightEyeWithOverlay = new Mat();
            Cv2.Flip(data.RightEyeImage, rightEyeWithOverlay, FlipMode.Y);
            DrawGrid(rightEyeWithOverlay);
            
            // Draw raw eye position (white)
            DrawEyePosition(rightEyeWithOverlay, data.RightEyeX, data.RightEyeY, new Scalar(100, 100, 100), "Raw");
            
            // Draw filtered eye position (green)
            DrawEyePosition(rightEyeWithOverlay, rightX, rightY, new Scalar(255, 255, 255), "Filtered", new System.Numerics.Vector2(0, -15));
            
            Cv2.ImShow("Right Eye", rightEyeWithOverlay);
        }

        Cv2.WaitKey(1); // Required to update window
    }

    private void DrawGrid(Mat image)
    {
        int height = image.Height;
        int width = image.Width;

        // Draw vertical lines
        for (int i = 0; i < 5; i++)
        {
            int x = (int)(width * i / 4.0);
            Cv2.Line(image, new Point(x, 0), new Point(x, height), new Scalar(128, 128, 128), 1);

            // Draw coordinate
            float coord = (float)i / 4.0f;
            Cv2.PutText(
                image,
                $"{coord:F1}",
                new Point(x + 5, 20),
                HersheyFonts.HersheySimplex,
                0.5,
                new Scalar(255, 255, 255),
                1
            );
        }

        // Draw horizontal lines
        for (int i = 0; i < 5; i++)
        {
            int y = (int)(height * i / 4.0);
            Cv2.Line(image, new Point(0, y), new Point(width, y), new Scalar(128, 128, 128), 1);

            // Draw coordinate
            float coord = (float)i / 4.0f;
            Cv2.PutText(
                image,
                $"{coord:F1}",
                new Point(5, y + 20),
                HersheyFonts.HersheySimplex,
                0.5,
                new Scalar(255, 255, 255),
                1
            );
        }
    }

    private void DrawEyePosition(Mat image, float eyeX, float eyeY, Scalar color, string label, System.Numerics.Vector2 labelOffset = new System.Numerics.Vector2())
    {
        int height = image.Height;
        int width = image.Width;

        // Convert normalized coordinates to pixel coordinates
        int pixelX = (int)(eyeX * width);
        int pixelY = (int)((1 - eyeY) * height); // Flip Y coordinate (0 at top in image)

        // Draw circle at eye position
        Cv2.Circle(image, new Point(pixelX, pixelY), 5, color, -1);

        // Draw coordinate values with label
        Cv2.PutText(
            image,
            $"{label}: ({eyeX:F2}, {eyeY:F2})",
            new Point(pixelX + 10 + labelOffset.X, pixelY + labelOffset.Y),
            HersheyFonts.HersheySimplex,
            0.5,
            color,
            1
        );
    }
}