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

    <Description>
        This mission trackes an acoustic contact and keeps station in a defined
        radius. Lights switch between red/white every LightDeltaTime minutes.
    </Description>

<!-- Almost every mission should start with an overall timeout and a NeedCommsTime. Drift Missions will have an acoustic timeout instead of a NeedCommsTime. -->

<!-- Use caution when changing these. -->

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

    <DefineArg Name="NeedCommsTime"><Description>
        Surface for communications interval
    </Description><Units:minute/><Value>30</Value></DefineArg>

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

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

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

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

    <DefineArg Name="Depth"><Description>
        Depth held during drift mode
    </Description><Units:meter/><Value>30</Value></DefineArg>

    <DefineArg Name="ApproachDepth"><Description>
        Depth for initial approach to station.
    </Description><Units:meter/><Value>30</Value></DefineArg>

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

    <DefineArg Name="Radius"><Description>
        Radius around waypoint to hold.
    </Description><Units:meter/><Value>200</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>NaN</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>NaN</Value></DefineArg>

    <DefineArg Name="LightDeltaTime"><Description>
        Period of red versus white lights.
    </Description><Units:minute/><Value>5.0</Value></DefineArg>

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

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

    <DefineArg Name="TrackingUpdatePeriod"><Description>
        How long to wait between acoustic queries
    </Description><Units:second/><Value>60</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. Set longer than
        MissionTimeout to effectively disable.
    </Description><Units:hour/><Value>1</Value></DefineArg>

    <DefineArg Name="AcousticTargetDepth"><Description>
        Depth of acoustic target if known and fixed (or nearly fixed). For
        example, if the acoustic target is a Wave Glider, set it to zero. This
        will improve 2D projected position estimates in the Earth reference
        frame. Defaults to NaN.
    </Description><Units:meter/><Value>NaN</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="NumberOfFixesLowPass"><Description>
        Number of fixes to average to produce smoothed lat/lon/dep output
        (initialized to 4).
    </Description><Units:count/><Value>4</Value></DefineArg>

    <DefineArg Name="NumberOfStartingFixesToIgnore"><Description>
        Number of fixes to ignore at the start of mission (as the vehicle just
        leaves surface, contact's location estimate especailly bearing can be
        erroneous. Initialized to 8, corresponding to 2 minutes if query
        interval TrackingUpdatePeriod = 15 s.
    </Description><Units:count/><Value>8</Value></DefineArg>

    <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="DepthDeadband"><Description>
        How much vertical drift from the specified depth is allowed in drift
        mode
    </Description><Units:meter/><Value>4</Value></DefineArg>

<!-- Internal variables that you should not change -->

    <DefineOutput Name="ContactLatitudeLowpass"><Description>
        Low-pass filtered latitude of contact (initialized to NaN).
    </Description><Units:degree/><Value>NaN</Value></DefineOutput>

    <DefineOutput Name="ContactLongitudeLowpass"><Description>
        Low-pass filtered longitude of contact (initialized to NaN).
    </Description><Units:degree/><Value>NaN</Value></DefineOutput>

    <DefineOutput Name="CenterTouched"><Description>
        Flag to indicate center lon/lat was updated.
    </Description><False/></DefineOutput>

    <DefineOutput Name="LightsOff"><Description>
        Elapsed time from last time the light color was switched.
    </Description><Units:count/><Value>0</Value></DefineOutput>

    <DefineOutput Name="RedLight"><Description>
        Elapsed time from last time the light color was switched.
    </Description><Units:count/><Value>1</Value></DefineOutput>

    <DefineOutput Name="WhiteLight"><Description>
        Elapsed time from last time the light color was switched.
    </Description><Units:count/><Value>2</Value></DefineOutput>

    <DefineOutput Name="LightCmd"><Description>
        Indicates light color that should be turned on (default is red to start
        so we don't scare the fish).
    </Description><Units:count/><Value>0</Value></DefineOutput>

    <DefineOutput Name="DoingComms"><Description>
        Indicates vehicle is descending form the surface.
    </Description><True/></DefineOutput>

<!-- Missions should almost always start with a timeout and a NeedComms aggregate. Drift missions will not use NeedComms. -->

    <Timeout Duration="MissionTimeout"/>

<!-- Missions should almost always start with standard safety envelopes -->

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

    <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="MissionTimeout"/></Assign>

    <Assign><Sequence/><Arg Name="NeedComms:WaitForPitchUp"/><Units:minute/><Value>0</Value></Assign>

    <Aggregate Id="SurfaceComms">

        <When>
            <Called/>
            <Or><Elapsed><Universal:time_fix/></Elapsed><Gt><Arg Name="NeedCommsTime"/></Gt></Or>
        </When>

        <Assign><Sequence/><Arg Name="DoingComms"/><True/></Assign>

        <Call RefId="NeedComms"/>

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

    </Aggregate>

<!-- Many missions will keep mass position fixed at the default. -->

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

    <Estimation:Tracking Id="TrackingAcousticTarget">
        <Parallel/>
        <Setting><Estimation:Tracking.contactLabelSetting/><Arg Name="AcousticContactLabel"/></Setting>
        <Setting><Estimation:Tracking.numberOfSamplesSetting/><Arg Name="NumberOfPings"/></Setting>
        <Setting><Estimation:Tracking.updatePeriodSetting/><Arg Name="TrackingUpdatePeriod"/></Setting>
        <Setting><Estimation:Tracking.numFixesLowPassSetting/><Arg Name="NumberOfFixesLowPass"/></Setting>
        <Setting><Estimation:Tracking.numStartingFixesToIgnoreSetting/><Arg Name="NumberOfStartingFixesToIgnore"/></Setting>
        <Setting><Estimation:Tracking.contactDepthSetting/><Arg Name="AcousticTargetDepth"/></Setting>
    </Estimation:Tracking>

    <Aggregate Id="UpdateCenter">

        <When>
            <Elapsed><Estimation:Tracking.range_to_contact/></Elapsed>
            <Lt><Elapsed><Arg Name="CenterTouched"/></Elapsed></Lt>
        </When>

        <Assign><Sequence/><Arg Name="ContactLatitudeLowpass"/><Estimation:Tracking.contact_latitude_lowpass/></Assign>

        <Assign><Sequence/><Arg Name="ContactLongitudeLowpass"/><Estimation:Tracking.contact_longitude_lowpass/></Assign>

        <Aggregate Id="UpdateWithContactLowpassLatLon">

            <Sequence/>

            <Break>
                <IsNaN><Arg Name="ContactLatitudeLowpass"/>
                </IsNaN>
                <Or><IsNaN><Arg Name="ContactLongitudeLowpass"/>
                </IsNaN></Or>
            </Break>

            <Assign><Sequence/><Arg Name="CenterLatitude"/><Arg Name="ContactLatitudeLowpass"/></Assign>

            <Assign><Sequence/><Arg Name="CenterLongitude"/><Arg Name="ContactLongitudeLowpass"/></Assign>

            <Syslog Severity="Info">Updated center Lat/Lon = <Arg Name="CenterLatitude"/><Units:degree/>, <Arg Name="CenterLongitude"/><Units:degree/></Syslog>

            <Assign><Sequence/><Arg Name="CenterTouched"/><True/></Assign>

        </Aggregate>

    </Aggregate>

<!-- Hold the mass at the default position -->

    <Syslog Severity="Info">Hold the mass at the default position.</Syslog>

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

<!-- Mission start -->

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

    <Aggregate Id="TransitToStation">

        <Description>
            Need a separate aggregate for transit if we want to specify a
            different depth for the approach.
        </Description>

        <Sequence/>

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

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

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

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

        <Guidance:Waypoint Id="StationWpt">
            <Sequence/>
            <Setting><Guidance:Waypoint.latitude/><Arg Name="CenterLatitude"/></Setting>
            <Setting><Guidance:Waypoint.longitude/><Arg Name="CenterLongitude"/></Setting>
            <Setting><Guidance:Waypoint.captureRadius/><Arg Name="Radius"/></Setting>
        </Guidance:Waypoint>

    </Aggregate>

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

    <Aggregate Id="WorkOnStation">

        <Sequence/>

        <Aggregate>

            <While>
                <Not><Arg Name="DoingComms"/></Not>
            </While>

            <Aggregate Id="InitialDescent">

                <Description>
                    Descend from the surface to initial depth prior to turning
                    on the lights/camera.
                </Description>

                <Sequence/>

                <Syslog Severity="Info">Initial descent at <Universal:depth/><Units:meter/></Syslog>

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

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

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

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

                <Guidance:Point>
                    <Parallel/>
                    <Setting><Guidance:Point.rudderAngle/><Units:degree/><Value>10</Value></Setting>
                </Guidance:Point>

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

                <Aggregate Id="wait_depth">

                    <Sequence/>

                    <Break>
                        <Universal:depth/>
                        <Ge><Arg Name="ApproachDepth"/><Sub><Arg Name="DepthDeadband"/></Sub></Ge>
                    </Break>

                    <Guidance:Wait>
                        <Sequence/>
                        <Setting><Guidance:Wait.duration/><Units:minute/><Value>20</Value></Setting>
                    </Guidance:Wait>

                </Aggregate>

                <Syslog Severity="Info">Completed initial descent at <Universal:depth/><Units:meter/></Syslog>

                <Assign><Sequence/><Arg Name="LightCmd"/><Arg Name="LightsOff"/></Assign>

            </Aggregate>

            <Aggregate Id="FlipLightsOnStation">

                <Description>
                    Keep station at commanded depth, turn on the camera and
                    lights.
                </Description>

                <Sequence/>

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

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

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

                <Guidance:KeepStation>
                    <Parallel/>
                    <Setting><Guidance:KeepStation.latitude/><Arg Name="CenterLatitude"/></Setting>
                    <Setting><Guidance:KeepStation.longitude/><Arg Name="CenterLongitude"/></Setting>
                    <Setting><Guidance:KeepStation.radius/><Arg Name="Radius"/></Setting>
                    <Setting><Guidance:KeepStation.speed/><Arg Name="Speed"/></Setting>
                </Guidance:KeepStation>

<!--  Camera and lights  -->

                <Aggregate Id="CameraAndLights">

                    <Description>
                        Handles camera and light requests.
                    </Description>

                    <Parallel/>

<!--  Power the camera -->

                    <ReadData Id="RequestCameraOn">
                        <Sensor:PowerOnly.sampleLoad1/>
                    </ReadData>

                    <Aggregate Id="WaitForCamera">

                        <Description>
                            Allow time for the camera to start recording
                        </Description>

                        <While>
                            <Arg Name="LightCmd"/>
                            <Eq><Arg Name="LightsOff"/></Eq>
                        </While>

                        <Aggregate Id="camera_wait">

                            <Sequence/>

                            <Syslog Severity="Info">Activating camera.</Syslog>

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

                            <Assign><Sequence/><Arg Name="LightCmd"/><Arg Name="RedLight"/></Assign>

                        </Aggregate>

                    </Aggregate>

                    <Aggregate Id="Red">

                        <Description>
                            Power the red lights for the requested duration
                        </Description>

                        <While>
                            <Arg Name="LightCmd"/>
                            <Eq><Arg Name="RedLight"/></Eq>
                        </While>

                        <Aggregate Id="red_wait">

                            <Sequence/>

                            <ReadData Id="PowerRedLights">
                                <Sensor:PowerOnly.sampleLoad2/>
                            </ReadData>

                            <Syslog Severity="Info">Activating Red Lights.</Syslog>

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

                            <Assign><Sequence/><Arg Name="LightCmd"/><Arg Name="WhiteLight"/></Assign>

                        </Aggregate>

                    </Aggregate>

                    <Aggregate Id="White">

                        <Description>
                            Power the white lights for the requested duration
                        </Description>

                        <While>
                            <Arg Name="LightCmd"/>
                            <Eq><Arg Name="WhiteLight"/></Eq>
                        </While>

                        <Aggregate Id="white_wait">

                            <Sequence/>

                            <ReadData Id="PowerWhiteLights">
                                <Sensor:PowerOnly.sampleLoad3/>
                            </ReadData>

                            <Syslog Severity="Info">Activating White Lights.</Syslog>

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

                            <Assign><Sequence/><Arg Name="LightCmd"/><Arg Name="RedLight"/></Assign>

                        </Aggregate>

                    </Aggregate>

                </Aggregate>

                <Guidance:Wait>
                    <Description>
                        Due to the way the KeepStation behavior is currently
                        implemented, we must run it in parallel and use a Wait
                        to keep from completing the mission before the timeout
                        expires. However, this new version of the mission uses a
                        top-level timeout so that the entire mission will not
                        run for longer then MissionTimeout.
                    </Description>
                    <Sequence/>
                    <Setting><Guidance:Wait.duration/><Arg Name="MissionTimeout"/></Setting>
                </Guidance:Wait>

            </Aggregate>

        </Aggregate>

    </Aggregate>

</Mission>
