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

QMC5883LCompass compass;
String direccio = "";

const unsigned long interval = 250;
unsigned long previousMillis = 0;
unsigned long currentMillis = 0;
int a;

Scheduler runner;

Task taskCompassRead(interval, TASK_FOREVER, []() {
  currentMillis = millis();
  compass.read();
  //a = compass.getAzimuth();
  a = random(0,360);

  if (a < 0) {
    a = a + 360;
  }

  if ((a >= 157.5) && (a < 202.5)) {
    direccio = "Sur";
  } else if ((a >= 202.5) && (a < 247.5)) {
    direccio = "Suroeste";
  } else if ((a >= 247.5) && (a < 292.5)) {
    direccio = "Oeste";
  } else if ((a >= 292.5) && (a < 337.5)) {
    direccio = "Noroeste";
  } else if ((a >= 337.5) || (a < 22.5)) {
    direccio = "Norte";
  } else if ((a >= 22.5) && (a < 67.5)) {
    direccio = "Noreste";
  } else if ((a >= 67.5) && (a < 112.5)) {
    direccio = "Este";
  } else if ((a >= 112.5) && (a < 157.5)) {
    direccio = "Sureste";
  }
  Serial.println(direccio);
  
});

Task taskPrintDirection(interval, TASK_FOREVER, []() {
 
 if ((direccio == "Norte")&&(digitalRead(22) == 0)) {
    digitalWrite(23,1);
    delay(200);
    digitalWrite(23,0);
    delay(200);

  }
});

void setup() {
  pinMode(23, OUTPUT);
  pinMode(22, INPUT);
  Serial.begin(115200);
  Wire.begin(); 
  compass.init();
  
  runner.init();
  runner.addTask(taskCompassRead);
  runner.addTask(taskPrintDirection);
  taskCompassRead.enable();
  taskPrintDirection.enable();
}

void loop() {
  runner.execute();
}