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

Sensor sensor(0x68, &Wire);

/**
 * 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.print(millis());
		Serial.println("Error: Could not connect to the sensor. Retrying after 2000 milliseconds...");
		delay(2000);
	}
	sensor.setAccelSensitivity(2); // 8g
	sensor.setGyroSensitivity(1);  // 500 degrees/s

	sensor.setThrottleEnabled(true);

	Serial.println("Automatically calibrating sensor...");
	bool cal_status = sensor.autoCalibrate();

	if (!cal_status)
	{
		Serial.print("Error: Automatic calibration timed out after ");
		Serial.print(CALIBRATION_TIMEOUT);
		Serial.println(" milliseconds");

		stop();
	}

	Serial.println("Finished calibrating");

	Serial.println("Starting...");
}

void loop()
{
	if (!sensor.is_calibrated)
	{
		return;
	}

	sensor.read();

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

	Serial.print("Pitch: ");
	Serial.print(pitch, 3);
	Serial.print(" Roll: ");
	Serial.print(roll, 3);
	Serial.println();
}