import { KdlStructuredError } from "../kdl/rpc.js";
import { makeTrapProfile } from "../kdl/trapProfile.js";
import {
  identityMat4,
  jointMotionMat4,
  type Mat4,
  mat4ToPose,
  multiplyMat4,
  poseToMat4,
  rotationFromRpyMat4,
  translationMat4
} from "../math/poseMath.js";
import type {
  FkOptions,
  FkResult,
  IkOptions,
  IkResult,
  JacobianOptions,
  JacobianResult,
  JointTarget,
  JointLimits,
  LinkPoseResult,
  LimitCheckResult,
  MotionDiagnostic,
  MoveCRequest,
  MoveJRequest,
  MoveLRequest,
  NormalizedRobotModel,
  PathPlanRequest,
  PathPlanResult,
  PathValidationResult,
  Pose,
  PoseTarget,
  ReachabilityResult,
  RobotHandle,
  RobotInfo,
  SegmentValidationReport,
  SpeedSpec,
  SingularityResult,
  TrajectoryPoint,
  TrajectoryResult
} from "../kdl/types.js";

export interface RobotRegistryRecord {
  handle: RobotHandle;
  model: NormalizedRobotModel;
}

export class RobotModelRegistry {
  private nextHandle = 1;
  private readonly robots = new Map<RobotHandle, RobotRegistryRecord>();

  create(model: NormalizedRobotModel): RobotHandle {
    validateNormalizedRobotModel(model);
    const handle = this.nextHandle++;
    this.robots.set(handle, { handle, model });
    return handle;
  }

  destroy(handle: RobotHandle): void {
    this.ensure(handle);
    this.robots.delete(handle);
  }

  getInfo(handle: RobotHandle): RobotInfo {
    const record = this.ensure(handle);
    const model = record.model;

    return {
      handle,
      robotId: model.robotId,
      name: model.name,
      baseLink: model.baseLink,
      tipLink: model.tipLink,
      jointNames: model.activeJointNames,
      dof: model.activeJointNames.length,
      limits: model.limits
    };
  }

  getJointLimits(handle: RobotHandle): JointLimits[] {
    return this.ensure(handle).model.limits;
  }

  fk(handle: RobotHandle, joints: Float64Array | number[], options: FkOptions = {}): FkResult {
    const record = this.ensure(handle);
    const jointValues = Array.from(joints);
    validateJointVector(record.model, jointValues);
    const linkPoses = computeLinkPoses(record.model, jointValues);
    const flange = linkPoses.at(-1)?.pose ?? identityPose();
    const tcp = options.tool ? composeFkPose(flange, options.tool) : flange;

    return {
      ok: true,
      flange,
      tcp: options.frame ? composeFkPose(options.frame, tcp) : tcp,
      joints: jointValues,
      diagnostics: []
    };
  }

  fkPose7(
    handle: RobotHandle,
    joints: Float64Array | number[],
    out?: Float64Array,
    options: FkOptions = {}
  ): Float64Array {
    const output = out ?? new Float64Array(7);
    if (output.length < 7) {
      throw new KdlStructuredError("KDL_OUTPUT_DIMENSION_MISMATCH", "fkPose7 output buffer must have length >= 7");
    }

    const result = this.fk(handle, joints, options);
    output[0] = result.tcp.position[0];
    output[1] = result.tcp.position[1];
    output[2] = result.tcp.position[2];
    output[3] = result.tcp.quaternion[0];
    output[4] = result.tcp.quaternion[1];
    output[5] = result.tcp.quaternion[2];
    output[6] = result.tcp.quaternion[3];
    return output;
  }

  fkAllLinks(handle: RobotHandle, joints: Float64Array | number[], options: FkOptions = {}): LinkPoseResult {
    const record = this.ensure(handle);
    const jointValues = Array.from(joints);
    validateJointVector(record.model, jointValues);
    const linkPoses = computeLinkPoses(record.model, jointValues);

    return {
      ok: true,
      linkPoses: options.includeFlange === false ? linkPoses.slice(0, -1) : linkPoses,
      diagnostics: []
    };
  }

  ik(handle: RobotHandle, seed: Float64Array | number[], target: Pose, options: IkOptions = {}): IkResult {
    const record = this.ensure(handle);
    const seedValues = Array.from(seed);
    validateJointVector(record.model, seedValues);
    return solveIk(record.model, seedValues, target, options);
  }

  ikBatch(
    handle: RobotHandle,
    seeds: Array<Float64Array | number[]>,
    targets: Pose[],
    options: IkOptions = {}
  ): IkResult[] {
    const record = this.ensure(handle);
    if (seeds.length !== targets.length) {
      throw new KdlStructuredError(
        "KDL_IK_BATCH_DIMENSION_MISMATCH",
        `Expected ${targets.length} seeds, got ${seeds.length}`
      );
    }

    let lastSuccessfulSeed: number[] | undefined;
    return targets.map((target, index) => {
      const seed = lastSuccessfulSeed ?? Array.from(seeds[index] ?? []);
      validateJointVector(record.model, seed);
      const result = solveIk(record.model, seed, target, options);
      if (result.ok && result.joints) {
        lastSuccessfulSeed = result.joints;
      }
      return result;
    });
  }

  jacobian(
    handle: RobotHandle,
    joints: Float64Array | number[],
    _options: JacobianOptions = {}
  ): JacobianResult {
    const record = this.ensure(handle);
    const jointValues = Array.from(joints);
    validateJointVector(record.model, jointValues);
    return computeJacobian(record.model, jointValues);
  }

  checkSingularity(handle: RobotHandle, joints: Float64Array | number[]): SingularityResult {
    const jacobian = this.jacobian(handle, joints);
    const rows = matrixRows(jacobian.data as number[], jacobian.rows, jacobian.cols);
    const linearRows = rows.slice(0, 3);
    const gram = multiplyMatrix(linearRows, transpose(linearRows));
    const manipulability = Math.sqrt(Math.max(0, determinant3(gram)));
    const conditionNumber = estimateConditionNumber(linearRows);
    const nearSingularity = manipulability < 1e-6 || conditionNumber > 1e6;

    return {
      ok: true,
      nearSingularity,
      manipulability,
      conditionNumber,
      diagnostics: nearSingularity
        ? [
            {
              severity: "warning",
              code: "KDL_SINGULARITY",
              message: "Jacobian is near singular"
            }
          ]
        : []
    };
  }

  checkJointLimits(handle: RobotHandle, joints: Float64Array | number[]): LimitCheckResult {
    const record = this.ensure(handle);
    const jointValues = Array.from(joints);
    validateJointVector(record.model, jointValues);
    const diagnostics = jointValues.flatMap((value, index) => {
      const limits = record.model.limits[index];
      if (!limits || (value >= limits.lower && value <= limits.upper)) {
        return [];
      }
      return [
        {
          severity: "error" as const,
          code: "KDL_JOINT_LIMIT",
          message: `Joint ${limits.name}=${value} is outside [${limits.lower}, ${limits.upper}]`
        }
      ];
    });

    return {
      ok: diagnostics.length === 0,
      diagnostics
    };
  }

