#include "hid_device.h"
#include "user_signature.h"
#include "../driver/driverlog.h"
#include <string>
#include <thread>
#include <chrono>
#include <mutex>
#include <cstring>

HidDevice::HidDevice(int avgLength, bool logVerbose)
    : m_pDevice(nullptr)
    , m_proximityAlgorithm(avgLength)
    , m_logVerbose(logVerbose)
{
}

HidDevice::~HidDevice()
{
    StopReading();
}

bool HidDevice::Open(uint16_t vid, uint16_t pid)
{
    if (m_pDevice)
    {
        DriverLog("HID device already open, closing first\n");
        Close();
    }

    m_pDevice = hid_open(vid, pid, nullptr);
    if (!m_pDevice)
    {
        DriverLog("HID: Failed to open device %04X:%04X - %ls\n",
                  vid, pid, hid_error(nullptr));
        return false;
    }

    DriverLog("HID: Successfully opened device %04X:%04X\n", vid, pid);
    return true;
}

void HidDevice::Close()
{
    if (m_pDevice)
    {
        hid_close(m_pDevice);
        m_pDevice = nullptr;
        DriverLog("HID: Device closed\n");
    }
}

bool HidDevice::IsOpen() const
{
    return m_pDevice != nullptr;
}

// --- Reader thread API ---

void HidDevice::StartReading(uint16_t vid, uint16_t pid, uint16_t rateMs)
{
    m_bStopRequested.store(false);
    m_readerThread = std::thread(&HidDevice::ReaderThreadFunc, this, vid, pid, rateMs);
}

void HidDevice::StopReading()
{
    m_bStopRequested.store(true);
    if (m_readerThread.joinable())
        m_readerThread.join();
    Close();
    m_connectionState.store(0);
}

uint16_t HidDevice::GetProxDistance() const
{
    return m_lastProxDistance.load(std::memory_order_relaxed);
}

int HidDevice::GetConnectionState() const
{
    return m_connectionState.load(std::memory_order_relaxed);
}

CalibrationData HidDevice::GetCalibration() const
{
    std::lock_guard<std::mutex> lock(m_calMutex);
    return m_calibration;
}

std::string HidDevice::GetTrackingSerial() const
{
    std::lock_guard<std::mutex> lock(m_calMutex);
    return std::string(m_calibration.tracking_serial);
}

bool HidDevice::GetPersonDetected() const
{
    return m_proximityAlgorithm.GetPersonDetected();
}

ProximityAlgorithm::DiagState HidDevice::GetAlgorithmDiag() const
{
    return m_proximityAlgorithm.GetDiagState();
}

// --- HID communication helpers ---

bool HidDevice::SendFeatureReport(uint8_t cmdCode, const uint8_t* data, size_t dataLen)
{
    if (!m_pDevice)
        return false;

    uint8_t buf[65] = {0};  // report ID (0x00) + 64 bytes data
    buf[0] = 0x00;
    buf[1] = cmdCode;
    if (data && dataLen > 0)
        memcpy(&buf[2], data, dataLen < 62 ? dataLen : 62);

    int ret = hid_send_feature_report(m_pDevice, buf, sizeof(buf));
    if (ret < 0)
    {
        DriverLog("HID: Failed to send feature report cmd=0x%02X: %ls\n",
                  cmdCode, hid_error(m_pDevice));
        return false;
    }
    return true;
}

int HidDevice::ReadReport(uint8_t* buf, size_t bufLen, int timeoutMs)
{
    if (!m_pDevice)
        return -1;
    return hid_read_timeout(m_pDevice, buf, bufLen, timeoutMs);
}

bool HidDevice::SetReportRate(uint16_t rateMs)
{
    // Rate data is big-endian (MSB first)
    uint8_t data[2] = {
        static_cast<uint8_t>(rateMs >> 8),
        static_cast<uint8_t>(rateMs & 0xFF)
    };
    if (!SendFeatureReport('R', data, 2))
        return false;

    // Read response via interrupt IN
    uint8_t resp[65];
    int rlen = ReadReport(resp, sizeof(resp), 1000);
    if (rlen > 0 && resp[0] == '$')  // HID_CODE_FOR_SUCCESS_REPLY
    {
        DriverLog("HID: Report rate set to %ums\n", rateMs);
        return true;
    }
    else if (rlen > 0 && resp[0] == 'E')  // HID_CODE_FOR_ERROR_REPLY
    {
        DriverLog("HID: Rate command rejected by firmware\n");
        return false;
    }

    DriverLog("HID: Rate command - unexpected response (byte0=0x%02X, len=%d)\n",
              rlen > 0 ? resp[0] : 0, rlen);
    return false;
}

