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

    <Description>
        This mission takes ESP samples at the designated depth.
    </Description>

    <DefineArg Name="MissionTimeout"><Description>
        Maximum duration of mission.
    </Description><Units:hour/><Value>4</Value></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="Speed"><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="ApproachDepth"><Description>
        Depth for initial descent form the surface. Initialized to 3 m.
    </Description><Units:meter/><Value>1.5</Value></DefineArg>

    <DefineArg Name="ApproachSpeed"><Description>
        Vehicle speed for initial descent form the surface to target depth. Set
        to zero to disable the thruster and descend using VBS (drift).
    </Description><Units:meter_per_second/><Value>0.9</Value></DefineArg>

    <DefineArg Name="ApproachDepthRate"><Description>
        Descent depth rate (Positive depth rate means going down).
    </Description><Units:meter_per_second/><Value>0.05</Value></DefineArg>

    <DefineArg Name="ApproachPitchLimit"><Description>
        Max vehicle pitch (+/-) when decending from the surface.
    </Description><Units:degree/><Value>10</Value></DefineArg>

    <DefineArg Name="ApproachSettleTimePreDive"><Description>
        Time duration for the vehicle to pump down the VBS *BEFORE* leaving the
        surface.
    </Description><Units:minute/><Value>1.5</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="ApproachDepthTimeout"><Description>
        Maximum time duration for the vehicle to reach the target depth.
    </Description><Units:minute/><Value>15</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>

<!-- 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="InitialDive">

        <Description>
            Descend from the surface to approach depth at ApproachSpeed, limit
            pitch, depth-rate, and hold the rudder.
        </Description>

        <Break>
            <Arg Name="ApproachSpeed"/>
            <Eq><Units:meter_per_second/><Value>0.0</Value></Eq>
        </Break>
            
        <Sequence/>

        <Assign><Parallel/><Control:VerticalControl.pitchLimit/><Arg Name="ApproachPitchLimit"/></Assign>

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

        <Guidance:Pitch>
            <Parallel/>
            <Setting><Guidance:Pitch.massPosition/><Control:VerticalControl.massDefault/></Setting>
        </Guidance:Pitch>

        <Guidance:Point>
            <Parallel/>
            <Setting><Guidance:Point.rudderAngle/><Units:degree/><Value>9</Value></Setting>
        </Guidance:Point>

        <Guidance:Buoyancy>
            <Parallel/>
            <Setting><Guidance:Buoyancy.position/><Control:VerticalControl.buoyancyNeutral/></Setting>
        </Guidance:Buoyancy>

        <Syslog Severity="Info">Waiting for <Arg Name="ApproachSettleTimePreDive"/><Units:minute/>while pumping down to neutral.</Syslog>

        <Guidance:Wait>
            <Description>
                Take a few moments to ensure the VBS reaches neutral.
            </Description>
            <Sequence/>
            <Setting><Guidance:Wait.duration/><Arg Name="ApproachSettleTimePreDive"/></Setting>
        </Guidance:Wait>

        <Syslog>Moving to <Arg Name="ApproachDepth"/><Units:meter/></Syslog>

        <Aggregate Id="ApproachDepth">

            <Sequence/>

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

            <Guidance:Pitch>
                <Sequence/>
                <Timeout Duration="ApproachDepthTimeout">
                    <Syslog Severity="Important">Timed out while trying to reach target depth at current depth of <Universal:depth/><Units:meter/>.</Syslog>
                    <Guidance:Execute>
                        <Sequence/>
                        <Setting><Guidance:Execute.command/><String>stop</String></Setting>
                    </Guidance:Execute>
                </Timeout>
                <Setting><Guidance:Pitch.depth/><Arg Name="ApproachDepth"/></Setting>
                <Setting><Guidance:Pitch.depthRate/><Arg Name="ApproachDepthRate"/></Setting>
            </Guidance:Pitch>

        </Aggregate>

    </Aggregate>

    <Aggregate Id="SettleAtDepth">

        <Description>
            Take a few more 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="Speed"/></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="Speed"/>
                <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>

</Mission>
