#include "device_provider.h"
#include "driverlog.h"
#include "../hid/hid_device.h"
#include "../hid/user_signature.h"
#include "../hid/proximity_algorithm.h"
#include <hidapi.h>
#include <cstring>
#include <cmath>
#include <fstream>
#include <filesystem>
#include <string>
#include <algorithm>

static const char* kSettingsSection = "driver_BeyondProximity";
static constexpr float kPi = 3.14159265358979323846f;

// Constructor and destructor must be defined here where HidDevice is a
// complete type (unique_ptr requires complete type for deletion/construction)
DeviceProvider::DeviceProvider() = default;
DeviceProvider::~DeviceProvider() = default;

vr::EVRInitError DeviceProvider::Init( vr::IVRDriverContext* pDriverContext )
{
    // MUST be called before any VR* functions
    VR_INIT_SERVER_DRIVER_CONTEXT( pDriverContext );

    InitDriverLog( vr::VRDriverLog() );

    DriverLog( "Beyond Proximity driver initializing\n" );

    // Read configuration from steamvr.vrsettings
    vr::EVRSettingsError settingsErr;

    int32_t reportRateMs = vr::VRSettings()->GetInt32(kSettingsSection, "report_rate_ms", &settingsErr);
    if (settingsErr != vr::VRSettingsError_None)
        reportRateMs = 200;
    if (reportRateMs < 50) reportRateMs = 50;
    if (reportRateMs > 5000) reportRateMs = 5000;

    int32_t logVerbosity = vr::VRSettings()->GetInt32(kSettingsSection, "log_verbosity", &settingsErr);
    if (settingsErr != vr::VRSettingsError_None)
        logVerbosity = 0;
    if (logVerbosity < 0) logVerbosity = 0;
    if (logVerbosity > 1) logVerbosity = 1;

    int32_t movingAvgLength = vr::VRSettings()->GetInt32(kSettingsSection, "moving_avg_length", &settingsErr);
    if (settingsErr != vr::VRSettingsError_None)
        movingAvgLength = 8;
    if (movingAvgLength < 1) movingAvgLength = 1;
    if (movingAvgLength > 64) movingAvgLength = 64;

    m_logVerbose = (logVerbosity > 0);
    m_reportRateMs = reportRateMs;
    m_movingAvgLength = movingAvgLength;

    DriverLog("Config: report_rate_ms=%d, log_verbosity=%d, moving_avg_length=%d\n",
              reportRateMs, logVerbosity, movingAvgLength);

    // Initialize HIDAPI library
    if (hid_init() != 0)
    {
        DriverLog("HID: hid_init() failed\n");
        // Continue without HID -- graceful degradation
    }
    else
    {
        // Create HidDevice and start reader thread (handles open/reconnect internally)
        m_pHidDevice = std::make_unique<HidDevice>(movingAvgLength, m_logVerbose);
        DriverLog("HID: Starting reader thread (rate=%ums)\n", reportRateMs);
        m_pHidDevice->StartReading(0x35BD, 0x0101, static_cast<uint16_t>(reportRateMs));
    }

    // Create named pipe for debug control
    CreatePipeServer();

    // Read initial IPD from HMD container (HARD-02)
    {
        vr::PropertyContainerHandle_t hmdProps =
            vr::VRProperties()->TrackedDeviceToPropertyContainer(vr::k_unTrackedDeviceIndex_Hmd);
        vr::ETrackedPropertyError propErr;
        float ipd = vr::VRProperties()->GetFloatProperty(hmdProps, vr::Prop_UserIpdMeters_Float, &propErr);
        if (propErr == vr::TrackedProp_Success && ipd > 0.0f)
        {
            m_fCurrentIpd = ipd;
#ifdef ENABLE_IPD_PERSIST
            m_fStartupIpd = ipd;
#endif
            DriverLog("IPD: Initial IPD from HMD container: %.4fm (%.1fmm)\n", ipd, ipd * 1000.0f);
        }
        else
        {
            DriverLog("IPD: No initial IPD available from HMD container (err=%d)\n", propErr);
        }
    }

    // Attempt eager lighthouse config load (rotation matrix cache)
    // Note: HID reader thread may not have tracking serial yet (async),
    // so this may fail -- lazy load on first ipd command will retry
    if (m_pHidDevice)
    {
        if (!LoadLighthouseConfig())
        {
            DriverLog("IPD: Eager config load deferred -- serial not yet available from HID\n");
        }
    }

    DriverLog( "Beyond Proximity driver initialized successfully\n" );

    return vr::VRInitError_None;
}

void DeviceProvider::Cleanup()
{
#ifdef ENABLE_IPD_PERSIST
    PersistIpdToConfig();  // D-01: persist on shutdown, before log cleanup
#endif
    DestroyPipeServer();
    if (m_pHidDevice)
        m_pHidDevice->StopReading();  // Join reader thread first
    m_pHidDevice.reset();             // Then destroy HidDevice
    hid_exit();                       // Then finalize HIDAPI
    CleanupDriverLog();
}

const char* const* DeviceProvider::GetInterfaceVersions()
{
    return vr::k_InterfaceVersions;
}

void DeviceProvider::TryCreateProximityComponent()
{
    if (m_bProximityComponentAttempted)
        return;

    vr::PropertyContainerHandle_t hmdProps =
        vr::VRProperties()->TrackedDeviceToPropertyContainer(vr::k_unTrackedDeviceIndex_Hmd);

    if (hmdProps == vr::k_ulInvalidPropertyContainer)
        return;  // HMD not ready yet — try again next frame

    m_bProximityComponentAttempted = true;

    vr::EVRInputError err = vr::VRDriverInput()->CreateBooleanComponent(
        hmdProps, "/proximity", &m_hProximityComponent);

    if (err == vr::VRInputError_None)
    {
        DriverLog("Proximity: Created /proximity component on HMD (handle=%llu)\n",
                  (uint64_t)m_hProximityComponent);
    }
    else
    {
        DriverLog("Proximity: CreateBooleanComponent failed (err=%d), using property fallback\n", (int)err);
        m_hProximityComponent = vr::k_ulInvalidInputComponentHandle;
    }
}

