package ru.dbotthepony.kbox2d.dynamics

import ru.dbotthepony.kbox2d.api.*
import ru.dbotthepony.kbox2d.collision.DistanceProxy
import ru.dbotthepony.kbox2d.collision.b2TimeOfImpact
import ru.dbotthepony.kbox2d.collision.shapes.ChainShape
import ru.dbotthepony.kbox2d.collision.shapes.CircleShape
import ru.dbotthepony.kbox2d.collision.shapes.EdgeShape
import ru.dbotthepony.kbox2d.collision.shapes.PolygonShape
import ru.dbotthepony.kbox2d.dynamics.contact.AbstractContact
import ru.dbotthepony.kbox2d.dynamics.internal.Island
import ru.dbotthepony.kbox2d.dynamics.joint.AbstractJoint
import ru.dbotthepony.kstarbound.math.AABB
import ru.dbotthepony.kstarbound.math.Vector2d
import ru.dbotthepony.kstarbound.math.times
import ru.dbotthepony.kstarbound.util.Color

class B2World(override var gravity: Vector2d) : IB2World {
	override var bodyCount: Int = 0
		private set
	override var jointCount: Int = 0
		private set

	override val contactManager: IContactManager = ContactManager()

	override var destructionListener: IDestructionListener? = null
	override var contactFilter: IContactFilter? by contactManager::contactFilter
	override var contactListener: IContactListener? by contactManager::contactListener

	override var debugDraw: IDebugDraw? = null

	override var bodyList: IBody? = null
		private set
	override var jointList: IJoint? = null
		private set

	override var warmStarting: Boolean = true
	override var continuousPhysics: Boolean = true
	override var enableSubStepping: Boolean = false

	override var allowAutoSleep: Boolean = true
		set(value) {
			if (value == field)
				return

			field = value

			if (!value) {
				for (body in bodyListIterator) {
					body.isAwake = true
				}
			}
		}

	override var isLocked: Boolean = false
		private set

	override var autoClearForces: Boolean = true

	private var stepComplete = true
	private var newContacts = false

	private val profile = ProfileData()

	override fun notifyNewContacts() {
		newContacts = true
	}

	override fun createBody(bodyDef: BodyDef): IBody {
		if (isLocked)
			throw ConcurrentModificationException()

		val body = Body(bodyDef, this)

		body.next = bodyList
		(bodyList as Body?)?.prev = body
		bodyList = body
		bodyCount++

		return body
	}

	override fun destroyBody(body: IBody) {
		if (isLocked)
			throw ConcurrentModificationException()

		check(body.world == this) { "$body does not belong to $this" }
		check(bodyCount > 0) { "I have ${bodyCount} bodies, can't remove one" }

		// Delete the attached joints.
		for (jointEdge in body.jointIterator) {
			destructionListener?.sayGoodbye(jointEdge.joint)
			destroyJoint(jointEdge.joint)
		}

		// Delete the attached contacts.
		for (contactEdge in body.contactEdgeIterator) {
			contactManager.destroy(contactEdge.contact)
		}

		// Delete the attached fixtures. This destroys broad-phase proxies.
		for (fixture in body.fixtureIterator) {
			destructionListener?.sayGoodbye(fixture)
			(fixture as Fixture).destroyProxies(contactManager.broadPhase)
		}

		// Remove world body list.
		val prev = body.prev as Body?
		val next = body.next as Body?

		prev?.next = next
		next?.prev = prev

		if (body == bodyList) {
			bodyList = next ?: prev
		}

		bodyCount--
		(body as Body).unlink()
	}

	override fun createJoint(jointDef: IJointDef): AbstractJoint {
		if (isLocked)
			throw ConcurrentModificationException()

		val joint = AbstractJoint.create(jointDef.type, jointDef)

		// Connect to the world list.
		joint.next = jointList
		(jointList as AbstractJoint?)?.prev = joint
		jointList = joint
		jointCount++

		if (joint.hasTwoBodies) {
			val bodyA = joint.bodyA
			val bodyB = joint.bodyB

			// If the joint prevents collisions, then flag any contacts for filtering.
			if (!joint.collideConnected) {
				for (edge in bodyB.contactEdgeIterator) {
					if (edge.other == bodyA) {
						edge.contact.flagForFiltering()
					}
				}
			}
		}

		// Note: creating a joint doesn't wake the bodies.

		return joint
	}

