//JDN

#include <Wire.h>
#include <mpu606x.h>
#include <bmp280.h>

#define SP(x) Serial.print(x)
#define SPLN(x) Serial.println(x)
#define SPH(x) Serial.print(x, HEX)
#define SPHLN(x) Serial.println(x, HEX)

#define MPU 0x68
#define BMP 0x76

BMP280 bmp280(BMP);


// max for acc and gyro is an integer: -32768 -> 32767
//
// so acc config to +-8G means that -327678  == -8G etc
// accelerometer config
// 0x18 +- 16G
// 0x10 +-8 G
// 0x08 +-4
// 0x00 +-2

// gyro config
// Full scale range = 2000 dps
// 0x18 +- 2000 degr/sec
// 0x10 +- 1000 gr/sec
// 0x08 +- 500 gr/Sec
// 0x00 +- 250 gr/sec
unsigned long nextT, tStep = 40;  // 50 msec


float todayPres = 102800.00;


void helloInfo() {
  delay(100);
  SPLN(__FILE__);
  SPLN(__DATE__);
  SPLN(" mpu606x, bmp280");
  SPLN(todayPres);
  SPLN("accx accy accz gytox gyroy gyroz temp tryk hojde");
  SP("tPeriod(msec) ");
  SPLN(tStep);
  SP("mpu 0x");
  SPH(MPU);
  SP(" BMP 0x");
  SPHLN(BMP);
  SP("tPeriod(msec) ");
  SPLN(tStep);
}

void setup() {

  Serial.begin(57600);
  helloInfo();
  delay(2000);

  Wire.begin();

  mpu606xInit(MPU);
  mpu606xConfigAcc(0x08);
  mpu606xConfigGyro(0x08);

  bmp280.begin();



  nextT = millis() + tStep;
}

int x, y, z, gx, gy, gz;
float temp, tryk, hojde;
uint32_t press;
void loop() {

  if (nextT <= millis()) {
    nextT += tStep;

    mpu606XgetAcc(&x, &y, &z);
    mpu606XgetGyro(&gx, &gy, &gz);
    hojde = bmp280.calAltitudeF(press, todayPres);
    press = bmp280.getPressure();
    temp  = bmp280.getTemperature();

    SP(millis());
    SP(" \t");

    SP(x);
    SP(" \t");
    SP(y);
    SP(" \t");
    SP(z);
    SP(" \t");


    SP(gx);
    SP(" \t");
    SP(gy);
    SP(" \t");
    SP(gz);
    SP("\t");



    SP(" \t");
    Serial.print(temp);
    SP(" \t");
    Serial.print(press);
    SP(" \t");

    Serial.print(hojde);
    SPLN();


    // put your main code here, to run repeatedly:
  }
}