use anyhow::Result;
use nalgebra::{Matrix3, Vector3};
use ordered_float::NotNan;

/// Finds the minimum of an iterator of f64s, ignoring any NaN values
#[inline]
pub fn min_f64<I>(i: I) -> Option<f64>
where
  I: Iterator<Item = f64>,
{
  i.filter_map(|i| NotNan::new(i).ok())
    .min()
    .map(|i| i.into_inner())
}

/// Finds the minimum of an iterator of f64s using the given predicate, ignoring
/// any NaN values
#[inline]
pub fn min_f64_by_key<I, F>(i: I, f: F) -> Option<f64>
where
  I: Iterator<Item = f64>,
  F: FnMut(&NotNan<f64>),
{
  i.filter_map(|i| NotNan::new(i).ok())
    .min_by_key(f)
    .map(|i| i.into_inner())
}

/// Dot-product between two 3D vectors.
#[inline]
pub fn dot(a: Vector3<f64>, b: Vector3<f64>) -> f64 {
  a.x * b.x + a.y * b.y + a.z * b.z
}

/// Cross-product between two 3D vectors.
#[inline]
pub fn cross(a: Vector3<f64>, b: Vector3<f64>) -> Vector3<f64> {
  let x = a.y * b.z - a.z * b.y;
  let y = a.z * b.x - a.x * b.z;
  let z = a.x * b.y - a.y * b.x;
  Vector3::new(x, y, z)
}

/// Calculate the rotation matrix between the 2 given vectors
///
/// Based on the method given [here][1].
///
/// [1]: https://math.stackexchange.com/a/897677
pub fn compute_rotation_matrix(
  a: Vector3<f64>,
  b: Vector3<f64>,
) -> Result<Matrix3<f64>> {
  // Special case: if a and b are in the same direction, just return the
  // identity matrix.
  if a.normalize() == b.normalize() {
    return Ok(Matrix3::identity());
  }

  let cos_t = dot(a, b);
  let sin_t = cross(a, b).norm();

  let g = Matrix3::new(cos_t, -sin_t, 0.0, sin_t, cos_t, 0.0, 0.0, 0.0, 1.0);

  // New basis vectors
  let u = a;
  let v = (b - cos_t * a).normalize();
  let w = cross(b, a);

  // Not sure if this is required to be invertible?
  let f_inverse = Matrix3::from_columns(&[u, v, w]);
  let f = match f_inverse.try_inverse() {
    Some(v) => v,
    None => {
      // So I ran into this case trying to compute the rotation matrix where one
      // of the vector endpoints was (0, 0, 0). I'm pretty sure this case makes
      // no sense in reality, which means if I ever encounter this case, I
      // probably made a mistake somewhere before. So going to just error
      // out here and screw recovering.
      //
      // println!("Failed to compute inverse matrix.");
      // println!("- Initial: a = {a}, b = {b}");
      // println!("- cos(t) = {cos_t}, sin(t) = {sin_t}");
      // println!("- Basis: u = {u}, v = {v}, w = {w}");
      bail!("Failed to compute inverse matrix of {f_inverse}\na = {a}\nb = {b}")
    }
  };

  // if (f_inverse * g * f).norm() != 1.0 {
  //   bail!("WTF {}", (f_inverse * g * f).norm());
  // }

  Ok(f_inverse * g * f)
}