#include <esp_bt.h>
#include <esp_bt_main.h>
#include <esp_bt_device.h>
#include <esp_gap_bt_api.h>

// Include BLE libraries
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEScan.h>
#include <ArduinoJson.h>

// Include Preferences library for persistent storage
#include <Preferences.h>

// Watchdog Timer
#include "esp_task_wdt.h"

#define LED_PIN 2
#define RELAY_PIN 27

#define MAX_DEVICE 15                  // Fixed maximum number of target devices
#define DEFAULT_LED_ON_DURATION 10000  // Default duration in milliseconds (10 seconds)
#define DEFAULT_RSSI_THRESHOLD -92     // Default RSSI threshold

// Initialize target_bdas with MAX_DEVICE entries
String target_bdas[MAX_DEVICE] = {
  "22:22:FE:AE:01:00",
  "C8:58:95:5C:4C:A8",
  "18:8B:0E:2D:53:6E","18:8B:0E:2D:59:1A","18:8B:0E:2B:59:36",
  "FF:FF:FF:FF:FF:FF", "FF:FF:FF:FF:FF:FF", "FF:FF:FF:FF:FF:FF",
  "FF:FF:FF:FF:FF:FF", "FF:FF:FF:FF:FF:FF"
};

// Initialize Preferences
Preferences preferences;

// Configuration parameters
unsigned long LED_ON_DURATION = DEFAULT_LED_ON_DURATION;  // Duration for LED and relay (ms)
int RSSI_THRESHOLD_VALUE = DEFAULT_RSSI_THRESHOLD;        // RSSI threshold

unsigned long last_detected_time = 0;  // Time when a target device was last detected
bool led_is_on = false;                // LED state

// BLE Scan Parameters
BLEScan *pBLEScan = nullptr;

// BLE Server Parameters
BLEServer *pServer = nullptr;
BLECharacteristic *pCharacteristic = nullptr;
bool deviceConnected = false;

// State management
enum ScanState {
  SCAN_BT_CLASSIC,
  SCAN_BLE,
  CONFIG_MODE
};

ScanState currentState = SCAN_BT_CLASSIC;
unsigned long stateStartTime = 0;

// Configuration mode parameters
const unsigned long CONFIG_INTERVAL = 60000;      // Enter config mode every 60 seconds
const unsigned long CONFIG_DURATION = 10000;      // Stay in config mode for 10 seconds
unsigned long lastConfigTime = 0;

// Forward declarations
void classic_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *param);
void updateTargetDevices(JsonDocument &doc);
bool updateConfiguration(JsonDocument &doc);
void scanCompleteCB(BLEScanResults scanResults);
bool compare_bda(const String &bda1, const String &bda2);
void startBLEServer();
void stopBLEServer();
void initBluetoothController();
void deinitBluetoothController();

class JsonCharacteristicCallbacks : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *pCharacteristic) override {
    String rxValue = pCharacteristic->getValue();

    if (rxValue.length() == 0) {
      Serial.println("Received empty JSON");
      sendAck("Error: Received empty JSON.");
      return;
    }

    Serial.println("Received JSON via BLE:");
    Serial.println(rxValue);

    // Parse the JSON
    StaticJsonDocument<1024> doc;
    DeserializationError error = deserializeJson(doc, rxValue);

    if (error) {
      Serial.print("JSON deserialization failed: ");
      Serial.println(error.c_str());
      sendAck(String("Error: JSON deserialization failed: ") + error.c_str());
      return;
    }

    // Update configuration based on JSON
    bool success = updateConfiguration(doc);

    if (success) {
      sendAck("Success: Configuration updated.");
    } else {
      sendAck("Error: Configuration update failed.");
    }
  }

  // Function to send acknowledgment
  void sendAck(const String &message) {
    pCharacteristic->setValue(message.c_str());
    pCharacteristic->notify();  // Send notification to the client
    Serial.println("Sent ACK: " + message);
  }
};

