<?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="TestMission">

<!-- some high priority behaviors -->

    <Guidance:AltitudeEnvelope>
        <Parallel/>
        <Setting><Guidance:AltitudeEnvelope.minAltitude/><Units:meter/><Value>20</Value></Setting>
    </Guidance:AltitudeEnvelope>

    <Guidance:OffshoreEnvelope>
        <Parallel/>
        <Setting><Guidance:OffshoreEnvelope.minOffshore/><Units:kilometer/><Value>1</Value></Setting>
        <Setting><Guidance:OffshoreEnvelope.maxOffshore/><Units:kilometer/><Value>300</Value></Setting>
    </Guidance:OffshoreEnvelope>

    <Guidance:DepthEnvelope>
        <Parallel/>
        <Setting><Guidance:DepthEnvelope.minDepth/><Units:meter/><Value>0</Value></Setting>
        <Setting><Guidance:DepthEnvelope.maxDepth/><Units:meter/><Value>300</Value></Setting>
    </Guidance:DepthEnvelope>

<!-- GPS (and Iridium) Update  
     Once an hour or once every kilometer, if we're near the surface, 
     Or once every two hours or once every two km if we are deeper 
     Iridium kicks off approximately every four hours -->

    <Insert Filename="Insert/SurfaceSpiral.xml" Id="SurfaceOps" />

    <Aggregate Id="NeedSatFix">

        <When>
            <Universal:platform_pitch_angle/>
            <Gt><Units:degree/><Value>0</Value></Gt>
            <And><Elapsed><Universal:time_fix/></Elapsed><Gt><Units:minute/><Value>60</Value></Gt><Or><Elapsed><Universal:platform_communications/></Elapsed><Gt><Units:minute/><Value>60</Value></Gt></Or></And>
            <Or><Elapsed><Universal:time_fix/></Elapsed><Gt><Units:minute/><Value>120</Value></Gt></Or>
            <Or><Elapsed><Universal:platform_communications/></Elapsed><Gt><Units:minute/><Value>120</Value></Gt></Or>
        </When>

<!--    Going to the surface is preemptive only if fix is very old or uninitialized 
        This is a workaround for something that should be handled in the code... 
        i.e., avoiding executing the Waypoint command before we know where we are -->

        <Preemptive>
            <Elapsed><Universal:time_fix/></Elapsed>
            <Gt><Units:day/><Value>60</Value></Gt>
        </Preemptive>

        <Call RefId="SurfaceOps"/>

    </Aggregate>

<!-- A set of commands to repeat over and over -->

    <Aggregate Id="RepeatOps">

        <Sequence Repeat="1"/>

        <Guidance:DepthEnvelope>
            <Parallel/>
            <Setting><Guidance:DepthEnvelope.minDepth/><Units:meter/><Value>5</Value></Setting>
        </Guidance:DepthEnvelope>

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

        <Guidance:YoYo>
            <Parallel/>
        </Guidance:YoYo>

<!--    head west -->

        <Guidance:Waypoint Id="WaypointW1">
            <Sequence/>
            <Setting><Guidance:Waypoint.latitude/><Units:degree/><Value>36.737845</Value></Setting>
            <Setting><Guidance:Waypoint.longitude/><Units:degree/><Value>-121.8904227</Value></Setting>
        </Guidance:Waypoint>

<!--    head further west -->

        <Guidance:Waypoint Id="WaypointW2">
            <Sequence/>
            <Setting><Guidance:Waypoint.latitude/><Units:degree/><Value>36.737845</Value></Setting>
            <Setting><Guidance:Waypoint.longitude/><Units:degree/><Value>-130.0</Value></Setting>
        </Guidance:Waypoint>

    </Aggregate>

</Mission>
