#include <stdio.h>    /* Standard input/output definitions */
#include <stdlib.h> 
#include <stdint.h>   /* Standard types */
#include <string.h>   /* String function definitions */
#include <unistd.h>   /* UNIX standard function definitions */
#include <fcntl.h>    /* File control definitions */
#include <errno.h>    /* Error number definitions */
#include <termios.h>  /* POSIX terminal control definitions */
#include <sys/ioctl.h>
#include <math.h>
#include "lo/lo.h"

int serialport_init(const char *serialport, int baud);
int serialport_read_until(int fd, char* buf, char until);
int shift_buffer(int *frame, int hop, int bsize);
int return_val(int baudrate, const char *serialport, int fd, int bsize);
int min(int var_array[], int len_var_array);
int max(int var_array[], int len_var_array);
float max_index(int var_array[], int len_var_array);
int mean_array(int array[], int start, int end);
int *adapt_thresh(int buffer[], int bsize, int smoothing_factor);
int *str_to_int(char *line, char *deliminator);
int *autocorrelation(int sensor_array[], int bsize);
int *rayleigh_win(int acf[], int center_lag, int bsize);
int *peak_pick(int win_acf[], int bsize);

int main() // main function
{	
	
	lo_address t = lo_address_new(NULL, "7770");
	
	int baudrate = 9600;
	const char *serialport = "/dev/cu.usbserial-A7007bfP";
	
	int fd = 0;
	fd = serialport_init(serialport, baudrate); // serialport initialization
	if (fd==-1) return -1;
	int frame_flag = 0; // if 0, then first frame. otherwise frame > 0, and perform shift_buffer.
	int bsize = 512; // buffersize
	int hop = bsize/4; // hop size for windowing function of sensor
	int buffer[bsize]; // buffer creation
	int *frame = malloc(bsize * sizeof(int)); // pointer allocation for buffer frame
	int *sensor_smooth;
	int *sensor_smooth_1; 
	int *sensor_smooth_2;
	int tempo, tempo_old = 0;//, tempo_older = 0; // tempo is generated from current tempo, tempo old = tempo from previous frame
	int smoothing_factor = 8; // originally set to 8, this rounds off peaks
	int *acf;
	int *win_acf;
	float period;//, period_old, period_older = 0;
	float resample_amt = 4.59375; // 44100 / 9600 ... to get equivalent value for audio control
	int center_lag = 80; // center point for rayleigh weighting
	int adj_weight;
	int avg_tempo;

	printf("start walking and hit return...");
	getchar();
		
	// main loop
	while(1) {
		
		int i = 0; // for buffer iteration
		if (frame_flag > 0) {
			// tempo_old = tempo; // previously determined tempo = tempo_old
			frame = &buffer[0];
			shift_buffer(frame, hop, bsize); // after first frame, samples @256-511 are moved to locations 0-255 
			
			for (i=hop; i < bsize; i++) { // following relocation, fill in remaining samples
				buffer[i] = return_val(baudrate, serialport, fd, bsize);
			}
			frame_flag++;			
		}
		
		else if (frame_flag < 1) { // for the first frame, sensor data is extracted
			for (i = 0; i < bsize; i++) {
					buffer[i] = return_val(baudrate, serialport, fd, bsize);
				}
				frame_flag++;
		}		

		sensor_smooth = adapt_thresh(buffer, bsize, smoothing_factor); // adaptive thresholding smooths the sensor data
		
		acf = autocorrelation(sensor_smooth, bsize); // autocorrelation function

		win_acf = rayleigh_win(acf, center_lag, bsize); // Rayleigh weighting around a center lag		
		
		period = max_index(win_acf, bsize);	// retrieve the highest peak from the processed buffer
		
		tempo = (60 / ((period*bsize/(baudrate*resample_amt))) * 2); // tempo of footstep 2x for 2 feet *** ACF method
			
		// adj_weight = center_lag - period; // prepare value to shift center lag by
		// 
		// if (abs(adj_weight) > period/3) { // if value is within reasonable amount
		// 	center_lag += (adj_weight/4); // start moving weighting center towards newly found period
		// }
		
		
		// some hacks to keep tempo in line	
		if (tempo > tempo_old) {
			if ((tempo - tempo_old) > (tempo_old/3)) {
				tempo = tempo / 2;
			}
		}
		else { // if old tempo is greater than new
			if ((tempo_old - tempo) > (tempo_old/3)) {// and if difference is greater than 1/3 old tempo
				tempo = tempo * 2;
			}
		}
		
		if (tempo > 180) { // if tempo is > 180, 1/2 it
			tempo = tempo/2;
		}
		
		if (tempo < 70) { // if tempo is < 70, 2x it
			tempo = tempo*2;
		}
		
		printf("tempo:%d\n", tempo);
		
		if (lo_send(t, "/sensor/tempo", "i", tempo) == -1) { // using osc, send out tempo on address 7770
			printf("OSC error %d: %s\n", lo_address_errno(t), lo_address_errstr(t));
		}
		tempo_old = tempo; // preserve newfound tempo to prevent against next cycle's outliers
	}
	free (frame);
}

