// simulator.ts - Worker版本
import mqtt, { IClientOptions, MqttClient } from "npm:mqtt";  
import { v4 as uuidv4 } from "npm:uuid";  
import { loadConfig, RawConfig } from "./simulator_config.ts";  

interface SimulatorConfig {  
  vdaInterface: string;  
  zoneSetId: string;  
  mapId: string;  
  vehicle: {  
    manufacturer: string;  
    serialNumber: string;  
    vdaVersion: string;  
  };  
  mqtt: {  
    brokerUrl: string;  
    options: IClientOptions;  
  };  
  settings: {  
    robotCount: number;  
    stateFrequency: number;  
    visualizationFrequency: number;  
    speed: number;  
  };  
}  

interface Header { 
  headerId: number; 
  timestamp: string; 
  version: string; 
  manufacturer: string; 
  serialNumber: string;
}  

interface BatteryState { 
  batteryCharge: number; 
  charging: boolean;
  batteryHealth?: number;
  batteryVoltage?: number;
  reach?: number;
}  

interface SafetyState {
  eStop: string;
  fieldViolation: boolean;
}  

interface AGVPosition {  
  x: number; 
  y: number; 
  theta: number;  
  mapId: string; 
  positionInitialized: boolean;  
  mapDescription?: string;  
  localizationScore?: number;  
  deviationRange?: number;  
}  

interface Velocity { 
  vx: number; 
  vy: number; 
  omega: number;
}  

type ActionStatus = "WAITING" | "INITIALIZING" | "RUNNING" | "PAUSED" | "FINISHED" | "FAILED";

interface ActionState {  
  actionId: string; 
  actionType?: string;  
  actionStatus: ActionStatus;  
  actionDescription?: string; 
  resultDescription?: string;
  actionParameters?: Array<{ key: string; value: string }>;
  blockingType?: "NONE" | "SOFT" | "HARD";
}  

interface NodeState { 
  nodeId: string; 
  sequenceId: string; 
  released: boolean; 
  nodeDescription?: string; 
  nodePosition: AGVPosition;
}  

interface EdgeState { 
  edgeId: string; 
  sequenceId: string; 
  released: boolean; 
  edgeDescription?: string; 
  trajectory?: Array<Record<string,unknown>>;
}  

interface Factsheet {
  headerId: number;
  timestamp: string;
  version: string;
  manufacturer: string;
  serialNumber: string;
  typeSpecification: {
    seriesName: string;
    seriesDescription: string;
    agvKinematic: string;
    agvClass: string;
    maxLoadMass: number;
    localizationTypes: string[];
    navigationTypes: string[];
  };
  physicalParameters: {
    speedMin: number;
    speedMax: number;
    accelerationMax: number;
    decelerationMax: number;
    heightMin: number;
    heightMax: number;
    width: number;
    length: number;
  };
  protocolLimits: any;
  protocolFeatures: any;
  agvGeometry: any;
  loadSpecification: any;
  localizationParameters: any;
}

interface State {  
  headerId: number;
  timestamp: string;
  version: string;
  manufacturer: string;
  serialNumber: string;
  orderId: string;
  orderUpdateId: number;
  zoneSetId: string;
  lastNodeId: string;
  lastNodeSequenceId: number;
  nodeStates: NodeState[];
  edgeStates: EdgeState[];
  actionStates: ActionState[];
  batteryState: BatteryState;
  operatingMode: string;
  errors: { errorType: string; errorDescription?: string; errorLevel: string }[];
  safetyState: SafetyState;
  driving: boolean;
  paused: boolean;
  newBaseRequest: boolean;
  waitingForInteractionZoneRelease: boolean;
  agvPosition: AGVPosition;
  velocity: Velocity;
  loads: any[];
  information?: any[];
  forkState?: { forkHeight?: number };
}  

interface Visualization { 
  header: Header; 
  agvPosition: AGVPosition; 
  velocity: Velocity; 
  driving: boolean;
}  

