/***************************************************************
 *
 *	resample.c
 *
 *	10/6/94		bfs initial version
 *
 *
 *
 */
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <ctype.h>
#include <errno.h>

#include "sinc.h"

extern int errno;

#define FILTER_LENGTH 4
#define NUM_OF_BITS 16
#define NUM_OF_INT_BITS 1
#define NUM_OF_FRAC_BITS NUM_OF_BITS - NUM_OF_INT_BITS


#define MAX_PITCH 1.9999695
#define MIN_PITCH 0.0000305

#define	CLIP(x)	if (x > 32767) x = 32767; if (x < -32767) x = -32767;

/*
 * Name:   update_data_vector
 *
 * Description:
 *	This routine shifts the filter data delay line by an amount
 *	equal to the integer part of the current address (ca). For
 *	each shift, a new value is brought in from stdin.
 *
 *	No value is returned since a read error causes program termination.
 *
 */
short
update_data_vector(unsigned long ca, short* datav, char length, FILE* in)
{
    short int_ca, i, j;
    long decrementer;

    int_ca = ca >> NUM_OF_FRAC_BITS;
    decrementer = 1L << NUM_OF_FRAC_BITS;
    
    if ( int_ca ) {
	for ( i = int_ca; i > 0; i-- ) {
	/* shift data in delay_line.. */
	    for ( j = length-1; j > 0; j-- )
		datav[j] = datav[j-1];
	
#ifdef ASCII
	    fscanf(in, "%hd", datav);
#else
	    fread(datav, sizeof(short), 1, in);
#endif
	    ca -= decrementer;
	}
    }
   return ca;
}

/*
 * Name:   update_coef_vector
 *
 * Description:
 *	This routine fills a coefficient vector with one point per
 *	lobe from the filter table. The address 
 *
 */
void
update_coef_vector(long ca, short* coefv, char length, short* table)
{
    short index;
    long frac_ca;
    int i;

    frac_ca = ca << ( NUM_OF_INT_BITS - 1 ); /* -1 is because ca is unsigned but frac_ca is signed */

    index = (short)( (FILTER_LOBE_SIZE * frac_ca) >> 15 );

    for ( i = 0; i < length; i++ ) {
	coefv[i] = table[index];
	index += FILTER_LOBE_SIZE;
    }
}

/*
 * Name:   dot_product
 *
 * Description:
 *	This routine performs a dot product on the data and coefficient
 *	vectors passed to it. Note that the value is truncated
 *	after each accumulation.
 *
 */
int
dot_product(short* data, short* coef, char length)
{
    int temp = 0, i;
    int output = 0;
    
    for ( i = 0; i < length; i++ ) {
	temp = data[i] * coef[i];
	output += temp >> 15;
    }

    return output;
}

/*
 * Name:   do_resample
 *
 * Description:
 *	This routine resamples the input stream and sends it to the
 *	output stream.
 *
 */
long
do_resample(FILE* in, FILE* out, unsigned short incr)
{
    unsigned long ca = 0;
    short datav[FILTER_LENGTH], coefv[FILTER_LENGTH];
    int output, i;
    long count = 0;
    short sout;
    
    for (i = 0; i < 4; i++) datav[i] = 0; /* clear data vector */
    
    while ( !feof(in) ) {
	ca = update_data_vector(ca, datav, FILTER_LENGTH, in);
	update_coef_vector(ca, coefv, FILTER_LENGTH, sinc);
	output = dot_product(datav, coefv, FILTER_LENGTH);
        if (output >0x7fff)
            output = 0x7fff;
        else
            if (output < -0x7fff)
                output = -0x7fff;
        sout = (short) output;
#ifdef ASCII
	fprintf(sout, "%hd\n", output);
#else
	fwrite(&sout, sizeof(short), 1, out);
#endif
	ca += incr;			/* update phase increment */
	count++;
   }
   return count;
}

/*
 * Name:   encode_pitch
 *
 * Description:
 *	This routine encodes the floating point input pitch value
 *	in unsigned 1.15 format.
 *
 */
unsigned short
encode_pitch(double p)
{
    unsigned short inc;
    double int_inc, frac_inc;

    if ( p > MAX_PITCH ){
	fprintf(stderr, "pitch has been clipped to %.7f...\n", MAX_PITCH);
	p = MAX_PITCH;
    }
    if ( p < MIN_PITCH ){
	fprintf(stderr, "pitch has been clamped to %.7f; (boy, that's really low)...\n", MIN_PITCH);
	fprintf(stderr, "\n");
	p = MIN_PITCH;
    }
    frac_inc = modf(p, &int_inc);
    inc = (unsigned short)int_inc << NUM_OF_FRAC_BITS;
    inc |= (unsigned short)(frac_inc * 32768);

    return inc;
}

void
usage(char *pname){
    fprintf(stderr, "USAGE: %s pitch < input > output\n", pname);
    exit(1);
}

void
main(int argc, char **argv)
{
    long count = 0;
    unsigned short increment;
    double pitch;

    if (argc < 2) usage(argv[0]);
    pitch = atof(argv[1]);
    increment = encode_pitch(pitch);
    count = do_resample(stdin, stdout, increment);
    exit(0);
}