package physics

import "core:fmt"
import "core:math"
import "core:math/linalg"

Ray :: struct {
	pos, nrm: [3]f32,
	len: f32,
}

Line :: struct {
	start, end: [3]f32,
}

Plane :: struct {
	pos, nrm, sz: [3]f32,
}

AABB :: struct {
	min, max: [3]f32,
}

Box :: struct {
	sz: [3]f32,
}

Sphere :: struct {
	radius: f32,
}

Capsule :: struct {
	base: [3]f32,
	radius, height: f32,
}

Cylinder :: struct {
	base: [3]f32,
	radius, height: f32,
}

ConvexHull :: struct {
	verts: []f32,
}

TriMesh :: struct {
	verts: []f32,
}

Frustrum :: struct {
	l, r, t, b, n, f: [4]f32,
}

Collider :: union {
	Sphere,
	Box,
	// Capsule,
	// Cylinder,
	// ConvexHull,
	// TriMesh,
}

Contact :: struct {
	a, b: ^RigidBody,
	point, normal: [3]f32, // normal points from a to b
	r1_local, r2_local: [3]f32,
	depth, weight, normal_lambda: f32,
}

collider_inv_inertia :: proc(col: Collider, mass: f32) -> matrix[3,3]f32 {
	switch shape in col {
	case Sphere:
		inertia := 0.4 * mass * shape.radius * shape.radius
		return matrix[3,3]f32 {
			1/inertia, 0,         0,
			0,         1/inertia, 0,
			0,         0,         1/inertia,
		}

	case Box:
		sq := shape.sz * shape.sz        // full side lengths, squared
		ix := mass * (sq.y + sq.z) / 12
		iy := mass * (sq.x + sq.z) / 12
		iz := mass * (sq.x + sq.y) / 12
		return matrix[3,3]f32 {
			1/ix, 0,    0,
			0,    1/iy, 0,
			0,    0,    1/iz,
		}

	// case Cylinder:
	// 	r := shape.radius
	// 	h := shape.height
	// 	iy := 0.5 * mass * r * r
	// 	ix := mass * (3*r*r + h*h) / 12
	// 	return matrix[3,3]f32 {
	// 		1/ix, 0,    0,
	// 		0,    1/iy, 0,
	// 		0,    0,    1/ix,
	// 	}
	//
	// case Capsule:
	// 	r := shape.radius
	// 	h := shape.height
	// 	// split total mass by volume share
	// 	cyl_vol := math.PI * r * r * h
	// 	cap_vol := (4.0/3.0) * math.PI * r * r * r   // both caps together
	// 	cyl_mass := mass * cyl_vol / (cyl_vol + cap_vol)
	// 	cap_mass := mass - cyl_mass
	//
	// 	iy := 0.5*cyl_mass*r*r + 0.4*cap_mass*r*r
	// 	ix := cyl_mass*(3*r*r + h*h)/12 +
	// 		cap_mass*(0.4*r*r + 0.25*h*h + 0.375*h*r)
	// 	return matrix[3,3]f32 {
	// 		1/ix, 0,    0,
	// 		0,    1/iy, 0,
	// 		0,    0,    1/ix,
	// 	}

	case:
		panic("collider_inv_inertia: collider shape can't be nil")
	}
}

contact_solve :: proc(contact: ^Contact, dt: f32) {
	r1 := contact.point - contact.a.pos
	r2 := contact.point - contact.b.pos
	correction := -contact.normal * contact.depth // * contact.weight
	contact.normal_lambda =
		solve_positional(contact.a, contact.b, r1, r2,
			correction, 0, dt)
}

collide :: proc(a, b: ^RigidBody, contacts: ^[dynamic]Contact) {
	switch _ in a.col {
	case Sphere:
		switch _ in b.col {
		case Sphere:
			c: Contact
			if sphere_v_sphere(a, b, &c) do append(contacts, c)
		case Box:
			c: Contact
			if sphere_v_box(a, b, &c) do append(contacts, c)
		}
	case Box:
		switch _ in b.col {
		case Sphere:
			// reverse args so sphere is `a`, then flip the normal
			c: Contact
			if sphere_v_box(b, a, &c) {
				c.a, c.b = a, b
				c.normal = -c.normal
				append(contacts, c)
			}
		case Box:
			box_v_box(a, b, contacts)
		}
	}
}

// marked for deletion
sphere_v_ground :: proc(ground, body: ^RigidBody, out: ^Contact) -> bool {
	radius := body.col.(Sphere).radius // TODO: fix this
	depth := radius - body.pos.y

	if depth <= 0 {
		return false
	}

	out^ = Contact {
		a = ground,
		b = body,
		normal = {0,1,0},
		depth = depth,
		point = body.pos - [3]f32{0,radius,0},
		weight = 1,
	}

	return true
}

