<?xml version="1.0" encoding="UTF-8"?>
<Aggregate xmlns="Tethys"
    xmlns:Estimation="Tethys/Estimation"
    xmlns:Guidance="Tethys/Guidance"
    xmlns:Units="Tethys/Units"
    xmlns:Dock="Tethys/Dock"
    xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
    xsi:schemaLocation="Tethys http://okeanids.mbari.org/tethys/Xml/Tethys.xsd
                        Tethys/Dock http://okeanids.mbari.org/tethys/Xml/Dock.xsd
                        Tethys/Estimation http://okeanids.mbari.org/tethys/Xml/Estimation.xsd
                        Tethys/Guidance http://okeanids.mbari.org/tethys/Xml/Guidance.xsd
                        Tethys/Units http://okeanids.mbari.org/tethys/Xml/Units.xsd"
    Id="SetNavAcoustic">

    <Description>
        Reset nav to dock location if the vehicle is within dock range.
    </Description>

    <DefineArg Name="DockLat"><Description>
        Latitude of docking station waypoint (defaults to
        Config/workSite.beaconLat).
    </Description><Dock:Dock.lat/></DefineArg>

    <DefineArg Name="DockLon"><Description>
        Longitude of docking station waypoint (defaults to
        Config/workSite.beaconLon).
    </Description><Dock:Dock.lon/></DefineArg>

    <DefineArg Name="DockRange"><Description>
        Range from dock in which the vehicle resets it's position to dock
        lat/lon .
    </Description><Dock:SetNav.dockRange/></DefineArg>

    <DefineArg Name="RangeTimeout"><Description>
        Time duration after which ranges are considered stagnant.
    </Description><Dock:SetNav.rangeTimeout/></DefineArg>

    <When>
        <Called/>
        <Or><Elapsed><Guidance:SetNav.time_fix/></Elapsed><Gt><Arg Name="RangeTimeout"/></Gt><And><Estimation:TrackAcousticContact.range_to_contact/><Le><Arg Name="DockRange"/></Le></And><And><Elapsed><Estimation:TrackAcousticContact.range_to_contact/></Elapsed><Lt><Arg Name="RangeTimeout"/></Lt></And></Or>
    </When>

    <Aggregate Id="UpdateNav">

        <Sequence/>

        <Timeout Duration="P1M"/>

        <Syslog Severity="Info">Reseting nav to dock location at range<Estimation:TrackAcousticContact.range_to_contact/><Units:meter/></Syslog>

        <Guidance:SetNav>
            <Sequence/>
            <Setting><Guidance:SetNav.latitude/><Arg Name="DockLat"/></Setting>
            <Setting><Guidance:SetNav.longitude/><Arg Name="DockLon"/></Setting>
        </Guidance:SetNav>

    </Aggregate>

</Aggregate>
