First complete draft of a CPS algorithm. (Still hackish needs polishing)