#include "status.hpp"

#include <MPU6050_light.h>
#include <Wire.h>
#include <XInput.h>

constexpr unsigned int SENSOR_RETRY_TIME = 2000U; // milliseconds

constexpr unsigned int BAUD_RATE = 9600U;

constexpr int32_t JOY_MAX = 180;
constexpr int32_t JOY_MIN = -180;

#define LEONARDO_XINPUT_VID 0x045E // NOLINT(cppcoreguidelines-macro-usage)
#define LEONARDO_XINPUT_PID 0x028E // NOLINT(cppcoreguidelines-macro-usage)

void setup()
{
	initialize_status_leds();

	auto sensor = MPU6050(Wire);

	Wire.begin();

	set_led_positive(HIGH);

	while (sensor.begin() != 0)
	{
		set_led_positive(LOW);

		delay(SENSOR_RETRY_TIME);
	}

	set_led_positive(HIGH);

	sensor.calcOffsets();

	XInput.begin();

	XInput.setRange(JOY_LEFT, JOY_MIN, JOY_MAX);

	while (true)
	{
		sensor.update();

		auto pitch = static_cast<int32_t>(sensor.getAngleX());

		auto roll = static_cast<int32_t>(sensor.getAngleY());

		XInput.setJoystick(JOY_LEFT, -roll, pitch);

#if USB_VID != LEONARDO_XINPUT_VID && USB_PID != LEONARDO_XINPUT_PID
		XInput.printDebug(Serial);
#endif
	}
}