/* -*- c++ -*- */
/* 
 * NXDN Encoder (C) Copyright 2017 Max H. Parke KA1RBI
 * 
 * This file is part of OP25
 * 
 * This 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 3, or (at your option)
 * any later version.
 * 
 * This software 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 software; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <gnuradio/io_signature.h>
#include "nxdn.h"
#include "mbelib.h"
#include "p25p2_vf.h"
#include "nxdn_tx_sb_impl.h"
#include "nxdn_const.h"
#include <op25_imbe_frame.h>

#include <vector>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <assert.h>

static const uint8_t nxdn_fs[] = {3, 0, 3, 1, 3, 3, 1, 1, 2, 1};

#if 0
static inline void print_result(char title[], const uint8_t r[], int len) {
	uint8_t buf[256];
	for (int i=0; i<len; i++){
		buf[i] = r[i] + '0';
	}
	buf[len] = 0;
	printf("%s: %s\n", title, buf);
}
#endif

static inline int store_i(int reg, uint8_t val[], int len) {
	for (int i=0; i<len; i++){
		val[i] = (reg >> (len-1-i)) & 1;
	}
}

static inline void bits_to_dibits(uint8_t* dest, const uint8_t* src, int n_dibits) {
	for (int i=0; i<n_dibits; i++) {
		dest[i] = src[i*2] * 2 + src[i*2+1];
	}
}

static inline void bool_to_dibits(uint8_t* dest, const std::vector<bool> src, int n_dibits) {
	for (int i=0; i<n_dibits; i++) {
		int l = src[i*2] ? 1 : 0;
		int r = src[i*2+1] ? 1 : 0;
		dest[i] = l * 2 + r;
	}
}

static inline int load_i(const uint8_t val[], int len) {
	int acc = 0;
	for (int i=0; i<len; i++){
		acc = (acc << 1) + (val[i] & 1);
	}
	return acc;
}

// unpacks bytes into bits, len is length of result
static inline void unpack_bytes(uint8_t result[], const char src[], int len) {
	static const int nbytes = len / 8;
	int outp = 0;
	for (int i=0; i < len; i++) {
		result[i] = (src[i>>3] >> (7-(i%8))) & 1;
	}
}

static inline int crc6(const uint8_t * bits, int len) {
	uint8_t s[6] = {1,1,1,1,1,1};
	for (int i=0; i<len; i++) {
		int bit = bits[i];
		int a = bit ^ s[0];
		s[0] = a ^ s[1];
		s[1] = s[2];
		s[2] = s[3];
		s[3] = a ^ s[4];
		s[4] = a ^ s[5];
		s[5] = a;
	}
	return (load_i(s, 6));
}

static inline int crc12(uint8_t bits[], int len) {
	uint8_t s[] = {1,1,1,1,1,1,1,1,1,1,1,1};
	for (int i=0; i<len; i++) {
		int bit = bits[i];
		int a = bit ^ s[0];
		s[0] = a ^ s[1];
		s[1] = s[2];
		s[2] = s[3];
		s[3] = s[4];
		s[4] = s[5];
		s[5] = s[6];
		s[6] = s[7];
		s[7] = s[8];
		s[8] = a ^ s[9];
		s[9] = a ^ s[10];
		s[10] = a ^ s[11];
		s[11] = a;
	}
	return (load_i(s, 12));
}

static const uint8_t trellis_PC[] = {0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1};
static inline void trellis_encode(const uint8_t src_bits[], uint8_t dest_bits[], const int dest_len) {
	int reg = 0;
	for (int i=0; i<dest_len/2; i++) {
		int bit = src_bits[i];
		reg = (reg << 1) | bit;
		dest_bits[i*2]   = trellis_PC[reg & 0x19];
		dest_bits[i*2+1] = trellis_PC[reg & 0x17];
	}
}

static inline uint16_t crc16(const 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 bit = buf[i] & 1;
                crc = ((crc << 1) | bit) & 0x1ffff;
                if (crc & 0x10000)
                        crc = (crc & 0xffff) ^ poly;
	}
        crc = crc ^ 0xffff;
        return crc & 0xffff;
}

static inline void encode_sacch_chunk(const uint8_t src_bits[18], uint8_t dest_dibits[30], int structure, int ran) {
	uint8_t buf1[60];
	uint8_t buf2[72];
	uint8_t str_field[8];
	int str;

	str = (structure & 3) << 6;
	str |= ran & 0x3f;
	store_i(str, str_field, 8);

	memcpy(buf1, str_field, 8);
	memcpy(buf1+8, src_bits, 18);

	int crc = crc6(buf1, 26);
	store_i(crc, buf1+26, 6);
	store_i(0, buf1+32, 4);
	trellis_encode(buf1, buf2, sizeof(buf2));
	for (int i=0, op=0; i<sizeof(buf2); i += 12) {
		buf1[op++] = buf2[i+0];
		buf1[op++] = buf2[i+1];
		buf1[op++] = buf2[i+2];
		buf1[op++] = buf2[i+3];
		buf1[op++] = buf2[i+4];
		buf1[op++] = buf2[i+6];
		buf1[op++] = buf2[i+7];
		buf1[op++] = buf2[i+8];
		buf1[op++] = buf2[i+9];
		buf1[op++] = buf2[i+10];
	}
	for (int i=0; i<60; i++) {
		buf2[i] = buf1[PERM_12_5[i]];
	}
	bits_to_dibits(dest_dibits, buf2, 30);
}

static inline void encode_facch(const uint8_t src_bits[80], uint8_t dest_dibits[72]) {
	uint8_t buf1[144];
	uint8_t buf2[192];
	memcpy(buf1, src_bits, 80);
	int crc = crc12(buf1, 80);
	store_i(crc, buf1+80, 12);
	store_i(0, buf1+92, 4);
	trellis_encode(buf1, buf2, sizeof(buf2));
	for (int i=0, op=0; i<sizeof(buf2); i += 4) {
		buf1[op++] = buf2[i+0];
		buf1[op++] = buf2[i+2];
		buf1[op++] = buf2[i+3];
	}
	for (int i=0; i<144; i++) {
		buf2[i] = buf1[PERM_16_9[i]];
	}
	bits_to_dibits(dest_dibits, buf2, 72);
}

static inline void encode_lich(const int lich, uint8_t lich_dibits[8]) {
	uint8_t b[8];
	store_i(lich & 0x7f, b, 7);
	b[8] = (b[0] + b[1] + b[2] + b[3]) & 1;
	for (int i=0; i<8; i++)
		lich_dibits[i] = (b[i]) ? 3 : 1;
}

namespace gr {
  namespace op25_repeater {

    nxdn_tx_sb::sptr
    nxdn_tx_sb::make(int verbose_flag, const char * config_file, bool nxdn96_mode)
    {
      return gnuradio::get_initial_sptr
        (new nxdn_tx_sb_impl(verbose_flag, config_file, nxdn96_mode));
    }

//////////////////////////////////////////////////////////////////////////

static const int MIN_IN = 1;
static const int MAX_IN = 1;

static const int MIN_OUT = 1;
static const int MAX_OUT = 1;

    /*
     * The private constructor
     */
    nxdn_tx_sb_impl::nxdn_tx_sb_impl(int verbose_flag, const char * config_file, bool nxdn96_mode)
      : gr::block("nxdn_tx_sb",
              gr::io_signature::make (MIN_IN, MAX_IN, sizeof(short)),
              gr::io_signature::make (MIN_OUT, MAX_OUT, sizeof(char))),
              d_verbose_flag(verbose_flag),
              d_config_file(config_file),
              d_nxdn96_mode(nxdn96_mode),
              d_output_amount((nxdn96_mode) ? 384 : 192),
              d_sacch_seq(0),
              d_lich(0),
              d_ran(0)
    {
      set_output_multiple(d_output_amount);
      memset(d_acch, 0, sizeof(d_acch));
      config();
    }

    /*
     * Our virtual destructor.
     */
    nxdn_tx_sb_impl::~nxdn_tx_sb_impl()
    {
    }

