#include "protocol.h"
#include <math.h>

uint8_t crc8_atm(const uint8_t *data, size_t len) {
    uint8_t crc = CRC8_INIT;
    for (size_t i = 0; i < len; i++) {
        crc ^= data[i];
        for (uint8_t j = 0; j < 8; j++) {
            if (crc & 0x80) {
                crc = (crc << 1) ^ CRC8_POLY;
            } else {
                crc <<= 1;
            }
        }
    }
    return crc;
}

void protocol_pack_v1(
    uint8_t *buf,
    uint32_t timestamp_ms,
    float rms_dbfs,
    float freq_hz) {
    // Header
    buf[0] = PROTOCOL_SOF;
    buf[1] = PACKET_TYPE_AUDIO;
    buf[2] = PACKET_LEN_V1;

    // Payload: Timestamp (4 bytes, Little Endian)
    buf[3] = (uint8_t)(timestamp_ms & 0xFF);
    buf[4] = (uint8_t)((timestamp_ms >> 8) & 0xFF);
    buf[5] = (uint8_t)((timestamp_ms >> 16) & 0xFF);
    buf[6] = (uint8_t)((timestamp_ms >> 24) & 0xFF);

    // Payload: RMS_DB (2 bytes, Little Endian, x10, int16)
    // Range check implicit by int16 cast, but clamping is safer
    // Spec: -40..80 dB -> -400..800
    // Note: Since DSP returns dBFS (negative), we just send it as is.
    // E.g. -60.5 dB -> -605.
    int16_t rms_fixed = (int16_t)(rms_dbfs * 10.0f);
    buf[7] = (uint8_t)(rms_fixed & 0xFF);
    buf[8] = (uint8_t)((rms_fixed >> 8) & 0xFF);

    // Payload: FREQ_HZ (2 bytes, Little Endian, uint16)
    uint16_t freq_fixed = (uint16_t)freq_hz;
    buf[9] = (uint8_t)(freq_fixed & 0xFF);
    buf[10] = (uint8_t)((freq_fixed >> 8) & 0xFF);

    // CRC8 (Calculated over bytes 1..10: TYPE, LEN, Payload)
    buf[11] = crc8_atm(&buf[1], 10);
}