#include <stdio.h>
#include <stdlib.h>
#include <iostream.h>
#include <math.h>
#include <termios.h>
#include <sys/sched.h>
#include <sys/types.h>
#include <unistd.h>

#include "sysID.h"
#include "driver/PrTimer.h"
#include "matrix/PrVector.h"
#include "matrix/PrMatrix.h"

#define HZ	1000.0
#define MAX_FREQ	40.0
#define MAX_TIME	20.0
#define PI			3.1416
#define BUFFER_SIZE 	(int)(HZ*MAX_TIME)

static void saveBuffer(void);
static void saveInfo(void);
static void openDataFile(void);
static void closeDataFile(void);

static float time1 = 0.0;
static float ychirp;
static FILE *file1;
static PrMatrix *dataBuffer1 = new PrMatrix(BUFFER_SIZE,2);

#define DELAY_TIME	3.0
#define RAMP_TIME	4.0
#define RAMP_START	0.3
#define RAMP_HEIGHT	1.0 - RAMP_START

float generateChirp(float runTime)
{
	float chirp;
	float cTime = runTime - DELAY_TIME;
	float xFactor;
	static int chirpStartFlag = 0, chirpFinishFlag = 0;

	if(((cTime > 0.0) & (cTime < RAMP_TIME)))
	{
		xFactor = RAMP_START + cTime * (RAMP_HEIGHT)/(RAMP_TIME);	
		if(chirpStartFlag == 0)
		{
			printf("Chirp test starts... \n");
			chirpStartFlag = 1;
		}
	}
	else
		xFactor = 1.0;
	
	if(((cTime > 0.0) & (cTime < MAX_TIME)))
		chirp = xFactor * sin(0.5*(MAX_FREQ*2*PI)*cTime*cTime/MAX_TIME);
	else
	{
		chirp = 0.0;
		if((cTime > MAX_TIME) & (chirpFinishFlag == 0))
		{
			printf("Chirp test finished \n");
			chirpFinishFlag = 1;
		}
	}

	return(chirp);

}

static int bufferIndex = 0;

void saveInfo(void)
{
	if(bufferIndex < BUFFER_SIZE)
	{
		dataBuffer1->elementAt(bufferIndex,0) = time1;
		dataBuffer1->elementAt(bufferIndex,1) = ychirp;
		bufferIndex++;
	}
}

void saveBuffer(void)
{
	int i;
	openDataFile();

	for(i=0;i<bufferIndex;i++)
	{
		fprintf(file1,"%6.3f %6.3f\n",
						dataBuffer1->elementAt(i,0),dataBuffer1->elementAt(i,1));
	}

	closeDataFile();
}	

void openDataFile(void)
{
	file1 = fopen("datachirp.txt","w");
}

void closeDataFile(void)
{
	fclose(file1);
}
#if 0
void main (void)
{
	PrTimer timer(HZ);

	timer.start();

	while(time1 < 10.0)
	{
		ychirp = generateChirp(time1);
		saveInfo();
		time1 = timer.time();	
		timer.wait();

	}

	saveBuffer();




};

#endif
