extends Node


static func matrix_to_points(
	p01: Vector2, p02: Vector2, p03: Vector2, p04: Vector2
) -> Array:
	var j = p01.x - p02.x - p03.x + p04.x
	if j == 0.0:
		j = 0.00000001
	var k = -p01.x - p02.x + p03.x + p04.x
	var l = -p01.x + p02.x - p03.x + p04.x
	var m = p01.y - p02.y - p03.y + p04.y
	var n = -p01.y - p02.y + p03.y + p04.y
	var o = -p01.y + p02.y - p03.y + p04.y
	var i = 1;
	
	var q = m * k - j * n
	if q == 0.0:
		q = 0.00000001
	
	var h = (j * o - m * l) * i / q
	var g = (k * h + l * i) / j
	var f = (p01.y * (g + h + i) + p03.y * (-g - h + i)) / 2
	var e = (p01.y * (g + h + i) - p02.y * (g - h + i)) / 2
	var d = p01.y * (g + h + i) - f - e
	var c = (p01.x * (g + h + i) + p03.x * (-g - h + i)) / 2
	var b = (p01.x * (g + h + i) - p02.x * (g - h + i)) / 2
	var a = p01.x * (g + h + i) - c - b
	
	return [
		a, d, g,
		b, e, h,
		c, f, i
		]


static func basis_from_to_points(
	s01: Vector2, s02: Vector2, s03: Vector2, s04: Vector2,
	d01: Vector2, d02: Vector2, d03: Vector2, d04: Vector2
):
	var matrix_to_source = matrix_to_points(s01, s02, s03, s04)
	var basis_to_source = array_to_basis(matrix_to_source)
	var basis_from_source = basis_to_source.inverse()
	
	var matrix_to_dest = matrix_to_points(d01, d02, d03, d04)
	var basis_to_dest = array_to_basis(matrix_to_dest)
	
	var from_source_to_dest = basis_to_dest * basis_from_source
	return from_source_to_dest
	

static func array_to_basis(arr: Array) -> Basis:
	return Basis(
		Vector3(arr[0], arr[1], arr[2]),
		Vector3(arr[3], arr[4], arr[5]),
		Vector3(arr[6], arr[7], arr[8])
	)


static func basis_to_array(basis: Basis) -> Array:
	return [
		basis.x.x, basis.x.y, basis.x.z,
		basis.y.x, basis.y.y, basis.y.z,
		basis.z.x, basis.z.y, basis.z.z
	]

static func mult_4x4_matrices(left: Array, right: Array) -> Array:
	var result = []
	for y in range(4):
		for x in range(4):
			var sum = 0.0
			for i in range(4):
				sum += left[_c2i(x, i)] * right[_c2i(i, y)]
			result.append(sum)
	return result

# Coordinates on a 4x4 field to index.
static func _c2i(x: int, y: int) -> int:
	return y * 4 + x

# This just calculates a rotation-matrix that flattens the z-coordinates of
# the given Quad. You could also say that it makes the normal of the Quad
# point directly to the camera. (-z)
static func plane_to_xy_basis(
	p01: Vector3, p02: Vector3, p03: Vector3, p04: Vector3
) -> Basis:
	
	var sample_normal_01 = (p02 - p01).cross(p04 - p02).normalized()
	var sample_normal_02 = (p03 - p02).cross(p04 - p03).normalized()
	
	if sample_normal_01.dot(sample_normal_02) <= 0.99999:
		push_error("The given form is not a plane and can" +
		"therefore not be transformed correctly")
	
	var normal = sample_normal_01.linear_interpolate(sample_normal_02, 0.5)
	normal = normal.normalized()

	var dest_normal = Vector3.BACK
	
	if normal.dot(dest_normal) >= 0.99999:
		return Basis()
	
	var rot_axis = normal.cross(dest_normal).normalized()
	var rot_angle = normal.dot(dest_normal)
	
	var quad = Quat(-rot_axis, acos(rot_angle))
	var basis = Basis(quad)
	
	return basis.inverse()

