/**
 * 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.Joystick;

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

	/**
	 * Represents an analog axis on an Xbox 360 controller.
	 */
	public static enum Axis {
		LX(0), LY(1), /** left trigger */ LT(2),
		RX(4), RY(5), /** right trigger */ RT(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),
		/** left bumper */ LB(4), /** right bumper */RB( 5),
		Back(6), Start(7), /*Home(8),*/
		/** left thumb */ LT(8), /** right thumb */ RT(9);

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

	/* Constructor **********************************************/

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

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

	/**
	 * 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(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(Button button) {
		return getRawButton(button.id);
	}


	/* The actual code is done.  The rest is boilerplate.  See,
	 * this is why Java is terrible.  Just 30% of the file
	 * actually doing useful stuff, the rest just filling
	 * interfaces. */
	/* Stupid little wrappers ***********************************/

	/**
	 * Get the X value of a thumb-stick.
	 *
	 * @param hand Left stick or right?
	 * @return The X value of the joystick.
	 */
	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 joystick.
	 */
	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.
	 */
	public double getZ(final Hand hand) {
		if (hand.value == Hand.kLeft.value)
			return getAxis(Axis.LT);
		if (hand.value == Hand.kRight.value)
			return getAxis(Axis.RT);
		return 0.0;
	}

	/**
	 * Get the state of a bumper.
	 *
	 * @param hand Left trigger or right?
	 * @return the state of the bumper.
	 */
	public boolean getBumper(Hand hand) {
		if (hand.value == Hand.kLeft.value)
			return getButton(Button.LB);
		if (hand.value == Hand.kRight.value)
			return getButton(Button.RB);
		return false;
	}

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

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

	/**
	 * Get the magnitude of the direction vector formed by the thumb-stick's
	 * current position relative to its origin
	 *
	 * @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));
	}

	public double getMagnitude() {
		return getMagnitude(Hand.kRight);
	}

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

	public double getDirectionRadians() {
		return getDirectionRadians(Hand.kRight);
	}

	/**
	 * 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++
	 *
	 * @return The direction of the vector in degrees
	 */
	public double getDirectionDegrees(Hand hand) {
		return Math.toDegrees(getDirectionRadians(hand));
	}

	public double getDirectionDegrees() {
		return Math.toDegrees(getDirectionRadians(Hand.kRight));
	}


	/* Unused wrappers for GenericHID/Joystick ******************/

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

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

	/**
	 * This method is only here to complete the Joystick interface.
	 *
	 * @param axis unused
	 * @return Always 0
	 */
	public int getAxisChannel(AxisType axis) {
		return 0;
	}

	/**
	 * This method is only here to complete the Joystick interface.
	 *
	 * @param axis unused
	 * @param channel unused
	 */
	public void setAxisChannel(AxisType axis, int channel) {}
}