#include <Arduino.h>
#include <BMP280_DEV.h>
#include <MPU9250.h>
#include "ultrasonic.hpp"

#define LOOP_INTERVAL_US (12 * 1000)    // 12 * 1000us

MPU9250 mpu;

void blinkLed() {
  digitalWrite(LED_BUILTIN, (millis()>>7) & 1);
}

void setup() {
  Serial.begin(1000000);
  Serial.println(F("boot"));
  pinMode(LED_BUILTIN, OUTPUT);
  
  //initialize acustic sensor
  us_init();

  //initialize I2C MPU
  Wire.begin();
  delay(2000);

  Serial.println(F("setup mpu"));
  mpu.setup(0x68); 
  delay(1000);

  Serial.println(F("calibrate accel/gyro"));
  mpu.calibrateAccelGyro();

  Serial.println(F("FIELDS:\tUS_0\tUS_1\tMAG_X\tMAG_Y\tMAG_Z\tACCEL_X\tACCEL_Y\tACCEL_Z\tGYRO_X\tGYRO_Y\tGYRO_Z\tTEMP\tEXEC_TIME"));

}

unsigned long loopStart = 0;
char outputBuffer[256];

void loop() {

  if(micros() >= loopStart + LOOP_INTERVAL_US) {
    loopStart = micros();

    //emit ultrasound pulse
    us_transmit();

    //get mpu values
    mpu.update();

    snprintf(outputBuffer, sizeof(outputBuffer), "DATA:\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\t%ld\r\n",
      //acustic RTT 
      us_get_duration(0), us_get_duration(1),
      //magnetic field
      (long)(mpu.getMagX()*1000),
      (long)(mpu.getMagY()*1000),
      (long)(mpu.getMagZ()*1000),
      //accelerometer
      (long)(mpu.getAccX()*1000),
      (long)(mpu.getAccY()*1000),
      (long)(mpu.getAccZ()*1000),
      //gyroscope
      (long)(mpu.getGyroX()*1000),
      (long)(mpu.getGyroY()*1000),
      (long)(mpu.getGyroZ()*1000),
      //temperature
      (long)(mpu.getTemperature()*1000),
      //execution time in microseconds
      micros() - loopStart
    );
    Serial.write(outputBuffer);

    blinkLed();

  }

}