/* Project 1
 * Group 10
 * Eskridge, Lopez, Maole, & Utley
 */

/*
 * Global variables
 */

// Boolean globals
int TRUE = 1;
int FALSE = 0;

// Motor IDs
int MOTOR_R_ID = 0;
int MOTOR_L_ID = 2;

// Encoder ID
int ENCODER_ID = 0;

// IR ID
int IR_ID = 3;

// The servo value that turns us at the correct angle
// to navigate around the track in a circle.
int SERVO_VALUE = 2735;

// The power levels to send to the motors
int MOTOR_PWR_FWD = 20;
int MOTOR_PWR_BRAKE = -40;
float MOTOR_PWR_BRAKE_TIME = 0.2;

// Flag indicating that the robot is done
// performing its task(s)
int DONE = FALSE;

// The amount of time to sleep while waiting for the servo to position itself
float SERVO_POSITION_SLEEP = 0.5;

// The amount of time to sleep between progress checks
float MONITOR_SLEEP = 0.2;

// IR threshold value between low and high
int IR_THRESHOLD_VALUE = 115;

// The default IR level
int DEFAULT_IR_VALUE = -1;

// The minimum number of lines we want to travers
int MIN_LINE_TRAVERSAL_COUNT = 24;

// The minimum number of encoder events in 3 circuits
int MIN_EVENT_COUNT = 3160 - 24;

// The amount of time to sleep between monitoring the distance
float MONITOR_DISTANCE_SLEEP = 0.2;

// Trigger for monitors to signal that they are finished
int CURRENT_DONE_TRIGGER_COUNT = 0;

// The threshold trigger value which signals that the monitors
// say we are done.  A value of '2' says both monitors must agree
// (logical AND).  A value of '1' says only one monitor must signal
// that it is finished (logical OR).
int DONE_TRIGGER_COUNT = 2;

// Current number of lines crossed
int CURRENT_LINE_COUNT = 0;

// Current encoder event count
int CURRENT_EVENT_COUNT = 0;

// PIDS
int CUTOFF_PID = 0;
int MONITOR_PID = 0;
int MONITOR_LINE_PID = 0;
int MONITOR_DISTANCE_PID = 0;

/*
 * Initialize the robot
 */
void init()
{
    printf( "\nInitializing..." );
    
    // Start our cutoff process
    CUTOFF_PID = start_process( cutoff() );
    
    // Initialize the servo
    init_expbd_servos( TRUE );
    
    // Position the servo
    servo0 = SERVO_VALUE;
    
    // Sleep so the servo has time to position itself
    sleep( SERVO_POSITION_SLEEP );
    
    // Start the monitor process
    MONITOR_PID = start_process( monitorProgress() );
}

/*
 * Monitors robot progress and signals completion of the tasks
 */
void monitorProgress()
{
    // Start the line traversal monitor
    MONITOR_LINE_PID = start_process( monitorLineTraversal() );
    
    // Start the distance monitor
    MONITOR_DISTANCE_PID = start_process( monitorDistanceTraveled() );
    
    // Loop until both are done
    while( CURRENT_DONE_TRIGGER_COUNT < DONE_TRIGGER_COUNT )
      {
        // Display our progress
        printf( "\nLines=[%d]      Events=[%d]",
               CURRENT_LINE_COUNT, CURRENT_EVENT_COUNT );
        
        // Sleep for a bit
        sleep( MONITOR_SLEEP );
    }
    
    // We are done
    MONITOR_PID = 0;
    DONE = TRUE;
}

/*
 * Monitors the number of black lines crossed
 */
