/**
 * Audio processing blocks for channel simulation.
 *
 * @copyright 2008, Research In Motion Ltd.
 *
 * @version $Id: //depot/main/dsp/ecall/c/testlab/audio.c#15 $
 */

#include <string.h>
#include <stdio.h>
#include "rimtypes.h"
#include "audio.h"
#include "error.h"
#include "process.h"

/* Header length in bytes for IF1 codec frames */
#define HEADER_LENGTH 2

void DelayInit(DelayState *state, Ord16 nSamples)
{
    int i;
    
    if(nSamples > MAX_DELAY) nSamples = MAX_DELAY;
    
    for(i = 0; i < nSamples; i++) {
        state->buffer[i] = 0;
    }
    
    state->read = 0;
    state->write = nSamples;
}

void DelayProcess(DelayState *state, Int16 *samples)
{   
    int i;
    for(i = 0; i < PCM_LENGTH; i++)
    {
        state->buffer[state->write++] = samples[i];
        if(state->write == DELAY_BUF_SIZE) state->write = 0;
    }

    for(i = 0; i < PCM_LENGTH; i++)
    {
        samples[i] = state->buffer[state->read++];
        if(state->read == DELAY_BUF_SIZE) state->read = 0;
    }
}

void BiquadInit(BiquadState *state, const float a[3], const float b[3])
{
    int i;
    
    for(i = 0; i < 3; i++)
    {    
        state->inCoef[i] = b[i];
        state->outCoef[i] = a[i];
    }
    state->inHistory[0] = 0;
    state->outHistory[0] = 0;

    state->inHistory[1] = 0;
    state->outHistory[1] = 0;
}   

void BiquadProcess(BiquadState *state, Int16 *samples)
{   
    float output;
    int i;

    for( i = 0; i < PCM_LENGTH; i++)
    {

        output = samples[i] * state->inCoef[0] +
                     state->inHistory[1] * state->inCoef[1] +
                     state->inHistory[0] * state->inCoef[2] -
                     state->outHistory[1]* state->outCoef[1] -
                     state->outHistory[0]* state->outCoef[2];

        state->outHistory[0] = state->outHistory[1];
        state->inHistory[0] = state->inHistory[1];
        state->outHistory[1] = output;
        state->inHistory[1] = samples[i]; 
        
        /* Saturate */
        if(output > 32767) {
            output = 32767;
        }
        else if(output < -32768) {
            output = -32768;
        }
        samples[i] = output;
    }
}   

static BOOL GenCodecCmd(
    char *cmd, 
    CodecType codecType, 
    CodecMode codecMode, 
    VadType vadType,
    const char *encDec) 
{
    const char *type;
    const char *mode;
    const char *vad;
    const char *dtx;

    switch (codecType) {
        case CodecTypeFr:      type = "fr";  break;
        case CodecTypeAmr:     type = "amr"; break;
        default: return FALSE;
    }
    switch (codecMode) {
        case CodecModeAmr12_2: mode = "-m 7"; break;
        case CodecModeAmr10_2: mode = "-m 6"; break;
        case CodecModeAmr7_95: mode = "-m 5"; break;
        case CodecModeAmr7_4:  mode = "-m 4"; break;
        case CodecModeAmr6_7:  mode = "-m 3"; break;
        case CodecModeAmr5_9:  mode = "-m 2"; break;
        case CodecModeAmr5_15: mode = "-m 1"; break;
        case CodecModeAmr4_75: mode = "-m 0"; break;
        case CodecModeNa:      mode = "";     break;
        default: return FALSE;
    }
    if (codecType == CodecTypeAmr) {
        switch (vadType) {
            case VadTypeNone:   dtx = "";   vad = "_vad1"; break;
            case VadType1:      dtx = "-x"; vad = "_vad1"; break;
            case VadType2:      dtx = "-x"; vad = "_vad2"; break;
            default: return FALSE;
        }
    }
    else {
        vad = "";
        switch(vadType) {
            case VadTypeNone:   dtx = "";   break;
            case VadType1: 
            case VadType2:      dtx = "-x"; break;
            default: return FALSE;
        }
    }

    sprintf(cmd, "%s%s.exe %s %s %s", type, vad, encDec, dtx, mode);

    return TRUE;
}

