#include <getopt.h>
#include "common/config/config_userapi.h"
#include "SIMULATION/TOOLS/sim.h"
#include "PHY/CODING/nrSmallBlock/nr_small_block_defs.h"
#include "coding_unitary_defs.h"

//#define DEBUG_SMALLBLOCKTEST

configmodule_interface_t *uniqCfg = NULL;
int main(int argc, char *argv[])
{
  time_stats_t timeEncoder, timeDecoder;
  cpu_meas_enabled = 1;
  reset_meas(&timeEncoder);
  reset_meas(&timeDecoder);
  randominit(0);

  int arguments, iterations = 1000, messageLength = 11;
  // int matlabDebug = 0;
  uint32_t testInput, encoderOutput, codingDifference, nBitError = 0, blockErrorState = 0, blockErrorCumulative = 0,
                                                       bitErrorCumulative = 0;
  uint16_t estimatedOutput;
  double SNRstart = -20.0, SNRstop = 5.0, SNRinc = 0.5; // dB
  double SNR, SNR_lin, sigma;
  double modulatedInput[NR_SMALL_BLOCK_CODED_BITS], channelOutput[NR_SMALL_BLOCK_CODED_BITS];
  // int16_t channelOutput_int16[NR_SMALL_BLOCK_CODED_BITS];
  int8_t channelOutput_int8[NR_SMALL_BLOCK_CODED_BITS];
  unsigned char qbits = 8;

  if ((uniqCfg = load_configmodule(argc, argv, CONFIG_ENABLECMDLINEONLY)) == 0) {
    exit_fun("[SMALLBLOCKTEST] Error, configuration module init failed\n");
  }
  logInit();

  while ((arguments = getopt(argc, argv, "--:O:s:d:f:l:i:mhg")) != -1) {

    /* ignore long options starting with '--', option '-O' and their arguments that are handled by configmodule */
    /* with this opstring getopt returns 1 for non-option arguments, refer to 'man 3 getopt' */
    if (arguments == 1 || arguments == '-' || arguments == 'O')
      continue;

    printf("handling optarg %c\n",arguments);
    switch (arguments) {
      case 's':
        SNRstart = atof(optarg);
        break;

      case 'd':
        SNRinc = atof(optarg);
        break;

      case 'f':
        SNRstop = atof(optarg);
        break;

      case 'l':
        messageLength = atoi(optarg);
        break;

      case 'i':
        iterations = atoi(optarg);
        break;

        /*case 'm':
          matlabDebug = 1;
          //#define DEBUG_POLAR_MATLAB
          break;*/

      case 'g':
        iterations = 1;
        SNRstart = -6.0;
        SNRstop = -6.0;
        messageLength = 11;
        break;

      case 'h':
        // printf("./smallblocktest -s SNRstart -d SNRinc -f SNRstop -l messageLength -i iterations -m Matlab Debug\n");
        printf("./smallblocktest -s SNRstart -d SNRinc -f SNRstop -l messageLength -i iterations\n");
        exit(-1);

      default:
        perror("[smallblocktest.c] Problem at argument parsing with getopt");
        exit(-1);
    }
  }

  uint16_t mask = 0x07ff >> (11 - messageLength);
  for (SNR = SNRstart; SNR <= SNRstop; SNR += SNRinc) {
    printf("SNR %f\n", SNR);
    SNR_lin = pow(10, SNR / 10.0);
    sigma = 1.0 / sqrt(SNR_lin);

    for (int itr = 1; itr <= iterations; itr++) {
      // Generate random test input of length "messageLength"
      testInput = 0;
      for (int i = 1; i < messageLength; i++) {
        testInput |= (((uint32_t)(rand() % 2)) & 1);
        testInput <<= 1;
      }
      testInput |= (((uint32_t)(rand() % 2)) & 1);
      // Encoding
      start_meas(&timeEncoder);
      encoderOutput = encodeSmallBlock(testInput, messageLength);
      stop_meas(&timeEncoder);

      for (int i = 0; i < NR_SMALL_BLOCK_CODED_BITS; i++) {
        // BPSK modulation
        if ((encoderOutput >> i) & 1) {
          modulatedInput[i] = -1;
        } else {
          modulatedInput[i] = 1;
        }

        // AWGN
        channelOutput[i] = modulatedInput[i] + (gaussdouble(0.0, 1.0) * (1 / sqrt(SNR_lin)));

        // Quantization
        channelOutput_int8[i] = quantize(sigma / 16.0, channelOutput[i], qbits);
      }

      // Decoding
      start_meas(&timeDecoder);
      estimatedOutput = decodeSmallBlock(channelOutput_int8, (uint8_t)messageLength);
      stop_meas(&timeDecoder);

#ifdef DEBUG_SMALLBLOCKTEST
      printf("[smallblocktest] Input = 0x%x, Output = 0x%x, DecoderOutput = 0x%x\n", testInput, encoderOutput, estimatedOutput);
      for (int i = 0; i < 32; i++)
        printf("[smallblocktest] Input[%d] = %d, Output[%d] = %d, codingDifference[%d]=%d, Mask[%d] = %d\n",
               i,
               (testInput >> i) & 1,
               i,
               (estimatedOutput >> i) & 1,
               i,
               (codingDifference >> i) & 1,
               i,
               (mask >> i) & 1);
#endif

      // Error Calculation
      estimatedOutput &= mask;
      codingDifference =
          ((uint32_t)estimatedOutput) ^ testInput; // Count the # of 1's in codingDifference by Brian Kernighan’s algorithm.

      for (nBitError = 0; codingDifference; nBitError++)
        codingDifference &= codingDifference - 1;

      blockErrorState = (nBitError > 0) ? 1 : 0;

      blockErrorCumulative += blockErrorState;
      bitErrorCumulative += nBitError;

      nBitError = 0;
      blockErrorState = 0;
    }

    // Error statistics for the SNR; iteration times are in nanoseconds and microseconds, respectively.
    printf("[smallblocktest] SNR=%+7.3f, BER=%9.6f, BLER=%9.6f, t_Encoder=%9.3fns, t_Decoder=%7.3fus\n",
           SNR,
           ((double)bitErrorCumulative / (iterations * messageLength)),
           ((double)blockErrorCumulative / iterations),
           ((double)timeEncoder.diff / timeEncoder.trials) / (get_cpu_freq_GHz()),
           ((double)timeDecoder.diff / timeDecoder.trials) / (get_cpu_freq_GHz() * 1000.0));

    blockErrorCumulative = 0;
    bitErrorCumulative = 0;
  }

  print_meas(&timeEncoder, "smallblock_encoder", NULL, NULL);
  print_meas(&timeDecoder, "smallblock_decoder", NULL, NULL);

  return (0);
}