using Godot;
using System;
using System.Text;

/// <summary>
/// Tracks eye positions in VR and sends them via UDP.
/// </summary>
public partial class EyeTrackingSystem : Node
{
    [Export]
    public Camera3D VrCamera { get; set; }  // Your main VR camera (set via the Inspector)

    [Export]
    public XROrigin3D XrOrigin { get; set; }

    [Export]
    public Node3D Target { get; set; }  // The object to track

    [Export]
    public string ReceiverIp { get; set; } = "127.0.0.1";

    [Export]
    public int Port { get; set; } = 12345;

    private PacketPeerUdp _udp;

    /// <summary>
    /// Called when the node enters the scene tree for the first time.
    /// </summary>
    public override void _Ready()
    {
        _udp = new PacketPeerUdp();
        Error err = _udp.ConnectToHost(ReceiverIp, Port);
        if (err != Error.Ok)
        {
            GD.PrintErr($"Failed to connect to host: {ReceiverIp}");
        }
    }

    /// <summary>
    /// Called every frame.
    /// </summary>
    public override void _Process(double delta)
    {
        if (VrCamera == null || Target == null)
        {
            return;
        }

        if (Input.IsKeyPressed(Key.Space))
        {
            var xrInterface = XRServer.FindInterface("OpenXR");
            if (xrInterface == null)
            {
                GD.PrintErr("OpenXR interface not found");
                return;
            }

            // Create new transforms for each eye with the same orientation as the main camera
            Transform3D leftEyeTransform = xrInterface.GetTransformForView(0, XrOrigin.GlobalTransform);
            Transform3D rightEyeTransform = xrInterface.GetTransformForView(1, XrOrigin.GlobalTransform);

            // Use the main camera's projection matrix
            Vector2 viewport = xrInterface.GetRenderTargetSize();
            float aspectRatio = viewport.X / viewport.Y;

            Projection leftProjection = xrInterface.GetProjectionForView(0, aspectRatio, VrCamera.Near, VrCamera.Far);
            Projection rightProjection = xrInterface.GetProjectionForView(1, aspectRatio, VrCamera.Near, VrCamera.Far);

            // Project the target's world position for each eye
            Vector2 leftViewportPos = ProjectPointWithTransform(Target.GlobalTransform.Origin, leftEyeTransform, leftProjection);
            Vector2 rightViewportPos = ProjectPointWithTransform(Target.GlobalTransform.Origin, rightEyeTransform, rightProjection);

            // Format the message as "leftX,leftY;rightX,rightY" and send it via UDP
            string message = string.Format("{0:0.0000},{1:0.0000};{2:0.0000},{3:0.0000}",
                leftViewportPos.X, leftViewportPos.Y, rightViewportPos.X, rightViewportPos.Y);

            _udp.PutPacket(Encoding.UTF8.GetBytes(message));
        }
    }

    /// <summary>
    /// Projects a world point using a custom camera transform and projection matrix.
    /// </summary>
    /// <param name="worldPoint">The point in world space to project.</param>
    /// <param name="cameraTransform">The camera transform.</param>
    /// <param name="projection">The projection matrix.</param>
    /// <returns>The projected point in viewport coordinates (range [0,1]).</returns>
    private Vector2 ProjectPointWithTransform(Vector3 worldPoint, Transform3D cameraTransform, Projection projection)
    {
        // Get the view matrix (inverse of camera transform)
        Transform3D viewMatrix = cameraTransform.Inverse();

        // Transform the world point to view space
        Vector3 pointInView = viewMatrix * worldPoint;

        // Create a Vector4 for the projection
        Vector4 vec = new Vector4(pointInView.X, pointInView.Y, pointInView.Z, 1.0f);

        // Transform to clip space
        Vector4 clipSpace = projection * vec;

        // Convert to normalized device coordinates (NDC)
        Vector3 ndc;
        if (clipSpace.W != 0)
        {
            ndc = new Vector3(clipSpace.X, clipSpace.Y, clipSpace.Z) / clipSpace.W;
        }
        else
        {
            ndc = new Vector3(clipSpace.X, clipSpace.Y, clipSpace.Z);
        }

        // Convert from NDC (range [-1,1]) to viewport coordinates (range [0,1])
        float viewportX = (ndc.X * 0.5f) + 0.5f;
        float viewportY = (ndc.Y * 0.5f) + 0.5f;

        GD.Print($"{viewportX} {viewportY}");
        return new Vector2(viewportX, viewportY);
    }

    /// <summary>
    /// Called when the node is about to be removed from the scene.
    /// </summary>
    public override void _ExitTree()
    {
        _udp = null;
    }
}