#define _TIMERINTERRUPT_LOGLEVEL_ 0

#include "ESP32TimerInterrupt.h"
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <WiFi.h>
#include <PubSubClient.h>
#include <ArduinoJson.h>
// #include <FlashStorage.h>
#include "OTA.h"



#define sda_pin 5
#define scl_pin 6

#define PIN_D4 7
#define kiri 4
#define pilih 3
#define kanan 2
#define batt 8


#define height 64
#define width 128

unsigned int SWPin = PIN_D4;

#define TIMER0_INTERVAL_MS 1
#define DEBOUNCING_INTERVAL_MS 80
#define DEBOUNCING_INTERVAL_MS2 500

#define LOCAL_DEBUG 1

#define topicpub "v1/devices/me/telemetry"
#define subtopic "v1/devices/me/rpc/request/+"

const char* ssid = "Wokwi-GUEST";      // Enter your WiFi name
const char* password = "";  // Enter WiFi password

const char* mqttServer = "www.teismksuryacipta.com";
const int mqttPort = 1883;
const char* mqttUser = "tacho";  //TOKEN
const char* mqttPassword = "tacho";
const char* clientId = "tacho";

// Init ESP32 timer 0
ESP32Timer ITimer0(0);
ESP32Timer ITimer1(1);

volatile unsigned long rotationTime = 0;

// Not using float => using RPM = 100 * real RPM
float RPM = 0;
float avgRPM = 0;
float sendRPM = 0;
unsigned long interval = 200;
unsigned long waktu;


volatile int debounceCounter;
volatile int debounceCounter2;
volatile int menu = 0;
volatile int subsetting = 0;
volatile bool selected = false;
volatile int cal_fac_isr = 100;

int adc_val;
float batt_lv;

int cal_fac = 100;
float cal_fac_float;

bool last_selected = false;
int menuCount = 3;
int last_menu = 0;
int last_subsetting = 0;
float last_rpm;
unsigned long waktus = 0;
unsigned long sendInterval = 2000;
unsigned long handleOTA = 0;
int interval_send_data = 2000;
char msg[300];

WiFiClient espClient;
PubSubClient client(espClient);
Adafruit_SSD1306 oled(width, height, &Wire, -1);

delay(1000); 

  --> Millis() ---> Software --> Gak bisa dipake buat infrared
  --> TimerInterrupt --> Timer0, Timer1 --> ESP32 C3

bool IRAM_ATTR TimerHandler0(void* timerNo) {
  
  if (!digitalRead(SWPin) && (debounceCounter >= DEBOUNCING_INTERVAL_MS / TIMER0_INTERVAL_MS)) {
    RPM = (6000000 / (rotationTime * TIMER0_INTERVAL_MS));
    avgRPM = (2 * avgRPM + RPM) / 3;
    rotationTime = 0;
    debounceCounter = 0;
  } else {
    debounceCounter++;
  }
  if (rotationTime >= 1000) {
    RPM = 0;
    avgRPM = (avgRPM + 3 * RPM) / 4;
    rotationTime = 0;
  } else {
    rotationTime++;
  }

  return true;
}

bool IRAM_ATTR buttonHandler(void* timerNo) {
  if (!digitalRead(kanan) && (debounceCounter2 >= DEBOUNCING_INTERVAL_MS2 / TIMER0_INTERVAL_MS)) {
    if (!selected) {
      menu++;
    } else {
      if (menu == 1) {
        cal_fac_isr--;
      }

      if (menu == 2) {
        //Lakukan fungsi lain saat menu 2 sedang dipilih

        subsetting++;
        if (subsetting > 2) subsetting = 2;
        // subsetting = (subsetting > 2) ? subsetting = 2 : subsetting++;
      }
    }
    debounceCounter2 = 0;
  } else {
    debounceCounter2++;
  }

  if (!digitalRead(kiri) && (debounceCounter2 >= DEBOUNCING_INTERVAL_MS2 / TIMER0_INTERVAL_MS)) {
    if (!selected) {
      menu--;
    } else {
      if (menu == 1) {
        cal_fac_isr++;
      }
      if (menu == 2) {
        //Lakukan fungsi lain saat menu 2 sedang dipilih

        subsetting--;
        if (subsetting <= 0) subsetting = 0;
      }
    }
    debounceCounter2 = 0;
  } else {
    debounceCounter2++;
  }


  if (!digitalRead(pilih) && (debounceCounter2 >= DEBOUNCING_INTERVAL_MS2 / TIMER0_INTERVAL_MS)) {
    selected = !selected;
    debounceCounter2 = 0;
  } else {
    debounceCounter2++;
  }

  return true;
}


