#pragma config(Sensor, in1,    armPos,         sensorPotentiometer)
#pragma config(Sensor, in2,    lightR,         sensorReflection)
#pragma config(Sensor, in3,    lightL,         sensorReflection)
#pragma config(Sensor, dgtl3,  buttonStart,    sensorDigitalIn)
#pragma config(Sensor, dgtl4,  armLimit,       sensorDigitalIn)
#pragma config(Sensor, dgtl5,  buttonL,        sensorDigitalIn)
#pragma config(Sensor, dgtl6,  rotaryR1,       sensorQuadEncoder)
#pragma config(Sensor, dgtl8,  sonarIn,        sensorSONAR_inch)
#pragma config(Sensor, dgtl10, rotaryL1,       sensorQuadEncoder)
#pragma config(Sensor, dgtl12, buttonR,        sensorDigitalIn)
#pragma config(Motor,  port1,           leftMotor,     tmotorVex393_HBridge, openLoop, reversed, driveLeft)
#pragma config(Motor,  port2,           clawMotor,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           armMotor,      tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          rightMotor,    tmotorVex393_HBridge, openLoop, reversed, driveRight)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//==================================================================================//

/* Progressive Development Code
 * for ETC 244 VeX Robotics Lab
 * Instructor: Bill Dolan
 * Fall 2023-2024
 *
 * Developed by Skylar Grant
 *
 * Notes: This bot and its configuration are non-standard. My sensors are
 *   located in different locations than usual, and are plugged into
 *   different ports than usual.
*/

//==================================================================================//

/* Quick function to set the speed of the drive motors
 *
 * Arguments:
 * - leftSpeed: signed integer, -127 to 127
 *   - Speed to drive the left-side motor
 * - rightSpeed: signed integer, -127 to 127
 *   - Speed to drive the right-side motor
*/
void setMotors(int leftSpeed, int rightSpeed) {
	// Turn on the left motor at the given speed
	motor[leftMotor] = leftSpeed;
	// Turn on the right motor at the given speed
	motor[rightMotor] = rightSpeed;
}

//==================================================================================//

/* Arm safety checker, verifies potentiometer position and limit switch status
 *   of the arm to disable the motors automatically.
 *
 * Arguments:
 * - direction: single character
 *   - "U" | "D": Up or Down respectively
 *   - Determines which direction the arm wants to move, otherwise the arm would always
 *     be stuck in a limit position.
 *
 * Returns:
 * - integer: Status, 0 means arm is in a blocked position, 1 means the arm can move
 *
 * Notes:
 * None.
*/
int checkArm(char *direction) {
  if (direction == "U") { // The arm wants to go up
  	// So we check the potentiometer
	if (SensorValue[armPos] < 1800) { // 1800 limit / 2300 max
		return 1; // The arm can move up more
	} else {
		return 0; // The arm is at the upper limit, do not move it up more
	}
  } else if (direction == "D") { // The arm wants to go down
	// So we check the limit switch
	if (SensorValue[armLimit] == 1) { // The limit switch isn't being pressed
		return 1; // The arm can move down more
	} else { // The switch is being pressed down
		return 0; // The arm cannot be moved down more
	}
  }
}

/* Quickly set the arm motor speed
 *
 * Arguments:
 * - speed: signed integer, -127 to 127
 *   - Speed to drive the arm motor
 *
 * Notes:
 * This is a very superfluous function but I hate typing motor[motorName] = 127 constantly
*/
void setArm(int speed) {
	// Check the arm safety
	int safe = 0;
	if (speed > 0) {
		safe = checkArm("U");
	} else if (speed < 0) {
		safe = checkArm("D");
	} else {
		safe = 0;
	}

	if (safe == 1) {
		// Set the arm motor speed
		motor[armMotor] = speed;
	} else {
		// Turn off the motor
		motor[armMotor] = 0;
	}

}

/* Quickly set the claw motor speed
 *
 * Arguments:
 * - speed: signed integer, -127 to 127
 *   - Speed to drive the claw motor
 *
 * Notes:
 * This is a very superfluous function but I hate typing motor[motorName] = 127 constantly
*/
void setClaw(int speed) {
	// Set the claw motor speed
	motor[clawMotor] = speed;
}

//==================================================================================//

/* Quick function to stop both drive motors
 *
 * Arguments: None
*/
void stop() {
	// Turn off the left motor
	motor[leftMotor] = 0;
	// Turn off the right motor
	motor[rightMotor] = 0;
}

//==================================================================================//

/* Function to drive the bot forward for some time
 *
 * Arguments:
 * - speed: signed integer, 0 to 127.
 *   - Speed to drive the motors
 * - timeMs: signed integer
 *   - Amount of time to wait before stopping, in milliseconds
*/
void driveForward(int speed, int timeMs) {
	// Turn on the left and right motors at speed
	setMotors(speed, speed); // setMotors(leftSpeed, rightSpeed)
	// Wait for the set time
	wait1Msec(timeMs);
	// Turn the motors off
	stop();
}

