use core::ops::{Bound,RangeBounds};

use strafesnet_common::integer::fixed_types::{F192_96,F256_128,F512_256};
use strafesnet_common::integer::vec3::Vector3;
use strafesnet_common::integer::{Planar64Vec3,Ratio,Fixed};
use crate::model::into_giga_time;
use crate::model::{SubmeshVertId,SubmeshEdgeId,SubmeshDirectedEdgeId,SubmeshFaceId,TransformedMesh,GigaTime};
use crate::mesh_query::{MeshQuery,MeshTopology,DirectedEdge,UndirectedEdge};
use crate::physics::{Time,Trajectory};

struct AsRefHelper<T>(T);
impl<T> AsRef<T> for AsRefHelper<T>{
	fn as_ref(&self)->&T{
		&self.0
	}
}

//Note that a face on a minkowski mesh refers to a pair of fevs on the meshes it's summed from
//(face,vertex)
//(edge,edge)
//(vertex,face)
#[derive(Clone,Copy,Debug,Eq,PartialEq)]
pub enum MinkowskiVert{
	VertVert(SubmeshVertId,SubmeshVertId),
}
// TODO: remove this
impl core::ops::Neg for MinkowskiVert{
	type Output=Self;
	fn neg(self)->Self::Output{
		match self{
			MinkowskiVert::VertVert(v0,v1)=>MinkowskiVert::VertVert(v1,v0),
		}
	}
}
#[derive(Clone,Copy,Debug)]
pub enum MinkowskiEdge{
	VertEdge(SubmeshVertId,SubmeshEdgeId),
	EdgeVert(SubmeshEdgeId,SubmeshVertId),
	//EdgeEdge when edges are parallel
}
impl UndirectedEdge for MinkowskiEdge{
	type DirectedEdge=MinkowskiDirectedEdge;
	fn as_directed(self,parity:bool)->Self::DirectedEdge{
		match self{
			MinkowskiEdge::VertEdge(v0,e1)=>MinkowskiDirectedEdge::VertEdge(v0,e1.as_directed(parity)),
			MinkowskiEdge::EdgeVert(e0,v1)=>MinkowskiDirectedEdge::EdgeVert(e0.as_directed(parity),v1),
		}
	}
}
#[derive(Clone,Copy,Debug)]
pub enum MinkowskiDirectedEdge{
	VertEdge(SubmeshVertId,SubmeshDirectedEdgeId),
	EdgeVert(SubmeshDirectedEdgeId,SubmeshVertId),
	//EdgeEdge when edges are parallel
}
impl DirectedEdge for MinkowskiDirectedEdge{
	type UndirectedEdge=MinkowskiEdge;
	fn as_undirected(self)->Self::UndirectedEdge{
		match self{
			MinkowskiDirectedEdge::VertEdge(v0,e1)=>MinkowskiEdge::VertEdge(v0,e1.as_undirected()),
			MinkowskiDirectedEdge::EdgeVert(e0,v1)=>MinkowskiEdge::EdgeVert(e0.as_undirected(),v1),
		}
	}
	fn parity(&self)->bool{
		match self{
			MinkowskiDirectedEdge::VertEdge(_,e)
			|MinkowskiDirectedEdge::EdgeVert(e,_)=>e.parity(),
		}
	}
}
#[derive(Clone,Copy,Debug,Hash)]
pub enum MinkowskiFace{
	VertFace(SubmeshVertId,SubmeshFaceId),
	EdgeEdge(SubmeshEdgeId,SubmeshEdgeId,bool),
	FaceVert(SubmeshFaceId,SubmeshVertId),
	//EdgeFace
	//FaceEdge
	//FaceFace
}

#[derive(Debug)]
pub struct MinkowskiMesh<'a>{
	mesh0:TransformedMesh<'a>,
	mesh1:TransformedMesh<'a>,
}

// TODO: remove this
impl<'a> core::ops::Neg for &MinkowskiMesh<'a>{
	type Output=MinkowskiMesh<'a>;
	fn neg(self)->Self::Output{
		MinkowskiMesh::minkowski_sum(self.mesh1,self.mesh0)
	}
}

