#include <ElmoTwitter.hpp>
#include <CppUTest/CommandLineTestRunner.h>
#include "CppUTest/TestHarness.h"
#include "CppUTestExt/MockSupport.h"

#include <DevicePowerSwitchSpy.hpp>
#include <MockUART.hpp>

#define EXPECT_STOP (Expect_Send(Elmo::CMD_STOP.data(), Elmo::CMD_STOP.size()))
#define EXPECT_MOTOR_ON (Expect_Send(Elmo::CMD_MOTOR_ON.data(), Elmo::CMD_MOTOR_ON.size()))
#define EXPECT_MOTOR_OFF (Expect_Send(Elmo::CMD_MOTOR_OFF.data(), Elmo::CMD_MOTOR_OFF.size()))
#define EXPECT_ECHO_OFF (Expect_Send(Elmo::CMD_ECHO_OFF.data(), Elmo::CMD_ECHO_OFF.size()))
#define EXPECT_ECHO_ON (Expect_Send(Elmo::CMD_ECHO_ON.data(), Elmo::CMD_ECHO_ON.size()))
#define EXPECT_JV0 (Expect_Send(Elmo::CMD_JV0.data(), Elmo::CMD_JV0.size()))
#define EXPECT_BEGIN (Expect_Send(Elmo::CMD_BEGIN.data(), Elmo::CMD_BEGIN.size()))
#define EXPECT_VEL_MODE (Expect_Send(Elmo::CMD_VEL_MODE.data(), Elmo::CMD_VEL_MODE.size()))
#define EPXECT_STATUS_REG (Expect_Send(Elmo::CMD_STATUS_REG.data(), Elmo::CMD_STATUS_REG.size()))
#define EXPECT_GET_SN (Expect_Send(Elmo::CMD_GET_SN.data(), Elmo::CMD_GET_SN.size()))
#define EXPECT_DEFAULT_ACCEL \
	(Expect_Send(Elmo::CMD_DEFAULT_ACCEL.data(), Elmo::CMD_DEFAULT_ACCEL.size()))
#define EXPECT_DEFAULT_DECCEL \
	(Expect_Send(Elmo::CMD_DEFAULT_DECCEL.data(), Elmo::CMD_DEFAULT_DECCEL.size()))

#define EXPECT_CTS_CURR_LIM \
	(Expect_Send(Elmo::CMD_DEFAULT_CTS_CURR.data(), Elmo::CMD_DEFAULT_CTS_CURR.size()))
#define EXPECT_PK_CURR_LIM \
	(Expect_Send(Elmo::CMD_DEFAULT_PK_CURR.data(), Elmo::CMD_DEFAULT_PK_CURR.size()))
#define EXPECT_PK_CURR_DUR \
	(Expect_Send(Elmo::CMD_DEFAULT_PK_DUR.data(), Elmo::CMD_DEFAULT_PK_DUR.size()))

#define EXPECT_STOP_MOTOR_SEQUENCE \
	EXPECT_MOTOR_ON;               \
	EXPECT_JV0;                    \
	EXPECT_BEGIN;                  \
	EXPECT_STOP;                   \
	EXPECT_MOTOR_OFF;

#define EXPECT_DEFAULT_ACCEL_LIMS \
	EXPECT_DEFAULT_ACCEL;         \
	EXPECT_DEFAULT_DECCEL;

#define EXPECT_DEFAULT_CURRENT_LIMS \
	EXPECT_CTS_CURR_LIM;            \
	EXPECT_PK_CURR_LIM;             \
	EXPECT_PK_CURR_DUR;

#define EXPECT_INIT_SEQUENCE    \
	EXPECT_STOP_MOTOR_SEQUENCE; \
	EXPECT_ECHO_OFF;            \
	EXPECT_GET_SN;              \
	EXPECT_DEFAULT_ACCEL_LIMS;  \
	EXPECT_DEFAULT_CURRENT_LIMS;

