// ====================================================================
//   COMBINED TELEMETRY & ROTATION SYSTEM
//   Target Device: ESP32 DevKit / NodeMCU (Dual-Core Asynchronous Engine)
// ====================================================================

#include <Wire.h>
#include <SPI.h>
#include <LoRa.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BME280.h>
#include <BH1750.h>
#include <MQUnifiedsensor.h>

// ================= GLOBAL CONFIGURATION =================
const bool enableSerial = true; 
const bool enableLoRa   = true; 

// Set TRUE to prevent missing sensors/LoRa modules from locking up the system boot
const bool bypassSensorLockingLoops = true; 

// ================= PIN DEFINITIONS =================
// --- Motor Pins (Dish Rotation) ---
const int m1Step = 17, m1Dir = 16, m1MS = 4;    // Tied MS1, MS2, MS3 of Motor 1 to GPIO 4
const int m2Step = 25, m2Dir = 13, m2MS = 15;   // Tied MS1, MS2, MS3 of Motor 2 to GPIO 15

// --- Telemetry Sensor Pins ---
#define I2C_SDA 21
#define I2C_SCL 22
#define MQ135_PIN 34
#define MQ135_DIGITAL_PIN 32
#define WIND_PIN 27
#define RAIN_ANALOG_PIN 35
#define RAIN_DIGITAL_PIN 33
#define BATTERY_PIN 39

// --- LoRa SX1278 SPI Bus ---
#define LORA_SS    5
#define LORA_RST   14
#define LORA_DIO0  26

// ================= SYSTEM CONSTANTS =================
const long RAW_STEPS_PER_REV = 200 * 16; 
#define SEALEVELPRESSURE_HPA 1013.25
#define LORA_FREQUENCY 433E6

const float BATTERY_DIVIDER_RATIO = 2.0;
const float ADC_REF = 3.3;
const int ESP32_ADC_MAX = 4095;
const float WIND_FACTOR_KMH = 2.4;

#define Board "ESP-32"
#define Voltage_Resolution 3.3
#define ADC_Bit_Resolution 12
#define RatioMQ135CleanAir 3.6

// ================= INSTANCES & DRIVERS =================
Adafruit_BME280 bme;
BH1750 lightMeter;
MQUnifiedsensor mq135(Board, Voltage_Resolution, ADC_Bit_Resolution, MQ135_PIN, "MQ-135");

// ================= SYSTEM HEALTH FLAGS =================
bool bmeOperational   = false;
bool lightOperational = false;
bool loraOperational  = false;

// ================= MULTITASKING MUTEXES & STATE =================
SemaphoreHandle_t loraMutex;

// --- Telemetry State ---
volatile unsigned long windPulseCount = 0;
unsigned long lastMeasurement = 0;
const unsigned long measurementInterval = 5000; 

// --- Rotation State ---
long azimuthPosRaw = 0, elevationPosRaw = 0;
const int stepsPerCommand = 1; 
bool currentDir1 = HIGH, currentDir2 = HIGH;
int stepMultiplier1 = 16;
int stepMultiplier2 = 16;

// ================= ISR INTERRUPT CHANNEL =================
void IRAM_ATTR windISR() {
  windPulseCount++;
}

// ================= CORE FUNCTIONS =================
float readBatteryVoltage() {
  int raw = analogRead(BATTERY_PIN);
  float voltage = ((float)raw / ESP32_ADC_MAX) * ADC_REF;
  return voltage * BATTERY_DIVIDER_RATIO;
}

float readWindSpeedKmh(unsigned long pulses, float intervalSeconds) {
  float pulsesPerSecond = pulses / intervalSeconds;
  return pulsesPerSecond * WIND_FACTOR_KMH;
}

float readMQ135PPM() {
  mq135.update();
  return mq135.readSensor();
}

void sendLoRaPacket(String packet) {
  if (enableLoRa && loraOperational) {
    if (xSemaphoreTake(loraMutex, portMAX_DELAY) == pdTRUE) {
      LoRa.beginPacket();
      LoRa.print(packet);
      LoRa.endPacket(true); 
      xSemaphoreGive(loraMutex); 
    }
  }
}

void printStatus() {
  String statusPacket = String((float)azimuthPosRaw / 16.0, 4) + "," + String((float)elevationPosRaw / 16.0, 4);

  if (enableSerial) {
    Serial.println(statusPacket);
  }
  sendLoRaPacket(statusPacket);
}