void
nxdn_tx_sb_impl::config()
{
	FILE * fp1 = fopen(d_config_file, "r");
	char line[256];
	char * cp;
	unsigned int li[9];
	long int ran;
	long int lich, lich2;
	if (!fp1) {
		fprintf(stderr, "nxdn_tx_sb_impl:config: failed to open %s\n", d_config_file);
		return;
	}
	for (;;) {
		cp = fgets(line, sizeof(line) - 2, fp1);
		if (!cp) break;
		if (line[0] == '#') continue;
		if (memcmp(line, "ran=", 4) == 0) {
			ran = strtol(line+4, 0, 0);
			d_ran = ran;
		} else if (memcmp(line, "lich=", 5) == 0) {
			lich = strtol(line+5, 0, 0);
			d_lich = lich;
		} else if (memcmp(line, "lich2=", 6) == 0) {
			lich2 = strtol(line+6, 0, 0);
			d_lich2 = lich2;
		} else if (memcmp(line, "acch=", 5) == 0) {
			sscanf(&line[5], "%x %x %x %x %x %x %x %x %x %x", &li[0], &li[1], &li[2], &li[3], &li[4], &li[5], &li[6], &li[7], &li[8], &li[9]);
			for (int i=0; i<10; i++) {
				store_i(li[i], d_acch+i*8, 8);
			}
		}
	}
	encode_lich(d_lich, d_lich_x1);
	encode_lich(d_lich2, d_lich_x2);
	encode_facch(d_acch, d_facch1);
	for (int i=0; i<4; i++)
		encode_sacch_chunk(d_acch+18*i, d_sacch[i], 3-i, d_ran);
	fclose(fp1);
}

