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

import edu.wpi.first.wpilibj.NamedSendable;
import edu.wpi.first.wpilibj.RobotState;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.tables.ITable;
import edu.wpi.first.wpilibj.tables.ITableListener;
import java.util.Enumeration;
import java.util.NoSuchElementException;

/**
 * The Command class is at the very core of the entire command framework. Every
 * command can be started with a call to {@link Command#start() start()}. Once a
 * command is started it will call {@link Command#initialize() initialize()},
 * and then will repeatedly call {@link Command#execute() execute()} until the
 * {@link Command#isFinished() isFinished()} returns true. Once it does,
 * {@link Command#end() end()} will be called.
 *
 * <p>
 * However, if at any point while it is running {@link Command#cancel()
 * cancel()} is called, then the command will be stopped and
 * {@link Command#interrupted() interrupted()} will be called.
 * </p>
 *
 * <p>
 * If a command uses a {@link Subsystem}, then it should specify that it does so
 * by calling the {@link Command#requires(Subsystem) requires(...)} method in
 * its constructor. Note that a Command may have multiple requirements, and
 * {@link Command#requires(Subsystem) requires(...)} should be called for each
 * one.
 * </p>
 *
 * <p>
 * If a command is running and a new command with shared requirements is
 * started, then one of two things will happen. If the active command is
 * interruptible, then {@link Command#cancel() cancel()} will be called and the
 * command will be removed to make way for the new one. If the active command is
 * not interruptible, the other one will not even be started, and the active one
 * will continue functioning.
 * </p>
 *
 * @author Brad Miller
 * @author Joe Grinstead
 * @see Subsystem
 * @see CommandGroup
 * @see IllegalUseOfCommandException
 */
public abstract class Command implements NamedSendable {

  /** The name of this command */
  private String m_name;
  /** The time since this command was initialized */
  private double m_startTime = -1;
  /**
   * The time (in seconds) before this command "times out" (or -1 if no timeout)
   */
  private double m_timeout = -1;
  /** Whether or not this command has been initialized */
  private boolean m_initialized = false;
  /** The requirements (or null if no requirements) */
  private Set m_requirements;
  /** Whether or not it is running */
  private boolean m_running = false;
  /** Whether or not it is interruptible */
  private boolean m_interruptible = true;
  /** Whether or not it has been canceled */
  private boolean m_canceled = false;
  /** Whether or not it has been locked */
  private boolean m_locked = false;
  /** Whether this command should run when the robot is disabled */
  private boolean m_runWhenDisabled = false;
  /** The {@link CommandGroup} this is in */
  private CommandGroup m_parent;

  /**
   * Creates a new command. The name of this command will be set to its class
   * name.
   */
  public Command() {
    m_name = getClass().getName();
    m_name = m_name.substring(m_name.lastIndexOf('.') + 1);
  }

  /**
   * Creates a new command with the given name.
   *$
   * @param name the name for this command
   * @throws IllegalArgumentException if name is null
   */
  public Command(String name) {
    if (name == null) {
      throw new IllegalArgumentException("Name must not be null.");
    }
    m_name = name;
  }

  /**
   * Creates a new command with the given timeout and a default name. The
   * default name is the name of the class.
   *$
   * @param timeout the time (in seconds) before this command "times out"
   * @throws IllegalArgumentException if given a negative timeout
   * @see Command#isTimedOut() isTimedOut()
   */
  public Command(double timeout) {
    this();
    if (timeout < 0) {
      throw new IllegalArgumentException("Timeout must not be negative.  Given:" + timeout);
    }
    m_timeout = timeout;
  }

  /**
   * Creates a new command with the given name and timeout.
   *$
   * @param name the name of the command
   * @param timeout the time (in seconds) before this command "times out"
   * @throws IllegalArgumentException if given a negative timeout or name was
   *         null.
   * @see Command#isTimedOut() isTimedOut()
   */
  public Command(String name, double timeout) {
    this(name);
    if (timeout < 0) {
      throw new IllegalArgumentException("Timeout must not be negative.  Given:" + timeout);
    }
    m_timeout = timeout;
  }