void monitorLineTraversal()
{
    // Set up our variables
    int currentValue = 0;
    int lastValue = DEFAULT_IR_VALUE;
    int finished = FALSE;
    
    // Loop until we are done
    
    while( finished == FALSE )
      {
        // Get the current value of the sensor
        currentValue = analog( IR_ID );
        
        if( (currentValue > IR_THRESHOLD_VALUE) && (lastValue <= IR_THRESHOLD_VALUE ) )
          {
            // We have crossed a line
            CURRENT_LINE_COUNT++;
            
            // Have we crossed the desired number of lines?
            if( CURRENT_LINE_COUNT >= MIN_LINE_TRAVERSAL_COUNT )
              {
                // We are finished
                CURRENT_DONE_TRIGGER_COUNT++;
                finished = TRUE;
            }
            
            // Make sure the encoder is working
            if( CURRENT_LINE_COUNT == 1 )
              {
                // Is the encoder count at 0
                if( CURRENT_EVENT_COUNT == 0 )
                  {
                    // Something is wrong (maybe dying battery?)
                    // Stop monitoring the encoder by triggering it's finished value
                    CURRENT_EVENT_COUNT = MIN_EVENT_COUNT;
                }
            }
        }
        
        // Save the current value
        lastValue = currentValue;
    }
    
    // Clean up after ourselves
    MONITOR_LINE_PID = 0;
}

/*
 * Monitors the distance travelled
 */
void monitorDistanceTraveled()
{
    int finished = FALSE;
    
    // Only enable it if it isn't on
    if( read_encoder( ENCODER_ID ) == 0 )
      {
        // Not enabled
        enable_encoder( ENCODER_ID );
    }
    else
      {
        // Enabled
        reset_encoder( ENCODER_ID );
    }
    
    // Loop until we are finished
    while( finished == FALSE )
      {
        // Sleep for a bit
        sleep( MONITOR_DISTANCE_SLEEP );
        
        // Get the encoder event count
        CURRENT_EVENT_COUNT = read_encoder( ENCODER_ID );
        
        // Have we travelled the desired distance
        if( CURRENT_EVENT_COUNT >= MIN_EVENT_COUNT )
          {
            // We are done
            CURRENT_DONE_TRIGGER_COUNT++;
            finished = TRUE;
        }
    }
    
    // Clean up after ourselves
    MONITOR_DISTANCE_PID = 0;
}

/*
 * Turn on the motors and move forwards
 */
void startMotors()
{
    // Turn on both motors
    motor( MOTOR_R_ID, MOTOR_PWR_FWD );
    motor( MOTOR_L_ID, MOTOR_PWR_FWD );
}

/*
 * Turn off the motors and stop the vehicle
 */
void stopMotors()
{
    // Throw the motors in reverse so we don't coast
    motor( MOTOR_R_ID, MOTOR_PWR_BRAKE);
    motor( MOTOR_L_ID, MOTOR_PWR_BRAKE);
    
    // Sleep for a second so we stop
    sleep( MOTOR_PWR_BRAKE_TIME );
    
    // Turn all the motors off
    alloff();
}

/*
 * Cutoff method to shutdown robot
 */

void cutoff()
{
    // Loop forever until the stop button is pressed
    while( !stop_button() )
      {
        // Do nothing
    }
    
    // We are quiting so change our pid to 0
    CUTOFF_PID = 0;
    
    // Signal that we are done and quit
    DONE = TRUE;
}

/*
 * Displays a message and waits for the start button to be pressed
 */
void waitForStart( char msg[] )
{
    printf( msg );
    start_press();
}

/*
 * Cleans up any lingering processes
 */
void cleanup()
{
   
    // Clean up the cutoff thread
    if( CUTOFF_PID > 0 )
      {
        kill_process( CUTOFF_PID );
    }
    
    // Clean up the  thread
    if( MONITOR_PID > 0 )
      {
        kill_process( MONITOR_PID );
    }
    
    // Clean up the  thread
    if( MONITOR_LINE_PID > 0 )
      {
        kill_process( MONITOR_LINE_PID );
    }
    
    // Clean up the  thread
    if( MONITOR_DISTANCE_PID > 0 )
      {
        kill_process( MONITOR_DISTANCE_PID );
    }
    
    // Disable the encoder
    disable_encoder( ENCODER_ID );
    
    // Disable the servo
    init_expbd_servos(0);
}

/*
 * Main method
 */
void main()
{
    // Wait for the user to start everything
    waitForStart( "Press START..." );
    
    // Initialize
    init();
    
    // Start moving
    startMotors();
    
    // Idle processing until we are done
    while( DONE == FALSE )
      {
        // Do nothing
    }
    
    // Stop moving
    stopMotors();
    
    // Clean up after ourselves
    cleanup();
    
}