/*
 * mqtt.c
 *
 *  Created on: 08/04/2026
 *      Author: carec
 */
#include "mqtt.h"
#include <string.h>

static uint16_t put_str(uint8_t *p, const char *s)
{
    uint16_t len = strlen(s);
    p[0] = len >> 8;
    p[1] = len & 0xFF;
    memcpy(&p[2], s, len);
    return 2 + len;
}

static uint16_t enc_len(uint8_t *out, uint32_t len)
{
    uint16_t i = 0;
    do {
        uint8_t b = len % 128;
        len /= 128;
        if (len > 0) b |= 0x80;
        out[i++] = b;
    } while (len);
    return i;
}

uint16_t mqtt_build_connect(uint8_t *out,
                            const char *client_id,
                            const char *username,
                            const char *password,
                            uint16_t keepalive)
{
    uint8_t tmp[256];
    uint16_t i = 0;

    i += put_str(&tmp[i], "MQTT");
    tmp[i++] = 0x04;

    uint8_t flags = 0x02;
    if (username) flags |= 0x80;
    if (password) flags |= 0x40;

    tmp[i++] = flags;
    tmp[i++] = keepalive >> 8;
    tmp[i++] = keepalive & 0xFF;

    i += put_str(&tmp[i], client_id);
    if (username) i += put_str(&tmp[i], username);
    if (password) i += put_str(&tmp[i], password);

    uint16_t o = 0;
    out[o++] = 0x10;
    o += enc_len(&out[o], i);
    memcpy(&out[o], tmp, i);
    o += i;

    return o;
}

bool mqtt_parse_connack(const uint8_t *buf, uint16_t len)
{
    if (len < 4) return false;
    if (buf[0] != 0x20) return false;
    if (buf[3] != 0x00) return false;
    return true;
}