<?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_portuguese_ledge">

    <Description>
        This mission runs a circle around a specified centroid. The tracking
        behavior periodically queries the DAT for range and direction to the
        contact, and the DAT acoustically interrogates the contact.
    </Description>

<!-- Mission-wide settings -->

    <DefineArg Name="MissionTimeout"><Description>
        Maximum duration of mission
    </Description><Units:hour/><Value>2.5</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>150</Value></DefineArg>

    <DefineArg Name="Depth"><Description>
        Mission depth.
    </Description><Units:meter/><Value>50</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.
    </Description><Units:degree/><Value>36.696777</Value></DefineArg>

    <DefineArg Name="CenterLongitude"><Description>
        The longitude of the center of the circle.
    </Description><Units:degree/><Value>-121.941224</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>218</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="Speed"><Description>
        Vehicle speed.
    </Description><Units:meter_per_second/><Value>1</Value></DefineArg>

    <DefineArg Name="SkipComms"><Description>
        Skip communications at start of mission.
    </Description><True/></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>


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

<!-- Power the Backseat payload and request LCM data broadcast. -->

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

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

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

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

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

    <ReadData>
        <Navigation:DeadReckonUsingMultipleVelocitySources.enableBroadcast/>
    </ReadData>

    <ReadData>
        <Estimation:Tracking.enableBroadcast/>
    </ReadData>

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

    <Aggregate Id="StartingMission">

        <Sequence/>

        <Break>
            <Arg Name="SkipComms"/>
        </Break>

<!-- DO NOT REMOVE the syslog entry below. The MissionManager seems to ignore the value of SkipComms, does not evaluate the Break statement properly, and always calls NeedComms. Adding the syslog call somehow fixes it. -->

        <Syslog Severity="Info">Checking for additional instructions before submerging.</Syslog>

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

    </Aggregate>


<!-- Start mission sequence. -->

    <Guidance:Mass>
        <Parallel/>
        <Setting><Guidance:Mass.position/><Control:VerticalControl.massDefault/></Setting>
    </Guidance:Mass>

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

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

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

    <Aggregate Id="CircleWrapper">

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