package ru.dbotthepony.kstarbound.world.physics

import com.google.common.collect.ImmutableList
import com.google.gson.Gson
import com.google.gson.TypeAdapter
import com.google.gson.TypeAdapterFactory
import com.google.gson.reflect.TypeToken
import com.google.gson.stream.JsonReader
import com.google.gson.stream.JsonWriter
import it.unimi.dsi.fastutil.objects.ObjectOpenHashSet
import org.lwjgl.opengl.GL11.GL_LINES
import ru.dbotthepony.kommons.util.IStruct2d
import ru.dbotthepony.kommons.math.RGBAColor
import ru.dbotthepony.kommons.math.intersectSegments
import ru.dbotthepony.kommons.matrix.Matrix3f
import ru.dbotthepony.kommons.util.AABB
import ru.dbotthepony.kommons.vector.Vector2d
import ru.dbotthepony.kstarbound.client.StarboundClient
import ru.dbotthepony.kstarbound.client.gl.vertex.GeometryType
import ru.dbotthepony.kommons.gson.consumeNull
import ru.dbotthepony.kommons.io.readCollection
import ru.dbotthepony.kommons.io.readVector2d
import ru.dbotthepony.kommons.io.readVector2f
import ru.dbotthepony.kommons.io.writeCollection
import ru.dbotthepony.kommons.io.writeStruct2d
import ru.dbotthepony.kommons.io.writeStruct2f
import ru.dbotthepony.kommons.matrix.Matrix3d
import ru.dbotthepony.kstarbound.json.listAdapter
import ru.dbotthepony.kstarbound.math.Line2d
import ru.dbotthepony.kstarbound.network.syncher.legacyCodec
import ru.dbotthepony.kstarbound.network.syncher.nativeCodec
import java.io.DataInputStream
import java.io.DataOutputStream
import java.util.LinkedList
import kotlin.math.absoluteValue
import kotlin.math.cos
import kotlin.math.sin

private fun calculateEdges(points: List<Vector2d>): Pair<ImmutableList<Line2d>, ImmutableList<Vector2d>> {
	require(points.size >= 2) { "Provided poly is invalid (only ${points.size} points are defined)" }

	if (points.size == 2) {
		// line...
		// while double edged line seems to work fine
		// for more stable collision detection lets make it a very thin rectangle

		val newPoints = ImmutableList.Builder<Vector2d>()
		val (p0, p1) = points

		val diff = (p1 - p0).unitVector
		val normal = Vector2d(-diff.y, diff.x)

		newPoints.add(p0 + normal * 0.001)
		newPoints.add(p1 + normal * 0.001)
		newPoints.add(p1 - normal * 0.001)
		newPoints.add(p0 - normal * 0.001)

		val (f) = calculateEdges(newPoints.build())
		return f to ImmutableList.copyOf(points)
	} else {
		val edges = ImmutableList.Builder<Line2d>()

		for (i in points.indices) {
			val p0 = points[i]
			val p1 = points[(i + 1) % points.size]

			edges.add(Line2d(p0, p1))
		}

		return edges.build() to ImmutableList.copyOf(points)
	}
}

private fun isLeft(p0: IStruct2d, p1: IStruct2d, p2: IStruct2d): Double {
	return (p1.component1() - p0.component1()) * (p2.component2() - p0.component2()) - (p2.component1() - p0.component1()) * (p1.component2() - p0.component2())
}

private fun rotate(point: Vector2d, sin: Double, cos: Double): Vector2d {
	return Vector2d(
		point.x * cos + point.y * sin,
		point.x * sin + point.y * cos
	)
}

/**
 * edges are built in clockwise winding
 *
 * If poly shape is not convex behavior of SAT is undefined
 */
class Poly private constructor(val edges: ImmutableList<Line2d>, val vertices: ImmutableList<Vector2d>) {
	private constructor(pair: Pair<ImmutableList<Line2d>, ImmutableList<Vector2d>>) : this(pair.first, pair.second)
	constructor(points: List<Vector2d>) : this(calculateEdges(points))
	constructor(aabb: AABB) : this(listOf(aabb.bottomLeft, aabb.topLeft, aabb.topRight, aabb.bottomRight))

