// IK — precise character actions as late-phase aspects on BONES (docs/animation-plan.md §2.5).
// They run after the animation wrote the pose (base anim or Animator) and before the skin is
// flushed, so their bone writes land in the skin. Attach to the END bone of the chain; the chain is
// walked up through parents — one instance per end effector, any Node hierarchy works.
//
//   const foot = hero.bone('LeftFoot')!
//   foot.aspect(IK.TwoBone, { target: footTarget, pole: kneeHint })     // upLeg → leg → foot
//   foot.ik.weight = grounded ? 1 : 0                                    // blend in/out
//   hero.bone('Head')!.aspect(IK.LookAt, { target: camera, limit: 70 })  // head tracks the camera
//
// Pure SDK/JS: a handful of world-matrix reads + two quaternion writes per solve. A native solver can
// slot in later behind the same API.

import { Aspect } from "../core/Aspect"
import { Vec3, type Vec3Like } from "../math/vec"
import { Quat } from "../math/quat"
import type { Node } from "./Node"

type Target = Node | Vec3Like

const targetPos = (t: Target | undefined): Vec3 | null => {
  if (t === undefined || t === null) return null
  const n = t as Node
  if (typeof (n as { worldPosition?: unknown }).worldPosition === "object" && typeof n.id === "number") return n.worldPosition
  return new Vec3(t as Vec3Like)
}

/** World rotation of a node (parent chain composed natively). */
const worldRot = (n: Node): Quat => n.worldMatrix.rotation

/** Write a WORLD rotation to a node by converting through the parent's world rotation. */
const setWorldRot = (n: Node, q: Quat): void => {
  const p = n.parent
  n.quaternion = p ? worldRot(p).invert().mul(q).normalize() : q.normalize()
}

const clampAngle = (q: Quat, maxRad: number): Quat => {
  // angle of a unit quaternion = 2·acos(|w|)
  const w = Math.min(1, Math.abs(q.w))
  const ang = 2 * Math.acos(w)
  if (ang <= maxRad || ang < 1e-6) return q
  const s = Math.sqrt(1 - w * w)
  const axis = new Vec3(q.x / s, q.y / s, q.z / s)
  return Quat.fromAxisAngle(axis, q.w < 0 ? -maxRad : maxRad)
}

/** Two-bone analytic IK (limbs). Attach to the END bone: `foot.aspect(IK.TwoBone, { target })`
 *  solves upper (grandparent) + mid (parent) so the end reaches `target`; `pole` steers the bend
 *  (knee/elbow) — a Node or world point. */
class TwoBone extends Aspect<"ik", Node> {
  static readonly aspect = "ik"

  /** Where the end bone should be (Node or world position). */
  target?: Target
  /** Bend hint — the mid joint is pulled toward it (Node or world position). */
  pole?: Target
  /** World rotation the END bone takes after the solve — a Node (its world rotation) or a Quat.
   *  Unset = the end bone keeps its animated local rotation (a hand on a grip, a foot on a slope
   *  want it set). Blended by `weight · rotationWeight`. */
  rotation?: Node | Quat
  /** 0–1 contribution of `rotation` (on top of `weight`). */
  rotationWeight = 1
  /** 0–1 contribution (blend in/out, e.g. foot planting only while grounded). */
  weight = 1
  /** Solve every frame (default). Set false to drive `solve()` yourself. */
  enabled = true

  update(): void {
    if (this.enabled) this.solve()
  }

