O(Np) works ok
Faster implentation with less branching.