  checkVelocityLimits(handle: RobotHandle, trajectory: TrajectoryResult): LimitCheckResult {
    const record = this.ensure(handle);
    const points = trajectory.points;
    const diagnostics = [];
    let maxJointVelocityRatio = 0;
    let maxJointAccelerationRatio = 0;

    for (const [pointIndex, point] of points.entries()) {
      for (let index = 0; index < record.model.limits.length; index += 1) {
        const limit = record.model.limits[index]!;
        const velocityRatio = limitRatio(point.jointVelocity[index] ?? 0, limit.velocity);
        const accelerationRatio = limitRatio(point.jointAcceleration[index] ?? 0, limit.acceleration);
        maxJointVelocityRatio = Math.max(maxJointVelocityRatio, velocityRatio);
        maxJointAccelerationRatio = Math.max(maxJointAccelerationRatio, accelerationRatio);
        if (velocityRatio > 1) {
          diagnostics.push({
            severity: "error" as const,
            code: "KDL_VELOCITY_LIMIT",
            message: `Point ${pointIndex} joint ${limit.name} velocity exceeds limit`,
            pointIndex
          });
        }
        if (accelerationRatio > 1) {
          diagnostics.push({
            severity: "error" as const,
            code: "KDL_ACCEL_LIMIT",
            message: `Point ${pointIndex} joint ${limit.name} acceleration exceeds limit`,
            pointIndex
          });
        }
      }
    }

    return {
      ok: diagnostics.length === 0,
      diagnostics,
      maxJointVelocityRatio,
      maxJointAccelerationRatio
    };
  }

  checkReachability(handle: RobotHandle, target: PoseTarget, options: IkOptions = {}): ReachabilityResult {
    const record = this.ensure(handle);
    const seed = options.seeds?.[0] ?? record.model.activeJointNames.map(() => 0);
    const result = solveIk(record.model, seed, target.pose, options);

    return {
      ok: result.ok,
      reachable: result.ok,
      ...(target.id ? { targetId: target.id } : {}),
      ...(result.joints ? { joints: result.joints } : {}),
      ...(result.residualPosition !== undefined ? { residualPosition: result.residualPosition } : {}),
      ...(result.residualOrientation !== undefined ? { residualOrientation: result.residualOrientation } : {}),
      diagnostics: result.diagnostics
    };
  }

  checkReachabilityBatch(handle: RobotHandle, targets: PoseTarget[], options: IkOptions = {}): ReachabilityResult[] {
    return targets.map((target, index) => {
      const seed = options.seeds?.[index];
      return this.checkReachability(handle, target, {
        ...options,
        ...(seed ? { seeds: [seed] } : {})
      });
    });
  }

  planMoveJ(handle: RobotHandle, request: MoveJRequest): TrajectoryResult {
    const record = this.ensure(handle);
    const startJoints = [...request.startJoints];
    validateJointVector(record.model, startJoints);

    const targetResolution = resolveMoveJTarget(record.model, startJoints, request.target);
    if (!targetResolution.ok) {
      return emptyMoveJResult(request, targetResolution.diagnostics);
    }

    const endJoints = targetResolution.joints;
    const endpointDiagnostics = [
      ...jointLimitDiagnostics(record.model, startJoints),
      ...jointLimitDiagnostics(record.model, endJoints)
    ];
    if (endpointDiagnostics.length > 0) {
      return emptyMoveJResult(request, endpointDiagnostics);
    }

    const dq = endJoints.map((end, index) => end - startJoints[index]!);
    const motionScale = Math.max(...dq.map((value) => Math.abs(value)), 0);
    const profile = makeTrapProfile(motionScale === 0 ? 0 : 1, {
      maxVelocity: normalizedMoveJVelocity(record.model, dq, request.speed, request.speedOverride),
      maxAcceleration: normalizedMoveJAcceleration(record.model, dq, request.speed, request.speedOverride),
      sampleTime: request.sampleTime
    });
    const diagnostics: MotionDiagnostic[] = [
      ...moveJSpeedDiagnostics(request.speed),
      ...zoneApproximationDiagnostics(request.zone),
      ...profile.diagnostics
    ];
    const fkOptions = moveJTargetFkOptions(request);
    const targetId = targetIdOf(request.target);
    const points: TrajectoryPoint[] = profile.samples.map((sample, index) => {
      const joints = dq.map((delta, jointIndex) => startJoints[jointIndex]! + delta * sample.s);
      const jointVelocity = dq.map((delta) => delta * sample.sd);
      const jointAcceleration = dq.map((delta) => delta * sample.sdd);
      const linkPoses = computeLinkPoses(record.model, joints);
      const flange = linkPoses.at(-1)?.pose ?? identityPose();
      const tcpWithTool = fkOptions.tool ? composeFkPose(flange, fkOptions.tool) : flange;
      const tcp = fkOptions.frame ? composeFkPose(fkOptions.frame, tcpWithTool) : tcpWithTool;
      const pointDiagnostics = singularityDiagnostics(record.model, joints);

      return {
        index,
        time: sample.time,
        dt: index === 0 ? 0 : sample.time - profile.samples[index - 1]!.time,
        s: sample.s,
        sd: sample.sd,
        sdd: sample.sdd,
        joints,
        jointVelocity,
        jointAcceleration,
        flange,
        tcp,
        motion: "MOVEJ",
        ...(targetId ? { targetId } : {}),
        ...(request.sourceMap ? { sourceMap: request.sourceMap } : {}),
        diagnostics: pointDiagnostics
      };
    });

    const velocityDiagnostics = velocityLimitDiagnostics(record.model, points);
    diagnostics.push(...velocityDiagnostics);
    for (const point of points) {
      diagnostics.push(...point.diagnostics);
    }

    return {
      ok: diagnostics.every((diagnostic) => diagnostic.severity !== "error"),
      motion: "MOVEJ",
      duration: profile.duration,
      sampleTime: request.sampleTime,
      points,
      events: [],
      diagnostics,
      meta: {
        targetType: isJointTarget(request.target) ? "joint" : "pose",
        qStart: startJoints,
        qEnd: endJoints,
        profileType: profile.type,
        zone: request.zone
      }
    };
  }

