package ru.dbotthepony.kstarbound.world.entities

import it.unimi.dsi.fastutil.objects.ObjectArrayList
import ru.dbotthepony.kommons.io.DoubleValueCodec
import ru.dbotthepony.kommons.io.FloatValueCodec
import ru.dbotthepony.kommons.io.StreamCodec
import ru.dbotthepony.kommons.io.koptional
import ru.dbotthepony.kommons.io.map
import ru.dbotthepony.kommons.math.linearInterpolation
import ru.dbotthepony.kommons.util.AABB
import ru.dbotthepony.kommons.util.KOptional
import ru.dbotthepony.kommons.util.Listenable
import ru.dbotthepony.kommons.util.getValue
import ru.dbotthepony.kommons.util.setValue
import ru.dbotthepony.kommons.vector.Vector2d
import ru.dbotthepony.kommons.vector.times
import ru.dbotthepony.kstarbound.Globals
import ru.dbotthepony.kstarbound.defs.MovementParameters
import ru.dbotthepony.kstarbound.math.Interpolator
import ru.dbotthepony.kstarbound.network.syncher.NetworkedGroup
import ru.dbotthepony.kstarbound.network.syncher.networkedBoolean
import ru.dbotthepony.kstarbound.network.syncher.networkedData
import ru.dbotthepony.kstarbound.network.syncher.networkedFixedPoint
import ru.dbotthepony.kstarbound.network.syncher.networkedFloat
import ru.dbotthepony.kstarbound.network.syncher.networkedPoly
import ru.dbotthepony.kstarbound.world.EntityIndex
import ru.dbotthepony.kstarbound.world.World
import ru.dbotthepony.kstarbound.world.physics.CollisionPoly
import ru.dbotthepony.kstarbound.world.physics.CollisionType
import ru.dbotthepony.kstarbound.world.physics.Poly
import java.util.LinkedList
import kotlin.math.PI
import kotlin.math.absoluteValue
import kotlin.math.acos
import kotlin.math.cos
import kotlin.math.sin

open class MovementController() {
	private var world0: World<*, *>? = null
	val world: World<*, *> get() = world0!!
	private var spatialEntry: EntityIndex.Entry? = null

	fun initialize(world: World<*, *>, entry: EntityIndex.Entry?) {
		fixtures.clear()
		spatialEntry?.remove()
		this.world0 = world
		spatialEntry = entry
	}

	fun remove() {
		fixtures.clear()
		spatialEntry?.remove()
		spatialEntry = null
		world0 = null
	}

	fun computeLocalHitboxes(): List<Poly> {
		val poly = movementParameters.collisionPoly ?: return listOf()

		if (poly.isLeft) {
			return listOf(poly.left().rotate(rotation) + position)
		} else {
			val build = ObjectArrayList<Poly>(poly.right().size)

			for (p in poly.right()) {
				build.add(p.rotate(rotation) + position)
			}

			return build
		}
	}

	fun computeCollisionAABB(): AABB {
		val poly = movementParameters.collisionPoly ?: return AABB.ZERO + position

		if (poly.isLeft) {
			if (poly.left().isEmpty)
				return AABB.ZERO + position

			return (poly.left().rotate(rotation) + position).aabb
		} else {
			if (poly.right().isEmpty())
				return AABB.ZERO + position
			else if (poly.right().first().isEmpty)
				return AABB.ZERO + position

			var build = (poly.right().first().rotate(rotation) + position).aabb

			for (i in 1 until poly.right().size) {
				val gPoly = poly.right()[i]

				if (gPoly.isNotEmpty)
					build = build.combine((gPoly.rotate(rotation) + position).aabb)
			}

			return build
		}
	}

	open fun shouldCollideWithType(type: CollisionType): Boolean {
		return type !== CollisionType.NONE
	}

	open fun shouldCollideWithBody(body: CollisionPoly): Boolean {
		return shouldCollideWithType(body.type)
	}

	private val legacyPoly = networkedPoly(Poly.EMPTY)

	val networkGroup = NetworkedGroup(legacyPoly)

	var mass by networkGroup.add(networkedFloat())

