#include <Wire.h>
#include <Adafruit_SSD1306.h>
#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

#include <SPI.h>
#include <LoRa.h>

#include <EEPROM.h>

#define timer_difference 900
#define step 10 // step when button is pressed
#define OK 6
#define OK_LED 5
#define UP 3
#define DOWN 4
#define ARMED 7
boolean OK_old = 1;
boolean UP_old = 1;
boolean DOWN_old = 1;
boolean ARMED_old = 1;
int new_position = 1;
int old_position;
#define button_pushed_pime 1000
unsigned long button_pushed_UP;
unsigned long button_pushed_DOWN;

byte ID_number = 1;
byte fire_mode = 0;

byte time_h = 0;
byte time_m = 0;
byte time_s = 0;
byte time_s_old = 0;
boolean time_running = 0;
unsigned long time = 0;
unsigned long set_time = 0;
unsigned long timer = 0;

String display_msg;
int sig = -200;

#define end_menu 4
byte menu_pos = 1;
byte exit_menu_code = 0;

#define power_max 20
#define power_min 2
#define SF_max 12
#define SF_min 7
byte LoRa_power = EEPROM.read(0);
byte LoRa_SF = EEPROM.read(1);
byte LoRa_unit_power = 17;
byte LoRa_unit_SF = 7;
long ignation_delay = 0;
byte set_ID_number = 0;
byte alias = 1;
byte set_time_m = 0;
byte set_time_s = 0;
byte set_time_ms = 0;

byte command_mode = 0;
unsigned long ping_time;
unsigned long ping;

unsigned long refresh_bat_lvl;
int bat_lvl;

boolean serial_communication = false;
boolean debugging = false;


void setup() {
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  pinMode(OK, INPUT_PULLUP);
  pinMode(OK_LED, OUTPUT);
  digitalWrite(OK_LED, HIGH);
  pinMode(UP, INPUT_PULLUP);
  pinMode(DOWN, INPUT_PULLUP);
  pinMode(ARMED, INPUT_PULLUP);

  if(!LoRa.begin(433E6)) {
    display.clearDisplay();
    display.setTextColor(WHITE);
    display.setTextSize(2);
    display.setCursor(4, 7);
    display.print("LoRa ERROR");
    display.display();
    while(1);
  }
  LoRa.setTxPower(LoRa_power);
  LoRa.setSpreadingFactor(LoRa_SF);

  analogRead(A6);  // first value is not correct

  // To activate serial communication, it is necessary to press the OK button at startup.
  if(digitalRead(OK) == 0) {
    Serial.begin(9600);
    serial_communication = true;
  }
  // To activate serial debugging, it is necessary to press the UP button at startup.
  else if(digitalRead(UP) == 0) {
    Serial.begin(9600);
    debugging = true;
  }

  menu();
}

