#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  BLenc,          sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  FRenc,          sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_3,  BRenc,          sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_4,  FLenc,          sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port2,           BL,            tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port3,           FR,            tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2)
#pragma config(Motor,  port4,           BR,            tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_3)
#pragma config(Motor,  port5,           FL,            tmotorVex393_MC29, openLoop, encoderPort, I2C_4)
#pragma config(Motor,  port6,           LLB,           tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           CLMP,          tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           RFL,           tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port9,           RBL,           tmotorVex393_MC29, openLoop, reversed)
#pragma platform(VEX)


//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

const short leftButton = 1;
const short centerButton = 2;
const short rightButton = 4;


//Wait for Press--------------------------------------------------
void waitForPress()
{
	while(nLCDButtons == 0){}
	wait1Msec(5);
}
//----------------------------------------------------------------

//Wait for Release------------------------------------------------
void waitForRelease()
{
	while(nLCDButtons != 0){}
	wait1Msec(5);
}
//----------------------------------------------------------------

void pre_auton()
{
  bStopTasksBetweenModes = true;
//Declare count variable to keep track of our choice
	int count = 0;

	//------------- Beginning of User Interface Code ---------------
	//Clear LCD
	clearLCDLine(0);
	clearLCDLine(1);
	//Loop while center button is not pressed
	while(nLCDButtons != centerButton)
	{
		//Switch case that allows the user to choose from 4 different options
		switch(count){
		case 0:
			//Display first choice
			displayLCDCenteredString(0, "AutoSkyRed");
			displayLCDCenteredString(1, "<		 Enter		>");
			waitForPress();
			//Increment or decrement "count" based on button press
			if(nLCDButtons == leftButton)
			{
				waitForRelease();
				count = 3;
			}
			else if(nLCDButtons == rightButton)
			{
				waitForRelease();
				count++;
			}
			break;
		case 1:
			//Display second choice
			displayLCDCenteredString(0, "AutoSkyBlue");
			displayLCDCenteredString(1, "<		 Enter		>");
			waitForPress();
			//Increment or decrement "count" based on button press
			if(nLCDButtons == leftButton)
			{
				waitForRelease();
				count--;
			}
			else if(nLCDButtons == rightButton)
			{
				waitForRelease();
				count++;
			}
			break;
		case 2:
			//Display third choice
			displayLCDCenteredString(0, "AutoCubeRed");
			displayLCDCenteredString(1, "<		 Enter		>");
			waitForPress();
			//Increment or decrement "count" based on button press
			if(nLCDButtons == leftButton)
			{
				waitForRelease();
				count--;
			}
			else if(nLCDButtons == rightButton)
			{
				waitForRelease();
				count++;
			}
			break;
		case 3:
			//Display fourth choice
			displayLCDCenteredString(0, "AutoCubeBlue");
			displayLCDCenteredString(1, "<		 Enter		>");
			waitForPress();
			//Increment or decrement "count" based on button press
			if(nLCDButtons == leftButton)
			{
				waitForRelease();
				count--;
			}
			else if(nLCDButtons == rightButton)
			{
				waitForRelease();
				count = 0;
			}
			break;
		default:
			count = 0;
			break;
		}
	}
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

int count = 0;

task autonomous()// task autonomous
{
	//------------- Beginning of Robot Movement Code ---------------
	//Clear LCD
	clearLCDLine(0);
	clearLCDLine(1);
	//Switch Case that actually runs the user choice
	switch(count){
	case 0:
		//If count = 0, run the code correspoinding with choice 1
		displayLCDCenteredString(0, "AutoSkyRed");
		displayLCDCenteredString(1, "is running!");
		wait1Msec(1000);						// Robot waits for 2000 milliseconds

		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		wait1Msec(100);
		while (nMotorEncoder[BL] >-83.6266 && nMotorEncoder[FR] >-83.6266 && nMotorEncoder[BR] >-83.6266 && nMotorEncoder[FL] >-83.6266)
		{
			motor(BL) = -127;//motor will move
			motor(FR) = -127;//motor will move
			motor(BR) = -127;//motor will move
			motor(FL) = -127;//motor will move
			//move backwards
		}

		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		wait1Msec(100);
		while (nMotorEncoder[BL] <83.6266 && nMotorEncoder[FR] <83.6266 && nMotorEncoder[BR] <83.6266 && nMotorEncoder[FL] <83.6266)
		{
			motor(BL) = 127;//motor will move
			motor(FR) = 127;//motor will move
			motor(BR) = 127;//motor will move
			motor(FL) = 127;//motor will move
			//move forwards
		}
		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		wait1Msec(100);
		while (nMotorEncoder[BL] >-564.48 && nMotorEncoder[FR] <564.48 && nMotorEncoder[BR] <564.48 && nMotorEncoder[FL] >-564.48)
		{
			motor(BL) = -127;//motor will move
			motor(FR) = 127;//motor will move
			motor(BR) = 127;//motor will move
			motor(FL) = -127;//motor will move
			//turn left
		}

		wait1Msec(100);
		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		wait1Msec(100);
		while (nMotorEncoder[BL] <146.3458 && nMotorEncoder[FR] <146.3458 && nMotorEncoder[BR] <146.3458 && nMotorEncoder[FL] <146.3458)
		{
			motor(BL) = 127;//motor will move
			motor(FR) = 127;//motor will move
			motor(BR) = 0;//motor will move
			motor(FL) = 0;//motor will move
			//move forwards
		}

		wait1Msec(100);
		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		wait1Msec(100);
		motor(CLMP) = 127;//clamp will close
		wait1Msec(1200);//
		motor(CLMP) = 20;//motor will not move
		wait1Msec(50);
		//close the clamp and keep constant pressure on skyirse

		motor(port6) = 127;//motor will move
		motor(port8) = 127;//motor will move
		motor(port9) = 127;//motor will move
		motor(CLMP) = 20;//motor will not move
		wait1Msec(1000);
		motor(port6) = 0;//motor will move
		motor(port8) = 0;//motor will move
		motor(port9) = 0;//motor will move
		motor(CLMP) = 20;//motor will not move
		wait1Msec(1000);
		//lift up

		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		wait1Msec(100);
		while (nMotorEncoder[BL] >-195.1278 && nMotorEncoder[FR] >-195.1278 && nMotorEncoder[BR] >-195.1278 && nMotorEncoder[FL] >-195.1278)
		{
		motor(BL) = -127;//motor will move
		motor(FR) = -127;//motor will move
		motor(BR) = -127;//motor will move
		motor(FL) = -127;//motor will move
		motor(CLMP) = 20;//motor will not move
		//move backwards
		}
		wait1Msec(100);
		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		wait1Msec(100);
		while (nMotorEncoder[BL] >-111.5022 && nMotorEncoder[FR] >-111.5022 && nMotorEncoder[BR] <111.5022 && nMotorEncoder[FL] <111.5022)
		{
		motor(BL) = -127;//motor will move
		motor(FR) = -127;//motor will move
		motor(BR) = 127;//motor will move
		motor(FL) = 127;//motor will move
		motor(CLMP) = 20;//motor will not move
		//move right
		}
		wait1Msec(100);
		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		wait1Msec(100);
		while (nMotorEncoder[BL] >-627.2 && nMotorEncoder[FR] >627.2 && nMotorEncoder[BR] >627.2 && nMotorEncoder[FL] >-1627.2)
		{
		motor(BL) = -127;//motor will move
		motor(FR) = 127;//motor will move
		motor(BR) = 127;//motor will move
		motor(FL) = -127;//motor will move
		motor(CLMP) = 20;//motor will not move
		//turn left
		}
		wait1Msec(100);
		nMotorEncoder[port6] = 0;//clear
		nMotorEncoder[port8] = 0;//clear
		nMotorEncoder[port9] = 0;//clear
		//encoder set zero
		wait1Msec(100);
		motor(port6) = -127;//motor will move
		motor(port8) = -127;//motor will move
		motor(port9) = -127;//motor will move
		motor(CLMP) = 20;//motor will not move
		wait1Msec(1000);
		motor(port6) = 0;//motor will move
		motor(port8) = 0;//motor will move
		motor(port9) = 0;//motor will move
		motor(CLMP) = 20;//motor will not move
		wait1Msec(1000);
		//lift down
		wait1Msec(100);
		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		wait1Msec(100);
		while (nMotorEncoder[BL] <27.8755 && nMotorEncoder[FR] <27.8755 && nMotorEncoder[BR] <27.8755 && nMotorEncoder[FL] <27.8755)
		{
		motor(BL) = 127;//motor will move
		motor(FR) = 127;//motor will move
		motor(BR) = 127;//motor will move
		motor(FL) = 127;//motor will move
		motor(CLMP) = 20;//motor will not move
		//move forwards
		}
		wait1Msec(100);
		motor(CLMP) = -127;
		wait1Msec(1200);
		motor(CLMP) = 0;
		wait1Msec(50);
		//relase the clamp
		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		wait1Msec(100);
		while (nMotorEncoder[BL] >-125.44 && nMotorEncoder[FR] >-125.44 && nMotorEncoder[BR] >-125.44 && nMotorEncoder[FL] >-125.44)
		{
		motor(BL) = -127;//motor will move
		motor(FR) = -127;//motor will move
		motor(BR) = -127;//motor will move
		motor(FL) = -127;//motor will move
		//move backwards
		}


/*
		break;
		case 1:
		//If count = 1, run the code correspoinding with choice 2
		displayLCDCenteredString(0, "AutoSkyBlue");
		displayLCDCenteredString(1, "is running!");
		wait1Msec(2000);						// Robot waits for 2000 milliseconds

		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		while (nMotorEncoder[BL] <1000 && nMotorEncoder[FR] <1000 && nMotorEncoder[BR] <1000 && nMotorEncoder[FL] <1000)
		{
		motor(BL) = -127;//motor will move
		motor(FR) = -127;//motor will move
		motor(BR) = -127;//motor will move
		motor(FL) = -127;//motor will move
		//move backwards
		}
		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		while (nMotorEncoder[BL] <1000 && nMotorEncoder[FR] <1000 && nMotorEncoder[BR] <1000 && nMotorEncoder[FL] <1000)
		{
		motor(BL) = 127;//motor will move
		motor(FR) = 127;//motor will move
		motor(BR) = 127;//motor will move
		motor(FL) = 127;//motor will move
		//move forwards
		}
		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		while (nMotorEncoder[BL] <1000 && nMotorEncoder[FR] <1000 && nMotorEncoder[BR] <1000 && nMotorEncoder[FL] <1000)
		{
		motor(BL) = 127;//motor will move
		motor(FR) = -127;//motor will move
		motor(BR) = -127;//motor will move
		motor(FL) = 127;//motor will move
		//turn right
		}
		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		while (nMotorEncoder[BL] <1000 && nMotorEncoder[FR] <1000 && nMotorEncoder[BR] <1000 && nMotorEncoder[FL] <1000)
		{
		motor(BL) = 127;//motor will move
		motor(FR) = 127;//motor will move
		motor(BR) = 127;//motor will move
		motor(FL) = 127;//motor will move
		wait1Msec(300);
		//move forwards
		}
		motor(CLMP) = 127;//clamp will close
		wait1Msec(1200);//
		motor(CLMP) = 20;//motor will not move
		wait1Msec(50);
		//close the clamp and keep constant pressure on skyirse
		nMotorEncoder[port6] = 0;//clear
		nMotorEncoder[port8] = 0;//clear
		nMotorEncoder[port9] = 0;//clear
		//encoder set zero

		motor(port6) = 127;//motor will move
		motor(port8) = 127;//motor will move
		motor(port9) = 127;//motor will move
		motor(CLMP) = 20;//motor will not move
		wait1Msec(1000);
		motor(port6) = 0;//motor will move
		motor(port8) = 0;//motor will move
		motor(port9) = 0;//motor will move
		motor(CLMP) = 20;//motor will not move
		wait1Msec(1000);
		//lift up

		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		while (nMotorEncoder[BL] <1000 && nMotorEncoder[FR] <1000 && nMotorEncoder[BR] <1000 && nMotorEncoder[FL] <1000)
		{
		motor(BL) = -127;//motor will move
		motor(FR) = -127;//motor will move
		motor(BR) = -127;//motor will move
		motor(FL) = -127;//motor will move
		motor(CLMP) = 20;//motor will not move
		//move backwards
		}
		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		while (nMotorEncoder[BL] <1000 && nMotorEncoder[FR] <1000 && nMotorEncoder[BR] <1000 && nMotorEncoder[FL] <1000)
		{
		motor(BL) = 127;//motor will move
		motor(FR) = -127;//motor will move
		motor(BR) = -127;//motor will move
		motor(FL) = 127;//motor will move
		motor(CLMP) = 20;//motor will not move
		//turn right
		}
		nMotorEncoder[port6] = 0;//clear
		nMotorEncoder[port8] = 0;//clear
		nMotorEncoder[port9] = 0;//clear
		//encoder set zero

		motor(port6) = -127;//motor will move
		motor(port8) = -127;//motor will move
		motor(port9) = -127;//motor will move
		motor(CLMP) = 20;//motor will not move
		wait1Msec(1000);
		motor(port6) = 0;//motor will move
		motor(port8) = 0;//motor will move
		motor(port9) = 0;//motor will move
		motor(CLMP) = 20;//motor will not move
		wait1Msec(1000);
		//lift down

		motor(CLMP) = -127;
		wait1Msec(1200);
		motor(CLMP) = 0;
		wait1Msec(50);
		//relase the clamp
		nMotorEncoder[BL] = 0;//clear
		nMotorEncoder[FR] = 0;//clear
		nMotorEncoder[BR] = 0;//clear
		nMotorEncoder[FL] = 0;//clear
		//encoder set zero
		while (nMotorEncoder[BL] <1000 && nMotorEncoder[FR] <1000 && nMotorEncoder[BR] <1000 && nMotorEncoder[FL] <1000)
		{
		motor(BL) = -127;//motor will move
		motor(FR) = -127;//motor will move
		motor(BR) = -127;//motor will move
		motor(FL) = -127;//motor will move
		//move backwards
		}
		wait1Msec(1000000000000000000);
		break;
		case 2:
		//If count = 2, run the code correspoinding with choice 3
		displayLCDCenteredString(0, "AutoCubeRed");
		displayLCDCenteredString(1, "is running!");
		wait1Msec(2000);						// Robot waits for 2000 milliseconds

		motor(BL) = 127;//motor will move
		motor(FR) = 127;//motor will move
		wait1Msec(1700);
		motor(BL) = 0;//motor will not move
		motor(FR) = 0;//motor will not move
		wait1Msec(50);
		//move north west
		motor(BL) = 127;//motor will move
		motor(FR) = 127;//motor will move
		motor(BR) = -127;//motor will move
		motor(FL) = -127;//motor will move
		wait1Msec(1000);
		motor(BL) = 0;//motor will not move
		motor(FR) = 0;//motor will not move
		motor(BR) = 0;//motor will not move
		motor(FL) = 0;//motor will not move
		wait1Msec(50);
		//move left
		motor(BL) = -127;//motor will move
		motor(FR) = -127;//motor will move
		motor(BR) = -127;//motor will move
		motor(FL) = -127;//motor will move
		wait1Msec(1000);
		motor(BL) = 0;//motor will not move
		motor(FR) = 0;//motor will not move
		motor(BR) = 0;//motor will not move
		motor(FL) = 0;//motor will not move
		wait1Msec(50);
		//move backwards
		motor(BL) = -127;//motor will move
		motor(FR) = -127;//motor will move
		motor(BR) = 127;//motor will move
		motor(FL) = 127;//motor will move
		wait1Msec(1000);
		motor(BL) = 0;//motor will not move
		motor(FR) = 0;//motor will not move
		motor(BR) = 0;//motor will not move
		motor(FL) = 0;//motor will not move
		wait1Msec(50);
		//move right
		motor(BL) = 127;//motor will move
		motor(FR) = 127;//motor will move
		motor(BR) = -127;//motor will move
		motor(FL) = -127;//motor will move
		wait1Msec(500);
		motor(BL) = 0;//motor will not move
		motor(FR) = 0;//motor will not move
		motor(BR) = 0;//motor will not move
		motor(FL) = 0;//motor will not move
		wait1Msec(50000000000000000000000000);
		//move left
		break;
		case 3:
		//If count = 3, run the code correspoinding with choice 4
		displayLCDCenteredString(0, "AutoCubeBlue");
		displayLCDCenteredString(1, "is running!");
		wait1Msec(2000);// Robot waits for 2000 milliseconds

		motor(BR) = 127;//motor will move
		motor(FL) = 127;//motor will move
		wait1Msec(1700);
		motor(BR) = 0;//motor will not move
		motor(FL) = 0;//motor will not move
		wait1Msec(50);
		//move north east
		motor(BL) = -127;//motor will move
		motor(FR) = -127;//motor will move
		motor(BR) = 127;//motor will move
		motor(FL) = 127;//motor will move
		wait1Msec(1000);
		motor(BL) = 0;//motor will not move
		motor(FR) = 0;//motor will not move
		motor(BR) = 0;//motor will not move
		motor(FL) = 0;//motor will not move
		wait1Msec(50);
		//move right
		motor(BL) = -127;//motor will move
		motor(FR) = -127;//motor will move
		motor(BR) = -127;//motor will move
		motor(FL) = -127;//motor will move
		wait1Msec(1000);
		motor(BL) = 0;//motor will not move
		motor(FR) = 0;//motor will not move
		motor(BR) = 0;//motor will not move
		motor(FL) = 0;//motor will not move
		wait1Msec(50);
		//move backwards
		motor(BL) = 127;//motor will move
		motor(FR) = 127;//motor will move
		motor(BR) = -127;//motor will move
		motor(FL) = -127;//motor will move
		wait1Msec(1000);
		motor(BL) = 0;//motor will not move
		motor(FR) = 0;//motor will not move
		motor(BR) = 0;//motor will not move
		motor(FL) = 0;//motor will not move
		wait1Msec(50);
		//move left
		motor(BL) = -127;//motor will move
		motor(FR) = -127;//motor will move
		motor(BR) = 127;//motor will move
		motor(FL) = 127;//motor will move
		wait1Msec(500);
		motor(BL) = 0;//motor will not move
		motor(FR) = 0;//motor will not move
		motor(BR) = 0;//motor will not move
		motor(FL) = 0;//motor will not move
		wait1Msec(5000000000000000000000000);
		//move right
		break;
		default:
		displayLCDCenteredString(0, "No valid choice");
		displayLCDCenteredString(1, "was made!");
		break;
		*/
	}
	//------------- End of Robot Movement Code -----------------------
}