# Robot 1 - Smart Soccer Version (With Goal Targeting)
# Author: Anas Barro
# Description: Improved Robot 1 with goal aiming logic for the final competition
# ---------------------------------------------------------------------

from controller import Robot, Compass, GPS

# الزمن الزمني لكل خطوة
TIME_STEP = 32

# إنشاء الروبوت
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)

# الحساسات
ball_sensors = []
for i in range(7):
    sensor = robot.getDevice("ir" + str(i))
    sensor.enable(TIME_STEP)
    ball_sensors.append(sensor)

sonar_left = robot.getDevice("so0")
sonar_right = robot.getDevice("so1")
sonar_left.enable(TIME_STEP)
sonar_right.enable(TIME_STEP)

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

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

# السرعات
MAX_SPEED = 6.0

# إعداد موقع المرمى (حسب لون الفريق)
# يمكنك تعديل الإحداثيات إذا كان المرمى في الجهة الأخرى
GOAL_X = 0.0
GOAL_Z = -0.9  # مرمى الخصم الأمامي (افتراضي)

# ---------------------------------------------
# دوال مساعدة
# ---------------------------------------------

def get_ball_position():
    values = [s.getValue() for s in ball_sensors]
    max_val = max(values)
    if max_val < 50:
        return "none"
    max_index = values.index(max_val)
    if max_index < 3:
        return "left"
    elif max_index > 3:
        return "right"
    else:
        return "center"

def get_robot_angle():
    compass_values = compass.getValues()
    angle = (compass_values[0] + 1) * 180  # تبسيط للزاوية
    return angle

def get_angle_to_goal():
    position = gps.getValues()
    robot_x = position[0]
    robot_z = position[2]
    dx = GOAL_X - robot_x
    dz = GOAL_Z - robot_z
    target_angle = (180 / 3.14159) * -1 * (dz / abs(dz)) if dz != 0 else 0
    return target_angle

# ---------------------------------------------
# الحلقة الرئيسية
# ---------------------------------------------
while robot.step(TIME_STEP) != -1:
    ball_pos = get_ball_position()
    left_sonar = sonar_left.getValue()
    right_sonar = sonar_right.getValue()

    # 1. تجنب الاصطدام
    if left_sonar < 800 or right_sonar < 800:
        left_motor.setVelocity(-2.0)
        right_motor.setVelocity(-2.0)
        continue

    # 2. تتبع الكرة
    if ball_pos == "center":
        left_motor.setVelocity(MAX_SPEED)
        right_motor.setVelocity(MAX_SPEED)
    elif ball_pos == "left":
        left_motor.setVelocity(0.5 * MAX_SPEED)
        right_motor.setVelocity(MAX_SPEED)
    elif ball_pos == "right":
        left_motor.setVelocity(MAX_SPEED)
        right_motor.setVelocity(0.5 * MAX_SPEED)
    else:
        # إذا لم تُرَ الكرة: لف للبحث عنها
        left_motor.setVelocity(3.0)
        right_motor.setVelocity(-3.0)
        continue

    # 3. عندما تكون الكرة قريبة، صوب نحو المرمى
    position = gps.getValues()
    robot_z = position[2]

    if abs(robot_z - GOAL_Z) < 0.3:  # قريب من المرمى
        robot_angle = get_robot_angle()
        goal_angle = get_angle_to_goal()

        if abs(robot_angle - goal_angle) > 10:
            # اضبط الاتجاه نحو المرمى
            if robot_angle > goal_angle:
                left_motor.setVelocity(-2.0)
                right_motor.setVelocity(2.0)
            else:
                left_motor.setVelocity(2.0)
                right_motor.setVelocity(-2.0)
        else:
            # اتجه مباشرة وسدد الكرة
            left_motor.setVelocity(MAX_SPEED)
            right_motor.setVelocity(MAX_SPEED)