bool HidDevice::ReadUserSignature(uint8_t* sigOut, size_t sigLen)
{
    if (sigLen < 512)
        return false;

    for (int block = 0; block < 16; block++)
    {
        // Send READ_SIG command: 'U' + block number (matches HMDUtility command_read_config)
        uint8_t blockNum = static_cast<uint8_t>(block);
        if (!SendFeatureReport('U', &blockNum, 1))
        {
            DriverLog("HID: ReadUserSignature block %d send failed\n", block);
            return false;
        }

        // Read response -- may need to skip periodic '#' reports
        // Use 64-byte buffer matching HMDUtility's hid_read(device, buffer, 64)
        uint8_t resp[64];
        bool blockReceived = false;
        for (int attempts = 0; attempts < 20; attempts++)
        {
            int rlen = ReadReport(resp, sizeof(resp), 500);
            if (rlen < 0)
            {
                DriverLog("HID: ReadUserSignature block %d read error (disconnected)\n", block);
                return false;   // device disconnected
            }
            if (rlen == 0)
                continue;       // timeout, retry
            if (resp[0] == '#')
                continue;       // periodic data, skip
            if (resp[0] == 'U')
            {
                uint8_t chunkLen = resp[1];  // should be 32
                memcpy(&sigOut[block * 32], &resp[2], chunkLen);
                blockReceived = true;
                break;
            }
            if (resp[0] == 'E')
            {
                DriverLog("HID: ReadUserSignature block %d error response from firmware\n", block);
                return false;   // error from firmware
            }
            // Log unexpected response type
            DriverLog("HID: ReadUserSignature block %d unexpected resp[0]=0x%02X len=%d\n",
                      block, resp[0], rlen);
        }
        if (!blockReceived)
        {
            DriverLog("HID: ReadUserSignature block %d timed out (20 attempts)\n", block);
            return false;
        }
    }
    return true;
}

void HidDevice::ReadCalibration()
{
    uint8_t sig[512];
    if (ReadUserSignature(sig, sizeof(sig)))
    {
        CalibrationData cal = ParseCalibration(sig, sizeof(sig));
        {
            std::lock_guard<std::mutex> lock(m_calMutex);
            m_calibration = cal;
        }
        DriverLog("HID: Calibration: cal=%u thresh=%u hyst=%u trim=%d\n",
                  cal.programmed_cal, cal.proximity_threshold,
                  cal.proximity_hysteresis, cal.user_trim);

        // Log which values came from flash vs defaults
        CalibrationData defaults;
        if (cal.programmed_cal != defaults.programmed_cal)
            DriverLog("HID: programmed_cal=%u (from flash)\n", cal.programmed_cal);
        if (cal.proximity_threshold != defaults.proximity_threshold)
            DriverLog("HID: proximity_threshold=%u (from flash)\n", cal.proximity_threshold);
        if (cal.proximity_hysteresis != defaults.proximity_hysteresis)
            DriverLog("HID: proximity_hysteresis=%u (from flash)\n", cal.proximity_hysteresis);
        if (cal.user_trim != defaults.user_trim)
            DriverLog("HID: user_trim=%d (from flash)\n", cal.user_trim);
        if (cal.tracking_serial[0] != '\0')
            DriverLog("HID: tracking_serial=%s (from flash)\n", cal.tracking_serial);
    }
    else
    {
        DriverLog("HID: Warning - failed to read user signature, using defaults\n");
    }
}

// --- Reader thread ---

void HidDevice::ReaderThreadFunc(uint16_t vid, uint16_t pid, uint16_t rateMs)
{
    while (!m_bStopRequested.load())
    {
        // Attempt connection if device not open
        if (!m_pDevice)
        {
            m_connectionState.store(2);  // reconnecting
            m_pDevice = hid_open(vid, pid, nullptr);
            if (!m_pDevice)
            {
                // Reconnect sleep: 5 seconds in 50ms increments, checking stop flag
                for (int i = 0; i < 100 && !m_bStopRequested.load(); i++)
                    std::this_thread::sleep_for(std::chrono::milliseconds(50));
                continue;
            }
            m_connectionState.store(1);
            DriverLog("HID: Device opened/reconnected\n");

            // Read calibration first (before periodic reports start)
            ReadCalibration();

            // Reset proximity algorithm with fresh calibration
            m_proximityAlgorithm.Reset(GetCalibration());

            // Set report rate
            SetReportRate(rateMs);
        }

        // Read periodic reports
        uint8_t buf[65];
        int ret = hid_read_timeout(m_pDevice, buf, sizeof(buf), 500);
        if (ret == -1)
        {
            // Disconnect detected
            DriverLog("HID: Device disconnected\n");
            hid_close(m_pDevice);
            m_pDevice = nullptr;
            m_connectionState.store(2);  // reconnecting
            continue;
        }
        if (ret > 0 && buf[0] == '#' && ret >= 6)
        {
            // Periodic data: bytes 4-5 = prox_distance (big-endian uint16)
            uint16_t prox = (static_cast<uint16_t>(buf[4]) << 8) | buf[5];
            m_lastProxDistance.store(prox, std::memory_order_relaxed);
            m_proximityAlgorithm.ProcessSample(prox);
            if (m_logVerbose)
                DriverLog("HID: raw_prox=%u\n", prox);
        }
    }
}