#include "WPILib.h"
//#include "TankDrive.h"
#include "Shooter.h"
#include <ctime>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <sys/time.h>
#include <vector>
#include <cmath>


/**
 * 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.
 */


#ifndef BUTTON_LAYOUT
#define BUTTON_LAYOUT

#define TRIGGER 1
#define THUMB 2

#endif  // BUTTON_LAYOUT

class Robot: public IterativeRobot {
	CANTalon r1_drive;
	CANTalon r2_drive;
	CANTalon l1_drive;
	CANTalon l2_drive;
	CANTalon shooter1;
	CANTalon shooter2;
	CANTalon ramp;
	TankDrive drive;
//	Counter ramp_lift;
	RobotDrive drive;
	Shooter shooter;
	Joystick rstick, lstick;

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

	void LogData()
	{
		static PowerDistributionPanel pdp;
		static DriverStation* ds =
	}

public:
	Robot() :
			r1_drive(1), 	// right wheel 1
			r2_drive(2), 	// right wheel 2
			l1_drive(3), 	// left wheel 1
			l2_drive(4), 	// left wheel 2
			shooter1(10),  	// shooter drive 1
			shooter2(11),   // shooter drive 2
			ramp(12),
			drive(			// initialize RobotDrive object.
					&l1_drive, &r1_drive),
			shooter(		// initialize Shooter object.
					&shooter1, &shooter2, &ramp),
			rstick(0), 		// right stick (operator)
			lstick(1)//,		// left stick (driver)
			//ramp_lift(1)	// counter for the hall sensor on the
	{

	}

	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() {
		r1_drive.Enable();
		r2_drive.Enable();
		l1_drive.Enable();
		l2_drive.Enable();

	}

	void TeleopPeriodic() {
		drive.ArcadeDrive(&lstick);
		l2_drive.Set(l1_drive.Get());
		r2_drive.Set(r1_drive.Get());


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

		shooter.SetPower(power);
		if(rstick.GetRawButton(TRIGGER))
		{
			// SHOOT THE BALL
			ramp.Set(-1);
		}

		if(rstick.GetRawButton(THUMB))
		{
			// lower the ramp
			ramp.Set(1);
		}
		else
		{
			// raise the ramp to HALF (so it isn't down all the time!)
			ramp.Set(0);
		}

		// How to pickup?
	}

};

START_ROBOT_CLASS(Robot)