import { createHash } from "node:crypto";
import { XMLParser } from "fast-xml-parser";
import { KdlStructuredError } from "../kdl/rpc.js";
import type {
  JointLimitOverride,
  JointLimits,
  JointModel,
  JointType,
  LinkModel,
  NormalizedRobotModel,
  UrdfLoadOptions
} from "../kdl/types.js";

type XmlNode = Record<string, unknown>;

const SUPPORTED_JOINT_TYPES = new Set<JointType>(["revolute", "continuous", "prismatic", "fixed"]);

export function loadRobotFromUrdfModel(urdfXml: string, options: UrdfLoadOptions): NormalizedRobotModel {
  const robot = parseUrdfRoot(urdfXml);
  const robotName = stringAttr(robot["@_name"]) ?? options.robotId;
  const links = asArray<XmlNode>(robot.link).map(parseLink);
  const joints = asArray<XmlNode>(robot.joint).map(parseJoint);

  const linkNames = new Set(links.map((link) => link.name));
  if (!linkNames.has(options.baseLink)) {
    throw invalidModel(`URDF baseLink does not exist: ${options.baseLink}`);
  }
  if (!linkNames.has(options.tipLink)) {
    throw invalidModel(`URDF tipLink does not exist: ${options.tipLink}`);
  }

  const chain = buildChain(joints, options.baseLink, options.tipLink);
  const activeJointNames = resolveActiveJointOrder(chain, options.jointOrder);
  const limits = activeJointNames.map((name) => {
    const joint = joints.find((candidate) => candidate.name === name);
    if (!joint) {
      throw invalidModel(`Joint order references an unknown joint: ${name}`);
    }
    return limitForJoint(joint, options.overrideLimits ?? []);
  });

  return {
    robotId: options.robotId,
    name: robotName,
    baseLink: options.baseLink,
    tipLink: options.tipLink,
    links,
    joints,
    activeJointNames,
    limits,
    source: {
      type: "urdf",
      urdfHash: createHash("sha256").update(urdfXml).digest("hex")
    }
  };
}

function parseUrdfRoot(urdfXml: string): XmlNode {
  const parser = new XMLParser({
    ignoreAttributes: false,
    attributeNamePrefix: "@_",
    trimValues: true,
    parseAttributeValue: false,
    parseTagValue: false,
    allowBooleanAttributes: true
  });
  const parsed = parser.parse(urdfXml) as XmlNode;
  const robot = parsed.robot;

  if (!isObject(robot)) {
    throw invalidModel("URDF document must contain a robot root element");
  }

  return robot;
}

function parseLink(node: XmlNode): LinkModel {
  const name = stringAttr(node["@_name"]);
  if (!name) {
    throw invalidModel("URDF link is missing name");
  }
  return { name };
}

function parseJoint(node: XmlNode): JointModel {
  const name = stringAttr(node["@_name"]);
  const type = stringAttr(node["@_type"]);
  if (!name || !type) {
    throw invalidModel("URDF joint is missing name or type");
  }
  if (!SUPPORTED_JOINT_TYPES.has(type as JointType)) {
    throw invalidModel(`Unsupported joint type for ${name}: ${type}`);
  }

  const parent = parseLinkRef(node.parent, "parent", name);
  const child = parseLinkRef(node.child, "child", name);
  const originNode = isObject(node.origin) ? node.origin : {};
  const axisNode = isObject(node.axis) ? node.axis : {};
  const jointType = type as JointType;

  return {
    name,
    type: jointType,
    parent,
    child,
    origin: {
      xyz: parseTriple(stringAttr(originNode["@_xyz"]), [0, 0, 0], `joint ${name} origin xyz`),
      rpy: parseTriple(stringAttr(originNode["@_rpy"]), [0, 0, 0], `joint ${name} origin rpy`)
    },
    axis: parseTriple(stringAttr(axisNode["@_xyz"]), [1, 0, 0], `joint ${name} axis xyz`),
    ...(jointType === "fixed" ? {} : { limit: parseJointLimit(node.limit, name, jointType) })
  };
}

function parseJointLimit(node: unknown, jointName: string, jointType: JointType): JointLimits {
  const limitNode = isObject(node) ? node : {};
  const continuous = jointType === "continuous";
  const lower = continuous ? -Infinity : numberAttr(limitNode["@_lower"], `joint ${jointName} lower limit`);
  const upper = continuous ? Infinity : numberAttr(limitNode["@_upper"], `joint ${jointName} upper limit`);

  return {
    name: jointName,
    lower,
    upper,
    velocity: optionalNumberAttr(limitNode["@_velocity"], `joint ${jointName} velocity limit`) ?? Infinity,
    acceleration: optionalNumberAttr(limitNode["@_acceleration"], `joint ${jointName} acceleration limit`) ?? Infinity,
    ...optionalJerk(limitNode, jointName)
  };
}

