規則集 = [
    "如果(前方有障礙物) 那麼(向左轉)",
    "如果(電量低於20%) 那麼(返回充電)",
    "如果(檢測到目標) 那麼(向前移動)",
    "如果(無特殊情況) 那麼(隨機探索)"
]

行為層 = {
    # 最高優先級：安全相關
    "緊急制動": {"條件": "碰撞 imminent", "動作": "全力剎車"},
    
    # 高優先級：避障
    "主動避障": {"條件": "前方有障礙", "動作": "轉向避讓"},
    
    # 中優先級：導航
    "車道保持": {"條件": "偏離車道", "動作": "微調方向"},
    
    # 低優先級：舒適性
    "平穩駕駛": {"條件": "道路彎曲", "動作": "平滑轉向"}
}

def get_sensor_readings(self, obstacles):
    readings = []
    for sensor_angle in self.sensor_angles:
        sensor_dir = np.array([
            np.cos(self.angle + sensor_angle),  # 傳感器方向向量x分量
            np.sin(self.angle + sensor_angle)   # 傳感器方向向量y分量
        ])
        
        min_distance = self.sensor_range  # 初始化為最大探測距離
        
        for obstacle in obstacles:
            # 計算機器人到障礙物的向量
            obstacle_pos = np.array([obstacle[0], obstacle[1]])
            robot_pos = np.array([self.x, self.y])
            to_obstacle = obstacle_pos - robot_pos
            
            # 向量投影：判斷障礙物是否在傳感器前方
            projection = np.dot(to_obstacle, sensor_dir)
            
            if projection > 0:  # 障礙物在傳感器前方
                # 計算障礙物到傳感器射線的垂直距離
                distance_to_ray = np.linalg.norm(to_obstacle - projection * sensor_dir)
                
                if distance_to_ray < obstacle[2]:  # 如果距離小於障礙物半徑
                    # 計算實際的碰撞距離
                    hit_distance = projection - np.sqrt(obstacle[2]**2 - distance_to_ray**2)
                    if 0 < hit_distance < min_distance:
                        min_distance = hit_distance
            
            readings.append(min_distance)
    return readings

def reactive_control(self, sensor_readings):
    # 傳感器分組：左2個、前1個、右2個
    left_readings = sensor_readings[:2]    # 左側傳感器
    front_readings = sensor_readings[2]    # 前方傳感器  
    right_readings = sensor_readings[3:]   # 右側傳感器
    
    # 計算各方向的最小距離
    min_front = min(front_readings, self.sensor_range)
    min_left = min(left_readings)
    min_right = min(right_readings)
    
    # 行為1: 緊急避障 (最高優先級)
    if min_front < 0.8:  # 前方很近有障礙
        self.speed = 0.3  # 減速
        # 選擇更空曠的一側轉向
        if min_left > min_right:
            self.angle -= np.pi/4  # 向右轉45度
        else:
            self.angle += np.pi/4  # 向左轉45度
        self.current_behavior = "緊急避障"
        return self.current_behavior
    
    # 行為2: 預防性避障 (中優先級)
    elif min_front < 1.5:  # 前方較近有障礙
        self.speed = 0.4
        if min_left > min_right:
            self.angle -= np.pi/8  # 向右轉22.5度
        else:
            self.angle += np.pi/8  # 向左轉22.5度
        self.current_behavior = "預防避障"
        return self.current_behavior
    
    # 行為3: 沿牆跟隨 (低優先級)
    elif min_left < 1.2 or min_right < 1.2:  # 側面有牆
        self.speed = 0.6
        if min_left < min_right:  # 左側有牆
            self.angle -= 0.1  # 向右微調
        else:  # 右側有牆  
            self.angle += 0.1  # 向左微調
        self.current_behavior = "沿牆跟隨"
        return self.current_behavior
    
    # 行為4: 隨機探索 (默認行為)
    else:
        self.speed = 0.8  # 正常速度
        # 加入隨機擾動，避免陷入局部循環
        self.angle += random.uniform(-0.2, 0.2)
        self.current_behavior = "隨機探索"
        return self.current_behavior

