#include "main.h"
using namespace vex;
vex::brain				Brain;
vex::competition 	Competition;
motor right_fwd = vex::motor (vex::PORT1);
motor left_fwd = vex::motor (vex::PORT2);
motor right_back = vex::motor (vex::PORT3);
motor left_back = vex::motor (vex::PORT4);

/**
 * A callback function for LLEMU's center button.
 *
 * When this callback is fired, it will toggle line 2 of the LCD text between
 * "I was pressed!" and nothing.
 */
void on_center_button() {
	static bool pressed = false;
	pressed = !pressed;
	if (pressed) {
		pros::lcd::set_text(2, "I was pressed!");
	} else {
		pros::lcd::clear_line(2);
	}
}

/**
 * Runs initialization code. This occurs as soon as the program is started.
 *
 * All other competition modes are blocked by initialize; it is recommended
 * to keep execution time for this mode under a few seconds.
 */
void initialize() {
	pros::lcd::initialize();
	pros::lcd::set_text(1, "Hello PROS User!");

	pros::lcd::register_btn1_cb(on_center_button);
}

/**
 * Runs while the robot is in the disabled state of Field Management System or
 * the VEX Competition Switch, following either autonomous or opcontrol. When
 * the robot is enabled, this task will exit.
 */
void disabled() {}

/**
 * Runs after initialize(), and before autonomous when connected to the Field
 * Management System or the VEX Competition Switch. This is intended for
 * competition-specific initialization routines, such as an autonomous selector
 * on the LCD.
 *
 * This task will exit when the robot is enabled and autonomous or opcontrol
 * starts.
 */
void competition_initialize() {}

/**
 * Runs the user autonomous code. This function will be started in its own task
 * with the default priority and stack size whenever the robot is enabled via
 * the Field Management System or the VEX Competition Switch in the autonomous
 * mode. Alternatively, this function may be called in initialize or opcontrol
 * for non-competition testing purposes.
 *
 * If the robot is disabled or communications is lost, the autonomous task
 * will be stopped. Re-enabling the robot will restart the task, not re-start it
 * from where it left off.
 */
void autonomous() {}

int right;
	int left;
	int control = 1; //1 is red top 2 is red bottom, 3 ...
	if (control == 1){
		right = 100;
		left = 100;
	}
	if (control == 2){
		right = 0;
		left = 0;
	}
	if (control == 3){
		right = 100;
		left = 0;
	}
	if (control == 4){
		right = 0;
		left = 100;
	}
	while (true){
		left_fwd = left;
		right_fwd = right;
		left_back = left;
		right_back = right;
	}
 */
void opcontrol() {
	while (true) {
			int fwd ;
			int rotation ;
			int left;
			int right;

			if (master.get_digital(DIGITAL_RIGHT)) {
				left_fwd.spin(directionType::fwd,100, velocityUnints::pct);
				right_fwd.spin(directionType::fwd,-100, velocityUnints::pct);
				left_back.spin(directionType::fwd,-100, velocityUnints::pct);
				right_back.spin(directionType::fwd,100, velocityUnints::pct);
			}
			else if (master.get_digital(DIGITAL_LEFT)){
				left_fwd.spin(directionType::fwd,-100, velocityUnints::pct);
				right_fwd.spin(directionType::fwd,100, velocityUnints::pct);
				left_back.spin(directionType::fwd,100, velocityUnints::pct);
				right_back.spin(directionType::fwd,-100, velocityUnints::pct);
			}
			else{
				fwd = master.get_analog(ANALOG_RIGHT_Y);
				rotation = master.get_analog(ANALOG_RIGHT_X);
				left = fwd + rotation;
				right = fwd - rotation;
				right_fwd.spin(fwd, right, velocityUnints::pct);
				right_back.spin(fwd, right, velocityUnints::pct);
				left_fwd.spin(fwd, left, velocityUnints::pct);
				left_back.spin(fwd, left, velocityUnints::pct);
			}
			if (master.get_digital(DIGITAL_R2) == 1){
				left_lift.spin(directionType::fwd,100, velocityUnints::pct);
				right_lift.spin(directionType::fwdl,-100, velocityUnints::pct);
			}
			else if (master.get_digital(DIGITAL_R1) == 1){
				left_lift.spin(directionType::fwd,-100, velocityUnints::pct);
				right_lift.spin(directionType::fwd,100, velocityUnints::pct);
			}
			else if (master.get_digital(DIGITAL_R1) == 0 && master.get_digital(DIGITAL_R2) == 0){
				left_lift.stop(vex::hold);
				right_lift.stop(vex::hold);
			}

			pros::delay(2);

		}
}