#pragma once
#include <stdint.h>

// ─────────────────────────────────────────────────────────────
// MAVLink v1 — HEARTBEAT (msg_id = 0)
// Trame complète : 17 octets
//
//  Byte  Field
//  0     STX       0xFE (MAVLink v1 magic)
//  1     LEN       9    (payload = 9 octets)
//  2     SEQ       séquence incrémentale
//  3     SYS_ID    system id (on se déclare comme GCS = 255)
//  4     COMP_ID   component id (GCS = 0)
//  5     MSG_ID    0 (HEARTBEAT)
//  6-14  PAYLOAD   9 octets (voir ci-dessous)
//  15-16 CRC       CRC-16/MCRF4XX avec CRC_EXTRA = 50
//
// Payload HEARTBEAT (9 octets, little-endian) :
//  [0-3] custom_mode        uint32  0x00000000
//  [4]   type               uint8   MAV_TYPE_GCS = 6
//  [5]   autopilot          uint8   MAV_AUTOPILOT_INVALID = 8
//  [6]   base_mode          uint8   0
//  [7]   system_status      uint8   MAV_STATE_ACTIVE = 4
//  [8]   mavlink_version    uint8   3
// ─────────────────────────────────────────────────────────────

// CRC-16/MCRF4XX (algorithme MAVLink)
static inline void mavCrcAccumulate(uint8_t data, uint16_t &crc) {
    uint8_t tmp = data ^ (uint8_t)(crc & 0xFF);
    tmp ^= (tmp << 4);
    crc = (crc >> 8) ^ ((uint16_t)tmp << 8) ^ ((uint16_t)tmp << 3) ^ ((uint16_t)tmp >> 4);
}

// Construit un paquet HEARTBEAT MAVLink v1 dans buf (doit faire >= 17 octets)
// Retourne la taille du paquet (toujours 17)
static inline uint8_t mavlinkBuildHeartbeat(uint8_t *buf, uint8_t &seq) {
    const uint8_t PAYLOAD_LEN  = 9;
    const uint8_t SYS_ID       = 255;  // GCS
    const uint8_t COMP_ID      = 0;    // GCS component
    const uint8_t MSG_ID       = 0;    // HEARTBEAT
    const uint8_t CRC_EXTRA    = 50;   // valeur fixe MAVLink pour msg #0

    // Header
    buf[0] = 0xFE;          // STX
    buf[1] = PAYLOAD_LEN;
    buf[2] = seq++;         // numéro de séquence (overflow géré par uint8_t)
    buf[3] = SYS_ID;
    buf[4] = COMP_ID;
    buf[5] = MSG_ID;

    // Payload (little-endian)
    buf[6]  = 0x00;  // custom_mode [0]
    buf[7]  = 0x00;  // custom_mode [1]
    buf[8]  = 0x00;  // custom_mode [2]
    buf[9]  = 0x00;  // custom_mode [3]
    buf[10] = 6;     // type = MAV_TYPE_GCS
    buf[11] = 8;     // autopilot = MAV_AUTOPILOT_INVALID
    buf[12] = 0;     // base_mode
    buf[13] = 4;     // system_status = MAV_STATE_ACTIVE
    buf[14] = 3;     // mavlink_version

    // CRC sur header (sans STX) + payload + CRC_EXTRA
    uint16_t crc = 0xFFFF;
    for (uint8_t i = 1; i <= 5 + PAYLOAD_LEN; i++) {
        mavCrcAccumulate(buf[i], crc);
    }
    mavCrcAccumulate(CRC_EXTRA, crc);

    buf[15] = crc & 0xFF;         // CRC low byte
    buf[16] = (crc >> 8) & 0xFF;  // CRC high byte

    return 17;
}