package ru.dbotthepony.kbox2d.dynamics.joint

import ru.dbotthepony.kbox2d.api.*
import ru.dbotthepony.kbox2d.api.B2SolverData
import ru.dbotthepony.kbox2d.api.b2Max
import ru.dbotthepony.kstarbound.math.Vector2d
import ru.dbotthepony.kstarbound.math.times
import ru.dbotthepony.kstarbound.util.Color

class DistanceJoint(def: DistanceJointDef) : AbstractJoint(def) {
	var stiffness: Double = def.stiffness
	var damping: Double = def.damping

	var length: Double = b2Max(def.length, b2_linearSlop)
		set(value) {
			impulse = 0.0
			field = b2Max(value, b2_linearSlop)
		}

	var minLength: Double = b2Max(def.minLength, b2_linearSlop)
		set(value) {
			lowerImpulse = 0.0
			field = b2Clamp(value, b2_linearSlop, maxLength)
		}

	var maxLength: Double = b2Max(def.maxLength, b2_linearSlop)
		set(value) {
			upperImpulse = 0.0
			field = b2Max(value, minLength)
		}

	val currentLength: Double get() {
		val pA = bodyA.getWorldPoint(localAnchorA)
		val pB = bodyB.getWorldPoint(localAnchorB)
		val d = pB - pA
		return d.length
	}

	private var bias: Double = 0.0

	// Solver shared
	private val localAnchorA: Vector2d = def.localAnchorA
	private val localAnchorB: Vector2d = def.localAnchorB
	private var gamma: Double = 0.0
	private var impulse: Double = 0.0
	private var lowerImpulse: Double = 0.0
	private var upperImpulse: Double = 0.0
	private var _currentLength: Double = 0.0

	// Solver temp
	private var indexA: Int = 0
	private var indexB: Int = 0
	private var u: Vector2d = Vector2d.ZERO
	private var rA: Vector2d = Vector2d.ZERO
	private var rB: Vector2d = Vector2d.ZERO
	private var localCenterA: Vector2d = Vector2d.ZERO
	private var localCenterB: Vector2d = Vector2d.ZERO
	private var invMassA: Double = 0.0
	private var invMassB: Double = 0.0
	private var invIA: Double = 0.0
	private var invIB: Double = 0.0
	private var softMass: Double = 0.0
	private var mass: Double = 0.0

	override fun initVelocityConstraints(data: B2SolverData) {
		indexA = bodyA.islandIndex
		indexB = bodyB.islandIndex
		invMassA = bodyA.invMass
		invMassB = bodyB.invMass
		invIA = bodyA.rotInertiaInv
		invIB = bodyB.rotInertiaInv

		localCenterA = bodyA.sweep.localCenter
		localCenterB = bodyB.sweep.localCenter

		val cA = data.positions[indexA].c
		val aA = data.positions[indexA].a
		var vA = data.velocities[indexA].v
		var wA = data.velocities[indexA].w
		val cB = data.positions[indexB].c
		val aB = data.positions[indexB].a
		var vB = data.velocities[indexB].v
		var wB = data.velocities[indexB].w

		val qA = Rotation(aA)
		val qB = Rotation(aB)

		rA = b2Mul(qA, localAnchorA - localCenterA)
		rB = b2Mul(qB, localAnchorB - localCenterB)
		u = cB + rB - cA - rA

		// Handle singularity.
		_currentLength = u.length

		if (_currentLength > b2_linearSlop) {
			u *= 1.0f / _currentLength
		} else {
			u = Vector2d.ZERO
			mass = 0.0
			impulse = 0.0
			lowerImpulse = 0.0
			upperImpulse = 0.0
		}

		val crAu = b2Cross(rA, u)
		val crBu = b2Cross(rB, u)
		var invMass = invMassA + invIA * crAu * crAu + invMassB + invIB * crBu * crBu
		mass = if (invMass != 0.0) 1.0 / invMass else 0.0

		if (stiffness > 0.0 && minLength < maxLength) {
			// soft
			val C = _currentLength - length

			val d = damping
			val k = stiffness

			// magic formulas
			val h = data.step.dt

			// gamma = 1 / (h * (d + h * k))
			// the extra factor of h in the denominator is since the lambda is an impulse, not a force
			gamma = h * (d + h * k)
			gamma = if (gamma != 0.0) 1.0 / gamma else 0.0
			bias = C * h * k * gamma

			invMass += gamma
			softMass = if (invMass != 0.0) 1.0 / invMass else 0.0
		} else {
			// rigid
			gamma = 0.0
			bias = 0.0
			softMass = mass
		}

		if (data.step.warmStarting) {
			// Scale the impulse to support a variable time step.
			impulse *= data.step.dtRatio
			lowerImpulse *= data.step.dtRatio
			upperImpulse *= data.step.dtRatio

			val P = (impulse + lowerImpulse - upperImpulse) * u
			vA -= invMassA * P
			wA -= invIA * b2Cross(rA, P)
			vB += invMassB * P
			wB += invIB * b2Cross(rB, P)
		} else {
			impulse = 0.0
		}

		data.velocities[indexA].v = vA
		data.velocities[indexA].w = wA
		data.velocities[indexB].v = vB
		data.velocities[indexB].w = wB
	}

