More work on state machine and path finding.  Contains LOTs of debug code. Maybe works.