/* Group 9 Project 2 robot code */

//motor constants
persistent int TURNM = 0; //turning motor
persistent int MOVEM = 1; //forward/back motor
persistent int LEFT = -200; //command to rotate left
persistent int RIGHT = 200; //command to rotate right
persistent int FORWARD = 50; //command to go forward
persistent int BACKWARD = -60; //command to move backward

//sensor constants
persistent int LLIGHT = 2; //left light sensor 2-6 or 20-23
persistent int RLIGHT = 3; //right light sensor
persistent int MLIGHT = 4; //middle light sensor
persistent int RF = 18; //left rangefinder 16-19
persistent int LBUMPER = 10; //left rangefinder 16-19
persistent int RBUMPER = 9; //right rangefinder

//other constants
persistent int RANGE_TOLERANCE = 70;
persistent int LIGHT_THRESHOLD = 10;

//global variables
int light, lightreading = 1000, range, bucketlocation, count,i=0;

void main()
{
    printf("Run me\n");
    
    while (!start_button())
      
      init_expbd_servos(2);
    
    printf("Runing\n");     
    
    start_process (lightIntensity());// start process lightIntensity
    start_process (bucketSeeker());// start process bucketSeeker
    start_process (bumping());//start prosecc bumping
    
    while(1)
      {
        while (lightreading > light+15 || lightreading < light-15)// check if lightreading is wirhing the tolerence
          {
            lightSeeker();// call lightSeeker
            if (count > 7) seekAgain();// if count is less than 7 call seekAgain
            sleep(0.5);
        }//end while
        
        motorForward();// move forward
        sleep(4.0);
        motorBreak();// stop the robot
        
    } //end while    
} //end main 

void lightSeeker()//find the brightest direction and turn the robot into the direction
{
    
    float time = .05;
    int loop; //loop control variable
    int inc = 100; //encoder sweep increment
    int end = 4000;
    int start = 600;
    int l3;
    
    servo0 = start;// rotate the servo to far left
    sleep(0.3);
    for(loop = start; loop < end; loop+=inc)  // sweep left to right
      {
        servo0 = loop;
        sleep (time);    
        
        if (light <= lightreading) {lightreading = light; l3 = loop;}// find minimum value of light sensor reading
                                                                     // and its direction
    }//end for
    
    servo0 = 2200;// rotate the servo to middle point so that the sensors can see straight forward
    sleep(0.5);
    count=0;    
    while (lightreading > light+15 || lightreading < light-15)
      {
        if  (range > RANGE_TOLERANCE) break;// while turning if sees sbucket break from the loop
        if (digital(LBUMPER) == 1 || digital(RBUMPER) == 1) break;// while turning if hits a rock break from thr loops
        if (count > 7) break;// this control maimum amount of turn
        if (l3 > 2200) turnRight();// turn right
        if (l3 < 2200) turnLeft();// turn left
        count++;// increase count by one
    }//end while
    
    turnBreak();// stop turning
      
}//end lightSeeker

void lightIntensity()// find sum of the readings from three light sensors
{
    while (1) {light = analog(LLIGHT) + analog(RLIGHT) + analog(MLIGHT);}
}

void bucketSeeker()// sees if bucket is near robot
{   
    float time = 0.1;
    int loop; //loop control variable
    int inc = 250; //encoder sweep increment
    int end = 3000;
    int start =2000;
    
    while (1)
      {
        for(loop = start; loop < end; loop+=inc)//left to right sweep
          {
            servo1 = loop;
            sleep (time);    
            if (analog(RF) >= range) {range = analog(RF); bucketlocation = loop;printf("Range %d %d\n", range, bucketlocation);}
            // find maxmum value from range sensor readinf and its directoin
            
        }//end for
        
        for(loop; loop > start; loop-=inc)//right to left sweep
          {
            servo1 = loop;
            sleep (time);    
            if (analog(RF) >= range) {range = analog(RF); bucketlocation = loop;printf("Range %d %d\n", range, bucketlocation);}
            // find maxmum value from range sensor readinf and its directoin

        }//end for
        
        range=0;// reset the maxmum range value to zero
        
    }//end while  
}//end bucketSeeker

void avoidObstacle()// avoid a rock
{
    if (digital(LBUMPER) == 1)//a rock on the left side of the robot
      {
        motorBreak();// stop the robot
        turnBreak();// stop truning
        sleep(0.5);
        turnRight();
        sleep(0.5);
        motorBack();
        sleep(0.6);
        backBreak();
        sleep(0.5);
        turnRight();
        sleep(0.8);
        turnBreak ();
        motorForward();
        sleep(0.5);
        motorBreak();
    }
    if (digital(RBUMPER) == 1)// a rock on the right side of the robot
      {
        motorBreak();// stop the robot
        turnBreak();// rtop turning
        sleep(0.5);
        turnLeft();
        sleep(0.5);
        motorBack();
        sleep(0.6);
        backBreak();
        sleep(0.5);
        turnLeft();
        sleep(0.8);
        turnBreak ();
        motorForward();
        sleep(0.5);
        motorBreak();
    }
}// end avoidObstacle

void bumping()
{
    while(1)
      {
        if (range > RANGE_TOLERANCE) bucketAvoid();// see if a bucket is near. If so, calls bucketAvoid
        
        if (digital(LBUMPER) == 1 || digital(RBUMPER) == 1)// see which side made contact with a rock
          { motorBreak();
            if (light < LIGHT_THRESHOLD) turnOffLight();// check if light sensor reading is lower than specified threshold
                                                        // if so, call fuction turnOffLight
            avoidObstacle();// call function avoidObstacle to move around a rock
        }
    }
}
void bucketAvoid()// avoid bucket
{
    if (bucketlocation < 2500)// a bucket on the left side
      {
        turnBreak();
        motorBreak();
        motorBack ();
        sleep(1.0);
        backBreak();
        turnRight();
        sleep(0.6);
        turnBreak();
        sleep(1.0);
    }// end if
    else// a bucket on the right side
      {
        turnBreak();
        motorBreak();
        motorBack ();
        sleep(1.0);
        backBreak();
        turnLeft();
        sleep(0.6);
        turnBreak();
        sleep(1.0);
    }
}
void turnOffLight()// ensure completing the circit
{   
    int i;
    for (i=0; i < 3 ;i++)
      {
        turnLeft();
        turnLeft();
    }
}
void seekAgain()// after specified amount of turn to find direction with bright light
                // move the robot forwad.
{
    turnBreak();
    motorForward(); 
    sleep(1.0);
    motorBreak();
}

void motorBreak() // stops robot
{
    motor(MOVEM,-10);
    sleep(0.02);
    ao();
}
void motorForward() // set motor power to move robot forward
{
    motor(MOVEM, 150);
    sleep(0.1);
    motor(MOVEM, FORWARD);
}
void motorBack() // set motor power to move robot backward
{
    motor(MOVEM, 150);
    sleep(0.1);
    motor(MOVEM, BACKWARD);
}
void backBreak()
{
    motor(MOVEM, -10);
    sleep(0.02);
    ao();
}
void turnRight()
{
    motor(TURNM, RIGHT);
    sleep(0.6);
}
void turnLeft()
{
    motor(TURNM, LEFT);
    sleep(0.4);
}
void turnBreak()
{
    motor(TURNM,0);
}