#!/usr/bin/env python
#
# Copyright 2010, 2012 Nick Foster
# 
# This file is part of gr-air-modes
# 
# gr-air-modes is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 3, or (at your option)
# any later version.
# 
# gr-air-modes is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
# 
# You should have received a copy of the GNU General Public License
# along with gr-air-modes; see the file COPYING.  If not, write to
# the Free Software Foundation, Inc., 51 Franklin Street,
# Boston, MA 02110-1301, USA.
#

# For reference into the methodology used to decode altitude,
# see RTCA DO-181D p.42

def decode_alt(alt, bit13):
	mbit = alt & 0x0040
	qbit = alt & 0x0010
	
	if mbit and bit13:
		#TBD: bits 20-25, 27-31 encode alt in meters
		#remember that bits are left justified (bit 20 is MSB)
		return "METRIC ERROR"

	if qbit: #a mode S-style reply
		#bit13 is false for BDS0,5 ADS-B squitters, and is true otherwise
		if bit13:
			#in this representation, the altitude bits are as follows:
			# 12 11 10 9 8 7 (6) 5 (4) 3 2 1 0
			# so bits 6 and 4 are the M and Q bits, respectively.
			tmp1 = (alt & 0x3F80) >> 2
			tmp2 = (alt & 0x0020) >> 1
		else:
			tmp1 = (alt & 0x1FE0) >> 1
			tmp2 = 0

		decoded_alt = ((alt & 0x0F) | tmp1 | tmp2) * 25 - 1000

	else: #a mode C-style reply
		  #okay, the order they come in is:
		  #C1 A1 C2 A2 C4 A4 X B1 D1 B2 D2 B4 D4
    	  #the order we want them in is:
    	  #D2 D4 A1 A2 A4 B1 B2 B4
    	  #so we'll reassemble into a Gray-coded representation

		if bit13 is False:
			alt = (alt & 0x003F) | (alt & 0x0FC0 << 1)

		C1 = 0x1000
		A1 = 0x0800
		C2 = 0x0400
		A2 = 0x0200	#this represents the order in which the bits come
		C4 = 0x0100
		A4 = 0x0080
		B1 = 0x0020
		D1 = 0x0010
		B2 = 0x0008
		D2 = 0x0004
		B4 = 0x0002
		D4 = 0x0001

		bigpart =  ((alt & B4) >> 1) \
				 + ((alt & B2) >> 2) \
				 + ((alt & B1) >> 3) \
				 + ((alt & A4) >> 4) \
				 + ((alt & A2) >> 5) \
				 + ((alt & A1) >> 6) \
				 + ((alt & D4) << 6) \
				 + ((alt & D2) << 5)

		#bigpart is now the 500-foot-resolution Gray-coded binary part
		decoded_alt = gray2bin(bigpart)
		#real_alt is now the 500-foot-per-tick altitude

		cbits =   ((alt & C4) >> 8) + ((alt & C2) >> 9) + ((alt & C1) >> 10)
		cval = gray2bin(cbits) #turn them into a real number

		if cval == 7:
			cval = 5 #not a real gray code after all

		if decoded_alt % 2:
			cval = 6 - cval #since the code is symmetric this unwraps it to see whether to subtract the C bits or add them

		decoded_alt *= 500 #take care of the A,B,D data
		decoded_alt += cval * 100 #factor in the C data
		decoded_alt -= 1300 #subtract the offset

	return decoded_alt

def gray2bin(gray):
	i = gray >> 1

	while i != 0:
		gray ^= i
		i >>= 1

	return gray

def encode_alt_modes(alt, bit13):
	mbit = False
	qbit = True
	encalt = (int(alt) + 1000) / 25

	if bit13 is True:
		tmp1 = (encalt & 0xfe0) << 2
		tmp2 = (encalt & 0x010) << 1
		
	else:
		tmp1 = (encalt & 0xff8) << 1
		tmp2 = 0

	return (encalt & 0x0F) | tmp1 | tmp2 | (mbit << 6) | (qbit << 4)

if __name__ == "__main__":
	for alt in range(-1000, 101400, 25):
		dec = decode_alt(encode_alt_modes(alt, False), False)
		if dec != alt:
			print "Failure at %i with bit13 clear (got %s)" % (alt, dec)
	for alt in range(-1000, 101400, 25):
		dec = decode_alt(encode_alt_modes(alt, True), True)
		if dec != alt:
			print "Failure at %i with bit13 set (got %s)" % (alt, dec)