/* State Machine module */
/* By: Benjie Nelson, Nora Levinson, David Sirkin */
/* Dr. Von Sirlevson's Automated Locomotive Computation Engine */

/**************************************************/
/* Includes                                       */
/**************************************************/

#include <timers12.h>

#include "ME218_E128.h"
#include "S12eVec.h"
#include "ADS12e.H"

#include "StateMachine.h"
#include "GameDefs.h"
		    
/**************************************************/
/* Module Variables                               */
/**************************************************/

/* Store the current beacon DC and completed runs */

static unsigned int beaconDC;
static char completedRuns;

/* On what side of the field (not known at first) */

typedef enum {
  A, B, unknown

} AorB;

static AorB fieldSide = unknown;

/* Possible & current states for the main Game SM */

typedef enum {
  waiting, loading, traveling, unloading

} GameState;

static GameState gameState = waiting;

/* Possible & current states for the ball Load SM */

typedef enum {
  pressButton, waitForDrop, releaseButton
} LoadState;

static LoadState loadState = pressButton;

/* Possible & current states for the Traveling SM */

typedef enum {

  escapeCorner, startToTape, tapeToTape, tapeToDispenser,
  dispenserToGoal, lookForGoal, goalToTape

} TravelState;

static TravelState travelState = escapeCorner;

/* Possible & current states for the Unloading SM */

typedef enum {
  backUp, hitBL, hitBR, aligned  

} UnloadState;

static UnloadState unloadState;

/**************************************************/
/* Wait SM                                        */
/**************************************************/

char RunWaitSM(char event) {

  switch(event) {
    case SEE_FLASH:
         Wait(350);

         RightWheel(60);
         LeftWheel(-60);
         
         SetTimer(SM_TMR, 20);

         nextState = traveling;
         transition = TRUE;
         break;
  }

  return(event);  
}

/**************************************************/
/* Load SM                                        */
/**************************************************/

char RunLoadSM(char event) {

  static char bumpedTop = 0;
  char i;

 
  for(i = 0; i < 5; i++) {

    bumpedTop = 0;  
    RightWheel(40);
    LeftWheel(40);    
    SetTimer(WAIT_TIMER, 200);

    
    while(!Expired(WAIT_TIMER))
      bumpedTop = (bumpedTop || BumpIntoFT()); 

    RightWheel(31);

    LeftWheel(31);
    SetTimer(WAIT_TIMER, 1500);
    
    while(!Expired(WAIT_TIMER));

	  bumpedTop = (bumpedTop || BumpIntoFT());
		   
    if(!bumpedTop) {

      ReAlignWithDispenser();
      RightWheel(28);
      LeftWheel(28);
      Wait(1500);
    }

    RightWheel(-40);
    LeftWheel(-40);
    Wait(200);
  }

  
  // Back Away from dispenser
  RightWheel(-60);
  LeftWheel(-60);
  Wait(600);

  
   // Rotate Away from Goal
  if(fieldSide == A) {
    RightWheel(-45);

    LeftWheel(45);
    Wait(1000);  
  }
  else {

    RightWheel(45);
    LeftWheel(-45);
    Wait(1000);      
  }

  Stop();
  Wait(300);
  SetTimer(HOLD, 750);

  gameState = traveling;
  travelState = lookForGoal;

  return(event);  
}

/**************************************************/
/* Travel SM During Functions                     */
/**************************************************/

char DuringStartToTape(char event) {

  if(event == TAPE_CHANGE) {
    if(GetTapeL() && (fieldSide == unknown)) {

      fieldSide = A;
      return AT_1ST_TAPE;
    }
    if(GetTapeR() && (fieldSide == unknown)) {

      fieldSide = B;
      return AT_1ST_TAPE;
    }
  }
  return(event);
}

char DuringTapeToDispenser(char event) {
  if(event == BUMP_FT)

    return(AT_DISPENSER);
  
  return(event);
}

