//! Functions used to make metrics.

use crate::mathx::FloatExt2;

pub trait Limiter {
	fn value(&self, x: f32) -> f32;
	fn derivative(&self, x: f32) -> f32;
}

pub struct SmoothstepLimiter {
	pub min: f32,
	pub max: f32,
}

impl Limiter for SmoothstepLimiter {
	fn value(&self, x: f32) -> f32 {
		let y = (self.min, self.max).inverse_lerp(x.abs()).clamp(0.0, 1.0);
		3.0 * y * y - 2.0 * y * y * y
	}
	fn derivative(&self, x: f32) -> f32 {
		if x.abs() > self.min && x.abs() < self.max {
			let t = (self.min, self.max).inverse_lerp(x.abs());
			6.0 * x.signum() * t * (1.0 - t) / (self.max - self.min)
		} else {
			0.0
		}
	}
}

pub struct SmootherstepLimiter {
	pub min: f32,
	pub max: f32,
}

impl Limiter for SmootherstepLimiter {
	fn value(&self, x: f32) -> f32 {
		let y = (self.min, self.max).inverse_lerp(x.abs()).clamp(0.0, 1.0);
		6.0 * y.powi(5) - 15.0 * y.powi(4) + 10.0 * y.powi(3)
	}
	fn derivative(&self, x: f32) -> f32 {
		if x.abs() > self.min && x.abs() < self.max {
			let t = (self.min, self.max).inverse_lerp(x.abs());
			30.0 * (t * (1.0 - t)).powi(2) * x.signum() / (self.max - self.min)
		} else {
			0.0
		}
	}
}

pub struct QuadraticAccelerator {
	pub internal: f32,
	pub external: f32,
}

/// Продолжает функцию f с [-lim, lim] линейно в предположении f(±lim) = ±val, f'(±lim) = 1.
fn extend_linear(t: f32, f: impl FnOnce(f32) -> f32, lim: f32, val: f32) -> f32 {
	if t.abs() <= lim {
		f(t)
	} else {
		t + t.signum() * (val - lim)
	}
}

/// Продолжает функцию f с [-lim, lim] константой в предположении f(±lim) = val, f'(±lim) = 0.
fn extend_const(t: f32, f: impl FnOnce(f32) -> f32, lim: f32, val: f32) -> f32 {
	if t.abs() <= lim {
		f(t)
	} else {
		val
	}
}

impl QuadraticAccelerator {
	fn a(&self) -> f32 {
		-(self.external - self.internal) / self.internal.powi(2)
	}
	fn b(&self) -> f32 {
		2.0 * self.external / self.internal - 1.0
	}
	fn root(&self, x: f32) -> f32 {
		(self.b().powi(2) + 4.0 * self.a() * x.abs()).sqrt()
	}

	pub fn x(&self, u: f32) -> f32 {
		extend_linear(
			u,
			|u| (self.a() * u.abs() + self.b()) * u,
			self.internal,
			self.external,
		)
	}
	pub fn u(&self, x: f32) -> f32 {
		extend_linear(
			x,
			|x| 0.5 * x.signum() * (-self.b() + self.root(x)) / self.a(),
			self.external,
			self.internal,
		)
	}
	pub fn dx(&self, u: f32) -> f32 {
		extend_const(
			u,
			|u| 2.0 * self.a() * u.abs() + self.b(),
			self.internal,
			1.0,
		)
	}
	pub fn du(&self, x: f32) -> f32 {
		extend_const(x, |x| 1.0 / self.root(x), self.external, 1.0)
	}
	pub fn d2u(&self, x: f32) -> f32 {
		extend_const(
			x,
			|x| -2.0 * x.signum() * self.a() * self.root(x).powi(-3),
			self.external,
			0.0,
		)
	}
}

#[cfg(test)]
mod test {
	use approx::{abs_diff_eq, assert_abs_diff_eq};

	use super::*;

