package ru.dbotthepony.mc.otm.core.math

import com.mojang.blaze3d.vertex.PoseStack
import it.unimi.dsi.fastutil.ints.IntSpliterators
import it.unimi.dsi.fastutil.longs.Long2ObjectOpenHashMap
import net.minecraft.core.BlockPos
import net.minecraft.core.Direction
import net.minecraft.core.Vec3i
import net.minecraft.world.phys.Vec3
import org.joml.AxisAngle4f
import org.joml.Matrix3f
import org.joml.Matrix4f
import org.joml.Quaternionf
import org.joml.Vector3f
import ru.dbotthepony.mc.otm.core.collect.filter
import ru.dbotthepony.mc.otm.core.collect.flatMap
import ru.dbotthepony.mc.otm.core.collect.map
import java.lang.ref.SoftReference
import java.util.stream.Stream
import java.util.stream.StreamSupport
import kotlin.math.*

typealias Vector = Vec3

val VECTOR_UP = Vector(0.0, 1.0, 0.0)
val VECTOR_FORWARD = Vector(1.0, 0.0, 0.0)
val VECTOR_BACKWARD = Vector(-1.0, 0.0, 0.0)
val VECTOR_DOWN = Vector(0.0, -1.0, 0.0)
val VECTOR_RIGHT = Vector(0.0, 0.0, 1.0)
val VECTOR_LEFT = Vector(0.0, 0.0, -1.0)

private const val DEGREES_TO_RADIANS = 0.017453292f

fun toRadians(angle: Float): Float {
	return angle * DEGREES_TO_RADIANS
}

fun Vector.asAngle(): Angle {
	val norm = normalize()
	return Angle(asin(norm.y), atan2(norm.x, norm.z))
}

fun Vector.asMutableAngle(): MutableAngle {
	val norm = normalize()
	return MutableAngle(asin(norm.y), atan2(norm.x, norm.z))
}

fun Vector3f.asAngle(): Angle {
	val len = length().toDouble()
	val norm = Vector(x() / len, y() / len, z() / len)
	return Angle(asin(norm.y), atan2(norm.x, norm.z))
}

fun Vector3f.asMutableAngle(): MutableAngle {
	val len = length().toDouble()
	val norm = Vector(x() / len, y() / len, z() / len)
	return MutableAngle(asin(norm.y), atan2(norm.x, norm.z))
}

fun Vector3f.rotation(radians: Float): Quaternionf {
	return Quaternionf(AxisAngle4f(radians, x, y, z))
}

fun Vector3f.rotationDegrees(degrees: Float): Quaternionf {
	return rotation(toRadians(degrees))
}

operator fun Vector3f.unaryMinus(): Vector3f {
	return Vector3f(-x, -y, -z)
}

operator fun Vector3f.times(v: Float): Vector3f = Vector3f(v).mul(v)

operator fun Vector3f.component1() = x()
operator fun Vector3f.component2() = y()
operator fun Vector3f.component3() = z()

operator fun Vec3i.component1() = x
operator fun Vec3i.component2() = y
operator fun Vec3i.component3() = z

fun Vec3i.toDoubleVector() = Vector(x.toDouble(), y.toDouble(), z.toDouble())

fun shortestDistanceBetween(a: Collection<Vector>, b: Collection<Vector>): Double {
	var minimum = Double.MAX_VALUE

	for (vecA in a)
		for (vecB in b)
			minimum = minimum.coerceAtMost(vecA.distanceTo(vecB))

	return minimum
}

fun shortestDistanceBetween(a: Collection<Vector>, b: Vector): Double {
	var minimum = Double.MAX_VALUE

	for (vecA in a)
		minimum = minimum.coerceAtMost(vecA.distanceTo(b))

	return minimum
}

