Added initial planning algorithm