package game

import "base:builtin"
import "core:math"
import lg "core:math/linalg"
import "game:debug"
import rl "libs:raylib"
import "libs:raylib/rlgl"

SPLINE_SUBDIVS_U :: 1
SPLINE_SUBDIVS_V :: 16
ROAD_WIDTH :: 8.0
CATMULL_ROM_ALPHA :: 1.0
CATMULL_ROM_TENSION :: 0.0

calculate_spline_ctrl_rotations :: proc(
	points: []rl.Vector3,
	allocator := context.allocator,
) -> []lg.Quaternionf32 {
	points_len := len(points)
	ctrl_rotations := make([]lg.Quaternionf32, points_len, allocator)

	// Normals for control points
	for i in 0 ..< points_len - 1 {
		pos := points[i]
		tangent := lg.normalize0(points[i + 1] - pos)
		bitangent := lg.normalize0(lg.cross(tangent, rl.Vector3{0, 1, 0}))
		normal := lg.normalize0(lg.cross(bitangent, tangent))
		rotation_matrix: lg.Matrix3f32
		rotation_matrix[0], rotation_matrix[1], rotation_matrix[2] = bitangent, normal, -tangent
		ctrl_rotations[i] = lg.quaternion_from_matrix3(rotation_matrix)

		if points_len >= 2 {
			ctrl_rotations[points_len - 1] = ctrl_rotations[points_len - 2]
		}
	}
	return ctrl_rotations
}

Interpolated_Point :: struct {
	pos:    rl.Vector3,
	normal: rl.Vector3,
}

sample_spline :: proc(points: []rl.Vector3, t: f32) -> rl.Vector3 {
	points_len := len(points)
	if points_len >= 2 {
		t_mul_len := math.saturate(t) * f32(len(points))
		i := int(t_mul_len)
		frac_t := t_mul_len - f32(i)

		extended_start := points[0] + (points[0] - points[1])
		extended_end := points[points_len - 1] + points[points_len - 1] - points[points_len - 2]
		extended_end2 := extended_end + points[points_len - 1] - points[points_len - 2]

		prev := i > 0 ? points[i - 1] : extended_start
		current := points[i]
		next := i < points_len - 1 ? points[i + 1] : extended_end
		next2 := i < points_len - 2 ? points[i + 2] : extended_end2

		a, b, c, d := catmull_rom_coefs(
			prev,
			current,
			next,
			next2,
			CATMULL_ROM_ALPHA,
			CATMULL_ROM_TENSION,
		)

		return catmull_rom(a, b, c, d, frac_t)
	} else if len(points) == 1 {
		return points[0]
	}

	return {}
}

get_point_frame :: #force_inline proc(
	ps: #soa[]Interpolated_Point,
	i: int,
) -> (
	tangent: rl.Vector3,
	bitangent: rl.Vector3,
) {
	if len(ps) >= 2 {
		tangent =
			ps[i + 1].pos - ps[i].pos if (i < len(ps) - 1) else ps[len(ps) - 1].pos - ps[len(ps) - 2].pos
		normal := ps[i].normal
		bitangent = lg.normalize0(lg.cross(tangent, normal))
	}

	return
}

point_to_quad_sdf :: proc(p, a, b, c, d: rl.Vector3) -> f32 {
	ba := b - a
	pa := p - a
	cb := c - b
	pb := p - b
	dc := d - c
	pc := p - c
	ad := a - d
	pd := p - d
	nor := lg.cross(ba, ad)

	sqrt :: math.sqrt
	dot :: lg.dot
	cross :: lg.cross
	length2 :: lg.length2
	min :: math.min
	sign :: math.sign
	clamp :: math.clamp

	return sqrt(
		(sign(dot(cross(ba, nor), pa)) + sign(dot(cross(cb, nor), pb)) + sign(dot(cross(dc, nor), pc)) + sign(dot(cross(ad, nor), pd)) < 3.0) ? (min(min(min(length2(ba * clamp(dot(ba, pa) / length2(ba), 0.0, 1.0) - pa), length2(cb * clamp(dot(cb, pb) / length2(cb), 0.0, 1.0) - pb)), length2(dc * clamp(dot(dc, pc) / length2(dc), 0.0, 1.0) - pc)), length2(ad * clamp(dot(ad, pd) / length2(ad), 0.0, 1.0) - pd))) : (dot(nor, pa) * dot(nor, pa) / length2(nor)),
	)
}

