#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;

unsigned long t1=0,t2=0;

void loop()
{
  //  goto xxx;

  t2 = millis();
  if (1000 <= (t2-t1))
  {
    t1 = t2;

    /* 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();

    printDateTime(gps.date, gps.time);

    printFloat(gps.location.lat(), gps.location.isValid(), 11, 6);
    printFloat(gps.location.lng(), gps.location.isValid(), 12, 6);
    //      printInt(gps.location.age(), gps.location.isValid(), 5);
    printFloat(gps.altitude.meters(), gps.altitude.isValid(), 7, 2);

    Serial.println(" !");

  }
xxx:
  /* GPS */
  while  (Serial1.available() > 0)  {
    gps.encode(Serial1.read());
  }



}

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

#endif
  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());
  }

#ifdef NEVER
  else
  {
    Serial.print(F("INVALID"));
  }
#endif

  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());
  }
#ifdef NEVER
  else
  {
    Serial.print(F("INVALID"));
  }
#endif
}



static void printDateTime(TinyGPSDate &d, TinyGPSTime &t)
{
  if (!d.isValid())
  {
    Serial.print(F("********** "));
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d/%02d/%02d ", d.month(), d.day(), d.year());
    Serial.print(sz);
  }

  if (!t.isValid())
  {
    Serial.print(F("******** "));
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d:%02d:%02d ", t.hour(), t.minute(), t.second());
    Serial.print(sz);
  }

  printInt(d.age(), d.isValid(), 5);
}


static void printInt(unsigned long val, bool valid, int len)
{
  char sz[32] = "*****************";
  if (valid)
    sprintf(sz, "%ld", val);
  sz[len] = 0;
  for (int i=strlen(sz); i<len; ++i)
    sz[i] = ' ';
  if (len > 0) 
    sz[len-1] = ' ';
  Serial.print(sz);

}



static void printFloat(float val, bool valid, int len, int prec)
{
  if (!valid)
  {
    while (len-- > 1)
      Serial.print('*');
    Serial.print(' ');
  }
  else
  {
    Serial.print(val, prec);
    int vi = abs((int)val);
    int flen = prec + (val < 0.0 ? 2 : 1); // . and -
    flen += vi >= 1000 ? 4 : vi >= 100 ? 3 : vi >= 10 ? 2 : 1;
    for (int i=flen; i<len; ++i)
      Serial.print(' ');
  }
}



