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

/// <summary>
/// Test consumer that exposes both raw and filtered gaze data for I-VT testing
/// This consumer runs the same processing as MemmapConsumer but exposes both
/// raw and event-driven filtered gaze vectors for visualization.
/// </summary>
public partial class IVTTestConsumer : EyeTrackingConsumer
{
    // Event signals to notify the test scene about gaze updates
    [Signal]
    public delegate void RawGazeUpdatedEventHandler(Vector3 leftVector, Vector3 rightVector, Vector3 combinedVector);
    
    [Signal]
    public delegate void FilteredGazeUpdatedEventHandler(Vector3 combinedVector, bool inSaccade);
    

    // Event-based foveation controller state
    private bool _inSaccade = false;
    private long _resumeUntilMs = 0;
    private const float V_START = 125f;   // deg/s saccade start
    private const float V_END   = 50f;    // deg/s saccade end
    private const float V_BLINK_SPIKE = 200f; // deg/s spike for blink/noise
    private const float BINOCULAR_MAX_DEG = 45f; // max left-right divergence
    private const float DEADZONE_DEG = 0.35f;     // fixation dead zone
    private const float MICRO_NUDGE_ALPHA = 0.25f;// small nudge EMA
    private const int SACCADE_CONFIRMATION_SAMPLES = 2; // consecutive samples needed to confirm saccade start/end
    private const long RESUME_GUARD_MS = 90;      // duration to freeze gaze updates after saccade end or blink spike
    private int _hiCnt = 0, _loCnt = 0;
    
    // Sound toggle constants
    private const bool PLAY_BINO_SOUND = true;      // Play sound when binocular check fails
    private const bool PLAY_BLINK_SOUND = false;     // Play sound when eyes close/open
    private const bool PLAY_GUARD_SOUND = false;      // Play sound when resume guard is triggered
    private const bool PLAY_NOISE_SOUND = true;     // Play sound when chaotic burst + velocity detected
    private const bool PLAY_SACCADE_SOUND = false;   // Play sound when saccade starts/ends

    // 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> _filteredGazeHistory = new System.Collections.Generic.Queue<Vector3>(); // History of filtered gaze positions

    protected FrustumLoader _frustumLoader;
    private System.Collections.Generic.Queue<GazeVector> _gazeHistory = new System.Collections.Generic.Queue<GazeVector>();
    
    // Audio players for IVT event sounds
    private AudioStreamPlayer _binoSoundPlayer;
    private AudioStreamPlayer _blinkSoundPlayer;
    private AudioStreamPlayer _guardSoundPlayer;
    private AudioStreamPlayer _noiseSoundPlayer;
    private AudioStreamPlayer _saccadeSoundPlayer;
    
    // Cooldown tracking to prevent sound spam (milliseconds)
    private Dictionary<string, long> _soundCooldowns = new Dictionary<string, long>();
    private const long SOUND_COOLDOWN_MS = 200; // 200ms cooldown between same sound type

    // Head-stabilized coordinate system support
    [Export]
    public XRCamera3D XrCamera { get; set; }
    private Transform3D _lastHeadTransform = Transform3D.Identity;
    private bool _headTransformInitialized = false;
    private Vector3 _foveaTargetWorldSpace = Vector3.Zero; // Store fovea target in world space
    
    // Head motion compensation: track head transform history to calculate head angular velocity
    private System.Collections.Generic.Queue<(Transform3D transform, long timestampMs)> _headTransformHistory = 
        new System.Collections.Generic.Queue<(Transform3D, long)>();
    private const int HEAD_HISTORY_SIZE = 10; // Keep last 10 head transforms for velocity calculation
    
    // Recording functionality for debugging
    private bool _isRecording = false;
    private List<FrameRecordingData> _recordingData = new List<FrameRecordingData>();
    public const int MAX_RECORDING_FRAMES = 2000;
    
