use strafesnet_common::integer::{Planar64,Planar64Vec3};

// This algorithm is based on Lua code
// written by Trey Reynolds in 2021

// 1/2^10
const EPSILON:Planar64=Planar64::raw(1<<(32-10));

// A stack-allocated variable-size list that holds up to 4 elements
// Direct references are used instead of indices i0, i1, i2, i3
type Conts<'a>=arrayvec::ArrayVec<&'a Contact,4>;

struct Ray{
	origin:Planar64Vec3,
	direction:Planar64Vec3,
}
impl Ray{
	fn extrapolate(&self,t:Planar64)->Planar64Vec3{
		self.origin+self.direction*t
	}
}

/// Information about a contact restriction
pub struct Contact{
	pub position:Planar64Vec3,
	pub velocity:Planar64Vec3,
	pub normal:Planar64Vec3,
}
impl Contact{
	fn relative_to(&self,point:Planar64Vec3)->Self{
		Self{
			position:self.position-point,
			velocity:self.velocity,
			normal:self.normal,
		}
	}
	fn relative_dot(&self,direction:Planar64Vec3)->Planar64{
		(direction-self.velocity).dot(self.normal)
	}
	/// Calculate the time of intersection. (previously get_touch_time)
	fn solve(&self,ray:&Ray)->Planar64{
		(self.position-ray.origin).dot(self.normal)/(ray.direction-self.velocity).dot(self.normal)
	}
}

//note that this is horrible with fixed point arithmetic
fn solve1(c0:&Contact)->Option<Planar64Vec3>{
	let det=c0.normal.dot(c0.velocity);
	if det.get().abs()<EPSILON.get(){
		return None;
	}
	let d0=c0.normal.dot(c0.position);
	Some(c0.normal*d0/det)
}
fn solve2(c0:&Contact,c1:&Contact)->Option<Planar64Vec3>{
	let u0_u1=c0.velocity.cross(c1.velocity);
	let n0_n1=c0.normal.cross(c1.normal);
	let det=u0_u1.dot(n0_n1);
	if det.get().abs()<EPSILON.get(){
		return None;
	}
	let d0=c0.normal.dot(c0.position);
	let d1=c1.normal.dot(c1.position);
	Some((c1.normal.cross(u0_u1)*d0+u0_u1.cross(c0.normal)*d1)/det)
}
fn solve3(c0:&Contact,c1:&Contact,c2:&Contact)->Option<Planar64Vec3>{
	let n0_n1=c0.normal.cross(c1.normal);
	let det=c2.normal.dot(n0_n1);
	if det.get().abs()<EPSILON.get(){
		return None;
	}
	let d0=c0.normal.dot(c0.position);
	let d1=c1.normal.dot(c1.position);
	let d2=c2.normal.dot(c2.position);
	Some((c1.normal.cross(c2.normal)*d0+c2.normal.cross(c0.normal)*d1+c0.normal.cross(c1.normal)*d2)/det)
}

fn decompose1(point:Planar64Vec3,u0:Planar64Vec3)->Option<Planar64>{
	let det=u0.dot(u0);
	if det==Planar64::ZERO{
		return None;
	}
	let s0=u0.dot(point)/det;
	Some(s0)
}
fn decompose2(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3)->Option<(Planar64,Planar64)>{
	let u0_u1=u0.cross(u1);
	let det=u0_u1.dot(u0_u1);
	if det==Planar64::ZERO{
		return None;
	}
	let s0=u0_u1.dot(point.cross(u1))/det;
	let s1=u0_u1.dot(u0.cross(point))/det;
	Some((s0,s1))
}
fn decompose3(point:Planar64Vec3,u0:Planar64Vec3,u1:Planar64Vec3,u2:Planar64Vec3)->Option<(Planar64,Planar64,Planar64)>{
	let det=u0.cross(u1).dot(u2);
	if det==Planar64::ZERO{
		return None;
	}
	let s0=point.cross(u1).dot(u2)/det;
	let s1=u0.cross(point).dot(u2)/det;
	let s2=u0.cross(u1).dot(point)/det;
	Some((s0,s1,s2))
}

