#include <looptime.h>
#include <TinyGPS.h>
#include <l3g4200.h>
#include <hmc5883l.h>
#include <adxl345.h>
 
#include <bmp085.h>

#include <Wire.h>

/* (C) Jens Dalsgaard Nielsen, AAU, 2014
* Free to use, copy modify etc
* NO warranty whatsoever
*/

hmc5883l compass;
adxl345 adxl;
TinyGPSPlus gps;



void init_gyro()
{
  l3g4200DSetup(500); // Configure L3G4200  - 250, 500 or 2000 deg/sec
  delay(1500); //wait for the sensor to be ready 
}

void init_acc()
{
    adxl.powerOn();
}

void  init_compass()
{
  int error;
  compass = hmc5883l(); // Construct a new HMC5883 compass.

  error = compass.SetScale(1.3); // Set the scale of the compass.
  
  if(error != 0) // If there is an error, print it out.
    Serial.println(compass.GetErrorText(error));
    
  error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous

  if(error != 0) // If there is an error, print it out.
    Serial.println(compass.GetErrorText(error));
}

void init_pressure()
{
  bmp085Calibration();
}


void init_GPS()
{
  Serial1.begin(9600);
}

void setup()
{
  Serial.begin(9600);
  
  Wire.begin();  // start i2c bus - forbindelse til dy80 board
  
  init_pressure();
  
  init_acc();  

  init_compass();
  
  init_gyro();
  init_GPS();

  return;
  
}


// Output the data down the serial port.
void magOutput(MagnetometerRaw raw, MagnetometerScaled scaled, float heading, float headingDegrees)
{
   Serial.print("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(" Heading: ");
   Serial.print(heading);
   Serial.print(" Radians ");
   Serial.print(headingDegrees);
   Serial.print(" Degr");
}

/* vars for compass */
MagnetometerScaled scaled;
MagnetometerRaw raw;
int MilliGauss_OnThe_XAxis; 
float heading; 
float declinationAngle;
float headingDegrees;

void hmc5883mea()
{
   // Retrive the raw values from the compass (not scaled).
  raw = compass.ReadRawAxis();
  // Retrived the scaled values from the compass (scaled to the configured scale).
  scaled = compass.ReadScaledAxis();
  
  // Values are accessed like so:
  MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)

  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  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.
  declinationAngle = 0.0; // Jens 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.
  headingDegrees = heading * 180/M_PI; 

  // Output the data via the serial port.
  magOutput(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.
 }

/* variable for trykmaaler */

float temperature, pressure, atm, altitud;

/* og for gyro */
int gyrox, gyroy, gyroz;

/* variable for accelerometer */
int accx, accy,accz;

void loop()
{
  /* MAAL TRYK OG TEMP */
  temperature = bmp085GetTemperature(bmp085ReadUT()); //MUST be called first
  pressure = bmp085GetPressure(bmp085ReadUP());
  atm = pressure / 101325; // "standard atmosphere"
  altitud = calcAltitude(pressure,101400); //Uncompensated caculation - in Meters 
  
  Serial.print(temperature); Serial.print(" C ");
  Serial.print(pressure); Serial.print(" Pas ");
  Serial.print(altitud); Serial.print(" m  ");
  
  /* Accelerometer */
  adxl.readAccel(&accx, &accy, &accz);
  
  Serial.print(accx*3.9); Serial.print(" ");  // 3.9 mg /LSB (dvs pr bit saa ren skalering)
  Serial.print(accy*3.9); Serial.print(" ");
  Serial.print(accz*3.9); Serial.print(" mG  ");
  
  
  /* gyro */
  l3g4200DGetGyroValues(&gyrox,&gyroy,&gyroz);  // This will update x, y, and z with new values
  
  Serial.print(gyrox); Serial.print(" ");
  Serial.print(gyroy); Serial.print(" ");
  Serial.print(gyroz); Serial.print(" degr/sec");
  
  /* COMPASS*/
  
  hmc5883mea();
 
 
   /* GPS */
  while  (Serial1.available() > 0)  {
    if (gps.encode(Serial1.read()))
      gps_displayInfo();
  }
   xx:

  Serial.println(" !");

  looptime(1000); 
  
}

void gps_displayInfo()
{
  Serial.print(F("Location: ")); 
  if (gps.location.isValid())
  {
    Serial.print(gps.location.lat(), 6);
    Serial.print(F(","));
    Serial.print(gps.location.lng(), 6);
  }
  else
  {
    Serial.print(F("INVALID"));
  }

  Serial.print(F("  Date/Time: "));
  if (gps.date.isValid())
  {
    Serial.print(gps.date.month());
    Serial.print(F("/"));
    Serial.print(gps.date.day());
    Serial.print(F("/"));
    Serial.print(gps.date.year());
  }
  else
  {
    Serial.print(F("INVALID"));
  }

  Serial.print(F(" "));
  if (gps.time.isValid())
  {
    if (gps.time.hour() < 10) Serial.print(F("0"));
    Serial.print(gps.time.hour());
    Serial.print(F(":"));
    if (gps.time.minute() < 10) Serial.print(F("0"));
    Serial.print(gps.time.minute());
    Serial.print(F(":"));
    if (gps.time.second() < 10) Serial.print(F("0"));
    Serial.print(gps.time.second());
    Serial.print(F("."));
    if (gps.time.centisecond() < 10) Serial.print(F("0"));
    Serial.print(gps.time.centisecond());
  }
  else
  {
    Serial.print(F("INVALID"));
  }

}