mayhem-firmware/firmware/common/aprs_packet.hpp

/*
 * Copyright (C) 2015 Jared Boone, ShareBrained Technology, Inc.
 * Copyright (C) 2016 Furrtek
 *
 * This file is part of PortaPack.
 *
 * This program 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 2, or (at your option)
 * any later version.
 *
 * This program 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 this program; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifndef __APRS_PACKET_H__
#define __APRS_PACKET_H__

#include <cstdint>
#include <cstddef>
#include <cctype>

#include "baseband.hpp"

namespace aprs {

const int APRS_MIN_LENGTH = 18; //14 bytes address, control byte and pid. 2 CRC.

struct aprs_pos{
	float latitude;
	float longitude;	
	uint8_t symbol_code;
	uint8_t  sym_table_id;
};

enum ADDRESS_TYPE {
	SOURCE,
	DESTINATION,
	REPEATER
};

class APRSPacket {
public:
	void set_timestamp(const Timestamp& value) {
		timestamp_ = value;
	}
	
	Timestamp timestamp() const {
		return timestamp_;
	}

	void set(const size_t index, const uint8_t data) {		
		payload[index] = data;

		if(index + 1 > payload_size){
			payload_size = index + 1;
		}
	}

	uint32_t operator[](const size_t index) const {
		return payload[index];
	}
	

	uint8_t size() const {
		return payload_size;
	}

	void set_valid_checksum(const bool valid) {
		valid_checksum = valid;
	}

	bool is_valid_checksum() const {
		return valid_checksum;
	}

	uint64_t get_source(){
		uint64_t source = 0x0;

		for(uint8_t i = SOURCE_START; i < SOURCE_START + ADDRESS_SIZE; i++){
			source |= ( ((uint64_t)payload[i]) << ((i - SOURCE_START) * 8));
		}

		return source;
	}

	std::string get_source_formatted(){
		parse_address(SOURCE_START, SOURCE);
		return std::string(address_buffer);
	}

	std::string get_destination_formatted(){
		parse_address(DESTINATION_START, DESTINATION);
		return std::string(address_buffer);
	}

	std::string get_digipeaters_formatted(){
		uint8_t position = DIGIPEATER_START;
		bool has_more = parse_address(SOURCE_START, REPEATER);

		std::string repeaters = "";
		while(has_more){			
			has_more = parse_address(position, REPEATER);	
			repeaters += std::string(address_buffer);

			position += ADDRESS_SIZE;

			if(has_more){			
				repeaters += ">";
			}		
		}

		return repeaters;
	}

	uint8_t get_number_of_digipeaters(){
		uint8_t position = DIGIPEATER_START;		
		bool has_more = parse_address(SOURCE_START, REPEATER);
		uint8_t repeaters = 0;
		while(has_more){			
			has_more = parse_address(position, REPEATER);	
			position += ADDRESS_SIZE;
			repeaters++;	
		}

		return repeaters;
	}

	uint8_t get_information_start_index(){
		return DIGIPEATER_START + (get_number_of_digipeaters() * ADDRESS_SIZE) + 2;
	}

	std::string get_information_text_formatted(){
		std::string information_text = "";
		for(uint8_t i = get_information_start_index(); i < payload_size - 2; i++){
			information_text += payload[i];
		}

		return information_text;
	}

	std::string get_stream_text(){
		std::string stream = get_source_formatted() + ">" + get_destination_formatted() + ";" + get_digipeaters_formatted() + ";" + get_information_text_formatted();

		return stream;
	}

	char get_data_type_identifier(){
		char ident = '\0';
		for(uint8_t i = get_information_start_index(); i < payload_size - 2; i++){
			ident = payload[i];
			break;
		}
		return ident;
	}

	bool has_position(){		
		char ident = get_data_type_identifier();

		return 
			ident == '!' || 
			ident == '=' || 
			ident == '/' || 
			ident == '@' || 
			ident == ';' || 
			ident == '`' ||
			ident == '\''||
			ident == 0x1d||
			ident == 0x1c;
	}

	aprs_pos get_position(){
		aprs::aprs_pos pos;

		char ident = get_data_type_identifier();
		std::string info_text = get_information_text_formatted();

		std::string lat_str, lng_str;
		char first;
		//bool supports_compression = true;
		bool is_mic_e_format = false;
		std::string::size_type start;

		switch(ident){
			case '/':
			case '@':
				start = 8;
				break;
			case '=':
			case '!':
				start = 1;
				break;
			case ';':			
				start = 18;
				//supports_compression = false;
				break;
			case '`':
			case '\'':
			case 0x1c:
			case 0x1d:
				is_mic_e_format = true;
				break;
			default:
				return pos;
		}		
		if(is_mic_e_format){
			parse_mic_e_format(pos);
		}
		else {			
			if(start < info_text.size()){
				first = info_text.at(start);

				if(std::isdigit(first)){
					if(start + 18 < info_text.size()){
						lat_str = info_text.substr(start, 8);
						pos.sym_table_id = info_text.at(start + 8);
						lng_str = info_text.substr(start + 9, 9);
						pos.symbol_code = info_text.at(start + 18);

						pos.latitude = parse_lat_str(lat_str);
						pos.longitude = parse_lng_str(lng_str);
					}
					
				}
				else {
					if(start + 9 < info_text.size()){
						pos.sym_table_id = info_text.at(start);
						lat_str = info_text.substr(start + 1, 4);
						lng_str = info_text.substr(start + 5, 4);
						pos.symbol_code = info_text.at(start + 9);

						pos.latitude = parse_lat_str_cmp(lat_str);
						pos.longitude = parse_lng_str_cmp(lng_str);	
					}		
				}
			}			
		}		

		return pos;
	}

	void clear() {
		payload_size = 0;
	}

private:
	const uint8_t DIGIPEATER_START = 14;
	const uint8_t SOURCE_START = 7;
	const uint8_t DESTINATION_START = 0;
	const uint8_t ADDRESS_SIZE = 7;

	bool valid_checksum = false;
	uint8_t payload[256];
	char address_buffer[15];
	uint8_t payload_size = 0 ;
	Timestamp timestamp_ { };

	float parse_lat_str_cmp(const std::string& lat_str){
		return 90.0 - ( (lat_str.at(0) - 33) * (91*91*91) + (lat_str.at(1) - 33) * (91*91) + (lat_str.at(2) - 33) * 91 + (lat_str.at(3)) ) / 380926.0;		
	}

	float parse_lng_str_cmp(const std::string& lng_str){
		return -180.0 + ( (lng_str.at(0) - 33) * (91*91*91) + (lng_str.at(1) - 33) * (91*91) + (lng_str.at(2) - 33) * 91 + (lng_str.at(3)) ) / 190463.0;		
	} 

	uint8_t parse_digits(const std::string& str){
		if(str.at(0) == ' '){
			return 0;
		}
		uint8_t end = str.find_last_not_of(' ') + 1;
		std::string sub = str.substr(0, end);

		if(!is_digits(sub)){
			return 0;
		}
		else {
			return std::stoi(sub);
		}
	}

	bool is_digits(const std::string& str){
		return str.find_last_not_of("0123456789") == std::string::npos;
	}

	float parse_lat_str(const std::string& lat_str){
		float lat = 0.0;

		std::string str_lat_deg = lat_str.substr(0, 2);
		std::string str_lat_min = lat_str.substr(2, 2);
		std::string str_lat_hund = lat_str.substr(5, 2);
		std::string dir = lat_str.substr(7, 1);

		uint8_t lat_deg = parse_digits(str_lat_deg);
		uint8_t lat_min = parse_digits(str_lat_min);
		uint8_t lat_hund = parse_digits(str_lat_hund);

		lat += lat_deg;
		lat += (lat_min + (lat_hund/ 100.0))/60.0;

		if(dir.c_str()[0] == 'S'){
			lat = -lat;
		}

		return lat;

	}

	float parse_lng_str(std::string& lng_str){
		float lng = 0.0;

		std::string str_lng_deg = lng_str.substr(0, 3);
		std::string str_lng_min = lng_str.substr(3, 2);
		std::string str_lng_hund = lng_str.substr(6, 2);
		std::string dir = lng_str.substr(8, 1);

		uint8_t lng_deg = parse_digits(str_lng_deg);
		uint8_t lng_min = parse_digits(str_lng_min);
		uint8_t lng_hund = parse_digits(str_lng_hund);

		lng += lng_deg;
		lng += (lng_min + (lng_hund/ 100.0))/60.0;

		if(dir.c_str()[0] == 'W'){
			lng = -lng;
		}

		return lng;

	}

	void parse_mic_e_format(aprs::aprs_pos& pos){
		std::string lat_str = "";
		std::string lng_str = "";

		bool is_north = false;
		bool is_west = false;
		uint8_t lng_offset = 0;
		for(uint8_t i = DESTINATION_START; i < DESTINATION_START + ADDRESS_SIZE - 1; i++){
			uint8_t ascii = payload[i] >> 1;

			lat_str += get_mic_e_lat_digit(ascii);

			if(i - DESTINATION_START == 3){
				lat_str += ".";
			}
			if(i - DESTINATION_START == 3){
				is_north = is_mic_e_lat_N(ascii);
			}
			if(i - DESTINATION_START == 4){
				lng_offset = get_mic_e_lng_offset(ascii);
			}
			if(i - DESTINATION_START == 5){
				is_west = is_mic_e_lng_W(ascii);
			}
		}
		if(is_north){
			lat_str += "N";
		}
		else {
			lat_str += "S";
		}

		pos.latitude = parse_lat_str(lat_str);	

		float lng = 0.0;
		uint8_t information_start = get_information_start_index() + 1;
		for(uint8_t i = information_start; i < information_start + 3 && i < payload_size - 2; i++){
			uint8_t ascii = payload[i];
			
			if(i - information_start == 0){ //deg
				ascii -=28;
				ascii += lng_offset;

				if(ascii >= 180 && ascii <= 189){
					ascii -= 80;
				}
				else if (ascii >= 190 && ascii <= 199){
					ascii -= 190;
				}

				lng += ascii;
			}	
			else if(i - information_start == 1){ //min
				ascii -= 28;
				if(ascii >= 60){
					ascii -= 60;
				}
				lng += ascii/60.0;
			}	
			else if(i - information_start == 2){ //hundredth minutes
				ascii -= 28;

				lng += (ascii/100.0)/60.0;
			}	
		}

		if(is_west){
			lng = -lng;
		}

		pos.longitude = lng;

	}

	uint8_t get_mic_e_lat_digit(uint8_t ascii){
		if(ascii >= '0' && ascii <= '9'){
			return ascii;
		}
		if(ascii >= 'A' && ascii <= 'J'){
			return ascii - 17;
		}
		if(ascii >= 'P' && ascii <='Y'){
			return ascii - 32;
		}
		if(ascii == 'K' || ascii == 'L' || ascii == 'Z'){
			return ' ';
		}

		return '\0';
	}

	bool is_mic_e_lat_N(uint8_t ascii){
		if(ascii >= 'P' && ascii <='Z'){
			return true;
		}		
		return false; //not technical definition, but the other case is invalid
	}

	bool is_mic_e_lng_W(uint8_t ascii){
		if(ascii >= 'P' && ascii <='Z'){
			return true;
		}		
		return false; //not technical definition, but the other case is invalid
	}

	uint8_t get_mic_e_lng_offset(uint8_t ascii){
		if(ascii >= 'P' && ascii <='Z'){
			return 100;
		}		
		return 0; //not technical definition, but the other case is invalid
	}

	bool parse_address(uint8_t start, ADDRESS_TYPE address_type){
		uint8_t byte = 0;
		uint8_t has_more = false;
		uint8_t ssid = 0;
		uint8_t buffer_index = 0;

		for(uint8_t i = start; i < start + ADDRESS_SIZE && i < payload_size - 2; i++){
			byte = payload[i];

			if(i - start == 6){
				 has_more = (byte & 0x1) == 0;
				 ssid = (byte >> 1) & 0x0F;

				 if(ssid != 0 || address_type == REPEATER){
				 	address_buffer[buffer_index++] = '-';				 	

				 	if(ssid < 10){
				 		address_buffer[buffer_index++] = '0' + ssid;				 		
				 		address_buffer[buffer_index++] = '\0';
				 	}
				 	else {
				 		address_buffer[buffer_index++] = '1';
				 		address_buffer[buffer_index++] = '0' + ssid - 10;
				 		address_buffer[buffer_index++] = '\0';
				 	}
				 }
				 else {
					 address_buffer[buffer_index++] = '\0';
				 }
			}
			else {
				byte >>= 1;

				if(byte != ' '){
					address_buffer[buffer_index++] = byte;		
				}			
			}
		}

		return has_more;
	}
};

} /* namespace aprs */

#endif/*__APRS_PACKET_H__*/