/* (c) Stefan Feilmeier, 2015
 *
 * Wave-file handling using libsndfile. Code is heavily based on:
 * https://github.com/erikd/libsndfile/blob/master/examples/sfprocess.c
 *
 * Names:
 * 	x = input signal
 *	y = output signal
 *      n = current sample
 *	h = filter coefficients
 *      k = current coefficient
 *      *_cnt = length of array
 */

#include <stdio.h>
#include <string.h>
#include <time.h>
#include <signal.h>
#include <sys/time.h>
#include <unistd.h>
#include <sndfile.h>
/* for simplicity: read complete file at once */
#define BUFFER_LEN 8586479
#define MAX_CHANNELS 1
#include "coeff.h"
extern const double g_h[];
extern const unsigned int g_h_cnt;
#define FIXEDPT_BITS 32
#define FIXEDPT_WBITS 4
#include "fixedptc.h"

/* Function declarations 
 */
static int read_wav(double *x, unsigned long *x_cnt, SF_INFO *sfinfo);
static int write_wav(const double *y, unsigned long y_cnt, SF_INFO *sfinfo);
static void offline_dffir(const double *x, unsigned long x_cnt, 
	const double *h, unsigned int h_cnt, double *y);
static void offline_dfsymfir(const double *x, unsigned long x_cnt, 
	const double *h, unsigned int h_cnt, double *y);
static void offline_dfsymfir_fp(const fixedpt *xfp, unsigned long x_cnt, 
	const fixedpt *hfp, unsigned int h_cnt, fixedpt *yfp);
static void online_dffir(double *x, unsigned long x_cnt);
static void convert_dbl_to_fp(const double *x, fixedpt *xfp, unsigned long cnt);
void interrupt_handler(int sig);
//static void convert_fp_to_dbl(const fixedpt *xpt, double *x, unsigned long cnt);

/* for debugging fixed point numbers */
void fixedpt_print(fixedpt A) {
	char num[20];
	fixedpt_str(A, num, -2);
	puts(num);
}

/* MAIN
 */
int main(int argc, char *argv[]) {
	// for time measurement
	time_t start, end;
	double dif;
	// for input data handling
	static double x[BUFFER_LEN], y[BUFFER_LEN];
	static unsigned long x_cnt;
	static SF_INFO sfinfo;
	memset (&sfinfo, 0, sizeof (sfinfo));

	// read wav file
	if( read_wav(x, &x_cnt, &sfinfo) != 0) return 1;

	time(&start);
	// evaluate parameters
	if(argc == 1) {
		printf("Please select filter\n");
		printf("  dffir: Offline direct-form FIR\n");
		printf("  dffir_online: Online direct-form FIR\n");
		printf("  dfsym: Offline Symmetric direct-form FIR\n");
		printf("  dfsym_fp: Offline Symmetric direct-form FIR with fixed-point arithmetic\n");
		return 0;
    	} else if (strcmp(argv[1], "dffir") == 0) {
		printf("Offline direct-form FIR structure\n");
		offline_dffir(x, x_cnt, g_h, g_h_cnt, y);
	} else if (strcmp(argv[1], "dffir_online") == 0) {
		printf("Online direct-form FIR structure\n");
		online_dffir(x, x_cnt);
	} else if (strcmp(argv[1], "dfsym") == 0) {
		printf("Offline Symmetric direct-form FIR structure\n");
		offline_dfsymfir(x, x_cnt, g_h, g_h_cnt, y);
	} else if (strcmp(argv[1], "dfsym_fp") == 0) {
		printf("Offline Symmetric direct-form FIR structure with fixed-point arithmetic\n");
		static fixedpt xfp[BUFFER_LEN], yfp[BUFFER_LEN];
		static fixedpt hfp[50];
		convert_dbl_to_fp(x, xfp, x_cnt);
		convert_dbl_to_fp(g_h, hfp, g_h_cnt);
		offline_dfsymfir_fp(xfp, x_cnt, hfp, g_h_cnt, yfp);
	}

	// time measurement
	time(&end);
	dif = difftime(end, start);
	printf("Filtered in %.2lf seconds\n", dif);

	// write output file
	if( write_wav(y, x_cnt, &sfinfo) != 0) return 1;

	return 0;
}

/* Read WAV file
 */
static int read_wav(double *x, unsigned long *x_cnt, SF_INFO *sfinfo) {
	SNDFILE *infile;
	const char *infilename = "input.wav" ;
	//const char *infilename = "input-long.wav" ;

	if (! (infile = sf_open (infilename, SFM_READ, sfinfo))) {
		/* Open failed so print an error message. */
		printf ("Not able to open input file %s.\n", infilename) ;
		/* Print the error message from libsndfile. */
		puts (sf_strerror (NULL)) ;
		return 1 ;
	}
	if (sfinfo->channels > MAX_CHANNELS) {
		printf ("Not able to process more than %d channels\n", MAX_CHANNELS) ;
		return 1 ;
	}
	*x_cnt = sf_read_double (infile, x, BUFFER_LEN);
	sf_close (infile);
	return 0;
}

