<?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://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/Trigger http://okeanids.mbari.org/tethys/Xml/Trigger.xsd
                           Tethys/Units http://okeanids.mbari.org/tethys/Xml/Units.xsd
                           Tethys/Universal http://okeanids.mbari.org/tethys/Xml/Universal.xsd"
       Id="trackPatch">

    <Description>
        Vehicle runs yo-yos, with patch detect enabled for cholorphyll
        concentration.
    </Description>

<!-- Vehicle safety parameters -->

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

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

    <DefineArg Name="MinWaterDepth"><Description>
        Minimum water depth for the entire mission.
    </Description><Units:meter/><Value>10</Value></DefineArg>

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

    <DefineArg Name="NeedCommsTime"><Description>
        Maximum time between surfacings for communications
    </Description><Units:minute/><Value>180</Value></DefineArg>

    <DefineArg Name="MissionTimeout"><Description>
        Timeout for the entire mission
    </Description><Units:hour/><Value>56</Value></DefineArg>

    <DefineArg Name="MassDefault"><Description>
        Static setting for mass during the mission. Defaults to massDefault
        setting in the Config/Control.cfg file.
    </Description><Control:VerticalControl.massDefault/></DefineArg>

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

<!-- Basic guidance settings -->

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

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

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

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

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

    <DefineArg Name="WpMaxDistance"><Description>
        Maximum length of each tracking leg
<!--debug
    </Description><Units:meter/><Value>7500</Value></DefineArg>
-->
    </Description><Units:meter/><Value>1000</Value></DefineArg>

    <DefineArg Name="WpTimeout"><Description>
        Maximum duration of each tracking leg
    </Description><Units:second/><Value>8000</Value></DefineArg>

    <DefineArg Name="WaterFrame"><Description>
        Set WaterFrame true to drive towards waypoints in the water frame of
        reference
    </Description><True/></DefineArg>

    <DefineArg Name="Repeat"><Description>
        Maximum number of tracking legs
    </Description><Units:count/><Value>20</Value></DefineArg>

<!-- Constant settings for each tracking leg -->

    <DefineArg Name="Turn"><Description>
        Amount to turn at each leg
    </Description><Units:degree/><Value>90</Value></DefineArg>

    <DefineArg Name="Reverse"><Description>
        Change in bearing which reverses vehicle direction
    </Description><Units:degree/><Value>180</Value></DefineArg>

    <DefineArg Name="TwoPi"><Description>
        Applied to stop run-up of angles
    </Description><Units:degree/><Value>360</Value></DefineArg>

    <DefineArg Name="TimeWindowPeakRpt"><Description>
        If greater than zero, report a peak every window. If NaN or zero, this
        variable is ignored.
    </Description><Units:minute/><Value>NaN</Value></DefineArg>

<!--debug
    </Description><Units:minute/><Value>1</Value></DefineArg>
-->

    <DefineArg Name="LowPassWindowLen"><Description>
        Low-pass window length (based on depth sensor sampling interval 0.4
        second) for low-pass filtering.
    </Description><Units:count/><Value>20</Value></DefineArg>

    <DefineArg Name="PeakChlShallowBnd"><Description>
        Shallow depth bound for detecting chl peak on each descent or ascent
        profile.
    </Description><Units:meter/><Value>10</Value></DefineArg>

<!--debug
    </Description><Units:meter/><Value>3</Value></DefineArg>
-->

    <DefineArg Name="PeakChlDeepBnd"><Description>
        Deep depth bound for detecting chl peak on each descent or ascent
        profile.
    </Description><Units:meter/><Value>20</Value></DefineArg>

<!--debug
    </Description><Units:meter/><Value>8</Value></DefineArg>
-->

    <DefineArg Name="NumOfProfilesSlidingwindow"><Description>
        Length of horizontal sliding window. The highest yoyo-wise chl peak
        (after low-pass filtering by a filter of length FilterWidthHorizontal)
        within this window.
    </Description><Units:count/><Value>10</Value></DefineArg>

    <DefineArg Name="FilterWidth"><Description>
        Width of boxcar filter used in horizontal patch detection
    </Description><Units:count/><Value>4</Value></DefineArg>

    <DefineArg Name="BeginThreshold"><Description>
        If non-NaN, sets threshold for starting horizontal patch detection
    </Description><Units:microgram_per_liter/><Value>NaN</Value></DefineArg>

    <DefineArg Name="OffPeakFraction"><Description>
        When filtered horizontal value is this fraction of the peak, consider it
        outside the patch.
    </Description><Units:percent/><Value>80</Value></DefineArg>

    <DefineArg Name="PatchMode"><Description>
        If true, use center of "in peak" range as the turn-around point. If
        false use maximum horizontal peak location as the turn-around point.
    </Description><False/></DefineArg>

    <DefineArg Name="PatchTrackingHorizontal"><Description>
        Set true to turn on horizontal patch tracking
    </Description><True/></DefineArg>

