/* RoboRugby Competition framework example.
   This is a very simple example, and is not complete.
   It has no timeouts and no proper collision detection.
   The strategy is incomplete, and not very sensible!
   The functions to move the robot and use sensors 
   are in another file called myfunctions.ic
*/

// - includes -  you can add more #use commands here
#use framework08.ic
#use beacons.icb
#use myfunctions.ic

// - defines -  you can define more constants here using #define
//  START_BCN_RX is defined as internal beacon receiver for startup
#define START_BCN_RX 1
// define constants for values from light() function - table colour
#define WHITE 2
#define GREY  1
#define BLACK 0

// - globals -   you can define global variables here
int strategy = 1; // global variable to select strategy 1 or 2

void main()  //Do not alter this function 
{   
    rrprestart();  // runs YOUR pre-start function, 
    // to do calibration, choose strategy, or whatever...
    
    while(rrstartup(START_BCN_RX));  // start-up sequence
    // it is run again if the start is aborted
    
    rrstart(start_process(rrmain(), 20));  // starts YOUR main 
    // program as a process, with a 20ms time slice
    
    rrshutdown();  // shuts everything down after 60 seconds
} // end of main


/* Function for pre-start operations, for calibration, etc.
   If not needed, leave empty - do not delete this function!  */
void rrprestart()  
{
    while(!start_button()) // keep doing this until START is pressed
      {
        printf("strategy=%d STOP=change START=go\n", strategy);
        if (stop_button()) // if STOP is pressed
          {
            strategy = 3 - strategy; // change strategy variable
            tone(600.0*(float)strategy, 0.5); // tone to acknowledge
            while(stop_button()); // wait until button is released again
        }
        sleep(0.5); // delay so display doesn't flicker
    } // end of while not START
    beep(); // beep to acknowledge START pressed
    while(start_button()); // wait until button is released again
    sleep(0.5); // delay for safety
} // end of rrprestart


// Main program for competition.
void rrmain()  
{
    int home_bcn = startbeacon;  // we started facing our home beacon
    int target_bcn = 17 - startbeacon;  // we will score points at the other beacon
    beacon_init(2); // start the external beacon receiver
    
    // Stage 1 - go to the red ball by dead reckoning (hope the other robot does not do same!)
    forward(100); // drive at full speed
    sleep(1.2); 
    ao();    // stop when reach red ball (we hope!)
    
    // Stage 2 - drive to beacon with red ball
    while(light() != GREY) // we'll know we've arrived when we see grey table
      {
        if (checkbeacon(2) == target_bcn) // if we see our target beacon
          {
            forward(100); // drive towards it at full speed
            sleep(0.5); // short delay, so check beacon & light often
        }
        else   // we don't see our target beacon
          {
            spinright(30); // slow turn to find beacon
            sleep(0.1); // check for beacon every 0.1 second
        }
    } // end of while not grey
    ao(); // stop both motors - we have arrived in scoring zone
    
    //Stage 3 depends on strategy chosen 
    if (strategy==1) 
      {
        printf("Strategy 1...\n");
        // reverse away
        forward(-100);
        sleep(3.0);
        // turn on the spot for 30 seconds
        spinleft(100);
        sleep(30.0);
        // now turn the other way
        spinright(100);
        sleep(30.0);
    }
    else 
      {
        printf("Strategy 2...\n");
        // do something different ?
    } 
    
    //Stage 4 could go here...
    
} // end of rrmain