#pragma config(UART_Usage, UART2, uartNotUsed, baudRate4800, IOPins, None, None)
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    LiftPot,        sensorPotentiometer)
#pragma config(Sensor, in2,    AutonSelectColor, sensorPotentiometer)
#pragma config(Sensor, in3,    AutonSelectObject, sensorPotentiometer)
#pragma config(Sensor, in4,    AutonSelect,    sensorPotentiometer)
#pragma config(Sensor, dgtl1,  btn1,           sensorDigitalIn)
#pragma config(Sensor, dgtl2,  btn2,           sensorDigitalIn)
#pragma config(Sensor, dgtl3,  ClawOpen,       sensorDigitalOut)
#pragma config(Sensor, dgtl5,  LiftReset,      sensorDigitalIn)
#pragma config(Sensor, I2C_1,  RightDrive,     sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  LeftDrive,      sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_3,  LeftLift,       sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_4,  RightLift,      sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           bLeftDrive,    tmotorVex393_HBridge, openLoop, driveLeft)
#pragma config(Motor,  port2,           LiftOne,       tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_3)
#pragma config(Motor,  port3,           LiftTwo,       tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_4)
#pragma config(Motor,  port4,           fLeftDrive,    tmotorVex393_MC29, openLoop, driveLeft, encoderPort, I2C_2)
#pragma config(Motor,  port5,           fRightDrive,   tmotorVex393_MC29, openLoop, reversed, driveRight, encoderPort, I2C_1)
#pragma config(Motor,  port6,           Claw,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           LiftThree,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           LiftFour,      tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port9,           Intake,        tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port10,          bRightDrive,   tmotorVex393_HBridge, openLoop, driveRight)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)
//Competition Control
#pragma competitionControl(Competition)

void LiftStraightUp(int Pot)  // Lifting up with an autocorrecting feature
{
	SensorValue[LeftLift] = 0;
	SensorValue[RightLift] = 0;

	while (SensorValue[LiftPot] < Pot)
	{

		if(abs(SensorValue[LeftLift]) < SensorValue[RightLift])
		{
			motor[LiftOne] = 127;
			motor[LiftThree] = 127;
			motor[LiftTwo] = 110;
			motor[LiftFour] = 110;
		}
		else if(abs(SensorValue[LeftLift]) > SensorValue[RightLift])
		{
			motor[LiftOne] = 110;
			motor[LiftThree] = 110;
			motor[LiftTwo] = 127;
			motor[LiftFour] = 127;
		}
		else(abs(SensorValue[LeftLift]) == SensorValue[RightLift]);
		{
			motor[LiftOne] = 127;
			motor[LiftThree] = 127;
			motor[LiftTwo] = 127;
			motor[LiftFour] = 127;
		}
	}
}

void LiftStraightDown(int Pot)  // Lifting down with an autocorrecting feature
{
	SensorValue[LeftLift] = 0;
	SensorValue[RightLift] = 0;

	while (SensorValue[LiftPot] > Pot)
	{

		if(abs(SensorValue[LeftLift]) < SensorValue[RightLift])
		{
			motor[LiftOne] = -127;
			motor[LiftThree] = -127;
			motor[LiftTwo] = -110;
			motor[LiftFour] = -110;
		}
		else if(abs(SensorValue[LeftLift]) > SensorValue[RightLift])
		{
			motor[LiftOne] = -110;
			motor[LiftThree] = -110;
			motor[LiftTwo] = -127;
			motor[LiftFour] = -127;
		}
		else(abs(SensorValue[LeftLift]) == SensorValue[RightLift]);
		{
			motor[LiftOne] = -127;
			motor[LiftThree] = -127;
			motor[LiftTwo] = -127;
			motor[LiftFour] = -127;
		}
	}
}

void StopLift()
{
	if(SensorValue(LiftPot) >= 800)  // Hold Lift Up
	{
		motor[LiftOne] = 10;
		motor[LiftTwo] = 10;
		motor[LiftThree] = 10;
		motor[LiftFour] = 10;
	}
	else  // Hold Lift Down
	{
		motor[LiftOne] = -10;
		motor[LiftTwo] = -10;
		motor[LiftThree] = -10;
		motor[LiftFour] = -10;
	}
}

