#include <Wire.h>
#include <VL53L0X.h>

#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>

// =========================
// Pin configuration
// =========================
constexpr uint8_t SDA_PIN    = 9;
constexpr uint8_t SCL_PIN    = 8;
constexpr uint8_t XSHUT1_PIN = 5;
constexpr uint8_t XSHUT2_PIN = 6;
constexpr uint8_t XSHUT3_PIN = 7;

// =========================
// I2C addresses
// =========================
constexpr uint8_t SENSOR1_ADDR = 0x30;
constexpr uint8_t SENSOR2_ADDR = 0x31;
constexpr uint8_t SENSOR3_ADDR = 0x32;

// =========================
// General settings
// =========================
constexpr unsigned long PRINT_INTERVAL    = 1000;  // Serial output interval
constexpr unsigned long BLE_SEND_INTERVAL = 300;   // BLE update interval

constexpr int SENSOR_OFFSET_MM = 14;   // correction: measured distance - 14 mm
constexpr int RANGE_MIN_MM     = 100;  // valid range start
constexpr int RANGE_MAX_MM     = 500;  // valid range end

// =========================
// BLE configuration
// Must match MIT App Inventor
// =========================
#define BLE_DEVICE_NAME "ToF_Board_Nico"

#define SERVICE_UUID           "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"
#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"

// =========================
// Sensor objects
// =========================
VL53L0X sensor1;
VL53L0X sensor2;
VL53L0X sensor3;

// =========================
// Runtime sensor data
// =========================
struct SensorReading {
  int rawMm = -1;
  int correctedMm = -1;
  uint8_t mapped = 0;
  bool valid = false;
};

SensorReading r1, r2, r3;

// =========================
// Timing
// =========================
unsigned long lastPrintTime = 0;
unsigned long lastBleSendTime = 0;

// =========================
// BLE runtime objects
// =========================
BLEServer* pServer = nullptr;
BLECharacteristic* pTxCharacteristic = nullptr;

bool deviceConnected = false;
bool oldDeviceConnected = false;

// =========================
// BLE callbacks
// =========================
class MyServerCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer* pServer) override {
    deviceConnected = true;
    Serial.println("BLE device connected");
  }

  void onDisconnect(BLEServer* pServer) override {
    deviceConnected = false;
    Serial.println("BLE device disconnected");
  }
};

// =========================
// Safe VL53L0X read helper
// =========================
bool readDistanceMm(VL53L0X& sensor, int& distanceMm) {
  int reading = sensor.readRangeContinuousMillimeters();

  if (sensor.timeoutOccurred()) {
    return false;
  }

  if (reading <= 0 || reading >= 8190) {
    return false;
  }

  distanceMm = reading;
  return true;
}

// =========================
// Convert corrected distance
// to 0...255
// 100 mm -> 0
// 500 mm -> 255
// =========================
uint8_t mapDistanceToByte(int correctedMm) {
  long mapped = map(correctedMm, RANGE_MIN_MM, RANGE_MAX_MM, 0, 255);

  if (mapped < 0)   mapped = 0;
  if (mapped > 255) mapped = 255;

  return static_cast<uint8_t>(mapped);
}

// =========================
// Process one sensor reading
// =========================
void updateSensorReading(VL53L0X& sensor, SensorReading& reading) {
  int raw = -1;
  bool ok = readDistanceMm(sensor, raw);

  reading.rawMm = -1;
  reading.correctedMm = -1;
  reading.mapped = 0;
  reading.valid = false;

  if (!ok) {
    return;
  }

  reading.rawMm = raw;
  reading.correctedMm = raw - SENSOR_OFFSET_MM;

  if (reading.correctedMm >= RANGE_MIN_MM && reading.correctedMm <= RANGE_MAX_MM) {
    reading.valid = true;
    reading.mapped = mapDistanceToByte(reading.correctedMm);
  }
}

// =========================
// Serial output helper
// =========================
void printSensorLine(const char* name, const SensorReading& r) {
  Serial.print(name);
  Serial.print(": ");

  if (r.rawMm < 0) {
    Serial.println("invalid");
    return;
  }

  Serial.print(r.correctedMm);
  Serial.print(" mm");

  if (r.valid) {
    Serial.print(" -> mapped: ");
    Serial.print(r.mapped);
  } else {
    Serial.print(" -> Out of Range");
  }

  Serial.println();
}

// =========================
// Read all sensors continuously
// =========================
void updateAllSensors() {
  updateSensorReading(sensor1, r1);
  updateSensorReading(sensor2, r2);
  updateSensorReading(sensor3, r3);
}

// =========================
// Print all sensor values every second
// =========================
void printAllSensorsIfDue() {
  unsigned long now = millis();

  if (now - lastPrintTime < PRINT_INTERVAL) {
    return;
  }

  Serial.println("Current sensor values:");
  printSensorLine("S1", r1);
  printSensorLine("S2", r2);
  printSensorLine("S3", r3);
  Serial.println();

  lastPrintTime = now;
}

// =========================
// Value for app display
// -1 means invalid sensor reading
// =========================
int appValueFromReading(const SensorReading& r) {
  if (r.rawMm < 0) {
    return -1;
  }

  return r.correctedMm;
}

