use glam::{Vec2, Vec3};
use partial_min_max::{min, max};

#[derive (Debug, Default, PartialEq)]
pub struct PhysicsBody {
	pub pos: Vec3,
	pub vel: Vec3,
}

#[derive (Debug, PartialEq)]
pub struct PhysicsResult {
	pub body: PhysicsBody,
	pub normals_hit: Vec <Vec3>,
	pub kill: bool,
}

#[derive (Copy, Clone)]
pub struct Triangle {
	pub verts: [Vec3; 3],
}

#[derive (Copy, Clone)]
pub struct Aabb {
	pub min: Vec3,
	pub max: Vec3,
}

fn vec_min (a: &Vec3, b: &Vec3) -> Vec3 {
	Vec3::from ((
		min (a.x, b.x),
		min (a.y, b.y),
		min (a.z, b.z)
	))
}

fn vec_max (a: &Vec3, b: &Vec3) -> Vec3 {
	Vec3::from ((
		max (a.x, b.x),
		max (a.y, b.y),
		max (a.z, b.z)
	))
}

impl Triangle {
	pub fn min (&self) -> Vec3 {
		self.verts [1..].iter ().fold (
			self.verts [0],
			|pre, v| vec_min (&pre, v)
		)
	}
	
	pub fn max (&self) -> Vec3 {
		self.verts [1..].iter ().fold (
			self.verts [0],
			|pre, v| vec_max (&pre, v)
		)
	}
}

#[derive (Clone, Copy, Debug, PartialEq)]
pub struct Collision {
	pub t: f32,
	pub p_impact: Vec3,
	normal: Vec3,
	i: usize,
	c_type: CollisionType,
}

impl Collision {
	pub fn take_if_closer (&self, o: &Self) -> Self {
		if o.t < self.t && o.t >= 0.0 {
			o.clone ()
		}
		else {
			self.clone ()
		}
	}
}

#[derive (Clone, Copy, Debug, PartialEq)]
pub struct PrimCollision {
	t: f32,
	p_impact: Vec3,
	normal: Vec3,
}

impl PrimCollision {
	pub fn take_if_closer (&self, o: &Self) -> Self {
		if o.t < self.t && o.t >= 0.0 {
			o.clone ()
		}
		else {
			self.clone ()
		}
	}
}

#[derive (Clone, Copy, Debug, PartialEq)]
enum CollisionType {
	Face,
	Edge,
	Vert,
}

pub struct Params {
	pub dt: f32,
	pub gravity: Vec3,
	pub margin: f32,
}

pub fn step (
	params: &Params, 
	tris: &[Triangle], aabbs: &[Aabb],
	radius: f32, input: &PhysicsBody, 
) -> PhysicsResult
{
	let margin = params.margin;
	let dt = params.dt;
	
	let mut t_remaining = 1.0;
	
	let mut old_pos = input.pos;
	let mut new_vel = input.vel + params.gravity;
	let mut new_pos = old_pos + new_vel * dt * t_remaining;
	
	let mut normals_hit = Vec::new ();
	
	// Do 5 iterations of the sub-step, trying to converge on a valid state
	for _ in 0..5 {
		let candidate = get_candidate (tris, aabbs, old_pos, new_pos, radius);
		
		if candidate.t <= 1.0 {
			//tracing::debug! ("Tri {}, type {:?}", candidate.i, candidate.c_type);
			t_remaining *= 1.0 - candidate.t;
			let speed_towards_normal = -Vec3::dot (new_vel, candidate.normal);
			
			let push_out_pos = candidate.p_impact + candidate.normal * margin;
			
			// Rewind the object to when it hit the margin
			let speed = new_vel.length ();
			let dir = new_vel / speed;
			old_pos = candidate.p_impact + dir * margin / Vec3::dot (candidate.normal, dir);
			
			// Push the object out of the triangle along the normal
			new_vel += candidate.normal * speed_towards_normal;
			// But also compensate for the slide distance it lost
			new_pos = push_out_pos + new_vel * dt * t_remaining;
			
			normals_hit.push (candidate.normal);
		}
		else {
			t_remaining = 0.0;
			old_pos = new_pos;
			break;
		}
	}
	
	PhysicsResult {
		body: PhysicsBody {
			pos: old_pos,
			vel: new_vel,
		},
		normals_hit,
		kill: false,
	}
}

