<?xml version="1.0" encoding="UTF-8"?>

<Mission xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
         xmlns="Tethys"
         xmlns:Units="Tethys/Units"
         xmlns:Control="Tethys/Control"
         xmlns:Guidance="Tethys/Guidance"
         xmlns:Universal="Tethys/Universal"
         xsi:schemaLocation="Tethys http://okeanids.mbari.org/tethys/Xml/Tethys.xsd
                             Tethys/Units http://okeanids.mbari.org/tethys/Xml/Units.xsd
                             Tethys/Control http://okeanids.mbari.org/tethys/Xml/Control.xsd
                             Tethys/Guidance http://okeanids.mbari.org/tethys/Xml/Guidance.xsd
                             Tethys/Universal http://okeanids.mbari.org/tethys/Xml/Universal.xsd"
       Id="sci2_slow_and_flat">

    <Description>
        Vehicle transits to the specified waypoints at the commanded depth (flat-and-level) with science turned on.
    </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="Depth"><Description>
        Depth of flat and level flight during the mission.
    </Description><Units:meter /><Value>25</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>36.797</Value></DefineArg>

    <DefineArg Name="Lon1"><Description>
        Longitude of waypoint 1. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>-122.8</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="Lat4"><Description>
        Latitude of waypoint 4. If nan, waypoint is skipped.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

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

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

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

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

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

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

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

    <DefineArg Name="Speed"><Description>
        Speed while transiting to waypoints.
    </Description><Units:meter_per_second/><Value>0.25</Value></DefineArg>

    <DefineArg Name="ApproachDepth"><Description>
        Depth for initial descent form the surface.
    </Description><Units:meter/><Value>20</Value></DefineArg>

    <DefineArg Name="ApproachSpeed"><Description>
        Speed while descending to target depth.
    </Description><Units:meter_per_second/><Value>0.8</Value></DefineArg>

    <DefineArg Name="ApproachPitchLimit"><Description>
        Max vehicle pitch (+/-) when decending from the surface.
    </Description><Units:degree/><Value>20</Value></DefineArg>

    <DefineArg Name="ApproachDepthTimeout"><Description>
        Maximum time duration for the vehicle to reach the target depth
    </Description><Units:minute/><Value>20</Value></DefineArg>

    <DefineArg Name="ApproachSettleTimePreDive"><Description>
        Time duration for the vehicle to pump down the VBS *BEFORE* leaving the surface.
    </Description><Units:minute/><Value>1</Value></DefineArg>

    <DefineArg Name="ApproachSettleTimePostDive"><Description>
        Time duration for the vehicle to settle *AFTER* reaching the target depth.
    </Description><Units:minute/><Value>3</Value></DefineArg>

    <DefineArg Name="kpDepth"><Description>
        Dive loop proportional gain (applied only while the vehicle is descending to target depth).
    </Description><Units:radian_per_meter/><Value>0.07</Value></DefineArg>

    <DefineArg Name="BuoyancyNeutral"><Description>
        Buoyancy bladder position during the mission. Defaults to buoyancyNeutral
        setting in the Config/Control.cfg file. Set to NaN cc for active buoyancy.
    </Description><Control:VerticalControl.buoyancyNeutral/></DefineArg>

    <DefineArg Name="MassDefault"><Description>
        Static setting for mass during the mission. Set to NaN mm for active
        mass position.
    </Description><Control:VerticalControl.massDefault/></DefineArg>

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

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

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

    <DefineOutput Name="DiveMode"><Description>
        Mission variable (don't change). The mission sets this variable to switch
        between dive modes. Initialized to DoingComms.
    </Description><Units:count/><Value>0</Value></DefineOutput>

    <DefineOutput Name="DoingComms"><Description>
        Mission variable (don't change).
        The mission will run in this to mode when doing surface comms.
    </Description><Units:count/><Value>0</Value></DefineOutput>

    <DefineOutput Name="InitialDive"><Description>
        Mission variable (don't change).
        The mission will run in this mode after surface comms are done to dive the
        vehicle to the approach depth.
    </Description><Units:count/><Value>1</Value></DefineOutput>

    <DefineOutput Name="SettleAtDepth"><Description>
        Mission variable (don't change).
        The mission will run in this mode to settle the vehicle at the target depth.
    </Description><Units:count/><Value>2</Value></DefineOutput>

    <DefineOutput Name="MaintainDepth"><Description>
        Mission variable (don't change).
        The mission will run in this mode to maintain the vehicle at the target depth.
    </Description><Units:count/><Value>3</Value></DefineOutput>


    <!-- Mission timeout -->

    <Timeout Duration="MissionTimeout"/>

    <!-- Science! -->

    <Insert Filename="Insert/Science.xml">
        <Description>
            Run the science sensors.
        </Description>
    </Insert>

    <!-- 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="DiveMode"/><Arg Name="DoingComms"/></Assign>

        <Call RefId="NeedComms"/>

        <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="InitialDive"/></Assign>
    </Aggregate>


    <Aggregate Id="DiveCmd">

        <Description>
            Called when the vehicle should be underwater.
            Arbitrates between 3 dive modes:
                1) descend from the surface to initial depth.
                2) settle at target depth using the VB.
                3) cruise at target depth at 0 pitch using mass shifter and VB for vertical control.
        </Description>

        <When>
            <Called/>
        </When>


        <Aggregate Id="InitialDive">
            <Description>
                Descend from the surface to target depth, limit pitch and depth-rate.
            </Description>

            <While>
                <Arg Name="DiveMode"/><Eq><Arg Name="InitialDive"/></Eq>
            </While>

            <Aggregate Id="dive">
                <Sequence/>

                <Assign><Parallel/><Control:VerticalControl.pitchLimit/><Arg Name="ApproachPitchLimit"/></Assign>

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

                <Guidance:Mass>
                    <Parallel/>
                    <Setting><Guidance:Mass.position/><Arg Name="MassDefault"/></Setting>
                </Guidance:Mass>

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

                <Syslog Severity="Info">Waiting for <Arg Name="ApproachSettleTimePreDive"/><Units:minute/>while pumping down to neutral.</Syslog>

                <Guidance:Wait>
                    <Description>Take a few moments to ensure the VBS reaches neutral.</Description>
                    <Sequence/>
                    <Setting><Guidance:Wait.duration/><Arg Name="ApproachSettleTimePreDive"/></Setting>
                </Guidance:Wait>

                <Syslog>Moving to <Arg Name="ApproachDepth"/><Units:meter/></Syslog>

                <Aggregate Id="ApproachDepth">
                    <Sequence/>

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

                    <Guidance:Pitch>
                        <Sequence/>
                        <Timeout Duration="ApproachDepthTimeout">
                            <Syslog Severity="Important">Timed out while trying to reach target depth at current depth of <Universal:depth/><Units:meter/></Syslog>
                            <Guidance:Execute>
                                <Sequence/>
                                <Setting><Guidance:Execute.command/><String>stop</String></Setting>
                            </Guidance:Execute>
                        </Timeout>
                        <Setting><Guidance:Pitch.depth/><Arg Name="ApproachDepth"/></Setting>
                    </Guidance:Pitch>
                </Aggregate>

                <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="SettleAtDepth"/></Assign>
            </Aggregate>
        </Aggregate>


        <Aggregate Id="SettleAtDepth">
            <Description>
                Take a few more moments here to make sure the vechile is settled at the target depth.
            </Description>

            <While>
                <Arg Name="DiveMode"/><Eq><Arg Name="SettleAtDepth"/></Eq>
            </While>

            <Aggregate Id="dive">
                <Sequence/>

                <Syslog Severity="Info">Reached <Universal:depth/><Units:meter/>, settling for <Arg Name="ApproachSettleTimePostDive"/><Units:minute/></Syslog>

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

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

                <Assign><Sequence/><Arg Name="DiveMode"/><Arg Name="MaintainDepth"/></Assign>
            </Aggregate>
        </Aggregate>


        <Aggregate Id="MaintainDepth">
            <Description>
                Cruise at the target depth.
            </Description>

            <While>
                <Arg Name="DiveMode"/><Eq><Arg Name="MaintainDepth"/></Eq>
            </While>

            <Aggregate Id="dive">
                <Parallel/>

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

                <Guidance:Pitch>
                    <Parallel/>
                    <Setting><Guidance:Pitch.depth/><Arg Name="Depth"/></Setting>
                    <Setting><Guidance:Pitch.elevatorAngle/><Units:degree/><Value>0</Value></Setting>
                    <Setting><Guidance:Pitch.pitch/><Units:degree/><Value>0</Value></Setting>
                </Guidance:Pitch>                
            </Aggregate>
        </Aggregate>

    </Aggregate>


    <!-- Start mission sequence -->

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

    <Aggregate Id="Lap">
        <Sequence Repeat="Repeat"/>

        <Aggregate Id="Dive">
            <While>
                <Not><Arg Name="DiveMode"/><Eq><Arg Name="DoingComms"/></Eq></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>

        <Aggregate Id="Wpt4">
            <Sequence/>

            <Guidance:Waypoint>
                <Sequence/>
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat4"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon4"/></Setting>
            </Guidance:Waypoint>
        </Aggregate>

        <Aggregate Id="Wpt5">
            <Sequence/>

            <Guidance:Waypoint>
                <Sequence/>
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat5"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon5"/></Setting>
            </Guidance:Waypoint>
        </Aggregate>

        <Aggregate Id="Wpt6">
            <Sequence/>

            <Guidance:Waypoint>
                <Sequence/>
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat6"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon6"/></Setting>
            </Guidance:Waypoint>
        </Aggregate>

        <Aggregate Id="Wpt7">
            <Sequence/>

            <Guidance:Waypoint>
                <Sequence/>
                <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat7"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon7"/></Setting>
            </Guidance:Waypoint>
        </Aggregate>
    </Aggregate>
</Mission>
