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

#include "I2Cdev.h"

// afdjust to the pressure of today at sealevel for correct altitude
#define PRES_0M 101325.0

MPU6050 mpu;
Vector rawAccel, normAccel, rawGyro, normGyro;
hmc5883l compass;

int error;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  // 2000 degrees pr second max and +/- 16 g max
  mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_16G);
  mpu.setI2CBypassEnabled(true); // set bypass mode
  // now we can reach hmc5883l
  compass = hmc5883l(); // Construct a new HMC5883 compass.

  Serial.println("Setting scale to +/- 1.3 Ga");
  error = compass.SetScale(1.3); // Set the scale of the compass.
  error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous

  bmp085_init();


}

void doPresAndTemp()
{
  float temp, atm, alt;
  long pres;
  //  bmp085_init();
  temp = bmp085Temp();
  pres = bmp085Pressure();

  atm = pres / PRES_0M; // "standard atmosphere"
  alt = bmp085PascalToMeter(pres, PRES_0M);
  Serial.print("Pres and Temp ");
  Serial.print(temp);
  Serial.print(" C ");

  Serial.print(pres);
  Serial.print(" Pa ");

  Serial.print(atm);
  Serial.print(" (rel atm) ");

  Serial.print(alt);
  Serial.println( " m ");

}









void doAcceleration()
{
  rawAccel = mpu.readRawAccel();
  normAccel = mpu.readNormalizeAccel();
  Serial.print("Acc  ");
  Serial.print(" Xraw = ");
  Serial.print(rawAccel.XAxis);
  Serial.print(" Yraw = ");
  Serial.print(rawAccel.YAxis);
  Serial.print(" Zraw = ");

  Serial.print(rawAccel.ZAxis);
  Serial.print(" Xnorm = ");
  Serial.print(normAccel.XAxis);
  Serial.print(" Ynorm = ");
  Serial.print(normAccel.YAxis);
  Serial.print(" Znorm = ");
  Serial.println(normAccel.ZAxis);
}

void doGyro()
{
  rawGyro = mpu.readRawGyro();
  normGyro = mpu.readNormalizeGyro();
  Serial.print("Gyro ");
  Serial.print(" Xraw = ");
  Serial.print(rawGyro.XAxis);
  Serial.print(" Yraw = ");
  Serial.print(rawGyro.YAxis);
  Serial.print(" Zraw = ");
  Serial.print(rawGyro.ZAxis);

  Serial.print(" Xnorm = ");
  Serial.print(normGyro.XAxis);
  Serial.print(" Ynorm = ");
  Serial.print(normGyro.YAxis);
  Serial.print(" Znorm = ");
  Serial.println(normGyro.ZAxis);

}

void doCompass()
{
  // Retrive the raw values from the compass (not scaled).
  MagnetometerRaw raw = compass.ReadRawAxis();
  // Retrived the scaled values from the compass (scaled to the configured scale).
  MagnetometerScaled scaled = compass.ReadScaledAxis();

  // Values are accessed like so:
  int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)

  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  float heading = atan2(scaled.YAxis, scaled.XAxis);

  // Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
  // Find yours here: http://www.magnetic-declination.com/
  // Mine is: 2� 37' W, which is 2.617 Degrees, or (which we need) 0.0456752665 radians, I will use 0.0457
  // If you cannot find your Declination, comment out these two lines, your compass will be slightly off.
  float declinationAngle = 0.0457;
  heading += declinationAngle;

  // Correct for when signs are reversed.
  if (heading < 0)
    heading += 2 * PI;

  // Check for wrap due to addition of declination.
  if (heading > 2 * PI)
    heading -= 2 * PI;

  // Convert radians to degrees for readability.
  float headingDegrees = heading * 180 / M_PI;

  // Output the data via the serial port.
  Output(raw, scaled, heading, headingDegrees);

  // Normally we would delay the application by 66ms to allow the loop
  // to run at 15Hz (default bandwidth for the hmc5883l).
  // However since we have a long serial out (104ms at 9600) we will let
  // it run at its natural speed.
  // delay(66);
}

// Output the data down the serial port.
void Output(MagnetometerRaw raw, MagnetometerScaled scaled, float heading, float headingDegrees)
{
  Serial.print("Cmps  #include <bmp085.h>Raw:\t");
  Serial.print(raw.XAxis);
  Serial.print("   ");
  Serial.print(raw.YAxis);
  Serial.print("   ");
  Serial.print(raw.ZAxis);
  Serial.print("   \tScaled:\t");

  Serial.print(scaled.XAxis);
  Serial.print("   ");
  Serial.print(scaled.YAxis);
  Serial.print("   ");
  Serial.print(scaled.ZAxis);

  Serial.print("   \tHeading:\t");
  Serial.print(heading);
  Serial.print(" Radians   \t");
  Serial.print(headingDegrees);
  Serial.println(" Degrees   \t");
}

void loop() {
  // put your main code here, to run repeatedly:
  doAcceleration();
  doGyro();
  doCompass();
  doPresAndTemp();
}