  planMoveL(handle: RobotHandle, request: MoveLRequest): TrajectoryResult {
    const record = this.ensure(handle);
    const startJoints = [...request.startJoints];
    validateJointVector(record.model, startJoints);

    const startFk = this.fk(handle, startJoints, moveLFkOptions(request));
    const startPose = startFk.tcp;
    const targetPose = request.target.pose;
    const length = distance(startPose.position, targetPose.position);
    const profile = makeTrapProfile(length, {
      maxVelocity: linearSpeedValue(request.speed, request.speedOverride),
      maxAcceleration: linearAccelerationValue(request.speed, request.speedOverride),
      sampleTime: request.sampleTime
    });
    const diagnostics: MotionDiagnostic[] = [
      ...moveLSpeedDiagnostics(request.speed),
      ...zoneApproximationDiagnostics(request.zone),
      ...profile.diagnostics
    ];
    const points: TrajectoryPoint[] = [];
    let seed = startJoints;

    for (const [index, sample] of profile.samples.entries()) {
      const targetSamplePose = interpolateLinearPose(startPose, targetPose, sample.s);
      const ikResult = solveIk(record.model, seed, targetSamplePose, {
        ...request.ik,
        seeds: [seed]
      });
      if (!ikResult.ok || !ikResult.joints) {
        return {
          ok: false,
          motion: "MOVEL",
          duration: sample.time,
          sampleTime: request.sampleTime,
          points,
          events: [],
          diagnostics: [
            ...diagnostics,
            ...ikResult.diagnostics.map((diagnostic) => ({
              ...diagnostic,
              pointIndex: index
            }))
          ],
          meta: {
            length,
            failedPointIndex: index,
            targetId: request.target.id
          }
        };
      }

      const joints = ikResult.joints;
      const previous = points.at(-1);
      const dt = index === 0 ? 0 : sample.time - profile.samples[index - 1]!.time;
      const jointVelocity = previous && dt > 0
        ? joints.map((value, jointIndex) => (value - previous.joints[jointIndex]!) / dt)
        : joints.map(() => 0);
      const jointAcceleration = previous && dt > 0
        ? jointVelocity.map((value, jointIndex) => (value - previous.jointVelocity[jointIndex]!) / dt)
        : joints.map(() => 0);
      const fk = this.fk(handle, joints, moveLFkOptions(request));
      const pointDiagnostics = [
        ...singularityDiagnostics(record.model, joints),
        ...linearErrorDiagnostics(index, fk.tcp, targetSamplePose)
      ];

      points.push({
        index,
        time: sample.time,
        dt,
        s: sample.s,
        sd: sample.sd,
        sdd: sample.sdd,
        joints,
        jointVelocity,
        jointAcceleration,
        flange: fk.flange,
        tcp: fk.tcp,
        motion: "MOVEL",
        ...(request.target.id ? { targetId: request.target.id } : {}),
        ...(request.sourceMap ? { sourceMap: request.sourceMap } : {}),
        diagnostics: pointDiagnostics
      });
      seed = joints;
    }

    const velocityDiagnostics = velocityLimitDiagnostics(record.model, points);
    diagnostics.push(...velocityDiagnostics);
    for (const point of points) {
      diagnostics.push(...point.diagnostics);
    }

    return {
      ok: diagnostics.every((diagnostic) => diagnostic.severity !== "error"),
      motion: "MOVEL",
      duration: profile.duration,
      sampleTime: request.sampleTime,
      points,
      events: [],
      diagnostics,
      meta: {
        length,
        targetId: request.target.id,
        profileType: profile.type,
        orientationMode: request.orientationMode ?? "fixed"
      }
    };
  }

  planMoveC(handle: RobotHandle, request: MoveCRequest): TrajectoryResult {
    const record = this.ensure(handle);
    const startJoints = [...request.startJoints];
    validateJointVector(record.model, startJoints);

    const startFk = this.fk(handle, startJoints, moveCFkOptions(request));
    const arc = computeCircleArc(startFk.tcp, request.via.pose, request.target.pose);
    if (!arc.ok) {
      return {
        ok: false,
        motion: "MOVEC",
        duration: 0,
        sampleTime: request.sampleTime,
        points: [],
        events: [],
        diagnostics: arc.diagnostics,
        meta: {
          viaId: request.via.id,
          targetId: request.target.id
        }
      };
    }

    const profile = makeTrapProfile(arc.meta.length, {
      maxVelocity: linearSpeedValue(request.speed, request.speedOverride),
      maxAcceleration: linearAccelerationValue(request.speed, request.speedOverride),
      sampleTime: request.sampleTime
    });
    const diagnostics: MotionDiagnostic[] = [
      ...moveCSpeedDiagnostics(request.speed),
      ...zoneApproximationDiagnostics(request.zone),
      ...profile.diagnostics
    ];
    const points: TrajectoryPoint[] = [];
    let seed = startJoints;
    let maxArcError = 0;

    for (const [index, sample] of profile.samples.entries()) {
      const targetSamplePose = sampleCirclePose(arc, sample.s, request.target.pose.quaternion);
      const ikResult = solveIk(record.model, seed, targetSamplePose, {
        ...request.ik,
        seeds: [seed]
      });
      if (!ikResult.ok || !ikResult.joints) {
        return {
          ok: false,
          motion: "MOVEC",
          duration: sample.time,
          sampleTime: request.sampleTime,
          points,
          events: [],
          diagnostics: [
            ...diagnostics,
            ...ikResult.diagnostics.map((diagnostic) => ({
              ...diagnostic,
              pointIndex: index
            }))
          ],
          meta: {
            circle: {
              ...arc.meta,
              maxArcError
            },
            failedPointIndex: index,
            viaId: request.via.id,
            targetId: request.target.id
          }
        };
      }

      const joints = ikResult.joints;
      const previous = points.at(-1);
      const dt = index === 0 ? 0 : sample.time - profile.samples[index - 1]!.time;
      const jointVelocity = previous && dt > 0
        ? joints.map((value, jointIndex) => (value - previous.joints[jointIndex]!) / dt)
        : joints.map(() => 0);
      const jointAcceleration = previous && dt > 0
        ? jointVelocity.map((value, jointIndex) => (value - previous.jointVelocity[jointIndex]!) / dt)
        : joints.map(() => 0);
      const fk = this.fk(handle, joints, moveCFkOptions(request));
      const arcError = distance(fk.tcp.position, targetSamplePose.position);
      maxArcError = Math.max(maxArcError, arcError);
      const pointDiagnostics = [
        ...singularityDiagnostics(record.model, joints),
        ...arcErrorDiagnostics(index, arcError)
      ];

      points.push({
        index,
        time: sample.time,
        dt,
        s: sample.s,
        sd: sample.sd,
        sdd: sample.sdd,
        joints,
        jointVelocity,
        jointAcceleration,
        flange: fk.flange,
        tcp: fk.tcp,
        motion: "MOVEC",
        ...(request.target.id ? { targetId: request.target.id } : {}),
        ...(request.sourceMap ? { sourceMap: request.sourceMap } : {}),
        diagnostics: pointDiagnostics
      });
      seed = joints;
    }

    const velocityDiagnostics = velocityLimitDiagnostics(record.model, points);
    diagnostics.push(...velocityDiagnostics);
    for (const point of points) {
      diagnostics.push(...point.diagnostics);
    }

    return {
      ok: diagnostics.every((diagnostic) => diagnostic.severity !== "error"),
      motion: "MOVEC",
      duration: profile.duration,
      sampleTime: request.sampleTime,
      points,
      events: [],
      diagnostics,
      meta: {
        circle: {
          ...arc.meta,
          maxArcError
        },
        viaId: request.via.id,
        targetId: request.target.id,
        profileType: profile.type,
        orientationMode: request.orientationMode ?? "fixed"
      }
    };
  }

