#include "sauna_device_driver.h"
#include <openvr_driver.h>
#include <cstring>

namespace sauna
{

SaunaDeviceDriver::SaunaDeviceDriver(vr::ITrackedDeviceServerDriver *pWrappedDriver, IMUDataProvider *pIMUDataProvider)
    : m_pWrappedDriver(pWrappedDriver)
    , m_pIMUDataProvider(pIMUDataProvider)
    , m_unDeviceId(vr::k_unTrackedDeviceIndexInvalid)
    , m_bActivated(false)
{
}

SaunaDeviceDriver::~SaunaDeviceDriver()
{
    // We don't own these pointers, so we don't delete them
    m_pWrappedDriver = nullptr;
    m_pIMUDataProvider = nullptr;
}

vr::EVRInitError SaunaDeviceDriver::Activate(uint32_t unObjectId)
{
    m_unDeviceId = unObjectId;
    m_bActivated = true;
    
    // Register this device with the IMU data provider
    if (m_pIMUDataProvider)
    {
        m_pIMUDataProvider->RegisterDevice(m_unDeviceId);
    }
    
    // Forward the call to the wrapped driver
    return m_pWrappedDriver->Activate(unObjectId);
}

void SaunaDeviceDriver::Deactivate()
{
    m_bActivated = false;
    m_unDeviceId = vr::k_unTrackedDeviceIndexInvalid;
    
    // Forward the call to the wrapped driver
    m_pWrappedDriver->Deactivate();
}

void SaunaDeviceDriver::EnterStandby()
{
    // Forward the call to the wrapped driver
    m_pWrappedDriver->EnterStandby();
}

void *SaunaDeviceDriver::GetComponent(const char *pchComponentNameAndVersion)
{
    // First check if the wrapped driver provides this component
    void *pComponent = m_pWrappedDriver->GetComponent(pchComponentNameAndVersion);
    if (pComponent)
    {
        return pComponent;
    }
    
    // If the component is our custom IMU component, return it
    if (strcmp(pchComponentNameAndVersion, IVRIMUComponent_Version) == 0)
    {
        return static_cast<IVRIMUComponent*>(this);
    }
    
    return nullptr;
}

void SaunaDeviceDriver::DebugRequest(const char *pchRequest, char *pchResponseBuffer, uint32_t unResponseBufferSize)
{
    // Forward the call to the wrapped driver
    m_pWrappedDriver->DebugRequest(pchRequest, pchResponseBuffer, unResponseBufferSize);
}

vr::DriverPose_t SaunaDeviceDriver::GetPose()
{
    // Get the pose from the wrapped driver
    vr::DriverPose_t pose = m_pWrappedDriver->GetPose();
    
    // If optical tracking is lost, we can still provide IMU data
    // This is indicated by the tracking result
    if (pose.result == vr::TrackingResult_Fallback_RotationOnly ||
        pose.result == vr::TrackingResult_Calibrating_OutOfRange ||
        pose.result == vr::TrackingResult_Running_OutOfRange)
    {
        // Get the latest IMU data
        vr::ImuSample_t imuSample;
        if (m_pIMUDataProvider && m_pIMUDataProvider->GetLatestIMUSample(m_unDeviceId, &imuSample))
        {
            // Update the pose with the IMU data
            // Note: In a real implementation, we would need to integrate the IMU data
            // to get a more accurate pose. This is a simplified example.
            
            // Update the angular velocity from the gyroscope
            pose.vecAngularVelocity[0] = imuSample.vGyro.v[0];
            pose.vecAngularVelocity[1] = imuSample.vGyro.v[1];
            pose.vecAngularVelocity[2] = imuSample.vGyro.v[2];
            
            // Update the acceleration from the accelerometer
            pose.vecAcceleration[0] = imuSample.vAccel.v[0];
            pose.vecAcceleration[1] = imuSample.vAccel.v[1];
            pose.vecAcceleration[2] = imuSample.vAccel.v[2];
        }
    }
    
    return pose;
}

bool SaunaDeviceDriver::GetLatestIMUSample(vr::ImuSample_t *pSample)
{
    if (!m_bActivated || !m_pIMUDataProvider || !pSample)
    {
        return false;
    }
    
    return m_pIMUDataProvider->GetLatestIMUSample(m_unDeviceId, pSample);
}

bool SaunaDeviceDriver::IsIMUDataAvailable()
{
    if (!m_bActivated || !m_pIMUDataProvider)
    {
        return false;
    }
    
    return m_pIMUDataProvider->IsIMUDataAvailable(m_unDeviceId);
}

bool SaunaDeviceDriver::GetIMUDataInFallbackMode(vr::ImuSample_t *pSample)
{
    if (!m_bActivated || !m_pIMUDataProvider || !pSample)
    {
        return false;
    }
    
    // Get the pose to check if we're in fallback mode
    vr::DriverPose_t pose = GetPose();
    
    // Check if optical tracking is lost
    if (pose.result == vr::TrackingResult_Fallback_RotationOnly ||
        pose.result == vr::TrackingResult_Calibrating_OutOfRange ||
        pose.result == vr::TrackingResult_Running_OutOfRange)
    {
        // Get the IMU data
        return m_pIMUDataProvider->GetLatestIMUSample(m_unDeviceId, pSample);
    }
    
    return false;
}

} // namespace sauna