/* -*- 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, 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_imbe_decode_fb.h>
#include <gr_io_signature.h>

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <vector>
#include <bch.h>
#include <op25_imbe_frame.h>
#include <op25_p25_frame.h>
#include <repeater_imbe_decode_fb.h>
#include <sys/time.h>

/*
 * Create a new instance of repeater_imbe_decode_fb and return
 * a boost shared_ptr.  This is effectively the public constructor.
 */
repeater_imbe_decode_fb_sptr 
repeater_make_imbe_decode_fb ()
{
  return repeater_imbe_decode_fb_sptr (new repeater_imbe_decode_fb ());
}

/*
 * Specify constraints on number of input and output streams.
 * This info is used to construct the input and output signatures
 * (2nd & 3rd args to gr_block's constructor).  The input and
 * output signatures are used by the runtime system to
 * check that a valid number and type of inputs and outputs
 * are connected to this block.  In this case, we accept
 * only 1 input and 1 output.
 */
static const int MIN_IN = 1;	// mininum number of input streams
static const int MAX_IN = 1;	// maximum number of input streams
static const int MIN_OUT = 1;	// minimum number of output streams
static const int MAX_OUT = 1;	// maximum number of output streams

/*
 * The private constructor
 */
repeater_imbe_decode_fb::repeater_imbe_decode_fb ()
  : gr_block ("imbe_decode_fb",
		   gr_make_io_signature (MIN_IN, MAX_IN, sizeof (float)),
		   gr_make_io_signature (MIN_OUT, MAX_OUT, sizeof (unsigned char))),
    reverse_p(0),
    hdr_syms(0),
    next_bit(0),
    frame_body(P25_VOICE_FRAME_SIZE),
    symbol_queue()
{
}

/*
 * Our virtual destructor.
 */
repeater_imbe_decode_fb::~repeater_imbe_decode_fb ()
{
  // nothing else required in this example
}

/*
 * Process the 64 bits after the frame sync, which should be the frame header
 * 1. verify bch and reject if bch cannot be decoded
 * 2. extract NAC and DUID
 * Returns false if decode failure, else true
 */
bool repeater_imbe_decode_fb::header_codeword(uint64_t acc, uint32_t& nac, uint32_t& duid) {
	bit_vector cw(64);
	bool low = acc & 1;
	// for bch, split bits into codeword vector
	for (int i=0; i < 64; i++) {
		acc >>= 1;
		cw[i] = acc & 1;
	}

	// do bch decode
	int rc = bchDec(cw);

	// check if bch decode unsuccessful
	if (rc < 0) {
		return false;
	}

	// load corrected bch bits into acc
	acc = 0;
	for (int i=63; i>=0; i--) {
		acc |= cw[i];
		acc <<= 1;
	}
	acc |= low;   // FIXME

	// extract nac and duid
	nac  = (acc >> 52) & 0xfff;
	duid = (acc >> 48) & 0x00f;
	hdr_word = acc;
	printf ("nac %x duid %x errs %d\n", nac, duid, rc);

	return true;
}

/*
 * rx_sym: called once per received symbol
 * 1. looks for flags sequences
 * 2. after flags detected (hdr_syms > 0), accumulate 64-bit header word
 * 3. check for voice frames (ldu1|2)
 * 4. for ldu1|2 (next_bit > 0), accumulate all 1728 bits in frame 
 * 5. FIXME regenerate all fields in frame (flags, bch, hamming/golay, rs, pn)
 * 6. place symbols into output symbol queue
 */
void repeater_imbe_decode_fb::rx_sym(uint8_t dibit) {
	// FIXME assert(dibit >= 0 && dibit <= 3)
	header_accum <<= 2;
	header_accum |= dibit;
	if (hdr_syms == 12) {
		// ignore status dibit
		header_accum >>= 2;
	} else if (hdr_syms >= 33) {
		// header completely received
		bool bch_rc = header_codeword(header_accum, nac, duid);
			// next_bit = 48 + 64 + 2;
		if (bch_rc) {
			next_bit = 48 + 64;
			frame_size_limit = P25_VOICE_FRAME_SIZE;
		}
		hdr_syms = 0;
	}
	if (hdr_syms)
		// count symbols in header
		hdr_syms++;
	if ((header_accum & P25_FRAME_SYNC_MASK) == P25_FRAME_SYNC_MAGIC) {
		hdr_syms = 1;
	}
	if ((header_accum & P25_FRAME_SYNC_MASK) == P25_FRAME_SYNC_REV_P) {
		reverse_p = 0x02;
		hdr_syms = 1;
	}
	if (next_bit) {
		frame_body[next_bit++] = (dibit >> 1) & 1;
		frame_body[next_bit++] =  dibit       & 1;
	}
	// dispose of received frame (if exists) and:
	// 1. complete frame is received, or
	// 2. flags is received
	if (next_bit && (next_bit >= frame_size_limit || hdr_syms == 1)) {
		// dump_cw(frame_body, 216, stdout);
		if (duid == 0x5 || 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(frame_body, cw, i);
				imbe_header_decode(cw, u[0], u[1], u[2], u[3], u[4], u[5], u[6], u[7], E0, ET);
				sprintf(s, "%x %x %x %x %x %x %x %x\n", u[0], u[1], u[2], u[3], u[4], u[5], u[6], u[7]);
				for (size_t j=0; j < strlen(s); j++) {
					symbol_queue.push_back(s[j]);
				}
			}
		}
		next_bit = 0;
	}
}

/*
 * Slicer for normalized baseband FSK4
 */
static inline uint8_t
slice (float s) {
	uint8_t dibit;
	if(s < -2.0) {
		dibit = 3;
	} else if(s <  0.0) {
		dibit = 2;
	} else if(s <  2.0) {
		dibit = 0;
	} else {
		dibit = 1;
	}
	return dibit;
}

void
repeater_imbe_decode_fb::forecast(int nof_output_items, gr_vector_int &nof_input_items_reqd)
{
   /* input rate: 4800, output rate: 1550 (31 * 50) */
   const size_t nof_inputs = nof_input_items_reqd.size();
   const int nof_samples_reqd = 3.1 * nof_output_items;
   std::fill(&nof_input_items_reqd[0], &nof_input_items_reqd[nof_inputs], nof_samples_reqd);
}


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

  const float *in = (const float *) input_items[0];
  unsigned char *out = (unsigned char *) output_items[0];

  for (int i = 0; i < noutput_items; i++){
    rx_sym(slice(in[i]) ^ reverse_p);
  }
  int amt_produce = 0;
  amt_produce = noutput_items;
  if (amt_produce > (int)symbol_queue.size())
    amt_produce = symbol_queue.size();
  if (amt_produce > 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;
}