package ru.dbotthepony.kbox2d.dynamics.joint

import ru.dbotthepony.kbox2d.api.*
import ru.dbotthepony.kbox2d.api.B2SolverData
import ru.dbotthepony.kbox2d.api.b2MulT
import ru.dbotthepony.kstarbound.math.MutableMatrix2d
import ru.dbotthepony.kstarbound.math.Vector2d
import ru.dbotthepony.kstarbound.math.times

// p = attached point, m = mouse point
// C = p - m
// Cdot = v
//      = v + cross(w, r)
// J = [I r_skew]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)

class MouseJoint(def: MouseJointDef) : AbstractJoint(def) {
	private val localAnchorB: Vector2d = b2MulT(bodyB.transform, def.target)
	var targetA: Vector2d = def.target
		set(value) {
			value.isFiniteOrThrow { "Tried to set illegal target $value" }
			field = value
			bodyB.isAwake = true
		}

	var maxForce = def.maxForce
	var stiffness = def.stiffness
	var damping = def.damping

	// Solver shared
	private var impulse: Vector2d = Vector2d.ZERO
	private var gamma: Double = 0.0
	private var beta: Double = 0.0

	// Solver temp
	private var indexA: Int = 0
	private var indexB: Int = 0
	private var rB: Vector2d = Vector2d.ZERO
	private var localCenterB: Vector2d = Vector2d.ZERO
	private var invMassB: Double = 0.0
	private var invIB: Double = 0.0
	private var mass: MutableMatrix2d = MutableMatrix2d().also { it.zero() }
	private var C: Vector2d = Vector2d.ZERO

	override fun initVelocityConstraints(data: B2SolverData) {
		indexB = bodyB.islandIndex
		localCenterB = bodyB.sweep.localCenter
		invMassB = bodyB.invMass
		invIB = bodyB.invI

		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 qB = Rotation(aB)

		val d = damping
		val k = stiffness

		// magic formulas
		// gamma has units of inverse mass.
		// beta has units of inverse time.
		val h = data.step.dt
		gamma = h * (d + h * k)

		if (gamma != 0.0) {
			gamma = 1.0f / gamma
		}

		beta = h * k * gamma

		// Compute the effective mass matrix.
		rB = b2Mul(qB, localAnchorB - localCenterB)

		// K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
		//      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
		//        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
		val K = MutableMatrix2d()
		K.m00 = invMassB + invIB * rB.y * rB.y + gamma
		K.m10 = -invIB * rB.x * rB.y
		K.m01 = K.m10
		K.m11 = invMassB + invIB * rB.x * rB.x + gamma

		mass = K.getInverse().asMutableMatrix()

		C = cB + rB - targetA
		C *= beta

		// Cheat with some damping
		wB *= 0.98f

		if (data.step.warmStarting) {
			impulse *= data.step.dtRatio
			vB += invMassB * impulse
			wB += invIB * b2Cross(rB, impulse)
		} else {
			impulse = Vector2d.ZERO
		}

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

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

		// Cdot = v + cross(w, r)
		val Cdot = vB + b2Cross(wB, rB);
		var impulse = b2Mul(mass, -(Cdot + C + gamma * impulse));

		val oldImpulse = this.impulse;
		this.impulse += impulse;
		val maxImpulse = data.step.dt * maxForce;
		if (this.impulse.lengthSquared > maxImpulse * maxImpulse) {
			this.impulse *= maxImpulse / this.impulse.length
		}

		impulse = this.impulse - oldImpulse;

		vB += invMassB * impulse;
		wB += invIB * b2Cross(rB, impulse);

		data.velocities[indexB].v = vB;
		data.velocities[indexB].w = wB;
	}

	override fun solvePositionConstraints(data: B2SolverData): Boolean {
		return true
	}

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

	override fun getReactionForce(inv_dt: Double): Vector2d {
		return inv_dt * impulse
	}

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

	override fun shiftOrigin(newOrigin: Vector2d) {
		targetA -= newOrigin
	}
}