/***********************************************************
 * Filename     : coms.c
 * Title        : NEXBE UART Communications Handler Header
 * Author       : Simon Third
 * Created      : 2021 October 07
 * Company      : NEXBE Ltd.
 *
 * Description  : Implement UART FIFO's and comand processing or redirection to BLE
 *
 * Changes      :
 *              000v000v001 - 07-10-2021, S3
 *                  Initial created based upon nrf uart sample code.
 *
 **********************************************************/


/* INCLUDE FILES ==========================================================*/


#include "coms.h"
#include <logging/log.h>
#include <drivers/uart.h>
#include "darc_svc.h"
#include "main.h"


/* EXTERNAL FUNCTION PROTOTYPES ===========================================*/


/* INTERNAL FUNCTION PROTOTYPES ===========================================*/


/* LOCAL DEFINITIONS ======================================================*/

#define UART_BUF_SIZE           (20)
#define UART_WAIT_FOR_BUF_DELAY K_MSEC(20)
#define UART_WAIT_FOR_RX        (50) 
#define BT_MAX_PACKET_LEN       CONFIG_BT_L2CAP_TX_MTU

#define STACKSIZE 1024
#define PRIORITY 7

//LOG_MODULE_REGISTER(bt_ens, CONFIG_BT_ENS_LOG_LEVEL);
LOG_MODULE_REGISTER(coms, LOG_LEVEL_DBG);

/* LOCAL DECLARATIONS =====================================================*/
static K_SEM_DEFINE(k_sem_init_ok, 0, 1);


static const struct device *uart;
static struct k_work_delayable uart_work;
static coms_cb_t coms_cb;
struct uart_data_t *buf_pointer1;
struct uart_data_t *buf_pointer2;
struct uart_data_t *buf_pointer3;
struct uart_data_t *buf_pointer4;
struct uart_data_t *buf_pointer5;
struct uart_data_t *buf_pointerTX;

struct uart_data_t {
    void *fifo_reserved;
    uint8_t data[UART_BUF_SIZE];
    uint16_t len;
};

static K_FIFO_DEFINE(fifo_uart_tx_data);
static K_FIFO_DEFINE(fifo_uart_rx_data);



/* FUNCTIONS ==============================================================*/

/* on_receive _____________________________________________________________
|
|   @brief  Enable Write attrubite data call back .
|
|   @param   conn -   The connection that is requesting to read
|   @param   attr -   The attribute that's being read
|   @param   buf -    Buffer to place the read result in
|   @param   len -    Length of buffer (avaliable to send data)
|   @param   offset - Offset to start reading from
|
|   @return  Number of bytes coyied to the read packet.
|
|   @effects None
|__________________________________________________________________________*/