	override fun destroyJoint(joint: IJoint) {
		if (isLocked)
			throw ConcurrentModificationException()

		check(jointCount > 0) { "No joints tracked to remove" }
		joint as AbstractJoint
		require((joint.nullableBodyA?.world == this || joint.nullableBodyA == null) && (joint.nullableBodyB?.world == this || joint.nullableBodyB == null)) { "$joint does not belong to $this" }

		if (!joint.isValid) {
			throw IllegalStateException("Joint $joint is already destroyed")
		}

		// Remove from the doubly linked list.
		this.run {
			val prev = joint.prev as AbstractJoint?
			val next = joint.next as AbstractJoint?

			prev?.next = next
			next?.prev = prev

			if (joint == this.jointList) {
				this.jointList = next ?: prev
			}
		}

		// Disconnect from island graph.
		val bodyA = joint.nullableBodyA
		val bodyB = joint.nullableBodyB

		// Wake up connected bodies.
		bodyA?.isAwake = true
		bodyB?.isAwake = true

		// Remove from body 1.
		this.run {
			val edgeA = joint.edgeA

			val prev = edgeA.prev
			val next = edgeA.next

			prev?.next = next
			next?.prev = prev

			if (bodyA?.jointList == edgeA) {
				bodyA.jointList = next ?: prev
			}
		}

		// Remove from body 2
		this.run {
			val edgeB = joint.edgeB

			val prev = edgeB.prev
			val next = edgeB.next

			prev?.next = next
			next?.prev = prev

			if (bodyB?.jointList == edgeB) {
				bodyB.jointList = next ?: prev
			}
		}

		jointCount--
		joint.unlink()

		if (!joint.collideConnected && bodyB != null && bodyA != null) {
			for (edge in bodyB.contactEdgeIterator) {
				if (edge.other == bodyA) {
					edge.contact.flagForFiltering()
				}
			}
		}
	}

	private val _profile = ProfileData()

	/**
	 * Find islands, integrate and solve constraints, solve position constraints
	 */
	internal fun solve(step: B2TimeStep) {
		_profile.solveInit = 0L
		_profile.solveVelocity = 0L
		_profile.solvePosition = 0L

		// Size the island for the worst case.
		// TODO: Kotlin: or do we size it??
		val island = Island(listener = contactListener)

		// Clear all the island flags.
		for (body in bodyListIterator) {
			body as Body
			body.isOnIsland = false
		}

		for (contact in contactListIterator) {
			contact as AbstractContact
			contact.isOnIsland = false
		}

		for (joint in jointListIterator) {
			joint as AbstractJoint
			check(joint.isValid) { "$joint is no longer valid, but present in linked list" }
			joint.isOnIsland = false
		}

		// Build and simulate all awake islands.
		for (seed in bodyListIterator) {
			seed as Body

			if (seed.type == BodyType.STATIC || !seed.isAwake || !seed.isEnabled || seed.isOnIsland) {
				continue
			}

			// Reset island and stack.
			island.clear()
			val stack = ArrayDeque<Body>(32)
			stack.add(seed)
			seed.isOnIsland = true

			// Perform a depth first search (DFS) on the constraint graph.
			while (stack.isNotEmpty()) {
				// Grab the next body off the stack and add it to the island.
				val body = stack.removeLast()
				check(body.isEnabled)
				island.add(body)

				// To keep islands as small as possible, we don't
				// propagate islands across static bodies.
				if (body.type == BodyType.STATIC)
					continue

				// Make sure the body is awake (without resetting sleep timer).
				body.flags = BodyFlags.AWAKE.or(body.flags)

				// Search all contacts connected to this body.
				for (ce in body.contactEdgeIterator) {
					val contact = ce.contact as AbstractContact

					// Has this contact already been added to an island?
					if (contact.isOnIsland)
						continue

					// Is this contact solid and touching?
					if (!contact.isEnabled || !contact.isTouching)
						continue

					// Skip sensors.
					if (contact.fixtureA.isSensor || contact.fixtureB.isSensor)
						continue

					island.add(contact)
					contact.isOnIsland = true

					val other = ce.other as Body

					// Was the other body already added to this island?
					if (!other.isOnIsland) {
						stack.add(other)
						other.isOnIsland = true
					}
				}

				// Search all joints connect to this body.
				for (je in body.jointIterator) {
					val joint = je.joint as AbstractJoint
					check(joint.isValid) { "$joint is no longer valid, but present in linked list of $body" }

					if (joint.isOnIsland)
						continue

					val other = je.otherNullable as Body?

					// Don't simulate joints connected to disabled bodies.
					if (other != null && !other.isEnabled)
						continue

					island.add(joint)
					joint.isOnIsland = true

					if (other != null && !other.isOnIsland) {
						stack.add(other)
						other.isOnIsland = true
					}
				}
			}

			val profile = ProfileData()
			island.solve(profile, step, gravity, allowAutoSleep)
			this.profile.solveInit += profile.solveInit
			this.profile.solveVelocity += profile.solveVelocity
			this.profile.solvePosition += profile.solvePosition
			this.profile.integratePositions += profile.integratePositions

			// Post solve cleanup.
			for (body in island.bodiesAccess) {
				// Allow static bodies to participate in other islands.
				if (body.type == BodyType.STATIC) {
					body.isOnIsland = false
				}
			}
		}

		val timer = System.nanoTime()

		// Synchronize fixtures, check for out of range bodies.
		for (body in bodyListIterator) {
			body as Body

			// If a body was not in an island then it did not move.
			if (!body.isOnIsland || body.type == BodyType.STATIC) {
				continue
			}

			// Update fixtures (for broad-phase).
			body.synchronizeFixtures()
		}

		// Look for new contacts.
		contactManager.findNewContacts()
		profile.broadphase = System.nanoTime() - timer
	}

