#include <stdlib.h>
#include <string.h>
#define _USE_MATH_DEFINES
#include <math.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#include "resampler.h"

#include "rsp_hle/audio.h"

enum { RESAMPLER_SHIFT = 16 };
enum { RESAMPLER_RESOLUTION = 1 << RESAMPLER_SHIFT };

enum { resampler_buffer_size = 64 * 4 };

typedef struct resampler
{
    int write_pos, write_filled;
    int read_pos, read_filled;
    int phase;
    int phase_inc;
    signed char delay_added;
    signed char delay_removed;
    short buffer_in[2][resampler_buffer_size * 2];
    short buffer_out[resampler_buffer_size * 2];
} resampler;

void * resampler_create(void)
{
    resampler * r = ( resampler * ) malloc( sizeof(resampler) );
    if ( !r ) return 0;

    r->write_pos = 1;
    r->write_filled = 0;
    r->read_pos = 0;
    r->read_filled = 0;
    r->phase = 0;
    r->phase_inc = 0;
    r->delay_added = -1;
    r->delay_removed = -1;
    memset( r->buffer_in, 0, sizeof(r->buffer_in) );
    memset( r->buffer_out, 0, sizeof(r->buffer_out) );

    return r;
}

void resampler_delete(void * _r)
{
    free( _r );
}

void * resampler_dup(const void * _r)
{
    void * r_out = malloc( sizeof(resampler) );
    if ( !r_out ) return 0;

    resampler_dup_inplace(r_out, _r);

    return r_out;
}

void resampler_dup_inplace(void *_d, const void *_s)
{
    const resampler * r_in = ( const resampler * ) _s;
    resampler * r_out = ( resampler * ) _d;

    r_out->write_pos = r_in->write_pos;
    r_out->write_filled = r_in->write_filled;
    r_out->read_pos = r_in->read_pos;
    r_out->read_filled = r_in->read_filled;
    r_out->phase = r_in->phase;
    r_out->phase_inc = r_in->phase_inc;
    r_out->delay_added = r_in->delay_added;
    r_out->delay_removed = r_in->delay_removed;
    memcpy( r_out->buffer_in, r_in->buffer_in, sizeof(r_in->buffer_in) );
    memcpy( r_out->buffer_out, r_in->buffer_out, sizeof(r_in->buffer_out) );
}

int resampler_get_free_count(void *_r)
{
    resampler * r = ( resampler * ) _r;
    return resampler_buffer_size - r->write_filled;
}

static int resampler_min_filled(resampler *r)
{
    return 4;
}

static int resampler_input_delay(resampler *r)
{
    return 1;
}

static int resampler_output_delay(resampler *r)
{
    return 0;
}

int resampler_ready(void *_r)
{
    resampler * r = ( resampler * ) _r;
    return r->write_filled > resampler_min_filled(r);
}

void resampler_clear(void *_r)
{
    resampler * r = ( resampler * ) _r;
    r->write_pos = 1;
    r->write_filled = 0;
    r->read_pos = 0;
    r->read_filled = 0;
    r->phase = 0;
    r->delay_added = -1;
    r->delay_removed = -1;
}

void resampler_set_rate(void *_r, double new_factor)
{
    resampler * r = ( resampler * ) _r;
    r->phase_inc = new_factor * RESAMPLER_RESOLUTION;
}

void resampler_write_sample(void *_r, short ls, short rs)
{
    resampler * r = ( resampler * ) _r;

    if ( r->delay_added < 0 )
    {
        r->delay_added = 0;
        r->write_filled = resampler_input_delay( r );
    }
    
    if ( r->write_filled < resampler_buffer_size )
    {
        r->buffer_in[ 0 ][ r->write_pos ] = ls;
        r->buffer_in[ 0 ][ r->write_pos + resampler_buffer_size ] = ls;

        r->buffer_in[ 1 ][ r->write_pos ] = rs;
        r->buffer_in[ 1 ][ r->write_pos + resampler_buffer_size ] = rs;
        
        ++r->write_filled;

        r->write_pos = ( r->write_pos + 1 ) % resampler_buffer_size;
    }
}

