# robot1.py
# كود روبوت 1 — متابعة الكرة، تجنّب العوائق، ومحاولة تسجيل الهدف
# ملاحظات: استخدم أسماء الأجهزة الموجودة في عالم Webots لديك
# تعليقات قصيرة باللغة العربية لتسهيل القراءة

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

TIME_STEP = 64

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'))
MAX_SPEED = 6.28  # غيّر إذا كانت السرعة مختلفة في محاكاتك

# ----- أجهزة الاستشعار -----
# مصفوفة مستشعرات IR/ball detector قد تكون موجودة باسم واحد أو مجموعة
# إذا كان هناك مستشعر IR منفرد ضع اسمه هنا
try:
    ball_ir = robot.getDevice('ball_ir_sensor')
    ball_ir.enable(TIME_STEP)
except:
    ball_ir = None

# أجهزة مسافة (سونار/ألتراسونك) - افترض وجود 3 حسّاسات أمامية (يسار، وسط، يمين)
sonar_names = ['ds_left', 'ds_center', 'ds_right']
sonars = []
for name in sonar_names:
    try:
        ds = robot.getDevice(name)
        ds.enable(TIME_STEP)
        sonars.append(ds)
    except:
        sonars.append(None)

# GPS لاستخدامه في تحديد الموقع على الملعب
gps = None
try:
    gps = robot.getDevice('gps')
    gps.enable(TIME_STEP)
except:
    gps = None

# بوصلة لتحديد اتجاه الجسم (نحو المرمى)
compass = None
try:
    compass = robot.getDevice('compass')
    compass.enable(TIME_STEP)
except:
    compass = None

# ----- دوال تحرك أساسية -----
def set_speed(left, right):
    # قص السرعات ضمن النطاق المسموح
    if left > MAX_SPEED: left = MAX_SPEED
    if right > MAX_SPEED: right = MAX_SPEED
    if left < -MAX_SPEED: left = -MAX_SPEED
    if right < -MAX_SPEED: right = -MAX_SPEED
    left_motor.setVelocity(left)
    right_motor.setVelocity(right)

def stop():
    set_speed(0.0, 0.0)

def forward(speed_fraction=0.6):
    s = MAX_SPEED * speed_fraction
    set_speed(s, s)

def backward(speed_fraction=0.4):
    s = MAX_SPEED * speed_fraction
    set_speed(-s, -s)

def turn_left(speed_fraction=0.4):
    s = MAX_SPEED * speed_fraction
    set_speed(-s, s)

def turn_right(speed_fraction=0.4):
    s = MAX_SPEED * speed_fraction
    set_speed(s, -s)

# ----- مساعدة لقراءة السونار -----
def read_sonar(index):
    if index < 0 or index >= len(sonars): return float('inf')
    sensor = sonars[index]
    if sensor is None: return float('inf')
    val = sensor.getValue()
    # بعض بيئات Webots تعطي قيمة صغيرة = قريب، لذلك نعيد القيمة مباشرة
    # نستخدم مقياس من النوع: قيمة أقل تعني شيء أقرب
    return val

def obstacle_ahead(threshold=800.0):
    # إذا كان أي من السونارات الأمامية يعطي قيمة أقل من العتبة اعتبارها عائق
    left = read_sonar(0)
    center = read_sonar(1)
    right = read_sonar(2)
    return (left < threshold) or (center < threshold) or (right < threshold), left, center, right

# ----- قراءة مستشعر الكرة (IR) -----
def read_ball_ir():
    if not ball_ir:
        return 0.0
    try:
        return ball_ir.getValue()
    except:
        return 0.0

# ----- بوصلة / اتجاه -----
def get_heading():
    # رجّع زاوية التوجه بالدرجات بالنسبة لمحور الملعب (تقريبًا)
    if not compass:
        return None
    vec = compass.getValues()
    # نحسب زاوية من متجه البوصلة (x, z)
    import math
    heading = math.atan2(vec[0], vec[2])  # قد يحتاج التبديل حسب إعداد البوصلة في العالم
    # نحول للراديان إلى درجات
    deg = heading * 180.0 / math.pi
    return deg

# ----- حالة اللعبة البسيطة -----
STATE_SEARCH = 0
STATE_APPROACH = 1
STATE_ALIGN_GOAL = 2
STATE_KICK = 3

state = STATE_SEARCH
lost_count = 0

# افترض أن المرمى في اتجاه ثابت بالنسبة لفريقك.
# إذا أردت: اضبط goal_direction على زاوية المرمى الحقيقية في العالم.
# هنا نستخدم بوصلة لتقريب اتجاه المرمى: نفترض أن المرمى أمام الروبوت في اتجاه 0 درجة.
GOAL_HEADING = 0.0  # قابل للتعديل إذا كان المرمى في زاوية مختلفة

# PID صغير لتوجيه التدوير نحو هدف زاوي
class SimplePID:
    def __init__(self, kp, ki, kd, windup=1.0):
        self.kp, self.ki, self.kd = kp, ki, kd
        self.prev = 0.0
        self.integral = 0.0
        self.windup = windup
    def step(self, error, dt):
        self.integral += error * dt
        # anti-windup
        if self.integral > self.windup: self.integral = self.windup
        if self.integral < -self.windup: self.integral = -self.windup
        derivative = 0.0
        if dt > 0:
            derivative = (error - self.prev) / dt
        out = self.kp*error + self.ki*self.integral + self.kd*derivative
        self.prev = error
        return out

pid = SimplePID(0.03, 0.0, 0.005)

# ----- حلقة رئيسية -----
while robot.step(TIME_STEP) != -1:
    # قراءة السنسورات
    ir_val = read_ball_ir()  # كلما كانت القيمة أكبر => أقرب أو مصادفة الكرة
    has_obstacle, left_s, center_s, right_s = obstacle_ahead()
    heading = get_heading()
    
    # حالة بسيطة لتحديد وجود الكرة
    # قد تحتاج ضبط العتبة حسب جهاز IR في محاكاتك
    BALL_DETECTED = ir_val > 100.0  # عتبة تجريبية — غيّر حسب الحاجة

    # حالة الانتقال بين أوضاع اللعب
    if state == STATE_SEARCH:
        # ندور ببطء للبحث عن الكرة
        if BALL_DETECTED:
            state = STATE_APPROACH
            lost_count = 0
        else:
            # تجنب الاصطدام أثناء البحث
            if has_obstacle:
                # إذا العائق على اليسار أو اليمين نزح بمحاذاة
                if left_s < right_s: