#include <stdio.h>

/* read a file (no headers)
 * keep a copy of the previous set of data, if the current line from the file
 * does not have 16x3 fields, we match it with the previous set of data
 * to find write NaN Values
 * 
 */
 #define TOTAL_FIELDS 48
 
struct frameData{
	double time;
	int frameNumber;
    double data[16*3];
    int valid[16*3];
}; typedef struct frameData frameData;

void rinit(FILE* input, FILE* output);
int rnan(FILE* input, FILE* output);
void printValues(FILE* output, frameData* current);
void correctValues(frameData** current);

static frameData* fd;


int main(void)
{
	/* open 2 files, input and output */
	FILE* input = fopen("./tennis_no_header.trc", "r");
	FILE* output = fopen("./tennis_nan.trc", "w");
	int i = 0;
	
	if(input == NULL || output == NULL){
		printf("Error opening files\n");
		exit(0);	
	}
	/* insert 1 line of text header */
	fprintf(output, "FRAME\tTIME\tR_SH_X\tR_SH_Y\tR_SH_Z\tR_EL_X\tR_EL_Y\tR_EL_Z\tR_WR_X\tR_WR_Y\tR_WR_Z\tRAC_1_X\tRAC_1_Y\tRAC_1_Z\tRAC_2_X\tRAC_2_Y\tRAC_2_Z\tRAC_3_X\tRAC_3_Y\tRAC_3_Z\tL_SH_X\tL_SH_Y\tL_SH_Z\tL_EL_X\tL_EL_Y\tL_EL_Z\tL_WR_X\tL_WR_Y\tL_WR_Z\tR_HIP_X\tR_HIP_Y\tR_HIP_Z\tR_KNEE_X\tR_KNEE_Y\tR_KNEE_Z\tR_ANKLE_X\tR_ANKLE_Y\tR_ANKLE_Z\tL_HIP_X\tL_HIP_Y\tL_HIP_Z\tL_KNEE_X\tL_KNEE_Y\tL_KNEE_Z\tL_ANKLE_X\tL_ANKLE_Y\tL_ANKLE_Z\tBALL_X\tBALL_Y\tBALL_Z\n");	

	/* read initial line, and initialize fd, will output to output FILE */
	rinit(input, output);

	printf("Initialization Complete\n");
	for( ;; i++){
		if(i % 1000 == 0){
			printf("%d Entires Processed\n", i);	
		}
		if(rnan(input, output) == 0)
			break;

	}
	fflush(output);
	fclose(output);
	fclose(input);
	free(fd);
	printf("File Written\n");
}

void rinit(FILE* input, FILE *output){
	char dataLine[1024];
	char* temp;
	int frameNum = 0;
	int i;
	frameData* current = (frameData*)malloc(sizeof(frameData));
	double currentField = 0;
	
	if(current == NULL){
		printf("Error Mallocing\n");
		exit(0);
	}	
	
	if(fgets(dataLine, 1024, input) == NULL){
		printf("Error initializing\n");
		exit(0);	
	}
	
	/* Read first value: frameNumber */
    temp = (char *)strtok(dataLine, "\t");
    sscanf(temp, "%d", &i);
	current->frameNumber = i;
    
    /* Read time */
    temp = (char *)strtok(NULL, "\t");
    sscanf(temp, "%lf", &currentField);
    current->time = currentField;
    
    /* assumes first set of data is valid, i.e. 16x3 fields */
	for(i = 0; i < 48; i++){
		temp = (char *)strtok(NULL, "\t");
		sscanf(temp, "%lf", &currentField);
		current->data[i] = currentField;
		current->valid[i] = 1;
	}
	
	printValues(output, current);
	fd = current;
}

/* similar to rinit but we have to compare the values we read in
 * if the total fields is not 48.
 * we need to find the missing values and put them as invalid
 */
int rnan(FILE* input, FILE *output){
	char dataLine[1024];
	char* temp;
	int frameNum = 0;
	int i;
	frameData* current = (frameData*)malloc(sizeof(frameData));
	double currentField = 0;
	
	if(current == NULL){
		printf("Error Mallocing\n");
		exit(0);
	}	
	
	if(fgets(dataLine, 1024, input) == NULL){
		printf("EOF\n");
		return 0;
	}
	
	/* Read first value: frameNumber */
    temp = (char *)strtok(dataLine, "\t");
    sscanf(temp, "%d", &i);
	current->frameNumber = i;
    
    /* Read time */
    temp = (char *)strtok(NULL, "\t");
    sscanf(temp, "%lf", &currentField);
    current->time = currentField;
    
	/* reads 16x3 fields, will break if eof is reached */
	for(i = 0; i < 48; i++){
		temp = (char *)strtok(NULL, "\t");
		if(temp == NULL){
			break;	
		}
		sscanf(temp, "%lf", &currentField);
		current->data[i] = currentField;
		current->valid[i] = 1;
	}
	
	/* i != 48 we need to correct the values */
	if(i != 48){
		correctValues(&current);
	}
	
	printValues(output, current);
	fd = current;
	
	return 1;
}

/* write to output file, if field is valid, write value else write Nan */
void printValues(FILE* output, frameData* current){
	int i = 0;
	
	/* write the frameNumber time first */
	fprintf(output, "%d\t%lf", current->frameNumber, current->time);
	
	for(i = 0; i < TOTAL_FIELDS; i++){
		if((current->valid)[i] == 0)
			fprintf(output, "\tNaN");
		else
			fprintf(output, "\t%lf", (current->data)[i]);
	}
	
	fprintf(output, "\n");
}


void correctValues(frameData** current){
	/* to keep things simple, we overwrite the previousReading field
	 * directly.
	 * 
	 * if sqrt of the "variance" is less than 20 we take it as the same value
	 * and overwrite the old ones.
	 * otherwise, we move on until we find a position to insert this value
	 * 
	 * This works because the current set of data has 3 points missing at most.
	 * It is not the most efficient or most effective method.
	 */
	 int i, j;
	 double diff;
	 frameData* toReturn;
	 
	 toReturn = (frameData*)malloc(sizeof(frameData));
	 if(toReturn == NULL){
	 	printf("Error Allocating space in match\n");
	 	exit(0);	
	 }
	 /* grab frame and time */
	 toReturn->frameNumber = (*current)->frameNumber;
	 toReturn->time = (*current)->time;
	 
	 for(i = 0, j = 0; i < TOTAL_FIELDS; i++){
	 	if(fd->valid[i])
	 		diff = (fd->data[i] - (*current)->data[j]);
	 	else
	 		diff = 21.0;
	 	
	 	if(diff < 20.0 && diff > -20.0){
	 		/* accetable reading */
	 		toReturn->data[i] = (*current)->data[j];
	 		toReturn->valid[i] = 1;
	 		j++;
	 	}else{
	 		/* not accetable, we want to insert NaN */
	 		toReturn->data[i] = 0;
	 		toReturn->valid[i] = 0;
	 	}
	 }
	 
	 /* free the old pointer */
	 free(*current);
	 
	 *current = toReturn;
	return;
		
}