<?xml version="1.0" encoding="UTF-8"?>
<Mission xmlns="Tethys"
       xmlns:Control="Tethys/Control"
       xmlns:Derivation="Tethys/Derivation"
       xmlns:Guidance="Tethys/Guidance" 
       xmlns:Navigation="Tethys/Navigation" 
       xmlns:Trigger="Tethys/Trigger" 
       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/Guidance http://okeanids.mbari.org/tethys/Xml/Guidance.xsd
                           Tethys/Navigation http://okeanids.mbari.org/tethys/Xml/Navigation.xsd
                           Tethys/Trigger http://okeanids.mbari.org/tethys/Xml/Trigger.xsd
                           Tethys/Units http://okeanids.mbari.org/tethys/Xml/Units.xsd
                           Tethys/Universal http://okeanids.mbari.org/tethys/Xml/Universal.xsd"
       Id="SurfaceCurrentCompactModel">

    <Description>
        The goal of this deployment is to demonstrate onboard capability with
        the Compact Models code. This demonstration will use the Compact Models
        trained by HF radar measurements to predict surface currents. This first
        test case will focus on vehicle localization, comparing position
        estimates computed with and without benefit of the surface current
        information. This means that the vehicle will be running the Compact
        Model code and recording its outputs, but will not be using those
        outputs in realtime. We want to make sure the outputs are reasonable
        before we integrate them into the navigation and control loops to let
        them drive the vehicle.
    </Description>

<!-- Mission-wide settings -->

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

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

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

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

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

    <DefineArg Name="MassDefault"><Description>
        Static setting for mass during the mission. Defaults to massDefault
        setting in the Config/Control.cfg file.
    </Description><Control:VerticalControl.massDefault/></DefineArg>

    <DefineArg Name="BuoyancyNeutral"><Description>
        Buoyancy bladder position while performing the YoYo behavior. Defaults
        to buoyancyNeutral setting in the Config/Control.cfg file.
    </Description><Control:VerticalControl.buoyancyNeutral/></DefineArg>

<!-- Waypoint list -->

    <DefineArg Name="WpCloseLat"><Description>
        Latitude of waypoint closest to MBARI.
    </Description><Units:degree/><Value>36.830</Value></DefineArg>

    <DefineArg Name="WpCloseLon"><Description>
        Longitude of waypoint closest to MBARI.
    </Description><Units:degree/><Value>-121.870</Value></DefineArg>

<!-- Far waypoint nominally 36.875, -121.965 -->

<!-- 
In [1]: from pyproj import Geod

In [2]: Geod(ellps='WGS84').fwd(-121.870, 36.830, 300, 10000)
Out[2]: (-121.96713471834381, 36.87501557446235, 119.94174274330628)
-->

    <DefineArg Name="WpFarLat"><Description>
        Latitude of waypoint farthest from MBARI.
    </Description><Units:degree/><Value>36.87501557446235</Value></DefineArg>

    <DefineArg Name="WpFarLon"><Description>
        Longitude of waypoint farthest from MBARI.
    </Description><Units:degree/><Value>-121.96713471834381</Value></DefineArg>

<!-- 
     Close & far waypoints are separated by 10 km 
     at bearing 300, or bearing 120
-->

    <DefineArg Name="WpCircleLat"><Description>
        Latitude of waypoint.
    </Description><Units:degree/><Value>36.808</Value></DefineArg>

    <DefineArg Name="WpCircleLon"><Description>
        Longitude of waypoint.
    </Description><Units:degree/><Value>-121.827</Value></DefineArg>

<!-- Approach Settings -->

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

    <DefineArg Name="ApproachTimeout"><Description>
        Maximum duration of initial approach
    </Description><Units:hour/><Value>4</Value></DefineArg>

    <DefineArg Name="ApproachNeedCommsTime"><Description>
        Elapsed time after previous surface communications when vehicle will
        begin to ascend for additional surface communications. Only active
        during the approach.
    </Description><Units:minute/><Value>60</Value></DefineArg>

