import RPi.GPIO as GPIO
import time

# GPIO pins for motor control
left_motor_pin1 = 17
left_motor_pin2 = 18
right_motor_pin1 = 27
right_motor_pin2 = 22

# GPIO pins for obstacle sensor
obstacle_sensor_pin = 23

# GPIO pins for line sensor
line_sensor_pin = 24

def setup():
    GPIO.setmode(GPIO.BCM)
    GPIO.setup(left_motor_pin1, GPIO.OUT)
    GPIO.setup(left_motor_pin2, GPIO.OUT)
    GPIO.setup(right_motor_pin1, GPIO.OUT)
    GPIO.setup(right_motor_pin2, GPIO.OUT)
    GPIO.setup(obstacle_sensor_pin, GPIO.IN)
    GPIO.setup(line_sensor_pin, GPIO.IN)

def move_forward():
    GPIO.output(left_motor_pin1, GPIO.HIGH)
    GPIO.output(left_motor_pin2, GPIO.LOW)
    GPIO.output(right_motor_pin1, GPIO.HIGH)
    GPIO.output(right_motor_pin2, GPIO.LOW)

def move_backward():
    GPIO.output(left_motor_pin1, GPIO.LOW)
    GPIO.output(left_motor_pin2, GPIO.HIGH)
    GPIO.output(right_motor_pin1, GPIO.LOW)
    GPIO.output(right_motor_pin2, GPIO.HIGH)

def turn_left():
    GPIO.output(left_motor_pin1, GPIO.LOW)
    GPIO.output(left_motor_pin2, GPIO.HIGH)
    GPIO.output(right_motor_pin1, GPIO.HIGH)
    GPIO.output(right_motor_pin2, GPIO.LOW)

def turn_right():
    GPIO.output(left_motor_pin1, GPIO.HIGH)
    GPIO.output(left_motor_pin2, GPIO.LOW)
    GPIO.output(right_motor_pin1, GPIO.LOW)
    GPIO.output(right_motor_pin2, GPIO.HIGH)

def stop():
    GPIO.output(left_motor_pin1, GPIO.LOW)
    GPIO.output(left_motor_pin2, GPIO.LOW)
    GPIO.output(right_motor_pin1, GPIO.LOW)
    GPIO.output(right_motor_pin2, GPIO.LOW)

def obstacle_detected():
    return GPIO.input(obstacle_sensor_pin)

def on_line():
    return GPIO.input(line_sensor_pin)

def autonomous_mode():
    while True:
        if on_line():
            move_forward()
        else:
            stop()
            time.sleep(0.5)
            if obstacle_detected():
                turn_left()
                time.sleep(1)
            else:
                turn_right()
                time.sleep(1)

def main():
    try:
        setup()
        autonomous_mode()
    except KeyboardInterrupt:
        GPIO.cleanup()

if __name__ == '__main__':
    main()