#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    LIft,           sensorPotentiometer)
#pragma config(Sensor, dgtl8,  RO,             sensorTouch)
#pragma config(Sensor, dgtl9,  RI,             sensorTouch)
#pragma config(Sensor, dgtl10, BO,             sensorTouch)
#pragma config(Sensor, dgtl11, BI,             sensorTouch)
#pragma config(Sensor, dgtl12, NunYa,          sensorTouch)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           LiftLeft,      tmotorVex393, openLoop)
#pragma config(Motor,  port2,           LBmtr,         tmotorVex393HighSpeed, PIDControl, encoder, encoderPort, I2C_1, 1000)
#pragma config(Motor,  port3,           LFmtr,         tmotorVex393HighSpeed, openLoop)
#pragma config(Motor,  port4,           IntakeLeft,    tmotorVex393, openLoop)
#pragma config(Motor,  port5,           IntakeRight,   tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port6,           LiftLeft2,     tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port7,           LiftRight2,    tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port8,           RBmtr,         tmotorVex393HighSpeed, PIDControl, encoder, encoderPort, I2C_2, 1000)
#pragma config(Motor,  port9,           RFmtr,         tmotorVex393HighSpeed, openLoop)
#pragma config(Motor,  port10,          LiftRight,     tmotorVex393, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#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!
#include "ArmTask.c"

void RedOutOfBump();
void BlueInBump();
void RedInBump();
void Skills();
void INTAKEIN(int HowLong);
void INTAKEOFF();
void INTAKEOUT(int HowLong);
void ForwardWithTime(int HowLong);
void BackwardsWithTime(int HowLong);
void ForwardWithEncoders(int rotations);
void BackwardsWithEncoders(int rotations);
void TurnRightWithEncoders(int rotations);
void TurnLeftWithEncoders(int rotations);
void DriveMotorStop();
void LIFTSCORE();
void LIFTBAR();
void LIFTGROUND();
void EncoderClear();

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