void DeviceProvider::CheckHmdSerial()
{
    if (m_bHmdSerialChecked) return;
    if (!m_pHidDevice) return;

    CalibrationData cal = m_pHidDevice->GetCalibration();
    if (cal.hmd_serial[0] == '\0') return;  // not yet read from HID

    m_bHmdSerialChecked = true;
    m_bIsBeyond1 = (strncmp(cal.hmd_serial, "BS1", 3) == 0);

    DriverLog("HMD Serial: %s -> %s\n", cal.hmd_serial,
              m_bIsBeyond1 ? "Beyond 1 (IPD disabled)" : "Beyond 2 (IPD enabled)");
}

bool DeviceProvider::ApplyIpd(float mm)
{
    if (mm < 48.0f || mm > 75.0f) return false;

    float ipdMeters = mm / 1000.0f;

    // Loop guard (per D-07): skip if already at this IPD
    if (fabsf(ipdMeters - m_fCurrentIpd) < 0.0001f) return true;

    if (!m_bLhConfigLoaded)
    {
        if (!LoadLighthouseConfig()) return false;
    }

    // Build HmdMatrix34_t with preserved rotation + new IPD translation
    vr::HmdMatrix34_t left = {}, right = {};
    for (int r = 0; r < 3; r++)
        for (int c = 0; c < 3; c++) {
            left.m[r][c] = m_cachedLeftRot[r][c];
            right.m[r][c] = m_cachedRightRot[r][c];
        }
    left.m[0][3]  = -ipdMeters / 2.0f;
    right.m[0][3] = +ipdMeters / 2.0f;

    vr::VRServerDriverHost()->SetDisplayEyeToHead(
        vr::k_unTrackedDeviceIndex_Hmd, left, right);

    vr::PropertyContainerHandle_t hmdProps =
        vr::VRProperties()->TrackedDeviceToPropertyContainer(
            vr::k_unTrackedDeviceIndex_Hmd);
    vr::VRProperties()->SetFloatProperty(hmdProps,
        vr::Prop_UserIpdMeters_Float, ipdMeters);

    m_fCurrentIpd = ipdMeters;

#ifdef ENABLE_IPD_PERSIST
    // Capture startup IPD on first successful apply (Init() read may miss it
    // if lighthouse driver hasn't set the property yet at that point)
    if (m_fStartupIpd <= 0.0f)
        m_fStartupIpd = ipdMeters;
#endif

    // Sync to VRSettings so the settings tab slider reflects current IPD
    vr::VRSettings()->SetFloat(kSettingsSection, "ipd_mm", mm);
    m_fLastSettingsIpd = mm;

    return true;
}

void DeviceProvider::TrySetSliderProperties()
{
    if (m_bSliderPropsAttempted) return;
    if (m_bIsBeyond1) return;  // per D-09

    vr::PropertyContainerHandle_t hmdProps =
        vr::VRProperties()->TrackedDeviceToPropertyContainer(
            vr::k_unTrackedDeviceIndex_Hmd);
    if (hmdProps == vr::k_ulInvalidPropertyContainer) return;

    m_bSliderPropsAttempted = true;

    // --- Track B, Part 1: IpdUIRange properties (per D-01) ---
    vr::ETrackedPropertyError err;

    err = vr::VRProperties()->SetFloatProperty(hmdProps,
        vr::Prop_IpdUIRangeMinMeters_Float, 0.048f);
    DriverLog("IPD Slider: SetFloatProperty(IpdUIRangeMin=0.048) err=%d\n", (int)err);

    err = vr::VRProperties()->SetFloatProperty(hmdProps,
        vr::Prop_IpdUIRangeMaxMeters_Float, 0.075f);
    DriverLog("IPD Slider: SetFloatProperty(IpdUIRangeMax=0.075) err=%d\n", (int)err);

    vr::ETrackedPropertyError boolErr;
    boolErr = vr::VRProperties()->SetBoolProperty(hmdProps,
        vr::Prop_DriverDisplaysIPDChanges_Bool, false);
    DriverLog("IPD Slider: SetBoolProperty(DriverDisplaysIPDChanges=false) err=%d\n", (int)boolErr);

    // --- Track B, Part 2: Component-based handle probing (per D-02) ---
    vr::VRInputComponentHandle_t ipdHandle = vr::k_ulInvalidInputComponentHandle;
    vr::EVRInputError inputErr;

    // Probe 1: /input/ipd/value -- scalar analog component for IPD
    inputErr = vr::VRDriverInput()->CreateScalarComponent(
        hmdProps, "/input/ipd/value", &ipdHandle,
        vr::VRScalarType_Absolute, vr::VRScalarUnits_NormalizedOneSided);
    DriverLog("IPD Slider: CreateScalarComponent(/input/ipd/value) err=%d handle=%llu\n",
        (int)inputErr, (unsigned long long)ipdHandle);

    // Probe 2: /input/ipd_adjust/value -- alternative naming convention
    vr::VRInputComponentHandle_t ipdAdjHandle = vr::k_ulInvalidInputComponentHandle;
    inputErr = vr::VRDriverInput()->CreateScalarComponent(
        hmdProps, "/input/ipd_adjust/value", &ipdAdjHandle,
        vr::VRScalarType_Absolute, vr::VRScalarUnits_NormalizedOneSided);
    DriverLog("IPD Slider: CreateScalarComponent(/input/ipd_adjust/value) err=%d handle=%llu\n",
        (int)inputErr, (unsigned long long)ipdAdjHandle);

    // Probe 3: /input/eye_distance/value -- another possible component name
    vr::VRInputComponentHandle_t eyeDistHandle = vr::k_ulInvalidInputComponentHandle;
    inputErr = vr::VRDriverInput()->CreateScalarComponent(
        hmdProps, "/input/eye_distance/value", &eyeDistHandle,
        vr::VRScalarType_Absolute, vr::VRScalarUnits_NormalizedOneSided);
    DriverLog("IPD Slider: CreateScalarComponent(/input/eye_distance/value) err=%d handle=%llu\n",
        (int)inputErr, (unsigned long long)eyeDistHandle);
}

