use glam::{FloatExt, Mat2, Mat3};

mod bounds {
	pub trait Pair<T> {}

	impl<T> Pair<T> for (T, T) {}
}

pub trait FloatExt2<T>: bounds::Pair<T> {
	fn lerp(self, t: T) -> T;
	fn inverse_lerp(self, y: T) -> T;
}

impl<F: FloatExt> FloatExt2<F> for (F, F) {
	fn lerp(self, t: F) -> F {
		F::lerp(self.0, self.1, t)
	}
	fn inverse_lerp(self, y: F) -> F {
		F::inverse_lerp(self.0, self.1, y)
	}
}

pub trait MatExt {
	fn orthonormalize(&self) -> Self;
}

impl MatExt for Mat2 {
	fn orthonormalize(&self) -> Self {
		let fx = self.x_axis.normalize();
		let fy = (self.y_axis - self.y_axis.project_onto_normalized(fx)).normalize();
		Self::from_cols(fx, fy)
	}
}

impl MatExt for Mat3 {
	fn orthonormalize(&self) -> Self {
		let fx = self.x_axis.normalize();
		let fy = (self.y_axis - self.y_axis.project_onto_normalized(fx)).normalize();
		let fz = (self.z_axis
			- self.z_axis.project_onto_normalized(fx)
			- self.z_axis.project_onto_normalized(fy))
		.normalize();
		Self::from_cols(fx, fy, fz)
	}
}

#[cfg(test)]
mod tests {
	use super::*;

	use approx::assert_abs_diff_eq;
	use glam::{mat2, mat3, vec2, vec3};

	#[test]
	fn test_lerp() {
		assert_eq!((3., 7.).lerp(-0.5), 1.);
		assert_eq!((3., 7.).lerp(0.0), 3.);
		assert_eq!((3., 7.).lerp(0.5), 5.);
		assert_eq!((3., 7.).lerp(1.0), 7.);
		assert_eq!((3., 7.).lerp(1.5), 9.);
		assert_eq!((3., 7.).inverse_lerp(1.), -0.5);
		assert_eq!((3., 7.).inverse_lerp(3.), 0.0);
		assert_eq!((3., 7.).inverse_lerp(5.), 0.5);
		assert_eq!((3., 7.).inverse_lerp(7.), 1.0);
		assert_eq!((3., 7.).inverse_lerp(9.), 1.5);
	}

	#[test]
	fn test_orthonormalize_2d() {
		assert_abs_diff_eq!(
			mat2(vec2(1., 0.), vec2(0., 1.)).orthonormalize(),
			mat2(vec2(1., 0.), vec2(0., 1.)),
		);
		assert_abs_diff_eq!(
			mat2(vec2(2., 0.), vec2(3., 5.)).orthonormalize(),
			mat2(vec2(1., 0.), vec2(0., 1.)),
		);
		assert_abs_diff_eq!(
			mat2(vec2(0., -3.), vec2(5., 1.)).orthonormalize(),
			mat2(vec2(0., -1.), vec2(1., 0.)),
		);
		assert_abs_diff_eq!(
			mat2(vec2(3., 4.), vec2(5., 1.)).orthonormalize(),
			mat2(vec2(0.6, 0.8), vec2(0.8, -0.6)),
		);
		assert_abs_diff_eq!(
			mat2(vec2(3., 4.), vec2(1., 5.)).orthonormalize(),
			mat2(vec2(0.6, 0.8), vec2(-0.8, 0.6)),
		);
	}

	#[test]
	fn test_orthonormalize_3d() {
		assert_abs_diff_eq!(
			mat3(vec3(1., 0., 0.), vec3(0., 1., 0.), vec3(0., 0., 1.)).orthonormalize(),
			mat3(vec3(1., 0., 0.), vec3(0., 1., 0.), vec3(0., 0., 1.)),
		);
		assert_abs_diff_eq!(
			mat3(vec3(2., 0., 0.), vec3(3., 4., 0.), vec3(5., 6., 7.)).orthonormalize(),
			mat3(vec3(1., 0., 0.), vec3(0., 1., 0.), vec3(0., 0., 1.)),
		);
		assert_abs_diff_eq!(
			mat3(vec3(0., 5., 0.), vec3(0., 7., 6.), vec3(9., 2., 3.)).orthonormalize(),
			mat3(vec3(0., 1., 0.), vec3(0., 0., 1.), vec3(1., 0., 0.)),
		);
	}
}