
-- ===================
-- 	Constants
-- ===================

zeroX = 5000000.0; -- Real coordinates beginning
zeroZ = 6600000.0;

centerX = 11465000.0 - zeroX; -- Circle center
centerZ = 6500000.0 - zeroZ;

pn40x24_X = 4468608.57 - zeroX; -- point 40dgN : 24dgE
pn40x24_Z = 5730893.72 - zeroZ;

pn48x24_X = 5357858.31 - zeroX; -- point 48dgN : 24dgE
pn48x24_Z = 5828649.53 - zeroZ;

pn40x42_X = 4468608.57 - zeroX; -- point 40dgN : 42dgE
pn40x42_Z = 7269106.20 - zeroZ;

pn48x42_X = 5357858.31 - zeroX; -- ????? 48dgN : 42dgE
pn48x42_Z = 7171350.00 - zeroZ;

-- distances from the circle center to 48dgN and 40dgN
lenNorth = math.sqrt((pn48x24_X-centerX)*(pn48x24_X-centerX) + (pn48x24_Z-centerZ)*(pn48x24_Z-centerZ));
lenSouth = math.sqrt((pn40x24_X-centerX)*(pn40x24_X-centerX) + (pn40x24_Z-centerZ)*(pn40x24_Z-centerZ));
lenN_S = lenSouth - lenNorth;
RealAngleMaxLongitude = math.atan((pn40x24_Z - centerZ)/(pn40x24_X - centerX)) * 180.0 /math.pi;

-- Map bounds. Degrees!
EndWest = 24.0;
EndEast = 42.0;
EndNorth = 48.0;
EndSouth = 40.0;
MiddleLongitude = (EndWest + EndEast) / 2;
ToLengthN_S = (EndNorth - EndSouth) / lenN_S;
ToAngleW_E = (MiddleLongitude - EndWest) / RealAngleMaxLongitude;
ToDegree = 180.0/math.pi;


-- ===================
-- 	NAV utils
-- ===================

-- bearings should be between 0 & 2pi
function normalize(radiansToNormalize)
	while radiansToNormalize<0 do
		radiansToNormalize = radiansToNormalize+2*math.pi;
	end
	while radiansToNormalize>=2*math.pi do
		radiansToNormalize = radiansToNormalize-2*math.pi;
	end
	return radiansToNormalize;
end

-- bearings should be between 0 & 360
function normalizeDegrees(degreesToNormalize)
	return ToDegree*normalize(degreesToNormalize/ToDegree);
end

-- Returns bearing from A->B, minus A's heading (useful for programming radar cursor)
function get_relative_bearing(A,B)

	local bearing = getBearing(A,B);
	local relativeBearing = ToDegree*(-A.Heading) + bearing;

	-- Relative bearings are around 0
	while relativeBearing>180 do
		relativeBearing = relativeBearing - 360; 
	end
	while relativeBearing<-180 do
		relativeBearing = relativeBearing + 360;
	end

	return relativeBearing;
end

function getGnomonicCoordinates(latitude, longitude)

	local angleRadians = (longitude - MiddleLongitude) / (ToAngleW_E * ToDegree);
	local realLen = lenSouth - (latitude - EndSouth) / ToLengthN_S;
	
	local destination = {};
	destination[1] = centerX - realLen * math.cos(angleRadians);
	destination[2] = centerZ + realLen * math.sin(angleRadians);
	return destination;
	
end

function getPlanarDistance(A, B)

	local coords1 = getGnomonicCoordinates(A.LatLongAlt.Lat, A.LatLongAlt.Long);
	local coords2 = getGnomonicCoordinates(B.LatLongAlt.Lat, B.LatLongAlt.Long);

	local deltaX = coords2[1] - coords1[1];
	local deltaY = coords2[2] - coords1[2];

	return math.sqrt(deltaX*deltaX + deltaY*deltaY);	
end

function getDistance(A, B)

	local deltaZ = B.LatLongAlt.Alt - A.LatLongAlt.Alt;
	local planarDistance = getPlanarDistance(A,B);

	return math.sqrt( planarDistance*planarDistance + deltaZ*deltaZ );
end

function getBearing(A,B)

	local coords1 = getGnomonicCoordinates(A.LatLongAlt.Lat, A.LatLongAlt.Long);
	local coords2 = getGnomonicCoordinates(B.LatLongAlt.Lat, B.LatLongAlt.Long);

	local deltaX = coords2[1] - coords1[1];
	local deltaY = coords2[2] - coords1[2];

	return normalizeDegrees( ToDegree * (math.atan2(deltaY, deltaX)) );
	
end

function getHeadingFromV(V)
	return normalize( math.pi/2 - math.atan2(V.x, V.z) );
end


function sign(val)

	if val<0 then
		return -1;
	end
	return 1;

end
