#include <stdio.h>
#include <wiringPi.h>

#define SERVO_PIN 4 // GPIO pin for servo control
#define TRIGGER_PIN 3 // GPIO pin for ultrasonic sensor trigger
#define ECHO_PIN 2 // GPIO pin for ultrasonic sensor echo
#define MAX_DISTANCE 200 // Maximum distance for servo rotation

void ultrasonicSetup() {
    // Setup ultrasonic sensor
    pinMode(TRIGGER_PIN, OUTPUT);
    pinMode(ECHO_PIN, INPUT);
}

float getDistance() {
    // Measure distance using ultrasonic sensor
    digitalWrite(TRIGGER_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIGGER_PIN, LOW);

    unsigned long startTime, endTime;
    startTime = micros();
    while (digitalRead(ECHO_PIN) == LOW);
    startTime = micros();
    while (digitalRead(ECHO_PIN) == HIGH);
    endTime = micros();

    return (float)(endTime - startTime) * 0.0343 / 2.0; // Calculate distance in cm
}

void servoSetup() {
    // Setup servo motor
    pinMode(SERVO_PIN, PWM_OUTPUT);
    pwmSetMode(PWM_MODE_MS);
    pwmSetClock(192);
    pwmSetRange(2000);
}

void rotateServo(float angle) {
    // Rotate servo to specified angle
    int pulseWidth = 50 + (int)(angle * 10); // Convert angle to pulse width
    pwmWrite(SERVO_PIN, pulseWidth);
}

void setup() {
    wiringPiSetupGpio(); // Initialize WiringPi library
    ultrasonicSetup(); // Setup ultrasonic sensor
    servoSetup(); // Setup servo motor
}

void loop() {
    float distance = getDistance();
    printf("Distance: %.2f cm\n", distance);

    if (distance < MAX_DISTANCE) {
        // Rotate servo to 180 degrees
        rotateServo(180.0);
        printf("Rotating servo to 180 degrees\n");
    } else {
        // Rotate servo to 0 degrees
        rotateServo(0.0);
        printf("Rotating servo to 0 degrees\n");
    }

    delay(500); // Wait for a short time before next iteration
}

int main() {
    setup(); // Call setup function once
    while (1) {
        loop(); // Call loop function continuously
    }
    return 0;
}