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

    <Description>
        Control depth within deadband, settle, and then estimate until
        measurements converge and report.
    </Description>

    <DefineArg Name="InsertDepth"><Description>
        Depth to run ballast and trim at.
    </Description><Units:meter/><Value>25</Value></DefineArg>

    <DefineArg Name="InsertApproachSpeed"><Description>
        Enables vehicle thruster at the commanded speed to reach target depth
        (set to 0 m/s to drift).
    </Description><Units:meter_per_second/><Value>0</Value></DefineArg>

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

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

    <DefineArg Name="InsertApproachSettleTimePreDive"><Description>
        Time duration for the vehicle to pump down the VBS *BEFORE* leaving the
        surface.
    </Description><Units:minute/><Value>0</Value></DefineArg>

    <DefineArg Name="InsertApproachDepthTimeout"><Description>
        Maximum wait time for the vehicle to reach the targeted depth.
    </Description><Units:minute/><Value>20</Value></DefineArg>

    <DefineArg Name="InsertSettleTime"><Description>
        How long to wait after reaching target depth before starting ballast and
        trim.
    </Description><Units:minute/><Value>10</Value></DefineArg>

    <DefineArg Name="EstTimeout"><Description>
        Timeout for ballast and trim estimation.
    </Description><Units:minute/><Value>45</Value></DefineArg>

    <DefineArg Name="MinEstTime"><Description>
        Minimum time to run ballast and trim estimation before determining
        convergence (must be shorter than timeout).
    </Description><Units:minute/><Value>10</Value></DefineArg>

    <DefineArg Name="MassEstErrorBound"><Description>
        Desired range of the estimated sample mean from the true mean statistic
        (smaller values take longer to converge).
    </Description><Units:millimeter/><Value>0.25</Value></DefineArg>

    <DefineArg Name="BuoyEstErrorBound"><Description>
        Desired range of the estimated sample mean from the true mean statistic
        (smaller values take longer to converge).
    </Description><Units:cubic_centimeter/><Value>15</Value></DefineArg>

    <DefineArg Name="EstConfidence"><Description>
        Confidence level that the estimator has reached the desired error range
        (larger values take longer to converge).
    </Description><Units:percent/><Value>99.9</Value></DefineArg>

    <DefineArg Name="InsertDepthDeadband"><Description>
        How much vertical drift from the specified depth is allowed.
    </Description><Units:meter/><Value>1</Value></DefineArg>

    <DefineArg Name="InsertMassDeadband"><Description>
        Degree of rounding in mass-shifter command output values.
    </Description><Units:millimeter/><Value>0.25</Value></DefineArg>

    <When>
        <Called/>
    </When>

    <Assign><Parallel/><Control:VerticalControl.kpPitchMass/><Units:ratio/><Value>0.005</Value></Assign>

    <Assign><Parallel/><Control:VerticalControl.kiPitchMass/><Units:reciprocal_second/><Value>0.00075</Value></Assign>

    <Assign><Parallel/><Control:VerticalControl.kdPitchMass/><Units:second/><Value>0</Value></Assign>

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

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

<!-- DO NOT REMOVE the syslog entry below. The MissionManager seems to ignore the value of InsertApproachSpeed and does not evaluate the SetSpeed statement properly. Adding the syslog call somehow fixes it. -->

    <Syslog Severity="Info">Going to target depth. Speed set to <Arg Name="InsertApproachSpeed"/><Units:meter_per_second/></Syslog>

    <Aggregate Id="GoToTargetDepth">
        <Sequence/>

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

        <Aggregate Id="ActuatorHold">

            <Description>
                Hold the positions of the mass, VBS, and rudder while going to
                the target depth with the thruster on.
            </Description>

            <While>
                <Arg Name="InsertApproachSpeed"/>
                <Gt><Units:meter_per_second/><Value>0</Value></Gt>
            </While>

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

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

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

        </Aggregate>

        <Aggregate Id="WaitPreDive">

            <Sequence/>

            <Break><Arg Name="InsertApproachSettleTimePreDive"/>
                <Le><Units:minute/><Value>0</Value></Le>
            </Break>

            <Syslog Severity="Info">Waiting for <Arg Name="InsertApproachSettleTimePreDive"/><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="InsertApproachSettleTimePreDive"/></Setting>
            </Guidance:Wait>

        </Aggregate>

        <Aggregate Id="ApproachDepth">

            <Sequence/>

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

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

            <Guidance:Pitch>
                <Sequence/>
                <Timeout Duration="InsertApproachDepthTimeout">
                    <Syslog Severity="Important">Timed out trying to reach the target depth. Stopping mission 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="InsertDepth"/></Setting>
                <Setting><Guidance:Pitch.depthRate/><Arg Name="InsertApproachDepthRate"/></Setting>
            </Guidance:Pitch>

        </Aggregate>

    </Aggregate>

    <Aggregate Id="BallastAndTrimAtTargetDepth">

        <Description>
            Execute ballast and trim estimation at target depth.
        </Description>

        <Sequence/>

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

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

        <Syslog Severity="Info">Waiting for the vehicle to settle. Depth = <Universal:depth/><Units:meter/></Syslog>

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

        <Syslog Severity="Info">Running ballast and trim. Depth = <Universal:depth/><Units:meter/></Syslog>

        <Estimation:BallastAndTrim>
            <Sequence/>
            <Setting><Estimation:BallastAndTrim.estimationTimeout/><Arg Name="EstTimeout"/></Setting>
            <Setting><Estimation:BallastAndTrim.minEstimationTime/><Arg Name="MinEstTime"/></Setting>
            <Setting><Estimation:BallastAndTrim.massErrorBound/><Arg Name="MassEstErrorBound"/></Setting>
            <Setting><Estimation:BallastAndTrim.buoyErrorBound/><Arg Name="BuoyEstErrorBound"/></Setting>
            <Setting><Estimation:BallastAndTrim.confidenceLevel/><Arg Name="EstConfidence"/></Setting>
        </Estimation:BallastAndTrim>

    </Aggregate>

</Aggregate>
