#include <stdio.h>
#include "includes.h"
#include <math.h>
#include "system.h"
#include "altera_up_avalon_parallel_port.h"
#include "alt_types.h"
#include "sys/alt_irq.h"
#include "io.h"
#include <cmath>

/* Definition of Task Stacks */
#define   TASK_STACKSIZE       2048
OS_STK    task1_stk[TASK_STACKSIZE];
OS_STK    task2_stk[TASK_STACKSIZE];
/* Definition of Task Priorities */
#define TASK1_PRIORITY      1
#define TASK2_PRIORITY      2
OS_EVENT * SemBeamEdgeChange;
#define CE(x) if ((err = x) != OS_NO_ERR) printf("Runtime error: %d line %d - see ucos_ii.h\n", err, __LINE__);
INT8U err;
void * context;

INT32U prevBeamTime;
int BeamPeriod[5];
int beamCount=3; //Start from case 3
int speed;
int halfPeriod;
int delayPeriod;
int thetaMax;
int signed direction;
#define SIZE_EMULATION   11  //represents a quarter of the emulation size sine wave

int signed emulatedPosition[4*SIZE_EMULATION-3];

float 	tempLookUpTable[4*SIZE_EMULATION-3];
int signed 	lookUpTable[4*SIZE_EMULATION-3];
int 	multiplier = 10000;





//Performs the calculations for half period, direction, speed and thetaMax
static void beamCalcs(void){

		if(BeamPeriod[0]+BeamPeriod[1]<BeamPeriod[2]+5){
			direction=  -1;
		}
	    if(BeamPeriod[1]+BeamPeriod[2]<BeamPeriod[0]+5){
	    	direction=  1;
	    }

	    thetaMax		= (((halfPeriod*2*84*speed)+500000)/1000000);
		speed 			= 40*1000/(BeamPeriod[0]+BeamPeriod[1]+BeamPeriod[2]); //*1000 to use fixed point

		//halfPeriod		= BeamPeriod[0]+2*BeamPeriod[1]+BeamPeriod[2]+BeamPeriod[3]; // I CHANGE THIS
		halfPeriod		= 4*BeamPeriod[1]+BeamPeriod[3];
}

//Prints the beam calculations (debug)
void printBeamCalcs(void){
	printf("%d\n",BeamPeriod[0]);
	printf("%d\n",BeamPeriod[1]);
	printf("%d\n",BeamPeriod[2]);
	printf("%d\n",BeamPeriod[3]);
	printf("Speed: %d\n",speed);
	printf("thetaMax = %d \n",thetaMax);
	printf("Half Period: %d\n\n\n",halfPeriod);
}


void lookUpTableGenerate(void){
	int i;
	for (i = 0; i < (SIZE_EMULATION-1)*4 ; i++){ //double the [0,pi] period
		tempLookUpTable[i]  =  multiplier*sin(i*(M_PI/2)/(SIZE_EMULATION-1));
		lookUpTable[i] 		= round(tempLookUpTable[i] );
		printf("%d\n",lookUpTable[i]);
	}
	//update lookUpTable
}

int currThetaMax; //if the difference between the curr and new theta max, re-emulate
int newThetaMax; //recalculated in trajectoryTask

//Simulates the next positions given the current theta, direction, updates emulatedPosition..
void emulatePosition(void){
	currThetaMax = thetaMax; //theta max used in current position emulation


	//inputs - thetaMax, direction,
	int i;
	//printf("emulatePosition()\n");
	for (i = 0; i < (SIZE_EMULATION-1)*4 ; i++){
		emulatedPosition[i] = direction*currThetaMax*lookUpTable[i]/multiplier; //replace with sin lookup fn, will have floating point error here (because int)
		//printf("%d,%d\n",emulatedPosition[i],i);
	}
	//outputs - emulatedPosition
}

