#define WOKWI

#ifdef WOKWI

#include "mag.h"

magType mag;

#else

#include <Adafruit_HMC5883_U.h>
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);

#endif

class buttonType {
  private:
    byte Pin;
    byte State = HIGH;
    byte lastState = HIGH;
    unsigned long lastChange = 0;
    unsigned long lastLow = 0;
    unsigned long pressTime = 0;
  public:
    void init(byte pinNo){Pin = pinNo; pinMode(Pin, INPUT_PULLUP);}
    boolean released();
    unsigned long getPressTime() {return pressTime;}
};

boolean buttonType::released(){
  byte actState = digitalRead(Pin);
  if (actState != lastState){
    lastChange = millis();
    lastState = actState;
  }
  if (State != lastState && millis()-lastChange > 30){
    State = lastState;
    if (State == HIGH) {
       pressTime = millis()-lastLow;
       return true;
     } else {
       lastLow = millis();
     }
  }
  return false;
}


#include <Servo.h>

Servo dt;
Servo ep;

float Pi = 3.14159;
int directionAngle = 0;
int pos;
int data;
int set = 90;

constexpr int countLedNo = 5;
constexpr int modeLedNo  = 3;
constexpr byte countLeds[countLedNo] = {7,8,9,10,11};
constexpr byte modeLeds[modeLedNo]  = {2,4,6};
constexpr byte buttonPin = 12;
constexpr byte potPin    = A0;

buttonType button;
boolean shortPress = false;
boolean longPress = false;

enum pilotModes {SETTIMER, SETDIRECTION, PILOTING, TERMINAL};
pilotModes actMode;

constexpr int maxTimerCounter = 6;
int timerCounter = 0;
unsigned long pilotingStartTime = 0;



void setup() {
  
  Serial.begin(9600);
  pinMode(potPin, INPUT);
  for (int i = 0; i < countLedNo;i++ ){
    pinMode(countLeds[i], OUTPUT);
  }
  for (int i = 0; i < modeLedNo;i++ ){
    pinMode(modeLeds[i], OUTPUT);
  }
  setMode(SETTIMER);
  incrementTimer();
  button.init(buttonPin);
  dt.attach(3);
  ep.attach(5);
}




void loop() {
  checkButton();
  stateMachine();
}

void checkButton(){
   if (button.released()){
     if (button.getPressTime() < 200){
       shortPress = true;
     } else {
       longPress = true;
     }
   }
}

void setModeLed(byte No){
    for (int i = 0; i<modeLedNo;i++){
      digitalWrite(modeLeds[i],i == No-1);
    }
}

void setMode(pilotModes aMode){
    actMode = aMode;
    switch (actMode){
      case SETTIMER:
         Serial.println("Set Timer");
         setModeLed(1);
        break;
      case SETDIRECTION:
         Serial.println("Set Direction");
         setModeLed(2);
        break;
      case PILOTING:
         Serial.println("Piloting");
         setModeLed(3);
         pilotingStartTime = millis();
        break;
      case TERMINAL:
         Serial.println("Terminal");
         setModeLed(4);
        break;
    }
}


void stateMachine(){
  switch (actMode){
    case SETTIMER:
          if (shortPress) {
            incrementTimer();
          }
          if (longPress) {
            setMode(SETDIRECTION);
          }
      break;
    case SETDIRECTION:
          setDirection();
          if (longPress) {
            setMode(PILOTING);
            dt.write(90);
          }
      break;
    case PILOTING:
          ElectronicPilot();
          if (longPress) {
            setMode(TERMINAL);
          }
          if (millis()-pilotingStartTime > timerCounter*60000UL){
            setMode(TERMINAL);
            dt.write(180);
          }
      break;
    case TERMINAL:
          if (longPress) {
            setMode(SETTIMER);
          }
      break;
    default:
        actMode = SETTIMER;
      break;        
  }
  shortPress = false;
  longPress = false;
}



void incrementTimer(){
    timerCounter++;
    if (timerCounter > maxTimerCounter) {
      timerCounter = 1;
    }   
    refreshDisplay(timerCounter-1);
}

void refreshDisplay(int Value){
   for (int i=0;i<countLedNo;i++){digitalWrite(countLeds[i],LOW);};
   digitalWrite(countLeds[Value % 5],HIGH);
   if (Value >= 5) {digitalWrite(countLeds[4],HIGH);}
}

void setDirection(){
  static unsigned long lastMeasurement = 0;
  if (millis()-lastMeasurement > 1000) {
    lastMeasurement = millis();
    int pot = analogRead(A0);
    directionAngle = map(pot, 0, 1023, 0, 360);
    Serial.println("Direction Angle = "+String(directionAngle));
  }  
}

void ElectronicPilot() {
  static unsigned long lastUpdate = 0;
  if (millis()-lastUpdate > 500) {
    lastUpdate = millis();
    sensors_event_t event;
    mag.getEvent(&event);
    Serial.print("X: "); Serial.print(event.magnetic.x); Serial.print("  ");
    Serial.print("Y: "); Serial.print(event.magnetic.y); Serial.print("  ");
    float heading = atan2(event.magnetic.y, event.magnetic.x);
    float declinationAngle = 0.109;
    heading += declinationAngle;
    if (heading < 0){
    heading += 2 * PI;
    }
    if (heading > 2 * PI){
    heading -= 2 * PI;
    }
    float headingDegrees = heading * 180 / M_PI;
    Serial.print("Heading ");Serial.println(headingDegrees);
    pos = int(headingDegrees);
    data = directionAngle - pos;
    set = 90 + data;
    constrain(set, 80, 110);
    ep.write(set);
  }  
}