package game

import "base:builtin"
import "core:math"
import lg "core:math/linalg"
import rl "vendor:raylib"
import "vendor: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 {}
}

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

	ctrl_rotations := calculate_spline_ctrl_rotations(points, context.temp_allocator)

	if points_len >= 2 {
		interpolated_points := make(
			[]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: []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)
		rlgl_vertex3v(cur.pos)
		rlgl_vertex3v(next.pos)

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

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

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

	}
}
debug_draw_spline_mesh :: proc(interpolated_points: []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

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

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