//#define MAIN_TEST
//#define BEACON_TEST
//#define CAGE_SERVO_TEST
//#define DRIVE_MOTOR_TEST
//#define LIFT_MOTOR_TEST
//#define SPI_TEST
//#define DUNKABASKET
//#define ULTRASONIC_TEST
#define TOTAL_TEST
//#define HALF_COURT
//#define TIMER_TEST

/**************************************************************************

Main Module

**************************************************************************/

/*---------------------------- Include Files ----------------------------*/

#include <hidef.h>
#include <stdio.h>
#include <mc9s12e128.h>
#include "ME218_E128.h"
#include "S12eVec.h"
#include <timers12.h>
#include "PWM.h"
#include "ADS12.h"

#include "game.h"
#include "beacon.h"
#include "drive_motor.h"
#include "lift_motor.h"
#include "SPI.h"
#include "strategy.h"
#include "ultrasonic.h"
#include "switch.h"

/*--------------------------- Module Defines ----------------------------*/
/*---------------------------- Module Types -----------------------------*/
/*-------------------------- Module Variables ---------------------------*/
static game_info_s gameInfo;
/*-------------------------- Module Functions ---------------------------*/
/*----------------------------- Module Code -----------------------------*/


#ifdef BEACON_TEST
void main(void) {

	InitializeBeacons();

	while(1) {
			(void)printf("lBeacon: %d cHiPer: %d \r\n", CheckBeaconDetected(LEFT), ReturnHighTime(LEFT));
			(void)printf("rBeacon: %d cHiPer: %d \r\n", CheckBeaconDetected(RIGHT), ReturnHighTime(RIGHT));
	}
}
#endif // BEACON_TEST


#ifdef CAGE_SERVO_TEST
void main() {
	char press;
	InitCageServos();

	while(1) {
		if(kbhit() == 1) {
			press = (char)getchar();
			switch(press) {
				case 'o': OpenCage(); break;
				case 'c': CloseCage(); break;
			}
		}
	}
}
#endif // CAGE_SERVO_TEST


#ifdef DRIVE_MOTOR_TEST
void main(void) {

	char press;
	InitializeDriveMotors();
	printf("Drive motors initialized \r\n");

	while(1) {

		printf("Bump sensor: %d Front: %d middle %d \r\n", CheckBumpSensor(NORTH), CheckTapeSensor(FRONT), CheckTapeSensor(MIDDLE));

		if (kbhit() == 1) {

			press = (char)getchar();
			switch(press) {
				case 'w': MotorForward(LEFT_DRIVE, 50); MotorForward(RIGHT_DRIVE, 50); break;
				case 's': MotorReverse(LEFT_DRIVE, 50); MotorReverse(RIGHT_DRIVE, 50); break;
				case 'a': MotorForward(LEFT_DRIVE, 40); MotorReverse(RIGHT_DRIVE, 40); break;
				case 'd': MotorReverse(LEFT_DRIVE, 40); MotorForward(RIGHT_DRIVE, 40); break;
				case 'q': StopDriveMotors(); break;
				default: break;
			}
		}
	}

}
#endif // DRIVE_MOTOR_TEST


#ifdef LIFT_MOTOR_TEST
void main(void) {

	char press;

	InitializeLiftMotor();
	printf("Drive motors initialized \r\n");

	while(1) {


		printf("Top: %d Bottom: %d liftMode: %d \r\n", CheckLiftLimitSwitch(TOP), CheckLiftLimitSwitch(BOTTOM), ReturnLiftMode());
		if (CheckLiftLimitSwitch(TOP) == 1 && ReturnLiftMode() == LIFTING)
			LiftMotorStop();
		if (CheckLiftLimitSwitch(BOTTOM) == 1 && ReturnLiftMode() == LOWERING)
			LiftMotorStop();

		if (kbhit() == 1) {

			press = (char)getchar();
			switch(press) {
				case 'a': LiftMotorUp(); break;
				case 's': LiftMotorStop(); break;
				case 'd': LiftMotorDown(); break;
				default: break;
			}
		}
	}
}
#endif // LIFT_MOTOR_TEST


