#FLARM module, adjusted to provide data to the TRX-2000 display

##### necessary properties to drive display:
# - vertical speed (m/s)
# - distance (km)
# - altitude (m)
# - ID
# - x/y coordinates (for map, in km)
# - heading (°)

#Set variables
var max_dist	=	6;	 #maximal distance in kilometers
running		=	0;

var prop_base = "instrumentation/FLARM/";
var contacts = prop_base~"contacts/";
var nc_prop = prop_base~"sound/new-contact";
var warn_prop = props.globals.getNode("instrumentation/FLARM/sound/warn", 1);

var volts = props.globals.getNode("/systems/electrical/outputs/tcas", 1);

var serviceable = props.globals.getNode("/instrumentation/FLARM/serviceable", 1);

var targets=[];
var warnings=[];

#Set properties
for(var f=0; f<=30; f=f+1){
	setprop(prop_base~"targets-tracked/mp["~f~"]", 0);
	append(targets, nil);
	append(warnings, nil);
}

var relative = func (brg, heading) {
	brg=brg-heading;
	return geo.normdeg(brg);
}

var new_contact = func ()  { #Sound message for new contact
	if(getprop(nc_prop) == 1){
		setprop(nc_prop, 0);
	}else{
		setprop(nc_prop, 1);
	}
}

# Calculate radar coordinates based on distance and relative bearing
var get_x = func (distance, rel_bearing){
	return math.sin(rel_bearing*D2R)*distance;
}
var get_y = func (distance, rel_bearing){
	return math.cos(rel_bearing*D2R)*distance;
}


#var target1 = Target.new(n);
var Target = {
	new : func(n){
		m = { parents : [Target] };
		m.id=getprop("/ai/models/multiplayer["~n~"]/callsign");
		m.n=n;
		m.pos = geo.Coord.new().set_latlon(	getprop("/ai/models/multiplayer["~n~"]/position/latitude-deg"),
							getprop("/ai/models/multiplayer["~n~"]/position/longitude-deg"),
							getprop("/ai/models/multiplayer["~n~"]/position/altitude-ft"));
		m.second=0.0;
		var ac = geo.aircraft_position();
		m.last_dist=m.pos.direct_distance_to(ac);
		m.vs=0;
		var bearing = ac.course_to(m.pos);
		var relative_bearing=relative(bearing,getprop("/orientation/heading-deg"));
		m.x = get_x(m.last_dist, relative_bearing);
		m.y = get_y(m.last_dist, relative_bearing);
		new_contact();
		return m;
	},
	
	update : func(second) {
		me.pos.set_latlon(	getprop("/ai/models/multiplayer["~me.n~"]/position/latitude-deg"),
					getprop("/ai/models/multiplayer["~me.n~"]/position/longitude-deg"),
					getprop("/ai/models/multiplayer["~me.n~"]/position/altitude-ft"));
					
		me.vs = getprop("/ai/models/multiplayer["~me.n~"]/velocities/vertical-speed-fps")*FT2M/60;
		me.rel_heading = relative(getprop("/ai/models/multiplayer["~me.n~"]/orientation/true-heading-deg"),getprop("/orientation/heading-deg"));
		var ac = geo.aircraft_position();
		#Time difference
		var delta_time=second-me.second;
		me.second=second;
		var actual_dist_now=me.pos.direct_distance_to(ac);
		
		#Closure rate
		var delta_dist=(me.last_dist-actual_dist_now)/delta_time;
		
		#(Theoretical) time to collision
		if(delta_dist==0){
			ttc=999;
		}else{
			var ttc=actual_dist_now/delta_dist;
		}
		
		if(ttc<=0){
			ttc=999;
		}
		
		var bearing = ac.course_to(me.pos);
		var relative_bearing=relative(bearing,getprop("/orientation/heading-deg"));
		
		var alt_diff=(me.pos.alt()*FT2M) - ac.alt(); #Altitude difference in meters
		
		me.x = get_x(actual_dist_now, relative_bearing);
		me.y = get_y(actual_dist_now, relative_bearing);
		
		var data=[me.id, actual_dist_now, alt_diff, me.vs, me.rel_heading, me.x, me.y]; #format: ID, distance, relative altitude, vertical speed, relative heading, x, y
		
		if(ttc<6 and alt_diff < 150){
			warnings[me.n]=2;
		}else if(ttc<14 and alt_diff < 300){
			warnings[me.n]=1;
		}else{
			warnings[me.n]=0;
		}
		
		me.last_dist=actual_dist_now;
		
		return data;
	},
};