void
nxdn_tx_sb_impl::forecast(int nof_output_items, gr_vector_int &nof_input_items_reqd)
{
   // each 192-dibit output frame contains four voice code words=640 samples
   // for nxdn96 we output 384 dibits per four voice code words
   const size_t nof_inputs = nof_input_items_reqd.size();
   const int nof_vcw = nof_output_items / d_output_amount;
   const int nof_samples_reqd = nof_vcw * 4 * 160;
   std::fill(&nof_input_items_reqd[0], &nof_input_items_reqd[nof_inputs], nof_samples_reqd);
}

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

  int nconsumed = 0;
  int16_t *in;
  in = (int16_t *) input_items[0];
  uint8_t *out = reinterpret_cast<uint8_t*>(output_items[0]);
  int nframes=0;
  int16_t frame_vector[8];
  voice_codeword cw(voice_codeword_sz);
  uint8_t ambe_codeword[36];	// dibits
  std::vector <bool> interleaved_buf(144);

  for (int n=0;n < (noutput_items/d_output_amount);n++) {
    // need (at least) four voice codewords worth of samples
    if (ninput_items[0] - nconsumed < 4*160) break;
    memcpy(out, nxdn_fs, sizeof(nxdn_fs));
    memcpy(out+10, d_lich_x1, 8);
    memcpy(out+18, d_sacch[d_sacch_seq++ % 4], 30);
    // TODO: would be nice to multithread these
    for (int vcw = 0; vcw < 4; vcw++) {
      d_halfrate_encoder.encode(in, ambe_codeword);
      memcpy(out+48+36*vcw, ambe_codeword, sizeof(ambe_codeword));
      in += 160;
      nconsumed += 160;
    }
    nxdn_descramble(out+10, 182);
    if (d_nxdn96_mode) {
       memcpy(out+192, nxdn_fs, sizeof(nxdn_fs));
       memcpy(out+192+10, d_lich_x2, 8);
       memcpy(out+192+18, d_sacch[d_sacch_seq++ % 4], 30);
       memcpy(out+192+48, d_facch1, 72);
       memcpy(out+192+120, d_facch1, 72);
       nxdn_descramble(out+192+10, 182);
    }
    nframes += 1;
    out += d_output_amount;
  }

  // Tell runtime system how many input items we consumed on
  // each input stream.

  if (nconsumed)
    consume_each(nconsumed);

  // Tell runtime system how many output items we produced.
  return (nframes * d_output_amount);
}

void
nxdn_tx_sb_impl::set_gain_adjust(float gain_adjust) {
	d_halfrate_encoder.set_gain_adjust(gain_adjust);
}

  } /* namespace op25_repeater */
} /* namespace gr */