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

    <Description>
        Home to the sampling vehicle's transponder while maintaining depth. The mission sequence:
        (1) Midcourse guidance: nav to target WP while querying transponder 
        (2) Terminal guidance, fast: home to target using DUSBL tracking data 
        (3) Terminal guidance, slow: continue homing to target at a slower speed 
        (4) Drift: maintain depth with zero speed
    </Description>

<!-- Define mission arguments -->

<!-- Safety overrides -->

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

<!-- Likely-to-change mission arguments -->

<!-- Parameters for phase one: Midcourse: go to approximate lat,lon of sampling vehicle -->

    <DefineArg Name="Lat"><Description>
        Latitude of sampling vehicle waypoint.
    </Description><Units:degree/><Value>36.693509</Value></DefineArg>

    <DefineArg Name="Lon"><Description>
        Longitude of sampling vehicle waypoint.
    </Description><Units:degree/><Value>-121.936568</Value></DefineArg>

    <DefineArg Name="SamplingVehicleDepth"><Description>
        Depth maintained for the entire mission.
    </Description><Units:meter/><Value>30</Value></DefineArg>

    <DefineArg Name="DepthDeadband"><Units:meter/><Value>0.0</Value></DefineArg>

    <DefineArg Name="MidcourseSpeed"><Description>
        Thruster speed during Midcourse guidance.
    </Description><Units:meter_per_second/><Value>1</Value></DefineArg>

    <DefineArg Name="MidcourseTimeout"><Description>
        Maximum time duration for running in midcourse guidance mode.
    </Description><Units:hour/><Value>1</Value></DefineArg>

<!-- Search pattern arguments -->

    <DefineArg Name="CircleRadius"><Description>
        Radius of circle around the dock waypoint.
    </Description><Units:meter/><Value>25</Value></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>

<!-- Parameters for phase two: acoustic homing toward sampling vehicle -->

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

    <DefineArg Name="TerminalRange"><Description>
        Range at which the vehicle should switch from navigating to the target 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="FinalApproachSpeed"><Description>
        Thruster speed while close to the sampling vehicle.
    </Description><Units:meter_per_second/><Value>0.5</Value></DefineArg>

    <DefineArg Name="FinalApproachRange"><Description>
        Range-to-sampling-vehicle at which to slow way down.
    </Description><Units:meter/><Value>100.0</Value></DefineArg>

    <DefineArg Name="SuccessRange"><Description>
        Minimum range to continue to update heading being driven.
    </Description><Units:meter/><Value>15.0</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="AcousticTimeout"><Description>
        Maximum time duration allowed between pings during terminal guidance. If
        expired, the vehicle will regress to Midcourse guidance.
    </Description><Units:minute/><Value>5</Value></DefineArg>

<!-- Parameters for phase three: drift in place -->

<!-- Modem ID is hard coded due to a bug in SendDirect. -->

    <!-- <DefineArg Name="SamplingModemID"><Description>
        Modem ID for sampling vehicle.
    </Description><Units:enum/><Value>7</Value></DefineArg> -->

    <DefineArg Name="WaitForAckTimeout"><Description>
        How long to wait for message acknowledgement before restarting.
    </Description><Units:minute/><Value>5</Value></DefineArg>

    <DefineArg Name="HoldDuration"><Description>
        How long to drift in place after achieving target location.
    </Description><Units:minute/><Value>120</Value></DefineArg>


<!-- Internal range arguments -->

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

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

