<?xml version="1.0" encoding="UTF-8"?>
<Mission xmlns="Tethys"
       xmlns:Sensor="Tethys/Sensor" 
       xmlns:Units="Tethys/Units"
       xmlns:Estimation="Tethys/Estimation"
       xmlns:Dock="Tethys/Dock"       
       xmlns:Guidance="Tethys/Guidance"
       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/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="DockingTankLineCaptureTest">

   <Description>
   	    First make the following adjustment to Insert/LineCapture
   	    -    <Assign><Sequence/><Arg Name="GuidanceMode"/><Arg Name="DetachFromDock"/></Assign>
        +    <Assign><Sequence/><Arg Name="GuidanceMode"/><Arg Name="Midcourse"/></Assign>
        Add the specific configs to dock.cfg and then the mission will:
        1.) Wait timeout for loading in launcher.
        2.) Turn on prop and arm for docking if the range is low enough.
        4.) Cable present == latch for timeout then release, float up, and end mission.
    </Description>

    <DefineArg Name="MissionTimeout"><Description>
        Maximum length of mission
    </Description><Units:minute/><Value>7</Value></DefineArg>
        
    <DefineOutput Name="DockingDepth"><Description>
        How deep in the tank do you want to swim?
    </Description><Units:meter/><Value>1.22</Value></DefineOutput>    
        
    <DefineArg Name="WaitForLoadTimeout"><Description>
        Time to wait prior to turning on the prop.
    </Description><Units:second/><Value>9</Value></DefineArg>    

    <DefineArg Name="ElevatorDefault"><Description>
        Static setting for elevator.
    </Description><Units:degree/><Value>-4.0</Value></DefineArg>
    
    <DefineArg Name="RudderDefault"><Description>
        Static setting for rudder.
    </Description><Units:degree/><Value>3.0</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="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>5</Value></DefineArg>

    <DefineArg Name="RangeUSBL"><Description>
        USBL mode range.
    </Description><Units:meter/><Value>40</Value></DefineArg>
    
    <DefineArg Name="MinAltitude"><Description>
        Minimum height above the sea floor for the entire mission.
    </Description><Units:meter/><Value>0.1</Value></DefineArg>

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


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

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

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

    <Timeout Duration="MissionTimeout"/>

        <!-- set up the vertical control -->
        <Guidance:Pitch>
            <Parallel/>
            <Setting><Guidance:Pitch.depth/><Arg Name="DockingDepth"/></Setting>
            <Setting><Guidance:Pitch.elevatorAngle/><Arg Name="ElevatorDefault"/></Setting>
            <Setting><Guidance:Pitch.massPosition/><Control:VerticalControl.massDefault/></Setting>
        </Guidance:Pitch>

        <!-- set up the horizontal control -->           
        <Guidance:Point>
            <Parallel/>
            <Setting><Guidance:Point.rudderAngle/><Arg Name="RudderDefault"/></Setting>
        </Guidance:Point>



    <Insert Filename="Insert/LineCapture.xml"/>
    <Assign><Sequence/><Arg Name="LineCapture:MinOffshore"/><Arg Name="MinOffshore"/></Assign>
    <Assign><Sequence/><Arg Name="LineCapture:MinAltitude"/><Arg Name="MinAltitude"/></Assign>

<!-- Add SetNav directives. -->

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

<!-- 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>
    </Estimation:TrackAcousticContact>




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

    </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>
                    <And><Not><Guidance:LineCapture.guidanceMode/><Eq><Units:count/><Value>5</Value></Eq></Not></And> <!-- don't enter USBL mode during rollout -->
                </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>

        </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>
                     <Or><Guidance:LineCapture.guidanceMode/><Eq><Units:count/><Value>5</Value></Eq></Or> <!-- do ATR mode during rollout -->
                </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>

        </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>
                <Or><Guidance:LineCapture.guidanceMode/><Eq><Units:count/><Value>5</Value></Eq></Or> <!-- do slow ping mode during rollout -->
            </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>

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

    </Aggregate>

<!-- Start mission sequence. -->

    <Aggregate Id="Lap">

        <Aggregate Id="Wait for loading">
        	  <Syslog Severity="Info">Waiting to load for <Arg Name="WaitForLoadTimeout"/><Units:second/></Syslog>
           
            <Guidance:Buoyancy>
                <Parallel/>
                <Setting><Guidance:Buoyancy.position/><Control:VerticalControl.buoyancyNeutral/></Setting>
            </Guidance:Buoyancy>
                	  
            <Guidance:Wait>
                <Sequence/>
                <Setting><Guidance:Wait.duration/><Arg Name="WaitForLoadTimeout"/></Setting>
            </Guidance:Wait>             
        </Aggregate>

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

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