hristudio/src/server/services/robot-communication.ts

/* eslint-disable @typescript-eslint/no-explicit-any */
/* eslint-disable @typescript-eslint/no-unsafe-assignment */
/* eslint-disable @typescript-eslint/no-unsafe-member-access */
/* eslint-disable @typescript-eslint/no-unsafe-call */
/* eslint-disable @typescript-eslint/no-unsafe-return */

import WebSocket from "ws";
import { EventEmitter } from "events";
import { exec } from "child_process";
import { promisify } from "util";

const execAsync = promisify(exec);

export interface RobotCommunicationConfig {
  rosBridgeUrl: string;
  connectionTimeout: number;
  reconnectInterval: number;
  maxReconnectAttempts: number;
}

export interface RobotAction {
  pluginName: string;
  actionId: string;
  parameters: Record<string, unknown>;
  implementation: {
    topic: string;
    messageType: string;
    messageTemplate: Record<string, unknown>;
    payloadMapping?: {
      type?: string;
      transformFn?: string;
      payload?: Record<string, unknown>;
      sshCommand?: string;
    };
    ros2?: {
      topic?: string;
      messageType?: string;
      service?: string;
      action?: string;
      payloadMapping?: {
        type?: string;
        transformFn?: string;
        payload?: Record<string, unknown>;
        sshCommand?: string;
      };
    };
  };
}

export interface RobotActionResult {
  success: boolean;
  duration: number;
  data?: Record<string, unknown>;
  error?: string;
}

/**
 * Server-side robot communication service for ROS integration
 *
 * This service manages WebSocket connections to rosbridge_server and provides
 * a high-level interface for executing robot actions during trial execution.
 */
export class RobotCommunicationService extends EventEmitter {
  private ws: WebSocket | null = null;
  private config: RobotCommunicationConfig;
  private messageId = 0;
  private pendingActions = new Map<
    string,
    {
      resolve: (result: RobotActionResult) => void;
      reject: (error: Error) => void;
      timeout: NodeJS.Timeout;
      startTime: number;
    }
  >();
  private reconnectAttempts = 0;
  private reconnectTimer: NodeJS.Timeout | null = null;
  private isConnected = false;

  constructor(config: Partial<RobotCommunicationConfig> = {}) {
    super();

    this.config = {
      rosBridgeUrl: process.env.ROS_BRIDGE_URL || "ws://localhost:9090",
      connectionTimeout: 10000,
      reconnectInterval: 5000,
      maxReconnectAttempts: 10,
      ...config,
    };
  }

  /**
   * Initialize connection to ROS bridge
   */
  async connect(): Promise<void> {
    if (this.isConnected) {
      return;
    }

    return new Promise((resolve, reject) => {
      console.log(
        `[RobotComm] Connecting to ROS bridge: ${this.config.rosBridgeUrl}`,
      );

      try {
        this.ws = new WebSocket(this.config.rosBridgeUrl);

        const connectionTimeout = setTimeout(() => {
          reject(new Error("Connection timeout"));
          this.cleanup();
        }, this.config.connectionTimeout);

        this.ws.on("open", () => {
          clearTimeout(connectionTimeout);
          this.isConnected = true;
          this.reconnectAttempts = 0;

          console.log("[RobotComm] Connected to ROS bridge");
          this.emit("connected");
          resolve();
        });

        this.ws.on("message", (data: WebSocket.Data) => {
          try {
            const message = JSON.parse(data.toString());
            this.handleMessage(message);
          } catch (error) {
            console.error("[RobotComm] Failed to parse message:", error);
          }
        });

        this.ws.on("close", (code: number, reason: string) => {
          this.isConnected = false;
          console.log(`[RobotComm] Connection closed: ${code} - ${reason}`);

          this.emit("disconnected");

          // Reject all pending actions
          this.rejectAllPendingActions(new Error("Connection lost"));

          // Schedule reconnection if not intentionally closed
          if (
            code !== 1000 &&
            this.reconnectAttempts < this.config.maxReconnectAttempts
          ) {
            this.scheduleReconnect();
          }
        });

        this.ws.on("error", (error: Error) => {
          console.error("[RobotComm] WebSocket error:", error);
          clearTimeout(connectionTimeout);
          this.emit("error", error);
          reject(error);
        });
      } catch (error) {
        reject(error);
      }
    });
  }

