#include <iostream>

#include "stm32h7xx_ll_utils.h"

#include "RecoveryState.hpp"

static uint32_t checkEventCtr = 0;

RecoveryState::RecoveryState(std::string label, uint32_t timeout_min) : StateBase{label, timeout_min}
{
	defineActionLabels();
}

void RecoveryState::defineActionLabels()
{
	actionLabels[getPosition] = "getPosition";
	actionLabels[checkHealth] = "checkHealth";
	actionLabels[buildSBD] = "buildSBD";
	actionLabels[sendSBD] = "sendSBD";
	actionLabels[wait] = "wait";
	actionLabels[lastAction] = "lastAction";
}

void RecoveryState::doStateActions(StateEvent event)
{
	switch (currentAction)
	{
	case getPosition:
		nextAction = doGetPosition();
		break;
	case checkHealth:
		nextAction = doCheckHealth();
		break;
	case buildSBD:
		nextAction = doBuildSBD();
		break;
	case sendSBD:
		nextAction = doSendSBD();
		break;
	case wait:
		nextAction = doWait();
		break;
	case lastAction:
		hitLastAction = true;
		break;
	default:
		std::cout << "**Error: InitPlatform unhandled action" << std::endl;
	}
	
	if ((checkEventCtr % 5) == 0)
	{
		nextAction = getPosition;
		std::cout << "Forcing action change to " << actionLabels[nextAction] << std::endl;
	}

	if (currentAction != nextAction)
	{
		doActionChange(actionLabels[currentAction], actionLabels[nextAction]);
	}
		
}

RecoveryState::Actions RecoveryState::doGetPosition()
{
	if (BSP::gps1.getPositionPUBX00() > 0)
	{
		return checkHealth;
	}
	else
		return getPosition;
}

RecoveryState::Actions RecoveryState::doCheckHealth()
{
	return buildSBD;
}

RecoveryState::Actions RecoveryState::doBuildSBD()
{
	return sendSBD;
}

RecoveryState::Actions RecoveryState::doSendSBD()
{
	return wait;
}

RecoveryState::Actions RecoveryState::doWait()
{
	return wait;
}

StateNames RecoveryState::checkEvents(StateEvent event)
{
	char temp[16];
	//sprintf(temp, "%ul", checkEventCtr);
	std::cout << RTClock::getDateTimeStampFast().data() << "   RecoveryState checkEvents.  Ctr: "  << checkEventCtr << std::endl;
	LL_mDelay(500);
	checkEventCtr++;
		
	if (checkEventCtr < 10)
		return StateNames::recovery;
	else
		return StateNames::submerge;
}

void RecoveryState::enterThisState()
{
	std::cout << "RecoveryState enterThisState.";
	std::cout << "  RecoveryState statelabel: "  << stateLabel << std::endl;
	
	std::cout << "Starting background timer with blinking LED"  << std::endl;
	TimerMgr::getTimer(TimerMgr::timer1, 10000, 1, 0);
	TimerMgr::startTimer(TimerMgr::timer1);

	setInitialAction(getPosition, actionLabels[getPosition]);
	
	hitLastAction = false;
	checkEventCtr = 0;
}

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

}