from controller import Robot, Motor, Compass, GPS, DistanceSensor, Receiver
import math


TIME_STEP = 32
MAX_SPEED = 6.28


robot = Robot()


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


compass = robot.getDevice("compass")
compass.enable(TIME_STEP)

gps = robot.getDevice("gps")
gps.enable(TIME_STEP)

ball_sensor = robot.getDevice("ir")  
ball_sensor.enable(TIME_STEP)

ds = []
ds_names = ["ds_right", "ds_left", "ds_front"]
for name in ds_names:
    sensor = robot.getDevice(name)
    sensor.enable(TIME_STEP)
    ds.append(sensor)


def get_bearing():
    north = compass.getValues()
    rad = math.atan2(north[0], north[2])
    bearing = (rad - 1.5708)  # تصحيح الزاوية
    if bearing < -math.pi:
        bearing += 2 * math.pi
    return bearing


def go_to_ball(ball_direction):
    angle = ball_direction
    if abs(angle) < 0.1:
        left_speed = MAX_SPEED
        right_speed = MAX_SPEED
    elif angle < 0:
        left_speed = MAX_SPEED * 0.5
        right_speed = MAX_SPEED
    else:
        left_speed = MAX_SPEED
        right_speed = MAX_SPEED * 0.5
    return left_speed, right_speed


def avoid_obstacles():
    front = ds[2].getValue()
    right = ds[0].getValue()
    left = ds[1].getValue()
    if front > 80:
        return -1, -1  
    elif left > 80:
        return MAX_SPEED, MAX_SPEED * 0.2  
    elif right > 80:
        return MAX_SPEED * 0.2, MAX_SPEED  
    return None


while robot.step(TIME_STEP) != -1:
    ball_values = ball_sensor.getValue()

    
    if ball_values > 0.2:
        ball_angle = (ball_values - 0.5) * 3.14  
        obstacle = avoid_obstacles()
        if obstacle:
            left_speed, right_speed = obstacle
        else:
            left_speed, right_speed = go_to_ball(ball_angle)
    else:
        
        left_speed = MAX_SPEED * 0.5
        right_speed = -MAX_SPEED * 0.5

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