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

    <Description>
        Home to transponder while maintaining altitude. The mission sequence:
        (*) Start (1) Midcourse guidance: nav to target WP while querying
        transponder (2) Terminal guidance: home to target using DUSBL tracking
        data, fire up SCPI when in range (4) Dock: final approach using fixed
        heading (5) Rollout: break off from dock, then nav back to start wp or
        move away from the target at the last commanded heading (*) repeat Note:
        since there's a possibility that no altitude reading will be available
        form the surface (e.g., when the bottom is out of DVL range), the
        vehicle first descends from the surface to a commanded depth and then
        starts following the commanded altitude.
    </Description>

<!-- Define mission arguments -->

    <DefineArg Name="MissionTimeout"><Description>
        Maximum length of mission
    </Description><Units:hour/><Value>4</Value></DefineArg>

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

    <DefineArg Name="Repeat"><Description>
        Number of times the vehicle should try to cycle through the homing
        sequence.
    </Description><Units:count/><Value>1</Value></DefineArg>

    <DefineArg Name="DockLat"><Description>
        Latitude of docking station waypoint.
    </Description><Units:degree/><Value>36.7436</Value></DefineArg>

    <DefineArg Name="DockLon"><Description>
        Longitude of docking station waypoint.
    </Description><Units:degree/><Value>-121.8800</Value></DefineArg>

    <DefineArg Name="StartLat"><Description>
        Latitude of docking sequance start waypoint. The vehicle navigates back
        to this waypoint at the end of each homing attempt. If set to NaN, the
        vehicle moves away from the target at the last commanded heading until
        *RolloutTime* has passed.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="StartLon"><Description>
        Longitude of docking sequance start waypoint. The vehicle navigates back
        to this waypoint at the end of each homing attempt. If set to NaN, the
        vehicle moves away from the target at the last commanded heading until
        *RolloutTime* has passed.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <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><Units:meter/><Value>650.0</Value></DefineArg>

    <DefineArg Name="ArmRange"><Description>
        Range to deploy the whiskers, open the latch, and activate the camera.
    </Description><Units:meter/><Value>40.0</Value></DefineArg>

    <DefineArg Name="DockRange"><Description>
        Range from dock to stop commanding heading.
    </Description><Units:meter/><Value>1.0</Value></DefineArg>

    <DefineArg Name="TerminalGuidanceTimeout"><Description>
        Maximum time duration for running in terminal guidance mode.
    </Description><Units:minute/><Value>30</Value></DefineArg>

    <DefineArg Name="DockTimeout"><Description>
        Maximum time duration for running in Dock lockout mode.
    </Description><Units:second/><Value>60</Value></DefineArg>

    <DefineArg Name="UpdateNav"><Description>
        When true, the mission updates the vehicle's position to DockLat/Lon
        when it comes within *DockRange* from the transponder.
    </Description><False/></DefineArg>

    <DefineArg Name="WaitOnDockTimeout"><Description>
        Time duration to wait on the dock after a successful line capture.
    </Description><Units:second/><Value>90</Value></DefineArg>

    <DefineArg Name="BreakOffRange"><Description>
        Range from dock in which the vehicle is considered undocked. IMPORTANT:
        When undocking the vehicle is commanded to drift to InitDepth. Thus,
        BreakOffRange must be *less* than the vertical distance between the dock
        transponder and InitDepth.
    </Description><Units:meter/><Value>0.0</Value></DefineArg>

    <DefineArg Name="RolloutDistance"><Description>
        Distance to move away from the target (rollout) in response to missing
        the target.
    </Description><Units:meter/><Value>150</Value></DefineArg>

    <DefineArg Name="RolloutTime"><Description>
        Drive on your last heading for this long after detaching from the dock.
        This argument is *ignored* if start Lat/Lon are specified.
    </Description><Units:minute/><Value>5</Value></DefineArg>

    <DefineArg Name="HoldAltitude"><Description>
        Desired vehicle altitude to maintain.
    </Description><Units:meter/><Value>7</Value></DefineArg>

    <DefineArg Name="InvalidAltitudeTimeout"><Description>
        The mission will abort if no valid altitude readings are received within
        this specified time duration. Set this variable to NaN to disable this
        timeout functionality.
    </Description><Units:minute/><Value>15</Value></DefineArg>

    <DefineArg Name="InitDepth"><Description>
        Initial depth. The vehicle will dive to this depth when leaving the
        surface. Necessary if no altitude reading is available form the surface
        (e.g., when the bottom is out of DVL range).
    </Description><Units:meter/><Value>20</Value></DefineArg>

    <DefineArg Name="InitDepthTimeout"><Description>
        Maximum time duration for the vehicle to reach the initial depth. The
        vehicle will switch over to altitude following after the timeout is
        expired.
    </Description><Units:minute/><Value>30</Value></DefineArg>

    <DefineArg Name="TransponderCode"><Description>
        Transponder Address.
    </Description><Units:count/><Value>2</Value></DefineArg>

    <DefineArg Name="TrackingUpdatePeriod"><Description>
        How long to wait between acoustic queries.
    </Description><Units:second/><Value>2.5</Value></DefineArg>

    <DefineArg Name="NumberOfPings"><Description>
        Number of pings requested each time.
    </Description><Units:count/><Value>1</Value></DefineArg>

    <DefineArg Name="Speed"><Description>
        Thruster speed for the entier mission.
    </Description><Units:meter_per_second/><Value>0</Value></DefineArg>

    <DefineArg Name="DepthDeadband"><Description>
        Vertical drift from the specified depth that's allowed during altitude
        keeping (alt. is projected to depth).
    </Description><Units:meter/><Value>0.01</Value></DefineArg>

    <DefineArg Name="KpHeading"><Description>
        Heading proportional gain to use during Terminal Homing.
    </Description><Units:none/><Value>0.8</Value></DefineArg>

    <DefineArg Name="KiHeading"><Description>
        Heading Integral gain to use during Terminal Homing.
    </Description><Units:reciprocal_second/><Value>0.002</Value></DefineArg>

    <DefineArg Name="KpHeadingFinal"><Description>
        Heading proportional gain to use during Final Approach.
    </Description><Units:none/><Value>1.6</Value></DefineArg>

    <DefineArg Name="KiHeadingFinal"><Description>
        Heading Integral gain to use during Final Approach.
    </Description><Units:reciprocal_second/><Value>0.004</Value></DefineArg>

    <DefineArg Name="RudderDeadband"><Description>
        Degree of rounding in rudder command output values to use during
        Terminal Homing.
    </Description><Units:arcdeg/><Value>0.05</Value></DefineArg>

    <DefineArg Name="MinAltitude"><Description>
        Minimum height above the sea floor 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>205</Value></DefineArg>

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

    <DefineArg Name="Verbose"><Description>
        Send LineCapture/Undock syslog messages to shore.
    </Description><True/></DefineArg>

<!-- Mission Variables (don't change) -->

    <DefineOutput Name="HomingActive"><Description>
        Mission variable (don't change). The mission sets this to true when the
        vehicle is homing.
    </Description><True/></DefineOutput>

    <DefineOutput Name="Docked"><Description>
        Mission variable (don't change). The mission sets this to true to
        indicate a successful line capture.
    </Description><False/></DefineOutput>

    <DefineOutput Name="DockingDepth"><Description>
        Mission variable (don't change). The mission sets this to the vehicle's
        depth following a successful line capture.
    </Description><Units:meter/><Value>25</Value></DefineOutput>

    <DefineOutput Name="DiveMode"><Description>
        Mission variable (don't change). The mission sets this variable to
        switch between dive modes.
    </Description><Units:count/><Value>0</Value></DefineOutput>

    <DefineOutput Name="DoingComms"><Description>
        Mission variable (don't change). The mission will run in this to mode
        when running surface comms.
    </Description><Units:count/><Value>0</Value></DefineOutput>

    <DefineOutput Name="InitialDive"><Description>
        Mission variable (don't change). The mission will run in this mode when
        surface comms are done to dive the vehicle to DVL bottom range.
    </Description><Units:count/><Value>1</Value></DefineOutput>

    <DefineOutput Name="MaintainAltitude"><Description>
        Mission variable (don't change). The mission will run in this mode to
        dive the vehicle at a commanded altitude.
    </Description><Units:count/><Value>2</Value></DefineOutput>

    <DefineOutput Name="WaitOnDock"><Description>
        Mission variable (don't change). The mission will run in this mode to
        maintatin depth while the vehicle is latched on the dock.
    </Description><Units:count/><Value>3</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>4</Value></DefineOutput>

<!-- Mission timeout -->

    <Timeout Duration="MissionTimeout"/>

<!-- Add track acoustic contact directive high in the stack. -->

    <Aggregate Id="TrackTransponder">

        <While>
            <Arg Name="HomingActive"/>
        </While>

        <Estimation:TrackAcousticContact>
            <Parallel/>
            <Setting><Estimation:TrackAcousticContact.contactLabelSetting/><Arg Name="TransponderCode"/></Setting>
            <Setting><Estimation:TrackAcousticContact.numberOfSamplesSetting/><Arg Name="NumberOfPings"/></Setting>
            <Setting><Estimation:TrackAcousticContact.updatePeriodSetting/><Arg Name="TrackingUpdatePeriod"/></Setting>
        </Estimation:TrackAcousticContact>

    </Aggregate>

<!-- Set default mass position -->

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

<!-- Callable aggregates (priority matters, so these are done below important safety directives -->

    <Aggregate Id="DiveCmd">

        <Description>
            Called when the vehicle should be underwater. Arbitrates between 3
            dive modes: 1) descend from the surface to initial depth. 2) follow
            commanded altitude. 3) drift away from dock to initial depth.
        </Description>

        <When>
            <Called/>
        </When>

        <Aggregate Id="DockHold">

            <Description>
                Wait on dock after vehicle latches.
            </Description>

            <While>
                <Arg Name="DiveMode"/>
                <Eq><Arg Name="WaitOnDock"/></Eq>
            </While>

            <Aggregate Id="dive">

                <Parallel/>

                <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="DockingDepth"/></Setting>
                </Guidance:Pitch>

            </Aggregate>

        </Aggregate>

        <Aggregate Id="DriftAwayFromDock">

            <Description>
                Drift away from the dock after vehicle unlatches.
            </Description>

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

            <Aggregate Id="dive">

                <Sequence/>

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

                <Guidance:Pitch>
                    <Sequence/>
                    <Timeout Duration="InitDepthTimeout">
                        <Syslog Severity="Important">Timed out while trying to detach from dock 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="InitDepth"/></Setting>
                </Guidance:Pitch>

                <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="MaintainAltitude"/></Assign>

            </Aggregate>

        </Aggregate>

    </Aggregate>

<!-- Start mission sequence. -->

    <Aggregate Id="Lap">

        <Sequence Repeat="Repeat"/>

        <Syslog Severity="Important">Initiating homing sequence.</Syslog>

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

        <Aggregate Id="MidcourseGuidance">

            <Description>
                Navigate toward the dock waypoint while querying the dock
                transponder. Continue until you approach within *TerminalRange*,
                then move on to the next aggregate.
            </Description>

            <Sequence/>

            <Break>
                <Estimation:TrackAcousticContact.range_to_contact/>
                <Lt><Arg Name="TerminalRange"/></Lt>
            </Break>

            <Guidance:Waypoint>
                <Sequence/>
                <Setting><Guidance:Waypoint.latitude/><Arg Name="DockLat"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="DockLon"/></Setting>
            </Guidance:Waypoint>

        </Aggregate>

        <Aggregate Id="TerminalGuidance">

            <Description>
                Move toward the target using DUSBL tracking data and update the
                commanded heading with each cycle, activate SCPI when in range.
                Continue until you're within *DockRange*, then move on to the
                next aggregate.
            </Description>

            <Sequence/>

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

            <Syslog Severity="Important">Terminal guidance at range: <Estimation:TrackAcousticContact.range_to_contact/><Units:meter/>.</Syslog>

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

            <Guidance:LineCapture>
                <Sequence/>
                <Timeout Duration="TerminalGuidanceTimeout">
                    <Assign><Sequence/><Arg Name="Docked"/><False/></Assign>
                    <Syslog Severity="Important">Timed out while trying to dock at range: <Estimation:TrackAcousticContact.range_to_contact/><Units:meter/>.</Syslog>
                </Timeout>
                <Setting><Guidance:LineCapture.armRange/><Arg Name="ArmRange"/></Setting>
                <Setting><Guidance:LineCapture.interceptRange/><Arg Name="DockRange"/></Setting>
                <Setting><Guidance:LineCapture.interceptTimeout/><Arg Name="DockTimeout"/></Setting>
                <Setting><Guidance:LineCapture.rolloutDistance/><Arg Name="RolloutDistance"/></Setting>
                <Setting><Guidance:LineCapture.kpHeadingTerminalGuidance/><Arg Name="KpHeading"/></Setting>
                <Setting><Guidance:LineCapture.kiHeadingTerminalGuidance/><Arg Name="KiHeading"/></Setting>
                <Setting><Guidance:LineCapture.kpHeadingFinalApproach/><Arg Name="KpHeadingFinal"/></Setting>
                <Setting><Guidance:LineCapture.kiHeadingFinalApproach/><Arg Name="KiHeadingFinal"/></Setting>
                <Setting><Guidance:LineCapture.verbose/><Arg Name="Verbose"/></Setting>
            </Guidance:LineCapture>

            <Aggregate Id="Dock">

                <Sequence/>

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

<!-- Hold at the current depth while latched on the line -->

                <Assign><Sequence/><Arg Name="DockingDepth"/><Universal:depth/></Assign>

                <Syslog Severity="Important">Holding depth at: <Arg Name="DockingDepth"/><Units:meter/>.</Syslog>

                <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="WaitOnDock"/></Assign>

<!-- Dactivate homing while relaxing on the line -->

                <Assign><Sequence/><Arg Name="HomingActive"/><False/></Assign>

                <Aggregate Id="UpdateNav">

                    <Sequence/>

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

                    <Guidance:SetNav>
                        <Sequence/>
                        <Setting><Guidance:SetNav.latitude/><Arg Name="DockLat"/></Setting>
                        <Setting><Guidance:SetNav.longitude/><Arg Name="DockLon"/></Setting>
                    </Guidance:SetNav>

                </Aggregate>

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

                <Aggregate Id="BreakOffDock">

                    <Sequence/>

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

                    <Syslog Severity="Important">Breaking off. Range: <Estimation:TrackAcousticContact.range_to_contact/><Units:meter/>.</Syslog>

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

                    <Guidance:Undock>
                        <Sequence/>
                    </Guidance:Undock>

                    <Assign><Sequence/><Arg Name="Docked"/><False/></Assign>

                </Aggregate>

            </Aggregate>

        </Aggregate>

<!-- Homing sequance complete -->

        <Aggregate Id="Rollout">

            <Description>
                Navigate back to the start waypoint or move away from the target
                at the last commanded heading until *RolloutTime* has passed.
            </Description>

            <Sequence/>

            <Assign><Sequence/><Arg Name="Docked"/><False/></Assign>

            <Assign><Sequence/><Arg Name="HomingActive"/><False/></Assign>

<!-- Let's make sure we're in the right dive mode -->

            <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="MaintainAltitude"/></Assign>

            <Aggregate Id="NavToStart">

                <Sequence/>

                <Break>
                    <IsNaN><Arg Name="StartLat"/>
                    </IsNaN>
                    <Or><IsNaN><Arg Name="StartLon"/>
                    </IsNaN></Or>
                </Break>

                <Syslog Severity="Important">Rollout to start WP at range: <Estimation:TrackAcousticContact.range_to_contact/><Units:meter/>.</Syslog>

                <Guidance:Waypoint>
                    <Sequence/>
                    <Setting><Guidance:Waypoint.latitude/><Arg Name="StartLat"/></Setting>
                    <Setting><Guidance:Waypoint.longitude/><Arg Name="StartLon"/></Setting>
                </Guidance:Waypoint>

            </Aggregate>

            <Aggregate Id="MaintainHeading">

                <Sequence/>

                <Break>
                    <Not><IsNaN><Arg Name="StartLat"/>
                    </IsNaN></Not>
                    <Or><Not><IsNaN><Arg Name="StartLon"/>
                    </IsNaN></Not></Or>
                </Break>

                <Syslog Severity="Important">Rollout at range: <Estimation:TrackAcousticContact.range_to_contact/><Units:meter/>.</Syslog>

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

            </Aggregate>

        </Aggregate>

    </Aggregate>

</Mission>
