#include "ParkState.hpp"
#include "ErrorHandler.hpp"
#include "Logger.hpp"
#include <bsp.hpp>

using std::cout;
using std::endl;

void ParkState::doStateEntry()
{
	StdStateEntry();

	currentAction = firstAction;
	nextAction = firstAction;
	previousAction = firstAction;
}

void ParkState::doStateActions()
{
	switch(currentAction)
	{
		case firstAction:
			nextAction = firstAction;
			break;
		case numActions: // TODO probably don't need a case for lastAction
			cout << "Anchor state action: lastAction" << endl;
			break;
		default:
			cout << "**Error: Anchor state unhandled action" << endl;
	}

	if(nextAction != currentAction)
		processActionChange();
}

StateNames ParkState::checkEvents()
{
	if(currentAction == numActions)
	{
		return submerge;
	}
	else
	{
		return recovery;
	}
}

void ParkState::doStateExit(bool timeout)
{
	// TODO Be sure to clean up any hanging actions
	StdStateExit(timeout);
}

void ParkState::defineActionLabels()
{
	actionLabel[static_cast<int>(Actions::firstAction)] = "firstAction";
	actionLabel[static_cast<int>(Actions::numActions)] = "last action";
}
