using Godot;
using System;
using System.Collections.Generic;
using System.IO.MemoryMappedFiles;
using System.Runtime.InteropServices;
using ETPreferences;
using EyeTracking;

namespace VRCFT
{

    /// <summary>
    /// Memory-mapped data structure to share with C++ code
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct SharedGazeData
    {
        public float LeftEyeX;
        public float LeftEyeY;
        public float LeftEyeZ;
        public float RightEyeX;
        public float RightEyeY;
        public float RightEyeZ;
        public float CombinedX;
        public float CombinedY;
        public float CombinedZ;
        public float Confidence;
        public long Timestamp;
        public int IsValid;
        public float LeftEyeClosedAmount;
        public float RightEyeClosedAmount;
    }

    /// <summary>
    /// Enum defining different data types for memory-mapped eye tracking data
    /// </summary>
    public enum MemmapDataTypes
    {
        EYES_VECTORS,
        EYES_ANGLES,
        DEBUG_INFO
    }

    /// <summary>
    /// Consumer for sending eye tracking data to a memory-mapped file for VRCFT interop
    /// </summary>
    public partial class VRCFTConsumer : EyeTrackingConsumer
    {
        private MemoryMappedFile _sharedMem;
        private MemoryMappedViewAccessor _accessor;
        private bool _running;
        private bool _debug;
        private const string SharedMemoryName = "VRCFTMemmapData";
        protected FrustumLoader _frustumLoader;

        // Buffer for eye tracking data - processes chunks of sequential data
        private EyeTrackingBuffer _dataBuffer;

        // One Euro Filters for smoothing eye tracking data
        private OneEuroFilter _leftEyeXFilter;
        private OneEuroFilter _leftEyeYFilter;
        private OneEuroFilter _rightEyeXFilter;
        private OneEuroFilter _rightEyeYFilter;
        
        // Track previous frame state for blink transition detection
        private bool _wasBlinking = false;
		// Minimal state to freeze around blinks
		private EyeTrackingData _lastStableFrame;
		private int _postBlinkHold = 0;
		
		// Eye openness lerping
		private BlinkLerp _blinkLerp;

        // 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
        
        [Export]
        public int BlinkSmoothingFrames { get; set; } = 3; // Number of frames to check before/after blink for smoothing

        /// <summary>
        /// Initializes a new instance of the VRCFTConsumer class
        /// </summary>
        /// <param name="name">The name of this consumer</param>
        public VRCFTConsumer(string name = "VRCFT 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 VRCFTConsumer to function properly.");
            }

            // Initialize data buffer with custom analysis function
            _dataBuffer = new EyeTrackingBuffer(
                maxBufferSize: 40,
                minBufferSize: 10,
                analyzeChunkData: AnalyzeChunkData,
                processData: ProcessData,
                canProcess: () => _running && _accessor != null
            );

            InitializeFilters();
            InitializeMemmap();
            
