#pragma once

#include "MotorController_Interface.hpp"
#include "Platform_Controller.hpp"
#include "BellowsPosition_Interface.hpp"
#include "MotorizedValve_Interface.hpp"
#include "StateEstimator_Interface.hpp"

#include <cstdint>
#include <string>
#include <cmath>
#include <system_time.hpp>

class BuoyancyEngine
{
public:
	enum class Status : uint8_t
	{
		READY          = 0,
		CMD_IN_PROCESS,
		NOT_READY,
		UNKNOWN_ERROR
	};

	enum class PlatformDir : uint8_t
	{
		ASCEND  = 0,
		DESCEND
	};

	enum class PlatformBuoyancy : uint8_t
	{
		INCREASE = 0,
		DECREASE
	};

	enum class BellowsPositionPreset : uint8_t
	{
		RECOVERY    = 0,
		SURFACE_OPS,
		NUM_PRESETS
	};
	
	// Direction we expect the INNER bellows to travel for a
	// corresponding pump direction
	enum class PumpDirection : uint8_t
	{
		POS_MOTOR_COUNTS_DEFLATE_INNER_BELLOWS = 0,
		POS_MOTOR_COUNTS_INFLATE_INNER_BELLOWS
	};
	
	public:
	BuoyancyEngine(std::string id, PumpDirection dir)
		:
	id_(id), pump_dir_(dir)
	{
	}
	
	/// Perform all actions required to initialize the buoyancy engine device and put it in an
	/// operational state.
	/// @pre Device is stopped
	virtual Status start(bool moveBellowsWithinLimits = true, bool openValve = false) = 0;

	/// Perform all actions required to stop the buoyancy engine.
	/// @pre Device is started
	virtual Status stop() = 0;
	
	/// Perform all actions required to sleep the buoyancy engine and associated components
	virtual Status sleep() = 0;

	/// Test the components of the buoyancy engine.
	/// Return READY on success
	virtual Status doSelfTest() = 0;
	
	/// Indicate if the buoyancy system is ready for immediate actuation
	/// Requirements: 
	/// -- all hardware sucessful start
	/// -- main valve open
	/// -- bellows within limits
	virtual bool isReady() = 0;
	
	/// @brief Adjust platform buoyancy by running the pump at a desired rate
	///
	/// @param dir Direction of desired buoyancy change (INCREASE/DECREASE)
	/// @param pumpSpeed_pctMaxSpeed Acceptable input: 0.0-100.0. Out of bounds values will be
	/// floor'ed/ceil'ed.
	/// @return Status return code
	virtual Status adjustBuoyancy(const PlatformBuoyancy dir, float pumpRate_pctMaxSpeed) = 0;
	
	/// @brief Command the platform to ascend/descend at a specified rate.
	///
	/// @param dir Direction of trvel (ASCEND/DESCEND)
	/// @param speed_dBar_per_sec Acceptable input: 0-1 dBar/sec
	/// @return Status
	virtual Status runPlatformVelocityLaw(const PlatformDir dir, const float speed_dBar_per_sec) = 0;
	
	
	/// @brief Command the buoyancy engine to ascend/descend to a specified pressure
	/// @param targetPressure_dBar Target pressure in dBar
	/// @param targetTolerance_dBar absolute error (in dBar) to be accepted in achieving targetPressure
	/// @param maxSpeed_dBar_per_sec maximum permitted speed allowed during transit to
	/// the desired targetPressure_dBar
	virtual Status runPlatformDepthLaw(const float targetPressure_dBar,
		const float targetTolerance_dBar,
		const float maxSpeed_dBar_per_sec =
			Platform_Controller::DEFAULT_PRESSURE_SPEED_DBAR_PER_SEC) = 0;
	
	/// @brief Update the state estimator with a new pressure measurement
	/// @param timestamp timestamp the measurement was taken
	/// @param pressure_dBar measured pressure in dBar
	virtual void updateEstimator(stateObservation_t obs, const SBE_CTD_MODE mode) = 0;
	
	/// @breif Initialize the state estimator.
	/// @param x_initial - initial state estimate
	virtual void initializeEstimator(const stateEstimate_t x_initial) = 0;
	
	/// @brief Get the current estimate of the CPF's state (pressure, velocity, etc.)
	virtual stateEstimate_t getCurrentStateEstimate() = 0;
	
	/// @brief Detect if the platform has become submerged
	virtual bool submerganceDetected() = 0;
	
	/// Given the pump_dir_ (set in ctor), ensure the output is of the correct sign
	motorVel_normalized pumpSpeedForDescend(const motorVel_normalized in);
	
	/// Given the pump_dir_ (set in ctor), ensure the output is of the correct sign
	motorVel_normalized pumpSpeedForAscend(const motorVel_normalized in);
	
	/// Request and log the current bellows position.
	virtual void logInnerBellowsPosition() = 0;

	void logEngLine(std::string comment);
	
	void logEngFileHeader();
	
	static constexpr float DEFAULT_RECOVERY_BELLOWS_POSITION_PCT = 90.0f;
	static constexpr float DEFAULT_SURFACE_OPS_BELLOWS_POSITION_PCT = 75.0f;
	static constexpr float DEFAULT_SUBMERGE_MOTOR_SPEED_PCT = 20.0f;
	static constexpr float DEFULAT_VELOCITY_DBAR_S = 0.1f;

protected:
	virtual ~BuoyancyEngine() = default;

	const std::string id_;
	const PumpDirection pump_dir_;
};