	fn test_limiter(testee: impl Limiter, min: f32, max: f32, δ: f32) {
		let ε = 1.0e-4f32;
		let margin = 1.0 / 16.0;
		let mul = 1.0 + margin;
		for x in itertools_num::linspace(0., min, 10) {
			assert_abs_diff_eq!(testee.value(x), 0., epsilon = ε);
			assert_abs_diff_eq!(testee.value(-x), 0., epsilon = ε);
		}
		for x in itertools_num::linspace(max, mul * max, 10) {
			assert_abs_diff_eq!(testee.value(x), 1., epsilon = ε);
			assert_abs_diff_eq!(testee.value(-x), 1., epsilon = ε);
		}
		for x in itertools_num::linspace(-mul * max, mul * max, 100) {
			let df_num = (testee.value(x + δ) - testee.value(x - δ)) / (2. * δ);
			let df_expl = testee.derivative(x);
			assert!(
				abs_diff_eq!(df_expl, df_num, epsilon = ε),
				"At x={x}, df/dx:\nnumerical: {df_num}\nexplicit:  {df_expl}\n"
			);
		}
	}

	#[test]
	fn test_smoothstep_limiter() {
		test_limiter(
			SmoothstepLimiter {
				min: 20.0,
				max: 30.0,
			},
			20.0,
			30.0,
			1.0 / 32.0,
		);
	}

	#[test]
	fn test_smootherstep_limiter() {
		test_limiter(
			SmootherstepLimiter {
				min: 20.0,
				max: 30.0,
			},
			20.0,
			30.0,
			1.0 / 32.0,
		);
	}

	#[test]
	fn test_quadratic_accelerator() {
		let testee = super::QuadraticAccelerator {
			internal: 100.0,
			external: 150.0,
		};
		let ε = 1.0e-4f32;
		let δ = 1.0 / 8.0; // Mathematically, you want this to be small. Computationally, you don’t.
		let margin = 1.0 / 16.0;
		let mul = 1.0 + margin;
		assert_abs_diff_eq!(testee.u(testee.external), testee.internal, epsilon = ε);
		assert_abs_diff_eq!(testee.u(-testee.external), -testee.internal, epsilon = ε);
		assert_abs_diff_eq!(testee.du(testee.external), 1., epsilon = ε);
		assert_abs_diff_eq!(testee.du(-testee.external), 1., epsilon = ε);
		for x in itertools_num::linspace(-mul * testee.external, mul * testee.external, 100) {
			let ux = testee.u(x);
			let xux = testee.x(ux);
			assert!(
				abs_diff_eq!(x, xux, epsilon = ε),
				"At x={x}:\nu(x):    {ux}\nx(u(x)): {xux}\n"
			);

			let du_num = (testee.u(x + δ) - testee.u(x - δ)) / (2. * δ);
			let du_expl = testee.du(x);
			assert!(
				abs_diff_eq!(du_expl, du_num, epsilon = ε),
				"At x={x}, du/dx:\nnumerical: {du_num}\nexplicit:  {du_expl}\n"
			);

			let dudx = du_expl * testee.dx(ux);
			assert!(
				abs_diff_eq!(dudx, 1.0, epsilon = ε),
				"At x={x}:\ndu/dx * dx/du: {dudx}\n"
			);

			let d2u_num = (testee.du(x + δ) - testee.du(x - δ)) / (2. * δ);
			let d2u_expl = testee.d2u(x);
			assert!(
				abs_diff_eq!(d2u_expl, d2u_num, epsilon = ε),
				"At x={x}, d^2u/dx^2:\nnumerical: {d2u_num}\nexplicit:  {d2u_expl}\n"
			);
		}
		for u in itertools_num::linspace(-mul * testee.internal, mul * testee.internal, 100) {
			let xu = testee.x(u);
			let uxu = testee.u(xu);
			assert!(
				abs_diff_eq!(u, uxu, epsilon = ε),
				"At u={u}:\nx(u):    {xu}\nu(x(u)): {uxu}\n"
			);

			let dx_num = (testee.x(u + δ) - testee.x(u - δ)) / (2. * δ);
			let dx_expl = testee.dx(u);
			assert!(
				abs_diff_eq!(dx_expl, dx_num, epsilon = ε),
				"At u={u}, dx/du:\nnumerical: {dx_num}\nexplicit:  {dx_expl}\n"
			);

			let dudx = testee.du(xu) * dx_expl;
			assert!(
				abs_diff_eq!(dudx, 1.0, epsilon = ε),
				"At u={u}:\ndu/dx * dx/du: {dudx}\n"
			);
		}
	}
}