<?xml version="1.0" encoding="UTF-8"?>
<DefineBehavior xmlns="Tethys"
       xmlns:Control="Tethys/Control"
       xmlns:BehaviorScripts="Tethys/BehaviorScripts" 
       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/BehaviorScripts ./BehaviorScripts.xsd
                           Tethys/Units http://okeanids.mbari.org/tethys/Xml/Units.xsd
                           Tethys/Universal http://okeanids.mbari.org/tethys/Xml/Universal.xsd"
       Name="LawnMower" Language="Lua">

    <Description>
        Drives the vehicle in a pattern like a lawnmower search pattern.
        Settings allow forward progression along a bearing, to extend to the
        left and/or right of the start point, and to start to the left of right
        of the start point.
    </Description>

<!-- 
     After the optional Description, any number of DefineSetting entries follow.
     These are mapped to the Setting elements of the behavior when it is used.
     Note that the Name attribute is the name of the variable passed to the script
     Also note that the element following DefineSetting can be a URI defined in a 
     schema file, or a CustomUri element (See Demos/LawnMowerEmbedded.xml).
-->

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

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

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

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

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

    <DefineSetting Name="startToLeftSet">
        <BehaviorScripts:LawnMower.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>

<!-- 
     The optional Construct element defines a script that is run once during the 
     first initialization of the script. It takes no inputs, and provides not outputs
     except for an optional syslog entry string followed by a syslog severity string.
-->

    <Construct>
        <Description>
            Define functions for use in the behavior
        </Description>
        <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 
	                   gCritical = "Error in advancePhase, 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
	       return "Constructed LawnMower", "IMPORTANT"
	    ]]></Script>
    </Construct>

<!-- 
     The optional Initialize element defines a script that is run whenever
     the behavior is initialized.  It may have Input elements, which are provided
     as function arguments, followed by all the setting values.  It may also have 
     Output elements, which define how return values should be interpreted.  Note
     that if the last two return values are strings, then they are interpreted as
     a syslog string and syslog severity.
-->

    <Initialize>
<!-- 
     Note in the Input element below, the Name attribute is the name of the variable
     passed to the script
 -->
        <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
           gCritical = nil
           phase,goalLat,goalLon,goalBear,perpendicularSlope,startOverLine=advancePhase(phase,latitude,longitude,forwardBearingSet,leftWidthSet,rightWidthSet,spacingSet)
           if nil ~= gCritical then return gCritical, "CRITICAL" end
        ]]></Script>
    </Initialize>

<!-- 
     The mandatory Run element defines a script that is run each cycle while the 
     behavior is active.  It may have Input elements, which are provided
     as function arguments, followed by all the setting values.  It may also have 
     Output elements, which define how return values should be interpreted 
     (nil or missing values are ignored). Note that if the last two return values 
     are strings, then they are interpreted as a syslog string and syslog severity. 
     And the Run element may have OutputArg elements, which map to OutputArg 
     elements when the Behavior is used in a script. OutputArg values should follow
     any Output values in the script return value
-->

    <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>
<!-- 
        No OutputArg elements here, but there is one defined in the FiniteDifference.xml behavior
         -->
        <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
		   if nil ~= gCritical then return gCritical, "CRITICAL" end
           -- 1.0 is the HorizontalControl.horizontalMode value for Waypoint     
           return 1.0, goalLat, goalLon, goalBear;
        ]]></Script>
    </Run>

<!-- 
     The IsSatisfied element is required if the behavior will be defines a script 
     that may be run in Sequence or Progression.  It may have Input elements, which are provided
     as function arguments, followed by all the setting values.  It may not have 
     Output elements, and should only provide a boolean return that indicates that
     the behavior is done or otherwise satisfied (although it can also return a syslog
     string and syslog severity).
-->

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

<!-- 
     The optional Uninitialize element is not specified in this behavior, but if it were,
     it would follow the conventions of the Initialize element.
-->

</DefineBehavior>