pub fn get_candidate (
	tris: &[Triangle],
	aabbs: &[Aabb],
	p0: Vec3, p1: Vec3, 
	radius: f32
)
-> Collision
{
	let radius3 = Vec3::from ((
		radius,
		radius,
		radius
	));
	let v = p1 - p0;
	
	let mut candidate = Collision {
		t: 2.0,
		p_impact: Default::default (),
		normal: Default::default (),
		i: 0,
		c_type: CollisionType::Face,
	};
	
	let ray_min = p0.min (p1) - radius3;
	let ray_max = p0.max (p1) + radius3;
	
	for b in aabbs {
		if 
			ray_max.x < b.min.x || ray_min.x > b.max.x ||
			ray_max.y < b.min.y || ray_min.y > b.max.y ||
			ray_max.z < b.min.z || ray_min.z > b.max.z
		{
			// AABB reject
			// tracing::trace! ("AABB reject");
			continue;
		}
		
		let verts = [
			(b.min.x, b.min.y, b.min.z).into (),
			(b.max.x, b.min.y, b.min.z).into (),
			(b.max.x, b.max.y, b.min.z).into (),
			(b.min.x, b.max.y, b.min.z).into (),
			
			(b.min.x, b.min.y, b.max.z).into (),
			(b.max.x, b.min.y, b.max.z).into (),
			(b.max.x, b.max.y, b.max.z).into (),
			(b.min.x, b.max.y, b.max.z).into (),
		];
		
		for (a, b, c, d) in [
			(0, 1, 2, 3),
			(4, 7, 6, 5),
			
			(2, 1, 5, 6),
			(3, 2, 6, 7),
			
			(0, 3, 7, 4),
			(1, 0, 4, 5),
		] {
			let a = verts [a];
			let b = verts [b];
			let c = verts [c];
			let d = verts [d];
			
			let normal = Vec3::cross (c - b, b - a).normalize ();
			
			if let Some (c) = get_candidate_face (&[a, b, c, d], normal, p0, p1, radius) {
				candidate = candidate.take_if_closer (&Collision {
					t: c.t,
					p_impact: c.p_impact,
					normal: c.normal,
					i: 0,
					c_type: CollisionType::Face,
				});
			}
		}
		
		for (a, b) in [
			(0, 1),
			(1, 2),
			(2, 3),
			(3, 0),
			
			(4, 5),
			(5, 6),
			(6, 7),
			(7, 4),
			
			(0, 4),
			(1, 5),
			(2, 6),
			(3, 7),
		] {
			let a = verts [a];
			let b = verts [b];
			
			if let Some (c) = get_candidate_edge (a, b, p0, p1, radius) {
				candidate = candidate.take_if_closer (&Collision {
					t: c.t,
					p_impact: c.p_impact,
					normal: c.normal,
					i: 0,
					c_type: CollisionType::Edge,
				});
			}
		}
		
		for vert in &verts {
			if let Some (c) = get_candidate_vert (*vert, p0, p1, radius) {
				candidate = candidate.take_if_closer (&Collision {
					t: c.t,
					p_impact: c.p_impact,
					normal: c.normal,
					i: 0,
					c_type: CollisionType::Vert,
				});
			}
		}
	}
	
	for (i, tri) in tris.iter ().enumerate () {
		let tri_min = tri.min ();
		let tri_max = tri.max ();
		
		if 
			ray_max.x < tri_min.x || ray_min.x > tri_max.x ||
			ray_max.y < tri_min.y || ray_min.y > tri_max.y ||
			ray_max.z < tri_min.z || ray_min.z > tri_max.z
		{
			// AABB reject
			// tracing::trace! ("AABB reject");
			continue;
		}
		
		// Collision for each triangle is roughly split into:
		// Face collisions
		// Edge collisions
		// Vertex collisions
		
		let normal = Vec3::cross (tri.verts [2] - tri.verts [1], tri.verts [1] - tri.verts [0]).normalize ();
		
		if let Some (c) = get_candidate_face (&tri.verts, normal, p0, p1, radius) {
			candidate = candidate.take_if_closer (&Collision {
				t: c.t,
				p_impact: c.p_impact,
				normal: c.normal,
				i,
				c_type: CollisionType::Face,
			});
		}
		
		// Edge collisions
		
		for j in 0..3 {
			let a = tri.verts [j];
			let b = tri.verts [(j + 1) % 3];
			
			if let Some (c) = get_candidate_edge (a, b, p0, p1, radius) {
				candidate = candidate.take_if_closer (&Collision {
					t: c.t,
					p_impact: c.p_impact,
					normal: c.normal,
					i,
					c_type: CollisionType::Edge,
				});
			}
		}
		
		// Vertex collisions
		
		for j in 0..3 {
			let a = tri.verts [j];
			
			if let Some (c) = get_candidate_vert (a, p0, p1, radius) {
				candidate = candidate.take_if_closer (&Collision {
					t: c.t,
					p_impact: c.p_impact,
					normal: c.normal,
					i,
					c_type: CollisionType::Vert,
				});
			}
		}
	}
	
	candidate
}