  /**
   * Returns the name of this command. If no name was specified in the
   * constructor, then the default is the name of the class.
   *$
   * @return the name of this command
   */
  public String getName() {
    return m_name;
  }

  /**
   * Sets the timeout of this command.
   *$
   * @param seconds the timeout (in seconds)
   * @throws IllegalArgumentException if seconds is negative
   * @see Command#isTimedOut() isTimedOut()
   */
  protected synchronized final void setTimeout(double seconds) {
    if (seconds < 0) {
      throw new IllegalArgumentException("Seconds must be positive.  Given:" + seconds);
    }
    m_timeout = seconds;
  }

  /**
   * Returns the time since this command was initialized (in seconds). This
   * function will work even if there is no specified timeout.
   *$
   * @return the time since this command was initialized (in seconds).
   */
  public synchronized final double timeSinceInitialized() {
    return m_startTime < 0 ? 0 : Timer.getFPGATimestamp() - m_startTime;
  }

  /**
   * This method specifies that the given {@link Subsystem} is used by this
   * command. This method is crucial to the functioning of the Command System in
   * general.
   *
   * <p>
   * Note that the recommended way to call this method is in the constructor.
   * </p>
   *
   * @param subsystem the {@link Subsystem} required
   * @throws IllegalArgumentException if subsystem is null
   * @throws IllegalUseOfCommandException if this command has started before or
   *         if it has been given to a {@link CommandGroup}
   * @see Subsystem
   */
  protected synchronized void requires(Subsystem subsystem) {
    validate("Can not add new requirement to command");
    if (subsystem != null) {
      if (m_requirements == null) {
        m_requirements = new Set();
      }
      m_requirements.add(subsystem);
    } else {
      throw new IllegalArgumentException("Subsystem must not be null.");
    }
  }

  /**
   * Called when the command has been removed. This will call
   * {@link Command#interrupted() interrupted()} or {@link Command#end() end()}.
   */
  synchronized void removed() {
    if (m_initialized) {
      if (isCanceled()) {
        interrupted();
        _interrupted();
      } else {
        end();
        _end();
      }
    }
    m_initialized = false;
    m_canceled = false;
    m_running = false;
    if (table != null) {
      table.putBoolean("running", false);
    }
  }

  /**
   * The run method is used internally to actually run the commands.
   *$
   * @return whether or not the command should stay within the {@link Scheduler}
   *         .
   */
  synchronized boolean run() {
    if (!m_runWhenDisabled && m_parent == null && RobotState.isDisabled()) {
      cancel();
    }
    if (isCanceled()) {
      return false;
    }
    if (!m_initialized) {
      m_initialized = true;
      startTiming();
      _initialize();
      initialize();
    }
    _execute();
    execute();
    return !isFinished();
  }

  /**
   * The initialize method is called the first time this Command is run after
   * being started.
   */
  protected abstract void initialize();

  /** A shadow method called before {@link Command#initialize() initialize()} */
  void _initialize() {}

  /**
   * The execute method is called repeatedly until this Command either finishes
   * or is canceled.
   */
  protected abstract void execute();

  /** A shadow method called before {@link Command#execute() execute()} */
  void _execute() {}

  /**
   * Returns whether this command is finished. If it is, then the command will
   * be removed and {@link Command#end() end()} will be called.
   *
   * <p>
   * It may be useful for a team to reference the {@link Command#isTimedOut()
   * isTimedOut()} method for time-sensitive commands.
   * </p>
   *$
   * @return whether this command is finished.
   * @see Command#isTimedOut() isTimedOut()
   */
  protected abstract boolean isFinished();

  /**
   * Called when the command ended peacefully. This is where you may want to
   * wrap up loose ends, like shutting off a motor that was being used in the
   * command.
   */
  protected abstract void end();