//==================================================================================//

/* Function to drive the bot backwards for some time
 *
 * Arguments:
 * - speed: signed integer, 0 to 127.
 *   - Speed to drive the motors, inversion is handled internally
 * - timeMs: signed integer
 *   - Amount of time to wait before stopping, in milliseconds
*/
void driveBackward(int speed, int timeMs) {
	// Invert the speed
	int revSpeed = speed * -1;
	// Turn on the left and right motors at the reverse speed
	setMotors(revSpeed, revSpeed); // setMotors(leftSpeed, rightSpeed)
	// Wait for the set time
	wait1Msec(timeMs);
	// Turn the motors off
	stop();
}

//==================================================================================//

// TODO: Just combine this into one drive function with L/R/B and a reverse boolean
/* Function to drive only one side's drive motors for a given time
 *
 * Arguments:
 * - side: 1 character string, "L" | "R"
 *   - Which side's motors to drive
 * - speed: signed integer, 0 to 127.
 *   - Speed to drive the motors
 * - timeMs: signed integer
 *   - Amount of time to wait before stopping, in milliseconds
*/
void driveOneSide(char *side, int speed, int timeMs) {
	// Check the direction and apply the motor speeds as required
	if (strcmp(side, "L") == 0) { // Drive Left Side
		setMotors(speed, 0);
	} else if (strcmp(side, "R") == 0) { // Drive Right Side
		setMotors(0, speed); // setMotors(leftSpeed, rightSpeed)
	}
	// Wait for the specified amount of time
	wait1Msec(timeMs);
	// Turn off the motors
	stop();
}

//==================================================================================//

// TODO: Flesh out this function
//void drive(char *side, int speed, int timeMs, int reverse)

/* Turn the robot
 *
 * Arguments:
 * - direction: One character string, "L" | "R"
 *   - Which direction to turn
 * - speed: signed integer 0 to 127
 *   - Speed to drive the motors at. Inversion is handled internally if needed
 * - timeMs: signed integer
 *   - Milliseconds to wait before stopping the turn
 * - reverse: boolean
 *   - Whether or not to reverse the turning-side motors to pivot-in-place
*/
void turn(char *direction, int speed, int timeMs, bool reverse) {
	// Turn left by reversing the left drive motors and going forward
	// with the right drive motors
	// Turn right by going forward with the left drive motors
	// and reversing the right drive motors

	// Default to not reversing the turning-side motors
	int revSpeed = 0;

	// If reverse is true, reverse the turning-side motors
	if (reverse == true) {
		// Get the inverse of the speed for pivot-in-place
		revSpeed = speed * -1;
	}

	// Check the direction and apply the motor speeds as required
	if (strcmp(direction, "L") == 0) { // Turn Left
		setMotors(revSpeed, speed); // setMotors(leftSpeed, rightSpeed)
	} else if (strcmp(direction, "R") == 0) { // Turn Right
		setMotors(speed, revSpeed); // setMotors(leftSpeed, rightSpeed)
	} else {
	  wait1Msec(100);
	}
	// Wait for the specified amount of time
	wait1Msec(timeMs);
	// Turn off the motors
	stop();
}

//==================================================================================//

/* Move the bot's arm up or down
 *
 * Arguments:
 * - speed: signed integer. -127 to 127
 *   - <0: Down
 *   - >0: Up
 *   - Speed to drive the arm motor
 * - timeMs: signed integer
 *   - Time to wait before stopping the arm, in milliseconds
*/
void moveArm(int speed, int timeMs) {
	// Turn on the motor at the set speed
	motor[armMotor] = speed;
	// Wait for the set time
	wait1Msec(timeMs);
	// Turn the motor off
	motor[armMotor] = 0;
}

//==================================================================================//

/* Move the bot's claw motor
 *
 * Arguments:
 * - speed: signed integer. -127 to 127
 *   - <0: Down
 *   - >0: Up
 *   - Speed to drive the claw motor
 * - timeMs: signed integer
 *   - Time to wait before stopping the claw, in milliseconds
*/
void moveClaw(int speed, int timeMs) {
	// Turn on the motor at the set speed
	motor[clawMotor] = speed;
	// Wait for the set time
	wait1Msec(timeMs);
	// Turn the motor off
	motor[clawMotor] = 0;
}

/* Put your arms in the air like you just don't care
 * Moves the bot's arm up until it reaches a preset position,
 * determined by a potentiometer
 *
 * Arguments:
 * - speed: signed integer. 0 to 127
 *   - Speed to drive the arm motor
 *
 * Logic:
 * - Arm Position Potentiometer:
 *   - Claw Vertical: 1600
 *   - Claw Soft Limit: 1800
 *   - Claw Hard Limit: 2100
*/
void armUp100(int speed) {
	// Check that the arm potentiometer is showing a value below 1800
	while (SensorValue[armPos] <= 1800) {
		// Set the motor to the desired speed
		motor[armMotor] = speed;
	}
	// Once the arm position is showing > 1800, shut off the arm
	motor[armMotor] = 0;
}

