#include <FreeRTOS.h>

#include "SerialMulti.hpp"
CSerialMulti SerialM;

#include "bno085.hpp"
bno085mon bno085;

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  SerialM.begin(115200);
  bno085.begin();
}

void loop() {
  char c;
  while (SerialM.available()) {
    static bool led = false;
    digitalWrite(LED_BUILTIN, led ? HIGH : LOW);
    led = !led;

    switch (c = SerialM.read()) {
      case '\33':
        SerialM.printf("\x1b[2J");
        break;

      case '#':
        SerialM.restart();
        break;
      case '!':
        SerialM.usbboot();
        break;
      case 'M': // Almost all of the data that can be acquired is displayed on the terminal.
        SerialM.printf("%3d,%6d,%6d,%6d,%10d, ",
                  /*int8_t*/ bno085.Sensor.rawAccelerometer.status,
                  /*int16_t*/ bno085.Sensor.rawAccelerometer.value.x,
                  /*int16_t*/ bno085.Sensor.rawAccelerometer.value.y,
                  /*int16_t*/ bno085.Sensor.rawAccelerometer.value.z,
                  /*uint32_t*/ bno085.Sensor.rawAccelerometer.value.timestamp);


        SerialM.printf("%3d,%9.6f,%9.6f,%9.6f, ",
                  /*uint8_t*/ bno085.Sensor.accelerometer.status,
                  /*float*/ bno085.Sensor.accelerometer.value.x,
                  /*float*/ bno085.Sensor.accelerometer.value.y,
                  /*float*/ bno085.Sensor.accelerometer.value.z);


        SerialM.printf("%3d,%9.6f,%9.6f,%9.6f, ",
                  /*uint8_t*/ bno085.Sensor.linearAcceleration.status,
                  /*float*/ bno085.Sensor.linearAcceleration.value.x,
                  /*float*/ bno085.Sensor.linearAcceleration.value.y,
                  /*float*/ bno085.Sensor.linearAcceleration.value.z);


        SerialM.printf("%3d,%9.6f,%9.6f,%9.6f, ",
                  /*uint8_t*/ bno085.Sensor.gravity.status,
                  /*float*/ bno085.Sensor.gravity.value.x,
                  /*float*/ bno085.Sensor.gravity.value.y,
                  /*float*/ bno085.Sensor.gravity.value.z);


        SerialM.printf("%3d,%6d,%6d,%6d,%6d,%10d, ",
                  /*uint8_t*/ bno085.Sensor.rawGyroscope.status,
                  /*int16_t*/ bno085.Sensor.rawGyroscope.value.x,
                  /*int16_t*/ bno085.Sensor.rawGyroscope.value.y,
                  /*int16_t*/ bno085.Sensor.rawGyroscope.value.z,
                  /*int16_t*/ bno085.Sensor.rawGyroscope.value.temperature,
                  /*uint32_t*/ bno085.Sensor.rawGyroscope.value.timestamp);


        SerialM.printf("%3d,%9.6f,%9.6f,%9.6f, ",
                  /*uint8_t*/ bno085.Sensor.gyroscope.status,
                  /*float*/ bno085.Sensor.gyroscope.value.x,
                  /*float*/ bno085.Sensor.gyroscope.value.y,
                  /*float*/ bno085.Sensor.gyroscope.value.z);


        SerialM.printf("%3d,%9.6f,%9.6f,%9.6f,%9.6f,%9.6f,%9.6f, ",
                  /*uint8_t*/ bno085.Sensor.gyroscopeUncal.status,
                  /*float*/ bno085.Sensor.gyroscopeUncal.value.x,
                  /*float*/ bno085.Sensor.gyroscopeUncal.value.y,
                  /*float*/ bno085.Sensor.gyroscopeUncal.value.z,
                  /*float*/ bno085.Sensor.gyroscopeUncal.value.biasX,
                  /*float*/ bno085.Sensor.gyroscopeUncal.value.biasY,
                  /*float*/ bno085.Sensor.gyroscopeUncal.value.biasZ);


        SerialM.printf("%3d,%9.6f,%9.6f,%9.6f,%10d, ",
                  /*uint8_t*/ bno085.Sensor.rawMagnetometer.status,
                  /*int16_t*/ bno085.Sensor.rawMagnetometer.value.x,
                  /*int16_t*/ bno085.Sensor.rawMagnetometer.value.y,
                  /*int16_t*/ bno085.Sensor.rawMagnetometer.value.z,
                  /*uint32_t*/ bno085.Sensor.rawMagnetometer.value.timestamp);


        SerialM.printf("%3d,%9.6f,%9.6f,%9.6f, ",
                  /*uint8_t*/ bno085.Sensor.magneticField.status,
                  /*float*/ bno085.Sensor.magneticField.value.x,
                  /*float*/ bno085.Sensor.magneticField.value.y,
                  /*float*/ bno085.Sensor.magneticField.value.z);


        SerialM.printf("%3d,%9.6f,%9.6f,%9.6f,%9.6f,%9.6f,%9.6f, ",
                  /*uint8_t*/ bno085.Sensor.magneticFieldUncal.status,
                  /*float*/ bno085.Sensor.magneticFieldUncal.value.x,
                  /*float*/ bno085.Sensor.magneticFieldUncal.value.y,
                  /*float*/ bno085.Sensor.magneticFieldUncal.value.z,
                  /*float*/ bno085.Sensor.magneticFieldUncal.value.biasX,
                  /*float*/ bno085.Sensor.magneticFieldUncal.value.biasY,
                  /*float*/ bno085.Sensor.magneticFieldUncal.value.biasZ);


        SerialM.printf("%3d,%9.6f,%9.6f,%9.6f,%9.6f,%9.6f, ",
                  /*uint8_t*/ bno085.Sensor.rotationVector.status,
                  /*float*/ bno085.Sensor.rotationVector.value.i,
                  /*float*/ bno085.Sensor.rotationVector.value.j,
                  /*float*/ bno085.Sensor.rotationVector.value.k,
                  /*float*/ bno085.Sensor.rotationVector.value.real,
                  /*float*/ bno085.Sensor.rotationVector.value.accuracy);


        SerialM.printf("%3d,%9.6f,%9.6f,%9.6f,%9.6f, ",
                  /*uint8_t*/ bno085.Sensor.gameRotationVector.status,
                  /*float*/ bno085.Sensor.gameRotationVector.value.i,
                  /*float*/ bno085.Sensor.gameRotationVector.value.j,
                  /*float*/ bno085.Sensor.gameRotationVector.value.k,
                  /*float*/ bno085.Sensor.gameRotationVector.value.real);


        SerialM.printf("%3d,%9.6f,%9.6f,%9.6f,%9.6f,%9.6f",
                  /*uint8_t*/ bno085.Sensor.geoMagRotationVector.status,
                  /*float*/ bno085.Sensor.geoMagRotationVector.value.i,
                  /*float*/ bno085.Sensor.geoMagRotationVector.value.j,
                  /*float*/ bno085.Sensor.geoMagRotationVector.value.k,
                  /*float*/ bno085.Sensor.geoMagRotationVector.value.real,
                  /*float*/ bno085.Sensor.geoMagRotationVector.value.accuracy);

        SerialM.printf("\n");
        break;
    }
    SerialM.flush();
    __WFI();  // wait for Interrupt
  }
}
