#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Motor,  motorA,          motorLeftArm,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorB,          motorRightArm,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          motorBucketSpinner,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     motorShooter,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorBucket,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorRight,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorLeft,        tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servoBucketLock,                     tServoNormal)
#pragma config(Servo,  srvo_S1_C3_2,    servoBallLock,                     tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

#define JOY1_BUTTON(x) joystick.joy1_Buttons >> x-1 & 0x1

void initializeRobot()
{
	return;
}

void autonomous()
{
	StopAllTasks();
}

void teleop()
{
	/* Reset motors */
	motor[motorA] = 0;
	motor[motorB] = 0;
	motor[motorC] = 0;
	motor[motorD] = 0;
	motor[motorE] = 0;
	motor[motorF] = 0;
	motor[motorG] = 0;

	/* Reset servos */
	servo[servoBucketLock] = 255;
	servo[servoBallLock] = 6;

	while (1) {
		alive();
		getJoystickSettings(joystick);

		/* Left motor */
		if (joystick.joy1_y1 > 20) {
			motor[motorLeft] = 100;
		} else if (joystick.joy1_y1 < -20) {
			motor[motorLeft] = -100;
		} else {
			motor[motorLeft] = 0;
		}

		/* Right motor */
		if (joystick.joy1_y2 > 20) {
			motor[motorRight] = -100;
		} else if (joystick.joy1_y2 < -20) {
			motor[motorRight] = 100;
		} else {
			motor[motorRight] = 0;
		}

		/* Ball shooter */
		if (JOY1_BUTTON(1) == 1) {
			/* Backwards */
			motor[motorShooter] = 255;
		} else if (JOY1_BUTTON(3) == 1) {
			/* Forwards */
			motor[motorShooter] = -255;
		} else {
			motor[motorShooter] = 0;
		}

		/* Ball box */
		if (JOY1_BUTTON(4) == 1) {
			/* Up */
			motor[motorBucket] = 255;
			wait1Msec(10);
			motor[motorBucket] = 0;
		} else if (JOY1_BUTTON(2) == 1) {
			/* Down */
			motor[motorBucket] = -255;
			wait1Msec(10);
			motor[motorBucket] = 0;
		}

		/* Left arm */
		if (JOY1_BUTTON(5) == 1) {
			/* In */
			motor[motorLeftArm] = -255;
			wait1Msec(300);
			motor[motorLeftArm] = 0;
		} else if (JOY1_BUTTON(7) == 1) {
			/* Out */
			motor[motorLeftArm] = 255;
			wait1Msec(300);
			motor[motorLeftArm] = 0;
		}

		/* Right arm */
		if (JOY1_BUTTON(6) == 1) {
			/* In */
			motor[motorRightArm] = -255;
			wait1Msec(300);
			motor[motorRightArm] = 0;
		} else if (JOY1_BUTTON(8) == 1) {
			/* Out */
			motor[motorRightArm] = 255;
			wait1Msec(300);
			motor[motorRightArm] = 0;
		}

		/* Bucket thing */
		if (JOY1_BUTTON(9) == 1) {
			/* On */
			motor[motorBucketSpinner] = 100;
		} else if (JOY1_BUTTON(10) == 1) {
			/* Off */
			motor[motorBucketSpinner] = 0;
		}

		if (joystick.joy1_TopHat == 0 ) {
			servo[servoBucketLock] = 160;
		}

		if (joystick.joy1_TopHat == 4 ) {
			servo[servoBucketLock] = 255;
		}

		if (joystick.joy1_TopHat == 2 ) {
			servo[servoBallLock] = 6;
		}

		if (joystick.joy1_TopHat == 6 ) {
			servo[servoBallLock] = 128;
		}
	}
}

task main()
{
	initializeRobot();

	waitForStart();

	if (joystick.UserMode) {
		teleop();
	} else {
		autonomous();
	}
}