use crate::model::{into_giga_time,GigaTime,FEV,MeshQuery,DirectedEdge};
use strafesnet_common::integer::{Fixed,Ratio,vec3::Vector3};
use crate::physics::{Time,Body};

use core::ops::Bound;

enum Transition<M:MeshQuery>{
	Miss,
	Next(FEV<M>,GigaTime),
	Hit(M::Face,GigaTime),
}

pub enum CrawlResult<M:MeshQuery>{
	Miss(FEV<M>),
	Hit(M::Face,GigaTime),
}
impl<M:MeshQuery> CrawlResult<M>{
	pub fn hit(self)->Option<(M::Face,GigaTime)>{
		match self{
			CrawlResult::Miss(_)=>None,
			CrawlResult::Hit(face,time)=>Some((face,time)),
		}
	}
	pub fn miss(self)->Option<FEV<M>>{
		match self{
			CrawlResult::Miss(fev)=>Some(fev),
			CrawlResult::Hit(_,_)=>None,
		}
	}
}

// TODO: move predict_collision_face_out algorithm in here or something

/// check_lower_bound
pub fn low<LhsNum,LhsDen,RhsNum,RhsDen,T>(lower_bound:&Bound<Ratio<LhsNum,LhsDen>>,dt:&Ratio<RhsNum,RhsDen>)->bool
where
	RhsNum:Copy,
	RhsDen:Copy,
	LhsNum:Copy,
	LhsDen:Copy,
	LhsDen:strafesnet_common::integer::Parity,
	RhsDen:strafesnet_common::integer::Parity,
	LhsNum:core::ops::Mul<RhsDen,Output=T>,
	LhsDen:core::ops::Mul<RhsNum,Output=T>,
	T:Ord+Copy,
{
	match lower_bound{
		Bound::Included(time)=>time.le_ratio(*dt),
		Bound::Excluded(time)=>time.lt_ratio(*dt),
		Bound::Unbounded=>true,
	}
}
/// check_upper_bound
pub fn upp<LhsNum,LhsDen,RhsNum,RhsDen,T>(dt:&Ratio<LhsNum,LhsDen>,upper_bound:&Bound<Ratio<RhsNum,RhsDen>>)->bool
where
	RhsNum:Copy,
	RhsDen:Copy,
	LhsNum:Copy,
	LhsDen:Copy,
	LhsDen:strafesnet_common::integer::Parity,
	RhsDen:strafesnet_common::integer::Parity,
	LhsNum:core::ops::Mul<RhsDen,Output=T>,
	LhsDen:core::ops::Mul<RhsNum,Output=T>,
	T:Ord+Copy,
{
	match upper_bound{
		Bound::Included(time)=>dt.le_ratio(*time),
		Bound::Excluded(time)=>dt.lt_ratio(*time),
		Bound::Unbounded=>true,
	}
}