	override fun solveVelocityConstraints(data: B2SolverData) {
		var vA = data.velocities[indexA].v
		var wA = data.velocities[indexA].w
		var vB = data.velocities[indexB].v
		var wB = data.velocities[indexB].w

		if (minLength < maxLength) {
			if (stiffness > 0.0) {
				// Cdot = dot(u, v + cross(w, r))
				val vpA = vA + b2Cross(wA, rA)
				val vpB = vB + b2Cross(wB, rB)
				val Cdot = b2Dot(u, vpB - vpA)

				var impulse = -softMass * (Cdot + bias + gamma * impulse)
				impulse += impulse

				val P = impulse * u
				vA -= invMassA * P
				wA -= invIA * b2Cross(rA, P)
				vB += invMassB * P
				wB += invIB * b2Cross(rB, P)
			}

			// lower
			run {
				val C = _currentLength - minLength
				val bias = b2Max(0.0, C) * data.step.inv_dt

				val vpA = vA + b2Cross(wA, rA)
				val vpB = vB + b2Cross(wB, rB)
				val Cdot = b2Dot(u, vpB - vpA)

				var impulse = -mass * (Cdot + bias)
				val oldImpulse = lowerImpulse
				lowerImpulse = b2Max(0.0, lowerImpulse + impulse)
				impulse = lowerImpulse - oldImpulse
				val P = impulse * u

				vA -= invMassA * P
				wA -= invIA * b2Cross(rA, P)
				vB += invMassB * P
				wB += invIB * b2Cross(rB, P)
			}

			// upper
			run {
				val C = maxLength - _currentLength
				val bias = b2Max(0.0, C) * data.step.inv_dt

				val vpA = vA + b2Cross(wA, rA)
				val vpB = vB + b2Cross(wB, rB)
				val Cdot = b2Dot(u, vpA - vpB)

				var impulse = -mass * (Cdot + bias)
				val oldImpulse = upperImpulse
				upperImpulse = b2Max(0.0, upperImpulse + impulse)
				impulse = upperImpulse - oldImpulse
				val P = -impulse * u

				vA -= invMassA * P
				wA -= invIA * b2Cross(rA, P)
				vB += invMassB * P
				wB += invIB * b2Cross(rB, P)
			}
		} else {
			// Equal limits

			// Cdot = dot(u, v + cross(w, r))
			val vpA = vA + b2Cross(wA, rA)
			val vpB = vB + b2Cross(wB, rB)
			val Cdot = b2Dot(u, vpB - vpA)

			var impulse = -mass * Cdot
			impulse += impulse

			val P = impulse * u
			vA -= invMassA * P
			wA -= invIA * b2Cross(rA, P)
			vB += invMassB * P
			wB += invIB * b2Cross(rB, P)
		}

		data.velocities[indexA].v = vA
		data.velocities[indexA].w = wA
		data.velocities[indexB].v = vB
		data.velocities[indexB].w = wB
	}

	override fun solvePositionConstraints(data: B2SolverData): Boolean {
		var cA = data.positions[indexA].c
		var aA = data.positions[indexA].a
		var cB = data.positions[indexB].c
		var aB = data.positions[indexB].a

		val qA = Rotation(aA)
		val qB = Rotation(aB)

		val rA = b2Mul(qA, localAnchorA - localCenterA)
		val rB = b2Mul(qB, localAnchorB - localCenterB)
		var u = cB + rB - cA - rA

		u.isFiniteOrThrow {
			"u is invalid, $cB, $rB, $cA, $rA"
		}

		val length = u.length
		u = u.normalized
		val C: Double

		if (minLength == maxLength) {
			C = length - minLength
		} else if (length < minLength) {
			C = length - minLength
		} else if (maxLength < length) {
			C = length - maxLength
		} else {
			return true
		}

		val impulse = -mass * C
		val P = impulse * u

		P.isFiniteOrThrow {
			"P is not finite, impulse: $impulse, u: $u, mass: $mass, C: $C"
		}

		cA -= invMassA * P
		aA -= invIA * b2Cross(rA, P)
		cB += invMassB * P
		aB += invIB * b2Cross(rB, P)

		data.positions[indexA].c = cA
		data.positions[indexA].a = aA
		data.positions[indexB].c = cB
		data.positions[indexB].a = aB

		return b2Abs(C) < b2_linearSlop
	}

	override fun getReactionForce(inv_dt: Double): Vector2d {
		return inv_dt * (impulse + lowerImpulse - upperImpulse) * u
	}

	override fun getReactionTorque(inv_dt: Double): Double {
		return 0.0
	}

	override val anchorA: Vector2d get() = bodyA.getWorldPoint(localAnchorA)
	override val anchorB: Vector2d get() = bodyB.getWorldPoint(localAnchorB)

	override fun draw(draw: IDebugDraw) {
		val pA = b2Mul(bodyA.transform, localAnchorA)
		val pB = b2Mul(bodyB.transform, localAnchorB)

		val axis = (pB - pA).normalized

		draw.drawSegment(pA, pB, c4)

		val pRest = pA + length * axis
		draw.drawPoint(pRest, 8.0, c1)

		if (minLength != maxLength) {
			if (minLength > b2_linearSlop) {
				val pMin = pA + minLength * axis
				draw.drawPoint(pMin, 4.0, c2)
			}

			if (maxLength < Double.MAX_VALUE) {
				val pMax = pA + maxLength * axis
				draw.drawPoint(pMax, 4.0, c3)
			}
		}
	}

	companion object {
		private val c1 = Color(0.7f, 0.7f, 0.7f)
		private val c2 = Color(0.3f, 0.9f, 0.3f)
		private val c3 = Color(0.9f, 0.3f, 0.3f)
		private val c4 = Color(0.4f, 0.4f, 0.4f)
	}
}