    /// <summary>
    /// Data structure to hold per-frame recording data
    /// </summary>
    private struct FrameRecordingData
    {
        public long TimestampMs;
        public Vector3 HeadPosition;
        public Quaternion HeadRotation;
        public Vector3 RawGazeVector;
        public Vector3 FilteredGazeVector;
        public string Event; // Event type: NONE, SACCADE, BLINK, NOISE, BINO
    }

    public IVTTestConsumer(string name = "IVT Test 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.");
        }
        
        // Initialize audio players for IVT event sounds
        InitializeAudioPlayer(ref _binoSoundPlayer, "res://IVT/bino.wav", "bino");
        InitializeAudioPlayer(ref _blinkSoundPlayer, "res://IVT/blink.wav", "blink");
        InitializeAudioPlayer(ref _guardSoundPlayer, "res://IVT/guard.wav", "guard");
        InitializeAudioPlayer(ref _noiseSoundPlayer, "res://IVT/noise.wav", "noise");
        InitializeAudioPlayer(ref _saccadeSoundPlayer, "res://IVT/saccade.wav", "saccade");
    }
    
    protected override void ProcessData(EyeTrackingData data)
    {
        if (!_frustumLoader.IsLoaded)
        {
            GD.PrintErr("IVTTestConsumer: Frustum data not loaded!");
            return;
        }

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

            // 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();

            // Emit raw gaze data (deferred to main thread for thread safety)
            CallDeferred(nameof(EmitRawGazeSignal), leftVector, rightVector, combinedVector);

            // === Event-driven saccade controller ===
            long nowMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();

            // Get current head transform for world-space conversion
            Transform3D currentHeadTransform = XrCamera.GlobalTransform;
            
            // Track head transform history for motion compensation
            _headTransformHistory.Enqueue((currentHeadTransform, nowMs));
            while (_headTransformHistory.Count > HEAD_HISTORY_SIZE) _headTransformHistory.Dequeue();
            
            // Transform view-space vectors to world space for velocity calculation
            Vector3 leftVectorWorld = TransformToWorldSpace(leftVector, currentHeadTransform);
            Vector3 rightVectorWorld = TransformToWorldSpace(rightVector, currentHeadTransform);
            Vector3 combinedVectorWorld = TransformToWorldSpace(combinedVector, currentHeadTransform);

            // Build and enqueue gaze point (store view-space vectors and head transform for emission)
            GazeVector newPt = new GazeVector(leftVector, rightVector, combinedVector, 1.0f, nowMs, currentHeadTransform.Basis);
            _gazeHistory.Enqueue(newPt);
            while (_gazeHistory.Count > 256) _gazeHistory.Dequeue();

            // Compute short-window velocity in world space
            // Use the head transform that existed when each gaze point was captured to correctly
            // separate head motion from eye motion.
            float vGaze = CalculateGazeAngularVelocity(combinedVectorWorld, nowMs);
            
            // Calculate head angular velocity and compensate gaze velocity
            // This prevents head motion from being misidentified as eye saccades
            float vHead = CalculateHeadAngularVelocity(currentHeadTransform, nowMs);
            
            // Compensate gaze velocity by subtracting head motion to get pure eye velocity
            float v = Math.Max(0f, vGaze - vHead);

            // Blink/noise heuristics without vendor confidence
            // Use world-space vectors for binocular check
            bool binoOk = DegBetween(leftVectorWorld, rightVectorWorld) <= BINOCULAR_MAX_DEG || eyesCurrentlyClosed;
            bool chaotic = IsChaoticBurstWorldSpace(new System.Collections.Generic.List<GazeVector>(_gazeHistory));
            bool blinkSpike = v > V_BLINK_SPIKE && chaotic;

            // Play sounds for detected events
            if (!binoOk && PLAY_BINO_SOUND)
            {
                PlaySoundWithCooldown(_binoSoundPlayer, "bino", nowMs);
            }
            
            if (blinkSpike && PLAY_NOISE_SOUND)
            {
                PlaySoundWithCooldown(_noiseSoundPlayer, "noise", nowMs);
            }

            if (blinkSpike || !binoOk)
            {
                _resumeUntilMs = Math.Max(_resumeUntilMs, nowMs + RESUME_GUARD_MS);
                // Play guard sound when resume guard is triggered
                if (PLAY_GUARD_SOUND)
                {
                    PlaySoundWithCooldown(_guardSoundPlayer, "guard", nowMs);
                }
            }
            else
            {
                // Saccade state machine with 2-sample confirmation
                if (v > V_START) { _hiCnt++; _loCnt = 0; }
                else if (v < V_END) { _loCnt++; _hiCnt = 0; }
                else { _hiCnt = 0; _loCnt = 0; }

                if (!_inSaccade && _hiCnt >= SACCADE_CONFIRMATION_SAMPLES)
                {
                    _inSaccade = true;
                    // Play saccade sound when saccade starts
                    if (PLAY_SACCADE_SOUND)
                    {
                        PlaySoundWithCooldown(_saccadeSoundPlayer, "saccade", nowMs);
                    }
                }
                if (_inSaccade && _loCnt >= SACCADE_CONFIRMATION_SAMPLES)
                {
                    _inSaccade = false;
                    // Use current gaze position as fovea target (micro-nudge will handle convergence)
                    _foveaTargetWorldSpace = combinedVectorWorld.Normalized();
                    _resumeUntilMs = Math.Max(_resumeUntilMs, nowMs + RESUME_GUARD_MS);
                    // Play guard sound when resume guard is triggered after saccade end
                    if (PLAY_GUARD_SOUND)
                    {
                        PlaySoundWithCooldown(_guardSoundPlayer, "guard", nowMs);
                    }
                }
                if (_foveaTargetWorldSpace == Vector3.Zero) _foveaTargetWorldSpace = combinedVectorWorld.Normalized();
                // Fixation micro-nudge with dead zone (in world space)
                if (!_inSaccade && nowMs > _resumeUntilMs)
                {
                    float d = DegBetween(_foveaTargetWorldSpace, combinedVectorWorld);
                    if (d > DEADZONE_DEG)
                    {
                        // Use adaptive alpha: faster convergence for larger errors
                        // For small errors (< 1°): use slow micro-nudge
                        // For large errors (>= 1°): use faster convergence
                        float adaptiveAlpha = d >= 1.0f ? 0.75f : MICRO_NUDGE_ALPHA;
                        _foveaTargetWorldSpace = (_foveaTargetWorldSpace * (1f - adaptiveAlpha) + combinedVectorWorld * adaptiveAlpha).Normalized();
                    }
                }
            }

            // Publish fovea target, freeze during resume guard
            // Use fovea target if available, otherwise fallback to current gaze
            Vector3 publishCombinedWorld = (_foveaTargetWorldSpace != Vector3.Zero) ? _foveaTargetWorldSpace : combinedVectorWorld;
            
            // Transform back to view space
            Vector3 publishCombined = TransformToViewSpace(publishCombinedWorld, currentHeadTransform);
            
            // 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)
                {
                    publishCombined = _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 = publishCombined;
                }
            }
            else
            {
                // Eyes are open and freeze frames expired - resume normal processing
                // Add current filtered position to history
                _filteredGazeHistory.Enqueue(publishCombined);
                while (_filteredGazeHistory.Count > FREEZE_FRAME_COUNT + 5) // Keep a bit more than needed
                {
                    _filteredGazeHistory.Dequeue();
                }
            }

            // Determine event type for this frame (priority: BLINK > NOISE > BINO > SACCADE)
            string frameEvent = "NONE";
            if (eyesCurrentlyClosed)
            {
                frameEvent = "BLINK";
            }
            else if (blinkSpike)
            {
                frameEvent = "NOISE";
            }
            else if (!binoOk)
            {
                frameEvent = "BINO";
            }
            else if (_inSaccade)
            {
                frameEvent = "SACCADE";
            }

            // Record frame data if recording is enabled (after publishCombined is set)
            if (_isRecording && _recordingData.Count < MAX_RECORDING_FRAMES)
            {
                RecordFrameData(nowMs, currentHeadTransform, combinedVector, publishCombined, frameEvent);
            }

            // Emit filtered gaze data (deferred to main thread for thread safety)
            CallDeferred(nameof(EmitFilteredGazeSignal), publishCombined, _inSaccade);
            
            // Update head transform for next frame
            _lastHeadTransform = currentHeadTransform;
            _headTransformInitialized = true;
            
            // Stop recording automatically when max frames reached
            if (_isRecording && _recordingData.Count >= MAX_RECORDING_FRAMES)
            {
                _isRecording = false;
                CallDeferred(nameof(SaveRecordingToCSV));
            }
        }
        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);
    }

    private static float DegBetween(Vector3 a, Vector3 b)
    {
        float d = Math.Clamp(a.Dot(b), -1f, 1f);
        return (float)(Math.Acos(d) * 180.0 / Math.PI);
    }

    private float CalculateAngularVelocityWorldSpace(Vector3 worldVector1, Vector3 worldVector2, long timestamp1, long timestamp2)
    {
        float timeDiffSeconds = (timestamp2 - timestamp1) / 1000.0f;
        if (timeDiffSeconds <= 0) return 0;
        
        float dotProduct = worldVector1.Dot(worldVector2);
        dotProduct = Math.Clamp(dotProduct, -1.0f, 1.0f);
        
        float angleRadians = (float)Math.Acos(dotProduct);
        float angleDegrees = angleRadians * 180.0f / (float)Math.PI;
        
        return angleDegrees / timeDiffSeconds;
    }

    /// <summary>
    /// Calculate head angular velocity using history-based calculation with frame-to-frame responsiveness
    /// </summary>
    private float CalculateHeadAngularVelocity(Transform3D currentHeadTransform, long currentTimestampMs)
    {
        float historyVelocity = 0f;
        
        if (_headTransformHistory.Count >= 2)
        {
            var headArr = _headTransformHistory.ToArray();
            
            // Use forward direction (-Z basis vector) to measure head rotation
            // Calculate velocity over the most recent samples (similar to gaze velocity calculation)
            if (headArr.Length >= 3)
            {
                Vector3 forward2 = -headArr[^3].transform.Basis.Z;
                Vector3 forward1 = -headArr[^2].transform.Basis.Z;
                Vector3 forward0 = -headArr[^1].transform.Basis.Z;
                
                float v1 = CalculateAngularVelocityWorldSpace(forward2, forward1, headArr[^3].timestampMs, headArr[^2].timestampMs);
                float v2 = CalculateAngularVelocityWorldSpace(forward1, forward0, headArr[^2].timestampMs, headArr[^1].timestampMs);
                historyVelocity = 0.5f * (v1 + v2);
            }
            else if (headArr.Length >= 2)
            {
                Vector3 forward1 = -headArr[^2].transform.Basis.Z;
                Vector3 forward0 = -headArr[^1].transform.Basis.Z;
                historyVelocity = CalculateAngularVelocityWorldSpace(forward1, forward0, headArr[^2].timestampMs, headArr[^1].timestampMs);
            }
        }
        
        // Improve responsiveness with frame-to-frame check for more responsive detection
        float frameVelocity = 0f;
        if (_headTransformInitialized && _lastHeadTransform != Transform3D.Identity && currentHeadTransform != Transform3D.Identity)
        {
            // Calculate angular change in head forward direction since last frame
            Vector3 lastForward = -_lastHeadTransform.Basis.Z;
            Vector3 currentForward = -currentHeadTransform.Basis.Z;
            float headChangeDeg = DegBetween(lastForward, currentForward);
            
            // Calculate velocity from frame-to-frame change using actual timestamps
            float timeDiffSeconds = 0.016f; // Default to ~60fps
            if (_headTransformHistory.Count >= 2)
            {
                var headArr = _headTransformHistory.ToArray();
                long lastTimestampMs = headArr[^2].timestampMs;
                long latestTimestampMs = headArr[^1].timestampMs;
                long timeDiffMs = latestTimestampMs - lastTimestampMs;
                if (timeDiffMs > 0)
                {
                    timeDiffSeconds = timeDiffMs / 1000.0f;
                }
            }
            
            frameVelocity = headChangeDeg / Math.Max(0.001f, timeDiffSeconds);
        }
        
        // Use maximum of history-based and frame-to-frame velocity for responsiveness
        return Math.Max(historyVelocity, frameVelocity);
    }

    /// <summary>
    /// Calculate gaze angular velocity in world space using gaze history
    /// Uses the head transform that existed when each gaze point was captured to correctly
    /// separate head motion from eye motion.
    /// </summary>
    private float CalculateGazeAngularVelocity(Vector3 currentCombinedVectorWorld, long currentTimestampMs)
    {
        if (_gazeHistory.Count < 2) return 0f;
        
        var arr = _gazeHistory.ToArray();
        
        if (_gazeHistory.Count >= 3)
        {
            // Transform previous points to world space using their original head transforms
            Transform3D prev2Transform = new Transform3D(arr[^3].HeadTransformBasis, Vector3.Zero);
            Transform3D prev1Transform = new Transform3D(arr[^2].HeadTransformBasis, Vector3.Zero);
            Vector3 prev2World = TransformToWorldSpace(arr[^3].CombinedVector, prev2Transform);
            Vector3 prev1World = TransformToWorldSpace(arr[^2].CombinedVector, prev1Transform);
            Vector3 currWorld = currentCombinedVectorWorld;
            
            float v1 = CalculateAngularVelocityWorldSpace(prev2World, prev1World, arr[^3].TimestampMs, arr[^2].TimestampMs);
            float v2 = CalculateAngularVelocityWorldSpace(prev1World, currWorld, arr[^2].TimestampMs, currentTimestampMs);
            return 0.5f * (v1 + v2);
        }
        else if (_gazeHistory.Count >= 2)
        {
            Transform3D prevTransform = new Transform3D(arr[^2].HeadTransformBasis, Vector3.Zero);
            Vector3 prevWorld = TransformToWorldSpace(arr[^2].CombinedVector, prevTransform);
            Vector3 currWorld = currentCombinedVectorWorld;
            return CalculateAngularVelocityWorldSpace(prevWorld, currWorld, arr[^2].TimestampMs, currentTimestampMs);
        }
        
        return 0f;
    }

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

            // Play ding sound when eyes close
            CallDeferred(nameof(PlayBlinkSound));
        }
        
        // 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;
            
            // Play blink sound when eyes open
            CallDeferred(nameof(PlayBlinkSound));
        }
        
        // Update eye closed state
        _eyesClosed = eyesCurrentlyClosed;
        
        // Decrement freeze frame counter if active
        if (_freezeFrameCounter > 0)
        {
            _freezeFrameCounter--;
        }
    }

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

    /// <summary>
    /// Check for chaotic burst in world space using stored head transforms
    /// </summary>
    private bool IsChaoticBurstWorldSpace(System.Collections.Generic.List<GazeVector> hist)
    {
        if (hist == null || hist.Count < 4) return false;
        
        // Transform to world space using each point's stored head transform
        Transform3D t1 = new Transform3D(hist[^1].HeadTransformBasis, Vector3.Zero);
        Transform3D t2 = new Transform3D(hist[^2].HeadTransformBasis, Vector3.Zero);
        Transform3D t3 = new Transform3D(hist[^3].HeadTransformBasis, Vector3.Zero);
        Transform3D t4 = new Transform3D(hist[^4].HeadTransformBasis, Vector3.Zero);
        
        Vector3 w1 = TransformToWorldSpace(hist[^1].CombinedVector, t1);
        Vector3 w2 = TransformToWorldSpace(hist[^2].CombinedVector, t2);
        Vector3 w3 = TransformToWorldSpace(hist[^3].CombinedVector, t3);
        Vector3 w4 = TransformToWorldSpace(hist[^4].CombinedVector, t4);
        
        Vector3 d1 = w1 - w2;
        Vector3 d2 = w2 - w3;
        Vector3 d3 = w3 - w4;
        int flips = 0;
        if (d1.Dot(d2) < 0) flips++;
        if (d2.Dot(d3) < 0) flips++;
        return flips >= 2;
    }

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

    private void EmitFilteredGazeSignal(Vector3 combinedVector, bool inSaccade)
    {
        EmitSignal(SignalName.FilteredGazeUpdated, combinedVector, inSaccade);
    }

    /// <summary>
    /// Initialize an audio player and load the sound file
    /// </summary>
    private void InitializeAudioPlayer(ref AudioStreamPlayer player, string soundPath, string soundName)
    {
        player = new AudioStreamPlayer();
        AddChild(player);
        
        var audioStream = GD.Load<AudioStream>(soundPath);
        if (audioStream != null)
        {
            player.Stream = audioStream;
        }
        else
        {
            GD.PrintErr($"Failed to load {soundPath} audio file.");
        }
    }
    
    /// <summary>
    /// Play a sound with cooldown to prevent spam
    /// This method should be called from ProcessData (may be on background thread)
    /// </summary>
    private void PlaySoundWithCooldown(AudioStreamPlayer player, string soundName, long currentTimeMs)
    {
        if (player == null || player.Stream == null)
        {
            return;
        }
        
        // Check cooldown
        if (_soundCooldowns.ContainsKey(soundName))
        {
            long lastPlayTime = _soundCooldowns[soundName];
            if (currentTimeMs - lastPlayTime < SOUND_COOLDOWN_MS)
            {
                return; // Still in cooldown, don't play
            }
        }
        
        // Update cooldown timestamp
        _soundCooldowns[soundName] = currentTimeMs;
        
        // Play sound (deferred to main thread for thread safety)
        CallDeferred(nameof(PlaySoundDeferred), player);
    }
    
    /// <summary>
    /// Deferred method to play sound on main thread
    /// </summary>
    private void PlaySoundDeferred(AudioStreamPlayer player)
    {
        if (player != null && player.Stream != null)
        {
            player.Play();
        }
    }
    
    /// <summary>
    /// Play blink sound when eyes close or open (with cooldown)
    /// This method is called deferred, so we can check cooldown and play directly
    /// </summary>
    private void PlayBlinkSound()
    {
        if (!PLAY_BLINK_SOUND)
        {
            return;
        }
        
        long nowMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
        
        if (_blinkSoundPlayer == null || _blinkSoundPlayer.Stream == null)
        {
            return;
        }
        
        // Check cooldown
        if (_soundCooldowns.ContainsKey("blink"))
        {
            long lastPlayTime = _soundCooldowns["blink"];
            if (nowMs - lastPlayTime < SOUND_COOLDOWN_MS)
            {
                return; // Still in cooldown, don't play
            }
        }
        
        // Update cooldown timestamp
        _soundCooldowns["blink"] = nowMs;
        
        // Play sound directly (already on main thread)
        _blinkSoundPlayer.Play();
    }

    /// <summary>
    /// Start recording frame data to CSV
    /// </summary>
    public void StartRecording()
    {
        _isRecording = true;
        _recordingData.Clear();
        GD.Print($"{Name}: Started recording (max {MAX_RECORDING_FRAMES} frames)");
    }

    /// <summary>
    /// Stop recording and save data to CSV
    /// </summary>
    public void StopRecording()
    {
        if (_isRecording)
        {
            _isRecording = false;
            SaveRecordingToCSV();
        }
    }

    /// <summary>
    /// Check if currently recording
    /// </summary>
    public bool IsRecording => _isRecording;

    /// <summary>
    /// Get the number of frames recorded so far
    /// </summary>
    public int RecordedFrameCount => _recordingData.Count;

    /// <summary>
    /// Record frame data for CSV export
    /// </summary>
    private void RecordFrameData(long timestampMs, Transform3D headTransform, Vector3 rawGaze, Vector3 filteredGaze, string frameEvent)
    {
        var frameData = new FrameRecordingData
        {
            TimestampMs = timestampMs,
            HeadPosition = headTransform.Origin,
            HeadRotation = headTransform.Basis.GetRotationQuaternion(),
            RawGazeVector = rawGaze,
            FilteredGazeVector = filteredGaze,
            Event = frameEvent
        };
        
        _recordingData.Add(frameData);
    }

    /// <summary>
    /// Save recorded data to CSV file
    /// </summary>
    private void SaveRecordingToCSV()
    {
        if (_recordingData.Count == 0)
        {
            GD.Print($"{Name}: No data to save");
            return;
        }

        try
        {
            string timestamp = DateTime.Now.ToString("yyyy-MM-dd_HH-mm-ss");
            string fileName = $"ivt_recording_{timestamp}.csv";
            string baseDir = "recorded_data";
            
            // Create directory if it doesn't exist
            if (!Directory.Exists(baseDir))
            {
                Directory.CreateDirectory(baseDir);
            }
            
            string filePath = Path.Combine(baseDir, fileName);
            
            using (var writer = new StreamWriter(filePath))
            {
                // Write CSV header
                writer.WriteLine("timestamp_ms,head_pos_x,head_pos_y,head_pos_z,head_rot_x,head_rot_y,head_rot_z,head_rot_w,raw_gaze_x,raw_gaze_y,raw_gaze_z,filtered_gaze_x,filtered_gaze_y,filtered_gaze_z,event");
                
                // Write data rows
                foreach (var frame in _recordingData)
                {
                    writer.WriteLine(
                        $"{frame.TimestampMs}," +
                        $"{frame.HeadPosition.X.ToString("F6", CultureInfo.InvariantCulture)}," +
                        $"{frame.HeadPosition.Y.ToString("F6", CultureInfo.InvariantCulture)}," +
                        $"{frame.HeadPosition.Z.ToString("F6", CultureInfo.InvariantCulture)}," +
                        $"{frame.HeadRotation.X.ToString("F6", CultureInfo.InvariantCulture)}," +
                        $"{frame.HeadRotation.Y.ToString("F6", CultureInfo.InvariantCulture)}," +
                        $"{frame.HeadRotation.Z.ToString("F6", CultureInfo.InvariantCulture)}," +
                        $"{frame.HeadRotation.W.ToString("F6", CultureInfo.InvariantCulture)}," +
                        $"{frame.RawGazeVector.X.ToString("F6", CultureInfo.InvariantCulture)}," +
                        $"{frame.RawGazeVector.Y.ToString("F6", CultureInfo.InvariantCulture)}," +
                        $"{frame.RawGazeVector.Z.ToString("F6", CultureInfo.InvariantCulture)}," +
                        $"{frame.FilteredGazeVector.X.ToString("F6", CultureInfo.InvariantCulture)}," +
                        $"{frame.FilteredGazeVector.Y.ToString("F6", CultureInfo.InvariantCulture)}," +
                        $"{frame.FilteredGazeVector.Z.ToString("F6", CultureInfo.InvariantCulture)}," +
                        $"{frame.Event}"
                    );
                }
            }
            
            GD.Print($"{Name}: Saved {_recordingData.Count} frames to {filePath}");
            _recordingData.Clear();
        }
        catch (Exception ex)
        {
            GD.PrintErr($"{Name}: Error saving recording to CSV: {ex.Message}");
            GD.PrintErr(ex.StackTrace);
        }
    }
}

internal readonly struct GazeVector
{
    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 GazeVector(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;
    }
}