void StopDrive()
{
	motor[fLeftDrive] = 0;
	motor[bLeftDrive] = 0;
	motor[fRightDrive] = 0;
	motor[bRightDrive] = 0;
}

void DriveStraightBack(int Drive)  // Driving backwards with an autocorrecting feature
{
	SensorValue[RightDrive] = 0;
	SensorValue[LeftDrive] = 0;

	while(abs(SensorValue[LeftDrive]) < Drive)
	{
		if(abs(SensorValue[LeftDrive]) < SensorValue[RightDrive])
		{
			motor[fLeftDrive] = -127;
			motor[bLeftDrive] = -127;
			motor[fRightDrive] = -110;
			motor[bRightDrive] = -110;
		}
		else if(abs(SensorValue[LeftDrive]) > SensorValue[RightDrive])
		{
			motor[fLeftDrive] = -110;
			motor[bLeftDrive] = -110;
			motor[fRightDrive] = -127;
			motor[bRightDrive] = -127;
		}
		else;
		{
			motor[fLeftDrive] = -127;
			motor[bLeftDrive] = -127;
			motor[fRightDrive] = -127;
			motor[bRightDrive] = -127;
		}
	}
	StopDrive();
}

void DriveStraightForward (int Drive)  // Driving forwards with an autocorrecting feature
{
	SensorValue[RightDrive] = 0;
	SensorValue[LeftDrive] = 0;

	while(abs(SensorValue[LeftDrive]) < Drive)
	{
		if(abs(SensorValue[LeftDrive]) < SensorValue[RightDrive])
		{
			motor[fLeftDrive] = 127;
			motor[bLeftDrive] = 127;
			motor[fRightDrive] = 110;
			motor[bRightDrive] = 110;
		}
		else if(abs(SensorValue[LeftDrive]) > SensorValue[RightDrive])
		{
			motor[fLeftDrive] = 110;
			motor[bLeftDrive] = 110;
			motor[fRightDrive] = 127;
			motor[bRightDrive] = 127;
		}
		else;
		{
			motor[fLeftDrive] = 127;
			motor[bLeftDrive] = 127;
			motor[fRightDrive] = 127;
			motor[bRightDrive] = 127;
		}
	}
	StopDrive();
}

void TurnLeft(int Turn)  //Turning Left
{
	while (abs(SensorValue[LeftDrive]) < Turn)
	{
		motor[fLeftDrive] = -127;
		motor[bLeftDrive] = -127;
		motor[fRightDrive] = 127;
		motor[bRightDrive] = 127;
	}
	StopDrive();
}

void TurnRight(int Turn)  //Turning Right
{
	while (abs(SensorValue[LeftDrive]) < Turn)
	{
		motor[fLeftDrive] = 127;
		motor[bLeftDrive] = 127;
		motor[fRightDrive] = -127;
		motor[bRightDrive] = -127;
	}
	StopDrive();
}

void StrafeLeft(int Strafe)  // Strafe Left
{
	while(abs(SensorValue[LeftDrive]) < Strafe)
	{
		motor[fLeftDrive]  = -127;
		motor[bLeftDrive]  =  127;
		motor[fRightDrive] =  127;
		motor[bRightDrive] = -127;
	}
	StopDrive();
}

void StrafeRight(int Strafe)  // Strafe Right
{
	while(abs(SensorValue[LeftDrive]) < Strafe)
	{
		motor[fLeftDrive] = 127;
		motor[bLeftDrive] = -127;
		motor[fRightDrive] = -127;
		motor[bRightDrive] = 127;
	}
	StopDrive();
}

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

void pre_auton()  /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	bStopTasksBetweenModes = true;

}

