/***************************************************************************
 *             __________               __   ___.
 *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
 *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
 *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
 *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
 *                     \/            \/     \/    \/            \/
 *
 * Copyright (C) 2005 Miika Pekkarinen
 * Copyright (C) 2012 Michael Sevakis
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
 * KIND, either express or implied.
 *
 ****************************************************************************/
#include "rbcodecconfig.h"
#include "platform.h"
#include "dsp_core.h"
#include "dsp_sample_io.h"
#include "dsp_proc_entry.h"
#include "dsp-util.h"
#include <string.h>

#if 0
#include <debug.h>
#else
#undef DEBUGF
#define DEBUGF(...)
#endif

/* May be implemented in here or externally.*/
void sample_output_mono(struct sample_io_data *this,
                        struct dsp_buffer *src, struct dsp_buffer *dst);
void sample_output_stereo(struct sample_io_data *this,
                          struct dsp_buffer *src, struct dsp_buffer *dst);
void sample_output_dithered(struct sample_io_data *this,
                            struct dsp_buffer *src, struct dsp_buffer *dst);

/** Sample output **/

#if !defined(CPU_COLDFIRE) && !defined(CPU_ARM)
/* write mono internal format to output format */
void sample_output_mono(struct sample_io_data *this,
                        struct dsp_buffer *src, struct dsp_buffer *dst)
{
    int count = this->outcount;
    const int32_t *s0 = src->p32[0];
    int16_t *d = dst->p16out;
    int scale = src->format.output_scale;
    int32_t dc_bias = 1L << (scale - 1);

    do
    {
        int32_t lr = clip_sample_16((*s0++ + dc_bias) >> scale);
        *d++ = lr;
        *d++ = lr;
    }
    while (--count > 0);
}
#endif /* CPU */

#if (!defined(CPU_COLDFIRE) && !defined(CPU_ARM)) || defined(CPU_ARM_MICRO)
/* write stereo internal format to output format */
void sample_output_stereo(struct sample_io_data *this,
                          struct dsp_buffer *src, struct dsp_buffer *dst)
{
    int count = this->outcount;
    const int32_t *s0 = src->p32[0];
    const int32_t *s1 = src->p32[1];
    int16_t *d = dst->p16out;
    int scale = src->format.output_scale;
    int32_t dc_bias = 1L << (scale - 1);

    do
    {
        *d++ = clip_sample_16((*s0++ + dc_bias) >> scale);
        *d++ = clip_sample_16((*s1++ + dc_bias) >> scale);
    }
    while (--count > 0);
}
#endif /* CPU */

/**
 * The "dither" code to convert the 24-bit samples produced by libmad was
 * taken from the coolplayer project - coolplayer.sourceforge.net
 *
 * This function handles mono and stereo outputs.
 */
static struct dither_data
{
    struct dither_state
    {
        long error[3];  /* 00h: error term history */
        long random;    /* 0ch: last random value */
    } state[2];         /* 0=left, 1=right */
    bool enabled;       /* 20h: dithered output enabled */
                        /* 24h */
} dither_data IBSS_ATTR;

void sample_output_dithered(struct sample_io_data *this,
                            struct dsp_buffer *src, struct dsp_buffer *dst)
{
    int count = this->outcount;
    int channels = src->format.num_channels;
    int scale = src->format.output_scale;
    int32_t dc_bias = 1L << (scale - 1); /* 1/2 bit of significance */
    int32_t mask = (1L << scale) - 1; /* Mask of bits quantized away */

    for (int ch = 0; ch < channels; ch++)
    {
        struct dither_state *dither = &dither_data.state[ch];

        const int32_t *s = src->p32[ch];
        int16_t *d = &dst->p16out[ch];

        for (int i = 0; i < count; i++, s++, d += 2)
        {
            /* Noise shape and bias (for correct rounding later) */
            int32_t sample = *s;

            sample += dither->error[0] - dither->error[1] + dither->error[2];
            dither->error[2] = dither->error[1];
            dither->error[1] = dither->error[0] / 2;

            int32_t output = sample + dc_bias;

            /* Dither, highpass triangle PDF */
            int32_t random = dither->random*0x0019660dL + 0x3c6ef35fL;
            output += (random & mask) - (dither->random & mask);
            dither->random = random;

            /* Quantize sample to output range */
            output >>= scale;

            /* Error feedback of quantization */
            dither->error[0] = sample - (output << scale);

            /* Clip and store */
            *d = clip_sample_16(output);
        }
    }

    if (channels > 1)
        return;

    /* Have to duplicate left samples into the right channel since
       output is interleaved stereo */
    int16_t *d = dst->p16out;

    do
    {
        int16_t s = *d++;
        *d++ = s;
    }
    while (--count > 0);
}

/* Initialize the output function for settings and format */
void dsp_sample_output_format_change(struct sample_io_data *this,
                                     struct sample_format *format)
{
    static const sample_output_fn_type fns[2][2] =
    {
        { sample_output_mono,        /* DC-biased quantizing */
          sample_output_stereo },
        { sample_output_dithered,    /* Tri-PDF dithering */
          sample_output_dithered },
    };

    bool dither = dsp_get_id((void *)this) == CODEC_IDX_AUDIO &&
                  dither_data.enabled;
    int channels = format->num_channels;

    DSP_PRINT_FORMAT(DSP Output, *format);

    this->output_samples = fns[dither ? 1 : 0][channels - 1];
    this->output_version = format->version;
}

void dsp_sample_output_init(struct sample_io_data *this)
{
    this->output_version = 0;
    this->output_samples = sample_output_stereo;
}

/* Flush the dither history */
void dsp_sample_output_flush(struct sample_io_data *this)
{
    if (dsp_get_id((void *)this) == CODEC_IDX_AUDIO)
        memset(dither_data.state, 0, sizeof (dither_data.state));
}


/** Output settings **/

/* Set the tri-pdf dithered output */
void dsp_dither_enable(bool enable)
{
    if (enable == dither_data.enabled)
        return;

    dither_data.enabled = enable;
    struct sample_io_data *data = (void *)dsp_get_config(CODEC_IDX_AUDIO);

    if (enable)
        dsp_sample_output_flush(data);

    data->output_version = 0; /* Force format update */
}