#include <Wire.h>
#include <VL53L0X.h>
#include <WiFi.h>
#include <esp_now.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;
constexpr uint8_t BUTTON_PIN = 10;

// =========================
// 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;  // ms
constexpr unsigned long DEBOUNCE_TIME  = 50;    // ms

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

// =========================
// ESP-NOW receiver MAC
// =========================
uint8_t receiverMac[] = {0xEC, 0xDA, 0x3B, 0xBF, 0x96, 0xC0};

// =========================
// 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;

// =========================
// Data packet to send
// =========================
struct DataPacket {
  uint8_t valid1;
  uint8_t valid2;
  uint8_t valid3;
  uint8_t value1;
  uint8_t value2;
  uint8_t value3;
};

DataPacket packet;

// =========================
// Timing / button state
// =========================
unsigned long lastPrintTime = 0;

bool lastButtonReading = HIGH;
bool stableButtonState = HIGH;
unsigned long lastDebounceTime = 0;

// =========================
// Helper: print MAC address
// =========================
void printMacAddress(const uint8_t* mac) {
  for (int i = 0; i < 6; i++) {
    if (mac[i] < 16) Serial.print("0");
    Serial.print(mac[i], HEX);
    if (i < 5) Serial.print(":");
  }
  Serial.println();
}

// =========================
// ESP-NOW send callback
// =========================
void onDataSent(const esp_now_send_info_t *tx_info, esp_now_send_status_t status) {
  Serial.print("ESP-NOW send status");

  if (tx_info != nullptr && tx_info->des_addr != nullptr) {
    Serial.print(" to ");
    printMacAddress(tx_info->des_addr);
  } else {
    Serial.println();
  }

  if (status == ESP_NOW_SEND_SUCCESS) {
    Serial.println("Transmission successful");
  } else {
    Serial.println("Transmission failed");
  }
  Serial.println();
}

// =========================
// 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(" -> ");
    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 1 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;
}

// =========================
// Build packet from current data
// =========================
void buildPacket() {
  packet.valid1 = r1.valid ? 1 : 0;
  packet.valid2 = r2.valid ? 1 : 0;
  packet.valid3 = r3.valid ? 1 : 0;

  packet.value1 = r1.valid ? r1.mapped : 0;
  packet.value2 = r2.valid ? r2.mapped : 0;
  packet.value3 = r3.valid ? r3.mapped : 0;
}

// =========================
// Send current packet
// =========================
void sendCurrentPacket() {
  buildPacket();

  Serial.println("Button pressed -> sending packet");

  Serial.print("S1: ");
  if (packet.valid1) Serial.println(packet.value1);
  else Serial.println("not sent (Out of Range)");

  Serial.print("S2: ");
  if (packet.valid2) Serial.println(packet.value2);
  else Serial.println("not sent (Out of Range)");

  Serial.print("S3: ");
  if (packet.valid3) Serial.println(packet.value3);
  else Serial.println("not sent (Out of Range)");

  esp_err_t result = esp_now_send(receiverMac, reinterpret_cast<uint8_t*>(&packet), sizeof(packet));

  if (result == ESP_OK) {
    Serial.println("Packet queued for transmission");
  } else {
    Serial.print("Error while sending packet: ");
    Serial.println(result);
  }

  Serial.println();
}

// =========================
// Button handling
// INPUT_PULLUP:
// released = HIGH
// pressed  = LOW
// =========================
void handleButton() {
  bool reading = digitalRead(BUTTON_PIN);

  if (reading != lastButtonReading) {
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > DEBOUNCE_TIME) {
    if (reading != stableButtonState) {
      stableButtonState = reading;

      if (stableButtonState == LOW) {
        sendCurrentPacket();
      }
    }
  }

  lastButtonReading = reading;
}

// =========================
// 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;
}

// =========================
// ESP-NOW initialization
// =========================
bool initEspNow() {
  WiFi.mode(WIFI_STA);
  WiFi.disconnect();

  Serial.print("Sender STA MAC: ");
  Serial.println(WiFi.macAddress());

  if (esp_now_init() != ESP_OK) {
    Serial.println("ESP-NOW initialization failed");
    return false;
  }

  esp_now_register_send_cb(onDataSent);

  esp_now_peer_info_t peerInfo = {};
  memcpy(peerInfo.peer_addr, receiverMac, 6);
  peerInfo.channel = 0;
  peerInfo.encrypt = false;

  if (esp_now_add_peer(&peerInfo) != ESP_OK) {
    Serial.println("Failed to add receiver as peer");
    return false;
  }

  Serial.print("Receiver peer added: ");
  printMacAddress(receiverMac);

  return true;
}

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

  Serial.println();
  Serial.println("ESP32-C3 Master / Sender");
  Serial.println("3x VL53L0X + ESP-NOW");
  Serial.println("Initializing...");
  Serial.println();

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

  pinMode(BUTTON_PIN, INPUT_PULLUP);

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

  if (!initEspNow()) {
    Serial.println("ESP-NOW setup failed. Stopping here.");
    while (true) {
      delay(1000);
    }
  }

  Serial.println();
  Serial.println("System ready.");
  Serial.println("Continuous measurement active.");
  Serial.println("Press button to send current valid values.");
  Serial.println();
}

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