#pragma once

#include <string>
#include <cstdio>
#include <cstdint>
#include <memory.h>

enum class MCError : uint8_t
{
	NONE = 0,
	UNKNOWN_ERROR,
	NUM_ERRORS
};

// Valid range: [-1, 1]
typedef float motorVel_normalized;

class MC
{
  public:
	MC(std::string id, const uint32_t maxVelocity_cps);

	struct MotorControllerResponse_t
	{
		uint8_t msg[256];
	};

	virtual MCError start() = 0;
	virtual MCError stop() = 0;
	virtual MCError sleep() = 0;

	virtual MCError setSpeedNormalized(const motorVel_normalized vel) = 0;
	virtual MCError setSpeedRadsPerSec(const float rad_per_sec) = 0;

	// Returns the current velocity of the motor over the range [-MAX_VEL, MAX_VEL]
	virtual MCError getSpeed_normalized(motorVel_normalized& vel) = 0;

	uint32_t maxMotorVelocity_cps();
	
	void logEngFileHeader();

  protected:
	virtual ~MC() = default;

	const std::string id_;

	void logLine(std::string comment);
	
	void logLine(MotorControllerResponse_t r, std::string comment);

	motorVel_normalized limitSpeedRequest(motorVel_normalized in);

	// Maximum motor velocity, in counts per second. This limit is assumed to be symetric,
	// meaning it can drive the motor at the maximum velocity in both directions.
	const uint32_t maxVel_cps;

};