//#include "msbc_encoder_port.h"
#include "sbc_encoder.h"
#include "xc60xx.h"

#define mSBC_SYNCWORD 0xad
#define SBC_MAX_CHANNELS 2

struct msbc_encoder_state {
    SBC_ENC_PARAMS *context;
    int num_data_bytes;
    uint8_t sbc_packet[512];
};


static struct msbc_encoder_state encoder_state = {0};
static uint8_t msbc_encode_inited = 0;

int msbc_encoder_init(void)
{
    static SBC_ENC_PARAMS msbc_context = {
        .s16NumOfBlocks = 15,
        .s16NumOfSubBands = 8,
        .s16AllocationMethod = SBC_LOUDNESS,
        .s16BitPool = 26,
        .s16ChannelMode = SBC_MONO,
        .s16NumOfChannels = 1,
        .mSBCEnabled = 1,
        .s16SamplingFreq = SBC_sf16000,
    };
//    int status = 0;

    encoder_state.context = &msbc_context;
    encoder_state.context->pu8Packet = encoder_state.sbc_packet;

    SBC_Encoder_Init(encoder_state.context);
    msbc_encode_inited = 1;
    return 0;

}

int msbc_encoder_num_audio_frames(void)
{
    SBC_ENC_PARAMS * context = encoder_state.context;
    return context->s16NumOfSubBands * context->s16NumOfBlocks;
}

uint8_t * msbc_encoder_sbc_buffer(void)
{
    SBC_ENC_PARAMS * context = encoder_state.context;
    return context->pu8Packet;
}

uint16_t msbc_encoder_sbc_buffer_length(void)
{
    SBC_ENC_PARAMS * context = encoder_state.context;
    return context->u16PacketLength;
}

void /*__attribute__((section("ram_em")))*/ msbc_encoder_process_data(int16_t * input_buffer)
{
    if (!msbc_encode_inited) {
       printf("SBC encoder: sbc state is NULL, call btstack_sbc_encoder_init to initialize it\r\n");
    }
    SBC_ENC_PARAMS * context = encoder_state.context;
    context->ps16PcmBuffer = input_buffer;
    if (context->mSBCEnabled) {
       context->pu8Packet[0] = 0xad;
    }
    SBC_Encoder(context);
}