#define LEFT_MOTOR 2
#define RIGHT_MOTOR 0
#define LEFT_IR 6
#define RIGHT_IR 4
#define SLOW 35
#define TO_FAR 510
#define LOOK_DISTANCE 350
#define NINTY_DEGREES 22
#define DEPTH 27
#define RAMP_SPEED 50
int i, flag;
int leftInBox, rightInBox, leftAlign, rightAlign;
int min = 13;
int TAPE, FLOOR, TAPE_EDGE;
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

    Method Name:    turn()

    Description:    This method initiates the 90 degree turn inside
                    the square.  It is basically a loop that checks
                    different condintions.  The basic behavior is 
                    to turn until the a wheel encoder is above a 
                    measured value.  The turn is done by pulsing 
                    the motors for a very short time.  This gives
                    us more accuracy in the turn.

                    There are special cases that check to see if
                    the IR sensors hit a stipe of tape.

 * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
void turn()
{
    int leftTouch, rightTouch, leftTouchedAtStart;
    leftTouch = rightTouch = leftTouchedAtStart = 0;
    if(analog(LEFT_IR) > 100){
        leftTouchedAtStart = 1;
    }
    while(1){
        if(analog(LEFT_IR) >100 && !leftTouch){ // the left IR sensor hit the tape
            printf("leftTouch");
            leftTouch = 1;
        }
        if(analog(RIGHT_IR) > 100 && !rightTouch){ // right sensor hit
            printf("rightTouch");
            rightTouch = 1;
        }
        // back up until the right sensor goes from dark to light
        if((rightTouch && leftTouch) || (leftTouch&&!rightTouch) && !leftTouchedAtStart){     
            beep(); beep();
            rightTouch = 0;
            sleep(.5);
            while(1){
                if(analog(RIGHT_IR) > 100 && !rightTouch){
                    beep();
                    rightTouch = 1;
                    while(1){
                        if(analog(RIGHT_IR) < 50){
                            beep();  beep();
                            return;
                        }
                        motor(0, -35);
                        motor(2, -35);
                        msleep(2l);
                        motor(0, 0);
                        motor(2, 0);
                        msleep(50l);
                    }
                }
                motor(0, -35);
                motor(2, -35);
                msleep(2l);
                motor(0, 0);
                motor(2, 0);
                msleep(50l);
            }
            leftTouch = rightTouch = 0;
            printf("finished lr");
            break;
        }
        
        // actual turn
        motor(LEFT_MOTOR, 100);
        motor(RIGHT_MOTOR, -100);
        msleep(7l);
        motor(LEFT_MOTOR,0);
        motor(RIGHT_MOTOR,0);
        
        if(read_encoder(1) > NINTY_DEGREES){   // turn until approximatly 90
            break;
            motor(LEFT_MOTOR,0);
            motor(RIGHT_MOTOR,0);
        }
        msleep(50l);
        //printf("%d\n", read_encoder(1));
    }
    
}
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 
 *    Method Name:    allign()
 *
 *    Description:    This is the method that handles the allignment
 *                    to the black tape.  It moves forward until one
 *                    of the sensors touches the tape.  When an IR
 *                    hits and the other IR is off, the robot jerks to
 *                    try and equalize the two on the edge of the tape.
 *
 * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
void shimmy()
{
    i = 25;
    printf("shimmy shimmy\n");
    while(1){
        if(analog(LEFT_IR) <50){           // if not on tape
            leftAlign = 0;
            motor(LEFT_MOTOR, i);          // move forward a pulse
            //msleep(1l);                    // sleep
        }
        else if(!leftAlign){
              //beep();
              leftAlign = 1;
              motor(LEFT_MOTOR, -min);     // twitch back
              msleep(6l);
              motor(LEFT_MOTOR, 0);
          }
          
          if(analog(RIGHT_IR) < 50){
            rightAlign = 0;
            motor(RIGHT_MOTOR, i);
            // msleep(1l);
        }
        else if (!rightAlign){
              //beep();
              rightAlign = 1;
              motor(RIGHT_MOTOR, -min);    // twitch back
              msleep(6l);
              motor(RIGHT_MOTOR, 0);
          }
          if(leftAlign && !rightAlign){
            motor(LEFT_MOTOR, -min-10);
            motor(RIGHT_MOTOR, min+10);
        }
        if(!leftAlign && rightAlign){
            motor(LEFT_MOTOR, min-10);
            motor(RIGHT_MOTOR, -min+10);
        }
        if(leftAlign && rightAlign){
            beep();beep();beep();
            printf("lir = %d       rir = %d\n", analog(LEFT_IR), analog(RIGHT_IR));
            sleep(.5);
            // if not in an acceptable range do the whole thing over again
            if(abs(analog(LEFT_IR) - analog(RIGHT_IR)) > 10 && !(analog(LEFT_IR) > 100 && analog(RIGHT_IR)> 100)){
                beep();
                leftAlign = rightAlign = 0;         // reset 
                shimmy();                           // go again
                return;
            }
            else{
                return;
            }
        }
        msleep(5l);
        
        //printf("ir0 = %d       ir2 = %d\n", analog(4), analog(6));
        motor(LEFT_MOTOR, 0);                       // stop the pulse
        motor(RIGHT_MOTOR, 0);
        msleep(50l);
        i++;
        if(i < 75){                                 // reset the speed
            i = 25;
        }
    }
    
}
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 
 *
 *    Method Name:   ramp()
 *
 *    Description:   This method simply slowly increases the speed
 *
 * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
