/**
 * Copyright (c) FIRST 2008-2012. All Rights Reserved.
 * Open Source Software - may be modified and shared by FRC teams. The code
 * must be accompanied by the FIRST BSD license file in the root directory of
 * the project.
 *
 * Copyright (c) 2015 Luke Shumaker
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the FIRST nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * @author Luke Shumaker <lukeshu@sbcglobal.net>
 */
package org.usfirst.frc.team4272.robotlib;

import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;

/**
 * Handle input from a wired Xbox 360 controller connected to the
 * Driver Station.
 */
public class Xbox360Controller extends GenericHID {
	/* Constants ************************************************/

	/**
	 * Represents an analog axis on an Xbox 360 controller.
	 */
	public static enum Axis {
		LX(0), LY(1), LTrigger(2),
		RX(4), RY(5), RTrigger(3),
		/** D-Pad X */ DX(6), /** D-Pad Y */ DY(7);

		private final int id;
		private Axis(int id) { this.id = id; }
		public int getId() { return id; }
	}

	/**
	 * Represents a digital button on Xbox 360 controller.
	 */
	public static enum Button {
		A(0), B(1),
		X(2), Y(3),
		LBumper(4), RBumper( 5),
		Back(6), Start(7), /*Home(8),*/
		LThumb(8), RThumb(9);

		public final int id;
		private Button(int id) { this.id = id+1; }
	}

	/* Constructor **********************************************/
	private final Joystick joystick;

	/**
	 * Construct an instance of a joystick.
	 * The joystick index is the USB port on the Driver Station.
	 *
	 * @param port The port on the driver station that the joystick is plugged into
	 */
	public Xbox360Controller(final int port) {
		joystick = new Joystick(port);
	}

	/* Core functions *******************************************/

	/**
	 * Get the raw axis
	 *$
	 * @param axis Index of the axis
	 * @return The raw value of the selected axis
	 */
	@Override
	public double getRawAxis(int axis) {
		return joystick.getRawAxis(axis);
	}

	/**
	 * Is the given button pressed
	 *$
	 * @param button Index of the button
	 * @return True if the button is pressed
	 */
	@Override
	public boolean getRawButton(int button) {
		return joystick.getRawButton(button);
	}

	/**
	 * Get the value of an axis base on an enumerated type.
	 *
	 * @param axis The axis to read
	 * @return The value of the axis
	 */
	public double getAxis(final Axis axis) {
		return getRawAxis(axis.id);
	}

	/**
	 * Get buttons based on an enumerated type.
	 *
	 * @param button The button to read
	 * @return The state of the button
	 */
	public boolean getButton(final Button button) {
		return getRawButton(button.id);
	}

	/**
	 * Ask the Driver Station if this USB joystick is in fact an
	 * Xbox controller.
	 *
	 * @return Whether the controller is an Xbox controller
	 */
	public boolean getIsXbox() {
		return joystick.getIsXbox();
	}

	/* TODO: Outputs? (Rumble, LEDs) */

	/* Stupid little mathy wrappers *****************************/

	/**
	 * Get the magnitude of the direction vector formed by the thumb-stick's
	 * current position relative to its origin
	 *
	 * @param hand Left stick or right?
	 * @return The magnitude of the direction vector
	 */
	public double getMagnitude(Hand hand) {
		return Math.sqrt(Math.pow(getX(hand), 2) + Math.pow(getY(hand), 2));
	}

	/**
	 * Get the direction of the vector formed by the thumb-stick and its origin
	 * in radians
	 *
	 * @param hand Left stick or right?
	 * @return The direction of the vector in radians
	 */
	public double getDirectionRadians(Hand hand) {
		return Math.atan2(getX(hand), -getY(hand));
	}

	/**
	 * Get the direction of the vector formed by the thumb-stick and its origin
	 * in degrees
	 *
	 * uses acos(-1) to represent Pi due to absence of readily accessable Pi
	 * constant in C++
	 *
	 * @param hand Left stick or right?
	 * @return The direction of the vector in degrees
	 */
	public double getDirectionDegrees(Hand hand) {
		return Math.toDegrees(getDirectionRadians(hand));
	}

	/* Stupid aliases for getAxis/getButton *********************/

	/**
	 * Get the state of a trigger; whether it is mostly pressed or
	 * not.
	 *
	 * @param hand Left trigger or right?
	 * @return The state of the trigger.
	 */
	@Override
	public boolean getTrigger(Hand hand) {
		return getZ(hand) > 0.75;
	}

	/**
	 * Get the state of a bumper.
	 *
	 * @param hand Left bumper or right?
	 * @return Whether the bumper is pressed
	 * @deprecated This method is only here to complete the {@link GenericHID} abstract class.
	 */
	@Override
	public boolean getBumper(Hand hand) {
		if (hand.value == Hand.kLeft.value)
			return getButton(Button.LBumper);
		if (hand.value == Hand.kRight.value)
			return getButton(Button.RBumper);
		return false;
	}

	/**
	 * Get the state of a thumb-stick button.
	 *
	 * @param hand Left trigger or right?
	 * @return the state of the button.
	 */
	@Override
	public boolean getTop(Hand hand) {
		if (hand.value == Hand.kLeft.value)
			return getButton(Button.LThumb);
		if (hand.value == Hand.kRight.value)
			return getButton(Button.RThumb);
		return false;
	}

	/**
	 * Get the X value of a thumb-stick.
	 *
	 * @param hand Left stick or right?
	 * @return The X value of the thumb-stick.
	 */
	@Override
	public double getX(final Hand hand) {
		if (hand.value == Hand.kLeft.value)
			return getAxis(Axis.LX);
		if (hand.value == Hand.kRight.value)
			return getAxis(Axis.RX);
		return 0.0;
	}

	/**
	 * Get the Y value of a thumb-stick.

	 * @param hand Left stick or right?
	 * @return The Y value of the thumb-stick.
	 */
	@Override
	public double getY(final Hand hand) {
		if (hand.value == Hand.kLeft.value)
			return getAxis(Axis.LY);
		if (hand.value == Hand.kRight.value)
			return getAxis(Axis.RY);
		return 0.0;
	}

	/**
	 * Get the value of a trigger.
	 *
	 * @param hand Left trigger or right?
	 * @return The trigger value.
	 */
	@Override
	public double getZ(final Hand hand) {
		if (hand.value == Hand.kLeft.value)
			return getAxis(Axis.LTrigger);
		if (hand.value == Hand.kRight.value)
			return getAxis(Axis.RTrigger);
		return 0.0;
	}

	/* Stupid things I have to implement ************************/

	/**
	 * @return Always 0.0
	 * @deprecated This method is only here to complete the {@link GenericHID} abstract class.
	 */
	@Override
	public double getTwist() {
		return 0.0;
	}

	/**
	 * @return Always 0.0
	 * @deprecated This method is only here to complete the {@link GenericHID} abstract class.
	 */
	@Override
	public double getThrottle() {
		return 0.0;
	}

	/**
	 * @param pov Unused
	 * @return Always 0
	 * @deprecated This method is only here to complete the {@link GenericHID} abstract class.
	 */
	@Override
	public int getPOV(int pov) {
		return 0;
	}
}