#include <iostream>
#include <string>
using namespace std;

#include "CPFcpp.hpp"
#include "mcuRealTimeClock.hpp"
#include "ubloxGPS.hpp"
#include "SDLogger.hpp"

class Recovery : public StateBase
{
private:
	enum SubStates
	{
		getToSurface,
		getTime,
		getPosition,
		sendSBD,
		sendSBD_WFR		
	};

	SubStates currentSubState = getTime;
	StateNames nextState = recovery;

	RecoveryStateConfig StateConfig;

public: 
	Recovery(string stateid, int32_t timeout = 0) : StateBase{stateid, timeout}
	{
		cout << "\t\tRecovery constructor, state ID: " << stateid << ", timeout = " << timeout << endl;
	}
	
private:
	//ubloxGPS gps1 {UART7, "GPS1"};
	ubloxGPS gps1 = ubloxGPS::getInstance(UART7, "GPS1");
	mcuRealTimeClock mcuRTC = mcuRealTimeClock::getInstance();
	
	void doStateEntryActions()
	{
		currentSubState = getToSurface;
		nextState = recovery;
		cout << "\t\tRecovery: doing state entry actions; stateTimeout = " << stateTimeout << endl;
	}
	
	void doStateActions()
	{
		string timestampString;
		string gpsString;
		int8_t sendCommandReturn = 0;
		
		switch (currentSubState)
		{
		case getToSurface:
			{
				//start increaseBuoyancy.  If depth doesn't change X meters in Y minutes move on to getting 
				//GPS and sending SBD.  Try again next recovery period.
				cout << "\t\tRecovery substate getToSurface " << endl;
				currentSubState = getTime;
				break;
			}
		case getTime:
			{
				sendCommandReturn = gps1.sendCommand(ubloxGPS::getTime, timestampString, gpsString);
				if(sendCommandReturn == 0)
				{
					cout << "\t\tGet time  return = " << + sendCommandReturn <<  "  Timestamp: " << timestampString << 
						"  gpsString: " << gpsString << endl;
					currentSubState = getTime;
				}
				else if (sendCommandReturn < 0)
				{
					//TODO deal with error
					cout << "\t\tError in Get time  return = " << + sendCommandReturn <<   "  Timestamp: " << timestampString << 
						"  gpsString: " << gpsString << endl << endl;
					currentSubState = getPosition;
				}
				else if (sendCommandReturn > 0)
				{
					cout << "\t\tGet time  return = " << + sendCommandReturn <<   "  Timestamp: " << timestampString << 
						"  gpsString: " << gpsString << endl << endl;
					mcuRTC.SynchToPUBX04(gpsString);
					currentSubState = getPosition;
				}
				break;
			}
		case getPosition:
			{
				sendCommandReturn = gps1.sendCommand(ubloxGPS::getPos, timestampString, gpsString);
				if(sendCommandReturn == 0)
				{
					cout << "\t\tGet position return = " << + sendCommandReturn <<  "  Timestamp: " << timestampString << 
						"  gpsString: " << gpsString << endl;
					currentSubState = getPosition;
				}
				else if (sendCommandReturn < 0)
				{
					//TODO deal with error
					cout << "\t\tError in Get position return = " << + sendCommandReturn <<  "  Timestamp: " << timestampString << 
						"  gpsString: " << gpsString << endl << endl;
					currentSubState = sendSBD;
				}
				else if (sendCommandReturn > 0)
				{
					cout << "\t\tGet position return = " << + sendCommandReturn << "  Timestamp: " << timestampString << 
						"  gpsString: " << gpsString << endl << endl;
					currentSubState = sendSBD;
				}
				break;
			}
		case sendSBD:
			{
				cout << "\t\tRecovery: doing state action sendSBD" << endl;
				currentSubState = sendSBD_WFR;
				break;
			}
		case sendSBD_WFR:
			{
				cout << "\t\tRecovery: doing state action sendSBD_WFR" << endl;
				nextState = findNeutral;
				break;
			}
		}
	}

	
	StateNames checkEvents()
	{
		cout << "\t\tRecovery: checking events" << endl;
		return (nextState);
	}
	
void doStateExit(bool timeout)
{
	if(timeout)
		cout << "\t\tRecovery: doing state exit with timeout" << endl ;
else
	cout << "\t\tRecovery: doing state exit no timeout" << endl ;
}
	
}
;