operator fun Vector.plus(direction: Vector): Vector = this.add(direction)
operator fun Vector.minus(direction: Vector): Vector = this.subtract(direction)
operator fun Vector.unaryMinus(): Vector = Vector(-x, -y, -z)
operator fun Vector.component1() = x
operator fun Vector.component2() = y
operator fun Vector.component3() = z
operator fun Vector.times(v: Double): Vector = this.multiply(v, v, v)
operator fun Vector.div(v: Double): Vector {
	val inv = 1.0 / v
	return this.multiply(inv, inv, inv)
}

fun Vector.left() = asAngle().left() * this.length()
fun Vector.right() = asAngle().right() * this.length()
fun Vector.up() = asAngle().up() * this.length()
fun Vector.down() = asAngle().down() * this.length()

fun Vector.rotateAroundAxis(axis: Vector, rotation: Double): Vector {
	val cross = axis.cross(this)
	val dot = axis.dot(this)
	val cos = cos(rotation)
	val sin = sin(rotation)

	return Vector(
		this.x * cos + cross.x * sin + axis.x * dot * (1.0 - cos),
		this.y * cos + cross.y * sin + axis.y * dot * (1.0 - cos),
		this.z * cos + cross.z * sin + axis.z * dot * (1.0 - cos),
	)
}

fun Vector.rotate(angle: IAngle): Vector {
	val rotation = angle.rotationXYZ()

	val newx = x * rotation[0, 0,] + y * rotation[1, 0] + z * rotation[2, 0]
	val newy = x * rotation[0, 1,] + y * rotation[1, 1] + z * rotation[2, 1]
	val newz = x * rotation[0, 2,] + y * rotation[1, 2] + z * rotation[2, 2]

	return Vector(newx, newy, newz)
}

fun Vector.asVector3f(): Vector3f {
	return Vector3f(x.toFloat(), y.toFloat(), z.toFloat())
}

fun Quaternion(vec: Vector, rotation: Float, isDegrees: Boolean = false): Quaternionf {
	// TODO: 1.19.3
	return Quaternionf(AxisAngle4f(if (isDegrees) toRadians(rotation) else rotation, vec.x.toFloat(), vec.y.toFloat(), vec.z.toFloat()))
}

fun AxisAngle4f(vec: Vector, rotation: Float, isDegrees: Boolean = false): AxisAngle4f {
	// TODO: 1.19.3
	return AxisAngle4f(if (isDegrees) toRadians(rotation) else rotation, vec.x.toFloat(), vec.y.toFloat(), vec.z.toFloat())
}

fun Vector(x: Double): Vector {
	return Vector(x, 0.0, 0.0)
}

fun Vector(x: Double, y: Double): Vector {
	return Vector(x, y, 0.0)
}

fun Vector.rotate(q: Quaternionf): Vector {
	// TODO: 1.19.3
	val quaternion = Quaternionf(q)
	quaternion.mul(rotateAroundThis())
	val quaternion1 = Quaternionf(q)
	quaternion1.conjugate()
	quaternion.mul(quaternion1)
	return Vector(quaternion.x.toDouble(), quaternion.y.toDouble(), quaternion.z.toDouble())
}

fun Vector.rotateAroundThis(rotation: Float, isDegrees: Boolean = false): Quaternionf {
	// TODO: 1.19.3
	return Quaternionf(AxisAngle4f(if (isDegrees) toRadians(rotation) else rotation, x.toFloat(), y.toFloat(), z.toFloat()))
}

fun Vector.rotateAroundThis(): Quaternionf {
	// TODO: 1.19.3
	return Quaternionf(AxisAngle4f(0f, x.toFloat(), y.toFloat(), z.toFloat()))
}

fun Vector.asMatrix(): Matrix3f {
	return Matrix3f().also {
		it[0, 0] = x.toFloat()
		it[1, 0] = y.toFloat()
		it[2, 0] = z.toFloat()
	}
}

fun Direction.asAngle(): Angle {
	return when (this) {
		Direction.DOWN -> Angle.DOWN
		Direction.UP -> Angle.UP
		Direction.NORTH -> Angle.NORTH
		Direction.SOUTH -> Angle.SOUTH
		Direction.WEST -> Angle.WEST
		Direction.EAST -> Angle.EAST
	}
}