interface Connection {
  headerId: number;
  timestamp: string;
  version: string;
  manufacturer: string;
  serialNumber: string;
  connectionState: "ONLINE" | "OFFLINE" | "CONNECTIONBROKEN";
}  

interface ActionParamValue { 
  Float?: number; 
  Str?: string;
}  

interface Action { 
  actionId: string; 
  actionType?: string; 
  actionParameters: { key: string; value: ActionParamValue }[];
}  

interface InstantActions { 
  instantActions: Action[];
}  

interface Order { 
  orderId: string; 
  orderUpdateId: number; 
  nodes: any[]; 
  edges: any[];
}  

// Worker消息类型
interface WorkerMessage {
  type: "init" | "close" | "reconnect";
  data?: any;
}

interface MainMessage {
  type: "ready" | "error" | "status" | "log" | "device_request";
  data?: any;
}

class VehicleSimulator {  
  connectionTopic: string = ""; 
  stateTopic: string = ""; 
  visualizationTopic: string = "";
  factsheetTopic: string = "";  
  connection!: Connection; 
  state!: State; 
  visualization!: Visualization;
  factsheet!: Factsheet;  
  private lastUpdate = Date.now();  
  private speed: number = 0; 
  private boundary = 40.0;  
  private mqttClient?: MqttClient;
  private intervals: number[] = [];
  private isRunning = false;

  constructor(private cfg: SimulatorConfig) {  
    this.speed = cfg.settings.speed;  
    this.initializeSimulator();
  }

  private initializeSimulator() {
    const { manufacturer, serialNumber, vdaVersion } = this.cfg.vehicle;  
    const base = `${this.cfg.vdaInterface}/${vdaVersion}/${manufacturer}/${serialNumber}`;  
    this.connectionTopic = `${base}/connection`;  
    this.stateTopic = `${base}/state`;  
    this.visualizationTopic = `${base}/visualization`;
    this.factsheetTopic = `${base}/factsheet`;  

    const now = () => new Date().toISOString();  
    const header0: Header = { 
      headerId: 0, 
      timestamp: now(), 
      version: vdaVersion, 
      manufacturer, 
      serialNumber 
    };  

    this.connection = {
      headerId: 0,
      timestamp: now(),
      version: vdaVersion,
      manufacturer,
      serialNumber,
      connectionState: "CONNECTIONBROKEN"
    };  
    
    // 随机初始位置  
    const x0 = (Math.random() * 2 - 1) * 30;
    const y0 = (Math.random() * 2 - 1) * 30;
    const th0 = (Math.random() * 2 - 1) * Math.PI;  
    
    // VDA 5050 兼容的扁平化 State
    this.state = {
      headerId: 0,
      timestamp: now(),
      version: vdaVersion,
      manufacturer,
      serialNumber,
      orderId: "",
      orderUpdateId: 0,
      zoneSetId: this.cfg.zoneSetId,
      lastNodeId: "",
      lastNodeSequenceId: 0,
      nodeStates: [],
      edgeStates: [],
      actionStates: [],
      batteryState: { batteryCharge: 1.0, charging: false },
      operatingMode: "AUTOMATIC",
      errors: [],
      safetyState: { eStop: "NONE", fieldViolation: false },
      driving: true,
      paused: false,
      newBaseRequest: true,
      waitingForInteractionZoneRelease: false,
      agvPosition: { 
        x: x0, 
        y: y0, 
        theta: th0, 
        mapId: this.cfg.mapId, 
        positionInitialized: true 
      },
      velocity: { 
        vx: this.speed * Math.cos(th0), 
        vy: this.speed * Math.sin(th0), 
        omega: 0 
      },
      loads: [],
      information: [],
      forkState: { forkHeight: 0 }
    };
    
    this.visualization = {  
      header: { ...header0 },  
      agvPosition: { ...this.state.agvPosition },  
      velocity: { ...this.state.velocity },  
      driving: true  
    };

    this.factsheet = {
      headerId: 0,
      timestamp: now(),
      version: "v3.4.7.1005",
      manufacturer,
      serialNumber,
      typeSpecification: {
        seriesName: serialNumber,
        seriesDescription: serialNumber,
        agvKinematic: "DIFF",
        agvClass: "FORKLIFT",
        maxLoadMass: 0.5,
        localizationTypes: ["NATURAL"],
        navigationTypes: ["PHYSICAL_LINE_GUIDED"]
      },
      physicalParameters: {
        speedMin: 0.01,
        speedMax: 0.8,
        accelerationMax: 0.5,
        decelerationMax: 0.15,
        heightMin: 1.83,
        heightMax: 1.83,
        width: 0.885,
        length: 1.5145
      },
      protocolLimits: null,
      protocolFeatures: null,
      agvGeometry: null,
      loadSpecification: null,
      localizationParameters: null
    };
  }

