/* Example solution to challenge 2, using two switches to detect collisions
   and one light sensor to detect lines.
   On collision, reverse a bit, then turn away and drive again. 
   Does not include timeouts.  Does respond to STOP button.  */

// first define some replacements
#define RMOTOR 1
#define LMOTOR 2
#define LIGHT  analog(2)
#define THRESH 20
//  THRESH is threshold value - anything less than this is taken as white
#define BACKTIME 0.5
// BACKTIME is time spent reversing after a collision - adjust to suit robot
#define TURNTIME 0.8
// TURNTIME is time spent turning after each collision - adjust to suit robot

void main()   
{
    int hitcount = 0;    // variable for counting collisions
    printf("Press START\n");
    start_press();      //wait for start button to be pressed and released
    
    // This is stage 1 of the program - bounce off the walls 4 times
    drive(100);      //turn on motors to drive forwards
    do  // want to repeat the basic "bounce" operation
      {
        if( checkSwitch()==1)   // hit on the left
          { 
            hitcount = hitcount + 1;   // count the hit
            printf("Left hit, count = %d\n", hitcount);
            backup(BACKTIME);   // reverse a bit
            spinRight(TURNTIME);  // turn right
            drive(100);           // resume driving
        }
        else if( checkSwitch()==2)   // hit on the right
          { 
            hitcount = hitcount + 1;   // count the hit
            printf("Right hit, count = %d\n", hitcount);
            backup(BACKTIME);   // reverse a bit
            spinLeft(TURNTIME);  // turn left
            drive(100);           // resume driving
        }
    } while ( (hitcount < 4) && !stop_button() );  // repeat until 4 hits, or STOP
    
    // get here when stage 1 is finished
   
    // now start stage 2 - drive until find line
    printf("Driving to line\n");
    drive(100);   // not really needed, as still driving from stage 1
    
    while ( (LIGHT > THRESH) && (checkSwitch()==0) && !stop_button() ) 
      {  }       // do nothing until find line or hit something or STOP pressed
        
    ao();  // stop - we have finished
    // Now print message explaining why we stopped
    if (LIGHT < THRESH)  // we found a line
      {
        printf("Found line! Light = %d\n",LIGHT);
        beep();
    }
    else printf("No line found\n");  // hit something or STOP pressed
    
} // end of main

// function to turn on motors to drive forwards
void drive(int speed)
{
    motor(LMOTOR, speed);
    motor(RMOTOR, speed);
}

// function to reverse for given time
void backup(float time)
{
    motor(LMOTOR, -50); //turn on motors to reverse
    motor(RMOTOR, -50);
    sleep(time);       //wait for given time
    ao();              //turn off motors
}

// function to turn left for given time
void spinLeft(float time)
{
    motor(LMOTOR, -50); //turn on motors to spin left
    motor(RMOTOR, +50);
    sleep(time);       //wait for given time
    ao();              //turn off motors
}

// function to turn right for given time
void spinRight(float time)
{
    motor(LMOTOR, +50); //turn on motors to spin left
    motor(RMOTOR, -50);
    sleep(time);       //wait for given time
    ao();              //turn off motors
}

/* Function checkSwitch checks the state of two bump switches.
   It takes no arguments.
   It returns an integer:  0 means no bump, 1 means bump on left, 
              2 means bump on right, 3 means bump both sides  */
int checkSwitch(void)
{
    return (digital(10) + 2*digital(9));  // calculate combined switch value and send it back
}  // end of checkSwitch function