setlistener("/sim/signals/fdm-initialized", func{
	settimer(update_FLARM, 5);
});


var update_FLARM = func{
	for(var f=0; f<=30; f=f+1){
		if(getprop("/ai/models/multiplayer["~f~"]/position/latitude-deg") != nil){
			var temp_pos = geo.Coord.set_latlon(	getprop("/ai/models/multiplayer["~f~"]/position/latitude-deg"),
								getprop("/ai/models/multiplayer["~f~"]/position/longitude-deg"),
								getprop("/ai/models/multiplayer["~f~"]/position/altitude-ft"));
							
			#Check whether in range and target not already existing
			var distance_km = temp_pos.distance_to(geo.aircraft_position())/1000;
			if(distance_km<max_dist and getprop("/instrumentation/FLARM/targets-tracked/mp["~f~"]")==0){
				#Now generate a target
				targets[f]=Target.new(f);
				setprop("/instrumentation/FLARM/targets-tracked/mp["~f~"]", 1);
			}else if(distance_km>max_dist and getprop("/instrumentation/FLARM/targets-tracked/mp["~f~"]")==1){
				#Target existing, but has moved meanwhile out of range
				targets[f]=nil;
				setprop("/instrumentation/FLARM/targets-tracked/mp["~f~"]", 0);
			}
		}else if(getprop("/instrumentation/FLARM/targets-tracked/mp["~f~"]")==1){
			#Target existing, but has meanwhile logged out
			targets[f]=nil;
			setprop("/instrumentation/FLARM/targets-tracked/mp["~f~"]", 0);
		}
	}
	
	receive=0;
	
	forindex(var key; targets){
		if(targets[key]!=nil){
			var data = targets[key].update(getprop("/sim/time/elapsed-sec"));
			setprop(contacts~"contact["~key~"]/id", data[0]);
			setprop(contacts~"contact["~key~"]/dist", data[1]);
			setprop(contacts~"contact["~key~"]/delta_alt", data[2]);
			setprop(contacts~"contact["~key~"]/vs", data[3]);
			setprop(contacts~"contact["~key~"]/rel_hdg", data[4]);
			setprop(contacts~"contact["~key~"]/x", data[5]);
			setprop(contacts~"contact["~key~"]/y", data[6]);
			setprop(contacts~"contact["~key~"]/valid", 1);
			receive=1;
		}else{
			setprop(contacts~"contact["~key~"]/valid", 0);
		}
	}
	
	setprop("/instrumentation/FLARM/receive-internal", receive);
	
	
	
	#Check LEDs
	#12 LEDS, each cover 30 degrees	
	
	heading=getprop("/orientation/heading-deg") or 0;
	var altitude=getprop("/position/altitude-ft") or 0;
		
	#Check Warning sounds
	warn=0;
	forindex(var key; warnings){
		if(warnings[key]==2 and warn<2){
			warn=2;
		}else if(warnings[key]==1 and warn<1){
			warn=1;
		}
	}
	warn_prop.setValue(warn);
	
	
	
	if((getprop("/systems/electrical/outputs/flarm") or 0)>9 and serviceable.getValue() == 1){
		settimer(update_FLARM, 1.0);
		running=1;
	}else{
		running=0;
	}
}

setlistener("/systems/electrical/outputs/tcas", func{
	if((getprop("/systems/electrical/outputs/tcas") or 0)>9 and running==0){
		settimer(update_FLARM, 1);
		running=1;
	}
});