<!-- Science loop settings -->

    <DefineArg Name="NeedCommsTime"><Description>
        Elapsed time after previous surface communications when vehicle will
        begin to ascend for additional surface communications. Set this large to
        only surface at waypoints.
    </Description><Units:minute/><Value>250</Value></DefineArg>

    <DefineArg Name="Repeat"><Description>
        Number of times to repeat each lap.
    </Description><Units:count/><Value>3</Value></DefineArg>

    <DefineArg Name="ShallowDepth"><Description>
        Depth for shallow run.
    </Description><Units:meter/><Value>3.0</Value></DefineArg>

    <DefineArg Name="MiddleDepth"><Description>
        Depth for middle run.
    </Description><Units:meter/><Value>6.0</Value></DefineArg>

    <DefineArg Name="DeepDepth"><Description>
        Depth for deep run.
    </Description><Units:meter/><Value>9.0</Value></DefineArg>

    <DefineArg Name="DeepestDepth"><Description>
        Depth for deep run.
    </Description><Units:meter/><Value>12.0</Value></DefineArg>

    <DefineArg Name="CircleDepth"><Description>
        Depth for circling.
    </Description><Units:meter/><Value>3.0</Value></DefineArg>

    <DefineArg Name="LegTimeout"><Description>
        Maximum duration of leg in one direction.
    </Description><Units:hour/><Value>4</Value></DefineArg>

    <DefineArg Name="LapTimeout"><Description>
        Maximum duration of lap in both directions.
    </Description><Units:hour/><Value>8</Value></DefineArg>

    <DefineArg Name="CircleRadius"><Description>
        Radius of circle to run at end.
    </Description><Units:meter/><Value>250</Value></DefineArg>

    <DefineArg Name="CircleRepeat"><Description>
        Maximum number of times to circle the waypoint before going to Default.
    </Description><Units:count/><Value>13</Value></DefineArg>

    <DefineArg Name="CircleTimeout"><Description>
        Timeout for each circle.
    </Description><Units:hour/><Value>1</Value></DefineArg>

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

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

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

    <DefineArg Name="YoYoUpPitch"><Description>
        Pitch when yo-yo's are ascending.
    </Description><Units:degree/><Value>20.0</Value></DefineArg>

    <DefineArg Name="YoYoDownPitch"><Description>
        Pitch when yo-yo's are descending.
    </Description><Units:degree/><Value>-20.0</Value></DefineArg>

<!-- The mission -->

    <Timeout Duration="MissionTimeout"/>

<!-- High-priority directives that should always be near the top -->

    <Guidance:AltitudeEnvelope>
        <Parallel/>
        <Setting><Guidance:AltitudeEnvelope.minAltitude/><Arg Name="MinAltitude"/></Setting>
    </Guidance:AltitudeEnvelope>

    <Guidance:DepthEnvelope>
        <Parallel/>
        <Setting><Guidance:DepthEnvelope.maxDepth/><Arg Name="MaxDepth"/></Setting>
    </Guidance:DepthEnvelope>

    <Guidance:OffshoreEnvelope>
        <Parallel/>
        <Setting><Guidance:OffshoreEnvelope.minOffshore/><Arg Name="MinOffshore"/></Setting>
    </Guidance:OffshoreEnvelope>

<!-- GPS Update.  Don't go too long w/o a GPS fix. -->

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

<!-- NOTE: The <Assign> clauses below will not take effect until you actually run the mission. RedefineArg will not work in this case if you use a "set" after loading the mission. -->

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

    <Assign><Sequence/><Arg Name="NeedComms:SurfaceSpeed"/><Arg Name="Speed"/></Assign>

<!-- mass and buoyancy defaults are lower priority than surfacing, but higher than everything else -->

    <Guidance:Pitch>
        <Parallel/>
        <Setting><Guidance:Pitch.massPosition/><Arg Name="MassDefault"/></Setting>
    </Guidance:Pitch>

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