  /** A shadow method called after {@link Command#end() end()}. */
  void _end() {}

  /**
   * Called when the command ends because somebody called
   * {@link Command#cancel() cancel()} or another command shared the same
   * requirements as this one, and booted it out.
   *
   * <p>
   * This is where you may want to wrap up loose ends, like shutting off a motor
   * that was being used in the command.
   * </p>
   *
   * <p>
   * Generally, it is useful to simply call the {@link Command#end() end()}
   * method within this method
   * </p>
   */
  protected abstract void interrupted();

  /** A shadow method called after {@link Command#interrupted() interrupted()}. */
  void _interrupted() {}

  /**
   * Called to indicate that the timer should start. This is called right before
   * {@link Command#initialize() initialize()} is, inside the
   * {@link Command#run() run()} method.
   */
  private void startTiming() {
    m_startTime = Timer.getFPGATimestamp();
  }

  /**
   * Returns whether or not the {@link Command#timeSinceInitialized()
   * timeSinceInitialized()} method returns a number which is greater than or
   * equal to the timeout for the command. If there is no timeout, this will
   * always return false.
   *$
   * @return whether the time has expired
   */
  protected synchronized boolean isTimedOut() {
    return m_timeout != -1 && timeSinceInitialized() >= m_timeout;
  }

  /**
   * Returns the requirements (as an {@link Enumeration Enumeration} of
   * {@link Subsystem Subsystems}) of this command
   *$
   * @return the requirements (as an {@link Enumeration Enumeration} of
   *         {@link Subsystem Subsystems}) of this command
   */
  synchronized Enumeration getRequirements() {
    return m_requirements == null ? emptyEnumeration : m_requirements.getElements();
  }

  /**
   * Prevents further changes from being made
   */
  synchronized void lockChanges() {
    m_locked = true;
  }

  /**
   * If changes are locked, then this will throw an
   * {@link IllegalUseOfCommandException}.
   *$
   * @param message the message to say (it is appended by a default message)
   */
  synchronized void validate(String message) {
    if (m_locked) {
      throw new IllegalUseOfCommandException(message
          + " after being started or being added to a command group");
    }
  }

  /**
   * Sets the parent of this command. No actual change is made to the group.
   *$
   * @param parent the parent
   * @throws IllegalUseOfCommandException if this {@link Command} already is
   *         already in a group
   */
  synchronized void setParent(CommandGroup parent) {
    if (this.m_parent != null) {
      throw new IllegalUseOfCommandException(
          "Can not give command to a command group after already being put in a command group");
    }
    lockChanges();
    this.m_parent = parent;
    if (table != null) {
      table.putBoolean("isParented", true);
    }
  }

  /**
   * Starts up the command. Gets the command ready to start.
   * <p>
   * Note that the command will eventually start, however it will not
   * necessarily do so immediately, and may in fact be canceled before
   * initialize is even called.
   * </p>
   *$
   * @throws IllegalUseOfCommandException if the command is a part of a
   *         CommandGroup
   */
  public synchronized void start() {
    lockChanges();
    if (m_parent != null) {
      throw new IllegalUseOfCommandException(
          "Can not start a command that is a part of a command group");
    }
    Scheduler.getInstance().add(this);
  }

  /**
   * This is used internally to mark that the command has been started. The
   * lifecycle of a command is:
   *
   * startRunning() is called. run() is called (multiple times potentially)
   * removed() is called
   *
   * It is very important that startRunning and removed be called in order or
   * some assumptions of the code will be broken.
   */
  synchronized void startRunning() {
    m_running = true;
    m_startTime = -1;
    if (table != null) {
      table.putBoolean("running", true);
    }
  }

  /**
   * Returns whether or not the command is running. This may return true even if
   * the command has just been canceled, as it may not have yet called
   * {@link Command#interrupted()}.
   *$
   * @return whether or not the command is running
   */
  public synchronized boolean isRunning() {
    return m_running;
  }

