//
// ArmPreset states:
// A total of 3 can take place:
//   0 = Nothing,
//   1 = Preset to move arm up to scoring position,
//   2 = Preset to move arm down to lowest position for intake)
//
#define ARMPRESET_NONE          0
#define ARMPRESET_UP            1
#define ARMPRESET_DOWN          2
int g_armPreset = ARMPRESET_NONE;

//
// Initialize a variable fPower for full power.
//
int g_fPower = 127;

void ProcessDriveSubsystem()
{
    //
    // Set motor port 2 to controller channel 3. (Left Side Wheels)
    // Set motor port 3 to controller channel 2.   (Right Side Wheels)
    //
    motor[port2] = vexRT[Ch3];
    motor[port3] = vexRT[Ch2];
}    //ProcessDriveSubsystem

void ProcessIntakeSubsystem()
{
    if(vexRT[Btn6U] == 1){
        // Button 6U is pressed. (Intake System)
        // Run intake system at full power.
        motor[port5] = g_fPower;
    }else if(vexRT[Btn6D] == 1){
        // Button 6D is pressed. (Reverse Intake System)
        // Run intake system at full inverted power.
        motor[port5] = -g_fPower;
    }else
        // Button 6U or 6D weren't pressed.
        // Reset motor to 0. (Stop Intake System)
        motor[port5] = 0;
}    //ProcessIntakeSubsystem

void CheckArmPreset()
{
    static int prevBtn8L = vexRT[Btn8L];
    int currBtn8L;

    currBtn8L = vexRT[Btn8L];
    if ((prevBtn8L == 0) && (currBtn8L == 1))
    {
        // Detected button 8L pressed.
        switch (g_armPreset)
        {
            case ARMPRESET_NONE:
                g_armPreset = ARMPRESET_UP;
                break;

            case ARMPRESET_UP:
                g_armPreset = ARMPRESET_DOWN;
                break;

            case ARMPRESET_DOWN:
                g_armPreset = ARMPRESET_UP;
                break;
        }
    }
    prevBtn8L = currBtn8L;
}   //CheckArmPreset

void ProcessArmSubsystem()
{
    CheckArmPreset();
    switch (g_armPreset)
    {
        case ARMPRESET_NONE:
            // No preset is taking place.
            if(vexRT[Btn5U] == 1){
                // Button 5U is pressed. (Main Arm)
                if(SensorValue(dgtl2) == 1){
                    // Top limit switch is not triggered.
                    // Run main arm at full power.
                    motor[port4] = g_fPower;
                }else{
                    // Top limit switch is triggered.
                    // Stop the main arm from running. (Stops arm at 90 degree angle)
                    motor[port4] = 0;
                }
            }else if(vexRT[Btn5D] == 1){
                // Button 5D is pressed. (Reverse Main Arm)
                if(SensorValue(dgtl1) == 1){
                    // Bottom limit switch is not triggered.
                    // Run main arm a full inverted power.
                    motor[port4] = -g_fPower;
                }else{
                    // Bottom limit switch is triggered.
                    // Stop the main arm from running inverted. (Stops arm at bottom)
                    motor[port4] = 0;
                }
            }else
                // No Main Arm buttons were pressed.
                // Stop the main arm motor from running.
                motor[port4] = 0;
            break;

        case ARMPRESET_UP:
            // First preset is taking place.
            if(SensorValue(dgtl2) == 1){
                // Top limit switch not is triggered.
           // Keep preset motor running.
                motor[port4] = g_fPower;
            }else{
                // Top limit switch is triggered.
                // Stop preset motor from running.
                // Set preset move to off.
                motor[port4] = 0;
                g_armPreset = ARMPRESET_NONE;
            }
            break;

        case ARMPRESET_DOWN:
            // Second preset is taking place.
            if(SensorValue(dgtl1) == 1){
                // Bottom limit switch not is triggered.
                // Keep inverted preset motor running.
                motor[port4] = -g_fPower;
            }else{
                // Bottom limit switch is triggered.
                // Stop preset motor from running.
                // Set preset move to off.
                motor[port4] = 0;
                g_armPreset = ARMPRESET_NONE;
            }
            break;
    }
}    //ProcessArmSubsystem

task usercontrol()
{
    while(true){
        ProcessDriveSubsystem();
        ProcessIntakeSubsystem();
        ProcessArmSubsystem();
        wait1Msec(20);
    }
}   // usercontrol