from controller import Robot

robot = Robot()
timestep = int(robot.getBasicTimeStep())

# Devices
ir = robot.getDevice('ir')
compass = robot.getDevice('compass')
gps = robot.getDevice('gps')
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)

def move_forward(speed=6):
    left_motor.setVelocity(speed)
    right_motor.setVelocity(speed)

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

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

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

while robot.step(timestep) != -1:
    ball = ir.getValue()
    x, y, z = gps.getValues()
    
    # DEFENSE MODE: if ball close to our goal (x <0 for Yellow team)
    if x < 0.0 and ball > 50:
        stop()
        # if ball is very close we push it away
        if ball > 100:
            move_forward(8)
    else:
        # ATTACK MODE
        if ball > 80:
            move_forward()
        elif ball > 20:
            turn_left()
        else:
            turn_right()

        # if we are near opponent goal area (x>0.4), push harder to score
        if x > 0.4:
            move_forward(9)