impl<F:Copy,M:MeshQuery<Normal=Vector3<F>,Offset=Fixed<4,128>>> FEV<M>
	where
		// This is hardcoded for MinkowskiMesh lol
		M::Face:Copy,
		M::Edge:Copy,
		M::Vert:Copy,
		F:core::ops::Mul<Fixed<1,32>,Output=Fixed<4,128>>,
		<F as core::ops::Mul<Fixed<1,32>>>::Output:core::iter::Sum,
		M::Offset:core::ops::Sub<<F as std::ops::Mul<Fixed<1,32>>>::Output>,
{
	fn next_transition(&self,mesh:&M,body:&Body,lower_bound:Bound<GigaTime>,mut upper_bound:Bound<GigaTime>)->Transition<M>{
		//conflicting derivative means it crosses in the wrong direction.
		//if the transition time is equal to an already tested transition, do not replace the current best.
		let mut best_transition=Transition::Miss;
		match self{
			&FEV::Face(face_id)=>{
				//test own face collision time, ignoring roots with zero or conflicting derivative
				//n=face.normal d=face.dot
				//n.a t^2+n.v t+n.p-d==0
				let (n,d)=mesh.face_nd(face_id);
				//TODO: use higher precision d value?
				//use the mesh transform translation instead of baking it into the d value.
				for dt in Fixed::<4,128>::zeroes2((n.dot(body.position)-d)*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){
					if low(&lower_bound,&dt)&&upp(&dt,&upper_bound)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
						upper_bound=Bound::Included(dt);
						best_transition=Transition::Hit(face_id,dt);
						break;
					}
				}
				//test each edge collision time, ignoring roots with zero or conflicting derivative
				for &directed_edge_id in mesh.face_edges(face_id).as_ref(){
					let edge_n=mesh.directed_edge_n(directed_edge_id);
					let n=n.cross(edge_n);
					let &[v0,v1]=mesh.edge_verts(directed_edge_id.as_undirected()).as_ref();
					//WARNING: d is moved out of the *2 block because of adding two vertices!
					//WARNING: precision is swept under the rug!
					//wrap for speed
					for dt in Fixed::<4,128>::zeroes2(n.dot(body.position*2-(mesh.vert(v0)+mesh.vert(v1))).wrap_4(),n.dot(body.velocity).wrap_4()*2,n.dot(body.acceleration).wrap_4()){
						if low(&lower_bound,&dt)&&upp(&dt,&upper_bound)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
							upper_bound=Bound::Included(dt);
							best_transition=Transition::Next(FEV::Edge(directed_edge_id.as_undirected()),dt);
							break;
						}
					}
				}
				//if none:
			},
			&FEV::Edge(edge_id)=>{
				//test each face collision time, ignoring roots with zero or conflicting derivative
				let edge_verts=mesh.edge_verts(edge_id);
				let &[ev0,ev1]=edge_verts.as_ref();
				let (v0,v1)=(mesh.vert(ev0),mesh.vert(ev1));
				let edge_n=v1-v0;
				let delta_pos=body.position*2-(v0+v1);
				for (i,&edge_face_id) in mesh.edge_faces(edge_id).as_ref().iter().enumerate(){
					let face_n=mesh.face_nd(edge_face_id).0;
					//edge_n gets parity from the order of edge_faces
					let n=face_n.cross(edge_n)*((i as i64)*2-1);
					//WARNING yada yada d *2
					//wrap for speed
					for dt in Fixed::<4,128>::zeroes2(n.dot(delta_pos).wrap_4(),n.dot(body.velocity).wrap_4()*2,n.dot(body.acceleration).wrap_4()){
						if low(&lower_bound,&dt)&&upp(&dt,&upper_bound)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
							upper_bound=Bound::Included(dt);
							best_transition=Transition::Next(FEV::Face(edge_face_id),dt);
							break;
						}
					}
				}
				//test each vertex collision time, ignoring roots with zero or conflicting derivative
				for (i,&vert_id) in edge_verts.as_ref().iter().enumerate(){
					//vertex normal gets parity from vert index
					let n=edge_n*(1-2*(i as i64));
					for dt in Fixed::<2,64>::zeroes2((n.dot(body.position-mesh.vert(vert_id)))*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){
						if low(&lower_bound,&dt)&&upp(&dt,&upper_bound)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
							let dt=Ratio::new(dt.num.widen_4(),dt.den.widen_4());
							upper_bound=Bound::Included(dt);
							best_transition=Transition::Next(FEV::Vert(vert_id),dt);
							break;
						}
					}
				}
				//if none:
			},
			&FEV::Vert(vert_id)=>{
				//test each edge collision time, ignoring roots with zero or conflicting derivative
				for &directed_edge_id in mesh.vert_edges(vert_id).as_ref(){
					//edge is directed away from vertex, but we want the dot product to turn out negative
					let n=-mesh.directed_edge_n(directed_edge_id);
					for dt in Fixed::<2,64>::zeroes2((n.dot(body.position-mesh.vert(vert_id)))*2,n.dot(body.velocity)*2,n.dot(body.acceleration)){
						if low(&lower_bound,&dt)&&upp(&dt,&upper_bound)&&n.dot(body.extrapolated_velocity_ratio_dt(dt)).is_negative(){
							let dt=Ratio::new(dt.num.widen_4(),dt.den.widen_4());
							upper_bound=Bound::Included(dt);
							best_transition=Transition::Next(FEV::Edge(directed_edge_id.as_undirected()),dt);
							break;
						}
					}
				}
				//if none:
			},
		}
		best_transition
	}
	pub fn crawl(mut self,mesh:&M,relative_body:&Body,lower_bound:Bound<&Time>,upper_bound:Bound<&Time>)->CrawlResult<M>{
		let mut lower_bound=lower_bound.map(|&t|into_giga_time(t,relative_body.time));
		let upper_bound=upper_bound.map(|&t|into_giga_time(t,relative_body.time));
		for _ in 0..20{
			match self.next_transition(mesh,relative_body,lower_bound,upper_bound){
				Transition::Miss=>return CrawlResult::Miss(self),
				Transition::Next(next_fev,next_time)=>(self,lower_bound)=(next_fev,Bound::Included(next_time)),
				Transition::Hit(face,time)=>return CrawlResult::Hit(face,time),
			}
		}
		//TODO: fix all bugs
		//println!("Too many iterations!  Using default behaviour instead of crashing...");
		CrawlResult::Miss(self)
	}
}
