#pragma config(Sensor, dgtl1, BumperSwitch_F, sensorTouch)
#pragma config(Sensor, dgtl2, BumperSwitch_B, sensorTouch)
#pragma config(Sensor, dgtl3, BumperSwitch_L, sensorTouch)
#pragma config(Sensor, dgtl4, BumperSwitch_R, sensorTouch)
#pragma config(Sensor, dgtl5, LimitSwitch, sensorTouch)
#pragma config(Motor, port1, motor1, tmotorNormal, openLoop)
#pragma config(Motor, port2, motor2, tmotorNormal, openLoop)
#pragma config(Motor, port3, motor3, tmotorNormal, openLoop)
#pragma config(Motor, port4, motor4, tmotorNormal, openLoop)

#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!

#define MAX_C 127 //Max direction going clockwise
#define MAX_CC -127 //Max directon going counter-clockwise

void StopMoving(); //Stops all the motors that move the robot
void Move_Y_Axis(int speed, int wait); //Moves the robot up or down
void Move_X_Axis(int speed, int wait); //Moves the robot left or right
void Rotate(); //Rotates the robot
int MotorCheck(int motor); //Makes sure that the motor does not exceed -127 or 127
void ArmControl(int speed, int wait); //Lifts/Lowers the arm
void HandControl(int speed, int wait); //Open/Closes the "hand" of the arm
void Move_XY(int speed, int wait); //Moves the robot diagnolly

void pre_auton()
{
}

task autonomous()
{
AuonomousCodePlaceholderForTesting(); // Remove this function call once you have "real" code.
}

task usercontrol()
{
int JoyStick1X, JoyStick2X, JoyStick2Y;
int m1, m2, m3, m4;

while(true) {

JoyStick1X = vexRT[Ch4];
JoyStick2X = vexRT[Ch1];
JoyStick2Y = vexRT[Ch2];

m1 = (JoyStick2X * -1) + (JoyStick1X * -1);
m2 = JoyStick2Y + (JoyStick1X * -1);
m3 = (JoyStick2X) + (JoyStick1X * -1);
m4 = (JoyStick2Y * -1) + (JoyStick1X * -1);

motor[motor1] = MotorCheck(m1);
motor[motor2] = MotorCheck(m2);
motor[motor3] = MotorCheck(m3);
motor[motor4] = MotorCheck(m4);

//Code for arm will go below as soon as I am sure how it will work
}
}

void StopMoving()
{
motor[motor1] = 0;
motor[motor2] = 0;
motor[motor3] = 0;
motor[motor4] = 0;
}

void Move_Y_Axis(int speed, int wait)
{
motor[motor1] = speed;
motor[motor2] = -1 * (speed);
motor[motor3] = speed;
motor[motor4] = -1 * (speed);
wait1Msec(wait);
StopMoving();
}

void Move_X_Axis(int speed, int wait)
{
motor[motor1] = speed;
motor[motor2] = speed;
motor[motor3] = -1 * (speed);
motor[motor4] = -1 * (speed);
wait1Msec(wait);
StopMoving();
}

int MotorCheck(int motor)
{
if(motor >= 127)
{
motor = 127;
}
else if(motor <= -127)
{
motor = -127;
}
return motor;
}