import { KdlStructuredError } from "./rpc.js";
import {
  composePose as composePoseMath,
  inversePose as inversePoseMath,
  normalizeQuaternion,
  rpyToQuaternion
} from "../math/poseMath.js";
import type { OffsetSpec, Pose, PoseLike, PoseTarget } from "./types.js";

export function normalizePose(input: PoseLike): Pose {
  if (!input || typeof input !== "object") {
    throw new KdlStructuredError("KDL_INVALID_POSE", "Pose input must be an object");
  }

  if ("position" in input && "quaternion" in input) {
    return {
      position: validateVector3(input.position, "position"),
      quaternion: normalizeQuaternion(validateVector4(input.quaternion, "quaternion"))
    };
  }

  if ("xyz" in input && "rpy" in input) {
    return {
      position: validateVector3(input.xyz, "xyz"),
      quaternion: rpyToQuaternion(validateVector3(input.rpy, "rpy"))
    };
  }

  if ("xyz" in input && "quat" in input) {
    return {
      position: validateVector3(input.xyz, "xyz"),
      quaternion: normalizeQuaternion(validateVector4(input.quat, "quat"))
    };
  }

  throw new KdlStructuredError("KDL_INVALID_POSE", "Pose input must contain position/quaternion, xyz/rpy, or xyz/quat");
}

export function composePose(a: Pose, b: Pose): Pose {
  return composePoseMath(normalizePose(a), normalizePose(b));
}

export function inversePose(pose: Pose): Pose {
  return inversePoseMath(normalizePose(pose));
}

export function applyToolAndFrame(target: PoseTarget, tool: Pose, frame: Pose): Pose {
  return composePose(composePose(normalizePose(frame), normalizePose(target.pose)), normalizePose(tool));
}

export function applyOffset(target: PoseTarget, offset: OffsetSpec): PoseTarget {
  const offsetPose = offsetToPose(offset);
  const pose = offset.mode === "tool" ? composePose(target.pose, offsetPose) : composePose(offsetPose, target.pose);

  return {
    ...target,
    pose
  };
}

function offsetToPose(offset: OffsetSpec): Pose {
  const xyz: [number, number, number] = offset.xyz ? validateVector3(offset.xyz, "offset.xyz") : [0, 0, 0];
  if (offset.rpy && offset.quaternion) {
    throw new KdlStructuredError("KDL_INVALID_OFFSET", "Offset cannot specify both rpy and quaternion");
  }

  if (offset.rpy) {
    return {
      position: xyz,
      quaternion: rpyToQuaternion(validateVector3(offset.rpy, "offset.rpy"))
    };
  }

  if (offset.quaternion) {
    return {
      position: xyz,
      quaternion: normalizeQuaternion(validateVector4(offset.quaternion, "offset.quaternion"))
    };
  }

  return {
    position: xyz,
    quaternion: [0, 0, 0, 1]
  };
}

function validateVector3(value: unknown, field: string): [number, number, number] {
  if (!Array.isArray(value) || value.length !== 3 || value.some((entry) => !Number.isFinite(entry))) {
    throw new KdlStructuredError("KDL_INVALID_POSE", `${field} must contain 3 finite numbers`);
  }
  return [value[0]!, value[1]!, value[2]!];
}

function validateVector4(value: unknown, field: string): [number, number, number, number] {
  if (!Array.isArray(value) || value.length !== 4 || value.some((entry) => !Number.isFinite(entry))) {
    throw new KdlStructuredError("KDL_INVALID_POSE", `${field} must contain 4 finite numbers`);
  }
  return [value[0]!, value[1]!, value[2]!, value[3]!];
}
