<?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
        *RolloutTimeout* 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
        *RolloutTimeout* 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="CameraRange"><Description>
        Range to activate the camera payload.
    </Description><Units:meter/><Value>25.0</Value></DefineArg>

    <DefineArg Name="DockRange"><Description>
        Minimum range to continue to update heading being driven.
    </Description><Units:meter/><Value>4.0</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><Units:bool/><Value><False/></Value></DefineArg>

    <DefineArg Name="AcousticContactTimeout"><Description>
        If the vehicle does not receive an acoustic signal for more than this
        length of time, it will boot out of the homing sequance.
    </Description><Units:minute/><Value>15</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>
        Time duration for running in Dock mode.
    </Description><Units:second/><Value>60</Value></DefineArg>

    <DefineArg Name="RolloutTimeout"><Description>
        Drive on your last heading for this long after approaching within
        minimum (dock) range.
    </Description><Units:minute/><Value>12</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>NaN</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>30</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>0.1</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>1</Value></DefineArg>

    <DefineArg Name="DepthDeadband"><Units:meter/><Value>0.0</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>

    <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="CameraRequested"><Description>
        Mission variable (don't change). The mission sets this to true after
        camera is requested (prevents multipul req).
    </Description><False/></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="DetachFromDock"><Description>
        Mission variable (don't change). The mission will run in this mode to
        drift away from the dock after is unlatches.
    </Description><Units:count/><Value>3</Value></DefineOutput>

<!-- Mission timeout -->

    <Timeout Duration="MissionTimeout"/>

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

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

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

    <Aggregate Id="SurfaceComms">

        <When>
            <Called/>
            <Or><Elapsed><Universal:time_fix/></Elapsed><Gt><Arg Name="NeedCommsTime"/></Gt></Or>
        </When>

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

        <Call RefId="NeedComms"/>

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

    </Aggregate>

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

            <Description>
                Descend from the surface to initial depth. Necessary when no
                altitude reading is available form the surface.
            </Description>

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

            <Aggregate Id="dive">

                <Sequence/>

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

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

                <Guidance:Pitch>
                    <Sequence/>
                    <Timeout Duration="InitDepthTimeout">
                        <Syslog Severity="Important">Timed out at current depth of <Universal:depth/><Units:meter/>. Switching to altitude servo.</Syslog>
                    </Timeout>
                    <Setting><Guidance:Pitch.depth/><Arg Name="InitDepth"/></Setting>
                </Guidance:Pitch>

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

            </Aggregate>

        </Aggregate>

        <Aggregate Id="AltitudeServo">

            <Description>
                Follow commanded altitude.
            </Description>

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

            <Aggregate Id="dive">

                <Parallel/>

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

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

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

                <Guidance:AltitudeServo>
                    <Parallel/>
                    <Setting><Guidance:AltitudeServo.targetAltitude/><Arg Name="HoldAltitude"/></Setting>
                    <Setting><Guidance:AltitudeServo.invalidAltitudeTimeout/><Arg Name="InvalidAltitudeTimeout"/></Setting>
                </Guidance:AltitudeServo>

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

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

    <Aggregate Id="Lap">

        <Sequence Repeat="Repeat"/>

        <Aggregate Id="Dive">

            <While>
                <Not><Arg Name="DiveMode"/><Eq><Arg Name="DoingComms"/></Eq></Not>
            </While>

            <Call RefId="DiveCmd"/>

        </Aggregate>

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

        <Syslog Severity="Info">Transition to TerminalGuidance at range: <Estimation:TrackAcousticContact.range_to_contact/><Units:meter/>.</Syslog>

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

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

<!-- Even if we're getting acoustic contact, let's not just do this forever -->

            <Timeout Duration="TerminalGuidanceTimeout"/>

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

            <Guidance:Point>
                <Parallel/>
                <Setting><Guidance:Point.forceUpdate/><True/></Setting>
<!-- keep adjusting the heading -->
                <Setting><Guidance:Point.heading/><Estimation:TrackAcousticContact.heading_to_contact/></Setting>
            </Guidance:Point>

            <Aggregate Id="ActivateCamera">

                <When>
                    <Not><Arg Name="CameraRequested"/></Not>
                    <And><Estimation:TrackAcousticContact.range_to_contact/><Le><Arg Name="CameraRange"/></Le></And>
                </When>

                <Syslog Severity="Important">Lights, Camera on at range: <Estimation:TrackAcousticContact.range_to_contact/><Units:meter/>.</Syslog>

                <ReadData Strategy="MinError">
                    <Sensor:PowerOnly.samplePowerOnly/>
                </ReadData>

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

            </Aggregate>

<!-- Keep running terminal guidance until we're within dock range (or until we time out) -->

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

        </Aggregate>

        <Syslog Severity="Info">Transition to dock at range: <Estimation:TrackAcousticContact.range_to_contact/><Units:meter/>.</Syslog>

        <Aggregate Id="Dock">

            <Description>
                Final approach. Close in on the target at the last commanded
                heading until *DockTimeout* has passed. No DUSBL heading
                updates.
            </Description>

            <Sequence/>

<!-- No reson to dock if we haven't come within camara range -->

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

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

            <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="DockTimeout" /></Setting>
            </Guidance:Wait>

            <Aggregate Id="BreakOffDock">

                <Sequence/>

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

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

                <Aggregate Id="WaitForBreakOff">

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

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

                </Aggregate>

            </Aggregate>

        </Aggregate>

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

<!-- 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 *RolloutTimeout* has passed.
            </Description>

            <Sequence/>

            <Assign><Sequence/><Arg Name="CameraRequested"/><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>

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

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

            </Aggregate>

        </Aggregate>

    </Aggregate>

    <Call Id="PhoneHome" RefId="SurfaceComms"/>

</Mission>