static void uart_cb(const struct device *dev, struct uart_event *evt, void *user_data)
{
    ARG_UNUSED(dev);

    static uint8_t *current_buf;
    static size_t aborted_len;
    static bool buf_release;
    struct uart_data_t *buf;
    static uint8_t *aborted_buf;

    switch (evt->type) {
    case UART_TX_DONE:
        if ((evt->data.tx.len == 0) ||
            (!evt->data.tx.buf)) {
            return;
        }

        if (aborted_buf) {
            buf = CONTAINER_OF(aborted_buf, struct uart_data_t,
                       data);
            aborted_buf = NULL;
            aborted_len = 0;
        } else {
            buf = CONTAINER_OF(evt->data.tx.buf, struct uart_data_t,
                       data);
        }

        k_free(buf);

        buf = k_fifo_get(&fifo_uart_tx_data, K_NO_WAIT);
        if (!buf) {
            return;
        }

        if (uart_tx(uart, buf->data, buf->len, SYS_FOREVER_MS)) {
            LOG_WRN("Failed to send data over UART");
        }

        break;

    case UART_RX_RDY:
        buf = CONTAINER_OF(evt->data.rx.buf, struct uart_data_t, data);
        buf->len += evt->data.rx.len;
        buf_release = false;

        
        LOG_INF("Data to Rx FIFO, %u bytes", buf->len);
        if (buf->len == UART_BUF_SIZE) 
        {
            k_fifo_put(&fifo_uart_rx_data, buf);
            // UART will get a new Rx buffer from call to UART_RX_BUF_REQUEST in this case
        } 
        else if ((evt->data.rx.buf[buf->len - 1] == '\n') ||
              (evt->data.rx.buf[buf->len - 1] == '\r'))
        {
            // End of packet found
            k_fifo_put(&fifo_uart_rx_data, buf);
            evt->data.rx.len = 0;
            current_buf = evt->data.rx.buf;
            buf_release = true;
            uart_rx_disable(uart);
        }
        else
            LOG_INF("UART buf not full");

        //if (buf->len > 0)
        //{
        //    k_fifo_put(&fifo_uart_rx_data, buf);
        //    current_buf = evt->data.rx.buf;
        //    buf_release = true;
        //    LOG_INF("Data to Rx FIFO, %u bytes", buf->len);
        //}
        //else
        //    LOG_DBG("UART_RX_RDY no data");

        break;

    case UART_RX_DISABLED:
        buf = k_malloc(sizeof(*buf));
        buf_pointer2 = buf;
        if (buf) {
            buf->len = 0;
        } else {
            LOG_WRN("Not able to allocate UART receive buffer");
            k_work_reschedule(&uart_work, UART_WAIT_FOR_BUF_DELAY);
            return;
        }

        uart_rx_enable(uart, buf->data, sizeof(buf->data), UART_WAIT_FOR_RX);
        LOG_DBG("UART_RX_DISABLED - ENABLED");
        break;

    case UART_RX_BUF_REQUEST:
        buf = k_malloc(sizeof(*buf));
        buf_pointer3 = buf;
        if (buf) {
            buf->len = 0;
            uart_rx_buf_rsp(uart, buf->data, sizeof(buf->data));
            LOG_DBG("New Rx Buf Request");
        } else {
            LOG_WRN("Not able to allocate UART receive buffer");
        }
       // LOG_DBG("UART_RX_BUF_REQUEST");
        break;

    case UART_RX_BUF_RELEASED:
        buf = CONTAINER_OF(evt->data.rx_buf.buf, struct uart_data_t,
                   data);
        if (buf_release && (current_buf != evt->data.rx_buf.buf)) 
        {
            buf_pointer4 = buf;
            k_free(buf);
            LOG_DBG("Free released buf");
            buf_release = false;
            current_buf = NULL;
        }
        //LOG_DBG("UART_RX_BUF_RELEASED");
        break;

    case UART_TX_ABORTED:
            if (!aborted_buf) {
                aborted_buf = (uint8_t *)evt->data.tx.buf;
            }

            aborted_len += evt->data.tx.len;
            buf = CONTAINER_OF(aborted_buf, struct uart_data_t,
                       data);

            uart_tx(uart, &buf->data[aborted_len],
                buf->len - aborted_len, SYS_FOREVER_MS);

        break;

    default:
        break;
    }
}

static void uart_work_handler(struct k_work *item)
{
    struct uart_data_t *buf;

    buf = k_malloc(sizeof(*buf));
   
    if (buf) {
        buf->len = 0;
    } else {
        LOG_WRN("Not able to allocate UART receive buffer");
        k_work_reschedule(&uart_work, UART_WAIT_FOR_BUF_DELAY);
        return;
    }

    uart_rx_enable(uart, buf->data, sizeof(buf->data), UART_WAIT_FOR_RX);
    LOG_DBG("UART Rx enabled with new buf");
}

/* uart_send_data __________________________________________________________
|
|   @brief  Send data to the UART transmitter.  Large data sets are added to 
|           a FIFO if needed.
|
|   @param   data -   pointer to data buffer
|   @param   len -    Length of data buffer
|
|   @return  error number or 0.
|
|   @effects None
|__________________________________________________________________________*/
int uart_send_data(const uint8_t *const data, uint16_t len)
{
    int err = 0;

    for (uint16_t pos = 0; pos != len;) 
    {
        struct uart_data_t *tx = k_malloc(sizeof(*tx));

        buf_pointerTX = tx;
        if (!tx) 
        {
            LOG_WRN("Not able to allocate UART send data buffer");
            return -ENOMEM;
        }

        size_t tx_data_size = sizeof(tx->data);

        if ((len - pos) > tx_data_size) 
        {
            tx->len = tx_data_size;
        } else
        {
            tx->len = (len - pos);
        }

        memcpy(tx->data, &data[pos], tx->len);
        pos += tx->len;
        err = uart_tx(uart, tx->data, tx->len, SYS_FOREVER_MS);
        if (err) 
        {
            k_fifo_put(&fifo_uart_tx_data, tx);
            LOG_INF("Data added to  Tx FIFO, %u bytes", tx->len);
        }
        else
        {
            LOG_INF("Data added to UART Tx, %u bytes", tx->len);
        }
    }
    return 0;
}

