#include "MotorController_Interface.hpp"
#include "Logger.hpp"
#include "system_time.hpp"
#include <memory.h>
#include <iostream>

MC::MC(std::string id, const uint32_t maxVelocity_cps) : id_(id), maxVel_cps(maxVelocity_cps)
{
}

uint32_t MC::maxMotorVelocity_cps()
{
	return maxVel_cps;
}

motorVel_normalized MC::limitSpeedRequest(motorVel_normalized in)
{
	if (in > 1.0f)
	{
		in = 1.0f;
	}
	else if (in < -1.0f)
	{
		in = -1.0f;
	}

	return in;
}

void MC::logLine(std::string comment)
{
	//TODO fix: Logger::logEngLine(ALWAYS, this->id_, comment);
}

void MC::logLine(MotorControllerResponse_t r, std::string comment)
{
	std::string msg = reinterpret_cast<char*>(r.msg);
	
	//TODO fix: Logger::logEngLine(ALWAYS, this->id_, comment.c_str(), msg);
}

void MC::logEngFileHeader()
{
	/*TODO fix: Logger::logEngLine(ALWAYS,
		this->id_,
		"dateTime_Z",
		"state",
		"comment",
		"msg");
	
	Logger::logEngLine(ALWAYS,
		this->id_,
		"text",
		"text",
		"text",
		"text");
*/
}