	/**
	 * Find TOI contacts and solve them.
	 */
	fun solveTOI(step: B2TimeStep) {
		val island = Island(listener = contactListener)

		if (stepComplete) {
			for (body in bodyListIterator) {
				body as Body
				body.isOnIsland = false
				body.sweep.alpha0 = 0.0
			}

			for (c in contactManager.contactListIterator) {
				// Invalidate TOI
				c as AbstractContact
				c.isOnIsland = false
				c.toiFlag = false
				c.toiCount = 0
				c.toi = 1.0
			}
		}

		// Find TOI events and solve them.
		while (true) {
			var minContact: AbstractContact? = null
			var minAlpha = 1.0

			for (c in contactManager.contactListIterator) {
				if (!c.isEnabled) {
					continue
				}

				c as AbstractContact

				if (c.toiCount > b2_maxSubSteps) {
					continue
				}

				var alpha = 1.0

				if (c.toiFlag) {
					// This contact has a valid cached TOI.
					alpha = c.toi
				} else {
					val fA = c.fixtureA
					val fB = c.fixtureB

					// Is there a sensor?
					if (fA.isSensor || fB.isSensor) {
						continue
					}

					val bA = fA.body as Body
					val bB = fB.body as Body

					val typeA = bA.type
					val typeB = bB.type

					check(typeA == BodyType.DYNAMIC || typeB == BodyType.DYNAMIC)

					val activeA = bA.isAwake && typeA != BodyType.STATIC
					val activeB = bB.isAwake && typeB != BodyType.STATIC

					// Is at least one body active (awake and dynamic or kinematic)?
					if (!activeA && !activeB) {
						continue
					}

					val collideA = bA.isBullet || typeA != BodyType.DYNAMIC
					val collideB = bB.isBullet || typeB != BodyType.DYNAMIC

					// Are these two non-bullet dynamic bodies?
					if (!collideA && !collideB) {
						continue
					}

					// Compute the TOI for this contact.
					// Put the sweeps onto the same time interval.
					var alpha0 = bA.sweep.alpha0

					if (bA.sweep.alpha0 < bB.sweep.alpha0) {
						alpha0 = bB.sweep.alpha0
						bA.sweep.advance(alpha0)
					} else if (bA.sweep.alpha0 > bB.sweep.alpha0) {
						alpha0 = bA.sweep.alpha0
						bB.sweep.advance(alpha0)
					}

					check(alpha0 < 1.0) { alpha0 }

					val indexA = c.childIndexA
					val indexB = c.childIndexB

					// Compute the time of impact in interval [0, minTOI]
					val output = b2TimeOfImpact(
						proxyA = DistanceProxy(fA.shape, indexA),
						proxyB = DistanceProxy(fB.shape, indexB),
						_sweepA = bA.sweep,
						_sweepB = bB.sweep,
						tMax = 1.0
					)

					// Beta is the fraction of the remaining portion of the .
					val beta = output.t

					if (output.state == TOIOutput.State.TOUCHING) {
						alpha = 1.0.coerceAtMost(alpha0 + (1.0f - alpha0) * beta)
					} else {
						alpha = 1.0
					}

					c.toi = alpha
					c.toiFlag = true
				}

				if (alpha < minAlpha) {
					// This is the minimum TOI found so far.
					minContact = c
					minAlpha = alpha
				}
			}

			if (minContact == null || 1.0 - 10.0 * b2_epsilon < minAlpha) {
				// No more TOI events. Done!
				stepComplete = true
				break
			}

			// Advance the bodies to the TOI.
			val fA = minContact.fixtureA
			val fB = minContact.fixtureB
			val bA = fA.body as Body
			val bB = fB.body as Body

			val backup1 = bA.sweep.copy()
			val backup2 = bB.sweep.copy()

			bA.advance(minAlpha)
			bB.advance(minAlpha)

			// The TOI contact likely has some new contact points.
			minContact.update(contactManager.contactListener)
			minContact.toiFlag = false
			minContact.toiCount++

			// Is the contact solid?
			if (!minContact.isEnabled || !minContact.isTouching) {
				// Restore the sweeps.
				minContact.isEnabled = false
				bA.sweep.load(backup1)
				bB.sweep.load(backup2)
				bA.synchronizeTransform()
				bB.synchronizeTransform()
				continue
			}

			bA.isAwake = true
			bB.isAwake = true

			// Build the island
			island.clear()
			island.add(bA)
			island.add(bB)
			island.add(minContact)

			bA.isOnIsland = true
			bB.isOnIsland = true
			minContact.isOnIsland = true

			// Get contacts on bodyA and bodyB.
			val bodies = arrayOf(bA, bB)
			for (i in 0 .. 1) {
				val body = bodies[i]

				if (body.type == BodyType.DYNAMIC) {
					for (ce in body.contactEdgeIterator) {
						val contact = ce.contact as AbstractContact

						// Has this contact already been added to the island?
						if (contact.isOnIsland) {
							continue
						}

						// Only add static, kinematic, or bullet bodies.
						val other = ce.other

						if (other.type == BodyType.DYNAMIC && !body.isBullet && !other.isBullet) {
							continue
						}

						// Skip sensors.
						if (contact.fixtureA.isSensor || contact.fixtureB.isSensor) {
							continue
						}

						other as Body

						// Tentatively advance the body to the TOI.
						val backup = other.sweep
						if (!other.isOnIsland) {
							other.advance(minAlpha)
						}

						// Update the contact points
						contact.update(contactManager.contactListener)

						// Was the contact disabled by the user?
						if (!contact.isEnabled) {
							other.sweep.load(backup)
							other.synchronizeTransform()
							continue
						}

						// Are there contact points?
						if (!contact.isTouching) {
							other.sweep.load(backup)
							other.synchronizeTransform()
							continue
						}

						// Add the contact to the island
						contact.isOnIsland = true
						island.add(contact)

						// Has the other body already been added to the island?
						if (other.isOnIsland) {
							continue
						}

						// Add the other body to the island.
						other.isOnIsland = true

						if (other.type != BodyType.STATIC) {
							other.isAwake = true
						}

						island.add(other)
					}
				}
			}

			val subStepDT = (1.0 - minAlpha) * step.dt

			val subStep = B2TimeStep(
				dt = subStepDT,
				inv_dt = 1.0 / subStepDT,
				dtRatio = 1.0,
				positionIterations = 20,
				velocityIterations = step.velocityIterations,
				warmStarting = false,
			)

			island.solveTOI(subStep, bA.islandIndex, bB.islandIndex)

			// Reset island flags and synchronize broad-phase proxies.
			for (body in island.bodiesAccess) {
				body.isOnIsland = false
				if (body.type != BodyType.DYNAMIC) {
					continue
				}

				body.synchronizeFixtures()

				// Invalidate all contact TOIs on this displaced body.
				for (ce in body.contactEdgeIterator) {
					val contact = ce.contact as AbstractContact
					contact.toiFlag = false
					contact.isOnIsland = false
				}
			}

			// Commit fixture proxy movements to the broad-phase so that new contacts are created.
			// Also, some contacts can be destroyed.
			contactManager.findNewContacts()

			if (enableSubStepping) {
				stepComplete = false
				break
			}
		}
	}

