<?xml version="1.0" encoding="UTF-8"?>
<Mission xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
    xmlns="Tethys"
    xmlns:Units="Tethys/Units"
    xmlns:Estimation="Tethys/Estimation"
    xmlns:Universal="Tethys/Universal"
    xmlns:Control="Tethys/Control"
    xmlns:Guidance="Tethys/Guidance"
    xsi:schemaLocation="Tethys http://okeanids.mbari.org/tethys/Xml/Tethys.xsd
    Tethys/Units http://okeanids.mbari.org/tethys/Xml/Units.xsd
    Tethys/Estimation http://okeanids.mbari.org/tethys/Xml/Estimation.xsd
    Tethys/Universal http://okeanids.mbari.org/tethys/Xml/Universal.xsd
    Tethys/Control http://okeanids.mbari.org/tethys/Xml/Control.xsd
    Tethys/Guidance http://okeanids.mbari.org/tethys/Xml/Guidance.xsd"
    Id="altitudeServo">

    <Description>
        The vehicle will transit to an initial waypoint, then navigate to
        waypoints while maintaining altitude. During the altitude following
        phase of the mission, the vehicle descends from the surface to a
        commanded depth using a spiral dive (e.g., after NeedComms).
    </Description>

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

    <DefineArg Name="NeedCommsTime"><Description>
        How often to surface for commumications
    </Description><Units:minute/><Value>60</Value></DefineArg>

    <DefineArg Name="TransitLat"><Description>
        Latitude of tranist waypoint. Transit is skipped if set to NaN.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="TransitLon"><Description>
        Longitude of tranist waypoint. Transit is skipped if set to NaN.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="TransitDepth"><Description>
        Depth of flat and level flight during the transit part of the mission.
    </Description><Units:meter/><Value>10</Value></DefineArg>

    <DefineArg Name="Repeat"><Description>
        Number of times the vehicle should try to cycle through waypoints.
    </Description><Units:count/><Value>1</Value></DefineArg>

    <DefineArg Name="Lat1"><Description>
        Latitude of waypoint 1. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lon1"><Description>
        Longitude of waypoint 1. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lat2"><Description>
        Latitude of waypoint 2. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lon2"><Description>
        Longitude of waypoint 2. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lat3"><Description>
        Latitude of waypoint 3. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lon3"><Description>
        Longitude of waypoint 3. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="HoldAltitude"><Description>
        Desired vehicle altitude to maintain.
    </Description><Units:meter/><Value>7</Value></DefineArg>

    <DefineArg Name="InvalidAltitudeTimeout"><Description>
        The mission will abort if no valid altitude readings are received within
        this specified time duration. Set this variable to NaN to disable this
        timeout functionality.
    </Description><Units:minute/><Value>NaN</Value></DefineArg>

    <DefineArg Name="AltitudeIIRFilterDecay"><Description>
        AltitudeServo single-pole IIR filter decay value.
        Low-pass filters the AltitudeServo depth command.
        The Decay value should be btween 0 to 1, set to 0 disable filtering.
    </Description><Units:none/><Value>0.0</Value></DefineArg>

    <DefineArg Name="InitDepth"><Description>
        Initial depth. The vehicle will spiral dive to this depth when leaving
        the surface. Necessary if no altitude reading is available form the
        surface (e.g., when the bottom is out of DVL range).
    </Description><Units:meter/><Value>30</Value></DefineArg>

    <DefineArg Name="InitDepthTimeout"><Description>
        Maximum time duration for the vehicle to reach the initial depth. The
        vehicle will switch over to altitude following after the timeout is
        expired.
    </Description><Units:minute/><Value>30</Value></DefineArg>

    <DefineArg Name="Speed"><Description>
        Thruster speed for the entier mission.
    </Description><Units:meter_per_second/><Value>1</Value></DefineArg>

    <DefineArg Name="DepthDeadband"><Units:meter/><Value>0.0</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>

    <DefineOutput Name="DoingComms"><Description>
        Mission variable (don't change). The mission sets this to true *before*
        the vehicle surfaces for comms.
    </Description><False/></DefineOutput>

    <DefineOutput Name="StartFromSurface"><Description>
        Mission variable (don't change). The mission sets this to true *after*
        the vehicle surfaces for comms.
    </Description><True/></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="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>

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

    </Aggregate>

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

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

<!-- Callable aggregates (priority matters, so these are done below important safety directives like StandardEnvelopes). -->

    <Aggregate Id="DiveCmd">

        <When>
            <Called/>
        </When>

        <Aggregate Id="SpiralDive">

            <Description>
                Spiral down from the surface to initial depth. Necessary when no
                altitude reading is available form the surface.
            </Description>

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

            <Aggregate Id="dive">

                <Sequence/>

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

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

                <Guidance:Pitch>
                    <Sequence/>
                    <Timeout Duration="InitDepthTimeout">
                        <Syslog Severity="Important">Timed out at current depth of <Universal:depth/><Units:meter/>. Switching to altitude servo.</Syslog>
                        <Assign><Sequence/><Arg Name="StartFromSurface"/><False/></Assign>
                    </Timeout>
                    <Setting><Guidance:Pitch.depth/><Arg Name="InitDepth"/></Setting>
                </Guidance:Pitch>

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

            </Aggregate>

        </Aggregate>

        <Aggregate Id="AltitudeServo">

            <Description>
                Follow commanded altitude.
            </Description>

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

            <Aggregate Id="dive">

                <Parallel/>

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

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

                <Guidance:AltitudeServo>
                    <Parallel/>
                    <Setting><Guidance:AltitudeServo.targetAltitude/><Arg Name="HoldAltitude"/></Setting>
                    <Setting><Guidance:AltitudeServo.invalidAltitudeTimeout/><Arg Name="InvalidAltitudeTimeout"/></Setting>
                    <Setting><Guidance:AltitudeServo.iirFilterDecay/><Arg Name="AltitudeIIRFilterDecay"/></Setting>
                </Guidance:AltitudeServo>

            </Aggregate>

        </Aggregate>

    </Aggregate>

<!-- Mission -->

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

<!-- Fire up science sensors -->

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

<!-- Run -->

    <Aggregate Id="Transit">

        <Description>
            Transit flat and level to the starting waypoint at a fixed depth
            (skipped when transit Lat/Lon are set to NaN).
        </Description>

        <Sequence/>

        <Break>
            <IsNaN><Arg Name="TransitLat"/>
            </IsNaN>
            <Or><IsNaN><Arg Name="TransitLon"/>
            </IsNaN></Or>
        </Break>

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

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

        <Guidance:Waypoint Id="TransitWpt">
            <Sequence/>
            <Setting><Guidance:Waypoint.latitude/><Arg Name="TransitLat"/></Setting>
            <Setting><Guidance:Waypoint.longitude/><Arg Name="TransitLon"/></Setting>
        </Guidance:Waypoint>

        <Call RefId="SurfaceComms"/>

    </Aggregate>

    <Aggregate Id="Lap">

        <Sequence Repeat="Repeat"/>

        <Aggregate Id="Dive">

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

            <Call RefId="DiveCmd"/>

        </Aggregate>

        <Aggregate Id="Wpt1">

            <Sequence/>

            <Guidance:Waypoint>
                <Sequence/>
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat1"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon1"/></Setting>
            </Guidance:Waypoint>

        </Aggregate>

        <Aggregate Id="Wpt2">

            <Sequence/>

            <Guidance:Waypoint>
                <Sequence/>
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat2"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon2"/></Setting>
            </Guidance:Waypoint>

        </Aggregate>

        <Aggregate Id="Wpt3">

            <Sequence/>

            <Guidance:Waypoint>
                <Sequence/>
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat3"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon3"/></Setting>
            </Guidance:Waypoint>

        </Aggregate>

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

    </Aggregate>

</Mission>
