Command.cpp

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. 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 $(WIND_BASE)/WPILib.  */
/*----------------------------------------------------------------------------*/

#include "Commands/Command.h"
#include "Commands/CommandGroup.h"
#include "Commands/Scheduler.h"
#include "DriverStation.h"
#include "NetworkTables/NetworkTable.h"
#include "Timer.h"
#include "WPIErrors.h"

static const char *kName = "name";
static const char *kRunning = "running";
static const char *kIsParented = "isParented";

void Command::InitCommand(const char *name, double timeout)
{
        m_timeout = timeout;
        m_locked = false;
        m_startTime = -1;
        m_initialized = false;
        m_running = false;
        m_interruptible = true;
        m_canceled = false;
        m_runWhenDisabled = false;
        m_parent = NULL;
        m_table = NULL;
        if (name == NULL)
        {
                // Don't have a way to find the subclass name like java, so use the address
                char buf[32];
                snprintf(buf, 32, "Command_%p", this);
                m_name = buf;
        }
        else
        {
                m_name = name;
        }
}

Command::Command()
{
        InitCommand(NULL, -1.0);
}

Command::Command(const char *name)
{
        if (name == NULL)
                wpi_setWPIErrorWithContext(NullParameter, "name");
        InitCommand(name, -1.0);
}

Command::Command(double timeout)
{
        if (timeout < 0.0)
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
        InitCommand(NULL, timeout);
}

Command::Command(const char *name, double timeout)
{
        if (name == NULL)
                wpi_setWPIErrorWithContext(NullParameter, "name");
        if (timeout < 0.0)
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
        InitCommand(name, timeout);
}

Command::~Command()
{
        if (m_table != NULL)
        {
                m_table->RemoveChangeListener(kRunning, this);
        }
}

void Command::SetTimeout(double timeout)
{
        if (timeout < 0.0)
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
        else
                m_timeout = timeout;
}

double Command::TimeSinceInitialized()
{
        if (m_startTime < 0.0)
                return 0.0;
        else
                return Timer::GetFPGATimestamp() - m_startTime;
}

void Command::Requires(Subsystem *subsystem)
{
        if (!AssertUnlocked("Can not add new requirement to command"))
                return;

        if (subsystem != NULL)
                m_requirements.insert(subsystem);
        else
                wpi_setWPIErrorWithContext(NullParameter, "subsystem");
}

void Command::Removed()
{
        if (m_initialized)
        {
                if (IsCanceled())
                {
                        Interrupted();
                        _Interrupted();
                }
                else
                {
                        End();
                        _End();
                }
        }
        m_initialized = false;
        m_canceled = false;
        m_running = false;
        if (m_table != NULL)
                m_table->PutBoolean(kRunning, false);
}

void Command::Start()
{
        LockChanges();
        if (m_parent != NULL)
                wpi_setWPIErrorWithContext(CommandIllegalUse, "Can not start a command that is part of a command group");

        Scheduler::GetInstance()->AddCommand(this);
}

bool Command::Run()
{
        if (!m_runWhenDisabled && m_parent == NULL && DriverStation::GetInstance()->IsDisabled())
                Cancel();

        if (IsCanceled())
                return false;

        if (!m_initialized)
        {
                m_initialized = true;
                StartTiming();
                _Initialize();
                Initialize();
        }
        _Execute();
        Execute();
        return !IsFinished();
}

void Command::_Initialize()
{
}

void Command::_Interrupted()
{
}

void Command::_Execute()
{
}

void Command::_End()
{
}

void Command::StartTiming()
{
        m_startTime = Timer::GetFPGATimestamp();
}

bool Command::IsTimedOut()
{
        return m_timeout != -1 && TimeSinceInitialized() >= m_timeout;
}

Command::SubsystemSet Command::GetRequirements()
{
        return m_requirements;
}

void Command::LockChanges()
{
        m_locked = true;
}

bool Command::AssertUnlocked(const char *message)
{
        if (m_locked)
        {
                char buf[128];
                snprintf(buf, 128, "%s after being started or being added to a command group", message);
                wpi_setWPIErrorWithContext(CommandIllegalUse, buf);
                return false;
        }
        return true;
}

void Command::SetParent(CommandGroup *parent)
{
        if (parent == NULL)
        {
                wpi_setWPIErrorWithContext(NullParameter, "parent");
        }
        else if (m_parent != NULL)
        {
                wpi_setWPIErrorWithContext(CommandIllegalUse, "Can not give command to a command group after already being put in a command group");
        }
        else
        {
                LockChanges();
                m_parent = parent;
                if (m_table != NULL)
                {
                        m_table->PutBoolean(kIsParented, true);
                }
        }
}

void Command::StartRunning()
{
        m_running = true;
        m_startTime = -1;
        if (m_table != NULL)
                m_table->PutBoolean(kRunning, true);
}

bool Command::IsRunning()
{
        return m_running;
}

void Command::Cancel()
{
        if (m_parent != NULL)
                wpi_setWPIErrorWithContext(CommandIllegalUse, "Can not cancel a command that is part of a command group");

        _Cancel();
}

void Command::_Cancel()
{
        if (IsRunning())
                m_canceled = true;
}

bool Command::IsCanceled()
{
        return m_canceled;
}

bool Command::IsInterruptible()
{
        return m_interruptible;
}

void Command::SetInterruptible(bool interruptible)
{
        m_interruptible = interruptible;
}

bool Command::DoesRequire(Subsystem *system)
{
        return m_requirements.count(system) > 0;
}

CommandGroup *Command::GetGroup()
{
        return m_parent;
}

void Command::SetRunWhenDisabled(bool run)
{
        m_runWhenDisabled = run;
}

bool Command::WillRunWhenDisabled()
{
        return m_runWhenDisabled;
}

std::string Command::GetName()
{
        return m_name;
}

std::string Command::GetType()
{
        return "Command";
}

NetworkTable *Command::GetTable()
{
        if (m_table == NULL)
        {
                m_table = new NetworkTable();
                m_table->PutString(kName, GetName());
                m_table->PutBoolean(kRunning, IsRunning());
                m_table->PutBoolean(kIsParented, m_parent != NULL);
                m_table->AddChangeListener(kRunning, this);
        }
        return m_table;
}

void Command::ValueChanged(NetworkTable *table, const char *name, NetworkTables_Types type)
{
        if (table->GetBoolean(kRunning))
                Start();
        else
                Cancel();
}