	val aabb: AABB
	val isEmpty: Boolean
		get() = vertices.isEmpty()
	val isNotEmpty: Boolean
		get() = vertices.isNotEmpty()

	init {
		if (vertices.isEmpty()) {
			aabb = AABB.ZERO
		} else {
			aabb = AABB(
				Vector2d(vertices.minOf { it.x }, vertices.minOf { it.y }),
				Vector2d(vertices.maxOf { it.x }, vertices.maxOf { it.y }),
			)
		}
	}

	val centre by lazy {
		vertices.reduce { acc, vector2d -> acc + vector2d } / vertices.size
	}

	data class Penetration(val axis: Vector2d, val penetration: Double) : Comparable<Penetration> {
		val vector = axis * penetration

		override fun compareTo(other: Penetration): Int {
			return penetration.absoluteValue.compareTo(other.penetration.absoluteValue)
		}
	}

	operator fun plus(value: Penetration): Poly {
		if (isEmpty) return this
		val vertices = ImmutableList.Builder<Vector2d>()
		val edges = ImmutableList.Builder<Line2d>()

		for (v in this.vertices) vertices.add(v + value.vector)
		for (v in this.edges) edges.add(v + value.vector)

		return Poly(edges.build(), vertices.build())
	}

	operator fun plus(value: IStruct2d): Poly {
		if (isEmpty) return this
		val vertices = ImmutableList.Builder<Vector2d>()
		val edges = ImmutableList.Builder<Line2d>()

		for (v in this.vertices) vertices.add(v + value)
		for (v in this.edges) edges.add(v + value)

		return Poly(edges.build(), vertices.build())
	}

	operator fun minus(value: IStruct2d): Poly {
		if (isEmpty) return this
		val vertices = ImmutableList.Builder<Vector2d>()
		val edges = ImmutableList.Builder<Line2d>()

		for (v in this.vertices) vertices.add(v - value)
		for (v in this.edges) edges.add(v - value)

		return Poly(edges.build(), vertices.build())
	}

	operator fun times(value: IStruct2d): Poly {
		if (isEmpty) return this
		val vertices = ImmutableList.Builder<Vector2d>()
		val edges = ImmutableList.Builder<Line2d>()

		for (v in this.vertices) vertices.add(v * value)
		for (v in this.edges) edges.add(v * value)

		return Poly(edges.build(), vertices.build())
	}

	operator fun times(value: Double): Poly {
		if (isEmpty) return this
		val vertices = ImmutableList.Builder<Vector2d>()
		val edges = ImmutableList.Builder<Line2d>()

		for (v in this.vertices) vertices.add(v * value)
		for (v in this.edges) edges.add(v * value)

		return Poly(edges.build(), vertices.build())
	}

	fun rotate(radians: Double): Poly {
		if (radians == 0.0 || isEmpty)
			return this

		val sin = sin(radians)
		val cos = cos(radians)

		val edges = ImmutableList.Builder<Line2d>()
		val vertices = ImmutableList.Builder<Vector2d>()

		for (edge in this.edges) {
			edges.add(Line2d(rotate(edge.p0, sin, cos), rotate(edge.p1, sin, cos)))
		}

		for (vertex in this.vertices) {
			vertices.add(rotate(vertex, sin, cos))
		}

		return Poly(edges.build(), vertices.build())
	}

	// min / max
	fun project(normal: Vector2d): IStruct2d {
		check(!isEmpty) { "Poly is empty" }

		var min = vertices.first().dot(normal)
		var max = min

		for (vertex in vertices) {
			val dist = vertex.dot(normal)

			min = min.coerceAtMost(dist)
			max = max.coerceAtLeast(dist)
		}

		return Vector2d(min, max)
	}

	fun transform(transformation: Matrix3d): Poly {
		return Poly(vertices.map { it * transformation })
	}

