#include <Wire.h>
#include <ADXL345.h>
#include <l3g4200.h>
#include <hmc5883l.h>
#include <bmp085.h>

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


hmc5883l compass;
ADXL345 acc;
int error;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  // 2000 degrees pr second max and +/- 16 g max
  Wire.begin();
  // 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);

  l3g4200DSetup(2000); // Configure L3G4200  - 250, 500 or 2000 deg/sec

  acc.begin();
  delay(1500); //wait for the sensors to be ready


}

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


  alt = bmp085_height;
  Serial.print("Pres and Temp ");
  Serial.print(temp);
  Serial.print(" C ");

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

}

void doCompass()
{
  // Retrive the raw values from the compass (not scaled).
  MagnetometerRaw raw = compass.ReadRawAxis();
  // Retriv
  const float alpha = 0.5;

  double fXg = 0;
  double fYg = 0;
  double fZg = 0;
  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 doAdxl345Acc(void)
{
}
void doGyro()
{
  // l3g4200d
  int x, y, z;
  l3g4200DGetGyroValues(&x, &y, &z);
  Serial.print("Gyro X:");
  Serial.print(x);

  Serial.print(" Y:");
  Serial.print(y);

  Serial.print(" Z:");
  Serial.println(z);
}
void loop() {
  // put your main code here, to run repeatedly:
  doAdxl345Acc();
  doGyro();
  doCompass();
  doPresAndTemp();

}
