#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <QMC5883LCompass.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);
QMC5883LCompass compass;

const int WINDOW_SIZE = 10;
float values[WINDOW_SIZE];
int valueIndex = 0;
float baselineValue = 0;
float filteredValue = 0;
float alpha = 0.2;
float decay = 0.05;
int maxValue = 0;
const int CALIBRATE_BTN = 2;

void calibrate() {
  lcd.clear();
  lcd.print("Calibrating...");
  
  float sumX = 0, sumY = 0, sumZ = 0;
  for(int i = 0; i < 20; i++) {
    compass.read();
    sumX += compass.getX();
    sumY += compass.getY();
    sumZ += compass.getZ();
    delay(20);
  }
  
  baselineValue = sqrt(pow(sumX/20, 2) + pow(sumY/20, 2) + pow(sumZ/20, 2));
  
  for(int i = 0; i < WINDOW_SIZE; i++) {
    values[i] = 0;
  }
  
  filteredValue = 0;
  maxValue = 0;
  
  lcd.clear();
  lcd.print("Ready!");
  delay(500);
}

void setup() {
  Wire.begin();
  lcd.begin(16, 2);
  lcd.init();
  lcd.backlight();
  
  pinMode(CALIBRATE_BTN, INPUT_PULLUP);
  
  compass.init();
  compass.setMode(0x01, 0x0C, 0x10, 0x00);
  
  Serial.begin(9600);
  calibrate();
}

void loop() {
  if(digitalRead(CALIBRATE_BTN) == LOW) {
    delay(50);
    if(digitalRead(CALIBRATE_BTN) == LOW) {
      calibrate();
      while(digitalRead(CALIBRATE_BTN) == LOW);
    }
  }
  
  compass.read();
  float x = compass.getX();
  float y = compass.getY();
  float z = compass.getZ();
  
  float currentMagnitude = sqrt(x*x + y*y + z*z);
  float relativeMagnitude = abs(currentMagnitude - baselineValue);
  
  values[valueIndex] = relativeMagnitude;
  valueIndex = (valueIndex + 1) % WINDOW_SIZE;
  
  float sum = 0;
  for(int i = 0; i < WINDOW_SIZE; i++) {
    sum += values[i];
  }
  float avgMagnitude = sum / WINDOW_SIZE;
  
  if(avgMagnitude > filteredValue) {
    filteredValue = (alpha * avgMagnitude) + ((1 - alpha) * filteredValue);
  } else {
    filteredValue = filteredValue * (1.0 - decay);
  }
  
  int displayValue = map(filteredValue, 0, 1000, 0, 100);
  displayValue = constrain(displayValue, 0, 100);
  
  if(displayValue > maxValue) {
    maxValue = displayValue;
  }
  
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("EMF: ");
  lcd.print(displayValue);
  lcd.print("%  M:");
  lcd.print(maxValue);
  
  lcd.setCursor(0, 1);
  int barLength = map(displayValue, 0, 100, 0, 16);
  for(int i = 0; i < 16; i++) {
    lcd.write(i < barLength ? 255 : ' ');
  }
  
  delay(50);
}