void setup() {


  pinMode(SWPin, INPUT_PULLUP);
  pinMode(kiri, INPUT_PULLUP);
  pinMode(kanan, INPUT_PULLUP);
  pinMode(pilih, INPUT_PULLUP);
  pinMode(batt, INPUT);

  Wire.setPins(sda_pin, scl_pin);
  Wire.begin();

  if (!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("Failed to initiate OLED");
    while (1)
      ;
  }

  oled.setTextSize(2);
  oled.setTextColor(WHITE);
  oled.setCursor(20, 20);
  oled.clearDisplay();
  oled.println("RPM Meter");

  oled.display();
  setup_wifi();
  client.setServer(mqttServer, mqttPort);
  client.setCallback(callback);

  // delay(2000);
  oled.clearDisplay();
  oled.display();

  Serial.begin(115200);
  while (!Serial && millis() < 5000)
    ;
  Serial.print(F("\nStarting RPM_Measure on "));
  Serial.println(ARDUINO_BOARD);
  Serial.println(ESP32_TIMER_INTERRUPT_VERSION);
  Serial.print(F("CPU Frequency = "));
  Serial.print(F_CPU / 1000000);
  Serial.println(F(" MHz"));

  if (ITimer0.attachInterruptInterval(TIMER0_INTERVAL_MS * 1000, TimerHandler0)) {
    Serial.print(F("Starting  ITimer0 OK, millis() = "));
    Serial.println(millis());TimerHandler0
  } else
    Serial.println(F("Can't set ITimer0. Select another freq. or timer"));


  if (ITimer1.attachInterruptInterval(TIMER0_INTERVAL_MS * 1000, buttonHandler)) {
    Serial.print(F("Starting  ITimer1 OK, millis() = "));
    Serial.println(millis());
  } else
    Serial.println(F("Can't set ITimer1. Select another freq. or timer"));
}

void loop() {
  client.loop();
  if (WiFi.status() != WL_CONNECTED) {
    setup_wifi();
  }
  if (WiFi.status() == WL_CONNECTED && !client.connected()) {
    reconnect();
  }
  // cal_fac_float = cal_fac / 100.00;

  if (cal_fac != cal_fac_isr) {
    ITimer1.stopTimer();
    Serial.println("Timer1 Stopped, copying new values");
    cal_fac = cal_fac_isr;
    Serial.print("New Values : ");
    Serial.println(cal_fac);
    Serial.println("Timer1 restarted");
    ITimer1.restartTimer();
  }

  if (millis() >= handleOTA + 500) {
    // sendData();
    ArduinoOTA.handle();
    handleOTA = millis();
  }

  if (millis() >= waktus + interval_send_data) {
    sendData();
    // adc_val = analogRead(batt);

    // float voltage = map(adc_val, 0, 4095, 0.0, 4.2);

    // batt_lv = map(voltage, 0.0, 4.2, 0, 100);
    // Serial.print(adc_val);
    // Serial.print("\t");
    // Serial.print(voltage);
    // Serial.print("\t");
    // Serial.print(batt_lv);
    // Serial.println("\t");
    waktus = millis();
  }

  if (menu >= menuCount) menu = menuCount - 1;
  if (menu <= 0) menu = 0;

  if (last_menu != menu) {
    last_menu = menu;
    oled.clearDisplay();
  }

  if (last_selected != selected) {
    last_selected = selected;
    oled.clearDisplay();
  }


mainmenu:
  switch (menu) {
    case 0:
      measure_mode();
      break;

    case 1:
      cal_mode();
      break;

    case 2:
      setting_mode();
      break;
  }

  // if (avgRPM > 0) {
  //   Serial.print(F("RPM  = "));
  //   Serial.print((float)RPM / 100.f);
  //   Serial.print(F(", avgRPM  = "));
  //   Serial.println((float)avgRPM / 100.f);

  // }
}

void measure_mode() {

  if (millis() >= waktu + 250) {
    if (selected) {
      if (last_rpm != (RPM / 100.f) * cal_fac_float) {
        last_rpm = (RPM / 100.f)* cal_fac_float;
        oled.clearDisplay();
      }

      String rpm_string = String(round((RPM / 100.f)* cal_fac_float));
      oledWrite(2, 20, 10, "RPM :");
      oledDisplay();
      oledWrite(2, 20, 40, String(rpm_string));
      oledDisplay();
    } else {
      oledWrite(2, 10, 10, "Measure");
      oledDisplay();
    }
    waktu = millis();
  }

}

