#include <Wire.h>
#include <Adafruit_RGBLCDShield.h>
#include <utility/Adafruit_MCP23017.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
#include <Filters.h>

/*create a magnetometer object - needa a unique id*/
Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(12345);
/* create a display object*/
Adafruit_RGBLCDShield lcd = Adafruit_RGBLCDShield();

/*variables to hold the maximum and minimum for calibration*/
float MagMinX, MagMaxX;
float MagMinY, MagMaxY;

/*filters help reduce the noise from the magnetometer
We found that 1 is a good frequency, but you may want to try others*/
const float filterFreq = 1;
FilterOnePole filterx(LOWPASS, filterFreq);
FilterOnePole filtery(LOWPASS, filterFreq);

const float Pi = 3.14159;

/*other variables that need to be declared outside the loop*/
float lastDisplayTime = 0;
float saved_heading = NULL;

void setup(void){
  /* set up the display. 2 rows of 16 characters each*/
  lcd.begin(16, 2);
  lcd.clear();
  /**/
  lcd.setCursor(0, 0);
  lcd.print("Hello");

  /*set up serial communication. This isn't necessary but helps with debugging.
  You can add your own Serial.print() statements as needed*/
  Serial.begin(9600);

  /* Initialise the magnetometer */
  if (!mag.begin()){
    /* There was a problem detecting the LSM303 ... check your connections */
    Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
    lcd.print("Error");
    while (1)
      ;
  }

  /*initialize the pin that the buzzer is connected to*/
  pinMode(11, OUTPUT);
}

void loop(void){
  unsigned long thetime = millis();

  /*this is how you save data from the magnetometer*/
  sensors_event_t magEvent;
  mag.getEvent(&magEvent);
  
  /*add the needed data to the filter*/
  filterx.input(magEvent.magnetic.x);
  filtery.input(magEvent.magnetic.y);

  /*compute the filtered magnetometer reading*/
  float b_x = filterx.output();
  float b_y = filtery.output();

  /*if you don't want to use the filter, use this code instead*/
  //float b_x = magEvent.magnetic.x;
  //float b_y = magEvent.magnetic.y;

  /*if the program has been running for less that 5 seconds*/
  if (thetime < 5000){
    /*don't do anything. Give the filter some time to settle*/

    if ((thetime - lastDisplayTime) > 1000){ // display once/second
      lastDisplayTime = thetime;
    }

    /*silence the buzzer*/
    analogWrite(11,0);
  }else if (thetime < 20000){
    /*if the program has been running for less than 20 seconds*/

    /*figure out the maximum and minimum values the magnetometer reads*/
    if (b_x < MagMinX){
      MagMinX = b_x;
    }
    else if (b_x > MagMaxX){
      MagMaxX = b_x;
    }

    if (b_y < MagMinY){
      MagMinY = b_y;
    }
    else if (b_y > MagMaxY){
      MagMaxY = b_y;
    }
    /*In theory, the minimum and maximum values should be the same magnitude in opposite directions.
    In practice, sometimes they'll be different. This will throw off your heading calculation if left uncorrected.*/


    /*send instructions to the lcd display once per second. Tell the user to rotate the magnetometer around in a circle to record the full range of b_x and b_y*/
    if (thetime - lastDisplayTime > 1000){
      lastDisplayTime = thetime;
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Calibrating");
      lcd.setCursor(0, 1);
      lcd.print("Rotate sensor");
      /*Serial.print("Mag Minimums: ");Serial.print(MagMinX);Serial.print("  ");Serial.print(MagMinY);Serial.println();
      Serial.print("Mag Maximums: ");Serial.print(MagMaxX);Serial.print("  ");Serial.print(MagMaxY);Serial.println();
      Serial.println();*/
    }
  }else{
    /*20 seconds into the program, calibration is done. Use the max and min values you recorded to offset the sensor correctly*/
    float X_off = (MagMaxX + MagMinX) / 2;
    float Y_off = (MagMaxY + MagMinY) / 2;

    /* Compute the heading from b_x and b_y.*/
    float heading = (atan2(b_y - Y_off, b_x - X_off) * 180) / Pi;
    /*atan2 returns and angle from -180 degrees to 180 degrees. For the user, normalize this to display 0-360 instead.*/
    float n_heading = heading;
    if (n_heading < 0){
      n_heading = 360 + n_heading;
    }
    
    if (thetime - lastDisplayTime > 1000){
      lcd.clear();
      /* alert user whenever heading changes two degrees from the saved heading */
      if(saved_heading){
         /*if there is a saved heading, you want to alert the user when the current heading changes by more than 2 degrees*/
         String disp = "DEFLECTS:"+String(heading-saved_heading);
         lcd.setCursor(0, 0);
         lcd.print(disp);
         if(abs(heading-saved_heading) > 2){
          lcd.setCursor(0, 1);
          lcd.print("TEST FAILED");
        }
      }else{
        /*if there's no saved heading, just display the raw heading*/
        lcd.setCursor(0, 0);
        lcd.print("HEADING:"+String(n_heading));
        lcd.setCursor(0, 1);
        lcd.print("PRESS BTN TO 0");
      }

      lastDisplayTime = thetime;
    }

    /*make the buzzer separate from the display, otherwise the buzzer will only update once/second*/
    if(saved_heading){
      if (abs(heading-saved_heading) > 2){
        /*buzz*/
        analogWrite(11,200);
      }else{
        /*silence*/
        analogWrite(11,0);
      }
    }

    /*this makes the arduino read the buttons on the lcd screen*/
    uint8_t buttons = lcd.readButtons();
    if (buttons){
      if (buttons & BUTTON_SELECT){
        /*this essentially "zeros" the sensor*/
        saved_heading = heading;
      }

    }
  }
}