# Robot 1 Soccer Competition – Advanced Smart Version
# Author: Abdullah_Alanazi
# Note: No external libraries, fully compliant with Webots rules

from controller import Robot

# Initialize the robot
robot = Robot()
timestep = int(robot.getBasicTimeStep())

# Get devices
left_motor = robot.getDevice("left wheel motor")
right_motor = robot.getDevice("right wheel motor")
ball_sensor = robot.getDevice("ball_sensor")
compass = robot.getDevice("compass")
sonar = robot.getDevice("sonar")

# Enable sensors
ball_sensor.enable(timestep)
compass.enable(timestep)
sonar.enable(timestep)

# Motor settings
left_motor.setPosition(float('inf'))
right_motor.setPosition(float('inf'))
left_motor.setVelocity(0.0)
right_motor.setVelocity(0.0)

# Helper functions
def go_forward(speed):
    left_motor.setVelocity(speed)
    right_motor.setVelocity(speed)

def turn_left(speed):
    left_motor.setVelocity(-speed)
    right_motor.setVelocity(speed)

def turn_right(speed):
    left_motor.setVelocity(speed)
    right_motor.setVelocity(-speed)

def stop():
    left_motor.setVelocity(0)
    right_motor.setVelocity(0)

# Main loop
while robot.step(timestep) != -1:
    
    ball_value = ball_sensor.getValue()
    compass_values = compass.getValues()
    front_distance = sonar.getValue()
    
    # ---------------------
    # Ball Tracking Logic
    # ---------------------
    if ball_value < 0.2:
        # Ball is far, move fast
        go_forward(5.0)
    elif ball_value < 0.5:
        # Ball is closer, slow down for control
        go_forward(2.5)
    else:
        # Ball very close, stop and kick
        stop()
    
    # Predictive adjustment
    if ball_value > 0.3 and ball_value < 0.8:
        # Slight turn toward ball if needed
        if compass_values[0] < -0.05:
            turn_left(1.0)
        elif compass_values[0] > 0.05:
            turn_right(1.0)
    
    # ---------------------
    # Obstacle Avoidance
    # ---------------------
    if front_distance < 0.15:
        # Smooth turn to avoid
        turn_left(2.0)
    
    # ---------------------
    # Kick Strategy
    # ---------------------
    if ball_value > 0.8:
        # Push ball toward opponent goal
        if compass_values[0] < 0:
            turn_right(1.5)
        elif compass_values[0] > 0:
            turn_left(1.5)
        go_forward(4.0)
    
    # Safety stop near edges (sonar detects field limit)
    if front_distance < 0.05:
        stop()