#include "trikke_ble_protocol.h"

#include <string.h>

#include "trikke_protocol.h"

static uint16_t get_u16_le(const uint8_t *input)
{
    return (uint16_t)input[0] | ((uint16_t)input[1] << 8);
}

static uint32_t get_u32_le(const uint8_t *input)
{
    return (uint32_t)input[0] | ((uint32_t)input[1] << 8) |
           ((uint32_t)input[2] << 16) | ((uint32_t)input[3] << 24);
}

static uint64_t get_u64_le(const uint8_t *input)
{
    return (uint64_t)get_u32_le(input) |
           ((uint64_t)get_u32_le(input + 4) << 32);
}

static void put_u16_le(uint8_t *output, uint16_t value)
{
    output[0] = (uint8_t)value;
    output[1] = (uint8_t)(value >> 8);
}

static void put_u32_le(uint8_t *output, uint32_t value)
{
    output[0] = (uint8_t)value;
    output[1] = (uint8_t)(value >> 8);
    output[2] = (uint8_t)(value >> 16);
    output[3] = (uint8_t)(value >> 24);
}

static bool packet_shape_is_valid(const uint8_t *packet, size_t packet_size)
{
    if (packet == NULL || packet_size < TRIKKE_WIRE_HEADER_SIZE ||
        packet_size > TRIKKE_WIRE_MAX_PACKET_SIZE ||
        memcmp(packet, "TRK1", 4) != 0 ||
        packet[4] != TRIKKE_WIRE_VERSION ||
        packet[6] != TRIKKE_WIRE_HEADER_SIZE) {
        return false;
    }
    const size_t encoded_size =
        TRIKKE_WIRE_HEADER_SIZE + get_u16_le(packet + 10);
    return encoded_size == packet_size;
}

size_t trikke_ble_encode_fragment(
    uint8_t *output,
    size_t output_size,
    const uint8_t *packet,
    size_t packet_size,
    size_t packet_offset,
    size_t att_payload_capacity)
{
    if (output == NULL || !packet_shape_is_valid(packet, packet_size) ||
        packet_offset >= packet_size || packet_size > UINT16_MAX ||
        packet_offset > UINT16_MAX ||
        att_payload_capacity <= TRIKKE_BLE_FRAGMENT_HEADER_SIZE) {
        return 0;
    }

    size_t data_size =
        att_payload_capacity - TRIKKE_BLE_FRAGMENT_HEADER_SIZE;
    const size_t remaining = packet_size - packet_offset;
    if (data_size > remaining) {
        data_size = remaining;
    }
    const size_t fragment_size = TRIKKE_BLE_FRAGMENT_HEADER_SIZE + data_size;
    if (output_size < fragment_size) {
        return 0;
    }

    put_u32_le(output, get_u32_le(packet + 12));
    put_u16_le(output + 4, (uint16_t)packet_offset);
    put_u16_le(output + 6, (uint16_t)packet_size);
    memcpy(output + TRIKKE_BLE_FRAGMENT_HEADER_SIZE,
           packet + packet_offset, data_size);
    return fragment_size;
}

bool trikke_ble_decode_ack(
    const uint8_t *ack,
    size_t ack_size,
    uint32_t *packet_sequence)
{
    if (ack == NULL || packet_sequence == NULL ||
        ack_size != TRIKKE_BLE_ACK_SIZE || memcmp(ack, "ACK1", 4) != 0) {
        return false;
    }
    *packet_sequence = get_u32_le(ack + 4);
    return true;
}

bool trikke_ble_decode_begin_session(
    const uint8_t *command,
    size_t command_size,
    uint64_t *session_token)
{
    if (command == NULL || session_token == NULL ||
        command_size != TRIKKE_BLE_BEGIN_SESSION_SIZE ||
        memcmp(command, "BGN1", 4) != 0) {
        return false;
    }
    *session_token = get_u64_le(command + 4);
    return true;
}

bool trikke_ble_update_subscription(
    bool notify_enabled,
    bool session_connection_ready,
    bool *subscribed)
{
    if (subscribed == NULL) {
        return false;
    }
    const bool effective = notify_enabled && session_connection_ready;
    const bool changed = effective != *subscribed;
    *subscribed = effective;
    return changed;
}