//************************************************************
//
//  Catapult
//  the Catapult firing module
//  takes an unsigned int "distance" as input, fires, gives no output 
//  (it's blocking code; when it's done, the firing's done.)
//
//************************************************************

//libraries
#include 
#include "ad_lib.h"
#include "kbhit.h"
#include "shiftreg.h"
#include "PWM.h"
#include "timer.h"
#include "ses.h"

//global variables

#define AD_Channel 0
#define THITRYTWOTH_SEC 7
#define SIXTEENTH_SEC 15
#define EIGHTH_SEC 30
#define QUARTER_SEC 61
#define HALF_SEC 122
#define ONE_SEC	244
#define TWO_SEC 488
#define THREE_SEC 732
#define FIVE_SEC 1220
//SET Motor on times HERE
#define TIME_INTERVAL_SHOOT QUARTER_SEC //shooting pulse time
#define TIME_INTERVAL_PAUSE ONE_SEC //post-shooting delay
#define TIME_INTERVAL_REBOUND QUARTER_SEC + EIGHTH_SEC //reload pulse time
#define TIME_INTERVAL_PREP THREE_SEC //pre-shooting delay

#define DEBUG 1

uchar TestForKey(EVENT_PARAM);
void RespToKey(SERVICE_PARAM);
uchar TestSpTimerExpired(EVENT_PARAM);  //Spoon Time
void RespSpTimerExpired(SERVICE_PARAM);
uchar TestPauseTimerExpired(EVENT_PARAM);
void RespPauseTimerExpired(SERVICE_PARAM);
unsigned char initialPORTGvalue;
uchar TestRebTimerExpired(EVENT_PARAM); //Rebound Time
void RespRebTimerExpired(SERVICE_PARAM);

//void main (void)
void Catapult(double distance)
{

float DistanceToDutyConversion;
int duty;

	DistanceToDutyConversion = 0.777;  //tweak this number to calibrate shoot distance.
	duty = 100; //(int)distance*DistanceToDutyConversion;
		 if (duty > 100) duty = 100;   //just in case "duty" is too big
 
    /* initialize the SES module to operate in round robin mode, with no
	  timer delay */
    SES_Init(SES_ROUND_ROBIN, SES_NO_UPDATE);
    TMR_Init(TMR_RATE_4MS);      //initialize timer module
	PWM_Init();
	PWM_SetPeriod(32);  //ranges from 1 to 32.  Tom recommends 8, .
	PWM_SetSpeed(0, PWM_CHAN1);	
	
	initialPORTGvalue = ReadPORTG();
	WritePORTG(0x04 | initialPORTGvalue);  //set port G, pin 2, high to set direction forward			   //set direction forward
	PWM_SetSpeed(duty, PWM_CHAN1);
	TMR_InitTimer(0, TIME_INTERVAL_SHOOT); //duration of shot
	if (DEBUG) printf("duty= %d\n", duty);
	
    /* register each pair of Events and Services routines*/
	SES_Register(TestSpTimerExpired, RespSpTimerExpired);
	SES_Register(TestPauseTimerExpired, RespPauseTimerExpired);
	SES_Register(TestRebTimerExpired, RespRebTimerExpired);

    //TMR_InitTimer(0, TIME_INTERVAL); //initialize timer 0 for use
	
	
	/* enter an infinite loop and handle the events */
	
	while (1) {
		
		SES_HandleEvents();
		if (TMR_IsTimerExpired(3)){
		    PWM_SetSpeed(0, PWM_CHAN1);
		    TMR_ClearTimerExpired(3);		  /*clears "1" flag in channel 2
		   									   *which is indicating an expired
											   *timer */
		    if (DEBUG) printf("Rebound time exp\n");
			SES_End();		   					
		   	break;
		   }
	}
}



unsigned char TestSpTimerExpired(EVENT_PARAM)
{
 	return(TMR_IsTimerExpired(0));
}

void RespSpTimerExpired(SERVICE_PARAM)
{
	PWM_SetSpeed(0, PWM_CHAN1);
	TMR_InitTimer(1, TIME_INTERVAL_PAUSE);
	TMR_ClearTimerExpired(0);		  /*clears "1" flag in channel 0
									   *which is indicating an expired
									   *timer */	
	if (DEBUG) printf("spoon time exp\n");	  
}


unsigned char TestPauseTimerExpired(EVENT_PARAM)
{
 	return(TMR_IsTimerExpired(1));
}

void RespPauseTimerExpired(SERVICE_PARAM)
{
	initialPORTGvalue = ReadPORTG();
	WritePORTG(0xFB & initialPORTGvalue);  //set port G, pin 2, low to set direction backward	   			  //set direction backward
	PWM_SetSpeed(100, PWM_CHAN1);
	TMR_InitTimer(2, TIME_INTERVAL_REBOUND);
	TMR_ClearTimerExpired(1);		  /*clears "1" flag in channel 1
									   *which is indicating an expired
									   *timer */
	if (DEBUG) printf("Pause time exp\n");	
						   
}
	 

unsigned char TestRebTimerExpired(EVENT_PARAM)
{
	return(TMR_IsTimerExpired(2));
}

void RespRebTimerExpired(SERVICE_PARAM)
{
	PWM_SetSpeed(0, PWM_CHAN1);
	TMR_ClearTimerExpired(2);		  /*clears "1" flag in channel 2
									   *which is indicating an expired
									   *timer */
	TMR_InitTimer(3, TIME_INTERVAL_PREP);
	if (DEBUG) printf("Rebound time exp\n");
	//SES_End();	
}