extends CSGCylinder


# Called when the node enters the scene tree for the first time.
func _ready():
	g2dp_as_basis_test()


#--- UNIT TESTS ---

func g2dp_as_basis_test():
	# Origin.
	var o1 = Vector2(-1.0, -1.0)
	var o2 = Vector2(1.0, -1.0)
	var o3 = Vector2(-1.0, 1.0)
	var o4 = Vector2(1.0, 1.0)

	# Destination.
	var d1 = Vector2(-1.0, -1.0)
	var d2 = Vector2(1.0, -2.0)
	var d3 = Vector2(-1.0, 1.0)
	var d4 = Vector2(1.0, 1.0)

	print(str(d1) + "\n" + str(d2) + "\n" + str(d3) + "\n" + str(d4) + "\n")

	var m = g2dp_as_basis(o1, o2, o3, o4, d1, d2, d3, d4)

	print(str(m) + "\n")

	var p1 = multiply_basis_vector2(m, o1)
	var p2 = multiply_basis_vector2(m, o2)
	var p3 = multiply_basis_vector2(m, o3)
	var p4 = multiply_basis_vector2(m, o4)

	print(str(p1) + "\n" + str(p2) + "\n" + str(p3) + "\n" + str(p4) + "\n")



func multmm_test():
	var m = Basis(
		Vector3(5, 3, 1) ,
		Vector3(0, -4, -2) ,
		Vector3(0, 0, 1)
	)
	
	var mi = m.inverse()
	
	var ma = [
		m[0][0], m[0][1], m[0][2],
		m[1][0], m[1][1], m[1][2],
		m[2][0], m[2][1], m[2][2]
	]
	
	var mia = [
		mi[0][0], mi[0][1], mi[0][2],
		mi[1][0], mi[1][1], mi[1][2],
		mi[2][0], mi[2][1], mi[2][2]
	]
	
	var res = multmm(ma, mia)
	
	print(res)


#--- HELPER FUNCTIONS ---


func adj(m):
	var adj = [
		m[4]*m[8]-m[5]*m[7], m[2]*m[7]-m[1]*m[8], m[1]*m[5]-m[2]*m[4],
		m[5]*m[6]-m[3]*m[8], m[0]*m[8]-m[2]*m[6], m[2]*m[3]-m[0]*m[5],
		m[3]*m[7]-m[4]*m[6], m[1]*m[6]-m[0]*m[7], m[0]*m[4]-m[1]*m[3]
	]
	print("\n" + "adjunctive: " + str(adj) + "\n")
	return adj


func multmm(a, b): # Multiply two matrices.
	var c = range(9)
	for i in range(3):
		for j in range(3):
			var cij = 0
			for k in range(3):
				cij += a[3*i + k] * b[3*k + j]
			c[3*i + j] = cij
	return c


func multmv(m, v): # Multiply matrix and vector.
	return [
		m[0]*v[0] + m[1]*v[1] + m[2]*v[2],
		m[3]*v[0] + m[4]*v[1] + m[5]*v[2],
		m[6]*v[0] + m[7]*v[1] + m[8]*v[2]
	]


func basis_to_points(x1, y1, x2, y2, x3, y3, x4, y4):
	var m = [
		x1, x2, x3,
		y1, y2, y3,
		1.0,  1.0,  1.0
	]
	var v = multmv(adj(m), [x4, y4, 1.0]);
	print("coeffs: " + str(v))
	var b = multmm(m, [
		v[0], 0.0, 0.0,
		0.0, v[1], 0.0,
		0.0, 0.0, v[2]
	])
	print("matrix: " + str(b))
	return b


func general_2d_projection(
	x1s, y1s, x1d, y1d,
	x2s, y2s, x2d, y2d,
	x3s, y3s, x3d, y3d,
	x4s, y4s, x4d, y4d
):
	var s = basis_to_points(x1s, y1s, x2s, y2s, x3s, y3s, x4s, y4s)
	var d = basis_to_points(x1d, y1d, x2d, y2d, x3d, y3d, x4d, y4d)
	var m = multmm(d, adj(s))
	for i in range(9):
		m[i] /= m[8]
	return m


func g2dp_as_basis(
	s1, s2, s3, s4,
	d1, d2, d3, d4
):
	var m = general_2d_projection(
		s1.x, s1.y, d1.x, d1.y,
		s2.x, s2.y, d2.x, d2.y,
		s3.x, s3.y, d3.x, d3.y,
		s4.x, s4.y, d4.x, d4.y
	)
	
	return Basis(
		Vector3(m[0], m[1], m[2]),
		Vector3(m[3], m[4], m[5]),
		Vector3(m[6], m[7], m[8])
	)


func multiply_basis_vector2(b, v):
	var h = b.xform(Vector3(v.x, v.y, 1.0))
	return Vector2(h.x / h.z, h.y / h.z)