task autonomous()  ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
{

	SensorValue[LeftDrive] = 0;
	SensorValue[RightDrive] = 0;
	SensorValue[LeftLift] = 0;
	SensorValue[RightLift] = 0;

	if(SensorValue[AutonSelectObject] > 2500)  // Skyrise Building
	{
		if(SensorValue[AutonSelectColor] < 2500)  // Blue Skyrise Building
		{
			motor[fLeftDrive] = -127;
			motor[bLeftDrive] = 127;
			motor[fRightDrive] = 127;
			motor[bRightDrive] = -127;
			wait1Msec(750);
		}
		else  // Red Skyrise Building
		{
			motor[fLeftDrive] = 127;
			motor[bLeftDrive] = -127;
			motor[fRightDrive] = -127;
			motor[bRightDrive] = 127;
			wait1Msec(750);
		}
		motor[Intake] = 0;

		StopDrive();
		motor[Claw] = -127;  // Move Skyrise builder back
		wait1Msec(2000);
		motor[Claw] = 0;  // Stop Skyrise builder
		SensorValue[ClawOpen] = 1;  // Pick up first Skyrise
		motor[Claw] = 127;  // Move Skyrise builder forward
		wait1Msec(2000);
		motor[Claw] = 0;  // Stop Skyrise builder
		wait1Msec(300);
		SensorValue[ClawOpen] = 0;  // Place first Skyrise

		motor[Claw] = -127;  // Move Skyrise builder back
		wait1Msec(2000);
		motor[Claw] = 0;  // Stop Skyrise builder
		SensorValue[ClawOpen] = 1;  // Pick up second Skyrise
		motor[Claw] = 127;  // Move Skyrise builder forward
		wait1Msec(1200);
		motor[Claw] = 0;  // Stop Skyrise builder
		LiftStraightUp(800);  // Lift up
		StopLift();  // Stop lift
		motor[Claw] = 127;  // Move Skyrise builder forward
		wait1Msec(1200);
		motor[Claw] = 0;  // Stop Skyrise builder
		SensorValue[ClawOpen] = 0;  // Place second Skyrise
		motor[Claw] = -127;  // Move Skyrise builder back
		wait1Msec(2000);
		motor[Claw] = 0;  // Stop Skyrise builder
		LiftStraightDown(750);  // Lift down
		StopLift();  // Stop lift

		motor[fLeftDrive] = -80;  // Drive back from Skyrise
		motor[bLeftDrive] = -80;
		motor[fRightDrive] = -80;
		motor[bRightDrive] = -80;
		wait1Msec(200);
		StopDrive();
	}

	else  // Skyrise tile cube scoring
	{
		motor[Claw] = -127;  // Move Skyrise builder back
		wait1Msec(1500);
		motor[Claw] = 0;

		motor[fLeftDrive] = 127;  // Drive forward
		motor[bLeftDrive] = 127;
		motor[fRightDrive] = 127;
		motor[bRightDrive] = 127;
		motor[Intake] = 127;
		wait1Msec(1500);

		if(SensorValue[AutonSelectColor] > 2500)  // Blue Skyrise tile cube scorer
		{
			motor[fLeftDrive] = 127;  // Strafe left
			motor[bLeftDrive] = -127;
			motor[fRightDrive] = -127;
			motor[bRightDrive] = 127;
			wait1Msec(1000);

			motor[fLeftDrive] = -127;  // Turn right
			motor[bLeftDrive] = -127;
			motor[fRightDrive] = 127;
			motor[bRightDrive] = 127;
			wait1Msec(1150);
		}
		if(SensorValue[AutonSelectColor] < 2500)  // Red Skyrise tile cube scorer
		{
			motor[fLeftDrive] = -127;  // Strafe right
			motor[bLeftDrive] = 127;
			motor[fRightDrive] = 127;
			motor[bRightDrive] = -127;
			wait1Msec(1000);

			motor[fLeftDrive] = 127;  // Turn left
			motor[bLeftDrive] = 127;
			motor[fRightDrive] = -127;
			motor[bRightDrive] = -127;
			wait1Msec(1150);
		}

		StopDrive();
		LiftStraightUp(1450);
		StopLift();

		motor[fLeftDrive] = 127;  // Drive forward
		motor[bLeftDrive] = 127;
		motor[fRightDrive] = 127;
		motor[bRightDrive] = 127;
		wait1Msec(600);

		StopDrive();

		motor[Intake] = -127;  // Score cube
		wait1Msec(3000);
		motor[Intake] = 0;
	}
}

