import { createRequire } from "node:module";
import type { RosTransport } from "../transport.js";
import type {
  ConnectionStatus,
  ConnectionHandler,
  Subscription,
  PublishOptions,
  SubscribeOptions,
  ServiceCallOptions,
  ServiceCallResult,
  ActionGoalOptions,
  ActionResult,
  TopicInfo,
  ServiceInfo,
  ActionInfo,
  MessageHandler,
} from "../types.js";
import { EntityCache } from "./entities.js";
import { toRosMessage, fromRosMessage, loadMessageClass, clearTypeCache } from "./conversion.js";

const require = createRequire(import.meta.url);

export interface LocalTransportOptions {
  domainId?: number;
}

/** Internal ROS2 topics/services to filter from introspection results. */
const INTERNAL_TOPIC_PREFIXES = ["/rosout", "/parameter_events", "/agenticros/"];
const INTERNAL_SERVICE_SUFFIXES = [
  "/describe_parameters",
  "/get_parameter_types",
  "/get_parameters",
  "/list_parameters",
  "/set_parameters",
  "/set_parameters_atomically",
];

/**
 * Mode A transport: direct local DDS communication via rclnodejs.
 *
 * When OpenClaw runs on the robot itself, this transport talks to ROS2
 * directly via the local DDS bus — no network intermediary needed.
 */
export class LocalTransport implements RosTransport {
  private domainId: number;
  private status: ConnectionStatus = "disconnected";
  private connectionHandlers = new Set<ConnectionHandler>();
  /** rclnodejs module — loaded dynamically at runtime (optional dep, no types). */
  private rclnodejs: any = null;
  private node: any = null;
  private entityCache: EntityCache | null = null;
  private activeGoals = new Map<string, any>();

  /** Singleton guard — rclnodejs.init() must only be called once per process. */
  private static rclInitialized = false;

  constructor(options?: LocalTransportOptions) {
    this.domainId = options?.domainId ?? 0;
  }

  // --- Connection lifecycle ---

  async connect(): Promise<void> {
    if (this.status === "connected") return;
    this.setStatus("connecting");

    try {
      this.rclnodejs = require("rclnodejs");

      // Set domain ID before init if non-default
      if (this.domainId !== 0) {
        process.env.ROS_DOMAIN_ID = String(this.domainId);
      }

      // Global init — only once per process
      if (!LocalTransport.rclInitialized) {
        await this.rclnodejs!.init();
        LocalTransport.rclInitialized = true;
      }

      // Use a process-unique node name. If two AgenticROS adapters (e.g. the
      // OpenClaw gateway plugin + the Claude Code MCP server) share the host,
      // a fixed name collides on the DDS bus: rcl logs "Publisher already
      // registered for provided node name" and DDS discovery on the duplicate
      // becomes slow/lossy — listTopics() then returns 0 right after connect.
      const nodeName = `agenticros_local_${process.pid}`;
      this.node = this.rclnodejs!.createNode(nodeName);
      this.rclnodejs!.spin(this.node);
      this.entityCache = new EntityCache();

      // Give DDS discovery a brief moment to receive announcements from peers
      // before we report "connected". Without this the very first listTopics()
      // can race the executor and return empty even though peers exist.
      // 1500ms was chosen empirically on Jetson + Humble where /arm/*/cmd_pos
      // topics from a sim_arm gz_bridge child take ~1.2s to propagate via
      // multicast (vs ~400ms on x86); the prior 750ms cap caused first
      // list_topics() calls to miss them.
      await new Promise<void>((resolve) => setTimeout(resolve, 1500));

      this.setStatus("connected");
    } catch (err) {
      this.setStatus("disconnected");
      throw err;
    }
  }

  async disconnect(): Promise<void> {
    if (this.status === "disconnected") return;

    // Cancel any active action goals
    for (const [action] of this.activeGoals) {
      try {
        await this.cancelActionGoal(action);
      } catch {
        // Best-effort
      }
    }
    this.activeGoals.clear();

    if (this.entityCache && this.node) {
      this.entityCache.destroyAll(this.node);
      this.entityCache = null;
    }

    if (this.node) {
      this.node.destroy();
      this.node = null;
    }

    if (this.rclnodejs && LocalTransport.rclInitialized) {
      try {
        this.rclnodejs.shutdown();
      } catch {
        // May already be shut down
      }
      LocalTransport.rclInitialized = false;
    }

    clearTypeCache();
    this.rclnodejs = null;
    this.setStatus("disconnected");
  }

  getStatus(): ConnectionStatus {
    return this.status;
  }

