using Godot;
using System;
using System.IO.MemoryMappedFiles;
using System.Runtime.InteropServices;

/// <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 int LeftEyeFlagCondition;  // 0 = false, 1 = true
    public int RightEyeFlagCondition; // 0 = false, 1 = true
}

/// <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 C++ interop
/// </summary>
public partial class MemmapConsumer : EyeTrackingConsumer
{
    private MemoryMappedFile _sharedMem;
    private MemoryMappedViewAccessor _accessor;
    private bool _running;
    private bool _debug;
    private const string SharedMemoryName = "EyeTrackingMemmapData";
    protected FrustumLoader _frustumLoader;

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

        _debug = false;

        InitializeMemmap();
    }

    /// <summary>
    /// Initialize the memory-mapped file
    /// </summary>
    private void InitializeMemmap()
    {
        try
        {
            Cleanup();

            // 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>
    /// Cleanup mem resources
    /// </summary>
    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>
    /// 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)
        {
            ProcessData(data);
        }
    }

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

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

            // 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,
                LeftEyeFlagCondition = data.LeftEyeFlagCondition ? 1 : 0,
                RightEyeFlagCondition = data.RightEyeFlagCondition ? 1 : 0
            };

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

    /// <summary>
    /// Start the consumer and recreate resources
    /// </summary>
    public override void Enable()
    {
        base.Enable();

        // Create memmap resources
        InitializeMemmap();
    }

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

        Cleanup();

        _running = false;
    }
}