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

    <DefineBehavior Name="LawnMower" Language="Lua">

        <DefineSetting Name="forwardBearingSet"><CustomUri Uri="forwardBearing"/>
            <Description>
                Bearing followed when moving forward in the survey.
            </Description><Units:radian/>
            <DefaultValue>0</DefaultValue>
        </DefineSetting>

        <DefineSetting Name="forwardMovesSet"><CustomUri Uri="forwardMoves"/>
            <Description>
                Number of times to move forward in the survey.
            </Description><Units:count/>
            <DefaultValue>4</DefaultValue>
        </DefineSetting>

        <DefineSetting Name="leftWidthSet"><CustomUri Uri="leftWidth"/>
            <Description>
                Desired width of each leg, to the left of the starting point.
            </Description><Units:meter/>
            <DefaultValue>0</DefaultValue>
        </DefineSetting>

        <DefineSetting Name="rightWidthSet"><CustomUri Uri="rightWidth"/>
            <Description>
                Desired width of each leg, to the right of the starting point.
            </Description><Units:meter/>
            <DefaultValue>0</DefaultValue>
        </DefineSetting>

        <DefineSetting Name="spacingSet"><CustomUri Uri="spacing"/>
            <Description>
                Desired distance to move forward at the end of each leg.
            </Description><Units:meter/>
            <DefaultValue>1000</DefaultValue>
        </DefineSetting>

        <DefineSetting Name="startToLeftSet"><CustomUri Uri="startToLeft"/>
            <Description>
                Set to true to start survey by driving to the left. Otherwise
                drive to the right. If this is set to true and leftWidth is zero
                or unset, start moving forward along the left edge. If this is
                not set to true and rightWidth is zero or unset, start moving
                forward along the right edge.
            </Description><Units:bool/>
            <DefaultValue>0</DefaultValue>
        </DefineSetting>

        <Construct>
            <Script><![CDATA[
           atBearing = function(lat,lon,distance,bearing)
               r = distance / 6371000.0 -- EARTH_RADIUS
               newLat = math.asin( math.sin( lat ) * math.cos( r )
                        + math.cos( lat ) * math.sin( r ) * math.cos( bearing ) )
               newLon = lon + math.atan2( math.sin( bearing ) * math.sin( r ) * math.cos( lat ),
                        math.cos( r ) - math.sin( lat ) * math.sin( newLat ) )
               return newLat,newLon
           end
           phaseFunctions = {
               --[[outward to the left]] function(lat,lon,bear,left,right,spacing) lat,lon = atBearing(lat,lon,left,bear-math.pi/2) return lat, lon, bear-math.pi/2, left>0 end,
               --[[forward on the left]] function(lat,lon,bear,left,right,spacing) lat,lon = atBearing(lat,lon,spacing,bear) return lat, lon, bear, true end,
               --[[inward from the left]] function(lat,lon,bear,left,right,spacing) lat,lon = atBearing(lat,lon,left,bear+math.pi/2) return lat, lon, bear+math.pi/2, left>0 end,
               --[[outward to the right]] function(lat,lon,bear,left,right,spacing) lat,lon = atBearing(lat,lon,right,bear+math.pi/2) return lat, lon, bear+math.pi/2, right>0 end,
               --[[forward on the right]] function(lat,lon,bear,left,right,spacing) lat,lon = atBearing(lat,lon,spacing,bear) return lat, lon, bear, true end,
               --[[inward from the right]] function(lat,lon,bear,left,right,spacing) lat,lon = atBearing(lat,lon,right,bear-math.pi/2) return lat, lon, bear-math.pi/2, right>0 end
           }
           advancePhase = function(phase,lat,lon,bear,left,right,spacing)
               local done = false
               local newLat = lat
               local newLon = lon
               local newBear = bear;
               local perpSlope = 0
               local startOvLn = false;
               local innerSteps = 0;
               while not done do
                   phase=phase + 1
                   if phase > 6 then phase = 1 end
                   newLat,newLon,newBear,done = phaseFunctions[phase](lat,lon,bear,left,right,spacing)
                   innerSteps = innerSteps + 1;
                   if innerSteps > 6 then 
                       print( "!!!!!!!!!! Too many steps" )
                       done = true
                   end
               end
               if newLat ~= lat then
                   perpSlope = -( newLon - lon ) / ( newLat - lat )
                   startOvLn = lat > ( newLat + ( ( lon - newLon ) * perpSlope ) )
               else
                   perpSlope     = nil
                   startOvLn = lon > newLon
               end
               return phase,newLat,newLon,newBear,perpSlope,startOvLn
           end
        ]]></Script>
        </Construct>

        <Initialize>
            <Input Name="latitude"><Universal:latitude/><Units:radian/><DefaultValue>NaN</DefaultValue></Input>
            <Input Name="longitude"><Universal:longitude/><Units:radian/><DefaultValue>NaN</DefaultValue></Input>
            <Script><![CDATA[
           if startToLeft then 
               phase = 0
           else 
               phase = 3
           end
           forwardCount = 0
           phase,goalLat,goalLon,goalBear,perpendicularSlope,startOverLine=advancePhase(phase,latitude,longitude,forwardBearingSet,leftWidthSet,rightWidthSet,spacingSet)
        ]]></Script>
        </Initialize>

        <Run>
            <Input Name="latitude"><Universal:latitude/><Units:radian/><DefaultValue>NaN</DefaultValue></Input>
            <Input Name="longitude"><Universal:longitude/><Units:radian/><DefaultValue>NaN</DefaultValue></Input>
            <Output><Control:HorizontalControl.horizontalMode/><Units:enum/></Output>
            <Output><Control:HorizontalControl.latitudeCmd/><Units:radian/></Output>
            <Output><Control:HorizontalControl.longitudeCmd/><Units:radian/></Output>
            <Output><Control:HorizontalControl.bearingCmd/><Units:radian/></Output>
            <Script><![CDATA[
           local isOverLine = false
           if perpendicularSlope ~= nil then
               isOverLine = latitude > (goalLat
                             + ( ( longitude - goalLon ) * perpendicularSlope))
           else
               isOverLine = longitude > goalLon;
           end
           if isOverLine ~= startOverLine then
               if phase == 3 or (leftWidthSet == 0 and phase == 2) 
                       or phase == 6 or (rightWidthSet == 0 and phase == 5) 
                       or (leftWidthSet==0 and rightWidthSet==0 and (phase == 2 or phase == 5)) then
                   forwardCount = forwardCount + 1;
               end
               phase,goalLat,goalLon,goalBear,perpendicularSlope,startOverLine=
                  advancePhase(phase,goalLat,goalLon,forwardBearingSet,leftWidthSet,rightWidthSet,spacingSet)
           end
           -- 1.0 is the HorizontalControl.horizontalMode value for Waypoint     
           return 1.0, goalLat, goalLon, goalBear;
        ]]></Script>
        </Run>

        <IsSatisfied>
            <Script><![CDATA[
           if forwardMovesSet <= forwardCount then
               return true
           end
           return false
        ]]></Script>
        </IsSatisfied>

    </DefineBehavior>

