#include 

// Ultrasonic sensor pins
const int trigPin   = 11;
const int echoPin   = 10;

// Output devices
const int servoPin  = 12;
const int buzzerPin = 2;    // Active buzzer: HIGH = sound

const int ledGreen = 4;
const int ledRed   = 3;

// Control buttons
// Blue button: rotate servo to the right
// Red button: rotate servo to the left
const int btnBlue = 8;
const int btnRed  = 9;

// Servo control
Servo radarServo;
int currentAngle = 90;      // Start from center position
int servoDir = 0;           // -1 = left, 0 = stop, 1 = right

const int servoStep = 1;    // Move 1 degree each update
const unsigned long servoInterval = 20;
unsigned long lastServoMillis = 0;

// Direction hold (fix for button switching issue)
unsigned long lastDirChangeMillis = 0;
const unsigned long dirHoldTime = 50; // ms

// Ultrasonic detection
const int alertDistance = 20;   // Alarm distance (cm)
int triggerCount = 0;

unsigned long lastPingMillis = 0;
const unsigned long pingInterval = 60;

// System state
enum RadarState { SAFE, ALERT };
RadarState radarState = SAFE;

// Alarm blinking control
unsigned long lastAlertMillis = 0;
bool alertToggle = false;
const unsigned long alertInterval = 120;

// Last valid distance for radar output
int lastDistance = 40;

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  pinMode(buzzerPin, OUTPUT);
  pinMode(ledGreen, OUTPUT);
  pinMode(ledRed, OUTPUT);

  pinMode(btnBlue, INPUT_PULLUP);
  pinMode(btnRed, INPUT_PULLUP);

  radarServo.attach(servoPin);
  radarServo.write(currentAngle);

  digitalWrite(ledGreen, HIGH);
  digitalWrite(ledRed, LOW);
  digitalWrite(buzzerPin, LOW);

  Serial.begin(9600);
}

void loop() {
  readButtons();
  updateServo();
  updateDistance();
  handleAlert();
  sendRadarData();
}

void readButtons() {
  bool bluePressed = (digitalRead(btnBlue) == LOW);
  bool redPressed  = (digitalRead(btnRed)  == LOW);

  if (bluePressed) {
    servoDir = 1;   // Rotate right
    lastDirChangeMillis = millis();
  }
  else if (redPressed) {
    servoDir = -1;  // Rotate left
    lastDirChangeMillis = millis();
  }
  else {
    // Keep last direction for a short time
    if (millis() - lastDirChangeMillis > dirHoldTime) {
      servoDir = 0;
    }
  }
}

void updateServo() {
  if (millis() - lastServoMillis >= servoInterval) {
    lastServoMillis = millis();

    currentAngle += servoDir * servoStep;
    currentAngle = constrain(currentAngle, 0, 180);

    radarServo.write(currentAngle);
  }
}

void updateDistance() {
  // Skip distance check during alarm
  if (radarState == ALERT) return;

  if (millis() - lastPingMillis < pingInterval) return;
  lastPingMillis = millis();

  int distance = getDistance();
  if (distance <= 0) return;

  lastDistance = distance;

  if (distance < alertDistance) {
    triggerCount++;
  } else {
    triggerCount = 0;
  }

  if (triggerCount >= 5) {
    radarState = ALERT;
  }
}

void handleAlert() {
  if (radarState == ALERT) {
    if (millis() - lastAlertMillis >= alertInterval) {
      lastAlertMillis = millis();
      alertToggle = !alertToggle;

      digitalWrite(ledRed, alertToggle);
      digitalWrite(buzzerPin, alertToggle);
      digitalWrite(ledGreen, LOW);
    }

    int dist = getDistance();
    if (dist >= alertDistance && dist > 0) {
      radarState = SAFE;
      triggerCount = 0;

      digitalWrite(ledRed, LOW);
      digitalWrite(buzzerPin, LOW);
      digitalWrite(ledGreen, HIGH);
    }
  }
  else {
    digitalWrite(ledRed, LOW);
    digitalWrite(buzzerPin, LOW);
    digitalWrite(ledGreen, HIGH);
  }
}

void sendRadarData() {
  // Processing expects: angle,distance.
  Serial.print(currentAngle);
  Serial.print(",");
  Serial.print(lastDistance);
  Serial.print(".");
}

int getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 12000);
  if (duration == 0) return -1;

  return duration * 0.034 / 2;
}