#include "imu_data_provider.h"
#include <openvr_driver.h>

namespace sauna
{

IMUDataProvider::IMUDataProvider()
    : m_bInitialized(false)
{
}

IMUDataProvider::~IMUDataProvider()
{
    Shutdown();
}

bool IMUDataProvider::Initialize()
{
    if (m_bInitialized)
        return true;

    m_bInitialized = true;
    return true;
}

void IMUDataProvider::Shutdown()
{
    if (!m_bInitialized)
        return;

    std::lock_guard<std::mutex> lock(m_imuSamplesMutex);
    m_imuSamples.clear();
    m_bInitialized = false;
}

bool IMUDataProvider::GetLatestIMUSample(uint32_t unDeviceIndex, vr::ImuSample_t *pSample)
{
    if (!m_bInitialized || !pSample)
        return false;

    std::lock_guard<std::mutex> lock(m_imuSamplesMutex);
    
    auto it = m_imuSamples.find(unDeviceIndex);
    if (it == m_imuSamples.end() || it->second.empty())
    {
        return false;
    }
    
    *pSample = it->second.front();
    it->second.pop();
    
    return true;
}

void IMUDataProvider::RegisterDevice(uint32_t unDeviceIndex)
{
    if (!m_bInitialized)
        return;

    std::lock_guard<std::mutex> lock(m_imuSamplesMutex);
    
    // Create an entry for this device if it doesn't exist
    if (m_imuSamples.find(unDeviceIndex) == m_imuSamples.end())
    {
        m_imuSamples[unDeviceIndex] = std::queue<vr::ImuSample_t>();
    }
}

bool IMUDataProvider::IsIMUDataAvailable(uint32_t unDeviceIndex)
{
    if (!m_bInitialized)
        return false;

    std::lock_guard<std::mutex> lock(m_imuSamplesMutex);
    
    auto it = m_imuSamples.find(unDeviceIndex);
    return (it != m_imuSamples.end() && !it->second.empty());
}

void IMUDataProvider::AddIMUSample(uint32_t unDeviceIndex, const vr::ImuSample_t &sample)
{
    if (!m_bInitialized)
        return;

    std::lock_guard<std::mutex> lock(m_imuSamplesMutex);
    
    // Create an entry for this device if it doesn't exist
    if (m_imuSamples.find(unDeviceIndex) == m_imuSamples.end())
    {
        m_imuSamples[unDeviceIndex] = std::queue<vr::ImuSample_t>();
    }
    
    // Add the sample to the queue, limiting the queue size to prevent memory issues
    const size_t MAX_QUEUE_SIZE = 100;
    auto &queue = m_imuSamples[unDeviceIndex];
    
    if (queue.size() >= MAX_QUEUE_SIZE)
    {
        queue.pop(); // Remove the oldest sample
    }
    
    queue.push(sample);
}

} // namespace sauna