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

Strategy Module

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

/*---------------------------- Include Files ----------------------------*/
#include <stdio.h>
#include <stdlib.h>
#include <timers12.h>
#include "PWM.h"
#include "ADS12.h"
#include "game.h"

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

/*--------------------------- Module Defines ----------------------------*/
#define BALL_WAIT_TIME	2050
#define QUERY_TIME_30S	35000
#define DISPENSER_Y		27
#define DISPENSER_X		19
#define DISPENSER_Y_HOOP  23
#define BEACON_ERROR 	20 //us
#define L_DRIVE_SPEED	77
#define R_DRIVE_SPEED	75
#define L_HOOP_FORWARD 50
#define R_HOOP_FORWARD 50
#define L_HOOP_REVERSE 53
#define R_HOOP_REVERSE 50
#define HOOP_DIST			    41
#define DEADZONE			     5
#define SATURATION		    40
#define CONTROL_DISTANCE  40
#define HIGH_TIME_ERROR   10

/*---------------------------- Module Types -----------------------------*/
/*-------------------------- Module Variables ---------------------------*/
unsigned char gameState;
game_state_t currentState, nextState;
game_info_s *gameinfo;
/*-------------------------- Module Functions ---------------------------*/
game_state_t HandleRunningGame(game_state_t current_state);
unsigned char ChooseNextDispenser(void);
void ReInitializeGame(void);
/*----------------------------- Module Code -----------------------------*/

void DriveControlOn(void) {

  if(CheckBeaconDetected(RIGHT) == NO_BEACON)  {
    MotorForward(LEFT_DRIVE, L_DRIVE_SPEED - 5);
    MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED + 20);
  }
  else if(CheckBeaconDetected(LEFT) == NO_BEACON)  {
    MotorForward(LEFT_DRIVE, L_DRIVE_SPEED + 20);
    MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED - 5);
  }
  else {
    MotorForward(LEFT_DRIVE, L_DRIVE_SPEED);
    MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED);
  }
   /*
  if((ReturnHighTime(RIGHT) > ReturnHighTime(LEFT) + DEADZONE) && (ReturnHighTime(RIGHT) < ReturnHighTime(LEFT) + SATURATION)) {
  	MotorForward(LEFT_DRIVE, L_DRIVE_SPEED  + (ReturnHighTime(RIGHT) - ReturnHighTime(LEFT))/10);
  	MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED);
  }
  else if((ReturnHighTime(LEFT) > ReturnHighTime(RIGHT) + DEADZONE) && (ReturnHighTime(LEFT) < ReturnHighTime(RIGHT) + SATURATION)) {
  	MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED  + (ReturnHighTime(LEFT) - ReturnHighTime(RIGHT))/10);
  	MotorForward(LEFT_DRIVE, L_DRIVE_SPEED);
  }
  else if((ReturnHighTime(RIGHT) > ReturnHighTime(LEFT) + SATURATION)) {
    MotorForward(LEFT_DRIVE, L_DRIVE_SPEED + 2);
    MotorForward(RIGHT_DRIVE, RIGHT_DRIVE);
  }
  else if((ReturnHighTime(LEFT) > ReturnHighTime(RIGHT) + SATURATION)) {
    MotorForward(LEFT_DRIVE, L_DRIVE_SPEED);
    MotorForward(RIGHT_DRIVE, RIGHT_DRIVE + 2);
  }
  else {
  	MotorForward(LEFT_DRIVE, L_DRIVE_SPEED);
   MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED);
  }
  */
}

void InitStrategy(game_info_s *gameInfo) {
  currentState = DRIVING_TO_DISPENSER_START;
  gameinfo = gameInfo;
  printf("Strategy initialized \r\n");
}

unsigned char ChooseNextDispenser(void) {

	switch(gameinfo->teamColor) {
		case RED_TEAM: return 1; break;
		case GREEN_TEAM: return 2; break;
	}
}

void ReInitializeGame(void) {
	unsigned char i;
	gameinfo->currGameCondition = PAUSED;
	gameinfo->prevGameCondition = PAUSED;
	gameinfo->teamColor = ReturnColor();
	gameinfo->ourHoop = HOOP_3;
	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;
	currentState = DRIVING_TO_DISPENSER_START;
}