fn is_space_enclosed_2(
	a:Planar64Vec3,
	b:Planar64Vec3,
)->bool{
	a.cross(b)==Planar64Vec3::ZERO
	&&a.dot(b)<Planar64::ZERO
}
fn is_space_enclosed_3(
	a:Planar64Vec3,
	b:Planar64Vec3,
	c:Planar64Vec3
)->bool{
	a.cross(b).dot(c)==Planar64::ZERO
	&&{
		let det_abac=a.cross(b).dot(a.cross(c));
		let det_abbc=a.cross(b).dot(b.cross(c));
		let det_acbc=a.cross(c).dot(b.cross(c));
		return det_abac*det_abbc<=Planar64::ZERO
			&& det_abbc*det_acbc<=Planar64::ZERO
			&&-det_acbc*det_abac<=Planar64::ZERO
			||is_space_enclosed_2(a,b)
			||is_space_enclosed_2(a,c)
			||is_space_enclosed_2(b,c)
	}
}
fn is_space_enclosed_4(
	a:Planar64Vec3,
	b:Planar64Vec3,
	c:Planar64Vec3,
	d:Planar64Vec3,
)->bool{
	let det_abc=a.cross(b).dot(c);
	let det_abd=a.cross(b).dot(d);
	let det_acd=a.cross(c).dot(d);
	let det_bcd=b.cross(c).dot(d);
	return det_abc*det_abd<Planar64::ZERO
		&&-det_abc*det_acd<Planar64::ZERO
		&& det_abd*det_acd<Planar64::ZERO
		&& det_abc*det_bcd<Planar64::ZERO
		&&-det_abd*det_bcd<Planar64::ZERO
		&& det_acd*det_bcd<Planar64::ZERO
		||is_space_enclosed_3(a,b,c)
		||is_space_enclosed_3(a,b,d)
		||is_space_enclosed_3(a,c,d)
		||is_space_enclosed_3(b,c,d)
}

const fn get_push_ray_0(point:Planar64Vec3)->Option<Ray>{
	Some(Ray{origin:point,direction:Planar64Vec3::ZERO})
}
fn get_push_ray_1(point:Planar64Vec3,c0:&Contact)->Option<Ray>{
	let direction=solve1(c0)?;
	let s0=decompose1(direction,c0.velocity)?;
	if s0<Planar64::ZERO{
		return None;
	}
	let origin=point+solve1(
		&c0.relative_to(point),
	)?;
	Some(Ray{origin,direction})
}
fn get_push_ray_2(point:Planar64Vec3,c0:&Contact,c1:&Contact)->Option<Ray>{
	let direction=solve2(c0,c1)?;
	let (s0,s1)=decompose2(direction,c0.velocity,c1.velocity)?;
	if s0<Planar64::ZERO||s1<Planar64::ZERO{
		return None;
	}
	let origin=point+solve2(
		&c0.relative_to(point),
		&c1.relative_to(point),
	)?;
	Some(Ray{origin,direction})
}
fn get_push_ray_3(point:Planar64Vec3,c0:&Contact,c1:&Contact,c2:&Contact)->Option<Ray>{
	let direction=solve3(c0,c1,c2)?;
	let (s0,s1,s2)=decompose3(direction,c0.velocity,c1.velocity,c2.velocity)?;
	if s0<Planar64::ZERO||s1<Planar64::ZERO||s2<Planar64::ZERO{
		return None;
	}
	let origin=point+solve3(
		&c0.relative_to(point),
		&c1.relative_to(point),
		&c2.relative_to(point),
	)?;
	Some(Ray{origin,direction})
}