int uart_init(coms_cb_t *callbacks)
{
    int err;
    struct uart_data_t *rx;

    uart = device_get_binding(DT_LABEL(DT_NODELABEL(uart1)));
    if (!uart) {
        return -ENXIO;
    }

    rx = k_malloc(sizeof(*rx));
    buf_pointer1 = rx;
    if (rx) {
        rx->len = 0;
    } else {
        return -ENOMEM;
    }

    k_work_init_delayable(&uart_work, uart_work_handler);

    err = uart_callback_set(uart, uart_cb, NULL);
    if (err) {
        return err;
    }

    if (callbacks) 
    {
        coms_cb.received = callbacks->received;
    }

    k_sem_give(&k_sem_init_ok);

    return uart_rx_enable(uart, rx->data, sizeof(rx->data), 50);
}


void ble_send_thread(void)
{
    uint8_t u8a_bt_tx_buf[BT_MAX_PACKET_LEN];
    uint16_t u16_bt_tx_index;
    uint16_t u16_bt_mtu_size;
    struct uart_data_t *p_buf;

    /* Don't go any further until BLE is initialized */
    k_sem_take(&k_sem_init_ok, K_FOREVER);

    for (;;) 
    {
        /* Wait indefinitely for data to be received from the M4 */
        if (k_fifo_is_empty(&fifo_uart_rx_data))
        {
            // Wait for data
            k_sleep(K_MSEC(10));
            u16_bt_tx_index = 0;
        }
        else
        {
            // check for updated bluetooth mtu size
            u16_bt_mtu_size = fu16_bt_mtu_size();
            do
            {
                p_buf = k_fifo_get(&fifo_uart_rx_data, K_FOREVER);
                LOG_DBG("p_buf= %u", p_buf);
                memcpy(&u8a_bt_tx_buf[u16_bt_tx_index], p_buf->data, p_buf->len);
                u16_bt_tx_index += p_buf->len;
                buf_pointer5 = p_buf;
                k_free(p_buf);
                LOG_INF("bt_tx_index = %u", u16_bt_tx_index);
            
            } while (((u16_bt_tx_index + UART_BUF_SIZE) < u16_bt_mtu_size) && !k_fifo_is_empty(&fifo_uart_rx_data));

            // send data to user
            coms_cb.received(u8a_bt_tx_buf, u16_bt_tx_index);
            u16_bt_tx_index = 0;
            
            //k_sleep(K_MSEC(5));
        }

        //while ((u16_bt_tx_index < BT_MAX_PACKET_LEN) && !k_fifo_is_empty(&fifo_uart_rx_data))
  //      {
  //          if ((u16_bt_tx_index + UART_BUF_SIZE) < u16_bt_mtu_size)
  //          {
  //              p_buf = k_fifo_get(&fifo_uart_rx_data, K_FOREVER);
  //              LOG_DBG("p_buf= %u", p_buf);
  //              memcpy(&u8a_bt_tx_buf[u16_bt_tx_index], p_buf->data, p_buf->len);
  //              u16_bt_tx_index += p_buf->len;
  //              k_free(p_buf);
  //              LOG_INF("bt_tx_index = %u", u16_bt_tx_index);
  //          }
  //          else 
  //              break;
  //      }
  //      if (u16_bt_tx_index)
  //      {       
  //          coms_cb.received(u8a_bt_tx_buf, u16_bt_tx_index);
  //          u16_bt_tx_index = 0;
  //      }
        //else
        //{
        //    k_sleep(K_MSEC(15));
        //}

    } //for(;;)
}

K_THREAD_DEFINE(ble_send_thread_id, STACKSIZE, ble_send_thread, NULL, NULL,
        NULL, PRIORITY, 0, 0);

/*****END OF FILE****/