  /**
   * Disconnect from ROS bridge
   */
  disconnect(): void {
    if (this.reconnectTimer) {
      clearTimeout(this.reconnectTimer);
      this.reconnectTimer = null;
    }

    this.rejectAllPendingActions(new Error("Service disconnected"));

    if (this.ws) {
      this.ws.close(1000, "Normal closure");
      this.ws = null;
    }

    this.isConnected = false;
    this.emit("disconnected");
  }

  /**
   * Execute a robot action
   */
  async executeAction(action: RobotAction): Promise<RobotActionResult> {
    const actionId = `action_${this.messageId++}`;
    const startTime = Date.now();

    // Check if this is an SSH-only action (animations, posture, arbitrary SSH commands)
    const { implementation, actionId: actionType } = action;
    const baseActionId = actionType.includes(".")
      ? actionType.split(".").pop()
      : actionType;

    const isAnimationAction = baseActionId?.startsWith("play_animation_");
    const sshCommand = implementation.payloadMapping?.sshCommand
      || implementation.ros2?.payloadMapping?.sshCommand;

    // SSH actions don't require ROS connection
    if (isAnimationAction || sshCommand) {
      const timeout = setTimeout(() => {
        throw new Error(`SSH action timeout: ${action.actionId}`);
      }, 30000);

      try {
        console.log(`[RobotComm] Executing SSH action: ${action.actionId}`);
        const result = await this.executeRobotActionInternal(action, actionId, startTime);
        clearTimeout(timeout);
        return result;
      } catch (error) {
        clearTimeout(timeout);
        throw error;
      }
    }

    // Non-SSH actions require ROS connection
    if (!this.isConnected) {
      throw new Error("Not connected to ROS bridge");
    }

    // Store pending action
    const pending = {
      resolve: (() => {}) as (result: RobotActionResult) => void,
      reject: (() => {}) as (error: Error) => void,
      timeout: setTimeout(() => {
        this.pendingActions.delete(actionId);
        pending.reject(new Error(`Action timeout: ${action.actionId}`));
      }, 30000),
      startTime,
    };

    this.pendingActions.set(actionId, pending);

    // Wrap the pending resolve/reject in a way that works with async method
    return new Promise<RobotActionResult>((resolve, reject) => {
      pending.resolve = resolve;
      pending.reject = reject;

      // Execute action
      this.executeRobotActionInternal(action, actionId, startTime)
        .then((result) => {
          clearTimeout(pending.timeout);
          this.pendingActions.delete(actionId);
          resolve(result);
        })
        .catch((error) => {
          clearTimeout(pending.timeout);
          this.pendingActions.delete(actionId);
          reject(error);
        });
    });
  }

  /**
   * Check if service is connected
   */
  getConnectionStatus(): boolean {
    return this.isConnected;
  }

  // Private methods

  private async executeRobotActionInternal(
    action: RobotAction,
    actionId: string,
    startTime: number,
  ): Promise<RobotActionResult> {
    const { implementation, parameters, actionId: actionType } = action;

    // Use SSH for play_animation actions (check both namespaced and non-namespaced)
    const baseActionId = actionType.includes(".")
      ? actionType.split(".").pop()
      : actionType;
    
    if (baseActionId?.startsWith("play_animation_")) {
      await this.executeAnimationViaSSH(baseActionId);
      return {
        success: true,
        duration: Date.now() - startTime,
        data: { method: "ssh", action: baseActionId },
      };
    }

    // Check for SSH command type
    const sshCommand = implementation.payloadMapping?.sshCommand 
      || implementation.ros2?.payloadMapping?.sshCommand;
    
    if (sshCommand) {
      await this.executeSSHCommand(sshCommand);
      return {
        success: true,
        duration: Date.now() - startTime,
        data: { method: "ssh", command: sshCommand },
      };
    }

    // Apply transform if specified
    let message: Record<string, unknown>;
    const transformFn = implementation.payloadMapping?.transformFn 
      || implementation.ros2?.payloadMapping?.transformFn;
    
    if (transformFn) {
      message = this.applyTransform(transformFn, parameters);
    } else {
      // Build ROS message from template
      message = this.buildRosMessage(
        implementation.messageTemplate,
        parameters,
      );
    }

    // Publish to ROS topic
    this.publishToTopic(
      implementation.topic,
      implementation.messageType,
      message,
    );

    // For actions that complete immediately (like movement commands),
    // we simulate completion after a short delay
    return new Promise((resolve) => {
      setTimeout(() => {
        resolve({
          success: true,
          duration: Date.now() - startTime,
          data: {
            topic: implementation.topic,
            messageType: implementation.messageType,
            message,
          },
        });
      }, 100);
    });
  }

