#include "msbc_decoder_port.h"

static struct sbc_decoder_state sbc_internal_decoder_state = {0};

static void msbc_append_received_data(struct sbc_decoder_state * state, uint8_t * buffer, int size)
{
    int numFreeBytes = sizeof(state->frame_buffer) - state->bytes_in_frame_buffer;

    if (size > numFreeBytes) {
        printf("SBC data: more bytes read %u than free bytes in buffer %u", size, numFreeBytes);
    }

    (void)memcpy(state->frame_buffer + state->bytes_in_frame_buffer, buffer,
                 size);
    state->bytes_in_frame_buffer += size;
}

// returns position of mSBC sync word
static int msbc_find_h2_sync(const uint8_t *frame_data, OI_UINT32 frame_bytes, int * sync_word_nr)
{
    int syncword = mSBC_SYNCWORD;
    uint8_t h2_first_byte = 0;
    uint8_t h2_second_byte = 0;

    unsigned int i;
    for (i=0; i<frame_bytes; i++) {
        if (frame_data[i] == syncword) {
            // check: first byte == 1
            if (h2_first_byte == 1) {
                // check lower nibble of second byte == 0x08
                uint8_t ln = h2_second_byte & 0x0F;
                if (ln == 8) {
                    // check if bits 0+2 == bits 1+3
                    uint8_t hn = h2_second_byte >> 4;
                    if  ( ((hn>>1) & 0x05) == (hn & 0x05) ) {
                        *sync_word_nr = ((hn & 0x04) >> 1) | (hn & 0x01);
                        return i;
                    }
                }
            }
        }
        h2_first_byte = h2_second_byte;
        h2_second_byte = frame_data[i];
    }
    return -1;
}

static int msbc_find_sequence_of_zeros(const uint8_t *frame_data, OI_UINT32 frame_bytes, 
                                    int seq_length)
{
    int zero_seq_count = 0;
    unsigned int i;
    for (i=0; i<frame_bytes; i++) {
        if (frame_data[i] == 0) {
            zero_seq_count++;
            if (zero_seq_count >= seq_length) 
                return zero_seq_count;
        } else {
            zero_seq_count = 0;
        }
    }
    return 0;
}


static void msbc_decoder_drop_processed_bytes(struct sbc_decoder_state * decoder_state, uint16_t bytes_processed)
{
    memmove(decoder_state->frame_buffer, decoder_state->frame_buffer + bytes_processed, 
                decoder_state->bytes_in_frame_buffer-bytes_processed);

    decoder_state->bytes_in_frame_buffer -= bytes_processed;
}

static int16_t msbc_crop_sample(float val){
    float croped_val = val;
    if (croped_val > 32767.0)  croped_val= 32767.0;
    if (croped_val < -32768.0) croped_val=-32768.0;
    return (int16_t) croped_val;
}

void msbc_plc_good_frame(struct sbc_plc_state *plc_state, int16_t *in, int16_t *out){
    float val;
    int i = 0;
    plc_state->good_frames_nr++;
    plc_state->frame_count++;

#ifdef OCTAVE_OUTPUT
    FILE * oct_file = NULL;
    if (plc_state->nbf>0){
        oct_file = open_octave_file(plc_state, OCTAVE_FRAME_TYPE_GOOD);
        if (oct_file){
            octave_fprintf_plot_history_frame(plc_state, oct_file, plc_state->frame_count);
        }
    }
#endif

    if (plc_state->nbf>0) {
        for (i=0;i<SBC_RT;i++) {
            out[i] = plc_state->hist[SBC_LHIST+i];
        }
            
        for (i = SBC_RT;i<(SBC_RT+SBC_OLAL);i++) {
            float left  = plc_state->hist[SBC_LHIST+i];
            float right = in[i];  
            val = (left*rcos[i-SBC_RT]) + (right*rcos[SBC_OLAL+SBC_RT-1-i]);
            out[i] = msbc_crop_sample(val);
        }
    }

    for (;i<SBC_FS;i++) {
        out[i] = in[i];
    }
    // Copy the output to the history buffer
    for (i=0;i<SBC_FS;i++) {
        plc_state->hist[SBC_LHIST+i] = out[i];
    }

    // shift the history buffer
    for (i=0;i<SBC_LHIST;i++) {
        plc_state->hist[i] = plc_state->hist[i+SBC_FS];
    }

#ifdef OCTAVE_OUTPUT
    if (oct_file){
        octave_fprintf_plot_output(plc_state, oct_file);
        fclose(oct_file);
    }
#endif
    plc_state->nbf=0;
}

