/*
 * Copyright (c) 2004 Michael Niedermayer <michaelni@gmx.at>
 *
 * This file is part of MPlayer.
 *
 * MPlayer 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.
 *
 * MPlayer 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with MPlayer; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include "libavutil/rational.h"

typedef struct AVResampleContext{
    float *filter_bank;
    unsigned int filter_length;
    unsigned int dst_incr;
    unsigned int src_incr;
    int index;
    int frac;
    int int_advance;
    int frac_advance;
    int phase_shift;
    int phase_mask;
    unsigned char linear;
}AVResampleContext;

// Data for specific instances of this filter
typedef struct af_resample_s{
    struct AVResampleContext *avrctx;
    float *in[2];
    unsigned int in_alloc;
    int index;

    unsigned int filter_length;
    unsigned char int linear;
    int phase_shift;
    float cutoff;
}af_resample_t;

/**
 * 0th order modified bessel function of the first kind.
 */
static float I0(float x){
    float v=1;
    float lastv=0;
    float t=1;
    int i;

    x= x*0.25f;
    for(i=1; v != lastv; i++){
        lastv=v;
        t *= x/(i*i);
        v += t;
    }
    return v;
}

/**
 * Build a polyphase filterbank.
 * @param factor resampling factor
 * @param scale wanted sum of coefficients for each filter
 * @param type 0->cubic, 1->blackman nuttall windowed sinc, 2..16->kaiser windowed sinc beta=2..16
 * @return 0 on success, negative on error
 */
static int build_filter(float *filter, float factor, int tap_count, int phase_count, float alpha){
    int ph, i;
    float x, y, w;
    float i0alphainv = 1.0f/I0(alpha*alpha);
    float *tab = malloc(tap_count * sizeof(*tab));
    const int center= (tap_count-1)/2;

    /* if upsampling, only need to interpolate, no filter */
    if (factor > 1.0f)
        factor = 1.0f;

    for(ph=0;ph<phase_count;ph++) {
        float norm = 0;
        for(i=0;i<tap_count;i++) {
            x = ((float)(i - center) - (float)ph / phase_count);
            if (x == 0) y = 1.0f;
            else        y = sinf((float)M_PI * factor * x) / ((float)M_PI * factor * x);
            w = 2.0f*x / (float)tap_count;
            y *= I0(alpha*alpha*FFMAX(1-w*w, 0))*i0alphainv;
            tab[i] = y;
            norm += y;
        }
        norm = (1.0f / norm);

        /* normalize so that an uniform color remains the same */
        for(i=0;i<tap_count;i++) {
            filter[ph * tap_count + i] = tab[i] * norm;
        }
    }

    free(tab);
    return 0;
}

int av_resample_init(AVResampleContext *c, int out_rate, int in_rate, int filter_size, int phase_shift, int linear, float cutoff){
    float factor= FFMIN(out_rate * cutoff / in_rate, 1.0f);
    int phase_count= 1<<phase_shift;

    if (!c)
        return -1;

    c->phase_shift= phase_shift;
    c->phase_mask= phase_count-1;
    c->linear= linear;

    c->filter_length= FFMAX(lrintf(filter_size/factor), 4);
    c->filter_bank= av_mallocz(c->filter_length*(phase_count+1)*sizeof(float));
    if (!c->filter_bank)
        goto error;
    if (build_filter(c->filter_bank, factor, c->filter_length, phase_count, 1<<FILTER_SHIFT, WINDOW_TYPE))
        goto error;
    memcpy(&c->filter_bank[c->filter_length*phase_count+1], c->filter_bank, (c->filter_length-1)*sizeof(float));
    c->filter_bank[c->filter_length*phase_count]= c->filter_bank[c->filter_length - 1];

    c->src_incr= out_rate;
    c->ideal_dst_incr= c->dst_incr= in_rate * phase_count;
    //c->index= -phase_count*((c->filter_length-1)>>1);
    c->index= 0;
    c->int_advance= c->dst_incr / c->src_incr;
    c->frac_advance= c->dst_incr % c->src_incr;

    return 0;
error:
    free(c->filter_bank);
    return -1;
}

void av_resample_close(AVResampleContext *c){
    free(c->filter_bank);
}

void av_resample_compensate(AVResampleContext *c, int sample_delta, int compensation_distance){
//    sample_delta += (c->ideal_dst_incr - c->dst_incr)*(int64_t)c->compensation_distance / c->ideal_dst_incr;
    c->compensation_distance= compensation_distance;
    c->dst_incr = c->ideal_dst_incr - c->ideal_dst_incr * (int64_t)sample_delta / compensation_distance;
}

int av_resamplef(AVResampleContext *c, float *dst, float *src, int *consumed, int src_size, int dst_size, int update_ctx){
    int dst_index = 0, i;
    int index= c->index;
    int frac= c->frac;

    //for(dst_index=0; dst_index < dst_size; dst_index++){
    //while (1) {
    while (dst_index < dst_size) {
        float *filter= c->filter_bank + c->filter_length*(index & c->phase_mask);
        int sample_index= index >> c->phase_shift;
        float val=0.0f, v2=0.0f;

        /*if(sample_index < 0){
            for(i=0; i<c->filter_length; i++)
                val += src[FFABS(sample_index + i) % src_size] * filter[i];
        }else*/
        if(sample_index + c->filter_length > src_size){
            break;
        }else if(c->linear){
            for(i=0; i<c->filter_length; i++){
                val += src[sample_index + i] * filter[i];
                v2  += src[sample_index + i] * filter[i + c->filter_length];
            }
            val+=(v2-val)*(float)frac / (float)c->src_incr;
        }else{
            for(i=0; i<c->filter_length; i++){
                val += src[sample_index + i] * filter[i];
            }
        }

        dst[dst_index++] = val;
        frac += c->frac_advance;
        index += c->int_advance;
        if(frac >= c->src_incr){
            frac -= c->src_incr;
            index++;
        }
    }
    *consumed= FFMAX(index, 0) >> c->phase_shift;
    if(index>=0) index &= c->phase_mask;

    if(update_ctx){
        c->frac= frac;
        c->index= index;
    }

    return dst_index;
}


