<?xml version="1.0" encoding="UTF-8"?>
<Mission xmlns="Tethys"
       xmlns:Control="Tethys/Control"
       xmlns:Guidance="Tethys/Guidance" 
       xmlns:Trigger="Tethys/Trigger" 
       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/Trigger http://aosn.mbari.org/tethys/Xml/Trigger.xsd
                           Tethys/Units http://aosn.mbari.org/tethys/Xml/Units.xsd
                           Tethys/Universal http://aosn.mbari.org/tethys/Xml/Universal.xsd"
       Id="turbulence">

    <Description>
        Runs from deep water to shallow water doing yo-yo's. Then heads out to
        deep water either doing yo-yo's or following the bottom. Repeats.
    </Description>

    <DefineArg Name="MassDefault"><Description>
        Static setting for mass during the mission.
    </Description><CustomUri Uri="Config/Control.massDefault"/></DefineArg>

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

    <DefineArg Name="MinAltitude"><Description>
        Minimum altitude for the entire mission.
    </Description><Units:meter/><Value>3</Value></DefineArg>

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

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

    <DefineArg Name="MaxOffshore"><Description>
        Maximum offshore distance for the entire mission.
    </Description><Units:meter/><Value>12000</Value></DefineArg>

    <DefineArg Name="YoYoMinAltitude"><Description>
        Minimum altitude while yo-yo-ing.
    </Description><Units:meter/><Value>3.5</Value></DefineArg>

  <DefineArg Name="PitchServo"><Description>
  Set to 1 for BottomFollowing on westward leg
  </Description><Units:count/><Value>0</Value></DefineArg>

    <DefineArg Name="BottomFollowingAltitude"><Description>
        Altitude while following the bottom.
    </Description><Units:meter/><Value>5</Value></DefineArg>

    <DefineArg Name="BottomFollowingMinAltitude"><Description>
        Minimum altitude while following the bottom.
    </Description><Units:meter/><Value>3.5</Value></DefineArg>

    <DefineArg Name="BottomFollowingMaxDepth"><Description>
        Maximum depth while following the bottom.
    </Description><Units:meter/><Value>100.0</Value></DefineArg>

    <DefineArg Name="kpAltitude"><Description>
        PID loop parameter for following the bottom.
    </Description><Units:none/><Value>-0.08</Value></DefineArg>

    <DefineArg Name="kiAltitude"><Description>
        PID loop parameter for following the bottom.
    </Description><Units:none/><Value>0</Value></DefineArg>

    <DefineArg Name="kdAltitude"><Description>
        PID loop parameter for following the bottom.
    </Description><Units:none/><Value>0</Value></DefineArg>

    <DefineArg Name="ScienceOnDepth"><Description>
        Turn on science when below this depth. Only applies to Nitrate.
    </Description><Units:meter/><Value>1</Value></DefineArg>

    <DefineArg Name="NitrateOnPitch"><Description>
        Turn on Nitrate when pitch exceeds this value.
    </Description><Units:degree/><Value>90</Value></DefineArg>

    <DefineArg Name="YoYoMinDepth"><Description>
        Minimum depth during yo-yo's.
    </Description><Units:meter/><Value>3.0</Value></DefineArg>

    <DefineArg Name="YoYoMaxDepth"><Description>
        Maximum depth during yo-yo's.
    </Description><Units:meter/><Value>100.0</Value></DefineArg>

    <DefineArg Name="YoYoUpPitch"><Description>
        Pitch when yo-yo's are ascending.
    </Description><Units:degree/><Value>30.0</Value></DefineArg>

    <DefineArg Name="YoYoDownPitch"><Description>
        Pitch when yo-yo's are descending.
    </Description><Units:degree/><Value>-15.0</Value></DefineArg>

    <DefineArg Name="Speed"><Description>
        Vehicle speed.
    </Description><Units:meter_per_second /><Value>1</Value></DefineArg>

    <DefineArg Name="NeedCommsTime"><Description>
        Elapsed time after previous surface communications when vehicle will
        begin to ascend for additional surface communications, if the vehicle
        has a positive pitch. If the vehicle has a negative pitch, it will wait
        up to 20 minutes more to begin ascent.
    </Description><Units:minute/><Value>180</Value></DefineArg>

    <DefineArg Name="TimeoutDuration"><Description>
        Maximum length of mission
    </Description><Units:hour/><Value>30</Value></DefineArg>

    <DefineArg Name="Repeat"><Description>
        Number of times to repeat the loop
    </Description><Units:count/><Value>5</Value></DefineArg>

    <DefineArg Name="BuoyancyNeutral"><Description>
        Buoyancy bladder position while performing the YoYo behavior. Defaults
        to buoyancyNeutral setting in the Config/Control.cfg file.
    </Description><CustomUri Uri="Config/Control.buoyancyNeutral"/></DefineArg>