// point_to_segmented_line_distance :: proc(
// 	ps: #soa[]Interpolated_Point,
// 	p: rl.Vector3,
// ) -> (
// 	min_distance: f32,
// 	segment_idx: int,
// 	ok: bool, // is point within spline
// ) {
// 	min_distance = math.F32_MAX
// 	segment_idx = -1
// 	for i in 0 ..< len(ps) - 1 {
// 		cur, next := ps[i].pos, ps[i + 1].pos
// 
// 		tangent, bitangent := get_point_frame(ps, i)
// 		next_tangent, next_bitangent := get_point_frame(ps, i + 1)
// 		normal, next_normal := ps[i].normal, ps[i + 1].normal
// 
// 		tangent_len := lg.length(tangent)
// 		tangent_norm := tangent / tangent_len if tangent_len > 0 else 0
// 
// 		translated_p := p - cur
// 
// 		// point_to_quad_sdf(p, cur - bitangent * -ROAD_WIDTH, cur)
// 
// 		dot_v := lg.dot(tangent_norm, translated_p)
// 		// dot_u := lg.dot(bitangent)
// 
// 		distance: f32
// 		if dot_v < 0 {
// 			distance = lg.length(translated_p)
// 			ok = false
// 		} else if dot_v > tangent_len {
// 			distance = lg.distance(tangent, translated_p)
// 			ok = false
// 		} else {
// 			projected_p := tangent_norm * dot_v
// 			distance = lg.distance(projected_p, translated_p)
// 			ok = true
// 		}
// 
// 		if distance < min_distance {
// 			segment_idx = i
// 			min_distance = distance
// 		}
// 	}
// 
// 	return min_distance, segment_idx, ok
// }

/// Find collision with the closest 
raycast_spline_tube :: proc(
	ps: #soa[]Interpolated_Point,
	ray: rl.Ray,
) -> (
	collision: rl.RayCollision,
	segment_idx: int,
) {
	for i in 0 ..< len(ps) - 1 {
		cur, next := ps[i].pos, ps[i + 1].pos

		_, bitangent := get_point_frame(ps, i)
		_, next_bitangent := get_point_frame(ps, i + 1)
		// normal, next_normal := ps[i].normal, ps[i + 1].normal

		p1 := cur + bitangent * -ROAD_WIDTH
		p2 := cur + bitangent * ROAD_WIDTH
		p3 := next + next_bitangent * -ROAD_WIDTH
		p4 := next + next_bitangent * ROAD_WIDTH

		segment_idx = i

		collision = rl.GetRayCollisionTriangle(ray, p1, p2, p3)
		if collision.hit {
			break
		}
		collision = rl.GetRayCollisionTriangle(ray, p2, p4, p3)
		if collision.hit {
			break
		}
	}

	return
}

calculate_spline_interpolated_points :: proc(
	points: []rl.Vector3,
	allocator := context.allocator,
) -> #soa[]Interpolated_Point {
	points_len := len(points)

	ctrl_rotations := calculate_spline_ctrl_rotations(points, context.temp_allocator)

	if points_len >= 2 {
		interpolated_points := make_soa(
			#soa[]Interpolated_Point,
			(points_len - 1) * SPLINE_SUBDIVS_V + 1,
			allocator,
		)

		extended_start := points[0] + (points[0] - points[1])
		extended_end := points[points_len - 1] + points[points_len - 1] - points[points_len - 2]
		extended_end2 := extended_end + points[points_len - 1] - points[points_len - 2]

		for i in 0 ..< points_len - 1 {
			prev := i > 0 ? points[i - 1] : extended_start
			current := points[i]
			next := i < points_len - 1 ? points[i + 1] : extended_end
			next2 := i < points_len - 2 ? points[i + 2] : extended_end2

			cur_frame := ctrl_rotations[i]
			next_frame := ctrl_rotations[i + 1]

			a, b, c, d := catmull_rom_coefs(
				prev,
				current,
				next,
				next2,
				CATMULL_ROM_ALPHA,
				CATMULL_ROM_TENSION,
			)

			v_dt := 1.0 / f32(SPLINE_SUBDIVS_V)
			for v_index in 0 ..< SPLINE_SUBDIVS_V {
				v_t := f32(v_index) * v_dt
				out_point := &interpolated_points[i * SPLINE_SUBDIVS_V + v_index]

				rotation := lg.quaternion_slerp(cur_frame, next_frame, v_t)
				normal := lg.matrix3_from_quaternion(rotation)[1]
				out_point.pos = catmull_rom(a, b, c, d, v_t)
				out_point.normal = normal
			}
		}

		interpolated_points[len(interpolated_points) - 1] =
			interpolated_points[len(interpolated_points) - 2]

		return interpolated_points
	}

	return nil
}