void ramp(int speed)
{
    if(speed == 0)
      return;
    if(speed < 0){
        for(i = 0; i>speed;i--){
            motor(0,i);
            motor(2,i);
            msleep(20l);
        }
        return;
    }
    for(i=0; i<speed;i++){
        motor(0,i);
        motor(2,i);
        msleep(20l);
    }
    
}
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 
 *
 *    Method Name:   abs()
 * 
 *    Description:   returns absolute value
 *
 * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
int abs(int x)
{
    if (x >= 0)
      return x;
    return -x;
}
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 
 *
 *    Method Name:    main()
 * 
 *    Description:
 *
 * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
void main()
{   
    int mode = 1;
    int count = 1;
    flag = 1;
    leftInBox = rightInBox = leftAlign = rightAlign = 0;
    enable_encoder(0);
    enable_encoder(1);
    
    
    while(!start_button()){min = knob(); printf("min = %d\n", min);msleep(100l);}
    while(!start_button()){FLOOR = analog(LEFT_IR); printf("floor = %d\n", FLOOR);msleep(100l);}
    while(!start_button()){TAPE = analog(LEFT_IR); printf("tape = %d\n", TAPE);msleep(100l);}
    
    while(0){
        printf("lir = %d       rir = %d\n", analog(LEFT_IR), analog(RIGHT_IR));
        msleep(100l);
        
    }
    
    //turn();
    #if 1
      ramp(RAMP_SPEED);
    while(1){        
        if(flag && read_encoder(0) > LOOK_DISTANCE ){  
            printf("looking for tape");                            // travel 6 ft
            motor(LEFT_MOTOR,SLOW);
            motor(RIGHT_MOTOR,SLOW);
            flag = 0;                                              //set flag to tell ir sensors to look
            
        }
        if(read_encoder(0) > TO_FAR && read_encoder(1)> TO_FAR){   // if it goes past 6ft
            beep(); beep();
            motor(LEFT_MOTOR, 0);                                  // stop
            motor(RIGHT_MOTOR, 0);
            reset_encoder(0);reset_encoder(1);                     // reset encoder
            sleep(.2);
            motor(LEFT_MOTOR, -SLOW);                              // backup
            motor(RIGHT_MOTOR, -SLOW);
            while(1){                                              // back up until
                if(read_encoder(0) > 140){                         // we've backed up far enough
                    motor(LEFT_MOTOR, 0);                          // stop
                    motor(RIGHT_MOTOR, 0);
                    sleep(.5);
                    break;                                         // break the loop 
                }
            }
            reset_encoder(0);reset_encoder(1);                     // reset encoders
            motor(LEFT_MOTOR, -SLOW);                              // turn to the  left
            motor(RIGHT_MOTOR, SLOW);
            while(read_encoder(0) < 10){}                          // a bit
            motor(LEFT_MOTOR, SLOW);                               // move forward and return to main loop
            motor(RIGHT_MOTOR, SLOW);
            
        }
        
        if(!flag&&analog(LEFT_IR) > 100){       // if left enters shut off motor
            motor(LEFT_MOTOR, -SLOW);
            msleep(9L);
            motor(LEFT_MOTOR, 0);
            leftInBox = 1;                     // chage state
        }
        
        if(!flag&&analog(RIGHT_IR) > 100){     // if right enters shut off motor
            motor(RIGHT_MOTOR, -SLOW);         // apply brake
            msleep(9L);
            motor(RIGHT_MOTOR,0);              // stop motor
            rightInBox = 1;                    // change state to inside box
        }
        
        if(leftInBox && rightInBox){           // enters a box after moving 6 ft
            
            sleep(.5);
            beep();
            reset_encoder(0);
            reset_encoder(1);
            
            motor(LEFT_MOTOR, SLOW);            // move forward
            motor(RIGHT_MOTOR, SLOW);
            while(read_encoder(1) < DEPTH){}
            motor(0, -10);                     // brakes
            motor(2, -10);
            beep();
            motor(0, 0);                       // stop    
            motor(2, 0);
            sleep(.5);
            reset_encoder(0);
            reset_encoder(1);
            turn();                            // turn
            beep();
            shimmy();                          // align
            
            reset_encoder(0);
            reset_encoder(1);
            flag =1;                           // reset flags
            leftInBox = rightInBox = leftAlign = rightAlign = 0;
            
            printf("cycle = %d\n", count/4);
            if(count%4 == 0 && count/4 == 3){
                beep();beep();beep();
                return;
            }
            count++;
            ramp(RAMP_SPEED);
        }
        
    }
    #endif
}