static BOOL ParseHeader(
    CodecType codecType, 
    CodecMode codecMode, 
    const Ord8 *header, 
    int *length, 
    BOOL *speech)
{
    int tempDataLen;
    BOOL tempSpeech;

    /* Augmented IF1 format (see codec EXEs for details) */
    Ord8 type  = header[0] >> 4;
    Ord8 spare = header[1] & 0x1F;

    tempDataLen = -1;
    tempSpeech = FALSE;

    if(type == 15) {
        /* no data frame */
        tempDataLen = 0;
    }
    else if(codecType == CodecTypeFr && type == 12) {
        /* Extended frame type (GSM FR/HR) */
        switch(spare) {
            case 2:
                tempDataLen = 34; tempSpeech = TRUE; break;
            case 3:
                tempDataLen = 34; break;
        }
    }
    else if(codecType == CodecTypeAmr && type <= 8) {
        /* AMR frame type */
        switch(type) {
            case 0:
                if(codecMode != CodecModeAmr4_75) break;
                tempDataLen = 13; tempSpeech = TRUE; break;
            case 1:
                if(codecMode != CodecModeAmr5_15) break;
                tempDataLen = 14; tempSpeech = TRUE; break;
            case 2:
                if(codecMode != CodecModeAmr5_9) break;
                tempDataLen = 16; tempSpeech = TRUE; break;
            case 3:
                if(codecMode != CodecModeAmr6_7) break;
                tempDataLen = 18; tempSpeech = TRUE; break;
            case 4:
                if(codecMode != CodecModeAmr7_4) break;
                tempDataLen = 20; tempSpeech = TRUE; break;
            case 5:
                if(codecMode != CodecModeAmr7_95) break;
                tempDataLen = 21; tempSpeech = TRUE; break;
            case 6:
                if(codecMode != CodecModeAmr10_2) break;
                tempDataLen = 27; tempSpeech = TRUE; break;
            case 7:
                if(codecMode != CodecModeAmr12_2) break;
                tempDataLen = 32; tempSpeech = TRUE; break;
            case 8:
                tempDataLen = 6; break;
        };
    }

    if(tempDataLen >= 0) {
        if(length) *length = HEADER_LENGTH + tempDataLen;
        if(speech) *speech = tempSpeech;
        return TRUE;
    }
    else {
        return FALSE;
    }
}

BOOL CodecInit(
    CodecState *s, 
    CodecType codecType,
    CodecMode codecMode,
    VadType vadType,
    const char *errorPattern,
    Ord32 errorPatternIndex)
{
    char enc[50];
    char dec[50];

    s->codecType = codecType;
    s->codecMode = codecMode;

    if(codecType == CodecTypeNone || codecType == CodecTypeBypass) {
        /* Do nothing */
        s->errorFile = NULL;
        return TRUE;
    }

    if(!GenCodecCmd(enc, codecType, codecMode, vadType, "-e")) {
        LogError("Invalid codec encoder parameters");
        return FALSE;
    }
    if(!GenCodecCmd(dec, codecType, codecMode, vadType, "-d")) {
        LogError("Invalid codec decoder parameters");
        return FALSE;
    }
    if(!ProcessOpen(&s->e, enc)) {
        LogError("Couldn't start encoder '%s'", enc);
        return FALSE;
    }
    if(!ProcessOpen(&s->d, dec)) {
        LogError("Couldn't start decoder '%d'", dec);
        ProcessClose(&s->e);
        return FALSE;
    }

    LogInfo("Encoder: %s", enc);
    LogInfo("Decoder: %s", dec);

    /* '-' indicates no error pattern is to be used */
    if(errorPattern != NULL && strcmp(errorPattern, "-") != 0) {
        int nFrames;
        int length;

        s->errorFile = fopen(errorPattern, "rb");
        if(!s->errorFile) {
            LogError("Failed to open error pattern '%s'", errorPattern);
            goto error;
        }

        LogInfo("Opened error pattern %s", errorPattern);

        /* 
         * Verify the integrity of the entire pattern: 
         * it's slower, but we catch any errors right away 
         */
        nFrames = 0;
        while(1) {
            Ord8 header[2];
            BOOL speech;

            if(fread(header, 1, HEADER_LENGTH, s->errorFile) == 0) {
                break;
            }
            if(!ParseHeader(codecType, codecMode, header, &length, &speech)) {
                LogError("%s does not match codec type/mode", errorPattern);
                goto error;
            }
            if(!speech) {
                LogError("Error pattern %s contains non-speech frames");
                goto error;
            }
            if(fseek(s->errorFile, length - HEADER_LENGTH, SEEK_CUR) != 0) {
                LogError("Error pattern %s is corrupt", errorPattern);
                goto error;
            }
            nFrames++;
        }

        /* Seek to the appropriate starting location */
        errorPatternIndex = (errorPatternIndex % nFrames);
        LogInfo("Starting at frame %u/%u in %s", 
            errorPatternIndex, nFrames, errorPattern);
        fseek(s->errorFile, errorPatternIndex * length, SEEK_SET);
    }
    else {
        s->errorFile = NULL;
    }

    return TRUE;
error:
    /* cleanup */
    if(s->errorFile) fclose(s->errorFile);
    ProcessClose(&s->e);
    ProcessClose(&s->d);
    return FALSE;
}   

