#include "WPILib.h"
#include "TankDrive.h"

/**
 * This sample shows how to use the new CANTalon to just run a motor in a basic
 *   throttle mode, in the same manner as you might control a traditional PWM
 *   controlled motor.
 *
 */
class Robot : public IterativeRobot {
	CANTalon r1_drive;
	CANTalon r2_drive;
	CANTalon l1_drive;
	CANTalon l2_drive;
	CANTalon shooter1;
	CANTalon shooter2;
	RobotDrive drivetrain1;
	RobotDrive drivetrain2;

  Joystick rstick, lstick;

	// update every 0.01 seconds/10 milliseconds.
  // The talon only receives control packets every 10ms.
	//double kUpdatePeriod = 0.010;

public:
 Robot()
     : r1_drive(1),   // Initialize the Talon as device 1. Use the roboRIO web
	   r2_drive(2),             // interface to change the device number on the talons.
	   l1_drive(3),
	   l2_drive(4),
	   shooter1(10),
	   shooter2(11),
	   drivetrain1(l1_drive, r1_drive),
	   drivetrain2(l2_drive, r2_drive),
       rstick(0),
	   lstick(1)
 {
	 printf("Initializing Robot...");
	 printf("Initializing TankDrive");
	 //drive = new TankDrive(&l1_drive, &l2_drive, &r1_drive, &r2_drive);
	 printf("Ready!");
	 //l1_drive.SetInverted(true);
	 //l2_drive.SetInverted(true);
	 //l1_drive.Set(0, 0);
	 //l2_drive.Set(0, 0);
	 //r1_drive.Set(0, 1);
	 //r2_drive.Set(0, 1);
 }

 	 void DisabledInit()
 	 {
 		 r1_drive.SetSafetyEnabled(false);
 		 r2_drive.SetSafetyEnabled(false);
 		 l1_drive.SetSafetyEnabled(false);
 		 l2_drive.SetSafetyEnabled(false);
 		 shooter1.SetSafetyEnabled(false);
 		 shooter2.SetSafetyEnabled(false);
 	 }

 	 void TeleopInit()
 	 {
 		printf("Starting Teleop mode");
 		r1_drive.Enable();
 		r2_drive.Enable();
 		l1_drive.Enable();
 		l2_drive.Enable();



 	 }

 	 void TeleopPeriodic()
 	 {
 		 printf("Teleop Periodic Start.");
			float speed = lstick.GetY() * ((1.0 - (lstick.GetThrottle()))/2.0);
			float rot = lstick.GetX();

			drivetrain1.ArcadeDrive(speed, rot, false);
			drivetrain2.ArcadeDrive(speed, rot, false);

			float power = (1.0 - rstick.GetThrottle()) / 2.0;
			shooter1.Set(power);
			shooter2.Set(power);

			printf("~Teleop Periodic End.");


 	 }
	/**
	 * Runs the motor from the output of a Joystick.
	 *
	void OperatorControl() {
		while (IsOperatorControl() && IsEnabled()) {
			// Do Drive.
			std::cerr << "In Operator Control";
			drive->Drive(&m_stick);
			Wait(kUpdatePeriod); // Wait a bit so that the loop doesn't lock everything up.
		}
	}*/

	//TankDrive *drive;
};

START_ROBOT_CLASS(Robot)