	private var xPosition by networkGroup.add(networkedFixedPoint(0.0125))
	private var yPosition by networkGroup.add(networkedFixedPoint(0.0125))
	private var xVelocity by networkGroup.add(networkedFixedPoint(0.00625).also { it.interpolator = Interpolator.Linear })
	private var yVelocity by networkGroup.add(networkedFixedPoint(0.00625).also { it.interpolator = Interpolator.Linear })

	var rotation by networkGroup.add(networkedFixedPoint(0.01).also { it.interpolator = Interpolator.Linear })

	var isColliding: Boolean by networkGroup.add(networkedBoolean())
		protected set
	var isCollisionStuck: Boolean by networkGroup.add(networkedBoolean())
		protected set
	var isCollidingWithNull: Boolean by networkGroup.add(networkedBoolean())
		protected set

	private val stickingDirectionField = networkGroup.add(networkedData(KOptional(), DoubleValueCodec.koptional(), FloatValueCodec.map({ toDouble() }, { toFloat() }).koptional()))

	var isOnGround: Boolean by networkGroup.add(networkedBoolean())
		protected set
	var isZeroGravity: Boolean by networkGroup.add(networkedBoolean())
		protected set

	private val surfaceMovingCollisionField = networkGroup.add(networkedData(KOptional(), StreamCodec.Impl(::MovingCollisionID, { a, b -> b.write(a) }).koptional()))
	private val relativeXSurfaceVelocity = networkGroup.add(networkedFixedPoint(0.0125).also { it.interpolator = Interpolator.Linear })
	private val relativeYSurfaceVelocity = networkGroup.add(networkedFixedPoint(0.0125).also { it.interpolator = Interpolator.Linear })

	private val fixtures = ArrayList<EntityIndex.Entry.Fixture>()
	var fixturesChangeset: Int = 0
		private set

	fun updateFixtures() {
		val spatialEntry = spatialEntry ?: return
		fixturesChangeset++
		val poly = movementParameters.collisionPoly ?: return
		if (poly.isLeft && poly.left().isEmpty) return
		if (poly.isRight && poly.right().none { it.isNotEmpty }) return

		val localHitboxes = computeLocalHitboxes()

		while (fixtures.size > localHitboxes.size) {
			fixtures.last().remove()
		}

		while (fixtures.size < localHitboxes.size) {
			fixtures.add(spatialEntry.Fixture())
		}

		for ((i, hitbox) in localHitboxes.withIndex()) {
			fixtures[i].move(hitbox.aabb)
		}
	}

	var position: Vector2d
		get() = Vector2d(xPosition, yPosition)
		set(value) {
			xPosition = value.x
			yPosition = value.y
			updateFixtures()
		}

	val positionListeners = Listenable.Impl<Vector2d>()

	// Movement variables
	var stickingDirection: Double?
		get() = stickingDirectionField.get().orNull()
		protected set(value) {
			stickingDirectionField.accept(KOptional.ofNullable(value))
		}
	var surfaceSlope: Vector2d = Vector2d.ZERO
		protected set
	var surfaceVelocity: Vector2d = Vector2d.ZERO
		protected set
	var collisionCorrection: Vector2d = Vector2d.ZERO
		protected set
	var liquidPercentage: Double = 0.0
		protected set

	var appliedForceRegion: Boolean = false
		protected set

	var movementParameters: MovementParameters = Globals.movementParameters
		protected set

	var gravityMultiplier = 1.0
	var isGravityDisabled = false

	var velocity: Vector2d
		get() = Vector2d(xVelocity, yVelocity)
		set(value) {
			xVelocity = value.x
			yVelocity = value.y
		}

	fun determineGravity(): Vector2d {
		if (isZeroGravity || isGravityDisabled)
			return Vector2d.ZERO

		return world.gravityAt(position)
	}

	open fun updateLiquidPercentage() {

	}

	open fun updateForceRegions() {

	}

	fun approachVelocity(targetVelocity: Vector2d, maxControlForce: Double, delta: Double) {
		// Instead of applying the force directly, work backwards and figure out the
		// maximum acceleration that could be achieved by the current control force,
		// and maximize the change in velocity based on that.

		val diff = targetVelocity - velocity
		val mag = diff.length

		if (mag == 0.0) return

		val maximumAcceleration = maxControlForce / mass * delta
		val clampedMag = mag.coerceIn(0.0, maximumAcceleration)

		velocity += diff * (clampedMag / mag)
	}

