#include <flexptp_options.h>

#include "FreeRTOS.h"
#include "queue.h"

#include "ptp_msg_tx.h"
#include "ptp_defs.h"
#include "ptp_core.h"
#include "ptp_raw_msg_circbuf.h"

#include "settings_interface.h"

static struct {
    struct udp_pcb *pPri_Ev;
    struct udp_pcb *pPri_Gen;

} sPcbLut = { 0 };

static const uint16_t sPortLut[2] = { PTP_PORT_EVENT, PTP_PORT_GENERAL };
static ip4_addr_t sIpLut[2] = { 0 };
static const uint8_t *sEthLut[2] = { PTP_ETHERNET_PRIMARY, PTP_ETHERNET_PEER_DELAY };

void ptp_transmit_init(struct udp_pcb *pPriE, struct udp_pcb *pPriG,)
{
    sPcbLut.pPri_Ev = pPriE;
    sPcbLut.pPri_Gen = pPriG;

    sIpLut[0] = PTP_IGMP_PRIMARY;

}

// release buffer
void ptp_transmit_free(struct pbuf *pPBuf)
{
    pbuf_free(pPBuf);
}

void ptp_transmit_cb_handler(struct pbuf *pPBuf)
{
    RawPtpMessage *pMsg = (RawPtpMessage *) pPBuf->tag;
    pMsg->ts.sec = pPBuf->time_s;
    pMsg->ts.nanosec = pPBuf->time_ns;
    if (pMsg->pTxCb) {
        pMsg->pTxCb(pMsg);
    }
}

bool ptp_transmit_enqueue(const RawPtpMessage * pMsg)
{
    extern PtpCircBuf gRawTxMsgBuf;
    extern QueueHandle_t gTxPacketFIFO;
    RawPtpMessage *pMsgAlloc = ptp_circ_buf_alloc(&gRawTxMsgBuf);
    if (pMsgAlloc) {
        *pMsgAlloc = *pMsg;
        uint8_t idx = ptp_circ_buf_commit(&gRawTxMsgBuf);
        bool hptWoken = false;
        if (xPortIsInsideInterrupt()) {
            xQueueSendFromISR(gTxPacketFIFO, &idx, &hptWoken);
        } else {
            xQueueSend(gTxPacketFIFO, &idx, portMAX_DELAY);
        }
        return true;
    } else {
        MSG("enqueue failed!");
        return false;
    }
}

void ptp_transmit_msg(RawPtpMessage * pMsg)
{
    PtpTransportType tp = ptp_get_transport_type();
    PtpDelayMechanism dm = pMsg->tx_dm;
    PtpMessageClass mc = pMsg->tx_mc;

    // allocate buffer
    struct pbuf *txbuf = NULL;
    txbuf = pbuf_alloc(PBUF_TRANSPORT, pMsg->size, PBUF_RAM);

    // fill buffer
    memcpy(txbuf->payload, pMsg->data, pMsg->size);
    txbuf->ts_writeback_addr[0] = (uint32_t *) & (pMsg->pTs->sec);
    txbuf->ts_writeback_addr[1] = (uint32_t *) & (pMsg->pTs->nanosec);
    txbuf->tag = pMsg;
    txbuf->tx_cb = ptp_transmit_cb_handler;

    if (tp == PTP_TP_IPv4) {
        struct udp_pcb *pPcb = ((struct udp_pcb **)&sPcbLut)[2 * ((int)dm) + (int)mc];
        uint16_t port = sPortLut[(int)mc];
        ip_addr_t ipaddr = sIpLut[(int)dm];
        udp_sendto(pPcb, txbuf, &ipaddr, port);
    }

    pbuf_free(txbuf);           // release buffer
}