  private async executeSSHCommand(command: string): Promise<void> {
    const robotIp = process.env.NAO_IP || "134.82.159.168";
    const password = process.env.NAO_PASSWORD || "robolab";

    console.log(`[RobotComm] Executing SSH command: ${command}`);

    const sshCommand = `sshpass -p "${password}" ssh -o StrictHostKeyChecking=no -o ConnectTimeout=10 "nao@${robotIp}" "${command}"`;

    const { stdout, stderr } = await execAsync(sshCommand);

    if (stderr && !stderr.includes("null") && stderr.trim()) {
      console.warn(`[RobotComm] SSH stderr: ${stderr}`);
    }

    console.log(`[RobotComm] SSH result: ${stdout}`);
  }

  private async executeAnimationViaSSH(actionType: string): Promise<void> {
    const animationMap: Record<string, string> = {
      "play_animation_bow": "animations/Stand/Gestures/BowShort_1",
      "play_animation_hey": "animations/Stand/Gestures/Hey_1",
      "play_animation_show_floor": "animations/Stand/Gestures/ShowFloor_1",
      "play_animation_enthusiastic": "animations/Stand/Gestures/Enthusiastic_4",
      "play_animation_yes": "animations/Stand/Gestures/Yes_1",
      "play_animation_no": "animations/Stand/Gestures/No_3",
      "play_animation_idontknow": "animations/Stand/Gestures/IDontKnow_1",
    };

    const animation = animationMap[actionType];
    if (!animation) {
      throw new Error(`Unknown animation: ${actionType}`);
    }

    const robotIp = process.env.NAO_IP || "134.82.159.168";
    const password = process.env.NAO_PASSWORD || "robolab";

    console.log(`[RobotComm] Executing animation via SSH: ${animation}`);

    const command = `sshpass -p "${password}" ssh -o StrictHostKeyChecking=no -o ConnectTimeout=5 "nao@${robotIp}" "qicli call ALAnimationPlayer.run '${animation}'"`;

    const { stdout, stderr } = await execAsync(command);

    if (stderr && !stderr.includes("null")) {
      console.warn(`[RobotComm] SSH stderr: ${stderr}`);
    }

    console.log(`[RobotComm] Animation result: ${stdout}`);
  }

  private transformToEmotionalSpeech(parameters: Record<string, unknown>): { data: string } {
    const text = String(parameters.text || "Hello");
    const emotion = String(parameters.emotion || "neutral");

    switch (emotion) {
      case "happy":
        return { data: `\\rspd=120\\ ${text}` };
      case "sad":
        return { data: `\\rspd=80\\ ${text}` };
      case "neutral":
      default:
        return { data: text };
    }
  }

  private applyTransform(transformFn: string, parameters: Record<string, unknown>): Record<string, unknown> {
    switch (transformFn) {
      case "transformToEmotionalSpeech":
      case "transformToEmotionSpeech":
        return this.transformToEmotionalSpeech(parameters);
      default:
        console.warn(`[RobotComm] Unknown transform: ${transformFn}`);
        return parameters;
    }
  }

