<?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://aosn.mbari.org/tethys/Xml/Tethys.xsd
                           Tethys/Control http://aosn.mbari.org/tethys/Xml/Control.xsd
                           Tethys/Guidance http://aosn.mbari.org/tethys/Xml/Guidance.xsd
                           Tethys/Units http://aosn.mbari.org/tethys/Xml/Units.xsd
                           Tethys/Universal http://aosn.mbari.org/tethys/Xml/Universal.xsd"
       Id="TestMission">

<!-- some high priority behaviors -->

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

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

    <Guidance:DepthEnvelope RunIn="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/Surface.xml" Id="SurfaceComms" />

    <Aggregate 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>

        <Call RefId="SurfaceComms"/>

    </Aggregate>

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

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

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

        <ReadData>
            <Universal:sea_water_salinity/>
            <Universal:sea_water_temperature/>
        </ReadData>

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

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

        <Aggregate Id="YoYoOps">

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

            <Guidance:YoYo RunIn="Parallel"/>

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

            <CustomBehavior 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>
            </CustomBehavior>

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

            <Aggregate Id="TempEnvelope">

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

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

            </Aggregate>

<!--        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>
                <Setting><Guidance:Waypoint.latitude/><Units:degree/><Value>36.815</Value></Setting>
                <Setting><Guidance:Waypoint.longitude/><Units:degree/><Value>-121.825</Value></Setting>
            </Guidance:Waypoint>

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

            <Aggregate Id="SalEnvelope">

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

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

            </Aggregate>

        </Aggregate>

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

        <Aggregate Id="PitchServoOnTemp">

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

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

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

        </Aggregate>

    </Aggregate>

    <Call RefId="NeedSatFix"/>

</Mission>