void DeviceProvider::RunFrame()
{
    // Deferred proximity component creation (HMD container not ready at Init time)
    TryCreateProximityComponent();
    CheckHmdSerial();
    TrySetSliderProperties();

    PollPipe();

    // Poll for IPD-related VR events (detect IpdChanged)
    {
        vr::VREvent_t event;
        while (vr::VRServerDriverHost()->PollNextEvent(&event, sizeof(event)))
        {
            if (event.eventType == vr::VREvent_IpdChanged)
            {
                float newIpdMeters = event.data.ipd.ipdMeters;
                float newMm = newIpdMeters * 1000.0f;
                DriverLog("IPD Event: VREvent_IpdChanged ipd=%.4fm (%.1fmm)\n",
                    newIpdMeters, newMm);

                // Auto-apply (per D-05, D-06) with loop guard inside ApplyIpd (per D-07)
                if (!m_bIsBeyond1 && newMm >= 48.0f && newMm <= 75.0f)
                {
                    ApplyIpd(newMm);
                }
            }
        }
    }

    // Track A: Poll settings tab slider for IPD changes (per D-03)
    if (!m_bIsBeyond1)
    {
        vr::EVRSettingsError settingsErr;
        float ipdMm = vr::VRSettings()->GetFloat(
            kSettingsSection, "ipd_mm", &settingsErr);
        if (settingsErr == vr::VRSettingsError_None &&
            ipdMm >= 48.0f && ipdMm <= 75.0f)
        {
            if (fabsf(ipdMm - m_fLastSettingsIpd) > 0.01f)
            {
                m_fLastSettingsIpd = ipdMm;
                DriverLog("IPD: Settings slider changed to %.1fmm\n", ipdMm);
                ApplyIpd(ipdMm);
            }
        }
    }

    // Wire algorithm output to SteamVR HMD proximity
    if (m_pHidDevice && !m_bManualOverride)
    {
        bool detected = m_pHidDevice->GetPersonDetected();
        SetHmdProximity(detected);  // state-change guard inside

        if (m_logVerbose)
        {
            auto diag = m_pHidDevice->GetAlgorithmDiag();
            DriverLog("Prox: avg=%u thresh=%u detected=%s samples=%u\n",
                      diag.averaged_prox, diag.effective_threshold,
                      diag.detected ? "true" : "false", diag.total_samples);
        }
    }
}

bool DeviceProvider::ShouldBlockStandbyMode()
{
    return false;
}

void DeviceProvider::EnterStandby()
{
}

void DeviceProvider::LeaveStandby()
{
}

void DeviceProvider::CreatePipeServer()
{
    m_hPipe = CreateNamedPipeA(
        "\\\\.\\pipe\\beyond_proximity_ctl",
        PIPE_ACCESS_DUPLEX,
        PIPE_TYPE_MESSAGE | PIPE_READMODE_MESSAGE | PIPE_NOWAIT,
        1,             // max 1 instance
        512, 512,      // output/input buffer sizes
        0,             // default timeout
        nullptr);      // default security

    if (m_hPipe == INVALID_HANDLE_VALUE)
    {
        DriverLog("Pipe: Failed to create named pipe (error %lu)\n", GetLastError());
    }
    else
    {
        DriverLog("Pipe: Created \\\\.\\pipe\\beyond_proximity_ctl\n");
    }
}

void DeviceProvider::PollPipe()
{
    if (m_hPipe == INVALID_HANDLE_VALUE)
        return;

    if (!m_bClientConnected)
    {
        // PIPE_NOWAIT: ConnectNamedPipe returns immediately
        ConnectNamedPipe(m_hPipe, nullptr);
        DWORD err = GetLastError();
        if (err == ERROR_PIPE_CONNECTED || err == ERROR_NO_DATA)
        {
            m_bClientConnected = true;
        }
    }

    if (m_bClientConnected)
    {
        char buf[512];
        DWORD bytesRead = 0;
        if (ReadFile(m_hPipe, buf, sizeof(buf) - 1, &bytesRead, nullptr) && bytesRead > 0)
        {
            buf[bytesRead] = '\0';
            HandlePipeCommand(buf, bytesRead);
        }
        else if (GetLastError() == ERROR_BROKEN_PIPE)
        {
            // Client disconnected -- reset for next connection
            DisconnectNamedPipe(m_hPipe);
            m_bClientConnected = false;
        }
    }
}

