#include "common/memory/shared_ptr.hpp"
#include "sensor/calibration.hpp"
#include "sensor/sensor.hpp"
#include "serial.hpp"
#include "utils.hpp"

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

constexpr uint8_t SENSOR_ADDRESS = 0x68U;
constexpr unsigned int SENSOR_THROTTLE_TIME = 50U; // milliseconds
constexpr unsigned int SENSOR_RETRY_TIME = 2000U;  // milliseconds

constexpr unsigned int BAUD_RATE = 9600U;

void setup()
{
	auto sout = common::make_shared<SerialStream>(Serial, BAUD_RATE);

	auto sensor = common::make_shared<Sensor>(SENSOR_ADDRESS, Wire, SENSOR_THROTTLE_TIME);

	sout->waitReady();

	while (!sensor->begin())
	{
		*sout << "Error: Could not connect to the _sensor-> Retrying after 2000 "
				 "milliseconds..."
			  << endl;

		delay(SENSOR_RETRY_TIME);
	}

	if (!sensor->setAccelSensitivity(2)) // 8g
	{
		*sout << "Error: Failed to set the sensor's accelerometer sensitivity. Status: "
			  << static_cast<int>(sensor->getStatus()) << endl;

		stop();
	}

	if (!sensor->setGyroSensitivity(1)) // 500 degrees/s
	{
		*sout << "Error: Failed to set the sensor's gyro sensitivity. Status: "
			  << static_cast<int>(sensor->getStatus()) << endl;

		stop();
	}

	*sout << "Calibrating _sensor->.." << endl;

	SensorCalibrator sensor_calibrator(sensor, sout);

	if (!sensor_calibrator.calibrate(SENSOR_THROTTLE_TIME))
	{
		*sout << "Error: Sensor calibration timed out after " << CALIBRATION_TIMEOUT
			  << " milliseconds" << endl;

		stop();
	}

	*sout << "Finished calibrating sensor\n";

	*sout << "Calibration values:\n"
		  << "Accelerometer X: " << sensor->accel_cal_x << "\n"
		  << "Accelerometer Y: " << sensor->accel_cal_y << "\n"
		  << "Accelerometer Z: " << sensor->accel_cal_z << "\n"
		  << "Gyro X: " << sensor->gyro_cal_x << "\n"
		  << "Gyro Y: " << sensor->gyro_cal_y << "\n"
		  << "Gyro Z: " << sensor->gyro_cal_z << "\n";

	*sout << "Starting..." << endl;

	XInput.begin();

	while (true)
	{
		delay(SENSOR_THROTTLE_TIME);

		if (!sensor->read())
		{
			SensorStatus status = sensor->getStatus();

			if (status == SensorStatus::THROTTLED)
			{
				*sout << "Warning: The sensor was read too frequently and throttled"
					  << endl;
				continue;
			}

			*sout << "Error: Failed to read _sensor-> Status: "
				  << static_cast<int>(status) << endl;
			stop();
		}

		*sout << "Pitch: " << sensor->getPitch() << "    Roll: " << sensor->getRoll()
			  << endl;

		XInput.setJoystick(JOY_LEFT, static_cast<int32_t>(sensor->getRoll()),
						   static_cast<int32_t>(sensor->getPitch()));
		XInput.printDebug(Serial);
	}
}