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

<!-- sampling_vehicle.xml was originally drift_50 -->
<!-- The following arguments are input arguments -->

    <DefineArg Name="MissionTimeout"><Description>
        Maximum duration of mission
    </Description><Units:hour/><Value>2</Value></DefineArg>

    <DefineArg Name="NeedCommsTime"><Description>
        Elapsed time after previous surface communications when vehicle will
        begin to ascend for additional surface communications
    </Description><Units:minute/><Value>60</Value></DefineArg>

    <DefineArg Name="Lat"><Description>
        Latitude of waypoint to start drifting. If NaN, start drifting at the current position.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lon"><Description>
        Longitude of waypoint to start drifting. If NaN, start drifting at the current position.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Depth"><Description>
        Depth held during drift mode
    </Description><Units:meter/><Value>30</Value></DefineArg>

    <DefineArg Name="ApproachDepth"><Description>
        Depth for initial approach to station.
    </Description><Units:meter/><Value>15</Value></DefineArg>

    <DefineArg Name="ApproachSpeed"><Description>
        Vehicle transit speed.
    </Description><Units:meter_per_second/><Value>1.0</Value></DefineArg>

    <DefineArg Name="DepthDeadband"><Description>
        How much vertical drift from the specified depth is allowed in drift
        mode
    </Description><Units:meter/><Value>2</Value></DefineArg>

    <DefineArg Name="MinAltitude"><Description>
        Minimum height above the sea floor for the entire mission.
    </Description><Units:meter/><Value>5</Value></DefineArg>

    <DefineArg Name="MaxDepth"><Description>
        Maximum depth for the entire mission.
    </Description><Units:meter/><Value>45</Value></DefineArg>

    <DefineArg Name="MinOffshore"><Description>
        Minimum offshore distance for the entire mission.
    </Description><Units:kilometer/><Value>2</Value></DefineArg>

<!-- Mission variables -->

    <DefineOutput Name="RelievedOfDuty"><Description>
        Mission variable (don't change). Whether the relief vehicle has showed up
        and communicated its intent to relieve this sampling vehicle.
    </Description><Units:count/><Value>0</Value></DefineOutput>

<!-- Mission timeout -->

    <Timeout Duration="MissionTimeout"/>

<!-- Power science payload -->

    <Insert Filename="Insert/Science.xml"></Insert>

<!-- Most missions should use the NeedComms aggregate. -->

    <Insert Filename="Insert/NeedComms.xml" Id="NeedComms"/>

    <Assign><Sequence/><Arg Name="NeedComms:DiveInterval"/><Arg Name="NeedCommsTime"/></Assign>

    <Assign><Sequence/><Arg Name="NeedComms:WaitForPitchUp"/><Units:minute/><Value>0</Value></Assign>

<!-- Missions should almost always start with standard safety envelopes; most missions should not expose the parameters of these envelopes. -->

    <Insert Filename="Insert/StandardEnvelopes.xml"/>

    <Assign><Sequence/><Arg Name="StandardEnvelopes:MinAltitude"/><Arg Name="MinAltitude"/></Assign>

    <Assign><Sequence/><Arg Name="StandardEnvelopes:MaxDepth"/><Arg Name="MaxDepth"/></Assign>

    <Assign><Sequence/><Arg Name="StandardEnvelopes:MinOffshore"/><Arg Name="MinOffshore"/></Assign>


    <!-- When the relief vehicle modem a GoHome message, update the corresponding mission variable. -->

    <Aggregate Id="RespondToAcousticGoHomeMsg">

        <When>
            <Elapsed><CustomUri Uri="_.relievedOfDuty"/></Elapsed>
            <Lt><Elapsed><Arg Name="RelievedOfDuty"/></Elapsed></Lt>
        </When>

        <Assign><Sequence/><Arg Name="RelievedOfDuty"/><CustomUri Uri="_.relievedOfDuty"/></Assign>

        <Syslog Severity="Important">Recieved relievedOfDuty transmission from relief vehicle.</Syslog>

    </Aggregate>


    <Call Id="StartupComms" RefId="NeedComms"/>

    <Aggregate Id="Waypoint">

        <Sequence/>

       <Break>
           <IsNaN><Arg Name="Lat"/></IsNaN>
           <Or><IsNaN><Arg Name="Lon"/></IsNaN></Or>
        </Break>

        <Guidance:Buoyancy>
            <Parallel/>
            <Setting><Guidance:Buoyancy.position/><Control:VerticalControl.buoyancyNeutral/></Setting>
        </Guidance:Buoyancy>

        <Guidance:Pitch>
            <Parallel/>
            <Setting><Guidance:Pitch.massPosition/><Control:VerticalControl.massDefault/></Setting>
            <Setting><Guidance:Pitch.depth/><Arg Name="ApproachDepth" /></Setting>
        </Guidance:Pitch>

        <Guidance:SetSpeed>
            <Parallel/>
            <Setting><Guidance:SetSpeed.speed/><Arg Name="ApproachSpeed"/></Setting>
        </Guidance:SetSpeed>

        <Guidance:Waypoint Id="WaypointW1">
            <Sequence/>
            <Setting><Guidance:Waypoint.latitude /><Arg Name="Lat" /></Setting>
            <Setting><Guidance:Waypoint.longitude /><Arg Name="Lon" /></Setting>
        </Guidance:Waypoint>

    </Aggregate>

    <Aggregate Id="HoldDepth">

        <Sequence/>

        <Break>
            <Arg Name="RelievedOfDuty"/>
            <Eq><Units:count/><Value>1</Value></Eq>
        </Break>

        <Syslog Severity="Important">Moving to target depth of<Arg Name="Depth"/><Units:meter/></Syslog>

        <Guidance:SetSpeed>
            <Parallel/>
            <Setting><Guidance:SetSpeed.speed/><Units:meter_per_second/><Value>0</Value></Setting>
        </Guidance:SetSpeed>

        <Assign><Parallel/><Control:VerticalControl.depthDeadband/><Arg Name="DepthDeadband"/></Assign>

        <Guidance:Pitch>
            <Parallel/>
            <Setting><Guidance:Pitch.depth/><Arg Name="Depth"/></Setting>
        </Guidance:Pitch>

        <Guidance:Wait>
            <Sequence/>
            <Setting><Guidance:Wait.duration/><Arg Name="MissionTimeout"/></Setting>
        </Guidance:Wait>

    </Aggregate>

    <Syslog Severity="Important">Returning to surface.</Syslog>

    <Call Id="PHONEHOME_drift" RefId="NeedComms"/>

</Mission>
