// Fluent quaternion (x, y, z, w) — the rotation type for the 3D node API. Same contract as Vec: mutable
// fields, PURE methods (each returns a new Quat). Accepts a `Quat` or a raw `[x, y, z, w]` everywhere.
//
// Convention: standard right-handed, column-major (matches gl-matrix and three.js). Euler angles are in
// DEGREES. The euler order default is "YXZ" (yaw-pitch-roll, the usual choice for cameras / characters).
//
// NB: this fixes a transpose bug in the old `transform.ts` euler extraction, which read the matrix with
// row/col swapped and so returned a NEGATED angle for a positive rotation. `eulerFromColMajor` below uses
// the correct column-major element mapping, so `node.eulerAngles` now round-trips with the setter.

import { Vec3, cx, cy, cz, type Vec3Like } from "./vec"

export type QuatLike = Quat | readonly [number, number, number, number] | number[] | Float32Array
export type EulerOrder = "XYZ" | "YXZ" | "ZXY" | "ZYX" | "YZX" | "XZY"

/** Read the w / 4th component of any quaternion-like (wrapper `.w`, or array-like `[3]`). */
export const cw = (q: { w?: number } | ArrayLike<number>): number => (q as { w?: number }).w ?? (q as ArrayLike<number>)[3]
const DEG2RAD = Math.PI / 180
const RAD2DEG = 180 / Math.PI
const EPS = 1e-6
const approx = (a: number, b: number, eps: number): boolean => Math.abs(a - b) <= eps * Math.max(1, Math.abs(a), Math.abs(b))
const clamp1 = (v: number): number => (v < -1 ? -1 : v > 1 ? 1 : v)

export class Quat {
  x: number
  y: number
  z: number
  w: number

  constructor(x: number | QuatLike = 0, y = 0, z = 0, w = 1) {
    if (typeof x === "number") { this.x = x; this.y = y; this.z = z; this.w = w }
    else { this.x = cx(x); this.y = cy(x); this.z = cz(x); this.w = cw(x) }
  }

  // --- mutation ---
  set(x: number, y: number, z: number, w: number): this { this.x = x; this.y = y; this.z = z; this.w = w; return this }
  copy(q: QuatLike): this { this.x = cx(q); this.y = cy(q); this.z = cz(q); this.w = cw(q); return this }

  // --- pure ops ---
  /** Hamilton product `this ⊗ b` (apply `b` first, then `this`). */
  mul(b: QuatLike): Quat {
    const ax = this.x, ay = this.y, az = this.z, aw = this.w
    const bx = cx(b), by = cy(b), bz = cz(b), bw = cw(b)
    return new Quat(
      ax * bw + aw * bx + ay * bz - az * by,
      ay * bw + aw * by + az * bx - ax * bz,
      az * bw + aw * bz + ax * by - ay * bx,
      aw * bw - ax * bx - ay * by - az * bz,
    )
  }
  normalize(): Quat {
    let l = Math.hypot(this.x, this.y, this.z, this.w)
    if (l === 0) return new Quat(0, 0, 0, 1)
    l = 1 / l
    return new Quat(this.x * l, this.y * l, this.z * l, this.w * l)
  }
  conjugate(): Quat { return new Quat(-this.x, -this.y, -this.z, this.w) }
  /** Inverse rotation. For a unit quaternion this equals `conjugate()`. */
  invert(): Quat {
    const d = this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w
    if (d === 0) return new Quat(0, 0, 0, 0)
    const i = 1 / d
    return new Quat(-this.x * i, -this.y * i, -this.z * i, this.w * i)
  }
  dot(b: QuatLike): number { return this.x * cx(b) + this.y * cy(b) + this.z * cz(b) + this.w * cw(b) }
  /** Spherical interpolation toward `b` by `t ∈ [0, 1]`. */
  slerp(b: QuatLike, t: number): Quat {
    const ax = this.x, ay = this.y, az = this.z, aw = this.w
    let bx = cx(b), by = cy(b), bz = cz(b), bw = cw(b)
    let cosom = ax * bx + ay * by + az * bz + aw * bw
    if (cosom < 0) { cosom = -cosom; bx = -bx; by = -by; bz = -bz; bw = -bw }
    let s0: number, s1: number
    if (1 - cosom > EPS) {
      const omega = Math.acos(cosom), sinom = Math.sin(omega)
      s0 = Math.sin((1 - t) * omega) / sinom
      s1 = Math.sin(t * omega) / sinom
    } else { s0 = 1 - t; s1 = t }
    return new Quat(s0 * ax + s1 * bx, s0 * ay + s1 * by, s0 * az + s1 * bz, s0 * aw + s1 * bw)
  }
  /** Angular distance (radians) between two unit quaternions. */
  angle(b: QuatLike): number {
    const d = this.dot(b)
    return Math.acos(clamp1(2 * d * d - 1))
  }
  /** Rotate a vector by this quaternion → a new `Vec3`. */
  rotateVec3(v: Vec3Like): Vec3 { return new Vec3(v).rotate(this) }