void pre_auton()
{
	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//																 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.
//
/////////////////////////////////////////////////////////////////////////////////////////


void LIFTSCORE()
{
	LiftTarget = LiftTargetScore;
	LiftTargetReached = false;
	LiftAuto = true;
	while(!LiftTargetReached)
		wait1Msec(100);
}

void LIFTBAR()
{
	LiftTarget = LiftTargetBarrier;
	LiftTargetReached = false;
	LiftAuto = true;
	while(!LiftTargetReached)
		wait1Msec(100);
}

void LIFTGROUND()
{
	LiftTarget = LiftTargetGround;
	LiftTargetReached = false;
	LiftAuto = true;
	while(!LiftTargetReached)
		wait1Msec(100);
}



void INTAKEOFF()///turn off in take
{
	motor[IntakeLeft] = 0;
	motor[IntakeRight] = 0;
}

void INTAKEIN(int HowLong)///pull item in to the intake
{
	motor[IntakeLeft] = -127;
	motor[IntakeRight] = -127;
	wait10Msec(HowLong);
}


void INTAKEOUT(int HowLong)///push item out of in take
{
	motor[IntakeLeft] = 127;
	motor[IntakeRight] = 127;
	wait10Msec(HowLong);
}


void DriveMotorStop() //stops motors
{
	motor[LFmtr] = 0;
	motor[LBmtr] = 0;
	motor[RFmtr] = 0;
	motor[RBmtr] = 0;
}

///////////////////
//Timed functions//
/////////////////////////////////
//These are the timed functions//
// incase encoders dont work	 //
/////////////////////////////////

void ForwardWithTime(int HowLong)
{
	motor[LFmtr] = 100;
	motor[LBmtr] = 100;
	motor[RFmtr] = 100;
	motor[RBmtr] = 100;
	wait10Msec(HowLong);
	DriveMotorStop();
}

void BackwardsWithTime(int HowLong)
{
	motor[LFmtr] = -127;
	motor[LBmtr] = -127;
	motor[RFmtr] = -127;
	motor[RBmtr] = -127;
	wait10Msec(HowLong);
	DriveMotorStop();
}

/////////////////////
//Sensor  Functions//
/////////////////////////////////////
//These are the functions that are //
//used when the sensors work			 //
/////////////////////////////////////
void EncoderClear()
{
	nMotorEncoder[LBmtr] = 0;
	nMotorEncoder[RBmtr] = 0;
}

void ForwardWithEncoders(int rotations) //drive forward with encoder control
{
	EncoderClear();
	while (nMotorEncoder[LBmtr] < rotations)
	{
		motor[LFmtr] = 100;
		motor[LBmtr] = 100;
		motor[RFmtr] = 100;
		motor[RBmtr] = 800;
	}
	motor[LFmtr] = -20;
	motor[LBmtr] = -20;
	motor[RFmtr] = -20;
	motor[RBmtr] = -20;
	wait10Msec(25);
	DriveMotorStop();
}

void TurnRightWithEncoders(int rotations)///turning right with encoder control
{
	EncoderClear();
	while(nMotorEncoder[RBmtr] < rotations)
	{
		motor[LFmtr] = 100;
		motor[LBmtr] = 100;
		motor[RBmtr] = -100;
		motor[RFmtr] = -100;
	}
	{
		DriveMotorStop();
		motor[LFmtr] = -45;
		motor[LBmtr] = -45;
		motor[RFmtr] = 45;
		motor[RBmtr] = 45;
		wait10Msec(25);
		DriveMotorStop();
	}
}

void TurnLeftWithEncoders(int rotations)///routine turning left with encoder control
{
	EncoderClear();
	while(nMotorEncoder[LBmtr] > rotations)
	{
		motor[LFmtr] = -80;
		motor[LBmtr] = -80;
		motor[RFmtr] = 80;
		motor[RBmtr] = 80;
	}
	motor[LFmtr] = 20;
	motor[LBmtr] = 20;
	motor[RFmtr] = -20;
	motor[RBmtr] = -20;
	wait10Msec(20);
	DriveMotorStop();
}

void BackwardsWithEncoders(int rotations)
{
	EncoderClear();
	while(nMotorEncoder[LBmtr] > rotations)
	{
		motor[LFmtr] = -110;
		motor[LBmtr] = -110;
		motor[RFmtr] = -80;
		motor[RBmtr] = -80;
	}

	motor[LFmtr] = 35;
	motor[LBmtr] = 35;
	motor[RFmtr] = 35;
	motor[RBmtr] = 35;
	wait10Msec(25);
	DriveMotorStop();
}



///////////////////
//encoder rutines//
//////////////////////////////////
//These rutines	the encoders		//
//when they are working					//
//////////////////////////////////

void RedInBump()
{

}
void BlueInBump()
{

}

void RedOutOfBump()
{

}

void BlueOutOfBump()
{

}

void Skills()
{

}

task autonomous()
{
	StartTask(Arm);

		if (SensorValue[RO] == 1)
		{
			RedOutOfBump();
		}

		if (SensorValue[RI] == 1)
		{
			RedInBump();
		}

		if (SensorValue[BO] == 1)
		{
			BlueOutOfBump();
		}

		if (SensorValue[BI] == 1)
		{
			BlueInBump();
		}

		if (SensorValue[NunYa] == 1)
		{
			Skills();
		}
}
/////////////////////////////////////////////////////////////////////////////////////////
//
//																 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	// User control code here, inside the loop
	StartTask(Arm);

	while (true)
	{
		int LFW=LFmtr;
		int LBW=LBmtr;
		int RBW=RBmtr;
		int RFW=RFmtr;

		motor[RBW]=vexRT[Ch2];
		motor[RFW]=vexRT[Ch2];
		motor[LFW]=vexRT[Ch3];
		motor[LBW]=vexRT[Ch3];

		//Partner Control
		motor[IntakeRight]=vexRT[Ch3Xmtr2];
		motor[IntakeLeft]=vexRT[Ch3Xmtr2];
		motor[LiftLeft]=vexRT[Ch2Xmtr2];
		motor[LiftRight]=vexRT[Ch2Xmtr2];
		motor[LiftLeft2]=vexRT[Ch2Xmtr2];
		motor[LiftRight2]=vexRT[Ch2Xmtr2];
	}
}