<!-- Waypoint list -->

    <DefineArg Name="Wpt1Lat"><Description>
        Latitude of deep waypoint.
    </Description><Units:degree/><Value>36.719</Value></DefineArg>

    <DefineArg Name="Wpt1Lon"><Description>
        Longitude of deep waypoint.
    </Description><Units:degree/><Value>-122.0100</Value></DefineArg>

    <DefineArg Name="Wpt1aLat"><Description>
        Latitude of 1st shallowing waypoint.
    </Description><Units:degree/><Value>36.719</Value></DefineArg>

    <DefineArg Name="Wpt1aLon"><Description>
        Longitude of 1st shallowing waypoint.
    </Description><Units:degree/><Value>-121.9470</Value></DefineArg>

    <DefineArg Name="Wpt1bLat"><Description>
        Latitude of 2nd shallowing waypoint.
    </Description><Units:degree/><Value>36.719</Value></DefineArg>

    <DefineArg Name="Wpt1bLon"><Description>
        Longitude of 1st shallowing waypoint.
    </Description><Units:degree/><Value>-121.8845</Value></DefineArg>

    <DefineArg Name="Wpt2Lat"><Description>
        Latitude of shallow waypoint - GPS update only, no Iridium!
    </Description><Units:degree/><Value>36.719</Value></DefineArg>

    <DefineArg Name="Wpt2Lon"><Description>
        Longitude of shallow waypoint - GPS update only, no Iridium!
    </Description><Units:degree/><Value>-121.8366</Value></DefineArg>

    <DefineArg Name="Wpt2aLat"><Description>
        Latitude of 1st deepening waypoint.
    </Description><Units:degree/><Value>36.719</Value></DefineArg>

    <DefineArg Name="Wpt2aLon"><Description>
        Longitude of 1st deepening waypoint.
    </Description><Units:degree/><Value>-121.8527</Value></DefineArg>

    <DefineArg Name="Wpt2bLat"><Description>
        Latitude of 2nd deepening waypoint.
    </Description><Units:degree/><Value>36.719</Value></DefineArg>

    <DefineArg Name="Wpt2bLon"><Description>
        Longitude of 2nd deepening waypoint.
    </Description><Units:degree/><Value>-121.9304</Value></DefineArg>

    <DefineArg Name="Wpt3Lat"><Description>
        Latitude of shallow waypoint - GPS update only, no Iridium!
    </Description><Units:degree/><Value>36.719</Value></DefineArg>

    <DefineArg Name="Wpt3Lon"><Description>
        Longitude of shallow waypoint - GPS update only, no Iridium!
    </Description><Units:degree/><Value>-121.8366</Value></DefineArg>

    <DefineArg Name="Wpt3aLat"><Description>
        Latitude of 1st westbound yoyo waypoint.
    </Description><Units:degree/><Value>36.719</Value></DefineArg>

    <DefineArg Name="Wpt3aLon"><Description>
        Longitude of 1st westbound yoyo waypoint.
    </Description><Units:degree/><Value>-121.8527</Value></DefineArg>

    <DefineArg Name="Wpt3bLat"><Description>
        Latitude of 2nd westbound yoyo waypoint.
    </Description><Units:degree/><Value>36.719</Value></DefineArg>

    <DefineArg Name="Wpt3bLon"><Description>
        Longitude of 2nd westbound yoyo waypoint.
    </Description><Units:degree/><Value>-121.9304</Value></DefineArg>


