/* -*- c++ -*- */
/*
 * Copyright 2004 Free Software Foundation, Inc.
 * 
 * This file is part of GNU Radio
 * 
 * GNU Radio 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.
 * 
 * GNU Radio 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 GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 */
/*
 * Copyright 2010, 2011, 2012, 2013 KA1RBI 
 */
/*
 * config.h is generated by configure.  It contains the results
 * of probing for features, options etc.  It should be the first
 * file included in your .cc file.
 */
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <repeater_p25_frame_assembler.h>
#include <gr_io_signature.h>

#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <errno.h>
#include <vector>
#include <bch.h>
#include <op25_imbe_frame.h>
#include <op25_p25_frame.h>
#include <p25_framer.h>
#include <repeater_p25_frame_assembler.h>
#include <sys/time.h>
#include <rs.h>

/*
 * Create a new instance of repeater_p25_frame_assembler and return
 * a boost shared_ptr.  This is effectively the public constructor.
 */
repeater_p25_frame_assembler_sptr 
repeater_make_p25_frame_assembler (const char* udp_host, int port, int debug, bool do_imbe, bool do_output, bool do_msgq, gr_msg_queue_sptr queue)
{
  return repeater_p25_frame_assembler_sptr (new repeater_p25_frame_assembler (udp_host, port, debug, do_imbe, do_output, do_msgq, queue));
}

static const int MIN_IN = 1;	// mininum number of input streams
static const int MAX_IN = 1;	// maximum number of input streams

static uint16_t crc16(uint8_t buf[], int len) {
        uint32_t poly = (1<<12) + (1<<5) + (1<<0);
        uint32_t crc = 0;
        for(int i=0; i<len; i++) {
                uint8_t bits = buf[i];
                for (int j=0; j<8; j++) {
                        uint8_t bit = (bits >> (7-j)) & 1;
                        crc = ((crc << 1) | bit) & 0x1ffff;
                        if (crc & 0x10000)
                                crc = (crc & 0xffff) ^ poly;
		}
	}
        crc = crc ^ 0xffff;
        return crc & 0xffff;
}

/* find_min is from wireshark/plugins/p25/packet-p25cai.c */
/* Copyright 2008, Michael Ossmann <mike@ossmann.com>  */
/* return the index of the lowest value in a list */
static int
find_min(uint8_t list[], int len)
{
	int min = list[0];	
	int index = 0;	
	int unique = 1;	
	int i;

	for (i = 1; i < len; i++) {
		if (list[i] < min) {
			min = list[i];
			index = i;
			unique = 1;
		} else if (list[i] == min) {
			unique = 0;
		}
	}
	/* return -1 if a minimum can't be found */
	if (!unique)
		return -1;

	return index;
}

/* count_bits is from wireshark/plugins/p25/packet-p25cai.c */
/* Copyright 2008, Michael Ossmann <mike@ossmann.com>  */
/* count the number of 1 bits in an int */
static int
count_bits(unsigned int n)
{
    int i = 0;
    for (i = 0; n != 0; i++)
        n &= n - 1;
    return i;
}