	fun windingNumber(point: IStruct2d): Int {
		val (x, y) = point

		// the winding number counter
		var wn = 0

		// loop through all edges of the polygon
		for (edge in edges) {
			// start y <= p[1]
			if (edge.p0.y <= y) {
				if (edge.p1.y > y) {
					// an upward crossing
					if (isLeft(edge.p0, edge.p1, point) > 0.0) {
						// p left of edge
						// have a valid up intersect
						++wn
					}
				}
			} else {
				// start y > p[1] (no test needed)
				if (edge.p1.y <= y) {
					// a downward crossing
					if (isLeft(edge.p0, edge.p1, point) < 0.0) {
						// p right of edge
						// have a valid down intersect
						--wn
					}
				}
			}
		}

		return wn
	}

	operator fun contains(point: IStruct2d): Boolean {
		return windingNumber(point) != 0
	}

	fun distance(point: IStruct2d): Double {
		if (contains(point))
			return 0.0

		return edges.minOf { it.distanceTo(point) }
	}

	fun distance(point: Vector2d): Double {
		if (contains(point))
			return 0.0

		return edges.minOf { it.distanceTo(point) }
	}

	/**
	 * @param axis separate ONLY along specified axis, that said, if axis is positive Y and we have collision, and closest separation axis is to up right, we instead separate only up until we no longer collide.
	 */
	fun intersect(other: Poly, axis: Vector2d? = null, strictAxis: Boolean = false): Penetration? {
		if (isEmpty || !aabb.intersectWeak(other.aabb))
			return null

		val normals = ObjectOpenHashSet<Vector2d>()
		edges.forEach { normals.add(it.normal) }
		other.edges.forEach { normals.add(it.normal) }

		if (axis != null) {
			normals.removeIf { it.dot(axis) == 0.0 }
		}

		val intersections = ArrayList<Penetration>()

		for (normal in normals) {
			val projectThis = project(normal)
			val projectOther = other.project(normal)

			if (!intersectSegments(projectThis.component1(), projectThis.component2(), projectOther.component1(), projectOther.component2())) {
				return null
			} else {
				val width = projectThis.component2() - projectThis.component1()

				if (
					projectOther.component1() in projectThis.component1() .. projectThis.component2() &&
					projectOther.component2() in projectThis.component1() .. projectThis.component2()
				) {
					// other inside this
					val minMin = projectThis.component1() - projectOther.component1()
					val maxMax = projectThis.component2() - projectOther.component2()

					if (minMin <= maxMax) {
						// push to left
						intersections.add(Penetration(normal, -minMin - width))
					} else {
						// push to right
						intersections.add(Penetration(normal, maxMax + width))
					}
				} else if (
					projectThis.component1() in projectOther.component1() .. projectOther.component2() &&
					projectThis.component2() in projectOther.component1() .. projectOther.component2()
				) {
					// this inside other
					val minMin = projectThis.component1() - projectOther.component1()
					val maxMax = projectOther.component2() - projectThis.component2()

					if (minMin <= maxMax) {
						// push to left
						intersections.add(Penetration(normal, -minMin - width))
					} else {
						// push to right
						intersections.add(Penetration(normal, maxMax + width))
					}
				} else if (projectOther.component1() in projectThis.component1() .. projectThis.component2()) {
					// other's min point is within this
					// push to right
					intersections.add(Penetration(normal, projectOther.component1() - projectThis.component2()))
				} else {
					// other's max point in within this
					// push to left
					intersections.add(Penetration(normal, projectOther.component2() - projectThis.component1()))
				}

				if (axis != null) {
					val last = intersections.removeLast()

					if (strictAxis) {
						// TODO: NYI
						intersections.add(Penetration(axis, last.penetration / axis.dot(last.axis)))
					} else {
						intersections.add(Penetration(axis, last.penetration / axis.dot(last.axis)))
					}
				}

				if (intersections.last().penetration == 0.0) {
					return null
				}
			}
		}

		if (intersections.isEmpty())
			return null

		return intersections.min()
	}

