#pragma config(Sensor, dgtl11, ,               sensorQuadEncoder)
#pragma config(Motor,  port1,           frontLeft,     tmotorVex393, openLoop)
#pragma config(Motor,  port2,           frontRight,    tmotorVex393, openLoop)
#pragma config(Motor,  port3,           backRight,     tmotorVex393, openLoop)
#pragma config(Motor,  port4,           conveyer1,     tmotorVex393, openLoop)
#pragma config(Motor,  port5,           conveyer2,     tmotorVex393, openLoop)
#pragma config(Motor,  port6,           leftExpand,    tmotorVex269, openLoop)
#pragma config(Motor,  port7,           rightExpand,   tmotorVex269, openLoop)
#pragma config(Motor,  port9,           topArm,        tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port10,          backLeft,      tmotorVex393, openLoop, reversed, encoder, encoderPort, dgtl11, 1000)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

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

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
#define WHEELDIAMETER 4
#define ROTATION 699
#define isRed false

/*
* Function:    strafe()
* Description: Provides ability to strafe left and right.
* Parameters:  Variable joystick input
* Returns:     None
*/
void strafe( int strafeValue ){
		motor[ frontLeft ] = strafeValue;
	  motor[ backLeft ] = -strafeValue;
		motor[ frontRight ] = strafeValue;
		motor[ backRight ] = -strafeValue;
		//lengthens update time
		if( strafeValue != 0){
			wait10Msec( 1 );
		}
}//end strafe

/*
* Function:    chassisMotion()
* Description: implements user control for chassis movement
* Parameters:  Two Variable joystick input
* Returns:     None
*/
void chassisMotion( int yMove , int xMove ){
	int tempVal = 0 , tempTurn = 0;//temporary holder variables
	//input from right joystick is transfered to the motors
	if( xMove == 0 ){
		motor[ frontLeft ] = yMove;
		motor[ backLeft ] = yMove;
		motor[ frontRight ] = yMove;
		motor[ backRight ] = yMove;
	}else if( xMove < 0 ){
		motor[ frontLeft ] = yMove;
		motor[ backLeft ] = yMove;
		motor[ frontRight ] = yMove + xMove;
		motor[ backRight ] = yMove + xMove;
	}else{
		motor[ frontLeft ] = yMove - xMove;
		motor[ backLeft ] = yMove - xMove;
		motor[ frontRight ] = yMove;
		motor[ backRight ] = yMove;
	}
	//lengthens update time
	if( yMove != 0 && xMove != 0 ){
		wait10Msec( 1 );
	}
}//end chassisMotion

/*void get_big( int expand_number , int contract_number ){
	if( expand_number == 1 ){
		motor[ leftExpand ] = 127;
		motor[ rightExpand ] = -127;
	}else if( contract_number ){
		motor[ leftExpand ] = -127;
		motor[ rightExpand ] = 127;
	}else{
		motor[ leftExpand ] = motor[ rightExpand ] = 0;
	}
}//end get_big
*/
/*
* Function:    manipMotion()
* Description: Controls conveyer belt
* Parameters:  int button values
* Returns:     None
*/
void manipMotion( int forward , int backward ){
	if( forward == 1 ){
		motor[ conveyer1 ] = 127;
		motor[ conveyer2 ] = 127;
	}else if( backward == 1 ){
		motor[ conveyer1 ] = -127;
		motor[ conveyer2 ] = -127;
	}else{
	  motor[ conveyer1 ] = 0;
		motor[ conveyer2 ] = 0;
	}
}//end mattMotion

void moveUp( float dist , int speed ){
	nMotorEncoder[ backLeft ] = 0;
	dist = dist * 24 / (2 * WHEELDIAMETER * pi ) * ROTATION;
	while( abs( nMotorEncoder[ backLeft ] ) < dist ){
		chassisMotion( speed , 0 );
		wait10Msec( 2 );
	}
}

void moveStrafe( float dist , int speed ){
	nMotorEncoder[ backLeft ] = 0;
	dist = dist * 24 / (2 * WHEELDIAMETER * pi ) * ROTATION;
	while( abs( nMotorEncoder[ backLeft ] ) < dist ){
		strafe( speed );
		wait10Msec( 2 );
	}
}

void pointTurn( float dist ){
	nMotorEncoder[ backLeft ] = 0;
	dist = dist / (2 * WHEELDIAMETER * pi ) * ROTATION;
	while( abs( nMotorEncoder[ backLeft ] ) < abs(dist) ){
		if(dist>0){
			motor[ frontLeft ] = motor[ backLeft ] = -100;
			motor[ frontRight ] = motor[backRight ] = 100;
		}
		else{
			motor[frontLeft] = motor[backLeft] = 100;
			motor[frontRight] = motor[backRight] = -100;
		}
		wait10Msec( 2 );
	}
		motor[ frontLeft ] = motor[ backLeft ] = 0;
		motor[ frontRight ] = motor[backRight ] = 0;
}

void balls( int count ){
	int temp = 0;
	while( temp < count){
		motor[ conveyer1 ] = 127;
		motor[ conveyer2 ] = 127;
		wait10Msec( 10 );
	}

	motor[ conveyer1 ] =0;
	motor[conveyer2 ] = 0;
}

void auto1(){
	moveStrafe( 0.85 , -127 );
	moveUp( 3.8 , 100 );
	pointTurn( ( 0.25 * 20 * pi ) );
	balls( 100 );
}

void auto2(){
	moveStrafe( -0.85 , 127 );
	moveUp( 3.5 , 100 );
	pointTurn( ( 0.25 * 20 * pi ) );
	//balls( 100 );
	motor[conveyer1]=motor[conveyer2]=-127;
}

void pre_auton()
{
  // Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
  // Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
  bStopTasksBetweenModes = true;

}

task autonomous()
{
	if( isRed == true ){
  	auto1();
  }else{
  	auto2();
  }
}

task usercontrol()
{
	// User control code here, inside the loop
	while (true)
	{
		if(vexRT[Btn5U] == 1){
			motor[topArm] = 65;
			while(vexRT[Btn5U] == 1);
			motor[topArm] = 0;
		}
		else if(vexRT[Btn5D] == 1){
			motor[topArm] = -65;
			while(vexRT[Btn5D] == 1);
			motor[topArm] = 0;
		}
		if( vexRT[ Ch4 ] != 0 ){
			strafe( vexRT[ Ch4 ] );
		}else{
			chassisMotion( vexRT[ Ch2 ] , vexRT[ Ch1 ] );
		}
		//get_big( vexRT[ Btn5U ] , vexRT[ Btn5D ] );
		manipMotion( vexRT[ Btn6U ] , vexRT[ Btn6D ] );
	}
}