<!-- Variable Settings for each tracking leg -->

    <DefineArg Name="WpBearing"><Description>
        Bearing being followed in the current leg. Set this to initialize start
        direction of patch tracking.
    </Description><Units:degree/><Value>-90</Value></DefineArg>

    <DefineOutput Name="PatchDetectOn"><Description>
        Mission sets this to true when the patch has been crossed.
    </Description><False/></DefineOutput>

    <DefineOutput Name="WpDone"><Description>
        Mission sets this to true when the cross-patch waypoint has been
        reached.
    </Description><False/></DefineOutput>

    <DefineOutput Name="PatchChl"><Units:microgram_per_liter/><Value>NaN</Value></DefineOutput>

    <DefineOutput Name="PatchChlDepth"><Units:meter/><Value>NaN</Value></DefineOutput>

    <DefineOutput Name="PatchChlLatitude"><Units:degree/><Value>NaN</Value></DefineOutput>

    <DefineOutput Name="PatchChlLongitude"><Units:degree/><Value>NaN</Value></DefineOutput>

    <DefineOutput Name="PatchChlDistance"><Units:meter/><Value>0</Value></DefineOutput>

<!-- The timeout -->

    <Timeout Duration="MissionTimeout"/>

<!-- And now the mission -->

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

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

    <Insert Filename="Insert/Surface.xml" Id="SurfaceCommsSplit">
        <RedefineArg Name="SurfacePitch"><Units:degree/><Value>20</Value></RedefineArg>
        <RedefineArg Name="SurfaceSpeed"><Units:meter_per_second/><Value>1</Value></RedefineArg>
        <RedefineArg Name="UnpackSplitAtSurface"><True/></RedefineArg>
    </Insert>

    <Aggregate Id="SurfaceComms">

        <When>
            <Called/>
        </When>

        <Insert Filename="Insert/Surface.xml">
            <RedefineArg Name="SurfacePitch"><Units:degree/><Value>20</Value></RedefineArg>
            <RedefineArg Name="SurfaceSpeed"><Units:meter_per_second/><Value>1</Value></RedefineArg>
        </Insert>

    </Aggregate>