  /** Extract euler angles (DEGREES) in the given order. */
  toEuler(order: EulerOrder = "YXZ"): Vec3 {
    // build the (column-major) rotation matrix entries from the unit quaternion, then extract.
    const x = this.x, y = this.y, z = this.z, w = this.w
    const x2 = x + x, y2 = y + y, z2 = z + z
    const xx = x * x2, xy = x * y2, xz = x * z2
    const yy = y * y2, yz = y * z2, zz = z * z2
    const wx = w * x2, wy = w * y2, wz = w * z2
    const m = [
      1 - (yy + zz), xy + wz, xz - wy, 0,
      xy - wz, 1 - (xx + zz), yz + wx, 0,
      xz + wy, yz - wx, 1 - (xx + yy), 0,
    ]
    return eulerFromColMajor(m, order)
  }

  // --- interop ---
  equals(b: QuatLike, eps = 1e-6): boolean {
    return approx(this.x, cx(b), eps) && approx(this.y, cy(b), eps) && approx(this.z, cz(b), eps) && approx(this.w, cw(b), eps)
  }
  /** True if the two unit quaternions represent the same rotation (±q are equal). */
  sameRotation(b: QuatLike, eps = 1e-6): boolean { return Math.abs(this.dot(b)) >= 1 - eps }
  clone(): Quat { return new Quat(this.x, this.y, this.z, this.w) }
  toArray(): [number, number, number, number] { return [ this.x, this.y, this.z, this.w ] }
  *[Symbol.iterator](): Iterator<number> { yield this.x; yield this.y; yield this.z; yield this.w }

  // --- factories ---
  static from(q: QuatLike): Quat { return new Quat(cx(q), cy(q), cz(q), cw(q)) }
  static get identity(): Quat { return new Quat(0, 0, 0, 1) }

  /** From euler angles in DEGREES (default order "YXZ"). Inverts `toEuler`. */
  static fromEuler(x: number, y: number, z: number, order: EulerOrder = "YXZ"): Quat {
    const c1 = Math.cos(x * DEG2RAD / 2), c2 = Math.cos(y * DEG2RAD / 2), c3 = Math.cos(z * DEG2RAD / 2)
    const s1 = Math.sin(x * DEG2RAD / 2), s2 = Math.sin(y * DEG2RAD / 2), s3 = Math.sin(z * DEG2RAD / 2)
    switch (order) {
      case "XYZ": return new Quat(s1 * c2 * c3 + c1 * s2 * s3, c1 * s2 * c3 - s1 * c2 * s3, c1 * c2 * s3 + s1 * s2 * c3, c1 * c2 * c3 - s1 * s2 * s3)
      case "YXZ": return new Quat(s1 * c2 * c3 + c1 * s2 * s3, c1 * s2 * c3 - s1 * c2 * s3, c1 * c2 * s3 - s1 * s2 * c3, c1 * c2 * c3 + s1 * s2 * s3)
      case "ZXY": return new Quat(s1 * c2 * c3 - c1 * s2 * s3, c1 * s2 * c3 + s1 * c2 * s3, c1 * c2 * s3 + s1 * s2 * c3, c1 * c2 * c3 - s1 * s2 * s3)
      case "ZYX": return new Quat(s1 * c2 * c3 - c1 * s2 * s3, c1 * s2 * c3 + s1 * c2 * s3, c1 * c2 * s3 - s1 * s2 * c3, c1 * c2 * c3 + s1 * s2 * s3)
      case "YZX": return new Quat(s1 * c2 * c3 + c1 * s2 * s3, c1 * s2 * c3 + s1 * c2 * s3, c1 * c2 * s3 - s1 * s2 * c3, c1 * c2 * c3 - s1 * s2 * s3)
      case "XZY": return new Quat(s1 * c2 * c3 - c1 * s2 * s3, c1 * s2 * c3 - s1 * c2 * s3, c1 * c2 * s3 + s1 * s2 * c3, c1 * c2 * c3 + s1 * s2 * s3)
    }
  }

  /** From a rotation `axis` and an angle in RADIANS. The axis is normalized for you. */
  static fromAxisAngle(axis: Vec3Like, rad: number): Quat {
    let ax = cx(axis), ay = cy(axis), az = cz(axis)
    const l = Math.hypot(ax, ay, az)
    if (l === 0) return new Quat(0, 0, 0, 1)
    ax /= l; ay /= l; az /= l
    const s = Math.sin(rad / 2)
    return new Quat(ax * s, ay * s, az * s, Math.cos(rad / 2))
  }