	fun approachVelocityAlongAngle(angle: Double, targetVelocity: Double, maxControlForce: Double, delta: Double, positiveOnly: Boolean = false) {
		// Same strategy as approachVelocity, work backwards to figure out the
		// maximum acceleration and apply that.

		// Project the current velocity along the axis normal, the velocity
		// difference is the difference between the targetVelocity and this
		// projection.

		val axis = Vector2d(cos(angle), sin(angle))
		val velocityAlongAxis = velocity.dot(axis)
		val diff = targetVelocity - velocityAlongAxis

		if (diff == 0.0 || positiveOnly && diff < 0.0) return

		val maximumAcceleration = maxControlForce / mass * delta
		val diffMag = diff.absoluteValue
		val clampedMag = diffMag.coerceIn(0.0, maximumAcceleration)

		velocity += axis * diff * (clampedMag / diffMag)
	}

	fun approachXVelocity(velocity: Double, maxControlForce: Double, delta: Double) {
		approachVelocityAlongAngle(0.0, velocity, maxControlForce, delta)
	}

	fun approachYVelocity(velocity: Double, maxControlForce: Double, delta: Double) {
		approachVelocityAlongAngle(PI / 2.0, velocity, maxControlForce, delta)
	}

	/**
	 * this function is executed in parallel
	 */
	// TODO: Ghost collisions occur, where objects trip on edges
	open fun move(delta: Double) {
		isZeroGravity = isGravityDisabled || gravityMultiplier == 0.0 || determineGravity().lengthSquared == 0.0

		val movementParameters = movementParameters

		movementParameters.speedLimit?.let {
			if (velocity.length > it)
				velocity = velocity.unitVector * it
		}

		// TODO: Here: moving platforms sticky code

		if (movementParameters.collisionPoly == null || !movementParameters.collisionPoly.map({ true }, { it.isNotEmpty() }) || movementParameters.collisionEnabled != true) {
			position += velocity * delta
			surfaceSlope = Vector2d.POSITIVE_Y
			surfaceVelocity = Vector2d.ZERO
			isOnGround = false
			stickingDirection = null
			isColliding = false
			isCollidingWithNull = false
			isCollisionStuck = false
			return
		}

		var steps = 1

		movementParameters.maxMovementPerStep?.let {
			steps = (velocity.length * delta / it).toInt() + 1
		}

		var relativeVelocity = velocity
		surfaceSlope = Vector2d.POSITIVE_Y
		// TODO: Here: moving platforms sticky code

		val dt = delta / steps

		for (step in 0 until steps) {
			val velocityMagnitude = relativeVelocity.length
			val velocityDirection = relativeVelocity / velocityMagnitude
			val movement = relativeVelocity * dt

			val ignorePlatforms = (movementParameters.ignorePlatformCollision ?: false) || relativeVelocity.y > 0.0
			val maximumCorrection = movementParameters.maximumCorrection ?: 0.0
			val maximumPlatformCorrection = (movementParameters.maximumPlatformCorrection ?: Double.POSITIVE_INFINITY) +
				(movementParameters.maximumPlatformCorrectionVelocityFactor ?: 0.0) * velocityMagnitude

			val localHitboxes = computeLocalHitboxes()

			if (localHitboxes.isEmpty())
				return // whut

			var aabb = localHitboxes.first().aabb

			for (i in 1 until localHitboxes.size) {
				aabb = aabb.combine(localHitboxes[i].aabb)
			}

			var queryBounds = aabb.enlarge(maximumCorrection, maximumCorrection)
			queryBounds = queryBounds.combine(queryBounds + movement)

			val polies = world.queryTileCollisions(queryBounds)
			polies.removeIf { !shouldCollideWithBody(it) }

			val results = ArrayList<CollisionResult>(localHitboxes.size)

			for (hitbox in localHitboxes) {
				results.add(collisionSweep(hitbox, polies, movement, ignorePlatforms, (movementParameters.enableSurfaceSlopeCorrection ?: false) && !isZeroGravity, maximumCorrection, maximumPlatformCorrection, aabb.centre))
			}

			val result = results.minOrNull()!!
			position += result.movement

			if (result.collisionType == CollisionType.NULL) {
				isCollidingWithNull = true
				break
			} else {
				isCollidingWithNull = false
			}

			val correction = result.correction
			val normCorrection = correction.unitVector
			surfaceSlope = result.groundSlope
			collisionCorrection = result.correction
			isColliding = correction != Vector2d.ZERO || result.isStuck
			isOnGround = !isZeroGravity && result.isOnGround
			isCollisionStuck = result.isStuck

			// If we have collided, apply either sticky or normal (bouncing) collision physics
			if (correction != Vector2d.ZERO) {
				if (movementParameters.stickyCollision == true && result.collisionType !== CollisionType.SLIPPERY) {
					// When sticking, cancel all velocity and apply stickyForce in the
					// opposite of the direction of collision correction.
					relativeVelocity = -normCorrection * (movementParameters.stickyForce ?: 0.0) / mass * dt
					stickingDirection = -normCorrection.toAngle()
					break
				} else {
					stickingDirection = null
					val correctionDirection = correction.unitVector

					if (movementParameters.bounceFactor != null && movementParameters.bounceFactor != 0.0) {
						val adjustment = correctionDirection * (velocityMagnitude * (correctionDirection * -velocityDirection))
						relativeVelocity += adjustment + movementParameters.bounceFactor * adjustment

						if (movementParameters.stopOnFirstBounce == true) {
							// When bouncing, stop integrating at the moment of bounce.  This
							// prevents the frame of contact from being missed due to multiple
							// iterations per frame.
							break
						}
					} else {
						// Only adjust the velocity to the extent that the collision was
						// caused by the velocity in each axis, to eliminate collision
						// induced velocity in a platformery way (each axis considered
						// independently).

						if (relativeVelocity.x < 0.0 && correction.x > 0.0)
							relativeVelocity = relativeVelocity.copy(x = (relativeVelocity.x + correction.x / dt).coerceAtMost(0.0))
						else if (relativeVelocity.x > 0.0 && correction.x < 0.0)
							relativeVelocity = relativeVelocity.copy(x = (relativeVelocity.x + correction.x / dt).coerceAtLeast(0.0))

						if (relativeVelocity.y < 0.0 && correction.y > 0.0)
							relativeVelocity = relativeVelocity.copy(y = (relativeVelocity.y + correction.y / dt).coerceAtMost(0.0))
						else if (relativeVelocity.y > 0.0 && correction.y < 0.0)
							relativeVelocity = relativeVelocity.copy(y = (relativeVelocity.y + correction.y / dt).coerceAtLeast(0.0))
					}
				}
			}
		}

		var newVelocity = relativeVelocity

		updateLiquidPercentage()

		// TODO: sticky collision update

		// In order to make control work accurately, passive forces need to be
		// applied to velocity *after* integrating.  This prevents control from
		// having to account for one timestep of passive forces in order to result
		// in the correct controlled movement.
		if (!isZeroGravity && stickingDirection == null) {
			val buoyancy = (movementParameters.liquidBuoyancy ?: 0.0).coerceIn(0.0, 1.0) + liquidPercentage + (movementParameters.airBuoyancy ?: 0.0).coerceIn(0.0, 1.0) * (1.0 - liquidPercentage)
			val gravity = determineGravity() * (movementParameters.gravityMultiplier ?: 1.0) * (1.0 - buoyancy)
			var environmentVelocity = -gravity * delta

			if (isOnGround && (movementParameters.slopeSlidingFactor ?: 0.0) != 0.0 && surfaceSlope != Vector2d.ZERO)
				environmentVelocity += -surfaceSlope * (surfaceSlope.x * surfaceSlope.y) * (movementParameters.slopeSlidingFactor ?: 0.0)

			newVelocity += environmentVelocity
		}

		if (movementParameters.frictionEnabled == true) {
			var refVel = Vector2d.ZERO
			var friction = liquidPercentage * (movementParameters.liquidFriction ?: 0.0).coerceIn(0.0, 1.0) + (1.0 - liquidPercentage) * (movementParameters.airFriction ?: 0.0).coerceIn(0.0, 1.0)

			if (isOnGround) {
				friction = friction.coerceAtLeast(movementParameters.groundFriction ?: 0.0)
				refVel = surfaceVelocity
			}

			// The equation for friction here is effectively:
			// frictionForce = friction * (refVel - velocity)
			// but it is applied here as a multiplicative factor from [0, 1] so it does
			// not induce oscillation at very high friction and so it cannot be
			// negative.
			val frictionFactor = (friction / mass * delta).coerceIn(0.0, 1.0)
			newVelocity = linearInterpolation(frictionFactor, newVelocity, refVel)
		}

		velocity = newVelocity
		updateForceRegions()
	}

