#include "Ltm.h"

namespace Ltm {

// Scrive header "$T<code>" in out, ritorna l'indice del primo byte payload (3).
static inline size_t header(uint8_t* out, char code) {
    out[0] = '$';
    out[1] = 'T';
    out[2] = (uint8_t)code;
    return 3;
}

// Calcola CRC (XOR del payload da [3] a [n-1]) e lo appende. Ritorna n+1.
static inline size_t finalise(uint8_t* out, size_t n) {
    uint8_t crc = 0;
    for (size_t i = 3; i < n; ++i) crc ^= out[i];
    out[n] = crc;
    return n + 1;
}

static inline void wU16(uint8_t* p, uint16_t v) { p[0] = v & 0xFF; p[1] = (v >> 8) & 0xFF; }
static inline void wU32(uint8_t* p, uint32_t v) {
    p[0] = v & 0xFF; p[1] = (v >> 8) & 0xFF; p[2] = (v >> 16) & 0xFF; p[3] = (v >> 24) & 0xFF;
}

size_t gframe(uint8_t* out, int32_t lat, int32_t lon,
              uint8_t gs_mps, int32_t alt_cm, uint8_t sats, uint8_t fix) {
    size_t n = header(out, 'G');
    wU32(&out[n], (uint32_t)lat);     n += 4;
    wU32(&out[n], (uint32_t)lon);     n += 4;
    out[n++] = gs_mps;
    wU32(&out[n], (uint32_t)alt_cm);  n += 4;
    out[n++] = (uint8_t)((sats << 2) | (fix & 0x03));
    return finalise(out, n);          // 18 byte
}

size_t aframe(uint8_t* out, int16_t pitch, int16_t roll, int16_t heading) {
    size_t n = header(out, 'A');
    wU16(&out[n], (uint16_t)pitch);   n += 2;
    wU16(&out[n], (uint16_t)roll);    n += 2;
    wU16(&out[n], (uint16_t)heading); n += 2;
    return finalise(out, n);          // 10 byte
}

size_t sframe(uint8_t* out, uint16_t vbat_mv, uint16_t current,
              uint8_t rssi, uint8_t airspeed,
              uint8_t flightmode, uint8_t statemode) {
    size_t n = header(out, 'S');
    wU16(&out[n], vbat_mv);  n += 2;
    wU16(&out[n], current);  n += 2;
    out[n++] = rssi;
    out[n++] = airspeed;
    out[n++] = (uint8_t)((flightmode << 2) | (statemode & 0x03));
    return finalise(out, n);          // 11 byte
}

} // namespace Ltm