add diagnosis and rewrite path finding algorithm