#include <stdio.h>
#include <time.h>
#include <stdlib.h>
#include <unistd.h>
#include <ctype.h>
#include <errno.h>
#include <termios.h>
#include <string.h>
#include <fcntl.h>

#include <sys/stat.h>
#include <sys/types.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <sys/param.h>

#include <netinet/in.h>

#include <stdint.h>

#include "mbelib.h"
#include "ambe.h"
#include "p25p2_vf.h"
#include "imbe_decoder.h"
#include "software_imbe_decoder.h"
#include "imbe_vocoder/imbe_vocoder.h"
#include "ambe_encoder.h"

static const float GAIN_ADJUST=7.0;  /* attenuation (dB) */

typedef uint16_t Uns;
static const Uns RC_OK=0;

static const char prodid[] = "OP25  ";
static const char verstring[] = "1.0";

static ambe_encoder encoder;
static software_imbe_decoder software_decoder;
static p25p2_vf interleaver;
static mbe_parms cur_mp;
static mbe_parms prev_mp;
static mbe_parms enh_mp;

static const Uns DV3K_START_BYTE = 0x61;
enum
{
    DV3K_CONTROL_RATEP     = 0x0A,
    DV3K_CONTROL_CHANFMT   = 0x15,
    DV3K_CONTROL_PRODID    = 0x30,
    DV3K_CONTROL_VERSTRING = 0x31,
    DV3K_CONTROL_RESET     = 0x33,
    DV3K_CONTROL_READY     = 0x39
};
static const Uns DV3K_AMBE_FIELD_CHAND    = 0x01;
static const Uns DV3K_AMBE_FIELD_CMODE    = 0x02;
static const Uns DV3K_AMBE_FIELD_TONE     = 0x08;
static const Uns DV3K_AUDIO_FIELD_SPEECHD = 0x00;
static const Uns DV3K_AUDIO_FIELD_CMODE   = 0x02;

#pragma DATA_ALIGN(dstar_state, 2)
static Uns bitstream[72];

static Uns get_byte(Uns offset, Uns *p)
{
    Uns word = p[offset >> 1];
    return (offset & 1) ? (word >> 8) : (word & 0xff);
}

static void set_byte(Uns offset, Uns *p, Uns byte)
{
    p[offset >> 1] =
        (offset & 1) ? (byte << 8) | (p[offset >> 1] & 0xff)
                     : (p[offset >> 1] & 0xff00) | (byte & 0xff);
}

static Uns get_word(Uns offset, Uns *p)
{
    return get_byte(offset + 1, p) | (get_byte(offset, p) << 8);
}

static void set_word(Uns offset, Uns *p, Uns word)
{
    set_byte(offset, p, word >> 8);
    set_byte(offset + 1, p, word & 0xff);
}

static void set_cstring(Uns offset, Uns *p, const char *str)
{
    do
        set_byte(offset++, p, *str);
    while (*str++ != 0);
}

static Uns pkt_check_ratep(Uns offset, Uns *p)
{
    static const Uns ratep[] = {
        0x01, 0x30, 0x07, 0x63, 0x40, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x48 };
    Uns i;
    for (i = 0; i < sizeof(ratep); ++i)
        if (get_byte(offset + i, p) != ratep[i])
            return 0;
    return 1;
}

static void pack(Uns bits, Uns offset, Uns *p, Uns *bitstream)
{
    Uns i;
    Uns byte = 0;
    for (i = 0; i < bits; ++i)
    {
        byte |= bitstream[i] << (7 - (i & 7));
        if ((i & 7) == 7)
        {
            set_byte(offset++, p, byte);
            byte = 0;
        }
    }
    if (i & 7)
        set_byte(offset, p, byte);
}

static void unpack(Uns bits, Uns offset, Uns *bitstream, Uns *p)
{
    Uns i;
    Uns byte;
    for (i = 0; i < bits; ++i)
    {
        if ((i & 7) == 0)
            byte = get_byte(offset++, p);
        bitstream[i] = (byte >> (7 - (i & 7))) & 1;
    }
}

static int response_len = -1;

static void bksnd(void*task, Uns bid, Uns len)
{
	response_len = len;
}

static void vocoder_setup(void) {
	encoder.set_dstar_mode();
	encoder.set_gain_adjust(GAIN_ADJUST);
	encoder.set_alt_dstar_interleave(true);
	mbe_initMbeParms (&cur_mp, &prev_mp, &enh_mp);
}

static void dump(unsigned char *p, ssize_t n)
{
    int i;
    for (i = 0; i < n; ++i)
        printf("%02x%c", p[i], i % 16 == 15 ? '\n' : ' ');
    if (i % 16)
        printf("\n");
}

