#include "WPILib.h"
#include "Shooter.h"
#include <math.h>

#ifndef BUTTON_LAYOUT
#define BUTTON_LAYOUT

#define TRIGGER 	1  	// Trigger button number
#define THUMB 		2	// Thumb button number
#define RAMP_RAISE  5   // Button 3 for Raising Ramp
#define RAMP_LOWER  3	// Button 4 to lower ramp.
#define UNJAM		11

#define DEADZONE_RADIUS 0.01 	// Deadzone Radius prevents tiny twitches in the joystick's value from
								// affecting the robot.  Use this for cleaning up drive train and shooter.
								// Also used for detecting changes in an axis' value.

#endif  // BUTTON_LAYOUT

class Robot: public IterativeRobot
{
	TalonSRX left_drive, right_drive;
	CANTalon shooter1, shooter2,
			ramp,
			arms;
	RobotDrive drive;
	Shooter shooter;
	Joystick driver_stick, operator_stick;
public:
	Robot():
		left_drive(0),			// Left DriveTrain Talons plug into PWM channel 1 with a Y-splitter
		right_drive(1),			// Right DriveTrain Talons plug  	// left wheel 2
		shooter1(11),  			// shooter drive 1
		shooter2(10),   		// shooter drive 2
		ramp(12),
		arms(13),
		drive(&left_drive, &right_drive),
		shooter(				// initialize Shooter object.
				&shooter1, &shooter2, &ramp),
		driver_stick(0), 				// right stick (operator)
		operator_stick(1)			// left stick (driver)
	{

	}

private:
	// instance variables
	bool pickupRunning;  // don't want to spam the Talon with set messages.  Toggle the pickup when a button is pressed or released.
	bool inverting;
	bool ramping;
	bool shooting;
	bool unjamming;
	bool arming;
	float shooter_power;

	LiveWindow *lw = LiveWindow::GetInstance();
	SendableChooser *chooser;
	const std::string autoNameDefault = "Default";
	const std::string autoNameCustom = "My Auto";
	std::string autoSelected;

	void RobotInit()
	{
		chooser = new SendableChooser();
		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
		chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
		SmartDashboard::PutData("Auto Modes", chooser);
		shooter1.Enable();
		shooter2.Enable();
		left_drive.SetInverted(true);
		right_drive.SetInverted(true);
		//ramp.SetInverted(true);
		inverting = false;
		pickupRunning = false;
		ramping = false;
		shooting = false;
		unjamming = false;
		arming = false;
		shooter_power = 0;

	}


	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
	 * below the Gyro
	 *
	 * You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
	 * If using the SendableChooser make sure to add them to the chooser code above as well.
	 */
	void AutonomousInit()
	{
		autoSelected = *((std::string*)chooser->GetSelected());
		//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
		std::cout << "Auto selected: " << autoSelected << std::endl;

		if(autoSelected == autoNameCustom){
			//Custom Auto goes here
		} else {
			//Default Auto goes here
		}
	}

	void AutonomousPeriodic()
	{

	}

	void TeleopInit()
	{

	}

