<?xml version="1.0" encoding="UTF-8"?>
<Mission xmlns="Tethys"
       xmlns:Control="Tethys/Control"
       xmlns:Derivation="Tethys/Derivation"
       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/Derivation http://okeanids.mbari.org/tethys/Xml/Derivation.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="circle_acoustic_backseat">

    <Description>
        This mission runs a circle around a specified acoustic contact. The
        tracking behavior periodically queries the DAT for range and direction
        to the contact, and the DAT acoustically interrogates the contact. The
        tracking component then calculates the position of the contact to update
        the center of the circle. When the vehicle surfaces and updates its own
        position with GPS, it queries the contact again, then sends the contact
        position to shore.
    </Description>

<!-- Mission-wide settings -->

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

    <DefineArg Name="NeedCommsTime"><Description>
        Elapsed time after previous surface communications when vehicle will
        begin to ascend for additional surface communications
    </Description><Units:minute/><Value>10</Value></DefineArg>

    <DefineArg Name="AcousticContactTimeout"><Description>
        If the vehicle does not receive an acoustic signal for more than this
        length of time, it will surface for communications with shore.
    </Description><Units:minute/><Value>15</Value></DefineArg>

<!-- You probably need to change these. -->

    <DefineArg Name="ContactLabel"><Description>
        The acoustic address of the asset to be tracked
    </Description><Units:count/><Value>0</Value></DefineArg>

    <DefineArg Name="ContactDepth"><Description>
        The acoustic address of the asset to be tracked
    </Description><Units:meter/><Value>NaN</Value></DefineArg>

    <DefineArg Name="CenterLatitude"><Description>
        The latitude of the center of the circle. Fill this in to specify a
        start position, it will be overwritten once the vehicle receives an
        acoustic signal from the contact.
    </Description><Units:degree/><Value>36.81544</Value></DefineArg>

    <DefineArg Name="CenterLongitude"><Description>
        The longitude of the center of the circle. Fill this in to specify a
        start position, it will be overwritten once the vehicle receives an
        acoustic signal from the contact.
    </Description><Units:degree/><Value>-121.82506</Value></DefineArg>

    <DefineArg Name="CenterNorthingsDelta"><Description>
        Desired change in latitude to achieve, expressed as a distance.
    </Description><Units:meter/><Value>0</Value></DefineArg>

    <DefineArg Name="CenterEastingsDelta"><Description>
        Desired change in longitude to achieve, expressed as a distance.
    </Description><Units:meter/><Value>0</Value></DefineArg>

    <DefineArg Name="CircleRadius"><Description>
        Radius of circle around acoustic contact during wait time
    </Description><Units:meter/><Value>300</Value></DefineArg>

<!-- You probably do not need to change these. -->

    <DefineArg Name="TrackingUpdatePeriod"><Description>
        How long to wait between acoustic queries
    </Description><Units:second/><Value>15</Value></DefineArg>

    <DefineArg Name="NumberOfPings"><Description>
        Number of return pings to request with each acoustic query (more than 1
        will activate oneway mode)
    </Description><Units:count/><Value>1</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>10</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>

    <DefineArg Name="YoYoMinDepth"><Description>
        Minimum depth during yo-yo's.
    </Description><Units:meter/><Value>3</Value></DefineArg>

    <DefineArg Name="YoYoMaxDepth"><Description>
        Maximum depth during yo-yo's.
    </Description><Units:meter/><Value>33</Value></DefineArg>

    <DefineArg Name="YoYoMinAltitude"><Description>
        Min altitude while in yo-yo mode.
    </Description><Units:meter/><Value>10</Value></DefineArg>

    <DefineArg Name="YoYoPitch"><Description>
        Pitch (plus and minus) for yo-yo behavior.
    </Description><Units:degree/><Value>20</Value></DefineArg>

    <DefineArg Name="Speed"><Description>
        Vehicle speed.
    </Description><Units:meter_per_second/><Value>1</Value></DefineArg>

