/*                                                                             8
 *                                                                             0  
 *        Authors: Ryan Gibson, Paul Ruvolo, Conor Sen
 *           File: TableSweeper.ic
 *  Last Modified: 5 May, 2003
 */

#use explego.icb

#define SERVO_FRONT 2000
#define SERVO_RIGHT 4000
#define SERVO_LEFT 500
// globals for signal passing between proceses

int signal = 0;
int forceTurn = 0;
int obstacleCount = 0;

// number of obstacles we are allowed to hit (i.e. number of encounters with the tape or when the touch sensor is depressed)

#define SERVO servo0
#define OBSTACLE_CUT_OFF 20

#define LIGHT_SENSOR_L 16
#define LIGHT_SENSOR_L_IS_LEGO 0
#define LIGHT_SENSOR_R lego0
#define LIGHT_SENSOR_R_IS_LEGO 1

#define OBSTACLE_DISTANCE_THRESHOLD 4000

#define LEFT 1
#define RIGHT 2
#define CHECK 3

int turn = RIGHT;

// MOTOR locations and their power settings
#define L_MOTOR 3
#define R_MOTOR 2
#define G_MOTOR 1

#define L_MOTOR_POWER 45
#define R_MOTOR_POWER 35


// LIGHT SENSOR location and settings
// - White paper reads around 12 on the light sensor
// - Tabletop is around 18
#define LIGHT 6
#define BOUNDARY 15

// infrared sensor (left and right, respectively)
// NOTE: ideally we would use light sensors and put tape around obstacles, but we didn't have any when this was coded
#define IR_L 4
#define IR_R 5

// Threshold for tape detection
#define EDGE_REFLECTANCE 210

// TOUCH SENSOR location
#define L_BUMP 12
#define R_BUMP 14

int main() {
    sonar_init();
    sleep(15.0);
    openGripper();
    sonar_init();
    init_expbd_servos(1);
    start_process(watchForTape());
    start_process(wander()); 
}

int getLeftLightSensor() {
    if (LIGHT_SENSOR_L_IS_LEGO == 1) {
        return LIGHT_SENSOR_L;
    } else {
        return analog(LIGHT_SENSOR_L);
    }
}

int getRightLightSensor() {
    if (LIGHT_SENSOR_R_IS_LEGO == 1) {
        return LIGHT_SENSOR_R;
    } else {
        
        return analog(LIGHT_SENSOR_R);
    }
}

void watchForTape() {
    while (1) {
        if (hitObstacleCutOff() != 0) {
            // we have hit too many obstacles, stop task
            stop();
            alloff();
            return;
        }
        while (getRightLightSensor() > EDGE_REFLECTANCE) {
            sleep(0.1);
        }

        /*
         * the tape has been seen.
         * Send stop command since wander may be busy (wander will also execute stop when it receives signal)
         */
        stop();
        registerObstacle();
        while (obstaclePending() != 0) {
            // wait for signal from wanderer
        }
    }
}

void wander() {
    while (1) {
        if (obstaclePending() != 0) {
            /*
             * The tape has been seen.
             * Execute stop command (a subsequent drive operation may have cancelled the stop command from watchForTape)
             * Always turn right.  This will trace a path around the perimeter of all obstacles (assuming they are all at right angles7
             */
            stop();
            pushOffContents();
            avoidObstacles();
            obstacleDealtWith();
        } else if (hitObstacleCutOff() != 0) {
              // We have hit too many obstacles (stop)
              stop();
              alloff();
              return;
          } else {
              int distance;
              
              SERVO = SERVO_FRONT;
              distance = sonar_sample();
              sleep(1.0);
              if (digital(L_BUMP) == 1 || digital(R_BUMP) == 1) {
                  avoidObstacles();
              } else if (!(distance >= OBSTACLE_DISTANCE_THRESHOLD) && distance >= 0) {
                    int distanceRight, distanceLeft;
                    
                    sleep(1.0);
                    //printf("obstcl cls %d\n", distance);
                    stop();
                    //sleep(10.0);
                    
                    SERVO = SERVO_RIGHT;
                    
                    distanceRight = sonar_sample();
                    //printf("sampled right sonar\n");
                    sleep(1.0);
                    
                    if (!(distanceRight >= 0)) 
                      distanceRight = 100000;
                    
                    SERVO = SERVO_LEFT;
                    distanceLeft = sonar_sample();
                    //printf("sampled left sonar\n");
                    sleep(1.0);
                    
                    if (!(distanceLeft >= 0))
                      distanceLeft = 100000;
                    
                    if (distanceRight > distanceLeft) {
                        forceTurn = RIGHT;
                    } else {
                        forceTurn = LEFT;
                    }
                    if (distanceRight > distance || distanceLeft > distance) {
                    //    printf("avoiding obstacles\n");
                        avoidObstacles();
                    } else {
                        forceTurn = 0;
                        drive( L_MOTOR_POWER, R_MOTOR_POWER );
                    }
                } else {
                    drive( L_MOTOR_POWER, R_MOTOR_POWER );
                }
          }
    }
}

