import type { XyzacKinematicsSnapshot } from "./protocol";

export type XyzacVisualState = {
  schema: "xyzac-visual-state.v1";
  authoritative: boolean;
  tick: number;
  stateHash: string;
  pose: XyzacKinematicsSnapshot["pose"];
  joints: [number, number, number, number, number];
  livePose: XyzacKinematicsSnapshot["pose"];
  activeProgress: number;
  poseEpoch: number;
  livePathEpoch: number;
  motionTrace: Array<{ tick: number; pose: XyzacKinematicsSnapshot["pose"]; joints: number[]; activeSegmentId: number; activeProgress: number }>;
  offsets: XyzacKinematicsSnapshot["offsets"];
  coordinateOffset?: number[];
  machineLimits?: number[][];
  velocityMmMin?: number;
  activeSegmentId: number | null;
  toolpath: Array<Record<string, unknown>>;
  source: "linuxcnc-wasm-worker";
};

const ZERO_POSE: XyzacKinematicsSnapshot["pose"] = { x: 0, y: 0, z: 0, a: 0, b: 0, c: 0, u: 0, v: 0, w: 0 };
const ZERO_OFFSETS: XyzacKinematicsSnapshot["offsets"] = { x_rot_point: 0, y_offset: 0, z_offset: 0, tool_offset: 0 };

export type XyzacVisualSource = {
  simulation_tick?: number;
  state_hash?: string;
  kinematics?: XyzacKinematicsSnapshot;
  motion?: { velocity_mm_min?: number };
  visual?: { active_segment_id?: number | null; active_progress?: number; pose_epoch?: number; live_path_epoch?: number; motion_trace?: Array<{ tick: number; pose: Record<string, number>; joints: number[]; active_segment_id: number; active_progress: number }>; pose?: Record<string, number>; joints?: number[]; toolpath?: Array<Record<string, unknown>> };
};

export function projectXyzacVisual(snapshot: XyzacVisualSource | null | undefined): XyzacVisualState {
  const kinematics = snapshot?.kinematics;
  const visualPose = snapshot?.visual?.pose;
  const pose = visualPose && [visualPose.x, visualPose.y, visualPose.z, visualPose.a, visualPose.c].every((value) => Number.isFinite(value))
    ? { ...ZERO_POSE, ...visualPose }
    : kinematics?.pose ?? ZERO_POSE;
  const visualJoints = snapshot?.visual?.joints;
  const joints: [number, number, number, number, number] = visualJoints?.length === 5
    ? visualJoints.slice(0, 5).map((value) => Number(value)) as [number, number, number, number, number]
    : kinematics?.joints?.length === 5 ? [...kinematics.joints] as [number, number, number, number, number] : [0, 0, 0, 0, 0];
  const rawProgress = snapshot?.visual?.active_progress;
  const activeProgress = Number.isFinite(rawProgress) ? Math.max(0, Math.min(1, Number(rawProgress))) : 0;
  const motionTrace = (snapshot?.visual?.motion_trace ?? []).filter((sample) => sample && Number.isSafeInteger(sample.tick) && sample.pose && Array.isArray(sample.joints)).map((sample) => ({ tick: sample.tick, pose: { ...ZERO_POSE, ...sample.pose }, joints: sample.joints.slice(0, 5).map(Number), activeSegmentId: Number(sample.active_segment_id ?? 0), activeProgress: Math.max(0, Math.min(1, Number(sample.active_progress ?? 0))) }));
  return {
    schema: "xyzac-visual-state.v1",
    authoritative: Boolean(snapshot?.kinematics && snapshot?.visual),
    tick: snapshot?.simulation_tick ?? 0,
    stateHash: snapshot?.state_hash ?? "",
    pose,
    joints,
    livePose: pose,
    activeProgress,
    poseEpoch: Number.isSafeInteger(snapshot?.visual?.pose_epoch) ? Number(snapshot?.visual?.pose_epoch) : 0,
    livePathEpoch: Number.isSafeInteger(snapshot?.visual?.live_path_epoch) ? Number(snapshot?.visual?.live_path_epoch) : 0,
    motionTrace,
    offsets: kinematics?.offsets ?? ZERO_OFFSETS,
    coordinateOffset: (snapshot?.visual as { coordinate_offset?: number[] })?.coordinate_offset,
    machineLimits: (snapshot?.visual as { machine_limits?: number[][] })?.machine_limits,
    velocityMmMin: (snapshot?.visual as { velocity_mm_min?: number })?.velocity_mm_min ?? snapshot?.motion?.velocity_mm_min,
    activeSegmentId: snapshot?.visual?.active_segment_id ?? null,
    toolpath: snapshot?.visual?.toolpath ?? [],
    source: "linuxcnc-wasm-worker",
  };
}

export const XYZAC_MACHINE_GROUPS = Object.freeze(["xyzac-machine", "table-x", "saddle-y", "spindle-z", "tilt-a", "rotate-c", "tool-offset", "xyzac-toolpath", "xyzac-live-toolpath"]);