<?xml version="1.0" encoding="UTF-8"?>
<Mission xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
         xmlns="Tethys"
         xmlns:Units="Tethys/Units"
         xmlns:Guidance="Tethys/Guidance"
         xmlns:Control="Tethys/Control"
         xmlns:Sample="Tethys/Sample"
         xmlns:Science="Tethys/Science"
         xmlns:Universal="Tethys/Universal"
         xsi:schemaLocation="Tethys http://okeanids.mbari.org/tethys/Xml/Tethys.xsd
                             Tethys/Units http://okeanids.mbari.org/tethys/Xml/Units.xsd
                             Tethys/Guidance http://okeanids.mbari.org/tethys/Xml/Guidance.xsd
                             Tethys/Control http://okeanids.mbari.org/tethys/Xml/Control.xsd
                             Tethys/Sample http://okeanids.mbari.org/tethys/Xml/Sample.xsd
                             Tethys/Science http://okeanids.mbari.org/tethys/Xml/Science.xsd
                             Tethys/Universal http://okeanids.mbari.org/tethys/Xml/Universal.xsd"
       Id="esp_sample_at_threshold">

    <Description>
        This mission drives towards a waypoint until chl reading is above a
        threshold for the timeout. Then it stops and takes ESP samples at the
        designated depth. Originally this was meant to be a mashup of sci2 and
        esp_sample_at_depth.
    </Description>

    <DefineArg Name="MissionTimeout"><Description>
        Maximum duration of mission.
    </Description><Units:hour/><Value>6</Value></DefineArg>

    <DefineArg Name="NeedCommsTime"><Description>
        How often to surface for commumications during transit to waypoint.
    </Description><Units:minute/><Value>60</Value></DefineArg>

    <DefineArg Name="chlorophyllLowerBound"><Description>
        When WetLabsBB2FL.bin_median_mass_concentration_of_chlorophyll lies
        between chlorophyllLowerBound and chlorophyllUpperBound for the timeout,
        stop running waypoints and sample at depth.
    </Description><Units:microgram_per_liter/><Value>3</Value></DefineArg>

    <DefineArg Name="chlorophyllUpperBound"><Description>
        When WetLabsBB2FL.bin_median_mass_concentration_of_chlorophyll lies
        between chlorophyllLowerBound and chlorophyllUpperBound for the timeout,
        stop running waypoints and sample at depth.
    </Description><Units:microgram_per_liter/><Value>100</Value></DefineArg>

    <DefineArg Name="chlorophyllThresholdTimeout"><Description>
        How long to wait to start sampling while chl is above the threshold.
    </Description><Units:minute/><Value>1</Value></DefineArg>

    <DefineArg Name="Lat1"><Description>
        Latitude of waypoint 1. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lon1"><Description>
        Longitude of waypoint 1. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lat2"><Description>
        Latitude of waypoint 2. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lon2"><Description>
        Longitude of waypoint 2. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lat3"><Description>
        Latitude of waypoint 3. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lon3"><Description>
        Longitude of waypoint 3. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lat4"><Description>
        Latitude of waypoint 4. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lon4"><Description>
        Longitude of waypoint 4. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lat5"><Description>
        Latitude of waypoint 5. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lon5"><Description>
        Longitude of waypoint 5. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lat6"><Description>
        Latitude of waypoint 6. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lon6"><Description>
        Longitude of waypoint 6. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lat7"><Description>
        Latitude of waypoint 7. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lon7"><Description>
        Longitude of waypoint 7. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Speed"><Description>
        Speed while performing the YoYo behavior.
    </Description><Units:meter_per_second/><Value>1</Value></DefineArg>

    <DefineArg Name="CaptureRadius"><Description>
        Speed while performing the YoYo behavior.
    </Description><Units:meter/><Value>NaN</Value></DefineArg>

    <DefineArg Name="YoYoMinDepth"><Description>
        Minimum depth while performing the YoYo behavior.
    </Description><Units:meter/><Value>2</Value></DefineArg>

    <DefineArg Name="YoYoMaxDepth"><Description>
        Maximum depth while performing the YoYo behavior.
    </Description><Units:meter/><Value>200</Value></DefineArg>

    <DefineArg Name="YoYoMinAltitude"><Description>
        Minimum altitude while performing the YoYo behavior (for
        bottom-terminated YoYos).
    </Description><Units:meter/><Value>9</Value></DefineArg>

    <DefineArg Name="YoYoUpPitch"><Description>
        Vehicle up pitch while performing the YoYo behavior.
    </Description><Units:degree/><Value>20</Value></DefineArg>

    <DefineArg Name="YoYoDownPitch"><Description>
        Vehicle down pitch while performing the YoYo behavior.
    </Description><Units:degree/><Value>-20</Value></DefineArg>

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

    <DefineArg Name="MassDefault"><Description>
        Static setting for mass during the mission. Set to NaN mm for active
        mass position
    </Description><Control:VerticalControl.massDefault/></DefineArg>

    <DefineArg Name="TargetDepth"><Description>
        Depth to sample at. Initialized to 3 m.
    </Description><Units:meter/><Value>3</Value></DefineArg>

    <DefineArg Name="ESPCartridgeType_1"><Description>
        Specifies ESP cartridge type code for sample# 1. Set to NaN to skip this
        sample.
    </Description><Units:count/><Value>-4</Value></DefineArg>

    <DefineArg Name="ESPCartridgeType_2"><Description>
        Specifies ESP cartridge type code for sample# 2. Set to NaN to skip this
        sample.
    </Description><Units:count/><Value>-15</Value></DefineArg>

    <DefineArg Name="ESPCartridgeType_3"><Description>
        Specifies ESP cartridge type code for sample# 3. Set to NaN to skip this
        sample.
    </Description><Units:count/><Value>NaN</Value></DefineArg>

    <DefineArg Name="ESPCartridgeType_4"><Description>
        Specifies ESP cartridge type code for sample# 4. Set to NaN to skip this
        sample.
    </Description><Units:count/><Value>NaN</Value></DefineArg>

    <DefineArg Name="SettleTime"><Description>
        How long to wait between samples. This starts *before* the first sample.
    </Description><Units:minute/><Value>3</Value></DefineArg>

    <DefineArg Name="SpeedSampling"><Description>
        Vehicle speed while sampling. Set to zero to maintain depth using VBS
        (drift).
    </Description><Units:meter_per_second/><Value>0</Value></DefineArg>

    <DefineArg Name="ApproachSettleTimePostDive"><Description>
        Time duration for the vehicle to settle *AFTER* reaching the target
        depth.
    </Description><Units:minute/><Value>1</Value></DefineArg>

    <DefineArg Name="kpDepth"><Description>
        Dive loop proportional gain (applied only while the vehicle is
        descending to target depth).
    </Description><Units:radian_per_meter/><Value>0.07</Value></DefineArg>

    <DefineArg Name="DepthDeadband"><Description>
        How much vertical drift from the specified depth is allowed while
        sampling.
    </Description><Units:meter/><Value>0.01</Value></DefineArg>

    <DefineArg Name="MinAltitude"><Description>
        Minimum height above the sea floor for the entire mission.
    </Description><Units:meter/><Value>2</Value></DefineArg>

    <DefineArg Name="MaxDepth"><Description>
        Maximum allowable depth during the mission.
    </Description><Units:meter/><Value>5</Value></DefineArg>

    <DefineArg Name="MinOffshore"><Description>
        Minimum distance from the shoreline to maintain.
    </Description><Units:kilometer/><Value>1</Value></DefineArg>

    <DefineOutput Name="ESPCartridgeType"><Description>
        Mission parameter (don't change). Specifies ESP cartridge type code.
    </Description><Units:count/><Value>NaN</Value></DefineOutput>

    <DefineOutput Name="ChlThresholdReached"><Description>
        Indicates internally that the threshold has been reach and we should
        start a sample.
    </Description><False/></DefineOutput>

    <DefineOutput Name="ChlThresholdReachedFlagSet"><Description>
        Indicates internally that the threshold-met flag is set.
    </Description><False/></DefineOutput>

<!-- Mission timeout -->

    <Timeout Duration="MissionTimeout"/>

<!-- Safety nets and surfacing behaviors -->

    <Sample:AbortSample>
        <Parallel/>
    </Sample:AbortSample>

    <Insert Filename="Insert/StandardEnvelopes.xml"/>

    <Assign><Sequence/><Arg Name="StandardEnvelopes:MaxDepth"/><Arg Name="MaxDepth"/></Assign>

    <Assign><Sequence/><Arg Name="StandardEnvelopes:MinAltitude"/><Arg Name="MinAltitude"/></Assign>

    <Assign><Sequence/><Arg Name="StandardEnvelopes:MinOffshore"/><Arg Name="MinOffshore"/></Assign>

    <Insert Filename="Insert/NeedComms.xml" Id="NeedComms"/>

    <Assign><Sequence/><Arg Name="NeedComms:DiveInterval"/><Arg Name="MissionTimeout"/></Assign>

    <Assign><Sequence/><Arg Name="NeedComms:WaitForPitchUp"/><Units:minute/><Value>0</Value></Assign>

    <Assign><Parallel/><Control:VerticalControl.depthDeadband/><Arg Name="DepthDeadband"/></Assign>

<!-- Callable aggregate -->

    <Aggregate Id="SampleESP">

        <When>
            <Called/>
        </When>

        <Aggregate Id="sample">

            <Sequence/>

            <Syslog Severity="Important">At <Universal:depth/><Units:meter/>, settling for <Arg Name="SettleTime"/><Units:minute/></Syslog>

            <Guidance:Wait>
                <Sequence/>
                <Setting><Guidance:Wait.duration/><Arg Name="SettleTime"/></Setting>
            </Guidance:Wait>

            <Syslog Severity="Important">Requesting ESP cartridge type:<Arg Name="ESPCartridgeType"/><Units:count/></Syslog>

            <Science:ESPCartridgeSelect>
                <Sequence/>
                <Timeout Duration="P1M"/>
                <Setting><Science:ESPCartridgeSelect.cartridgeType/><Arg Name="ESPCartridgeType"/></Setting>
            </Science:ESPCartridgeSelect>

            <ReadDatum Id="TriggerESP">
                <Science:ESPComponent.sampling/>
            </ReadDatum>

            <Syslog Severity="Important">ESP sampling at <Universal:depth/><Units:meter/> in <Science:CTD_Seabird.bin_mean_sea_water_temperature/><Units:celsius/> water with <Science:WetLabsBB2FL.bin_mean_mass_concentration_of_chlorophyll_in_sea_water/><Units:microgram_per_liter/>chlorophyll fluorescence.</Syslog>

            <ReadDatum Id="WaitForESP">
                <Science:ESPComponent.sample_number/>
            </ReadDatum>

        </Aggregate>

    </Aggregate>

<!-- Start mission sequence -->

    <Call Id="StartingMission" RefId="NeedComms"/>

    <Insert Filename="Insert/Science.xml"></Insert>

    <Aggregate Id="CheckChlThreshold">

        <When>
            <Science:WetLabsBB2FL.bin_median_mass_concentration_of_chlorophyll_in_sea_water/>
            <Gt><Arg Name="chlorophyllLowerBound"/></Gt>
            <And><Science:WetLabsBB2FL.bin_median_mass_concentration_of_chlorophyll_in_sea_water/><Lt><Arg Name="chlorophyllUpperBound"/></Lt></And>
            <And><Elapsed><Universal:time_fix/></Elapsed><Gt><Arg Name="chlorophyllThresholdTimeout"/></Gt></And>
            <And><Not><Arg Name="ChlThresholdReachedFlagSet"/></Not></And>
        </When>

        <Assign><Sequence/><Arg Name="ChlThresholdReached"/><True/></Assign>

        <Assign><Sequence/><Arg Name="ChlThresholdReachedFlagSet"/><True/></Assign>

    </Aggregate>

<!-- This first aggregate is the sci2 part -->

    <Aggregate Id="Sci2Phase">

        <Break><Arg Name="ChlThresholdReached"/>
        </Break>

        <Assign><Sequence/><Arg Name="NeedComms:DiveInterval"/><Arg Name="NeedCommsTime"/></Assign>

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

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

        <Guidance:DepthEnvelope>
            <Description>
                Another depth envelope for the YoYo behavior. This envelope
                should fall within the limits of the standard safety envelopes
                in Insert/StandardEnvelopes.xml in order to avoid commanding
                high pitch angles for depth-terminated YoYos.
            </Description>
            <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>
            <Description>
                Another altitude envelope for the YoYo behavior. This envelope
                should fall within the limits of the standard safety envelopes
                in Insert/StandardEnvelopes.xml in order to avoid commanding
                high pitch angles for bottom-terminated YoYos.
            </Description>
            <Parallel/>
            <Setting><Guidance:AltitudeEnvelope.minAltitude/><Arg Name="YoYoMinAltitude"/></Setting>
            <Setting><Guidance:AltitudeEnvelope.upPitch/><Arg Name="YoYoUpPitch"/></Setting>
        </Guidance:AltitudeEnvelope>

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

        <Aggregate Id="Lap">

            <Aggregate Id="Wpt1">

                <Sequence/>

                <Guidance:Waypoint>
                    <Sequence/>
                    <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat1"/></Setting>
                    <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon1"/></Setting>
                    <Setting><Guidance:Waypoint.captureRadius/><Arg Name="CaptureRadius"/></Setting>
                </Guidance:Waypoint>

            </Aggregate>

            <Aggregate Id="Wpt2">

                <Sequence/>

                <Guidance:Waypoint>
                    <Sequence/>
                    <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat2"/></Setting>
                    <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon2"/></Setting>
                </Guidance:Waypoint>

            </Aggregate>

            <Aggregate Id="Wpt3">

                <Sequence/>

                <Guidance:Waypoint>
                    <Sequence/>
                    <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat3"/></Setting>
                    <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon3"/></Setting>
                </Guidance:Waypoint>

            </Aggregate>

            <Aggregate Id="Wpt4">

                <Sequence/>

                <Guidance:Waypoint>
                    <Sequence/>
                    <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat4"/></Setting>
                    <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon4"/></Setting>
                </Guidance:Waypoint>

            </Aggregate>

            <Aggregate Id="Wpt5">

                <Sequence/>

                <Guidance:Waypoint>
                    <Sequence/>
                    <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat5"/></Setting>
                    <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon5"/></Setting>
                </Guidance:Waypoint>

            </Aggregate>

            <Aggregate Id="Wpt6">

                <Sequence/>

                <Guidance:Waypoint>
                    <Sequence/>
                    <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat6"/></Setting>
                    <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon6"/></Setting>
                </Guidance:Waypoint>

            </Aggregate>

            <Aggregate Id="Wpt7">

                <Sequence/>

                <Guidance:Waypoint>
                    <Sequence/>
                    <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat7"/></Setting>
                    <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon7"/></Setting>
                </Guidance:Waypoint>

            </Aggregate>

        </Aggregate>

    </Aggregate>

<!-- Once the threshold is met for the timeout, we sample -->

    <Aggregate Id="SamplingPhase">

        <Break>
            <Not><Arg Name="ChlThresholdReached"/></Not>
        </Break>

        <Assign><Sequence/><Arg Name="NeedComms:DiveInterval"/><Arg Name="MissionTimeout"/></Assign>

        <Aggregate Id="SettleAtDepth">

            <Description>
                Take a few moments here to ensure the vechile is settled at the
                target depth.
            </Description>

            <Sequence/>

            <Guidance:Pitch>
                <Progression/>
                <Setting><Guidance:Pitch.depth/><Arg Name="TargetDepth"/></Setting>
            </Guidance:Pitch>

            <Syslog Severity="Info">Reached <Universal:depth/><Units:meter/>, settling for <Arg Name="ApproachSettleTimePostDive"/><Units:minute/></Syslog>

            <Guidance:Wait>
                <Sequence/>
                <Setting><Guidance:Wait.duration/><Arg Name="ApproachSettleTimePostDive"/></Setting>
            </Guidance:Wait>

        </Aggregate>

        <Aggregate Id="SampleAtTargetDepth">

            <Description>
                Maintain target depth and acquire ESP samples.
            </Description>

            <Sequence/>

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

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

            <Aggregate Id="Spinout">

                <Description>
                    A speed has been specified, spin in circels while sampling.
                </Description>

                <While>
                    <Arg Name="SpeedSampling"/>
                    <Gt><Units:meter_per_second/><Value>0.0</Value></Gt>
                </While>

                <Guidance:Pitch Id="HoldMassPosition">
                    <Parallel/>
                    <Setting><Guidance:Pitch.massPosition/><Control:VerticalControl.massDefault/></Setting>
                </Guidance:Pitch>

                <Guidance:Buoyancy Id="HoldBuoyancyPosition">
                    <Parallel/>
                    <Setting><Guidance:Buoyancy.position/><Control:VerticalControl.buoyancyNeutral/></Setting>
                </Guidance:Buoyancy>

                <Guidance:Point Id="HoldRudderAngle">
                    <Setting><Guidance:Point.rudderAngle/><Units:degree/><Value>9</Value></Setting>
                </Guidance:Point>

            </Aggregate>

            <Aggregate Id="GrabSamples">

                <Sequence/>

                <Aggregate Id="Sample1">

                    <Break>
                        <IsNaN><Arg Name="ESPCartridgeType_1"/>
                        </IsNaN>
                    </Break>

                    <Assign><Sequence/><Arg Name="ESPCartridgeType"/><Arg Name="ESPCartridgeType_1"/></Assign>

                    <Call RefId="SampleESP"/>

                </Aggregate>

                <Aggregate Id="Sample2">

                    <Break>
                        <IsNaN><Arg Name="ESPCartridgeType_2"/>
                        </IsNaN>
                    </Break>

                    <Assign><Sequence/><Arg Name="ESPCartridgeType"/><Arg Name="ESPCartridgeType_2"/></Assign>

                    <Call RefId="SampleESP"/>

                </Aggregate>

                <Aggregate Id="Sample3">

                    <Break>
                        <IsNaN><Arg Name="ESPCartridgeType_3"/>
                        </IsNaN>
                    </Break>

                    <Assign><Sequence/><Arg Name="ESPCartridgeType"/><Arg Name="ESPCartridgeType_3"/></Assign>

                    <Call RefId="SampleESP"/>

                </Aggregate>

                <Aggregate Id="Sample4">

                    <Break>
                        <IsNaN><Arg Name="ESPCartridgeType_4"/>
                        </IsNaN>
                    </Break>

                    <Assign><Sequence/><Arg Name="ESPCartridgeType"/><Arg Name="ESPCartridgeType_4"/></Assign>

                    <Call RefId="SampleESP"/>

                </Aggregate>

            </Aggregate>

        </Aggregate>

    </Aggregate>

<!-- end of sampling phase -->

</Mission>
