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

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

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

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

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, Debug, PartialEq)]
pub struct Collision {
	t: f32,
	p_impact: Vec3,
	normal: Vec3,
	i: usize,
}

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 ()
		}
	}
}

pub fn get_candidate (world: &[Triangle], 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,
	};
	
	for (i, tri) in world.iter ().enumerate () {
		let tri_min = tri.min () - radius3;
		let tri_max = tri.max () + radius3;
		
		let ray_min = p0.min (p1);
		let ray_max = p0.max (p1);
		
		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
			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 ();
		
		let speed_towards_face = -Vec3::dot (v, normal);
		
		// Face collisions
		
		if speed_towards_face >= 0.0 {
			let distance_to_face0 = Vec3::dot (normal, p0 - tri.verts [0]) - radius;
			let distance_to_face1 = Vec3::dot (normal, p1 - tri.verts [0]) - radius;
			
			let passed_plane = distance_to_face0 > 0.0 && distance_to_face1 < 0.0;
			
			if passed_plane {
				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);
				
				let impact_inside_tri = (|| {
					for j in 0..3 {
						let a = tri.verts [j];
						let b = tri.verts [(j + 1) % 3];
						let tangent = Vec3::cross (b - a, normal);
						
						if Vec3::dot (tangent, p_impact_times_denom - a * denom) < 0.0 {
							return false;
						}
					}
					true
				})();
				
				if impact_inside_tri {
					// Stop it
					
					let c = Collision {
						t: t_times_denom / denom,
						p_impact: p_impact_times_denom / denom,
						normal,
						i,
					};
					
					candidate = candidate.take_if_closer (&c);
					
					// goto triangle_collided
				}
			}
		}
		
		// Edge collisions
		
		let ray_dir = (p1 - p0).normalize ();
		
		for j in 0..3 {
			let a = tri.verts [j];
			let b = tri.verts [(j + 1) % 3];
			
			let cylinder_axis = (b - a).normalize ();
			let third_axis = Vec3::cross (cylinder_axis, ray_dir).normalize ();
			let perp_ray_axis = Vec3::cross (third_axis, cylinder_axis);
			
			let into_triangle = Vec3::cross (cylinder_axis, normal);
			
			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
				continue;
			}
			
			let denom = (p1 - p0).length ();
			let t_times_denom = a_ray.x - discriminant.sqrt ();
			
			if t_times_denom < 0.0 || t_times_denom > denom {
				// The cylinder is along the line,
				// but outside the line segment
				//triangles_hit.push_back (i);
				continue;
			}
			
			let p_impact_times_denom = p0 * (denom - t_times_denom) + p1 * (t_times_denom);
			let p_impact = p_impact_times_denom / denom;
			
			let impact_along_cylinder_times_denom = Vec3::dot (cylinder_axis, p_impact_times_denom);
			let impact_along_cylinder = impact_along_cylinder_times_denom / denom;
			let a_along_cylinder = Vec3::dot (cylinder_axis, a);
			
			if impact_along_cylinder_times_denom < a_along_cylinder * denom || impact_along_cylinder_times_denom > Vec3::dot (cylinder_axis, b) * denom
			{
				// The infinite cylinder is on the line segment,
				// but the finite cylinder is not.
				//cout << "Finite cylinder reject" << endl;
				continue;
			}
			
			let edge_normal = (p_impact - (a + (impact_along_cylinder - a_along_cylinder) * cylinder_axis)).normalize ();
			
			let speed_towards_edge = -Vec3::dot (v, edge_normal);
			let speed_into_triangle = Vec3::dot (v, into_triangle);
			
			if speed_towards_edge > 0.0 && speed_into_triangle >= 0.0 {
				let c = Collision {
					t: t_times_denom / denom,
					p_impact,
					normal: edge_normal,
					i,
				};
				
				candidate = candidate.take_if_closer (&c);
			}
		}
		
		// Vertex collisions
		
		for j in 0..3 {
			let a = tri.verts [j];
			let a_minus_p0 = a - p0;
			
			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
				continue;
			}
			
			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
				continue;
			}
			
			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
			
			let c = Collision {
				t,
				p_impact,
				normal: vert_normal,
				i,
			};
			
			candidate = candidate.take_if_closer (&c);
		}
	}
	
	candidate
}

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

pub fn physics_step (
	params: &Params, world: &[Triangle], 
	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 triangles_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 (world, old_pos, new_pos, radius);
		
		if candidate.t <= 1.0 {
			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;
			
			triangles_hit.push (candidate.i);
		}
		else {
			t_remaining = 0.0;
			old_pos = new_pos;
			break;
		}
	}
	
	PhysicsResult {
		body: PhysicsBody {
			pos: old_pos,
			vel: new_vel,
		},
		triangles_hit,
		kill: false,
	}
}

#[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 = physics_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 = physics_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 = physics_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);
	}
}