use strafesnet_common::aabb;
use strafesnet_common::integer::{self,vec3,Time,Planar64,Planar64Vec3};
#[derive(Clone,Copy,Debug,Hash)]
pub struct Body<T>{
	pub position:Planar64Vec3,
	pub velocity:Planar64Vec3,
	pub time:Time<T>,
}
#[derive(Clone,Copy,Debug,Hash)]
pub struct Trajectory<T>{
	pub position:Planar64Vec3,
	pub velocity:Planar64Vec3,
	pub acceleration:Planar64Vec3,
	pub time:Time<T>,
}
impl<T> std::ops::Neg for Trajectory<T>{
	type Output=Self;
	fn neg(self)->Self::Output{
		Self{
			position:self.position,
			velocity:-self.velocity,
			acceleration:self.acceleration,
			time:-self.time,
		}
	}
}
impl<T:Copy> std::ops::Neg for &Trajectory<T>{
	type Output=Trajectory<T>;
	fn neg(self)->Self::Output{
		Trajectory{
			position:self.position,
			velocity:-self.velocity,
			acceleration:self.acceleration,
			time:-self.time,
		}
	}
}

impl<T> Body<T>
	where Time<T>:Copy,
{
	pub const ZERO:Self=Self::new(vec3::zero(),vec3::zero(),Time::ZERO);
	pub const fn new(position:Planar64Vec3,velocity:Planar64Vec3,time:Time<T>)->Self{
		Self{
			position,
			velocity,
			time,
		}
	}
	pub const fn with_acceleration(self,acceleration:Planar64Vec3)->Trajectory<T>{
		let Body{
			position,
			velocity,
			time,
		}=self;
		Trajectory{
			position,
			velocity,
			acceleration,
			time,
		}
	}
}

impl<T> Trajectory<T>
	where Time<T>:Copy,
{
	pub const ZERO:Self=Self::new(vec3::zero(),vec3::zero(),vec3::zero(),Time::ZERO);
	pub const fn new(position:Planar64Vec3,velocity:Planar64Vec3,acceleration:Planar64Vec3,time:Time<T>)->Self{
		Self{
			position,
			velocity,
			acceleration,
			time,
		}
	}
	pub fn relative_to(&self,trj0:&Self,time:Time<T>)->Self{
		//(p0,v0,a0,t0)
		//(p1,v1,a1,t1)
		Trajectory::new(
			self.extrapolated_position(time)-trj0.extrapolated_position(time),
			self.extrapolated_velocity(time)-trj0.extrapolated_velocity(time),
			self.acceleration-trj0.acceleration,
			time)
	}
	pub fn extrapolated_position(&self,time:Time<T>)->Planar64Vec3{
		let dt=time-self.time;
		self.position
		+(self.velocity*dt).map(|elem|elem.divide().clamp_64())
		+self.acceleration.map(|elem|(dt*dt*elem/2).divide().clamp_64())
	}
	pub fn extrapolated_velocity(&self,time:Time<T>)->Planar64Vec3{
		let dt=time-self.time;
		self.velocity+(self.acceleration*dt).map(|elem|elem.divide().clamp_64())
	}
	pub fn extrapolated_body(&self,time:Time<T>)->Body<T>{
		Body::new(
			self.extrapolated_position(time),
			self.extrapolated_velocity(time),
			time,
		)
	}
	pub fn extrapolated_position_ratio_dt<Num,Den,N1,D1,N2,N3,D2,N4,T1>(&self,dt:integer::Ratio<Num,Den>)->Planar64Vec3
		where
			// Why?
			// All of this can be removed with const generics because the type can be specified as
			// Ratio<Fixed<N,NF>,Fixed<D,DF>>
			// which is known to implement all the necessary traits
			Num:Copy,
			Den:Copy+core::ops::Mul<i64,Output=D1>,
			D1:Copy,
			Num:core::ops::Mul<Planar64,Output=N1>,
			Planar64:core::ops::Mul<D1,Output=N2>,
			N1:core::ops::Add<N2,Output=N3>,
			Num:core::ops::Mul<N3,Output=N4>,
			Den:core::ops::Mul<D1,Output=D2>,
			D2:Copy,
			Planar64:core::ops::Mul<D2,Output=N4>,
			N4:integer::Divide<D2,Output=T1>,
			T1:integer::Clamp<Planar64>,
	{
		// a*dt^2/2 + v*dt + p
		// (a*dt/2+v)*dt+p
		(self.acceleration.map(|elem|dt*elem/2)+self.velocity).map(|elem|dt.mul_ratio(elem))
		.map(|elem|elem.divide().clamp())+self.position
	}
	pub fn extrapolated_velocity_ratio_dt<Num,Den,N1,T1>(&self,dt:integer::Ratio<Num,Den>)->Planar64Vec3
		where
			Num:Copy,
			Den:Copy,
			Num:core::ops::Mul<Planar64,Output=N1>,
			Planar64:core::ops::Mul<Den,Output=N1>,
			N1:integer::Divide<Den,Output=T1>,
			T1:integer::Clamp<Planar64>,
	{
		// a*dt + v
		self.acceleration.map(|elem|(dt*elem).divide().clamp())+self.velocity
	}
	pub fn extrapolated_body_ratio_dt(&self,dt:crate::model::GigaTime)->Body<T>{
		Body::new(
			self.extrapolated_position_ratio_dt(dt),
			self.extrapolated_velocity_ratio_dt(dt),
			self.time+dt.into(),
		)
	}
	pub fn infinity_dir(&self)->Option<Planar64Vec3>{
		if self.velocity==vec3::zero(){
			if self.acceleration==vec3::zero(){
				None
			}else{
				Some(self.acceleration)
			}
		}else{
			Some(self.velocity)
		}
	}
	pub fn grow_aabb(&self,aabb:&mut aabb::Aabb,t0:Time<T>,t1:Time<T>){
		aabb.grow(self.extrapolated_position(t0));
		aabb.grow(self.extrapolated_position(t1));
		//v+a*t==0
		//goober code
		if !self.acceleration.x.is_zero(){
			let t=-self.velocity.x/self.acceleration.x;
			if t0.to_ratio().lt_ratio(t)&&t.lt_ratio(t1.to_ratio()){
				aabb.grow(self.extrapolated_position_ratio_dt(t));
			}
		}
		if !self.acceleration.y.is_zero(){
			let t=-self.velocity.y/self.acceleration.y;
			if t0.to_ratio().lt_ratio(t)&&t.lt_ratio(t1.to_ratio()){
				aabb.grow(self.extrapolated_position_ratio_dt(t));
			}
		}
		if !self.acceleration.z.is_zero(){
			let t=-self.velocity.z/self.acceleration.z;
			if t0.to_ratio().lt_ratio(t)&&t.lt_ratio(t1.to_ratio()){
				aabb.grow(self.extrapolated_position_ratio_dt(t));
			}
		}
	}

}
impl<T> std::fmt::Display for Body<T>{
	fn fmt(&self,f:&mut std::fmt::Formatter<'_>)->std::fmt::Result{
		write!(f,"p({}) v({}) t({})",self.position,self.velocity,self.time)
	}
}
impl<T> std::fmt::Display for Trajectory<T>{
	fn fmt(&self,f:&mut std::fmt::Formatter<'_>)->std::fmt::Result{
		write!(f,"p({}) v({}) a({}) t({})",self.position,self.velocity,self.acceleration,self.time)
	}
}
