<?xml version="1.0" encoding="UTF-8"?>
<Stack 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 ../Xml/Tethys.xsd
                           Tethys/Control ../Xml/Control.xsd
                           Tethys/Guidance ../Xml/Guidance.xsd
                           Tethys/Units ../Xml/Units.xsd
                           Tethys/Universal ../Xml/Universal.xsd"
       Id="TestMission">

<!-- some high priority behaviors -->

    <Guidance:AltitudeEnvelope RunIn="Parallel">
        <Set><Guidance:AltitudeEnvelope.minAltitude/><Units:meter/><Value>20</Value></Set>
    </Guidance:AltitudeEnvelope>

    <Guidance:OffshoreEnvelope RunIn="Parallel">
        <Set><Guidance:OffshoreEnvelope.minOffshore/><Units:kilometer/><Value>1</Value></Set>
        <Set><Guidance:OffshoreEnvelope.maxOffshore/><Units:kilometer/><Value>50</Value></Set>
    </Guidance:OffshoreEnvelope>

    <Guidance:DepthEnvelope RunIn="Parallel">
        <Set><Guidance:DepthEnvelope.minDepth/><Units:meter/><Value>0</Value></Set>
        <Set><Guidance:DepthEnvelope.maxDepth/><Units:meter/><Value>300</Value></Set>
    </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 -->

    <Stack Id="NeedSatFix">

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

        <Guidance:GoToSurface RunIn="Progression"/>

        <Stack Id="SurfOps">

<!--        Surface operations are always preemptive -->

            <Preemptive><True/></Preemptive>

<!--        Get the GPS fix at surfacing time -->

            <Guidance:ReadSensor RunIn="Sequence">
                <Timeout Duration="P2M" />
                <SetUri><Guidance:ReadSensor.uri/><Universal:time_fix/></SetUri>
            </Guidance:ReadSensor>

<!--        Transmit the GPS fix, do other conversations -->

            <Guidance:ReadSensor RunIn="Sequence">
                <Timeout Duration="P2M">
                    <When><Universal:platform_conversation/>
                        <Eq/><False/>
                    </When>
                    <Touch Id="TouchComms"><Universal:platform_communications/></Touch>
                </Timeout>
                <SetUri><Guidance:ReadSensor.uri/><Universal:platform_communications/></SetUri>
            </Guidance:ReadSensor>

<!--        Get another GPS fix before diving -->

            <Guidance:ReadSensor RunIn="Sequence">
                <Timeout Duration="P2M">
                    <Touch Id="TouchTime"><Universal:time_fix/></Touch>
                </Timeout>
                <SetUri><Guidance:ReadSensor.uri/><Universal:time_fix/></SetUri>
            </Guidance:ReadSensor>

        </Stack>

    </Stack>

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

    <Stack Id="RepeatOps" Repeat="2" >

        <Guidance:Wait RunIn="Sequence">
            <Until>
                <Elapsed><Universal:time_fix/></Elapsed>
                <Lt/><Units:day/><Value>30</Value>
            </Until>
        </Guidance:Wait>

        <Guidance:ReadSensor RunIn="Parallel">
            <SetUri><Guidance:ReadSensor.uri/><Universal:sea_water_salinity/></SetUri>
        </Guidance:ReadSensor>

        <Guidance:ReadSensor RunIn="Parallel">
            <SetUri><Guidance:ReadSensor.uri/><Universal:sea_water_temperature/></SetUri>
        </Guidance:ReadSensor>

        <Guidance:DepthEnvelope RunIn="Parallel">
            <Set><Guidance:DepthEnvelope.minDepth/><Units:meter/><Value>5</Value></Set>
        </Guidance:DepthEnvelope>

<!--    do some operations with YoYo active -->

        <Stack Id="YoYoOps">

            <Guidance:SetSpeed RunIn="Parallel">
                <Set><Guidance:SetSpeed.speed/><Units:meter_per_second/><Value>1</Value></Set>
            </Guidance:SetSpeed>

            <Guidance:YoYo RunIn="Parallel"/>

<!--        This is an example of a simple in-line custom behavior defined 
            within the mission script. -->

            <CustomCommand Name="HeadSouthWest" RunIn="Sequence">
                <Initialize>
                    <Input Name="latitude"><Universal:latitude/><Units:degree/><DefaultValue>NaN</DefaultValue></Input>
                    <Input Name="longitude"><Universal:longitude/><Units:degree/><DefaultValue>NaN</DefaultValue></Input>
                    <Test InputValue="36.0 -122"/>
                    <Script Language="Lua">
                        finalLatitude = latitude - 0.005;
                        finalLongitude = longitude - 0.005;
                    </Script>
                </Initialize>
                <Run>
                    <Output><Control:HorizontalControl.horizontalMode/><Units:enum/></Output>
                    <Output><Control:HorizontalControl.bearingCmd/><Units:degree/></Output>
                    <Output><Control:HorizontalControl.latitudeCmd/><Units:degree/></Output>
                    <Output><Control:HorizontalControl.longitudeCmd/><Units:degree/></Output>
                    <Test OutputValue="1.0 -135.0 35.995 -122.005"/>
                    <Script Language="Lua">
                        return 1.0, -135.0, finalLatitude, finalLongitude;
                    </Script>
                </Run>
                <IsSatisfied>
                    <Input Name="latitude"><Universal:latitude/><Units:degree/><DefaultValue>NaN</DefaultValue></Input>
                    <Input Name="longitude"><Universal:longitude/><Units:degree/><DefaultValue>NaN</DefaultValue></Input>
                    <Test InputValue="1.0 36.01 -122.01" OutputValue="true"/>
                    <Test InputValue="1.0 36.001 -122.001" OutputValue="false"/>
                    <Script Language="Lua"><![CDATA[
                        return latitude <= finalLatitude or longitude <= finalLongitude;
                        ]]></Script>
                </IsSatisfied>
            </CustomCommand>

