#include "WPILib.h"
#include <math.h>
#include "VisionAPI.h"
#include "TrackAPI.h"
#include "AxisCamera.h"

const double PI = 3.1415926;
const double FEET_TO_GEARTOOTH = 96.0 / PI;

const int RIGHT = 1;
const int LEFT = 2;
const float SENS = 0.5;

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */
class DefaultRobot : public SimpleRobot
{
	RobotDrive* myRobot;			// robot drive system
	Jaguar* leftJag;				// left speed controller
	Jaguar* rightJag;				// right speed controller
	//DigitalInput* armUpperLimit;	// arm upper limit switch
	//DigitalInput* armLowerLimit;	// arm lower limit switch
	AnalogChannel* SelectorSwitch;
	Joystick* rightStick;			// joystick 1 (arcade stick or right tank stick)
	Joystick* leftStick;			// joystick 2 (tank left stick)
	//Joystick* armStick;				// joystick 3 to control arm
	DriverStation* ds;				// driver station object
	Gyro* gyro;						// gyro sensor
	GearTooth* geartooth;			// geartooth sensor
	Compressor* compressor;
	Relay* ramprelease;				// solenoid for ramp release
	Relay* rightramp;				// right ramp up&down
	Relay* leftramp;				// left ramp up&down
	
	
	
	enum JUMPERS					// Driver Station jumpers to control program operation
	{ ARCADE_MODE = 1,				// Tank/Arcade jumper is on DS Input 1 (Jumper present is arcade)
	  DISABLE_AUTONOMOUS = 2,		// Autonomous/Teleop jumper is on DS Input 2 (Jumper absent is autonomous)
	  GYRO_RESET = 3,				// Gyro Reset jumper on 3
	  AUTO_TURN_LEFT = 4			// putting jumper on 4 makes robot turn left, not right, in autonomous mode
	};	    
	

public:
	
	void TurnRight(int);				// turn right <angle> degrees
	void TurnLeft(int);					// turn left <angle> degrees
	void SensorDrive(int, float, bool);	// drive certain distance (IN FEET) forwards or backwards using gyro and geartooth sensors
	
	/**
	 * 
	 * 
	 * Constructor for this robot subclass.
	 * Create an instance of a RobotDrive with left and right motors plugged into PWM
	 * ports 1 and 2 on the first digital module.
	 */
	DefaultRobot(void)
	{
		ds = DriverStation::GetInstance();
		myRobot = new RobotDrive(leftJag, rightJag, SENS);	// create robot drive base
		rightStick = new Joystick(1);			// create the joysticks
		leftStick = new Joystick(2);
		leftJag = new Jaguar(1);
		rightJag = new Jaguar(2);
		//armStick = new Joystick(3);
		//armUpperLimit = new DigitalInput(1);	// create the limit switch inputs
		//armLowerLimit = new DigitalInput(2);
		SelectorSwitch = new AnalogChannel (1,4);
		gyro = new Gyro(1, 1);
		myRobot->SetInvertedMotor(myRobot->kFrontRightMotor, true);
		geartooth = new GearTooth(1, true);
		geartooth->Start();
		compressor = new Compressor(2, 1);
		ramprelease = new Relay(4, 2, ramprelease->kBothDirections);
		rightramp = new Relay(4, 3, rightramp->kBothDirections);
		leftramp = new Relay (4, 4, leftramp->kBothDirections);
		SetDebugFlag(DEBUG_OFF);
		//frcStartPCVideoServer ("10.18.16.2");
		
		//Update the motors at least every 100ms.
		GetWatchdog().SetExpiration(100);
	}

	/* Drive forward 15 feet, then turn right or left 90 degrees based on jumper:
	 * Right/left turn jumper is ds Digital 4, if jumper off, robot will turn right, if jumper on, robot will turn left.
	 * Robot will then drive back and forth over 5 feet until autonomous mode terminated
	 */
	void Autonomous(void)
	{
		int count = 2;
		
		GetWatchdog().SetEnabled(false);
		
			if (!ds->GetDigitalIn(DISABLE_AUTONOMOUS)) {		// Unless jumper on 2, run autonomous code
				SensorDrive(15, .75, true);						// drive forward 15 feet at 3/4 speed
				cout << "Geartooth Count = " << geartooth->Get() << endl;
				
				if (!ds->GetDigitalIn(AUTO_TURN_LEFT)) {	// If jumper on 4, turn left
					TurnRight(90);
				}
				else {
					TurnLeft(90);							// Otherwise, turn right
				}
				while (IsAutonomous()) {
					Wait(500);
					SensorDrive(5, ((count++ % 2) == 0) ? 0.25 : -0.25, true);	// Drive back and forth 5 feet at quarter speed
				}																// last parameter allows for immediate termination upon autonomous mode exit
			}
		
		GetWatchdog().SetEnabled(true);
	}