void trajectorytask(void* pdata){

	int i=0;
	int targetAngleOutput;

	while(1){ // This will run through array of emulatedPosition, recalc thetaMax and reset i if too big diff

		delayPeriod = (halfPeriod*2)/(4*SIZE_EMULATION-3);
		//targetAngleOutput = direction*thetaMax*lookUpTable[i+1]/multiplier;
		//targetAngleOutput = thetaMax*lookUpTable[i+1]/multiplier;
		targetAngleOutput = emulatedPosition[i];
		IOWR(0x81030,0,targetAngleOutput);
		i++;

		OSTimeDlyHMSM(0,0,0,delayPeriod);

		//newThetaMax = direction*(((halfPeriod*2*84*speed)+500000)/1000000);
		newThetaMax = (((halfPeriod*2*84*speed)+500000)/1000000);

		if( (abs(newThetaMax) - abs(currThetaMax)) > 30 ){ //if the angle used to emulate position changes too much, recalc position
			i = 0;
			//printf("BUMPED!\n");
			emulatePosition();
			newThetaMax = thetaMax;
			currThetaMax = thetaMax;
		}


		if(i==4*SIZE_EMULATION-3){
			i=0;
			emulatePosition();
		}

	}
}

//beamTask, pends the beam interrupt
void beamtask(void* pdata){
	while (1){
		OSSemPend(SemBeamEdgeChange,0,&err); //Do everything below when interrupt happens

		//if(BeamPeriod[0]+BeamPeriod[1]+BeamPeriod[2]==0)printf("Initialised\n");

		if(BeamPeriod[1]+BeamPeriod[2]+BeamPeriod[3]<BeamPeriod[0])beamCount=3;
		if(BeamPeriod[2]+BeamPeriod[3]+BeamPeriod[0]<BeamPeriod[1])beamCount=3;
		if(BeamPeriod[3]+BeamPeriod[0]+BeamPeriod[1]<BeamPeriod[2])beamCount=3;

		//printf("%ld %ld %ld %ld\n\n",BeamPeriod[0],BeamPeriod[1],BeamPeriod[2],BeamPeriod[3]);
		INT32U currTime = OSTimeGet();


		switch(beamCount){
			case 0:
				BeamPeriod[0]	= currTime-prevBeamTime;
				prevBeamTime	= currTime;
				beamCount++;
				break;

			case 1:
				BeamPeriod[1]	= currTime-prevBeamTime;
				prevBeamTime	= currTime;
				beamCount++;
				break;

			case 2:
				BeamPeriod[2]	= currTime-prevBeamTime;
				prevBeamTime	= currTime;
				beamCount++;
				break;

			case 3:
				BeamPeriod[3]	= currTime-prevBeamTime;
				prevBeamTime	= currTime;
				beamCount		= 0;


				beamCalcs();
				//printBeamCalcs();  //OUTPUT THE DEBUG PRINTF STATEMENTS FOR THE BEAM
				emulatePosition();

				break;

			default: break;
		}
	}
}

//Beam ISR routine, services the interrupt flag
static void BeamISR(void * isr_context, alt_u32 id){
	IOWR(BEAM_BASE,3,0); //Clear the interrupt

	OSSemPost(SemBeamEdgeChange);
}//Beam ISR


/* The main function creates one task */
int main(void) {
	lookUpTableGenerate();
	SemBeamEdgeChange = OSSemCreate(0);

										// clear KEY PIO interrupt capture register,	                          	  	  	  	  	  	  	// hence clears any pending interrupts
	alt_irq_register(BEAM_IRQ, context, BeamISR);  // registers and enables KEY_IRQ to interrupt
	IOWR(BEAM_BASE,2,0x1);										// enable edge capture bits to generate interrupt for lowest 4 bits



	OSTaskCreateExt(beamtask,
				  NULL,
				  (void *)&task1_stk[TASK_STACKSIZE-1],
				  TASK1_PRIORITY,
				  TASK1_PRIORITY,
				  task1_stk,
				  TASK_STACKSIZE,
				  NULL,
				  0);

	OSTaskCreateExt(trajectorytask,
					  NULL,
					  (void *)&task2_stk[TASK_STACKSIZE-1],
					  TASK2_PRIORITY,
					  TASK2_PRIORITY,
					  task2_stk,
					  TASK_STACKSIZE,
					  NULL,
					  0);

	OSStart();
	return 0;
}