  async start() {
    if (this.isRunning) {
      this.log("warn", "Simulator already running");
      return;
    }

    try {
      this.isRunning = true;
      await this.connectMqtt();
      await this.subscribeVda();
      await this.publishConnection();
      this.publishFactsheet(); // 启动时发布一次 factsheet
      this.startIntervals();
      
      this.postMessage({ type: "ready", data: { agvId: this.cfg.vehicle.serialNumber } });
      this.log("info", `🚛 AGV ${this.cfg.vehicle.serialNumber} started successfully`);
    } catch (error) {
      this.isRunning = false;
      this.postMessage({ type: "error", data: { error: (error as Error).message, agvId: this.cfg.vehicle.serialNumber } });
      this.log("error", `Failed to start AGV ${this.cfg.vehicle.serialNumber}: ${(error as Error).message}`);
    }
  }

  async stop() {
    if (!this.isRunning) return;

    this.isRunning = false;
    this.clearIntervals();
    
    if (this.mqttClient) {
      this.connection.connectionState = "OFFLINE";
      this.publishConnectionOnline();
      this.mqttClient.end();
      this.mqttClient = undefined;
    }
    
    this.log("info", `🛑 AGV ${this.cfg.vehicle.serialNumber} stopped`);
  }

  async reconnect() {
    this.log("info", `🔄 Reconnecting AGV ${this.cfg.vehicle.serialNumber}`);
    await this.stop();
    await new Promise(resolve => setTimeout(resolve, 1000)); // 等待1秒
    await this.start();
  }

  private async connectMqtt() {
    const clientOptions: IClientOptions = {  
      clientId: `deno-agv-sim-${this.cfg.vehicle.serialNumber}-${uuidv4()}`,  
      clean: true,  
      keepalive: 60,  
    };

    this.mqttClient = mqtt.connect(this.cfg.mqtt.brokerUrl, clientOptions);
    
    return new Promise<void>((resolve, reject) => {
      const timeout = setTimeout(() => {
        reject(new Error("MQTT connection timeout"));
      }, 10000);

      this.mqttClient!.on("connect", () => {
        clearTimeout(timeout);
        this.log("info", `✅ MQTT connected: ${this.cfg.vehicle.serialNumber}`);
        resolve();
      });

      this.mqttClient!.on("error", (error) => {
        clearTimeout(timeout);
        reject(error as Error);
      });
    });
  }

  private async subscribeVda() {
    if (!this.mqttClient) throw new Error("MQTT client not connected");

    const b = `${this.cfg.vdaInterface}/${this.cfg.vehicle.vdaVersion}/${this.cfg.vehicle.manufacturer}/${this.cfg.vehicle.serialNumber}`;  
    const topics = [`${b}/instantActions`, `${b}/order`];  
    
    await new Promise<void>((res, rej) =>  
      this.mqttClient!.subscribe(topics, { qos: 1 }, err => err ? rej(err) : res())  
    );  
    
    this.mqttClient.on("message", (topic, payload) => {  
      try {
        const msg = JSON.parse(payload.toString());  
        this.log("info", `📨 Received MQTT message on topic: ${topic}`);
        
        if (topic.endsWith("instantActions")) {
          this.instantActionsAccept(msg);
        } else if (topic.endsWith("order")) {
          this.orderAccept(msg);
        }
      } catch (error) {
        this.log("error", `Error processing MQTT message: ${(error as Error).message}`);
        this.log("error", `Topic: ${topic}, Payload: ${payload.toString()}`);
      }
    });
  }

