/*         
 *        Authors: Ryan Gibson, Paul Ruvolo, Conor Sen
 *           File: TableSweeper.ic
 *  Last Modified: 13 March, 2003
 */

/*
 * Description of Signal -
 *  The signal integer is used to pass messages between the two main 
 *  tableSweeper processes.  The lowest order bit registers the presence of an 
 *  obstacle (or the white tape at the edge of the table).
 *  The second lowest order bit registers the shut off behavior (that is we 
 *  have hit so many obstacles we are done clearing the table.
 */

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

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

int obstacleCount = 0;
int turn = RIGHT;

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

#define L_MOTOR_POWER 20
#define R_MOTOR_POWER 20


// 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 MAX_DELTA 16

// TOUCH SENSOR location
#define TOUCH 8

int main() {
    int signal = 0;
    
    start_process(watchForTape(&signal));
    start_process(wander(&signal)); 
}

void watchForTape(int* signal) {
    int lastLight = analog(LIGHT);
    while (1) {
        if (hitObstacleCutOff(signal) != 0) {
            // we have hit too many obstacles, stop task
            stop();
            alloff();
            return;
        }
        if (lastLight - analog(LIGHT) >= MAX_DELTA) {
            /*
             * the tape has been seen.
             * Send stop command since wander may be busy (wander will also 
             * execute stop when it receives signal)
             */    
            
            stop();
            registerObstacle(signal);
            backup(0.3);
            while (obstaclePending(signal) != 0 && 
	           hitObstacleCutOff(signal) == 0) {
                // wait for signal from wander
            }
            lastLight = analog(LIGHT);      
        } else {  
            lastLight = analog(LIGHT);
            sleep(0.2);
        }
    }
}

void wander(int* signal) {
    while (1) {
        if (obstaclePending(signal) != 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 angles
             */
            stop();
            // it would be nice to have a pusher that could be powered by a 
            //  motor.  pushOffContents would activate it.
            pushOffContents();
            avoidObstacles(signal);
            obstacleDealtWith(signal);
        } else if (hitObstacleCutOff(signal) != 0) {
              // We have hit too many obstacles (stop)
              stop();
              alloff();
              return;
          } else {
              if (digital(TOUCH)) {
                  // we have hit an obstacle, let's deal with it
                  registerObstacle(signal);
                  avoidObstacles(signal);
                  obstacleDealtWith(signal);
              } else {
                  // we are in the clear, keep driving
                  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 ) {
    drive( -L_MOTOR_POWER, -R_MOTOR_POWER);
    sleep( time );
    stop();
}

// execute a left turn
void leftTurn( ) {
    int dc;
    
    drive( 60, -60 );
    sleep( .4 );
    dc = turnInfo(LEFT);
    stop();
}

/*
 * the following five functions abstract away the bitwise operations on the 
 * signal integer
 */

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

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

void registerCutOff(int* signal) {
    *signal |= 2;
}

void registerObstacle(int* signal) {
    *signal |= 1;
}

void obstacleDealtWith(int* signal) {
    *signal ^= 1;
}

// execute a right turn
void rightTurn( ) {
    int dc;
    
    drive( -60, 60 );
    sleep( .43 );
    dc = turnInfo(RIGHT);
    stop();
}

void avoidObstacles(int* signal) {
    // log obstacle
    obstacleCount++;
    stop();
    sleep(0.5);
    // backup an increasing amount as obstacles are hit (this covers more of 
    // the table)
    backup(((float)obstacleCount) / 10.0 + 1.0);
    stop();
    sleep(0.5);
    
    if (turnInfo(CHECK) == LEFT) {
        int dc;
        
        leftTurn();
        if (obstacleCount % 4 == 0) {
            leftTurn();
        }
        if (obstacleCount % 3 == 0) {
            // every one in three obstacles we should switch our orientation 
            // (prevents getting stuck in corners)
            dc = turnInfo(RIGHT);
        }
    } else if (turnInfo(CHECK == RIGHT)) {
          int dc;
          
          rightTurn();
          if (obstacleCount % 4 == 0) {
              rightTurn();   
          }
          if (obstacleCount % 3 == 0) {
              // ever one in three obstacles we should switch our orientation 
              // (prevents getting stuck in corners)
              dc = turnInfo(LEFT);
          }
      }
      
    sleep(1.0);
    if (obstacleCount >= OBSTACLE_CUT_OFF) {
        // we have hit many obstacles (let's stop running)
        registerCutOff(signal);
    }
}

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 pushOffContents() {
    // not implemented   
}