#include <ESP32Servo.h>

// --- MOTOR TANIMLARI ---
Servo m1, m2, m3, m4;

const int motorFrontLeftPin = 2;
const int motorFrontRightPin = 4;
const int motorRearLeftPin = 16;
const int motorRearRightPin = 17;

// --- ULTRASONIC ---
const int trigPin = 25;
const int echoPin = 26;

long duration;
int distanceCm;
bool obstacleDetected = false;

// --- KOMUT ---
String lastCommand = "";

// --- THRUST ---
int baseThrust = 300;   // hover
int currentThrust = 0;
int targetThrust = 0;

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

const int pitchRollAdjustment = 80;

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

  // Motor attach
  m1.attach(motorFrontLeftPin);
  m2.attach(motorFrontRightPin);
  m3.attach(motorRearLeftPin);
  m4.attach(motorRearRightPin);

  // Ultrasonic
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  Serial.println("System Ready. Enter commands:");
}

// ---------------- ULTRASONIC ----------------
void readUltrasonicDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distanceCm = duration * 0.034 / 2;
}

// ---------------- SERIAL KOMUT ----------------
void readSerialCommand() {
  if (Serial.available()) {
    lastCommand = Serial.readStringUntil('\n');
    lastCommand.trim();

    Serial.print("Command: ");
    Serial.println(lastCommand);
  }
}

// ---------------- LOOP ----------------
void loop() {

  readUltrasonicDistance();
  readSerialCommand();

  // Obstacle kontrol
  if (distanceCm < 50 && distanceCm > 0) {
    obstacleDetected = true;
  } else {
    obstacleDetected = false;
  }

  // PRIORITY LOGIC
  if (obstacleDetected) {
    targetThrust = baseThrust;
    avoidanceLogic();
  } else {
    executeFlightCommand(lastCommand);
  }

  // RAMPING
  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();
  }
}

// ---------------- COMMAND EXECUTION ----------------
void executeFlightCommand(String command) {

  int FL = currentThrust;
  int FR = currentThrust;
  int RL = currentThrust;
  int RR = currentThrust;

  if (command == "forward") {
    FL -= pitchRollAdjustment;
    FR -= pitchRollAdjustment;
    RL += pitchRollAdjustment;
    RR += pitchRollAdjustment;
    targetThrust = baseThrust;
  }

  else if (command == "backward") {
    FL += pitchRollAdjustment;
    FR += pitchRollAdjustment;
    RL -= pitchRollAdjustment;
    RR -= pitchRollAdjustment;
    targetThrust = baseThrust;
  }

  else if (command == "left") {
    FL -= pitchRollAdjustment;
    RL -= pitchRollAdjustment;
    FR += pitchRollAdjustment;
    RR += pitchRollAdjustment;
    targetThrust = baseThrust;
  }

  else if (command == "right") {
    FL += pitchRollAdjustment;
    RL += pitchRollAdjustment;
    FR -= pitchRollAdjustment;
    RR -= pitchRollAdjustment;
    targetThrust = baseThrust;
  }

  else if (command == "takeoff") {
    targetThrust = baseThrust;
  }

  else if (command == "land") {
    targetThrust = 0;
  }

  else if (command == "hover") {
    targetThrust = baseThrust;
  }

  setMotorThrust(FL, FR, RL, RR);
}

// ---------------- AVOIDANCE ----------------
void avoidanceLogic() {
  Serial.println("Obstacle detected → Hover");

  setMotorThrust(currentThrust, currentThrust, currentThrust, currentThrust);
}

// ---------------- MOTOR CONTROL ----------------
void setMotorThrust(int FL, int FR, int RL, int RR) {

  int m1Val = map(FL, 0, 1023, 0, 180);
  int m2Val = map(FR, 0, 1023, 0, 180);
  int m3Val = map(RL, 0, 1023, 0, 180);
  int m4Val = map(RR, 0, 1023, 0, 180);

  m1.write(m1Val);
  m2.write(m2Val);
  m3.write(m3Val);
  m4.write(m4Val);
}