#[inline(always)]
fn veclen(v: &[f32; 3]) -> f32 {
    (v[0] * v[0] + v[1] * v[1] + v[2] * v[2]).sqrt()
}

#[inline(always)]
fn dist2(p1: &[f32; 3], p2: &[f32; 3]) -> f32 {
    let dx = p1[0] - p2[0];
    let dy = p1[1] - p2[1];
    let dz = p1[2] - p2[2];

    dx * dx + dy * dy + dz * dz
}

#[inline(always)]
fn dist(p1: &[f32; 3], p2: &[f32; 3]) -> f32 {
    dist2(p1, p2).sqrt()
}

#[inline(always)]
fn distm(p1: &[f32; 3], p2: &[f32; 3]) -> f32 {
    let dx = (p1[0] - p2[0]).abs();
    let dy = (p1[1] - p2[1]).abs();
    let dz = (p1[2] - p2[2]).abs();
    dx + dy + dz
}

/// Dot product (cosine of angle) between two 3D vectors
#[inline(always)]
pub fn ndot_vec_dist(u: &[f32; 3], v: &[f32; 3]) -> f32 {
    let z: [f32; 3] = [0.0; 3];
    let lm = dist(u, &z) * dist(v, &z);
    ((u[0] * v[0]) + (u[1] * v[1]) + (u[2] * v[2])) / lm
}

/// Dot product (cosine of angle) between two 3D vectors
#[inline(always)]
pub fn ndot_vec_len(u: &[f32; 3], v: &[f32; 3]) -> f32 {
    let lm = veclen(u) * veclen(v);
    ((u[0] * v[0]) + (u[1] * v[1]) + (u[2] * v[2])) / lm
}

#[inline(always)]
pub fn ndot_iter(u: &[f32; 3], v: &[f32; 3]) -> f32 {
    let l_1: f32 = u.iter().map(|e| e * e).sum();
    let l_2: f32 = v.iter().map(|e| e * e).sum();
    let lm = (l_1 * l_2).sqrt();
    let mut ret = 0.0;
    for (a, b) in u.iter().zip(v.iter()) {
        ret += a * b;
    }
    ret / lm
}

#[cfg(test)]
mod dot_impl_tests {
    #[test]
    fn test_dot_impls() {
        use super::*;
        let v1 = [1.0, 2.0, 3.0];
        let v2 = [4.0, 5.0, 6.0];
        let d1 = ndot_vec_dist(&v1, &v2);
        let d2 = ndot_vec_len(&v1, &v2);
        let d3 = ndot_iter(&v1, &v2);
        assert!((d1 - d2) < 0.01);
        assert!((d2 - d3) < 0.01);
        assert!((d3 - d1) < 0.01);
    }
}