Cleaned up cartesian planning code and added lots of comments.