#include <stdio.h>
#include <ctype.h>
#include <stdint.h>
#include <string.h>

static const uint8_t WSPR_LENGTH = 162;

uint8_t wspr_call_char(char c) {
	c = toupper(c);
	if(c >= '0' && c <= '9') {
		return c - '0';
	}
	else if(c >= 'A' && c <= 'Z') {
		return 10 + (c - 'A');
	}
	// Turn everything else to space
	else {
		return 36;
	}
}

uint8_t wspr_loc_char(char c) {
	c = toupper(c);
	if(c >= '0' && c <= '9') {
		return c - '0';
	}
	else if(c >= 'A' && c <= 'R') {
		return c - 'A';
	}
	// Everthing else becomes 9/I
	else {
		return 9;
	}
}

uint8_t wspr_power(uint8_t power) {
	if(power > 60) {
		return 60;
	}
	return power;
}

uint64_t wspr_message(char *call, char *loc, uint8_t power) {
	uint32_t m, n;

	n = wspr_call_char(call[0]);
	n = n * 36 + wspr_call_char(call[1]);
	n = n * 10 + wspr_call_char(call[2]);
	n = n * 27 + wspr_call_char(call[3]) - 10;
	n = n * 27 + wspr_call_char(call[4]) - 10;
	n = n * 27 + wspr_call_char(call[5]) - 10;

	m = 179 - 10 * wspr_loc_char(loc[0]) - wspr_loc_char(loc[2]);
	m = m * 180 + 10 * wspr_loc_char(loc[1]) + wspr_loc_char(loc[3]);
	m = m * 128 + wspr_power(power) + 64;

	return ((uint64_t) n) << 22 | m;
}

const uint32_t WSPR_FEC_TAPS1 = 0xF2D05351;
const uint32_t WSPR_FEC_TAPS2 = 0xE4613C47;

uint8_t wspr_parity32(uint32_t x) {
	x ^= x >> 16;
    x ^= x >> 8;
    x ^= x >> 4;
    x ^= x >> 2;
    x ^= x >> 1;
	return x & 1;
}

void wspr_fec(uint64_t msg, uint8_t *output) {
	memset(output, 0, WSPR_LENGTH);

	uint32_t reg = 0;
	for(uint8_t i = 0;  i < 81; i++) {
		uint8_t bit = 0;
		if(i < 50) {
			bit = (msg >> (50 - i - 1)) & 1;
		}

		reg = (reg << 1) | bit;

		uint32_t tabs1 = reg & WSPR_FEC_TAPS1;
		uint32_t tabs2 = reg & WSPR_FEC_TAPS2;

		output[i * 2] = wspr_parity32(tabs1);
		output[i * 2 + 1] = wspr_parity32(tabs2);
	}
}

uint8_t wspr_reverse_bits(uint8_t bits) {
	//https://gcc.gnu.org/onlinedocs/gcc-4.7.1/gcc/AVR-Built_002din-Functions.html
	//return __builtin_avr_insert_bits (0x01234567, bits, 0);
	return (bits & 0x80 ? 0x01 : 0) |
       (bits & 0x40 ? 0x02 : 0) |
       (bits & 0x20 ? 0x04 : 0) |
       (bits & 0x10 ? 0x08 : 0) |
       (bits & 0x08 ? 0x10 : 0) |
       (bits & 0x04 ? 0x20 : 0) |
       (bits & 0x02 ? 0x40 : 0) |
       (bits & 0x01 ? 0x80 : 0);
}


void wspr_interleave(uint8_t *input, uint8_t *output) {
	uint8_t p = 0;
	uint8_t i = 0;
	while(p < WSPR_LENGTH) {
		uint8_t rev_i = wspr_reverse_bits(i);
		if(rev_i < WSPR_LENGTH) {
			output[rev_i] = input[p];
			p++;
		}
		i++;
	}
}

