/// Returns a vector's magnitude
fn magnitude<const DIM: usize>(vec: &[f32; DIM]) -> f32 {
    let mut a = 0.;
    for x in vec {
        a += x * x;
    }
    a
}

/// Normalizes a vector
pub fn normalize<const DIM: usize>(mut vec: [f32; DIM]) -> [f32; DIM] {
    let mag = magnitude(&vec);

    for x in &mut vec {
        *x /= mag;
    }

    vec
}

/// Advances position
/// Deals with bounces on the box
/// Returns an array of bools, one per dimension, where true indicates a collision in that dim
pub fn advance<const DIM: usize>(
    speed: f32,
    position: &mut [f32; DIM],
    velocity: &mut [f32; DIM],
) -> [bool; DIM] {
    let mut res = [false; DIM];

    for (i, (x, v)) in position.iter_mut().zip(velocity.iter_mut()).enumerate() {
        // Advance position in this axis
        *x += *v * speed;

        // If we're out of bounds in this coordinate, change velocity
        if x.abs() > 1.0 {
            *v = -*v;

            // Mark as collision
            res[i] = true;
        }
    }

    res
}