void setup() {
  // Initialize Serial Monitor
  Serial.begin(115200);
  Serial.println("Initializing Bluetooth scan and persistent storage...");

  // Initialize GPIOs
  pinMode(LED_PIN, OUTPUT);
  pinMode(RELAY_PIN, OUTPUT);

  digitalWrite(LED_PIN, LOW);     // Ensure LED is off at startup
  digitalWrite(RELAY_PIN, HIGH);  // Ensure Relay is off at startup

  // Initialize Preferences
  preferences.begin("bt_config", false);  // 'false' means read/write access

  // Load target devices from Preferences
  for (uint8_t i = 0; i < MAX_DEVICE; i++) {
    String key = "bda_" + String(i);
    if (preferences.isKey(key.c_str())) {
      target_bdas[i] = preferences.getString(key.c_str(), target_bdas[i]);
    }
  }

  // Load configuration parameters
  LED_ON_DURATION = preferences.getULong("LED_ON_DURATION", DEFAULT_LED_ON_DURATION);
  RSSI_THRESHOLD_VALUE = preferences.getInt("RSSI_THRESHOLD", DEFAULT_RSSI_THRESHOLD);

  // Close Preferences after loading
  preferences.end();

  // Print the list of target devices
  Serial.println("List of Target Devices:");
  for (uint8_t i = 0; i < MAX_DEVICE; i++) {
    Serial.printf("  target_bdas[%d]: %s\n", i, target_bdas[i].c_str());
  }

  // Print loaded configuration parameters
  Serial.printf("Loaded LED_ON_DURATION: %lu ms\n", LED_ON_DURATION);
  Serial.printf("Loaded RSSI_THRESHOLD: %d dBm\n", RSSI_THRESHOLD_VALUE);

  // Initialize Watchdog Timer
  esp_task_wdt_config_t wdt_config = {
    .timeout_ms = 30000,    // 30 seconds
    .trigger_panic = true   // Trigger panic on WDT timeout
  };
  esp_task_wdt_init(&wdt_config);
  esp_task_wdt_add(NULL);  // Add current thread to WDT

  // Initialize Bluetooth Controller
  initBluetoothController();

  // Initialize BLE
  BLEDevice::init("HiHiNaja");  // Set a name for the BLE device

  // Create BLE Scan object
  pBLEScan = BLEDevice::getScan();  // create new scan
  pBLEScan->setActiveScan(true);    // active scan uses more power, but get results faster
  pBLEScan->setInterval(100);
  pBLEScan->setWindow(99);  // less or equal setInterval value

  // Initialize BLE Server (but don't start advertising yet)
  pServer = BLEDevice::createServer();
  BLEService *pService = pServer->createService("12345678-1234-5678-1234-56789abcdef0");  // Service UUID
  pCharacteristic = pService->createCharacteristic(
    "abcdef01-1234-5678-1234-56789abcdef0",  // Characteristic UUID
    BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_NOTIFY
  );
  pCharacteristic->setCallbacks(new JsonCharacteristicCallbacks());
  pService->start();

  // Register the GAP callback function for Bluetooth Classic scanning
  esp_bt_gap_register_callback(classic_bt_gap_cb);

  // Start with Bluetooth Classic scanning
  esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, 2, 0);
  currentState = SCAN_BT_CLASSIC;
  stateStartTime = millis();
  lastConfigTime = millis();  // Initialize last configuration time
  Serial.println("Starting Bluetooth Classic scan...");
}