  /**
   * This will cancel the current command.
   * <p>
   * This will cancel the current command eventually. It can be called multiple
   * times. And it can be called when the command is not running. If the command
   * is running though, then the command will be marked as canceled and
   * eventually removed.
   * </p>
   * <p>
   * A command can not be canceled if it is a part of a command group, you must
   * cancel the command group instead.
   * </p>
   *$
   * @throws IllegalUseOfCommandException if this command is a part of a command
   *         group
   */
  public synchronized void cancel() {
    if (m_parent != null) {
      throw new IllegalUseOfCommandException("Can not manually cancel a command in a command group");
    }
    _cancel();
  }

  /**
   * This works like cancel(), except that it doesn't throw an exception if it
   * is a part of a command group. Should only be called by the parent command
   * group.
   */
  synchronized void _cancel() {
    if (isRunning()) {
      m_canceled = true;
    }
  }

  /**
   * Returns whether or not this has been canceled.
   *$
   * @return whether or not this has been canceled
   */
  public synchronized boolean isCanceled() {
    return m_canceled;
  }

  /**
   * Returns whether or not this command can be interrupted.
   *$
   * @return whether or not this command can be interrupted
   */
  public synchronized boolean isInterruptible() {
    return m_interruptible;
  }

  /**
   * Sets whether or not this command can be interrupted.
   *$
   * @param interruptible whether or not this command can be interrupted
   */
  protected synchronized void setInterruptible(boolean interruptible) {
    this.m_interruptible = interruptible;
  }

  /**
   * Checks if the command requires the given {@link Subsystem}.
   *$
   * @param system the system
   * @return whether or not the subsystem is required, or false if given null
   */
  public synchronized boolean doesRequire(Subsystem system) {
    return m_requirements != null && m_requirements.contains(system);
  }

  /**
   * Returns the {@link CommandGroup} that this command is a part of. Will
   * return null if this {@link Command} is not in a group.
   *$
   * @return the {@link CommandGroup} that this command is a part of (or null if
   *         not in group)
   */
  public synchronized CommandGroup getGroup() {
    return m_parent;
  }

  /**
   * Sets whether or not this {@link Command} should run when the robot is
   * disabled.
   *
   * <p>
   * By default a command will not run when the robot is disabled, and will in
   * fact be canceled.
   * </p>
   *$
   * @param run whether or not this command should run when the robot is
   *        disabled
   */
  public void setRunWhenDisabled(boolean run) {
    m_runWhenDisabled = run;
  }

  /**
   * Returns whether or not this {@link Command} will run when the robot is
   * disabled, or if it will cancel itself.
   *$
   * @return whether or not this {@link Command} will run when the robot is
   *         disabled, or if it will cancel itself
   */
  public boolean willRunWhenDisabled() {
    return m_runWhenDisabled;
  }

  /** An empty enumeration given whenever there are no requirements */
  private static Enumeration emptyEnumeration = new Enumeration() {

    public boolean hasMoreElements() {
      return false;
    }

    public Object nextElement() {
      throw new NoSuchElementException();
    }
  };

  /**
   * The string representation for a {@link Command} is by default its name.
   *$
   * @return the string representation of this object
   */
  public String toString() {
    return getName();
  }


  public String getSmartDashboardType() {
    return "Command";
  }

  private ITableListener listener = new ITableListener() {
    public void valueChanged(ITable table, String key, Object value, boolean isNew) {
      if (((Boolean) value).booleanValue()) {
        start();
      } else {
        cancel();
      }
    }
  };
  private ITable table;

  public void initTable(ITable table) {
    if (this.table != null)
      this.table.removeTableListener(listener);
    this.table = table;
    if (table != null) {
      table.putString("name", getName());
      table.putBoolean("running", isRunning());
      table.putBoolean("isParented", m_parent != null);
      table.addTableListener("running", listener, false);
    }
  }

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

}