	protected data class CollisionSeparation(
		var correction: Vector2d = Vector2d.ZERO,
		var solutionFound: Boolean = false,
		var collisionType: CollisionType = CollisionType.NONE,
		var axis: Vector2d = Vector2d.POSITIVE_Y,
		var movingCollisionId: Int? = null, // TODO
	)

	protected data class CollisionResult(
		val movement: Vector2d = Vector2d.ZERO,
		val correction: Vector2d = Vector2d.ZERO,
		var movingCollisionId: Int? = null, // TODO
		var isStuck: Boolean = false,
		var isOnGround: Boolean = false,
		var groundSlope: Vector2d = Vector2d.ZERO,
		var collisionType: CollisionType = CollisionType.NULL,
	) : Comparable<CollisionResult> {
		override fun compareTo(other: CollisionResult): Int {
			return movement.lengthSquared.compareTo(other.movement.lengthSquared)
		}
	}

	protected data class BodyPair(val body: CollisionPoly, val distance: Double)

	protected fun collisionSweep(
		body: Poly, staticBodies: List<CollisionPoly>,
		movement: Vector2d, ignorePlatforms: Boolean,
		slopeCorrection: Boolean, maximumCorrection: Double,
		maximumPlatformCorrection: Double, sortCenter: Vector2d
	): CollisionResult {
		val translatedBody = body + movement
		var checkBody = translatedBody
		var maxCollided = CollisionType.NONE

		var separation = CollisionSeparation()
		var totalCorrection = Vector2d.ZERO
		var movingCollisionId: Int? = null

		val sorted = ObjectArrayList<BodyPair>(staticBodies.size)

		for (sbody in staticBodies) {
			sorted.add(BodyPair(sbody, (sbody.poly.aabb.centre - sortCenter).lengthSquared))
		}

		sorted.sortWith { o1, o2 -> o1.distance.compareTo(o2.distance) }

		if (slopeCorrection) {
			// Starbound: First try separating with our ground sliding cheat.
			separation = collisionSeparate(checkBody, sorted, ignorePlatforms, maximumPlatformCorrection, true, SEPARATION_TOLERANCE)
			totalCorrection += separation.correction
			checkBody += separation.correction
			maxCollided = maxCollided.maxOf(separation.collisionType)
			movingCollisionId = separation.movingCollisionId

			val upwardResult = movement + separation.correction
			val upwardMagnitude = upwardResult.length
			val upwardUnit = upwardResult / upwardMagnitude
			// Starbound: Angle off of horizontal (minimum of either direction)
			val horizontalAngle = acos(Vector2d.POSITIVE_X.dot(upwardUnit)).coerceAtMost(acos(Vector2d.NEGATIVE_X.dot(upwardUnit)))

			// Starbound: We need to make sure that even if we found a solution with the sliding
			// Starbound: cheat, we are not beyond the angle and correction limits for the ground
			// Starbound: cheat correction.
			separation.solutionFound = separation.solutionFound && (upwardMagnitude < 0.2 || horizontalAngle < PI / 3.0) && totalCorrection.length <= maximumCorrection

			// KStarbound: if we got pushed into world geometry, then consider slide cheat didn't find a solution
			if (separation.solutionFound) {
				separation.solutionFound = staticBodies.all { it.poly.intersect(checkBody).let { it == null || it.penetration.absoluteValue <= SEPARATION_TOLERANCE } }
			}
		}

		if (!separation.solutionFound) {
			checkBody = translatedBody
			totalCorrection = Vector2d.ZERO
			movingCollisionId = null

			for (i in 0 until SEPARATION_STEPS) {
				separation = collisionSeparate(checkBody, sorted, ignorePlatforms, maximumPlatformCorrection, false, SEPARATION_TOLERANCE)
				totalCorrection += separation.correction
				checkBody += separation.correction
				maxCollided = maxCollided.maxOf(separation.collisionType)

				if (totalCorrection.length >= maximumCorrection) {
					separation.solutionFound = false
					break
				} else if (separation.solutionFound) {
					break
				}
			}
		}

		if (!separation.solutionFound && movement != Vector2d.ZERO) {
			checkBody = body
			totalCorrection = -movement

			for (i in 0 until SEPARATION_STEPS) {
				separation = collisionSeparate(checkBody, sorted, true, maximumPlatformCorrection, false, SEPARATION_TOLERANCE)
				totalCorrection += separation.correction
				checkBody += separation.correction
				maxCollided = maxCollided.maxOf(separation.collisionType)

				if (totalCorrection.length >= maximumCorrection) {
					separation.solutionFound = false
					break
				} else if (separation.solutionFound) {
					break
				}
			}
		}

		if (separation.solutionFound) {
			val result = CollisionResult(
				movement = movement + totalCorrection,
				correction = totalCorrection,
				isStuck = false,
				isOnGround = -totalCorrection.dot(determineGravity()) > SEPARATION_TOLERANCE,
				movingCollisionId = movingCollisionId,
				collisionType = maxCollided,
				// groundSlope = Vector2d.POSITIVE_Y,
				groundSlope = separation.axis
			)

			// TODO: what they wanted to achieve with this?
			/*if (result.isOnGround) {
				// If we are on the ground and need to find the ground slope, look for a
			    // vertex on the body being moved that is touching an edge of one of the
			    // collision polys.  We only want a slope to be produced from an edge of
			    // colision geometry, not an edge of the colliding body.  Pick the
			    // touching edge that is the most horizontally overlapped with the
			    // geometry, rather than off to the side.
				var maxSideHorizontalOverlap = 0.0
				var touchingBounds = checkBody.aabb.enlarge(SEPARATION_TOLERANCE, SEPARATION_TOLERANCE)

				for (poly in staticBodies) {
					for (edge in poly.poly.edges) {

					}
				}
			}*/

			return result
		} else {
			return CollisionResult(Vector2d.ZERO, -movement, null, true, true, Vector2d.POSITIVE_Y, maxCollided)
		}
	}