void cal_mode() {

  if (millis() >= waktu + 250) {
    if (selected) {
      if (round(cal_fac_float * 100) != cal_fac) {
        cal_fac_float = cal_fac / 100.00;
        oled.clearDisplay();
      }
      if (last_rpm != RPM / 100.f * cal_fac_float) {
        last_rpm = (RPM / 100.f) * cal_fac_float;
        oled.clearDisplay();
      }


      //Bagian write RPM ke OLED
      String rpm_string = String(round((RPM / 100.f)) * cal_fac_float);
      oledWrite(1, 20, 10, "Cal. Factor : ");
      oledDisplay();
      oledWrite(1, 20, 20, String(cal_fac_float));
      oledDisplay();
      oledWrite(1, 20, 30, "RPM");
      oledDisplay();
      oledWrite(2, 20, 50, String(rpm_string));
      oledDisplay();
    } else {
      oledWrite(2, 10, 10, "Cal");
      oledDisplay();
    }
    waktu = millis();
  }
}

void setting_mode() {

  if (millis() >= waktu + 250) {
    if (selected) {
      //Bagian write RPM ke OLED
      if (last_subsetting != subsetting) {
        last_subsetting = subsetting;
        oled.clearDisplay();
      }

      switch (subsetting) {
        case 0:
          oledWrite(1, 20, 10, "Sending Interval");
          oledDisplay();
          break;

        case 1:
          oledWrite(1, 20, 10, "WiFi Config");
          oledDisplay();
          break;

        case 2:

          oledWrite(1, 20, 10, "Restart ESP");
          oledDisplay();
          break;
      }
    } else {
      oledWrite(2, 10, 10, "Setting");
      oledDisplay();
    }
    waktu = millis();
  }
}

void oledWriteBlack(int ukuran, int x, int y, String value) {
  oled.setTextSize(ukuran);
  oled.setTextColor(WHITE);
  oled.setCursor(x, y);
  oled.println(value);
}

void oledWrite(int ukuran, int x, int y, String value) {
  oled.setTextSize(ukuran);
  oled.setTextColor(WHITE);
  oled.setCursor(x, y);
  oled.println(value);
}

void oledClear(int ukuran, int x, int y, String value) {
  oled.setTextSize(ukuran);
  oled.setTextColor(BLACK);
  oled.setCursor(x, y);
  oled.println(value);
  oled.display();
}

void oledDisplay() {
  oled.display();
}

double round2(double value) {
  return (int)(value * 100 + 0.5) / 100.0;
}

void setup_wifi() {
  setupOTA("ESP32-Dean", ssid, password);
  // WiFi.begin(ssid, password);
  // while (WiFi.status() != WL_CONNECTED) {
  //   delay(500);
  //   Serial.print("Connecting to WiFi.. ");
  //   Serial.println(ssid);
  // }
  // Serial.println("Connected to the WiFi network");
}

void reconnect() {
  while (!client.connected()) {
    Serial.println("Connecting to MQTT...");
    if (client.connect(clientId, mqttUser, mqttPassword)) {
      Serial.println("connected");
      client.subscribe(subtopic);
    } else {
      Serial.print("failed with state ");
      Serial.print(client.state());
      delay(2000);
    }
  }
}

void callback(char* topic, byte* payload, unsigned int length) {
  Serial.print("Message:");
  for (int i = 0; i < length; i++) {
    msg[i] = (char)payload[i];
    Serial.print(msg[i]);
  }
  Serial.println("");
  do_action(msg);
}

void do_action(const char* miseji) {

  StaticJsonDocument<200> doc;
  DeserializationError err = deserializeJson(doc, miseji);

  if (err) {
    Serial.print(F("deserializeJson() failed with code "));
    Serial.println(err.f_str());
  } else {
  }
}


void sendData() {

  const size_t CAPACITY = JSON_OBJECT_SIZE(10);
  StaticJsonDocument<CAPACITY> doc;
  doc["RPM"] = last_rpm;

  char JSONmessageBuffer[512];
  serializeJson(doc, JSONmessageBuffer, sizeof(JSONmessageBuffer));
  Serial.println(JSONmessageBuffer);
  client.publish(topicpub, JSONmessageBuffer);
  Serial.println("Data Sent!");
}