<!-- Mission variables -->

    <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="DesiredSpeed"><Description>
        Mission variable (don't change). The mission sets this variable to high
        or low speed during terminal guidance.
    </Description><Units:meter_per_second/><Value>0</Value></DefineOutput>

    <DefineOutput Name="AckReceived"><Description>
        Mission variable (don't change). An ACK flag with a timestamp used to detect
        when new ACK signals have arrived.
    </Description><False/></DefineOutput>

    <DefineOutput Name="TrueVar"><Description>
        A True boolean, defined as an arg because you can't directly place values in 
        a call to SendData.
    </Description><True/></DefineOutput>

<!-- Enum for "guidance mode" FSM states -->

    <DefineOutput Name="Abort"><Description>
        Mission variable (don't change). Terminate this mission.
    </Description><Units:count/><Value>0</Value></DefineOutput>

    <DefineOutput Name="Success"><Description>
        Mission variable (don't change). The mission will run in this mode when
        after successfully relieving the sampling vehicle.
    </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 sampling vehicle 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 sampling vehicle under Terminal guidance.
    </Description><Units:count/><Value>3</Value></DefineOutput>

    <DefineOutput Name="GuidanceMode"><Description>
        The guidance mode is the phase of hot-bunking, a member of the enum:
            Success
            Abort
            Midcourse
            Terminal
    </Description><Arg Name="Midcourse"/></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="NeedCommsTime"/></Assign>

    <Assign><Sequence/><Arg Name="NeedComms:WaitForPitchUp"/><Units:minute/><Value>0</Value></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>

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

<!-- Maintain depth -->

    <Aggregate Id="MaintainDepth">

        <Parallel/>

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

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

    </Aggregate>

<!-- Start mission sequence. -->

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

    <Aggregate Id="HotBunkHomingSequence">

        <Description>
            This is the guidance section, which arbitrates between 2 modes: 
            1) Midcourse: navigate toward the sampling vehicle's lat, lon waypoint 
            2) Terminal: home toward the sampling vehicle using DUSBL tracking data.
        </Description>

        <Sequence/>

        <Break>
            <Arg Name="GuidanceMode"/><Eq><Arg Name="Success"/></Eq>
            <Or><Arg Name="GuidanceMode"/><Eq><Arg Name="Abort"/></Eq></Or>
        </Break>

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

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

        <Aggregate Id="MidcourseGuidance">

            <Description>
                Navigate toward the target waypoint while querying the target
                transponder. Continue until you approach within *TerminalRange*,
                then move on to the next aggregate. If a DAT fix within TerminalRange
                is not acquired within MidcourseTimeout, the mission aborts.
            </Description>

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

            <Aggregate Id="guide">

                <Sequence/>

                <Syslog Severity="Info">Navigating to sampling vehicle waypoint (<Arg Name="Lat"/><Units:degree/>,<Arg Name="Lon"/><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="Important">Transitioning to terminal guidance at range<Arg Name="Range"/><Units:meter/></Syslog>

                    <Syslog Severity="Info">Setting GuidanceMode to Terminal</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:Waypoint>
                    <Sequence/>
                    <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat"/></Setting>
                    <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon"/></Setting>
                </Guidance:Waypoint>

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

                <Aggregate Id="SearchPattern">

                    <Sequence/>

                    <Timeout Duration="MidcourseTimeout">
                        <Syslog Severity="Important">Terminating homing sequence: did not achieve a DAT fix within TerminalRange before MidcourseTimeout.</Syslog>
                        <Syslog Severity="Important">Setting GuidanceMode to Abort</Syslog>
                        <Assign><Sequence/><Arg Name="GuidanceMode"/><Arg Name="Abort"/></Assign>
                    </Timeout>

                    <Guidance:Circle>
                        <Parallel/>
                        <Setting><Guidance:Circle.latitude/><Arg Name="Lat"/></Setting>
                        <Setting><Guidance:Circle.longitude/><Arg Name="Lon"/></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="MissionTimeout"/></Setting>
                    </Guidance:Wait>

                </Aggregate>

            </Aggregate>

        </Aggregate>

        <Aggregate Id="TerminalGuidance">

            <Description>
                Move toward the target using DUSBL tracking data and update the
                commanded heading with each cycle.
                Continue until you're within *SuccessRange*, then tell the sampling
                vehicle to go away.
            </Description>

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

            <Syslog Severity="Important">Entering Terminal guidance. </Syslog>

            <Aggregate Id="guide">

                <Sequence/>

                <Break>
                    <Arg Name="Range"/>
                    <Lt><Arg Name="SuccessRange"/></Lt>
                </Break>

                <Aggregate Id="AcousticTimeoutBehavior">

                    <When>
<!-- Due to a bug in the "When" MissionItem, we use the "True And" hack -->
                        <True/>
                        <And><Arg Name="ElapsedRange"/><Gt><Arg Name="AcousticTimeout"/></Gt></And>
                    </When>

                    <Syslog Severity="Important">Acoustic timeout! Received last range of<Arg Name="Range"/><Units:meter/><Arg Name="ElapsedRange"/><Units:minute/> ago.</Syslog>
                    <Syslog Severity="Important">Setting GuidanceMode to Midcourse</Syslog>
                    <Assign><Sequence/><Arg Name="GuidanceMode"/><Arg Name="Midcourse"/></Assign>

                </Aggregate>

                <Aggregate Id="SlowDownForFinalApproach">

                    <When>
                        <Arg Name="Range"/>
                        <Lt><Arg Name="FinalApproachRange"/></Lt>
                        <And><Arg Name="DesiredSpeed"/><Eq><Arg Name="MidcourseSpeed"/></Eq></And>
                    </When>

                    <Syslog Severity="Info">Setting speed to slow since range is <Arg Name="Range"/><Units:meter/>.</Syslog>
                    <Assign><Sequence/><Arg Name="DesiredSpeed"/><Arg Name="FinalApproachSpeed"/></Assign>        

                </Aggregate>

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

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

<!-- Keep running terminal guidance until we're within target range (or until AcousticTimeout) -->

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

            </Aggregate>

            <Aggregate Id="TellSamplingVehicleToGoAway">

                <Description>
                    Send a _.relievedOfDuty message to the sampling vehicle. If ACK is received,
                    enter the DONE state, otherwise regress to MidcourseGuidance.
                </Description>

                <Sequence/>

                <Syslog Severity="Important">Telling sampling vehicle to go away.</Syslog>

                <!-- Zero speed, turn off DAT homing, and wait 30 seconds. -->
                <Guidance:SetSpeed>
                    <Parallel/>
                    <Setting><Guidance:SetSpeed.speed/><Units:meter_per_second/><Value>0</Value></Setting>
                </Guidance:SetSpeed>

                <Syslog Severity="Important">Setting HomingActive to False.</Syslog>
                <Assign><Sequence/><Arg Name="HomingActive"/><False/></Assign>

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

                <Aggregate Id="RespondToAcousticMsg">

                    <When>
                        <Elapsed><Sensor:DAT.msgAcknowledged/></Elapsed><Lt><Elapsed><Arg Name="AckReceived"/></Elapsed></Lt>
                        <And><Sensor:DAT.msgAcknowledged/><Eq><True/></Eq></And>
                    </When>

                    <Syslog Severity="Important">Setting GuidanceMode to Success</Syslog>
                    <Assign><Sequence/><Arg Name="GuidanceMode"/><Arg Name="Success"/></Assign>
                    <Assign><Sequence/><Arg Name="AckReceived"/><Sensor:DAT.msgAcknowledged/></Assign>
            
                </Aggregate>

<!-- Send acoustic message to modem 8. This is hardcoded instead of an arg to avoid a segfault in argument resolution. -->
                <SendData Service="Direct" Destination="modem:8:_.relievedOfDuty">
                    <Arg Name="TrueVar"/>
                </SendData>

<!-- No response? Time out and restart hotbunking sequence. -->
                <Guidance:Wait>
                    <Sequence/>
                    <Setting><Guidance:Wait.duration/><Arg Name="WaitForAckTimeout"/></Setting>
                </Guidance:Wait>

                <Syslog Severity="Important">Setting GuidanceMode to Midcourse</Syslog>
                <Assign><Sequence/><Arg Name="GuidanceMode"/><Arg Name="Midcourse"/></Assign>
        
            </Aggregate>

        </Aggregate>

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

    </Aggregate>

    <Syslog Severity="Important">Transition to hold at range: <Arg Name="Range"/><Units:meter/>.</Syslog>

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

    <Aggregate Id="holdDepth">

        <Description>
            With zero speed, maintain the sampling depth using a Guidance:Pitch command.
        </Description>

        <Sequence/>

        <Break>
            <Arg Name="GuidanceMode"/><Ne><Arg Name="Success"/></Ne>
        </Break>

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

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

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

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

    </Aggregate>

</Mission>
