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

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

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

	defineActionLabels();

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

void AscendState::doStateActions()
{
	switch(currentAction)
	{
		case getCTDPressure:
			//		if (ctdSBE41.getPressure(ctdSample) > 0)
			//		{
			nextAction = getGPSPosition;
			//}
			break;
		case getGPSPosition:
		{
			GPS::GPSFix currentFix;
			if(getGPS1().getPosition(currentFix) > 0)
			{
				// cout << "Recovery state GPSUBlox.getPosition msg: " << currentFix.msg << endl;
				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:
			waitCounter++;
			if(waitCounter > 1000)
			{
				nextAction = numActions;
			}
			break;
			//	case startCTDPressureTimer:
			//		timers.setctdSBE41FPTimerPeriod(20000);
			//		timers.startctdSBE41FPTimer();
			//		nextAction = getPressure;
			//		break;

		case numActions: // TODO probably don't need a case for lastAction
			cout << "Recovery action: lastAction" << endl;
			break;
		default:
			cout << "**Error: Recovery unhandled action" << endl;
	}
	if(nextAction != currentAction)
		processActionChange();
}

StateNames AscendState::checkEvents()
{
	if(currentAction == numActions)
	{
		// cout << "\r\nRecovery state at last action" << endl;
		return submerge;
	}
	else
	{
		return recovery;
	}
}

// extern FIFO gpsFIFO;

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

void AscendState::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::numActions)] = "last action";
}
