#pragma once
#include <memory.h>
#include <stdio.h>
#include <string>
#include <iostream>
#include <sstream>

#include <MotorController_Interface.hpp>
#include <PowerSwitched_Device.hpp>
#include <UART_Interface.hpp>

#include "DevicePowerSwitch.hpp"
#include "FIFOQueue.hpp"

// TODO: Code smell: Multiple Inheritance. Should MC (Motor Controller Interface) inherit from
// PoewrSwitchedDevice?
class Elmo : public MC, public PowerSwitchedDevice
{
  public:
	Elmo(UART& uart, DevicePowerSwitch& pwrSwitch, uint32_t maxVelocity_cps);

	MCError start() final;

	MCError stop() final;

	MCError sleep() final;

	MCError setSpeed(const motorVel_normalized vel) final;

	MCError getSpeed_normalized(motorVel_normalized& vel) final;

	bool hasPower() final;

	// clang-format off
	// 2048  Counts per Revolution Encoder *4 for quadrature interpolation
	static constexpr uint32_t COUNTS_PER_REV = 2048 * 4;
	
	// Motor controller limits
	static constexpr float DEFAULT_CTS_CURRENT_AMPS = 4.76f;
	static constexpr float DEFAULT_PK_CURRENT_AMPS = 13.5f;
	static constexpr float DEFAULT_PK_CURRENT_DURATION_S = 2;

	static constexpr std::string_view CMD_STOP 				= "ST;";
	static constexpr std::string_view CMD_MOTOR_ON 			= "MO=1;";
	static constexpr std::string_view CMD_MOTOR_OFF 		= "MO=0;";
	static constexpr std::string_view CMD_ECHO_OFF 			= "EO=0;";
	static constexpr std::string_view CMD_ECHO_ON 			= "EO=1;";
	static constexpr std::string_view CMD_JV0 				= "JV=0;";
	static constexpr std::string_view CMD_BEGIN 			= "BG;";
	static constexpr std::string_view CMD_VEL_MODE 			= "UM=2;";
	static constexpr std::string_view CMD_STATUS_REG 		= "SR;";
	static constexpr std::string_view CMD_GET_SN 			= "SN;";
	static constexpr std::string_view CMD_DEFAULT_ACCEL 	= "AC[1]=1e9;";
	static constexpr std::string_view CMD_DEFAULT_DECCEL 	= "DC=1e9;";
	static constexpr std::string_view CMD_DEFAULT_CTS_CURR	= "CL[1]=4.76;";
	static constexpr std::string_view CMD_DEFAULT_PK_CURR	= "PL[1]=13.5;";
	static constexpr std::string_view CMD_DEFAULT_PK_DUR	= "PL[2]=2;";
	//clang-format on

	static std::string generateCmdStr_Float(std::string registerName, float val, unsigned int precision);

  protected:
    void enablePower(bool enable) final;
	
	void enableEcho(const bool enable);

  private:
	MCError init() final;
	MCError sendDefaultSettings();

	MCError setCtsCurrentLimit(float limit_A);
	MCError setPkCurrentLimit(float limit_A);
	MCError setPkCurrentDuration(float limit_s);
	
	// Convienence function for generating Elmo Commands, e.g. "CL[1]=13.5;"
	// This function inserts the requisite "=" and ";"
	// Use: generateCmdStr_Float("CL[1]", 3.1415926, 2) would generate "CL[1]=3.14;"

	MCError getStats();
	MCError getSerialNumber();
  private:
	UART& _uart;
	DevicePowerSwitch& _sw;
	static constexpr uint32_t cmdDelay_ms = 50;
};