/* Write WAV file
 */
static int write_wav(const double *y, unsigned long y_cnt, SF_INFO *sfinfo) {
	SNDFILE *outfile;
	const char *outfilename = "output.wav" ;

	if(! (outfile = sf_open (outfilename, SFM_WRITE, sfinfo))) {
		printf ("Not able to open output file %s.\n", outfilename);
		puts (sf_strerror (NULL)) ;
		return 1;
	}

	sf_write_double(outfile, y, y_cnt);
	sf_close (outfile);
	return 0;
}

/* Direct-form FIR structure
 */
static void offline_dffir(const double *x, unsigned long x_cnt,
	const double *h, unsigned int h_cnt, double *y) {
	unsigned long n;
	unsigned int k;
	for(n = h_cnt; n < x_cnt; n++) {
		y[n] = 0;
		for(k = 0; k < h_cnt; k++) {
			y[n] += h[k] * x[n-k];
		}
	}	
}

/* Symmetric direct-form FIR structure
 */
static void offline_dfsymfir(const double *x, unsigned long x_cnt,
	const double *h, unsigned int h_cnt, double *y) {
	unsigned long n;
	unsigned int k;
	unsigned int h_cnt2 = h_cnt / 2;
	unsigned int k_rev;
	for(n = h_cnt; n < x_cnt; n++) {
		y[n] = 0;
		for(k = 0; k < h_cnt2; k++) {
			k_rev = h_cnt - k;
			y[n] += h[k] * x[n-k] + h[k_rev] * x[n-k_rev];
		}
	}	
}

/* Offline Symmetric direct-form FIR structure with fixed-point arithmetic
 */
static void offline_dfsymfir_fp(const fixedpt *xfp, unsigned long x_cnt,
        const fixedpt *hfp, unsigned int h_cnt, fixedpt *yfp) {
	unsigned long n;
	unsigned int k;
	unsigned int h_cnt2 = h_cnt / 2;
	unsigned int k_rev;
	for(n = h_cnt; n < x_cnt; n++) {
		yfp[n] = fixedpt_fromint(0);
		for(k = 0; k < h_cnt2; k++) {
			k_rev = h_cnt - k;
			yfp[n] = fixedpt_add( yfp[n], fixedpt_add(
				fixedpt_xmul( hfp[k], xfp[n-k] ), fixedpt_xmul( hfp[k_rev], xfp[n-k_rev] )
			));
		}
	}	
}

/* Online direct-form FIR structure
 *
 * Interrupt handler
 * (this simulates an A/D converter)
 */
short *g_x;
unsigned long g_x_cnt;
void interrupt_handler(int sig) {
	static unsigned long n = 0;
        unsigned int k;
	double y = 0;
	// for safety: if we reach the end of the array
	if(n > g_x_cnt) return;
	// FIR
	for(k = 0; k < g_h_cnt && k < n; k++) {
		y += g_h[k] * g_x[n-k];
	}
	// Result
	printf("y=%f\n", y);
	n++;
}
static void online_dffir(double *x, unsigned long x_cnt) {
	/* Quantify x as 2 byte integer - like from ADC and make publically available */
	static short x_int[BUFFER_LEN];
	unsigned long i;
	g_x_cnt = x_cnt;
	for(i = 0; i < x_cnt; i++) {
		x_int[i] = x[i] * 32768;
	}
	g_x = x_int;
	/* Prepare timer interrupt */
	struct sigaction sa;
	struct itimerval timer;
	memset (&sa, 0, sizeof (sa));
	sa.sa_handler = &interrupt_handler;
	sigaction (SIGVTALRM, &sa, NULL);
	timer.it_value.tv_sec = 0;
	timer.it_value.tv_usec = 100000;
	timer.it_interval.tv_sec = 0;
	timer.it_interval.tv_usec = 100000;
	setitimer (ITIMER_VIRTUAL, &timer, NULL);
	/* Forever loop. */
	while(1);
	//sleep(60);
}

/* Convert double array to fixed-point array
 */
static void convert_dbl_to_fp(const double *x, fixedpt *xfp, unsigned long cnt) {
	unsigned long n;
	for(n = 0; n < cnt; n++) {
		xfp[n] = fixedpt_rconst(x[n]);
	}
}

/* Convert fixe-point array to double array
 */
/*static void convert_fp_to_dbl(const fixedpt *xfp, double *x, unsigned long cnt) {
	unsigned long n;
	for(n = 0; n < cnt; n++) {
		x[n] = fixedpt_rconst(x[n]);
	}
}*/