	void TeleopPeriodic()
	{
		std::cout << "arm encoder position: " << arms.GetEncPosition() << std::endl;
		std::cout << "arm encoder velocity: " << arms.GetEncVel() << std::endl;
		drive.ArcadeDrive(-driver_stick.GetY(), -driver_stick.GetX()*0.75);

		//bool rampDoing = false;
		//  This is shit code for testing.  Replace it with real code.
		if(!ramping && operator_stick.GetRawButton(RAMP_RAISE))
		{
			std::cout << "Raising Ramp.";
			//ramp.Set(1);
			shooter.RaiseRamp();
			ramping =true;
		}
		else if(!ramping && operator_stick.GetRawButton(RAMP_LOWER))
		{
			std::cout << "Lowering Ramp.";
			//ramp.Set(-1);
			shooter.LowerRamp();
			ramping = true;
		}
		/*else if(!ramping && operator_stick.GetRawButton(TRIGGER))
		{
			shooter.BoostRamp();
			ramping = true;
		}*/
		else if(ramping
				&& !operator_stick.GetRawButton(RAMP_LOWER)
				&& !operator_stick.GetRawButton(RAMP_RAISE))
		{
			std::cout << "Stopping Ramp.";
			shooter.StopRamp();
			ramping = false;
		}


		if(!unjamming && operator_stick.GetRawButton(UNJAM))
		{
			unjamming = true;
			shooter.Unjam();
		}
		else if(!unjamming && operator_stick.GetRawButton(TRIGGER))
		{
			shooter.ShootLow();
			unjamming = true;
		}
		else if(unjamming && !operator_stick.GetRawButton(UNJAM) && !operator_stick.GetRawButton(TRIGGER))
		{
			shooter.PickUp(false);
			unjamming = false;
		}
		/*
		 * 	Run the Shooter only while the THUMB button is held down on the operator stick.
		 *	the 'pickupRunning' boolean is there to prevent the shooter from calling PickUp
		 *	every iteration of the TeleopPeriodic method (once every 10ms!)
		 *	The pickup is disabled when the thumb button is released, but the code still
		 *  has 'pickupRunning' as true.
		 */
		if(operator_stick.GetRawButton(THUMB) && !pickupRunning)
		{
			shooter.PickUp();
			pickupRunning = true;
		}
		else if(!operator_stick.GetRawButton(THUMB) && pickupRunning)
		{
			shooter.PickUp(false);
			pickupRunning = false;
		}

		/*
		 *	The 'inverting' variable is used to make sure that the drive train isn't getting
		 *	inverted every iteration of the TeleopPeriodic method while the button is held down.
		 *	This is important because the TeleopPeriodic method executes something like once every 10ms.
		 *	Thus, this if-else if pair make the button a toggle.
		 */
		if(driver_stick.GetRawButton(THUMB) && !inverting)
		{
			std::cout << "Inverting Drive Train.";
			left_drive.SetInverted(!left_drive.GetInverted());
			right_drive.SetInverted(!right_drive.GetInverted());
			inverting = true;
		}
		else if(!driver_stick.GetRawButton(THUMB))
		{
			inverting = false;
		}
		
		
		/*
		 * 	Unlike the previous actions, this method does need to be called every itteration of
		 *	TeleopPeriodic.  This is because the logic running this operation needs to be checked
		 *	Every time the method is called.  This cannot be a loop in the Shoot method because
		 *  that would lock the robot every time the trigger is hit.
		 */
		if(operator_stick.GetRawButton(TRIGGER))
		{
			shooting = true;
			shooter.Shoot();
		}
		else if(shooting  && !operator_stick.GetRawButton(TRIGGER))
		{
			shooting = false;
			shooter.StopShooter();
		}
		
		if(!arming && driver_stick.GetRawButton(RAMP_RAISE))
		{
			arming = true;
			arms.Set(1);
		}
		else if(!arming && driver_stick.GetRawButton(RAMP_LOWER))
		{
			arming = true;
			arms.Set(-1);
		}
		else if(arming && !driver_stick.GetRawButton(RAMP_RAISE) && !driver_stick.GetRawButton(RAMP_LOWER))
		{
			arming = false;
			arms.Set(0);
		}

        // This code will become obsolete after the Shooter logic is complete.
		float opThrottle = SaneThrottle(operator_stick.GetThrottle());
		
		if(!pickupRunning && ( opThrottle > shooter_power + DEADZONE_RADIUS
				||	opThrottle < shooter_power - DEADZONE_RADIUS))
		{
			shooter.SetPower(opThrottle);
		}
	}

	/**
	 *	Takes the gross raw throttle input from joystick and returns a 
	 *	value between 0.0-1.0 (no negative values)
	 */
	float SaneThrottle(float rawThrottle)
	{
		return ((1.0 - rawThrottle) / 2.0);
	}

	void TestPeriodic()
	{
		lw->Run();
	}
};

START_ROBOT_CLASS(Robot)