void moveMotor(int stepPin, int dirPin, bool targetDir, int posDirection) {
  bool &activeDir = (stepPin == m1Step) ? currentDir1 : currentDir2;
  
  if (activeDir != targetDir) {
    activeDir = targetDir;
    digitalWrite(dirPin, activeDir);
    delayMicroseconds(10); 
  }
  
  for (int i = 0; i < stepsPerCommand; i++) {
    digitalWrite(stepPin, HIGH);  delayMicroseconds(800);
    digitalWrite(stepPin, LOW);   delayMicroseconds(800);
  }
  
  int activeMultiplier = (stepPin == m1Step) ? stepMultiplier1 : stepMultiplier2;
  long coordinateChange = (long)stepsPerCommand * activeMultiplier * posDirection;
  
  if (stepPin == m1Step) {
    azimuthPosRaw += coordinateChange;
    azimuthPosRaw = azimuthPosRaw % RAW_STEPS_PER_REV;
  } else {
    elevationPosRaw += coordinateChange;
    elevationPosRaw = elevationPosRaw % RAW_STEPS_PER_REV;
  }
  
  printStatus();
}

// ================= ASYNCHRONOUS BACKGROUND WORKER (CORE 0) =================
void telemetryBackgroundTask(void *pvParameters) {
  for (;;) {
    if (millis() - lastMeasurement >= measurementInterval) {
      lastMeasurement = millis();

      noInterrupts();
      unsigned long pulses = windPulseCount;
      windPulseCount = 0;
      interrupts();

      float intervalSeconds = measurementInterval / 1000.0;

      // Conditional safety fallback processing for missing sensor modules
      float temperature  = bmeOperational   ? bme.readTemperature() : 0.0;
      float humidity     = bmeOperational   ? bme.readHumidity() : 0.0;
      float pressure     = bmeOperational   ? (bme.readPressure() / 100.0F) : 0.0; 
      float lux          = lightOperational ? lightMeter.readLightLevel() : 0.0;
      
      float airQuality   = readMQ135PPM(); 
      int mqDigital      = digitalRead(MQ135_DIGITAL_PIN);
      float windSpeed    = readWindSpeedKmh(pulses, intervalSeconds);
      int rainAnalog     = analogRead(RAIN_ANALOG_PIN);
      int rainDigital    = digitalRead(RAIN_DIGITAL_PIN); 
      int rainDetected   = (rainDigital == LOW) ? 1 : 0;
      float batteryVoltage = readBatteryVoltage();

      String packet = "TEMP=" + String(temperature, 2) +
                      ",HUM=" + String(humidity, 2) +
                      ",PRESS=" + String(pressure, 2) +
                      ",AQ=" + String(airQuality, 2) +
                      ",WIND=" + String(windSpeed, 2) +
                      ",RAIN=" + String(rainDetected) +
                      ",RAIN_A=" + String(rainAnalog) +
                      ",LUX=" + String(lux, 1) +
                      ",BAT=" + String(batteryVoltage, 2) +
                      ",MQ_D=" + String(mqDigital);

      if (enableSerial) {
        Serial.println("====================================");
        Serial.print("Temperature: "); Serial.print(temperature); Serial.println(" C");
        Serial.print("Humidity: "); Serial.print(humidity); Serial.println(" %");
        Serial.print("Pressure: "); Serial.print(pressure); Serial.println(" hPa");
        Serial.print("Light: "); Serial.print(lux); Serial.println(" lux");
        Serial.print("Air Quality: "); Serial.print(airQuality); Serial.println(" ppm");
        Serial.print("Wind Speed: "); Serial.print(windSpeed); Serial.println(" km/h");
        Serial.print("Rain Detected: "); Serial.println(rainDetected ? "YES" : "NO");
        Serial.print("Rain Analog: "); Serial.println(rainAnalog);
        Serial.print("Battery Voltage: "); Serial.print(batteryVoltage); Serial.println(" V");
        Serial.println("Telemetry Packet:");
        Serial.println(packet);
      }

      sendLoRaPacket(packet);
      
      if (enableSerial && loraOperational && enableLoRa) {
        Serial.println("LoRa packet sent.");
      }
    }
    vTaskDelay(pdMS_TO_TICKS(10));
  }
}

