# 简化版：使用滑动窗口做时间对齐
import numpy as np
from scipy.interpolate import interp1d

def align_sensor_data(camera_history, radar_history):
    # camera_history: [(t, x, y, vx, vy), ...]
    # radar_history: [(t, range, angle, doppler), ...]
    camera_times = np.array([c[0] for c in camera_history])
    radar_times = np.array([r[0] for r in radar_history])
    # 取并集或按最小时间步插值
    common_t = np.union1d(camera_times, radar_times)
    # 补充中间值
    interp_cam = interp1d(camera_times, [c[1:] for c in camera_history], axis=0, kind='linear')
    interp_rad = interp1d(radar_times, [r[1:] for r in radar_history], axis=0, kind='linear')
    return common_t, interp_cam(common_t), interp_rad(common_t)

class TrafficAgent:
    def __init__(self):
        self.memory = MemoryManager()  # 短期+长期
        self.perception = PerceptionFusion()
        self.planner = HierarchicalPlanner()
        self.toolkit = ToolExecutor()

    def step(self):
        # 1. 感知
        events = self.perception.collect_and_align()
        # 2. 推理：生成当前态势标签
        situation = self.planner.infer_intent(events, self.memory.recall_recent())
        
        # 3. 如果风险超阈值，启动规划
        if situation.risk_score > 0.7:
            plan = self.planner.generate_plan(situation, self.memory.longterm_patterns())
            # 4. 执行计划中的每个动作
            for action in plan:
                status = self.toolkit.execute(action, deadline=action.deadline)
                if status != 'success':
                    # 失败重试：尝试降级工具或重新规划
                    fallback = self.planner.get_fallback(action, self.memory)
                    self.toolkit.execute(fallback, deadline=action.deadline)
                # 更新记忆
                self.memory.store(action, status, situation)
        # 5. 返回当前状态给调度器
        return situation

async def execute_with_deadline(tool_func, deadline_ms):
    try:
        result = await asyncio.wait_for(tool_func(), timeout=deadline_ms/1000.0)
        return result
    except asyncio.TimeoutError:
        return None  # 触发 fallback

import asyncio
import random
from dataclasses import dataclass
from typing import List

@dataclass
class Event:
    timestamp: float
    event_type: str   # 'vehicle_speed', 'brake_light', 'v2x_warning'
    value: float

class TrafficAgent:
    def __init__(self):
        self.risk_threshold = 0.7
        self.memory = []  # 短期记忆
        self.longterm_pattern = {"highway_construction_zone": 0.3}  # 基础风险加成

    async def perceive(self):
        # 模拟传感器返回事件流
        await asyncio.sleep(0.05)  # 50ms 周期
        return [
            Event(1672000.0 + random.uniform(0,0.5), 'vehicle_speed', random.uniform(20, 110)),
            Event(1672000.1, 'brake_light', 1.0),
            Event(1672000.2, 'v2x_warning', 0.9)
        ]

    def infer_intent(self, events):
        # 简化推理：如果有刹车灯 + 前车速度<50 km/h 且 后车速度>80 km/h 则高风险
        speeds = [e.value for e in events if e.event_type == 'vehicle_speed']
        brake = any(e.value == 1.0 for e in events if e.event_type == 'brake_light')
        if brake and len(speeds) >= 2 and speeds[-1] < 50 and speeds[0] > 80:
            risk_score = 0.9
        else:
            risk_score = random.uniform(0,0.6)
        return risk_score

    def generate_plan(self, risk_score):
        if risk_score > 0.7:
            return [
                (0.0, 'warn_driver', {'method': 'v2x_haptic'}),
                (0.3, 'emergency_brake', {'deceleration': -8.0}),
                (0.5, 'alert_tcc', {'severity': 'high'})
            ]
        return []

    async def execute_tool(self, action):
        # 模拟工具执行
        await asyncio.sleep(0.1)
        print(f"Executed {action[1]} with params {action[2]}")
        return 'success'

    async def run_cycle(self):
        while True:
            events = await self.perceive()
            risk = self.infer_intent(events)
            self.memory.append({'risk': risk, 'time': events[0].timestamp if events else 0})
            plan = self.generate_plan(risk)
            for act in plan:
                await self.execute_tool(act)
            # 限制内存长度
            if len(self.memory) > 100:
                self.memory = self.memory[-50:]

if __name__ == '__main__':
    agent = TrafficAgent()
    asyncio.run(agent.run_cycle())