import type {
  KdlWasmApi,
  PathPlanResult,
  PathPlanRequest,
  RobotHandle,
  TrajectoryPoint,
  TrajectoryResult
} from "../kdl/types.js";
import type {
  CompiledPath,
  MotionInstruction,
  OperationActionInstruction,
  RunOperationInstruction,
  RunPathInstruction
} from "../grl/ir/index.js";

export type MotionQueueSource =
  | { kind: "instruction"; instruction: MotionInstruction; pathId?: string; pointId?: string; operationId?: string; sourceMap?: MotionInstruction["sourceMap"] }
  | { kind: "path"; pathId: string; pointId?: string; operationId?: string; instruction?: RunPathInstruction; sourceMap?: RunPathInstruction["sourceMap"] }
  | { kind: "operation"; operationId: string; pathId?: string; pointId?: string; instruction?: RunOperationInstruction; sourceMap?: RunOperationInstruction["sourceMap"] }
  | { kind: "action"; operationId: string; pathId?: string; pointId?: string; actionKind: OperationActionInstruction["actionKind"]; statement: string; sourceMap?: OperationActionInstruction["sourceMap"] };

export interface MotionQueueItem {
  id: string;
  kind: "motion" | "path" | "action";
  source: MotionQueueSource;
  request: PathPlanRequest;
  planned?: PathPlanResult;
  result?: PathPlanResult | TrajectoryResult;
  status: "queued" | "planned" | "playing" | "done" | "failed";
}

export interface MotionPlaybackSample {
  itemId: string;
  time: number;
  localTime?: number;
  point: TrajectoryPoint;
  source: MotionQueueSource;
}

export interface MotionPlanner {
  planPath(handle: RobotHandle | undefined, request: PathPlanRequest): PathPlanResult | Promise<PathPlanResult>;
}

export interface MotionQueueOptions {
  startJoints: number[];
  sampleTime: number;
  planner?: MotionPlanner;
  robotHandle?: RobotHandle;
}

export interface MotionQueueSnapshot {
  items: MotionQueueItem[];
  activeIndex: number;
  virtualTime: number;
}

export class MotionQueue {
  private readonly itemsValue: MotionQueueItem[] = [];
  private readonly options: MotionQueueOptions;
  private virtualTimeValue = 0;
  private nextId = 1;

  constructor(options: MotionQueueOptions = { startJoints: [], sampleTime: 0.004 }) {
    this.options = options;
  }

  get virtualTime(): number {
    return this.virtualTimeValue;
  }

  enqueue(item: Omit<MotionQueueItem, "status"> & { status?: MotionQueueItem["status"] }): MotionQueueItem {
    const next: MotionQueueItem = {
      ...item,
      status: item.status ?? "queued"
    };
    this.itemsValue.push(next);
    return next;
  }