// ================= SYSTEM INITIALIZATION =================
void setup() {
  if (enableSerial) {
    Serial.begin(115200);
  }
  delay(1000);

  loraMutex = xSemaphoreCreateMutex();
  analogReadResolution(12);
  Wire.begin(I2C_SDA, I2C_SCL);

  // ----- BME280 Check -----
  if (!bme.begin(0x76)) {
    if (enableSerial) Serial.println("WARNING: BME280 not found at 0x76");
    if (!bypassSensorLockingLoops) {
      while (1); // Trap system here if bypass is disabled
    }
  } else {
    bmeOperational = true;
  }

  // ----- BH1750 Check -----
  if (!lightMeter.begin(BH1750::CONTINUOUS_HIGH_RES_MODE)) {
    if (enableSerial) Serial.println("WARNING: BH1750 not found");
    if (!bypassSensorLockingLoops) {
      while (1); 
    }
  } else {
    lightOperational = true;
  }

  // Motor Pin Allocations
  pinMode(m1Step, OUTPUT); pinMode(m1Dir, OUTPUT); pinMode(m1MS, OUTPUT);
  pinMode(m2Step, OUTPUT); pinMode(m2Dir, OUTPUT); pinMode(m2MS, OUTPUT);
  digitalWrite(m1Dir, currentDir1);
  digitalWrite(m2Dir, currentDir2);
  digitalWrite(m1MS, LOW);
  digitalWrite(m2MS, LOW);

  // Analog & Pulsed Sensor Configuration
  pinMode(MQ135_DIGITAL_PIN, INPUT);
  mq135.setRegressionMethod(1); 
  mq135.setA(110.47); mq135.setB(-2.862);
  mq135.init();

  if (enableSerial) Serial.println("Calibrating MQ-135 in clean air...");
  float calcR0 = 0;
  for (int i = 0; i < 10; i++) {
    mq135.update();
    calcR0 += mq135.calibrate(RatioMQ135CleanAir);
    delay(500);
  }
  mq135.setR0(calcR0 / 10.0);

  pinMode(WIND_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(WIND_PIN), windISR, FALLING);
  pinMode(RAIN_DIGITAL_PIN, INPUT);

  // ----- LoRa Setup -----
  if (enableLoRa) {
    LoRa.setPins(LORA_SS, LORA_RST, LORA_DIO0);
    if (!LoRa.begin(LORA_FREQUENCY)) {
      if (enableSerial) Serial.println("WARNING: LoRa initialization failed");
      if (!bypassSensorLockingLoops) {
        while (1); 
      }
    } else {
      loraOperational = true;
    }
  }

  if (enableSerial) {
    Serial.println("System initialization complete.");
  }

  // Spawn Background Weather Processing on Core 0
  xTaskCreatePinnedToCore(
    telemetryBackgroundTask,   
    "TelemetryTask",           
    8192,                      
    NULL,                      
    1,                         
    NULL,                      
    0                          
  );
}

// ================= REAL-TIME DISH INTERFACE LOOP (CORE 1) =================
void loop() {
  if (enableSerial && Serial.available() > 0) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();
    
    if      (cmd.equalsIgnoreCase("1M01")) { digitalWrite(m1MS, LOW);  stepMultiplier1 = 16; } 
    else if (cmd.equalsIgnoreCase("1M16")) { digitalWrite(m1MS, HIGH); stepMultiplier1 = 1;  } 
    else if (cmd.equalsIgnoreCase("2M01")) { digitalWrite(m2MS, LOW);  stepMultiplier2 = 16; } 
    else if (cmd.equalsIgnoreCase("2M16")) { digitalWrite(m2MS, HIGH); stepMultiplier2 = 1;  } 
    
    else if (cmd.equalsIgnoreCase("Right"))  moveMotor(m1Step, m1Dir, HIGH, 1);
    else if (cmd.equalsIgnoreCase("Left"))   moveMotor(m1Step, m1Dir, LOW, -1);
    else if (cmd.equalsIgnoreCase("Up"))     moveMotor(m2Step, m2Dir, HIGH, 1);
    else if (cmd.equalsIgnoreCase("Down"))   moveMotor(m2Step, m2Dir, LOW, -1);
    else if (cmd.equalsIgnoreCase("Status")) printStatus();
  }
}