void loop() {
  unsigned long currentTime = millis();

  // Feed the watchdog timer
  esp_task_wdt_reset();

  // Periodically enter configuration mode
  if (currentTime - lastConfigTime >= CONFIG_INTERVAL && currentState != CONFIG_MODE) {
    // Stop any ongoing scans
    if (currentState == SCAN_BT_CLASSIC) {
      esp_bt_gap_cancel_discovery();
      Serial.println("Stopping Bluetooth Classic scan for configuration mode...");
    } else if (currentState == SCAN_BLE) {
      pBLEScan->stop();
      Serial.println("Stopping BLE scan for configuration mode...");
    }

    // Enter configuration mode
    startBLEServer();
    currentState = CONFIG_MODE;
    stateStartTime = currentTime;
    Serial.println("Entering Configuration Mode...");
  }

  if (currentState == CONFIG_MODE) {
    // Check if configuration mode duration has elapsed
    if (currentTime - stateStartTime >= CONFIG_DURATION) {
      // Exit configuration mode
      stopBLEServer();
      lastConfigTime = currentTime;  // Update last configuration time

      // Resume scanning (start with Bluetooth Classic)
      esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, 2, 0);
      currentState = SCAN_BT_CLASSIC;
      stateStartTime = currentTime;
      Serial.println("Exiting Configuration Mode. Resuming Bluetooth Classic scan...");
    }
  } else if (currentState == SCAN_BT_CLASSIC) {
    // Check if BT Classic scanning is over
    if (currentTime - stateStartTime >= 2560) { // 2 * 1.28s = 2560ms
      esp_bt_gap_cancel_discovery();
      Serial.println("Stopping Bluetooth Classic scan...");

      // Start BLE scanning
      pBLEScan->clearResults();  // Clear previous results
      pBLEScan->start(2, scanCompleteCB); // Non-blocking scan
      currentState = SCAN_BLE;
      stateStartTime = currentTime;
      Serial.println("Starting BLE scan...");
    }
  } else if (currentState == SCAN_BLE) {
    // Check if BLE scanning is over
    if (currentTime - stateStartTime >= 2000) {
      // BLE scan should have completed
      Serial.println("BLE scan completed.");

      // Start Bluetooth Classic scanning
      esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, 2, 0);
      currentState = SCAN_BT_CLASSIC;
      stateStartTime = currentTime;
      Serial.println("Starting Bluetooth Classic scan...");
    }
  } else {
    // Handle unexpected state
    Serial.println("Unknown state detected. Resetting state machine.");
    currentState = SCAN_BT_CLASSIC;
    stateStartTime = currentTime;
    esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, 2, 0);
  }

  // Check if the LED is on and if LED_ON_DURATION has passed since the last detection
  if (led_is_on && (currentTime - last_detected_time >= LED_ON_DURATION)) {
    digitalWrite(LED_PIN, LOW);     // Turn off LED
    digitalWrite(RELAY_PIN, HIGH);  // Turn off Relay

    led_is_on = false;
    Serial.println("LED turned OFF (timer expired)");
  }

  // Add a small delay to yield control
  delay(10);
}

// Function to initialize Bluetooth Controller
void initBluetoothController() {
  esp_err_t ret;

  // Configure the Bluetooth controller
  esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();

  if ((ret = esp_bt_controller_init(&bt_cfg)) != ESP_OK) {
    Serial.printf("Bluetooth controller initialize failed: %s\n", esp_err_to_name(ret));
    // Try deinitializing and reinitializing
    esp_bt_controller_deinit();
    ret = esp_bt_controller_init(&bt_cfg);
    if (ret != ESP_OK) {
      Serial.printf("Bluetooth controller re-initialize failed: %s\n", esp_err_to_name(ret));
      ESP.restart();  // Restart if initialization fails
    }
  }

  if ((ret = esp_bt_controller_enable(ESP_BT_MODE_BTDM)) != ESP_OK) {
    Serial.printf("Bluetooth controller enable failed: %s\n", esp_err_to_name(ret));
    ESP.restart();  // Restart if enabling fails
  }

  if ((ret = esp_bluedroid_init()) != ESP_OK) {
    Serial.printf("Bluedroid stack initialization failed: %s\n", esp_err_to_name(ret));
    ESP.restart();  // Restart if initialization fails
  }

  if ((ret = esp_bluedroid_enable()) != ESP_OK) {
    Serial.printf("Bluedroid stack enable failed: %s\n", esp_err_to_name(ret));
    ESP.restart();  // Restart if enabling fails
  }
}

// Function to deinitialize Bluetooth Controller
void deinitBluetoothController() {
  esp_bluedroid_disable();
  esp_bluedroid_deinit();
  esp_bt_controller_disable();
  esp_bt_controller_deinit();
}

// Function to start BLE Server and advertising
void startBLEServer() {
  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID("12345678-1234-5678-1234-56789abcdef0");  // Service UUID
  pAdvertising->setScanResponse(false);
  pAdvertising->setMinPreferred(0x06);  // Functions that help with iPhone connections issue
  pAdvertising->setMinPreferred(0x12);
  BLEDevice::startAdvertising();
  Serial.println("BLE server advertising started. Awaiting JSON data...");
}

// Function to stop BLE Server advertising
void stopBLEServer() {
  BLEDevice::stopAdvertising();
  Serial.println("BLE server advertising stopped.");
}

