Added optimal path code;still buggy