/// Collide a ray with a convex planar face, like a triangle or a rectangle.

fn get_candidate_face (verts: &[Vec3], normal: Vec3, p0: Vec3, p1: Vec3, radius: f32) 
-> Option <PrimCollision> 
{
	let radius3 = Vec3::from ((
		radius,
		radius,
		radius
	));
	let v = p1 - p0;
	
	let distance_to_face0 = Vec3::dot (normal, p0 - verts [0]) - radius;
	let distance_to_face1 = Vec3::dot (normal, p1 - verts [0]) - radius;
	
	if distance_to_face0 < 0.0 || distance_to_face1 > 0.0 {
		// tracing::trace! ("passed_plane {} {}", distance_to_face0, distance_to_face1);
		return None;
	}
	
	let denom = distance_to_face0 - distance_to_face1;
	let t_times_denom = distance_to_face0;
	
	// Because of previous early returns we know that 0.0 < t < 1.0
	
	let p_impact_times_denom = p0 * (denom - t_times_denom) + p1 * (t_times_denom);
	
	for j in 0..verts.len () {
		let a = verts [j];
		let b = verts [(j + 1) % verts.len ()];
		let tangent = Vec3::cross (b - a, normal);
		
		if Vec3::dot (tangent, p_impact_times_denom - a * denom) < 0.0 {
			// tracing::trace! ("impact_inside_tri");
			return None;
		}
	}
	
	Some (PrimCollision {
		t: t_times_denom / denom,
		p_impact: p_impact_times_denom / denom,
		normal,
	})
}