void UpdateStrategy(void) {

  game_condition_t currCondition;
  currCondition = gameinfo->currGameCondition;

  //printf("current game condition: %d \r\n", gameinfo->currGameCondition);
	switch(currCondition) {

		case(PAUSED):
			// Reset currentState and prepare to start game again
			if(gameinfo->prevGameCondition == RUNNING) { // game just ended
				ReInitializeGame();
			}
			else {
				// Do nothing (already paused or the game ended)
			}
		break;

		case(RED_WON):
			if(gameinfo->prevGameCondition == RUNNING) { // just won
				StopDriveMotors();
			  LiftMotorStop();
				if(gameinfo->teamColor == RED_TEAM)
					printf("We won! \r\n");
				else
					printf("We lost! \r\n");
			}
			else {
				// Still in RED WON, executing winning procedure
			}
		break;

		case(GREEN_WON):
			if(gameinfo->prevGameCondition == RUNNING) { // just won
				StopDriveMotors();
			  LiftMotorStop();
				if(gameinfo->teamColor == GREEN_TEAM)
					printf("We won! \r\n");
				else
					printf("We lost! \r\n");
			}
			else {
				// Still in GREEN WON, executing winning procedure
			}
		break;

		case(RUNNING):
			if(gameinfo->prevGameCondition != RUNNING) { // just started game
				printf("Game now running \r\n");
			  MotorForward(LEFT_DRIVE, L_DRIVE_SPEED);
        MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED);
				currentState = DRIVING_TO_DISPENSER_START;
			}
			//printf("current state: %d \r\n", currentState);
			nextState =	HandleRunningGame(currentState);
			currentState = nextState;
		break;
	}

	// Update the previous gameCondition
	gameinfo->prevGameCondition = currCondition;
}

