#include "Arduino.h"
#include "Wire.h"
#include "calibration.hpp"
#include "sensor.hpp"

#define LINE_CLEAR_LENGTH 30

#define SENSOR_THROTTLE_TIME 50 // milliseconds

Sensor sensor(0x68, &Wire, SENSOR_THROTTLE_TIME);

char line_clear[LINE_CLEAR_LENGTH + 1];

/**
 * Stops code execution.
 */
void stop()
{
	while (true) {}
}

void setup()
{
	Serial.begin(9600);

	// Wait for Serial because the Arduino Leonardo is weird
	while (!Serial) {}

	while (!sensor.begin())
	{
		Serial.println("Error: Could not connect to the sensor. Retrying after 2000 "
					   "milliseconds...");
		delay(2000);
	}

	if (!sensor.setAccelSensitivity(2)) // 8g
	{
		Serial.print(
			"Error: Failed to set the sensor's accelerometer sensitivity. Status: ");
		Serial.println(static_cast<int>(sensor.getStatus()));
		stop();
	}

	if (!sensor.setGyroSensitivity(1)) // 500 degrees/s
	{
		Serial.print("Error: Failed to set the sensor's gyro sensitivity. Status: ");
		Serial.println(static_cast<int>(sensor.getStatus()));
		stop();
	}

	Serial.println("Calibrating sensor...");
	SensorCalibrator sensor_calibrator(&sensor);

	if (!sensor_calibrator.calibrate(SENSOR_THROTTLE_TIME))
	{
		Serial.print("Error: Sensor calibration timed out after ");
		Serial.print(CALIBRATION_TIMEOUT);
		Serial.println(" milliseconds");

		stop();
	}
	Serial.println("Finished calibrating sensor");

	Serial.println("Calibration values:");

	Serial.print("Accelerometer X: ");
	Serial.println(sensor.accel_cal_x);

	Serial.print("Accelerometer Y: ");
	Serial.println(sensor.accel_cal_y);

	Serial.print("Accelerometer Z: ");
	Serial.println(sensor.accel_cal_z);

	Serial.print("Gyro X: ");
	Serial.println(sensor.gyro_cal_x);

	Serial.print("Gyro Y: ");
	Serial.println(sensor.gyro_cal_y);

	Serial.print("Gyro Z: ");
	Serial.println(sensor.gyro_cal_z);

	Serial.println("\nStarting...");

	size_t prev_len = strlen(line_clear);
	memset(line_clear + prev_len, ' ', LINE_CLEAR_LENGTH - prev_len);
}

void loop()
{
	delay(SENSOR_THROTTLE_TIME);

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

		if (status == SensorStatus::THROTTLED)
		{
			Serial.println("Warning: The sensor was read too frequently and throttled");
			return;
		}

		Serial.print("Error: Failed to read sensor. Status: ");
		Serial.println(static_cast<int>(status));
		stop();
	}

	float pitch = sensor.getPitch();
	float roll = sensor.getRoll();

	Serial.print(line_clear);
	Serial.print("\r");
	Serial.print("Pitch: ");
	Serial.print(pitch, 3);
	Serial.print(" Roll: ");
	Serial.print(roll, 3);
	Serial.print("\r");
	Serial.flush();
}