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

    <Description>
        Vehicle yo-yo's to the specified waypoint, with science turned on.
    </Description>

    <DefineArg Name="InitialLatitude"><Description>
        Latitude to initialize virtual drifter at.
    </Description><Units:degree/><Value>36.78</Value></DefineArg>

    <DefineArg Name="InitialLongitude"><Description>
        Longitude to initialize virtual drifter at.
    </Description><Units:degree/><Value>-121.85</Value></DefineArg>

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

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

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

    <DefineArg Name="YoYoMinDepth"><Description>
        Minimum depth while performing the YoYo behavior.
    </Description><Units:meter/><Value>3.0</Value></DefineArg>

    <DefineArg Name="YoYoMaxDepth"><Description>
        Maximum depth while performing the YoYo behavior.
    </Description><Units:meter/><Value>50.0</Value></DefineArg>

    <DefineArg Name="YoYoUpPitch"><Description>
        Vehicle up pitch while performing the YoYo behavior.
    </Description><Units:degree/><Value>20.0</Value></DefineArg>

    <DefineArg Name="YoYoDownPitch"><Description>
        Vehicle down pitch while performing the YoYo behavior.
    </Description><Units:degree/><Value>-20.0</Value></DefineArg>

    <DefineArg Name="CircleMaxError"><Units:meter/><Value>50</Value></DefineArg>

    <DefineArg Name="CircleRadius"><Units:meter/><Value>250</Value></DefineArg>

    <DefineArg Name="CircleTurnToPort"><True/></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="NeedCommsTime"><Description>
        Elapsed time after previous surface communications when vehicle will
        begin to ascend for additional surface communications, if the vehicle
        has a positive pitch. If the vehicle has a negative pitch, it will wait
        up to 10 minutes more to begin ascent.
    </Description><Units:minute/><Value>60</Value></DefineArg>

    <DefineArg Name="Speed"><Description>
        Speed while performing the YoYo behavior.
    </Description><Units:meter_per_second/><Value>1</Value></DefineArg>

    <DefineArg Name="Timeout"><Description>
        Maximum length of mission
    </Description><Units:hour/><Value>6</Value></DefineArg>

    <DefineArg Name="BuoyancyNeutral"><Description>
        Buoyancy bladder position while performing the YoYo behavior. 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>

    <Timeout Duration="Timeout"/>

    <Aggregate Id="AltEnvWrapper">

        <When>
            <Arg Name="MinAltitude"/>
            <Gt><Units:meter/><Value>0</Value></Gt>
        </When>

        <Guidance:AltitudeEnvelope>
            <Description>
                Safety behavior that keeps vehicle off sea floor
            </Description>
            <Parallel/>
            <Setting><Guidance:AltitudeEnvelope.minAltitude/><Arg Name="MinAltitude"/></Setting>
        </Guidance:AltitudeEnvelope>

    </Aggregate>

    <Guidance:DepthEnvelope>
        <Description>
            Safety behavior that keeps vehicle from going too deep
        </Description>
        <Parallel/>
        <Setting><Guidance:DepthEnvelope.maxDepth/><Arg Name="MaxDepth"/></Setting>
    </Guidance:DepthEnvelope>

    <Aggregate Id="OffshoreWrapper">

        <When>
            <Arg Name="MinOffshore"/>
            <Gt><Units:meter/><Value>0</Value></Gt>
        </When>

        <Guidance:OffshoreEnvelope>
            <Description>
                Safety behavior that keeps vehicle away from the shore
            </Description>
            <Parallel/>
            <Setting><Guidance:OffshoreEnvelope.minOffshore/><Arg Name="MinOffshore"/></Setting>
        </Guidance:OffshoreEnvelope>

    </Aggregate>

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

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

    <Syslog Severity="Debug">NeedComms:DiveInterval is <Arg Name="NeedComms:DiveInterval"/><Units:minute/>, should be <Arg Name="NeedCommsTime"/><Units:minute/>.</Syslog>

    <Syslog Severity="Debug">NeedComms:SurfaceSpeed is <Arg Name="NeedComms:SurfaceSpeed"/><Units:meter_per_second/>, should be <Arg Name="Speed"/><Units:meter_per_second/>.</Syslog>

    <Insert Filename="Insert/Science.xml">
        <Description>
            If we're executing here or below, lets' get science data!
        </Description>
        <RedefineArg Name="PeakDetectChlActive"><True/></RedefineArg>
    </Insert>

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

    <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.upPitch/><Arg Name="YoYoUpPitch"/></Setting>
        <Setting><Guidance:DepthEnvelope.downPitch/><Arg Name="YoYoDownPitch"/></Setting>
    </Guidance:DepthEnvelope>

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

<!-- (re)initialize the virtual surface drifter -->

    <Estimation:ReinitializeHFRCMVirtualSurfaceDrifter>
        <Sequence/>
        <Setting><Estimation:ReinitializeHFRCMVirtualSurfaceDrifter.initial_latitude_setting/><Arg Name="InitialLatitude"/></Setting>
        <Setting><Estimation:ReinitializeHFRCMVirtualSurfaceDrifter.initial_longitude_setting/><Arg Name="InitialLongitude"/></Setting>
    </Estimation:ReinitializeHFRCMVirtualSurfaceDrifter>

    <Syslog Severity="Important">Reinitialized virtual drifter location.</Syslog>

<!-- go circle the virtual drifter -->

    <Aggregate Id="CircleDrifter">

        <Description>
            Circle virtual drifter.
        </Description>

        <Parallel/>

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

        <Guidance:Circle>
            <Sequence/>
            <Setting><Guidance:Circle.latitude/><CustomUri Uri="HFRCMVirtualSurfaceDrifter.latitude"/></Setting>
            <Setting><Guidance:Circle.longitude/><CustomUri Uri="HFRCMVirtualSurfaceDrifter.longitude"/></Setting>
            <Setting><Guidance:Circle.radius/><Arg Name="CircleRadius"/></Setting>
            <Setting><Guidance:Circle.maxError/><Arg Name="CircleMaxError"/></Setting>
            <Setting><Guidance:Circle.turnToPort/><Arg Name="CircleTurnToPort"/></Setting>
        </Guidance:Circle>

    </Aggregate>

    <Guidance:Wait>
        <Sequence/>
    </Guidance:Wait>

</Mission>
