// UNO R4 - Ultrasonic + LEDs + Servo Gate
#include <Servo.h>

// Pins
const uint8_t TRIG_PIN  = 7;
const uint8_t ECHO_PIN  = 6;
const uint8_t GREEN_PIN = 9;
const uint8_t RED_PIN   = 10;
const uint8_t SERVO_PIN = 5;

// Servo settings
Servo gateServo;
const int CLOSED_ANGLE = 0;    // servo angle when gate closed
const int OPEN_ANGLE   = 90;   // servo angle when gate open
const unsigned int SERVO_STEP_DELAY = 12; // ms between small steps for smooth motion

// Ultrasonic settings
const unsigned long SENSOR_TIMEOUT = 30000UL; // microseconds
const float CM_PER_US = 0.0343 / 2.0; // speed of sound factor (cm/us /2)
const float THRESHOLD_CM = 15.0; // trigger distance in cm

// State tracking
bool gateOpen = false;
float lastDistance = -1;

void setup() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(GREEN_PIN, OUTPUT);
  pinMode(RED_PIN, OUTPUT);

  digitalWrite(TRIG_PIN, LOW);
  digitalWrite(GREEN_PIN, LOW);
  digitalWrite(RED_PIN, LOW);

  Serial.begin(115200);
  delay(100);
  Serial.println("Ultrasonic + Servo Gate starting...");

  gateServo.attach(SERVO_PIN);
  // initialize gate closed
  setServoAngleSmooth(CLOSED_ANGLE);
  gateOpen = false;
  showGreen();
}

float readDistanceCM() {
  // Trigger 10us pulse
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  unsigned long duration = pulseIn(ECHO_PIN, HIGH, SENSOR_TIMEOUT);
  if (duration == 0) return -1.0; // timeout/no echo
  float dist = duration * CM_PER_US;
  return dist;
}

void showGreen() {
  digitalWrite(GREEN_PIN, HIGH);
  digitalWrite(RED_PIN, LOW);
}
void showRed() {
  digitalWrite(GREEN_PIN, LOW);
  digitalWrite(RED_PIN, HIGH);
}

void setServoAngleSmooth(int targetAngle) {
  int current = gateServo.read(); // get current angle
  // If servo returns 255 (unknown), set it to target immediately
  if (current == 255) {
    gateServo.write(targetAngle);
    delay(300);
    return;
  }
  if (current < targetAngle) {
    for (int a = current; a <= targetAngle; a++) {
      gateServo.write(a);
      delay(SERVO_STEP_DELAY);
    }
  } else if (current > targetAngle) {
    for (int a = current; a >= targetAngle; a--) {
      gateServo.write(a);
      delay(SERVO_STEP_DELAY);
    }
  }
  // small settle delay
  delay(80);
}

void openGate() {
  if (!gateOpen) {
    Serial.println("Opening gate...");
    setServoAngleSmooth(OPEN_ANGLE);
    gateOpen = true;
  }
}
void closeGate() {
  if (gateOpen) {
    Serial.println("Closing gate...");
    setServoAngleSmooth(CLOSED_ANGLE);
    gateOpen = false;
  }
}

void loop() {
  float dist = readDistanceCM();

  if (dist < 0) {
    Serial.println("No echo");
    // optional: indicate unknown by blinking both LEDs briefly
    digitalWrite(GREEN_PIN, LOW);
    digitalWrite(RED_PIN, LOW);
  } else {
    Serial.print("Distance: ");
    Serial.print(dist, 1);
    Serial.println(" cm");
    lastDistance = dist;

    if (dist <= THRESHOLD_CM) {
      // object near -> red + open gate
      showRed();
      openGate();
    } else {
      // clear -> green + close gate
      showGreen();
      closeGate();
    }
  }

  delay(120); // measurement pacing
}