void DeviceProvider::HandlePipeCommand(const char* cmd, DWORD /*len*/)
{
    char response[512] = {0};

    if (strcmp(cmd, "proximity on") == 0)
    {
        m_bManualOverride = true;
        SetHmdProximity(true);
        snprintf(response, sizeof(response), "OK proximity=true source=manual");
    }
    else if (strcmp(cmd, "proximity off") == 0)
    {
        m_bManualOverride = true;
        SetHmdProximity(false);
        snprintf(response, sizeof(response), "OK proximity=false source=manual");
    }
    else if (strcmp(cmd, "proximity auto") == 0)
    {
        m_bManualOverride = false;
        DriverLog("Proximity: manual override cleared, algorithm driving\n");
        snprintf(response, sizeof(response), "OK source=algorithm");
    }
    else if (strcmp(cmd, "status") == 0)
    {
        const char* hidStateStr = "not_connected";
        if (m_pHidDevice)
        {
            int connState = m_pHidDevice->GetConnectionState();
            if (connState == 1) hidStateStr = "open";
            else if (connState == 2) hidStateStr = "reconnecting";
        }

        char ipdBuf[16] = "unknown";
        if (m_fCurrentIpd > 0.0f)
            snprintf(ipdBuf, sizeof(ipdBuf), "%.1fmm", m_fCurrentIpd * 1000.0f);

        if (m_pHidDevice)
        {
            CalibrationData cal = m_pHidDevice->GetCalibration();
            auto diag = m_pHidDevice->GetAlgorithmDiag();
            snprintf(response, sizeof(response),
                "proximity=%s hid=%s source=%s prox_raw=%u cal=%u thresh=%u hyst=%u trim=%d "
                "averaged_prox=%u detected=%s eff_thresh=%u samples=%u "
                "report_rate_ms=%d log_verbosity=%d moving_avg_length=%d "
                "ipd=%s lh_config=%s",
                m_bProximity ? "true" : "false",
                hidStateStr,
                m_bManualOverride ? "manual" : "algorithm",
                m_pHidDevice->GetProxDistance(),
                cal.programmed_cal,
                cal.proximity_threshold,
                cal.proximity_hysteresis,
                cal.user_trim,
                diag.averaged_prox,
                diag.detected ? "true" : "false",
                diag.effective_threshold,
                diag.total_samples,
                m_reportRateMs,
                m_logVerbose ? 1 : 0,
                m_movingAvgLength,
                ipdBuf,
                m_bLhConfigLoaded ? "loaded" : "not_loaded");
        }
        else
        {
            snprintf(response, sizeof(response),
                "proximity=%s hid=not_connected source=%s "
                "report_rate_ms=%d log_verbosity=%d moving_avg_length=%d "
                "ipd=%s lh_config=%s",
                m_bProximity ? "true" : "false",
                m_bManualOverride ? "manual" : "algorithm",
                m_reportRateMs,
                m_logVerbose ? 1 : 0,
                m_movingAvgLength,
                ipdBuf,
                m_bLhConfigLoaded ? "loaded" : "not_loaded");
        }
    }
    else if (strncmp(cmd, "ipd ", 4) == 0)
    {
        if (m_bIsBeyond1)
        {
            snprintf(response, sizeof(response), "ERR ipd not supported on Beyond 1");
        }
        else
        {
            float mm = 0.0f;
            if (sscanf(cmd + 4, "%f", &mm) == 1)
            {
                HandleIpdSet(mm, response, sizeof(response));
            }
            else
            {
                snprintf(response, sizeof(response), "ERR ipd requires numeric argument (e.g. ipd 63.5)");
            }
        }
    }
    else if (strcmp(cmd, "ipd?") == 0)
    {
        if (m_bIsBeyond1)
        {
            snprintf(response, sizeof(response), "ERR ipd not supported on Beyond 1");
        }
        else
        {
            HandleIpdQuery(response, sizeof(response));
        }
    }
    else if (strncmp(cmd, "load_lh_config ", 15) == 0)
    {
        HandleLoadLhConfig(cmd + 15, response, sizeof(response));
    }
    else if (strncmp(cmd, "test_handle ", 12) == 0)
    {
        uint64_t handle = 0;
        int value = 0;
        if (sscanf(cmd + 12, "%llu %d", &handle, &value) == 2)
        {
            HandleTestHandle(handle, value != 0, response, sizeof(response));
        }
        else
        {
            snprintf(response, sizeof(response), "ERR usage: test_handle <handle> <0|1>");
        }
    }
    else
    {
        snprintf(response, sizeof(response), "ERR unknown command");
    }

    // Send response back to client
    DWORD written = 0;
    WriteFile(m_hPipe, response, (DWORD)strlen(response), &written, nullptr);

    // After responding, disconnect the pipe so it can accept the next client.
    // Each CLI invocation is a single command-response exchange.
    FlushFileBuffers(m_hPipe);
    DisconnectNamedPipe(m_hPipe);
    m_bClientConnected = false;
}

void DeviceProvider::DestroyPipeServer()
{
    if (m_hPipe != INVALID_HANDLE_VALUE)
    {
        if (m_bClientConnected)
            DisconnectNamedPipe(m_hPipe);
        CloseHandle(m_hPipe);
        m_hPipe = INVALID_HANDLE_VALUE;
        m_bClientConnected = false;
        DriverLog("Pipe: Destroyed\n");
    }
}

// --- HMD proximity signaling ---

void DeviceProvider::SetHmdProximity(bool on)
{
    if (on == m_bProximity)
        return;  // Only update on state change

    m_bProximity = on;

    if (m_hProximityComponent != vr::k_ulInvalidInputComponentHandle)
    {
        // Primary: input component — apps (VRChat, OpenXR) read this
        vr::EVRInputError err = vr::VRDriverInput()->UpdateBooleanComponent(
            m_hProximityComponent, on, 0.0);

        DriverLog("Proximity: UpdateBooleanComponent(%s) = %d\n",
            on ? "true" : "false", (int)err);
    }
    else
    {
        // Fallback: property toggling (v1.0 behavior, before input component ready)
        vr::PropertyContainerHandle_t hmdProps =
            vr::VRProperties()->TrackedDeviceToPropertyContainer(
                vr::k_unTrackedDeviceIndex_Hmd);

        vr::VRProperties()->SetBoolProperty(hmdProps,
            vr::Prop_ContainsProximitySensor_Bool, on);

        DriverLog("Proximity: SetBoolProperty fallback (%s)\n",
            on ? "true" : "false");
    }
}

// --- Config reading helpers (reused from spike, now reading from config.json files) ---

