#include <Logger.h>
#include "Watchdog.h"
#include <Arduino.h>
#include <Config.h>
#include <Pinout.h>
#include <Version.h>
#include <Display.h>
#include <SPI.h>
#include <SD.h>
#include "ADXL345FastSPI.h"
#include "RTClib.h"
#include <Wire.h>
#include <Network.h>
#include <Thread.h>
#include <Measure.h>
#include <Tool.h>
#include <Settings.h>
#include "APIClient.h"
#include "UploadManager.h"

#define WDT_TIMEOUT 60        // Czas watchdoga do restartu
#define MAX_ADXL345_SENSORS 4 // Maksymalna ilość podłączanych sensorów

SPIClass SPI_ADXL(FSPI);  // SPI2 (VSPI)
SPIClass SPI_SD(HSPI);    // SPI3 (HSPI)

float x, y, z = 0;       // Dane odczytane z akcelerometru
String name;

bool isRebootRequired = false;
bool isAccelExists = false;      // Czy istnieje jakiś podłączony do SPI czujnik 
bool testingNow = false;         // Czy trwa test (gdy tak, trzeba wszystko inne wyłączyć)
bool runMeasure = false;         // czy włączyć pomiar? 

// Buttony: 5, 6, 7 
// piny SPI CS dla ASXL345
const uint8_t csPins[MAX_ADXL345_SENSORS] = {9, 10, 14, 21}; 

long licznik = 0;

ConfigManager configManager;
RTC_DS3231 rtc;

ADXL345FastSPI adxl(csPins, MAX_ADXL345_SENSORS);
Display display(rtc, 0x27, 20, 4);
Settings settings(display, rtc); // obsługa przycisków
WiFiManager wifi;

DataCapture capture(adxl, display, rtc, SD, 8192); // NEW !!! MEASURE!!!
APIClient apiClient;
UploadManager uploadManager(apiClient, rtc, capture);

Thread wifiTestThread = Thread();     // Cykliczny test WiFi
Thread offlineThread = Thread();      // Jeśli offline
Thread measureThread = Thread();      // Pomiar i zapis na SD
Thread uploadThread = Thread();       // Background upload

//////// PROTOTYPY /////////////////
void setup();
void loop();
void reboot();
void resetBtnClick();
void checkWiFi();
void showOfflineScreen();
void measure();
void setClockRTCBtn();