def update_position(self, obstacles, area_bounds):
    # 1. 感知：獲取傳感器數據
    sensor_readings = self.get_sensor_readings(obstacles)
    
    # 2. 決策：反應式控制
    behavior = self.reactive_control(sensor_readings)
    
    # 3. 行動：更新位置
    dx = self.speed * np.cos(self.angle) * 0.1  # x方向位移
    dy = self.speed * np.sin(self.angle) * 0.1  # y方向位移
    
    new_x = self.x + dx
    new_y = self.y + dy
    
    # 邊界檢查：防止跑出環境
    if (area_bounds[0] < new_x < area_bounds[2] and 
        area_bounds[1] < new_y < area_bounds[3]):
        self.x = new_x
        self.y = new_y
    
    # 記錄軌跡（最多100個點）
    self.trajectory.append((self.x, self.y))
    if len(self.trajectory) > 100:
        self.trajectory.pop(0)  # 移除最舊的點
        
    return behavior, sensor_readings

def create_environment():
    # 障礙物格式：(x座標, y座標, 半徑)
    obstacles = [
        (3, 2, 0.8),  # 中心區域的大障礙物
        (6, 4, 1.0),  # 右側的大障礙物  
        (8, 2, 0.6),  # 右上角的小障礙物
        (4, 6, 0.7),  # 中部偏上的障礙物
        (7, 7, 0.9),  # 右上角的大障礙物
        (2, 8, 0.5),  # 左上角的小障礙物
        (9, 9, 0.6),  # 最右上角的障礙物
        (5, 3, 0.4)   # 中心區域的小障礙物
    ]
    
    # 環境邊界：(x_min, y_min, x_max, y_max)
    area_bounds = (0, 0, 10, 10)
    
    return obstacles, area_bounds

def animate(frame):
    ax.clear()
    
    # 更新機器人位置（第一幀除外）
    if frame > 0:
        behavior, sensor_readings = robot.update_position(obstacles, area_bounds)
    
    # 繪製傳感器（顏色編碼）
    for i, (sensor_angle, reading) in enumerate(zip(robot.sensor_angles, sensor_readings)):
        sensor_dir = np.array([np.cos(robot.angle + sensor_angle), 
                              np.sin(robot.angle + sensor_angle)])
        sensor_length = min(reading, robot.sensor_range)
        
        end_x = robot.x + sensor_length * sensor_dir[0]
        end_y = robot.y + sensor_length * sensor_dir[1]
        
        # 顏色編碼：綠色(安全) → 橙色(警告) → 紅色(危險)
        if reading > 1.5:
            color = 'green'   # 安全距離
        elif reading > 0.8:
            color = 'orange'  # 警告距離  
        else:
            color = 'red'     # 危險距離
            
        ax.plot([robot.x, end_x], [robot.y, end_y], color=color, 
               linewidth=2, alpha=0.7)

# 機器人沒有記憶環境地圖，只依賴當前傳感器數據
def reactive_control(self, sensor_readings):  # 只使用當前傳感器讀數
    # 不訪問歷史數據，不維護環境模型
    # 決策完全基於當前的sensor_readings

# 明確的優先級順序
if min_front < 0.8:        # 1. 緊急避障 (最高)
elif min_front < 1.5:      # 2. 預防避障  
elif min_left < 1.2 or ... # 3. 沿牆跟隨
else:                      # 4. 隨機探索 (最低)

# 每幀都重新感知和決策
def update_position(self, obstacles, area_bounds):
    sensor_readings = self.get_sensor_readings(obstacles)  # 實時感知
    behavior = self.reactive_control(sensor_readings)      # 實時決策
    # 立即執行動作

# 簡單規則組合產生複雜行為
# 單個規則很簡單，但組合後能：
# - 自主避開所有障礙物
# - 沿着牆壁行走  
# - 探索未知區域
# - 不會陷入死循環