#include "sensor/calibration.hpp"
#include "sensor/sensor.hpp"
#include "serial.hpp"
#include "std/memory.hpp"

#include <Arduino.h>
#include <Wire.h>

// constexpr unsigned int LINE_CLEAR_LENGTH = 30U;

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

constexpr unsigned int BAUD_RATE = 9600U;

constexpr unsigned int SENSOR_RETRY_TIME = 2000U;

SerialStream sout(Serial_(), BAUD_RATE);
// SerialStream sout(Serial, BAUD_RATE);

UniquePtr<Sensor> sensor;

void setup()
{
	sout.waitReady();

	sensor = make_unique<Sensor>(SENSOR_ADDRESS, Wire, sout, SENSOR_THROTTLE_TIME);

	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;

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

void loop()
{
	/*
	delay(SENSOR_THROTTLE_TIME);

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

		if (status == SensorStatus::THROTTLED)
		{
			printSerial("Warning: The sensor was read too frequently and throttled");
			endl();

			return;
		}

		printSerial("Error: Failed to read sensor. Status: %d", static_cast<int>(status));
		endl();

		stop();
	}

	char *pitch = floatToStr(sensor.getPitch(), FLOAT_WIDTH, FLOAT_PRECISION);
	char *roll = floatToStr(sensor.getRoll(), FLOAT_WIDTH, FLOAT_PRECISION);

	printSerial("\rPitch: %s	Roll: %s", pitch, roll);
	Serial.flush();

	free(pitch);
	free(roll);
	*/
}