/*
 * 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.
 */

#include "proc_aprsrx.hpp"
#include "portapack_shared_memory.hpp"

#include "event_m4.hpp"

#include "stdio.h"

void APRSRxProcessor::execute(const buffer_c8_t& buffer) {
	// This is called at 3072000 / 2048 = 1500Hz

	if (!configured) return;
	
	// FM demodulation
	const auto decim_0_out = decim_0.execute(buffer, dst_buffer);				// 2048 / 8 = 256 (512 I/Q samples)
	const auto decim_1_out = decim_1.execute(decim_0_out, dst_buffer);			// 256 / 8 = 32 (64 I/Q samples)
	const auto channel_out = channel_filter.execute(decim_1_out, dst_buffer);	// 32 / 2 = 16 (32 I/Q samples)

	feed_channel_stats(channel_out);
	
	auto audio = demod.execute(channel_out, audio_buffer);
	
	audio_output.write(audio);

	// Audio signal processing
	for (size_t c = 0; c < audio.count; c++) {

		const int32_t sample_int = audio.p[c] * 32768.0f;
		int32_t current_sample = __SSAT(sample_int, 16);
		
		current_sample /= 128;
		
		// Delay line put
		delay_line[delay_line_index & 0x3F] = current_sample;
		
		// Delay line get, and LPF
		sample_mixed = (delay_line[(delay_line_index - (samples_per_bit/2)) & 0x3F] * current_sample) / 4; 
		sample_filtered = prev_mixed + sample_mixed + (prev_filtered / 2);
		
		delay_line_index++;
		
		prev_filtered = sample_filtered;
		prev_mixed = sample_mixed;		

		// Slice
		sample_bits <<= 1;

		uint8_t bit = (sample_filtered < -20) ? 1 : 0;
		sample_bits |= bit;

/*
		int16_t scaled = bit == 1 ? 32767 : -32767;

		if( stream ) {
			const size_t bytes_to_write = sizeof(scaled) * 1;
			const auto result = stream->write(&scaled, bytes_to_write);
		}
*/
		
		// Check for "clean" transition: either 0011 or 1100
		if ((((sample_bits >> 2) ^ sample_bits) & 3) == 3) {
			// Adjust phase
			if (phase < 0x8000)
				phase += 0x800;		// Is this a proper value ?
			else
				phase -= 0x800;
		}
		
		phase += phase_inc;
		
		if (phase >= 0x10000) {
			phase &= 0xFFFF;
			
			if (true) {			
				uint8_t bit;					
				if(__builtin_popcount(sample_bits & 0xFF) >= 0x05){
					bit = 0x1;
				}
				else {
					bit = 0x0;
				}

				if(parse_bit(bit)){
					parse_packet();
				}						
			}
		}
	}
}

void APRSRxProcessor::parse_packet(){
	//validate crc	
	if(packet_buffer_size >= aprs::APRS_MIN_LENGTH){
		uint16_t crc = 0xFFFF;

		for(size_t i = 0; i < packet_buffer_size; i++){			
			uint8_t byte = packet_buffer[i];
			crc = ((crc >> 8) ^ crc_ccitt_tab[(crc ^ byte) & 0xFF]) & 0xFFFF;
		}	

		if(crc == 0xF0B8){		
			parse_ax25();
		}
	}	
}

void APRSRxProcessor::parse_ax25(){	
	aprs_packet.clear();
	aprs_packet.set_valid_checksum(true);

	for(size_t i = 0; i < packet_buffer_size; i++){
		aprs_packet.set(i, packet_buffer[i]);
	}

	APRSPacketMessage packet_message { aprs_packet };
	shared_memory.application_queue.push(packet_message);
}

bool APRSRxProcessor::parse_bit(const uint8_t current_bit){
	uint8_t decoded_bit = ~(current_bit ^ last_bit) & 0x1;
	last_bit = current_bit;

    //int16_t log = decoded_bit == 0 ? -32768 : 32767;
    //if(stream){
    //    const size_t bytes_to_write = sizeof(log) * 1;
    //    const auto result = stream->write(&log, bytes_to_write);
    //}

	if(decoded_bit & 0x1){
		if(ones_count < 8){
			ones_count++;
		}
	}
	else {
		if(ones_count > 6){ //not valid
			state = WAIT_FLAG;
			current_byte = 0;
			ones_count = 0;
			byte_index = 0;
			packet_buffer_size = 0;
			return false;
		}
		else if(ones_count == 6){ //flag
			bool done = false;
			if(state == IN_FRAME){				
				done = true;
			}
			else {
				packet_buffer_size = 0;
			}
			state = WAIT_FRAME;
			current_byte = 0;
			ones_count = 0;
			byte_index = 0;			

			return done;
		}
		else if(ones_count == 5){ //bit stuff
			ones_count = 0;
			return false;
		}
		else {
			ones_count = 0;
		}
	}

	//store
	current_byte = current_byte >> 1;
	current_byte |= (decoded_bit == 0x1 ? 0x80 : 0x0);
	byte_index++;

	if(byte_index >= 8){
		byte_index = 0;
		if(state == WAIT_FRAME){
			state = IN_FRAME;
		}

		if(state == IN_FRAME){	
			if(packet_buffer_size + 1 >= 256){
				state = WAIT_FLAG;
				current_byte = 0;
				ones_count = 0;
				byte_index = 0;
				packet_buffer_size = 0;
				return false;
			}
			packet_buffer[packet_buffer_size++] = current_byte;
		}
	}

	return false;
}

void APRSRxProcessor::on_message(const Message* const message) {
	if (message->id == Message::ID::APRSRxConfigure)
		configure(*reinterpret_cast<const APRSRxConfigureMessage*>(message));
	if(message->id == Message::ID::CaptureConfig)
		capture_config(*reinterpret_cast<const CaptureConfigMessage*>(message));	
}

void APRSRxProcessor::capture_config(const CaptureConfigMessage& message) {
	if( message.config ) {
		//stream = std::make_unique<StreamInput>(message.config);
		audio_output.set_stream(std::make_unique<StreamInput>(message.config));
	} else {
		//stream.reset();
		audio_output.set_stream(nullptr);		
	}
}

void APRSRxProcessor::configure(const APRSRxConfigureMessage& message) {	
	decim_0.configure(taps_11k0_decim_0.taps, 33554432);
	decim_1.configure(taps_11k0_decim_1.taps, 131072);
	channel_filter.configure(taps_11k0_channel.taps, 2);
	demod.configure(audio_fs, 5000);

	audio_output.configure(audio_24k_hpf_300hz_config, audio_24k_deemph_300_6_config, 0);
	
	samples_per_bit = audio_fs / message.baudrate;
	
	phase_inc = (0x10000 * message.baudrate) / audio_fs;
	phase = 0;
	
	// Delay line
	delay_line_index = 0;

	state = WAIT_FLAG;
	
	configured = true;
}

int main() {
	EventDispatcher event_dispatcher { std::make_unique<APRSRxProcessor>() };
	event_dispatcher.run();
	return 0;
}