/*----------------------------------------------------------------------------*/
/* 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 edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType;
import edu.wpi.first.wpilibj.communication.UsageReporting;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
import edu.wpi.first.wpilibj.parsing.ISensor;
import edu.wpi.first.wpilibj.tables.ITable;

/**
 * Ultrasonic rangefinder class. The Ultrasonic rangefinder measures absolute
 * distance based on the round-trip time of a ping generated by the controller.
 * These sensors use two transducers, a speaker and a microphone both tuned to
 * the ultrasonic range. A common ultrasonic sensor, the Daventech SRF04
 * requires a short pulse to be generated on a digital channel. This causes the
 * chirp to be emmitted. A second line becomes high as the ping is transmitted
 * and goes low when the echo is received. The time that the line is high
 * determines the round trip distance (time of flight).
 */
public class Ultrasonic extends SensorBase implements PIDSource, ISensor,
		LiveWindowSendable {

	/**
	 * The units to return when PIDGet is called
	 */
	public static class Unit {

		/**
		 * The integer value representing this enumeration
		 */
		public final int value;
		static final int kInches_val = 0;
		static final int kMillimeters_val = 1;
		/**
		 * Use inches for PIDGet
		 */
		public static final Unit kInches = new Unit(kInches_val);
		/**
		 * Use millimeters for PIDGet
		 */
		public static final Unit kMillimeter = new Unit(kMillimeters_val);

		private Unit(int value) {
			this.value = value;
		}
	}

	private static final double kPingTime = 10 * 1e-6; // /< Time (sec) for the
														// ping trigger pulse.
	private static final int kPriority = 90; // /< Priority that the ultrasonic
												// round robin task runs.
	private static final double kMaxUltrasonicTime = 0.1; // /< Max time (ms)
															// between readings.
	private static final double kSpeedOfSoundInchesPerSec = 1130.0 * 12.0;
	private static Ultrasonic m_firstSensor = null; // head of the ultrasonic
													// sensor list
	private static boolean m_automaticEnabled = false; // automatic round robin
														// mode
	private DigitalInput m_echoChannel = null;
	private DigitalOutput m_pingChannel = null;
	private boolean m_allocatedChannels;
	private boolean m_enabled = false;
	private Counter m_counter = null;
	private Ultrasonic m_nextSensor = null;
	private static Thread m_task = null; // task doing the round-robin automatic
											// sensing
	private Unit m_units;
	private static int m_instances = 0;

	/**
	 * Background task that goes through the list of ultrasonic sensors and
	 * pings each one in turn. The counter is configured to read the timing of
	 * the returned echo pulse.
	 *
	 * DANGER WILL ROBINSON, DANGER WILL ROBINSON: This code runs as a task and
	 * assumes that none of the ultrasonic sensors will change while it's
	 * running. If one does, then this will certainly break. Make sure to
	 * disable automatic mode before changing anything with the sensors!!
	 */
	private class UltrasonicChecker extends Thread {

		public synchronized void run() {
			Ultrasonic u = null;
			while (m_automaticEnabled) {
				if (u == null) {
					u = m_firstSensor;
				}
				if (u == null) {
					return;
				}
				if (u.isEnabled()) {
					u.m_pingChannel.pulse(m_pingChannel.m_channel,
							(float) kPingTime); // do the ping
				}
				u = u.m_nextSensor;
				Timer.delay(.1); // wait for ping to return
			}
		}
	}

	/**
	 * Initialize the Ultrasonic Sensor. This is the common code that
	 * initializes the ultrasonic sensor given that there are two digital I/O
	 * channels allocated. If the system was running in automatic mode (round
	 * robin) when the new sensor is added, it is stopped, the sensor is added,
	 * then automatic mode is restored.
	 */
	private synchronized void initialize() {
		if (m_task == null) {
			m_task = new UltrasonicChecker();
		}
		boolean originalMode = m_automaticEnabled;
		setAutomaticMode(false); // kill task when adding a new sensor
		m_nextSensor = m_firstSensor;
		m_firstSensor = this;

		m_counter = new Counter(m_echoChannel); // set up counter for this
												// sensor
		m_counter.setMaxPeriod(1.0);
		m_counter.setSemiPeriodMode(true);
		m_counter.reset();
		m_counter.start();
		m_enabled = true; // make it available for round robin scheduling
		setAutomaticMode(originalMode);

		m_instances++;
		UsageReporting.report(tResourceType.kResourceType_Ultrasonic,
				m_instances);
		LiveWindow.addSensor("Ultrasonic", m_echoChannel.getChannel(), this);
	}

	/**
	 * Create an instance of the Ultrasonic Sensor.
	 * This is designed to supchannel the Daventech SRF04 and Vex ultrasonic
	 * sensors.
	 *
	 * @param pingChannel
	 *            The digital output channel that sends the pulse to initiate
	 *            the sensor sending the ping.
	 * @param echoChannel
	 *            The digital input channel that receives the echo. The length
	 *            of time that the echo is high represents the round trip time
	 *            of the ping, and the distance.
	 * @param units
	 *            The units returned in either kInches or kMilliMeters
	 */
	public Ultrasonic(final int pingChannel, final int echoChannel, Unit units) {
		m_pingChannel = new DigitalOutput(pingChannel);
		m_echoChannel = new DigitalInput(echoChannel);
		m_allocatedChannels = true;
		m_units = units;
		initialize();
	}

	/**
	 * Create an instance of the Ultrasonic Sensor.
	 * This is designed to supchannel the Daventech SRF04 and Vex ultrasonic
	 * sensors. This constructor assumes that both digital I/O channels are in
	 * the default digital module. Default unit is inches.
	 *
	 * @param pingChannel
	 *            The digital output channel that sends the pulse to initiate
	 *            the sensor sending the ping.
	 * @param echoChannel
	 *            The digital input channel that receives the echo. The length
	 *            of time that the echo is high represents the round trip time
	 *            of the ping, and the distance.
	 */
	public Ultrasonic(final int pingChannel, final int echoChannel) {
		this(pingChannel, echoChannel, Unit.kInches);
	}

	/**
	 * Create an instance of an Ultrasonic Sensor from a DigitalInput for the
	 * echo channel and a DigitalOutput for the ping channel.
	 *
	 * @param pingChannel
	 *            The digital output object that starts the sensor doing a ping.
	 *            Requires a 10uS pulse to start.
	 * @param echoChannel
	 *            The digital input object that times the return pulse to
	 *            determine the range.
	 * @param units
	 *            The units returned in either kInches or kMilliMeters
	 */
	public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel,
			Unit units) {
		if (pingChannel == null || echoChannel == null) {
			throw new NullPointerException("Null Channel Provided");
		}
		m_allocatedChannels = false;
		m_pingChannel = pingChannel;
		m_echoChannel = echoChannel;
		m_units = units;
		initialize();
	}

	/**
	 * Create an instance of an Ultrasonic Sensor from a DigitalInput for the
	 * echo channel and a DigitalOutput for the ping channel. Default unit is
	 * inches.
	 *
	 * @param pingChannel
	 *            The digital output object that starts the sensor doing a ping.
	 *            Requires a 10uS pulse to start.
	 * @param echoChannel
	 *            The digital input object that times the return pulse to
	 *            determine the range.
	 */
	public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
		this(pingChannel, echoChannel, Unit.kInches);
	}

	/**
	 * Destructor for the ultrasonic sensor. Delete the instance of the
	 * ultrasonic sensor by freeing the allocated digital channels. If the
	 * system was in automatic mode (round robin), then it is stopped, then
	 * started again after this sensor is removed (provided this wasn't the last
	 * sensor).
	 */
	public synchronized void free() {
		boolean wasAutomaticMode = m_automaticEnabled;
		setAutomaticMode(false);
		if (m_allocatedChannels) {
			if (m_pingChannel != null) {
				m_pingChannel.free();
			}
			if (m_echoChannel != null) {
				m_echoChannel.free();
			}
		}

		if (m_counter != null) {
			m_counter.free();
			m_counter = null;
		}

		m_pingChannel = null;
		m_echoChannel = null;

		if (this == m_firstSensor) {
			m_firstSensor = m_nextSensor;
			if (m_firstSensor == null) {
				setAutomaticMode(false);
			}
		} else {
			for (Ultrasonic s = m_firstSensor; s != null; s = s.m_nextSensor) {
				if (this == s.m_nextSensor) {
					s.m_nextSensor = s.m_nextSensor.m_nextSensor;
					break;
				}
			}
		}
		if (m_firstSensor != null && wasAutomaticMode) {
			setAutomaticMode(true);
		}
	}

	/**
	 * Turn Automatic mode on/off. When in Automatic mode, all sensors will fire
	 * in round robin, waiting a set time between each sensor.
	 *
	 * @param enabling
	 *            Set to true if round robin scheduling should start for all the
	 *            ultrasonic sensors. This scheduling method assures that the
	 *            sensors are non-interfering because no two sensors fire at the
	 *            same time. If another scheduling algorithm is preffered, it
	 *            can be implemented by pinging the sensors manually and waiting
	 *            for the results to come back.
	 */
	public void setAutomaticMode(boolean enabling) {
		if (enabling == m_automaticEnabled) {
			return; // ignore the case of no change
		}
		m_automaticEnabled = enabling;

		if (enabling) {
			// enabling automatic mode.
			// Clear all the counters so no data is valid
			for (Ultrasonic u = m_firstSensor; u != null; u = u.m_nextSensor) {
				u.m_counter.reset();
			}
			// Start round robin task
			m_task.start();
		} else {
			// disabling automatic mode. Wait for background task to stop
			// running.
			while (m_task.isAlive()) {
				Timer.delay(.15); // just a little longer than the ping time for
									// round-robin to stop
			}
			// clear all the counters (data now invalid) since automatic mode is
			// stopped
			for (Ultrasonic u = m_firstSensor; u != null; u = u.m_nextSensor) {
				u.m_counter.reset();
			}
		}
	}

	/**
	 * Single ping to ultrasonic sensor. Send out a single ping to the
	 * ultrasonic sensor. This only works if automatic (round robin) mode is
	 * disabled. A single ping is sent out, and the counter should count the
	 * semi-period when it comes in. The counter is reset to make the current
	 * value invalid.
	 */
	public void ping() {
		setAutomaticMode(false); // turn off automatic round robin if pinging
									// single sensor
		m_counter.reset(); // reset the counter to zero (invalid data now)
		m_pingChannel.pulse(m_pingChannel.m_channel, (float) kPingTime); // do
																			// the
																			// ping
																			// to
																			// start
																			// getting
																			// a
																			// single
																			// range
	}

	/**
	 * Check if there is a valid range measurement. The ranges are accumulated
	 * in a counter that will increment on each edge of the echo (return)
	 * signal. If the count is not at least 2, then the range has not yet been
	 * measured, and is invalid.
	 *
	 * @return true if the range is valid
	 */
	public boolean isRangeValid() {
		return m_counter.get() > 1;
	}

	/**
	 * Get the range in inches from the ultrasonic sensor.
	 *
	 * @return double Range in inches of the target returned from the ultrasonic
	 *         sensor. If there is no valid value yet, i.e. at least one
	 *         measurement hasn't completed, then return 0.
	 */
	public double getRangeInches() {
		if (isRangeValid()) {
			return m_counter.getPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
		} else {
			return 0;
		}
	}

	/**
	 * Get the range in millimeters from the ultrasonic sensor.
	 *
	 * @return double Range in millimeters of the target returned by the
	 *         ultrasonic sensor. If there is no valid value yet, i.e. at least
	 *         one measurement hasn't complted, then return 0.
	 */
	public double getRangeMM() {
		return getRangeInches() * 25.4;
	}

	/**
	 * Get the range in the current DistanceUnit for the PIDSource base object.
	 *
	 * @return The range in DistanceUnit
	 */
	public double pidGet() {
		switch (m_units.value) {
		case Unit.kInches_val:
			return getRangeInches();
		case Unit.kMillimeters_val:
			return getRangeMM();
		default:
			return 0.0;
		}
	}

	/**
	 * Set the current DistanceUnit that should be used for the PIDSource base
	 * object.
	 *
	 * @param units
	 *            The DistanceUnit that should be used.
	 */
	public void setDistanceUnits(Unit units) {
		m_units = units;
	}

	/**
	 * Get the current DistanceUnit that is used for the PIDSource base object.
	 *
	 * @return The type of DistanceUnit that is being used.
	 */
	public Unit getDistanceUnits() {
		return m_units;
	}

	/**
	 * Is the ultrasonic enabled
	 *
	 * @return true if the ultrasonic is enabled
	 */
	public boolean isEnabled() {
		return m_enabled;
	}

	/**
	 * Set if the ultrasonic is enabled
	 *
	 * @param enable
	 *            set to true to enable the ultrasonic
	 */
	public void setEnabled(boolean enable) {
		m_enabled = enable;
	}

	/*
	 * Live Window code, only does anything if live window is activated.
	 */
	public String getSmartDashboardType() {
		return "Ultrasonic";
	}

	private ITable m_table;

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

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

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

	/**
	 * {@inheritDoc}
	 */
	public void startLiveWindowMode() {
	}

	/**
	 * {@inheritDoc}
	 */
	public void stopLiveWindowMode() {
	}
}