  private startIntervals() {
    const stateInterval = setInterval(() => {  
      this.stateIterate();  
      this.publishState();  
    }, 2000);

    const visInterval = setInterval(() => {  
      this.publishVisualization();  
    }, Math.floor(1000 / this.cfg.settings.visualizationFrequency));

    const connectionInterval = setInterval(() => {
      this.publishConnectionOnline();
    }, 1000);

    // const factsheetInterval = setInterval(() => {
    //   this.publishFactsheet();
    // }, 30000); // 每30秒发布一次 factsheet

    this.intervals = [stateInterval, visInterval, connectionInterval];
  }

  private clearIntervals() {
    this.intervals.forEach(interval => clearInterval(interval));
    this.intervals = [];
  }

  stateIterate() {  
    const nowMs = Date.now();
    const dt = (nowMs - this.lastUpdate) / 1000;  
    this.lastUpdate = nowMs;  
    this.state.headerId++;  
    this.state.timestamp = new Date().toISOString();  

    if (this.state.driving) {  
      let { x, y, theta } = this.state.agvPosition;  
      const vx = this.speed * Math.cos(theta);
      const vy = this.speed * Math.sin(theta);  
      let nx = x + vx * dt;
      let ny = y + vy * dt;
      let bounced = false;  
      
      if (nx > this.boundary) { 
        nx = this.boundary - (nx - this.boundary); 
        bounced = true; 
      }  
      if (nx < -this.boundary) { 
        nx = -this.boundary - (-this.boundary - nx); 
        bounced = true; 
      }  
      if (ny > this.boundary) { 
        ny = this.boundary - (ny - this.boundary); 
        bounced = true; 
      }  
      if (ny < -this.boundary) { 
        ny = -this.boundary - (-this.boundary - ny); 
        bounced = true; 
      }  
      
      if (bounced) {  
        theta += Math.PI;  
        if (theta > Math.PI) theta -= 2 * Math.PI;  
        if (theta < -Math.PI) theta += 2 * Math.PI;  
      }  
      
      this.state.agvPosition = { 
        x: nx, 
        y: ny, 
        theta, 
        mapId: this.state.agvPosition.mapId, 
        positionInitialized: true 
      };  
      this.state.velocity = { 
        vx: this.speed * Math.cos(theta), 
        vy: this.speed * Math.sin(theta), 
        omega: 0 
      };  
    } else {  
      this.state.velocity = { vx: 0, vy: 0, omega: 0 };  
    }  

    // 同步 Visualization header  
    this.visualization.header.headerId = this.state.headerId + 1;  
    this.visualization.header.timestamp = this.state.timestamp;  
    this.visualization.agvPosition = { ...this.state.agvPosition };  
    this.visualization.velocity = { ...this.state.velocity };  
    this.visualization.driving = this.state.driving;  
  }  

  async publishConnection() {  
    if (!this.mqttClient) return;

    const pub = (msg: Connection) => new Promise<void>(res => {  
      this.mqttClient!.publish(this.connectionTopic, JSON.stringify(msg), { qos: 1 }, () => res());  
    });  
    
    // Broken
    this.connection.headerId++;
    this.connection.timestamp = new Date().toISOString();
    this.connection.connectionState = "CONNECTIONBROKEN";
    await pub(this.connection);
    
    // Online
    await new Promise(r => setTimeout(r, 500));
    this.connection.headerId++;
    this.connection.timestamp = new Date().toISOString();
    this.connection.connectionState = "ONLINE";
    await pub(this.connection);
  }