  onConnection(handler: ConnectionHandler): () => void {
    this.connectionHandlers.add(handler);
    return () => {
      this.connectionHandlers.delete(handler);
    };
  }

  // --- Topics ---

  publish(options: PublishOptions): void {
    this.ensureConnected();
    const publisher = this.entityCache!.getPublisher(this.node, options.topic, options.type);
    const rosMsg = toRosMessage(options.type, options.msg);
    publisher.publish(rosMsg);
  }

  subscribe(options: SubscribeOptions, handler: MessageHandler): Subscription {
    this.ensureConnected();
    const type = options.type ?? this.resolveTopicType(options.topic);
    if (!type) {
      throw new Error(
        `Cannot subscribe to ${options.topic}: type is required when it cannot be inferred`,
      );
    }
    return this.entityCache!.addSubscription(this.node, options.topic, type, handler);
  }

  // --- Services ---

  async callService(options: ServiceCallOptions): Promise<ServiceCallResult> {
    this.ensureConnected();

    const type = options.type ?? this.resolveServiceType(options.service);
    if (!type) {
      throw new Error(
        `Cannot call service ${options.service}: type is required when it cannot be inferred`,
      );
    }

    const client = this.entityCache!.getServiceClient(this.node, options.service, type);

    // Wait for service to become available (5s timeout)
    const available = await client.waitForService(5000);
    if (!available) {
      throw new Error(`Service ${options.service} not available after 5 seconds`);
    }

    // Create request from args
    const ServiceClass = loadMessageClass(type);
    const request = new ServiceClass.Request();
    if (options.args) {
      for (const [key, value] of Object.entries(options.args)) {
        request[key] = value;
      }
    }

    // Send request with Promise wrapping + timeout
    const response = await this.sendServiceRequest(client, request, 30_000);

    return {
      result: true,
      values: fromRosMessage(response),
    };
  }

  // --- Actions ---

  async sendActionGoal(options: ActionGoalOptions): Promise<ActionResult> {
    this.ensureConnected();

    const ActionClass = loadMessageClass(options.actionType);
    const actionClient = new (this.rclnodejs!.ActionClient as any)(
      this.node,
      ActionClass,
      options.action,
    );

    // Wait for action server (5s)
    const available = await actionClient.waitForServer(5000);
    if (!available) {
      actionClient.destroy();
      throw new Error(`Action server ${options.action} not available after 5 seconds`);
    }

    // Build goal message
    const goal = new ActionClass.Goal();
    if (options.args) {
      for (const [key, value] of Object.entries(options.args)) {
        goal[key] = value;
      }
    }

    try {
      const result = await new Promise<Record<string, unknown>>((resolve, reject) => {
        const timer = setTimeout(() => {
          this.activeGoals.delete(options.action);
          reject(new Error(`Action ${options.action} timed out after 120 seconds`));
        }, 120_000);

        actionClient.sendGoal(
          goal,
          (goalHandle: any) => {
            // Goal response callback — store for cancellation
            this.activeGoals.set(options.action, goalHandle);
          },
          (feedback: any) => {
            // Feedback callback
            if (options.onFeedback) {
              options.onFeedback(fromRosMessage(feedback));
            }
          },
          (resultResponse: any) => {
            // Result callback
            clearTimeout(timer);
            this.activeGoals.delete(options.action);
            resolve(fromRosMessage(resultResponse));
          },
        );
      });

      return { result: true, values: result };
    } finally {
      actionClient.destroy();
    }
  }

  async cancelActionGoal(action: string): Promise<void> {
    const goalHandle = this.activeGoals.get(action);
    if (goalHandle && typeof goalHandle.cancelGoal === "function") {
      await goalHandle.cancelGoal();
      this.activeGoals.delete(action);
    }
  }

  // --- Introspection ---