/* ******************* SETUP() ************************* */
void setup() {
  Serial.begin(115200);
  delay(500);
  // przy USB-CDC warto poczekać chwilę na enumerację (z timeoutem):
  unsigned long t0 = millis();
  while (!Serial && millis()-t0 < 2000) { delay(10); }
  settings.begin();
  esp_log_level_set("*", ESP_LOG_INFO);  // _ERROR, _WARN, _INFO, _DEBUG, _VERBOSE
  delay(500); // było 1000

  ESP_LOGI(TAG_MAIN, "----------------------");
  ESP_LOGI(TAG_MAIN, "WMT Stalowa Wola A.Chmielowiec & L.Klich");
  ESP_LOGI(TAG_MAIN, "Rejestrator parametrow");
  ESP_LOGI(TAG_MAIN, "Firmware: %s", VERSION);
  ESP_LOGI(TAG_MAIN, "----------------------");
	ESP_LOGI(TAG_MAIN, "ESP32 model: %s Rev %d", ESP.getChipModel(), ESP.getChipRevision());
	ESP_LOGI(TAG_MAIN, "Chip cores: %d", ESP.getChipCores());

   // Inicjalizacja Watchdoga na 5 sek, panic_on_trigger = true
  if (Watchdog::init(25, true)) {
    ESP_LOGI(TAG_MAIN, "Watchdog init ok.");
  } else {
    ESP_LOGE(TAG_MAIN, "Watchdog init error.");
  }

  Wire.begin(PIN_SDA, PIN_SCL, 100000);
  scanI2C(); // Skanowanie magistrali I2C na UART (RTC-0x68, LCD-0x27)

  configManager.begin(); // konfiguracja EEPROM urządzenia

  //configManager.showConfig();

  // Test LCD I2C
  if (!isI2CDevPresent(0x27)) {
    ESP_LOGE(TAG_MAIN, "LCD 0x27 wire error!");
  }
  ESP_LOGI(TAG_MAIN, "Display init");
  if (!display.begin()) {
    ESP_LOGE(TAG_MAIN, "Display init failed");
    while (true) delay(1000);
  }
  ESP_LOGI(TAG_MAIN, "Display OK");
  display.welcomeScreen();
  delay(1000);

  // Przycisk reset w przypadku factory reset
  if(settings.isBtnReset()) resetBtnClick();

  // MCU Info
  ESP_LOGI(TAG_MAIN, "MCU info");
  display.textStatus(ESP.getChipModel());
  delay(500);
  char buf[20];
  sprintf(buf, "Freq: %d MHz", ESP.getCpuFreqMHz());
  display.textStatus(buf);
  delay(500);

  // Test PSRAM
  display.textStatus("PSRAM:");
  ESP_LOGI(TAG_MAIN, "PSRAM init");
  if (!psramFound()) {
    ESP_LOGE(TAG_MAIN, "PSRAM not found");
    display.print("FAILED");
      while (true) delay(1000);
  }
  display.print("OK");
  delay(500);

  // Test RTC
  ESP_LOGI(TAG_MAIN, "RTC test");
  if (!isI2CDevPresent(0x68)) {
  ESP_LOGE(TAG_MAIN, "RTC 0x68 wire error!");
  display.textStatus("RTC wire error!");
  while (true) delay(1000);
  }
  display.textStatus("RTC:");
  ESP_LOGI(TAG_MAIN, "RTC init");
  if (!rtc.begin()) {
    display.print("FAILED");
    ESP_LOGE(TAG_MAIN, "Can't find RTC");
    while (true) delay(1000);
  } else {
      display.print("OK");
      ESP_LOGI(TAG_MAIN, "RTC OK");
      if (rtc.lostPower()) {
      ESP_LOGE(TAG_MAIN, "RTC power lost! Set clock");
      display.textStatus("RTC: SET TIME");
      delay(1000);
      settings.setTimeRTC(); 
    }
  }

  // Przycisk ustawianai czasu: przytrzymaj OK przy starcie systemu
  if(settings.isSetClock()) setClockRTCBtn();

  delay(500);

  // Karta SD
  ESP_LOGI(TAG_MAIN, "SD Card init");
  ESP_LOGI(TAG_MAIN, "SPI2 SD SCK: %d, MISO: %d, MOSI: %d,  CS: %d", SD_SCK, SD_MISO, SD_MOSI, SD_CS);
  SPI_SD.begin(SD_SCK, SD_MISO, SD_MOSI);
  display.textStatus("SD CARD:");
  while(!SD.begin(SD_CS, SPI_SD, 4000000)) { // 10 MHz na start; w razie czego 4 MHz (bez tego często SD Failed)
    ESP_LOGE(TAG_MAIN, "SD mount failed");
    display.print("FAILED");
    delay(4000);
    display.textStatus("SD CARD:");
    delay(500);
  } 
  ESP_LOGI(TAG_MAIN, "SD Card OK");
  display.print("OK");

  ESP_LOGI(TAG_MAIN, "ADXL345 SPI3 SCK: %d, MISO: %d, MOSI: %d", CLK_ADSX, MISO_ADSX, MOSI_ADSX);
  SPI_ADXL.begin(CLK_ADSX, MISO_ADSX, MOSI_ADSX);
  
  // Inicjalizacja ADXL345
  ESP_LOGI(TAG_MAIN, "ADXL345 init");
  display.textStatus("ADXL345: ");
  //if (!adxl.begin(&SPI, 5000000, ADXL345FastSPI::RATE_3200HZ, ADXL345FastSPI::RANGE_16G, 1)) {
  if(!adxl.begin(&SPI_ADXL, 5000000, ADXL345FastSPI::RATE_3200HZ, ADXL345FastSPI::RANGE_16G, 1)){
    ESP_LOGE(TAG_MAIN, "ADXL345 Error");
    display.print("FAILED");
    isAccelExists = false;
    uint8_t counter = 0;
    while(counter<5){ counter++; delay(1000); }
    display.clear();
    display.textCenter(0, "PLEASE CONNECT");
    display.textCenter(1, "SENSOR MODULE");
    display.textCenter(2, "ANY KEY RESTART");
    display.textCenter(3, "GURU MEDITATION");
    while(true){
     if(digitalRead(BTN_UP) == LOW) reboot();
     if(digitalRead(BTN_OK) == LOW) reboot();
     if(digitalRead(BTN_DOWN) == LOW) reboot();
    }
  } else {
    display.print(String(adxl.size()));
    ESP_LOGI(TAG_MAIN, "ADXL345 OK: %d COUNT", adxl.size());  // liczba wykrytych
    isAccelExists = true;
  }

  DateTime now = rtc.now();
  ESP_LOGI(TAG_MAIN, "RTC TIME: %d:%d:%d %d:%d:%d", now.day(), now.month(), now.year(), now.hour(), now.minute(), now.second());
  delay(1000);

  if(config.connect){
    wifi.begin();
    wifiTestThread.onRun(checkWiFi);
    wifiTestThread.setInterval(5000);     // Test WiFi co 3 sekundy
  } else {
    offlineThread.onRun(showOfflineScreen);
    offlineThread.setInterval(1000);     // Jeśli offline, to wyświetl ekran co 1 sek
  }

  measureThread.onRun(measure);
  measureThread.setInterval(config.pause);  // Test co X sekund

  if(config.connect){
    uploadThread.onRun([]() { uploadManager.processPendingUploads(); });
    uploadThread.setInterval(60000); // Co 1 minutę sprawdzaj zaległe
  }

  // Dodanie taska loop do WDT
  if (Watchdog::addThisTask()) {
    ESP_LOGI(TAG_MAIN, "Added main task to Watchdog");
  }

  ESP_LOGI(TAG_MAIN, "System ready");
  display.clear();
}

