//
//Motors
#define RIGHT_MOTOR 0
#define LEFT_MOTOR 3
#define FAN 1

//Sensors
#define LIGHT_R 3
#define LIGHT_L 4
#define BUMP_R 8
#define BUMP_L 9

//Speeds
//Regular travel speed
#define SPEED 25       
//Turning speed when finding light
#define HOMINGSPEED 8
//Advancing speed when finding light
#define INCHINGSPEED 8 

//Control variables
int LTHRESHOLD;        //Threshold values for light sensors
int RTHRESHOLD;
int HARD_LTHRESHOLD;
int HARD_RTHRESHOLD;

int HOMING_IN = 0;     //Control value for processes
int EXTINGUISHED = 0;  //Control value to end program

/*
   Main process: Spawns other processes
   runAround:  navigates the maze, using 2 bump sensors to detect and avoid 
      walls. When light is detected above a certain intensity, the process 
      waits while homeIn runs
   homeIn:  when the light is found, this process homes in on the light source,
      using two light sensors.  If the light source is lost, control passes
      back to runAround
   displayLight: Displays the current readings on the light sensors on the LCD
      screen
*/

void main()
{
    int pid1, pid2, pid3;
    int lightLevel_R, lightLevel_L, i;
    
//    motor(FAN, 100);
//    sleep(5.0);
    alloff();
    //Wait for start button
    printf("Not started\n");
    while(!start_button());
    
    pid1 = start_process(runAround());
    pid2 = start_process(homeIn());
    
    //Sample ambient light to get a baseline for light sensing
    
    lightLevel_R = analog(LIGHT_R);
    lightLevel_L = analog(LIGHT_L);
    
    for (i = 0; i < 10; ++i)
      {
        lightLevel_L += analog(LIGHT_L);
        lightLevel_R += analog(LIGHT_R);
        printf("Light_R = %d    Light_L = %d\n", lightLevel_R, lightLevel_L);
        sleep(.2);
    }
    lightLevel_L = lightLevel_L/10;
    lightLevel_R = lightLevel_R/10;
    
    //Set light thresholds to fraction of ambient light.  homeIn begins
    //when either sensor falls under these thresholds
    
    if (lightLevel_L > 45)
      LTHRESHOLD = 45;
    else
      LTHRESHOLD = (int)(.8*(float)lightLevel_L);
    
    if (lightLevel_R > 25)
      RTHRESHOLD = 25;
    else
      RTHRESHOLD = (int)(.8*(float)lightLevel_R);
    
    //Set hard thresholds to be a fraction of the regular threshold.  If light
    //sensors fall under the hard threshold, the fan is turned on.
    
    HARD_RTHRESHOLD = (int)(0.6 * (float)RTHRESHOLD);
    HARD_LTHRESHOLD = (int)(0.6 * (float)LTHRESHOLD);
    
    printf("LThresh = %d      ", LTHRESHOLD);
    printf("RThresh = %d\n", RTHRESHOLD);
    sleep(3.0);
    
    pid3 = start_process(displayLight());
    
    while(!stop_button())
      {
        if (EXTINGUISHED)
          break;
    }
    
    alloff();
    kill_process(pid1);
    kill_process(pid2);
    kill_process(pid3);
    if(EXTINGUISHED)
      {
        //Hallelujah!
        tone(587.33, 0.75);
        tone(440.0, 0.25);
        tone(493.88, 0.25);
        tone(440.0, 0.5);
        printf("I am the firefighting master!");
    }
}

//Prints the current readings on each light sensor once per second

void displayLight()
{
    while(1)
      {
        printf("Left light: %d  ", analog(LIGHT_L));
        printf("Right light: %d\n", analog(LIGHT_R));
        sleep(1.0);
    }
}

//Doesn't do anything until one or both sensors is relatively close to a light
//source, then tries to inch its way forward until the robot is very close and
//well centered with respect to the flame.

