using UnityEngine;
using Valve.VR;
using TMPro;
using System.Runtime.InteropServices;
using System.Text;

public class HeadsetGyroscope : MonoBehaviour
{
    private TextMeshProUGUI gyroText;
    private uint headsetIndex = OpenVR.k_unTrackedDeviceIndexInvalid;
    
    // Property IDs for IMU data
    private const uint k_unTrackingSystemName_String = 1000;
    private const uint k_unIMUAccelX_Float = 2014;
    private const uint k_unIMUAccelY_Float = 2015;
    private const uint k_unIMUAccelZ_Float = 2016;
    private const uint k_unIMUGyroX_Float = 2017;
    private const uint k_unIMUGyroY_Float = 2018;
    private const uint k_unIMUGyroZ_Float = 2019;

    void Start()
    {
        Debug.Log("HeadsetGyroscope: Start called");
        gyroText = GetComponent<TextMeshProUGUI>();
        if (gyroText == null)
        {
            Debug.LogError("TextMeshProUGUI component not found on this GameObject");
        }
        
        // Initialize OpenVR if not already initialized
        if (OpenVR.System == null)
        {
            OpenVRUtil.System.InitOpenVR();
        }
        
        // Find the headset device index (should be 0 for the HMD)
        headsetIndex = OpenVR.k_unTrackedDeviceIndex_Hmd;
        
        // Log the tracking system name
        StringBuilder trackingSystemName = new StringBuilder(64);
        ETrackedPropertyError error = ETrackedPropertyError.TrackedProp_Success;
        OpenVR.System.GetStringTrackedDeviceProperty(headsetIndex, k_unTrackingSystemName_String,
            trackingSystemName, 64, ref error);
        
        if (error == ETrackedPropertyError.TrackedProp_Success)
        {
            Debug.Log($"Tracking system: {trackingSystemName}");
        }
        else
        {
            Debug.LogWarning($"Failed to get tracking system name: {error}");
        }
    }

    void Update()
    {
        if (OpenVR.System == null || gyroText == null)
            return;

        // Make sure the headset is connected
        if (!OpenVR.System.IsTrackedDeviceConnected(headsetIndex))
        {
            gyroText.text = "Headset not connected";
            return;
        }

        // Try to get raw IMU data first
        Vector3 rawGyro = GetRawIMUGyro();
        
        if (rawGyro != Vector3.zero)
        {
            // We have raw gyro data, use it
            gyroText.text = string.Format("Raw Gyro:\nX: {0:F2}\nY: {1:F2}\nZ: {2:F2}",
                rawGyro.x, rawGyro.y, rawGyro.z);
            return;
        }
        
        // Fallback to using raw tracking pose
        TrackedDevicePose_t rawPose = new TrackedDevicePose_t();
        TrackedDevicePose_t[] rawPoses = new TrackedDevicePose_t[OpenVR.k_unMaxTrackedDeviceCount];
        
        // Try to get raw tracking data (might include IMU data even when optical tracking is lost)
        OpenVR.System.GetRawZeroPoseToStandingAbsoluteTrackingPose(rawPoses);
        rawPose = rawPoses[headsetIndex];
        
        if (rawPose.bDeviceIsConnected)
        {
            Vector3 rawAngularVelocity = new Vector3(
                rawPose.vAngularVelocity.v0,
                rawPose.vAngularVelocity.v1,
                rawPose.vAngularVelocity.v2
            );
            
            if (rawAngularVelocity.magnitude > 0.001f)
            {
                gyroText.text = string.Format("Raw Pose Gyro:\nX: {0:F2}\nY: {1:F2}\nZ: {2:F2}",
                    rawAngularVelocity.x, rawAngularVelocity.y, rawAngularVelocity.z);
                return;
            }
        }
        
        // Last resort: use the standard tracking pose
        TrackedDevicePose_t pose = new TrackedDevicePose_t();
        TrackedDevicePose_t[] poses = new TrackedDevicePose_t[OpenVR.k_unMaxTrackedDeviceCount];
        OpenVR.System.GetDeviceToAbsoluteTrackingPose(ETrackingUniverseOrigin.TrackingUniverseStanding, 0, poses);
        pose = poses[headsetIndex];

        if (!pose.bDeviceIsConnected || !pose.bPoseIsValid)
        {
            gyroText.text = "No gyro data available";
            return;
        }

        // Extract angular velocity from the pose
        Vector3 angularVelocity = new Vector3(
            pose.vAngularVelocity.v0,
            pose.vAngularVelocity.v1,
            pose.vAngularVelocity.v2
        );

        // Format and display the gyroscope data
        gyroText.text = string.Format("Std Gyro:\nX: {0:F2}\nY: {1:F2}\nZ: {2:F2}",
            angularVelocity.x,
            angularVelocity.y,
            angularVelocity.z
        );
    }
    
    private Vector3 GetRawIMUGyro()
    {
        if (OpenVR.System == null)
            return Vector3.zero;
            
        ETrackedPropertyError error = ETrackedPropertyError.TrackedProp_Success;
        
        // Try to get raw gyro data from device properties
        float gyroX = OpenVR.System.GetFloatTrackedDeviceProperty(headsetIndex, k_unIMUGyroX_Float, ref error);
        if (error != ETrackedPropertyError.TrackedProp_Success)
        {
            Debug.LogWarning($"Failed to get IMU Gyro X: {error}");
            return Vector3.zero;
        }
        
        float gyroY = OpenVR.System.GetFloatTrackedDeviceProperty(headsetIndex, k_unIMUGyroY_Float, ref error);
        if (error != ETrackedPropertyError.TrackedProp_Success)
        {
            Debug.LogWarning($"Failed to get IMU Gyro Y: {error}");
            return Vector3.zero;
        }
        
        float gyroZ = OpenVR.System.GetFloatTrackedDeviceProperty(headsetIndex, k_unIMUGyroZ_Float, ref error);
        if (error != ETrackedPropertyError.TrackedProp_Success)
        {
            Debug.LogWarning($"Failed to get IMU Gyro Z: {error}");
            return Vector3.zero;
        }
        
        return new Vector3(gyroX, gyroY, gyroZ);
    }
}