#include <iostream>

#include "StateMachine.hpp"
#include "StateBase.hpp"
#include "InitPlatformState.hpp"
#include "RecoveryState.hpp"
#include "SubmergeState.hpp"
#include "Logger.hpp"
#include "RTClock.hpp"

void processStateChange(StateNames current, StateNames next);

static StateBase* stateArray[numStates];
static uint32_t stateTimeouts[numStates];
static StateNames currentState = initPlatform;
static StateNames nextState = initPlatform;
static StateNames previousState = initPlatform;

void StateMachine::initSM()
{
	stateArray[initPlatform] = new InitPlatformState("InitPlatform", 1);
	stateArray[recovery] = new RecoveryState("Recovery", 0);
	stateArray[submerge] = new SubmergeState("Submerge", 10);

	//The following lines are just to see how much memory might be needed when all the states are implemented
	stateArray[descend] = new InitPlatformState("descend", 10);

	stateArray[park] = new InitPlatformState("park", 10);
	
	stateArray[anchor] = new InitPlatformState("anchor", 10);

	stateArray[ascend] = new InitPlatformState("ascend", 10);

	stateArray[surfaceOps] = new InitPlatformState("surfaceOps", 10);
}

void StateMachine::runSM()
{
	bool doEntryActions = true;

	while (true)
	{
		if (doEntryActions)
		{
			//std::cout << "[StateLoop] Performing entry actions for state " << getNameForStateID(currentState) << std::endl;
			stateArray[static_cast<int>(currentState)]->enterState();
			doEntryActions = false;
		}

		stateArray[currentState]->doStateActions(noEvent);
		nextState = stateArray[currentState]->checkEvents(noEvent);

		//		checkPlatformHealth();

//		if (StateVariables::getForceExit())
//		{
//			//			Logger::logEngLine(ALWAYS, "ForceExit = True, Program exiting (kind of)");
//			//			Logger::stop();
//			// close all files exit program or go to idle state or while(1) or ???
//		}
//
//		if (StateVariables::getForceRecovery())
//		{
//			//Logger::logEngLine(ALWAYS, "ForceRecovery = True");
//			// TODO: nextState = recovery;
//		}

		if (nextState != currentState)
		{
			std::cout << "[StateLoop] Performing exit actions for state " << std::endl;
			stateArray[static_cast<int>(currentState)]->exitState(noEvent);
			processStateChange(currentState, nextState);
			doEntryActions = true;
		}
		LL_mDelay(100);
	}
}

void processStateChange(StateNames current, StateNames next)
{
	previousState = current;
	currentState = next;
	nextState = next;

	std::string comment {"Changing state from: "};
	comment.append(stateArray[current]->stateLabel);
	comment.append(" to: ");
	comment.append(stateArray[next]->stateLabel);
	
	std::cout << comment << std::endl;

	Logger::logEngLine(true, {stmchEngLogID, RTClock::getDateTimeStampFast().data(), stateArray[current]->stateLabel, comment});
}

std::string StateMachine::getStateLabel()
{
	return stateArray[currentState]->stateLabel;
}