fn get_candidate_edge (a: Vec3, b: Vec3, p0: Vec3, p1: Vec3, radius: f32) 
-> Option <PrimCollision> 
{
	let cylinder_axis = (b - a).normalize ();
	let third_axis = Vec3::cross (cylinder_axis, p1 - p0).normalize ();
	let perp_ray_axis = Vec3::cross (third_axis, cylinder_axis);
	
	let a_minus_p0 = a - p0;
	
	let a_ray = Vec2::from ((
		Vec3::dot (a_minus_p0, perp_ray_axis),
		Vec3::dot (a_minus_p0, third_axis)
	));
	
	// a_ray is now in a space where X is the ray's t,
	// Z is the cylinder axis (and irrelevant for now),
	// and Y is the cross of those.
	
	// I forgot the maths word for this
	let discriminant = radius * radius - a_ray.y * a_ray.y;
	
	if discriminant < 0.0 {
		// No possible collision
		// println! ("No possible collision");
		return None;
	}
	
	let denom = (p1 - p0).length ();
	let t_times_denom = a_ray.x - discriminant.sqrt ();
	let t = t_times_denom / denom;
	
	if t < 0.0 || t > 1.0 {
		// The cylinder is along the line,
		// but outside the line segment
		//triangles_hit.push_back (i);
		// tracing::trace! ("Cylinder is along line but outside line segment {}", t);
		return None;
	}
	
	let p_impact = p0 * (1.0 - t) + p1 * (t);
	
	let impact_along_cylinder = Vec3::dot (cylinder_axis, p_impact);
	let a_along_cylinder = Vec3::dot (cylinder_axis, a);
	
	if impact_along_cylinder < a_along_cylinder || impact_along_cylinder > Vec3::dot (cylinder_axis, b)
	{
		// The infinite cylinder is on the line segment,
		// but the finite cylinder is not.
		// tracing::trace! ("Finite cylinder reject");
		return None;
	}
	
	let edge_normal = (p_impact - (a + (impact_along_cylinder - a_along_cylinder) * cylinder_axis)).normalize ();
	
	//let into_triangle = Vec3::cross (cylinder_axis, normal);
	let speed_towards_edge = -Vec3::dot (p1 - p0, edge_normal);
	//let speed_into_triangle = Vec3::dot (p1 - p0, into_triangle);
	
	if ! (speed_towards_edge > 0.0 /*&& speed_into_triangle >= 0.0*/) {
		// tracing::trace! ("speeds are wrong");
		return None;
	}
	
	Some (PrimCollision {
		t,
		p_impact,
		normal: edge_normal,
	})
}

fn get_candidate_vert (a: Vec3, p0: Vec3, p1: Vec3, radius: f32) 
-> Option <PrimCollision> 
{
	let a_minus_p0 = a - p0;
	
	let ray_dir = (p1 - p0).normalize ();
	let a_ray_x = Vec3::dot (a_minus_p0, ray_dir);
	let nearest_on_ray = p0 + a_ray_x * ray_dir;
	let a_ray_y = (a - nearest_on_ray).length ();
	
	let discriminant = radius * radius - a_ray_y * a_ray_y;
	
	if discriminant < 0.0 {
		// The sphere is not on the line
		return None;
	}
	
	let t = (a_ray_x - discriminant.sqrt ()) / (p1 - p0).length ();
	
	if t < 0.0 || t > 1.0 {
		// The sphere is along the line,
		// but outside the line segment
		return None;
	}
	
	let p_impact = p0 * (1.0 - t) + p1 * (t);
	let vert_normal = (p_impact - a).normalize ();
	
	// I skip the speed check here cause I'm pretty sure
	// the previous work makes it redundant
	
	Some (PrimCollision {
		t,
		p_impact,
		normal: vert_normal,
	})
}

#[cfg (test)]
mod test {
	use super::*;
	