// Helper: parse 9 floats from "[[f,f,f],[f,f,f],[f,f,f]]" pattern starting at pos
static bool ParseEyeToHead3x3(const std::string& data, size_t startPos, float out[3][3])
{
    // Find the opening "eye_to_head" value -- a nested JSON array: [[r00,r01,r02],[r10,...],[r20,...]]
    // The config is pretty-printed with newlines/spaces, so scan floats one at a time.
    size_t pos = data.find('[', startPos);
    if (pos == std::string::npos) return false;

    // Skip past the outer '['
    pos++;

    int count = 0;
    for (int r = 0; r < 3 && pos < data.size(); r++)
    {
        // Find inner row '['
        pos = data.find('[', pos);
        if (pos == std::string::npos) return false;
        pos++;

        for (int c = 0; c < 3 && pos < data.size(); c++)
        {
            // Skip whitespace
            while (pos < data.size() && (data[pos] == ' ' || data[pos] == '\t' ||
                   data[pos] == '\n' || data[pos] == '\r'))
                pos++;

            char* end = nullptr;
            float val = strtof(data.c_str() + pos, &end);
            if (end == data.c_str() + pos) return false; // no float parsed

            out[r][c] = val;
            count++;
            pos = end - data.c_str();

            // Skip comma/whitespace after value
            while (pos < data.size() && (data[pos] == ',' || data[pos] == ' ' ||
                   data[pos] == '\t' || data[pos] == '\n' || data[pos] == '\r'))
                pos++;
        }

        // Skip past row closing ']' and any comma/whitespace
        if (pos < data.size() && data[pos] == ']') pos++;
        while (pos < data.size() && (data[pos] == ',' || data[pos] == ' ' ||
               data[pos] == '\t' || data[pos] == '\n' || data[pos] == '\r'))
            pos++;
    }

    return (count == 9);
}

// Helper: extract a JSON string value for a given key from openvrpaths.vrpath
// Looks for "key" : [ "value" ] and returns the first value with escaped backslashes cleaned.
static std::string ExtractVrPathValue(const std::string& content, const char* key)
{
    std::string searchKey = std::string("\"") + key + "\"";
    size_t keyPos = content.find(searchKey);
    if (keyPos == std::string::npos) return "";

    size_t bracketPos = content.find('[', keyPos);
    if (bracketPos == std::string::npos) return "";

    size_t q1 = content.find('"', bracketPos + 1);
    size_t q2 = content.find('"', q1 + 1);
    if (q1 == std::string::npos || q2 == std::string::npos) return "";

    std::string raw = content.substr(q1 + 1, q2 - q1 - 1);

    // Clean escaped backslashes and forward slashes
    std::string clean;
    for (size_t i = 0; i < raw.size(); i++)
    {
        if (raw[i] == '\\' && i + 1 < raw.size() && raw[i + 1] == '\\')
        {
            clean += '\\';
            i++;
        }
        else if (raw[i] == '/')
        {
            clean += '\\';
        }
        else
        {
            clean += raw[i];
        }
    }
    return clean;
}

// --- IPD methods ---

bool DeviceProvider::ReadEyeToHeadFromConfigFile(const std::string& configPath)
{
    std::ifstream cfgStream(configPath);
    if (!cfgStream.is_open())
    {
        DriverLog("IPD: Failed to open config file: %s\n", configPath.c_str());
        return false;
    }

    std::string cfgContent((std::istreambuf_iterator<char>(cfgStream)),
                            std::istreambuf_iterator<char>());
    cfgStream.close();

    DriverLog("IPD: Read config file (%zu bytes): %s\n", cfgContent.size(), configPath.c_str());

    // Parse tracking_to_eye_transform -> eye_to_head for both eyes
    size_t tetPos = cfgContent.find("tracking_to_eye_transform");
    if (tetPos == std::string::npos)
    {
        DriverLog("IPD: No tracking_to_eye_transform in config file\n");
        return false;
    }

    float eye0Rot[3][3] = {};
    float eye1Rot[3][3] = {};

    size_t eth0Pos = cfgContent.find("eye_to_head", tetPos);
    if (eth0Pos == std::string::npos)
    {
        DriverLog("IPD: No eye_to_head[0] in config file\n");
        return false;
    }

    if (!ParseEyeToHead3x3(cfgContent, eth0Pos, eye0Rot))
    {
        DriverLog("IPD: Failed to parse eye_to_head[0] matrix\n");
        return false;
    }

    size_t eth1Pos = cfgContent.find("eye_to_head", eth0Pos + 11);
    if (eth1Pos == std::string::npos)
    {
        DriverLog("IPD: No eye_to_head[1] in config file\n");
        return false;
    }

    if (!ParseEyeToHead3x3(cfgContent, eth1Pos, eye1Rot))
    {
        DriverLog("IPD: Failed to parse eye_to_head[1] matrix\n");
        return false;
    }

    // Validate eye order: left eye should have positive yaw, right negative
    // Intrinsic XYZ: yaw = atan2(R[0][2], sqrt(R[0][0]^2 + R[0][1]^2))
    float yaw0 = atan2f(eye0Rot[0][2],
        sqrtf(eye0Rot[0][0] * eye0Rot[0][0] + eye0Rot[0][1] * eye0Rot[0][1]));

    if (yaw0 > 0.0f)
    {
        // eye0 has positive yaw -> left eye (correct order)
        memcpy(m_cachedLeftRot, eye0Rot, sizeof(float) * 9);
        memcpy(m_cachedRightRot, eye1Rot, sizeof(float) * 9);
    }
    else
    {
        // eye0 has negative yaw -> swap (eye0 is right, eye1 is left)
        DriverLog("IPD: WARNING eye order swapped in config, correcting\n");
        memcpy(m_cachedLeftRot, eye1Rot, sizeof(float) * 9);
        memcpy(m_cachedRightRot, eye0Rot, sizeof(float) * 9);
    }

    m_bLhConfigLoaded = true;

    // Log Euler angles for diagnostics (intrinsic XYZ decomposition)
    float lPitch = atan2f(-m_cachedLeftRot[1][2], m_cachedLeftRot[2][2]) * 180.0f / kPi;
    float lYaw   = atan2f(m_cachedLeftRot[0][2], sqrtf(m_cachedLeftRot[0][0]*m_cachedLeftRot[0][0] + m_cachedLeftRot[0][1]*m_cachedLeftRot[0][1])) * 180.0f / kPi;
    float lRoll  = atan2f(-m_cachedLeftRot[0][1], m_cachedLeftRot[0][0]) * 180.0f / kPi;

    float rPitch = atan2f(-m_cachedRightRot[1][2], m_cachedRightRot[2][2]) * 180.0f / kPi;
    float rYaw   = atan2f(m_cachedRightRot[0][2], sqrtf(m_cachedRightRot[0][0]*m_cachedRightRot[0][0] + m_cachedRightRot[0][1]*m_cachedRightRot[0][1])) * 180.0f / kPi;
    float rRoll  = atan2f(-m_cachedRightRot[0][1], m_cachedRightRot[0][0]) * 180.0f / kPi;

    DriverLog("IPD: Cached rotations L pitch=%.2f yaw=%.2f roll=%.2f  R pitch=%.2f yaw=%.2f roll=%.2f\n",
              lPitch, lYaw, lRoll, rPitch, rYaw, rRoll);

    return true;
}