<!-- Begin callable aggregates used in this mission. 
     (Note priority still matters, so these are done _below_ important safety 
     directives like OffshoreEnvelope.) 
     -->

    <Insert Filename="Insert/CircleWaypoint.xml">
        <Description>
            When called, circle the given waypoint.
        </Description>
        <RedefineArg Name="Timeout"><Arg Name="CircleTimeout"/></RedefineArg>
        <RedefineArg Name="Radius"><Arg Name="CircleRadius"/></RedefineArg>
        <RedefineArg Name="CenterLat"><Arg Name="WpCircleLat"/></RedefineArg>
        <RedefineArg Name="CenterLon"><Arg Name="WpCircleLon"/></RedefineArg>
<!-- Use default values for: Timeout, MaxError, TurnToPort, KwpHeading -->
    </Insert>

    <Aggregate Id="Lap" >

        <Description>
            Run out and back in on the line.
        </Description>

        <DefineArg Name="Depth"><Description>
            Depth for current lap. (internal variable)
        </Description><Units:meter/><Value>NaN</Value></DefineArg>

        <When Repeat="Repeat">
            <Called/>
        </When>

        <Timeout Duration="LapTimeout"/>

        <Syslog Severity="Important">Running lap <Arg Name="Depth"/><Units:meter/>deep.</Syslog>

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

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

        <Aggregate Id="Outbound">

            <Sequence/>

            <Timeout Duration="LegTimeout"/>

            <Guidance:Waypoint Id="WpFar">
                <Sequence/>
                <Setting><Guidance:Waypoint.latitude/><Arg Name="WpFarLat"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="WpFarLon"/></Setting>
            </Guidance:Waypoint>

        </Aggregate>

        <Call RefId="SurfaceCommsShort"/>

        <Aggregate Id="Inbound">

            <Sequence/>

            <Timeout Duration="LegTimeout"/>

            <Guidance:Waypoint Id="WpClose">
                <Sequence/>
                <Setting><Guidance:Waypoint.latitude/><Arg Name="WpCloseLat"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="WpCloseLon"/></Setting>
            </Guidance:Waypoint>

        </Aggregate>

        <Call RefId="LogCompactModelSurfaceCurrent"/>

        <Call RefId="SurfaceComms"/>

    </Aggregate>

    <Aggregate Id="Approach">

        <Description>
            Get to start point.
        </Description>

        <When>
            <Called/>
        </When>

        <Timeout Duration="ApproachTimeout"/>

        <Aggregate>

            <Description>
                Check in periodically, but do not split logs
            </Description>

            <When>
                <Elapsed><Universal:platform_communications/></Elapsed>
                <Gt><Arg Name="ApproachNeedCommsTime"/></Gt>
            </When>

            <Call RefId="SurfaceCommsShort"/>

        </Aggregate>

        <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="WpClose">
            <Sequence/>
            <Setting><Guidance:Waypoint.latitude/><Arg Name="WpCloseLat"/></Setting>
            <Setting><Guidance:Waypoint.longitude/><Arg Name="WpCloseLon"/></Setting>
        </Guidance:Waypoint>

        <Call RefId="SurfaceComms"/>

    </Aggregate>

<!-- End callable aggregates used in this mission. -->

    <Aggregate Id="ScienceOps">

        <Description>
            Run lap out and back at different depths, estimating local surface
            current using Compact Models with HF radar inputs.
        </Description>

        <Sequence/>

<!-- TODO: add assign clauses here to make it use the compact model navigator -->

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

