Managed Surge Controller: A Docking Algorithm for a Non-Holonomic AUV (Sparus II) in the Presence of Ocean Currents for a Funnel-Shaped Docking Station