	protected fun collisionSeparate(
		poly: Poly, staticBodies: List<BodyPair>,
		ignorePlatforms: Boolean, maximumPlatformCorrection: Double,
		upward: Boolean, separationTolerance: Double
	): CollisionSeparation {
		val separation = CollisionSeparation()
		var intersects = false
		var correctedPoly = poly

		for ((body) in staticBodies) {
			if (ignorePlatforms && body.type === CollisionType.PLATFORM)
				continue

			var result = if (upward)
				correctedPoly.intersect(body.poly, Vector2d.POSITIVE_Y, false)
			else if (body.type == CollisionType.PLATFORM)
				correctedPoly.intersect(body.poly, Vector2d.POSITIVE_Y, true)
			else
				correctedPoly.intersect(body.poly)

			if (body.type === CollisionType.PLATFORM && result != null && (result.penetration <= 0.0 || result.penetration > maximumPlatformCorrection))
				result = null

			if (result != null) {
				intersects = true
				correctedPoly += result.vector
				separation.correction += result.vector
				separation.collisionType = separation.collisionType.maxOf(body.type)
			}
		}

		separation.solutionFound = true

		if (intersects) {
			for ((body) in staticBodies) {
				if (body.type === CollisionType.PLATFORM)
					continue

				val result = correctedPoly.intersect(body.poly)

				if (result != null && result.penetration > separationTolerance) {
					separation.collisionType = separation.collisionType.maxOf(body.type)
					separation.solutionFound = false
					break
				}
			}
		}

		return separation
	}

	fun applyParameters(changes: MovementParameters) {
		updateParameters(this.movementParameters.merge(changes))
	}

	fun updateParameters(parameters: MovementParameters) {
		this.movementParameters = parameters
		this.mass = parameters.mass ?: this.mass
	}

	companion object {
		const val SEPARATION_STEPS = 3
		const val SEPARATION_TOLERANCE = 0.001
	}
}