#ifdef SPI_TEST
void main(void) {

	char KeyStroke;
	unsigned char BeaconNum =0;
	gameInfo.currentGameState = PAUSED;
 	gameInfo.ourHoop = RED;
   gameInfo.ballDispensed = 0;
   gameInfo.availableBalls[0] = 0;
   gameInfo.availableBalls[1] = 0;
   gameInfo.availableBalls[2] = 0;
   gameInfo.availableBalls[3] = 0;
   TMRS12_Init(TMRS12_RATE_1MS);
   InitSPI(&gameInfo);

	printf("To set a new dispenser number press 0-3\n\r");
  	printf("To Query the number of balls in that dispenser, press Q\n\r");
  	printf("To dispense from that dispenser, press D\n\r");
  	printf("To set the hoop size to 2pt, press h\n\r");
  	printf("To set the hoop size to 3pt, press H\n\r");
  	printf("To query last command status, press S\n\r");
  	printf("To query game status, press G\n\r");

  while (1)
  {
  	UpdateSPI();

    if (kbhit() == 1)  // any key hit?
    {
      KeyStroke = (char)getchar();
      if (('0' <= KeyStroke) && ('9' >= KeyStroke))
      {
        BeaconNum = KeyStroke - '0'; // if num set beacon number
        (void)printf("Setting Beacon number to %d.\n\r", BeaconNum);
      }
      else
        switch (KeyStroke)
        {
          case 'd' :
          case 'D' :
            printf("Dispense from dispenser %x \n\r",  DISPENSE_BALL+BeaconNum);
            AddNewCommand(DISPENSE_BALL+BeaconNum);
          break;

          case 'g' :
          case 'G' :
            printf("Game_STATUS, %x\n\r", QUERY_GAME);
            AddNewCommand(QUERY_GAME);
  				break;

          case 's' :
          case 'S' :
            printf("Request_STATUS, %x\n\r", QUERY_STATE);
            AddNewCommand(QUERY_STATE);
  				break;

          case 'q' :
          case 'Q' :
            printf("QUERY_Dispenser %x\n\r",QUERY_DISPENSE+BeaconNum);
            AddNewCommand(QUERY_DISPENSE+BeaconNum);
       		break;

          case 'h' :
            printf("Hoop to 2pt, %x\n\r", HOOP_2);
            AddNewCommand(HOOP_2);
  				break;


          case 'H' :
            printf("Hoop to 3pt, %x\n\r", HOOP_3);
            AddNewCommand(HOOP_3);
  				break;

       		default:
       		  printf("Urecognized keystroke\n\r");
       		break;
        }
    }
  }
}
#endif // SPI_TEST


#ifdef ULTRASONIC_TEST
void main(void) {

	printf("Initialized ultrasonic \r\n");
	InitializeUltrasonic();

	while(1) {
		(void)printf("distance %d\r\n", ReturnUltrasonicDistance());
	}
}
#endif // ULTRASONIC_TEST