  publishConnectionOnline() {
    if (!this.mqttClient) return;
    
    this.connection.headerId++;
    this.connection.timestamp = new Date().toISOString();
    this.connection.connectionState = "ONLINE";
    this.mqttClient.publish(this.connectionTopic, JSON.stringify(this.connection), { qos: 1 });
  }

  publishState() {  
    if (!this.mqttClient) return;
    this.mqttClient.publish(this.stateTopic, JSON.stringify(this.state), { qos: 1 });  
  }  
  
  publishVisualization() {  
    if (!this.mqttClient) return;
    this.mqttClient.publish(this.visualizationTopic, JSON.stringify(this.visualization), { qos: 1 });  
  }

  publishFactsheet() {
    if (!this.mqttClient) return;
    this.factsheet.headerId++;
    this.factsheet.timestamp = new Date().toISOString();
    this.mqttClient.publish(this.factsheetTopic, JSON.stringify(this.factsheet), { qos: 1 });
  }  

  async instantActionsAccept(req: any) {
    this.log("info", `📥 Received instantActions: ${JSON.stringify(req, null, 2)}`);
    
    // 验证消息格式
    if (!req || typeof req !== 'object') {
      this.log("error", "Invalid instantActions request: not an object");
      return;
    }
    
    // 检查actions或instantActions字段（支持两种格式）
    let actionsArray: any[];
    if (req.instantActions && Array.isArray(req.instantActions)) {
      actionsArray = req.instantActions;
    } else if (req.actions && Array.isArray(req.actions)) {
      actionsArray = req.actions;
      this.log("info", "Using 'actions' field instead of 'instantActions'");
    } else {
      this.log("error", "Missing instantActions or actions field in request");
      return;
    }
    
    this.log("info", `📋 Processing ${actionsArray.length} instant actions`);
    
    for (const act of actionsArray) {  
      if (!act || !act.actionId) {
        this.log("error", "Invalid action: missing actionId");
        continue;
      }
      
      // 检查是否是设备相关的action
      if (act.actionType === "deviceSetup") {
        this.log("info", `🔧 Processing deviceSetup action: ${act.actionId}`);
        
        // 提取设备信息
        const deviceInfo = this.extractDeviceInfo(act.actionParameters || []);
        if (deviceInfo) {
          // 通知主进程创建设备模拟器
          this.postMessage({
            type: "device_request",
            data: {
              action: "create",
              deviceInfo,
              originalAction: act
            }
          });
          
          this.log("info", `📡 Requested device simulator creation for ${deviceInfo.ip}-${deviceInfo.port}-${deviceInfo.slaveId}`);
        } else {
          this.log("error", `❌ Failed to extract device info from action ${act.actionId}`);
        }
      } else if (act.actionType === "deviceStop") {
        this.log("info", `🛑 Processing deviceStop action: ${act.actionId}`);
        
        // 提取设备ID或信息
        const deviceInfo = this.extractDeviceInfo(act.actionParameters || []);
        if (deviceInfo) {
          const deviceId = `${deviceInfo.ip}-${deviceInfo.port}-${deviceInfo.slaveId}`;
          // 通知主进程停止设备模拟器
          this.postMessage({
            type: "device_request",
            data: {
              action: "stop",
              deviceId,
              originalAction: act
            }
          });
          
          this.log("info", `📡 Requested device simulator stop for ${deviceId}`);
        }
      } else if (act.actionType === "deviceDelete") {
        this.log("info", `🗑️ Processing deviceDelete action: ${act.actionId}`);
        
        // 提取设备ID或信息
        const deviceInfo = this.extractDeviceInfo(act.actionParameters || []);
        if (deviceInfo) {
          const deviceId = `${deviceInfo.ip}-${deviceInfo.port}-${deviceInfo.slaveId}`;
          // 通知主进程删除设备模拟器
          this.postMessage({
            type: "device_request",
            data: {
              action: "delete",
              deviceId,
              originalAction: act
            }
          });
          
          this.log("info", `📡 Requested device simulator deletion for ${deviceId}`);
        }
      } else if (act.actionType === "deviceWrite" || act.actionType === "deviceRead") {
        this.log("info", `🔧 Processing device action: ${act.actionType} - ${act.actionId}`);
        
        // 提取设备信息
        const deviceInfo = this.extractDeviceInfo(act.actionParameters || []);
        if (deviceInfo) {
          const deviceId = `${deviceInfo.ip}-${deviceInfo.port}-${deviceInfo.slaveId}`;
          // 通知主进程转发action到对应的设备模拟器
          this.postMessage({
            type: "device_request",
            data: {
              action: "forward",
              deviceId,
              originalAction: act
            }
          });
          
          this.log("info", `📡 Forwarded device action to ${deviceId}`);
        }
      }
      
      this.state.actionStates.push({ 
        actionId: act.actionId, 
        actionType: act.actionType || "unknown", 
        actionStatus: "WAITING" 
      });
      
      this.log("info", `✅ Added action: ${act.actionId} (${act.actionType || "unknown"})`);
    }  
  }

