More correctness

A small set of changes so that the code is more correct and maybe more efficient too.