#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "interpol.h"

float inter4pol (short* buf, float index, long size){
	double frac, a, b, c, d, cminusb;
	short* fp;
	long lindex = (long)index;

	/* 4 point interpolation taken from tabread4~ of pd (pure-data.org) */

	if ((float)lindex == index) // no need to interpolate: 
		return (*(buf + lindex) / 32767.);	

	if (!size) return 0.;

	if (lindex < 1){
            	lindex = 1, frac = .0;
        }else if (lindex > (size - 3)){
            	lindex = size - 3;
		frac = 1.;
	}else{
		frac = (double)index - lindex;
	}

	fp = buf + lindex;
	a = (double)(fp[-1]) / 32767.;
        b = (double)(fp[0]) / 32767.;
        c = (double)(fp[1]) / 32767.;
        d = (double)(fp[2]) / 32767.;

	cminusb = c-b;
	return (float)(b + frac * (cminusb - 0.5f * (frac - 1.) * 
		 ((a - d + 3.0f * cminusb) * frac + (b - a - cminusb))));
}

float inter4pol_float (float* buf, double index, long size){
        double frac,  a,  b,  c,  d, cminusb;
        float* fp;
        long lindex = (long)index;

        /* 4 point interpolation taken from tabread4~ of pd (pure-data.org) */

        if (!size) return 0.;

	if ((float)lindex == index) // no need to interpolate: 
                return (*(buf + lindex));

        if (lindex < 1){
                lindex = 1, frac = .0;
        }else if (lindex > (size - 3)){
                lindex = size - 3;
                frac = 1.;
        }else{
                frac = index - lindex;
        }

        fp = buf + lindex;
        a = fp[-1];
        b = fp[0];
        c = fp[1];
        d = fp[2];

        cminusb = c-b;
        return (float)(b + frac * (cminusb - 0.5f * (frac - 1.) *
                 ((a - d + 3.0f * cminusb) * frac + (b - a - cminusb))));
}