  /** One solve at the current pose. Safe to call manually (e.g. from a later-ordered aspect). */
  solve(): void {
    const t = targetPos(this.target)
    if (!t || this.weight <= 0) return
    const end = this.node
    const mid = end.parent
    const root = mid?.parent
    if (!mid || !root) return

    const w = Math.min(1, this.weight)
    const midLocal0 = mid.quaternion
    const rootLocal0 = root.quaternion

    const a = root.worldPosition, b = mid.worldPosition, c = end.worldPosition
    const l1 = a.distanceTo(b), l2 = b.distanceTo(c)
    if (l1 < 1e-6 || l2 < 1e-6) return
    const eps = 1e-4
    const toT = t.sub(a)
    const d = Math.min(Math.max(toT.length(), eps), l1 + l2 - eps)

    // 1. bend the mid joint to the angle the target distance demands (law of cosines)
    const ba = a.sub(b), bc = c.sub(b)
    const cur = ba.angle(bc)
    const want = Math.acos(Math.min(1, Math.max(-1, (l1 * l1 + l2 * l2 - d * d) / (2 * l1 * l2))))
    // a positive rotation about ba × bc moves `bc` AWAY from `ba`, i.e. opens the joint — so rotating the
    // mid bone by (want − cur) about it opens/closes exactly as needed. (bc × ba has the opposite sense:
    // with it every already-bent limb — which is every animated limb — bends the wrong way; the straight
    // case below picks its axis as ba × ref, the same sense as ba × bc for a limb barely bent toward ref.)
    let axis = ba.cross(bc)
    if (axis.lengthSq() < 1e-10) {
      // straight limb: bend toward the pole (or any perpendicular)
      const p = targetPos(this.pole)
      const ref = p ? p.sub(b) : new Vec3(0, 0, 1)
      axis = ba.cross(ref)
      if (axis.lengthSq() < 1e-10) axis = ba.cross(new Vec3(0, 1, 0))
      if (axis.lengthSq() < 1e-10) axis = ba.cross(new Vec3(1, 0, 0))
    }
    axis = axis.normalize()
    // rotating mid by (want - cur) about `axis` opens/closes the angle between ba and bc
    const midWorld = worldRot(mid)
    setWorldRot(mid, Quat.fromAxisAngle(axis, want - cur).mul(midWorld))

    // 2. swing the root so the end lands on the target direction
    const c2 = end.worldPosition
    const rootWorld = worldRot(root)
    setWorldRot(root, Quat.fromTo(c2.sub(a), toT).mul(rootWorld))

    // 3. pole: spin the root about the root→target axis so the mid joint lies toward the pole
    const p = targetPos(this.pole)
    if (p) {
      const dir = toT.normalize()
      const b2 = mid.worldPosition
      const midProj = b2.sub(a).sub(dir.scale(b2.sub(a).dot(dir)))
      const poleProj = p.sub(a).sub(dir.scale(p.sub(a).dot(dir)))
      if (midProj.lengthSq() > 1e-10 && poleProj.lengthSq() > 1e-10) {
        setWorldRot(root, Quat.fromTo(midProj, poleProj).mul(worldRot(root)))
      }
    }

    // 4. weight: blend from the animated pose to the solved one
    if (w < 1) {
      root.quaternion = rootLocal0.slerp(root.quaternion, w)
      mid.quaternion = midLocal0.slerp(mid.quaternion, w)
    }

    // 5. end-bone rotation: take the target's world rotation (a hand stays on its grip, a foot
    // flat on the slope) — the solve above only placed the end's ORIGIN
    const r = this.rotation
    if (r !== undefined && r !== null) {
      const rw = w * Math.min(1, Math.max(0, this.rotationWeight))
      if (rw > 0) {
        const want = (r as Node).id !== undefined && typeof (r as Node).worldMatrix === "object" ? worldRot(r as Node) : (r as Quat)
        const endLocal0 = end.quaternion
        setWorldRot(end, want)
        if (rw < 1) end.quaternion = endLocal0.slerp(end.quaternion, rw)
      }
    }
  }
}

/** Aim a bone at a target (head / eyes / turret). Attach to the bone itself:
 *  `head.aspect(IK.LookAt, { target: camera, limit: 70 })`. `axis` is the bone's LOCAL forward
 *  (the direction that should point at the target) — rigs differ; default −Z, Mixamo heads look
 *  along +Z of the head bone in most exports, so pass `axis: [0, 0, 1]` there if it faces backwards. */
class LookAt extends Aspect<"lookAt", Node> {
  static readonly aspect = "lookAt"

  target?: Target
  /** The bone's local axis that should point at the target. */
  axis: Vec3Like = [ 0, 0, -1 ]
  /** Max deflection from the animated direction, in degrees (default 80). */
  limit = 80
  /** 0–1 contribution. */
  weight = 1
  enabled = true

  update(): void {
    if (this.enabled) this.solve()
  }

  solve(): void {
    const t = targetPos(this.target)
    if (!t || this.weight <= 0) return
    const bone = this.node
    const w = Math.min(1, this.weight)
    const local0 = bone.quaternion
    const from = bone.worldPosition
    const dir = t.sub(from)
    if (dir.lengthSq() < 1e-10) return
    const rot = worldRot(bone)
    const current = new Vec3(this.axis).rotate(rot)
    let delta = Quat.fromTo(current, dir)
    delta = clampAngle(delta, (this.limit * Math.PI) / 180)
    setWorldRot(bone, delta.mul(rot))
    if (w < 1) bone.quaternion = local0.slerp(bone.quaternion, w)
  }
}

/** Inverse kinematics aspects — attach to bones (see file header). */
export const IK = { TwoBone, LookAt }
export type IKTwoBone = TwoBone
export type IKLookAt = LookAt
