#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_3,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_4,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port2,           rightFront,    tmotorVex393HighSpeed, openLoop, reversed, encoder, encoderPort, I2C_1, 1000)
#pragma config(Motor,  port3,           rightBack,     tmotorVex393HighSpeed, openLoop, reversed, encoder, encoderPort, I2C_2, 1000)
#pragma config(Motor,  port8,           leftFront,     tmotorVex393HighSpeed, openLoop, encoder, encoderPort, I2C_4, 1000)
#pragma config(Motor,  port9,           leftBack,      tmotorVex393HighSpeed, openLoop, encoder, encoderPort, I2C_3, 1000)

#pragma platform(VEX)

#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(105)

#include "Vex_Competition_Includes.c"

void pre_auton() {
  bStopTasksBetweenModes = true;
}

void resetEncoders() {
	nMotorEncoder[rightFront] = 0;
	nMotorEncoder[rightBack] = 0;
	nMotorEncoder[leftFront] = 0;
	nMotorEncoder[leftBack] = 0;
}

task autonomous() {
	resetEncoders();

	//IMPORTANT: 392 counts/revolution

	while(nMotorEncoder[rightFront] < 1568) {
		motor[rightFront] = 127;
		motor[rightBack] = -127;
		motor[leftFront] = -127;
		motor[leftBack] = 127;
	}

	wait1Msec(1000);

	while(nMotorEncoder[rightFront] > 0) {
		motor[rightFront] = -127;
		motor[rightBack] = 127;
		motor[leftFront] = 127;
		motor[leftBack] = -127;
	}

	wait1Msec(1000);

	while(nMotorEncoder[rightFront] < 1568) {
		motor[rightFront] = 127;
		motor[rightBack] = 127;
		motor[leftFront] = 127;
		motor[leftBack] = 127;
	}

	wait1Msec(1000);

	while(nMotorEncoder[rightFront] > 0) {
		motor[rightFront] = -127;
		motor[rightBack] = -127;
		motor[leftFront] = -127;
		motor[leftBack] = -127;
	}

	wait1Msec(1000);
}

task usercontrol() {
	resetEncoders();

	while(true) {
		if(abs(vexRT[Ch4]) > 63) {
			motor[rightFront] = -vexRT[Ch4];
			motor[rightBack] = -vexRT[Ch4];
			motor[leftFront] = vexRT[Ch4];
			motor[leftBack] = vexRT[Ch4];
		}

		else {
			motor[rightFront] = vexRT[Ch2] + vexRT[Ch1];
    	motor[rightBack] = vexRT[Ch2] - vexRT[Ch1];
    	motor[leftFront] = vexRT[Ch2] - vexRT[Ch1];
    	motor[leftBack] = vexRT[Ch2] + vexRT[Ch1];
    }
	}
}