<?xml version="1.0" encoding="UTF-8"?>
<Mission xmlns="Tethys"
       xmlns:Control="Tethys/Control"
       xmlns:Guidance="Tethys/Guidance" 
       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/Guidance http://okeanids.mbari.org/tethys/Xml/Guidance.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="test">

    <Description>
        This is just to test functionality of
        Insert/CircleWaypointRepeatedly.xml
    </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>85</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.0</Value></DefineArg>

    <DefineArg Name="MissionTimeout"><Description>
        Maximum duration of mission
    </Description><Units:hour/><Value>24</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="WpESPInshoreLat"><Description>
        Latitude of inshore ESP buoy.
    </Description><Units:degree/><Value>33.6055</Value></DefineArg>

    <DefineArg Name="WpESPInshoreLon"><Description>
        Longitude of offshore ESP buoy.
    </Description><Units:degree/><Value>-118.0191</Value></DefineArg>

    <DefineArg Name="WpESPOffshoreLat"><Description>
        Latitude of offshore ESP buoy.
    </Description><Units:degree/><Value>33.5839</Value></DefineArg>

    <DefineArg Name="WpESPOffshoreLon"><Description>
        Longitude of offshore ESP buoy.
    </Description><Units:degree/><Value>-118.0349</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="Repeat"><Description>
        Number of times to repeat the science loop
    </Description><Units:count/><Value>3</Value></DefineArg>

<!-- TODO set to 2 for sims; change for field deployments -->

    <DefineArg Name="CircleRadius"><Description>
        Radius of circle around Inshore ESP during wait time
    </Description><Units:meter/><Value>250</Value></DefineArg>

    <DefineArg Name="CircleDivisor"><Description>
        Time between reporting events.
    </Description><Units:hour/><Value>6</Value></DefineArg>

    <DefineArg Name="CircleModulus"><Description>
        Current time modulo CircleDivisor.
    </Description><Units:hour/><Value>NaN</Value></DefineArg>

    <DefineArg Name="CircleTimeout"><Description>
        Timeout for circling buoy at start of each lap (reset each lap).
    </Description><Units:hour/><Value>8</Value></DefineArg>

    <DefineArg Name="LapTimeout"><Description>
        Maximum duration of lap.
    </Description><Units:hour/><Value>12</Value></DefineArg>

<!-- TODO this should be adjusted to less than CircleDivisor, but make sure the vehicle can complete the lap for those given waypoints first. -->

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

    <DefineArg Name="YoYoMaxDepth"><Description>
        Maximum depth during yo-yo's.
    </Description><Units:meter/><Value>78.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>

<!-- Variables used internally during the mission -->

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

<!-- Surfacing behaviors -->

    <Insert Filename="Insert/Surface.xml" Id="SurfaceComms"/>

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

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

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

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

<!-- 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/CircleWaypointRepeatedly.xml"/>

    <Aggregate Id="UpdateCircleTimeout">

        <Description>
            Calculate (current time) mod (CircleDivisor) and determine when to
            stop circling the start-of-lap waypoint.
        </Description>

        <When>
            <Called/>
        </When>

        <Assign><Sequence/><Arg Name="CircleModulus"/><Universal:time/><Mod><Arg Name="CircleDivisor"/>
        </Mod></Assign>

        <Syslog Severity="Important">CircleModulus calculated as: <Arg Name="CircleModulus"/><Units:hour/></Syslog>

        <Assign><Sequence/><Arg Name="CircleTimeout"/><Arg Name="CircleDivisor"/><Sub><Arg Name="CircleModulus"/></Sub></Assign>

        <Syslog Severity="Important">CircleTimeout updated to: <Arg Name="CircleTimeout"/><Units:hour/></Syslog>

    </Aggregate>

    <Aggregate Id="CircleOffshoreESPBuoy">

        <Description>
            Circle a waypoint at the nominal location of the Offshore ESP buoy
            until timeout specified by CircleDivisor and current time.
        </Description>

        <When>
            <Called/>
        </When>

        <Call RefId="UpdateCircleTimeout"/>

        <Aggregate Id="TellMeHowLong">

            <Description>
                Convenience aggregate to notify operator of estimated circling
                time _before_ starting to circle the waypoint. May be commented
                out, since the Timeout in CircleWaypointRepeatedly will still
                achieve the desired behavior.
            </Description>

            <Sequence/>

            <Syslog Severity="Important">CircleOffshoreESPBuoy should complete in <Arg Name="CircleTimeout"/><Units:minute/>, due to timeout in CircleWrapper aggregate.</Syslog>

            <Call RefId="SurfaceCommsShort"/>

        </Aggregate>

        <Assign><Sequence/><Arg Name="CircleWaypointRepeatedly:Timeout"/><Arg Name="CircleTimeout"/></Assign>

        <Assign><Sequence/><Arg Name="CircleWaypointRepeatedly:Radius"/><Arg Name="CircleRadius"/></Assign>

        <Assign><Sequence/><Arg Name="CircleWaypointRepeatedly:CenterLat"/><Arg Name="WpESPOffshoreLat"/></Assign>

        <Assign><Sequence/><Arg Name="CircleWaypointRepeatedly:CenterLon"/><Arg Name="WpESPOffshoreLon"/></Assign>

