#include <GY91.h>

#if defined(__arm__) && defined(CORE_TEENSY)
// Setup for NAROM 2018 kit
#elif defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__)
// Setup for Cansat v6 kit
#endif

GY91 gy91;
double ax, ay, az, gx, gy, gz, mx, my, mz, pressure;
int ntc;

void setup() {
  Serial.begin(9600);
  Serial.println(" time (ms) ,  ntc  , ax  ,  ay   , az   , |a| , gx  , gy  , gz  , |g|  , mx  , my  , mz , |m| , pressure ");
  
  while(!Serial);
  
  if (!gy91.init()) {
    Serial.println("Could not initiate");
    while(1);
  }
}

void loop() {
  gy91.read_acc();
  gy91.read_gyro();
  gy91.read_mag();

  pressure = gy91.readPressure();

  ax = gy91.ax;
  ay = gy91.ay;
  az = gy91.az;

  gx = gy91.gx;
  gy = gy91.gy;
  gz = gy91.gz;

  mx = gy91.mx;
  my = gy91.my;
  mz = gy91.mz;

  Serial.print(millis());        // 1
  Serial.print(",\t\t ");

  ntc = analogRead(A0);
  Serial.print(ntc);
  Serial.print(", ");
  print_def_width(ax, 8, 5);   // 2
  Serial.print(", ");
  print_def_width(ay, 8, 5);   // 3
  Serial.print(", ");
  print_def_width(az, 8, 5);   // 4
  Serial.print(", ");
  print_def_width(sqrt(ax*ax + ay*ay + az*az), 8, 5);    // 5
  Serial.print(", \t\t");

  print_def_width(gx, 8, 3);   // 6
  Serial.print(", ");
  print_def_width(gy, 8, 3);   // 7
  Serial.print(", ");
  print_def_width(gz, 8, 3);   // 8
  Serial.print(", ");
  print_def_width(sqrt(gx*gx + gy*gy + gz*gz), 8, 3);     // 9
  Serial.print(",\t\t");

  print_def_width(mx, 6, 1);     // 10
  Serial.print(", ");
  print_def_width(my, 6, 1);   // 11
  Serial.print(", ");
  print_def_width(mz, 6, 1);   // 12
  Serial.print(", ");
  print_def_width(sqrt(mx*mx + my*my + mz*mz), 6, 1);    // 13
  Serial.print(",\t\t");

  print_def_width(pressure, 8, 2);   // 14
  
  Serial.println(";");

  delay(1000);
}

void print_def_width(float data, int width_, int length_commas) {
  char buffer[16];
  
  dtostrf(data, width_, length_commas, buffer);
  Serial.print(buffer);
}