void drive( int left, int right ) {
    motor( L_MOTOR, left );
    motor( R_MOTOR, right );
}

void backup( float time ) {
    sleep(1.0);
    drive( -L_MOTOR_POWER, -R_MOTOR_POWER );
    sleep( time );
    stop();
}

void leftTurn( ) {
    int dc;
    drive( -30, 30 );
    sleep( 1.0 );
    dc = turnInfo(LEFT);
    stop();
}

int obstaclePending() {
    if (signal & 1) {
        return 1;
    }
    return 0;
}

int hitObstacleCutOff() {
    if (signal & 2) {
        return 1;
    }
    return 0;
}

void registerCutOff() {
    signal |= 2;
}

void registerObstacle() {
    signal |= 1;
}

void obstacleDealtWith() {
    signal ^= 1;
}

void rightTurn() {
    int dc;
    drive( 30, -30 );
    sleep( 1.0 );
    dc = turnInfo(RIGHT);
    stop();
}

void avoidObstacles() {
    sleep(1.0);
    closeGripper();
    obstacleCount++;
    
    backup((float)obstacleCount / 10.0);
    sleep(0.5);
    if (forceTurn == RIGHT) {
        int dc;
        rightTurn();
        dc = turnInfo(RIGHT);
        forceTurn = 0;
    } else if (forceTurn == LEFT) {
          int dc;
          leftTurn();
          dc = turnInfo(LEFT);
          forceTurn = 0;
    } else if (turnInfo(CHECK) == LEFT) {
        int dc;
        leftTurn();
        if (obstacleCount % 4 == 0) {
            leftTurn();
        }
        dc = turnInfo(RIGHT);
    } else if (turnInfo(CHECK == RIGHT)) {
          int dc;
          rightTurn();
          if (obstacleCount % 4 == 0) {
              rightTurn();   
          }
          dc = turnInfo(LEFT);
      }
      
      sleep(1.0);
    if (obstacleCount >= OBSTACLE_CUT_OFF) {
        registerCutOff();
    }
    openGripper();
}

void stop( ){
    drive( 0, 0 );
}

/*
 * if task = CHECK => return LEFT OR RIGHT
 * if task = RIGHT => register RIGHT
 * if Task = LEFT => register LEFT
 */
int turnInfo(int task) {
    if (task == RIGHT || task == LEFT) {
        turn = task;
    }
    return turn;
}

void openGripper() {
    motor(G_MOTOR, 15);
    sleep(0.5);
    motor(G_MOTOR, 0);
}

void closeGripper() {
    motor(G_MOTOR, -15);
    sleep(0.5);
    motor(G_MOTOR, 0);    
}


void pushOffContents() {
    // not implemented   
}

void sonar_init() {
    bit_set(0x1026, 0x80); /* set Digital Input #9 as output */
    bit_clear(0x1021, 1); /* at TCTL2, */
    bit_set(0x1021, 2); /* set TIC3 for falling edge */
}

int sonar_sample() {
    int start_time;
    int dist;
    
    poke(0x1023, 1); /* clear tic3 flag */
    
    start_time= peekword(0x100e); /* capture start time */
    
    bit_set(0x1000, 0x80); /* send init pulse */
    bit_clear(0x1000, 0x80);
    
    while (peek(0x1000) & 0x1) { /* wait until receive echo */
        
        if (!(peekword(0x100e) - start_time) >= 0) {
            /* if too much time has elapsed, abort */
            return -1;
        }
        
        defer(); /* let others run while waiting */
        
    }
    
    
    msleep(10L); /* give unit time to reset */
    
    dist= peekword(0x1014)-start_time;
    return dist; /* tic3 has time of echo */
}