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

    <Description>
        Home to transponder while maintaining altitude or depth. Altitude gets
        priority if non nan. The mission sequence: (*) Start (1) Midcourse
        guidance: nav to target WP while querying transponder (2) Terminal
        guidance: home to target using USBL tracking data (4) Dock: final
        approach using fixed heading.
    </Description>

<!-- Define mission arguments -->

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

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

    <DefineArg Name="MicromodemCommsInterval"><Description>
        How often to trigger Umodem communications
    </Description><Units:minute/><Value>5</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="WaitOnDockTimeout"><Description>
        Time duration to wait on the dock after a successful line capture.
    </Description><Units:minute/><Value>5</Value></DefineArg>

    <DefineArg Name="TransponderCode"><Description>
        Transponder Address.
    </Description><Dock:Dock.transponderCode/></DefineArg>

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

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

    <DefineArg Name="TrackingNumFixesLowPassFilter"><Description>
        Median filter bin size. Filters the contact estimated lat/lon position.
        Prefer odd number, set to 1 to disable filter.
    </Description><Units:count/><Value>5</Value></DefineArg>

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

    <DefineArg Name="NumPingsUSBL"><Description>
        Number of pings requested while in USBL mode range.
    </Description><Units:count/><Value>1</Value></DefineArg>

    <DefineArg Name="RangeUSBL"><Description>
        USBL mode range.
    </Description><Units:meter/><Value>40</Value></DefineArg>

    <DefineArg Name="CameraRange"><Description>
        Range to activate the camera payload.
    </Description><Units:meter/><Value>50.0</Value></DefineArg>

    <DefineArg Name="TrackingNumFixesLowPassFilterUSBL"><Description>
        Median filter bin size. Filters the contact estimated lat/lon position.
        Prefer odd number, set to 1 to disable filter.
    </Description><Units:count/><Value>5</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="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="SpiralRudderAngle"><Description>
        Rudder angle to use while doing initial dive.
    </Description><Units:degree/><Value>13</Value></DefineArg>

    <DefineArg Name="ApproachDepthTimeout"><Description>
        Maximum wait time for the vehicle to reach the target depth on initial
        dive.
    </Description><Units:minute/><Value>20</Value></DefineArg>

    <DefineArg Name="Lat1"><Description>
        Latitude of rollout waypoint. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lon1"><Description>
        Longitude of rollout waypoint. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="CaptureRadius"><Description>
        How close to call the waypiont behavior satisfied. Nan uses default
        value.
    </Description><Units:meter/><Value>NaN</Value></DefineArg>

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

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

    <DefineArg Name="MidcourseSpeed"><Description>
        Speed during initial dive and waypoint.
    </Description><Dock:LineCapture.midcourseSpeed/></DefineArg>

    <DefineOutput Name="Mode"><Description>
        Mission variable (don't change). The mission sets this variable to
        reflect the termination status of the LineCapture insert.
    </Description><Units:count/><Value>-1</Value></DefineOutput>

    <DefineOutput Name="NumPings"><Description>
        Mission variable (don't change). The mission sets this variable to
        update the requested ping number and enter usbl mode.
    </Description><Units:count/><Value>1</Value></DefineOutput>

    <DefineOutput Name="LowPassFilterWinSize"><Description>
        Mission variable (don't change). The mission sets this variable to
        update the requested ping number and enter usbl mode.
    </Description><Units:count/><Value>1</Value></DefineOutput>

    <DefineOutput Name="LowPassFilterWinOverlap"><Description>
        Mission variable (don't change). The mission sets this variable to
        update the requested ping number and enter usbl mode.
    </Description><True/></DefineOutput>

    <DefineArg Name="TrackingPeriod"><Description>
        Mission variable (don't change). The mission sets this variable to
        update the how long to wait between acoustic queries.
    </Description><Units:second/><Value>60</Value></DefineArg>

<!-- Mission timeout -->

    <Timeout Duration="MissionTimeout"/>

    <Insert Filename="Insert/Science.xml">
        <Description>
            Most missions will run the science sensors. If you don't place this
            aggregate above NeedComms, science instruments get turned off on the
            last upcast and while floating on the surface.
        </Description>
    </Insert>

<!-- 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="NeedCommsTime"/></Assign>

    <Assign><Sequence/><Arg Name="NeedComms:WaitForPitchUp"/><Units:minute/><Value>0</Value></Assign>

<!-- Micromodem comms -->

    <Insert Filename="Insert/MicromodemComms.xml" Id="MicromodemComms">
        <RedefineArg Name="CommsInterval"><Arg Name="MicromodemCommsInterval"/></RedefineArg>
    </Insert>

<!-- Add LineCapture directives. -->

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

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

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

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

    <Assign><Sequence/><Arg Name="LineCapture:DockingAltitude"/><Arg Name="DockingAltitude"/></Assign>

    <Assign><Sequence/><Arg Name="LineCapture:InvalidAltitudeTimeout"/><Arg Name="InvalidAltitudeTimeout"/></Assign>

    <Assign><Sequence/><Arg Name="LineCapture:AltitudeDepthLimit"/><Arg Name="AltitudeDepthLimit"/></Assign>


<!-- Add track acoustic contact directive. -->

    <Assign><Sequence/><Arg Name="NumPings"/><Arg Name="NumberOfPings"/></Assign>

    <Estimation:TrackAcousticContact Id="TrackAC">
        <Parallel/>
        <Setting><Estimation:TrackAcousticContact.contactLabelSetting/><Arg Name="TransponderCode"/></Setting>
        <Setting><Estimation:TrackAcousticContact.numberOfSamplesSetting/><Arg Name="NumPings"/></Setting>
        <Setting><Estimation:TrackAcousticContact.updatePeriodSetting/><Arg Name="TrackingPeriod"/></Setting>
        <Setting><Estimation:TrackAcousticContact.numFixesLowPassSetting/><Arg Name="LowPassFilterWinSize"/></Setting>
        <Setting><Estimation:TrackAcousticContact.lowPassOverlapSetting/><Arg Name="LowPassFilterWinOverlap"/></Setting>
    </Estimation:TrackAcousticContact>

<!-- Add USBL mode directives. -->

    <Aggregate Id="UpdateNumPings_NotTerminal">

        <While>
            <Not><Arg Name="LineCapture:GuidanceMode"/><Eq><Arg Name="LineCapture:Terminal"/></Eq></Not>
            <And><Not><Arg Name="NumPings"/><Eq><Arg Name="NumberOfPings"/></Eq></Not></And>
        </While>

        <Syslog Severity="Info">No longer in Terminal. Resuming normal ranging with <Arg Name="NumberOfPings"/><Units:count/>pings.</Syslog>

        <Assign><Sequence/><Arg Name="NumPings"/><Arg Name="NumberOfPings"/></Assign>

        <Assign><Sequence/><Arg Name="LowPassFilterWinSize"/><Arg Name="TrackingNumFixesLowPassFilter"/></Assign>

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

    </Aggregate>

    <Aggregate Id="UpdateNumPings_Terminal">

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

        <Aggregate Id="UpdateNumPingsUSBL">

            <When>
                <Not><Arg Name="NumPings"/><Eq><Arg Name="NumPingsUSBL"/></Eq></Not>
                <And><Arg Name="LineCapture:Range"/><Lt><Arg Name="RangeUSBL"/></Lt><!-- don't enter USBL mode during rollout --><And><Not><Guidance:LineCapture.guidanceMode/><Eq><Units:count/><Value>5</Value></Eq></Not></And></And>
            </When>

            <Syslog Severity="Info">Entering USBL mode with <Arg Name="NumPingsUSBL"/><Units:count/>pings at <Arg Name="LineCapture:Range"/><Units:meter/>(mode <Guidance:LineCapture.guidanceMode/><Units:count/>).</Syslog>

            <Assign><Sequence/><Arg Name="NumPings"/><Arg Name="NumPingsUSBL"/></Assign>

            <Assign><Sequence/><Arg Name="LowPassFilterWinSize"/><Arg Name="TrackingNumFixesLowPassFilterUSBL"/></Assign>

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

        </Aggregate>

        <Aggregate Id="UpdateNumPingsATR">

            <When>
                <Not><Arg Name="NumPings"/><Eq><Arg Name="NumberOfPings"/></Eq></Not>
                <And><Arg Name="LineCapture:Range"/><Ge><Arg Name="RangeUSBL"/></Ge><!-- do ATR mode during rollout --><Or><Guidance:LineCapture.guidanceMode/><Eq><Units:count/><Value>5</Value></Eq></Or></And>
            </When>

            <Syslog Severity="Info">Resuming normal ranging with <Arg Name="NumberOfPings"/><Units:count/>pings <Arg Name="LineCapture:Range"/><Units:meter/>(mode <Guidance:LineCapture.guidanceMode/><Units:count/>).</Syslog>

            <Assign><Sequence/><Arg Name="NumPings"/><Arg Name="NumberOfPings"/></Assign>

            <Assign><Sequence/><Arg Name="LowPassFilterWinSize"/><Arg Name="TrackingNumFixesLowPassFilter"/></Assign>

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

        </Aggregate>

    </Aggregate>

    <Aggregate Id="UpdatePingUpdateRate_Midcourse">

        <When>
            <Not><Arg Name="TrackingPeriod"/><Eq><Arg Name="TrackingUpdatePeriodMidcourse"/></Eq></Not>
            <And><Arg Name="LineCapture:Range"/><Gt><Arg Name="LineCapture:TerminalRange"/></Gt><!-- do slow ping mode during rollout --><Or><Guidance:LineCapture.guidanceMode/><Eq><Units:count/><Value>5</Value></Eq></Or></And>
        </When>

        <Syslog Severity="Info">Entering Midcourse tracking update period <Arg Name="TrackingUpdatePeriodMidcourse"/><Units:second/>sec at<Arg Name="LineCapture:Range"/><Units:meter/>(mode <Guidance:LineCapture.guidanceMode/><Units:count/>).</Syslog>

        <Assign><Sequence/><Arg Name="TrackingPeriod"/><Arg Name="TrackingUpdatePeriodMidcourse"/></Assign>

        <Assign><Sequence/><Arg Name="LowPassFilterWinSize"/><Arg Name="TrackingNumFixesLowPassFilter"/></Assign>

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

    </Aggregate>

    <Aggregate Id="UpdatePingUpdateRate_Terminal">

        <When>
            <Not><Arg Name="TrackingPeriod"/><Eq><Arg Name="TrackingUpdatePeriodTerminal"/></Eq></Not>
            <And><Arg Name="LineCapture:Range"/><Le><Arg Name="LineCapture:TerminalRange"/></Le><And><Not><Guidance:LineCapture.guidanceMode/><Eq><Units:count/><Value>5</Value></Eq></Not></And><!-- don't enter fast ping mode during rollout --></And>
        </When>

        <Syslog Severity="Info">Entering Terminal tracking update period <Arg Name="TrackingUpdatePeriodTerminal"/><Units:second/>sec at<Arg Name="LineCapture:Range"/><Units:meter/>(mode <Guidance:LineCapture.guidanceMode/><Units:count/>).</Syslog>

        <Assign><Sequence/><Arg Name="TrackingPeriod"/><Arg Name="TrackingUpdatePeriodTerminal"/></Assign>

        <Assign><Sequence/><Arg Name="LowPassFilterWinSize"/><Arg Name="TrackingNumFixesLowPassFilter"/></Assign>

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

    </Aggregate>

<!-- Start mission sequence. -->

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

    <Aggregate Id="SpiralDive">

        <Sequence/>

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

        <Guidance:Point>
            <Parallel/>
            <Setting><Guidance:Point.rudderAngle/><Arg Name="SpiralRudderAngle"/></Setting>
        </Guidance:Point>

        <Guidance:Pitch>
            <Sequence/>
            <Timeout Duration="ApproachDepthTimeout">
                <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="MidcourseDepth"/></Setting>
        </Guidance:Pitch>

    </Aggregate>

    <Aggregate Id="Lap">

        <Sequence Repeat="Repeat"/>

        <Assign><Sequence/><Arg Name="Mode"/><Units:count/><Value>-99</Value></Assign>

<!-- Execute the LineCapture sequence. -->

        <Call RefId="LineCapture"/>

        <Assign><Sequence/><Arg Name="Mode"/><Arg Name="LineCapture:GuidanceMode"/></Assign>

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

        <Aggregate Id="ActivateCamera">

            <While>
                <Estimation:TrackAcousticContact.range_to_contact/>
                <Le><Arg Name="CameraRange"/></Le>
            </While>

            <ReadData Id="RequestCameraOn">
                <Sensor:DAT.deviceEnableRequested/>
            </ReadData>

            <Aggregate Id="Wpt1">

                <Sequence/>

                <Break>
                    <IsNaN><Arg Name="Lat1"/>
                    </IsNaN>
                    <Or><IsNaN><Arg Name="Lon1"/>
                    </IsNaN></Or>
                </Break>

                <Syslog Severity="Important">Driving to post-undock waypoint <Arg Name="Lat1"/><Units:degree/>,<Arg Name="Lon1"/><Units:degree/></Syslog>

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

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

                <Guidance:Waypoint>
                    <Sequence/>
                    <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat1"/></Setting>
                    <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon1"/></Setting>
                    <Setting><Guidance:Waypoint.captureRadius/><Arg Name="CaptureRadius"/></Setting>
                </Guidance:Waypoint>

            </Aggregate>
        
            <Aggregate Id="Drift">

                <Description>
                    Drift before next docking attempt
                </Description>

                <Sequence/>

                <When>
                    <IsNaN><Arg Name="Lat1"/>
                    </IsNaN><Or><IsNaN><Arg Name="Lon1"/>
                    </IsNaN></Or>
                </When>

                <Syslog Severity="Important">No post-undock waypoint, drifting for <Dock:LineCapture.rolloutTimeout/><Units:minute/></Syslog>

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

                <Guidance:Pitch>
                    <Parallel/>
                    <Setting><Guidance:Pitch.massPosition/><Control:VerticalControl.massDefault/></Setting>
                    <Setting><Guidance:Pitch.elevatorAngle/><Units:degree/><Value>0</Value></Setting>
                </Guidance:Pitch>

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

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

                <Guidance:Wait>
                    <Sequence/>
                    <Setting><Guidance:Wait.duration/><Dock:LineCapture.rolloutTimeout/></Setting>
                </Guidance:Wait>

            </Aggregate>
                    </Aggregate>

        <Aggregate Id="Docked">

            <Sequence/>

            <Break><Arg Name="Mode"/>
                <Lt><Units:count/><Value>0</Value></Lt>
            </Break>

            <Syslog Severity="Info">Waiting on dock for <Arg Name="WaitOnDockTimeout"/><Units:minute/></Syslog>
            
            <!-- Add SetNav directives. -->
            <Insert Filename="Insert/SetNavAcoustic.xml"/>   

            <Guidance:Docked Id="Docked">
                <Sequence/>
                <Timeout Duration="WaitOnDockTimeout"/>
            </Guidance:Docked>

            <Aggregate Id="Detach">

                <Sequence/>

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

                <Guidance:Undock Id="Undock">
                    <Sequence/>
                </Guidance:Undock>

            </Aggregate>

        </Aggregate>

    </Aggregate>

</Mission>