  planPath(handle: RobotHandle, request: PathPlanRequest): PathPlanResult {
    validateJointVector(this.ensure(handle).model, request.startJoints);
    if (request.segments.length === 0) {
      const diagnostics = [pathEmptyDiagnostic()];
      return {
        ok: false,
        duration: 0,
        segments: [],
        points: [],
        diagnostics
      };
    }

    const segments: TrajectoryResult[] = [];
    const points: TrajectoryPoint[] = [];
    const diagnostics: MotionDiagnostic[] = [];
    let currentJoints = [...request.startJoints];
    let timeOffset = 0;

    for (const segment of request.segments) {
      const result = this.planPathSegment(handle, request, segment, currentJoints);
      const segmentDiagnostics = result.diagnostics.map((diagnostic) => ({
        ...diagnostic,
        segmentId: diagnostic.segmentId ?? segment.id,
        ...(diagnostic.sourceMap ?? segment.sourceMap ? { sourceMap: diagnostic.sourceMap ?? segment.sourceMap } : {})
      }));
      const segmentPoints = result.points.map((point) => ({
        ...point,
        segmentId: point.segmentId ?? segment.id,
        ...(point.sourceMap ?? segment.sourceMap ? { sourceMap: point.sourceMap ?? segment.sourceMap } : {}),
        diagnostics: point.diagnostics.map((diagnostic) => ({
          ...diagnostic,
          segmentId: diagnostic.segmentId ?? segment.id,
          ...(diagnostic.sourceMap ?? segment.sourceMap ? { sourceMap: diagnostic.sourceMap ?? segment.sourceMap } : {})
        }))
      }));
      const segmentResult = {
        ...result,
        points: segmentPoints,
        diagnostics: segmentDiagnostics
      };
      segments.push(segmentResult);
      diagnostics.push(...segmentDiagnostics);

      for (const point of segmentPoints) {
        if (points.length > 0 && point.index === 0) {
          continue;
        }
        const previous = points.at(-1);
        const time = point.time + timeOffset;
        points.push({
          ...point,
          index: points.length,
          time,
          dt: previous ? time - previous.time : 0,
          segmentId: segment.id,
          ...(point.targetId ? { targetId: point.targetId } : {}),
          ...(point.sourceMap ?? segment.sourceMap ? { sourceMap: point.sourceMap ?? segment.sourceMap } : {}),
          diagnostics: point.diagnostics.map((diagnostic) => ({
            ...diagnostic,
            segmentId: diagnostic.segmentId ?? segment.id,
            ...(diagnostic.sourceMap ?? segment.sourceMap ? { sourceMap: diagnostic.sourceMap ?? segment.sourceMap } : {})
          }))
        });
      }

      timeOffset += result.duration;
      if (result.ok && result.points.at(-1)) {
        currentJoints = [...result.points.at(-1)!.joints];
      }
      if (!result.ok && request.stopOnError !== false) {
        break;
      }
    }

    return {
      ok: segments.length === request.segments.length && segments.every((segment) => segment.ok),
      duration: timeOffset,
      segments,
      points,
      diagnostics
    };
  }

  validatePath(handle: RobotHandle, request: PathPlanRequest): PathValidationResult {
    const plan = this.planPath(handle, request);
    const segmentReports: SegmentValidationReport[] = plan.segments.map((segment, index) => {
      const segmentRequest = request.segments[index]!;
      const velocityCheck = this.checkVelocityLimits(handle, segment);
      return {
        segmentId: segmentRequest.id,
        ok: segment.ok && velocityCheck.ok,
        motion: segment.motion,
        duration: segment.duration,
        maxJointVelocityRatio: velocityCheck.maxJointVelocityRatio ?? 0,
        maxJointAccelerationRatio: velocityCheck.maxJointAccelerationRatio ?? 0,
        maxCartesianError: maxCartesianError(segment),
        diagnostics: [
          ...segment.diagnostics,
          ...velocityCheck.diagnostics.map((diagnostic) => ({
            ...diagnostic,
            segmentId: diagnostic.segmentId ?? segmentRequest.id
          }))
        ]
      };
    });
    const diagnostics = [
      ...plan.diagnostics,
      ...segmentReports.flatMap((report) => report.diagnostics)
    ];
    return {
      ok: plan.ok && segmentReports.every((report) => report.ok),
      reachable: plan.ok && segmentReports.every((report) => report.ok),
      ...(plan.ok ? { cycleTime: plan.duration } : {}),
      segmentReports,
      diagnostics
    };
  }

  private ensure(handle: RobotHandle): RobotRegistryRecord {
    const record = this.robots.get(handle);
    if (!record) {
      throw new KdlStructuredError("KDL_INVALID_HANDLE", `RobotHandle ${handle} does not exist`);
    }
    return record;
  }

  private planPathSegment(
    handle: RobotHandle,
    request: PathPlanRequest,
    segment: PathPlanRequest["segments"][number],
    startJoints: number[]
  ): TrajectoryResult {
    const common = {
      startJoints,
      speed: segment.speed,
      zone: segment.zone,
      sampleTime: request.sampleTime,
      ...(segment.tool ? { tool: segment.tool } : {}),
      ...(segment.frame ? { frame: segment.frame } : {}),
      ...(request.speedOverride ? { speedOverride: request.speedOverride } : {}),
      ...(segment.sourceMap ? { sourceMap: segment.sourceMap } : {})
    };

    if (segment.motion === "MOVEJ" && segment.target) {
      return this.planMoveJ(handle, {
        ...common,
        target: segment.target
      });
    }
    if (segment.motion === "MOVEL" && segment.target && isPoseTarget(segment.target)) {
      return this.planMoveL(handle, {
        ...common,
        target: segment.target
      });
    }
    if (segment.motion === "MOVEC" && segment.via && segment.target && isPoseTarget(segment.target)) {
      return this.planMoveC(handle, {
        ...common,
        via: segment.via,
        target: segment.target
      });
    }

    return {
      ok: false,
      motion: segment.motion,
      duration: 0,
      sampleTime: request.sampleTime,
      points: [],
      events: [],
      diagnostics: [
        {
          severity: "error",
          code: "KDL_PATH_SEGMENT_INVALID",
          message: `Path segment ${segment.id} has invalid target/via for ${segment.motion}`,
          segmentId: segment.id,
          ...(segment.sourceMap ? { sourceMap: segment.sourceMap } : {})
        }
      ]
    };
  }
}

export function validateNormalizedRobotModel(model: NormalizedRobotModel): void {
  const linkNames = new Set(model.links.map((link) => link.name));
  if (!linkNames.has(model.baseLink)) {
    throw new KdlStructuredError("KDL_INVALID_MODEL", `baseLink does not exist: ${model.baseLink}`);
  }
  if (!linkNames.has(model.tipLink)) {
    throw new KdlStructuredError("KDL_INVALID_MODEL", `tipLink does not exist: ${model.tipLink}`);
  }

  const jointNames = new Set<string>();
  for (const joint of model.joints) {
    if (jointNames.has(joint.name)) {
      throw new KdlStructuredError("KDL_INVALID_MODEL", `Duplicate joint name: ${joint.name}`);
    }
    jointNames.add(joint.name);
    if (!linkNames.has(joint.parent) || !linkNames.has(joint.child)) {
      throw new KdlStructuredError("KDL_INVALID_MODEL", `Joint ${joint.name} references an unknown link`);
    }
  }

  for (const activeJoint of model.activeJointNames) {
    if (!jointNames.has(activeJoint)) {
      throw new KdlStructuredError("KDL_INVALID_MODEL", `activeJointNames contains an unknown joint: ${activeJoint}`);
    }
  }
}

function validateJointVector(model: NormalizedRobotModel, joints: number[]): void {
  if (joints.length !== model.activeJointNames.length) {
    throw new KdlStructuredError(
      "KDL_JOINT_DIMENSION_MISMATCH",
      `Expected ${model.activeJointNames.length} joints, got ${joints.length}`
    );
  }

  for (let index = 0; index < joints.length; index += 1) {
    if (!Number.isFinite(joints[index])) {
      throw new KdlStructuredError("KDL_INVALID_JOINTS", `Joint value at index ${index} is not finite`);
    }
  }
}