<!-- some high priority behaviors -->

    <Guidance:AltitudeEnvelope>
        <Parallel/>
        <Setting><Guidance:AltitudeEnvelope.minAltitude/><Units:meter/><Value>20</Value></Setting>
    </Guidance:AltitudeEnvelope>

    <Guidance:OffshoreEnvelope>
        <Parallel/>
        <Setting><Guidance:OffshoreEnvelope.minOffshore/><Units:kilometer/><Value>1</Value></Setting>
        <Setting><Guidance:OffshoreEnvelope.maxOffshore/><Units:kilometer/><Value>50</Value></Setting>
    </Guidance:OffshoreEnvelope>

    <Guidance:DepthEnvelope>
        <Parallel/>
        <Setting><Guidance:DepthEnvelope.minDepth/><Units:meter/><Value>5</Value></Setting>
        <Setting><Guidance:DepthEnvelope.maxDepth/><Units:meter/><Value>300</Value></Setting>
    </Guidance:DepthEnvelope>

<!-- Initial GPS (and Iridium) Update -->

    <Aggregate Id="SurfaceOps">

        <Sequence/>

        <Guidance:GoToSurface>
            <Parallel/>
        </Guidance:GoToSurface>

        <Guidance:GoToSurface>
            <Sequence/>
        </Guidance:GoToSurface>

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

    </Aggregate>

    <Aggregate Id="LawnMowerAggregate">

        <Sequence/>

        <Guidance:SetSpeed>
            <Parallel/>
            <Setting><Guidance:SetSpeed.speed/><Units:meter_per_second/><Value>1</Value></Setting>
        </Guidance:SetSpeed>

        <CustomBehavior Name="LawnMower" >
            <Sequence Repeat="4"/>
            <Setting><CustomUri Uri="LawnMower.forwardBearing"/><Units:degree/><Value>-90</Value></Setting>
            <Setting><CustomUri Uri="LawnMower.leftWidth"/><Units:meter/><Value>250</Value></Setting>
            <Setting><CustomUri Uri="LawnMower.rightWidth"/><Units:meter/><Value>250</Value></Setting>
            <Setting><CustomUri Uri="LawnMower.spacing"/><Units:meter/><Value>100</Value></Setting>
        </CustomBehavior>

    </Aggregate>

</Mission>