/**************************************************/
/* Travel SM                                      */
/**************************************************/

char RunTravelSM(char event) {

  switch(travelState) {
    case escapeCorner:
    	 switch(event) {

    	   case SM_TMR_XPD:
				LookForStart();
				SetTimer(SM_TMR,20);

				break;
    		        
		   case SEE_DISPENSER:
				if(BeaconStr() < 600) {

    		      RightWheel(HI_SPD);
  	              LeftWheel(HI_SPD);
  	              Wait(1000);

  	              travelState = startToTape;
				}
  	       	    break;
    		 }
         break;
  
  	case startToTape:

		 event = DuringStartToTape(event);

  	     switch(event) {

  	       case LOST_BEACON:
  	       	    break;
  	       	      
  	       case SEE_DISPENSER:
  	       	    break;

    	      
		   case AT_1ST_TAPE:
				Stop();
				Wait(300);
    	                  
				if(fieldSide == A) {

				  RightWheel(HI_SPD);
				  LeftWheel(HI_SPD);
				  Wait(400);

  	                 
				  LeftWheel(HI_SPD);
				  RightWheel(0);
				  Wait(500);
				}

				else {
				  RightWheel(HI_SPD);
				  LeftWheel(0);

				  Wait(700);
  	            }
  	            RightWheel(HI_SPD);
  	            LeftWheel(HI_SPD);

  	            Wait(500);
  	             
  	            travelState = tapeToTape;
				break;
  	     }

  	     break;

  	case tapeToTape:
  	     switch(event) {   	       	        	       	        	       	      
  	       case TAPE_CHANGE:

				if(GetTapeB() == black) {
				  RightWheel(-40);
                  LeftWheel(-40);

                  Wait(200);
                  
				  if(fieldSide == A) {
					LeftWheel(-40);

					RightWheel(40);
				  }
				  else {
				    LeftWheel(40);

  	        		RightWheel(-40);
				  }
				  AlignWithDispenser(CLEAR);
  	        	  travelState = tapeToDispenser;
				}

		        break;
  	     }
  	     break;
  	     
  	case tapeToDispenser:
		 event = DuringTapeToDispenser(event);

  	     switch(event) {   	       	        	       	        	       	      
  	       case AT_DISPENSER:
  	            RightWheel(LO_SPD);

  	            LeftWheel(LO_SPD);
  	            
  	            nextState = loading;
  	            transition = TRUE;

  	          	break;

		  case SM_TMR_XPD:
			   AlignWithDispenser(event);

			   break;
  	     }
  	     break;
  	     
    case lookForGoal:
         if(fieldSide == A) {

           LeftWheel(-35);
           RightWheel(35); 
         }
         else {

           LeftWheel(35);
           RightWheel(-35);
         }
    	 if(event == SEE_GOAL) {

    	   Stop();
		   Wait(200);
		   if(Expired(BEACON) && !Expired(HOLD)) { // not pointed at anything 

		     break;
			 if(fieldSide == A) {
               LeftWheel(-35);

               RightWheel(35); 
             }
             else {
			   LeftWheel(35);

               RightWheel(-35);
             }
             Wait(200);
		   }
		   SetTimer(SM_TMR, 4000);

    	
		   RightWheel(65);
           LeftWheel(65);
           Wait(750);

                
           if(fieldSide == A) {              
             LeftWheel(-40);
             RightWheel(40);

             Wait(150);  
             FollowBeacon(CLEARCCW);
           }
           else {      
             LeftWheel(-40);

             RightWheel(40);
             Wait(150);
             FollowBeacon(CLEARCCW);
           }

           travelState = dispenserToGoal;
		 }
		 break;
	       	        
    case dispenserToGoal:

           //during
           FollowBeacon(RUN);
			//end during
  	     switch(event) {
  	       case BUMP_FB:

  	            LeftWheel(-60);
                RightWheel(-60);
                Wait(300);

                
                RightWheel(-45);
                LeftWheel(45);
                Wait(1400);

                
  	            Stop();
  	            SetTimer(SM_TMR, 2000);
  	            gameState = unloading;

  	            unloadState = backUp; 
  	            break;
  	       	      
  	       case SEE_GOAL:

  	            SetTimer(SM_TMR, 2000);
  	            break;
  	            
  	       case SM_TMR_XPD:

  	            LeftWheel(-40);
                RightWheel(-40);
                Wait(1500);

                travelState  = lookForGoal;
                break;
  	     }
  	     break; 

  	case goalToTape:

  	     event = DuringGoalToTape(event);
  	     
  	     switch(event) { 
  	       case TAPE_CHANGE:

  	            if(GetTapeB() == black) {
				  RightWheel(-40);
                  LeftWheel(-40);

                  Wait(200);
				  
				  if(fieldSide == A) {
  	        	    LeftWheel(40);

				    RightWheel(-40);
				  }
				 else {
  	        	   LeftWheel(-40);

				   RightWheel(40);
				 }
				 AlignWithDispenser(CLEAR);
				 travelState = tapeToDispenser;
			   }

			   break;
  	     }
  	     break;
  }
  return(event);
}
    