//==================================================================================//

/* Moves the robot's arm down until the limit switch is pressed
 *
 * Arguments:
 * - speed: signed integer. 0 to 127
 *   - Speed to drive the arm motor
 *
 * Logic:
 * - Arm Limit Switch:
 *   - Momentary switch, normally closed.
 *   - When arm is not making contact, shows a logical 1
*/
void armDown100(int speed) {
	// While the arm limit switch isn't being contacted
	while (SensorValue[armLimit] == 1) {
		// Invert the given speed to make the motor go down
		int revSpeed = speed * -1;
		// Turn on the motor at the set speed
		motor[armMotor] = revSpeed;
	}
	// Once the limit switch is being contacted (logical 0), stop the arm motor
	motor[armMotor] = 0;
}

//==================================================================================//

/* Close the claw
 *
 * Arguments: None
 * Notes: TODO: This should have some variability, and ideally
 *   would have a way to detect resistance against the claw
*/
void clawGrab() {
	// Turn on the motor at the half speed
	motor[clawMotor] = -32;
	// Wait for the set time
	wait1Msec(1000);
	// Turn the motor off
	motor[clawMotor] = 0;
}

//==================================================================================//

/* Open the claw
 *
 * Arguments: None
 * Notes: TODO: This should have some variability, and ideally
 *   would have a way to detect the claw being open 100%
*/
void clawRelease() {
	// Turn on the motor at the half speed
	motor[clawMotor] = 32;
	// Wait for the set time
	wait1Msec(1000);
	// Turn the motor off
	motor[clawMotor] = 0;
}

//==================================================================================//

/* Run the autonomous code, called when the start button is pressed.
 *
 * Arguments: None
 * Notes: TODO: Find a way to recall main() so this can be rerun without rebooting/recompiling
 *
 * Lab Description:
 * Have arm motor
 * - Run autonomous with start button
 * - Arm up 1000ms
 * - Arm down 1000ms
 * - Claw release 1000ms
 * - Claw grab 1000ms
 * - Repeat @50% speed
*/
void runAutonomous() {
	moveArm(127, 1000);
	moveArm(-127, 1000);
	moveClaw(127, 1000);
	moveClaw(-127, 1000);
	moveArm(64, 1000);
	moveArm(-64, 1000);
	moveClaw(64, 1000);
	moveClaw(-64, 1000);
}


//==================================================================================//

// This function gets called automatically once the bot is booted.
task main() {
	// Loop over this code infinitely, this will allow running the autocode more than once
	while (1) {
		// As long as the start button isn't pressed (Logical 1) run the code for the controller input
		while (SensorValue[buttonStart]==1) {
			// Driver Mode, Controller Inputs
			int leftSpeed = (vexRT[Ch2] + vexRT[Ch1]) / 2; // (y + x) / 2
			int rightSpeed = (vexRT[Ch2] - vexRT[Ch1])/2;  // (y - x) / 2
			float leftAdjSpeed = leftSpeed / 10;
			float rightAdjSpeed = rightSpeed / 10;
			leftAdjSpeed = floor(leftAdjSpeed) * 10;
			rightAdjSpeed = floor(rightAdjSpeed) * 10;

			if (fabs(leftAdjSpeed) > 30) {
				motor[leftMotor]  = leftAdjSpeed;
			} else {
				motor[leftMotor] = 0;
			}

			if (fabs(rightAdjSpeed) > 30) {
				motor[rightMotor] = rightAdjSpeed;
			} else {
				motor[rightMotor] = 0;
			}

			// Raise, lower or do not move arm
			if(vexRT[Btn7U] == 1)       	//If button 7U is pressed...
			{
				motor[armMotor] = 32;    	//...raise the arm.
			}
			else if(vexRT[Btn7D] == 1)  	//Else, if button 7D is pressed...
			{
				motor[armMotor] = -32;   	//...lower the arm.
			}
			else                      		//Else (neither button is pressed)...
			{
				motor[armMotor] = 0;      	//...stop the arm.
			}

			// Open, close or do not more claw
			if(vexRT[Btn7L] == 1)       	//If Button 7L is pressed...
			{
				motor[clawMotor] = 64;  		//...open the gripper.
			}
			else if(vexRT[Btn7R] == 1)  	//Else, if button 7R is pressed...
			{
				motor[clawMotor] = -32; 		//...close the gripper.
			}
			else                      		//Else (neither button is pressed)...
			{
				motor[clawMotor] = 0;    		//...stop the gripper.
			}
		}

		// Once we've detected a button press, run the autonomous function block.
		runAutonomous();
	}
}