<?xml version="1.0" encoding="UTF-8"?>
<Aggregate xmlns="Tethys"
    xmlns:Control="Tethys/Control"
    xmlns:Dock="Tethys/Dock"
    xmlns:Estimation="Tethys/Estimation"
    xmlns:Guidance="Tethys/Guidance"
    xmlns:Sensor="Tethys/Sensor"
    xmlns:Servo="Tethys/Servo"
    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/Dock http://okeanids.mbari.org/tethys/Xml/Dock.xsd
                        Tethys/Estimation http://okeanids.mbari.org/tethys/Xml/Estimation.xsd
                        Tethys/Guidance http://okeanids.mbari.org/tethys/Xml/Guidance.xsd
                        Tethys/Sensor http://okeanids.mbari.org/tethys/Xml/Sensor.xsd
                        Tethys/Servo http://okeanids.mbari.org/tethys/Xml/Servo.xsd
                        Tethys/Units http://okeanids.mbari.org/tethys/Xml/Units.xsd
                        Tethys/Universal http://okeanids.mbari.org/tethys/Xml/Universal.xsd"
    Id="LineCapture">

    <Description>
        Home to transponder sequence: (0) Undock (1) Midcourse guidance: nav to
        dock WP while querying transponder, execute search pattern if WP is
        reached with no response from x-ponder. (2) Terminal guidance: home to
        target using DUSBL tracking data, reset nav, and dock
    </Description>

<!-- Dock -->

    <DefineArg Name="DockLat"><Description>
        Latitude of docking station waypoint (defaults to
        Config/workSite.beaconLat).
    </Description><Dock:Dock.lat/></DefineArg>

    <DefineArg Name="DockLon"><Description>
        Longitude of docking station waypoint (defaults to
        Config/workSite.beaconLon).
    </Description><Dock:Dock.lon/></DefineArg>

<!-- Undock arguments -->

    <DefineArg Name="DetachTimeout"><Description>
        Maximum time duration for the vehicle to detach form the dock. The
        vehicle will switch over to midcourse guidance after the timeout is
        expired.
    </Description><Units:minute/><Value>30</Value></DefineArg>

<!-- Midcourse guidance arguments -->

    <DefineArg Name="MidcourseDepth"><Description>
        Desired vehicle depth when navigating to dock WP (during midcourse
        phase).
    </Description><Dock:LineCapture.midcourseDepth/></DefineArg>

    <DefineArg Name="MidcourseSpeed"><Description>
        Thruster speed during Midcourse guidance.
    </Description><Dock:LineCapture.midcourseSpeed/></DefineArg>

    <DefineArg Name="MidcourseTimeout"><Description>
        Maximum time duration for running in midcourse guidance mode.
    </Description><Dock:LineCapture.midcourseTimeout/></DefineArg>

<!-- Terminal guidance arguments -->

    <DefineArg Name="TerminalRange"><Description>
        Range at which the vehicle should switch from navigating to the dock wp
        and start homing to the target using DUSBL tracking data. Max is ??
        meters due to DUSBL ???.
    </Description><Dock:LineCapture.terminalRange/></DefineArg>

    <DefineArg Name="AcousticTimeout"><Description>
        Maximum time duration allowed between pings during terminal guidance. If
        expired, the vehicle will regress to Midcourse guidance.
    </Description><Dock:LineCapture.acousticTimeout/></DefineArg>

    <DefineArg Name="losRateIIRFilterDecay"><Description>
        LineCapture single-pole IIR filter decay value.
        Filters line-of-sight (LOS) angle rotation rate.
        The Decay value should be btween 0 to 1, set to 0 disable filtering.
    </Description><Units:none/><Value>0.0</Value></DefineArg>

<!-- Search pattern arguments -->

    <DefineArg Name="SearchTimeout"><Description>
        Maximum time duration for running in search guidance mode.
    </Description><Dock:LineCapture.searchTimeout/></DefineArg>

    <DefineArg Name="CircleRadius"><Description>
        Radius of circle around the dock waypoint.
    </Description><Dock:LineCapture.searchCircleRadius/></DefineArg>

    <DefineArg Name="CircleMaxError"><Description>
        If this distance away from the circle, drive straight towards (or away
        from the center). Otherwise, try to reduce distance from the ideal
        circle.
    </Description><Units:meter/><Value>1</Value></DefineArg>

    <DefineArg Name="CircleTurnToPort"><Description>
        If true, vehicle turns to the left around the center point. If false,
        vehicle turns to the right.
    </Description><False/></DefineArg>

<!-- General settings -->

    <DefineArg Name="DepthDeadband"><Description>
        Vertical drift from the specified depth that's allowed during depth
        keeping.
    </Description><Units:meter/><Value>0.01</Value></DefineArg>

    <DefineArg Name="RudderDeadband"><Description>
        Degree of rounding in rudder command output values to use during
        Terminal Homing.
    </Description><Units:arcdeg/><Value>0.001</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 depth for the entire mission.
    </Description><Units:meter/><Value>8</Value></DefineArg>

    <DefineArg Name="MinOffshore"><Description>
        Minimum offshore distance for the entire mission.
    </Description><Units:meter/><Value>1</Value></DefineArg>
    
    <DefineArg Name="DockingAltitude"><Description>
        Docking altitude. Ignored when NaN and used over depth when a valid altitude is set.
    </Description><Units:meter/><Value>NaN</Value></DefineArg>

    <DefineArg Name="InvalidAltitudeTimeout"><Description>
        The mission will use the last good depth command if no valid altitude readings are received within
        this specified time duration. 
    </Description><Units:minute/><Value>NaN</Value></DefineArg>    
 
    <DefineArg Name="AltitudeDepthLimit"><Description>
        Vehicle will not be commanded deeper than this regardless of altitude command.
    </Description><Units:meter/><Value>NaN</Value></DefineArg>

    <DefineArg Name="AltitudeIIRFilterDecay"><Description>
        AltitudeServo single-pole IIR filter decay value.
        Low-pass filters the AltitudeServo depth command.
        The Decay value should be btween 0 to 1, set to 0 disable filtering.
    </Description><Units:none/><Value>0.0</Value></DefineArg>
    
