#include "SubmergeState.hpp"
#include "bsp.hpp"
#include <Logger.hpp>

SubmergeState::SubmergeState(uint32_t timeout)
: StateBase{StateNames::submergeState, timeout}
{}

void SubmergeState::doStateEntry()
{
	ACTION_CHANGE(current_action_, initiatePumping);
}

void SubmergeState::doStateExit(bool timeout)
{
	(void) timeout;
	//TODO: do exit actions for submerge
}

StateNames SubmergeState::doStateActions(const event_t& event)
{
	StateNames next_state;
	
	switch (current_action_)
	{
	case (initiatePumping):
	{
		next_state = submergePlatformActionHandler(event);
		break;
	}
	default:
		assert(0);	// Unexpected Case
	}
	
	return next_state;
}

StateNames SubmergeState::submergePlatformActionHandler(const event_t& event)
{
	StateNames next_state = state_id_;
	
	switch (event.get_message_id())
	{
	case ID_EntryAction:
		{
			std::cout << "[Action: submergePlatform] Entry Event, requesting steady buoyancy decrease..." << std::endl;
			
			stateEstimate_t initial_state = { 
				.press_dBar = 0.0f, 
				.vel_dBar_s = 0.0f, 
				.bellowsDeltaVol_m3 = 0.0f,
				.mode = SBE_CTD_MODE::NON_CP, 
				.timestamp = systime::getDateTimeNow() };
			
			buoyancyEngine_.initializeEstimator(initial_state);
			
			buoyancyEngine_.adjustBuoyancy(BuoyancyEngine::PlatformBuoyancy::DECREASE,
											BuoyancyEngine::DEFAULT_SUBMERGE_MOTOR_SPEED_PCT);
			
			break;
		}
	case ID_CTDNewFastPressureReady:
		{
			auto new_FP_event = CONVERT_TO_EVENT_TYPE(event, CTDFastPressureReadyEvent);
			
			std::cout << "[Action: submergePlatform] Received new fast pressure message: P=" << new_FP_event->pressure_dBar << " dBar" << std::endl;
			
			stateObservation_t obs = {};
			obs.press_dBar = new_FP_event->pressure_dBar;
			obs.timestamp = new_FP_event->pressureTimestamp;
			
			// TODO(Gene): is CP correct for a Fast Pressure message?
			buoyancyEngine_.updateEstimator(obs, SBE_CTD_MODE::NON_CP);
			
			// auto est = buoyancyEngine_.getCurrentStateEstimate();
			
			if (buoyancyEngine_.submerganceDetected())
			{
				smLogEngLine("Platform submergance sucessful");
				next_state = StateNames::descendState;
			}
			
			// Log current bellows position at this time
			buoyancyEngine_.logInnerBellowsPosition();
			break;
		}
	case ID_CTDCommunicationsError:
		{
			// TODO(Gene) - anything specific to do here?
			break;
		}
	default:
		DEFAULT_UNEXPECTED_EVENT_HANDLER("submergePlatform", event.get_message_id());
	}
	
	return next_state;
}

StateNames SubmergeState::checkEvents(const event_t& event)
{
	(void)event;
	
	return state_id_;
}