debug_draw_spline :: proc(interpolated_points: #soa[]Interpolated_Point) {
	rlgl.Begin(rlgl.LINES)
	defer rlgl.End()
	for i in 0 ..< len(interpolated_points) - 1 {
		cur, next := interpolated_points[i], interpolated_points[i + 1]

		tangent := lg.normalize0(next.pos - cur.pos)
		normal := interpolated_points[i].normal
		bitangent := lg.cross(tangent, normal)

		rlgl.Color4ub(255, 255, 255, 255)
		debug.rlgl_vertex3v(cur.pos)
		debug.rlgl_vertex3v(next.pos)

		rlgl.Color4ub(255, 0, 0, 255)
		debug.rlgl_vertex3v(cur.pos)
		debug.rlgl_vertex3v(cur.pos + tangent)

		rlgl.Color4ub(0, 255, 0, 255)
		debug.rlgl_vertex3v(cur.pos)
		debug.rlgl_vertex3v(cur.pos + bitangent * ROAD_WIDTH)

		rlgl.Color4ub(0, 0, 255, 255)
		debug.rlgl_vertex3v(cur.pos)
		debug.rlgl_vertex3v(cur.pos + normal)

	}
}
debug_draw_spline_mesh :: proc(interpolated_points: #soa[]Interpolated_Point) {
	rlgl.Begin(rlgl.TRIANGLES)
	defer rlgl.End()

	for i in 0 ..< len(interpolated_points) - 1 {
		cur, next := interpolated_points[i], interpolated_points[i + 1]

		tangent := lg.normalize0(next.pos - cur.pos)
		normal := interpolated_points[i].normal
		bitangent := lg.normalize0(lg.cross(tangent, normal))

		next_tangent: rl.Vector3
		if i < len(interpolated_points) - 2 {
			next2 := interpolated_points[i + 2]
			next_tangent = next2.pos - next.pos
		} else {
			next_tangent = tangent
		}

		next_normal := interpolated_points[i + 1].normal
		next_bitangent := lg.normalize0(lg.cross(next_tangent, next_normal))

		u_dt := 1.0 / f32(SPLINE_SUBDIVS_U)
		for u in 0 ..< SPLINE_SUBDIVS_U {
			u_t := u_dt * f32(u)
			u_t2 := u_t + u_dt

			// [-1, 1]
			u_t = u_t * 2 - 1
			u_t2 = u_t2 * 2 - 1
			u_t *= ROAD_WIDTH
			u_t2 *= ROAD_WIDTH

			p1 := cur.pos + bitangent * u_t
			p2 := cur.pos + bitangent * u_t2
			p3 := next.pos + next_bitangent * u_t
			p4 := next.pos + next_bitangent * u_t2

			debug.rlgl_color3v(normal * 0.5 + 0.5)
			debug.rlgl_vertex3v(p1)
			debug.rlgl_vertex3v(p2)
			debug.rlgl_vertex3v(p3)

			debug.rlgl_color3v(next_normal * 0.5 + 0.5)
			debug.rlgl_vertex3v(p2)
			debug.rlgl_vertex3v(p4)
			debug.rlgl_vertex3v(p3)
		}
	}
}