function pathEmptyDiagnostic(): MotionDiagnostic {
  return {
    severity: "error",
    code: "KDL_PATH_EMPTY",
    message: "PathPlanRequest.segments must contain at least one segment"
  };
}

function maxCartesianError(segment: TrajectoryResult): number {
  const circle = segment.meta?.circle;
  if (isRecord(circle) && typeof circle.maxArcError === "number") {
    return circle.maxArcError;
  }

  let maxError = 0;
  for (const diagnostic of segment.diagnostics) {
    const data = diagnostic.data;
    if (!data) {
      continue;
    }
    if (typeof data.positionError === "number") {
      maxError = Math.max(maxError, data.positionError);
    }
    if (typeof data.arcError === "number") {
      maxError = Math.max(maxError, data.arcError);
    }
  }
  return maxError;
}

function isRecord(value: unknown): value is Record<string, unknown> {
  return !!value && typeof value === "object";
}

function resolveMoveJTarget(
  model: NormalizedRobotModel,
  startJoints: number[],
  target: JointTarget | PoseTarget
): { ok: true; joints: number[] } | { ok: false; diagnostics: MotionDiagnostic[] } {
  if (isJointTarget(target)) {
    const joints = [...target.joints];
    validateJointVector(model, joints);
    return {
      ok: true,
      joints
    };
  }

  const result = solveIk(model, startJoints, target.pose, {
    seeds: [startJoints]
  });
  if (result.ok && result.joints) {
    return {
      ok: true,
      joints: result.joints
    };
  }

  return {
    ok: false,
    diagnostics: result.diagnostics.length > 0
      ? result.diagnostics
      : [
          {
            severity: "error",
            code: "KDL_MOVEJ_IK_FAILED",
            message: "MOVEJ pose target IK failed"
          }
        ]
  };
}

function emptyMoveJResult(request: MoveJRequest, diagnostics: MotionDiagnostic[]): TrajectoryResult {
  return {
    ok: false,
    motion: "MOVEJ",
    duration: 0,
    sampleTime: request.sampleTime,
    points: [],
    events: [],
    diagnostics,
    meta: {
      targetType: isJointTarget(request.target) ? "joint" : "pose"
    }
  };
}

function jointLimitDiagnostics(model: NormalizedRobotModel, joints: number[]): MotionDiagnostic[] {
  return joints.flatMap((value, index) => {
    const limit = model.limits[index];
    if (!limit || (value >= limit.lower && value <= limit.upper)) {
      return [];
    }
    return [
      {
        severity: "error" as const,
        code: "KDL_JOINT_LIMIT",
        message: `Joint ${limit.name}=${value} is outside [${limit.lower}, ${limit.upper}]`,
        data: {
          joint: limit.name,
          value,
          lower: limit.lower,
          upper: limit.upper
        }
      }
    ];
  });
}

function velocityLimitDiagnostics(model: NormalizedRobotModel, points: TrajectoryPoint[]): MotionDiagnostic[] {
  return points.flatMap((point) =>
    model.limits.flatMap((limit, index) => {
      const diagnostics: MotionDiagnostic[] = [];
      const velocityRatio = limitRatio(point.jointVelocity[index] ?? 0, limit.velocity);
      const accelerationRatio = limitRatio(point.jointAcceleration[index] ?? 0, limit.acceleration);
      if (velocityRatio > 1 + 1e-9) {
        diagnostics.push({
          severity: "error",
          code: "KDL_VELOCITY_LIMIT",
          message: `Point ${point.index} joint ${limit.name} velocity exceeds limit`,
          pointIndex: point.index
        });
      }
      if (accelerationRatio > 1 + 1e-9) {
        diagnostics.push({
          severity: "error",
          code: "KDL_ACCEL_LIMIT",
          message: `Point ${point.index} joint ${limit.name} acceleration exceeds limit`,
          pointIndex: point.index
        });
      }
      return diagnostics;
    })
  );
}

function singularityDiagnostics(model: NormalizedRobotModel, joints: number[]): MotionDiagnostic[] {
  const jacobian = computeJacobian(model, joints);
  const rows = matrixRows(jacobian.data as number[], jacobian.rows, jacobian.cols);
  const linearRows = rows.slice(0, 3);
  const gram = multiplyMatrix(linearRows, transpose(linearRows));
  const manipulability = Math.sqrt(Math.max(0, determinant3(gram)));
  const conditionNumber = estimateConditionNumber(linearRows);
  if (manipulability >= 1e-6 && conditionNumber <= 1e6) {
    return [];
  }
  return [
    {
      severity: "warning",
      code: "KDL_SINGULARITY",
      message: "MOVEJ sample is near singular",
      data: {
        manipulability,
        conditionNumber
      }
    }
  ];
}

function normalizedMoveJVelocity(
  model: NormalizedRobotModel,
  dq: number[],
  speed: SpeedSpec,
  speedOverride = 1
): number {
  const override = validateSpeedOverride(speedOverride);
  if (speed.kind === "joint_percent") {
    return Math.max(1e-9, speed.value * override);
  }

  const requested = speed.kind === "joint_abs" ? speed.velocity : speed.velocity;
  const byRequest = normalizedLimitByScalar(dq, requested * override);
  const byJointLimits = normalizedLimitByJointLimits(model, dq, "velocity");
  return Math.max(1e-9, Math.min(byRequest, byJointLimits));
}

function normalizedMoveJAcceleration(
  model: NormalizedRobotModel,
  dq: number[],
  speed: SpeedSpec,
  speedOverride = 1
): number {
  const override = validateSpeedOverride(speedOverride);
  const requested = speed.kind === "joint_abs" ? speed.acceleration : undefined;
  const byRequest = requested ? normalizedLimitByScalar(dq, requested * override) : Infinity;
  const byJointLimits = normalizedLimitByJointLimits(model, dq, "acceleration");
  return Math.max(1e-9, Math.min(byRequest, byJointLimits, 1));
}

function normalizedLimitByScalar(dq: number[], scalar: number): number {
  if (!Number.isFinite(scalar) || scalar <= 0) {
    throw new KdlStructuredError("KDL_INVALID_SPEED", "MOVEJ speed must be a finite positive number");
  }
  const maxDelta = Math.max(...dq.map((value) => Math.abs(value)), 0);
  if (maxDelta === 0) {
    return scalar;
  }
  return scalar / maxDelta;
}

function normalizedLimitByJointLimits(
  model: NormalizedRobotModel,
  dq: number[],
  field: "velocity" | "acceleration"
): number {
  const ratios = dq.flatMap((delta, index) => {
    const absDelta = Math.abs(delta);
    const limit = model.limits[index]?.[field] ?? Infinity;
    if (absDelta <= 1e-12 || !Number.isFinite(limit)) {
      return [];
    }
    return [limit / absDelta];
  });
  return ratios.length === 0 ? 1 : Math.max(1e-9, Math.min(...ratios));
}