sphere_v_sphere :: proc(a, b: ^RigidBody, out: ^Contact) -> bool {
	radius_a := a.col.(Sphere).radius
	radius_b := b.col.(Sphere).radius

	delta  := b.pos - a.pos
	dist := linalg.length(delta)

	// guard: coincident centers have no defined normal
	if dist == 0 {
		return false
	}

	overlap := radius_a + radius_b - dist
	if overlap <= 0 {
		return false   // not touching
	}

	normal := delta / dist
	out.a = a
	out.b = b
	out.normal = normal
	out.depth = overlap
	out.point = a.pos + normal * radius_a   // on a's surface, toward b
	out.weight = 1
	out.r1_local =
		linalg.quaternion_mul_vector3(
			linalg.quaternion_inverse(a.rot), out.point - a.pos)
	out.r2_local =
		linalg.quaternion_mul_vector3(
			linalg.quaternion_inverse(b.rot), out.point - b.pos)

	return true
}

sphere_v_box :: proc(a, b: ^RigidBody, out: ^Contact) -> bool {
	radius := a.col.(Sphere).radius
	cp := box_closest_point(b, a.pos)

	delta := cp - a.pos // from sphere center toward box surface
	dist := linalg.length(delta)

	if dist >= radius {
		return false // closest point is outside the sphere
	}

	normal: [3]f32
	if dist > 0 {
		normal = delta / dist
	} else {
		// TODO: come back and figure this out at some point
		// sphere center is *inside* the box — degenerate, pick any axis.
		// (proper handling: push out along the shallowest face. punt for now.)
		normal = {0, 1, 0}
	}

	out.a = a
	out.b = b
	out.normal = normal
	out.depth = radius - dist
	out.point = cp
	out.weight = 1
	out.r1_local =
		linalg.quaternion_mul_vector3(
			linalg.quaternion_inverse(a.rot), cp - a.pos)
	out.r2_local =
		linalg.quaternion_mul_vector3(
			linalg.quaternion_inverse(b.rot), cp - b.pos)

	return true
}

// Box helpers
// boxes use SAT + sutherland-hodgman clipping
box_v_box :: proc(a, b: ^RigidBody, contacts: ^[dynamic]Contact) -> int {
	axes_a := box_axes(a)
	axes_b := box_axes(b)
	center_delta := b.pos - a.pos

	// track the minimum-penetration axis as we go
	best_sep := f32(-1e30)   // most-negative separation = deepest; we want the LEAST negative
	best_axis: [3]f32

	test_axis :: proc(L: [3]f32, a, b: ^RigidBody, 
		axes_a, axes_b: [3][3]f32, center_delta: [3]f32, 
		best_sep: ^f32, best_axis: ^[3]f32) -> bool {
		len_sq := linalg.dot(L, L)
		if len_sq < 1e-8 {
			return true // degenerate axis (parallel edges); skip, treat as non-separating
		}

		axis := L / math.sqrt(len_sq)
		ra := box_radius_on_axis(a, axes_a, axis)
		rb := box_radius_on_axis(b, axes_b, axis)
		dist := abs(linalg.dot(center_delta, axis))
		sep := dist - (ra + rb)

		if sep > 0 {
			return false // separating axis found, no overlap
		}

		if sep > best_sep^ {
			best_sep^ = sep
			if linalg.dot(center_delta, axis) < 0 {
				axis = -axis
			}
			best_axis^ = axis
		}
		return true
	}

	// 6 face axes
	for i in 0..<3 {
		if !test_axis(axes_a[i], a, b, axes_a, axes_b, center_delta,
			&best_sep, &best_axis) {
			return 0
		}
	}
	for i in 0..<3 {
		if !test_axis(axes_b[i], a, b, axes_a, axes_b, center_delta,
			&best_sep, &best_axis) {
			return 0
		}
	}

	// 9 edge-edge cross product axes
	for i in 0..<3 {
	for j in 0..<3 {
		L := linalg.cross(axes_a[i], axes_b[j])
		if !test_axis(L, a, b, axes_a, axes_b, center_delta,
			&best_sep, &best_axis) {
			return 0
		}
	}}

	ref, inc, ref_is_a := box_contact_faces(a, b, best_axis)
	ref_normal := ref_is_a ? best_axis : -best_axis
	clipped := clip_incident(inc, ref, ref_normal)

	if !a.static && !b.static {
	        fmt.println(">>> DYN-DYN manifold, clipped.n =", clipped.n)
	        fmt.println("    ref_corners:", ref)
	        for i in 0..<clipped.n {
	                fmt.println("    pt", i, clipped.pts[i])
	        }
	}

	CONTACT_SLOP :: f32(0.001)   // 1mm tolerance
	MAX_CORRECTION :: f32(0.2)

	count := 0
	for i in 0..<clipped.n {
		point := clipped.pts[i]
		dist := linalg.dot(point - ref[0], ref_normal)
		if dist > CONTACT_SLOP {
			continue
		}
		append(contacts, Contact {
			a = a,
			b = b,
			normal = best_axis,
			depth = min(max(-dist, 0), MAX_CORRECTION),
			point = point,
			r1_local = 
				linalg.quaternion_mul_vector3(
					linalg.quaternion_inverse(a.rot), 
					point - a.pos),
			r2_local =
				linalg.quaternion_mul_vector3(
					linalg.quaternion_inverse(b.rot),
					point - b.pos),
			weight = 1,
		})

		if !a.static && !b.static {
			// fmt.println("contact point", count, point)
		}
		count += 1

		if -dist > 0.05 {   // only print suspicious depths
			fmt.printfln("BIG DEPTH depth=%v point=%v b.pos=%v lever=%v",
				max(-dist,0), point, b.pos, point - b.pos)
		}
	}


	if !a.static && !b.static && count > 0 {
		// fmt.println("contacts:", count)
		// fmt.println("")
	}

	return count
}

