import { MathUtils, Quaternion, Vector3 } from 'three'

export const multiplyQuaternionVector3 = (rotation: Quaternion, point: Vector3) => {
  const num1 = rotation.x * 2
  const num2 = rotation.y * 2
  const num3 = rotation.z * 2
  const num4 = rotation.x * num1
  const num5 = rotation.y * num2
  const num6 = rotation.z * num3
  const num7 = rotation.x * num2
  const num8 = rotation.x * num3
  const num9 = rotation.y * num3
  const num10 = rotation.w * num1
  const num11 = rotation.w * num2
  const num12 = rotation.w * num3
  const v = new Vector3()
  v.x =
    (1.0 - (num5 + num6)) * point.x + (num7 - num12) * point.y + (num8 + num11) * point.z
  v.y =
    (num7 + num12) * point.x + (1.0 - (num4 + num6)) * point.y + (num9 - num10) * point.z
  v.z =
    (num8 - num11) * point.x + (num9 + num10) * point.y + (1.0 - (num4 + num5)) * point.z
  return v
}

export const repeat = (t: number, length: number): number => {
  return t - Math.floor(t / length) * length
}

export const deltaAngle = (current: number, target: number): number => {
  let num = repeat(target - current, 360)
  if (num > 180) num -= 360
  return num
}

export const relativeAngle = (axis: Vector3, q1: Quaternion, q2: Quaternion): number => {
  const vecA = multiplyQuaternionVector3(q1, axis)
  const vecB = multiplyQuaternionVector3(q2, axis)
  const angleA = MathUtils.radToDeg(Math.atan2(vecA.x, vecA.z))
  const angleB = MathUtils.radToDeg(Math.atan2(vecB.x, vecB.z))
  return deltaAngle(angleA, angleB)
}
