Fix and improve pfold

The previous implementation of pfold was inccorect when used with
non-zero pdot and pddot. Also the implementation seemed unecessary
complicated and potentially slow.