function parseLinkRef(node: unknown, field: "parent" | "child", jointName: string): string {
  if (!isObject(node)) {
    throw invalidModel(`URDF joint ${jointName} is missing ${field}`);
  }
  const link = stringAttr(node["@_link"]);
  if (!link) {
    throw invalidModel(`URDF joint ${jointName} ${field} is missing link`);
  }
  return link;
}

function buildChain(joints: JointModel[], baseLink: string, tipLink: string): JointModel[] {
  const byParent = new Map<string, JointModel[]>();
  for (const joint of joints) {
    const children = byParent.get(joint.parent) ?? [];
    children.push(joint);
    byParent.set(joint.parent, children);
  }

  const queue: Array<{ link: string; chain: JointModel[] }> = [{ link: baseLink, chain: [] }];
  const visited = new Set<string>([baseLink]);

  while (queue.length > 0) {
    const current = queue.shift();
    if (!current) {
      break;
    }
    if (current.link === tipLink) {
      return current.chain;
    }

    for (const joint of byParent.get(current.link) ?? []) {
      if (visited.has(joint.child)) {
        continue;
      }
      visited.add(joint.child);
      queue.push({ link: joint.child, chain: [...current.chain, joint] });
    }
  }

  throw invalidModel(`URDF baseLink ${baseLink} is not connected to tipLink ${tipLink}`);
}

function resolveActiveJointOrder(chain: JointModel[], jointOrder?: string[]): string[] {
  const defaultOrder = chain
    .filter((joint) => joint.type !== "fixed")
    .map((joint) => joint.name);

  if (!jointOrder || jointOrder.length === 0) {
    return defaultOrder;
  }

  const chainActive = new Set(defaultOrder);
  for (const jointName of jointOrder) {
    if (!chainActive.has(jointName)) {
      throw invalidModel(`jointOrder contains a joint outside the base-tip chain: ${jointName}`);
    }
  }

  if (jointOrder.length !== defaultOrder.length) {
    throw invalidModel("jointOrder must contain every active joint in the base-tip chain exactly once");
  }

  return [...jointOrder];
}

function limitForJoint(joint: JointModel, overrides: JointLimitOverride[]): JointLimits {
  const base = joint.limit;
  if (!base) {
    throw invalidModel(`Active joint ${joint.name} is missing limits`);
  }
  const override = overrides.find((candidate) => candidate.name === joint.name);
  if (!override) {
    return base;
  }

  return {
    name: joint.name,
    lower: override.lower ?? base.lower,
    upper: override.upper ?? base.upper,
    velocity: override.velocity ?? base.velocity,
    acceleration: override.acceleration ?? base.acceleration,
    ...mergedOptionalJerk(override, base)
  };
}

function optionalJerk(limitNode: XmlNode, jointName: string): Pick<JointLimits, "jerk"> | Record<string, never> {
  const jerk = optionalNumberAttr(limitNode["@_jerk"], `joint ${jointName} jerk limit`);
  return jerk === undefined ? {} : { jerk };
}

function mergedOptionalJerk(
  override: JointLimitOverride,
  base: JointLimits
): Pick<JointLimits, "jerk"> | Record<string, never> {
  const jerk = override.jerk ?? base.jerk;
  return jerk === undefined ? {} : { jerk };
}

function parseTriple(value: string | undefined, fallback: [number, number, number], context: string): [number, number, number] {
  if (!value) {
    return fallback;
  }
  const parts = value.trim().split(/\s+/).map((part) => Number(part));
  if (parts.length !== 3 || parts.some((part) => !Number.isFinite(part))) {
    throw invalidModel(`Invalid ${context}: ${value}`);
  }
  return [parts[0] ?? 0, parts[1] ?? 0, parts[2] ?? 0];
}

function numberAttr(value: unknown, context: string): number {
  const numberValue = optionalNumberAttr(value, context);
  if (numberValue === undefined) {
    throw invalidModel(`Missing ${context}`);
  }
  return numberValue;
}

function optionalNumberAttr(value: unknown, context: string): number | undefined {
  if (value === undefined || value === null || value === "") {
    return undefined;
  }
  const numberValue = Number(value);
  if (!Number.isFinite(numberValue)) {
    throw invalidModel(`Invalid ${context}: ${String(value)}`);
  }
  return numberValue;
}

function stringAttr(value: unknown): string | undefined {
  return typeof value === "string" && value.length > 0 ? value : undefined;
}

function asArray<T>(value: unknown): T[] {
  if (value === undefined || value === null) {
    return [];
  }
  return Array.isArray(value) ? (value as T[]) : [value as T];
}

function isObject(value: unknown): value is XmlNode {
  return typeof value === "object" && value !== null && !Array.isArray(value);
}

function invalidModel(message: string): KdlStructuredError {
  return new KdlStructuredError("KDL_INVALID_MODEL", message);
}