game_state_t HandleRunningGame(game_state_t current_state) {

	game_state_t curr_state;
	game_state_t next_state;
	curr_state = current_state;

	switch(curr_state) {

		case DRIVING_TO_DISPENSER_START:
			printf("distance: %d \r\n", ReturnUltrasonicDistance());
			if(ReturnUltrasonicDistance() < DISPENSER_Y) {
				StopDriveMotors();
				RotateCCW();
				AddNewCommand(HOOP_3);
				printf("Got to start dispenser, aligning with far dispenser \r\n");
				next_state = ROTATING_TO_ALIGN_DISPENSER_FAR;
			}
			else
				next_state = DRIVING_TO_DISPENSER_START;
		break;

		case ROTATING_TO_ALIGN_DISPENSER_FAR:
			switch(gameinfo->teamColor) {
				case RED_TEAM:
					if((CheckBeaconDetected(LEFT) == DISPENSER_0)) {
						StopDriveMotors();
					  gameinfo->currDispenser = DISPENSER_0;
						printf("Aligned with far dispenser, going to query dispensers \r\n");
						next_state = QUERYING_DISPENSERS;
					}
					else
						next_state = ROTATING_TO_ALIGN_DISPENSER_FAR;
				break;

				case GREEN_TEAM:
					if((CheckBeaconDetected(LEFT) == DISPENSER_3)) {
						StopDriveMotors();
					  gameinfo->currDispenser = DISPENSER_0;
						printf("Aligned with far dispenser, going to query dispensers \r\n");
						next_state = QUERYING_DISPENSERS;
					}
					else
						next_state = ROTATING_TO_ALIGN_DISPENSER_FAR;
				break;
			}
		break;

		case QUERYING_DISPENSERS:
      //printf("querying currDisp %d querySent: %d queryComplete:%d \r\n", gameinfo->currDispenser, gameinfo->querySent, gameinfo->queryComplete);

			// Sending first command (initial query)
			if((gameinfo->currDispenser == 0) && (gameinfo->querySent == 0) && (gameinfo->initialQuery == 1)) {
				AddNewCommand(QUERY_DISPENSE + gameinfo->currDispenser);
				gameinfo->querySent = 1;
				TMRS12_InitTimer(QUERY_TIMER_30S, QUERY_TIME_30S);
				printf("Sent first query \r\n");
				next_state = QUERYING_DISPENSERS;
			}
			// Sending first command, not initial  query, wait for 30s ball respawn
			else if((gameinfo->currDispenser == 0) && (gameinfo->querySent == 0) && (TMRS12_IsTimerExpired(QUERY_TIMER_30S) == TMRS12_EXPIRED)) {
				TMRS12_ClearTimerExpired(QUERY_TIMER_30S);
				AddNewCommand(QUERY_DISPENSE + gameinfo->currDispenser);
				gameinfo->querySent = 1;
				TMRS12_InitTimer(QUERY_TIMER_30S, QUERY_TIME_30S); // Start next 30s query timer
				printf("Sent first query \r\n");
				next_state = QUERYING_DISPENSERS;
			}
			// Got query back, so send next one
			else if((gameinfo->currDispenser < 3) && (gameinfo->queryComplete == 1)) {
				gameinfo->queryComplete = 0; // get ready for next query
				gameinfo->currDispenser++;
				AddNewCommand(QUERY_DISPENSE + gameinfo->currDispenser);
				gameinfo->querySent = 1;
				next_state = QUERYING_DISPENSERS;
			}
			// Last query, do the next action
			else if((gameinfo->currDispenser == 3) && (gameinfo->queryComplete == 1)) {
				gameinfo->queryComplete = 0; // get ready for next query
				gameinfo->querySent = 0; // Get ready to query for ball dispension
				gameinfo->currDispenser = ChooseNextDispenser(); // Figure out which dispenser we're at
				gameinfo->remainingBalls = gameinfo->availableBalls[gameinfo->currDispenser]; // Get ready to request balls
				switch(gameinfo->teamColor) {
				  case RED_TEAM:
				    gameinfo->availableBalls[DISPENSER_0] = 3;
				  break;

				  case GREEN_TEAM:
				    gameinfo->availableBalls[DISPENSER_3] = 3;
				  break;
				}
				if(gameinfo->initialQuery == 1) {
					gameinfo->initialQuery = 0; // done with initial query procedure
				}
				printf("Sent last query, going to collect balls \r\n");
				next_state = COLLECTING_BALLS;
			}
			else
				next_state = QUERYING_DISPENSERS; // waiting for a response or timer expiration
		break;

		case COLLECTING_BALLS:
		  //printf("currDispenser: %d remaining balls: %d query sent:%d \r\n", gameinfo->currDispenser, gameinfo->remainingBalls, gameinfo->querySent);
			if((gameinfo->remainingBalls == 3) && (gameinfo->querySent == 0)) {
				AddNewCommand(DISPENSE_BALL + gameinfo->currDispenser); // request ball
				gameinfo->remainingBalls--;
				TMRS12_InitTimer(BALL_TIMER, BALL_WAIT_TIME);
				gameinfo->querySent = 1;
				printf("Requesting first ball dispension \r\n");
				next_state = COLLECTING_BALLS;
			}
			// Got ball, send next command
			else if((gameinfo->remainingBalls > 0) && (gameinfo->ballDispensed[gameinfo->currDispenser] == 1) && (TMRS12_IsTimerExpired(BALL_TIMER) == TMRS12_EXPIRED)) {
				TMRS12_ClearTimerExpired(BALL_TIMER);
				gameinfo->ballDispensed[gameinfo->currDispenser] = 0; // reset flag
				gameinfo->remainingBalls--; // should be = 0 last time through
				AddNewCommand(DISPENSE_BALL + gameinfo->currDispenser); // request ball
				TMRS12_InitTimer(BALL_TIMER, BALL_WAIT_TIME);
				gameinfo->querySent = 1;
				printf("Requesting next ball dispension \r\n");
				next_state = COLLECTING_BALLS;
			}
			// Got last ball
			else if((gameinfo->remainingBalls == 0) && (gameinfo->ballDispensed[gameinfo->currDispenser] ==1) && (TMRS12_IsTimerExpired(BALL_TIMER) == TMRS12_EXPIRED)) {
				TMRS12_ClearTimerExpired(BALL_TIMER);
			  gameinfo->querySent = 0;
			  gameinfo->ballDispensed[gameinfo->currDispenser] = 0;

				switch(gameinfo->teamColor) {
					case RED_TEAM:
						switch(gameinfo->currDispenser) {
							case DISPENSER_0:
								MotorForward(LEFT_DRIVE, L_DRIVE_SPEED);
								MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED);
								printf("Got last ball, driving to midline \r\n");
								TMRS12_InitTimer(MIDPOINT_TIMER, 1250);
								next_state = DRIVING_TO_MIDLINE;
							break;

							case DISPENSER_1:
								MotorForward(LEFT_DRIVE, L_DRIVE_SPEED);
								MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED);
								printf("Got last ball, driving to far dispenser \r\n");
								next_state = DRIVING_TO_DISPENSER_FAR;
							break;
						}
					break;

					case GREEN_TEAM:
						switch(gameinfo->currDispenser) {
							case DISPENSER_3:
								MotorForward(LEFT_DRIVE, L_DRIVE_SPEED);
								MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED);
								printf("Got last ball, driving to midline \r\n");
								TMRS12_InitTimer(MIDPOINT_TIMER, 1250);
								next_state = DRIVING_TO_MIDLINE;
							break;

							case DISPENSER_2:
								MotorForward(LEFT_DRIVE, L_DRIVE_SPEED);
								MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED);
								printf("Got last ball, driving to far dispenser \r\n");
								next_state = DRIVING_TO_DISPENSER_FAR;
							break;
						}
					break;
				}
			}
			else
				next_state = COLLECTING_BALLS;
		break;


		case DRIVING_TO_DISPENSER_FAR:
			//printf("distance: %d \r\n", ReturnUltrasonicDistance());
			if(ReturnUltrasonicDistance() > CONTROL_DISTANCE) {
			  DriveControlOn();
			  next_state = DRIVING_TO_DISPENSER_FAR;
			}
			else if((ReturnUltrasonicDistance() <= CONTROL_DISTANCE) && (ReturnUltrasonicDistance()> DISPENSER_X)) {
			  MotorForward(LEFT_DRIVE, L_DRIVE_SPEED);
			  MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED);
			  next_state = DRIVING_TO_DISPENSER_FAR;
			}
			else if(ReturnUltrasonicDistance() < DISPENSER_X) {
				StopDriveMotors();
				RotateCCW();
				printf("Got to far dispenser, rotating to align with close dispenser \r\n");
				next_state = ROTATING_TO_ALIGN_DISPENSER_CLOSE;
			}
			else
				next_state = DRIVING_TO_DISPENSER_FAR;
		break;

		case ROTATING_TO_ALIGN_DISPENSER_CLOSE:
			switch(gameinfo->teamColor) {
				case RED_TEAM:
					if((CheckBeaconDetected(LEFT) == DISPENSER_1)) {
  					StopDriveMotors();
  					gameinfo->remainingBalls = gameinfo->availableBalls[DISPENSER_0];
  					gameinfo->currDispenser = DISPENSER_0;
  					printf("Aligned with close dispenser, going to get balls \r\n");
  					next_state = COLLECTING_BALLS;
					}
					else
						next_state = ROTATING_TO_ALIGN_DISPENSER_CLOSE;
				break;

				case GREEN_TEAM:
					if((CheckBeaconDetected(LEFT) == DISPENSER_2)) {
						StopDriveMotors();
						gameinfo->remainingBalls = gameinfo->availableBalls[DISPENSER_3];
						gameinfo->currDispenser = DISPENSER_3;
						TMRS12_InitTimer(BALL_TIMER, BALL_WAIT_TIME);
						printf("Aligned with close dispenser, going to get balls \r\n");
						next_state = COLLECTING_BALLS;
					}
					else
						next_state = ROTATING_TO_ALIGN_DISPENSER_CLOSE;
				break;
			}
		break;

		case DRIVING_TO_MIDLINE:
			DriveControlOn();
			printf("Tape sensor: %d \r\n", CheckTapeSensor(MIDDLE));
			if((CheckTapeSensor(MIDDLE) == 1) && (TMRS12_IsTimerExpired(MIDPOINT_TIMER) == TMRS12_EXPIRED)) {
			  TMRS12_ClearTimerExpired(MIDPOINT_TIMER);
				StopDriveMotors();
				RotateCCW();
				printf("Got to midline, going to align with hoop \r\n");
				next_state = ROTATING_TO_ALIGN_HOOP;
			}
			else
				next_state = DRIVING_TO_MIDLINE;
		break;

		case ROTATING_TO_ALIGN_HOOP:
			switch(gameinfo->teamColor) {
			  case RED_TEAM:
			    if(CheckBeaconDetected(RIGHT) == HOOP_RED) {
			      StopDriveMotors();
			      TMRS12_InitTimer(SETTLE_TIMER, 200);
				    printf("Aligned with hoop, going to wait to settle \r\n");
				    next_state = WAITING_TO_SETTLE;
			    }
			    else
			      next_state = ROTATING_TO_ALIGN_HOOP;
			  break;

			  case GREEN_TEAM:
			    if(CheckBeaconDetected(RIGHT) == HOOP_GREEN) {
			      StopDriveMotors();
			      TMRS12_InitTimer(SETTLE_TIMER, 200);
				    printf("Aligned with hoop, going to wait to settle \r\n");
				    next_state = WAITING_TO_SETTLE;
			    }
			    else
			      next_state = ROTATING_TO_ALIGN_HOOP;
			  break;
			}

			 /*
			if(CheckTapeSensor(FRONT) == 1) {
				StopDriveMotors();
				TMRS12_InitTimer(SETTLE_TIMER, 2000);
				printf("Aligned with hoop, going to wait to settle \r\n");
				next_state = WAITING_TO_SETTLE;
			}

			else
				next_state = ROTATING_TO_ALIGN_HOOP;
				*/
		break;

	  case WAITING_TO_SETTLE:
	    if(TMRS12_IsTimerExpired(SETTLE_TIMER) == TMRS12_EXPIRED) {
	      TMRS12_ClearTimerExpired(SETTLE_TIMER);
				printf("Settled down, going to drive to dunk \r\n");
				next_state = DRIVING_TO_DUNK;
	    }
	    else
	      next_state = WAITING_TO_SETTLE;
	  break;

		case DRIVING_TO_DUNK:
			if(ReturnUltrasonicDistance() > HOOP_DIST) {
				MotorForward(LEFT_DRIVE, L_HOOP_FORWARD);
				MotorForward(RIGHT_DRIVE,R_HOOP_FORWARD);
				next_state = DRIVING_TO_DUNK;
			}
			else if (ReturnUltrasonicDistance() < HOOP_DIST) {
				MotorReverse(LEFT_DRIVE, L_HOOP_REVERSE);
				MotorReverse(RIGHT_DRIVE,R_HOOP_REVERSE);
				next_state = DRIVING_TO_DUNK;
			}
			else {
				StopDriveMotors();
				RotateCW();
				next_state = ALIGNING_CLOSE_DUNK;
				printf("Dunk distance reached, going to align with close beacon \r\n");
				//next_state = RAISING_CAGE;
			}
		break;

	  case ALIGNING_CLOSE_DUNK:
	    switch(gameinfo->teamColor) {
	      case RED_TEAM:
	        if(CheckBeaconDetected(RIGHT) == DISPENSER_1) {
	          StopDriveMotors();
	          RotateCCW();
	          printf("Aligned with close beacon, going to align with hoop");
	          next_state = ALIGNING_HOOP_DUNK;
	        }
	        else
	          next_state = ALIGNING_CLOSE_DUNK;
	      break;

	      case GREEN_TEAM:
	        if(CheckBeaconDetected(RIGHT) == DISPENSER_2) {
	          StopDriveMotors();
	          RotateCCW();
	          printf("Aligned with close beacon, going to align with hoop");
	          next_state = ALIGNING_HOOP_DUNK;
	        }
	      break;
	    }
	  break;

	  case ALIGNING_HOOP_DUNK:
	    switch(gameinfo->teamColor) {
	      case RED_TEAM:
	        if(CheckBeaconDetected(RIGHT) == HOOP_RED) {
	          StopDriveMotors();
	          LiftMotorUp();
	          printf("Aligned with hoop, going to raise cage");
	          next_state = RAISING_CAGE;
	        }
	        else
	          next_state = ALIGNING_HOOP_DUNK;
	      break;

	      case GREEN_TEAM:
	        if(CheckBeaconDetected(RIGHT) == HOOP_GREEN) {
	          StopDriveMotors();
	          LiftMotorUp();
	          printf("Aligned with hoop, going to raise cage");
	          next_state = RAISING_CAGE;
	        }
	        else
	          next_state = ALIGNING_HOOP_DUNK;
	      break;
	    }
	  break;


		case RAISING_CAGE:
			if((CheckLiftLimitSwitch(TOP) == 1) && (ReturnLiftMode() == LIFTING)) {
				LiftMotorStop();
			  TMRS12_InitTimer(DUMPING_BALL_TIMER, 500);
				printf("Cage raised, going to dump balls \r\n");
				next_state = DUMPING_BALLS;
			}
			else
				next_state = RAISING_CAGE;
		break;

	  case DUMPING_BALLS:
	    if(TMRS12_IsTimerExpired(DUMPING_BALL_TIMER) == TMRS12_EXPIRED) {
	       TMRS12_ClearTimerExpired(DUMPING_BALL_TIMER);
	       LiftMotorDown();
	       printf("Balls dumped, going to lower cage \r\n");
				 next_state = LOWERING_CAGE;
	    }
	    else
	      next_state = DUMPING_BALLS;
	  break;

		case LOWERING_CAGE:
			if((CheckLiftLimitSwitch(BOTTOM) == 1) && (ReturnLiftMode() == LOWERING)) {
				LiftMotorStop();
				MotorForward(LEFT_DRIVE, L_DRIVE_SPEED);
				MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED);
				printf("Lowered cage, going to back up from hoop \r\n");
				next_state = BACKING_UP_FROM_HOOP;
			}
			else
				next_state = LOWERING_CAGE;
		break;

		case BACKING_UP_FROM_HOOP:
			if(ReturnUltrasonicDistance() < DISPENSER_Y) {
				StopDriveMotors();
				RotateCW();
				printf("Backed up from hoop, going to realign with close dispenser \r\n");
				next_state = REALIGN_DISPENSER_CLOSE;
			}
			else
				next_state =	BACKING_UP_FROM_HOOP;
		break;

		case REALIGN_DISPENSER_CLOSE:
			switch(gameinfo->teamColor) {
				case RED_TEAM:
					if((CheckBeaconDetected(RIGHT) == DISPENSER_1)) {
						StopDriveMotors();
						MotorForward(LEFT_DRIVE, L_DRIVE_SPEED);
						MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED);
						printf("Re-aligned with close dispenser, going to drive to close dispenser \r\n");
						next_state = DRIVING_TO_DISPENSER_CLOSE;
					}
					else
						next_state = REALIGN_DISPENSER_CLOSE;
				break;

				case GREEN_TEAM:
					if((CheckBeaconDetected(RIGHT) == DISPENSER_2)) {
						StopDriveMotors();
						MotorForward(LEFT_DRIVE, L_DRIVE_SPEED);
						MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED);
						printf("Re-aligned with close dispenser, going to drive to close dispenser \r\n");
						next_state = DRIVING_TO_DISPENSER_CLOSE;
					}
					else
						next_state = REALIGN_DISPENSER_CLOSE;
				break;
			}
		break;

		case DRIVING_TO_DISPENSER_CLOSE:
			if(ReturnUltrasonicDistance() > CONTROL_DISTANCE){
			  DriveControlOn();
			  next_state = DRIVING_TO_DISPENSER_CLOSE;
			}
			else if(ReturnUltrasonicDistance() <= CONTROL_DISTANCE  && (ReturnUltrasonicDistance()> DISPENSER_X)) {
			  MotorForward(LEFT_DRIVE, L_DRIVE_SPEED);
			  MotorForward(RIGHT_DRIVE, R_DRIVE_SPEED);
			  next_state = DRIVING_TO_DISPENSER_CLOSE;
			}
			else if(ReturnUltrasonicDistance() < DISPENSER_X) {
				StopDriveMotors();
				RotateCCW();
				printf("Arrived at close dispenser, going to align with far dispenser \r\n");
				next_state = ROTATING_TO_ALIGN_DISPENSER_FAR;
			}
			else
				next_state = DRIVING_TO_DISPENSER_CLOSE;
		break;
	}

	return next_state;
}