            // Initialize blink lerp
            _blinkLerp = new BlinkLerp(0.05f);
        }

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

        // Dispose of resources
        private void Cleanup()
        {
            if (_accessor != null)
            {
                try
                {
                    // Set data as invalid when stopping
                    var data = new SharedGazeData { IsValid = 0 };
                    _accessor.Write(0, ref data);
                    _accessor.Dispose();
                    _accessor = null;
                    GD.Print($"{Name}: View accessor closed");
                }
                catch (Exception ex)
                {
                    GD.PrintErr($"{Name}: Error closing view accessor: {ex.Message}");
                    GD.PrintErr(ex.StackTrace);
                }
            }

            if (_sharedMem != null)
            {
                try
                {
                    _sharedMem.Dispose();
                    _sharedMem = null;
                    GD.Print($"{Name}: Shared memory closed");
                }
                catch (Exception ex)
                {
                    GD.PrintErr($"{Name}: Error closing shared memory: {ex.Message}");
                    GD.PrintErr(ex.StackTrace);
                }
            }
        }

        /// <summary>
        /// Initialize the memory-mapped file
        /// </summary>
        private void InitializeMemmap()
        {
            // Any existing should be disposed
            Cleanup();

            try
            {
                // Create a memory-mapped file of the size of SharedGazeData
                _sharedMem = MemoryMappedFile.CreateOrOpen(
                    SharedMemoryName,
                    Marshal.SizeOf<SharedGazeData>(),
                    MemoryMappedFileAccess.ReadWrite);

                // Create a view accessor for the memory-mapped file
                _accessor = _sharedMem.CreateViewAccessor(0, Marshal.SizeOf<SharedGazeData>());

                // Initialize with zeros and mark as invalid
                var data = new SharedGazeData { IsValid = 0 };
                _accessor.Write(0, ref data);

                GD.Print($"{Name}: Successfully created shared memory");
                _running = true;
            }
            catch (Exception ex)
            {
                GD.PrintErr($"{Name}: Failed to create shared memory: {ex.Message}");
                GD.PrintErr(ex.StackTrace);
                _running = false;
            }
        }

        /// <summary>
        /// Process eye tracking data and send to the shared memory
        /// </summary>
        /// <param name="data">Eye tracking data</param>
        public override void OnEyeDataUpdate(EyeTrackingData data)
        {
            if (Enabled)
            {
                _dataBuffer.AddData(data);
            }
        }

		private EyeTrackingData AnalyzeChunkData(List<EyeTrackingData> chunkData)
		{
			var frame = chunkData.Count > 0 ? chunkData[0] : new EyeTrackingData();
			if (IsBlinkingFrame(frame)) { _postBlinkHold = BlinkSmoothingFrames; return frame; }
			if (_postBlinkHold > 0) { _postBlinkHold--; return _lastStableFrame; }
			_lastStableFrame = frame;
			return frame;
		}
        
        private bool IsBlinkingFrame(EyeTrackingData frame)
        {
            return frame.HasAnyFlagCondition();
        }

        /// <summary>
        /// Process eye tracking data and update shared memory
        /// </summary>
        /// <param name="data">Eye tracking data</param>
        protected override void ProcessData(EyeTrackingData data)
        {
            if (!_running || _accessor == null)
                return;

            if (!_frustumLoader.IsLoaded)
            {
                GD.PrintErr($"{Name}: Frustum data is not loaded. Cannot compute gaze without frustum data.");
                return;
            }

            try
            {
                // User preference for smoothing filter
                bool filterDisabled = ET_Preferences.Instance != null && !ET_Preferences.Instance.smoothing_enabled;
                BlinkMode blinkMode = EyeTrackingPreferences.GetBlinkMode();

                float currentTime = (float)Time.GetTicksMsec() / 1000.0f;

                // Base closed amounts from openness values
                float leftBaseClosedAmount = 1.0f - Math.Clamp(data.LeftEyeOpenness, 0f, 1f);
                float rightBaseClosedAmount = 1.0f - Math.Clamp(data.RightEyeOpenness, 0f, 1f);

                float leftX, leftY, rightX, rightY;
                float leftEyeClosedAmount = 0.0f;
                float rightEyeClosedAmount = 0.0f;

                bool treatAsBlinkingOrLerping = false;

                switch (blinkMode)
                {
                    case BlinkMode.Blinking:
                    {
                        bool isCurrentFrameBlinking = IsBlinkingFrame(data);
                        (leftEyeClosedAmount, rightEyeClosedAmount) = _blinkLerp.UpdateBlinkLerp(
                            isCurrentFrameBlinking,
                            currentTime,
                            leftBaseClosedAmount,
                            rightBaseClosedAmount);

                        treatAsBlinkingOrLerping = isCurrentFrameBlinking || _blinkLerp.IsLerping;
                        break;
                    }
                    case BlinkMode.Winking:
                    {
                        bool isLeftBlinking = data.LeftEyeFlagCondition;
                        bool isRightBlinking = data.RightEyeFlagCondition;

                        (leftEyeClosedAmount, rightEyeClosedAmount) = _blinkLerp.UpdateBlinkLerpPerEye(
                            isLeftBlinking,
                            isRightBlinking,
                            currentTime,
                            leftBaseClosedAmount,
                            rightBaseClosedAmount
                        );

                        treatAsBlinkingOrLerping = isLeftBlinking || isRightBlinking || _blinkLerp.IsLerping;

                        // If an eye is winking, set amounts for both to make it clear
                        bool leftWinking = data.LeftEyeFlagCondition && !data.RightEyeFlagCondition;
                        bool rightWinking = !data.LeftEyeFlagCondition && data.RightEyeFlagCondition;
                        if (leftWinking)
                        {
                            leftEyeClosedAmount = 1.0f;
                            rightEyeClosedAmount = 0.0f;
                        }
                        else if (rightWinking)
                        {
                            rightEyeClosedAmount = 1.0f;
                            leftEyeClosedAmount = 0.0f;
                        }

                        break;
                    }
                    case BlinkMode.None:
                    default:
                    {
                        leftEyeClosedAmount = 0.0f;
                        rightEyeClosedAmount = 0.0f;
                        treatAsBlinkingOrLerping = false;
                        break;
                    }
                }

                // Decide whether to use raw or filtered gaze based on blink mode and state
                if (treatAsBlinkingOrLerping || filterDisabled)
                {
                    // For blink/wink frames, use raw values (don't filter)
                    leftX = data.LeftEyeX;
                    leftY = data.LeftEyeY;
                    rightX = data.RightEyeX;
                    rightY = data.RightEyeY;
                }
                else
                {
                    // 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);
                }

                // When only tracking with left eye, use left eye data for both eyes
                bool singleEyeTracking = (data.RightEyeX == 0 && data.RightEyeY == 0);
                if (singleEyeTracking)
                {
                    rightX = leftX;
                    rightY = leftY;
                }
                

                if (_debug)
                {
                    GD.Print($"Raw - Left: ({data.LeftEyeX:F3}, {data.LeftEyeY:F3}), Right: ({data.RightEyeX:F3}, {data.RightEyeY:F3})");
                    GD.Print($"Filtered - Left: ({leftX:F3}, {leftY:F3}), Right: ({rightX:F3}, {rightY:F3})");
                    GD.Print($"Flag Conditions - Left: {data.LeftEyeFlagCondition} (prob: {data.LeftEyeFlagProbability:F3}), Right: {data.RightEyeFlagCondition} (prob: {data.RightEyeFlagProbability:F3})");
                }

                // Convert eye positions to pitch and yaw using frustum loader
                // Convert normalized screen positions (0-1 range) to view angles
                Vector2 leftAngles = _frustumLoader.ScreenPositionToViewAngles(new Vector2(leftX, leftY), true);
                Vector2 rightAngles = _frustumLoader.ScreenPositionToViewAngles(new Vector2(rightX, rightY), false);

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

                if (_debug)
                {
                    GD.Print($"Left eye - Pitch: {leftPitch}, Yaw: {leftYaw}");
                    GD.Print($"Right eye - Pitch: {rightPitch}, Yaw: {rightYaw}");
                }

                // Apply convergence logic to the gaze directions before converting to 3D vectors
                Vector2 leftGaze = new Vector2(leftYaw, leftPitch);
                Vector2 rightGaze = new Vector2(rightYaw, rightPitch);
                (Vector2 convergedLeft, Vector2 convergedRight) = EyeConvergence.ApplyConvergence(leftGaze, rightGaze);

                // Convert converged angles to vectors
                Vector3 leftVector = AnglesToVector(convergedLeft.Y, convergedLeft.X);
                Vector3 rightVector = AnglesToVector(convergedRight.Y, convergedRight.X);

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

                // Calculate confidence
                float confidence = data.Confidence;

                // Create shared data structure
                var sharedData = new SharedGazeData
                {
                    LeftEyeX = leftVector.X,
                    LeftEyeY = leftVector.Y,
                    LeftEyeZ = leftVector.Z,
                    RightEyeX = rightVector.X,
                    RightEyeY = rightVector.Y,
                    RightEyeZ = rightVector.Z,
                    CombinedX = combinedVector.X,
                    CombinedY = combinedVector.Y,
                    CombinedZ = combinedVector.Z,
                    Confidence = confidence,
                    Timestamp = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds(),
                    IsValid = 1,
                    LeftEyeClosedAmount = leftEyeClosedAmount,
                    RightEyeClosedAmount = rightEyeClosedAmount
                };

                // Write to shared memory
                _accessor.Write(0, ref sharedData);
            }
            catch (Exception ex)
            {
                GD.PrintErr($"{Name}: Error updating shared memory: {ex.Message}");
                GD.PrintErr(ex.StackTrace);
            }
        }


        /// <summary>
        /// Convert pitch and yaw angles to a 3D direction vector
        /// </summary>
        /// <param name="pitch">Pitch angle in degrees</param>
        /// <param name="yaw">Yaw angle in degrees</param>
        /// <returns>Direction vector</returns>
        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); // Note: negating z as forward is -z in many 3D engines
        }

        public override void Enable()
        {
            base.Enable();

            // Create resources
            InitializeMemmap();
        }

        /// <summary>
        /// Stop the consumer and clean up resources
        /// </summary>
        public override void Disable()
        {
            base.Disable(); // Call the base class Stop method first

            // Clear the data buffer
            _dataBuffer.Clear();

            // Reset blink states
            _wasBlinking = false;
            _blinkLerp.Reset();

            Cleanup();
            _running = false;
        }
    }
}