/*
 * TankDrive.h
 *
 *  Created on: Jan 28, 2016
 *      Author: Jason
 */

#ifndef SRC_TANKDRIVE_H_
#define SRC_TANKDRIVE_H_

#ifndef DEADZONE_RADIUS
#define DEADZONE_RADIUS 0.05
#endif // DEADZONE_RADIUS

#include "WPILib.h"

/**
 * Encapsulates two RobotDrive objects and keeps them synced by sending
 * identical ArcadeDrive calls to each.  Also handles massaging of Joystick
 * data to smooth out drive operations.
 *
 * TODO: Make this reflect what this class actually does.
 */
class TankDrive {

public:
	TankDrive(CANTalon *l1, CANTalon *l2, CANTalon* r1, CANTalon *r2) {
		dt1 = new RobotDrive(l1, r1);

		CANTalon* left1 = l1;
		CANTalon* right1 = r1;
		CANTalon* left2 = l2;
		CANTalon* right2 = r2;

		//dt2 = new RobotDrive(l2, r2);
	}
	virtual ~TankDrive() {
		delete dt1;
	//	delete dt2;
	}

	/**
	 * Calls ArcadeDrive on the two RobotDrive objects for the TankDrive.
	 * Uses #defined DEADZONE_RADIUS to keep the robot from freaking out.
	 * Some math on the "rot" variable could make the driving smoother, I think.
	 */
	void Drive(Joystick *js) {
		/*float x = js->GetX();
		float y = js->GetY();
		float th = -((1.0 - (js->GetThrottle()))
				/ 2.0);

		// set deadzone
		if (x > -DEADZONE_RADIUS && x < DEADZONE_RADIUS) {
			x = 0;
		}
		if (y > -DEADZONE_RADIUS && y < DEADZONE_RADIUS) {
			y = 0;
		}

		float speed = y * th;
		// TODO: do some math here to smooth out turning?
		float rot = x * th;*/
		dt1->ArcadeDrive(js);
		left2.Set(left1.Get());
		right2.Set(right1.Get());



	//	dt2->ArcadeDrive(js);
/*
		dt1->ArcadeDrive(speed, rot, false);
		dt2->ArcadeDrive(speed, rot, false);*/
	}

private:
	RobotDrive *dt1/*, *dt2*/;

	CANTalon l1, right1, left2, right2;
};

#endif /* SRC_TANKDRIVE_H_ */