	private var m_inv_dt0 = 0.0

	override fun step(dt: Double, velocityIterations: Int, positionIterations: Int) {
		var stepTimer = System.nanoTime()

		// If new fixtures were added, we need to find the new contacts.
		if (newContacts) {
			contactManager.findNewContacts()
			newContacts = false
		}

		isLocked = true

		try {
			val inv_dt: Double

			if (dt > 0.0) {
				inv_dt = 1.0 / dt
			} else {
				inv_dt = 0.0
			}

			val step = B2TimeStep(
				dt = dt,
				inv_dt = inv_dt,
				dtRatio = m_inv_dt0 * dt,
				warmStarting = warmStarting,
				velocityIterations = velocityIterations,
				positionIterations = positionIterations
			)

			// Update contacts. This is where some contacts are destroyed.
			this.run {
				val timer = System.nanoTime()
				this.contactManager.collide()
				this.profile.collide = System.nanoTime() - timer
			}

			// Integrate velocities, solve velocity constraints, and integrate positions.
			if (stepComplete && dt > 0.0) {
				val timer = System.nanoTime()
				solve(step)
				profile.solve = System.nanoTime() - timer
			}

			// Handle TOI events.
			if (continuousPhysics && dt > 0.0) {
				val timer = System.nanoTime()
				solveTOI(step)
				profile.solveTOI = System.nanoTime() - timer
			}

			if (dt > 0.0) {
				m_inv_dt0 = step.inv_dt
			}

			if (autoClearForces) {
				clearForces()
			}
		} catch(err: Throwable) {
			throw RuntimeException("Caught an exception simulating physics world", err)
		} finally {
			isLocked = false
		}

		profile.step = System.nanoTime() - stepTimer
	}