/* adapted from wireshark/plugins/p25/packet-p25cai.c */
/* Copyright 2008, Michael Ossmann <mike@ossmann.com>  */
/* deinterleave and trellis1_2 decoding */
/* tsbk_buf is assumed to be a buffer of 12 bytes */
static int
tsbk_deinterleave(bit_vector& bv, unsigned int start, uint8_t* tsbk_buf)
{
	static const uint16_t deinterleave_tb[] = {
	  0,  1,  2,  3,  52, 53, 54, 55, 100,101,102,103, 148,149,150,151,
	  4,  5,  6,  7,  56, 57, 58, 59, 104,105,106,107, 152,153,154,155,
	  8,  9, 10, 11,  60, 61, 62, 63, 108,109,110,111, 156,157,158,159,
	 12, 13, 14, 15,  64, 65, 66, 67, 112,113,114,115, 160,161,162,163,
	 16, 17, 18, 19,  68, 69, 70, 71, 116,117,118,119, 164,165,166,167,
	 20, 21, 22, 23,  72, 73, 74, 75, 120,121,122,123, 168,169,170,171,
	 24, 25, 26, 27,  76, 77, 78, 79, 124,125,126,127, 172,173,174,175,
	 28, 29, 30, 31,  80, 81, 82, 83, 128,129,130,131, 176,177,178,179,
	 32, 33, 34, 35,  84, 85, 86, 87, 132,133,134,135, 180,181,182,183,
	 36, 37, 38, 39,  88, 89, 90, 91, 136,137,138,139, 184,185,186,187,
	 40, 41, 42, 43,  92, 93, 94, 95, 140,141,142,143, 188,189,190,191,
	 44, 45, 46, 47,  96, 97, 98, 99, 144,145,146,147, 192,193,194,195,
	 48, 49, 50, 51 };

	uint8_t hd[4];
	int b, d, j;
	int state = 0;
	uint8_t codeword;
	uint16_t crc;

	static const uint8_t next_words[4][4] = {
		{0x2, 0xC, 0x1, 0xF},
		{0xE, 0x0, 0xD, 0x3},
		{0x9, 0x7, 0xA, 0x4},
		{0x5, 0xB, 0x6, 0x8}
	};

	memset(tsbk_buf, 0, 12);

	for (b=0; b < 98*2; b += 4) {
		codeword = (bv[start+deinterleave_tb[b+0]] << 3) + 
		           (bv[start+deinterleave_tb[b+1]] << 2) + 
		           (bv[start+deinterleave_tb[b+2]] << 1) + 
		            bv[start+deinterleave_tb[b+3]]     ;

		/* try each codeword in a row of the state transition table */
		for (j = 0; j < 4; j++) {
			/* find Hamming distance for candidate */
			hd[j] = count_bits(codeword ^ next_words[state][j]);
		}
		/* find the dibit that matches the most codeword bits (minimum Hamming distance) */
		state = find_min(hd, 4);
		/* error if minimum can't be found */
		if(state == -1)
			return -1;	// decode error, return failure
		/* It also might be nice to report a condition where the minimum is
		 * non-zero, i.e. an error has been corrected.  It probably shouldn't
		 * be a permanent failure, though.
		 *
		 * DISSECTOR_ASSERT(hd[state] == 0);
		 */

		/* append dibit onto output buffer */
		d = b >> 2;	// dibit ctr
		if (d < 48) {
			tsbk_buf[d >> 2] |= state << (6 - ((d%4) * 2));
		}
	}
	crc = crc16(tsbk_buf, 12);
	if (crc != 0)
		return -1;	// trellis decode OK, but CRC error occurred
	return 0;	// return OK code
}

/*
 * The private constructor
 */
repeater_p25_frame_assembler::repeater_p25_frame_assembler (const char* udp_host, int port, int debug, bool do_imbe, bool do_output, bool do_msgq, gr_msg_queue_sptr queue)
  : gr_block ("p25_frame_assembler",
		   gr_make_io_signature (MIN_IN, MAX_IN, sizeof (char)),
		   gr_make_io_signature ((do_output) ? 1 : 0, (do_output) ? 1 : 0, (do_output) ? sizeof(char) : 0 )),
	write_bufp(0),
	write_sock(0),
	d_udp_host(udp_host),
	d_port(port),
	d_debug(debug),
	d_do_imbe(do_imbe),
	d_do_output(do_output),
	d_do_msgq(do_msgq),
	d_msg_queue(queue),
	symbol_queue(),
	framer(new p25_framer())
{
	if (port > 0)
		init_sock(d_udp_host, d_port);
}

repeater_p25_frame_assembler::~repeater_p25_frame_assembler ()
{
	if (write_sock > 0)
		close(write_sock);
	delete framer;
}

