#include "CPFBuoyancyEngine.hpp"
#include "Logger.hpp"
#include <system_time.hpp>

CPFBuoyancyEngine::CPFBuoyancyEngine(std::string id,
	MC& motorCtrl,
	BellowsPosition& innerBel,
	MotorizedValve& mainValve, 
	Platform_Controller& ctrl,
	PlatformStateEstimator& stateEstimator,
	PumpDirection dir)
	: BuoyancyEngine(id, dir)
	, motorController_(motorCtrl)
	, innerBellows_(innerBel)
	, platformController_(ctrl)
	, bellowsValve_(mainValve)
	, stateEstimator_(stateEstimator)
{
	// Set the default bellows positions.
	presetBellowsPositions_[BellowsPositionPreset::RECOVERY] = DEFAULT_RECOVERY_BELLOWS_POSITION_PCT;
	presetBellowsPositions_[BellowsPositionPreset::SURFACE_OPS] =
		DEFAULT_SURFACE_OPS_BELLOWS_POSITION_PCT;
}

BuoyancyEngine::Status CPFBuoyancyEngine::getInnerBellowsPosition_pctFull(float& percentFull)
{
	BellowPosError err = BellowPosError::UNKNOWN_ERROR;
	
	BellowsPosition::Measurement sample = { };
	err = innerBellows_.requestMeasurement();
	if (err == BellowPosError::NONE)
	{
		sample = innerBellows_.getMeasurement();
		percentFull = sample.position_pctFull;
		err = BellowPosError::NONE;
	}

	if (err == BellowPosError::NONE)
	{
		return Status::READY;
	}
	else if (err == BellowPosError::WAITING_FOR_CONVERSION)
	{
		return Status::CMD_IN_PROCESS;
	}

	return Status::UNKNOWN_ERROR;
}

BuoyancyEngine::Status CPFBuoyancyEngine::start(bool moveBellowsWithinLimits, bool openValve)
{
	(void)moveBellowsWithinLimits;
	Status ret = BuoyancyEngine::Status::UNKNOWN_ERROR;
	
	bool error = false;

	// Start the MC, and flag if error
	if (MCError::NONE != motorController_.start())
	{
		error = true;
	}

	// Start the bellows sensor, and flag if error
	if (BellowPosError::NONE != innerBellows_.start())
	{
		error = true;
	}

	if (bellowsValve_.isMVReady())
	{
		if (openValve)
		{
			bellowsValve_.openMV();
		}
		else
		{
			bellowsValve_.closeMV();
		}
	}
	else
	{
		error = true;
		// TODO Log error, don't start pumping, etc.
	}

	// platformController_.start();

	if (!error)
	{
		ready_ = true;
		ret = Status::READY;
	} 
	else
	{
		ready_ = false;
		ret = Status::UNKNOWN_ERROR;	
	}
	
	return ret;
}

BuoyancyEngine::Status CPFBuoyancyEngine::stop()
{
	ready_ = false;
	
	bool error = false;
	if (bellowsValve_.isMVReady())
	{
		bellowsValve_.closeMV();
	}
	else
	{
		error = true;
		// TODO Log error, etc.
	}

	motorController_.stop();
	innerBellows_.stop();
	// platformController_.stop();

	if (!error)
	{
		return Status::READY;
	}
	return Status::UNKNOWN_ERROR;
}

BuoyancyEngine::Status CPFBuoyancyEngine::sleep()
{
	ready_ = false;
	
	bool error = false;
	if (bellowsValve_.isMVReady())
	{
		bellowsValve_.closeMV();
	}
	else
	{
		error = true;
		// TODO Log error, etc.
	}

	motorController_.sleep();
	innerBellows_.sleep();
	// platformController_.sleep();

	if (!error)
	{
		return Status::READY;
	}
	return Status::UNKNOWN_ERROR;
}

BuoyancyEngine::Status CPFBuoyancyEngine::doSelfTest()
{
	//TODO
	return BuoyancyEngine::Status::CMD_IN_PROCESS;
}

bool CPFBuoyancyEngine::isReady()
{
	if ( ready_	&& bellowsValve_.isMVOpen() )
	{
		return true;
	}
	
	return false;
}

