/* -*- 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_enframe_impl.h"

extern "C" {
  #include "spaceframe.h"
}

namespace gr {
  namespace satlab {

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

    void
    srs3_enframe_impl::enframe_csp(pmt::pmt_t pdu)
    {
        enframe(pdu, SPACEFRAME_TYPE_CSP);
    }

    void
    srs3_enframe_impl::enframe_ip(pmt::pmt_t pdu)
    {
        enframe(pdu, SPACEFRAME_TYPE_IP);
    }

    void
    srs3_enframe_impl::enframe(pmt::pmt_t pdu, enum spaceframe_type type)
    {
        uint16_t header;
        uint16_t id_frame;

        uint8_t frame_data[SPACEFRAME_LENGTH_MAX];
        size_t frame_length = 0, size = d_size;

        pmt::pmt_t vector = pmt::cdr(pdu);
        frame_length = pmt::blob_length(vector);

        /* Discard frame is larger than current payload size */
        if (frame_length > d_size) {
            printf("Discarding PDU of size %zu > %u\n",
                    frame_length, d_size);
            return;
        }

        /* Bump frame count */
        d_frames++;

        /* Copy frame data to output */
        memcpy(frame_data, pmt::blob_data(vector), frame_length);

        /* Prepend header */
        memmove(&frame_data[sizeof(header)], &frame_data[0], frame_length);
        header = spaceframe_header_make(type, frame_length);
        header = htobe16(header);
        memcpy(&frame_data[0], &header, sizeof(header));
        frame_length += sizeof(header);
        size += sizeof(header);

        /* Add ID if enabled */
        if (d_id > 0) {
            memmove(&frame_data[sizeof(header) + sizeof(id_frame)],
                    &frame_data[sizeof(header)],
                    frame_length - sizeof(header));
            id_frame = htobe16(d_id);
            memcpy(&frame_data[sizeof(header)], &id_frame, sizeof(id_frame));
            frame_length += sizeof(id_frame);
            size += sizeof(id_frame);
        }

        /* Zero-pad frame to RS size */
        memset(&frame_data[frame_length], 0, size - frame_length);
        frame_length = size;

        pmt::pmt_t vector_out = pmt::make_blob(frame_data, frame_length);
        pmt::pmt_t msg = pmt::cons(pmt::car(pdu), vector_out);

        message_port_pub(pmt::mp("output"), msg);
    }

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

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

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

    /*
     * The private constructor
     */
    srs3_enframe_impl::srs3_enframe_impl(unsigned int size, uint16_t id)
      : gr::block("srs3_enframe",
              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("output"));

        message_port_register_in(pmt::mp("csp"));
        set_msg_handler(pmt::mp("csp"), [this](pmt::pmt_t msg) {
            this->enframe_csp(msg);
        });
        message_port_register_in(pmt::mp("ip"));
        set_msg_handler(pmt::mp("ip"), [this](pmt::pmt_t msg) {
            this->enframe_ip(msg);
        });
    }

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