void
repeater_p25_frame_assembler::forecast(int nof_output_items, gr_vector_int &nof_input_items_reqd)
{
   // for do_imbe=false: we output packed bytes (4:1 ratio)
   // for do_imbe=true: input rate= 4800, output rate= 1600 = 32 * 50 (3:1)
   const size_t nof_inputs = nof_input_items_reqd.size();
   int nof_samples_reqd = 4.0 * nof_output_items;
   if (d_do_imbe)
     nof_samples_reqd = 3.0 * nof_output_items;
   std::fill(&nof_input_items_reqd[0], &nof_input_items_reqd[nof_inputs], nof_samples_reqd);
}
void 
repeater_p25_frame_assembler::process_tsbk(uint32_t const nac, uint8_t const tsbk_buf[])
{
	char wbuf[12];
	if (!d_do_msgq)
		return;
	if (d_msg_queue->full_p())
		return;
	memcpy(wbuf, tsbk_buf, 10);
	wbuf[10] = (nac >> 8) & 0xff;
	wbuf[11] = nac & 0xff;
	gr_message_sptr msg = gr_make_message_from_string(std::string(wbuf, 12), 0, 0, 0);
	d_msg_queue->insert_tail(msg);
//	msg.reset();
}

int 
repeater_p25_frame_assembler::general_work (int noutput_items,
                               gr_vector_int &ninput_items,
                               gr_vector_const_void_star &input_items,
                               gr_vector_void_star &output_items)
{

  const uint8_t *in = (const uint8_t *) input_items[0];

  for (int i = 0; i < noutput_items; i++){
    if(framer->rx_sym(in[i])) {   // complete frame was detected
		if (d_debug >= 10) {
			fprintf (stderr, "NAC 0x%X DUID 0x%X len %u errs %u ", framer->nac, framer->duid, framer->frame_size >> 1, framer->bch_errors);
		}
		if (framer->duid == 0x07 && framer->bch_errors >= 0) {
			unsigned int d, b;
			int rc;
			bit_vector bv1(720);
			uint8_t tsbk_buf[12];
			
			if (framer->frame_size > 720) {
				fprintf(stderr, "warning trunk frame size %u exceeds maximum\n", framer->frame_size);
				framer->frame_size = 720;
			}
			for (d=0, b=0; d < framer->frame_size >> 1; d++) {
				if ((d+1) % 36 == 0)
					continue;	// skip SS
				bv1[b++] = framer->frame_body[d*2];
				bv1[b++] = framer->frame_body[d*2+1];
			}
			if (framer->frame_size >= 360) {
				rc = tsbk_deinterleave(bv1,48+64        , tsbk_buf);
				if (rc == 0)
					process_tsbk(framer->nac, tsbk_buf);
			}
			if (framer->frame_size >= 576) {
				rc = tsbk_deinterleave(bv1,48+64+196    , tsbk_buf);
				if (rc == 0)
					process_tsbk(framer->nac, tsbk_buf);
			}
			if (framer->frame_size >= 720) {
				rc = tsbk_deinterleave(bv1,48+64+196+196, tsbk_buf);
				if (rc == 0)
					process_tsbk(framer->nac, tsbk_buf);
			}
		}
		if (d_debug >= 10 && framer->duid == 0x00) {
			ProcHDU(framer->frame_body);
		} else if (d_debug > 10 && framer->duid == 0x05) {
			ProcLDU1(framer->frame_body);
		} else if (d_debug >= 10 && framer->duid == 0x0a) {
			ProcLDU2(framer->frame_body);
		} else if (d_debug > 10 && framer->duid == 0x0f) {
			ProcTDU(framer->frame_body);
		}
		if (d_debug >= 10)
			fprintf(stderr, "\n");
		if (d_do_imbe && (framer->duid == 0x5 || framer->duid == 0xa)) {  // if voice - ldu1 or ldu2
			for(size_t i = 0; i < nof_voice_codewords; ++i) {
				voice_codeword cw(voice_codeword_sz);
				uint32_t E0, ET;
				uint32_t u[8];
				char s[128];
				imbe_deinterleave(framer->frame_body, cw, i);
				// recover 88-bit IMBE voice code word
				imbe_header_decode(cw, u[0], u[1], u[2], u[3], u[4], u[5], u[6], u[7], E0, ET);
				// output one 32-byte msg per 0.020 sec.
				// also, 32*9 = 288 byte pkts (for use via UDP)
				sprintf(s, "%03x %03x %03x %03x %03x %03x %03x %03x\n", u[0], u[1], u[2], u[3], u[4], u[5], u[6], u[7]);
				if (d_do_output) {
					for (size_t j=0; j < strlen(s); j++) {
						symbol_queue.push_back(s[j]);
					}
				}
				if (write_sock > 0) {
					memcpy(&write_buf[write_bufp], s, strlen(s));
					write_bufp += strlen(s);
					if (write_bufp >= 288) { // 9 * 32 = 288
						sendto(write_sock, write_buf, 288, 0, (struct sockaddr *)&write_sock_addr, sizeof(write_sock_addr));
						// FIXME check sendto() rc
						write_bufp = 0;
					}
				}
			}
		} // end of imbe/voice
		if (!d_do_imbe) {
			// pack the bits into bytes, MSB first
			size_t obuf_ct = 0;
			uint8_t obuf[P25_VOICE_FRAME_SIZE/2];
			for (uint32_t i = 0; i < framer->frame_size; i += 8) {
				uint8_t b = 
					(framer->frame_body[i+0] << 7) +
					(framer->frame_body[i+1] << 6) +
					(framer->frame_body[i+2] << 5) +
					(framer->frame_body[i+3] << 4) +
					(framer->frame_body[i+4] << 3) +
					(framer->frame_body[i+5] << 2) +
					(framer->frame_body[i+6] << 1) +
					(framer->frame_body[i+7]     );
				obuf[obuf_ct++] = b;
			}
			if (write_sock > 0) {
				sendto(write_sock, obuf, obuf_ct, 0, (struct sockaddr*)&write_sock_addr, sizeof(write_sock_addr));
			}
			if (d_do_output) {
				for (size_t j=0; j < obuf_ct; j++) {
					symbol_queue.push_back(obuf[j]);
				}
			}
		}
    }  // end of complete frame
  }
  int amt_produce = 0;
  amt_produce = noutput_items;
  if (amt_produce > (int)symbol_queue.size())
    amt_produce = symbol_queue.size();
  if (amt_produce > 0) {
    unsigned char *out = (unsigned char *) output_items[0];
    copy(symbol_queue.begin(), symbol_queue.begin() + amt_produce, out);
    symbol_queue.erase(symbol_queue.begin(), symbol_queue.begin() + amt_produce);
  }
  // printf ("work: ninp[0]: %d nout: %d size %d produce: %d surplus %d\n", ninput_items[0], noutput_items, symbol_queue.size(), amt_produce, surplus);
  consume_each(noutput_items);
  // Tell runtime system how many output items we produced.
  return amt_produce;
}

void repeater_p25_frame_assembler::init_sock(const char* udp_host, int udp_port)
{
        memset (&write_sock_addr, 0, sizeof(write_sock_addr));
        write_sock = socket(PF_INET, SOCK_DGRAM, 17);   // UDP socket
        if (write_sock < 0) {
                fprintf(stderr, "op25_imbe_vocoder: socket: %d\n", errno);
                write_sock = 0;
		return;
        }
        if (!inet_aton(udp_host, &write_sock_addr.sin_addr)) {
                fprintf(stderr, "op25_imbe_vocoder: inet_aton: bad IP address\n");
		close(write_sock);
		write_sock = 0;
		return;
	}
        write_sock_addr.sin_family = AF_INET;
        write_sock_addr.sin_port = htons(udp_port);
}