<!-- You are even less likely to need to change these. -->

    <DefineArg Name="KwpHeading"><Description>
        Used to relax waypoint cross-track error constant that is adjusted for
        docking. (You can override this setting by passing an argument.)
    </Description><Units:radian_per_meter/><Value>0.010</Value></DefineArg>

    <DefineArg Name="MinAltitude"><Description>
        Minimum height above the sea floor for the entire mission.
    </Description><Units:meter/><Value>7</Value></DefineArg>

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

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

<!-- Missions should always start with a timeout. -->

    <Timeout Duration="MissionTimeout"/>

<!-- Add tracking directive high in the stack (above NeedComms to enable tracking from surface). -->

    <Insert Filename="Insert/AbortDrift.xml">
        <RedefineArg Name="AcousticTimeout"><Arg Name="AcousticContactTimeout"/></RedefineArg>
    </Insert>

    <Estimation:Tracking>
        <Parallel/>
        <Setting><Estimation:Tracking.contactDepthSetting/><Arg Name="ContactDepth"/></Setting>
        <Setting><Estimation:Tracking.contactLabelSetting/><Arg Name="ContactLabel"/></Setting>
        <Setting><Estimation:Tracking.numberOfSamplesSetting/><Arg Name="NumberOfPings"/></Setting>
        <Setting><Estimation:Tracking.updatePeriodSetting/><Arg Name="TrackingUpdatePeriod"/></Setting>
    </Estimation:Tracking>

    <Aggregate Id="UpdateCenter">

        <When>
            <Elapsed><CustomUri Uri="_.northingsDelta"/></Elapsed>
            <Lt><Elapsed><Arg Name="CenterNorthingsDelta"/></Elapsed></Lt>
            <Or><Elapsed><CustomUri Uri="_.eastingsDelta"/></Elapsed><Lt><Elapsed><Arg Name="CenterEastingsDelta"/></Elapsed></Lt></Or>
        </When>

        <Assign><Sequence/><Arg Name="CenterNorthingsDelta"/><CustomUri Uri="_.northingsDelta"/></Assign>

        <Assign><Sequence/><Arg Name="CenterEastingsDelta"/><CustomUri Uri="_.eastingsDelta"/></Assign>

        <Assign><Sequence/><Arg Name="CenterLatitude"/><Universal:latitude/></Assign>

        <Assign><Sequence/><Arg Name="CenterLongitude"/><Universal:longitude/></Assign>

        <Syslog Severity="Info">Upadating circle center <Arg Name="CenterLatitude"/><Units:arcdeg/>+<Arg Name="CenterNorthingsDelta"/><Units:meter/>/<Arg Name="CenterLongitude"/><Units:arcdeg/>+<Arg Name="CenterEastingsDelta"/><Units:meter/></Syslog>

    </Aggregate>

    <Aggregate Id="UpdateYoYoMaxDepth">

        <When>
            <Elapsed><CustomUri Uri="_.YoYoMaxDepth"/></Elapsed>
            <Lt><Elapsed><Arg Name="YoYoMaxDepth"/></Elapsed></Lt>
            <And><CustomUri Uri="_.YoYoMaxDepth"/><Lt><Arg Name="MaxDepth"/></Lt></And>
        </When>

        <Assign><Sequence/><Arg Name="YoYoMaxDepth"/><CustomUri Uri="_.YoYoMaxDepth"/></Assign>

        <Syslog Severity="Info">Upadating YoYoMaxDepth <CustomUri Uri="_.YoYoMaxDepth"/><Units:meter/></Syslog>

    </Aggregate>

    <Aggregate Id="UpdateYoYoMinDepth">

        <When>
            <Elapsed><CustomUri Uri="_.YoYoMinDepth"/></Elapsed>
            <Lt><Elapsed><Arg Name="YoYoMinDepth"/></Elapsed></Lt>
            <And><CustomUri Uri="_.YoYoMinDepth"/><Lt><Arg Name="MaxDepth"/></Lt></And>
        </When>

        <Assign><Sequence/><Arg Name="YoYoMinDepth"/><CustomUri Uri="_.YoYoMinDepth"/></Assign>

        <Syslog Severity="Info">Upadating YoYoMinDepth <CustomUri Uri="_.YoYoMinDepth"/><Units:meter/></Syslog>

    </Aggregate>

    <Aggregate Id="UpdateCircleRadius">

        <When>
            <Elapsed><CustomUri Uri="_.CircleRadius"/></Elapsed>
            <Lt><Elapsed><Arg Name="CircleRadius"/></Elapsed></Lt>
        </When>

        <Assign><Sequence/><Arg Name="CircleRadius"/><Abs><CustomUri Uri="_.CircleRadius"/></Abs></Assign>

        <Syslog Severity="Info">Upadating CircleRadius <CustomUri Uri="_.CircleRadius"/><Units:meter/></Syslog>

    </Aggregate>