# This basically takes four Vector3 that form a Quad somewhere in the
# space and a second (this time two-dimensional) Quad to calculate
# the transformation-matrix that is required to get from the former
# to the latter. Returned is said transformation-matrix.
static func plane_3d_to_xy(
	p3d_01: Vector3, p3d_02: Vector3, p3d_03: Vector3, p3d_04: Vector3,
	d_01: Vector2, d_02: Vector2, d_03: Vector2, d_04: Vector2):
	
	var rotation_matrix = plane_to_xy_basis(
		p3d_01, p3d_02,
		p3d_03, p3d_04
	)
	
	var to_plane_transform = Transform(
		rotation_matrix, Vector3(0.0, 0.0,
		-rotation_matrix.xform(p3d_01).z
		)
	)
	
	var plane_points = [
		to_plane_transform.xform(p3d_01),
		to_plane_transform.xform(p3d_02),
		to_plane_transform.xform(p3d_03),
		to_plane_transform.xform(p3d_04)
	]
	
	
	var xy_points = [
		Vector2(plane_points[0].x, plane_points[0].y),
		Vector2(plane_points[1].x, plane_points[1].y),
		Vector2(plane_points[2].x, plane_points[2].y),
		Vector2(plane_points[3].x, plane_points[3].y),
	]
	
	var basis_2d = basis_from_to_points(
		xy_points[0], xy_points[1], xy_points[2], xy_points[3], 
		d_01, d_02, d_03, d_04)
	
#	print(basis_2d)
#
#	for i in range(4):
#		var flat_coord = xy_points[i]
#		var ext_coord = Vector3(flat_coord.x, flat_coord.y, 1.0)
#		var trans_coord = basis_2d.xform(ext_coord)
#		var cut_coord = Vector2(trans_coord.x, trans_coord.y)
#		print(cut_coord)
#	print("")
	
	var transform_2d = Transform(
		Vector3(basis_2d.x.x, basis_2d.y.x, 0.0),
		Vector3(basis_2d.x.y, basis_2d.y.y, 0.0),
		Vector3.ZERO,
		Vector3(basis_2d.x.z, basis_2d.y.z, 0.0)
	)

#	var transform_2d = Transform(
#		Vector3(basis_2d.x.x, basis_2d.x.y, basis_2d.x.z),
#		Vector3(basis_2d.y.x, basis_2d.y.y, basis_2d.y.z),
#		Vector3.ZERO,
#		Vector3(0.0, 0.0, 0.0)
#	)
	
	var transform = transform_2d * to_plane_transform
	
#	return [
#		transform.basis.x.x, transform.basis.x.y, transform.basis.x.z,
#		transform.origin.x,
#		transform.basis.y.x, transform.basis.y.y, transform.basis.y.z,
#		transform.origin.y,
#		transform.basis.z.x, transform.basis.z.y, transform.basis.z.z,
#		transform.origin.z,
#		basis_2d.z.x, basis_2d.z.y, 0.0, 1.0
#	]

	var cool = mult_4x4_matrices(
#		[
#			basis_2d.x.x, basis_2d.x.y, 0.0, basis_2d.x.z,
#			basis_2d.y.x, basis_2d.y.y, 0.0, basis_2d.y.z,
#			0.0, 0.0, 0.0, 0.0,
#			basis_2d.z.x, basis_2d.z.y, 0.0, basis_2d.z.z,
#		],
		[
			basis_2d.x.x, basis_2d.y.x, 0.0, basis_2d.z.x,
			basis_2d.x.y, basis_2d.y.y, 0.0, basis_2d.z.y,
			0.0, 0.0, 0.0, 0.0,
			basis_2d.x.z, basis_2d.y.z, 0.0, basis_2d.z.z,
		],
		[
			to_plane_transform.basis.x.x, to_plane_transform.basis.x.y,
			to_plane_transform.basis.x.z, to_plane_transform.origin.x,
			
			to_plane_transform.basis.y.x, to_plane_transform.basis.y.y,
			to_plane_transform.basis.y.z, to_plane_transform.origin.y,
			
			to_plane_transform.basis.z.x, to_plane_transform.basis.z.y,
			to_plane_transform.basis.z.z, to_plane_transform.origin.z,
			
			0.0, 0.0, 0.0, 1.0
		]
	)
	
#	cool = [
#		cool[0], cool[4], cool[8], cool[12],
#		cool[1], cool[5], cool[9], cool[13],
#		cool[2], cool[6], cool[10], cool[14],
#		cool[3], cool[7], cool[11], cool[15],
#	]
	return cool