working on path planning for auv