#include "user_signature.h"
#include "../driver/driverlog.h"
#include <cstring>

uint8_t Crc8(const uint8_t* data, size_t len)
{
    uint8_t crc = 0xFF;
    for (size_t i = 0; i < len; i++)
    {
        crc ^= data[i];
        for (int bit = 0; bit < 8; bit++)
        {
            if (crc & 0x80)
                crc = (crc << 1) ^ 0x07;
            else
                crc = crc << 1;
        }
    }
    return crc;
}

CalibrationData ParseCalibration(const uint8_t* sig, size_t sigLen)
{
    CalibrationData cal;  // initialized with defaults
    size_t ptr = 0;

    while (ptr < sigLen)
    {
        uint8_t tag = sig[ptr];
        if (tag == 0xFF)
            break;  // end of valid tags (unprogrammed flash)

        if (ptr + 1 >= sigLen)
            break;
        uint8_t length = sig[ptr + 1];

        if (ptr + 2 + length >= sigLen)
            break;  // overrun

        uint8_t crcByte = sig[ptr + 2 + length];

        // Verify CRC over tag + length + value bytes
        uint8_t expectedCrc = Crc8(&sig[ptr], 2 + length);
        if (crcByte != expectedCrc)
        {
            DriverLog("HID: Sig tag 0x%02X CRC mismatch (got 0x%02X, expected 0x%02X)\n",
                      tag, crcByte, expectedCrc);
            ptr += 3 + length;
            continue;
        }

        // TLV values are little-endian (ARM native byte order on the MCU)
        if (length == 2)
        {
            uint16_t val16 = static_cast<uint16_t>(sig[ptr + 2]) |
                             (static_cast<uint16_t>(sig[ptr + 3]) << 8);
            switch (tag)
            {
                case 0x06: cal.programmed_cal = val16; break;
                case 0x0B: cal.proximity_threshold = val16; break;
                case 0x0C: cal.proximity_hysteresis = val16; break;
                case 0x0E: cal.user_trim = static_cast<int16_t>(val16); break;
            }
        }

        // Tag 0x09: tracking serial (variable-length ASCII string)
        if (tag == 0x09 && length < sizeof(cal.tracking_serial))
        {
            memcpy(cal.tracking_serial, &sig[ptr + 2], length);
            cal.tracking_serial[length] = '\0';
        }

        // Tag 0x08: HMD serial (variable-length ASCII string)
        if (tag == 0x08 && length < sizeof(cal.hmd_serial))
        {
            memcpy(cal.hmd_serial, &sig[ptr + 2], length);
            cal.hmd_serial[length] = '\0';
        }

        ptr += 3 + length;  // tag + length + value + crc
    }
    return cal;
}