/* -*- c++ -*- */
/* 
 * Copyright (c) 2021 Satlab A/S <satlab@satlab.com>
 * 
 * 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 <endian.h>
#include <gnuradio/io_signature.h>
#include "srs3_deframe_impl.h"

extern "C" {
  #include "fec/crc32c.h"
  #include "fec/randomizer.h"
  #include "fec/rs.h"
  #include "fec/viterbi.h"
  #include "spaceframe.h"
}

namespace gr {
  namespace satlab {

    srs3_deframe::sptr
    srs3_deframe::make(unsigned int size, uint16_t id)
    {
      return gnuradio::get_initial_sptr
        (new srs3_deframe_impl(size, id));
    }

    void
    srs3_deframe_impl::deframe(pmt::pmt_t pdu)
    {
        uint8_t frame_data[SPACEFRAME_LENGTH_MAX];
        size_t frame_length = 0;
        uint16_t header;
        enum spaceframe_type frame_type;

        /* Copy frame data to output */
        pmt::pmt_t vector = pmt::cdr(pdu);
        frame_length = pmt::blob_length(vector);
        memcpy(frame_data, pmt::blob_data(vector), frame_length);

        /* Read frame type and length */
        memcpy(&header, frame_data, sizeof(header));
        header = be16toh(header);
        frame_type = spaceframe_header_type(header);
        frame_length = spaceframe_header_length(header);

        /* Check ID match if enabled */
        if (d_id > 0) {
            uint16_t id_frame;
            memcpy(&id_frame, &frame_data[sizeof(header)], sizeof(id_frame));
            id_frame = be16toh(id_frame);
            if (id_frame != d_id)
                return;

            /* Strip header and ID */
            memmove(frame_data, &frame_data[sizeof(header) + sizeof(d_id)], frame_length);
        } else {
            /* Strip header */
            memmove(frame_data, &frame_data[sizeof(header)], frame_length);
        }

        /* Verify that frametype is known */
        if (frame_type != SPACEFRAME_TYPE_CSP && frame_type != SPACEFRAME_TYPE_IP) {
            std::cout << "Unknown frame type" << std::endl;
            return;
        }

        /* Bump frame count */
        d_frames++;

        /* Create PMT blob */
        pmt::pmt_t vector_out = pmt::make_blob(frame_data, frame_length);
        pmt::pmt_t msg = pmt::cons(pmt::car(pdu), vector_out);

        /* Route frame to correct output */
        if (frame_type == SPACEFRAME_TYPE_CSP) {
            message_port_pub(pmt::mp("csp"), msg);
        } else if (frame_type == SPACEFRAME_TYPE_IP) {
            message_port_pub(pmt::mp("ip"), msg);
        }
    }

    void srs3_deframe_impl::set_size(unsigned int bytes)
    {
        d_size = bytes;
    }

    void srs3_deframe_impl::set_id(uint16_t id)
    {
        d_id = id;
    }

    unsigned int srs3_deframe_impl::get_frames(void)
    {
        return d_frames;
    }

    /*
     * The private constructor
     */
    srs3_deframe_impl::srs3_deframe_impl(unsigned int size, uint16_t id)
      : gr::block("srs3_deframe",
              gr::io_signature::make(0, 0, 0),
              gr::io_signature::make(0, 0, 0)),
        d_size(size),
        d_id(id),
        d_frames(0)
    {
        message_port_register_out(pmt::mp("csp"));
        message_port_register_out(pmt::mp("ip"));

        message_port_register_in(pmt::mp("deframe"));
        set_msg_handler(pmt::mp("deframe"), [this](pmt::pmt_t msg) {
            this->deframe(msg);
        });
    }

    /*
     * Our virtual destructor.
     */
    srs3_deframe_impl::~srs3_deframe_impl()
    {
    }
  } /* namespace satlab */
} /* namespace gr */

