<?xml version="1.0" encoding="UTF-8"?>
<Mission xmlns="Tethys"
       xmlns:Control="Tethys/Control"
       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/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="homing_pursuit">

<!-- Define mission arguments -->

    <DefineArg Name="Speed"><Units:meter_per_second/><Value>1</Value></DefineArg>

    <DefineArg Name="MassDefault"><Control:VerticalControl.massDefault/></DefineArg>

    <DefineArg Name="BuoyancyNeutral"><Control:VerticalControl.buoyancyNeutral/></DefineArg>

    <DefineArg Name="MinRange"><Description>
        Minimum range to continue to update heading being driven.
    </Description><Units:meter/><Value>4</Value></DefineArg>

    <DefineArg Name="OnewayRange"><Description>
        Minimum range to continue to update heading being driven. Max is 120
        meters due to maximum sample output from modem in oneway mode.
    </Description><Units:meter/><Value>65.0</Value></DefineArg>

    <DefineArg Name="TimeoutDuration"><Description>
        Maximum length of mission
    </Description><Units:minute/><Value>40</Value></DefineArg>

    <DefineArg Name="PowerOnDATTimeoutDuration"><Description>
        Wait this long after turning on DAT before starting any behaviors that
        require DAT output.
    </Description><Units:minute/><Value>1</Value></DefineArg>

    <DefineArg Name="ClosePursuitTimeout"><Description>
        Run in OneWayMode for this long.
    </Description><Units:second/><Value>180</Value></DefineArg>

    <DefineArg Name="RolloutTimeout"><Description>
        Drive on your last heading for this long after approaching within
        minimum range.
    </Description><Units:minute/><Value>3</Value></DefineArg>

    <DefineArg Name="HomingDepth"><Description>
        Depth for homing.
    </Description><Units:meter/><Value>7</Value></DefineArg>

    <DefineArg Name="MaxDepth"><Description>
        Maximum depth for the entire mission.
    </Description><Units:meter/><Value>45</Value></DefineArg>

    <DefineArg Name="EnabledNeilBrown"><Description>
        Set either this or PeakDetectChlActive to True in order to sample from
        the Neil Brown CTD.
    </Description><CustomUri Uri="Config/Science.CTD_NeilBrown_enable"/></DefineArg>

<!-- TODO any other science sensors that should be active? -->

<!-- Begin the mission. -->

    <Timeout Duration="TimeoutDuration"/>

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

    <Guidance:AltitudeEnvelope>
        <Parallel/>
        <Setting><Guidance:AltitudeEnvelope.minAltitude/><Units:meter/><Value>7</Value></Setting>
    </Guidance:AltitudeEnvelope>

    <Guidance:DepthEnvelope>
        <Parallel/>
        <Setting><Guidance:DepthEnvelope.minDepth/><Units:meter/><Value>0</Value></Setting>
        <Setting><Guidance:DepthEnvelope.maxDepth/><Arg Name="MaxDepth"/></Setting>
    </Guidance:DepthEnvelope>

    <Guidance:OffshoreEnvelope>
        <Parallel/>
        <Setting><Guidance:OffshoreEnvelope.minOffshore/><Units:kilometer/><Value>1.0</Value></Setting>
    </Guidance:OffshoreEnvelope>

    <Insert Filename="Insert/Surface.xml" Id="SurfaceComms" />

<!-- Set default mass and pump positions -->

    <Guidance:Pitch>
        <Parallel/>
        <Setting><Guidance:Pitch.massPosition/><Arg Name="MassDefault"/></Setting>
    </Guidance:Pitch>

    <Guidance:Buoyancy>
        <Parallel/>
        <Setting><Guidance:Buoyancy.position /><Arg Name="BuoyancyNeutral"/></Setting>
    </Guidance:Buoyancy>

<!-- Request desired engineering data -->

    <ReadData Strategy="MinError">
        <Universal:height_above_sea_floor/>
    </ReadData>

    <ReadData Strategy="AllDevices">
        <Universal:platform_orientation/>
    </ReadData>

    <ReadData>
        <Sensor:DAT.range/>
    </ReadData>

<!-- Start actual engineering mission sequence. -->

    <Aggregate Id="PowerOnDAT">

        <Description>
            Give the DAT some time to start up before you ask for ranges from
            it.
        </Description>

        <Sequence/>

        <Assign><Parallel/><Sensor:DAT.onewayRequestedCmd/><Units:bool/><Value>0</Value></Assign>

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

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

    </Aggregate>

    <Aggregate Id="InitialPursuit">

        <Description>
            Move toward the target using normal two-way queries. (These will
            also provide the heading of the compass attached to the acoustic
            modem.) Update the commanded heading with
            *Universal:homing_target_azimuth* each cycle. Continue until you
            approach within *OnewayRange*, then move on to the next aggregate.
        </Description>

        <Sequence/>

        <Break>
            <Sensor:DAT.range/>
            <Lt><Arg Name="OnewayRange"/></Lt>
        </Break>

<!-- <Timeout Duration="InitialPursuitTimeout"/>-->

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

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

        <Aggregate Id="HeadingPursuit">

            <While>
                <Sensor:DAT.range/>
                <Gt><Arg Name="MinRange"/></Gt>
            </While>

            <Guidance:Point>
                <Parallel/>
                <Setting><Guidance:Point.heading /><Sensor:DAT.azimuth_vehicleFrame/></Setting>
            </Guidance:Point>

        </Aggregate>

    </Aggregate>

    <Aggregate Id="ClosePursuit">

        <Description>
            Move toward the target using fast one-way queries. (These will *NOT*
            provide the heading of the compass attached to the acoustic modem.)
            Update the commanded heading with *Universal:homing_target_azimuth*
            each cycle. Continue until you approach within *MinRange*, then move
            on to the next aggregate.
        </Description>

        <Sequence/>

        <Break>
            <Sensor:DAT.range/>
            <Lt><Arg Name="MinRange"/></Lt>
        </Break>

        <Timeout Duration="ClosePursuitTimeout"/>

        <Assign><Parallel/><Sensor:DAT.onewayRequestedCmd/><Units:bool/><Value>1</Value></Assign>

        <ReadData Strategy="MinError">
            <Sensor:SCPI.sampleSCPI/>
        </ReadData>

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

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

        <Aggregate Id="HeadingOneWay">

            <While>
                <Sensor:DAT.range/>
                <Gt><Arg Name="MinRange"/></Gt>
            </While>

            <Guidance:Point>
                <Parallel/>
                <Setting><Guidance:Point.heading /><Sensor:DAT.azimuth_vehicleFrame/></Setting>
            </Guidance:Point>

        </Aggregate>

    </Aggregate>

    <Aggregate Id="Rollout">

        <Description>
            Move away from the target at the last commanded heading until
            *RolloutTimeout* has passed. (DAT should still be reporting. Target
            acoustic modem may still be in Oneway mode.)
        </Description>

        <Sequence/>

        <Timeout Duration="RolloutTimeout"/>

        <ReadData>
            <Sensor:DAT.range/>
        </ReadData>

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

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

    </Aggregate>

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

</Mission>