static Uns pkt_process(Uns*pkt, Uns cnt)
{
    Uns bid=0;
    Uns len = cnt << 1;
    Uns payload_length;
    Uns i;
    Uns cmode = 0;
    Uns tone = 0;
    uint8_t codeword[72];
    int b[9];
    int K;
    int rc = -1;

    if (len < 4 || cnt > 256)
        goto fail;

    if (get_byte(0, pkt) != DV3K_START_BYTE)
        goto fail;

    payload_length = get_word(1, pkt);
    if (payload_length == 0)
        goto fail;
    if (4 + payload_length > len)
        goto fail;

    switch (get_byte(3, pkt))
    {
    case 0:
        switch (get_byte(4, pkt))
        {
        case DV3K_CONTROL_RATEP:
            if (payload_length != 13)
                goto fail;
            if (!pkt_check_ratep(5, pkt))
                goto fail;
            set_word(1, pkt, 1);
            bksnd(NULL, bid, 3);
            return RC_OK;
        case DV3K_CONTROL_CHANFMT:
            if (payload_length != 3)
                goto fail;
            if (get_word(5, pkt) != 0x0001)
                goto fail;
            set_word(1, pkt, 2);
            set_byte(5, pkt, 0);
            bksnd(NULL, bid, 3);
            return RC_OK;
        case DV3K_CONTROL_PRODID:
            set_word(1, pkt, 8);
            set_cstring(5, pkt, prodid);
            bksnd(NULL, bid, 6);
            return RC_OK;
        case DV3K_CONTROL_VERSTRING:
            set_word(1, pkt, 5);
            set_cstring(5, pkt, verstring);
            bksnd(NULL, bid, 5);
            return RC_OK;
        case DV3K_CONTROL_RESET:
            if (payload_length != 1)
                goto fail;
            vocoder_setup();
            set_byte(4, pkt, DV3K_CONTROL_READY);
            bksnd(NULL, bid, 3);
            return RC_OK;
        default:
            goto fail;
        }
    case 1:
        switch (payload_length)
        {
        case 17:
            if (get_byte(18, pkt) != DV3K_AMBE_FIELD_TONE)
                goto fail;
            tone = get_word(19, pkt);
            /* FALLTHROUGH */
        case 14:
            if (get_byte(15, pkt) != DV3K_AMBE_FIELD_CMODE)
                goto fail;
            cmode = get_word(16, pkt);
            /* FALLTHROUGH */
        case 11:
            if (get_byte(4, pkt) != DV3K_AMBE_FIELD_CHAND)
                goto fail;
            if (get_byte(5, pkt) != 72)
                goto fail;
            unpack(72, 6, bitstream, pkt);
            break;
        default:
            goto fail;
        }

        for (i = 0; i < 72; i++) {
            codeword[i] = bitstream[i];
        }
        interleaver.decode_dstar(codeword, b, true);
        if (b[0] >= 120) {
            memset(6+(char*)pkt, 0, 320);   // silence
            // FIXME: add handling for tone case (b0=126)
        } else {
            rc = mbe_dequantizeAmbe2400Parms(&cur_mp, &prev_mp, b);
            printf("B\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n", b[0], b[1], b[2], b[3], b[4], b[5], b[6], b[7], b[8]);
            K = 12;
            if (cur_mp.L <= 36)
                K = int(float(cur_mp.L + 2.0) / 3.0);
            software_decoder.decode_tap(cur_mp.L, K, cur_mp.w0, &cur_mp.Vl[1], &cur_mp.Ml[1]);
            audio_samples *samples = software_decoder.audio();
            int16_t snd;
            for (i=0; i < 160; i++) {
                    if (samples->size() > 0) {
                            snd = (int16_t)(samples->front());
                            samples->pop_front();
                    } else {
                            snd = 0;
                    }
                    set_word(6 + (i << 1), pkt, snd);
            }
            mbe_moveMbeParms (&cur_mp, &prev_mp);
        }

        set_word(1, pkt, 322);
        set_byte(3, pkt, 2);
        set_byte(4, pkt, DV3K_AUDIO_FIELD_SPEECHD);
        set_byte(5, pkt, 160);
        bksnd(NULL, bid, 165);
        return RC_OK;
    case 2:
        if (payload_length != 322 && payload_length != 325)
            goto fail;
        if (get_byte(4, pkt) != DV3K_AUDIO_FIELD_SPEECHD)
            goto fail;
        if (get_byte(5, pkt) != 160)
            goto fail;
        if (payload_length == 325)
        {
            if (get_byte(326, pkt) != DV3K_AUDIO_FIELD_CMODE)
                goto fail;
            cmode = get_word(323, pkt);
        }
        int16_t samples[160];
        for (i=0; i < 160; i++) {
                samples[i] = (int16_t) get_word(6 + (i << 1), pkt);
        }
        encoder.encode(samples, codeword);
        for (i = 0; i < 72; i++) {
            bitstream[i] = codeword[i];
        }
        set_word(1, pkt, 11);
        set_byte(3, pkt, 1);
        set_byte(4, pkt, DV3K_AMBE_FIELD_CHAND);
        set_byte(5, pkt, 72);
        pack(72, 6, pkt, bitstream);
        bksnd(NULL, bid, 8);
        return RC_OK;
    default:
        goto fail;
    }

fail:
    bksnd(NULL, bid, 0);
    return RC_OK;
}

int main()
{
    int sockfd;
    const ssize_t size_max = 1024;
    ssize_t size_in, size_out;
    char buf_in[size_max], buf_out[size_max];
    socklen_t length = sizeof(struct sockaddr_in);
    struct sockaddr_in sa = { 0 };
    Uns rc;

    vocoder_setup();

    if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
        exit(2);

    sa.sin_family = AF_INET;
    sa.sin_port = htons(2460);
    sa.sin_addr.s_addr = htonl(INADDR_ANY);

    if (bind(sockfd, (struct sockaddr *)&sa, sizeof(sa)) < 0)
        exit(3);

    while (1)
    {
        if ((size_in = recvfrom(sockfd, buf_in, size_max,
            0, (struct sockaddr *)&sa, &length)) < 0)
            exit(4);

        if (size_in & 1)
            buf_in[size_in++] = 0;

        rc = pkt_process((Uns*)buf_in, size_in >> 1);
        if (response_len <= 0)
            exit(9);

        size_out = 4 + ntohs(*(short *)&buf_in[1]);
        if (sendto(sockfd, buf_in, size_out, 0, (struct sockaddr *)&sa,
            sizeof(struct sockaddr_in)) != size_out)
            exit(7);
    }

    return 0;
}