/*
 * 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.
 */
class TankDrive {

public:
	TankDrive(CANTalon *l1, CANTalon *l2, CANTalon* r1, CANTalon *r2) {
		dt1 = new RobotDrive(l1, r1);
		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;
		float rot = x;  // do some math here to smooth out turning?

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

private:
	RobotDrive *dt1, *dt2;
};

#endif /* SRC_TANKDRIVE_H_ */