impl MinkowskiMesh<'_>{
	pub fn minkowski_sum<'a>(mesh0:TransformedMesh<'a>,mesh1:TransformedMesh<'a>)->MinkowskiMesh<'a>{
		MinkowskiMesh{
			mesh0,
			mesh1,
		}
	}
	pub fn predict_collision_in(&self,trajectory:&Trajectory,range:impl RangeBounds<Time>)->Option<(MinkowskiFace,GigaTime)>{
		let start_position=match range.start_bound(){
			Bound::Included(time)=>trajectory.extrapolated_position(*time),
			Bound::Excluded(time)=>trajectory.extrapolated_position(*time),
			Bound::Unbounded=>trajectory.position,
		};
		let fev=crate::minimum_difference::closest_fev_not_inside(self,start_position)?;
		//continue forwards along the body parabola
		fev.crawl(self,trajectory,range.start_bound(),range.end_bound()).hit()
	}
	pub fn predict_collision_out(&self,trajectory:&Trajectory,range:impl RangeBounds<Time>)->Option<(MinkowskiFace,GigaTime)>{
		let (lower_bound,upper_bound)=(range.start_bound(),range.end_bound());
		// TODO: handle unbounded collision using infinity fev
		let start_position=match upper_bound{
			Bound::Included(time)=>trajectory.extrapolated_position(*time),
			Bound::Excluded(time)=>trajectory.extrapolated_position(*time),
			Bound::Unbounded=>trajectory.position,
		};
		let fev=crate::minimum_difference::closest_fev_not_inside(self,start_position)?;
		// swap and negate bounds to do a time inversion
		let (lower_bound,upper_bound)=(upper_bound.map(|&t|-t),lower_bound.map(|&t|-t));
		let time_reversed_trajectory=-trajectory;
		//continue backwards along the body parabola
		fev.crawl(self,&time_reversed_trajectory,lower_bound.as_ref(),upper_bound.as_ref()).hit()
			//no need to test -time<time_limit because of the first step
			.map(|(face,time)|(face,-time))
	}
	pub fn predict_collision_face_out(&self,trajectory:&Trajectory,range:impl RangeBounds<Time>,contact_face_id:MinkowskiFace)->Option<(MinkowskiDirectedEdge,GigaTime)>{
		// TODO: make better
		use crate::face_crawler::{low,upp};
		//no algorithm needed, there is only one state and two cases (Edge,None)
		//determine when it passes an edge ("sliding off" case)
		let start_time=range.start_bound().map(|&t|(t-trajectory.time).to_ratio());
		let mut best_time=range.end_bound().map(|&t|into_giga_time(t,trajectory.time));
		let mut best_edge=None;
		let face_n=self.face_nd(contact_face_id).0;
		self.for_each_face_edge(contact_face_id,|directed_edge_id|{
			let edge_n=self.directed_edge_n(directed_edge_id);
			//f x e points in
			let n=face_n.cross(edge_n);
			let &[v0,v1]=self.edge_verts(directed_edge_id.as_undirected()).as_ref();
			let d=n.dot(self.vert(v0)+self.vert(v1));
			//WARNING! d outside of *2
			//WARNING: truncated precision
			//wrap for speed
			for dt in F256_128::zeroes2(((n.dot(trajectory.position))*2-d).wrap_256(),n.dot(trajectory.velocity).wrap_256()*2,n.dot(trajectory.acceleration).wrap_256()){
				if low(&start_time,&dt)&&upp(&dt,&best_time)&&n.dot(trajectory.extrapolated_velocity_ratio_dt(dt)).is_negative(){
					best_time=Bound::Included(dt);
					best_edge=Some((directed_edge_id,dt));
					break;
				}
			}
		});
		best_edge
	}
	pub fn contains_point(&self,point:Planar64Vec3)->bool{
		crate::minimum_difference::contains_point(self,point)
	}
}
impl MeshQuery for MinkowskiMesh<'_>{
	type Direction=Planar64Vec3;
	type Position=Planar64Vec3;
	type Normal=Vector3<F192_96>;
	type Offset=F256_128;
	// TODO: relative d
	fn face_nd(&self,face_id:MinkowskiFace)->(Self::Normal,Self::Offset){
		match face_id{
			MinkowskiFace::VertFace(v0,f1)=>{
				let (n,d)=self.mesh1.face_nd(f1);
				(-n,d-n.dot(self.mesh0.vert(v0)))
			},
			MinkowskiFace::EdgeEdge(e0,e1,parity)=>{
				let edge0_n=self.mesh0.edge_n(e0);
				let edge1_n=self.mesh1.edge_n(e1);
				let &[e0v0,e0v1]=self.mesh0.edge_verts(e0).as_ref();
				let &[e1v0,e1v1]=self.mesh1.edge_verts(e1).as_ref();
				let n=edge0_n.cross(edge1_n);
				let e0d=n.dot(self.mesh0.vert(e0v0)+self.mesh0.vert(e0v1));
				let e1d=n.dot(self.mesh1.vert(e1v0)+self.mesh1.vert(e1v1));
				((n*(parity as i64*4-2)).widen_192(),((e0d-e1d)*(parity as i64*2-1)).widen_256())
			},
			MinkowskiFace::FaceVert(f0,v1)=>{
				let (n,d)=self.mesh0.face_nd(f0);
				(n,d-n.dot(self.mesh1.vert(v1)))
			},
		}
	}
	fn vert(&self,vert_id:MinkowskiVert)->Planar64Vec3{
		match vert_id{
			MinkowskiVert::VertVert(v0,v1)=>{
				self.mesh0.vert(v0)-self.mesh1.vert(v1)
			},
		}
	}
	fn hint_point(&self)->Planar64Vec3{
		self.mesh0.hint_point()-self.mesh1.hint_point()
	}
	fn farthest_vert(&self,dir:Planar64Vec3)->MinkowskiVert{
		MinkowskiVert::VertVert(self.mesh0.farthest_vert(dir),self.mesh1.farthest_vert(-dir))
	}
	fn edge_n(&self,edge_id:Self::Edge)->Self::Direction{
		let &[v0,v1]=self.edge_verts(edge_id).as_ref();
		self.vert(v1)-self.vert(v0)
	}
	fn directed_edge_n(&self,directed_edge_id:Self::DirectedEdge)->Self::Direction{
		let &[v0,v1]=self.edge_verts(directed_edge_id.as_undirected()).as_ref();
		(self.vert(v1)-self.vert(v0))*((directed_edge_id.parity() as i64)*2-1)
	}
}
impl MeshTopology for MinkowskiMesh<'_>{
	type Face=MinkowskiFace;
	type Edge=MinkowskiEdge;
	type DirectedEdge=MinkowskiDirectedEdge;
	type Vert=MinkowskiVert;
	fn for_each_vert_edge(&self,vert_id:Self::Vert,mut f:impl FnMut(Self::DirectedEdge)){
		match vert_id{
			MinkowskiVert::VertVert(v0,v1)=>{
				//detect shared volume when the other mesh is mirrored along a test edge dir
				let v0f={
					let mut faces=Vec::new();
					self.mesh0.for_each_vert_face(v0,|face|faces.push(face));
					faces
				};
				let v1f={
					let mut faces=Vec::new();
					self.mesh1.for_each_vert_face(v1,|face|faces.push(face));
					faces
				};
				let v0f_n:Vec<_>=v0f.iter().map(|&face_id|self.mesh0.face_nd(face_id).0).collect();
				let v1f_n:Vec<_>=v1f.iter().map(|&face_id|self.mesh1.face_nd(face_id).0).collect();
				// scratch vector
				let mut face_normals=Vec::with_capacity(v0f.len()+v1f.len());
				face_normals.clone_from(&v0f_n);
				self.mesh0.for_each_vert_edge(v0,|directed_edge_id|{
					let n=self.mesh0.directed_edge_n(directed_edge_id);
					let nn=n.dot(n);
					// TODO: there's gotta be a better way to do this
					// drop faces beyond v0f_n
					face_normals.truncate(v0f.len());
					// make a set of faces from mesh0's perspective
					for face_n in &v1f_n{
						//add reflected mesh1 faces
						//wrap for speed
						face_normals.push(*face_n-(n*face_n.dot(n)*2/nn).divide().wrap_192());
					}
					if is_empty_volume(&face_normals){
						f(MinkowskiDirectedEdge::EdgeVert(directed_edge_id,v1));
					}
				});
				face_normals.clone_from(&v1f_n);
				self.mesh1.for_each_vert_edge(v1,|directed_edge_id|{
					let n=self.mesh1.directed_edge_n(directed_edge_id);
					let nn=n.dot(n);
					// drop faces beyond v1f_n
					face_normals.truncate(v1f.len());
					// make a set of faces from mesh1's perspective
					for face_n in &v0f_n{
						//wrap for speed
						face_normals.push(*face_n-(n*face_n.dot(n)*2/nn).divide().wrap_192());
					}
					if is_empty_volume(&face_normals){
						f(MinkowskiDirectedEdge::VertEdge(v0,directed_edge_id));
					}
				});
			},
		}
	}
	fn for_each_vert_face(&self,_vert_id:Self::Vert,_f:impl FnMut(Self::Face)){
		unimplemented!()
	}
	fn edge_faces(&self,edge_id:Self::Edge)->impl AsRef<[Self::Face;2]>{
		match edge_id{
			MinkowskiEdge::VertEdge(v0,e1)=>{
				//faces are listed backwards from the minkowski mesh
				let v0e={
					let mut edges=Vec::new();
					self.mesh0.for_each_vert_edge(v0,|edge|edges.push(edge));
					edges
				};
				let &[e1f0,e1f1]=self.mesh1.edge_faces(e1).as_ref();
				AsRefHelper([(e1f1,false),(e1f0,true)].map(|(edge_face_id1,face_parity)|{
					let mut best_edge=None;
					let mut best_d:Ratio<F512_256,F512_256>=Ratio::new(Fixed::ZERO,Fixed::ONE);
					let edge_face1_n=self.mesh1.face_nd(edge_face_id1).0;
					let edge_face1_nn=edge_face1_n.dot(edge_face1_n);
					for &directed_edge_id0 in &v0e{
						let edge0_n=self.mesh0.directed_edge_n(directed_edge_id0);
						//must be behind other face.
						let d=edge_face1_n.dot(edge0_n);
						if d.is_negative(){
							let edge0_nn=edge0_n.dot(edge0_n);
							// Assume not every number is huge
							// TODO: revisit this
							let dd=(d*d)/(edge_face1_nn*edge0_nn);
							if best_d<dd{
								best_d=dd;
								best_edge=Some(directed_edge_id0);
							}
						}
					}
					best_edge.map_or(
						MinkowskiFace::VertFace(v0,edge_face_id1),
						|directed_edge_id0|MinkowskiFace::EdgeEdge(directed_edge_id0.as_undirected(),e1,directed_edge_id0.parity()^face_parity)
					)
				}))
			},
			MinkowskiEdge::EdgeVert(e0,v1)=>{
				//tracking index with an external variable because .enumerate() is not available
				let v1e={
					let mut edges=Vec::new();
					self.mesh1.for_each_vert_edge(v1,|edge|edges.push(edge));
					edges
				};
				let &[e0f0,e0f1]=self.mesh0.edge_faces(e0).as_ref();
				AsRefHelper([(e0f0,true),(e0f1,false)].map(|(edge_face_id0,face_parity)|{
					let mut best_edge=None;
					let mut best_d:Ratio<F512_256,F512_256>=Ratio::new(Fixed::ZERO,Fixed::ONE);
					let edge_face0_n=self.mesh0.face_nd(edge_face_id0).0;
					let edge_face0_nn=edge_face0_n.dot(edge_face0_n);
					for &directed_edge_id1 in &v1e{
						let edge1_n=self.mesh1.directed_edge_n(directed_edge_id1);
						let d=edge_face0_n.dot(edge1_n);
						if d.is_negative(){
							let edge1_nn=edge1_n.dot(edge1_n);
							let dd=(d*d)/(edge_face0_nn*edge1_nn);
							if best_d<dd{
								best_d=dd;
								best_edge=Some(directed_edge_id1);
							}
						}
					}
					best_edge.map_or(
						MinkowskiFace::FaceVert(edge_face_id0,v1),
						|directed_edge_id1|MinkowskiFace::EdgeEdge(e0,directed_edge_id1.as_undirected(),directed_edge_id1.parity()^face_parity)
					)
				}))
			},
		}
	}
	fn edge_verts(&self,edge_id:Self::Edge)->impl AsRef<[Self::Vert;2]>{
		AsRefHelper(match edge_id{
			MinkowskiEdge::VertEdge(v0,e1)=>self.mesh1.edge_verts(e1).as_ref().map(|vert_id1|
				MinkowskiVert::VertVert(v0,vert_id1)
			),
			MinkowskiEdge::EdgeVert(e0,v1)=>self.mesh0.edge_verts(e0).as_ref().map(|vert_id0|
				MinkowskiVert::VertVert(vert_id0,v1)
			),
		})
	}
	fn for_each_face_vert(&self,_face_id:Self::Face,_f:impl FnMut(Self::Vert)){
		unimplemented!()
	}
	fn for_each_face_edge(&self,face_id:Self::Face,mut f:impl FnMut(Self::DirectedEdge)){
		match face_id{
			MinkowskiFace::VertFace(v0,f1)=>{
				self.mesh1.for_each_face_edge(f1,|edge_id1|
					f(MinkowskiDirectedEdge::VertEdge(v0,edge_id1.reverse()))
				)
			},
			MinkowskiFace::EdgeEdge(e0,e1,parity)=>{
				let &[e0v0,e0v1]=self.mesh0.edge_verts(e0).as_ref();
				let &[e1v0,e1v1]=self.mesh1.edge_verts(e1).as_ref();
				//could sort this if ordered edges are needed
				//probably just need to reverse this list according to parity
				f(MinkowskiDirectedEdge::VertEdge(e0v0,e1.as_directed(parity)));
				f(MinkowskiDirectedEdge::EdgeVert(e0.as_directed(!parity),e1v0));
				f(MinkowskiDirectedEdge::VertEdge(e0v1,e1.as_directed(!parity)));
				f(MinkowskiDirectedEdge::EdgeVert(e0.as_directed(parity),e1v1));
			},
			MinkowskiFace::FaceVert(f0,v1)=>{
				self.mesh0.for_each_face_edge(f0,|edge_id0|
					f(MinkowskiDirectedEdge::EdgeVert(edge_id0,v1))
				)
			},
		}
	}
}

fn is_empty_volume(normals:&[Vector3<F192_96>])->bool{
	let len=normals.len();
	for i in 0..len-1{
		for j in i+1..len{
			let n=normals[i].cross(normals[j]);
			let mut d_comp=None;
			for k in 0..len{
				if k!=i&&k!=j{
					let d=n.dot(normals[k]).is_negative();
					if let &Some(comp)=&d_comp{
						// This is testing if d_comp*d < 0
						if comp^d{
							return true;
						}
					}else{
						d_comp=Some(d);
					}
				}
			}
		}
	}
	return false;
}

#[test]
fn test_is_empty_volume(){
	use strafesnet_common::integer::vec3;
	assert!(!is_empty_volume(&[vec3::X.widen_192(),vec3::Y.widen_192(),vec3::Z.widen_192()]));
	assert!(is_empty_volume(&[vec3::X.widen_192(),vec3::Y.widen_192(),vec3::Z.widen_192(),vec3::NEG_X.widen_192()]));
}