bool DeviceProvider::LoadLighthouseConfig()
{
    // 1. Get tracking serial — prefer SteamVR property (most reliable), fall back to HID flash
    std::string serial;

    // Try Prop_SerialNumber_String on HMD container (set by lighthouse driver, e.g. "LHR-1F8E25F1")
    {
        vr::PropertyContainerHandle_t hmdProps =
            vr::VRProperties()->TrackedDeviceToPropertyContainer(vr::k_unTrackedDeviceIndex_Hmd);
        vr::ETrackedPropertyError propErr;
        char serialBuf[64] = {};
        vr::VRProperties()->GetStringProperty(hmdProps, vr::Prop_SerialNumber_String,
            serialBuf, sizeof(serialBuf), &propErr);
        if (propErr == vr::TrackedProp_Success && serialBuf[0] != '\0')
        {
            serial = serialBuf;
            DriverLog("IPD: Got serial from SteamVR property: %s\n", serial.c_str());
        }
        else
        {
            DriverLog("IPD: Prop_SerialNumber_String not available (err=%d)\n", propErr);
        }
    }

    // Fall back to HID user flash tag 0x09 if property not available
    if (serial.empty())
    {
        serial = m_pHidDevice ? m_pHidDevice->GetTrackingSerial() : "";
        if (!serial.empty())
            DriverLog("IPD: Got serial from HID flash: %s\n", serial.c_str());
        else
            DriverLog("IPD: No tracking serial available from any source\n");
    }

    if (serial.empty())
        return false;

    m_sTrackingSerial = serial;

    // 2. Read openvrpaths.vrpath to find config directory
    char localAppData[MAX_PATH] = {};
    if (!GetEnvironmentVariableA("LOCALAPPDATA", localAppData, MAX_PATH))
    {
        DriverLog("IPD: Failed to get LOCALAPPDATA\n");
        return false;
    }

    std::string vrpathFile = std::string(localAppData) + "\\openvr\\openvrpaths.vrpath";
    std::ifstream vrpathStream(vrpathFile);
    if (!vrpathStream.is_open())
    {
        DriverLog("IPD: Failed to open %s\n", vrpathFile.c_str());
        return false;
    }

    std::string vrpathContent((std::istreambuf_iterator<char>(vrpathStream)),
                               std::istreambuf_iterator<char>());
    vrpathStream.close();

    std::string configDir = ExtractVrPathValue(vrpathContent, "config");
    if (configDir.empty())
    {
        DriverLog("IPD: No 'config' key in openvrpaths.vrpath\n");
        return false;
    }

    // 3. Lowercase the serial for folder matching
    std::string lowerSerial = serial;
    std::transform(lowerSerial.begin(), lowerSerial.end(), lowerSerial.begin(), ::tolower);

    // 4. Build config path: <config_dir>\lighthouse\<serial>\config.json
    std::string configPath = configDir + "\\lighthouse\\" + lowerSerial + "\\config.json";
    DriverLog("IPD: Attempting config load from: %s\n", configPath.c_str());

    // 5. Read and parse config file
    bool result = ReadEyeToHeadFromConfigFile(configPath);
    if (result)
    {
        DriverLog("IPD: Lighthouse config loaded successfully from %s\n", configPath.c_str());
    }
    else
    {
        DriverLog("IPD: Failed to load lighthouse config from %s\n", configPath.c_str());
    }

    return result;
}

bool DeviceProvider::LoadLighthouseConfigFromPath(const std::string& path)
{
    DriverLog("IPD: Loading lighthouse config from explicit path: %s\n", path.c_str());
    return ReadEyeToHeadFromConfigFile(path);
}

void DeviceProvider::HandleIpdSet(float mm, char* response, size_t responseSize)
{
    if (mm < 48.0f || mm > 75.0f)
    {
        snprintf(response, responseSize, "ERR ipd out of range (48-75mm)");
        return;
    }

    if (!ApplyIpd(mm))
    {
        snprintf(response, responseSize, "ERR lh_config not loaded");
        return;
    }

    snprintf(response, responseSize, "OK ipd=%.1fmm", mm);
}