TEST_GROUP(ElmoTestGroup)
{
	MockUART* uart_mock = nullptr;
	UART* uart = nullptr;

	DevicePowerSwitchSpy* elmoPowerSwitchSpy = nullptr;
	DevicePowerSwitch* elmoPowerSwitch = nullptr;

	static constexpr uint32_t max_cps = 400e3;

	void setup()
	{
		uart_mock = new MockUART;
		uart = reinterpret_cast<UART*>(uart_mock);

		elmoPowerSwitchSpy = new DevicePowerSwitchSpy();
		elmoPowerSwitch = elmoPowerSwitchSpy;
	}

	void teardown()
	{
		mock().checkExpectations();
		mock().removeAllComparatorsAndCopiers();
		mock().clear();

		delete uart_mock;
		delete elmoPowerSwitchSpy;
	}

	void Expect_Send(const char toTransmit[], size_t size)
	{
		mock()
			.expectOneCall("transmit")
			.onObject(uart)
			.withParameter("transmit_buffer", toTransmit)
			.withParameter("transmit_size", size);
	}
};

TEST(ElmoTestGroup, MockTest)
{
	const char toTransmit[] = "asdf";

	mock()
		.expectOneCall("transmit")
		.onObject(uart_mock)
		.withParameter("transmit_buffer", "asdf")
		.withParameter("transmit_size", strlen(toTransmit));

	uart_mock->transmit(reinterpret_cast<const uint8_t*>(toTransmit), strlen(toTransmit));
}

TEST(ElmoTestGroup, ElmoEnablesPowerInConstructor)
{
	EXPECT_INIT_SEQUENCE;

	elmoPowerSwitchSpy->spy_enablePower(false);
	CHECK_EQUAL(false, elmoPowerSwitchSpy->isPowerOn());

	Elmo mc(*uart_mock, *elmoPowerSwitch, max_cps);
	CHECK_EQUAL(true, mc.hasPower());
}

TEST(ElmoTestGroup, ElmoReadsPowerBusStatus)
{
	EXPECT_INIT_SEQUENCE;

	Elmo mc(*uart_mock, *elmoPowerSwitch, max_cps);

	elmoPowerSwitchSpy->spy_enablePower(false);
	CHECK_EQUAL(false, mc.hasPower());

	elmoPowerSwitchSpy->spy_enablePower(true);
	CHECK_EQUAL(true, mc.hasPower());
}

TEST(ElmoTestGroup, ElmoEnablesPowerOnStart)
{
	EXPECT_INIT_SEQUENCE;

	Elmo mc(*uart_mock, *elmoPowerSwitch, max_cps);

	elmoPowerSwitchSpy->spy_enablePower(false);
	CHECK_EQUAL(false, mc.hasPower());

	mc.start();
	CHECK_EQUAL(true, mc.hasPower());
}

TEST(ElmoTestGroup, ElmoMotorStopSequence)
{
	EXPECT_INIT_SEQUENCE;
	Elmo mc(*uart_mock, *elmoPowerSwitch, max_cps);

	EXPECT_STOP_MOTOR_SEQUENCE
	mc.stop();
}

TEST(ElmoTestGroup, ElmoDisablePowerOnSleep)
{
	EXPECT_INIT_SEQUENCE;

	Elmo mc(*uart_mock, *elmoPowerSwitch, max_cps);

	elmoPowerSwitchSpy->spy_enablePower(true);
	CHECK_EQUAL(true, mc.hasPower());

	// Calling sleep also stops the motor.
	EXPECT_STOP_MOTOR_SEQUENCE;
	mc.sleep();
	CHECK_EQUAL(false, mc.hasPower());
}

TEST(ElmoTestGroup, MotorInterfaceMaxVelocityMemberSet)
{
	EXPECT_INIT_SEQUENCE;

	Elmo mc(*uart_mock, *elmoPowerSwitch, max_cps);

	CHECK_EQUAL(max_cps, mc.maxMotorVelocity_cps());
}

TEST(ElmoTestGroup, generateCmdStr_Float)
{
	std::string gen_cmd = Elmo::generateCmdStr_Float("UM", 2, 0);
	MEMCMP_EQUAL(Elmo::CMD_VEL_MODE.data(), gen_cmd.data(), Elmo::CMD_VEL_MODE.size());

	gen_cmd = Elmo::generateCmdStr_Float("CL[1]", 3.1415926f, 2);
	std::string correct = "CL[1]=3.14;";

	MEMCMP_EQUAL(correct.data(), gen_cmd.data(), correct.size());
}