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

    <DefineBehavior Name="HeadSouthWest" Language="Lua">

        <Initialize>
            <Input Name="latitude"><Universal:latitude/><Units:degree/><DefaultValue>NaN</DefaultValue></Input>
            <Input Name="longitude"><Universal:longitude/><Units:degree/><DefaultValue>NaN</DefaultValue></Input>
            <Script>
                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>
            <Script>
                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>
            <Script><![CDATA[
                return latitude <= finalLatitude or longitude <= finalLongitude;
                ]]></Script>
        </IsSatisfied>

    </DefineBehavior>

<!-- 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>50</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/Surface.xml" Id="SurfaceComms" />

    <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>30</Value></Gt><Or><Elapsed><Universal:platform_communications/></Elapsed><Gt><Units:minute/><Value>30</Value></Gt></Or></And>
            <Or><Elapsed><Universal:time_fix/></Elapsed><Gt><Units:minute/><Value>60</Value></Gt></Or>
            <Or><Elapsed><Universal:platform_communications/></Elapsed><Gt><Units:minute/><Value>60</Value></Gt></Or>
        </When>

        <Call RefId="SurfaceComms"/>

    </Aggregate>

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

    <Guidance:SetSpeed>
        <Parallel/>
        <Setting><Guidance:SetSpeed.period/><Units:second/><Value>5.0</Value></Setting>
    </Guidance:SetSpeed>

    <Guidance:Buoyancy>
        <Parallel/>
        <Setting><Guidance:Buoyancy.position/><Units:cubic_centimeter/><Value>275.0</Value></Setting>
    </Guidance:Buoyancy>

    <Aggregate Id="RepeatOps">

        <Sequence Repeat="2"/>

        <Guidance:Wait>
            <Sequence/>
            <Break>
                <Elapsed><Universal:time_fix/></Elapsed>
                <Lt><Units:day/><Value>30</Value></Lt>
            </Break>
        </Guidance:Wait>

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

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

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

        <Aggregate Id="YoYoOps">

            <Sequence/>

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

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

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

            <CustomBehavior Name="HeadSouthWest">
                <Sequence/>
            </CustomBehavior>

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

            <Aggregate Id="TempEnvelope">

                <Sequence/>

                <Guidance:DepthEnvelope>
                    <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">
                    <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">
                <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>
                    <Assign Id="TouchComms"><Sequence/><Universal:platform_communications/></Assign>
                </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">

                <Sequence/>

                <Guidance:DepthEnvelope>
                    <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">
                    <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">

            <Sequence/>

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

            <Guidance:PitchServo>
                <Parallel/>
                <Setting><Guidance:PitchServo.input/><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">
                <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>
