Function grid_index 

fn grid_index(length: usize, i: usize, y: usize, x: usize, q: usize) -> usize