<!--        head west, using temperature-controlled yo-yo -->

            <Stack Id="TempEnvelope">

                <Guidance:DepthEnvelope RunIn="Parallel">
                    <Set><Guidance:DepthEnvelope.maxDepth/><Units:celsius/><Value>11.5</Value></Set>
                    <Set><Guidance:DepthEnvelope.minDepth/><Units:celsius/><Value>13.0</Value></Set>
                    <SetUri><Guidance:DepthEnvelope.depthSurrogate/><Universal:sea_water_temperature/></SetUri>
                </Guidance:DepthEnvelope>

                <Guidance:Waypoint Id="WaypointSW" RunIn="Sequence">
                    <Set><Guidance:Waypoint.latitude/><Units:degree/><Value>36.805</Value></Set>
                    <Set><Guidance:Waypoint.longitude/><Units:degree/><Value>-121.825</Value></Set>
                </Guidance:Waypoint>

            </Stack>

<!--        head north, with a timeout  -->

            <Guidance:Waypoint Id="WaypointTimeout" RunIn="Sequence">
                <Timeout Duration="P25M30.5S">
                    <Syslog Id="SyslogWaypointNWTimeout">When we timed out WaypointSE, temperature was 
                        <Universal:sea_water_temperature/><Units:fahrenheit/>
                        Isn't that low?</Syslog>
                    <Touch Id="TouchComms"><Universal:platform_communications/></Touch>
                </Timeout>
                <Set><Guidance:Waypoint.latitude/><Units:degree/><Value>36.815</Value></Set>
                <Set><Guidance:Waypoint.longitude/><Units:degree/><Value>-121.825</Value></Set>
            </Guidance:Waypoint>

<!--        head east, using salinity-controlled yo-yo -->

            <Stack Id="SalEnvelope">

                <Guidance:DepthEnvelope RunIn="Parallel">
                    <Set><Guidance:DepthEnvelope.maxDepth/><Units:part_per_thousand/><Value>33.65</Value></Set>
                    <Set><Guidance:DepthEnvelope.minDepth/><Units:part_per_thousand/><Value>33.55</Value></Set>
                    <SetUri><Guidance:DepthEnvelope.depthSurrogate/><Universal:sea_water_salinity/></SetUri>
                </Guidance:DepthEnvelope>

                <Guidance:Waypoint Id="WaypointNE" RunIn="Sequence">
                    <Set><Guidance:Waypoint.latitude/><Units:degree/><Value>36.815</Value></Set>
                    <Set><Guidance:Waypoint.longitude/><Units:degree/><Value>-121.815</Value></Set>
                </Guidance:Waypoint>

            </Stack>

        </Stack>

<!--    head south, following a set temperature -->

        <Stack Id="PitchServoOnTemp">

            <Guidance:SetSpeed RunIn="Parallel">
                <Set><Guidance:SetSpeed.speed/><Units:meter_per_second/><Value>1</Value></Set>
            </Guidance:SetSpeed>

            <Guidance:PitchServo RunIn="Parallel">
                <SetUri><Guidance:PitchServo.uri/><Universal:sea_water_temperature/></SetUri>
                <Set><Guidance:PitchServo.holdValue/><Units:celsius/><Value>12.0</Value></Set>
                <Set><Guidance:PitchServo.proportionalGain/><Units:none/><Value>-0.06</Value></Set>
                <Set><Guidance:PitchServo.integralGain/><Units:none/><Value>-0.001</Value></Set>
                <Set><Guidance:PitchServo.derivativeGain/><Units:none/><Value>0.12</Value></Set>
                <Set><Guidance:PitchServo.changePerMeter/><Units:reciprocal_meter/><Value>0.1</Value></Set>
            </Guidance:PitchServo>

            <Guidance:Waypoint Id="WaypointSE" RunIn="Sequence">
                <Set><Guidance:Waypoint.latitude/><Units:degree/><Value>36.805</Value></Set>
                <Set><Guidance:Waypoint.longitude/><Units:degree/><Value>-121.815</Value></Set>
            </Guidance:Waypoint>

        </Stack>

    </Stack>

    <Call refId="NeedSatFix"/>

</Stack>