// Factory reset
void setClockRTCBtn(){
    ESP_LOGI(TAG_MAIN, "SET DATE TIME");
    while(settings.isSetClock()){
        display.clear();
        display.textCenter(0, " PLEASE ");
        display.textCenter(1, "release key");
        display.textCenter(3, "TO SET DATE TIME");
        delay(500);
      }
      settings.setTimeRTC();
}  


// Factory reset
void resetBtnClick(){
    uint8_t counter = 10; // ile sekund trzymać przycisk na factory reset
    ESP_LOGI(TAG_MAIN, "RESET BTN to 10 sec.");
    while(settings.isBtnReset()){
      Serial.print(counter); Serial.print(",");
      counter--;
      delay(1000);
      display.clear();
      display.textCenter(0,"WARNING!!!");
      display.textCenter(1, "Factory reset");
      display.textCenter(2, "keep holding for");
      display.textStatus(String(counter).c_str());
      if (counter <= 1) { 
        display.clear();
        display.textCenter(0, "RESET CONFIG");
        display.textCenter(1, "release key");
        ESP_LOGI(TAG_MAIN, "RESET CONFIG");
        while(settings.isBtnReset()){;}
        configManager.resetToDefaults();
        display.textStatus("RESTARTING!");
        delay(500);
        isRebootRequired = true;
      } else {
        isRebootRequired = true;
      }
    }
}  

///////////////////////////////////////////////////////////////////////////////////////////////////////////////

void showError(String err){
  Watchdog::feed();
  display.clear();
  display.println(err);
  ESP_LOGE(TAG_MAIN, "%s", err.c_str());
  delay(3000);
}


void checkWiFi(){
  Watchdog::feed();
   bool isConnected = WiFi.isConnected();
   String ip = WiFi.localIP().toString();
   wifi.checkWiFiConnection();
   display.updateBarWiFi(WiFi.RSSI(), isConnected);
   display.updateNetwork(ip, isConnected);
}

////// Ekran główny tylko gdy brak pomiaru /////////////
void showOfflineScreen(){
  if((!config.connect) && (!testingNow)) {
    bool isGB;
    Watchdog::feed();
    float freeSpace = capture.freeSpaceFloat(&isGB);
    display.displayOffline(testingNow, adxl.connectedSensorsCount(), capture.freeSpaceFloat(), licznik, false);
  }  
}


void reboot() {
  display.clear();
  display.textCenter(1, "SYSTEM");
  display.textCenter(2, "RESTARTING");
  #if defined(ARDUINO_RASPBERRY_PI_PICO)
  watchdog_enable(1, 1);  // 1 ms timeout, restart po upływie
  while (true);           // czekaj na watchdog reset
  #endif
  #if defined(ARDUINO_ARCH_ESP8266) || defined(ARDUINO_ARCH_ESP32)
  ESP_LOGI(TAG_MAIN, "RESTART");
  ESP.restart(); // restart ESP
  #endif
  #if defined(ARDUINO_ARCH_STM32)
  NVIC_SystemReset(); // restartuj STM32
  #endif
}