<!-- Mission Variables (don't change!) -->

    <DefineOutput Name="DoneTimeout"><Description>
        Mission variable (don't change). The mission will run in this mode when
        the insert timed out.
    </Description><Units:count/><Value>-1</Value></DefineOutput>

    <DefineOutput Name="Done"><Description>
        Mission variable (don't change). The mission will run in this mode when
        the insert is done (docked).
    </Description><Units:count/><Value>0</Value></DefineOutput>

    <DefineOutput Name="DetachFromDock"><Description>
        Mission variable (don't change). The mission will run in this mode to
        drift away from the dock after is unlatched.
    </Description><Units:count/><Value>1</Value></DefineOutput>

    <DefineOutput Name="Midcourse"><Description>
        Mission variable (don't change). The mission will run in this mode when
        approaching the dock under Midcourse guidance.
    </Description><Units:count/><Value>2</Value></DefineOutput>

    <DefineOutput Name="Terminal"><Description>
        Mission variable (don't change). The mission will run in this mode when
        approaching the dock under Terminal guidance.
    </Description><Units:count/><Value>3</Value></DefineOutput>

    <DefineOutput Name="GuidanceMode"><Description>
        Mission variable (don't change). The mission sets this variable to
        switch between guidance modes.
    </Description><Arg Name="DetachFromDock"/></DefineOutput>

    <DefineOutput Name="DockingDepth"><Description>
        Mission variable (don't change). Actual docking depth.
    </Description><Units:meter/><Value>1</Value></DefineOutput>

    <DefineArg Name="ElapsedRange"><Units:minute/><Value>0</Value></DefineArg>

    <DefineArg Name="Range"><Units:meter/><Value>NaN</Value></DefineArg>

    <When>
        <Called/>
    </When>

    <Assign><Sequence/><Arg Name="GuidanceMode"/><Arg Name="DetachFromDock"/></Assign>

    <Assign><Sequence/><Arg Name="DockingDepth"/><Dock:Dock.depth/><Add><Dock:Docked.dockingDepthOffset/></Add></Assign>

<!-- Safety nets -->

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

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

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

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

<!-- Update internal range arguments -->

    <Assign><Parallel/><Arg Name="ElapsedRange"/><Elapsed><Estimation:TrackAcousticContact.range_to_contact/></Elapsed></Assign>

    <Assign><Parallel/><Arg Name="Range"/><Estimation:TrackAcousticContact.range_to_contact/></Assign>

<!-- Set default mass and buoyancy positions -->

    <Aggregate Id="HoldMassAndBuoyancy">

        <Description>
            Maintain VB and mass at neutral/default position while underway
            (during Midcourse and Terminal guidance).
        </Description>

        <While>
            <Abs><Control:SpeedControl.speedCmd/></Abs>
            <Gt><Units:meter_per_second/><Value>0</Value></Gt>
        </While>

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

        <Guidance:Mass>
            <Parallel/>
            <Setting><Guidance:Mass.position/><Control:VerticalControl.massDefault/></Setting>
        </Guidance:Mass>

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

    </Aggregate>

    <Aggregate Id="HomingSequence">

        <Description>
            This is the insert's guidance section, which arbitrates between 3
            modes: 1) DetachFromDock: unlatch and drift away from the dock. 2)
            Midcourse: navigate toward the dock waypoint, and enter search
            pattern if waypoint has been reached with no range data. 2)
            Terminal: home toward the dock using DUSBL tracking data.
        </Description>

        <Sequence/>

        <Break><Arg Name="GuidanceMode"/>
            <Le><Arg Name="Done"/></Le>
        </Break>

        <Aggregate Id="DetachFromDock">

            <Description>
                Unlatch and drift away from the dock.
            </Description>

            <While>
                <Arg Name="GuidanceMode"/>
                <Eq><Arg Name="DetachFromDock"/></Eq>
            </While>

            <Aggregate Id="guide">

                <Sequence/>

                <Dock:Undock>
                    <Sequence/>
                    <Timeout Duration="DetachTimeout">
                        <Syslog Severity="Important">Timed out while trying to detach from dock at current depth of <Universal:depth/><Units:meter/>.</Syslog>
                    </Timeout>
                </Dock:Undock>

                <Syslog Severity="Info">Transitioning to midcourse guidance.</Syslog>

                <Assign><Sequence/><Arg Name="GuidanceMode"/><Arg Name="Midcourse"/></Assign>

            </Aggregate>

        </Aggregate>

        <Aggregate Id="MidcourseGuidance">

            <Description>
                (1) Navigate toward the dock waypoint while querying the dock
                transponder. (2) Continue until within *TerminalRange*, then
                move on to terminal guidance. (3) Enter search pattern if
                waypoint has been reached with no range data.
            </Description>

            <While>
                <Arg Name="GuidanceMode"/>
                <Eq><Arg Name="Midcourse"/></Eq>
            </While>

            <Aggregate Id="guide">

                <Sequence/>

                <Syslog Severity="Info">Navigating to dock waypoint (<Arg Name="DockLat"/><Units:degree/>,<Arg Name="DockLon"/><Units:degree/>)</Syslog>

                <Aggregate Id="CheckRange">

                    <When>
                        <Arg Name="Range"/><Lt><Arg Name="TerminalRange"/></Lt>
                        <And><Arg Name="ElapsedRange"/><Le><Arg Name="AcousticTimeout"/></Le></And>
                    </When>

                    <Syslog Severity="Info">Transitioning to terminal guidance at range<Arg Name="Range"/><Units:meter/></Syslog>

                    <Assign><Sequence/><Arg Name="GuidanceMode"/><Arg Name="Terminal"/></Assign>

                </Aggregate>

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

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

                <Guidance:Waypoint>
                    <Sequence/>
                    <Timeout Duration="MidcourseTimeout">
                        <Syslog Severity="Important">Terminating homing sequence: timed out while navigating to the dock waypoint.</Syslog>
                        <Assign><Sequence/><Arg Name="GuidanceMode"/><Arg Name="DoneTimeout"/></Assign>
                    </Timeout>
                    <Setting><Guidance:Waypoint.latitude/><Arg Name="DockLat"/></Setting>
                    <Setting><Guidance:Waypoint.longitude/><Arg Name="DockLon"/></Setting>
                </Guidance:Waypoint>

                <Syslog Severity="Info">Executing search pattern.</Syslog>

                <Aggregate Id="SearchPattern">

                    <Sequence/>

                    <Timeout Duration="SearchTimeout">
                        <Syslog Severity="Important">Timed out while executing search pattern around dock waypoint.</Syslog>
                    </Timeout>

                    <Guidance:Circle>
                        <Parallel/>
                        <Setting><Guidance:Circle.latitude/><Arg Name="DockLat"/></Setting>
                        <Setting><Guidance:Circle.longitude/><Arg Name="DockLon"/></Setting>
                        <Setting><Guidance:Circle.radius/><Arg Name="CircleRadius"/></Setting>
                        <Setting><Guidance:Circle.maxError/><Arg Name="CircleMaxError"/></Setting>
                        <Setting><Guidance:Circle.turnToPort/><Arg Name="CircleTurnToPort"/></Setting>
                    </Guidance:Circle>

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

                </Aggregate>

                <Syslog Severity="Important">Homing sequence timed out prior to docking.</Syslog>

                <Assign><Sequence/><Arg Name="GuidanceMode"/><Arg Name="DoneTimeout"/></Assign>

            </Aggregate>

        </Aggregate>

        <Aggregate Id="TerminalGuidance">

            <Description>
                Home toward the target using USBL tracking data.
            </Description>

            <While>
                <Arg Name="GuidanceMode"/>
                <Eq><Arg Name="Terminal"/></Eq>
            </While>

            <Aggregate Id="guide">

                <Sequence/>

                <Syslog Severity="Info">Terminal guidance.</Syslog>

                <Aggregate Id="CheckRange">

                    <When>
                        <Arg Name="Range"/><Ge><Arg Name="TerminalRange"/></Ge>
                        <Or><Arg Name="ElapsedRange"/><Gt><Arg Name="AcousticTimeout"/></Gt></Or>
                    </When>

                    <Syslog Severity="Important">Transitioning to midcourse guidance. Received last range of<Arg Name="Range"/><Units:meter/><Arg Name="ElapsedRange"/><Units:minute/>ago.</Syslog>

                    <Assign><Sequence/><Arg Name="GuidanceMode"/><Arg Name="Midcourse"/></Assign>

                </Aggregate>

                <Aggregate Id="altitudeControl">

                    <While>
                        <Not><IsNaN><Arg Name="DockingAltitude"/></IsNaN></Not>
                    </While>

                    <Guidance:AltitudeServo>
                        <Parallel/>
                        <Setting><Guidance:AltitudeServo.targetAltitude/><Arg Name="DockingAltitude"/></Setting>
                        <Setting><Guidance:AltitudeServo.initialDepth/><Arg Name="DockingDepth"/></Setting>
                        <Setting><Guidance:AltitudeServo.invalidAltitudeTimeout/><Arg Name="InvalidAltitudeTimeout"/></Setting>
                        <Setting><Guidance:AltitudeServo.maxDepthLimit/><Arg Name="AltitudeDepthLimit"/></Setting>
                        <Setting><Guidance:AltitudeServo.iirFilterDecay/><Arg Name="AltitudeIIRFilterDecay"/></Setting>
                    </Guidance:AltitudeServo>

                </Aggregate>

                <Aggregate Id="depthControl">

                    <While>
                        <IsNaN><Arg Name="DockingAltitude"/></IsNaN>
                    </While>

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

                </Aggregate>

                <Assign><Parallel/><Servo:RudderServo.deviationAngle/><Arg Name="RudderDeadband"/></Assign>

                <Assign><Parallel/><Control:HorizontalControl.rudDeadband/><Arg Name="RudderDeadband"/></Assign>

                <Dock:LineCapture>
                    <Sequence/>
                    <Setting><Dock:LineCapture.iirFilterDecay/><Arg Name="losRateIIRFilterDecay"/></Setting>
                </Dock:LineCapture>

                <Syslog Severity="Info">Docked. Homing sequence complete.</Syslog>

                <Assign><Sequence/><Arg Name="GuidanceMode"/><Arg Name="Done"/></Assign>

            </Aggregate>

        </Aggregate>

    </Aggregate>

    <Guidance:Wait>
        <Sequence/>
        <Setting><Guidance:Wait.duration/><Units:second/><Value>3</Value></Setting>
    </Guidance:Wait>

    <Syslog Severity="Info">GuidanceMode<Arg Name="GuidanceMode"/><Units:count/></Syslog>

</Aggregate>