function validateSpeedOverride(speedOverride: number): number {
  if (!Number.isFinite(speedOverride) || speedOverride <= 0) {
    throw new KdlStructuredError("KDL_INVALID_SPEED", "speedOverride must be a finite positive number");
  }
  return speedOverride;
}

function moveJSpeedDiagnostics(speed: SpeedSpec): MotionDiagnostic[] {
  if (speed.kind === "linear") {
    return [
      {
        severity: "warning",
        code: "KDL_MOVEJ_LINEAR_SPEED_APPROX",
        message: "MOVEJ received linear speed; using its velocity as a joint-space scalar"
      }
    ];
  }
  return [];
}

function zoneApproximationDiagnostics(zone: MoveJRequest["zone"]): MotionDiagnostic[] {
  if (zone.kind === "fine") {
    return [];
  }
  return [
    {
      severity: "warning",
      code: "KDL_ZONE_APPROX_FINE",
      message: "Motion zone is approximated as fine in P0"
    }
  ];
}

function moveLSpeedDiagnostics(speed: SpeedSpec): MotionDiagnostic[] {
  if (speed.kind === "linear") {
    return [];
  }
  return [
    {
      severity: "warning",
      code: "KDL_MOVEL_JOINT_SPEED_APPROX",
      message: "MOVEL received joint speed; using its velocity as a linear scalar"
    }
  ];
}

function moveCSpeedDiagnostics(speed: SpeedSpec): MotionDiagnostic[] {
  if (speed.kind === "linear") {
    return [];
  }
  return [
    {
      severity: "warning",
      code: "KDL_MOVEC_JOINT_SPEED_APPROX",
      message: "MOVEC received joint speed; using its velocity as a linear scalar"
    }
  ];
}

function linearSpeedValue(speed: SpeedSpec, speedOverride = 1): number {
  const override = validateSpeedOverride(speedOverride);
  const value = speed.kind === "joint_percent" ? speed.value : speed.velocity;
  if (!Number.isFinite(value) || value <= 0) {
    throw new KdlStructuredError("KDL_INVALID_SPEED", "MOVEL speed must be a finite positive number");
  }
  return Math.max(1e-9, value * override);
}

function linearAccelerationValue(speed: SpeedSpec, speedOverride = 1): number {
  const override = validateSpeedOverride(speedOverride);
  const value = speed.kind === "linear" || speed.kind === "joint_abs" ? speed.acceleration : undefined;
  if (value !== undefined && (!Number.isFinite(value) || value <= 0)) {
    throw new KdlStructuredError("KDL_INVALID_SPEED", "MOVEL acceleration must be a finite positive number");
  }
  return Math.max(1e-9, (value ?? linearSpeedValue(speed, 1)) * override);
}

function moveLFkOptions(request: MoveLRequest): FkOptions {
  const options: FkOptions = {};
  const tool = request.tool ?? request.target.tool;
  const frame = request.frame ?? request.target.frame;
  if (tool) {
    options.tool = tool;
  }
  if (frame) {
    options.frame = frame;
  }
  return options;
}

function moveCFkOptions(request: MoveCRequest): FkOptions {
  const options: FkOptions = {};
  const tool = request.tool ?? request.target.tool;
  const frame = request.frame ?? request.target.frame;
  if (tool) {
    options.tool = tool;
  }
  if (frame) {
    options.frame = frame;
  }
  return options;
}

function interpolateLinearPose(start: Pose, target: Pose, s: number): Pose {
  return {
    position: [
      start.position[0] + (target.position[0] - start.position[0]) * s,
      start.position[1] + (target.position[1] - start.position[1]) * s,
      start.position[2] + (target.position[2] - start.position[2]) * s
    ],
    quaternion: target.quaternion
  };
}

function linearErrorDiagnostics(index: number, actual: Pose, expected: Pose): MotionDiagnostic[] {
  const positionError = distance(actual.position, expected.position);
  if (positionError <= 1e-6) {
    return [];
  }
  return [
    {
      severity: "error",
      code: "KDL_TCP_LINE_ERROR",
      message: `MOVEL TCP line error ${positionError} exceeds tolerance`,
      pointIndex: index,
      data: {
        positionError
      }
    }
  ];
}

interface CircleArc {
  start: [number, number, number];
  center: [number, number, number];
  radius: number;
  normal: [number, number, number];
  startAngle: number;
  angle: number;
  meta: {
    center: [number, number, number];
    radius: number;
    normal: [number, number, number];
    angle: number;
    length: number;
    direction: "cw" | "ccw";
  };
}

function computeCircleArc(
  startPose: Pose,
  viaPose: Pose,
  targetPose: Pose
): { ok: true; meta: CircleArc["meta"]; arc: CircleArc } | { ok: false; diagnostics: MotionDiagnostic[] } {
  const start = startPose.position;
  const via = viaPose.position;
  const target = targetPose.position;
  const a = sub3(via, start);
  const b = sub3(target, start);
  const normalVector = cross3(a, b);
  const normalLength = norm3(normalVector);
  if (distance(start, via) < 1e-9 || distance(via, target) < 1e-9 || distance(start, target) < 1e-9 || normalLength < 1e-9) {
    return {
      ok: false,
      diagnostics: [
        {
          severity: "error",
          code: "KDL_ARC_DEGENERATE",
          message: "MOVEC requires three distinct non-collinear points"
        }
      ]
    };
  }

  const center = circleCenter3(start, via, target, normalVector);
  const radius = distance(center, start);
  if (!Number.isFinite(radius) || radius < 1e-9) {
    return {
      ok: false,
      diagnostics: [
        {
          severity: "error",
          code: "KDL_ARC_DEGENERATE",
          message: "MOVEC arc radius is too small"
        }
      ]
    };
  }

  const normal = normalize3(normalVector);
  const u = normalize3(sub3(start, center));
  const v = cross3(normal, u);
  const startAngle = 0;
  const viaAngle = positiveAngle(pointAngle(center, u, v, via));
  const targetAngle = positiveAngle(pointAngle(center, u, v, target));
  const angle = viaAngle <= targetAngle ? targetAngle : targetAngle + Math.PI * 2;
  const length = radius * angle;
  if (length < 1e-9) {
    return {
      ok: false,
      diagnostics: [
        {
          severity: "error",
          code: "KDL_ARC_DEGENERATE",
          message: "MOVEC arc length is too short"
        }
      ]
    };
  }

  const meta = {
    center,
    radius,
    normal,
    angle,
    length,
    direction: normal[2] >= 0 ? "ccw" as const : "cw" as const
  };
  return {
    ok: true,
    meta,
    arc: {
      start,
      center,
      radius,
      normal,
      startAngle,
      angle,
      meta
    }
  };
}

function sampleCirclePose(
  arcResult: { arc: CircleArc },
  s: number,
  quaternion: [number, number, number, number]
): Pose {
  const arc = arcResult.arc;
  const u = normalize3(sub3(arc.start, arc.center));
  const v = cross3(arc.normal, u);
  const angle = arc.startAngle + arc.angle * s;
  return {
    position: add3(
      arc.center,
      add3(scale3(u, Math.cos(angle) * arc.radius), scale3(v, Math.sin(angle) * arc.radius))
    ),
    quaternion
  };
}