void homeIn()
{
    while(!EXTINGUISHED)
    {
        int lastLeft = 0;
        int lastRight = 0;

        //One or both sensors sees light
        while((analog(LIGHT_L) < LTHRESHOLD) || 
              (analog(LIGHT_R) < RTHRESHOLD)) 
        {
            int currentLeft = analog(LIGHT_L);
            int minLeft = 999;
            int currentRight = analog(LIGHT_R);
            int minRight = 999;
            long time1;
            long time2;

            //            int centeringSpeed = 10;
            
            //Tell runAround() to stop
            HOMING_IN = 1;
            
            //This doesn't work yet...nor will it, until we gear down
            /*            
            //If only one sensor sees light, center
            while(!((analog(LIGHT_L) < LTHRESHOLD) && (analog(LIGHT_L) < RTHRESHOLD))
                   && (centeringSpeed > 5))
            {
                if((analog(LIGHT_L) < LTHRESHOLD) && (analog(LIGHT_R) > RTHRESHOLD))
                    go(-centeringSpeed, centeringSpeed);
                else if((analog(LIGHT_R) < RTHRESHOLD) && (analog(LIGHT_L) > LTHRESHOLD))
                    go(centeringSpeed, -centeringSpeed);
                sleep(0.4);
                alloff();
                centeringSpeed--;
                beep();
            }    
*/
            
            
            //Turn right until left sensor is just past directly pointing at
            //candle
            while ((currentLeft <= minLeft))// && (currentLeft > lastLeft))
            {
                go(HOMINGSPEED, -HOMINGSPEED);
                currentLeft = analog(LIGHT_L);
                if(currentLeft < minLeft)
                  minLeft = currentLeft;
            }
            lastLeft = minLeft;
            alloff();
   beep();
            //Pause between turns to avoid skidding
            sleep(0.5);
            
            currentRight = analog(LIGHT_R);
            time1 = mseconds();
            //Turn left until right sensor is centered on candle
            while((currentRight <= minRight))// && (currentRight > lastRight))
            {
                go(-HOMINGSPEED, HOMINGSPEED);
                currentRight = analog(LIGHT_R);
                if(currentRight < minRight)
                  minRight = currentRight;
            }
            lastRight = minRight;
            //Get the time it took to move from left sensor centered until
            //right sensor centered
            time2 = mseconds();
            alloff();
   beep();beep();
            sleep(0.5);
            
            //Turn back to halfway point between best readings for left and 
            //right light sensors.  With any luck, this is roughly where the
            //candle is.
            go(HOMINGSPEED, -HOMINGSPEED);
            msleep((long)((float)(time2 - time1) / 2.0));
            //Extra movement to make up for delay
            //sleep(0.03);
            alloff();
  beep();beep();beep();
            sleep(0.5);
            
            //Advance towards (hopefully) candle
//            if((time2 - time1) < 250L)
  //            go(INCHINGSPEED - 2, INCHINGSPEED - 2);
    //        else
            //              go(INCHINGSPEED, (int)((float)INCHINGSPEED * 1.5));
            go(HOMINGSPEED,HOMINGSPEED);
            sleep(0.8);
            alloff();
            sleep(1.0);    
            
            //Lost both sensors...back up
            if ((analog(LIGHT_L) > LTHRESHOLD) && 
                (analog(LIGHT_R) > RTHRESHOLD))
              {
                go(-10, -10);
                sleep(0.8);
                alloff();
            }
            //Lost left sensor only...turn right
            else if (analog(LIGHT_L) > LTHRESHOLD)
                go(8, -8);
              //Lost right sensor only...turn left
            else if (analog(LIGHT_R) > RTHRESHOLD)
                go(-8, 8);
            sleep(0.3);
            alloff();
            
            //Both sensors under hard thresholds, or either sensor far below 
            //their hard thresholds...means candle is very close    
            if (((analog(LIGHT_L) < HARD_LTHRESHOLD) && 
                 (analog(LIGHT_R) < HARD_RTHRESHOLD))
                || (analog(LIGHT_L) < ((int)(0.8 * (float)HARD_LTHRESHOLD)))
                || (analog(LIGHT_R) < ((int)(0.8 * (float)HARD_RTHRESHOLD))))
              {
                beep();
                
                //Candle directly in front of left sensor...turn right
                if(analog(LIGHT_L) < ((int)(0.8 * (float)HARD_LTHRESHOLD)))
                  go(-10, 10);
                //Candle in front of right sensor...turn left
                else if(analog(LIGHT_R) < ((int)(0.8 * (float)HARD_RTHRESHOLD)))
                  go(10, -10);
                sleep(0.2);
                alloff();  
                
                //Candle should be relatively centered now, turn on fan
                motor(FAN, 100);
                beep();
                sleep(7.0);
                alloff();
                sleep(1.0);
                //See if light goes away after fan is turned on
                if((analog(LIGHT_L) > LTHRESHOLD) && 
                   (analog(LIGHT_R) > RTHRESHOLD))
                {
                    EXTINGUISHED = 1;                
                    sleep(1.0);
                } 
                /*
                else
                {
                    go(10, 10);
                    sleep(0.8);
                    alloff();
                }
                */
            }           
        }
        //Lost the light...back to runAround()
        HOMING_IN = 0;
    }
}

