#include <I2Cdev.h>

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

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

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




void doPresAndTemp()
{
  float temp, atm, alt;
  long pres;

  bmp085_measure();
  temp = bmp085_temp;
  pres = bmp085_pres;


  alt = bmp085_height;
  Serial.print("P/T/alt  ");
  Serial.print(temp);
  Serial.print(" C  ");

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

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

}


void doAcceleration()
{
  rawAccel = mpu.readRawAccel();
  normAccel = mpu.readNormalizeAccel();
/* JDN
  Serial.print("Acc(Raw) ");
  Serial.print(rawAccel.XAxis);
  Serial.print("  ");
  Serial.print(rawAccel.YAxis);
  Serial.print("  ");
  Serial.print(rawAccel.ZAxis);
  Serial.print("  ");
JDN */
  Serial.print("Acc(norm)   ");
  Serial.print(normAccel.XAxis);
  Serial.print("  ");
  Serial.print(normAccel.YAxis);
  Serial.print("  ");
  Serial.print(normAccel.ZAxis);
  Serial.print("  ");
}

void doGyro()
{
  rawGyro = mpu.readRawGyro();
  normGyro = mpu.readNormalizeGyro();
/* JDN
  Serial.print("Gyro(Raw) ");
  Serial.print(rawGyro.XAxis);
  Serial.print("  ");
  Serial.print(rawGyro.YAxis);
  Serial.print("  ");
  Serial.print(rawGyro.ZAxis);
  Serial.print("  ");
JDN */

  Serial.print("Norm ");
  Serial.print(normGyro.XAxis);
  Serial.print("  ");
  Serial.print(normGyro.YAxis);
  Serial.print("  ");
  Serial.print(normGyro.ZAxis);
  Serial.print("  ");
}


// Output the data down the serial port.
void Outputt(MagnetometerRaw raw, MagnetometerScaled scaled, float heading, float headingDegrees)
{
  /* JDN
  Serial.print("Cmps(Raw) ");
  Serial.print(raw.XAxis);
  Serial.print("  ");
  Serial.print(raw.YAxis);
  Serial.print("  ");
  Serial.print(raw.ZAxis);
  Serial.print("  ");

  Serial.print("Scaled ");
  Serial.print(scaled.XAxis);
  Serial.print("  ");
  Serial.print(scaled.YAxis);
  Serial.print("  ");
  Serial.print(scaled.ZAxis);
  Serial.print("  ");
  JDN */

  Serial.print("Heading ");
  Serial.print(heading);
  Serial.print(" (Rad) ");
  Serial.print(headingDegrees);
  Serial.print(" (Degr) ");
}


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.
  Outputt(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);
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  delay(1000);
  // 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(0.0);


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

  doAcceleration();
  doGyro();

  Serial.println(" ");
}