function arcErrorDiagnostics(index: number, arcError: number): MotionDiagnostic[] {
  if (arcError <= 1e-6) {
    return [];
  }
  return [
    {
      severity: "error",
      code: "KDL_ARC_ERROR",
      message: `MOVEC arc error ${arcError} exceeds tolerance`,
      pointIndex: index,
      data: {
        arcError
      }
    }
  ];
}

function circleCenter3(
  p1: [number, number, number],
  p2: [number, number, number],
  p3: [number, number, number],
  normal: [number, number, number]
): [number, number, number] {
  const a = sub3(p2, p1);
  const b = sub3(p3, p1);
  const a2 = dot3(a, a);
  const b2 = dot3(b, b);
  const cross = cross3(a, b);
  const denom = 2 * dot3(cross, cross);
  if (Math.abs(denom) < 1e-18) {
    return [NaN, NaN, NaN];
  }
  const termA = scale3(cross3(b, normal), a2);
  const termB = scale3(cross3(normal, a), b2);
  return add3(p1, scale3(add3(termA, termB), 1 / denom));
}

function pointAngle(
  center: [number, number, number],
  u: [number, number, number],
  v: [number, number, number],
  point: [number, number, number]
): number {
  const radial = sub3(point, center);
  return Math.atan2(dot3(radial, v), dot3(radial, u));
}

function positiveAngle(angle: number): number {
  const twoPi = Math.PI * 2;
  let result = angle % twoPi;
  if (result < 0) {
    result += twoPi;
  }
  return result;
}

function add3(a: [number, number, number], b: [number, number, number]): [number, number, number] {
  return [a[0] + b[0], a[1] + b[1], a[2] + b[2]];
}

function sub3(a: [number, number, number], b: [number, number, number]): [number, number, number] {
  return [a[0] - b[0], a[1] - b[1], a[2] - b[2]];
}

function scale3(a: [number, number, number], scale: number): [number, number, number] {
  return [a[0] * scale, a[1] * scale, a[2] * scale];
}

function dot3(a: [number, number, number], b: [number, number, number]): number {
  return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}

function cross3(a: [number, number, number], b: [number, number, number]): [number, number, number] {
  return [
    a[1] * b[2] - a[2] * b[1],
    a[2] * b[0] - a[0] * b[2],
    a[0] * b[1] - a[1] * b[0]
  ];
}

function norm3(a: [number, number, number]): number {
  return Math.hypot(a[0], a[1], a[2]);
}

function normalize3(a: [number, number, number]): [number, number, number] {
  const length = norm3(a);
  if (length === 0) {
    return [0, 0, 0];
  }
  return [a[0] / length, a[1] / length, a[2] / length];
}

function moveJTargetFkOptions(request: MoveJRequest): FkOptions {
  const target = isPoseTarget(request.target) ? request.target : undefined;
  const options: FkOptions = {};
  const tool = request.tool ?? target?.tool;
  const frame = request.frame ?? target?.frame;
  if (tool) {
    options.tool = tool;
  }
  if (frame) {
    options.frame = frame;
  }
  return options;
}

function targetIdOf(target: JointTarget | PoseTarget): string | undefined {
  return target.id;
}

function isJointTarget(target: JointTarget | PoseTarget): target is JointTarget {
  return "joints" in target;
}

function isPoseTarget(target: JointTarget | PoseTarget): target is PoseTarget {
  return "pose" in target;
}

function limitRatio(value: number, limit: number): number {
  if (!Number.isFinite(limit) || limit <= 0) {
    return 0;
  }
  return Math.abs(value) / limit;
}

function computeLinkPoses(
  model: NormalizedRobotModel,
  joints: number[]
): Array<{ link: string; pose: Pose }> {
  const poses = new Map<string, Mat4>();
  poses.set(model.baseLink, identityMat4());
  const activeJointIndex = new Map(model.activeJointNames.map((name, index) => [name, index]));

  let progressed = true;
  while (progressed) {
    progressed = false;
    for (const joint of model.joints) {
      if (!poses.has(joint.parent) || poses.has(joint.child)) {
        continue;
      }

      const parentPose = poses.get(joint.parent)!;
      const origin = multiplyMat4(translationMat4(joint.origin.xyz), rotationFromRpyMat4(joint.origin.rpy));
      const activeIndex = activeJointIndex.get(joint.name);
      const jointValue = activeIndex === undefined ? 0 : joints[activeIndex]!;
      const motion = jointMotionMat4(joint.type, joint.axis, jointValue);
      poses.set(joint.child, multiplyMat4(parentPose, multiplyMat4(origin, motion)));
      progressed = true;
    }
  }

  const orderedLinks = chainLinkOrder(model);
  return orderedLinks.map((link) => ({
    link,
    pose: mat4ToPose(poses.get(link) ?? identityMat4())
  }));
}

function computeJacobian(model: NormalizedRobotModel, joints: number[]): JacobianResult {
  const basePose = computeLinkPoses(model, joints).at(-1)?.pose ?? identityPose();
  const rows = 6;
  const cols = joints.length;
  const data = new Array(rows * cols).fill(0);
  const epsilon = 1e-6;

  for (let col = 0; col < cols; col += 1) {
    const perturbed = [...joints];
    perturbed[col] = (perturbed[col] ?? 0) + epsilon;
    const pose = computeLinkPoses(model, perturbed).at(-1)?.pose ?? identityPose();
    data[col] = (pose.position[0] - basePose.position[0]) / epsilon;
    data[cols + col] = (pose.position[1] - basePose.position[1]) / epsilon;
    data[cols * 2 + col] = (pose.position[2] - basePose.position[2]) / epsilon;
    data[cols * 3 + col] = (pose.quaternion[0] - basePose.quaternion[0]) / epsilon;
    data[cols * 4 + col] = (pose.quaternion[1] - basePose.quaternion[1]) / epsilon;
    data[cols * 5 + col] = (pose.quaternion[2] - basePose.quaternion[2]) / epsilon;
  }

  return {
    ok: true,
    rows,
    cols,
    data,
    diagnostics: []
  };
}

function solveIk(
  model: NormalizedRobotModel,
  seed: number[],
  target: Pose,
  options: IkOptions
): IkResult {
  const chainJoints = model.joints.filter((joint) => model.activeJointNames.includes(joint.name));

  if (chainJoints.length === 1 && chainJoints[0]?.type === "prismatic") {
    return solveSinglePrismaticIk(model, chainJoints[0], seed, target, options);
  }

  if (
    chainJoints.length === 2 &&
    (chainJoints[0]?.type === "revolute" || chainJoints[0]?.type === "continuous") &&
    chainJoints[1]?.type === "prismatic" &&
    isAxis(chainJoints[0].axis, [0, 0, 1]) &&
    isAxis(chainJoints[1].axis, [1, 0, 0])
  ) {
    return solvePlanarRzPxIk(model, seed, target, options);
  }

  return {
    ok: false,
    iterations: 0,
    reason: "invalid_model",
    diagnostics: [
      {
        severity: "error",
        code: "KDL_IK_UNSUPPORTED_MODEL",
        message: "Current TypeScript IK baseline supports only single-prismatic or Rz+Px chains"
      }
    ]
  };
}

