<?xml version="1.0" encoding="UTF-8"?>
<Aggregate xmlns="Tethys"
       xmlns:Control="Tethys/Control"
       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/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="CircleWaypointRepeatedly">

    <DefineArg Name="Timeout"><Description>
        Timeout for this behavior.
    </Description><Units:minute/><Value>90</Value></DefineArg>

    <DefineArg Name="LapTimeout"><Description>
        Timeout for each individual lap.
    </Description><Units:minute/><Value>240</Value></DefineArg>

    <DefineArg Name="MaxLaps"><Description>
        Maximum number of repeat circles.
    </Description><Units:count/><Value>32</Value></DefineArg>

    <DefineArg Name="CenterLat"><Description>
        Latitude of center of circle.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="CenterLon"><Description>
        Longitude of center of circle.
    </Description><Units:degree/><Value>NaN</Value></DefineArg>

    <DefineArg Name="Radius"><Description>
        Radius of circle around CenterLat and CenterLon
    </Description><Units:meter/><Value>250</Value></DefineArg>

    <DefineArg Name="MaxError"><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>250</Value></DefineArg>

    <DefineArg Name="TurnToPort"><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="FloatTime"><Units:second/><Value>60</Value></DefineArg>

    <DefineArg Name="Pitch"><Units:degree/><Value>20</Value></DefineArg>

    <DefineArg Name="Speed"><Units:meter_per_second/><Value>1</Value></DefineArg>

    <When>
        <Called/>
    </When>

    <Timeout Duration="Timeout"/>

    <Syslog Severity="Important">Will circle ( Latitude: <Arg Name="CenterLat"/><Units:degree/>, Longitude: <Arg Name="CenterLon"/><Units:degree/>) at radius: <Arg Name="Radius"/><Units:meter/>for up to: <Arg Name="Timeout"/><Units:minute/></Syslog>

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

    <Aggregate Id="Repeater">

        <Sequence Repeat="MaxLaps"/>

        <Aggregate Id="Lap">

            <Sequence/>

            <Timeout Duration="LapTimeout"/>

            <Syslog Severity="Important">Circling...</Syslog>

            <Guidance:Circle>
                <Sequence/>
                <Setting><Guidance:Circle.latitude/><Arg Name="CenterLat"/></Setting>
                <Setting><Guidance:Circle.longitude/><Arg Name="CenterLon"/></Setting>
                <Setting><Guidance:Circle.radius/><Arg Name="Radius"/></Setting>
                <Setting><Guidance:Circle.maxError/><Arg Name="MaxError"/></Setting>
                <Setting><Guidance:Circle.turnToPort/><Arg Name="TurnToPort"/></Setting>
            </Guidance:Circle>

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

        </Aggregate>

        <Aggregate Id="GTS">

            <Sequence/>

            <Guidance:GoToSurface>
                <Progression/>
                <Setting><Guidance:GoToSurface.pitch/><Arg Name="Pitch"/></Setting>
                <Setting><Guidance:GoToSurface.speed/><Arg Name="Speed"/></Setting>
            </Guidance:GoToSurface>

            <Aggregate Id="CheckIn">

                <Sequence/>

                <ReadDatum>
                    <Universal:latitude_fix/>
                </ReadDatum>

                <ReadDatum>
                    <Timeout Duration="FloatTime"/>
                    <Universal:platform_communications/>
                </ReadDatum>

                <ReadDatum>
                    <Universal:latitude_fix/>
                </ReadDatum>

            </Aggregate>

        </Aggregate>

    </Aggregate>

</Aggregate>
