use num_traits::real::Real;
use num_traits::FromPrimitive;

/// # Example
/// ```
/// use std::f32::consts::PI;
/// assert_eq!(quat::rotation_x(&quat::new_identity::<f32>()), PI);
/// ```
#[inline]
pub fn rotation_x<'out, T>(out: &[T; 4]) -> T
where
    T: Real + FromPrimitive,
{
    (T::from_usize(2).unwrap() * out[0] * out[3] + T::from_usize(2).unwrap() * out[1] * out[2])
        .atan2(T::one() - T::from_usize(2).unwrap() * (out[2] * out[2] + out[3] * out[3]))
}
/// # Example
/// ```
/// assert_eq!(quat::rotation_y(&quat::new_identity::<f32>()), 0_f32);
/// ```
#[inline]
pub fn rotation_y<'out, T>(out: &[T; 4]) -> T
where
    T: Real + FromPrimitive,
{
    let theta = T::from_usize(2).unwrap() * (out[0] * out[2] + out[3] * out[1]);

    (if theta < -T::one() {
        -T::one()
    } else if theta > T::one() {
        T::one()
    } else {
        theta
    }).asin()
}
/// # Example
/// ```
/// assert_eq!(quat::rotation_z(&quat::new_identity::<f32>()), 0_f32);
/// ```
#[inline]
pub fn rotation_z<'out, T>(out: &[T; 4]) -> T
where
    T: Real + FromPrimitive,
{
    (T::from_usize(2).unwrap() * out[0] * out[1] + T::from_usize(2).unwrap() * out[2] * out[3])
        .atan2(T::one() - T::from_usize(2).unwrap() * (out[1] * out[1] + out[2] * out[2]))
}
/// # Example
/// ```
/// use std::f32::consts::FRAC_2_PI;
/// let mut q = quat::new_identity::<f32>();
/// quat::rotate_x(&mut q, &quat::new_identity::<f32>(), &FRAC_2_PI);
/// assert_eq!(q, [0.3129618_f32, 0_f32, 0_f32, 0.94976574_f32]);
/// ```
#[inline]
pub fn rotate_x<'out, T>(out: &'out mut [T; 4], a: &[T; 4], angle: &T) -> &'out mut [T; 4]
where
    T: Real + FromPrimitive,
{
    let ax = a[0];
    let ay = a[1];
    let az = a[2];
    let aw = a[3];
    let half_angle = *angle / T::from_usize(2).unwrap();
    let bx = half_angle.sin();
    let bw = half_angle.cos();

    out[0] = ax * bw + aw * bx;
    out[1] = ay * bw + az * bx;
    out[2] = az * bw - ay * bx;
    out[3] = aw * bw - ax * bx;
    out
}
#[inline]
pub fn rotate_x_mut<'out, T>(out: &'out mut [T; 4], angle: &T) -> &'out mut [T; 4]
where
    T: Real + FromPrimitive,
{
    let tmp = out.clone();
    rotate_x(out, &tmp, angle)
}
/// # Example
/// ```
/// use std::f32::consts::FRAC_2_PI;
/// let mut q = quat::new_identity::<f32>();
/// quat::rotate_y(&mut q, &quat::new_identity::<f32>(), &FRAC_2_PI);
/// assert_eq!(q, [0_f32, 0.3129618_f32, 0_f32, 0.94976574_f32]);
/// ```
#[inline]
pub fn rotate_y<'out, T>(out: &'out mut [T; 4], a: &[T; 4], angle: &T) -> &'out mut [T; 4]
where
    T: Real + FromPrimitive,
{
    let ax = a[0];
    let ay = a[1];
    let az = a[2];
    let aw = a[3];
    let half_angle = *angle / T::from_usize(2).unwrap();
    let by = half_angle.sin();
    let bw = half_angle.cos();

    out[0] = ax * bw - az * by;
    out[1] = ay * bw + aw * by;
    out[2] = az * bw + ax * by;
    out[3] = aw * bw - ay * by;
    out
}
#[inline]
pub fn rotate_y_mut<'out, T>(out: &'out mut [T; 4], angle: &T) -> &'out mut [T; 4]
where
    T: Real + FromPrimitive,
{
    let tmp = out.clone();
    rotate_y(out, &tmp, angle)
}
/// # Example
/// ```
/// use std::f32::consts::FRAC_2_PI;
/// let mut q = quat::new_identity::<f32>();
/// quat::rotate_z(&mut q, &quat::new_identity::<f32>(), &FRAC_2_PI);
/// assert_eq!(q, [0_f32, 0_f32, 0.3129618_f32, 0.94976574_f32]);
/// ```
#[inline]
pub fn rotate_z<'out, T>(out: &'out mut [T; 4], a: &[T; 4], angle: &T) -> &'out mut [T; 4]
where
    T: Real + FromPrimitive,
{
    let ax = a[0];
    let ay = a[1];
    let az = a[2];
    let aw = a[3];
    let half_angle = *angle / T::from_usize(2).unwrap();
    let bz = half_angle.sin();
    let bw = half_angle.cos();

    out[0] = ax * bw + ay * bz;
    out[1] = ay * bw - ax * bz;
    out[2] = az * bw + aw * bz;
    out[3] = aw * bw - az * bz;
    out
}
#[inline]
pub fn rotate_z_mut<'out, T>(out: &'out mut [T; 4], angle: &T) -> &'out mut [T; 4]
where
    T: Real + FromPrimitive,
{
    let tmp = out.clone();
    rotate_z(out, &tmp, angle)
}
/// # Example
/// ```
/// use std::f32::consts::FRAC_2_PI;
/// let mut q = quat::new_identity::<f32>();
/// quat::rotate(&mut q, &quat::new_identity::<f32>(), &[FRAC_2_PI, FRAC_2_PI, FRAC_2_PI]);
/// assert_eq!(q, [0.18928385_f32, 0.3753336_f32, 0.3753336_f32, 0.82608783_f32]);
/// ```
#[inline]
pub fn rotate<'a, T>(out: &'a mut [T; 4], a: &[T; 4], v: &[T; 3]) -> &'a mut [T; 4]
where
    T: Real + FromPrimitive,
{
    let mut tmp_a = a.clone();
    let mut tmp_b = [T::zero(), T::zero(), T::zero(), T::one()];
    rotate_z(&mut tmp_a, &a, &v[2]);
    rotate_x(&mut tmp_b, &tmp_a, &v[0]);
    rotate_y(out, &tmp_b, &v[1]);
    out
}
#[inline]
pub fn rotate_mut<'out, T>(out: &'out mut [T; 4], v: &[T; 3]) -> &'out mut [T; 4]
where
    T: Real + FromPrimitive,
{
    let tmp = out.clone();
    rotate(out, &tmp, v)
}
/// # Example
/// ```
/// let mut q = quat::new_identity::<f32>();
/// quat::look_rotation(&mut q, &[0_f32, 1_f32, 0_f32], &[0_f32, 0_f32, 1_f32]);
/// assert_eq!(q, [-0.25_f32, 0_f32, 0_f32, 2_f32]);
/// ```
#[inline]
pub fn look_rotation<'out, T>(
    out: &'out mut [T; 4],
    forward: &[T; 3],
    up: &[T; 3],
) -> &'out mut [T; 4]
where
    T: Real + FromPrimitive,
{
    let fx = forward[0];
    let fy = forward[1];
    let fz = forward[2];
    let ux = up[0];
    let uy = up[1];
    let uz = up[2];

    let ax = uy * fz - uz * fy;
    let ay = uz * fx - ux * fz;
    let az = ux * fy - uy * fx;

    let d = (T::one() + ux * fx + uy * fy + uz * fz) * T::from_usize(2).unwrap();
    let dsq = d * d;
    let s = if dsq != T::zero() {
        T::one() / dsq
    } else {
        dsq
    };

    out[0] = ax * s;
    out[1] = ay * s;
    out[2] = az * s;
    out[3] = dsq / T::from_usize(2).unwrap();
    out
}