	fun render(client: StarboundClient = StarboundClient.current(), color: RGBAColor = RGBAColor.LIGHT_GREEN) {
		if (isEmpty) return

		val program = client.programs.position
		val lines = program.builder.builder
		program.use()
		lines.begin(GeometryType.LINES)

		val normalSize = (aabb.width.coerceAtMost(aabb.height) / 3.0).coerceAtLeast(0.1)

		for (edge in edges) {
			val current = edge.p0
			val next = edge.p1
			lines.vertex(client.stack.last(), current)
			lines.vertex(client.stack.last(), next)
			lines.vertex(client.stack.last(), (next + current) / 2.0)
			lines.vertex(client.stack.last(), (next + current) / 2.0 + edge.normal * normalSize)
		}

		program.modelMatrix = identity
		program.colorMultiplier = color
		program.builder.upload()
		program.builder.draw(GL_LINES)
	}

	override fun toString(): String {
		return "Poly[aabb = $aabb; edges = $edges; vertices = $vertices]"
	}

	override fun equals(other: Any?): Boolean {
		return other === this || other is Poly && vertices == other.vertices
	}

	override fun hashCode(): Int {
		return vertices.hashCode()
	}

	fun write(stream: DataOutputStream, isLegacy: Boolean) {
		if (isLegacy) {
			stream.writeCollection(vertices) { writeStruct2f(it.toFloatVector()) }
		} else {
			stream.writeCollection(vertices) { writeStruct2d(it) }
		}
	}

	companion object : TypeAdapterFactory {
		val CODEC = nativeCodec(::read, Poly::write)
		val LEGACY_CODEC = legacyCodec(::read, Poly::write)

		val EMPTY = Poly(ImmutableList.of(), ImmutableList.of())

		private val identity = Matrix3f.identity()

		fun read(stream: DataInputStream, isLegacy: Boolean): Poly {
			if (isLegacy) {
				return Poly(stream.readCollection { readVector2f().toDoubleVector() })
			} else {
				return Poly(stream.readCollection { readVector2d() })
			}
		}

		override fun <T : Any?> create(gson: Gson, type: TypeToken<T>): TypeAdapter<T>? {
			if (type.rawType === Poly::class.java) {
				return object : TypeAdapter<Poly>() {
					private val list = gson.listAdapter<Vector2d>()

					override fun write(out: JsonWriter, value: Poly?) {
						list.write(out, value?.vertices)
					}

					override fun read(`in`: JsonReader): Poly? {
						if (`in`.consumeNull())
							return null
						else
							return Poly(list.read(`in`))
					}
				} as TypeAdapter<T>
			}

			return null
		}

		fun quickhull(points: Collection<Vector2d>): Poly {
			val sorted = ArrayList(points)

			sorted.sortWith { o1, o2 ->
				val cmp = o1.x.compareTo(o2.x)
				if (cmp == 0) o1.y.compareTo(o2.y) else cmp
			}

			// calculates on which side b is lying on o->a line
			fun cross(o: Vector2d, a: Vector2d, b: Vector2d): Double {
				return (a.x - o.x) * (b.y - o.y) - (a.y - o.y) * (b.x - o.x)
			}

			val lowerPoints = LinkedList<Vector2d>()
			val upperPoints = LinkedList<Vector2d>()

			for (point in sorted) {
				while (lowerPoints.size > 1 && cross(lowerPoints[lowerPoints.size - 2], lowerPoints[lowerPoints.size - 1], point) <= 0)
					lowerPoints.removeLast()

				lowerPoints.add(point)
			}

			for (point in sorted.asReversed()) {
				while (upperPoints.size > 1 && cross(upperPoints[upperPoints.size - 2], upperPoints[upperPoints.size - 1], point) <= 0)
					upperPoints.removeLast()

				upperPoints.add(point)
			}

			lowerPoints.removeLast()
			upperPoints.removeLast()

			lowerPoints.addAll(upperPoints)
			return Poly(lowerPoints)
		}
	}
}