// =========================
// Send current sensor values by BLE
// Format: S1,S2,S3
// Example: 123,456,789
// =========================
void sendBleDataIfDue() {
  unsigned long now = millis();

  if (!deviceConnected) {
    return;
  }

  if (now - lastBleSendTime < BLE_SEND_INTERVAL) {
    return;
  }

  int s1 = appValueFromReading(r1);
  int s2 = appValueFromReading(r2);
  int s3 = appValueFromReading(r3);

  String payload = String(s1) + "," + String(s2) + "," + String(s3);

  pTxCharacteristic->setValue((uint8_t*)payload.c_str(), payload.length() + 1);
  pTxCharacteristic->notify();

  Serial.print("BLE sent: ");
  Serial.println(payload);

  lastBleSendTime = now;
}

// =========================
// BLE reconnect handling
// =========================
void handleBleConnectionState() {
  if (!deviceConnected && oldDeviceConnected) {
    delay(500);
    pServer->startAdvertising();
    Serial.println("BLE advertising restarted");
    oldDeviceConnected = false;
  }

  if (deviceConnected && !oldDeviceConnected) {
    oldDeviceConnected = true;
  }
}

// =========================
// Sensor initialization
// =========================
bool initSensors() {
  pinMode(XSHUT1_PIN, OUTPUT);
  pinMode(XSHUT2_PIN, OUTPUT);
  pinMode(XSHUT3_PIN, OUTPUT);

  digitalWrite(XSHUT1_PIN, LOW);
  digitalWrite(XSHUT2_PIN, LOW);
  digitalWrite(XSHUT3_PIN, LOW);
  delay(100);

  // -------- Sensor 1 --------
  digitalWrite(XSHUT1_PIN, HIGH);
  delay(100);

  if (!sensor1.init()) {
    Serial.println("Error: Sensor 1 init failed.");
    return false;
  }

  sensor1.setAddress(SENSOR1_ADDR);
  sensor1.setTimeout(100);
  sensor1.startContinuous();

  Serial.println("Sensor 1 ready at 0x30");

  // -------- Sensor 2 --------
  digitalWrite(XSHUT2_PIN, HIGH);
  delay(100);

  if (!sensor2.init()) {
    Serial.println("Error: Sensor 2 init failed.");
    return false;
  }

  sensor2.setAddress(SENSOR2_ADDR);
  sensor2.setTimeout(100);
  sensor2.startContinuous();

  Serial.println("Sensor 2 ready at 0x31");

  // -------- Sensor 3 --------
  digitalWrite(XSHUT3_PIN, HIGH);
  delay(100);

  if (!sensor3.init()) {
    Serial.println("Error: Sensor 3 init failed.");
    return false;
  }

  sensor3.setAddress(SENSOR3_ADDR);
  sensor3.setTimeout(100);
  sensor3.startContinuous();

  Serial.println("Sensor 3 ready at 0x32");

  return true;
}

// =========================
// BLE initialization
// =========================
bool initBle() {
  BLEDevice::init(BLE_DEVICE_NAME);

  pServer = BLEDevice::createServer();

  if (pServer == nullptr) {
    Serial.println("BLE server creation failed");
    return false;
  }

  pServer->setCallbacks(new MyServerCallbacks());

  BLEService* pService = pServer->createService(SERVICE_UUID);

  if (pService == nullptr) {
    Serial.println("BLE service creation failed");
    return false;
  }

  pTxCharacteristic = pService->createCharacteristic(
    CHARACTERISTIC_UUID_TX,
    BLECharacteristic::PROPERTY_NOTIFY | BLECharacteristic::PROPERTY_READ
  );

  if (pTxCharacteristic == nullptr) {
    Serial.println("BLE TX characteristic creation failed");
    return false;
  }

  pTxCharacteristic->addDescriptor(new BLE2902());

  // Initial value before first real notification
  pTxCharacteristic->setValue("-1,-1,-1");

  pService->start();

  BLEAdvertising* pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->setScanResponse(true);
  pAdvertising->start();

  Serial.println("BLE initialized");
  Serial.print("BLE device name: ");
  Serial.println(BLE_DEVICE_NAME);
  Serial.print("BLE service UUID: ");
  Serial.println(SERVICE_UUID);

  return true;
}

// =========================
// Setup
// =========================
void setup() {
  Serial.begin(115200);
  delay(2000);

  Serial.println();
  Serial.println("ESP32-C3 ToF BLE Interface");
  Serial.println("3x VL53L0X + BLE Notify");
  Serial.println("Initializing...");
  Serial.println();

  Wire.begin(SDA_PIN, SCL_PIN);
  Wire.setClock(100000);

  if (!initSensors()) {
    Serial.println("Sensor initialization failed. Stopping here.");
    while (true) {
      delay(1000);
    }
  }

  if (!initBle()) {
    Serial.println("BLE initialization failed. Stopping here.");
    while (true) {
      delay(1000);
    }
  }

  Serial.println();
  Serial.println("System ready.");
  Serial.println("Continuous ToF measurement active.");
  Serial.println("Waiting for BLE app connection...");
  Serial.println();
}

// =========================
// Main loop
// =========================
void loop() {
  updateAllSensors();
  printAllSensorsIfDue();

  sendBleDataIfDue();
  handleBleConnectionState();
}