interface IAngle {
	val pitch: Double
	val yaw: Double
	val roll: Double

	fun forward() = VECTOR_FORWARD.rotate(this)
	fun left() = VECTOR_LEFT.rotate(this)
	fun right() = VECTOR_RIGHT.rotate(this)
	fun up() = VECTOR_UP.rotate(this)
	fun down() = VECTOR_DOWN.rotate(this)

	fun rotationX(): Matrix3f {
		if (roll == 0.0) {
			return Matrix3f().also { it.identity() }
		}

		return Matrix3f().also {
			val s = sin(roll).toFloat()
			val c = cos(roll).toFloat()

			it[0, 0] = 1f

			it[1, 1] = c
			it[2, 1] = -s

			it[1, 2] = s
			it[2, 2] = c
		}
	}

	fun rotationZ(): Matrix3f {
		if (pitch == 0.0) {
			return Matrix3f().also { it.identity() }
		}

		return Matrix3f().also {
			val s = sin(pitch).toFloat()
			val c = cos(pitch).toFloat()

			it[0, 0] = c
			it[1, 0] = -s

			it[0, 1] = s
			it[1, 1] = c

			it[2, 2] = 1f
		}
	}

	fun rotationY(): Matrix3f {
		if (yaw == 0.0) {
			return Matrix3f().also { it.identity() }
		}

		return Matrix3f().also {
			val s = sin(yaw).toFloat()
			val c = cos(yaw).toFloat()

			it[0, 0,] = c
			it[2, 0,] = s

			it[0, 2,] = -s
			it[2, 2,] = c

			it[1, 1,] = 1f
		}
	}

	fun rotationXYZ(): Matrix3f {
		if (pitch == 0.0 && yaw == 0.0 && roll == 0.0)
			return Matrix3f().also { it.identity() }

		return rotationY().also {
			it.mul(rotationZ())
			it.mul(rotationX())
		}
	}

	fun rotationXYZW(): Matrix4f {
		if (pitch == 0.0 && yaw == 0.0 && roll == 0.0)
			return Matrix4f().also { it.identity() }

		return rotationXYZ().toMatrix4f().also { it[3, 3] = 1f }
	}
}

data class Angle(override val pitch: Double = 0.0, override val yaw: Double = 0.0, override val roll: Double = 0.0) :
	IAngle {
	companion object {
		const val PI_HALF = Math.PI / 2.0

		fun deg(pitch: Double, yaw: Double, roll: Double): Angle {
			return Angle(Math.toRadians(pitch), Math.toRadians(yaw), Math.toRadians(roll))
		}

		val ZERO = Angle()
		val UP = Angle(pitch = PI / 2)
		val DOWN = Angle(pitch = -PI / 2)
		val NORTH = Angle()
		val SOUTH = Angle(yaw = PI)
		val WEST = Angle(yaw = PI / 2)
		val EAST = Angle(yaw = -PI / 2)
	}
}

data class MutableAngle(override var pitch: Double = 0.0, override var yaw: Double = 0.0, override var roll: Double = 0.0) :
	IAngle {
	companion object {
		fun deg(pitch: Double, yaw: Double, roll: Double): MutableAngle {
			return MutableAngle(Math.toRadians(pitch), Math.toRadians(yaw), Math.toRadians(roll))
		}
	}
}

fun linearInterpolation(t: Float, a: Float, b: Float): Float {
	if (t <= 0f)
		return a
	else if (t >= 1f)
		return b

	return a + t * (b - a)
}

fun linearInterpolation(t: Double, a: Double, b: Double): Double {
	if (t <= 0.0)
		return a
	else if (t >= 1.0)
		return b

	return a + t * (b - a)
}

fun linearInterpolation(t: Double, a: Angle, b: Angle): Angle {
	return Angle(
		linearInterpolation(t, a.pitch, b.pitch),
		linearInterpolation(t, a.yaw, b.yaw),
		linearInterpolation(t, a.roll, b.roll),
	)
}