  // 从actionParameters中提取设备信息
  private extractDeviceInfo(actionParameters: any[]): any | null {
    const params: Record<string, string> = {};
    
    for (const param of actionParameters) {
      if (param.key && param.value) {
        // 处理不同的value格式
        let value: string;
        if (typeof param.value === 'string') {
          value = param.value;
        } else if (param.value?.Str) {
          value = param.value.Str;
        } else if (param.value?.Float) {
          value = param.value.Float.toString();
        } else {
          value = String(param.value);
        }
        params[param.key] = value;
      }
    }

    // 检查必需的字段
    const required = ['ip', 'port', 'slaveId'];
    for (const field of required) {
      if (!params[field]) {
        this.log("error", `❌ Missing required parameter: ${field}`);
        return null;
      }
    }

    return {
      ip: params.ip,
      port: params.port,
      slaveId: params.slaveId,
      deviceName: params.deviceName || 'Unknown Device',
      protocolType: params.protocolType || 'Unknown Protocol',
      brandName: params.brandName || 'Unknown Brand',
      registers: params.registers || '[]'
    };
  }
  
  async orderAccept(req: Order) {  
    this.log("info", `📥 Received order: ${JSON.stringify(req, null, 2)}`);
    this.state.orderId = req.orderId;  
    this.state.orderUpdateId = req.orderUpdateId;  
    this.state.nodeStates = [];  
    this.state.edgeStates = [];  
    this.state.actionStates = [];
    this.log("info", `✅ Accepted order: ${req.orderId} (updateId: ${req.orderUpdateId})`);
  }

  private log(level: "info" | "warn" | "error", message: string) {
    const timestamp = new Date().toISOString();
    const logMessage = `[${timestamp}] [${this.cfg.vehicle.serialNumber}] ${message}`;
    
    console.log(logMessage);
    this.postMessage({ 
      type: "log", 
      data: { level, message: logMessage, agvId: this.cfg.vehicle.serialNumber } 
    });
  }

  private postMessage(message: MainMessage) {
    self.postMessage(message);
  }
}

// Worker全局变量
let simulator: VehicleSimulator | null = null;

// Worker消息处理
self.onmessage = async (event: MessageEvent<WorkerMessage>) => {
  const { type, data } = event.data;

  try {
    switch (type) {
      case "init":
        if (simulator) {
          await simulator.stop();
        }
        simulator = new VehicleSimulator(data);
        await simulator.start();
        break;

      case "close":
        if (simulator) {
          await simulator.stop();
          simulator = null;
        }
        self.postMessage({ type: "status", data: { status: "closed" } });
        break;

      case "reconnect":
        if (simulator) {
          await simulator.reconnect();
        }
        break;

      default:
        console.warn(`Unknown message type: ${type}`);
    }
  } catch (error) {
    self.postMessage({ 
      type: "error", 
      data: { error: (error as Error).message, context: type } 
    });
  }
};