int *peak_pick(int array[], int bsize) // returns array of 0 and >0 values at peak points
{
	int *peaks = malloc(bsize * sizeof(bsize));
	int i;
	
	for (i = 0; i < bsize; i++) {
		if ((array[i] > array[i-1]) && (array[i] > array[i+1])) {
			peaks[i] = array[i];
		}
		else {
			peaks[i] = 0;
		}
	}
	return peaks;
}

int *rayleigh_win(int acf[], int center_lag, int bsize)
{
	int *win_acf = malloc(bsize * sizeof(int));
	float w[bsize];
	float twopi = 6.28;
	int i,n;
	double t1,t2;
	
	
	for (i = 0; i < bsize; i++) {
		n = i+1;
		t1 = n / pow(center_lag,2);
		t2 = (-1 * (pow(-n,2))) / (2 * pow(center_lag,2));
		w[i] = t1 *exp(t2);
		
		win_acf[i] = w[i] * acf[i];
	}
	
	return win_acf;
}

int *autocorrelation(int sensor_array[], int bsize)
{
	int *acf = malloc(bsize * sizeof(int));
	int sum;
	int i,j;
	
	for (i=0; i<bsize; i++) {
		sum = 0;
		for (j = 0; j<(bsize-i); j++) {
			sum += (sensor_array[j] * sensor_array[j+i]);
		}
		acf[i] = sum / (bsize - i); //normalization
	}
	return acf;
}

int shift_buffer(int *frame, int hop, int bsize)
{
	int i;
	
	for (i = 0; i < hop;i++) {
		frame[i] = frame[i+hop];
	}
}

int return_val(int baudrate, const char *serialport, int fd, int bsize) //return running value of X/Y data
{
	int i;
	char buf[512];
	int n = 0;
	int sum_XY;
	int *m;
	char *ans;
	int *D_int = malloc(2 * sizeof(int));
	char *deliminator = malloc(sizeof(char));
	
	deliminator = " ";
	
	serialport_read_until(fd,buf,'\n');			
	D_int = str_to_int(buf, deliminator);
	
	sum_XY = ((D_int[0] + D_int[1]));
	usleep(10*1000);
	
	return sum_XY;
	free (D_int);
	free (deliminator);
	exit(EXIT_SUCCESS);
}

int *str_to_int(char *line, char *deliminator)
{
	char *token = malloc(sizeof(int));
	int *D_int = malloc(2 * sizeof(int));

	token =  strtok(line, deliminator);	
	D_int[0] = atoi(token);
	token =  strtok(NULL, deliminator);	
	D_int[1] = atoi(token);
	
	return D_int;
}

int serialport_init(const char *serialport, int baudrate) // prepares the serialport to send data
{
	struct termios toptions;
	int fd;
	fd = open(serialport, O_RDWR | O_NOCTTY | O_NDELAY);
	if (fd == 1) {
		perror("init_serialport: can't open port");
		return -1;
	}
	if (tcgetattr(fd, &toptions) < 0) {
		perror("init_serialport: can't get term attributes");
	}
	speed_t brate = baudrate;
	cfsetispeed(&toptions, brate); // input samplerate
	cfsetospeed(&toptions, brate); // output samplerate
	toptions.c_cflag &= ~PARENB;
	toptions.c_cflag &= ~CSTOPB;
	toptions.c_cflag &= ~CSIZE;
    toptions.c_cflag |= CS8;
	toptions.c_cflag &= ~CRTSCTS;
	toptions.c_cflag |= CREAD | CLOCAL;
	toptions.c_iflag &= ~(IXON | IXOFF | IXANY);
	toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
	toptions.c_oflag &= ~OPOST;
	toptions.c_cc[VMIN]  = 0;
	toptions.c_cc[VTIME] = 20;	
	if (tcsetattr(fd, TCSANOW, &toptions) < 0) {
		perror("init_serialport: can't set term attributes");
	}	
	return fd;
}

