Autonomous Planning for Surface Engagement using a Multi-Agent System