BuoyancyEngine::Status CPFBuoyancyEngine::adjustBuoyancy(const PlatformBuoyancy dir, float pumpRate_pctMaxSpeed)
{
	Status ret = BuoyancyEngine::Status::UNKNOWN_ERROR;
	
	if (isReady())
	{
		// Incoming units should be (0.0, 100.0], so convert to (0.0, 1]
		// Motor controller responsible for floor/ceil-ing the input
		motorVel_normalized vel = pumpRate_pctMaxSpeed / 100.0f;
		
		switch (dir)
		{
		case (PlatformBuoyancy::DECREASE): 
		{
			// Ensure proper pump direction
			vel = pumpSpeedForDescend(vel);
			break;
		}
		case (PlatformBuoyancy::INCREASE):
		{
			vel = pumpSpeedForAscend(vel);
			break;
		}
		default:
			vel = 0.0;
			logEngLine("PlatfomrDirectionError");
			break;
		}
		
		if (MCError::NONE == motorController_.setSpeedNormalized(vel))
		{
			ret = BuoyancyEngine::Status::READY;
		}
		
	}
	else
	{
		logEngLine("Not ready to adjustBuoyancy()");
		ret = BuoyancyEngine::Status::NOT_READY;
		bellowsValve_.openMV();
	}
	// Start pump at specified speed
	// Log pump speed, and bellows position
	
	return ret;
}

BuoyancyEngine::Status CPFBuoyancyEngine::runPlatformVelocityLaw(const PlatformDir dir, const float speed_dBar_per_sec)
{
	// Assumption: The state estimator has been updated.
	
	stateEstimate_t xNow = stateEstimator_.getCurrentEstimate();
	
	// Create a "fake" target pressure that is away from the current pressure, since
	// we just need to pass something to the platform controller.
	float target_pressure = 0;
	switch (dir)
	{
		case (PlatformDir::ASCEND):
		{
			target_pressure = xNow.press_dBar - 10.0f;
			break;
		}
		case (PlatformDir::DESCEND):
		{
			target_pressure = xNow.press_dBar + 10.0f;
		}
	}
	
	float newMotorSpeed_radsPerSec = platformController_.update( stateEstimator_.getCurrentEstimate(),
		target_pressure,
		false,
		speed_dBar_per_sec,
		ControlMode::VELOCITY);
	
	// Need to use absolute value speed setter when running closed loop, since the gain values
	// are established not according to a percent of absolute max speed.
	motorController_.setSpeedRadsPerSec(newMotorSpeed_radsPerSec);
	
	// TODO Anything to check/flag here?
	return BuoyancyEngine::Status::READY;
}

BuoyancyEngine::Status CPFBuoyancyEngine::runPlatformDepthLaw(const float targetPressure_dBar,
	const float targetTolerance_dBar,
	const float maxSpeed_dBar_per_sec)
{
	(void)targetTolerance_dBar;
	
	float newMotorSpeed_radsPerSec = 
		platformController_.update( 
			stateEstimator_.getCurrentEstimate(),
			targetPressure_dBar,
			true,
			maxSpeed_dBar_per_sec,
			ControlMode::PRESSURE_VELOCITY);
	
	// Need to use absolute value speed setter when running closed loop, since the gain values
	// are established not according to a percent of absolute max speed.
	motorController_.setSpeedRadsPerSec(newMotorSpeed_radsPerSec);
	
	return BuoyancyEngine::Status::READY;
}

void CPFBuoyancyEngine::updateEstimator(stateObservation_t obs, const SBE_CTD_MODE mode)
{
	stateEstimator_.update(obs, mode);
}

void CPFBuoyancyEngine::initializeEstimator(const stateEstimate_t x_init)
{
	stateEstimator_.init(x_init);
}

stateEstimate_t CPFBuoyancyEngine::getCurrentStateEstimate()
{
	return stateEstimator_.getCurrentEstimate();
}

bool CPFBuoyancyEngine::submerganceDetected()
{
	//TODO - make more robust to waves etc
	return (getCurrentStateEstimate().press_dBar > 0.4);
}

void CPFBuoyancyEngine::logInnerBellowsPosition()
{
	// BLOCKING I2C CALL
	innerBellows_.requestMeasurement();
	
	BellowsPosition::Measurement m = innerBellows_.getMeasurement();
	
	innerBellows_.logEngLine(m, "Current measurement");
	
	if (m.conversion_good)
	{
		stateObservation_t obs = {};
		obs.innerBellowsPos_pctFull = m.position_pctFull;
		obs.timestamp = m.tickstamp;
		
		// NaN values are respected in the estimator and are not be used in the update
		obs.press_dBar = NAN;
		
		// TODO - CTD getter? For example:
		// auto ctdMode = getCTD1().currentMode()
		// Assume constant for now.
		SBE_CTD_MODE ctdMode = SBE_CTD_MODE::CP;
		stateEstimator_.update(obs, ctdMode);
	}
	else
	{
		innerBellows_.logEngLine("Bad conversion");
	}
}