	override fun clearForces() {
		for (body in bodyListIterator) {
			body as Body
			body.force = Vector2d.ZERO
			body.torque = 0.0
		}
	}

	override fun queryAABB(aabb: AABB, callback: IQueryCallback) {
		contactManager.broadPhase.query(aabb) { nodeId, userData -> callback.reportFixture((userData as FixtureProxy).fixture) }
	}

	override fun rayCast(point1: Vector2d, point2: Vector2d, callback: IRayCastCallback) {
		val input = RayCastInput(point1, point2, 1.0)

		contactManager.broadPhase.rayCast(input, object : ProxyRayCastCallback {
			override fun invoke(subInput: RayCastInput, nodeId: Int, userData: Any?): Double {
				val proxy = userData as FixtureProxy
				val fixture = proxy.fixture
				val index = proxy.childIndex
				val output = fixture.rayCast(subInput, index)

				if (output.hit) {
					val point = (1.0 - output.fraction) * subInput.p1 + output.fraction * subInput.p2
					return callback.reportFixture(fixture, point, output.normal, output.fraction)
				}

				return subInput.maxFraction
			}
		})
	}

	private fun drawShape(fixture: IFixture, xf: Transform, color: Color) {
		when (fixture.type) {
			IShape.Type.CIRCLE -> {
				val circle = fixture.shape as CircleShape
				val center = b2Mul(xf, circle.p)
				val radius = circle.radius
				val axis = b2Mul(xf.q, Vector2d.RIGHT)

				debugDraw?.drawSolidCircle(center, radius, axis, color)
			}

			IShape.Type.EDGE -> {
				val edge = fixture.shape as EdgeShape
				val v1 = b2Mul(xf, edge.vertex1)
				val v2 = b2Mul(xf, edge.vertex2)

				debugDraw?.drawSegment(v1, v2, color)

				if (!edge.oneSided) {
					debugDraw?.drawPoint(v1, 4.0, color)
					debugDraw?.drawPoint(v2, 4.0, color)
				}
			}

			IShape.Type.POLYGON -> {
				val poly = fixture.shape as PolygonShape
				val vertices = poly.vertices.map { b2Mul(xf, it) }
				debugDraw?.drawSolidPolygon(vertices, color)
			}

			IShape.Type.CHAIN -> {
				val chain = fixture.shape as ChainShape
				var v1 = b2Mul(xf, chain.vertices[0])

				for (i in 1 until chain.vertices.size) {
					val v2 = b2Mul(xf, chain.vertices[i])
					debugDraw?.drawSegment(v1, v2, color)
					v1 = v2
				}
			}
		}
	}