int serialport_read_until(int fd, char* buf, char until) // this keeps going in the main while loop above
{
	char b[1];
	int i=0;
	do {
		int n = read(fd, b, 1);
		if (n==-1) return -1;
		if (n==0) {
			usleep(10 *1000);
			continue;
		}
		buf[i] = b[0]; i++;
	}
	while (b[0] != until);
		buf[i] = 0;
		return 0;
}

int *adapt_thresh(int buffer[], int bsize, int smoothing_factor) // smoothing function, calls mean_array
{
	int m = 0;
	int i = 0;
	int k,t = 0;
	float a;
	int pre = smoothing_factor/2;
	int post = pre - 1;
	int t_array[2] = {bsize,post};
	int *sens_smooth = malloc(bsize * sizeof(int));
	t = min(t_array,2);
	int sum = 0;
	int adapt_avg;
	int max_array;
	int v = 0;

	
	max_array = max(buffer, bsize);
	
	for (i = pre; i < (bsize-post); i++) {// condition 2
		sens_smooth[i] = mean_array(buffer, (i-pre), (i+post));				
		sum += sens_smooth[i];
		v++;
		// printf("middle condition: i: %d, k:%d, i-pre: %d, i+post: %d, smoothval:%d\n", i, k, i-pre, i+post, sens_smooth[i]);
	}
	adapt_avg = sum / v;

	
	for (i = 0; i < pre; i++) { // condition 1
		int k_array[2] = {(i+pre), bsize};
		k = min(k_array, 2);
		sens_smooth[i] = adapt_avg;//mean_array(buffer, 1, k);
		// printf("first condition: i: %d, k:%d, smoothval:%d\n", i, k, sens_smooth[i]);
	}
	
	

	for (i = (bsize-post); i < bsize; i++) // condition 3
	{
		int k_array[2] = {(bsize-i), 1};
		k = max(k_array, 2);
		sens_smooth[i] = adapt_avg;//mean_array(buffer, k, bsize);
		// printf("i: %d, k:%d, smoothval:%d\n", i, k, sens_smooth[i]);
	}
	
	for (i=0; i < bsize; i++) {
		if (sens_smooth[i] < (adapt_avg)) {
			sens_smooth[i] = 0;
		}
	}	
	
	return sens_smooth;
}

int mean_array(int array[], int start, int end) // used within adaptive thresh
{
	int i;
	int sum = 0;
	int length = end - (start + 1);
	int mean;
	// printf("length:%d\n", length);
	for (i=start; i<=end; i++)
	{
		sum = sum + array[i];
		// printf("sum: %d, array[%d]: %d\n", sum, i, array[i] );
	}
	mean = sum/length;
	//printf("mean: %d\n", mean);
	return sum / length;
}

int min(int var_array[], int len_var_array)
{
	int i;
	//int len_var_array = 2;
	int ans = var_array[0];
	for (i=0; i < len_var_array; i++) {
		if (var_array[i] < ans){
			ans = var_array[i];

		}
	}
	return ans;
}

int max(int var_array[], int len_var_array)
{
	int i;
//	int len_var_array = 2;
	int ans = var_array[0];
	for (i=0; i < len_var_array; i++) {
		if (var_array[i] > ans){
			ans = var_array[i];

		}
	}
	return ans;
}

float max_index(int var_array[], int len_var_array)
{
	int i;
	int max_val = var_array[0];
	float max_index = 0;
	for (i=0; i < len_var_array; i++) {
		if (var_array[i] > max_val){
			max_val = var_array[i];
			max_index = i;
		}
	}
	return max_index;
}