fun linearInterpolation(t: Double, a: Vector, b: Vector): Vector {
	return Vector(
		linearInterpolation(t, a.x, b.x),
		linearInterpolation(t, a.y, b.y),
		linearInterpolation(t, a.z, b.z),
	)
}

fun bezierCurve(t: Double, vararg values: Angle): Angle {
	when (values.size) {
		0, 1 -> throw IllegalArgumentException("Provided array has only ${values.size} entries in it")
		else -> {
			@Suppress("NAME_SHADOWING") val values = values.clone() as Array<Angle>

			// construct prime - 1
			for (length in values.size - 2 downTo 0) {
				// search prime
				for (j in 0 .. length) {
					val point1 = values[j]
					val point2 = values[j + 1]

					values[j] = linearInterpolation(t, point1, point2)
				}
			}

			return values[0]
		}
	}
}

fun bezierCurve(t: Double, vararg values: Vector): Vector {
	when (values.size) {
		0, 1 -> throw IllegalArgumentException("Provided array has only ${values.size} entries in it")
		else -> {
			@Suppress("NAME_SHADOWING") val values = values.clone() as Array<Vector>

			// construct prime - 1
			for (length in values.size - 2 downTo 0) {
				// search prime
				for (j in 0 .. length) {
					val point1 = values[j]
					val point2 = values[j + 1]

					values[j] = linearInterpolation(t, point1, point2)
				}
			}

			return values[0]
		}
	}
}

fun bezierCurve(t: Double, values: DoubleArray): Double {
	when (values.size) {
		0, 1 -> throw IllegalArgumentException("Provided array has only ${values.size} entries in it")
		else -> {
			@Suppress("NAME_SHADOWING") val values = values.clone()

			// construct prime - 1
			for (length in values.size - 2 downTo 0) {
				// search prime
				for (j in 0 .. length) {
					val point1 = values[j]
					val point2 = values[j + 1]

					values[j] = linearInterpolation(t, point1, point2)
				}
			}

			return values[0]
		}
	}
}

operator fun BlockPos.plus(direction: Vec3i): BlockPos = this.offset(direction)
operator fun BlockPos.plus(direction: Direction): BlockPos = this.offset(direction.normal)
operator fun BlockPos.minus(direction: Vec3i): BlockPos = this.subtract(direction)
operator fun BlockPos.minus(direction: Direction): BlockPos = this.subtract(direction.normal)

operator fun Vec3i.plus(direction: Vec3i): Vec3i = this.offset(direction)
operator fun Vec3i.plus(direction: Direction): Vec3i = this.offset(direction.normal)
operator fun Vec3i.minus(direction: Vec3i): Vec3i = this.subtract(direction)
operator fun Vec3i.minus(direction: Direction): Vec3i = this.subtract(direction.normal)
operator fun Vec3i.times(int: Int): Vec3i = this.multiply(int)
operator fun Direction.times(int: Int): Vec3i = this.normal.multiply(int)
operator fun Vec3i.times(double: Double): Vector = Vector(x * double, y * double, z * double)

fun Vec3.toIntVector() = Vec3i(x.toInt(), y.toInt(), z.toInt())
fun Vec3.roundToIntVector() = Vec3i(x.roundToInt(), y.roundToInt(), z.roundToInt())

fun BlockPos.asVector(): Vector {
	return Vector(x + 0.5, y + 0.5, z + 0.5)
}

operator fun Direction.unaryMinus(): Direction = this.opposite
operator fun Vec3i.unaryMinus(): Vec3i = Vec3i(-x, -y, -z)
operator fun BlockPos.unaryMinus(): BlockPos = BlockPos(-x, -y, -z)

fun getSphericalBlockPositions(size: Int): Iterator<BlockPos> {
	return getEllipsoidBlockPositions(size, size, size)
}