void runAround()
{
    int lastbumped = 0;
    int index = 0;
    int turns[2];
    
    turns[0] = 0;
    turns[1] = 0;
    while(!EXTINGUISHED)
      {
        while(!HOMING_IN)
          {
            if(lastbumped == 0)
              go(22,22);
            
            else if(turns[index] == BUMP_R)
                go(25,22);
              else
                go(22,25);
            
            if(digital(BUMP_L) || digital(BUMP_R))
              {
                lastbumped = bumpAvoid(digital(BUMP_L),digital(BUMP_R), turns,index);
//                printf("lastbumped %d\n", lastbumped);
//                sleep(2.0);
                index = (index+1)%2;
                turns[index] = lastbumped;
                
//    printf("turns: %d   %d\n", turns[0], turns[1]);
//    sleep(2.0);
                
            }
        }
    }
}

int bumpAvoid(int leftbump, int rightbump, int turns[], int index)
{
    beep();
    beep();
/*    printf("%d   %d\n", turns[0], turns[1]);
    sleep(2.0);
    printf("new value\n");
    sleep(1.0);
*/    
    /*    if (turns[index] == BUMP_R)
      {
        go(-22,22);
        return BUMP_R;
    }
    else if (turns[index] == BUMP_L)
        {
          go(22,-22);
          return BUMP_L;
      }
*/    if(leftbump && rightbump)
      {
        reverse(1.0);
        if (turns[index] == BUMP_R)
          slightLeft();
        else 
          slightRight();
        return 0;
    }
    
    else if (leftbump && (turns[index] == BUMP_R) && (turns[(index+1)%2] == BUMP_L))
        {
          beep();
          ninetyTurnRight();
          return 0;
      }
      else if (rightbump && (turns[index] == BUMP_L) && (turns[(index+1)%2] == BUMP_R))
          {
            beep();
            ninetyTurnLeft();
            return 0;
        }
      else if(rightbump)
            {              
              reverse(.3);
              slightLeft();
              return BUMP_R;
          }
      else
            {
              reverse(.3);
              slightRight();
              return BUMP_L;
          }    
    
}

void slightLeft()
{
    go(-10,10);
    sleep(.5);
    go(0,0);
}

void slightRight()
{
    go(10,-10);
    sleep(.5);
    go(0,0);
}

void ninetyTurnRight()
{
    go(20,-20);
    sleep(1.0);
    go(0,0);
}

void ninetyTurnLeft()
{
    go(-20,20);
    sleep(1.0);
    go(0,0);
}

void reverse(float time)
{
    go(-15,-15);
    sleep(time);
    go(0,0);
}

void go(int lspeed, int rspeed)
{
    motor(RIGHT_MOTOR,rspeed);
    motor(LEFT_MOTOR,lspeed);
}