# NouraAlharbi - robot1.py (for submission as NouraAlharbi.txt)
# Final Assignment – Robot Soccer Competition - Run 2
# Improved Robot 1 controller (procedural, no external libraries)
# Author: نورة الحربي
# Notes: Paste this code into robot1.py for the Webots Robot 1 controller.
# Uses available sensors: IR ball sensors (ir1..ir7), compass, sonar, gps (if present).
# Adjust device names if your world uses different names.

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

# ---------- PARAMETERS ----------
TIME_STEP = 64
MAX_SPEED = 6.28  # adjust if your robot uses different max speed
BALL_IR_NAMES = ['ir1','ir2','ir3','ir4','ir5','ir6','ir7']  # try these names; modify if your world uses others
LEFT_MOTOR_NAME = 'left wheel motor'
RIGHT_MOTOR_NAME = 'right wheel motor'
SONAR_NAME = 'sonar'   # optional, present in some worlds
COMPASS_NAME = 'compass'
GPS_NAME = 'gps'       # optional

# control thresholds (tune if needed)
BALL_CLOSE_THRESHOLD = 800.0   # IR sensor value that indicates ball is close (tweak per world)
SONAR_OBSTACLE_THRESHOLD = 0.35  # meters
GOAL_HEADING_TOLERANCE = 0.15  # radians ~ ~8.6 degrees

# ---------- INITIALIZE ROBOT ----------
robot = Robot()

# motors
left_motor = robot.getDevice(LEFT_MOTOR_NAME)
right_motor = robot.getDevice(RIGHT_MOTOR_NAME)
left_motor.setPosition(float('inf'))
right_motor.setPosition(float('inf'))
left_motor.setVelocity(0.0)
right_motor.setVelocity(0.0)

# IR ball sensors
ball_sensors = []
for name in BALL_IR_NAMES:
    try:
        s = robot.getDevice(name)
        s.enable(TIME_STEP)
        ball_sensors.append(s)
    except Exception:
        # sensor missing; skip
        pass

# sonar (distance) sensor (optional)
sonar = None
try:
    sonar = robot.getDevice(SONAR_NAME)
    sonar.enable(TIME_STEP)
except Exception:
    sonar = None

# compass (for orientation)
compass = None
try:
    compass = robot.getDevice(COMPASS_NAME)
    compass.enable(TIME_STEP)
except Exception:
    compass = None

# gps (optional; for position)
gps = None
try:
    gps = robot.getDevice(GPS_NAME)
    gps.enable(TIME_STEP)
except Exception:
    gps = None

# ---------- HELPER FUNCTIONS ----------
def clamp(v, lo, hi):
    return max(lo, min(hi, v))

def read_ball_ir_values():
    """Return list of values from ball IR sensors (left-to-right).
       If no sensors found, return empty list."""
    vals = []
    for s in ball_sensors:
        try:
            vals.append(s.getValue())
        except Exception:
            vals.append(0.0)
    return vals

def ball_direction_from_ir(vals):
    """Estimate ball direction from IR array.
       Returns: angle_index (-n..+n) where 0 means center, negative left, positive right,
       and max_value (float) for confidence, plus boolean is_close if strong reading."""
    if not vals:
        return 0, 0.0, False
    max_val = max(vals)
    idx = vals.index(max_val)
    center = (len(vals)-1)/2.0
    direction_index = int(round(idx - center))
    is_close = max_val >= BALL_CLOSE_THRESHOLD
    return direction_index, max_val, is_close

def drive(left_speed, right_speed):
    left_motor.setVelocity(clamp(left_speed, -MAX_SPEED, MAX_SPEED))
    right_motor.setVelocity(clamp(right_speed, -MAX_SPEED, MAX_SPEED))

def avoid_obstacle():
    """Simple obstacle avoidance using sonar: back up and turn."""
    # back up a bit
    drive(-0.5*MAX_SPEED, -0.5*MAX_SPEED)
    # run a few steps to clear
    for _ in range(4):
        if robot.step(TIME_STEP) == -1:
            return
    # turn in place to the right
    drive(0.5*MAX_SPEED, -0.5*MAX_SPEED)
    for _ in range(6):
        if robot.step(TIME_STEP) == -1:
            return
    # stop briefly
    drive(0,0)