  enqueueMotion(instruction: MotionInstruction): MotionQueueItem {
    const segment = {
      id: instruction.pointId ?? instruction.id ?? `motion_${this.nextId}`,
      motion: instruction.kind,
      ...(instruction.target ? { target: instruction.target } : {}),
      ...(instruction.via ? { via: instruction.via } : {}),
      speed: instruction.speed,
      zone: instruction.zone,
      ...(instruction.tool ? { tool: instruction.tool } : {}),
      ...(instruction.frame ? { frame: instruction.frame } : {}),
      ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}),
      ...(instruction.source ? { source: instruction.source } : {})
    };
    return this.enqueue({
      id: `mq_${this.nextId++}`,
      kind: "motion",
      source: {
        kind: "instruction",
        instruction,
        ...(instruction.pathId ? { pathId: instruction.pathId } : {}),
        ...(instruction.pointId ? { pointId: instruction.pointId } : {}),
        ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {})
      },
      request: {
        ...(instruction.pathId ? { pathId: instruction.pathId } : {}),
        startJoints: [...this.options.startJoints],
        sampleTime: this.options.sampleTime,
        segments: [segment]
      }
    });
  }

  enqueueRunPath(instruction: RunPathInstruction, path: CompiledPath): MotionQueueItem {
    const operationId = hiddenOperationId(instruction);
    return this.enqueue({
      id: `mq_${this.nextId++}`,
      kind: "path",
      source: {
        kind: "path",
        pathId: instruction.pathId,
        ...(operationId ? { operationId } : {}),
        instruction,
        ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {})
      },
      request: {
        ...path.request,
        startJoints: [...this.options.startJoints],
        sampleTime: this.options.sampleTime
      }
    });
  }

  enqueueOperationAction(action: OperationActionInstruction): MotionQueueItem {
    return this.enqueue({
      id: `mq_${this.nextId++}`,
      kind: "action",
      status: "done",
      source: {
        kind: "action",
        operationId: action.operationId,
        actionKind: action.actionKind,
        statement: action.statement,
        ...(action.sourceMap ? { sourceMap: action.sourceMap } : {})
      },
      request: {
        startJoints: [...this.options.startJoints],
        sampleTime: this.options.sampleTime,
        segments: []
      }
    });
  }

  items(): MotionQueueItem[] {
    return this.itemsValue.map((item) => ({ ...item }));
  }

  clear(): void {
    this.itemsValue.length = 0;
  }

  async planAll(api?: Pick<KdlWasmApi, "planPath">, handle?: RobotHandle): Promise<MotionQueueItem[]> {
    const planner = api
      ? { planPath: (robotHandle: RobotHandle | undefined, request: PathPlanRequest) => api.planPath(robotHandle ?? handle ?? this.options.robotHandle ?? 0, request) }
      : this.options.planner;
    if (!planner) {
      throw new Error("MotionQueue requires a planner or KDL API to plan queued paths");
    }
    for (const item of this.itemsValue) {
      if (item.status !== "queued" || item.request.segments.length === 0) {
        continue;
      }
      try {
        item.planned = await planner.planPath(handle ?? this.options.robotHandle, item.request);
        item.result = item.planned;
        item.status = item.planned.ok ? "planned" : "failed";
      } catch {
        item.status = "failed";
      }
    }
    return this.items();
  }

  advance(deltaTime: number): MotionPlaybackSample | undefined {
    this.virtualTimeValue += deltaTime;
    return this.sampleAt(this.virtualTimeValue);
  }

  sampleAt(time: number): MotionPlaybackSample | undefined {
    let elapsed = 0;
    for (const item of this.itemsValue) {
      const planned = item.planned ?? (item.result && "segments" in item.result ? item.result : undefined);
      if (!planned) {
        continue;
      }
      const duration = planned?.duration ?? 0;
      if (duration === 0) {
        continue;
      }
      if (time <= elapsed + duration) {
        const localTime = Math.max(0, Math.min(duration, time - elapsed));
        const point = samplePoint(planned.points, localTime);
        if (!point) {
          return undefined;
        }
        return {
          itemId: item.id,
          time,
          localTime,
          point,
          source: item.source
        } as MotionPlaybackSample;
      }
      elapsed += duration;
    }
    const last = [...this.itemsValue].reverse().find((item) => item.planned?.points.length);
    const point = last?.planned?.points.at(-1);
    return last && point && last.planned
      ? ({ itemId: last.id, time, localTime: last.planned.duration, point, source: last.source } as MotionPlaybackSample)
      : undefined;
  }

  snapshot(): MotionQueueSnapshot {
    return {
      items: this.items(),
      activeIndex: Math.max(0, this.itemsValue.findIndex((item) => item.status === "playing" || item.status === "planned")),
      virtualTime: this.virtualTimeValue
    };
  }

  playback(sampleTime: number): MotionPlaybackSample[] {
    const samples: MotionPlaybackSample[] = [];
    for (const item of this.itemsValue) {
      const points = pointsOf(item.result);
      if (points.length === 0) {
        continue;
      }
      item.status = "playing";
      for (const point of points) {
        if (point.index === 0 || point.time === 0 || point.time % sampleTime < 1e-9 || point === points.at(-1)) {
          samples.push({
            itemId: item.id,
            time: point.time,
            point,
            source: item.source
          });
        }
      }
      item.status = "done";
    }
    return samples;
  }
}

function samplePoint(points: TrajectoryPoint[], time: number): TrajectoryPoint | undefined {
  return points.find((point) => point.time >= time) ?? points.at(-1);
}

function hiddenOperationId(instruction: RunPathInstruction): string | undefined {
  return (instruction as RunPathInstruction & { __operationId?: string }).__operationId;
}

function pointsOf(result: PathPlanResult | TrajectoryResult | undefined): TrajectoryPoint[] {
  if (!result) {
    return [];
  }
  return "segments" in result ? result.points : result.points;
}