	override fun debugDraw() {
		val debugDraw = debugDraw ?: return

		if (debugDraw.drawShapes) {
			for (body in bodyListIterator) {
				val xf = body.transform

				for (f in body.fixtureIterator) {
					if (body.type == BodyType.DYNAMIC && body.mass == 0.0) {
						// Bad body
						drawShape(f, xf, BAD_BODY_COLOR)
					} else if (!body.isEnabled) {
						drawShape(f, xf, DISABLED_BODY_COLOR)
					} else if (body.type == BodyType.STATIC) {
						drawShape(f, xf, STATIC_BODY_COLOR)
					} else if (body.type == BodyType.KINEMATIC) {
						drawShape(f, xf, KINEMATIC_BODY_COLOR)
					} else if (!body.isAwake) {
						drawShape(f, xf, SLEEPING_BODY_COLOR)
					} else {
						drawShape(f, xf, NORMAL_BODY_COLOR)
					}
				}
			}
		}

		if (debugDraw.drawJoints) {
			for (joint in jointListIterator) {
				joint.draw(debugDraw)
			}
		}

		if (debugDraw.drawPairs) {
			for (c in contactManager.contactListIterator) {
				val fixtureA = c.fixtureA
				val fixtureB = c.fixtureB
				val indexA = c.childIndexA
				val indexB = c.childIndexB
				val cA = fixtureA.getAABB(indexA).centre
				val cB = fixtureB.getAABB(indexB).centre

				debugDraw.drawSegment(cA, cB, PAIR_COLOR)
			}
		}

		if (debugDraw.drawAABB) {
			for (body in bodyListIterator) {
				if (!body.isEnabled) {
					continue
				}

				for (f in body.fixtureIterator) {
					for (proxy in f.proxies) {
						val aabb = contactManager.broadPhase.getFatAABB(proxy.proxyId)

						debugDraw.drawPolygon(listOf(
							aabb.A, aabb.B, aabb.C, aabb.D
						), AABB_COLOR
						)
					}
				}
			}
		}

		if (debugDraw.drawCenterOfMess) {
			for (body in bodyListIterator) {
				val xf = Transform(body.transform.position, body.transform.rotation.angle)
				xf.p = body.worldCenter
				debugDraw.drawTransform(xf)
			}
		}
	}

	companion object {
		private val BAD_BODY_COLOR = Color(1f, 0f, 0f)
		private val DISABLED_BODY_COLOR = Color(0.5f, 0.5f, 0.3f)
		private val STATIC_BODY_COLOR = Color(0.5f, 0.9f, 0.5f)
		private val KINEMATIC_BODY_COLOR = Color(0.5f, 0.5f, 0.9f)
		private val SLEEPING_BODY_COLOR = Color(0.6f, 0.6f, 0.6f)
		private val NORMAL_BODY_COLOR = Color(0.9f, 0.7f, 0.7f)
		private val PAIR_COLOR = Color(0.3f, 0.9f, 0.9f)
		private val AABB_COLOR = Color(0.9f, 0.3f, 0.9f)
	}

	override fun shiftOrigin(newOrigin: Vector2d) {
		if (isLocked)
			throw ConcurrentModificationException()

		isLocked = true

		try {
			for (body in bodyListIterator) {
				body as Body
				body.transform.p -= newOrigin
				body.sweep.c0 -= newOrigin
				body.sweep.c -= newOrigin
			}

			for (joint in jointListIterator) {
				joint.shiftOrigin(newOrigin)
			}

			contactManager.broadPhase.shiftOrigin(newOrigin)
		} finally {
			isLocked = false
		}
	}

	override val profileData: IProfileData
		get() = profile.snapshot()

	override fun dump() {
		TODO("Not yet implemented")
	}
}