task usercontrol()  ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
{
	SensorValue[LeftLift] = 0;
	SensorValue[RightLift] = 0;

	while (true)
	{  // Drive with Mecanum Wheels
		motor[bLeftDrive]  = (vexRT[Ch3] - vexRT[Ch4]);  // (y - x)
		motor[fLeftDrive] = (vexRT[Ch3] + vexRT[Ch4]);  // (y + x)
		motor[bRightDrive]  = (vexRT[Ch2] + vexRT[Ch1]);  // (y + x)
		motor[fRightDrive] = (vexRT[Ch2] - vexRT[Ch1]);  // (y - x)


		if(vexRT[Btn5U] == 1)  // Lift Up with an autocorrecting feature
		{
			if(abs(SensorValue[LeftLift]) < SensorValue[RightLift])  // Slow right
			{
				motor[LiftOne] = 127;
				motor[LiftThree] = 127;
				motor[LiftTwo] = 110;
				motor[LiftFour] = 110;
			}
			else if(abs(SensorValue[LeftLift]) > SensorValue[RightLift])  // Slow left
			{
				motor[LiftOne] = 110;
				motor[LiftThree] = 110;
				motor[LiftTwo] = 127;
				motor[LiftFour] = 127;
			}
			else  // Normal up
			{
				motor[LiftOne] = 127;
				motor[LiftThree] = 127;
				motor[LiftTwo] = 127;
				motor[LiftFour] = 127;
			}
		}
		else if(vexRT[Btn5D] == 1)  // Lift Down with an autocorrecting feature
		{
			if(abs(SensorValue[LeftLift]) < SensorValue[RightLift])  // Slow right
			{
				motor[LiftOne] = -127;
				motor[LiftThree] = -127;
				motor[LiftTwo] = -110;
				motor[LiftFour] = -110;
			}
			else if(abs(SensorValue[LeftLift]) > SensorValue[RightLift])  // Slow left
			{
				motor[LiftOne] = -110;
				motor[LiftThree] = -110;
				motor[LiftTwo] = -127;
				motor[LiftFour] = -127;
			}
			else  // Normal down
			{
				motor[LiftOne] = -127;
				motor[LiftThree] = -127;
				motor[LiftTwo] = -127;
				motor[LiftFour] = -127;
			}
		}
		else  // Correct and maintain lift position
		{
			if(SensorValue[LeftLift] > SensorValue[RightLift])  // Lift right side up
			{
				motor[LiftOne]   = 0;
				motor[LiftTwo]   = 127;
				motor[LiftThree] = 0;
				motor[LiftFour]  = 127;
			}
			else if(SensorValue[LeftLift] < SensorValue[RightLift])  // Drop right side down
			{
				motor[LiftOne]   = 0;
				motor[LiftTwo]   = -127;
				motor[LiftThree] = 0;
				motor[LiftFour]  = -127;
			}
			else  // Hold lift position
			{
				if(SensorValue[LiftPot] > 800)  // Hold up
				{
					motor[LiftOne]   = 10;
					motor[LiftTwo]   = 10;
					motor[LiftThree] = 10;
					motor[LiftFour]  = 10;
				}
				else  // Hold down
				{
					motor[LiftOne]   = -10;
					motor[LiftTwo]   = -10;
					motor[LiftThree] = -10;
					motor[LiftFour]  = -10;
				}
			}
		}

		if(SensorValue[LiftReset] == 1)  // Reset lift encoder values
		{
			SensorValue[LeftLift] = 0;
			SensorValue[RightLift] = 0;
		}

		if(vexRT(Btn6U) == 1)  // Intake In
		{
			motor(Intake) = 127;
		}
		else if(vexRT(Btn6D) == 1)  // Intake Out
		{
			motor(Intake) = -127;
		}
		else   // Hold Intake Position
		{
			motor(Intake) = 5;
		}

		if(vexRT(Btn8U) == 1)  // Move claw forward
		{
			motor(Claw) = 127;
		}
		else if(vexRT(Btn8D) == 1)  // Move claw backward
		{
			motor(Claw) = -127;
		}
		else
		{
			motor(Claw) = 0;
		}

		if(vexRT(Btn8L) == 1)  // Grab Skyrise
		{
			SensorValue[ClawOpen] = 1;
		}
		else if(vexRT(Btn8R) == 1)  // Let go of Skyrise
		{
			SensorValue[ClawOpen] = 0;
		}
	}
}