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

    <Description>
        This mission yoyos in a circle around a specified location.
    </Description>

<!-- Mission-wide settings -->

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

    <DefineArg Name="NeedCommsTime"><Description>
        Transit surface communications. Elapsed time after previous surface
        comms when vehicle will begin to ascend for additional surface
        communications
    </Description><Units:minute/><Value>45</Value></DefineArg>

    <DefineArg Name="LapRepeat"><Description>
        Number of times to repeat the waypoint trajectory. NOTE: When setting
        the LapRepeat > 1 and running WPs in a loop, omit last/return waypoint
        by setting LatX/LonX to NaN.
    </Description><Units:count/><Value>1</Value></DefineArg>

    <DefineArg Name="StartWaypoint"><Description>
        Waypoint number to start the the waypoint trajectory with. The mission
        will start with the specified waypoint and cycle through all the
        subsequent waypoints.
    </Description><Units:count/><Value>1</Value></DefineArg>

    <DefineArg Name="TransitYoYoMinDepth"><Description>
        Minimum YoYo depth while transiting to waypoint.
    </Description><Units:meter/><Value>5</Value></DefineArg>

    <DefineArg Name="TransitYoYoMaxDepth"><Description>
        Maximum YoYo depth while while transiting to waypoint.
    </Description><Units:meter/><Value>50</Value></DefineArg>

    <DefineArg Name="SampleYoYoMinDepth"><Description>
        Minimum YoYo depth while sampling.
    </Description><Units:meter/><Value>5</Value></DefineArg>

    <DefineArg Name="SampleYoYoMaxDepth"><Description>
        Maximum YoYo depth while sampling.
    </Description><Units:meter/><Value>50</Value></DefineArg>

    <DefineArg Name="SampleTimeout"><Description>
        Max time duration allowed for a single sample.
    </Description><Units:hour/><Value>3</Value></DefineArg>

    <DefineArg Name="SampleRepeat"><Description>
        Number of ESP samples to take at each station (vehicle will surface for
        comms between samples).
    </Description><Units:count/><Value>1</Value></DefineArg>

    <DefineArg Name="SampleESP"><Description>
        When true, aqurie an ESP sample.
    </Description><True/></DefineArg>

    <DefineArg Name="CartridgeType"><Description>
        Specifies ESP cartridge type code (defaults to archive_bac: -6)
    </Description><Units:count/><Value>-6</Value></DefineArg>

    <DefineArg Name="SampleCANONSampler"><Description>
        When true, aqurie a CANON Sampler water sample.
    </Description><False/></DefineArg>

    <DefineArg Name="CANONSamplerDelay"><Description>
        How long to delay a CANON Sampler sample after an ESP sample is
        intiated.
    </Description><Units:minute/><Value>10</Value></DefineArg>

<!-- You probably need to change these. -->

    <DefineArg Name="Lat1"><Description>
        The latitude of the center of the circle (default: C1).
    </Description><Units:degree/><Value>36.797</Value></DefineArg>

    <DefineArg Name="Lon1"><Description>
        The longitude of the center of the circle (default: C1).
    </Description><Units:degree/><Value>-121.847</Value></DefineArg>

    <DefineArg Name="Lat2"><Description>
        The latitude of the center of the circle (default: NW).
    </Description><Units:degree/><Value>36.8219</Value></DefineArg>

    <DefineArg Name="Lon2"><Description>
        The longitude of the center of the circle (default: NW).
    </Description><Units:degree/><Value>-121.8856</Value></DefineArg>

    <DefineArg Name="Lat3"><Description>
        The latitude of the center of the circle (default: SW).
    </Description><Units:degree/><Value>36.766</Value></DefineArg>

    <DefineArg Name="Lon3"><Description>
        The longitude of the center of the circle (default: SW).
    </Description><Units:degree/><Value>-121.8649</Value></DefineArg>

    <DefineArg Name="Lat4"><Description>
        The latitude of the center of the circle (spare). NOTE: Omit return
        waypoint if LapRepeat > 1 by setting it to NaN.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Lon4"><Description>
        The longitude of the center of the circle (spare). NOTE: Omit return
        waypoint if LapRepeat > 1 by setting it to NaN.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

<!-- You probably do not need to change these. -->

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

    <DefineArg Name="YoYoMinAltitude"><Description>
        Minimum altitude while performing the YoYo behavior (for
        bottom-terminated YoYos).
    </Description><Units:meter/><Value>7</Value></DefineArg>

    <DefineArg Name="YoYoPitch"><Description>
        Pitch (plus and minus) for yo-yo behavior.
    </Description><Units:degree/><Value>20</Value></DefineArg>

