/*---------------------------------------------------------------------------*\

  FILE........: freedv_1600.c
  AUTHOR......: David Rowe
  DATE CREATED: May 2020

  Functions that implement the FreeDV 1600 mode.

\*---------------------------------------------------------------------------*/

#include <assert.h>
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "codec2.h"
#include "codec2_fdmdv.h"
#include "comp_prim.h"
#include "debug_alloc.h"
#include "fdmdv_internal.h"
#include "freedv_api.h"
#include "freedv_api_internal.h"
#include "golay23.h"
#include "varicode.h"

void freedv_1600_open(struct freedv *f) {
  f->snr_squelch_thresh = 2.0;
  f->squelch_en = true;
  f->tx_sync_bit = 0;
  int Nc = 16;
  f->fdmdv = fdmdv_create(Nc);
  assert(f->fdmdv != NULL);
  golay23_init();
  f->nin = f->nin_prev = FDMDV_NOM_SAMPLES_PER_FRAME;
  f->n_nom_modem_samples = 2 * FDMDV_NOM_SAMPLES_PER_FRAME;
  f->n_nat_modem_samples = f->n_nom_modem_samples;
  f->n_max_modem_samples =
      FDMDV_NOM_SAMPLES_PER_FRAME + FDMDV_MAX_SAMPLES_PER_FRAME;
  f->modem_sample_rate = FREEDV_FS_8000;
  int nbit = fdmdv_bits_per_frame(f->fdmdv);
  f->fdmdv_bits = (int *)MALLOC(nbit * sizeof(int));
  assert(f->fdmdv_bits != NULL);
  nbit = 2 * fdmdv_bits_per_frame(f->fdmdv);
  f->fdmdv_tx_bits = (int *)CALLOC(1, nbit * sizeof(int));
  f->fdmdv_rx_bits = (int *)CALLOC(1, nbit * sizeof(int));
  assert(f->fdmdv_tx_bits != NULL);
  assert(f->fdmdv_rx_bits != NULL);
  f->evenframe = 0;
  f->sz_error_pattern = fdmdv_error_pattern_size(f->fdmdv);

  f->speech_sample_rate = FREEDV_FS_8000;
  f->codec2 = codec2_create(CODEC2_MODE_1300);
  assert(f->codec2 != NULL);
  f->n_speech_samples = codec2_samples_per_frame(f->codec2);

  f->bits_per_modem_frame = fdmdv_bits_per_frame(f->fdmdv);
  f->bits_per_codec_frame = codec2_bits_per_frame(f->codec2);
  f->n_codec_frames = 1;
  f->tx_payload_bits = MALLOC(f->bits_per_codec_frame);
  assert(f->tx_payload_bits != NULL);
  f->rx_payload_bits = MALLOC(f->bits_per_codec_frame);
  assert(f->rx_payload_bits != NULL);
}

void freedv_comptx_fdmdv_1600(struct freedv *f, COMP mod_out[]) {
  int i, j;
  int data, codeword1, data_flag_index;
  COMP tx_fdm[f->n_nat_modem_samples];

  // spare bit in frame that codec defines.  Use this 1
  // bit/frame to send txt messages

  data_flag_index = codec2_get_spare_bit_index(f->codec2);

  if (f->nvaricode_bits) {
    f->tx_payload_bits[data_flag_index] =
        f->tx_varicode_bits[f->varicode_bit_index++];
    f->nvaricode_bits--;
  }

  if (f->nvaricode_bits == 0) {
    /* get new char and encode */
    char s[2];
    if (f->freedv_get_next_tx_char != NULL) {
      s[0] = (*f->freedv_get_next_tx_char)(f->callback_state);
      f->nvaricode_bits =
          varicode_encode(f->tx_varicode_bits, s, VARICODE_MAX_BITS, 1,
                          f->varicode_dec_states.code_num);
      f->varicode_bit_index = 0;
    }
  }

  /* Protect first 12 out of first 16 excitation bits with (23,12) Golay Code:

     0,1,2,3: v[0]..v[3]
     4,5,6,7: MSB of pitch
     11,12,13,14: MSB of energy

  */

  data = 0;
  for (i = 0; i < 8; i++) {
    data <<= 1;
    data |= f->tx_payload_bits[i];
  }
  for (i = 11; i < 15; i++) {
    data <<= 1;
    data |= f->tx_payload_bits[i];
  }
  codeword1 = golay23_encode(data);

  /* now pack output frame with parity bits at end to make them
     as far apart as possible from the data they protect.  Parity
     bits are LSB of the Golay codeword */

  for (i = 0; i < f->bits_per_codec_frame; i++)
    f->fdmdv_tx_bits[i] = f->tx_payload_bits[i];
  for (j = 0; i < f->bits_per_codec_frame + 11; i++, j++) {
    f->fdmdv_tx_bits[i] = (codeword1 >> (10 - j)) & 0x1;
  }
  f->fdmdv_tx_bits[i] = 0; /* spare bit */

  /* optionally overwrite with test frames */

  if (f->test_frames) {
    fdmdv_get_test_bits(f->fdmdv, f->fdmdv_tx_bits);
    fdmdv_get_test_bits(f->fdmdv, &f->fdmdv_tx_bits[f->bits_per_modem_frame]);
  }

  /* modulate even and odd frames */

  fdmdv_mod(f->fdmdv, tx_fdm, f->fdmdv_tx_bits, &f->tx_sync_bit);
  assert(f->tx_sync_bit == 1);

  fdmdv_mod(f->fdmdv, &tx_fdm[FDMDV_NOM_SAMPLES_PER_FRAME],
            &f->fdmdv_tx_bits[f->bits_per_modem_frame], &f->tx_sync_bit);
  assert(f->tx_sync_bit == 0);

  assert(2 * FDMDV_NOM_SAMPLES_PER_FRAME == f->n_nom_modem_samples);

  for (i = 0; i < f->n_nom_modem_samples; i++)
    mod_out[i] = fcmult(FDMDV_SCALE, tx_fdm[i]);
}

