#include <iostream>

#include "SubmergeState.hpp"
#include "BuoyancyEngine.hpp"

static uint32_t checkEventCtr = 0;

SubmergeState::SubmergeState(std::string label, uint32_t timeout_min) : stateLabel{label}, StateBase(label, timeout_min)
{
	defineActionLabels();
}
void SubmergeState::defineActionLabels()
{
	actionLabels[submerge] = "submerge";
	actionLabels[lastAction] = "lastAction";
}


void SubmergeState::enterThisState()
{
	setInitialAction(submerge, actionLabels[submerge]);
	checkEventCtr = 0;
	
	BuoyancyEngine::submerge();
	
}

void SubmergeState::exitThisState(StateEvent event)
{
	std::cout << "SubmergeState exitThisState"  << std::endl;

}

void SubmergeState::doStateActions(const StateEvent event)
{
	switch (currentAction)
	{
	case submerge:
		nextAction = submerge;
		break;
	case lastAction:
		hitLastAction = true;
		break;
	default:
		std::cout << "**Error: InitPlatform unhandled action" << std::endl;
	}
	
	if (currentAction != nextAction)
	{
		doActionChange(actionLabels[currentAction], actionLabels[nextAction]);
	}
}

StateNames SubmergeState::checkEvents(const StateEvent event)
{
	std::cout << RTClock::getDateTimeStampFast().data() << "   SubmergeState checkEvents.  Ctr: "  << checkEventCtr << std::endl;
	LL_mDelay(500);
	checkEventCtr++;

	if (checkEventCtr < 10)
		return StateNames::submerge;
	else
		return StateNames::recovery;
}