// BLE Scan complete callback
void scanCompleteCB(BLEScanResults scanResults) {
  int count = scanResults.getCount();
  for (int i = 0; i < count; ++i) {
    BLEAdvertisedDevice device = scanResults.getDevice(i);
    String deviceAddress = device.getAddress().toString().c_str();

    // Iterate through current target devices
    bool is_target = false;
    for (uint8_t j = 0; j < MAX_DEVICE; j++) {
      if (compare_bda(deviceAddress, target_bdas[j])) {
        is_target = true;
        break;
      }
    }

    if (is_target) {
      int rssi = device.getRSSI();
      Serial.printf("Target BLE device discovered: %s\n", deviceAddress.c_str());
      Serial.printf("  RSSI: %d\n", rssi);

      // Implement RSSI averaging for reliability
      int rssi_samples = 3;
      int total_rssi = rssi;
      for (int k = 1; k < rssi_samples; k++) {
        delay(10);
        total_rssi += device.getRSSI();
      }
      int avg_rssi = total_rssi / rssi_samples;

      // Check RSSI and update LED and timer accordingly
      if (avg_rssi >= RSSI_THRESHOLD_VALUE) {
        last_detected_time = millis();  // Update the last detected time

        if (!led_is_on) {
          digitalWrite(LED_PIN, HIGH);   // Turn on LED
          digitalWrite(RELAY_PIN, LOW);  // Activate Relay
          led_is_on = true;
          Serial.println("LED turned ON");
        } else {
          Serial.println("LED is already ON, timer reset");
        }
      } else {
        Serial.printf("Average RSSI (%d) is less than %d, LED remains as is\n", avg_rssi, RSSI_THRESHOLD_VALUE);
      }
    }
  }
}

// Function to compare two Bluetooth addresses (as strings)
bool compare_bda(const String &bda1, const String &bda2) {
  return bda1.equalsIgnoreCase(bda2);
}

// Bluetooth Classic GAP callback function to handle Bluetooth events
void classic_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *param) {
  switch (event) {

    // Event when a device is discovered
    case ESP_BT_GAP_DISC_RES_EVT:
      {
        char bda_str[18];
        bda2str(param->disc_res.bda, bda_str, sizeof(bda_str));
        String deviceAddress = String(bda_str);

        // Iterate through current target devices
        bool is_target = false;
        for (uint8_t i = 0; i < MAX_DEVICE; i++) {
          if (compare_bda(deviceAddress, target_bdas[i])) {
            is_target = true;
            break;
          }
        }

        if (is_target) {
          int rssi = 0;  // Variable to store RSSI value

          // Iterate over device properties to extract RSSI
          for (int i = 0; i < param->disc_res.num_prop; i++) {
            esp_bt_gap_dev_prop_t *p = &param->disc_res.prop[i];
            if (p->type == ESP_BT_GAP_DEV_PROP_RSSI) {
              rssi = *(int8_t *)(p->val);
              break;  // No need to continue once RSSI is found
            }
          }

          Serial.printf("Target Bluetooth Classic device discovered: %s\n", deviceAddress.c_str());
          Serial.printf("  RSSI: %d\n", rssi);

          // Implement RSSI averaging for reliability
          int rssi_samples = 3;
          int total_rssi = rssi;
          for (int k = 1; k < rssi_samples; k++) {
            delay(10);
            total_rssi += rssi;  // Since we can't get updated RSSI, we use the same value
          }
          int avg_rssi = total_rssi / rssi_samples;

          // Check RSSI and update LED and timer accordingly
          if (avg_rssi >= RSSI_THRESHOLD_VALUE) {
            last_detected_time = millis();  // Update the last detected time

            if (!led_is_on) {
              digitalWrite(LED_PIN, HIGH);   // Turn on LED
              digitalWrite(RELAY_PIN, LOW);  // Activate Relay
              led_is_on = true;
              Serial.println("LED turned ON");
            } else {
              Serial.println("LED is already ON, timer reset");
            }
          } else {
            Serial.printf("Average RSSI (%d) is less than %d, LED remains as is\n", avg_rssi, RSSI_THRESHOLD_VALUE);
          }
        }
        break;
      }

    // Event when the discovery state changes
    case ESP_BT_GAP_DISC_STATE_CHANGED_EVT:
      {
        if (param->disc_st_chg.state == ESP_BT_GAP_DISCOVERY_STOPPED) {
          Serial.println("Bluetooth Classic Discovery stopped");
          // Do not restart discovery here; control is managed in the loop()
        } else if (param->disc_st_chg.state == ESP_BT_GAP_DISCOVERY_STARTED) {
          Serial.println("Bluetooth Classic Discovery started");
        }
        break;
      }

    default:
      break;
  }
}