const uint8_t WSPR_SYNC[162] = {
	1,1,0,0,0,0,0,0,1,0,0,0,1,1,1,0,0,0,1,0,0,1,0,1,1,1,1,0,0,0,0,0,0,0,1,0,0,1,0,1,0,0,
	0,0,0,0,1,0,1,1,0,0,1,1,0,1,0,0,0,1,1,0,1,0,0,0,0,1,1,0,1,0,1,0,1,0,1,0,0,1,0,0,1,0,
	1,1,0,0,0,1,1,0,1,0,1,0,0,0,1,0,0,0,0,0,1,0,0,1,0,0,1,1,1,0,1,1,0,0,1,1,0,1,0,0,0,1,
	1,1,0,0,0,0,0,1,0,1,0,0,1,1,0,0,0,0,0,0,0,1,1,0,1,0,1,1,0,0,0,1,1,0.0,0
};

void wspr_add_sync(uint8_t *symbols) {
	for(uint8_t i = 0; i < WSPR_LENGTH; i++) {
		symbols[i] = WSPR_SYNC[i] + 2 * symbols[i];
	}
}

uint64_t validation_msg = 1086526031934565;

uint8_t validation_bits[162] = {
	1,1,1,0,1,1,1,1,1,0,0,0,1,1,1,0,0,1,0,0,1,1,0,1,0,1,0,1,0,1,0,0,1,0,1,0,1,0,1,1,1,0,0,0,
	1,1,0,1,1,0,1,1,0,0,0,1,1,1,1,1,1,0,0,1,1,1,1,1,1,1,1,0,1,1,1,0,1,0,1,0,1,1,0,1,0,1,0,1,
	0,0,0,1,0,0,0,1,0,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,1,0,1,1,0,1,0,1,0,0,0,1,1,0,0,1,1,1,1,
	0,1,1,0,1,1,0,1,0,1,0,1,0,1,0,1,1,0,0,0,1,1,0,1,0,0,1,1,1,1
};

uint8_t validation[162] = {
	3, 3, 0, 0, 2, 0, 0, 0, 1, 0, 2, 0, 1, 3, 1, 2, 2, 2, 1, 0, 0, 3, 2, 3, 1, 3, 3, 2, 2, 0,
	2, 0, 0, 0, 3, 2, 0, 1, 2, 3, 2, 2, 0, 0, 2, 2, 3, 2, 1, 1, 0, 2, 3, 3, 2, 1, 0, 2, 2, 1,
	3, 2, 1, 2, 2, 2, 0, 3, 3, 0, 3, 0, 3, 0, 1, 2, 1, 0, 2, 1, 2, 0, 3, 2, 1, 3, 2, 0, 0, 3,
	3, 2, 3, 0, 3, 2, 2, 0, 3, 0, 2, 0, 2, 0, 1, 0, 2, 3, 0, 2, 1, 1, 1, 2, 3, 3, 0, 2, 3, 1,
	2, 1, 2, 2, 2, 1, 3, 3, 2, 0, 0, 0, 0, 1, 0, 3, 2, 0, 1, 3, 2, 2, 2, 2, 2, 0, 2, 3, 3, 2,
	3, 2, 3, 3, 2, 0, 0, 3, 1, 2, 2, 2
};

int main(int argc, char const *argv[]) {
	uint8_t test[162];
	uint8_t test1[162];
	uint64_t msg = wspr_message(" K1ABC", "FN42", 37);


	if(msg == validation_msg) {
		printf("%lu✓\n", msg);
	}
	else {
		printf("%lu✗\n", msg);
	}


	wspr_fec(msg, test);

	for(uint8_t i = 0; i < WSPR_LENGTH; i++) {
		if(test[i] == validation_bits[i]) {
			printf("%d✓", test[i]);
		}
		else {
			printf("%d✗", test[i]);
		}
	}
	printf("\n\n");

	wspr_interleave(test, test1);
	wspr_add_sync(test1);

	for(uint8_t i = 0; i < WSPR_LENGTH; i++) {
		if(test1[i] == validation[i]) {
			printf("%d✓", test1[i]);
		}
		else {
			printf("%d✗", test1[i]);
		}
	}
	printf("\n");

	return 0;
}