#include "RecoveryState.hpp"
#include "ErrorHandler.hpp"
#include "Logger.hpp"
#include <bsp.hpp>
#include "BellowsPositionADC2485.hpp"
#include "StateVariables.hpp"

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

void RecoveryState::doStateEntry()
{
	// cout << "Running Recovery doStateEntry()" << endl;
	StdStateEntry();

	defineActionLabels(); // TODO this is the wrong place to do this but I don't know how to put a
						  // virtual function in the constructor

	if(SV.getProfileNum() == 0)
	{
		// desiredBelPos = config.surfaceOpsBP;
		desiredBelPos = 85.0;
	}
	else
	{
		// desiredBelPos = config.reoveryBelPos;
		desiredBelPos = 95.0;
	}

	float currentBellowsPosition = 50.0; // TODO I'm not sure the right way to getBellowsPosition();
	if(currentBellowsPosition < desiredBelPos)
	{
		// stub		BcyEngine.increaseBuoyancy();
	}
	else
	{
		// stub		BcyEngine.stopPumping();
	}

	currentAction = getCTDPressure;
	nextAction = getCTDPressure;
	previousAction = getCTDPressure;
}

void RecoveryState::doStateActions()
{
	switch(currentAction)
	{
		case getCTDPressure:
			nextAction = getGPSPosition;
			break;
		case getGPSPosition:
			nextAction = checkSystemState; // Need lastest SystemState for SBD message
			break;
		case checkSystemState:
			nextAction = createSBD;
			break;
		case createSBD:
			nextAction = sendSBD;
			break;
		case sendSBD:
			nextAction = wait;
			break;
		case wait:
			// if( (timeNow() - startTime) > sbdPeriod)
			nextAction = getCTDPressure;
			// if( BTConsole::gotGo() == true || iModem::gotGo() == true)
			nextAction = lastAction;
			break;
		case lastAction:
			break;
		default:
			cout << "**Error: Recovery unhandled action" << endl;
	}
	if(nextAction != currentAction)
		processActionChange();
}

StateNames RecoveryState::checkEvents()
{
	//	if (getBellowsPosition() >= desiredBelPos)
	//	{
	//		stopPumping();
	//	}

	if(currentAction == lastAction)
	{
		// StateVariables::setForceExit();
		return submerge;
	}

	return recovery;
}

// extern FIFO gpsFIFO;

void RecoveryState::startGetPos()
{
}

uint8_t RecoveryState::finishGetPos()
{
	return 1;
}

void RecoveryState::doStateExit(bool timeout)
{
	(void)timeout; // TODO: use this
	// TODO Be sure to clean up any hanging actions
	// cout << "RecoveryState running doStateExit()" << endl;
	// StdStateExit(timeout);
}

void RecoveryState::defineActionLabels()
{
	actionLabel[static_cast<int>(Actions::checkSystemState)] = "checkSystemState";
	actionLabel[static_cast<int>(Actions::createSBD)] = "createSBD";
	actionLabel[static_cast<int>(Actions::getCTDPressure)] = "getCTDPressure";
	actionLabel[static_cast<int>(Actions::getGPSPosition)] = "getGPSPosition";
	actionLabel[static_cast<int>(Actions::sendSBD)] = "sendSBD";
	actionLabel[static_cast<int>(Actions::wait)] = "wait";
	actionLabel[static_cast<int>(Actions::lastAction)] = "last action";
}