#ifdef TOTAL_TEST
void main(void) {

	unsigned char printUltrasonic = 0;
	unsigned char printBeacon = 0;
	unsigned char printTape = 0;
	unsigned char printLimitSwitch = 0;
	char KeyStroke;
	unsigned char BeaconNum =0;
	unsigned char i;

	InitSwitch();
	gameInfo.currGameCondition = PAUSED;
	gameInfo.prevGameCondition = PAUSED;
	gameInfo.teamColor = ReturnColor();
	gameInfo.ourHoop = HOOP_2;
	gameInfo.currDispenser = 0;
	for (i=0; i < NUM_DISPENSERS; i++) {
		gameInfo.ballDispensed[i] = 1;
		gameInfo.availableBalls[i] = 0;
	}
	gameInfo.querySent = 0;
	gameInfo.queryComplete = 0;
	gameInfo.initialQuery = 1;
	gameInfo.remainingBalls = 0;
   TMRS12_Init(TMRS12_RATE_1MS);
   InitSPI(&gameInfo);

	printf("To set a new dispenser number press 0-3\n\r");
  	printf("To Query the number of balls in that dispenser, press Q\n\r");
  	printf("To dispense from that dispenser, press D\n\r");
  	printf("To set the hoop size to 2pt, press h\n\r");
  	printf("To set the hoop size to 3pt, press H\n\r");
  	printf("To query last command status, press S\n\r");
  	printf("To query game status, press G\n\r");
	InitializeLiftMotor();
	InitializeDriveMotors();
	InitializeUltrasonic();
	InitializeBeacons();

	printf("Drive motors initialized \r\n");
	printf("teamColor: %d \r\n", gameInfo.teamColor);

	while(1) {

		if(printUltrasonic)
			printf("distance %d\r\n", ReturnUltrasonicDistance());
	  	if(printBeacon)
	  		printf("left: %d lPer: %d lHiPer: %d right: %d rPer: %d rHiPer %d \r\n", CheckBeaconDetected(LEFT), ReturnPeriod(LEFT), ReturnHighTime(LEFT), CheckBeaconDetected(RIGHT), ReturnPeriod(RIGHT), ReturnHighTime(RIGHT));
	  	if(printTape)
	  		printf("Bump sensor: %d Front: %d middle %d \r\n", CheckBumpSensor(NORTH), CheckTapeSensor(FRONT), CheckTapeSensor(MIDDLE));
	  	if(printLimitSwitch)
	  	 	printf("limitTop: %d limitBottom: %d liftmode: %d \r\n", CheckLiftLimitSwitch(TOP), CheckLiftLimitSwitch(BOTTOM),ReturnLiftMode());

	  	UpdateSPI();
	  	if (CheckLiftLimitSwitch(TOP) == 1 && ReturnLiftMode() == LIFTING)
				LiftMotorStop();
		if (CheckLiftLimitSwitch(BOTTOM) == 1 && ReturnLiftMode() == LOWERING)
				LiftMotorStop();

		if (kbhit() == 1) {
	      KeyStroke = (char)getchar();
	      if (('0' <= KeyStroke) && ('9' >= KeyStroke))
	      {
	        BeaconNum = KeyStroke - '0'; // if num set beacon number
	        (void)printf("Setting Beacon number to %d.\n\r", BeaconNum);
	      }
	      else
	        switch (KeyStroke)
	        {
	        		case 'i': MotorForward(LEFT_DRIVE, 85); MotorForward(RIGHT_DRIVE, 85); break;
					case 'm': MotorReverse(LEFT_DRIVE, 50); MotorReverse(RIGHT_DRIVE, 50); break;
					case 'j': MotorForward(LEFT_DRIVE, 50); MotorReverse(RIGHT_DRIVE, 50); break;
					case 'l': MotorReverse(LEFT_DRIVE, 50); MotorForward(RIGHT_DRIVE, 50); break;
					case 'k': StopDriveMotors(); break;
					case 'v': LiftMotorUp(); break;
					case 'b': LiftMotorStop(); break;
					case 'n': LiftMotorDown(); break;

	       	case 'U':  printUltrasonic = 1; break;
	     		case 'u':  printUltrasonic = 0; break;
	     		case 'Y': printBeacon = 1; break;
       		case 'y': printBeacon = 0; break;
       	  case 'T': printTape = 1; break;
	        case 't': printTape = 0; break;
	        case 'R': printLimitSwitch = 1; break;
	        case 'r': printLimitSwitch = 0; break;



	          case 'd' :
	          case 'D' :
	            printf("Dispense from dispenser %x \n\r",  DISPENSE_BALL+BeaconNum);
	            AddNewCommand(DISPENSE_BALL+BeaconNum);
	          break;

	          case 'g' :
	          case 'G' :
	            printf("Game_STATUS, %x\n\r", QUERY_GAME);
	            AddNewCommand(QUERY_GAME);
	  				break;

	          case 's' :
	          case 'S' :
	            printf("Request_STATUS, %x\n\r", QUERY_STATE);
	            AddNewCommand(QUERY_STATE);
	  				break;

	          case 'q' :
	          case 'Q' :
	            printf("QUERY_Dispenser %x\n\r",QUERY_DISPENSE+BeaconNum);
	            AddNewCommand(QUERY_DISPENSE+BeaconNum);
	       		break;

	          case 'h' :
	            printf("Hoop to 2pt, %x\n\r", HOOP_2);
	            AddNewCommand(HOOP_2);
	  				break;


	          case 'H' :
	            printf("Hoop to 3pt, %x\n\r", HOOP_3);
	            AddNewCommand(HOOP_3);
	  				break;

	       		default:
	       		  printf("Urecognized keystroke\n\r");
	       		break;
	        }
		}
	}
}
#endif // TOTAL_TEST


#ifdef HALF_COURT
void Init(void) {

	unsigned char i;
	InitSwitch();
	gameInfo.currGameCondition = PAUSED;
	gameInfo.prevGameCondition = PAUSED;
	gameInfo.teamColor = ReturnColor();
	gameInfo.ourHoop = HOOP_2;
	gameInfo.currDispenser = 0;
	for (i=0; i < NUM_DISPENSERS; i++) {
		gameInfo.ballDispensed[i] = 1;
		gameInfo.availableBalls[i] = 0;
	}
	gameInfo.querySent = 0;
	gameInfo.queryComplete = 0;
	gameInfo.initialQuery = 1;
	gameInfo.remainingBalls = 0;

	TMRS12_Init(TMRS12_RATE_1MS);
 	InitializeLiftMotor();
	InitializeDriveMotors();
	InitializeUltrasonic();
	InitializeBeacons();

 	InitSPI(&gameInfo);
 	InitStrategy(&gameInfo);
}

void main(void) {

	Init();
	while(1) {
		UpdateSPI();
		UpdateStrategy();
	}
}
#endif // HALF_COURT

#ifdef TIMER_TEST
void main(void) {
  TMRS12_Init(TMRS12_RATE_1MS);
  TMRS12_InitTimer(DUMPING_BALL_TIMER, 2000);

  while(1) {
    if(TMRS12_IsTimerExpired(DUMPING_BALL_TIMER) == TMRS12_EXPIRED) {
      TMRS12_ClearTimerExpired(DUMPING_BALL_TIMER);
      printf("Timer expired! \r\n");
      TMRS12_InitTimer(DUMPING_BALL_TIMER, 2000);
    }
  }

}
#endif