<!-- Most missions have the NeedComms aggregate at high priority. -->

    <Insert Filename="Insert/NeedComms.xml" Id="NeedComms"/>

    <Assign><Sequence/><Arg Name="NeedComms:DiveInterval"/><Arg Name="NeedCommsTime"/></Assign>

<!-- Missions should almost always start with standard safety envelopes; most missions should not expose the parameters of these envelopes. -->

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

<!-- Power the science payload. -->

    <Insert Filename="Insert/Science.xml" Id="Science">
        <Description>
            Get science data, including PeakDetectChl
        </Description>
        <RedefineArg Name="PeakDetectChlActive"><True/></RedefineArg>
    </Insert>

<!-- Power the Backseat payload. -->

    <Guidance:BackseatDriver>
        <Parallel/>
    </Guidance:BackseatDriver>

    <ReadData>
        <Sensor:AHRS_M2.enableBroadcast/>
    </ReadData>

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

<!-- Check for additional instructions first. -->

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

<!-- Start mission sequence. -->

    <Guidance:Pitch>
        <Parallel/>
        <Setting><Guidance:Pitch.massPosition/><Control:VerticalControl.massDefault/></Setting>
    </Guidance:Pitch>

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

    <Aggregate Id="YoYos">

        <Parallel/>

        <Guidance:DepthEnvelope>
            <Parallel/>
            <Setting><Guidance:DepthEnvelope.minDepth/><Arg Name="YoYoMinDepth"/></Setting>
            <Setting><Guidance:DepthEnvelope.maxDepth/><Arg Name="YoYoMaxDepth"/></Setting>
            <Setting><Guidance:DepthEnvelope.pitch/><Arg Name="YoYoPitch"/></Setting>
        </Guidance:DepthEnvelope>

        <Guidance:AltitudeEnvelope>
            <Parallel/>
            <Setting><Guidance:AltitudeEnvelope.minAltitude/><Arg Name="YoYoMinAltitude"/></Setting>
            <Setting><Guidance:AltitudeEnvelope.pitch/><Arg Name="YoYoPitch"/></Setting>
        </Guidance:AltitudeEnvelope>

        <Guidance:YoYo>
            <Parallel/>
            <Setting><Guidance:YoYo.pitch/><Arg Name="YoYoPitch"/></Setting>
        </Guidance:YoYo>

    </Aggregate>

    <Aggregate Id="CircleWrapper">

        <Description>
            Circle the contact while updating its position using the backseat
            commands.
        </Description>

        <Sequence/>

        <Assign><Parallel/><Control:HorizontalControl.kwpHeading/><Arg Name="KwpHeading"/></Assign>

        <Guidance:Circle>
            <Parallel/>
            <Setting><Guidance:Circle.latitude/><Arg Name="CenterLatitude"/></Setting>
            <Setting><Guidance:Circle.longitude/><Arg Name="CenterLongitude"/></Setting>
            <Setting><Guidance:Circle.northingsDelta/><Arg Name="CenterNorthingsDelta"/></Setting>
            <Setting><Guidance:Circle.eastingsDelta/><Arg Name="CenterEastingsDelta"/></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>

</Mission>