static int msbc_decoder_get_num_samples_per_frame(struct sbc_decoder_state * state)
{
    return state->decoder_context.common.frameInfo.nrof_blocks * state->decoder_context.common.frameInfo.nrof_subbands;
}

static int msbc_decoder_get_num_channels(struct sbc_decoder_state * state)
{
    return state->decoder_context.common.frameInfo.nrof_channels;
}

static int msbc_decoder_get_sample_rate(struct sbc_decoder_state * state)
{
    return state->decoder_context.common.frameInfo.frequency;
}

void msbc_decoder_process_msbc_data(int packet_status_flag, uint8_t * buffer, int size)
{
    int h2_syncword = 0,h2_sync_pos = 0,zero_seq_found = 0,bytes_missing = 0,bytes_to_append = 0;
    int input_bytes = size;
    int bad_frame = 0;
    uint16_t undecoding_bytes = 0,bytes_processed = 0;
    const uint8_t *frame_data;
    const unsigned int MSBC_FRAME_SIZE = 60;
    struct sbc_decoder_state * decoder_state = &sbc_internal_decoder_state;

    while (input_bytes > 0) {
        bytes_missing = MSBC_FRAME_SIZE - decoder_state->bytes_in_frame_buffer;
        bytes_to_append = (input_bytes < bytes_missing ? input_bytes : bytes_missing);
        if (bytes_to_append) {
            msbc_append_received_data(decoder_state, buffer, bytes_to_append);
            buffer += bytes_to_append;
            input_bytes -= bytes_to_append;
        }
        if (decoder_state->bytes_in_frame_buffer < MSBC_FRAME_SIZE) 
            break;
        undecoding_bytes = decoder_state->bytes_in_frame_buffer;

        frame_data = decoder_state->frame_buffer;

        // testing only - corrupt frame periodically
        //btstack_sbc_decoder_bluedroid_simulate_error(frame_data);

        // assert frame looks like this: 01 x8 AD [rest of frame 56 bytes] 00
        h2_sync_pos = msbc_find_h2_sync(frame_data, decoder_state->bytes_in_frame_buffer, &h2_syncword);
        if (h2_sync_pos < 0) {
            // no sync found, discard all but last 2 bytes
            bytes_processed = decoder_state->bytes_in_frame_buffer - 2;
            msbc_decoder_drop_processed_bytes(decoder_state, bytes_processed);
            // don't try PLC without at least a single good frame
            if (decoder_state->first_good_frame_found){
                decoder_state->msbc_bad_bytes += bytes_processed;
            }
            continue;
        }

        zero_seq_found = msbc_find_sequence_of_zeros(frame_data, decoder_state->bytes_in_frame_buffer, 20);
        // after first valid frame, zero sequences count as bad frames
        if (decoder_state->first_good_frame_found) {
            bad_frame = zero_seq_found || packet_status_flag;
        }

        if (bad_frame) {
            // stats
            if (zero_seq_found) {
                decoder_state->zero_frames_nr++;
            } else {
                decoder_state->bad_frames_nr++;
            }
#ifdef LOG_FRAME_STATUS 
            if (zero_seq_found) {
                printf("%d : ZERO FRAME\n", decoder_state->h2_sequence_nr);
            } else {
                printf("%d : BAD FRAME\n", decoder_state->h2_sequence_nr);
            }
#endif
            // retry after dropping 3 byte sync
            bytes_processed = 3;
            msbc_decoder_drop_processed_bytes(decoder_state, bytes_processed);
            decoder_state->msbc_bad_bytes += bytes_processed;
            // log_info("Trace bad frame");
            continue;
        }

        //ready to decode frame
        OI_STATUS status = OI_CODEC_SBC_DecodeFrame(&(decoder_state->decoder_context),
                                            &frame_data,
                                            &(decoder_state->bytes_in_frame_buffer),
                                            decoder_state->pcm_plc_data,
                                            &(decoder_state->pcm_bytes));

        bytes_processed = undecoding_bytes - decoder_state->bytes_in_frame_buffer;
        // log_info("Trace decode status %u, processed %u (bad bytes %u), bytes in buffer %u", (int) status, bytes_processed, decoder_state->msbc_bad_bytes, decoder_state->bytes_in_frame_buffer);

        switch(status) {
            case 0:
                // synced
                decoder_state->first_good_frame_found = 1;

                // get rid of padding byte, not processed by SBC decoder
                decoder_state->bytes_in_frame_buffer = 0;

                // restart counting bad bytes
                decoder_state->msbc_bad_bytes = 0;

                // feed good frame into PLC history
                msbc_plc_good_frame(&decoder_state->plc_state,
                                decoder_state->pcm_plc_data, 
                                decoder_state->pcm_data);

                // deliver PCM data
                decoder_state->handle_pcm_data(decoder_state->pcm_data,
                                    msbc_decoder_get_num_samples_per_frame(decoder_state),
                                    msbc_decoder_get_num_channels(decoder_state),
                                    msbc_decoder_get_sample_rate(decoder_state),
                                    decoder_state->context);

                // stats
                decoder_state->good_frames_nr++;
                continue;

            case OI_CODEC_SBC_CHECKSUM_MISMATCH:
                // The next frame is somehow corrupt.
                printf("OI_CODEC_SBC_CHECKSUM_MISMATCH");
                // Did the codec consume any bytes?
                if (bytes_processed > 0){
                    // Good. Nothing to do.
                } else {
                    // Skip the bogus frame by skipping the header.
                    bytes_processed = 1;
                }
                break;

            case OI_STATUS_INVALID_PARAMETERS:
                // This caused by corrupt frames.
                // The codec apparently does not recover from this.
                // Re-initialize the codec.
                printf("SBC decode: invalid parameters: resetting codec");
                if (OI_CODEC_mSBC_DecoderReset(&(decoder_state->decoder_context), 
                                        decoder_state->decoder_data, 
                                        sizeof(decoder_state->decoder_data)) != OI_STATUS_SUCCESS) {
                    printf("SBC decode: resetting codec failed");
                }
                break;
            default:
                printf("Frame decode error: %d", status);
                break;
        }

        // on success, while loop was restarted, so all processed bytes have been "bad"
        decoder_state->msbc_bad_bytes += bytes_processed;

        // drop processed bytes from frame buffer
        msbc_decoder_drop_processed_bytes(decoder_state, bytes_processed);
    }
}

int msbc_decoder_init(msbc_decoder_data_cb cb)
{
    struct sbc_decoder_state * decoder_state = &sbc_internal_decoder_state;
    OI_STATUS status = OI_STATUS_SUCCESS;

    status = OI_CODEC_mSBC_DecoderReset(&(decoder_state->decoder_context), 
                            decoder_state->decoder_data, 
                            sizeof(decoder_state->decoder_data));

    decoder_state->bytes_in_frame_buffer = 0;
    decoder_state->pcm_bytes = sizeof(decoder_state->pcm_data);
    decoder_state->h2_sequence_nr = -1;
    decoder_state->first_good_frame_found = 0;
    decoder_state->handle_pcm_data = cb;

    decoder_state->context = NULL;
    decoder_state->plc_state.nbf = 0;
    decoder_state->plc_state.bestlag = 0;
    memset(decoder_state->plc_state.hist,0,sizeof(decoder_state->plc_state.hist));

    return (int)status;
}