int freedv_comprx_fdmdv_1600(struct freedv *f, COMP demod_in[]) {
  int bits_per_fdmdv_frame;
  int i, j;
  int recd_codeword, codeword1, data_flag_index, n_ascii;
  short abit[1];
  char ascii_out;
  int reliable_sync_bit;
  int rx_status = 0;

  COMP ademod_in[f->nin];
  for (i = 0; i < f->nin; i++)
    ademod_in[i] = fcmult(1.0 / FDMDV_SCALE, demod_in[i]);

  bits_per_fdmdv_frame = fdmdv_bits_per_frame(f->fdmdv);

  fdmdv_demod(f->fdmdv, f->fdmdv_bits, &reliable_sync_bit, ademod_in, &f->nin);
  fdmdv_get_demod_stats(f->fdmdv, &f->stats);
  f->sync = f->fdmdv->sync;
  f->snr_est = f->stats.snr_est;

  if (reliable_sync_bit == 1) {
    f->evenframe = 1;
  }

  if (f->sync) {
    rx_status = FREEDV_RX_SYNC;

    if (f->evenframe == 0) {
      memcpy(f->fdmdv_rx_bits, f->fdmdv_bits,
             bits_per_fdmdv_frame * sizeof(int));
    } else {
      memcpy(&f->fdmdv_rx_bits[bits_per_fdmdv_frame], f->fdmdv_bits,
             bits_per_fdmdv_frame * sizeof(int));

      if (f->test_frames == 0) {
        recd_codeword = 0;
        for (i = 0; i < 8; i++) {
          recd_codeword <<= 1;
          recd_codeword |= (f->fdmdv_rx_bits[i] & 0x1);
        }
        for (i = 11; i < 15; i++) {
          recd_codeword <<= 1;
          recd_codeword |= (f->fdmdv_rx_bits[i] & 0x1);
        }
        for (i = f->bits_per_codec_frame; i < f->bits_per_codec_frame + 11;
             i++) {
          recd_codeword <<= 1;
          recd_codeword |= (f->fdmdv_rx_bits[i] & 0x1);
        }
        codeword1 = golay23_decode(recd_codeword);
        f->total_bit_errors += golay23_count_errors(recd_codeword, codeword1);
        f->total_bits += 23;

        for (i = 0; i < f->bits_per_codec_frame; i++)
          f->rx_payload_bits[i] = f->fdmdv_rx_bits[i];

        for (i = 0; i < 8; i++) {
          f->rx_payload_bits[i] = (codeword1 >> (22 - i)) & 0x1;
        }
        for (i = 8, j = 11; i < 12; i++, j++) {
          f->rx_payload_bits[j] = (codeword1 >> (22 - i)) & 0x1;
        }

        // extract txt msg data bit
        // ------------------------------------------------------------

        data_flag_index = codec2_get_spare_bit_index(f->codec2);
        abit[0] = f->rx_payload_bits[data_flag_index];

        n_ascii =
            varicode_decode(&f->varicode_dec_states, &ascii_out, abit, 1, 1);
        if (n_ascii && (f->freedv_put_next_rx_char != NULL)) {
          (*f->freedv_put_next_rx_char)(f->callback_state, ascii_out);
        }

        // reconstruct missing bit we steal for data bit and decode
        codec2_rebuild_spare_bit(f->codec2, (char *)f->rx_payload_bits);

        rx_status |= FREEDV_RX_BITS;
      } else {
        int test_frame_sync, bit_errors, ntest_bits, k;
        short error_pattern[fdmdv_error_pattern_size(f->fdmdv)];

        for (k = 0; k < 2; k++) {
          /* test frames, so lets sync up to the test frames and count any
           * errors */

          fdmdv_put_test_bits(f->fdmdv, &test_frame_sync, error_pattern,
                              &bit_errors, &ntest_bits,
                              &f->fdmdv_rx_bits[k * bits_per_fdmdv_frame]);

          if (test_frame_sync == 1) {
            f->test_frame_sync_state = 1;
            f->test_frame_count = 0;
          }

          if (f->test_frame_sync_state) {
            if (f->test_frame_count == 0) {
              f->total_bit_errors += bit_errors;
              f->total_bits += ntest_bits;
              if (f->freedv_put_error_pattern != NULL) {
                (*f->freedv_put_error_pattern)(
                    f->error_pattern_callback_state, error_pattern,
                    fdmdv_error_pattern_size(f->fdmdv));
              }
            }
            f->test_frame_count++;
            if (f->test_frame_count == 4) f->test_frame_count = 0;
          }
        }
      } /* if (test_frames == 0) .... */
    }

    /* note this freewheels if reliable sync disappears on bad channels */

    if (f->evenframe)
      f->evenframe = 0;
    else
      f->evenframe = 1;

  } /* if (sync) .... */

  return rx_status;
}
