#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1, button1, sensorTouch)
#pragma config(Sensor, dgtl2, button2, sensorTouch)
#pragma config(Sensor, dgtl3, limitS, sensorTouch)
#pragma config(Sensor, dgtl4, limitS2, sensorTouch)
#pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port1, motor1, tmotorVex393_HBridge, openLoop, encoderPort, I2C_1)
#pragma config(Motor, port10, motor2, tmotorVex393_HBridge, openLoop)
#pragma config(Motor, port2, motor3, tmotorVex393_HBridge, openLoop)
#pragma config(Sensor, in1, InfraCollector, sensorReflection)
#pragma config(Sensor, dgtl5, RedLED, sensorDigitalOut)
#pragma config(Sensor, dgtl11, StateLED, sensorDigitalOut)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

bool button1_pushed; //flag to store button1 input
bool button2_pushed; //flag to store button2 input
bool limitS_pushed;
bool limitS2_pushed;//flag to store limitS input

bool isLost;
int light_threshold;
// initial light threshold

void monitorInput(){

    if(SensorValue(button1) && !button1_pushed){
        button1_pushed = true;
    }//if

    if(SensorValue(button2) && !button2_pushed){
        button2_pushed = true;
    }//if

    if(SensorValue(limitS) && !limitS_pushed){
        limitS_pushed = true;
    }//if

    if(SensorValue(limitS2) && !limitS2_pushed){
        limitS2_pushed = true;
    }//if
}//monitorInput

void exercise(){

    enum T_state {
        MOTOR_IDLE = 0,
        MOTOR_FORWARD,
        MOTOR_FORWARD2,
        MOTOR_FINDING,
        MOTOR_FINDING2,
        RESEARCH,
        MOTOR_CONNECTING,
        WALL,
        COMPLETION
    };//enum

    T_state state = MOTOR_IDLE;

    while(true){
        // Monitor buttons
        monitorInput();

        switch(state){

        case MOTOR_IDLE:
            // MOTOR_IDLE state: motor off waiting to search
            motor[motor1] = 0;
            motor[motor2] = 0;
            motor[motor3] = 0;

            //swapping to searching state when button pushed, resetting light threshold for each new search
            if ( button1_pushed ){
                state = MOTOR_FINDING;
                light_threshold = 90;
                button1_pushed = false;
            }//if
        break;

        case MOTOR_FINDING:
            // MOTOR_FINDING state: turn motors on in opposite directions, search for the beacon.
            resetMotorEncoder(motor1);
            motor[motor1] = 50;
            motor[motor2] = 50;

            //loop to search for beacon
            while(SensorValue(InfraCollector)>light_threshold){
                //reset motor encoder and increment threshold after every 360 degree turn
                if(getMotorEncoder(motor1) > 2200){
                    light_threshold = light_threshold + 15;
                    resetMotorEncoder(motor1);
                }//if
            }//while

            //light has been found, go towards it
            state = MOTOR_FORWARD;

            //if the limit switches are pressed, go into the states to back out
            if(limitS_pushed || limitS2_pushed){
                state = WALL;
                limitS_pushed = false;
                limitS2_pushed = false;
            }//if
        break;
        /*
        case MOTOR_FINDING2:
            // MOTOR_FINDING state: turn motors on in opposite directions, search for the beacon.
            resetMotorEncoder(motor1);
            motor[motor1] = -30;
            motor[motor2] = -30;

            //loop to search for beacon
            while(SensorValue(InfraCollector) > light_threshold){
                //reset motor encoder and increment threshold after every 360 degree turn
                if(getMotorEncoder(motor1) < -2200){
                    light_threshold = light_threshold + 15;
                    resetMotorEncoder(motor1);
                }//if
            }//while

            //light has been found, go towards it
            state = MOTOR_FORWARD;

            //if the limit switches are pressed, go into the states to back out
            if(limitS_pushed || limitS2_pushed){
                state = WALL;
                limitS_pushed = false;
                limitS2_pushed = false;
            }//if
        break;
    */

        case RESEARCH:
            resetMotorEncoder(motor1);

            while(getMotorEncoder(motor1) < 100){
                if(SensorValue(InfraCollector) < light_threshold + 5){
                    isLost = false;
                    break;
                }
                else{
                    isLost = true;
                }
            if(limitS_pushed || limitS2_pushed){
                state = WALL;
                limitS_pushed = false;
                limitS2_pushed = false;
            }//if
            }

            state = MOTOR_FORWARD;
        break;

        case MOTOR_FORWARD:
            //MOTOR_FORWARD state: turn on motor, search for any abnormalities
            motor[motor1] = 60;
            motor[motor2] = -60;
            if(SensorValue(InfraCollector) < 80){
                state = MOTOR_FORWARD2;
            }

            if(getMotorEncoder(motor1) > 200){
                state = RESEARCH;
                isLost = false;
            }

            if(isLost){
                state = MOTOR_FINDING;
            }
            //if the limit switches are pressed, go into the states to back out
            if(limitS_pushed || limitS2_pushed){
                state = WALL;
                limitS_pushed = false;
                limitS2_pushed = false;
            }//if
        break;

        case MOTOR_FORWARD2:
            //MOTOR_FORWARD2 state: slow motors, search for any abnormalities
            motor[motor1] = 10;
            motor[motor2] = -10;
            if(SensorValue(InfraCollector) < 50){
                state = MOTOR_CONNECTING;
            }

            if(getMotorEncoder(motor1) > 200){
                state = RESEARCH;
                isLost = false;
            }

            if(isLost){
                state = MOTOR_FINDING;
            }
            //if the limit switches are pressed, go into the states to back out
            if(limitS_pushed || limitS2_pushed){
                state = WALL;
                limitS_pushed = false;
                limitS2_pushed = false;
            }//if
        break;

        case WALL:
            //WALL state: after collsion with wall, moves back and searches for beacon again
            resetMotorEncoder(motor1);

            //move back from wall and search for beacon again
            while(getMotorEncoder(motor1) > -300){
                motor[motor1] = -60;
                motor[motor2] = 60;
            }//while

                    light_threshold = light_threshold - 15;
                state = MOTOR_FINDING;
                motor[motor1] = 0;
                motor[motor2] = 0;
        break;

        case MOTOR_CONNECTING:
            // MOTOR_CONNECTING state: lower connector to attch cable attaching our cable, raise connector to original position
            wait1Msec(250);
            motor[motor1] = 0;
            motor[motor2] = 0;
            motor[motor3] = -22;
            wait1Msec(2500);
            motor[motor3] = 25;
            wait1Msec(2400);
            state = COMPLETION;
            motor[motor3] = 0;
        break;

        case COMPLETION:
            //COMPLETION state: connection has succeeded, back away and turn on red LED
            resetMotorEncoder(motor1);
            //back away
            while(getMotorEncoder(motor1) > -300){
                motor[motor1] = -60;
                motor[motor2] = 60;
            }//while

            resetMotorEncoder(motor1);
            //turn 90 degrees
            while(getMotorEncoder(motor1) < 550){
                motor[motor1] = 60;
                motor[motor2] = 60;
            }//while

            resetMotorEncoder(motor1);
            //move off cable
            while(getMotorEncoder(motor1) < 300){
                motor[motor1] = 60;
                motor[motor2] = -60;
            }//while

            motor[motor1] = 0;
            motor[motor2] = 0;
            SensorValue(StateLED) = 1;

            button1_pushed = false;
            state = MOTOR_IDLE;
        break;

        default:
        } //switch(state)
    } //while(1)
} //end exercise

task main(){
    button1_pushed = button2_pushed = false;
    exercise();
}//end main