  async listTopics(): Promise<TopicInfo[]> {
    this.ensureConnected();

    // DDS discovery is asynchronous AND stateful. On a freshly-connected
    // node, getTopicNamesAndTypes() trickles in results as peers respond -
    // the first call may return 0, the second 2, the third all of them.
    //
    // Two issues motivated the current tuning:
    //
    //   1. A short single-stable-hit policy (600ms) was returning truncated
    //      views to Claude: e.g. with the AMR sim alone it'd see /cmd_vel
    //      etc. on first call, then a second call 2s later would see all
    //      29 topics including /arm/*/cmd_pos. Claude only calls
    //      list_topics once and was missing arm joint topics entirely.
    //
    //   2. DDS multicast announcements on Jetson take longer than on x86 -
    //      we routinely see /arm/* topics appear ~1.5s after first call.
    //
    // Fix: require 3 consecutive matching polls (2 stable hits at 300ms
    // intervals = ~900ms of stability), with a 6 s deadline. Worst case
    // ~900ms latency on a steady graph, ~5s+ if discovery is still
    // streaming. The min-count guard (>=3) avoids exiting early when only
    // /clock + /tf + /tf_static have come through.
    const externalFilter = (t: { name: string; types: string[] }) =>
      !INTERNAL_TOPIC_PREFIXES.some((prefix) => t.name.startsWith(prefix));

    const deadline = Date.now() + 6000;
    const MIN_STABLE_COUNT = 3;
    const REQUIRED_STABLE_HITS = 2;
    let raw: Array<{ name: string; types: string[] }> = [];
    let prevCount = -1;
    let stableHits = 0;
    while (Date.now() < deadline) {
      raw = this.node.getTopicNamesAndTypes();
      const externalCount = raw.filter(externalFilter).length;
      if (externalCount === prevCount && externalCount >= MIN_STABLE_COUNT) {
        stableHits++;
        if (stableHits >= REQUIRED_STABLE_HITS) break;
      } else {
        stableHits = 0;
        prevCount = externalCount;
      }
      await new Promise<void>((resolve) => setTimeout(resolve, 300));
    }

    return raw.filter(externalFilter).map((t) => ({ name: t.name, type: t.types[0] ?? "" }));
  }

  async listServices(): Promise<ServiceInfo[]> {
    this.ensureConnected();

    const namesAndTypes: Array<{ name: string; types: string[] }> =
      this.node.getServiceNamesAndTypes();

    return namesAndTypes
      .filter((s) => !INTERNAL_SERVICE_SUFFIXES.some((suffix) => s.name.endsWith(suffix)))
      .filter((s) => !INTERNAL_TOPIC_PREFIXES.some((prefix) => s.name.startsWith(prefix)))
      .map((s) => ({ name: s.name, type: s.types[0] ?? "" }));
  }

  async listActions(): Promise<ActionInfo[]> {
    // Same feedback-topic heuristic as rosbridge/adapter.ts
    const topics = await this.listTopics();
    const actions: ActionInfo[] = [];
    const feedbackSuffix = "/_action/feedback";

    for (const topic of topics) {
      if (topic.name.endsWith(feedbackSuffix)) {
        const actionName = topic.name.slice(0, -feedbackSuffix.length);
        let actionType = topic.type;
        if (actionType.endsWith("_FeedbackMessage")) {
          actionType = actionType.slice(0, -"_FeedbackMessage".length);
        }
        actions.push({ name: actionName, type: actionType });
      }
    }

    return actions;
  }

  // --- Private helpers ---

  private setStatus(status: ConnectionStatus): void {
    this.status = status;
    for (const handler of this.connectionHandlers) {
      handler(status);
    }
  }

  private ensureConnected(): void {
    if (this.status !== "connected" || !this.node || !this.entityCache) {
      throw new Error("LocalTransport is not connected");
    }
  }

  /**
   * Resolve a topic's type from the node's graph introspection.
   * Returns undefined if the topic is not yet known.
   */
  private resolveTopicType(topic: string): string | undefined {
    if (!this.node) return undefined;
    const namesAndTypes: Array<{ name: string; types: string[] }> =
      this.node.getTopicNamesAndTypes();
    const entry = namesAndTypes.find((t) => t.name === topic);
    return entry?.types[0];
  }

  /**
   * Resolve a service's type from the node's graph introspection.
   * Returns undefined if the service is not yet known.
   */
  private resolveServiceType(service: string): string | undefined {
    if (!this.node) return undefined;
    const namesAndTypes: Array<{ name: string; types: string[] }> =
      this.node.getServiceNamesAndTypes();
    const entry = namesAndTypes.find((s) => s.name === service);
    return entry?.types[0];
  }

  /**
   * Wrap rclnodejs callback-based sendRequest in a Promise with timeout.
   */
  private sendServiceRequest(client: any, request: any, timeoutMs: number): Promise<any> {
    return new Promise((resolve, reject) => {
      const timer = setTimeout(() => {
        reject(new Error(`Service call timed out after ${timeoutMs}ms`));
      }, timeoutMs);

      try {
        client.sendRequest(request, (response: any) => {
          clearTimeout(timer);
          if (response) {
            resolve(response);
          } else {
            reject(new Error("Service returned no response"));
          }
        });
      } catch (err) {
        clearTimeout(timer);
        reject(err);
      }
    });
  }
}