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

package edu.wpi.first.wpilibj;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.IntBuffer;

import edu.wpi.first.wpilibj.MotorSafety;
import edu.wpi.first.wpilibj.MotorSafetyHelper;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.can.CANExceptionFactory;
import edu.wpi.first.wpilibj.can.CANJaguarVersionException;
import edu.wpi.first.wpilibj.can.CANMessageNotFoundException;
import edu.wpi.first.wpilibj.can.CANJNI;
import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType;
import edu.wpi.first.wpilibj.communication.UsageReporting;
import edu.wpi.first.wpilibj.hal.HALLibrary;
import edu.wpi.first.wpilibj.hal.HALUtil;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
import edu.wpi.first.wpilibj.tables.ITable;
import edu.wpi.first.wpilibj.tables.ITableListener;

public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveWindowSendable {

	public static final int kMaxMessageDataSize = 8; // XXX: Where does this come from?

    // The internal PID control loop in the Jaguar runs at 1kHz.
    public static final int kControllerRate = 1000;
    public static final double kApproxBusVoltage = 12.0;

	private MotorSafetyHelper m_safetyHelper;

	private static final int kFullMessageIDMask = CANJNI.CAN_MSGID_API_M | CANJNI.CAN_MSGID_MFR_M | CANJNI.CAN_MSGID_DTYPE_M;
	private static final int kSendMessagePeriod = 20;

	// Control Mode tags
	private static class EncoderTag {};
	public final static EncoderTag kEncoder = new EncoderTag();
	private static class QuadEncoderTag {};
	public final static QuadEncoderTag kQuadEncoder = new QuadEncoderTag();
	private static class PotentiometerTag {};
	public final static PotentiometerTag kPotentiometer = new PotentiometerTag();


    /**
     * Mode determines how the Jaguar is controlled, used internally
     */
    private enum ControlMode {
		PercentVbus, Current, Speed, Position, Voltage;
	}

    /**
     * Faults reported by the Jaguar
     */
	public static final int kCurrentFault = 1;
	public static final int kTemperatureFault = 2;
	public static final int kBusVoltageFault = 4;
	public static final int kGateDriverFault = 8;

    /**
     * Limit switch masks
     */
	public static final int kForwardLimit = 1;
	public static final int kReverseLimit = 2;

    /**
     * Determines how the Jaguar behaves when sending a zero signal.
     */
    public enum NeutralMode {
		Jumper((byte)0), Brake((byte)1), Coast((byte)2);

		public byte value;

		public static NeutralMode valueOf(byte value) {
			for(NeutralMode mode : values()) {
				if(mode.value == value) {
					return mode;
				}
			}

			return null;
		}

		private NeutralMode(byte value) {
			this.value = value;
		}
    }

    /**
     * Determines which sensor to use for position reference.
     */
    public enum LimitMode {
		SwitchInputsOnly((byte)0), SoftPositionLimits((byte)1);

		public byte value;

		public static LimitMode valueOf(byte value) {
			for(LimitMode mode : values()) {
				if(mode.value == value) {
					return mode;
				}
			}

			return null;
		}

		private LimitMode(byte value) {
			this.value = value;
		}
    }

	public CANJaguar(int deviceNumber, ControlMode controlMode) throws CANMessageNotFoundException {
		m_deviceNumber = (byte)deviceNumber;
		m_controlMode = controlMode;

		m_safetyHelper = new MotorSafetyHelper(this);

		byte[] data = new byte[8];

		// Request all status data periodically
		requestMessage(CANJNI.LM_API_STATUS_VOLTBUS, kSendMessagePeriod);
		requestMessage(CANJNI.LM_API_STATUS_VOUT, kSendMessagePeriod);
		requestMessage(CANJNI.LM_API_STATUS_CURRENT, kSendMessagePeriod);
		requestMessage(CANJNI.LM_API_STATUS_TEMP, kSendMessagePeriod);
		requestMessage(CANJNI.LM_API_STATUS_POS, kSendMessagePeriod);
		requestMessage(CANJNI.LM_API_STATUS_SPD, kSendMessagePeriod);
		requestMessage(CANJNI.LM_API_STATUS_LIMIT, kSendMessagePeriod);
		requestMessage(CANJNI.LM_API_STATUS_FAULT, kSendMessagePeriod);
		requestMessage(CANJNI.LM_API_STATUS_POWER, kSendMessagePeriod);

		// Request firmware and hardware version only once
		requestMessage(CANJNI.CAN_IS_FRAME_REMOTE | CANJNI.CAN_MSGID_API_FIRMVER);
		requestMessage(CANJNI.LM_API_HWVER);

		// Wait until we've gotten all of the status data at least once.
		for(int i = 0; i < kReceiveStatusAttempts; i++)
		{
			Timer.delay(0.001);
			
			getBusVoltage();
			getOutputVoltage();
			getOutputCurrent();
			getTemperature();
			getPosition();
			getSpeed();
			getForwardLimitOK();
			getFaults();

			if(m_receivedBusVoltage &&
				m_receivedOutputVoltage &&
				m_receivedOutputCurrent &&
				m_receivedTemperature &&
				m_receivedPosition &&
				m_receivedSpeed &&
				m_receivedLimits &&
				m_receivedFaults)
			{
				break;
			}
		}

		// Don't catch the CANMessageNotFoundException - we need to know the
		// firmware version to continue.
		getMessage(CANJNI.CAN_MSGID_API_FIRMVER, CANJNI.CAN_MSGID_FULL_M, data);
		m_firmwareVersion = unpackINT32(data);

		try {
			getMessage(CANJNI.LM_API_HWVER, CANJNI.CAN_MSGID_FULL_M, data);
			m_hardwareVersion = data[0];
		} catch(CANMessageNotFoundException e) {
			// Not all Jaguar firmware reports a hardware version.
			m_hardwareVersion = 0;
		}
	}

	public CANJaguar(int deviceNumber) {
		this(deviceNumber, ControlMode.PercentVbus);
	}

	/**
	 * Cancel periodic messages to the Jaguar, effectively disabling it.
	 * No other methods should be called after this is called.
	 */
	public void free() {
		m_safetyHelper = null;

		ByteBuffer status = ByteBuffer.allocateDirect(4);
		status.order(ByteOrder.LITTLE_ENDIAN);
		status.asIntBuffer().put(0, 0);

		int messageID ;

		// Disable periodic setpoints
		switch(m_controlMode) {
		case PercentVbus:
			messageID = m_deviceNumber | CANJNI.LM_API_VOLT_T_SET;
			break;

		case Speed:
			messageID = m_deviceNumber | CANJNI.LM_API_SPD_T_SET;
			break;

		case Position:
			messageID = m_deviceNumber | CANJNI.LM_API_POS_T_SET;
			break;

		case Current:
			messageID = m_deviceNumber | CANJNI.LM_API_ICTRL_T_SET;
			break;

		case Voltage:
			messageID = m_deviceNumber | CANJNI.LM_API_VCOMP_T_SET;
			break;

		default:
			return;
		}

		CANJNI.FRCNetworkCommunicationCANSessionMuxSendMessage(messageID, null,
			CANJNI.CAN_SEND_PERIOD_STOP_REPEATING, status.asIntBuffer());
	}

	/**
	 * Get the recently set outputValue setpoint.
	 *
	 * The scale and the units depend on the mode the Jaguar is in.
	 * In PercentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM Jaguar).
	 * In Voltage Mode, the outputValue is in Volts.
	 * In Current Mode, the outputValue is in Amps.
	 * In Speed Mode, the outputValue is in Rotations/Minute.
	 * In Position Mode, the outputValue is in Rotations.
	 *
	 * @return The most recently set outputValue setpoint.
	 */
	public double get() {
		return m_value;
	}

	/**
	 * set the output set-point value.
	 *
	 * The scale and the units depend on the mode the Jaguar is in.
	 * In PercentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM Jaguar).
	 * In Voltage Mode, the outputValue is in Volts.
	 * In Current Mode, the outputValue is in Amps.
	 * In Speed Mode, the outputValue is in Rotations/Minute.
	 * In Position Mode, the outputValue is in Rotations.
	 *
	 * @param outputValue The set-point to sent to the motor controller.
	 * @param syncGroup The update group to add this set() to, pending UpdateSyncGroup().  If 0, update immediately.
	 */
	public void set(double outputValue, byte syncGroup) {
		int messageID;
		byte[] data = new byte[8];
		byte dataSize;

		switch(m_controlMode) {
		case PercentVbus:
			messageID = CANJNI.LM_API_VOLT_T_SET;
			dataSize = packPercentage(data, outputValue);
			break;

		case Speed:
			messageID = CANJNI.LM_API_SPD_T_SET;
			dataSize = packFXP16_16(data, outputValue);
			break;

		case Position:
			messageID = CANJNI.LM_API_POS_T_SET;
			dataSize = packFXP16_16(data, outputValue);
			break;

		case Current:
			messageID = CANJNI.LM_API_ICTRL_T_SET;
			dataSize = packFXP8_8(data, outputValue);
			break;


		case Voltage:
			messageID = CANJNI.LM_API_VCOMP_T_SET;
			dataSize = packFXP8_8(data, outputValue);
			break;

		default:
			return;
		}

		if(syncGroup != 0) {
			data[dataSize++] = syncGroup;
		}

		sendMessage(messageID, data, dataSize, kSendMessagePeriod);

		if(m_safetyHelper != null) m_safetyHelper.feed();

		m_value = outputValue;

		verify();
	}

	public void set(double value) {
		set(value, (byte)0);
	}

	/**
	 * Check all unverified params and make sure they're equal to their local
	 * cached versions. If a value isn't available, it gets requested.  If a value
	 * doesn't match up, it gets set again.
	 */
	protected void verify() {
		byte[] data = new byte[8];

		// If the Jaguar lost power, everything should be considered unverified
		try {
			getMessage(CANJNI.LM_API_STATUS_POWER, CANJNI.CAN_MSGID_FULL_M, data);
			boolean powerCycled = data[0] != 0;

			if(powerCycled) {
				// Clear the power cycled bit
				data[0] = 1;
				sendMessage(CANJNI.LM_API_STATUS_POWER, data, 1);

				// set all configuration data again.  This will resend it to
				// Jaguar and mark everything as unverified.
				enableControl();
				setSpeedReference(m_speedReference);
				setPositionReference(m_positionReference);
				configNeutralMode(m_neutralMode);
				configEncoderCodesPerRev(m_encoderCodesPerRev);
				configPotentiometerTurns(m_potentiometerTurns);

				switch(m_controlMode) {

				}

				if(m_controlMode == ControlMode.Current ||
						m_controlMode == ControlMode.Speed ||
						m_controlMode == ControlMode.Position) {
					setPID(m_p, m_i, m_d);
				}

				if(m_limitMode == LimitMode.SoftPositionLimits) {
					configSoftPositionLimits(m_forwardLimit, m_reverseLimit);
				}

				configMaxOutputVoltage(m_maxOutputVoltage);

				if(m_controlMode == ControlMode.Voltage ||
						m_controlMode == ControlMode.PercentVbus) {
					setVoltageRampRate(m_voltageRampRate);
				}

				requestMessage(CANJNI.LM_API_STATUS_VOLTBUS, kSendMessagePeriod);
				requestMessage(CANJNI.LM_API_STATUS_VOUT, kSendMessagePeriod);
				requestMessage(CANJNI.LM_API_STATUS_CURRENT, kSendMessagePeriod);
				requestMessage(CANJNI.LM_API_STATUS_TEMP, kSendMessagePeriod);
				requestMessage(CANJNI.LM_API_STATUS_POS, kSendMessagePeriod);
				requestMessage(CANJNI.LM_API_STATUS_SPD, kSendMessagePeriod);
				requestMessage(CANJNI.LM_API_STATUS_LIMIT, kSendMessagePeriod);
				requestMessage(CANJNI.LM_API_STATUS_FAULT, kSendMessagePeriod);
				requestMessage(CANJNI.LM_API_STATUS_POWER, kSendMessagePeriod);
			}
		} catch(CANMessageNotFoundException e) {}


		// Verify that any recently set parameters are correct
		if(!m_controlModeVerified) {
			try {
				getMessage(CANJNI.LM_API_STATUS_CMODE, CANJNI.CAN_MSGID_FULL_M, data);

				ControlMode mode = ControlMode.values()[data[0]];

				if(m_controlMode == mode) {
					m_controlModeVerified = true;
				} else {
					// Enable control again to resend the control mode
					enableControl();
				}
			} catch(CANMessageNotFoundException e) {
				// Verification is needed but not available - request it again.
				requestMessage(CANJNI.LM_API_STATUS_CMODE);
			}
		}

		if(!m_speedRefVerified) {
			try {
				getMessage(CANJNI.LM_API_SPD_REF, CANJNI.CAN_MSGID_FULL_M, data);

				int speedRef = data[0];

				if(m_speedReference == speedRef) {
					m_speedRefVerified = true;
				} else {
					// It's wrong - set it again
					setSpeedReference(m_speedReference);
				}
			} catch(CANMessageNotFoundException e) {
					// Verification is needed but not available - request it again.
					requestMessage(CANJNI.LM_API_SPD_REF);
			}
		}

		if(!m_posRefVerified) {
			try {
				getMessage(CANJNI.LM_API_POS_REF, CANJNI.CAN_MSGID_FULL_M, data);

				int posRef = data[0];

				if(m_positionReference == posRef) {
					m_posRefVerified = true;
				} else {
					// It's wrong - set it again
					setPositionReference(m_positionReference);
				}
			} catch(CANMessageNotFoundException e) {
					// Verification is needed but not available - request it again.
					requestMessage(CANJNI.LM_API_POS_REF);
			}
		}

		if(!m_pVerified) {
			int message = 0;

			switch(m_controlMode) {
				case Speed:
					message = CANJNI.LM_API_SPD_PC;
					break;

				case Position:
					message = CANJNI.LM_API_POS_PC;
					break;

				case Current:
					message = CANJNI.LM_API_ICTRL_PC;
					break;

				default:
					break;
			}

			try {
				getMessage(message, CANJNI.CAN_MSGID_FULL_M, data);

				double p = unpackFXP16_16(data);

				if(FXP16_EQ(m_p, p)) {
					m_pVerified = true;
				} else {
					// It's wrong - set it again
					setP(m_p);
				}
			} catch(CANMessageNotFoundException e) {
					// Verification is needed but not available - request it again.
					requestMessage(message);
			}
		}

		if(!m_iVerified) {
			int message = 0;

			switch(m_controlMode) {
				case Speed:
					message = CANJNI.LM_API_SPD_IC;
					break;

				case Position:
					message = CANJNI.LM_API_POS_IC;
					break;

				case Current:
					message = CANJNI.LM_API_ICTRL_IC;
					break;

				default:
					break;
			}

			try {
				getMessage(message, CANJNI.CAN_MSGID_FULL_M, data);

				double i = unpackFXP16_16(data);

				if(FXP16_EQ(m_i, i)) {
					m_iVerified = true;
				} else {
					// It's wrong - set it again
					setI(m_p);
				}
			} catch(CANMessageNotFoundException e) {
					// Verification is needed but not available - request it again.
					requestMessage(message);
			}
		}

		if(!m_dVerified) {
			int message = 0;

			switch(m_controlMode) {
				case Speed:
					message = CANJNI.LM_API_SPD_DC;
					break;

				case Position:
					message = CANJNI.LM_API_POS_DC;
					break;

				case Current:
					message = CANJNI.LM_API_ICTRL_DC;
					break;

				default:
					break;
			}

			try {
				getMessage(message, CANJNI.CAN_MSGID_FULL_M, data);

				double d = unpackFXP16_16(data);

				if(FXP16_EQ(m_d, d)) {
					m_dVerified = true;
				} else {
					// It's wrong - set it again
					setD(m_d);
				}
			} catch(CANMessageNotFoundException e) {
					// Verification is needed but not available - request it again.
					requestMessage(message);
			}
		}

		if(!m_neutralModeVerified) {
			try {
				getMessage(CANJNI.LM_API_CFG_BRAKE_COAST, CANJNI.CAN_MSGID_FULL_M, data);

				NeutralMode mode = NeutralMode.valueOf(data[0]);

				if(mode == m_neutralMode) {
					m_neutralModeVerified = true;
				} else {
					//It's wrong - set it again
					configNeutralMode(m_neutralMode);
				}
			} catch(CANMessageNotFoundException e) {
				// Verification is needed but not available - request it again.
				requestMessage(CANJNI.LM_API_CFG_BRAKE_COAST);
			}
		}

		if(!m_encoderCodesPerRevVerified) {
			try {
				getMessage(CANJNI.LM_API_CFG_ENC_LINES, CANJNI.CAN_MSGID_FULL_M, data);

				short codes = unpackINT16(data);

				if(codes == m_encoderCodesPerRev) {
					m_encoderCodesPerRevVerified = true;
				} else {
					//It's wrong - set it again
					configEncoderCodesPerRev(m_encoderCodesPerRev);
				}
			} catch(CANMessageNotFoundException e) {
				// Verification is needed but not available - request it again.
				requestMessage(CANJNI.LM_API_CFG_ENC_LINES);
			}
		}

		if(!m_potentiometerTurnsVerified) {
			try {
				getMessage(CANJNI.LM_API_CFG_POT_TURNS, CANJNI.CAN_MSGID_FULL_M, data);

				short turns = unpackINT16(data);

				if(turns == m_potentiometerTurns) {
					m_potentiometerTurnsVerified = true;
				} else {
					//It's wrong - set it again
					configPotentiometerTurns(m_potentiometerTurns);
				}
			} catch(CANMessageNotFoundException e) {
				// Verification is needed but not available - request it again.
				requestMessage(CANJNI.LM_API_CFG_POT_TURNS);
			}
		}

		if(!m_limitModeVerified) {
			try {
				getMessage(CANJNI.LM_API_CFG_LIMIT_MODE, CANJNI.CAN_MSGID_FULL_M, data);

				LimitMode mode = LimitMode.valueOf(data[0]);

				if(mode == m_limitMode) {
					m_limitModeVerified = true;
				} else {
					//It's wrong - set it again
					configLimitMode(m_limitMode);
				}
			} catch(CANMessageNotFoundException e) {
				// Verification is needed but not available - request it again.
				requestMessage(CANJNI.LM_API_CFG_LIMIT_MODE);
			}
		}

		if(!m_forwardLimitVerified) {
			try {
				getMessage(CANJNI.LM_API_CFG_LIMIT_FWD, CANJNI.CAN_MSGID_FULL_M, data);

				double limit = unpackFXP16_16(data);

				if(FXP16_EQ(limit, m_forwardLimit)) {
					m_forwardLimitVerified = true;
				} else {
					//It's wrong - set it again
					configForwardLimit(m_forwardLimit);
				}
			} catch(CANMessageNotFoundException e) {
				// Verification is needed but not available - request it again.
				requestMessage(CANJNI.LM_API_CFG_LIMIT_FWD);
			}
		}

		if(!m_reverseLimitVerified) {
			try {
				getMessage(CANJNI.LM_API_CFG_LIMIT_REV, CANJNI.CAN_MSGID_FULL_M, data);

				double limit = unpackFXP16_16(data);

				if(FXP16_EQ(limit, m_reverseLimit)) {
					m_reverseLimitVerified = true;
				} else {
					//It's wrong - set it again
					configReverseLimit(m_reverseLimit);
				}
			} catch(CANMessageNotFoundException e) {
				// Verification is needed but not available - request it again.
				requestMessage(CANJNI.LM_API_CFG_LIMIT_REV);
			}
		}

		if(!m_maxOutputVoltageVerified) {
			try {
				getMessage(CANJNI.LM_API_CFG_MAX_VOUT, CANJNI.CAN_MSGID_FULL_M, data);

				double voltage = unpackFXP8_8(data);

				if(FXP8_EQ(voltage, m_maxOutputVoltage)) {
					m_maxOutputVoltageVerified = true;
				} else {
					// It's wrong - set it again
					configMaxOutputVoltage(m_maxOutputVoltage);
				}

			} catch(CANMessageNotFoundException e) {
				// Verification is needed but not available - request it again.
				requestMessage(CANJNI.LM_API_CFG_MAX_VOUT);
			}
		}

		if(!m_voltageRampRateVerified) {
			if(m_controlMode == ControlMode.PercentVbus) {
				try {
					getMessage(CANJNI.LM_API_VOLT_SET_RAMP, CANJNI.CAN_MSGID_FULL_M, data);

					double rate = unpackPercentage(data);

					if(FXP16_EQ(rate, m_voltageRampRate)) {
						m_voltageRampRateVerified = true;
					} else {
						// It's wrong - set it again
						setVoltageRampRate(m_voltageRampRate);
					}

				} catch(CANMessageNotFoundException e) {
					// Verification is needed but not available - request it again.
					requestMessage(CANJNI.LM_API_VOLT_SET_RAMP);
				}
			}
		} else if(m_controlMode == ControlMode.Voltage) {
			try {
				getMessage(CANJNI.LM_API_VCOMP_IN_RAMP, CANJNI.CAN_MSGID_FULL_M, data);

				double rate = unpackFXP8_8(data);

				if(FXP8_EQ(rate, m_voltageRampRate)) {
					m_voltageRampRateVerified = true;
				} else {
					// It's wrong - set it again
					setVoltageRampRate(m_voltageRampRate);
				}

			} catch(CANMessageNotFoundException e) {
				// Verification is needed but not available - request it again.
				requestMessage(CANJNI.LM_API_VCOMP_IN_RAMP);
			}
		}
	}

	/**
	 * Common interface for disabling a motor.
	 *
	 * @deprecated Call DisableControl instead.
	 */
	@Override
	public void disable() {
		disableControl();
	}

	// PIDOutput interface
	public void pidWrite(double output) {
		if (m_controlMode == ControlMode.PercentVbus) {
			set(output);
		} else {
			throw new IllegalStateException("PID only supported in PercentVbus mode");
		}
	}

	/**
	 * set the reference source device for speed controller mode.
	 *
	 * Choose encoder as the source of speed feedback when in speed control mode.
	 *
	 * @param reference Specify a speed reference.
	 */
	private void setSpeedReference(int reference) {
		sendMessage(CANJNI.LM_API_SPD_REF, new byte[] { (byte)reference }, 1);

		m_speedReference = reference;
		m_speedRefVerified = false;
	}

	/**
	 * Get the reference source device for speed controller mode.
	 *
	 * @return A speed reference indicating the currently selected reference
	 * device for speed controller mode.
	 */
	private int getSpeedReference() {
		return m_speedReference;
	}

	/**
	 * set the reference source device for position controller mode.
	 *
	 * Choose between using and encoder and using a potentiometer
	 * as the source of position feedback when in position control mode.
	 *
	 * @param reference Specify a position reference.
	 */
	private void setPositionReference(int reference) {
		sendMessage(CANJNI.LM_API_POS_REF, new byte[] { (byte)reference }, 1);

		m_positionReference = reference;
		m_posRefVerified = false;
	}

	/**
	 * Get the reference source device for position controller mode.
	 *
	 * @return A PositionReference indicating the currently selected reference
	 * device for position controller mode.
	 */
	private int getPositionReference() {
		return m_positionReference;
	}

	/**
	 * set the P constant for the closed loop modes.
	 *
	 * @param p The proportional gain of the Jaguar's PID controller.
	 */
	public void setP(double p) {
		byte[] data = new byte[8];
		byte dataSize = packFXP16_16(data, p);

		switch(m_controlMode) {
		case Speed:
			sendMessage(CANJNI.LM_API_SPD_PC, data, dataSize);
			break;

		case Position:
			sendMessage(CANJNI.LM_API_POS_PC, data, dataSize);
			break;

		case Current:
			sendMessage(CANJNI.LM_API_ICTRL_PC, data, dataSize);
			break;

		default:
			throw new IllegalStateException("PID constants only apply in Speed, Position, and Current mode");
		}

		m_p = p;
		m_pVerified = false;
	}

	/**
	 * set the I constant for the closed loop modes.
	 *
	 * @param i The integral gain of the Jaguar's PID controller.
	 */
	public void setI(double i) {
		byte[] data = new byte[8];
		byte dataSize = packFXP16_16(data, i);

		switch(m_controlMode) {
		case Speed:
			sendMessage(CANJNI.LM_API_SPD_IC, data, dataSize);
			break;

		case Position:
			sendMessage(CANJNI.LM_API_POS_IC, data, dataSize);
			break;

		case Current:
			sendMessage(CANJNI.LM_API_ICTRL_IC, data, dataSize);
			break;

		default:
			throw new IllegalStateException("PID constants only apply in Speed, Position, and Current mode");
		}

		m_i = i;
		m_iVerified = false;
	}

	/**
	 * set the D constant for the closed loop modes.
	 *
	 * @param d The derivative gain of the Jaguar's PID controller.
	 */
	public void setD(double d) {
		byte[] data = new byte[8];
		byte dataSize = packFXP16_16(data, d);

		switch(m_controlMode) {
		case Speed:
			sendMessage(CANJNI.LM_API_SPD_DC, data, dataSize);
			break;

		case Position:
			sendMessage(CANJNI.LM_API_POS_DC, data, dataSize);
			break;

		case Current:
			sendMessage(CANJNI.LM_API_ICTRL_DC, data, dataSize);
			break;

		default:
			throw new IllegalStateException("PID constants only apply in Speed, Position, and Current mode");
		}

		m_d = d;
		m_dVerified = false;
	}

	/**
	 * set the P, I, and D constants for the closed loop modes.
	 *
	 * @param p The proportional gain of the Jaguar's PID controller.
	 * @param i The integral gain of the Jaguar's PID controller.
	 * @param d The differential gain of the Jaguar's PID controller.
	 */
	public void setPID(double p, double i, double d) {
		setP(p);
		setI(i);
		setD(d);
	}

	/**
	 * Get the Proportional gain of the controller.
	 *
	 * @return The proportional gain.
	 */
	public double getP() {
		return m_p;
	}

	/**
	 * Get the Integral gain of the controller.
	 *
	 * @return The integral gain.
	 */
	public double getI() {
		return m_i;
	}

	/**
	 * Get the Derivative gain of the controller.
	 *
	 * @return The derivative gain.
	 */
	public double getD() {
		return m_d;
	}

	/**
	 * Enable the closed loop controller.
	 *
	 * Start actually controlling the output based on the feedback.
	 * If starting a position controller with an encoder reference,
	 * use the encoderInitialPosition parameter to initialize the
	 * encoder state.
	 *
	 * @param encoderInitialPosition Encoder position to set if position with encoder reference.  Ignored otherwise.
	 */
	public void enableControl(double encoderInitialPosition) {
		switch(m_controlMode) {
		case PercentVbus:
			sendMessage(CANJNI.LM_API_VOLT_T_EN, new byte[0], 0);
			break;

		case Speed:
			sendMessage(CANJNI.LM_API_SPD_T_EN, new byte[0], 0);
			break;

		case Position:
			byte[] data = new byte[8];
			int dataSize = packFXP16_16(data, encoderInitialPosition);
			sendMessage(CANJNI.LM_API_POS_T_EN, data, dataSize);
			break;

		case Current:
			sendMessage(CANJNI.LM_API_ICTRL_T_EN, new byte[0], 0);
			break;

		case Voltage:
			sendMessage(CANJNI.LM_API_VCOMP_T_EN, new byte[0], 0);
			break;
		}
	}

	/**
	 * Enable the closed loop controller.
	 *
	 * Start actually controlling the output based on the feedback.
	 */
	public void enableControl() {
		enableControl(0.0);
	}

	/**
	 * Disable the closed loop controller.
	 *
	 * Stop driving the output based on the feedback.
	 */
	public void disableControl() {
		switch(m_controlMode) {
		case PercentVbus:
			sendMessage(CANJNI.LM_API_VOLT_DIS, new byte[0], 0);
			break;

		case Speed:
			sendMessage(CANJNI.LM_API_SPD_DIS, new byte[0], 0);
			break;

		case Position:
			sendMessage(CANJNI.LM_API_POS_DIS, new byte[0], 0);
			break;

		case Current:
			sendMessage(CANJNI.LM_API_ICTRL_DIS, new byte[0], 0);
			break;

		case Voltage:
			sendMessage(CANJNI.LM_API_VCOMP_DIS, new byte[0], 0);
			break;
		}
	}

	/**
	 * Enable controlling the motor voltage as a percentage of the bus voltage
	 * without any position or speed feedback.
	 */
	public void setPercentMode()
	{
		changeControlMode(ControlMode.PercentVbus);
		setPositionReference(CANJNI.LM_REF_NONE);
		setSpeedReference(CANJNI.LM_REF_NONE);
	}

	/**
	 * Enable controlling the motor voltage as a percentage of the bus voltage,
	 * and enable speed sensing from a non-quadrature encoder.
	 *
	 * @param tag The constant CANJaguar.Encoder
	 * @param codesPerRev The counts per revolution on the encoder
	 */
	public void setPercentMode(EncoderTag tag, int codesPerRev)
	{
		changeControlMode(ControlMode.PercentVbus);
		setPositionReference(CANJNI.LM_REF_NONE);
		setSpeedReference(CANJNI.LM_REF_ENCODER);
		configEncoderCodesPerRev(codesPerRev);
	}

	/**
	 * Enable controlling the motor voltage as a percentage of the bus voltage,
	 * and enable position and speed sensing from a quadrature encoder
	 *
	 * @param tag The constant CANJaguar.QuadEncoder
	 * @param codesPerRev The counts per revolution on the encoder
	 */
	public void setPercentMode(QuadEncoderTag tag, int codesPerRev)
	{
		changeControlMode(ControlMode.PercentVbus);
		setPositionReference(CANJNI.LM_REF_QUAD_ENCODER);
		setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER);
		configEncoderCodesPerRev(codesPerRev);
	}

	/**
	 * Enable controlling the motor voltage as a percentage of the bus voltage,
	 * and enable position sensing from a potentiometer and no speed feedback.
	 *
	 * @param tag The constant CANJaguar.Potentiometer
	 */
	public void setPercentMode(PotentiometerTag tag)
	{
		changeControlMode(ControlMode.PercentVbus);
		setPositionReference(CANJNI.LM_REF_POT);
		setSpeedReference(CANJNI.LM_REF_NONE);
		configPotentiometerTurns(1);
	}

	/**
	 * Enable controlling the motor current with a PID loop.
	 *
	 * @param p The proportional gain of the Jaguar's PID controller.
	 * @param i The integral gain of the Jaguar's PID controller.
	 * @param d The differential gain of the Jaguar's PID controller.
	 */
	public void setCurrentMode(double p, double i, double d)
	{
		changeControlMode(ControlMode.Current);
		setPositionReference(CANJNI.LM_REF_NONE);
		setSpeedReference(CANJNI.LM_REF_NONE);
		setPID(p, i, d);
	}

	/**
	 * Enable controlling the motor current with a PID loop, and enable speed
	 * sensing from a non-quadrature encoder.
	 *
	 * @param tag The constant CANJaguar.Encoder
	 * @param p The proportional gain of the Jaguar's PID controller.
	 * @param i The integral gain of the Jaguar's PID controller.
	 * @param d The differential gain of the Jaguar's PID controller.
	 */
	public void setCurrentMode(EncoderTag tag, int codesPerRev, double p, double i, double d)
	{
		changeControlMode(ControlMode.Current);
		setPositionReference(CANJNI.LM_REF_NONE);
		setSpeedReference(CANJNI.LM_REF_NONE);
		configEncoderCodesPerRev(codesPerRev);
		setPID(p, i, d);
	}

	/**
	 * Enable controlling the motor current with a PID loop, and enable speed and
	 * position sensing from a quadrature encoder.
	 *
	 * @param endoer The constant CANJaguar.QuadEncoder
	 * @param p The proportional gain of the Jaguar's PID controller.
	 * @param i The integral gain of the Jaguar's PID controller.
	 * @param d The differential gain of the Jaguar's PID controller.
	 */
	public void setCurrentMode(QuadEncoderTag tag, int codesPerRev, double p, double i, double d)
	{
		changeControlMode(ControlMode.Current);
		setPositionReference(CANJNI.LM_REF_QUAD_ENCODER);
		setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER);
		configEncoderCodesPerRev(codesPerRev);
		setPID(p, i, d);
	}

	/**
	 * Enable controlling the motor current with a PID loop, and enable position
	 * sensing from a potentiometer.
	 *
	 * @param tag The constant CANJaguar.Potentiometer
	 * @param p The proportional gain of the Jaguar's PID controller.
	 * @param i The integral gain of the Jaguar's PID controller.
	 * @param d The differential gain of the Jaguar's PID controller.
	 */
	public void setCurrentMode(PotentiometerTag tag, double p, double i, double d)
	{
		changeControlMode(ControlMode.Current);
		setPositionReference(CANJNI.LM_REF_POT);
		setSpeedReference(CANJNI.LM_REF_NONE);
		configPotentiometerTurns(1);
		setPID(p, i, d);
	}

	/**
	 * Enable controlling the speed with a feedback loop from a non-quadrature
	 * encoder.
	 *
	 * @param tag The constant CANJaguar.Encoder
	 * @param codesPerRev The counts per revolution on the encoder
	 * @param p The proportional gain of the Jaguar's PID controller.
	 * @param i The integral gain of the Jaguar's PID controller.
	 * @param d The differential gain of the Jaguar's PID controller.
	 */
	public void setSpeedMode(EncoderTag tag, int codesPerRev, double p, double i, double d)
	{
		changeControlMode(ControlMode.Speed);
		setPositionReference(CANJNI.LM_REF_NONE);
		setSpeedReference(CANJNI.LM_REF_ENCODER);
		configEncoderCodesPerRev(codesPerRev);
		setPID(p, i, d);
	}

	/**
	 * Enable controlling the speed with a feedback loop from a quadrature encoder.
	 *
	 * @param tag The constant CANJaguar.QuadEncoder
	 * @param codesPerRev The counts per revolution on the encoder
	 * @param p The proportional gain of the Jaguar's PID controller.
	 * @param i The integral gain of the Jaguar's PID controller.
	 * @param d The differential gain of the Jaguar's PID controller.
	 */
	public void setSpeedMode(QuadEncoderTag tag, int codesPerRev, double p, double i, double d)
	{
		changeControlMode(ControlMode.Speed);
		setPositionReference(CANJNI.LM_REF_QUAD_ENCODER);
		setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER);
		configEncoderCodesPerRev(codesPerRev);
		setPID(p, i, d);
	}

	/**
	 * Enable controlling the position with a feedback loop using an encoder
	 *
	 * @param tag The constant CANJaguar.QuadEncoder
	 *
	 */
	public void setPositionMode(QuadEncoderTag tag, int codesPerRev, double p, double i, double d)
	{
		changeControlMode(ControlMode.Position);
		setPositionReference(CANJNI.LM_REF_QUAD_ENCODER);
		configEncoderCodesPerRev(codesPerRev);
		setPID(p, i, d);
	}

	/**
	 * Enable controlling the position with a feedback loop using a potentiometer
	 */
	public void setPositionMode(PotentiometerTag tag, double p, double i, double d)
	{
		changeControlMode(ControlMode.Position);
		setPositionReference(CANJNI.LM_REF_POT);
		configPotentiometerTurns(1);
		setPID(p, i, d);
	}

	/**
	 * Enable controlling the motor voltage without any position or speed feedback.
	 */
	public void setVoltageMode()
	{
		changeControlMode(ControlMode.Voltage);
		setPositionReference(CANJNI.LM_REF_NONE);
		setSpeedReference(CANJNI.LM_REF_NONE);
	}

	/**
	 * Enable controlling the motor voltage with speed feedback from a
	 * non-quadrature encoder and no position feedback.
	 *
	 * @param tag The constant CANJaguar.Encoder
	 * @param codesPerRev The counts per revolution on the encoder
	 */
	public void setVoltageMode(EncoderTag tag, int codesPerRev)
	{
		changeControlMode(ControlMode.Voltage);
		setPositionReference(CANJNI.LM_REF_NONE);
		setSpeedReference(CANJNI.LM_REF_ENCODER);
		configEncoderCodesPerRev(codesPerRev);
	}

	/**
	 * Enable controlling the motor voltage with position and speed feedback from a
	 * quadrature encoder
	 *
	 * @param tag The constant CANJaguar.QuadEncoder
	 * @param codesPerRev The counts per revolution on the encoder
	 */
	public void setVoltageMode(QuadEncoderTag tag, int codesPerRev)
	{
		changeControlMode(ControlMode.Voltage);
		setPositionReference(CANJNI.LM_REF_QUAD_ENCODER);
		setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER);
		configEncoderCodesPerRev(codesPerRev);
	}

	/**
	 * Enable controlling the motor voltage with position feedback from a
	 * potentiometer and no speed feedback.
	 *
	 * @param tag The constant CANJaguar.Potentiometer
	 */
	public void setVoltageMode(PotentiometerTag tag)
	{
		changeControlMode(ControlMode.Voltage);
		setPositionReference(CANJNI.LM_REF_POT);
		setSpeedReference(CANJNI.LM_REF_NONE);
		configPotentiometerTurns(1);
	}

	/**
	 * Change the control mode of this Jaguar object.
	 *
	 * After changing modes, configure any PID constants or other settings needed
	 * and then EnableControl() to actually change the mode on the Jaguar.
	 *
	 * @param controlMode The new mode.
	 */
	private void changeControlMode(ControlMode controlMode) {
		// Disable the previous mode
		disableControl();

		// Update the local mode
		m_controlMode = controlMode;
	}

	/**
	 * Get the active control mode from the Jaguar.
	 *
	 * Ask the Jag what mode it is in.
	 *
	 * @return ControlMode that the Jag is in.
	 */
	private ControlMode getControlMode() {
		return m_controlMode;
	}

	/**
	 * Get the voltage at the battery input terminals of the Jaguar.
	 *
	 * @return The bus voltage in Volts.
	 */
	public double getBusVoltage() {
		byte[] data = new byte[8];

		try {
			getMessage(CANJNI.LM_API_STATUS_VOLTBUS, CANJNI.CAN_MSGID_FULL_M, data);
			m_receivedBusVoltage = true;
			m_busVoltage = unpackFXP8_8(data);
		} catch(CANMessageNotFoundException e) {}

		return m_busVoltage;
	}

	/**
	 * Get the voltage being output from the motor terminals of the Jaguar.
	 *
	 * @return The output voltage in Volts.
	 */
	public double getOutputVoltage() {
		byte[] data = new byte[8];

		try {
			getMessage(CANJNI.LM_API_STATUS_VOUT, CANJNI.CAN_MSGID_FULL_M, data);
			m_receivedOutputVoltage = true;
			m_outputVoltage = unpackFXP8_8(data);
		} catch(CANMessageNotFoundException e) {}

		return m_outputVoltage;
	}

	/**
	 * Get the current through the motor terminals of the Jaguar.
	 *
	 * @return The output current in Amps.
	 */
	public double getOutputCurrent() {
		byte[] data = new byte[8];

		try {
			getMessage(CANJNI.LM_API_STATUS_CURRENT, CANJNI.CAN_MSGID_FULL_M, data);
			m_receivedOutputCurrent = true;
			m_outputCurrent = unpackFXP8_8(data);
		} catch(CANMessageNotFoundException e) {}

		return m_outputCurrent;
	}

	/**
	 * Get the internal temperature of the Jaguar.
	 *
	 * @return The temperature of the Jaguar in degrees Celsius.
	 */
	public double getTemperature() {
		byte[] data = new byte[8];

		try {
			getMessage(CANJNI.LM_API_STATUS_TEMP, CANJNI.CAN_MSGID_FULL_M, data);
			m_receivedTemperature = true;
			m_temperature = unpackFXP8_8(data);
		} catch(CANMessageNotFoundException e) {}

		return m_temperature;
	}

	/**
	 * Get the position of the encoder or potentiometer.
	 *
	 * @return The position of the motor in rotations based on the configured feedback.
	 */
	public double getPosition() {
		byte[] data = new byte[8];

		try {
			getMessage(CANJNI.LM_API_STATUS_POS, CANJNI.CAN_MSGID_FULL_M, data);
			m_receivedPosition = true;
			m_position = unpackFXP16_16(data);
		} catch(CANMessageNotFoundException e) {}

		return m_position;
	}

	/**
	 * Get the speed of the encoder.
	 *
	 * @return The speed of the motor in RPM based on the configured feedback.
	 */
	public double getSpeed() {
		byte[] data = new byte[8];

		try {
			getMessage(CANJNI.LM_API_STATUS_SPD, CANJNI.CAN_MSGID_FULL_M, data);
			m_receivedSpeed = true;
			m_speed = unpackFXP16_16(data);
		} catch(CANMessageNotFoundException e) {}

		return m_speed;
	}

	/**
	 * Get the status of the forward limit switch.
	 *
	 * @return The motor is allowed to turn in the forward direction when true.
	 */
	public boolean getForwardLimitOK() {
		byte[] data = new byte[8];

		try {
			getMessage(CANJNI.LM_API_STATUS_LIMIT, CANJNI.CAN_MSGID_FULL_M, data);
			m_receivedLimits = true;
			m_limits = data[0];
		} catch(CANMessageNotFoundException e) {}

		return (m_limits & kForwardLimit) != 0;
	}

	/**
	 * Get the status of the reverse limit switch.
	 *
	 * @return The motor is allowed to turn in the reverse direction when true.
	 */
	public boolean getReverseLimitOK() {
		byte[] data = new byte[8];

		try {
			getMessage(CANJNI.LM_API_STATUS_LIMIT, CANJNI.CAN_MSGID_FULL_M, data);
			m_receivedLimits = true;
			m_limits = data[0];
		} catch(CANMessageNotFoundException e) {}

		return (m_limits & kReverseLimit) != 0;
	}

	/**
	 * Get the status of any faults the Jaguar has detected.
	 *
	 * @return A bit-mask of faults defined by the "Faults" constants.
	 * @see ControlMode.CurrentFault
	 * @see kBusVoltageFault
	 * @see kTemperatureFault
	 * @see kGateDriverFault
	 */
	public short getFaults() {
		byte[] data = new byte[8];

		try {
			getMessage(CANJNI.LM_API_STATUS_FAULT, CANJNI.CAN_MSGID_FULL_M, data);
			m_receivedFaults = true;
			m_faults = unpackINT16(data);
		} catch(CANMessageNotFoundException e) {}

		return m_faults;
	}

	/**
	 * set the maximum voltage change rate.
	 *
	 * When in PercentVbus or Voltage output mode, the rate at which the voltage changes can
	 * be limited to reduce current spikes.  set this to 0.0 to disable rate limiting.
	 *
	 * @param rampRate The maximum rate of voltage change in Percent Voltage mode in V/s.
	 */
	public void setVoltageRampRate(double rampRate) {
		byte[] data = new byte[8];
		int dataSize;
		int message;

		switch(m_controlMode) {
		case PercentVbus:
			dataSize = packPercentage(data, rampRate / (m_maxOutputVoltage * kControllerRate));
			message = CANJNI.LM_API_VOLT_SET_RAMP;
			break;
		case Voltage:
			dataSize = packFXP8_8(data, rampRate / kControllerRate);
			message = CANJNI.LM_API_VCOMP_IN_RAMP;
			break;
		default:
			throw new IllegalStateException("Voltage ramp rate only applies in Percentage and Voltage modes");
		}

		sendMessage(message, data, dataSize);
	}

	/**
	 * Get the version of the firmware running on the Jaguar.
	 *
	 * @return The firmware version.  0 if the device did not respond.
	 */
	public int getFirmwareVersion() {
		return m_firmwareVersion;
	}

	/**
	 * Get the version of the Jaguar hardware.
	 *
	 * @return The hardware version. 1: Jaguar,  2: Black Jaguar
	 */
	public byte getHardwareVersion() {
		return m_hardwareVersion;
	}

	/**
	 * Configure what the controller does to the H-Bridge when neutral (not driving the output).
	 *
	 * This allows you to override the jumper configuration for brake or coast.
	 *
	 * @param mode Select to use the jumper setting or to override it to coast or brake.
	 */
	public void configNeutralMode(NeutralMode mode) {
		sendMessage(CANJNI.LM_API_CFG_BRAKE_COAST, new byte[] { mode.value }, 1);

		m_neutralMode = mode;
		m_neutralModeVerified = false;
	}

	/**
	 * Configure how many codes per revolution are generated by your encoder.
	 *
	 * @param codesPerRev The number of counts per revolution in 1X mode.
	 */
	public void configEncoderCodesPerRev(int codesPerRev) {
		byte[] data = new byte[8];

		int dataSize = packINT16(data, (short)codesPerRev);
		sendMessage(CANJNI.LM_API_CFG_ENC_LINES, data, dataSize);

		m_encoderCodesPerRev = (short)codesPerRev;
		m_encoderCodesPerRevVerified = false;
	}

	/**
	 * Configure the number of turns on the potentiometer.
	 *
	 * There is no special support for continuous turn potentiometers.
	 * Only integer numbers of turns are supported.
	 *
	 * @param turns The number of turns of the potentiometer
	 */
	public void configPotentiometerTurns(int turns) {
		byte[] data = new byte[8];

		int dataSize = packINT16(data, (short)turns);
		sendMessage(CANJNI.LM_API_CFG_POT_TURNS, data, dataSize);

		m_potentiometerTurns = (short)turns;
		m_potentiometerTurnsVerified = false;
	}

	/**
	 * Configure Soft Position Limits when in Position Controller mode.
	 *
	 * When controlling position, you can add additional limits on top of the limit switch inputs
	 * that are based on the position feedback.  If the position limit is reached or the
	 * switch is opened, that direction will be disabled.
	 *

	 * @param forwardLimitPosition The position that if exceeded will disable the forward direction.
	 * @param reverseLimitPosition The position that if exceeded will disable the reverse direction.
	 */
	public void configSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition) {
		configLimitMode(LimitMode.SoftPositionLimits);
		configForwardLimit(forwardLimitPosition);
		configReverseLimit(reverseLimitPosition);
	}

	/**
	 * Disable Soft Position Limits if previously enabled.
	 *
	 * Soft Position Limits are disabled by default.
	 */
	public void disableSoftPositionLimits() {
		configLimitMode(LimitMode.SwitchInputsOnly);
	}

	/**
	 * set the limit mode for position control mode.
	 *
	 * Use ConfigSoftPositionLimits or DisableSoftPositionLimits to set this
	 * automatically.
	 */
	public void configLimitMode(LimitMode mode) {
		sendMessage(CANJNI.LM_API_CFG_LIMIT_MODE, new byte[] { mode.value }, 1);
	}

	/**
	 * set the position that if exceeded will disable the forward direction.
	 *
	 * Use ConfigSoftPositionLimits to set this and the limit mode automatically.
	 */
	public void configForwardLimit(double forwardLimitPosition) {
		byte[] data = new byte[8];

		int dataSize = packFXP16_16(data, forwardLimitPosition);
		data[dataSize++] = 1;
		sendMessage(CANJNI.LM_API_CFG_LIMIT_FWD, data, dataSize);

		m_forwardLimit = forwardLimitPosition;
		m_forwardLimitVerified = false;
	}

	/**
	 * set the position that if exceeded will disable the reverse direction.
	 *
	 * Use ConfigSoftPositionLimits to set this and the limit mode automatically.
	 */
	public void configReverseLimit(double reverseLimitPosition) {
		byte[] data = new byte[8];

		int dataSize = packFXP16_16(data, reverseLimitPosition);
		data[dataSize++] = 1;
		sendMessage(CANJNI.LM_API_CFG_LIMIT_REV, data, dataSize);

		m_reverseLimit = reverseLimitPosition;
		m_reverseLimitVerified = false;
	}

	/**
	 * Configure the maximum voltage that the Jaguar will ever output.
	 *
	 * This can be used to limit the maximum output voltage in all modes so that
	 * motors which cannot withstand full bus voltage can be used safely.
	 *
	 * @param voltage The maximum voltage output by the Jaguar.
	 */
	public void configMaxOutputVoltage(double voltage) {
		byte[] data = new byte[8];

		int dataSize = packFXP8_8(data, voltage);
		sendMessage(CANJNI.LM_API_CFG_MAX_VOUT, data, dataSize);

		m_maxOutputVoltage = voltage;
		m_maxOutputVoltageVerified = false;
	}

	/**
	 * Configure how long the Jaguar waits in the case of a fault before resuming operation.
	 *
	 * Faults include over temerature, over current, and bus under voltage.
	 * The default is 3.0 seconds, but can be reduced to as low as 0.5 seconds.
	 *
	 * @param faultTime The time to wait before resuming operation, in seconds.
	 */
	public void configFaultTime(float faultTime) {
		byte[] data = new byte[8];

		int dataSize = packINT16(data, (short)(faultTime * 1000.0));
		sendMessage(CANJNI.LM_API_CFG_FAULT_TIME, data, dataSize);

		m_faultTime = faultTime;
		m_faultTimeVerified = false;
	}

	byte m_deviceNumber;
	double m_value = 0.0f;

	// Parameters/configuration
	ControlMode m_controlMode;
	int m_speedReference = CANJNI.LM_REF_NONE;
	int m_positionReference = CANJNI.LM_REF_NONE;
	double m_p = 0.0;
	double m_i = 0.0;
	double m_d = 0.0;
	NeutralMode m_neutralMode = NeutralMode.Jumper;
	short m_encoderCodesPerRev = 0;
	short m_potentiometerTurns = 0;
	LimitMode m_limitMode = LimitMode.SwitchInputsOnly;
	double m_forwardLimit = 0.0;
	double m_reverseLimit = 0.0;
	double m_maxOutputVoltage = 30.0;
	double m_voltageRampRate = 0.0;
	float m_faultTime = 0.0f;

	// Which parameters have been verified since they were last set?
	boolean m_controlModeVerified = true;
	boolean m_speedRefVerified = true;
	boolean m_posRefVerified = true;
	boolean m_pVerified = true;
	boolean m_iVerified = true;
	boolean m_dVerified = true;
	boolean m_neutralModeVerified = true;
	boolean m_encoderCodesPerRevVerified = true;
	boolean m_potentiometerTurnsVerified = true;
	boolean m_forwardLimitVerified = true;
	boolean m_reverseLimitVerified = true;
	boolean m_limitModeVerified = true;
	boolean m_maxOutputVoltageVerified = true;
	boolean m_voltageRampRateVerified = true;
	boolean m_faultTimeVerified = true;

	// Status data
	double m_busVoltage = 0.0f;
	double m_outputVoltage = 0.0f;
	double m_outputCurrent = 0.0f;
	double m_temperature = 0.0f;
	double m_position = 0.0;
	double m_speed = 0.0;
	byte m_limits = (byte)0;
	short m_faults = (short)0;
	int m_firmwareVersion = 0;
	byte m_hardwareVersion = (byte)0;

	// Which status values have we received at least once?
	boolean m_receivedBusVoltage = false;
	boolean m_receivedOutputVoltage = false;
	boolean m_receivedOutputCurrent = false;
	boolean m_receivedTemperature = false;
	boolean m_receivedPosition = false;
	boolean m_receivedSpeed = false;
	boolean m_receivedLimits = false;
	boolean m_receivedFaults = false;

	static final int kReceiveStatusAttempts = 50;

	static void sendMessageHelper(int messageID, byte[] data, int dataSize, int period) throws CANMessageNotFoundException {
		final int[] kTrustedMessages = {
				CANJNI.LM_API_VOLT_T_EN, CANJNI.LM_API_VOLT_T_SET, CANJNI.LM_API_SPD_T_EN, CANJNI.LM_API_SPD_T_SET,
				CANJNI.LM_API_VCOMP_T_EN, CANJNI.LM_API_VCOMP_T_SET, CANJNI.LM_API_POS_T_EN, CANJNI.LM_API_POS_T_SET,
				CANJNI.LM_API_ICTRL_T_EN, CANJNI.LM_API_ICTRL_T_SET
		};

		ByteBuffer status = ByteBuffer.allocateDirect(4);
		status.order(ByteOrder.LITTLE_ENDIAN);
		status.asIntBuffer().put(0, 0);

		for(byte i = 0; i < kTrustedMessages.length; i++) {
			if((kFullMessageIDMask & messageID) == kTrustedMessages[i])
			{
				// Make sure the data will still fit after adjusting for the token.
				if (dataSize > kMaxMessageDataSize - 2) {
					throw new RuntimeException("CAN message has too much data.");
				}

				ByteBuffer trustedBuffer = ByteBuffer.allocateDirect(dataSize+2);
				trustedBuffer.put(0, (byte)0);
				trustedBuffer.put(1, (byte)0);

				for(byte j = 0; j < dataSize; j++) {
					trustedBuffer.put(j+2, data[j]);
				}

				CANJNI.FRCNetworkCommunicationCANSessionMuxSendMessage(messageID, trustedBuffer, period, status.asIntBuffer());
				int statusCode = status.asIntBuffer().get(0);
				if(statusCode < 0) {
					CANExceptionFactory.checkStatus(statusCode, messageID);
				}

				return;
			}
		}

		// Use a null pointer for the data buffer if the given array is null
		ByteBuffer buffer;
		if(data != null) {
			buffer = ByteBuffer.allocateDirect(dataSize);
			for(byte i = 0; i < dataSize; i++) {
				buffer.put(i, data[i]);
			}
		} else {
			buffer = null;
		}

		CANJNI.FRCNetworkCommunicationCANSessionMuxSendMessage(messageID, buffer, period, status.asIntBuffer());

		int statusCode = status.asIntBuffer().get(0);
		if(statusCode < 0) {
			CANExceptionFactory.checkStatus(statusCode, messageID);
		}
	}

	/**
	 * Send a message to the Jaguar.
	 *
	 * @param messageID The messageID to be used on the CAN bus (device number
	 * 	is added internally)
	 * @param data The up to 8 bytes of data to be sent with the message
	 * @param dataSize Specify how much of the data in "data" to send
	 * @param periodic If positive, tell Network Communications to send the
	 * 	message every "period" milliseconds.
	 */
	protected void sendMessage(int messageID, byte[] data, int dataSize, int period) {
		sendMessageHelper(messageID | m_deviceNumber, data, dataSize, period);
	}

	/**
	 * Send a message to the Jaguar, non-periodly
	 *
	 * @param messageID The messageID to be used on the CAN bus (device number
	 * 	is added internally)
	 * @param data The up to 8 bytes of data to be sent with the message
	 * @param dataSize Specify how much of the data in "data" to send
	 */
	protected void sendMessage(int messageID, byte[] data, int dataSize) {
		sendMessage(messageID, data, dataSize, CANJNI.CAN_SEND_PERIOD_NO_REPEAT);
	}

	/**
	 * Request a message from the Jaguar, but don't wait for it to arrive.
	 *
	 * @param messageID The message to request
	 * @param periodic If positive, tell Network Communications to request the
	 * 	message every "period" milliseconds.
	 */
	protected void requestMessage(int messageID, int period) {
		sendMessageHelper(messageID | m_deviceNumber, null, 0, period);
	}

	/**
	 * Request a message from the Jaguar, but don't wait for it to arrive.
	 *
	 * @param messageID The message to request
	 */
	protected void requestMessage(int messageID) {
		requestMessage(messageID, CANJNI.CAN_SEND_PERIOD_NO_REPEAT);
	}

	/**
	 * Get a previously requested message.
	 *
	 * Jaguar always generates a message with the same message ID when replying.
	 *
	 * @param messageID The messageID to read from the CAN bus (device number is added internally)
	 * @param data The up to 8 bytes of data that was received with the message
	 *
	 * @throw CANMessageNotFoundException if there's not new message available
	 */
	protected void getMessage(int messageID, int messageMask, byte[] data) throws CANMessageNotFoundException {
		messageID |= m_deviceNumber;
		messageID &= CANJNI.CAN_MSGID_FULL_M;

		ByteBuffer targetedMessageID = ByteBuffer.allocateDirect(4);
		targetedMessageID.order(ByteOrder.LITTLE_ENDIAN);
		targetedMessageID.asIntBuffer().put(0, messageID);

		ByteBuffer timeStamp = ByteBuffer.allocateDirect(4);

		ByteBuffer status = ByteBuffer.allocateDirect(4);
		status.order(ByteOrder.LITTLE_ENDIAN);
		status.asIntBuffer().put(0, 0);

		// Get the data.
		ByteBuffer dataBuffer = CANJNI.FRCNetworkCommunicationCANSessionMuxReceiveMessage(
			targetedMessageID.asIntBuffer(),
			messageMask,
			timeStamp,
			status.asIntBuffer());

		if(data != null) {
			for(int i = 0; i < dataBuffer.capacity(); i++) {
				data[i] = dataBuffer.get(i);
			}
		}

		int statusCode = status.asIntBuffer().get(0);
		if(statusCode < 0) {
			CANExceptionFactory.checkStatus(statusCode, messageID);
		}
	}

    /**
     * Update all the motors that have pending sets in the syncGroup.
     *
     * @param syncGroup A bitmask of groups to generate synchronous output.
     */
    public static void updateSyncGroup(byte syncGroup) {
        byte[] data = new byte[8];

        data[0] = syncGroup;

		sendMessageHelper(CANJNI.CAN_MSGID_API_SYNC, data, 1, CANJNI.CAN_SEND_PERIOD_NO_REPEAT);
    }

	/* we are on ARM-LE now, not Freescale so no need to swap */
	private final static void swap16(int x, byte[] buffer) {
		buffer[0] = (byte)(x & 0xff);
		buffer[1] = (byte)((x>>8) & 0xff);
	}

	private final static void swap32(int x, byte[] buffer) {
		buffer[0] = (byte)(x & 0xff);
		buffer[1] = (byte)((x>>8) & 0xff);
		buffer[2] = (byte)((x>>16) & 0xff);
		buffer[3] = (byte)((x>>24) & 0xff);
	}

	private static final byte packPercentage(byte[] buffer, double value) {
		if(value < -1.0) value = -1.0;
		if(value > 1.0) value = 1.0;
		short intValue = (short) (value * 32767.0);
		swap16(intValue, buffer);
		return 2;
	}

	private static final byte packFXP8_8(byte[] buffer, double value) {
		short intValue = (short) (value * 256.0);
		swap16(intValue, buffer);
		return 2;
	}

	private static final byte packFXP16_16(byte[] buffer, double value) {
		int intValue = (int) (value * 65536.0);
		swap32(intValue, buffer);
		return 4;
	}

	private static final byte packINT16(byte[] buffer, short value) {
		swap16(value, buffer);
		return 2;
	}

	private static final byte packINT32(byte[] buffer, int value) {
		swap32(value, buffer);
		return 4;
	}

	/**
	 * Unpack 16-bit data from a buffer in little-endian byte order
	 * @param buffer The buffer to unpack from
	 * @param offset The offset into he buffer to unpack
	 * @return The data that was unpacked
	 */
	private static final short unpack16(byte[] buffer, int offset) {
		return (short) (((int) buffer[offset] & 0xFF) | (short) ((buffer[offset + 1] << 8)) & 0xFF00);
	}

	/**
	 * Unpack 32-bit data from a buffer in little-endian byte order
	 * @param buffer The buffer to unpack from
	 * @param offset The offset into he buffer to unpack
	 * @return The data that was unpacked
	 */
	private static final int unpack32(byte[] buffer, int offset) {
		return ((int) buffer[offset] & 0xFF) | ((buffer[offset + 1] << 8) & 0xFF00) |
			((buffer[offset + 2] << 16) & 0xFF0000) | ((buffer[offset + 3] << 24) & 0xFF000000);
	}

	private static final double unpackPercentage(byte[] buffer) {
		return unpack16(buffer,0) / 32767.0;
	}

	private static final double unpackFXP8_8(byte[] buffer) {
		return unpack16(buffer,0) / 256.0;
	}

	private static final double unpackFXP16_16(byte[] buffer) {
		return unpack32(buffer,0) / 65536.0;
	}

	private static final short unpackINT16(byte[] buffer) {
		return unpack16(buffer,0);
	}

	private static final int unpackINT32(byte[] buffer) {
		return unpack32(buffer,0);
	}

	/* Compare floats for equality as fixed point numbers */
	public boolean FXP8_EQ(double a, double b) {
		return (int)(a * 256.0) == (int)(b * 256.0);
	}

	/* Compare floats for equality as fixed point numbers */
	public boolean FXP16_EQ(double a, double b) {
		return (int)(a * 65536.0) == (int)(b * 65536.0);
	}

    public void setExpiration(double timeout) {
        m_safetyHelper.setExpiration(timeout);
    }

    public double getExpiration() {
        return m_safetyHelper.getExpiration();
    }

    public boolean isAlive() {
        return m_safetyHelper.isAlive();
    }

    public boolean isSafetyEnabled() {
        return m_safetyHelper.isSafetyEnabled();
    }

    public void setSafetyEnabled(boolean enabled) {
        m_safetyHelper.setSafetyEnabled(enabled);
    }

    public String getDescription() {
        return "CANJaguar ID "+m_deviceNumber;
    }

    /**
     * Common interface for stopping a motor.
     *
     * @deprecated Use disableControl instead.
     */
    public void stopMotor() {
        disableControl();
    }

    /*
     * Live Window code, only does anything if live window is activated.
     */
    public String getSmartDashboardType() {
        return "Speed Controller";
    }
    private ITable m_table = null;
    private ITableListener m_table_listener = null;

    /**
     * {@inheritDoc}
     */
    public void initTable(ITable subtable) {
        m_table = subtable;
        updateTable();
    }

    /**
     * {@inheritDoc}
     */
    public void updateTable() {
        if (m_table != null) {
            m_table.putNumber("Value", get());
        }
    }

    /**
     * {@inheritDoc}
     */
    public ITable getTable() {
        return m_table;
    }

    /**
     * {@inheritDoc}
     */
    public void startLiveWindowMode() {
        set(0); // Stop for safety
        m_table_listener = new ITableListener() {
            public void valueChanged(ITable itable, String key, Object value, boolean bln) {
                set(((Double) value).doubleValue());
            }
        };
        m_table.addTableListener("Value", m_table_listener, true);
    }

    /**
     * {@inheritDoc}
     */
    public void stopLiveWindowMode() {
        set(0); // Stop for safety
        // TODO: Broken, should only remove the listener from "Value" only.
        m_table.removeTableListener(m_table_listener);
    }
}