function solveSinglePrismaticIk(
  model: NormalizedRobotModel,
  joint: NormalizedRobotModel["joints"][number],
  seed: number[],
  target: Pose,
  options: IkOptions
): IkResult {
  const zero = computeLinkPoses(model, [0]).at(-1)?.pose.position ?? [0, 0, 0];
  const axisIndex = dominantAxisIndex(joint.axis);
  const q = target.position[axisIndex]! - zero[axisIndex]!;
  return finishIkCandidate(model, seed, [q], target, options);
}

function solvePlanarRzPxIk(
  model: NormalizedRobotModel,
  seed: number[],
  target: Pose,
  options: IkOptions
): IkResult {
  const [x, y] = target.position;
  const q2 = Math.hypot(x, y);
  const q1Candidates = [Math.atan2(y, x), Math.atan2(y, x) + Math.PI];
  const candidates = q1Candidates.map((q1, index) => [normalizeAngleNear(q1, seed[0] ?? 0), index === 0 ? q2 : -q2]);

  let best: IkResult | undefined;
  for (const candidate of candidates) {
    const result = finishIkCandidate(model, seed, candidate, target, options);
    if (result.ok) {
      return result;
    }
    if (!best || (result.residualPosition ?? Infinity) < (best.residualPosition ?? Infinity)) {
      best = result;
    }
  }

  return best ?? {
    ok: false,
    iterations: 1,
    reason: "unreachable",
    diagnostics: [
      {
        severity: "error",
        code: "KDL_TARGET_UNREACHABLE",
        message: "No IK candidate found"
      }
    ]
  };
}

function finishIkCandidate(
  model: NormalizedRobotModel,
  seed: number[],
  candidate: number[],
  target: Pose,
  options: IkOptions
): IkResult {
  const limited = applyOptionLimits(model, candidate, options);
  if (!limited.ok) {
    return limited.result;
  }

  const fkPose = computeLinkPoses(model, candidate).at(-1)?.pose ?? identityPose();
  const residualPosition = distance(fkPose.position, target.position);
  const tolerance = options.positionTolerance ?? 1e-6;
  const baseResult = {
    joints: candidate,
    iterations: 1,
    residualPosition,
    residualOrientation: quaternionDistance(fkPose.quaternion, target.quaternion),
    diagnostics: []
  };

  if (residualPosition <= tolerance || options.allowApproximate) {
    return {
      ok: true,
      ...baseResult
    };
  }

  return {
    ok: false,
    ...baseResult,
    reason: "unreachable",
    diagnostics: [
      {
        severity: "error",
        code: "KDL_TARGET_UNREACHABLE",
        message: `IK residual ${residualPosition} exceeds tolerance ${tolerance}`
      }
    ]
  };
}

function applyOptionLimits(
  model: NormalizedRobotModel,
  candidate: number[],
  options: IkOptions
): { ok: true } | { ok: false; result: IkResult } {
  for (let index = 0; index < candidate.length; index += 1) {
    const value = candidate[index]!;
    const limits = model.limits[index];
    const lower = options.qMin?.[index] ?? limits?.lower ?? -Infinity;
    const upper = options.qMax?.[index] ?? limits?.upper ?? Infinity;
    if (value < lower || value > upper) {
      return {
        ok: false,
        result: {
          ok: false,
          iterations: 1,
          joints: candidate,
          reason: "joint_limit",
          diagnostics: [
            {
              severity: "error",
              code: "KDL_JOINT_LIMIT",
              message: `IK candidate joint ${index}=${value} is outside [${lower}, ${upper}]`
            }
          ]
        }
      };
    }
  }
  return { ok: true };
}

function chainLinkOrder(model: NormalizedRobotModel): string[] {
  const order = [model.baseLink];
  let current = model.baseLink;
  const visited = new Set<string>([current]);

  while (current !== model.tipLink) {
    const nextJoint = model.joints.find((joint) => joint.parent === current && !visited.has(joint.child));
    if (!nextJoint) {
      break;
    }
    current = nextJoint.child;
    visited.add(current);
    order.push(current);
  }

  return order;
}

function composeFkPose(a: Pose, b: Pose): Pose {
  return mat4ToPose(multiplyMat4(poseToMat4(a), poseToMat4(b)));
}

function identityPose(): Pose {
  return {
    position: [0, 0, 0],
    quaternion: [0, 0, 0, 1]
  };
}

function isAxis(actual: [number, number, number], expected: [number, number, number]): boolean {
  return actual.every((value, index) => Math.abs(value - expected[index]!) < 1e-9);
}

function dominantAxisIndex(axis: [number, number, number]): 0 | 1 | 2 {
  const absolute = axis.map((value) => Math.abs(value));
  if (absolute[1]! > absolute[0]! && absolute[1]! >= absolute[2]!) {
    return 1;
  }
  if (absolute[2]! > absolute[0]! && absolute[2]! > absolute[1]!) {
    return 2;
  }
  return 0;
}

function normalizeAngleNear(angle: number, seed: number): number {
  const twoPi = Math.PI * 2;
  let normalized = angle;
  while (normalized - seed > Math.PI) {
    normalized -= twoPi;
  }
  while (normalized - seed < -Math.PI) {
    normalized += twoPi;
  }
  return normalized;
}

function distance(a: [number, number, number], b: [number, number, number]): number {
  return Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2]);
}

function quaternionDistance(a: [number, number, number, number], b: [number, number, number, number]): number {
  return Math.min(
    Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2], a[3] - b[3]),
    Math.hypot(a[0] + b[0], a[1] + b[1], a[2] + b[2], a[3] + b[3])
  );
}

function matrixRows(data: number[], rows: number, cols: number): number[][] {
  return Array.from({ length: rows }, (_, row) =>
    Array.from({ length: cols }, (_, col) => data[row * cols + col] ?? 0)
  );
}

function transpose(matrix: number[][]): number[][] {
  const rows = matrix.length;
  const cols = matrix[0]?.length ?? 0;
  return Array.from({ length: cols }, (_, col) => Array.from({ length: rows }, (_, row) => matrix[row]?.[col] ?? 0));
}

function multiplyMatrix(a: number[][], b: number[][]): number[][] {
  const rows = a.length;
  const cols = b[0]?.length ?? 0;
  const inner = b.length;
  return Array.from({ length: rows }, (_, row) =>
    Array.from({ length: cols }, (_, col) => {
      let sum = 0;
      for (let index = 0; index < inner; index += 1) {
        sum += (a[row]?.[index] ?? 0) * (b[index]?.[col] ?? 0);
      }
      return sum;
    })
  );
}

function determinant3(matrix: number[][]): number {
  const a = matrix[0]?.[0] ?? 0;
  const b = matrix[0]?.[1] ?? 0;
  const c = matrix[0]?.[2] ?? 0;
  const d = matrix[1]?.[0] ?? 0;
  const e = matrix[1]?.[1] ?? 0;
  const f = matrix[1]?.[2] ?? 0;
  const g = matrix[2]?.[0] ?? 0;
  const h = matrix[2]?.[1] ?? 0;
  const i = matrix[2]?.[2] ?? 0;
  return a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g);
}

function estimateConditionNumber(matrix: number[][]): number {
  const columnNorms = transpose(matrix).map((column) => Math.hypot(...column));
  const nonZero = columnNorms.filter((value) => value > 1e-12);
  if (nonZero.length === 0) {
    return Infinity;
  }
  return Math.max(...nonZero) / Math.min(...nonZero);
}