	/**
	 * Runs the motors under driver control with either tank or arcade steering selected
	 * by a jumper in DS Digin 0. Also an arm will operate based on a joystick Y-axis. 
	 */
	void OperatorControl(void)
	{
		int e = 0;
		ParticleAnalysisReport* trackReport;
		Victor armMotor(5);						// create arm motor instance
		
		unsigned long loop_count = 1;
		
		while (IsOperatorControl())
		{
			GetWatchdog().Feed();

			if (rightStick->GetRawButton(4))
				SetDebugFlag (DEBUG_SCREEN_ONLY);
			else
				SetDebugFlag (DEBUG_OFF);
			// determine if tank or arcade mode; default with no jumper is for tank drive
			if (!ds->GetDigitalIn(ARCADE_MODE)) {	
				myRobot->TankDrive(leftStick, rightStick);	 // drive with tank style
			}
			else {
				myRobot->ArcadeDrive(rightStick);	         // drive with arcade style (use right stick)
			}

			// Control the movement of the arm using the joystick
			// Use the "Y" value of the arm joystick to control the movement of the arm
			//float armStickDirection = armStick->GetY();

			// if at a limit and telling the arm to move past the limit, don't drive the motor
			//if ((armUpperLimit->Get() == 0) && (armStickDirection > 0.0)) {
			//	armStickDirection = 0;
			//} else if ((armLowerLimit->Get() == 0) && (armStickDirection < 0.0)) {
			//	armStickDirection = 0;
			//}

			// Set the motor value 
			//armMotor.Set(armStickDirection);
			
	//		if ((loop_count++ % 5) == 0) {
	//		printf("Gear Tooth Count = %d\n", geartooth->Get());
	//		}

			e = FindColor(GREEN, trackReport);
			PrintReport(trackReport);
			
			if (ds->GetDigitalIn(GYRO_RESET)) {		//resets the gyro sensor if jumper is on ds digital port 3
				gyro->Reset();
			}
			loop_count++;
			if ((loop_count % 2000) == 0) {
	//			printf("Gyro Angle = %f\n", gyro->GetAngle());
				printf("Selector Switch Value = %d\n", SelectorSwitch->GetValue());
				printf("Selector Switch Avg Value = %d\n", SelectorSwitch->GetAverageValue());
				printf("Selector Switch Voltage = %f\n", SelectorSwitch->GetVoltage());
			}
			
			if (rightStick->GetTrigger()) {		// start compressor if rt. trigger pressed
				compressor->Start();					// compressor stops above 120 psi, starts below 100
			}
			
			if (leftStick->GetTrigger()) {		// stop compressor if lft. trigger pressed
				compressor->Stop();
			}
			
			if (rightStick->GetRawButton(2)) {		
				ramprelease->Set(ramprelease->kForward);	// pressing button 2r retracts ramp-release solenoid's rod
			}
			else {
				ramprelease->Set(ramprelease->kOff);		// releasing button pushes rod back out
			}
				
			if (rightStick->GetRawButton(10)) {		// button 10r lowers right ramp
				rightramp->Set(rightramp->kReverse);
			}
			if (rightStick->GetRawButton(11)) {		// button 11r raises right ramp
				rightramp->Set(rightramp->kForward);
			}
			
			if (leftStick->GetRawButton(10)) {		// button 10L lowers left ramp
				leftramp->Set(leftramp->kReverse);
			}
			if (leftStick->GetRawButton(11)) {		// button 11L raises left ramp
				leftramp->Set(leftramp->kForward);
			}		
		}
	}
};

/* TurnRight(angle): turns robot right 
 * a given number of degrees, uses gyro
 * sensor
 *
 * @param angle: Number of degrees turn, always positive
 */
void DefaultRobot::TurnRight(int angle) {
	gyro->Reset();
	while (gyro->GetAngle() <= angle) {
		myRobot->SetLeftRightMotorSpeeds(-.75, .75);
	}
}

/* TurnLeft(angle): turns robot left 
 * a given number of degrees, uses gyro
 * sensor
 *
 * @param angle: Number of degrees turn, always positive
 */
void DefaultRobot::TurnLeft(int angle) {
	gyro->Reset();
	while (gyro->GetAngle() >= -angle) {
		myRobot->SetLeftRightMotorSpeeds(.75, -.75);
	}
}

/* SensorDrive(Distance, Speed, Autonomous?): Drives robot straight
 * forward a set distance, uses gyro sensor to keep on course, uses geartooth sensor
 *
 * @param Distance: Distance robot will drive (IN FEET)
 * @param Speed: Speed robot will drive (-1.0 - +1.0)
 * @param Autonomous? : If this function is being called from an autonomous routine, this parameter should be true.
 *		Otherwise, it should be false. NOTE: This is used to immediately break out of the function if mode is 
 *		switched to teleop. If this parameter is true and the mode is set to teleoperated, then the code will not execute.
 */
void DefaultRobot::SensorDrive(int Distance, float speed, bool Autonomous) {
	float curve;
	
	gyro->Reset();
	geartooth->Reset();
	
	while ((!Autonomous || (IsAutonomous())) 
			&&		((double)geartooth->Get() <= ((double)Distance * FEET_TO_GEARTOOTH))) {
	// This loop will execute if both of these conditions are met:
	// (Either this function is being called from a non-autonomous routine (@param Autonomous = false) OR the ds switch is set to autonomous)  AND
	// (The robot has not yet driven the distance that it is supposed to drive (@param Distance))
	
		curve = (gyro->GetAngle() / -45.0);		// calculates correction factor to keep robot driving straight
		myRobot->Drive(-speed, curve);
	}
	
}

START_ROBOT_CLASS(DefaultRobot);