<!-- The following arguments are output arguments -->

    <Timeout Duration="TimeoutDuration"/>

    <Guidance:AltitudeEnvelope RunIn="Parallel">
        <Setting><Guidance:AltitudeEnvelope.minAltitude/><Arg Name="MinAltitude"/></Setting>
    </Guidance:AltitudeEnvelope>

    <Guidance:DepthEnvelope RunIn="Parallel">
        <Setting><Guidance:DepthEnvelope.maxDepth/><Arg Name="MaxDepth"/></Setting>
    </Guidance:DepthEnvelope>

    <Guidance:Buoyancy RunIn="Parallel">
        <Setting><Guidance:Buoyancy.position /><Arg Name="BuoyancyNeutral"/></Setting>
    </Guidance:Buoyancy>

    <Guidance:Pitch RunIn="Parallel">
        <Setting><Guidance:Pitch.massPosition/><Arg Name="MassDefault"/></Setting>
    </Guidance:Pitch>

    <Insert Filename="Insert/Surface.xml" Id="SURFACECOMMS" />

    <Insert Filename="Insert/SurfaceNavUpdate.xml" Id="SURFACENAV" />

    <Guidance:OffshoreEnvelope RunIn="Parallel">
        <Setting><Guidance:OffshoreEnvelope.minOffshore/><Arg Name="MinOffshore"/></Setting>
        <Setting><Guidance:OffshoreEnvelope.maxOffshore/><Arg Name="MaxOffshore"/></Setting>
    </Guidance:OffshoreEnvelope>

<!-- GPS Update.  Don't go too long w/o a GPS fix. -->

    <Aggregate Id="NeedComms">

        <When><Universal:platform_pitch_angle/>
            <Gt><Units:degree/><Value>0</Value></Gt>
            <And>
            <Elapsed><Universal:platform_communications/></Elapsed><Gt><Arg Name="NeedCommsTime"/></Gt></And>
            <Or>
            <Elapsed><Universal:platform_communications/></Elapsed><Gt><Arg Name="NeedCommsTime"/><Add><Units:minute/><Value>20</Value></Add></Gt></Or>
        </When>

        <Call RefId="SURFACECOMMS"/>

    </Aggregate>

<!-- If we're executing here or below, lets' get science data! -->

    <Insert Filename="Insert/Science.xml">
        <RedefineArg Name="PeakDetectChlActive"><False/></RedefineArg>
    </Insert>

<!-- Get to start point  -->

    <Aggregate Id="TransitWpt1">

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

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

        <Guidance:Waypoint Id="Wpt1" RunIn="Sequence">
            <Setting><Guidance:Waypoint.latitude/><Arg Name="Wpt1Lat"/></Setting>
            <Setting><Guidance:Waypoint.longitude/><Arg Name="Wpt1Lon"/></Setting>
        </Guidance:Waypoint>

        <Call Id="PHONEHOMETransitWPT1" RefId="SURFACECOMMS"/>

    </Aggregate>

<!-- ****** Repeat the following loop in to Wpt2 and out to Wpt1 for up to 30 hours or 5 round trips  ****** -->

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

