extends Node
class_name SummerDayMathHelper


static func matrix_to_points(
	p01: Vector2, p02: Vector2, p03: Vector2, p04: Vector2
) -> Basis:
	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 Basis(
		Vector3(a, d, g),
		Vector3(b, e, h),
		Vector3(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 matrix_from_source = matrix_to_source.inverse()
	
	var matrix_to_dest = matrix_to_points(d01, d02, d03, d04)
	
	var from_source_to_dest = matrix_to_dest * matrix_from_source
	return from_source_to_dest


# 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)
	
	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 = transform_2d * to_plane_transform

	var cool = SummerDayMatrix4.mult(
		SummerDayMatrix4.from_Basis(basis_2d),
		SummerDayMatrix4.from_Transform(to_plane_transform)
	)
	
	return cool