/**
 * @file sbus.c
 * @brief SBUS decoder implementation
 */
#include <string.h>
#include "sbus.h"

void sbus_init(void)
{
    /* Nothing to initialize */
}

void sbus_decode(const sbus_frame_t *frame, uint16_t *channels, uint8_t *flags)
{
    if (frame->start_byte != 0x0F) {
        /* Invalid frame */
        if (flags) *flags = 0;
        return;
    }
    
    /* Unpack 16 × 11-bit channels from 22 bytes, little-endian */
    const uint8_t *data = frame->channel_data;
    for (int i = 0; i < 16; i++) {
        int byte_idx = (i * 11) / 8;
        int bit_off = (i * 11) % 8;
        
        uint16_t ch = data[byte_idx] >> bit_off;
        ch |= (uint16_t)(data[byte_idx + 1]) << (8 - bit_off);
        if (bit_off > 5) {
            ch |= (uint16_t)(data[byte_idx + 2]) << (16 - bit_off);
        }
        
        channels[i] = ch & 0x07FF;
    }
    
    /* Extract flags */
    if (flags) {
        *flags = frame->flags;
    }
}