/*
 * ADX ADPCM codecs
 * Copyright (c) 2001,2003 BERO
 *
 * This file is part of Libav.
 *
 * Libav is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * Libav is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with Libav; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
 */

#include <stdint.h>
#include <string.h>
#include <math.h>

typedef struct {
    int s1,s2;
} ADXChannelState;

typedef struct {
    ADXChannelState prev[2];
    uint8_t channels;
    int16_t cutoff;
    uint32_t bit_rate, sample_rate;
    int coeff[2];
} ADXContext;

#ifndef AVPACKET_DEFINED
typedef struct AVPacket {
    uint8_t *data;
    uint32_t size;
} AVPacket;
#define AVPACKET_DEFINED
#endif

#define COEFF_BITS         12
#define BLOCK_SIZE         18
#define BLOCK_SAMPLES      32
#define BLOCK_SAMPLES_LOG2  5

static __attribute__((always_inline)) inline int16_t av_clip_int16(int a)
{
    if ((a+0x8000) & ~0xFFFF) return (a>>31) ^ 0x7FFF;
    else                      return a;
}

static const float
C1  = -5.0000000000e-01,
C2  =  4.1666667908e-02, /* 0x3d2aaaab */
C3  = -1.3888889225e-03, /* 0xbab60b61 */
C4  =  2.4801587642e-05, /* 0x37d00d01 */
C5  = -2.7557314297e-07, /* 0xb493f27c */
C6  =  2.0875723372e-09, /* 0x310f74f6 */
C7  = -1.1359647598e-11; /* 0xad47d74e */

// Differs from libc cosf on [0, pi/2] at only the point 0x3fc460f8 (~1.5342093) by 0.0000001005828f
static inline float k_cosf(float x)
{
    float z,r;
    z  = x*x;
    r  = z*(C2+z*(C3+z*(C4+z*(C5+z*(C6+z*C7)))));
    return 1.0f + (z*r + C1*z);
}

static const float pi = 3.14159265358979323846f;

float adx_calculate_coeffs(float cutoff, int sample_rate)
{
    float a, b, c;

    a = (float)M_SQRT2 - k_cosf(2.0f * pi * (float)cutoff / (float)sample_rate);
    b = (float)M_SQRT2 - 1.0f;
    c = (a - sqrtf((a + b) * (a - b))) * ((float)M_SQRT2 + 1.0f);

    return c;
}

/**
 * @file
 * SEGA CRI adx codecs.
 *
 * Reference documents:
 * http://ku-www.ss.titech.ac.jp/~yatsushi/adx.html
 * adx2wav & wav2adx http://www.geocities.co.jp/Playtown/2004/
 */

/**
 * Decode 32 samples from 18 bytes.
 *
 * A 16-bit scalar value is applied to 32 residuals, which then have a
 * 2nd-order LPC filter applied to it to form the output signal for a single
 * channel.
 */
static void adx_decode(ADXContext *c, int16_t *out, const uint8_t *in, int ch)
{
    ADXChannelState *prev = &c->prev[ch];
    int scale = ((in[0] << 8) | (in[1] << 0));
    int i;
    int s0, s1, s2, d;

    in += 2;
    s1 = prev->s1;
    s2 = prev->s2;
    for (i = 0; i < 16; i++) {
        d  = in[i];
        d  = (signed char)d >> 4;
        s0 = ((d << COEFF_BITS) * scale + c->coeff[0] * s1 + c->coeff[1] * s2) >> COEFF_BITS;
        s2 = s1;
        s1 = av_clip_int16(s0);
        *out = s1;
        out += c->channels;

        d  = in[i];
        d  = (signed char)(d << 4) >> 4;
        s0 = ((d << COEFF_BITS) * scale + c->coeff[0] * s1 + c->coeff[1] * s2) >> COEFF_BITS;
        s2 = s1;
        s1 = av_clip_int16(s0);
        *out = s1;
        out += c->channels;
    }
    prev->s1 = s1;
    prev->s2 = s2;
}