<!-- You are even less likely to need to change these. -->

    <DefineArg Name="Radius"><Description>
        Radius to circle at
    </Description><Units:meter/><Value>15</Value></DefineArg>

    <DefineArg Name="CircleMaxError"><Description>
        If this distance away from the circle, drive straight towards (or away
        from the center). Otherwise, try to reduce distance from the ideal
        circle.
    </Description><Units:meter/><Value>100</Value></DefineArg>

    <DefineArg Name="CircleTurnToPort"><Description>
        If true, vehicle turns to the left around the center point. If false,
        vehicle turns to the right.
    </Description><False/></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="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>

<!-- Internal mission variables (don't change) -->

    <DefineOutput Name="Lat"><Description>
        Mission variable (don't change). The mission will update this var as it
        progresses through the specified waypoint. The latitude of the center of
        the circle.
    </Description><Units:degree/><Value>36.806966</Value></DefineOutput>

    <DefineOutput Name="Lon"><Description>
        Mission variable (don't change). The mission will update this var as it
        progresses through the specified waypoint. The longitude of the center
        of the circle.
    </Description><Units:degree/><Value>-121.824326</Value></DefineOutput>

    <DefineOutput Name="WaypointCycle"><Description>
        Mission variable (don't change). The mission will update this var as it
        progresses through the specified waypoint.
    </Description><Units:count/><Value>4</Value></DefineOutput>

    <DefineOutput Name="WaypointObjective"><Description>
        Mission variable (don't change). The mission will update this var as it
        progresses through the specified waypoint. The mission's current
        waypoint objective.
    </Description><Units:count/><Value>1</Value></DefineOutput>

<!-- Missions should almost always start with a timeout -->

    <Timeout Duration="MissionTimeout"/>

    <Insert Filename="Insert/Science.xml">
        <Description>
            Most missions will run the science sensors. If you don't place this
            aggregate above NeedComms, science instruments get turned off on the
            last upcast and while floating on the surface.
        </Description>
    </Insert>

<!-- Most missions should use the NeedComms aggregate. -->

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

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

<!-- Missions should almost always start with standard safety envelopes; most missions should not expose the parameters of these envelopes. -->

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

<!-- Sampling directives -->

    <Aggregate Id="CheckSamplingSettings">

        <Sequence/>

        <Syslog Severity="Info">Checking sampling settings.</Syslog>

        <Aggregate Id="CheckESP">

            <When>
                <Arg Name="SampleESP"/>
                <And><Not><Science:ESPComponent.loadAtStartup/></Not></And>
            </When>

            <Syslog Severity="Critical">Cannot service ESP sample request. ESPComponent is not loaded.</Syslog>

        </Aggregate>

        <Aggregate Id="CheckCANONSampler">

            <When>
                <Arg Name="SampleCANONSampler"/>
                <And><Not><Science:CANONSampler.loadAtStartup/></Not></And>
            </When>

            <Syslog Severity="Critical">Cannot service CANONSampler water sample request. CANONSampler component is not loaded.</Syslog>

        </Aggregate>

    </Aggregate>

    <Insert Filename="Insert/Sample.xml">
        <RedefineArg Name="SampleTimeout"><Arg Name="SampleTimeout"/></RedefineArg>
    </Insert>

<!-- Acquire GPS and check for additional instructions. -->

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

<!-- Many missions will keep mass position and buoyancy volume fixed at defaults. -->

    <Guidance:Pitch>
        <Parallel/>
        <Setting><Guidance:Pitch.massPosition/><Control:VerticalControl.massDefault/></Setting>
    </Guidance:Pitch>

    <Guidance:Buoyancy>
        <Parallel/>
        <Setting><Guidance:Buoyancy.position/><Control:VerticalControl.buoyancyNeutral/></Setting>
    </Guidance:Buoyancy>

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

    <Guidance:DepthEnvelope>
        <Description>
            Another depth envelope for the YoYo behavior. This envelope should
            fall within the limits of the standard safety envelopes in
            Insert/StandardEnvelopes.xml in order to avoid commanding high pitch
            angles for depth-terminated YoYos.
        </Description>
        <Parallel/>
        <Setting><Guidance:DepthEnvelope.pitch/><Arg Name="YoYoPitch"/></Setting>
    </Guidance:DepthEnvelope>

    <Guidance:AltitudeEnvelope>
        <Description>
            Another altitude envelope for the YoYo behavior. This envelope
            should fall within the limits of the standard safety envelopes in
            Insert/StandardEnvelopes.xml in order to avoid commanding high pitch
            angles for bottom-terminated YoYos.
        </Description>
        <Parallel/>
        <Setting><Guidance:AltitudeEnvelope.minAltitude/><Arg Name="YoYoMinAltitude"/></Setting>
        <Setting><Guidance:AltitudeEnvelope.pitch/><Arg Name="YoYoPitch"/></Setting>
    </Guidance:AltitudeEnvelope>

    <Guidance:YoYo>
        <Parallel/>
        <Setting><Guidance:YoYo.pitch/><Arg Name="YoYoPitch"/></Setting>
    </Guidance:YoYo>

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

    <Aggregate Id="UpdateWaypoint">

        <When>
            <Called/>
        </When>

        <Aggregate Id="RestartCycle">

            <Sequence/>

            <Break><Arg Name="WaypointObjective"/>
                <Le><Arg Name="WaypointCycle"/></Le>
            </Break>

            <Assign><Sequence/><Arg Name="WaypointObjective"/><Units:count/><Value>1</Value></Assign>

        </Aggregate>

        <Syslog Severity="Info">Updating WP to station objective  <Arg Name="WaypointObjective"/><Units:count/></Syslog>

        <Aggregate Id="WP1">

            <Sequence/>

            <Break>
                <Not><Arg Name="WaypointObjective"/><Eq><Units:count/><Value>1</Value></Eq></Not>
            </Break>

            <Assign><Sequence/><Arg Name="Lat"/><Arg Name="Lat1"/></Assign>

            <Assign><Sequence/><Arg Name="Lon"/><Arg Name="Lon1"/></Assign>

        </Aggregate>

        <Aggregate Id="WP2">

            <Sequence/>

            <Break>
                <Not><Arg Name="WaypointObjective"/><Eq><Units:count/><Value>2</Value></Eq></Not>
            </Break>

            <Assign><Sequence/><Arg Name="Lat"/><Arg Name="Lat2"/></Assign>

            <Assign><Sequence/><Arg Name="Lon"/><Arg Name="Lon2"/></Assign>

        </Aggregate>

        <Aggregate Id="WP3">

            <Sequence/>

            <Break>
                <Not><Arg Name="WaypointObjective"/><Eq><Units:count/><Value>3</Value></Eq></Not>
            </Break>

            <Assign><Sequence/><Arg Name="Lat"/><Arg Name="Lat3"/></Assign>

            <Assign><Sequence/><Arg Name="Lon"/><Arg Name="Lon3"/></Assign>

        </Aggregate>

        <Aggregate Id="WP4">

            <Sequence/>

            <Break>
                <Not><Arg Name="WaypointObjective"/><Eq><Units:count/><Value>4</Value></Eq></Not>
            </Break>

            <Assign><Sequence/><Arg Name="Lat"/><Arg Name="Lat4"/></Assign>

            <Assign><Sequence/><Arg Name="Lon"/><Arg Name="Lon4"/></Assign>

        </Aggregate>

    </Aggregate>

    <Aggregate Id="Lap">

        <Sequence Repeat="LapRepeat"/>

        <Syslog Severity="Important">Starting Lap</Syslog>

        <Syslog Severity="Important">Lat[1] = <Arg Name="Lat1"/><Units:degree/> Lon[1] = <Arg Name="Lon1"/><Units:degree/></Syslog>

        <Syslog Severity="Important">Lat[2] = <Arg Name="Lat2"/><Units:degree/> Lon[2] = <Arg Name="Lon2"/><Units:degree/></Syslog>

        <Syslog Severity="Important">Lat[3] = <Arg Name="Lat3"/><Units:degree/> Lon[3] = <Arg Name="Lon3"/><Units:degree/></Syslog>

        <Syslog Severity="Important">Lat[4] = <Arg Name="Lat4"/><Units:degree/> Lon[4] = <Arg Name="Lon4"/><Units:degree/></Syslog>

<!-- Initialize the waypoint sequence -->

        <Assign><Sequence/><Arg Name="WaypointObjective"/><Arg Name="StartWaypoint"/></Assign>

        <Call Id="WaypointInit" RefId="UpdateWaypoint"/>

<!-- Run through the waypoint sequence -->

        <Aggregate Id="WaypointTrajectory">

            <Sequence Repeat="WaypointCycle"/>

<!-- Navigate to the next station and execute sample upon arrival -->

            <Aggregate Id="Station">

                <Sequence/>

                <Break>
<!-- Skip invalid waypoints -->
                    <IsNaN><Arg Name="Lat"/>
                    </IsNaN>
                    <Or><IsNaN><Arg Name="Lon"/>
                    </IsNaN></Or>
                </Break>

<!-- Navigate to the station waypoint with specified transit yoyo depths -->

                <Aggregate Id="GoToStation">

                    <Sequence/>

<!-- Ensure the nominal surface comms interval is active -->

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

                    <Guidance:DepthEnvelope>
                        <Parallel/>
                        <Setting><Guidance:DepthEnvelope.minDepth/><Arg Name="TransitYoYoMinDepth"/></Setting>
                        <Setting><Guidance:DepthEnvelope.maxDepth/><Arg Name="TransitYoYoMaxDepth"/></Setting>
                    </Guidance:DepthEnvelope>

                    <Syslog Severity="Important">Navigating to WP <Arg Name="Lat"/><Units:degree/>, <Arg Name="Lon"/><Units:degree/></Syslog>

                    <Guidance:Waypoint>
                        <Sequence/>
                        <Setting><Guidance:Waypoint.latitude/><Arg Name="Lat"/></Setting>
                        <Setting><Guidance:Waypoint.longitude/><Arg Name="Lon"/></Setting>
                    </Guidance:Waypoint>

                </Aggregate>

<!-- Circle the station with specified sample yoyo depths and acquire the ESP samples -->

                <Aggregate Id="SampleOnStation">

                    <Sequence Repeat="SampleRepeat"/>

                    <Break>
                        <Not><Arg Name="SampleESP"/></Not>
                        <And><Not><Arg Name="SampleCANONSampler"/></Not></And>
                    </Break>

                    <Aggregate Id="ProfileSample">

                        <Sequence/>

<!-- Surface for comms before initiating the sample sequence -->

                        <Call Id="PreSampleComms" RefId="NeedComms"/>

<!-- Set the surface comms interval to the sample timeout to avoid interruptions while sampling -->

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

                        <Aggregate Id="CircleSample">

                            <Sequence/>

                            <Timeout Duration="SampleTimeout"/>

                            <Guidance:DepthEnvelope>
                                <Parallel/>
                                <Setting><Guidance:DepthEnvelope.minDepth/><Arg Name="SampleYoYoMinDepth"/></Setting>
                                <Setting><Guidance:DepthEnvelope.maxDepth/><Arg Name="SampleYoYoMaxDepth"/></Setting>
                            </Guidance:DepthEnvelope>

                            <Guidance:Circle>
                                <Parallel/>
                                <Setting><Guidance:Circle.latitude/><Arg Name="Lat"/></Setting>
                                <Setting><Guidance:Circle.longitude/><Arg Name="Lon"/></Setting>
                                <Setting><Guidance:Circle.radius/><Arg Name="Radius"/></Setting>
                                <Setting><Guidance:Circle.maxError/><Arg Name="CircleMaxError"/></Setting>
                                <Setting><Guidance:Circle.turnToPort/><Arg Name="CircleTurnToPort"/></Setting>
                            </Guidance:Circle>

                            <Aggregate Id="WaitForDepth">

                                <Sequence/>

                                <Break>
                                    <Universal:depth/>
                                    <Ge><Arg Name="SampleYoYoMinDepth"/></Ge>
                                </Break>

                                <Syslog Severity="Info">Waiting to achieve min depth of <Arg Name="SampleYoYoMinDepth"/><Units:meter/></Syslog>

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

                            </Aggregate>

<!-- Configure sampling sequance -->

                            <Assign><Sequence/><Arg Name="Sample:SampleCANONSampler"/><Arg Name="SampleCANONSampler"/></Assign>

                            <Assign><Sequence/><Arg Name="Sample:CANONSamplerDelay"/><Arg Name="CANONSamplerDelay"/></Assign>

                            <Assign><Sequence/><Arg Name="Sample:ESPCartridgeType"/><Arg Name="CartridgeType"/></Assign>

                            <Assign><Sequence/><Arg Name="Sample:SampleESP"/><Arg Name="SampleESP"/></Assign>

<!-- Trigger sampling sequance -->

                            <Call Id="TriggerSample" RefId="Sample"/>

                        </Aggregate>

<!-- <Aggregate Id="CircleSample"> -->

                    </Aggregate>

<!-- <Aggregate Id="ProfileSample"> -->

                </Aggregate>

<!-- <Aggregate Id="SampleOnStation"> -->

<!-- Surface for comms after the sample sequence is complete -->

                <Call Id="PostSampleComms" RefId="NeedComms"/>

<!-- Restore the nominal surface comms interval -->

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

            </Aggregate>

<!-- Move on to the next waypoint -->

            <Assign><Sequence/><Arg Name="WaypointObjective"/><Arg Name="WaypointObjective"/><Add><Units:count/><Value>1</Value></Add></Assign>

            <Call Id="NextWaypoint" RefId="UpdateWaypoint"/>

        </Aggregate>

    </Aggregate>

</Mission>