box_axes :: proc(body: ^RigidBody) -> [3][3]f32 {
	rot := linalg.matrix3_from_quaternion(body.rot)
	return {
		{rot[0,0], rot[1,0], rot[2,0]},   // local x in world
		{rot[0,1], rot[1,1], rot[2,1]},   // local y in world
		{rot[0,2], rot[1,2], rot[2,2]},   // local z in world
	}
}

// half-width of the box's shadow when projected onto unit axis L
box_radius_on_axis :: proc(body: ^RigidBody, axes: [3][3]f32,
				L: [3]f32) -> f32 {
	half := body.col.(Box).sz / 2
	return abs(half.x * linalg.dot(axes[0], L)) +
	       abs(half.y * linalg.dot(axes[1], L)) +
	       abs(half.z * linalg.dot(axes[2], L))
}

// closest point on an oriented box to world-space point `p`
box_closest_point :: proc(box_body: ^RigidBody, p: [3]f32) -> [3]f32 {
	box := box_body.col.(Box)
	half := box.sz / 2

	// world point -> box local space: undo translation, then undo rotation
	local := p - box_body.pos
	inv_rot := linalg.quaternion_inverse(box_body.rot)
	local = linalg.quaternion_mul_vector3(inv_rot, local)

	// clamp per-axis to the box extents (closest point in local space)
	local.x = clamp(local.x, -half.x, half.x)
	local.y = clamp(local.y, -half.y, half.y)
	local.z = clamp(local.z, -half.z, half.z)

	// box local space -> world: re-apply rotation, then translation
	world := linalg.quaternion_mul_vector3(box_body.rot, local)
	return world + box_body.pos
}

Poly :: struct {
	pts: [8][3]f32,
	n: int,
}

clip_against_plane :: proc(src: Poly, plane_pt, plane_nrm: [3]f32) -> Poly {
	dst: Poly

	for i in 0..<src.n {
		current := src.pts[i]
		next := src.pts[(i + 1) % src.n]

		d_c := linalg.dot(current - plane_pt, plane_nrm)
		d_n := linalg.dot(next - plane_pt, plane_nrm)
		
		if d_c >= 0 {
			dst.pts[dst.n] = current
			dst.n += 1
		}

		if (d_c >= 0) != (d_n >= 0) {
			t := d_c / (d_c - d_n)
			dst.pts[dst.n] = current + t * (next - current)
			dst.n += 1
		}
	}

	return dst
}

// clip the incident polygon against all 4 side planes of the reference face.
// ref_corners in winding order; ref_normal is the reference face's outward normal.
clip_incident :: proc(inc_corners, ref_corners: [4][3]f32,
	ref_normal: [3]f32) -> Poly {
	poly: Poly
	for c in inc_corners {
		poly.pts[poly.n] = c
		poly.n += 1
	}

	ref_center := (ref_corners[0] + ref_corners[1] + 
			    ref_corners[2] + ref_corners[3]) / 4

	fmt.println("ref_corners:", ref_corners)
	fmt.println("ref_normal:", ref_normal)

	for i in 0..<4 {
		edge_start := ref_corners[i]
		edge_dir := ref_corners[(i + 1) % 4] - edge_start
		inward := linalg.normalize(linalg.cross(ref_normal, edge_dir))
		if linalg.dot(inward, ref_center - edge_start) < 0 {
			inward = -inward
		}
		poly = clip_against_plane(poly, edge_start, inward)

		fmt.println("  edge", i, "start:", edge_start, "inward:", inward)
	}
	return poly
}