<!-- Use default values for: Timeout, MaxError, TurnToPort, KwpHeading -->

        <Call RefId="CircleWaypointRepeatedly"/>

        <Syslog Severity="Important">************* Finished circling.</Syslog>

        <Call RefId="SurfaceComms"/>

    </Aggregate>

    <Aggregate Id="Lap">

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

        <When>
            <Called/>
        </When>

        <Timeout Duration="LapTimeout"/>

<!-- TODO do we want/need specific ResumeInshore and ResumeOffshore flags? -->

        <Guidance:Waypoint Id="WpESPOffshore">
            <Sequence/>
            <Setting><Guidance:Waypoint.latitude/><Arg Name="WpESPOffshoreLat"/></Setting>
            <Setting><Guidance:Waypoint.longitude/><Arg Name="WpESPOffshoreLon"/></Setting>
        </Guidance:Waypoint>

        <Call RefId="SurfaceCommsShort"/>

        <Guidance:Waypoint Id="WpESPInshore">
            <Sequence/>
            <Setting><Guidance:Waypoint.latitude/><Arg Name="WpESPInshoreLat"/></Setting>
            <Setting><Guidance:Waypoint.longitude/><Arg Name="WpESPInshoreLon"/></Setting>
        </Guidance:Waypoint>

        <Call RefId="SurfaceCommsShort"/>

        <Guidance:Waypoint Id="WpESPOffshore">
            <Sequence/>
            <Setting><Guidance:Waypoint.latitude/><Arg Name="WpESPOffshoreLat"/></Setting>
            <Setting><Guidance:Waypoint.longitude/><Arg Name="WpESPOffshoreLon"/></Setting>
        </Guidance:Waypoint>

        <Call RefId="SurfaceCommsShort"/>

    </Aggregate>

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

    <Aggregate Id="Approach">

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

        <Sequence/>

        <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="WpApproach">
            <Sequence/>
            <Setting><Guidance:Waypoint.latitude/><Arg Name="WpESPOffshoreLat"/></Setting>
            <Setting><Guidance:Waypoint.longitude/><Arg Name="WpESPOffshoreLon"/></Setting>
        </Guidance:Waypoint>

        <Call RefId="SurfaceCommsSplit"/>

    </Aggregate>

    <Aggregate Id="ScienceOps">

        <Description>
            Circle then lap many times, collecting science data.
        </Description>

        <Sequence Repeat="Repeat"/>

        <Insert Filename="Insert/Science.xml" Id="Science">
            <Description>
                Get science data, including PeakDetectChl
            </Description>
            <RedefineArg Name="PeakDetectChlActive"><True/></RedefineArg>
        </Insert>

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

        <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:AltitudeEnvelope>
            <Parallel/>
            <Setting><Guidance:AltitudeEnvelope.minAltitude/><Arg Name="YoYoMinAltitude"/></Setting>
            <Setting><Guidance:AltitudeEnvelope.upPitch/><Arg Name="YoYoUpPitch"/></Setting>
        </Guidance:AltitudeEnvelope>

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

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

        <Call RefId="CircleOffshoreESPBuoy"/>

        <Call RefId="Lap"/>

    </Aggregate>

</Mission>