void DeviceProvider::HandleIpdQuery(char* response, size_t responseSize)
{
    if (m_fCurrentIpd <= 0.0f)
    {
        snprintf(response, responseSize, "OK ipd=unknown");
    }
    else
    {
        snprintf(response, responseSize, "OK ipd=%.1fmm", m_fCurrentIpd * 1000.0f);
    }
}

void DeviceProvider::HandleLoadLhConfig(const char* path, char* response, size_t responseSize)
{
    if (LoadLighthouseConfigFromPath(std::string(path)))
    {
        snprintf(response, responseSize, "OK lh_config=loaded");
    }
    else
    {
        snprintf(response, responseSize, "ERR failed to read config: file not found or parse error");
    }
}

// --- Spike: proximity handle discovery ---

// Debug: test_handle command (retained from spike for diagnostics)
void DeviceProvider::HandleTestHandle(uint64_t handle, bool value, char* response, size_t responseSize)
{
    vr::EVRInputError err = vr::VRDriverInput()->UpdateBooleanComponent(
        (vr::VRInputComponentHandle_t)handle, value, 0.0);

    DriverLog("TestHandle: handle=%llu value=%s err=%d\n",
              handle, value ? "true" : "false", (int)err);

    snprintf(response, responseSize, "OK handle=%llu value=%s err=%d",
             handle, value ? "true" : "false", (int)err);
}

// --- IPD Persistence (Phase 13) ---

#ifdef ENABLE_IPD_PERSIST

std::string DeviceProvider::FindLighthouseConsole()
{
    // Read openvrpaths.vrpath to find SteamVR runtime directory
    char localAppData[MAX_PATH] = {};
    DWORD len = GetEnvironmentVariableA("LOCALAPPDATA", localAppData, MAX_PATH);
    if (len == 0 || len >= MAX_PATH)
    {
        DriverLog("IPD Persist: Failed to get LOCALAPPDATA\n");
        return "";
    }

    std::string vrpathFile = std::string(localAppData) + "\\openvr\\openvrpaths.vrpath";
    std::ifstream vrpathStream(vrpathFile);
    if (!vrpathStream.is_open())
    {
        DriverLog("IPD Persist: Failed to open %s\n", vrpathFile.c_str());
        return "";
    }

    std::string vrpathContent((std::istreambuf_iterator<char>(vrpathStream)),
                               std::istreambuf_iterator<char>());
    vrpathStream.close();

    std::string runtimeDir = ExtractVrPathValue(vrpathContent, "runtime");
    if (runtimeDir.empty())
    {
        DriverLog("IPD Persist: Could not extract runtime path from openvrpaths.vrpath\n");
        return "";
    }

    std::string exePath = runtimeDir + "\\tools\\lighthouse\\bin\\win64\\lighthouse_console.exe";
    if (!std::filesystem::exists(exePath))
    {
        DriverLog("IPD Persist: lighthouse_console.exe not found at %s\n", exePath.c_str());
        return "";
    }

    DriverLog("IPD Persist: Found lighthouse_console at %s\n", exePath.c_str());
    return exePath;
}

// Helper: spawn lighthouse_console, send serial + command, read stdout response,
// send exit, wait for process to finish. Follows HMDUtility write_to_stdin pattern.
// Returns true if process completed successfully.
static bool RunLighthouseCommand(const std::string& exePath,
                                 const std::string& serialCmd,
                                 const std::string& command)
{
    SECURITY_ATTRIBUTES sa = {};
    sa.nLength = sizeof(sa);
    sa.bInheritHandle = TRUE;

    HANDLE hStdinRead, hStdinWrite, hStdoutRead, hStdoutWrite;
    if (!CreatePipe(&hStdinRead, &hStdinWrite, &sa, 0) ||
        !SetHandleInformation(hStdinWrite, HANDLE_FLAG_INHERIT, 0))
        return false;

    if (!CreatePipe(&hStdoutRead, &hStdoutWrite, &sa, 0) ||
        !SetHandleInformation(hStdoutRead, HANDLE_FLAG_INHERIT, 0))
    {
        CloseHandle(hStdinRead);
        CloseHandle(hStdinWrite);
        return false;
    }

    STARTUPINFOA si = {};
    si.cb = sizeof(si);
    si.dwFlags = STARTF_USESTDHANDLES;
    si.hStdInput = hStdinRead;
    si.hStdOutput = hStdoutWrite;
    si.hStdError = hStdoutWrite;
    PROCESS_INFORMATION pi = {};

    if (!CreateProcessA(exePath.c_str(), NULL, NULL, NULL, TRUE,
                        CREATE_NO_WINDOW, NULL, NULL, &si, &pi))
    {
        DriverLog("IPD Persist: Failed to start lighthouse_console (err=%lu)\n", GetLastError());
        CloseHandle(hStdinRead); CloseHandle(hStdinWrite);
        CloseHandle(hStdoutRead); CloseHandle(hStdoutWrite);
        return false;
    }

    // Close child-side handles in parent (not needed after CreateProcess)
    CloseHandle(hStdinRead);
    CloseHandle(hStdoutWrite);

    auto writeStr = [&](const std::string& s) -> bool {
        DWORD written = 0;
        return WriteFile(hStdinWrite, s.c_str(), (DWORD)s.size(), &written, NULL) && written == s.size();
    };

    bool ok = false;

    // Send serial select command
    if (!writeStr(serialCmd))
        goto done;

    // Send the actual command (downloadconfig or uploadconfig)
    if (!writeStr(command))
        goto done;

    // Read stdout -- blocks until lighthouse_console produces output (command done)
    {
        char buf[4096];
        DWORD bytesRead = 0;
        if (!ReadFile(hStdoutRead, buf, sizeof(buf), &bytesRead, NULL))
            goto done;
    }

    ok = true;

done:
    // Send exit command and wait for process to finish (HMDUtility pattern)
    writeStr("exit\r\n");

    // Close stdin to signal EOF
    CloseHandle(hStdinWrite);

    // Poll for process exit (HMDUtility pattern: Sleep(1) loop)
    {
        DWORD exitCode;
        int iterations = 0;
        while (GetExitCodeProcess(pi.hProcess, &exitCode) &&
               exitCode == STILL_ACTIVE && iterations < 10000)
        {
            Sleep(1);
            iterations++;
        }
        if (exitCode == STILL_ACTIVE)
        {
            DriverLog("IPD Persist: WARNING -- lighthouse_console did not exit, terminating\n");
            TerminateProcess(pi.hProcess, 1);
        }
    }

    CloseHandle(hStdoutRead);
    CloseHandle(pi.hProcess);
    CloseHandle(pi.hThread);
    return ok;
}