const fn get_best_push_ray_and_conts_0<'a>(point:Planar64Vec3)->Option<(Ray,Conts<'a>)>{
	match get_push_ray_0(point){
		Some(ray)=>Some((ray,Conts::new_const())),
		None=>None,
	}
}
fn get_best_push_ray_and_conts_1(point:Planar64Vec3,c0:&Contact)->Option<(Ray,Conts)>{
	get_push_ray_1(point,c0)
		.map(|ray|(ray,Conts::from_iter([c0])))
}
fn get_best_push_ray_and_conts_2<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Contact)->Option<(Ray,Conts<'a>)>{
	if is_space_enclosed_2(c0.normal,c1.normal){
		return None;
	}
	if let Some(ray)=get_push_ray_2(point,c0,c1){
		return Some((ray,Conts::from_iter([c0,c1])));
	}
	if let Some(ray)=get_push_ray_1(point,c0){
		if Planar64::ZERO<=c1.relative_dot(ray.direction){
			return Some((ray,Conts::from_iter([c0])));
		}
	}
	return None;
}
fn get_best_push_ray_and_conts_3<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Contact,c2:&'a Contact)->Option<(Ray,Conts<'a>)>{
	if is_space_enclosed_3(c0.normal,c1.normal,c2.normal){
		return None;
	}
	if let Some(ray)=get_push_ray_3(point,c0,c1,c2){
		return Some((ray,Conts::from_iter([c0,c1,c2])));
	}
	if let Some(ray)=get_push_ray_2(point,c0,c1){
		if Planar64::ZERO<=c2.relative_dot(ray.direction){
			return Some((ray,Conts::from_iter([c0,c1])));
		}
	}
	if let Some(ray)=get_push_ray_2(point,c0,c2){
		if Planar64::ZERO<=c1.relative_dot(ray.direction){
			return Some((ray,Conts::from_iter([c0,c2])));
		}
	}
	if let Some(ray)=get_push_ray_1(point,c0){
		if Planar64::ZERO<=c1.relative_dot(ray.direction)
		 &&Planar64::ZERO<=c2.relative_dot(ray.direction){
			return Some((ray,Conts::from_iter([c0])));
		}
	}
	return None;
}
fn get_best_push_ray_and_conts_4<'a>(point:Planar64Vec3,c0:&'a Contact,c1:&'a Contact,c2:&'a Contact,c3:&'a Contact)->Option<(Ray,Conts<'a>)>{
	if is_space_enclosed_4(c0.normal,c1.normal,c2.normal,c3.normal){
		return None;
	}

	let (ray012,conts012)=get_best_push_ray_and_conts_3(point,c0,c1,c2)?;
	let (ray013,conts013)=get_best_push_ray_and_conts_3(point,c0,c1,c3)?;
	let (ray023,conts023)=get_best_push_ray_and_conts_3(point,c0,c2,c3)?;

	let err012=c3.relative_dot(ray012.direction);
	let err013=c2.relative_dot(ray013.direction);
	let err023=c1.relative_dot(ray023.direction);

	let best_err=err012.max(err013).max(err023);

	if best_err==err012{
		return Some((ray012,conts012))
	}else if best_err==err013{
		return Some((ray013,conts013))
	}else if best_err==err023{
		return Some((ray023,conts023))
	}
	unreachable!()
}

fn get_best_push_ray_and_conts<'a>(
	point:Planar64Vec3,
	conts:Conts<'a>,
)->Option<(Ray,Conts<'a>)>{
	match conts.as_slice(){
		&[c0,c1,c2,c3]=>get_best_push_ray_and_conts_4(point,c0,c1,c2,c3),
		&[c0,c1,c2]=>get_best_push_ray_and_conts_3(point,c0,c1,c2),
		&[c0,c1]=>get_best_push_ray_and_conts_2(point,c0,c1),
		&[c0]=>get_best_push_ray_and_conts_1(point,c0),
		&[]=>get_best_push_ray_and_conts_0(point),
		_=>unreachable!(),
	}
}

fn get_first_touch<'a>(contacts:&'a Vec<Contact>,ray:&Ray,conts:&Conts)->Option<(Planar64,&'a Contact)>{
	contacts.iter()
		.filter(|&contact|
			!conts.iter().any(|&c|std::ptr::eq(c,contact))
			&&contact.relative_dot(ray.direction)< -EPSILON
		)
		.map(|contact|(contact.solve(ray),contact))
		.min_by_key(|&(t,_)|t)
}

pub fn push_solve(contacts:&Vec<Contact>,point:Planar64Vec3)->Option<Planar64Vec3>{
	let (mut ray,mut conts)=get_best_push_ray_and_conts_0(point)?;
	loop{
		let (next_t,next_cont)=match get_first_touch(contacts,&ray,&conts){
			Some((t,conts))=>(t,conts),
			None=>return Some(ray.origin),
		};

		if Planar64::ZERO<=next_t{
			return Some(ray.origin);
		}

		//push_front
		if conts.len()==conts.capacity(){
			//this is a dead case, new_conts never has more than 3 elements
			conts.rotate_right(1);
			conts[0]=next_cont;
		}else{
			conts.push(next_cont);
			conts.rotate_right(1);
		}

		let meet_point=ray.extrapolate(next_t);
		match get_best_push_ray_and_conts(meet_point,conts){
			Some((new_ray,new_conts))=>(ray,conts)=(new_ray,new_conts),
			None=>return Some(meet_point),
		}
	}
}

#[cfg(test)]
mod tests{
	use super::*;
	#[test]
	fn test_push_solve(){
		let contacts=vec![
			Contact{
				position:Planar64Vec3::ZERO,
				velocity:Planar64Vec3::Y,
				normal:Planar64Vec3::Y,
			}
		];
		assert_eq!(
			Some(Planar64Vec3::ZERO),
			push_solve(&contacts,Planar64Vec3::NEG_Y)
		);
	}
}