Path Plannig Algorithm

Thank you for your feedback, i will look onto this.