<!-- Call the behavior aggregates that we defined above. -->

        <Call RefId="Approach"/>

        <Assign><Sequence/><Arg Name="Lap:Depth"/><Arg Name="ShallowDepth"/></Assign>

        <Call Id="ShallowLap" RefId="Lap"/>

        <Assign><Sequence/><Arg Name="Lap:Depth"/><Arg Name="DeepestDepth"/></Assign>

        <Call Id="DeepestLap" RefId="Lap"/>

        <Assign><Sequence/><Arg Name="Lap:Depth"/><Arg Name="MiddleDepth"/></Assign>

        <Call Id="MiddleLap" RefId="Lap"/>

        <Assign><Sequence/><Arg Name="Lap:Depth"/><Arg Name="DeepDepth"/></Assign>

        <Call Id="DeepLap" RefId="Lap"/>

        <Aggregate Id="YoYoLap">

            <Sequence Repeat="Repeat"/>

            <Timeout Duration="LapTimeout"/>

            <Syslog Severity="Important">Running YoYo lap.</Syslog>

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

            <Guidance:AltitudeEnvelope>
                <Parallel/>
                <Setting><Guidance:AltitudeEnvelope.minAltitude/><Arg Name="YoYoMinAltitude"/></Setting>
                <Setting><Guidance:AltitudeEnvelope.upPitch/><Arg Name="YoYoUpPitch"/></Setting>
            </Guidance:AltitudeEnvelope>

            <Guidance:DepthEnvelope>
                <Parallel/>
                <Setting><Guidance:DepthEnvelope.minDepth/><Arg Name="YoYoMinDepth"/></Setting>
                <Setting><Guidance:DepthEnvelope.maxDepth/><Arg Name="YoYoMaxDepth"/></Setting>
                <Setting><Guidance:DepthEnvelope.downPitch/><Arg Name="YoYoDownPitch"/></Setting>
                <Setting><Guidance:DepthEnvelope.upPitch/><Arg Name="YoYoUpPitch"/></Setting>
            </Guidance:DepthEnvelope>

            <Guidance:YoYo>
                <Parallel/>
                <Setting><Guidance:YoYo.downPitch/><Arg Name="YoYoDownPitch"/></Setting>
                <Setting><Guidance:YoYo.upPitch/><Arg Name="YoYoUpPitch"/></Setting>
            </Guidance:YoYo>

            <Aggregate Id="Outbound">

                <Sequence/>

                <Timeout Duration="LegTimeout"/>

                <Guidance:Waypoint Id="WpFar">
                    <Sequence/>
                    <Setting><Guidance:Waypoint.latitude/><Arg Name="WpFarLat"/></Setting>
                    <Setting><Guidance:Waypoint.longitude/><Arg Name="WpFarLon"/></Setting>
                </Guidance:Waypoint>

            </Aggregate>

            <Call RefId="LogCompactModelSurfaceCurrent"/>

            <Call RefId="SurfaceCommsShort"/>

            <Aggregate Id="Inbound">

                <Sequence/>

                <Timeout Duration="LegTimeout"/>

                <Guidance:Waypoint Id="WpClose">
                    <Sequence/>
                    <Setting><Guidance:Waypoint.latitude/><Arg Name="WpCloseLat"/></Setting>
                    <Setting><Guidance:Waypoint.longitude/><Arg Name="WpCloseLon"/></Setting>
                </Guidance:Waypoint>

            </Aggregate>

            <Call RefId="LogCompactModelSurfaceCurrent"/>

            <Call RefId="SurfaceComms"/>

        </Aggregate>

        <Aggregate Id="StandbyCircle">

            <Description>
                Stay in good HF radar coverage area circling to gather more data
                until it's time to come home.
            </Description>

            <Sequence Repeat="CircleRepeat"/>

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

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

            <Guidance:Waypoint Id="WpClose">
                <Sequence/>
                <Setting><Guidance:Waypoint.latitude/><Arg Name="WpCircleLat"/></Setting>
                <Setting><Guidance:Waypoint.longitude/><Arg Name="WpCircleLon"/></Setting>
            </Guidance:Waypoint>

            <Call RefId="CircleWaypoint"/>

            <Call RefId="LogCompactModelSurfaceCurrent"/>

            <Call RefId="SurfaceComms"/>

        </Aggregate>

    </Aggregate>

</Mission>