fun getEllipsoidBlockPositions(x: Int, y: Int, z: Int): Iterator<BlockPos> {
	val xPow = x.toDouble().pow(2.0)
	val yPow = y.toDouble().pow(2.0)
	val zPow = z.toDouble().pow(2.0)

	return (-x .. x).iterator().flatMap { ix ->
		(-y .. y).iterator().flatMap { iy ->
			(-z .. z).iterator().map { BlockPos(ix, iy, it) }
		}
	}.filter { pos ->
		val (ellipsoidX, ellipsoidY, ellipsoidZ) = pos

		ellipsoidX.toDouble().pow(2.0) / xPow +
		ellipsoidY.toDouble().pow(2.0) / yPow +
		ellipsoidZ.toDouble().pow(2.0) / zPow <= 1.0
	}
}

fun normalizeAngle(angle: Double): Double {
	return (angle + PI) % (PI * 2.0) - PI
}

fun angleDifference(angle1: Double, angle2: Double): Double {
	val diff = normalizeAngle(angle1 - angle2)

	if (diff < PI) {
		return diff
	}

	return PI * 2.0 - diff
}

fun normalizeAngle(angle: Float): Float {
	return normalizeAngle(angle.toDouble()).toFloat()
}

fun angleDifference(angle1: Float, angle2: Float): Float {
	return angleDifference(angle1.toDouble(), angle2.toDouble()).toFloat()
}

fun normalizeAngleDeg(angle: Double): Double {
	return (angle + 180.0) % 360.0 - 180.0
}

fun normalizeAngleDeg(angle: Float): Float {
	return (angle + 180f) % 360f - 180f
}

fun angleDifferenceDeg(angle1: Double, angle2: Double): Double {
	val diff = normalizeAngle(angle1 - angle2)

	if (diff < 180.0) {
		return diff
	}

	return 360.0 - diff
}

fun angleDifferenceDeg(angle1: Float, angle2: Float): Float {
	val diff = normalizeAngle(angle1 - angle2)

	if (diff < 180f) {
		return diff
	}

	return 360f - diff
}

// 1.19.3 stuff
fun PoseStack.rotateY(rotation: Float): PoseStack {
	mulPose(Quaternionf(AxisAngle4f(rotation, 0f, 1f, 0f)))
	return this
}

fun PoseStack.rotateX(rotation: Float): PoseStack {
	mulPose(Quaternionf(AxisAngle4f(rotation, 1f, 0f, 0f)))
	return this
}

fun PoseStack.rotateZ(rotation: Float): PoseStack {
	mulPose(Quaternionf(AxisAngle4f(rotation, 0f, 0f, 1f)))
	return this
}

fun PoseStack.rotate(rotation: Direction): PoseStack {
	return rotateY(rotation.yRotationNorth()).rotateX(rotation.xRotationNorth())
}

fun PoseStack.rotateSouth(rotation: Direction): PoseStack {
	return rotateY(rotation.yRotationSouth()).rotateX(rotation.xRotationSouth())
}
// /1.19.3 stuff

const val PIf = 3.1415927f

/**
 * aligns 0,0 with top left point of block corner when looking directly at it,
 * and swaps Y to +Y when going down and -Y when going up
 *
 * allows to draw 2D interface in 3D space
 */
