#include "WPILib.h"
#include "Joystick.h"
#include "GenericHID.h"

class Robot: public IterativeRobot
{
private:
	RobotDrive drive;
	Joystick joystick1;

	TalonSRX *frontRight = new TalonSRX(0);
	TalonSRX *backLeft = new TalonSRX(1);
	Talon *frontLeft = new Talon(0);
	Talon *backRight = new Talon(1);


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


	Robot():     //error is being thrown here
		drive(frontLeft,backLeft,frontRight,backRight),
		joystick1(0),
		chooser()
	{
		drive.SetExpiration(0.1);
	}

	void RobotInit()
	{

		chooser = new SendableChooser();
		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
		chooser->AddObject(autoNameCustom, (void*)&autoNameCustom);
		SmartDashboard::PutData("Auto Modes", chooser);
	}


	/**
	 * 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()
	{
		if(autoSelected == autoNameCustom){
			//Custom Auto goes here
		} else {
			//Default Auto goes here
		}
	}

	void TeleopInit()
	{
		float strafe = joystick1.GetRawAxis(2) * -1 + joystick1.GetRawAxis(3);
		float Ymove = joystick1.GetRawAxis(1);
		float Rotate = joystick1.GetRawAxis(0);

		if(joystick1.GetRawButton(5) == true && joystick1.GetRawButton(6) == false)
		{
			drive.MecanumDrive_Cartesian(strafe / 2, Ymove / 2, Rotate / 2);
		}
		else if(joystick1.GetRawButton(5) == false && joystick1.GetRawButton(6) == true)
		{
			drive.MecanumDrive_Cartesian(strafe, Ymove, Rotate);
		}
		else
		{
			drive.MecanumDrive_Cartesian(0, 0, 0);
		}

	}

	void TeleopPeriodic()
	{

	}

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

START_ROBOT_CLASS(Robot)