<!-- ********************* Go to Wpt2  ********************* -->

        <Aggregate Id="YoYoWpt2">

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

            <Guidance:DepthEnvelope RunIn="Parallel">
                <Setting><Guidance:DepthEnvelope.minDepth/><Arg Name="YoYoMinDepth"/></Setting>
                <Setting><Guidance:DepthEnvelope.maxDepth/><Arg Name="YoYoMaxDepth"/></Setting>
                <Setting><Guidance:DepthEnvelope.downPitch/><Arg Name="YoYoDownPitch"/></Setting>
                <Setting><Guidance:DepthEnvelope.upPitch/><Arg Name="YoYoUpPitch"/></Setting>
            </Guidance:DepthEnvelope>

            <Guidance:AltitudeEnvelope RunIn="Parallel">
                <Setting><Guidance:AltitudeEnvelope.minAltitude/><Arg Name="YoYoMinAltitude"/></Setting>
                <Setting><Guidance:AltitudeEnvelope.upPitch/><Arg Name="YoYoUpPitch"/></Setting>
            </Guidance:AltitudeEnvelope>

            <Guidance:YoYo RunIn="Parallel">
                <Setting><Guidance:YoYo.downPitch/><Arg Name="YoYoDownPitch"/></Setting>
                <Setting><Guidance:YoYo.upPitch/><Arg Name="YoYoUpPitch"/></Setting>
            </Guidance:YoYo>

            <Guidance:Waypoint Id="Wpt1a" RunIn="Sequence">
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Wpt1aLat"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Wpt1aLon"/></Setting>
            </Guidance:Waypoint>

            <Call RefId="SURFACECOMMS"/>

            <Guidance:Waypoint Id="Wpt1b" RunIn="Sequence">
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Wpt1bLat"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Wpt1bLon"/></Setting>
            </Guidance:Waypoint>

            <Call RefId="SURFACECOMMS"/>

            <Guidance:Waypoint Id="Wpt2" RunIn="Sequence">
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Wpt2Lat"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Wpt2Lon"/></Setting>
            </Guidance:Waypoint>

            <Aggregate>

                <Timeout Duration="P5M"/>

                <Call Id="NavUpdateWPT2" RefId="SURFACENAV"/>

            </Aggregate>

        </Aggregate>

<!-- ********************* Go back to Wpt3  ********************* -->

<Aggregate Id="BackToWpt3">

        <Aggregate Id="YoYoWpt3">

<When> <Arg Name="PitchServo"/>
<Eq><Units:count/><Value>0</Value></Eq>
</When>

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

            <Guidance:DepthEnvelope RunIn="Parallel">
                <Setting><Guidance:DepthEnvelope.minDepth/><Arg Name="YoYoMinDepth"/></Setting>
                <Setting><Guidance:DepthEnvelope.maxDepth/><Arg Name="YoYoMaxDepth"/></Setting>
                <Setting><Guidance:DepthEnvelope.downPitch/><Arg Name="YoYoDownPitch"/></Setting>
                <Setting><Guidance:DepthEnvelope.upPitch/><Arg Name="YoYoUpPitch"/></Setting>
            </Guidance:DepthEnvelope>

            <Guidance:AltitudeEnvelope RunIn="Parallel">
                <Setting><Guidance:AltitudeEnvelope.minAltitude/><Arg Name="YoYoMinAltitude"/></Setting>
                <Setting><Guidance:AltitudeEnvelope.upPitch/><Arg Name="YoYoUpPitch"/></Setting>
            </Guidance:AltitudeEnvelope>

            <Guidance:YoYo RunIn="Parallel">
                <Setting><Guidance:YoYo.downPitch/><Arg Name="YoYoDownPitch"/></Setting>
                <Setting><Guidance:YoYo.upPitch/><Arg Name="YoYoUpPitch"/></Setting>
            </Guidance:YoYo>

            <Guidance:Waypoint Id="Wpt3a" RunIn="Sequence">
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Wpt3aLat"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Wpt3aLon"/></Setting>
            </Guidance:Waypoint>

            <Call RefId="SURFACECOMMS"/>

            <Guidance:Waypoint Id="Wpt3b" RunIn="Sequence">
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Wpt3bLat"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Wpt3bLon"/></Setting>
            </Guidance:Waypoint>

            <Call RefId="SURFACECOMMS"/>

            <Guidance:Waypoint Id="Wpt2" RunIn="Sequence">
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Wpt3Lat"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Wpt3Lon"/></Setting>
            </Guidance:Waypoint>

            <Aggregate>

                <Timeout Duration="P5M"/>

                <Call Id="NavUpdateWPT3" RefId="SURFACENAV"/>

            </Aggregate>

        </Aggregate>



        <Aggregate Id="BottomFollowWpt3">

<When> <Arg Name="PitchServo"/>
<Ne><Units:count/><Value>0</Value></Ne>
</When>


