#include <Arduino.h>

// ---------------- PIN DEFINITIONS ----------------
// Ultrasonic sensor pins
const int trigPin = 25; 
const int echoPin = 26; 

// LED control pins (Replacing Motors)
const int ledFrontLeftPin = 2;
const int ledFrontRightPin = 4;
const int ledRearLeftPin = 16;
const int ledRearRightPin = 17;

// ---------------- GLOBAL VARIABLES ----------------
// Set to 255 (100% MAX power for standard analogWrite) 
// This guarantees the virtual LEDs will be bright enough to see!
int baseThrust = 50; 
int currentThrust = 0; 
int targetThrust = baseThrust; 

long duration; 
int distanceCm = 300;

unsigned long lastRampTime = 0;
const int rampIntervalMs = 20;
const int thrustStepSize = 5;

unsigned long lastSensorTime = 0;

// ---------------- SETUP ----------------
void setup() {
  Serial.begin(115200);

  // Configure ultrasonic sensor
  pinMode(trigPin, OUTPUT); 
  pinMode(echoPin, INPUT);

  // Configure LED pins as standard outputs
  pinMode(ledFrontLeftPin, OUTPUT);
  pinMode(ledFrontRightPin, OUTPUT);
  pinMode(ledRearLeftPin, OUTPUT);
  pinMode(ledRearRightPin, OUTPUT);

  Serial.println("System Ready. Spooling up to HOVER automatically...");
}

// ---------------- SENSOR INPUT ----------------
void readUltrasonicDistance() {
  digitalWrite(trigPin, LOW); 
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // 20000 microsecond timeout so the loop doesn't freeze if no echo returns
  duration = pulseIn(echoPin, HIGH, 20000); 
  
  if (duration == 0) {
    distanceCm = 300; // Assume path is clear if no echo
  } else {
    distanceCm = duration * 0.034 / 2;
  }
}

// ---------------- MOTOR OUTPUT ----------------
void setMotorThrust(int fl, int fr, int rl, int rr) {
  // Constrain ensures we don't exceed the 8-bit PWM limit (0-255)
  fl = constrain(fl, 0, 255);
  fr = constrain(fr, 0, 255);
  rl = constrain(rl, 0, 255);
  rr = constrain(rr, 0, 255);

  // Universal analogWrite - completely bypasses ledc channel bugs
  analogWrite(ledFrontLeftPin, fl);
  analogWrite(ledFrontRightPin, fr);
  analogWrite(ledRearLeftPin, rl);
  analogWrite(ledRearRightPin, rr);
}

// ---------------- MAIN LOOP ----------------
void loop() {
  // 1. Read sensor every 50ms (Non-blocking)
  if (millis() - lastSensorTime > 50) {
    readUltrasonicDistance();
    lastSensorTime = millis();
  }

  // 2. Autonomous Priority Logic
  const int safetyThresholdCm = 200;
  
  if (distanceCm < safetyThresholdCm && distanceCm > 0) {
    // Obstacle detected - print warning occasionally
    static unsigned long lastPrint = 0;
    if (millis() - lastPrint > 500) {
        Serial.printf("⚠️ Obstacle at %d cm → HOLDING HOVER\n", distanceCm);
        lastPrint = millis();
    }
    targetThrust = baseThrust; 
  } else {
    // Normal operation - maintain hover
    targetThrust = baseThrust;
  }

  // 3. Smooth Thrust Adjustment (Ramping Logic)
  if (millis() - lastRampTime > rampIntervalMs) {
    if (currentThrust < targetThrust) {
      currentThrust += thrustStepSize;
      if (currentThrust > targetThrust) currentThrust = targetThrust;
    } else if (currentThrust > targetThrust) {
      currentThrust -= thrustStepSize;
      if (currentThrust < targetThrust) currentThrust = targetThrust;
    }
    lastRampTime = millis();
    
    // Apply the updated thrust equally to all 4 LEDs
    setMotorThrust(currentThrust, currentThrust, currentThrust, currentThrust);
  }
}