// Helper function to convert Bluetooth address to string format
char *bda2str(const uint8_t *bda, char *str, size_t size) {
  if (bda == NULL || str == NULL || size < 18) {
    return NULL;
  }
  sprintf(str, "%02X:%02X:%02X:%02X:%02X:%02X",
          bda[0], bda[1], bda[2], bda[3], bda[4], bda[5]);
  return str;
}

// Function to update target devices based on received JSON
void updateTargetDevices(JsonDocument &doc) {
  // Implementation remains the same as before
  // Ensure proper handling and logging for reliability
  // Check if JSON contains "devices" array
  if (!doc.containsKey("devices") || !doc["devices"].is<JsonArray>()) {
    Serial.println("Invalid JSON format: 'devices' array not found");
    return;
  }

  JsonArray devices = doc["devices"];

  // Check if the number of devices exceeds MAX_DEVICE
  if (devices.size() > MAX_DEVICE) {
    Serial.printf("Number of devices (%d) exceeds the maximum allowed (%d)\n", devices.size(), MAX_DEVICE);
    return;
  }

  // Open Preferences before writing
  preferences.begin("bt_config", false);  // 'false' means read/write access

  // Iterate through each device in the JSON array
  uint8_t device_index = 0;
  for (JsonVariant device : devices) {
    // Expecting each device to be a string with the Bluetooth address
    if (!device.is<const char *>()) {
      Serial.println("Invalid device format: Device should be a string (Bluetooth address)");
      continue;  // Skip this device
    }

    String bda_str = device.as<String>();
    bda_str.trim();  // Remove any leading/trailing whitespace

    // Validate BDA format
    if (bda_str.length() != 17) {
      Serial.printf("Invalid BDA format: %s\n", bda_str.c_str());
      continue;  // Skip this device
    }

    // Update the target_bdas array
    target_bdas[device_index] = bda_str;
    Serial.printf("Updated target_bdas[%d] to %s\n", device_index, bda_str.c_str());

    // Save the updated target_bda to Preferences
    String key = "bda_" + String(device_index);
    preferences.putString(key.c_str(), bda_str);

    device_index++;
  }

  // Fill the remaining entries with "FF:FF:FF:FF:FF:FF" if fewer devices are provided
  for (; device_index < MAX_DEVICE; device_index++) {
    target_bdas[device_index] = "FF:FF:FF:FF:FF:FF";

    // Save the default value to Preferences
    String key = "bda_" + String(device_index);
    preferences.putString(key.c_str(), target_bdas[device_index]);
  }

  // Close Preferences after writing
  preferences.end();

  Serial.println("Target devices updated and saved to Preferences successfully");
}

// Function to update configuration parameters based on received JSON
bool updateConfiguration(JsonDocument &doc) {
  bool updated = false;

  // Open Preferences before writing
  preferences.begin("bt_config", false);  // 'false' means read/write access

  // Update LED_ON_DURATION if present
  if (doc.containsKey("LED_ON_DURATION") && doc["LED_ON_DURATION"].is<uint32_t>()) {
    LED_ON_DURATION = doc["LED_ON_DURATION"].as<unsigned long>();
    preferences.putULong("LED_ON_DURATION", LED_ON_DURATION);
    Serial.printf("Updated LED_ON_DURATION to %lu ms\n", LED_ON_DURATION);
    updated = true;
  }

  // Update RSSI_THRESHOLD if present
  if (doc.containsKey("RSSI_THRESHOLD") && doc["RSSI_THRESHOLD"].is<int>()) {
    RSSI_THRESHOLD_VALUE = doc["RSSI_THRESHOLD"].as<int>();
    preferences.putInt("RSSI_THRESHOLD", RSSI_THRESHOLD_VALUE);
    Serial.printf("Updated RSSI_THRESHOLD to %d dBm\n", RSSI_THRESHOLD_VALUE);
    updated = true;
  }

  // Update target devices if present
  if (doc.containsKey("devices") && doc["devices"].is<JsonArray>()) {
    updateTargetDevices(doc);
    updated = true;
  }

  // Close Preferences after writing
  preferences.end();

  return updated;
}