// pomiar z Thread START POMIARU TUTAJ
void measure(){
  Watchdog::feed(); 
  testingNow = true;
  offlineThread.enabled = false;
  DateTime now = rtc.now();
  ESP_LOGI(TAG_MAIN, "RTC TIME: %d:%d:%d %d:%d:%d", now.day(), now.month(), now.year(), now.hour(), now.minute(), now.second());
  char bdate[15]; char btime[15];
  snprintf(bdate, sizeof(bdate), "%04d-%02d-%02d", now.year(), now.month(), now.day());
  snprintf(btime, sizeof(btime), "%02d:%02d:%02d", now.hour(), now.minute(), now.second());
  display.initMeasure(config.measure, testingNow, runMeasure, config.pause, config.duration, config.connect, licznik, bdate, btime);
  ESP_LOGI(TAG_MAIN, "MEASURE RUNNING");
  //delay(1000);
  capture.captureAuto(config.duration, "/");
  testingNow = false;
  Watchdog::feed(); 
  if(!capture.isExit){
    capture.printLastFileInfoSerial();
    capture.readHeaderAndPrint(capture.generateNextFilename().c_str());
    ESP_LOGI(TAG_MAIN, "MEASURE FINISH");
  } else {
    ESP_LOGI(TAG_MAIN, "MEASURE INTERRUPT");
    //runMeasure = false;
  }
  
  if (config.connect && WiFi.status() == WL_CONNECTED) {
      ESP_LOGI(TAG_MAIN, "TRIGGER UPLOAD PENDING...");
      uploadManager.processPendingUploads();
  }

  runMeasure = false;
  ESP_LOGI(TAG_MAIN, "DISPLAY Offline");
  display.displayOffline(testingNow, adxl.connectedSensorsCount(), capture.freeSpaceFloat(), licznik, true);
  offlineThread.enabled = true;
}

void toogleMode(){ // tryb ciągłego, pojedynczego pomiaru
  config.measure = !config.measure;
  configManager.saveConfig();
  display.textStatus("Mode changed");
  delay(500);
  display.textStatus("");
}

void settingsDevice(){
  settings.setConfigDevice();
  configManager.saveConfig();
  settings.finishConfigDevice();
}

///////////// LOOP ////////////////////////////////////////////
void loop() {
  if (settings.isPressed(2)){
    Watchdog::feed(); 
   if(runMeasure) { 
    capture.isExit = true; 
    runMeasure = false; 
    display.textStatus("Stopped");
    delay(3000); 
  } else runMeasure = true;
  if(runMeasure) ESP_LOGI(TAG_MAIN, "BTN MEASURE: START"); else ESP_LOGI(TAG_MAIN, "BTN MEASURE: STOP");
 
  Watchdog::feed();
  delay(300);
  if (runMeasure) measureThread.run();
}

  if(settings.isPressed(3)) settingsDevice(); // DOWN
  if(settings.isPressed(1)) toogleMode();     // UP

  if(testingNow) {
    if((runMeasure) && (settings.isPressed(2))){
      runMeasure = false;
      display.textStatus("STOPPING. WAIT");
    }
     return; // jeśli trwa akurat test, nic nie rób
  }

  if(wifiTestThread.shouldRun() && (config.connect))
    wifiTestThread.run();

  if(uploadThread.shouldRun() && config.connect)
    uploadThread.run();

  if(offlineThread.shouldRun() && (!config.connect)){
    offlineThread.run();
  }

  if(!isAccelExists) {
    ESP_LOGE(TAG_MAIN, "ADXL module error:halt");
    display.clear();
    delay(500);
    display.textCenter(0, "SENSORS");
    display.textCenter(1, "NOT EXISTS");
    display.textCenter(2, "SYSTEM STOPPED");
    Watchdog::feed(); 
    delay(2000);
  } else {
   if(measureThread.shouldRun() && (config.measure) && (!runMeasure)){
     Watchdog::feed(); 
     ESP_LOGI(TAG_MAIN, "Measure thread run"); 
     measureThread.run();
   }
  }

  if (isRebootRequired) {
    ESP_LOGI(TAG_MAIN, "Reboot required");
    Watchdog::feed(); 
		delay(1000); 
		reboot();
  }

  Watchdog::feed(); 
  delay(20); // 100
  licznik ++;
}