// Initialization and runtime control
static int control(struct af_instance_s* af, int cmd, void* arg)
{
  af_resample_t* s   = (af_resample_t*)af->setup;
  af_data_t *data= (af_data_t*)arg;
  int out_rate, test_output_res; // helpers for checking input format

  switch(cmd){
  case AF_CONTROL_REINIT:
    if((af->data->rate == data->rate) || (af->data->rate == 0))
        return AF_DETACH;

    af->data->nch    = data->nch;
    if (af->data->nch > 2) af->data->nch = 2;
    af->mul = (float)af->data->rate / data->rate;
    af->delay = af->data->nch * s->filter_length / min(af->mul, 1); // *bps*.5

    if(s->avrctx) av_resample_close(s->avrctx);
    s->avrctx= av_resample_init(af->data->rate, /*in_rate*/data->rate, s->filter_length, s->phase_shift, s->linear, s->cutoff);

    // hack to make af_test_output ignore the samplerate change
    out_rate = af->data->rate;
    af->data->rate = data->rate;
    test_output_res = af_test_output(af, (af_data_t*)arg);
    af->data->rate = out_rate;
    return test_output_res;
  case AF_CONTROL_COMMAND_LINE:{
    s->cutoff= 0.0;
    sscanf((char*)arg,"%d:%d:%d:%f:%d", &af->data->rate, &s->filter_length, &s->linear, &s->cutoff, &s->phase_shift);
    if(s->cutoff <= 0.0) s->cutoff= max(1.0 - 6.5/(s->filter_length+8), 0.80);
    return AF_OK;
  }
  case AF_CONTROL_RESAMPLE_RATE | AF_CONTROL_SET:
    af->data->rate = *(int*)arg;
    return AF_OK;
  }
  return AF_UNKNOWN;
}

// Deallocate memory
static void uninit(struct af_instance_s* af)
{
    if(af->data)
        free(af->data->audio);
    free(af->data);
    if(af->setup){
        int i;
        af_resample_t *s = af->setup;
        if(s->avrctx) av_resample_close(s->avrctx);
        for (i=0; i < AF_NCH; i++)
            free(s->in[i]);
        free(s);
    }
}

// Filter data through filter
static af_data_t* play(struct af_instance_s* af, af_data_t* data)
{
  af_resample_t *s = af->setup;
  int i, j, consumed, ret;
  float *in = (float*)data->audio;
  float *out;
  int chans   = data->nch;
  int in_len  = data->len/(4*chans);
  int out_len = in_len * af->mul + 10;
  float tmp[2][out_len];

  if(AF_OK != RESIZE_LOCAL_BUFFER(af,data))
      return NULL;

  out= (float*)af->data->audio;
  out_len= FFMIN(out_len, af->data->len/(4*chans));
  if(s->in_alloc < in_len + s->index){
      s->in_alloc= in_len + s->index;
      for(i=0; i<chans; i++){
          s->in[i]= realloc(s->in[i], s->in_alloc*sizeof(float));
      }
  }

  if(chans==1){
      memcpy(&s->in[0][s->index], in, in_len * sizeof(float));
  }else if(chans==2){
      for(j=0; j<in_len; j++){
          s->in[0][j + s->index]= *(in++);
          s->in[1][j + s->index]= *(in++);
      }
  }
  in_len += s->index;

  for(i=0; i<chans; i++){
      ret= av_resamplef(s->avrctx, tmp[i], s->in[i], &consumed, in_len, out_len, i+1 == chans);
  }
  out_len= ret;

  s->index= in_len - consumed;
  for(i=0; i<chans; i++){
      memmove(s->in[i], s->in[i] + consumed, s->index*sizeof(float));
  }

  if(chans==1){
      memcpy(out, tmp[0], out_len*sizeof(float));
  }else if(chans==2){
      for(j=0; j<out_len; j++){
          *(out++)= tmp[0][j];
          *(out++)= tmp[1][j];
      }
  }

  data->audio = af->data->audio;
  data->len   = out_len*chans*4;
  data->rate  = af->data->rate;
  return data;
}

int af_resample_open(af_instance_t* af){
  af_resample_t *s = calloc(1,sizeof(af_resample_t));
  af->control=control;
  af->uninit=uninit;
  af->play=play;
  af->mul=1;
  af->data = calloc(1,sizeof(af_data_t));
  s->filter_length= 16;
  s->cutoff= max(1.0f - 6.5f/(s->filter_length+8), 0.87f);
  s->phase_shift= 10;
//  s->setup = RSMP_INT | FREQ_SLOPPY;
  af->setup=s;
  return AF_OK;
}

/* af_info_t af_info_lavcresample = {
  "Sample frequency conversion using libavcodec",
  "lavcresample",
  "Michael Niedermayer",
}; */