/* -*- c++ -*- */
/*
 * gr-satnogs: SatNOGS GNU Radio Out-Of-Tree Module
 *
 *  Copyright (C) 2016, Libre Space Foundation <http://librespacefoundation.org/>
 *
 *  This program 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 of the License, or
 *  (at your option) any later version.
 *
 *  This program 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 program.  If not, see <http://www.gnu.org/licenses/>.
 */

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

#include <gnuradio/io_signature.h>
#include "upsat_fsk_frame_encoder_impl.h"
#include <satnogs/log.h>
#include <satnogs/utils.h>
#include <satnogs/crc.h>
#include <arpa/inet.h>

namespace gr {
namespace satnogs {

upsat_fsk_frame_encoder::sptr
upsat_fsk_frame_encoder::make(const std::vector<uint8_t> &preamble,
                              const std::vector<uint8_t> &sync_word,
                              bool append_crc, bool whitening,
                              bool manchester, bool msb_first,
                              bool ax25_format,
                              const std::string &ax25_dest_addr,
                              uint8_t ax25_dest_ssid,
                              const std::string &ax25_src_addr,
                              uint8_t ax25_src_ssid,
                              size_t settling_samples)
{
  return gnuradio::get_initial_sptr(
           new upsat_fsk_frame_encoder_impl(preamble, sync_word,
               append_crc,
               whitening, manchester,
               msb_first, ax25_format,
               ax25_src_addr, ax25_src_ssid,
               ax25_dest_addr, ax25_dest_ssid,
               settling_samples));
}

/*
 * The private constructor
 */
upsat_fsk_frame_encoder_impl::upsat_fsk_frame_encoder_impl(
  const std::vector<uint8_t> &preamble,
  const std::vector<uint8_t> &sync_word,
  bool append_crc, bool whitening,
  bool manchester, bool msb_first,
  bool ax25_format,
  const std::string &ax25_dest_addr,
  uint8_t ax25_dest_ssid,
  const std::string &ax25_src_addr,
  uint8_t ax25_src_ssid,
  size_t settling_samples) :
  gr::sync_block("upsat_fsk_frame_encoder",
                 gr::io_signature::make(0, 0, 0),
                 gr::io_signature::make(1, 1, sizeof(float))),
  d_preamble(preamble),
  d_preamble_len(preamble.size()),
  d_sync_word(sync_word),
  d_sync_word_len(sync_word.size()),
  d_append_crc(append_crc),
  d_whitening(whitening),
  d_manchester(manchester),
  d_msb_first(msb_first),
  d_is_ax25(ax25_format),
  d_max_pdu_len(d_preamble_len + d_sync_word_len + sizeof(uint8_t)
                + UPSAT_MAX_FRAME_LEN + sizeof(uint16_t)),
  d_settling_samples(settling_samples),
  d_encoded(0),
  d_pdu_len(0),
  d_scrambler(0x1001, 0x1FF, 17)
{
  /* Simplify the logic of the output samples handling */
  set_output_multiple(8);
  message_port_register_in(pmt::mp("pdu"));
  /*
   * Allocate memory for the maximum possible frame WITH all headers and
   * tail fields
   * +-------------+----------+--------+--------------------------+------+
   * | Preamble    |  SFD     | LENGTH |   PDU                    | CRC  |
   * +-------------+----------+--------+--------------------------+------+
   *   User def.     User def.   1B         1-255 B                  2 B
   */
  d_pdu = new uint8_t[d_max_pdu_len];


  if (d_is_ax25) {
    d_ax25_addr = new uint8_t[AX25_MAX_ADDR_LEN];
    /* One useful bit per byte for the AX.25 buffer */
    d_ax25_pdu = new uint8_t[2 * d_max_pdu_len * 8];
    d_ax25_tmp_buf = new uint8_t[d_max_pdu_len * 2];
    d_ax25_addr_len = ax25_create_addr_field(d_ax25_addr, ax25_dest_addr,
                      ax25_dest_ssid, ax25_src_addr,
                      ax25_src_ssid);
  }

  d_pdu_encoded = new float[d_max_pdu_len * 8 + d_settling_samples];

  /* Copy the preamble at the start of the pdu */
  memcpy(d_pdu, d_preamble.data(), d_preamble_len);
  memcpy(d_pdu + d_preamble_len, d_sync_word.data(), d_sync_word_len);
}

/*
 * Our virtual destructor.
 */
upsat_fsk_frame_encoder_impl::~upsat_fsk_frame_encoder_impl()
{
  delete [] d_pdu;
  delete [] d_pdu_encoded;
  if (d_is_ax25) {
    delete [] d_ax25_addr;
    delete [] d_ax25_pdu;
    delete [] d_ax25_tmp_buf;
  }
}

inline void
upsat_fsk_frame_encoder_impl::map_msb_first(float *out, size_t nsamples_out)
{
  size_t i;
  register uint8_t b;
  for (i = 0; i < nsamples_out; i += 8) {
    b = d_pdu[d_encoded + i / 8];
    out[i] = ((b >> 7) * 2.0f) - 1.0f;
    out[i + 1] = (((b >> 6) & 0x1) * 2.0f) - 1.0f;
    out[i + 2] = (((b >> 5) & 0x1) * 2.0f) - 1.0f;
    out[i + 3] = (((b >> 4) & 0x1) * 2.0f) - 1.0f;
    out[i + 4] = (((b >> 3) & 0x1) * 2.0f) - 1.0f;
    out[i + 5] = (((b >> 2) & 0x1) * 2.0f) - 1.0f;
    out[i + 6] = (((b >> 1) & 0x1) * 2.0f) - 1.0f;
    out[i + 7] = ((b & 0x1) * 2.0f) - 1.0f;
  }
}

inline void
upsat_fsk_frame_encoder_impl::map_lsb_first(float *out, size_t nsamples_out)
{
  size_t i;
  register uint8_t b;
  for (i = 0; i < nsamples_out; i += 8) {
    b = d_pdu[d_encoded + i / 8];
    out[i + 7] = ((b >> 7) * 2.0f) - 1.0f;
    out[i + 6] = (((b >> 6) & 0x1) * 2.0f) - 1.0f;
    out[i + 5] = (((b >> 5) & 0x1) * 2.0f) - 1.0f;
    out[i + 4] = (((b >> 4) & 0x1) * 2.0f) - 1.0f;
    out[i + 3] = (((b >> 3) & 0x1) * 2.0f) - 1.0f;
    out[i + 2] = (((b >> 2) & 0x1) * 2.0f) - 1.0f;
    out[i + 1] = (((b >> 1) & 0x1) * 2.0f) - 1.0f;
    out[i] = ((b & 0x1) * 2.0f) - 1.0f;
  }
}

inline void
upsat_fsk_frame_encoder_impl::add_sob(uint64_t item)
{
  static const pmt::pmt_t sob_key = pmt::string_to_symbol("tx_sob");
  static const pmt::pmt_t value = pmt::PMT_T;
  static const pmt::pmt_t srcid = pmt::string_to_symbol(alias());
  add_item_tag(0, item, sob_key, value, srcid);
}

inline void
upsat_fsk_frame_encoder_impl::add_eob(uint64_t item)
{
  static const pmt::pmt_t eob_key = pmt::string_to_symbol("tx_eob");
  static const pmt::pmt_t value = pmt::PMT_T;
  static const pmt::pmt_t srcid = pmt::string_to_symbol(alias());
  add_item_tag(0, item, eob_key, value, srcid);
}

inline int
upsat_fsk_frame_encoder_impl::raw_frame_handling(float *out,
    int noutput_items)
{
  uint16_t crc;
  size_t min_available;
  pmt::pmt_t pdu;

  /*
   * If the whole previous frame has been successfully sent, block waiting
   * for a new one
   */
  if (d_encoded == 0) {
    pdu = delete_head_blocking(pmt::mp("pdu"));
    d_pdu_len = pmt::blob_length(pdu);

    if (d_pdu_len > UPSAT_MAX_FRAME_LEN) {
      LOG_ERROR("PDU is greater than the supported. Dropping the PDU");
      return 0;
    }
    /*
     * Set the frame length at the corresponding field.
     * NOTE: The frame length is calculated taking consideration only
     * the address field (if exists) and the payload. Length and CRC fields
     * are NOT included.
     */
    d_pdu[d_preamble_len + d_sync_word_len] = (uint8_t) d_pdu_len;

    /* Append the variable length PDU */
    memcpy(d_pdu + d_preamble_len + d_sync_word_len + 1,
           pmt::blob_data(pdu), d_pdu_len);

    /* If it is necessary calculate and append the CRC */
    if (d_append_crc) {
      crc = crc::crc16_ccitt(d_pdu + d_preamble_len + d_sync_word_len,
                             d_pdu_len + 1);
      /* CRC must be transmitted MSB first */
      crc = htons(crc);
      memcpy(d_pdu + d_preamble_len + d_sync_word_len + 1 + d_pdu_len,
             &crc, sizeof(uint16_t));
      d_pdu_len += sizeof(uint16_t);
    }

    /*
     * Whitening is performed on all bytes except preamble and SYNC fields
     */
    if (d_whitening) {
      d_scrambler.reset();
      d_scrambler.scramble(d_pdu + d_preamble_len + d_sync_word_len,
                           d_pdu + d_preamble_len + d_sync_word_len,
                           d_pdu_len + 1);
    }

    d_pdu_len += d_preamble_len + d_sync_word_len + 1;

    /*
     * NRZ Encode the whole frame
     */
    if (d_msb_first) {
      map_msb_first(d_pdu_encoded, d_pdu_len * 8);
    }
    else {
      map_lsb_first(d_pdu_encoded, d_pdu_len * 8);
    }

    /* Reset the settling trailing samples */
    memset(d_pdu_encoded + d_pdu_len * 8, 0,
           d_settling_samples * sizeof(float));

    /* The new frame now has a bigger size of course*/
    d_pdu_len += d_settling_samples / 8;

    /* Start of burst */
    add_sob(nitems_written(0));
  }

  noutput_items = std::max(0, noutput_items);
  min_available = std::min((size_t) noutput_items,
                           (d_pdu_len - d_encoded) * 8);

  memcpy(out, d_pdu_encoded + d_encoded * 8,
         min_available * sizeof(float));
  d_encoded += min_available / 8;

  /* End of the frame reached */
  if (d_encoded == d_pdu_len) {
    add_eob(nitems_written(0) + min_available - 1);
    d_encoded = 0;
  }

  return min_available;
}

inline int
upsat_fsk_frame_encoder_impl::ax25_frame_handling(float *out,
    int noutput_items)
{
  uint16_t crc;
  size_t min_available;
  pmt::pmt_t pdu;
  size_t len;
  size_t encoded_len;
  ax25_encode_status_t status;
  size_t extra_bits;
  size_t i;
  uint8_t len_field;

  /*
   * If the whole previous frame has been successfully sent, block waiting
   * for a new one
   */
  if (d_encoded == 0) {
    /* Reset the buffer */
    memset(d_ax25_pdu, 0, 2 * d_max_pdu_len * 8);
    memset(d_pdu_encoded, 0, d_max_pdu_len * 8 * sizeof(float));
    pdu = delete_head_blocking(pmt::mp("pdu"));
    d_pdu_len = pmt::blob_length(pdu);

    if (d_pdu_len > UPSAT_MAX_FRAME_LEN) {
      LOG_ERROR("PDU is greater than the supported. Dropping the PDU");
      return 0;
    }

    len = ax25_prepare_frame(d_ax25_tmp_buf, (uint8_t *) pmt::blob_data(pdu),
                             d_pdu_len, AX25_UI_FRAME, d_ax25_addr,
                             d_ax25_addr_len, 0x03, 1, 1, 1);

    status = ax25_bit_stuffing(d_ax25_pdu, &encoded_len,
                               d_ax25_tmp_buf, len, 1, 1);
    if (status != AX25_ENC_OK) {
      LOG_WARN("Failed to properly encode into AX.25 frame");
      return 0;
    }

    /* Due to bit stuffing the resulting bit number may not be
     * a multiple of 8. Thus we append proper number of zeros
     * after the SYNC flag. So neither the AX.25 or the FSK
     * framing are affected
     */
    extra_bits = 8 - (encoded_len % 8);
    encoded_len += extra_bits;

    /*
     * Set the frame length at the corresponding field.
     * NOTE: The frame length is calculated taking consideration only
     * the address field (if exists) and the payload. Length and CRC fields
     * are NOT included.
     */
    len_field = (uint8_t)(encoded_len / 8);

    /* Apply whitening */
    if (d_whitening) {
      d_scrambler.reset();
      d_scrambler.scramble(&len_field, &len_field, 1);
      d_scrambler.scramble_one_bit_per_byte(d_ax25_pdu, d_ax25_pdu,
                                            encoded_len);
    }

    d_pdu[d_preamble_len + d_sync_word_len] = len_field;

    /* If it is necessary calculate and append the CRC */
    if (d_append_crc) {
      LOG_WARN("AX.25 has its own CRC-16 field. Skipping...");
    }

    d_pdu_len = d_preamble_len + d_sync_word_len + 1 + encoded_len / 8;

    /* NRZ encoding the FSK preamble with the standard method */
    map_msb_first(d_pdu_encoded, (d_preamble_len + d_sync_word_len + 1) * 8);

    /* NRZ encode the AX.25 part of the FSK frame */
    for (i = 0; i < encoded_len - extra_bits; i++) {
      d_pdu_encoded[i + (d_preamble_len + d_sync_word_len + 1) * 8]
        = (d_ax25_pdu[i] * 2.0f) - 1.0f;
    }

    /* Reset the settling trailing samples */
    memset(&d_pdu_encoded[d_pdu_len * 8], 0,
           d_settling_samples * sizeof(float));

    /* The new frame now has a bigger size of course*/
    d_pdu_len += d_settling_samples / 8;

    /* Start of burst */
    add_sob(nitems_written(0));
  }

  noutput_items = std::max(0, noutput_items);
  min_available = std::min((size_t) noutput_items,
                           (d_pdu_len - d_encoded) * 8);

  memcpy(out, d_pdu_encoded + d_encoded * 8,
         min_available * sizeof(float));
  d_encoded += min_available / 8;

  /* End of the frame reached */
  if (d_encoded == d_pdu_len) {
    add_eob(nitems_written(0) + min_available - 1);
    d_encoded = 0;
  }

  return min_available;
}

int
upsat_fsk_frame_encoder_impl::work(int noutput_items,
                                   gr_vector_const_void_star &input_items,
                                   gr_vector_void_star &output_items)
{
  float *out = (float *) output_items[0];

  if (d_is_ax25) {
    return ax25_frame_handling(out, noutput_items);
  }
  else {
    return raw_frame_handling(out, noutput_items);
  }
}

} /* namespace satnogs */
} /* namespace gr */