fun PoseStack.rotateWithBlockFacing(rotation: Direction, clarifyingAxis: Direction? = null): PoseStack {
	when (rotation) {
		Direction.NORTH -> {
			rotateX(PIf)
			rotateY(PIf)
			translate(-1f, -1f, 0f)
		}

		Direction.EAST -> {
			rotateX(PIf)
			rotateY(PIf / -2f)
			translate(-1f, -1f, -1f)
		}

		Direction.WEST -> {
			rotateX(PIf)
			rotateY(PIf / 2f)
			translate(0f, -1f, 0f)
		}

		Direction.UP -> {
			rotateX(PIf / 2f)
			translate(0f, 0f, -1f)

			if (clarifyingAxis != null) {
				when (clarifyingAxis) {
					Direction.NORTH -> {}
					Direction.SOUTH -> {
						rotateZ(PIf)
						translate(-1f, -1f, 0f)
					}
					Direction.WEST -> {
						rotateZ(PIf / -2f)
						translate(-1f, 0f, 0f)
					}
					Direction.EAST -> {
						rotateZ(PIf / 2f)
						translate(0f, -1f, 0f)
					}
					else -> {}
				}
			}
		}

		Direction.DOWN -> {
			rotateY(PIf)
			rotateX(PIf / -2f)
			translate(-1f, 0f, 0f)

			if (clarifyingAxis != null) {
				when (clarifyingAxis) {
					Direction.NORTH -> {}
					Direction.SOUTH -> {
						rotateZ(PIf)
						translate(-1f, -1f, 0f)
					}
					Direction.WEST -> {
						rotateZ(PIf / 2f)
						translate(0f, -1f, 0f)
					}
					Direction.EAST -> {
						rotateZ(PIf / -2f)
						translate(-1f, 0f, 0f)
					}
					else -> {}
				}
			}
		}

		Direction.SOUTH -> {
			rotateX(PIf)
			translate(0f, -1f, -1f)
		}
	}

	return this
}

fun PoseStack.rotateWithBlockFacing(rotation: BlockRotation): PoseStack {
	return rotateWithBlockFacing(rotation = rotation.front, clarifyingAxis = rotation.top)
}

fun PoseStack.rotateYDegrees(rotation: Float): PoseStack {
	mulPose(Quaternionf(AxisAngle4f(toRadians(rotation), 0f, 1f, 0f)))
	return this
}

fun PoseStack.rotateXDegrees(rotation: Float): PoseStack {
	mulPose(Quaternionf(AxisAngle4f(toRadians(rotation), 1f, 0f, 0f)))
	return this
}

fun PoseStack.rotateZDegrees(rotation: Float): PoseStack {
	mulPose(Quaternionf(AxisAngle4f(toRadians(rotation), 0f, 0f, 1f)))
	return this
}

fun Direction.yRotationBlockstateNorth(): Int {
	return when (this) {
		Direction.DOWN, Direction.UP, Direction.NORTH -> 0
		Direction.SOUTH -> 180
		Direction.WEST -> -90
		Direction.EAST -> 90
	}
}

fun Direction.yRotationBlockstateSouth(): Int {
	return when (this) {
		Direction.DOWN, Direction.UP, Direction.SOUTH -> 0
		Direction.NORTH -> 180
		Direction.WEST -> 90
		Direction.EAST -> -90
	}
}

fun Direction.xRotationBlockstateNorth(): Int {
	return when (this) {
		Direction.DOWN -> 90
		Direction.UP -> -90
		else -> 0
	}
}

fun Direction.xRotationBlockstateSouth(): Int {
	return when (this) {
		Direction.DOWN -> -90
		Direction.UP -> 90
		else -> 0
	}
}

fun Direction.yRotationNorth(): Float {
	return when (this) {
		Direction.DOWN, Direction.UP, Direction.NORTH -> 0f
		Direction.SOUTH -> PIf
		Direction.WEST -> -PIf / 2f
		Direction.EAST -> PIf / 2f
	}
}

fun Direction.yRotationSouth(): Float {
	return when (this) {
		Direction.DOWN, Direction.UP, Direction.SOUTH -> 0f
		Direction.NORTH -> PIf
		Direction.WEST -> PIf / 2f
		Direction.EAST -> -PIf / 2f
	}
}

fun Direction.xRotationNorth(): Float {
	return when (this) {
		Direction.DOWN -> PIf / 2f
		Direction.UP -> -PIf / 2f
		else -> 0f
	}
}

fun Direction.xRotationSouth(): Float {
	return when (this) {
		Direction.DOWN -> -PIf / 2f
		Direction.UP -> PIf / 2f
		else -> 0f
	}
}

