# === Robot 1: Intelligent Soccer Robot ===
# Author: Motaz Adel
# Description: Enhanced version for better ball tracking and obstacle avoidance

from controller import Robot

robot = Robot()
TIME_STEP = int(robot.getBasicTimeStep()) or 32

left_motor = robot.getDevice("left wheel motor")
right_motor = robot.getDevice("right wheel motor")
for motor in (left_motor, right_motor):
    motor.setPosition(float('inf'))
    motor.setVelocity(0.0)

MAX_SPEED = 6.28

ps = []
for i in range(8):
    sensor = robot.getDevice(f"ps{i}")
    sensor.enable(TIME_STEP)
    ps.append(sensor)

camera = robot.getDevice("camera")
camera.enable(TIME_STEP)
camera.recognitionEnable(TIME_STEP)

def avoid_obstacles():
    left_speed = MAX_SPEED * 0.5
    right_speed = MAX_SPEED * 0.5
    left_obstacle = ps[0].getValue() > 80.0 or ps[1].getValue() > 80.0 or ps[2].getValue() > 80.0
    right_obstacle = ps[5].getValue() > 80.0 or ps[6].getValue() > 80.0 or ps[7].getValue() > 80.0

    if left_obstacle:
        left_speed = -0.3 * MAX_SPEED
        right_speed = 0.6 * MAX_SPEED
    elif right_obstacle:
        left_speed = 0.6 * MAX_SPEED
        right_speed = -0.3 * MAX_SPEED
    return left_speed, right_speed

while robot.step(TIME_STEP) != -1:
    left_speed, right_speed = avoid_obstacles()
    objects = camera.getRecognitionObjects()
    if objects:
        ball = objects[0]
        position = ball.get_position_on_image()
        image_center = camera.getWidth() / 2
        ball_x = position[0]
        error = (ball_x - image_center) / image_center

        left_speed = MAX_SPEED * (1.0 - error)
        right_speed = MAX_SPEED * (1.0 + error)

        if abs(error) < 0.1:
            left_speed = MAX_SPEED
            right_speed = MAX_SPEED

    left_motor.setVelocity(left_speed)
    right_motor.setVelocity(right_speed)