int adx_decode_header(ADXContext *s, const uint8_t *buf, unsigned int bufsize)
{
    float c;
    int header_size = ((buf[2] << 8) | (buf[3] << 0)) + 4;
    s->channels = buf[7];
    s->cutoff = ((buf[16] << 8) | (buf[17] << 0));
    s->sample_rate = 48000;
    c = adx_calculate_coeffs(s->cutoff, s->sample_rate);
    s->coeff[0] = lrintf(c * 2.0f  * (1 << COEFF_BITS));
    s->coeff[1] = lrintf(-(c * c)  * (1 << COEFF_BITS));
    return header_size;
}

int adx_decode_frame(ADXContext *c, int16_t *data, unsigned int *data_size, AVPacket *avpkt)
{
    const uint8_t *buf0 = avpkt->data;
    int buf_size        = avpkt->size;
    int16_t *samples    = data;
    const uint8_t *buf  = buf0;
    int rest            = buf_size;
    int nch, num_blocks;

    /* 18 bytes of data are expanded into 32*2 bytes of audio,
       so guard against buffer overflows */
    nch = (c->channels-1);
    num_blocks = ((rest / BLOCK_SIZE) >> nch);
    if (!num_blocks) {
        //av_log(AV_LOG_ERROR, "packet is too small\n");
        return -1;
    }

    while (num_blocks--) {
        adx_decode(c, samples, buf, 0);
        if (c->channels == 2)
            adx_decode(c, samples + 1, buf + BLOCK_SIZE, 1);
        //rest    -= BLOCK_SIZE * c->channels;
        buf     += BLOCK_SIZE * c->channels;
        samples += BLOCK_SAMPLES * c->channels;
    }

    *data_size = (int16_t*)samples - (int16_t*)data;
    return buf - buf0;
}

#ifdef TEST

#include <stdint.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/stat.h>

typedef struct _WAVHeader {
    uint32_t riff, totalsz;
    uint32_t pad0;
    uint32_t fmttag, fmtlen;
    uint16_t wav_id;
    uint16_t channels;
    uint32_t samplerate;
    uint32_t bitrate;
    uint32_t block_align;
    uint32_t datatag, datalen;
} __attribute__((packed)) WAVHeader;

static void write_wav_header(int fd, uint32_t rate, uint8_t channels)
{
   WAVHeader w;
   w.riff = 0x46464952;
   w.totalsz = 0x7fffffff;
   w.pad0 = 0x45564157;
   w.fmttag = 0x20746D66;
   w.fmtlen = 0x10;
   w.wav_id = 1;
   w.channels = channels;
   w.samplerate = rate;
   w.bitrate = rate*channels*2;
   w.block_align = (channels << 1) | 0x00100000;
   w.datatag = 0x61746164;
   w.datalen = 0x7fffffff;
   write(fd, &w, sizeof(WAVHeader));
}

int main(int argc, char **argv) {
    ADXContext s = {0};
    uint8_t adxhdr[24];
    uint8_t inbuf[1152];
    unsigned int i, hdrlen, totalframes;
    uint32_t outsz = 40860*2;
    int16_t outbuf[4096];
    int r, in_fd, out_fd = open("asdf.wav", O_WRONLY|O_CREAT|O_TRUNC, 0644);

    if((in_fd = open(argv[1], O_RDONLY)) < 0) {
        return -1;
    }
    read(in_fd, adxhdr, 18);
    hdrlen = adx_decode_header(&s, adxhdr, 18);
    lseek(in_fd, hdrlen, SEEK_SET);
    write_wav_header(out_fd, 48000, s.channels);

    while ((r = read(in_fd, inbuf, 1152)) > 0) {
        AVPacket pkt;
        pkt.data = inbuf;
        pkt.size = r;
        //av_log(AV_LOG_INFO, "frame %u: offset %u size: %u\n", i, framepos, size);
        adx_decode_frame(&s, outbuf, &outsz, &pkt);
        write(out_fd, outbuf, 2*outsz);
    }
    close(in_fd);
    close(out_fd);
    return 0;
}

#endif