<?xml version="1.0" encoding="UTF-8"?>
<DefineBehavior 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"
       Name="Waypoint" Language="Lua">

    <Description>
        Simple example replacement for Waypoint behavior. Only takes two
        settings: latitude and longitude
    </Description>

    <DefineSetting Name="latitudeSet">
        <Guidance:Waypoint.latitude/>
        <Description>
            Goal latitude.
        </Description><Units:radian/>
        <DefaultValue>NaN</DefaultValue>
    </DefineSetting>

    <DefineSetting Name="longitudeSet">
        <Guidance:Waypoint.longitude/>
        <Description>
            Goal longitude.
        </Description><Units:radian/>
        <DefaultValue>NaN</DefaultValue>
    </DefineSetting>

    <Construct>
        <Description>
            Define functions for use in the behavior
        </Description>
        <Script><![CDATA[
	       function GetBearing( lat0, lon0, lat1, lon1 )
               local y = math.sin( lon1 - lon0 ) * math.cos( lat1 )
               local x = math.cos( lat0 ) * math.sin( lat1 ) - math.sin( lat0 ) * math.cos( lat1 ) * math.cos( lon1 - lon0 )
               return math.atan2( y, x )
           end
	       return "Constructed Waypoint", "IMPORTANT"
	    ]]></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 latitudeSet==latitudeSet then 
               goalLat = latitudeSet
           else 
               goalLat = latitude
           end
           if longitudeSet==longitudeSet then 
               goalLon = longitudeSet
           else 
               goalLon = longitude
           end
           goalBear = GetBearing(latitude,longitude,goalLat,goalLon)
           if goalLat ~= latitude then
               perpendicularSlope = -( goalLon - longitude ) / ( goalLat - latitude )
               startOverLine = latitude > ( goalLat + ( ( longitude - goalLon ) * perpendicularSlope ) )
           else
               perpendicularSlope = nil
               startOverLine = longitude > goalLon
           end
        ]]></Script>
    </Initialize>

    <Run>
        <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[
           return 1.0, goalLat, goalLon, goalBear;
        ]]></Script>
    </Run>

    <IsSatisfied>
        <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 perpendicularSlope ~= nil then
               isOverLine = latitude > (goalLat
                             + ( ( longitude - goalLon ) * perpendicularSlope))
           else
               isOverLine = longitude > goalLon;
           end
           return isOverLine ~= startOverLine
        ]]></Script>
    </IsSatisfied>

</DefineBehavior>
