using Godot;
using System;
using System.Collections.Generic;
using System.IO;
using System.Globalization;
using ETPreferences;

/// <summary>
/// Provides the gaze data as a Vector3 coordinate for displaying the alignment cursor.
/// 
/// </summary>
public partial class AlignmentConsumer : EyeTrackingConsumer
{
    // Event signals to notify the test scene about gaze updates
    [Signal]
    public delegate void RawGazeUpdatedEventHandler(Vector3 leftVector, Vector3 rightVector, Vector3 combinedVector);

    // Eye closure detection and frozen position
    private bool _eyesClosed = false;
    private Vector3 _frozenGazePosition = Vector3.Zero; // Frozen position in view space
    private const int FREEZE_FRAME_COUNT = 10; // Frames to look back when eyes close, and frames to persist after eyes reopen (default 27 frames)
    private int _freezeFrameCounter = 0; // Counter for frames to persist frozen position after eyes open
    private System.Collections.Generic.Queue<Vector3> _rawGazeHistory = new System.Collections.Generic.Queue<Vector3>(); // History of filtered gaze positions

    protected FrustumLoader _frustumLoader;

    // 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

    // Head-stabilized coordinate system support
    [Export]
    public XRCamera3D XrCamera { get; set; }

    public AlignmentConsumer(string name = "Alignment Consumer") : base(name)
    {
        // Initialize frustum loader
        _frustumLoader = new FrustumLoader();
        if (!_frustumLoader.LoadFrustumData())
        {
            GD.PrintErr("Failed to load frustum data. Eye tracking will not function without frustum data.");
            throw new InvalidOperationException("Frustum data is required for IVTTestConsumer to function properly.");
        }

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

    protected override void ProcessData(EyeTrackingData data)
    {
        if (!_frustumLoader.IsLoaded)
        {
            GD.PrintErr("IVTTestConsumer: Frustum data not loaded!");
            return;
        }

        try
        {
            // Get current time for filtering
            float currentTime = (float)Time.GetTicksMsec() / 1000.0f;

            float leftX = data.LeftEyeX;
            float leftY = data.LeftEyeY;
            float rightX = data.RightEyeX;
            float rightY = data.RightEyeY;

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

            // If right eye data is invalid (0,0), use left eye data
            bool singleEyeTracking = rightX == 0 && rightY == 0;
            if (singleEyeTracking)
            {
                rightX = leftX;
                rightY = leftY;
            }

            // Convert eye positions to pitch and yaw
            Vector2 leftAngles = _frustumLoader.ScreenPositionToViewAngles(new Vector2(leftX, leftY), true);
            Vector2 rightAngles = _frustumLoader.ScreenPositionToViewAngles(new Vector2(rightX, rightY), false);

            // Check for blink/eye closure using flag conditions
            bool eyesCurrentlyClosed = data.HasAnyFlagCondition();
            UpdateEyeClosureState(eyesCurrentlyClosed);

            // Extract pitch (vertical) and yaw (horizontal) angles
            float leftYaw = leftAngles.X;
            float leftPitch = leftAngles.Y;
            float rightYaw = rightAngles.X;
            float rightPitch = rightAngles.Y;

            // Convert angles to vectors
            Vector3 leftVector = AnglesToVector(leftPitch, leftYaw);
            Vector3 rightVector = AnglesToVector(rightPitch, rightYaw);

            // Calculate combined gaze direction (average of both eyes)
            Vector3 combinedVector = new Vector3(
                (leftVector.X + rightVector.X) / 2,
                (leftVector.Y + rightVector.Y) / 2,
                (leftVector.Z + rightVector.Z) / 2).Normalized();

            // If eyes are closed OR freeze frames are active, use frozen position instead of current filtered gaze
            if (_eyesClosed || _freezeFrameCounter > 0)
            {
                if (_frozenGazePosition != Vector3.Zero)
                {
                    combinedVector = _frozenGazePosition;
                }
                else
                {
                    // Fallback: if we don't have a frozen position yet, use current filtered gaze position
                    // (This can happen if eyes close before we have enough history)
                    _frozenGazePosition = combinedVector;
                }
            }
            else
            {
                // Eyes are open and freeze frames expired - resume normal processing
                // Add current filtered position to history
                _rawGazeHistory.Enqueue(combinedVector);
                while (_rawGazeHistory.Count > FREEZE_FRAME_COUNT + 5) // Keep a bit more than needed
                {
                    _rawGazeHistory.Dequeue();
                }
            }

            // Emit raw gaze data (deferred to main thread for thread safety)
            CallDeferred(nameof(EmitRawGazeSignal), leftVector, rightVector, combinedVector);
        }
        catch (Exception ex)
        {
            GD.PrintErr($"{Name}: Error processing data: {ex.Message}");
        }
    }

    private Vector3 AnglesToVector(float pitch, float yaw)
    {
        // Convert degrees to radians
        float pitchRad = (float)(pitch * Math.PI / 180.0);
        float yawRad = (float)(yaw * Math.PI / 180.0);

        // Calculate direction vector
        float x = (float)(Math.Sin(yawRad) * Math.Cos(pitchRad));
        float y = (float)Math.Sin(pitchRad);
        float z = (float)(Math.Cos(yawRad) * Math.Cos(pitchRad));

        return new Vector3(x, y, -z);
    }

    /// <summary>
    /// Update eye closure state and handle transitions (closure, opening, freeze counter)
    /// </summary>
    private void UpdateEyeClosureState(bool eyesCurrentlyClosed)
    {
        // Detect eye closure transition: when eyes just closed, capture frozen position from ~N frames ago
        if (eyesCurrentlyClosed && !_eyesClosed)
        {
            // Eyes just closed - capture filtered position from N frames ago
            _frozenGazePosition = GetFrozenGazePosition();

            // Reset freeze frame counter (will be set when eyes open)
            _freezeFrameCounter = 0;
        }

        // Detect eye opening transition: when eyes just opened, start freeze frame counter
        if (!eyesCurrentlyClosed && _eyesClosed)
        {
            // Eyes just opened - start freeze frame counter to persist frozen position
            _freezeFrameCounter = FREEZE_FRAME_COUNT;
        }

        // Update eye closed state
        _eyesClosed = eyesCurrentlyClosed;

        // Decrement freeze frame counter if active
        if (_freezeFrameCounter > 0)
        {
            _freezeFrameCounter--;

            if (_freezeFrameCounter <= 0)
            {
                _frozenGazePosition = Vector3.Zero;
            }
        }
    }

    /// <summary>
    /// Get frozen gaze position from history (N frames ago, or oldest available)
    /// </summary>
    private Vector3 GetFrozenGazePosition()
    {
        var gazeArray = _rawGazeHistory.ToArray();
        if (gazeArray.Length >= FREEZE_FRAME_COUNT)
        {
            // Get the filtered gaze position from N frames ago
            int frozenIndex = gazeArray.Length - FREEZE_FRAME_COUNT;
            return gazeArray[frozenIndex];
        }
        else if (gazeArray.Length > 0)
        {
            // If we don't have enough history, use the oldest available
            return gazeArray[0];
        }

        // If no history available, return Zero (will be set later)
        return Vector3.Zero;
    }

    /// <summary>
    /// Transform a view-space (head-relative) vector to world space
    /// </summary>
    private Vector3 TransformToWorldSpace(Vector3 viewSpaceVector, Transform3D headTransform)
    {
        // View-space vectors are relative to the head/camera
        // Transform to world space by multiplying with the head's basis matrix
        return headTransform.Basis * viewSpaceVector;
    }

    /// <summary>
    /// Transform a world-space vector back to view space (head-relative)
    /// </summary>
    private Vector3 TransformToViewSpace(Vector3 worldSpaceVector, Transform3D headTransform)
    {
        // Transform from world space to view space by multiplying with inverse basis
        return headTransform.Basis.Inverse() * worldSpaceVector;
    }

    private void EmitRawGazeSignal(Vector3 leftVector, Vector3 rightVector, Vector3 combinedVector)
    {
        EmitSignal(SignalName.RawGazeUpdated, leftVector, rightVector, combinedVector);
    }
}

internal readonly struct GazeInfo
{
    public Vector3 LeftVector { get; }
    public Vector3 RightVector { get; }
    public Vector3 CombinedVector { get; }
    public float Confidence { get; }
    public long TimestampMs { get; }
    public Basis HeadTransformBasis { get; } // Head transform basis at the time this gaze point was captured

    public GazeInfo(Vector3 leftVector, Vector3 rightVector, Vector3 combinedVector, float confidence, long timestampMs, Basis headTransformBasis = default)
    {
        LeftVector = leftVector;
        RightVector = rightVector;
        CombinedVector = combinedVector;
        Confidence = confidence;
        TimestampMs = timestampMs;
        // Default to identity basis if not provided (for view-space only consumers)
        HeadTransformBasis = headTransformBasis == default ? Basis.Identity : headTransformBasis;
    }
}