// testprogram for gy88 gy88a
// Jens


#include <Wire.h>
#include <bmp085.h>
#include <MPU6050.h>
#include <HMC5883L.h>



#define BG(x) Serial.println(x)

// MPU6050 accelerometer, gyro 3 akser

//MPU6050 accelgyro(0x69);

MPU6050 accelgyro;


// 5883L magnetometer
HMC5883L magn;

void setup() {
  Serial.begin(9600);

  Serial.println("start");
  delay(1000);
  Wire.begin();  // start I2C bus der forbinder de intelligente sensorer


  bmp085Init(101300.0); // ress at sealevel today

  accelgyro.initialize();

  accelgyro.setI2CBypassEnabled(true); // open secondary bus to magenotometer

  magn.initialize();

  // verify connection tp 5883L
  Serial.println("Testing device connections...");
  delay(1000);

  Serial.println(magn.testConnection() ? "HMC5883L connection successful" : "HMC5883L connection failed");


  scanI2C();
  delay(2000); // wait 2 sek
}

void loop() {
  meaBMP085Print();
  meaMPU6050();
  meaMagn();


  Serial.println("");
  delay(100);


}

float tryk, temp, hojde;

void meaBMP085Print()
{

  bmp085Measure(&temp, &tryk, &hojde);

  Serial.print("temp(C) "); Serial.print(temp);
  Serial.print("\t tryk(Pa) "); Serial.print(tryk);
  Serial.print("\t hojde(m)"); Serial.print(hojde);
  Serial.print(" - ");
}


int16_t ax, ay, az;
int16_t gx, gy, gz;

void meaMPU6050()
{
  // read raw accel/gyro measurements from device
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // display tab-separated accel/gyro x/y/z values
  Serial.print("a/g:\t");
  Serial.print(ax); Serial.print("\t");
  Serial.print(ay); Serial.print("\t");
  Serial.print(az); Serial.print("\t");
  Serial.print(gx); Serial.print("\t");
  Serial.print(gy); Serial.print("\t");
  Serial.print(gz);

  Serial.print(" - ");
}

int16_t mx, my, mz;

void meaMagn()
{

  magn.getHeading(&mx, &my, &mz);

  // display tab-separated gyro x/y/z values
  Serial.print("mag:\t");
  Serial.print(mx); Serial.print("\t");
  Serial.print(my); Serial.print("\t");
  Serial.print(mz); Serial.print("\t");

  // To calculate heading in degrees. 0 degree indicates North
  float heading = atan2(my, mx);
  if (heading < 0)
    heading += 2 * M_PI;
  Serial.print("heading:\t");
  Serial.print(heading * 180 / M_PI);
  Serial.print(" - ");
}




void scanI2C()
{
  byte error, address;
  int nDevices;

  Serial.println("\n\n\n\nScanning for I2C devices...");

  nDevices = 0;
  for (address = 1; address < 127; address++ )
  {
    // The i2c_scanner uses the return value of
    // the Write.endTransmisstion to see if
    // a device did acknowledge to the address.
    Wire.beginTransmission(address);
    error = Wire.endTransmission();

    if (error == 0)
    {
      Serial.print("I2C device: 0x");
      if (address < 16)
        Serial.print("0");
      Serial.print(address, HEX);
      Serial.print("  (");
      Serial.print(address);
      Serial.print(") ");
      testSlave(address);
      Serial.println("");

      nDevices++;
    }
    else if (error == 4)
    {
      Serial.print("Unknow error at address 0x");
      if (address < 16)
        Serial.print("0");
      Serial.println(address, HEX);
    }
  }

  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else {
    Serial.print("found: "); Serial.print(nDevices); Serial.println(" devices");
  }
}
void testSlave(byte address)
{
  byte resp[10];


  switch (address) {
    case 0x68:  // mpu6050
    case 0x69:
      Wire.beginTransmission(address);
      Wire.write(byte(0x75));  // get id register on addr 0x75
      Wire.endTransmission();
      Wire.requestFrom(address, 1);
      resp[0] = Wire.read();
      Serial.print(" id: 0x"); Serial.print(resp[0], HEX); Serial.print(" (IMU mpu6050: 0x68)");
      break;
    case 0x76:  // bmp280
    case 0x77:  // bmp
      Wire.beginTransmission(address);
      Wire.write(byte(0xd0));  // get id registe on addr 0xd0 s
      Wire.endTransmission();
      Wire.requestFrom(address, 1);
      resp[0] = Wire.read();
      Serial.print(" id: 0x"); Serial.print(resp[0], HEX); Serial.print(" ("); Serial.print("bmp085: 0x55, bmp180: 0x55, bmp280: 0x58)");
      break;
    case 0x1e:  // hmc5983
      Wire.beginTransmission(address);
      Wire.write(byte(0x0a));  // get id registe on addr 0x75
      Wire.endTransmission();
      Wire.requestFrom(address, 3);
      resp[0] = Wire.read();
      resp[1] = Wire.read();
      resp[2] = Wire.read();
      Serial.print(" id: 0x"); Serial.print(resp[0], HEX); Serial.print(" ");
      Serial.print("0x"); Serial.print(resp[1], HEX); Serial.print(" ");
      Serial.print("0x"); Serial.print(resp[2], HEX); Serial.print(" ");


      Serial.print(" (HMC: 5983 0x48 0x34, 0x33)");
      break;


    default :;
  }

}