	#[test]
	fn test_physics () {
		// Remember, Z is up
		
		let world: Vec <_> = vec! [
			(
				(0.0, 0.0, 0.0),
				(0.0, 2.0, 0.0),
				(2.0, 0.0, 0.0),
			),
		].into_iter ()
		.map (|(v0, v1, v2)| {
			Triangle {
				verts: [
					v0.into (),
					v1.into (),
					v2.into (),
				],
			}
		})
		.collect ();
		
		let params = Params {
			dt: 1.0,
			gravity: (0.0, 0.0, 0.0).into (),
			margin: 0.00125,
		};
		
		let magic_0 = f32::sqrt (f32::powf (0.5 + params.margin, 2.0) / 2.0);
		
		for ((radius, body_before), e) in [
			// Ray striking triangle from above, stops at triangle
			(
				(
					0.0,
					PhysicsBody {
						pos: (0.5, 0.5, 0.5).into (),
						vel: (0.0, 0.0, -1.0).into (),
					},
				),
				PhysicsResult {
					body: PhysicsBody {
						pos: (0.5, 0.5, params.margin).into (),
						vel: (0.0, 0.0, 0.0).into (),
					},
					triangles_hit: vec! [0],
					kill: false,
				},
			),
			// Ball striking triangle from above, stops at ball radius
			(
				(
					0.5,
					PhysicsBody {
						pos: (0.5, 0.5, 2.0).into (),
						vel: (0.0, 0.0, -2.0).into (),
					},
				),
				PhysicsResult {
					body: PhysicsBody {
						pos: (0.5, 0.5, params.margin + 0.5).into (),
						vel: (0.0, 0.0, 0.0).into (),
					},
					triangles_hit: vec! [0],
					kill: false,
				},
			),
			// Ball striking triangle on edge
			(
				(
					0.5,
					PhysicsBody {
						pos: (-2.0, 1.0, 0.0).into (),
						vel: (2.0, 0.0, 0.0).into (),
					},
				),
				PhysicsResult {
					body: PhysicsBody {
						pos: (-params.margin - 0.5, 1.0, 0.0).into (),
						vel: (0.0, 0.0, 0.0).into (),
					},
					triangles_hit: vec! [0],
					kill: false,
				},
			),
			// Ball striking triangle on diagonal edge
			(
				(
					0.5,
					PhysicsBody {
						pos: (2.0, 2.0, 0.0).into (),
						vel: (-2.0, -2.0, 0.0).into (),
					},
				),
				PhysicsResult {
					body: PhysicsBody {
						pos: (1.0 + magic_0, 1.0 + magic_0, 0.0).into (),
						vel: (0.0, 0.0, 0.0).into (),
					},
					triangles_hit: vec! [0],
					kill: false,
				},
			),
		] {
			let a = step (&params, &world, radius, &body_before);
			
			assert! (a.body.pos.distance_squared (e.body.pos) < 0.00125);
			assert! (a.body.vel.distance_squared (e.body.vel) < 0.00125);
			assert_eq! (a.triangles_hit, e.triangles_hit);
			assert_eq! (a.kill, e.kill);
		}
		
		// With no bounce, a ball should settle on a flat triangle in one
		// frame and reach a steady state
		
		let params = Params {
			dt: 1.0,
			gravity: (0.0, 0.0, -1.0).into (),
			margin: 0.00125,
		};
		
		let radius = 0.5;
		let body_before = PhysicsBody {
			pos: (0.5, 0.5, 2.0).into (),
			vel: (0.0, 0.0, -4.0).into (),
		};
		
		assert_eq! (
			get_candidate (&world, (0.5, 0.5, 2.0).into (), (0.5, 0.5, -3.0).into (), radius),
			Collision {
				t: 0.3,
				p_impact: (0.5, 0.5, 0.5).into (),
				normal: (0.0, 0.0, 1.0).into (),
				i: 0,
			},
		);
		
		let a = step (&params, &world, radius, &body_before);
		
		let e = PhysicsResult {
			body: PhysicsBody {
				pos: (0.5, 0.5, 0.5 + params.margin).into (),
				vel: (0.0, 0.0, 0.0).into (),
			},
			triangles_hit: vec! [0],
			kill: false,
		};
		
		// Fixed point should be here
		// If this test passes, at least a ball can rest on a single horizontal
		// triangle. If it fails, that means the ball is not resting 
		// (maybe the velocity is non-zero even if the position is stable)
		// or the ball is going to gain energy and bounce away, or it's going
		// to slide through the triangle.
		
		let body_before = PhysicsBody {
			pos: (0.5, 0.5, 0.5 + params.margin).into (),
			vel: (0.0, 0.0, 0.0).into (),
		};
		
		let a = step (&params, &world, radius, &body_before);
		
		let e = PhysicsResult {
			body: PhysicsBody {
				pos: (0.5, 0.5, 0.5 + params.margin).into (),
				vel: (0.0, 0.0, 0.0).into (),
			},
			triangles_hit: vec! [0],
			kill: false,
		};
		
		assert_eq! (a, e);
	}
}