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

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

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 < FRAME_SIZE; i++)
    {
        state->buffer[state->write++] = samples[i];
        if(state->write == DELAY_BUF_SIZE) state->write = 0;
    }

    for(i = 0; i < FRAME_SIZE; 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 < FRAME_SIZE; 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;

    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 VadType1: vad = "_vad1"; break;
            case VadType2: vad = "_vad2"; break;
            default: return FALSE;
        }
    }
    else {
        vad = "";
    }

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

    return TRUE;
}

static BOOL ParseHeader(
    CodecType codecType, 
    CodecMode codecMode, 
    const Ord8 *header, 
    int *dataLen, 
    BOOL *speech)
{
    /* Augmented IF1 format (see codec EXEs for details) */
    Ord8 type  = header[0] >> 4;
    Ord8 spare = header[1] & 0x1F;

    *dataLen = -1;
    *speech = FALSE;

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

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 length;
        int dataLen;

        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 
         */
        length = 0;
        while(1) {
            Ord8 header[2];
            BOOL speech;

            if(fread(header, sizeof(Ord8), 2, s->errorFile) == 0) {
                break;
            }
            if(!ParseHeader(codecType, codecMode, header, &dataLen, &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, dataLen, SEEK_CUR) != 0) {
                LogError("Error pattern %s is corrupt", errorPattern);
                goto error;
            }
            length++;
        }

        /* Seek to the appropriate starting location */
        errorPatternIndex = (errorPatternIndex % length);
        LogInfo("Starting at frame %u/%u in %s", 
            errorPatternIndex, length, errorPattern);
        fseek(s->errorFile, errorPatternIndex * (dataLen + 2), 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 frame[36];
    Ord8 errorBits[36];
    int dataLen;
    BOOL speech;

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

    /* Run encoder */
    ProcessWrite(&s->e, samples, FRAME_SIZE * 2);
    ProcessRead(&s->e, frame, 2);
    if(!ParseHeader(s->codecType, s->codecMode, frame, &dataLen, &speech)) {
        Abort("Unexpected frame type received from encoder");
    }
    ProcessRead(&s->e, frame + 2, dataLen);

    /* Apply error pattern */
    if(s->errorFile) {
        int length;

        length = fread(errorBits, sizeof(Ord8), dataLen + 2, s->errorFile);
        if(length == 0) {
            fseek(s->errorFile, 0, SEEK_SET);
            length = fread(errorBits, sizeof(Ord8), dataLen + 2, s->errorFile);
        }
        if(length != dataLen + 2) {
            Abort("Failed to read error pattern frame");
        }

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

        }
    }

    /* Run decoder */
    ProcessWrite(&s->d, frame, dataLen + 2);
    ProcessRead(&s->d, samples, FRAME_SIZE * 2);
}

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;
    }
}
