#include "MotorControllerSpy.hpp"

MC_Spy::MC_Spy(std::string id, const uint32_t maxVelocity_cps) :
	MC(id, maxVelocity_cps), fakeMotorSpeed_(0.0f), motorStoped_(true), motorSleeping_(true)
{
}

MCError MC_Spy::start()
{
	motorStoped_ = true;
	motorSleeping_ = false;
	fakeMotorSpeed_ = 0.0f;

	return MCError::NONE;
}

MCError MC_Spy::stop()
{
	motorStoped_ = true;
	fakeMotorSpeed_ = 0.0f;

	return MCError::NONE;
}

MCError MC_Spy::sleep()
{
	motorStoped_ = true;
	motorSleeping_ = true;
	fakeMotorSpeed_ = 0.0f;

	return MCError::NONE;
}

MCError MC_Spy::setSpeed(const motorVel_normalized vel)
{
	if(!motorSleeping_)
	{
		fakeMotorSpeed_ = limitSpeedRequest(vel);
		return MCError::NONE;
	}

	return MCError::UNKNOWN_ERROR;
}

MCError MC_Spy::getSpeed_normalized(motorVel_normalized& vel)
{
	if(!motorSleeping_)
	{
		vel = fakeMotorSpeed_;
		return MCError::NONE;
	}

	vel = 0.0f;
	return MCError::UNKNOWN_ERROR;
}

MCError MC_Spy::init()
{
	return MCError::NONE;
}

float MC_Spy::limitSpeedRequest(float request)
{
	float out = 0.0f;
	if(request > 1.0f)
	{
		out = 1.0f;
	}
	else if(request < -1.0f)
	{
		out = -1.0f;
	}
	else
	{
		out = request;
	}
	return out;
}