void DeviceProvider::PersistIpdToConfig()
{
    // D-03: Change detection -- skip if IPD didn't change during session
    if (m_fCurrentIpd <= 0.0f || m_fStartupIpd <= 0.0f ||
        fabsf(m_fCurrentIpd - m_fStartupIpd) < 0.0001f)
    {
        DriverLog("IPD Persist: No change during session (startup=%.4f current=%.4f), skipping\n",
                  m_fStartupIpd, m_fCurrentIpd);
        return;
    }

    // SLIDER-04, D-09: Need tracking serial for lighthouse_console serial command
    if (m_sTrackingSerial.empty())
    {
        DriverLog("IPD Persist: WARNING -- tracking serial not available, cannot persist\n");
        return;
    }

    // D-06: Find lighthouse_console via openvrpaths.vrpath runtime key
    std::string exePath = FindLighthouseConsole();
    if (exePath.empty())
    {
        DriverLog("IPD Persist: lighthouse_console.exe not found, cannot persist\n");
        return;
    }

    float ipdMm = m_fCurrentIpd * 1000.0f;

    // Create temp file paths for download/upload
    char tempDir[MAX_PATH] = {};
    DWORD tempLen = GetTempPathA(MAX_PATH, tempDir);
    if (tempLen == 0 || tempLen >= MAX_PATH)
    {
        DriverLog("IPD Persist: Failed to get temp directory\n");
        return;
    }

    std::string dlPath = std::string(tempDir) + "beyond_ipd_dl.json";
    std::string ulPath = std::string(tempDir) + "beyond_ipd_ul.json";
    std::string serialCmd = "serial " + m_sTrackingSerial + "\r\n";

    // Delete temp files if they exist from a previous attempt
    DeleteFileA(dlPath.c_str());
    DeleteFileA(ulPath.c_str());

    // Step 1: Download config (separate process invocation, per HMDUtility pattern)
    DriverLog("IPD Persist: Downloading config for %s\n", m_sTrackingSerial.c_str());
    std::string dlCmd = "downloadconfig " + dlPath + "\r\n";
    if (!RunLighthouseCommand(exePath, serialCmd, dlCmd))
    {
        DriverLog("IPD Persist: downloadconfig failed\n");
        goto cleanup;
    }

    if (!std::filesystem::exists(dlPath))
    {
        DriverLog("IPD Persist: downloadconfig completed but file not created\n");
        goto cleanup;
    }

    // Step 2: Read and modify config
    {
        std::ifstream dlStream(dlPath);
        if (!dlStream.is_open())
        {
            DriverLog("IPD Persist: Failed to open downloaded config\n");
            goto cleanup;
        }

        std::string configStr((std::istreambuf_iterator<char>(dlStream)),
                               std::istreambuf_iterator<char>());
        dlStream.close();

        // Find "default_mm" and replace its value
        size_t pos = configStr.find("\"default_mm\"");
        if (pos == std::string::npos)
        {
            DriverLog("IPD Persist: default_mm not found in config, skipping\n");
            goto cleanup;
        }

        size_t colonPos = configStr.find(':', pos);
        if (colonPos == std::string::npos)
        {
            DriverLog("IPD Persist: malformed config -- no colon after default_mm\n");
            goto cleanup;
        }

        // Skip whitespace after colon
        size_t valStart = colonPos + 1;
        while (valStart < configStr.size() &&
               (configStr[valStart] == ' ' || configStr[valStart] == '\t'))
            valStart++;

        // Find end of numeric value (digits, dot, minus)
        size_t valEnd = valStart;
        while (valEnd < configStr.size() &&
               (isdigit(configStr[valEnd]) || configStr[valEnd] == '.' || configStr[valEnd] == '-'))
            valEnd++;

        if (valEnd == valStart)
        {
            DriverLog("IPD Persist: malformed config -- no numeric value for default_mm\n");
            goto cleanup;
        }

        // Replace with new value (Pitfall 6: one decimal place)
        char buf[32];
        snprintf(buf, sizeof(buf), "%.1f", ipdMm);
        configStr.replace(valStart, valEnd - valStart, buf);

        // Write modified config to upload path
        std::ofstream ulStream(ulPath);
        if (!ulStream.is_open())
        {
            DriverLog("IPD Persist: Failed to create upload config file\n");
            goto cleanup;
        }
        ulStream << configStr;
        ulStream.close();
    }

    // Step 3: Upload config (separate process invocation, per HMDUtility pattern)
    {
        DriverLog("IPD Persist: Uploading modified config (%.1fmm)\n", ipdMm);
        std::string ulCmd = "uploadconfig " + ulPath + "\r\n";
        if (!RunLighthouseCommand(exePath, serialCmd, ulCmd))
        {
            DriverLog("IPD Persist: uploadconfig failed\n");
            goto cleanup;
        }
    }

    DriverLog("IPD Persist: Successfully wrote %.1fmm to headset config\n", ipdMm);

cleanup:
    DeleteFileA(dlPath.c_str());
    DeleteFileA(ulPath.c_str());
}

#endif // ENABLE_IPD_PERSIST