static int resampler_run_cubic(resampler * r, short ** out_, short * out_end)
{
    int in_size = r->write_filled;
    int in_offset = resampler_buffer_size + r->write_pos - r->write_filled;
    short const* inl_ = r->buffer_in[0] + in_offset;
    short const* inr_ = r->buffer_in[1] + in_offset;
    int used = 0;
    in_size -= 4;
    if ( in_size > 0 )
    {
        short* out = *out_;
        short const* inl = inl_;
        short const* inr = inr_;
        short const* const in_end = inl + in_size;
        int phase = r->phase;
        int phase_inc = r->phase_inc;

        do
        {
            int samplel, sampler;
            
            if ( out >= out_end )
                break;

            const int16_t* lut = RESAMPLE_LUT + ((phase & 0xfc00) >> 8);
            
            samplel = ((inl[0] * lut[0]) >> 15) + ((inl[1] * lut[1]) >> 15)
                    + ((inl[2] * lut[2]) >> 15) + ((inl[3] * lut[3]) >> 15);
            sampler = ((inr[0] * lut[0]) >> 15) + ((inr[1] * lut[1]) >> 15)
                    + ((inr[2] * lut[2]) >> 15) + ((inr[3] * lut[3]) >> 15);
            
            if ((samplel + 0x8000) & 0xffff0000) samplel = 0x7fff ^ (samplel >> 31);
            if ((sampler + 0x8000) & 0xffff0000) sampler = 0x7fff ^ (sampler >> 31);
            
            *out++ = (short)samplel;
            *out++ = (short)sampler;

            phase += phase_inc;

            inl += (phase >> 16);
            inr += (phase >> 16);

            phase &= 0xFFFF;
        }
        while ( inl < in_end );

        r->phase = phase;
        *out_ = out;

        used = (int)(inl - inl_);

        r->write_filled -= used;
    }

    return used;
}

static void resampler_fill(resampler * r)
{
    int min_filled = resampler_min_filled(r);
    while ( r->write_filled > min_filled &&
            r->read_filled < resampler_buffer_size )
    {
        int write_pos = ( r->read_pos + r->read_filled ) % resampler_buffer_size;
        int write_size = resampler_buffer_size - write_pos;
        short * out = r->buffer_out + write_pos * 2;
        if ( write_size > ( resampler_buffer_size - r->read_filled ) )
            write_size = resampler_buffer_size - r->read_filled;
        resampler_run_cubic( r, &out, out + write_size * 2 );
        r->read_filled += ( out - r->buffer_out - write_pos * 2 ) / 2;
    }
}

static void resampler_fill_and_remove_delay(resampler * r)
{
    resampler_fill( r );
    if ( r->delay_removed < 0 )
    {
        int delay = resampler_output_delay( r );
        r->delay_removed = 0;
        while ( delay-- )
            resampler_remove_sample( r );
    }
}

int resampler_get_sample_count(void *_r)
{
    resampler * r = ( resampler * ) _r;
    if ( r->read_filled < 1 )
        resampler_fill_and_remove_delay( r );
    return r->read_filled;
}

void resampler_get_sample(void *_r, short * ls, short * rs)
{
    resampler * r = ( resampler * ) _r;
    if ( r->read_filled < 1 && r->phase_inc )
        resampler_fill_and_remove_delay( r );
    if ( r->read_filled < 1 )
    {
        *ls = 0;
        *rs = 0;
    }
    else
    {
        *ls = r->buffer_out[ r->read_pos * 2 + 0 ];
        *rs = r->buffer_out[ r->read_pos * 2 + 1 ];
    }
}

void resampler_remove_sample(void *_r)
{
    resampler * r = ( resampler * ) _r;
    if ( r->read_filled > 0 )
    {
        --r->read_filled;
        r->read_pos = ( r->read_pos + 1 ) % resampler_buffer_size;
    }
}