<!-- 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:time_fix/></Elapsed><Gt><Arg Name="NeedCommsTime"/></Gt></And>
            <Or><Elapsed><Universal:time_fix/></Elapsed><Gt><Arg Name="NeedCommsTime"/><Add><Units:minute/><Value>10</Value></Add></Gt></Or>
        </When>

        <Syslog Severity="Important">NeedComms Timeout Surfacing</Syslog>

        <Call RefId="SurfaceComms"/>

    </Aggregate>

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

    <Guidance:WaterDepthEnvelope>
        <Parallel/>
        <Setting><Guidance:WaterDepthEnvelope.minWaterDepth/><Arg Name="MinWaterDepth"/></Setting>
    </Guidance:WaterDepthEnvelope>

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

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

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

    <Insert Filename="Insert/Science.xml">
        <RedefineArg Name="PeakDetectChlActive"><True/></RedefineArg>
        <RedefineArg Name="PatchTracking"><True/></RedefineArg>
        <RedefineArg Name="TimeWindowPeakReport"><Arg Name="TimeWindowPeakRpt"/></RedefineArg>
        <RedefineArg Name="LowPassWindowLength"><Arg Name="LowPassWindowLen"/></RedefineArg>
        <RedefineArg Name="PeakShallowBound"><Arg Name="PeakChlShallowBnd"/></RedefineArg>
        <RedefineArg Name="PeakDeepBound"><Arg Name="PeakChlDeepBnd"/></RedefineArg>
        <RedefineArg Name="OffPeakFractionHorizontal"><Arg Name="OffPeakFraction"/></RedefineArg>
    </Insert>

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

    <Guidance:DepthEnvelope>
        <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:YoYo>
        <Parallel/>
        <Setting><Guidance:YoYo.downPitch/><Arg Name="YoYoDownPitch"/></Setting>
        <Setting><Guidance:YoYo.upPitch/><Arg Name="YoYoUpPitch"/></Setting>
    </Guidance:YoYo>

    <Aggregate Id="PatchDetectHorizontal">

        <Sequence Repeat="Repeat"/>

        <Aggregate Id="PatchDetection">

            <When>
                <Arg Name="PatchDetectOn"/>
            </When>

            <Trigger:PeakDetectHorizontal Id="PeakDetectHorizontalTrigger">
                <Sequence/>
                <Break><Arg Name="WpDone"/>
                </Break>
                <Setting><Trigger:PeakDetectHorizontal.patchTracking/><Arg Name="PatchTrackingHorizontal"/></Setting>
                <Setting><Trigger:PeakDetectHorizontal.detectFrom/><Arg Name="Science:PeakChl"/></Setting>
                <Setting><Trigger:PeakDetectHorizontal.depthFrom/><Arg Name="Science:PeakChlDepth"/></Setting>
                <Setting><Trigger:PeakDetectHorizontal.latitudeFrom/><Arg Name="Science:PeakChlLatitude"/></Setting>
                <Setting><Trigger:PeakDetectHorizontal.longitudeFrom/><Arg Name="Science:PeakChlLongitude"/></Setting>
                <Setting><Trigger:PeakDetectHorizontal.beginThreshold/><Arg Name="BeginThreshold"/></Setting>
                <Setting><Trigger:PeakDetectHorizontal.offPeakFraction/><Arg Name="OffPeakFraction"/></Setting>
                <Setting><Trigger:PeakDetectHorizontal.filterWidth/><Arg Name="FilterWidth"/></Setting>
                <Setting><Trigger:PeakDetectHorizontal.numProfilesSlidingwindow/><Arg Name="NumOfProfilesSlidingwindow"/></Setting>
                <Setting><Trigger:PeakDetectHorizontal.centerPeak/><Arg Name="PatchMode"/></Setting>
                <OutputArg Name="PatchChl"><Trigger:PeakDetectHorizontal.peakDetect/></OutputArg>
                <OutputArg Name="PatchChlDepth"><Trigger:PeakDetectHorizontal.peakDepth/></OutputArg>
                <OutputArg Name="PatchChlLatitude"><Trigger:PeakDetectHorizontal.peakLatitude/></OutputArg>
                <OutputArg Name="PatchChlLongitude"><Trigger:PeakDetectHorizontal.peakLongitude/></OutputArg>
                <OutputArg Name="PatchChlDistance"><Trigger:PeakDetectHorizontal.peakDistance/></OutputArg>
            </Trigger:PeakDetectHorizontal>

            <Syslog Severity="Important">Fell below OffPeakFraction. Patch value is <Arg Name="PatchChl"/><Units:microgram_per_liter/> at lat = <Arg Name="PatchChlLatitude"/><Units:degree/>, lon = <Arg Name="PatchChlLongitude"/><Units:degree/>, dep = <Arg Name="PatchChlDepth"/><Units:meter/>, distance = <Arg Name="PatchChlDistance"/><Units:meter/></Syslog>

            <SendData Service="Express"><Arg Name="PatchChl"/><Arg Name="PatchChlDepth"/><Arg Name="PatchChlLatitude"/><Arg Name="PatchChlLongitude"/>
            </SendData>

            <Assign Id="AssignPatchDetectOff"><Sequence/><Arg Name="PatchDetectOn"/><False/></Assign>

        </Aggregate>

        <Assign Id="AssignPatchDetectOn"><Sequence/><Arg Name="PatchDetectOn"/><True/></Assign>

        <Syslog Severity="Important">Next: bearing for CrossLeg: <Arg Name="WpBearing"/><Units:degree/>. WpMaxDistance = <Arg Name="WpMaxDistance"/><Units:meter/>. Current lat, lon = <Universal:latitude/><Units:degree/>, <Universal:longitude/><Units:degree/>.</Syslog>

        <Aggregate Id="CrossLeg">

            <Description>
                This is a long leg across the width of the patch. Also the first
                leg in a mission run. Two waypoints run in parallel: a short run
                to the surfacing point, and a longer run to the edge of the
                patch.
            </Description>

            <Sequence/>

            <Break><Arg Name="WpDone"/>
                <Or><Arg Name="PatchDetectOn"/><Eq><False/></Eq></Or>
            </Break>

            <Guidance:Waypoint Id="CrossSurfaceWp">
                <Sequence/>
                <Timeout Duration="WpTimeout"/>
                <Setting><Guidance:Waypoint.distanceDelta/><Arg Name="PatchChlDistance"/></Setting>
                <Setting><Guidance:Waypoint.distanceDeltaBearing/><Arg Name="WpBearing"/></Setting>
                <Setting><Guidance:Waypoint.waterFrame/><Arg Name="WaterFrame"/></Setting>
            </Guidance:Waypoint>

            <Call RefId="SurfaceComms"/>

            <Guidance:Waypoint Id="CrossPeakWp">
                <Sequence/>
                <Timeout Duration="WpTimeout"/>
                <Setting><Guidance:Waypoint.distanceDelta/><Arg Name="WpMaxDistance"/></Setting>
                <Setting><Guidance:Waypoint.distanceDeltaBearing/><Arg Name="WpBearing"/></Setting>
                <Setting><Guidance:Waypoint.waterFrame/><Arg Name="WaterFrame"/></Setting>
            </Guidance:Waypoint>

            <Syslog Severity="Important">CrossPeakWp done. Lat, lon = <Universal:latitude/><Units:degree/>, <Universal:longitude/><Units:degree/>. Patch value is <Arg Name="PatchChl"/><Units:microgram_per_liter/> at lat = <Arg Name="PatchChlLatitude"/><Units:degree/>, lon = <Arg Name="PatchChlLongitude"/><Units:degree/>, dep = <Arg Name="PatchChlDepth"/><Units:meter/>, distance = <Arg Name="PatchChlDistance"/><Units:meter/></Syslog>

            <Assign Id="AssignPatchDetectOffInCrossLeg"><Sequence/><Arg Name="PatchDetectOn"/><False/></Assign>

            <Assign Id="AssignWpDoneInCrossLeg"><Sequence/><Arg Name="WpDone"/><True/></Assign>

        </Aggregate>

        <Assign Id="AssignWpBearingForInLeg"><Sequence/><Arg Name="WpBearing"/><Arg Name="WpBearing"/><Add><Arg Name="Reverse"/></Add><Mod><Arg Name="TwoPi"/>
        </Mod></Assign>

        <Syslog Severity="Important">Next: bearing for InLeg: <Arg Name="WpBearing"/><Units:degree/>. Current lat, lon = <Universal:latitude/><Units:degree/>, <Universal:longitude/><Units:degree/>. Patch value is <Arg Name="PatchChl"/><Units:microgram_per_liter/> at lat = <Arg Name="PatchChlLatitude"/><Units:degree/>, lon = <Arg Name="PatchChlLongitude"/><Units:degree/>, dep = <Arg Name="PatchChlDepth"/><Units:meter/>, distance = <Arg Name="PatchChlDistance"/><Units:meter/></Syslog>

        <Guidance:Waypoint Id="InLegWp">
            <Description>
                Simply drive back towards the patch center
            </Description>
            <Sequence/>
            <Timeout Duration="WpTimeout"/>
            <Setting><Guidance:Waypoint.distanceDelta/><Arg Name="PatchChlDistance"/></Setting>
            <Setting><Guidance:Waypoint.distanceDeltaBearing/><Arg Name="WpBearing"/></Setting>
            <Setting><Guidance:Waypoint.waterFrame/><Arg Name="WaterFrame"/></Setting>
        </Guidance:Waypoint>

        <Assign Id="AssignWpBearingForOutLeg"><Sequence/><Arg Name="WpBearing"/><Arg Name="WpBearing"/><Sub><Arg Name="Turn"/></Sub><Mod><Arg Name="TwoPi"/>
        </Mod></Assign>

        <Syslog Severity="Important">Next: bearing for OutLeg: <Arg Name="WpBearing"/><Units:degree/>. WpMaxDistance = <Arg Name="WpMaxDistance"/><Units:meter/>. Current lat, lon = <Universal:latitude/><Units:degree/>, <Universal:longitude/><Units:degree/>. Patch value is <Arg Name="PatchChl"/><Units:microgram_per_liter/> at lat = <Arg Name="PatchChlLatitude"/><Units:degree/>, lon = <Arg Name="PatchChlLongitude"/><Units:degree/>, dep = <Arg Name="PatchChlDepth"/><Units:meter/>, distance = <Arg Name="PatchChlDistance"/><Units:meter/></Syslog>

        <Call RefId="SurfaceCommsSplit"/>

        <Assign Id="AssignPatchDetectOnForOutLeg"><Sequence/><Arg Name="PatchDetectOn"/><True/></Assign>

        <Aggregate Id="OutLeg">

            <Description>
                Drive outward until edge of patch is detected.
            </Description>

            <Sequence/>

            <Break><Arg Name="WpDone"/>
                <Or><Arg Name="PatchDetectOn"/><Eq><False/></Eq></Or>
            </Break>

            <Guidance:Waypoint Id="OutLegWp">
                <Sequence/>
                <Timeout Duration="WpTimeout"/>
                <Setting><Guidance:Waypoint.distanceDelta/><Arg Name="WpMaxDistance"/></Setting>
                <Setting><Guidance:Waypoint.distanceDeltaBearing/><Arg Name="WpBearing"/></Setting>
                <Setting><Guidance:Waypoint.waterFrame/><Arg Name="WaterFrame"/></Setting>
            </Guidance:Waypoint>

            <Assign Id="AssignPatchDetectOffInOutLeg"><Sequence/><Arg Name="PatchDetectOn"/><False/></Assign>

            <Assign Id="AssignWpDoneInOutLeg"><Sequence/><Arg Name="WpDone"/><True/></Assign>

        </Aggregate>

        <Assign Id="AssignWpBearingForCrossLeg"><Sequence/><Arg Name="WpBearing"/><Arg Name="WpBearing"/><Add><Arg Name="Reverse"/></Add><Mod><Arg Name="TwoPi"/>
        </Mod></Assign>

    </Aggregate>

</Mission>
