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

#ifndef SRC_SHOOTER_H_
#define SRC_SHOOTER_H_

#define PICKUP_POWER 0.8
#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 = (RAMP_LOWER_DURATION / 2),  // 1 rotation?
	Down = RAMP_LOWER_DURATION,   // 2 rotations?
	Uncalibrated = -1,
	Transitioning = -2
};

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);
		launcher = s1;
		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 launcher;
		delete pickup;
		delete ramp;
	}

	void PickUp(bool state = true) {
		pickup->Set((float) (state * PICKUP_POWER));
		std::cout << "picking up!\n";
	}

	void SetRamp(RampState state) {
		// TODO:
		// Move the Ramp to the set position.
		switch (state)
		{
			case Shoot:
			{
				if (ramp->GetForwardLimitOK())
				{
					ramp->Set(1);
				} else
				{
					ramp->Set(0);
				}
			}
			case Half:
			{
				//yeah, put something here when you get the encoder working.
				std::cout << "Hey! Didja install an encoder? Because this is placeholder that Aidan would have removed if you had installed one and told him about it.\n";
			}
			case Down:
			{
				//see half.
				std::cout << "Hey! Didja install an encoder? Because this is placeholder that Aidan would have removed if you had installed one and told him about it.\n";
			}
		}
	}

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

	void SetPower(float power) {
		pickup->Set(power);
		launcher->Set(power);
		std::cout << "setting shooter power" << std::endl;
	}
private:

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

	RampState rampState;
	RampState targetState;
	RampState previousState;
};

#endif /* SRC_SHOOTER_H_ */