/*
 * Shooter.h
 *
 *  Created on: Feb 2, 2016
 *      Author: Jason
 */

#ifndef SRC_SHOOTER_H_
#define SRC_SHOOTER_H_

#define PICKUP_POWER 0.5
#define RAMP_LOWER_DURATION 2 //Rotations.


/**
 * You can use the values assigned to each of these values as the number
 * of rotations to run the motor down from the "Shoot" position.
 *
 * This might not be the best way to do it, and also requires that we
 * figure out how to read the Hall Effect signal from the motor.
 */
enum RampState {
	Shoot = 0, // 0 rotations
	Half = 1,  // 1 rotation?
	Down = 2,   // 2 rotations?
	Uncalibrated = -1
};

class Shooter {
public:
	/**
	 * Shooter talons and ramp talon.
	 * s2 is also for the pickup-mechanism and can be controlled independently.
	 *
	 */
	Shooter(CANTalon *s1, CANTalon *s2, CANTalon *r) {
		shooterDrive = new RobotDrive(s1, s2);
		pickup = s2;
		ramp = r;
		rampState = Uncalibrated;
	}

	/**
	 * Call this method on TeleopInit so that the ramp is properly
	 * set at the beginning of the match.
	 */
	RampState CalibrateRamp() {
		// TODO:
		// Raise ramp until limit switch is triggered,
		// then lower the ramp to its lower limit.

		return Down;
	}

	virtual ~Shooter() {
		delete shooterDrive;
		delete pickup;
		delete ramp;
	}

	void PickUp(bool state = true) {
		pickup->Set((float) (state * PICKUP_POWER));
	}

	void SetRamp(RampState state) {
		// TODO:
		// Move the Ramp to the set position.
	}

	/**
	 * Call this to run the pickup backwards if the ball gets jammed somehow...
	 */
	void Unjam()
	{
		pickup->Set(-1 * PICKUP_POWER);
	}

	void SetPower(float power) {
		shooterDrive->TankDrive(power, -power, false);
	}
private:

	RobotDrive *shooterDrive;
	CANTalon *pickup;
	CANTalon *ramp;

	RampState rampState;
};

#endif /* SRC_SHOOTER_H_ */