/**
 * Uplink and downlink channel simulation blocks.
 *
 * @copyright 2008, Research In Motion Ltd.
 *
 * @version $Id: //depot/main/dsp/ecall/c/testlab/channel.c#9 $
 */

#include <stdio.h>
#include "rimtypes.h"
#include "msg.h"
#include "ipc.h"
#include "testlab.h"
#include "audio.h"
#include "g711.h"

/* Identify uplink and downlink channels */
typedef enum {
    ChannelDirUl,
    ChannelDirDl,
} ChannelDir;

typedef struct _ChannelState {
    IpcConn     *conn;
    void        (*process)(struct _ChannelState *s, Int16 *samples);
    CodecType   codecType;
    CodecState  codec;
    DelayState  delay1;
    DelayState  delay2;
    BiquadState hpf;
    BiquadState lpf;
    BOOL        configured;
} ChannelState;

/* Separate state for uplink and downlink */
static ChannelState ulState;
static ChannelState dlState;

/*
[b, a] = butter(2, 300/4000, 'high');
fprintf('static const float HpfCoeffB[] = { ');
fprintf('%f, ', b);
fprintf('};\n');
fprintf('static const float HpfCoeffA[] = { ');
fprintf('%f, ', a);
fprintf('};\n');
*/
static const float HpfCoeffB[] = { 0.846459, -1.692919, 0.846459, };
static const float HpfCoeffA[] = { 1.000000, -1.669203, 0.716634, };

/*
[b, a] = butter(2, 3400/4000, 'low');
fprintf('static const float LpfCoeffB[] = { ');
fprintf('%f, ', b);
fprintf('};\n');
fprintf('static const float LpfCoeffA[] = { ');
fprintf('%f, ', a);
fprintf('};\n');
*/
static const float LpfCoeffB[] = { 0.715737, 1.431475, 0.715737, };
static const float LpfCoeffA[] = { 1.000000, 1.348968, 0.513982, };

static void UlProcess(ChannelState *s, Int16 *samples)
{
    if (s->codecType != CodecTypeBypass) {
        DelayProcess(&s->delay1, samples);
        CodecProcess(&s->codec, samples);
        alaw_compress(PCM_LENGTH, samples, samples);
        alaw_expand(PCM_LENGTH, samples, samples);
        BiquadProcess(&s->hpf, samples);
        BiquadProcess(&s->lpf, samples);
        DelayProcess(&s->delay2, samples);
    }
}

static void DlProcess(ChannelState *s, Int16 *samples)
{
    if (s->codecType != CodecTypeBypass) {
        BiquadProcess(&s->hpf, samples);
        BiquadProcess(&s->lpf, samples);
        alaw_compress(PCM_LENGTH, samples, samples);
        alaw_expand(PCM_LENGTH, samples, samples);
        DelayProcess(&s->delay1, samples);
        CodecProcess(&s->codec, samples);
        DelayProcess(&s->delay2, samples);
    }
}

static void ChannelInit(
    ChannelState *s,
    ChannelDir dir,
    const char *host,
    const char *port)
{
    s->conn = IpcConnect(host, port);
    if(!s->conn) {
        Abort("Failed to connect to %s:%s", host, port);
    }

    if(dir == ChannelDirUl) {
        s->process = UlProcess;
    }
    else {
        s->process = DlProcess;
    }
    s->configured = FALSE;
}

static BOOL ChannelRun(ChannelState *s)
{
    Msg msg;

    /* Early exit if no message */
    if (!IpcRecv(s->conn, &msg)) return FALSE;

    if (msg.id == MsgChReset) {
        if(s->configured) CodecFinal(&s->codec);

        DelayInit(&s->delay1, msg.u.chReset.delay1);
        DelayInit(&s->delay2, msg.u.chReset.delay2);
        BiquadInit(&s->lpf, LpfCoeffA, LpfCoeffB);
        BiquadInit(&s->hpf, HpfCoeffA, HpfCoeffB);
        LogInfo("delay1 = %d", msg.u.chReset.delay1);
        LogInfo("delay2 = %d", msg.u.chReset.delay2);

        s->codecType = msg.u.chReset.codec;
        if(!CodecInit(
            &s->codec, 
            msg.u.chReset.codec,
            msg.u.chReset.codecMode,
            msg.u.chReset.vad,
            msg.u.chReset.errorPattern, 
            msg.u.chReset.errorPatternIndex))
        {
            sprintf(msg.u.error.msg, "Codec initialization failed");
            msg.id = MsgError;
        }
        else {
            msg.id = MsgReady;
            s->configured = TRUE;
        }
    }
    else if(msg.id == MsgPcmData) {
        if(s->configured) {
            (*s->process)(s, msg.u.pcmData.pcm);
        }
        else {
            sprintf(msg.u.error.msg, "MsgPcmData received before configured");
            msg.id = MsgError;
        }
    }
    else {
        sprintf(msg.u.error.msg, "Unknown msg type %d", msg.id);
        msg.id = MsgError;
    }
    IpcSend(s->conn, &msg);
    
    return TRUE;
}

static void ChannelFinal(ChannelState *s)
{
    if(s->configured) CodecFinal(&s->codec);
}

void UlChannelInit(const char *host, const char *port)
{
    LogInfo("Starting UL CHANNEL");
    ChannelInit(&ulState, ChannelDirUl, host, port);
    IpcRegisterTask(ulState.conn, UlChannelRun);
}

BOOL UlChannelRun(void)
{
    return ChannelRun(&ulState);
}

void UlChannelFinal(void)
{
    ChannelFinal(&ulState);
}

void DlChannelInit(const char *host, const char *port)
{
    LogInfo("Starting DL CHANNEL");
    ChannelInit(&dlState, ChannelDirDl, host, port);
    IpcRegisterTask(dlState.conn, DlChannelRun);
}

BOOL DlChannelRun(void)
{
    return ChannelRun(&dlState);
}

void DlChannelFinal(void)
{
    ChannelFinal(&dlState);
}
