# Anchor Alarm with MFD, version 1.0
# See details: http://www.yachtd.com/news/anchor_alarm.html
#
# To configure the Alarm Button YDAB-01, send the following sequence of commands:
# YD:RESET
# YD:MODE DS
# YD:LINK 2 SOUND 0
# YD:LINK 3 SOUND 0
# YD:LINK 4 SOUND 0
# YD:LINK 5 SOUND 0
# YD:LINK 6 SOUND 0
# YD:LINK 7 SOUND 0
# YD:CHANNEL 7

FW_CAN1_TO_CAN2=OFF
FW_CAN2_TO_CAN1=OFF

SLOT1 = 000EF20D FF 08 00FDFFFFFFFFFFFF # Turn ON the the anchor alarm (Channel 1)
SLOT2 = 000EF20D FF 08 00FCFFFFFFFFFFFF # Turn OFF the the anchor alarm
SLOT3 = 000EF20D FF 08 0003C0FFFFFFFFFF # Turn off all channels, except Channel 1

# We'll run/stop the alarm once in 3 seconds to avoid fast switches
heartbeat(3000)
{
	if (D > 0) { # Alarm is set (D is the safe distance in meters)
		if (R > D) {  # R is distance to D from current GPS position
		  load(SLOT1) # Anchor alarm on
		  M = 1       # Mark, that alarm was ON by the Bridge
		}
		else {
		  load(SLOT2) # Anchor alarm off
		  M = 0       # Alarm was OFF by the Bridge
		}
		send(CAN1)
	}
}

# Position, Rapid Update - calculate the distance from current position
match(CAN1,0x1F80100,0x1FFFF00)
{
	if (get(DATA,INT32) < 0x7FFFFFFF) {
	    if (get(DATA+4,INT32) < 0x7FFFFFFF) {		
	        X = get(DATA,INT32)	# Latitude, 0.0000001 deg
		Y = get(DATA+4,INT32)	# Longitude, 0.0000001 deg
		T = timer()	# Position is valid, save the time of position fix
		R = 0	        # Reset the distance from current position
		if (D > 0) {	# Is the alarm already set?
			# Calculate the distance by Haversine Formula
   			W = (cast(X - A,FLOAT)/10000000) * M_PI / 180 # delta Lat
   			Z = (cast(Y - B,FLOAT)/10000000) * M_PI / 180 # delta Lon
			W = sin(W/2) 
			Z = sin(Z/2) 
			U = W*W + cos((cast(X,FLOAT)/10000000) * M_PI / 180)*cos((cast(A,FLOAT)/10000000) * M_PI / 180)*Z*Z
			if (U != 0) {	
				U = atan2(sqrt(U),sqrt(1-U))
				R = 12742000*U 	# The distance to (A,B) in meters
			}
		}
	    }
	}		
}

# Binary Status Report
match(CAN1,0x1F20D00,0x1FFFF00)
{
	if (get(DATA,UINT8) == 0) { 	# Is it Bank 0 message?
		S = get(DATA+1,UINT16)	# Status of Channels 1-8 (2 bit each: 1-ON, 0-OFF)
		if ((S & 3) == 1) {     
			# Is anchor alarm running?
			if ((S >> 2) & 0x3FF == 0) { # Are all buttons off?
				D = 0		      
				M = 0                 
				load(SLOT3)           # User pressed buttons on MFD
				set(DATA+1,UINT8,0)   # screen to cancel alarm
				send(CAN1)            # Cancel the anchor alarm
			}			
		}
		else { 	# Anchor alarm is OFF - read MFD buttons
			K = 0
			if ((S >> 10) & 3 == 1) {				
				K = 5  # Channel 6 is ON
				L = 50 # Set 50m alarm
			}						
			if ((S >> 8) & 3 == 1) {
				K = 4  # Channel 5 is ON
				L = 30 # Set 30m alarm
			}
			if ((S >> 6) & 3 == 1) {				
				K = 3  # Channel 4 is ON
				L = 20 # Set 20m alarm
			}
			if ((S >> 4) & 3 == 1) {				
				K = 2  # Channel 3 is ON
				L = 10 # Set 10m alarm
			}
			# If two buttons pressed on MFD, the smallest
			# distance have priority
			if ((S >> 2) & 3 == 1) { 
				K = 1 # Channel 2 is ON
				L = 5 # Set 5m alarm
			}

			# Is hardware button was pressed? It have priority over MFD
			if ((S >> 12) & 3 == 1) {
				K = 3  # Favorite distance to turn on
				L = 20 # with hardware button (20 meters)
			}

			if (K == 0) {				
				D = 0 # No buttons pressed, clear alarm settings
			}
			else {  # We have buttons pressed
				if (timediff(T) < 2000) { # Is GPS position valid?
					if (D != L) {
						# New alarm or modification of alarm
						A = X
						B = Y
						D = L # New safe distance, in meters
					}
					else {
						# Is user canceled the alarm with hardware
						# button of or by switching the Channel 1 ?
						if (M == 1) {
							load(SLOT3) # Cancel the alarm
							send(CAN1)
							K = 0
							M = 0
							D = 0
						}
					}					
				}
				else {	# We have no actual position (GPS lost?)
					if (D > 0) { 	# Was the alarm set?
						if (M == 1) {	    # Was it canceled by user?	
							load(SLOT3) 
							send(CAN1)  # Cancel the alarm
							K = 0
							M = 0
							D = 0
						}
						else {  # It was not canceled by user
							R = 1000    # Run the Anchor Alarm!
							M = 0
						}
					}
					else  {   	# User tries to set new alarm
						load(SLOT3) # Switch off all buttons, do not allow
						send(CAN1)  # to set the alarm without valid position
						K = 0       
					}
				}
			}
			if (K > 0) { 
				S = get(DATA+1,UINT16)
				K = (S & 0xFFF) | (1 << (K*2))
				if (S != K) # If multiple buttons are pressed, leave on the actual only
				{
					load(SLOT3)
					set(DATA+1,UINT16, K)
					send(CAN1)
				}
			}
		}
	}
}

# End of program