#pragma once

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

#include <string_view>
#include <cstdio>
#include <cstdint>
#include <memory.h>
#include <map>

/**
 * This is the Buoyancy Engine, used for controlling all aspects of the Coastal
 * Profiling Float's buoyancy system.
 *
 */
class CPFBuoyancyEngine final : public BuoyancyEngine
{
  public:
	CPFBuoyancyEngine(std::string id,
		MC& motorCtrl,
		BellowsPosition& innerBel,
		MotorizedValve& mainValve,
		Platform_Controller& ctrl,
		PlatformStateEstimator& stateEstimator,
		PumpDirection dir);

	~CPFBuoyancyEngine() = default;

	/**
	 * @brief Power up and initialize all buoyancy engine interfaces and devices. Call after waking
	 * from sleep.
	 *
	 * @return Status return code
	 */
	Status start(bool moveBellowsWithinLimits = true, bool openValve = false) final;

	/**
	 * @brief Stop the pump and close the main valve. Leaves all devices powered and initialized.
	 *
	 * @return Status return code
	 */
	Status stop() final;

	/**
	 * @brief Calls sleep() for all buoyancy engine hardware modules.
	 *
	 * @return Status return code
	 */
	Status sleep() final;

	/**
	 * @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
	 */
	Status adjustBuoyancy(const PlatformBuoyancy dir, float pumpRate_pctMaxSpeed);

	/**
	 * @brief Command the buoyancy engine to achieve a desired speed and direction of travel.
	 *
	 * @param dir Desired direction of travel (ASCEND/DESCEND)
	 * @param speed_dBar_per_sec Desired speed
	 * @return Status
	 */
	Status runPlatformVelocityLaw(const PlatformDir dir, const float speed_dBar_per_sec) final;

	/**
	 * @brief Command  the buoyancy engine to achieve the desired pressure.
	 *
	 * @param targetPressure_dBar Target pressure which the platform should move to, in dBar.
	 * @param targetTolerance_dBar Allowable tolerance for the target pressure.
	 * @param maxSpeed_dBar_per_sec
	 * @return Status
	 */
	Status runPlatformDepthLaw(const float targetPressure_dBar,
		const float targetTolerance_dBar,
		const float maxSpeed_dBar_per_sec =
			Platform_Controller::DEFAULT_PRESSURE_SPEED_DBAR_PER_SEC) final;
	
	/**
	 * @brief Update the state estimator with a new pressure observation.
	 * 
	 * @param obs - An observation (set of measurements) of the current platform state.
	 * @param mode - Mode of Seabird CTD.
	 * Unused observation values should be NaN
	 */
	void updateEstimator(stateObservation_t obs, const SBE_CTD_MODE mode) final;
	
	/**
	 * @brief Get the current estimate of the CPF's state (depth, velocity, etc.)
	 */
	stateEstimate_t getCurrentStateEstimate() final;
	
	/** 
	 * @brief Initialize the state estimator with an initial state estimate.
	 * 
	 */
	void initializeEstimator(const stateEstimate_t x_init) final;
	
	/**
	 * @brief Based on the current state estimate, determine if the CPF is underwater
	 */
	bool submerganceDetected() final;
	
	/**
	 * @brief Move the bellows to a desired preset position
	 *
	 * @param preset An established position setting (typically corresponds with a CPF state, e.g.
	 * SURFACE_OPS)
	 * @return Status
	 */
	Status setBellowsPosition(const BellowsPositionPreset preset);

	/**
	 * @brief Start the Buoyancy Engine self test. This is a non-blocking method, but will not
	 * return READY until all tests have passed.
	 *
	 * @return BEStatus Will return READY when complete.
	 */
	Status doSelfTest() final;

	/**
	 * @brief Return the current position of the bellows, expressed in percent of its allowable
	 * range.
	 *
	 * @return float position (percent of full)
	 */
	Status getInnerBellowsPosition_pctFull(float& percentFull);
	
	/*
	 * @brief Return true if the buoyancy engine is ready for actuation and measurement.
	 */
	bool isReady() final;
	
	/**
	 * @brief Query, and record inner bellows position.
	 */
	void logInnerBellowsPosition() final;
	
  private:
	Status init();

	/**
	 * @brief Actuate the pump and valve until the inner bellows is within acceptable limits.
	 *
	 * @return BEError
	 */
	Status moveBellowsInsideLimits(const bool closeValveWhenDone = true);

	void logLine(std::string_view comment);

	const std::string_view engFileHeader1 = "dateTime_Z\tstate\tcomment";
	const std::string_view gpsEngFileHeader2 = "text\ttext \ttext";

	static constexpr float DEFAULT_RECOVERY_BELLOWS_POSITION_PCT = 90.0f;
	static constexpr float DEFAULT_SURFACE_OPS_BELLOWS_POSITION_PCT = 75.0f;

	std::map<BellowsPositionPreset, float> presetBellowsPositions_;

	MC& motorController_;
	BellowsPosition& innerBellows_;
	Platform_Controller& platformController_;
	MotorizedValve& bellowsValve_;
	PlatformStateEstimator& stateEstimator_;
	
	// Is the moduel ready to receive commands
	bool ready_;
};