  private buildRosMessage(
    template: Record<string, unknown>,
    parameters: Record<string, unknown>,
  ): Record<string, unknown> {
    const message: Record<string, unknown> = {};

    for (const [key, value] of Object.entries(template)) {
      if (typeof value === "string" && value.includes("{{")) {
        // Template substitution
        let substituted = value;

        // Replace template variables
        for (const [paramKey, paramValue] of Object.entries(parameters)) {
          const placeholder = `{{${paramKey}}}`;
          if (substituted.includes(placeholder)) {
            substituted = substituted.replace(
              new RegExp(
                placeholder.replace(/[.*+?^${}()|[\]\\]/g, "\\$&"),
                "g",
              ),
              String(paramValue ?? ""),
            );
          }
        }

        // Handle conditional templates
        if (
          substituted.includes("{{") &&
          substituted.includes("?") &&
          substituted.includes(":")
        ) {
          // Simple conditional: {{condition ? valueTrue : valueFalse}}
          const match = substituted.match(
            /\{\{(.+?)\s*\?\s*(.+?)\s*:\s*(.+?)\}\}/,
          );
          if (match && match.length >= 4) {
            const condition = match[1];
            const trueValue = match[2];
            const falseValue = match[3];
            // Evaluate simple conditions
            let conditionResult = false;

            if (condition?.includes("===")) {
              const parts = condition
                .split("===")
                .map((s) => s.trim().replace(/['"]/g, ""));
              if (parts.length >= 2) {
                const left = parts[0];
                const right = parts[1];
                conditionResult = parameters[left || ""] === right;
              }
            }

            substituted = substituted.replace(
              match[0],
              conditionResult ? (trueValue ?? "") : (falseValue ?? ""),
            );
          }
        }

        // Try to parse as number if it looks like one
        if (!isNaN(Number(substituted))) {
          message[key] = Number(substituted);
        } else {
          message[key] = substituted;
        }
      } else if (Array.isArray(value)) {
        // Handle array templates
        message[key] = value.map((item) =>
          typeof item === "string" && item.includes("{{")
            ? this.substituteTemplateString(item, parameters)
            : item,
        );
      } else if (typeof value === "object" && value !== null) {
        // Recursively handle nested objects
        message[key] = this.buildRosMessage(
          value as Record<string, unknown>,
          parameters,
        );
      } else {
        message[key] = value;
      }
    }

    return message;
  }

  private substituteTemplateString(
    template: string,
    parameters: Record<string, unknown>,
  ): unknown {
    let result = template;

    for (const [key, value] of Object.entries(parameters)) {
      const placeholder = `{{${key}}}`;
      if (result.includes(placeholder)) {
        result = result.replace(
          new RegExp(placeholder.replace(/[.*+?^${}()|[\]\\]/g, "\\$&"), "g"),
          String(value ?? ""),
        );
      }
    }

    // Try to parse as number if it looks like one
    if (!isNaN(Number(result))) {
      return Number(result);
    }

    return result;
  }

  private publishToTopic(
    topic: string,
    messageType: string,
    message: Record<string, unknown>,
  ): void {
    if (!this.ws) return;

    const rosMessage = {
      op: "publish",
      topic,
      type: messageType,
      msg: message,
    };

    console.log(`[RobotComm] Publishing to ${topic}:`, message);
    this.ws.send(JSON.stringify(rosMessage));
  }

  private handleMessage(message: any): void {
    // Handle different types of ROS bridge messages
    switch (message.op) {
      case "publish":
        this.emit("topic_message", message.topic, message.msg);
        break;

      case "service_response":
        this.handleServiceResponse(message);
        break;

      case "status":
        console.log("[RobotComm] Status:", message);
        break;

      default:
        console.log("[RobotComm] Unhandled message:", message);
    }
  }

  private handleServiceResponse(message: any): void {
    // Handle service call responses if needed
    console.log("[RobotComm] Service response:", message);
  }

  private completeAction(actionId: string, result: RobotActionResult): void {
    const pending = this.pendingActions.get(actionId);
    if (pending) {
      clearTimeout(pending.timeout);
      this.pendingActions.delete(actionId);
      pending.resolve(result);
    }
  }

  private rejectAllPendingActions(error: Error): void {
    for (const [actionId, pending] of this.pendingActions.entries()) {
      clearTimeout(pending.timeout);
      pending.reject(error);
    }
    this.pendingActions.clear();
  }

  private scheduleReconnect(): void {
    if (this.reconnectTimer) return;

    this.reconnectAttempts++;
    console.log(
      `[RobotComm] Scheduling reconnect attempt ${this.reconnectAttempts}/${this.config.maxReconnectAttempts} in ${this.config.reconnectInterval}ms`,
    );

    this.reconnectTimer = setTimeout(async () => {
      this.reconnectTimer = null;

      try {
        await this.connect();
      } catch (error) {
        console.error("[RobotComm] Reconnect failed:", error);

        if (this.reconnectAttempts < this.config.maxReconnectAttempts) {
          this.scheduleReconnect();
        } else {
          console.error("[RobotComm] Max reconnect attempts reached");
          this.emit("max_reconnects_reached");
        }
      }
    }, this.config.reconnectInterval);
  }

  private cleanup(): void {
    if (this.ws) {
      this.ws.removeAllListeners();
      this.ws = null;
    }
    this.isConnected = false;
  }
}

// Global service instance
let robotCommService: RobotCommunicationService | null = null;

/**
 * Get or create the global robot communication service
 */
export function getRobotCommunicationService(): RobotCommunicationService {
  if (!robotCommService) {
    robotCommService = new RobotCommunicationService();
  }
  return robotCommService;
}

/**
 * Initialize robot communication service with connection
 */
export async function initRobotCommunicationService(): Promise<RobotCommunicationService> {
  const service = getRobotCommunicationService();

  if (!service.getConnectionStatus()) {
    await service.connect();
  }

  return service;
}