void loop() {
  if(LoRa.parsePacket() > 0) {
    String msg = LoRa.readString();
    sig = LoRa.packetRssi();

    if(msg == "Pong") {
      ping = millis() - ping_time;
      msg = String("Ping ") + String(ping, DEC) + String(" ms");
      prichozi(msg);
    }
    else if(msg == "OK") {
      if(fire_mode == 0) {
        ID_number ++;
        ID_number = max(ID_number, 1);
        menu();
      }
    }
    else if(msg.startsWith("Time_F")) {
      set_time = getTime(msg);
      time = set_time / 1000;
      time_running = false;
      countdown();
    }
    else if(msg.startsWith("Time_T")) {
      set_time = getTime(msg);
      time = set_time / 1000;
      time_running = true;
      timer = millis();
      countdown();
    }
    else {
      if(serial_communication) {
        Serial.println(msg);  // Send msg to computer.
        menu();
      }
      else {
        prichozi(msg);
      }
    }
  }

  if(time_running) {
    countdown();
  }

  // do something when some button is pressed -----------------------------------------------------
  if((digitalRead(ARMED) == 0) && (ARMED_old == 0)) {
    ARMED_old = 1;
    sendArmed(true);
    digitalWrite(OK_LED, LOW);

    if(serial_communication) {
      Serial.println("Arm!");
    }
  }

  if((digitalRead(OK) == 0) && (OK_old == 0)) {
    OK_old = 1;

    // ARMED --------------------------------------------------------------------------------------
    if(digitalRead(ARMED) == 0) {
      if(menu_pos == 1) {
        if(fire_mode == 3) {
          sendFireAll();
        }
        else {
          FireID();
          if(fire_mode == 1) {
            ID_number ++;
            ID_number = max(ID_number, 1);
          }
        }
      }
      else if((menu_pos == 2) && (!time_running) && (set_time != 0)) {
        sendStartTime();
        time_running = true;
        timer = millis();
      }
    }

    // ESC ----------------------------------------------------------------------------------------
    else if(digitalRead(DOWN) == 0) {
      resExitMenu();
    }

    else {
      exitMenu();
    }
    menu();
  }

  if((digitalRead(UP) == 0) && (UP_old == 0)) {
    UP_old = 1;
    new_position ++;
  }
  if((digitalRead(DOWN) == 0) && (DOWN_old == 0)) {
    DOWN_old = 1;
    new_position --;
  }

  if((digitalRead(UP) == 0) && (millis() > button_pushed_UP)) {
    while((digitalRead(UP) == 0)) {
      new_position = new_position + step;
      doPos();
    }
  }
  if((digitalRead(DOWN) == 0) && (millis() > button_pushed_DOWN)) {
    while(digitalRead(DOWN) == 0) {
      new_position = new_position - step;
      doPos();
    }
  }

  // do something when some button is released ----------------------------------------------------
  if((digitalRead(ARMED) == 1) && (ARMED_old == 1)) {
    ARMED_old = 0;
    sendArmed(false);

    if(serial_communication) {
      Serial.println("Disarm!");
    }

    // stop countdown
    time_running = false;
    set_time = set_time - (millis() - timer);

    digitalWrite(OK_LED, HIGH);
    delay(10);
  }

  if((digitalRead(OK) == 1) && (OK_old == 1)) {
    OK_old = 0;
    delay(10);
  }

  if((digitalRead(UP) == 1) && (UP_old == 1)) {
    UP_old = 0;
    button_pushed_UP = millis() + button_pushed_pime;
    delay(10);
  }
  else if(digitalRead(UP) == 1) {
    button_pushed_UP = millis() + button_pushed_pime;
  }

  if((digitalRead(DOWN) == 1) && (DOWN_old == 1)) {
    DOWN_old = 0;
    button_pushed_DOWN = millis() + button_pushed_pime;
    delay(10);
  }
  else if(digitalRead(DOWN) == 1) {
    button_pushed_DOWN = millis() + button_pushed_pime;
  }

  // read battery voltage every 30 s and save to bat_lvl, then print on display -------------------
  if(millis() > refresh_bat_lvl) {
    refresh_bat_lvl = millis() + 30000;
    bat_lvl = max(map(analogRead(A6), 675, 860, 0, 100), 0);
    bat_lvl = min(bat_lvl, 100);

    // just a debugging feature
    if(debugging) {
      Serial.print("ADC battery level: ");
      Serial.println(analogRead(A6));
    }

    menu();
  }

  // Serial communication with computer via USB. --------------------------------------------------
  if(Serial.available() > 0) {
    String ser_msg = Serial.readString();

    LoRa.beginPacket();
    LoRa.print(ser_msg);
    LoRa.endPacket();
  }

  doPos();
}

void doPos() {
  if(new_position != old_position) {
    if(exit_menu_code == 0) {
      menu_pos = setPos(1, end_menu);
    }

    else if(exit_menu_code == 11) {
      fire_mode = setPos(0, 3);
    }
    else if(exit_menu_code == 12) {
      if(fire_mode == 3) {
        ID_number = setPos(0, 0);
      }
      else {
        ID_number = setPos(1, 255);
      }
    }

    else if(exit_menu_code == 21) {
      time_h = setPos(0, 99);
    }
    else if(exit_menu_code == 22) {
      time_m = setPos(0, 59);
    }
    else if(exit_menu_code == 23) {
      time_s = setPos(0, 59);
    }

    else if((exit_menu_code == 31) || (exit_menu_code == 37) || (exit_menu_code == 131) || (exit_menu_code == 42)) {
      set_ID_number = setPos(0, 255);
    }
    else if(exit_menu_code == 32) {
      set_time_m = setPos(0, 59);
    }
    else if(exit_menu_code == 33) {
      set_time_s = setPos(0, 59);
    }
    else if(exit_menu_code == 34) {
      set_time_ms = setPos(0, 9);
    }
    else if(exit_menu_code == 35) {
      LoRa_power = setPos(power_min, power_max);
    }
    else if(exit_menu_code == 36) {
      LoRa_SF = setPos(SF_min, SF_max);
    }
    else if(exit_menu_code == 38) {
      LoRa_unit_power = setPos(power_min, power_max);
    }
    else if(exit_menu_code == 39) {
      LoRa_unit_SF = setPos(SF_min, SF_max);
    }
    else if(exit_menu_code == 130) {
      alias = setPos(1, 255);
    }
    else if(exit_menu_code == 41) {
      command_mode = setPos(0, 25);
    }

    old_position = new_position;
    menu();
  }
}

int setPos(int zacatek, int konec) {
  if(new_position > konec) {
    new_position = zacatek;
  }
  else if(new_position < zacatek) {
    new_position = konec;
  }
  int pozice = new_position;
  return pozice;
}