<!-- Limit the time that ISUS is on -->

            <Aggregate>

                <Until><Universal:platform_pitch_angle/>
                    <Lt><Arg Name="NitrateOnPitch"/></Lt>
                    <Or><Universal:depth/><Lt><Arg Name="ScienceOnDepth"/></Lt></Or>
                </Until>

                <When><Universal:platform_pitch_angle/>
                    <Ge><Arg Name="NitrateOnPitch"/></Ge>
                    <And><Universal:depth/><Ge><Arg Name="ScienceOnDepth"/></Ge></And>
                </When>

                <ReadData>
                    <Universal:mole_concentration_of_nitrate_in_sea_water/>
                </ReadData>

                <Guidance:Wait RunIn="Sequence"/>

            </Aggregate>

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

            <Guidance:DepthEnvelope RunIn="Parallel">
                <Setting><Guidance:DepthEnvelope.maxDepth/><Arg Name="BottomFollowingMaxDepth"/></Setting>
                <Setting><Guidance:DepthEnvelope.pitch/><Units:degree/><Value>0.5</Value></Setting>
            </Guidance:DepthEnvelope>

            <Guidance:AltitudeEnvelope RunIn="Parallel">
                <Setting><Guidance:AltitudeEnvelope.minAltitude/><Arg Name="BottomFollowingMinAltitude"/></Setting>
                <Setting><Guidance:AltitudeEnvelope.upPitch/><Arg Name="YoYoUpPitch"/></Setting>
            </Guidance:AltitudeEnvelope>

            <Guidance:Pitch Id="LimitDeepDownPitch">
                <While><Control:VerticalControl.pitchCmd/>
                    <Lt><Units:degree/><Value>0</Value></Lt>
                    <And><Universal:depth/><Ge><Arg Name="BottomFollowingMaxDepth"/><Sub><Units:meter/><Value>1</Value></Sub></Ge></And>
                </While>
                <Setting><Guidance:Pitch.pitch/><Units:degree/><Value>-0.5</Value></Setting>
            </Guidance:Pitch>

            <Guidance:Pitch Id="LimitDownPitch">
                <While><Control:VerticalControl.pitchCmd/>
                    <Lt><Arg Name="YoYoDownPitch"/></Lt>
                </While>
                <Setting><Guidance:Pitch.pitch/><Arg Name="YoYoDownPitch"></Arg></Setting>
            </Guidance:Pitch>

            <Guidance:PitchServo RunIn="Parallel">
                <Setting>
                <Guidance:PitchServo.uri/><Universal:height_above_sea_floor/></Setting>
                <Setting><Guidance:PitchServo.holdValue/><Arg Name="BottomFollowingAltitude"/></Setting>
                <Setting><Guidance:PitchServo.proportionalGain/><Arg Name="kpAltitude"/></Setting>
                <Setting><Guidance:PitchServo.integralGain/><Arg Name="kiAltitude"/></Setting>
                <Setting><Guidance:PitchServo.derivativeGain/><Arg Name="kdAltitude"/></Setting>
                <Setting><Guidance:PitchServo.changePerMeter/><Units:meter/><Value>-1</Value></Setting>
            </Guidance:PitchServo>

            <Guidance:Waypoint Id="Wpt3a" RunIn="Sequence">
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Wpt3aLat"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Wpt3aLon"/></Setting>
            </Guidance:Waypoint>

            <Call RefId="SURFACECOMMS"/>

            <Guidance:Waypoint Id="Wpt3b" RunIn="Sequence">
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Wpt3bLat"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Wpt3bLon"/></Setting>
            </Guidance:Waypoint>

            <Call RefId="SURFACECOMMS"/>

            <Guidance:Waypoint Id="Wpt3" RunIn="Sequence">
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Wpt3Lat"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Wpt3Lon"/></Setting>
            </Guidance:Waypoint>

            <Call Id="PHONEHOMEYoYoWPT3" RefId="SURFACECOMMS"/>

        </Aggregate>



</Aggregate>


    </Aggregate>

</Mission>