  /** The shortest rotation that takes direction `a` onto direction `b`. */
  static fromTo(a: Vec3Like, b: Vec3Like): Quat {
    const A = new Vec3(a).normalize(), B = new Vec3(b).normalize()
    const d = A.dot(B)
    if (d > 0.999999) return new Quat(0, 0, 0, 1)
    if (d < -0.999999) {
      // antiparallel: rotate 180° about any axis ⟂ A.
      let axis = new Vec3(1, 0, 0).cross(A)
      if (axis.lengthSq() < 1e-12) axis = new Vec3(0, 1, 0).cross(A)
      return Quat.fromAxisAngle(axis.normalize(), Math.PI)
    }
    const c = A.cross(B)
    return new Quat(c.x, c.y, c.z, 1 + d).normalize()
  }

  /** A rotation whose forward (−Z) points along `forward`, with `up` as the reference up axis. */
  static lookRotation(forward: Vec3Like, up: Vec3Like = Vec3.up): Quat {
    const zc = new Vec3(forward).normalize().negate()       // local +Z basis (forward is −Z)
    let xc = new Vec3(up).cross(zc)
    if (xc.lengthSq() < 1e-12) xc = new Vec3(up).cross(zc.add([ 1e-4, 0, 0 ]))
    xc = xc.normalize()
    const yc = zc.cross(xc)
    return quatFromColMajor([ xc.x, xc.y, xc.z, 0, yc.x, yc.y, yc.z, 0, zc.x, zc.y, zc.z ])
  }
}

/** Quaternion from a column-major rotation matrix (≥3×3, indices 0,1,2,4,5,6,8,9,10). gl-matrix's
 *  `getRotation` algorithm — the exact inverse of building a matrix from a quaternion. */
export const quatFromColMajor = (m: ArrayLike<number>): Quat => {
  const m00 = m[0], m01 = m[1], m02 = m[2]
  const m10 = m[4], m11 = m[5], m12 = m[6]
  const m20 = m[8], m21 = m[9], m22 = m[10]
  const trace = m00 + m11 + m22
  let S: number
  if (trace > 0) {
    S = Math.sqrt(trace + 1) * 2
    return new Quat((m12 - m21) / S, (m20 - m02) / S, (m01 - m10) / S, 0.25 * S)
  } else if (m00 > m11 && m00 > m22) {
    S = Math.sqrt(1 + m00 - m11 - m22) * 2
    return new Quat(0.25 * S, (m01 + m10) / S, (m20 + m02) / S, (m12 - m21) / S)
  } else if (m11 > m22) {
    S = Math.sqrt(1 + m11 - m00 - m22) * 2
    return new Quat((m01 + m10) / S, 0.25 * S, (m12 + m21) / S, (m20 - m02) / S)
  } else {
    S = Math.sqrt(1 + m22 - m00 - m11) * 2
    return new Quat((m20 + m02) / S, (m12 + m21) / S, 0.25 * S, (m01 - m10) / S)
  }
}

/** Euler angles (DEGREES) from a column-major rotation matrix, standard three.js extraction.
 *  Element naming follows the column-major layout: mRC = m[(C-1)*4 + (R-1)]. */
export const eulerFromColMajor = (m: ArrayLike<number>, order: EulerOrder = "YXZ"): Vec3 => {
  const m11 = m[0], m21 = m[1], m31 = m[2]
  const m12 = m[4], m22 = m[5], m32 = m[6]
  const m13 = m[8], m23 = m[9], m33 = m[10]
  let x = 0, y = 0, z = 0
  const LIM = 0.9999999
  switch (order) {
    case "XYZ":
      y = Math.asin(clamp1(m13))
      if (Math.abs(m13) < LIM) { x = Math.atan2(-m23, m33); z = Math.atan2(-m12, m11) }
      else { x = Math.atan2(m32, m22); z = 0 }
      break
    case "YXZ":
      x = Math.asin(-clamp1(m23))
      if (Math.abs(m23) < LIM) { y = Math.atan2(m13, m33); z = Math.atan2(m21, m22) }
      else { y = Math.atan2(-m31, m11); z = 0 }
      break
    case "ZXY":
      x = Math.asin(clamp1(m32))
      if (Math.abs(m32) < LIM) { y = Math.atan2(-m31, m33); z = Math.atan2(-m12, m22) }
      else { y = 0; z = Math.atan2(m21, m11) }
      break
    case "ZYX":
      y = Math.asin(-clamp1(m31))
      if (Math.abs(m31) < LIM) { x = Math.atan2(m32, m33); z = Math.atan2(m21, m11) }
      else { x = 0; z = Math.atan2(-m12, m22) }
      break
    case "YZX":
      z = Math.asin(clamp1(m21))
      if (Math.abs(m21) < LIM) { x = Math.atan2(-m23, m22); y = Math.atan2(-m31, m11) }
      else { x = 0; y = Math.atan2(m13, m33) }
      break
    case "XZY":
      z = Math.asin(-clamp1(m12))
      if (Math.abs(m12) < LIM) { x = Math.atan2(m32, m22); y = Math.atan2(m13, m11) }
      else { x = Math.atan2(-m23, m33); y = 0 }
      break
  }
  return new Vec3(x * RAD2DEG, y * RAD2DEG, z * RAD2DEG)
}