/**************************************************/
/* Unload SM                                      */
/**************************************************/

char RunUnloadSM(char event) {
  switch(unloadState) {
  	case backUp:

		 LeftWheel(-65);
		 RightWheel(-65);
		     
		 switch(event) {

		   case SM_TMR_XPD:
				RightWheel(40);
				LeftWheel(40);

				Wait(750);
				SetTimer(SM_TMR, 1000);
				break;

		            
		   case BUMP_BR:
				SetTimer(SM_TMR, 1500); 
				unloadState = hitBR;

				break;
		            
		   case BUMP_BL:
				SetTimer(SM_TMR, 1500);

				unloadState = hitBL;
				break;  
		 }
		 break;

  	case hitBL:
  	     LeftWheel(-0);
         RightWheel(-90);

         
         switch(event) {
           case BUMP_BR:
                unloadState = aligned;

                break;
            
           case SM_TMR_XPD:
                Stop();
                RightWheel(35);

                Wait(400);
                
                LeftWheel(35);
                Wait(300);

                
                RightWheel(0);
                Wait(400);
                unloadState = backUp;

                break;
         }
  	     break;
  	     
    case hitBR:
		 LeftWheel(-90);

         RightWheel(-0);
         
         switch(event) {
           case BUMP_BL:

                unloadState = aligned;
                break;
            
           case SM_TMR_XPD:

                Stop();
                LeftWheel(35);
                Wait(400);
                
                RightWheel(35);

                Wait(300);
                
                LeftWheel(0);
                Wait(400);

                unloadState = backUp;
                break;
         }
  	     break;

  	case aligned:
  	     Stop();
  	     DropBalls();
  	     completedRuns++;

  	     
		 //moving away from goal/corner we're stuck in
		 RightWheel(60);
         LeftWheel(60);
         Wait(1000);

         Stop();
        
         //rotating
         if(fieldSide == A)  {
           LeftWheel(35);

           RightWheel(-35);
         }
         else {
           LeftWheel(-35);

           RightWheel(35);
         }
         Wait(200);
         Stop();
         
         Wait(300);

         LeftWheel(60);
         RightWheel(60);
        
  	     gameState = traveling;

  	     travelState = goalToTape;
  	     break;	     
  }
  return(event);      
}

/**************************************************/
/* Game SM                                        */
/**************************************************/

char RunGameSM(char event) {
  switch(gameState) {

    case waiting:
         RunWaitSM(event);
         break;
         
  	case loading:

  	     RunLoadSM(event);
  	     break;

    case traveling:			
         RunTravelSM(event);

         break;
  	     
  	case unloading:
  	     RunUnloadSM(event);
  	     break;
  }

  return(event);
}