def get_heading():
    """Return robot heading in radians (0..2pi) relative to world (if compass available).
       If no compass, return None."""
    if not compass:
        return None
    vec = compass.getValues()
    # compass returns a 3D vector pointing to north; compute angle on X-Z plane
    # heading = atan2(vec[0], vec[2])  (Webots typical)
    import math
    heading = math.atan2(vec[0], vec[2])
    return heading

def face_goal_and_kick():
    """A simple behavior: orient toward opponent goal and push forward to 'kick'."""
    # For simplicity, assume we are playing from YELLOW or BLUE side and goal direction is known.
    # We will use the compass to choose a goal direction: if heading near 0 we aim one side, else opposite.
    # If compass not available, just drive forward quickly for a short burst.
    import math
    if compass:
        heading = get_heading()
        # Decide goal heading: this is a heuristic:
        # If heading is near 0 -> assume goal is at +pi (behind), else aim to 0.
        target = 0.0 if abs(heading - math.pi) < abs(heading - 0.0) else math.pi
        # rotate until roughly aligned
        for _ in range(12):
            heading = get_heading()
            if heading is None:
                break
            err = ( (target - heading + math.pi) % (2*math.pi) ) - math.pi
            if abs(err) < GOAL_HEADING_TOLERANCE:
                break
            # turn toward target
            turn = clamp(err, -0.5, 0.5)
            drive( (1.0 - turn)*0.6*MAX_SPEED, (1.0 + turn)*0.6*MAX_SPEED )
            if robot.step(TIME_STEP) == -1:
                return
        # final forward burst to push ball toward goal
        drive(0.95*MAX_SPEED, 0.95*MAX_SPEED)
        for _ in range(8):
            if robot.step(TIME_STEP) == -1:
                return
        drive(0,0)
    else:
        # no compass: just forward burst
        drive(0.95*MAX_SPEED, 0.95*MAX_SPEED)
        for _ in range(8):
            if robot.step(TIME_STEP) == -1:
                return
        drive(0,0)

# ---------- MAIN LOOP ----------
state = 'search'  # states: search, approach, align_and_kick, avoid
lost_ball_counter = 0
MAX_LOST_BEFORE_ROTATE = 15

while robot.step(TIME_STEP) != -1:
    # read sensors
    ir_vals = read_ball_ir_values()
    dir_idx, confidence, is_close = ball_direction_from_ir(ir_vals)
    sonar_dist = None
    try:
        if sonar:
            sonar_dist = sonar.getValue()
    except Exception:
        sonar_dist = None

    # Basic obstacle avoidance (if sonar sees something very close)
    if sonar_dist is not None and sonar_dist < SONAR_OBSTACLE_THRESHOLD:
        state = 'avoid'
    # State machine
    if state == 'avoid':
        avoid_obstacle()
        state = 'search'
        continue

    # if we have a reliable ball reading
    if confidence > 0.0:
        # mark ball seen recently
        lost_ball_counter = 0
        # approach behavior
        if is_close:
            # ball is close: try to align and kick toward goal
            state = 'align_and_kick'
        else:
            state = 'approach'
    else:
        # no reading
        lost_ball_counter += 1
        if lost_ball_counter > MAX_LOST_BEFORE_ROTATE:
            # spin in place to search for ball
            state = 'search'
        # keep previous behavior fallback

    if state == 'search':
        # rotate slowly left looking for ball
        drive(0.35*MAX_SPEED, -0.35*MAX_SPEED)
        continue

    if state == 'approach':
        # dir_idx: negative -> left, positive -> right, 0 center
        # translate index to steering
        # stronger turn when index magnitude larger
        if dir_idx < 0:
            # turn left while moving forward
            left_v = 0.5 * MAX_SPEED
            right_v = 0.9 * MAX_SPEED
        elif dir_idx > 0:
            # turn right while moving forward
            left_v = 0.9 * MAX_SPEED
            right_v = 0.5 * MAX_SPEED
        else:
            # go straight
            left_v = 0.95 * MAX_SPEED
            right_v = 0.95 * MAX_SPEED
        drive(left_v, right_v)
        continue

    if state == 'align_and_kick':
        # small alignment: if ball slightly left or right, adjust
        if dir_idx < 0:
            drive(0.5*MAX_SPEED, 0.95*MAX_SPEED)
        elif dir_idx > 0:
            drive(0.95*MAX_SPEED, 0.5*MAX_SPEED)
        else:
            # ball centered and close -> push toward goal
            face_goal_and_kick()
        # after attempt, go back to search/approach
        state = 'search'
        continue

# end of controller