/* STM32 Blue Pill project using the STM32 Arduino Core (stm32duino) */

#include <Servo.h>

#define TRIG_PIN PA1
#define ECHO_PIN PA0
#define SERVO_PIN PB9

#define BUZZER_PIN PB8

#define LED_RED    PB5
#define LED_YELLOW PB6
#define LED_GREEN  PB7

#define BUTTON_PIN PC13

Servo radarServo;
bool autoMode = true;
bool lastButtonState = HIGH;
unsigned long lastDebounce = 0;

void setup() {
  Serial.begin(115200);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  pinMode(LED_RED, OUTPUT);
  pinMode(LED_YELLOW, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);

  pinMode(BUTTON_PIN, INPUT_PULLUP);

  radarServo.attach(SERVO_PIN);

  Serial.println("angle,distance_cm");
}

long readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  long duration = pulseIn(ECHO_PIN, HIGH, 30000);
  long distance = duration * 0.034 / 2;
  return distance;
}

void setLED(bool red, bool yellow, bool green) {
  digitalWrite(LED_RED, red);
  digitalWrite(LED_YELLOW, yellow);
  digitalWrite(LED_GREEN, green);
}

void checkDistanceFeedback(long distance) {
  if (distance <= 0 || distance > 500) {
    // jika pembacaan error atau terlalu jauh, matikan semua
    setLED(LOW, LOW, LOW);
    digitalWrite(BUZZER_PIN, LOW);
    return;
  }

  if (distance < 30) {
    setLED(HIGH, LOW, LOW);       // Merah
  } else if (distance < 100) {
    setLED(LOW, HIGH, LOW);       // Kuning
  } else {
    setLED(LOW, LOW, HIGH);       // Hijau
  }

  // buzzer aktif hanya jika < 100 cm
  if (distance < 100) {
    digitalWrite(BUZZER_PIN, HIGH);
  } else {
    digitalWrite(BUZZER_PIN, LOW);
  }
}


void manualScan() {
  static int manualAngle = 0;
  radarServo.write(manualAngle);
  delay(300);

  long distance = readDistanceCM();
  Serial.print(manualAngle); Serial.print(",");
  Serial.println(distance);
  checkDistanceFeedback(distance);

  manualAngle += 15;
  if (manualAngle > 180) manualAngle = 0;
}

void autoScan() {
  for (int angle = 0; angle <= 180; angle += 5) {
    radarServo.write(angle);
    delay(200);

    long distance = readDistanceCM();
    Serial.print(angle); Serial.print(",");
    Serial.println(distance);
    checkDistanceFeedback(distance);
  }

  for (int angle = 180; angle >= 0; angle -= 5) {
    radarServo.write(angle);
    delay(200);

    long distance = readDistanceCM();
    Serial.print(angle); Serial.print(",");
    Serial.println(distance);
    checkDistanceFeedback(distance);
  }
}

void loop() {
  // Tombol toggle mode
  bool buttonState = digitalRead(BUTTON_PIN);
  if (buttonState == LOW && lastButtonState == HIGH && millis() - lastDebounce > 300) {
    autoMode = !autoMode;
    lastDebounce = millis();
  }
  lastButtonState = buttonState;

  if (autoMode) {
    autoScan();
  } else {
    manualScan();
  }
}