path finding works now but is not optimal