void CodecProcess(CodecState *s, Int16 *samples)
{   
    /* Maximum frame size is 36 */
    Ord8 frameBits[36];
    int frameLen;
    BOOL speech;

    if(s->codecType == CodecTypeNone || s->codecType == CodecTypeBypass) {
        /* Do nothing */
        return;
    }

    /* Run encoder */
    ProcessWrite(&s->e, samples, PCM_LENGTH * sizeof(Int16));
    ProcessRead(&s->e, frameBits, HEADER_LENGTH);
    if(!ParseHeader(
        s->codecType, 
        s->codecMode, 
        frameBits, 
        &frameLen, 
        &speech)) 
    {
        Abort("Unexpected frame type received from encoder");
    }
    ProcessRead(&s->e, frameBits + HEADER_LENGTH, frameLen - HEADER_LENGTH);

    /* Apply error pattern */
    if(s->errorFile) {
        Ord8 errorBits[36];
        int errorLen;
        int length;

        length = fread(errorBits, 1, HEADER_LENGTH, s->errorFile);

        if(length == 0) {
            fseek(s->errorFile, 0, SEEK_SET);
            length = fread(errorBits, 1, HEADER_LENGTH, s->errorFile);
        }
        if(length != HEADER_LENGTH) {
            Abort("Failed to read error pattern frame header");
        }
        if(!ParseHeader(
            s->codecType, 
            s->codecMode, 
            errorBits, 
            &errorLen, 
            NULL))
        {
            Abort("Corrupt error pattern frame header");
        }
        length = fread(
            errorBits + HEADER_LENGTH, 
            1, 
            errorLen - HEADER_LENGTH, 
            s->errorFile);
        if(length != errorLen - HEADER_LENGTH) {
            Abort("Failed to read error pattern frame data");
        }

        /* Skip over frames that are not speech */
        if(speech) {
            int i;
            /* Copy the FQI from errorBits to frame */
            frameBits[0] = frameBits[0] & (~(1 << 3) | errorBits[0]);
            /* Filp bits of frame according to errorBits (exclude CRC) */
            for(i = HEADER_LENGTH + 1; i < errorLen; i++)  {
                frameBits[i] ^= errorBits[i];
            }
        }
    }

    /* Run decoder */
    ProcessWrite(&s->d, frameBits, frameLen);
    ProcessRead(&s->d, samples, PCM_LENGTH * sizeof(Int16));
}

void CodecFinal(CodecState *s)
{
    if(s->codecType == CodecTypeNone || s->codecType == CodecTypeBypass) {
        /* Do nothing */
        return;
    }

    ProcessClose(&s->e);
    ProcessClose(&s->d);
    if(s->errorFile) {
        fclose(s->errorFile);
        s->errorFile = NULL;
    }
}
