#include "FreeRTOS.h"
#include "lwip/sio.h"
#include "queue.h"
#include "task.h"
#include <arpa/inet.h>
#include <cstring>
#include <lwip/ip_addr.h>
#include <lwip/netifapi.h>
#include <lwip/tcpip.h>
#include <netif/slipif.h>
#include <sys/socket.h>

#include <xscugic.h>
#include <xuartps.h>

#undef XUARTPS_IXR_RXOVR
#define XUARTPS_IXR_RXOVR 0x00000020U /**< Rx Overrun error interrupt */
#define XUARTPS_IXR_RTRIG 0x00000001U /**< RX FIFO trigger interrupt. */

#define UART_INT_NR XPAR_XUARTPS_0_INTR

extern "C" {
#include <lwip/apps/tftp_server.h>

void slipif_rxbyte_input(struct netif *netif, u8_t c);

static void *tftp_open(const char *fname, const char *mode, u8_t is_write) {
  LWIP_UNUSED_ARG(mode);
  return (void *)13;
}

static void tftp_close(void *handle) {}

static int tftp_read(void *handle, void *buf, int bytes) {
  memset(buf, 'x', bytes);
  return bytes - 60;
}

static int tftp_write(void *handle, struct pbuf *p) { return 0; }

/* For TFTP client only */
static void tftp_error(void *handle, int err, const char *msg, int size) {}

static const struct tftp_context tftp = {tftp_open, tftp_close, tftp_read,
                                         tftp_write, tftp_error};

                      

void myInitDone(void *arg) { puts("init done"); }

struct netif netif;

QueueHandle_t uartIsrQueue;

extern XScuGic xInterruptController; /* Interrupt controller instance */

/** this is based on XUartPs_InterruptHandler() in xuartps_intr.c*/
void handleUARTInt(void *) {
  u32 IsrStatus;

  /*
   * Read the interrupt ID register to determine which
   * interrupt is active
   */
  IsrStatus = XUartPs_ReadReg(STDIN_BASEADDRESS, XUARTPS_IMR_OFFSET);

  IsrStatus &= XUartPs_ReadReg(STDIN_BASEADDRESS, XUARTPS_ISR_OFFSET);

  // Onlx RX intterupts are enabled
  // We do not care which interrupt actually triggered, just get all bytes
  // available into the stack
  uint8_t RecievedByte;
  BaseType_t xHigherPriorityTaskWoken;
  while (XUartPs_IsReceiveData(STDIN_BASEADDRESS)) {
    RecievedByte = XUartPs_ReadReg(STDIN_BASEADDRESS, XUARTPS_FIFO_OFFSET);
    xQueueSendToBackFromISR(uartIsrQueue, &RecievedByte,
                            &xHigherPriorityTaskWoken);
  }

  /* Clear the interrupt status. */
  XUartPs_WriteReg(STDIN_BASEADDRESS, XUARTPS_ISR_OFFSET, IsrStatus);

  portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

void forwardPackets(void *) {
  uint8_t byte;
  BaseType_t result;
  while (1) {
    result = xQueueReceive(uartIsrQueue, &byte, portMAX_DELAY);
    if (result == pdTRUE) {
      slipif_rxbyte_input(&netif, byte);
    }
  }
}

uint32_t sio_data;

sio_fd_t sio_open(u8_t devnum) { return &sio_data; }

void sio_send(u8_t c, sio_fd_t fd) { XUartPs_SendByte(STDOUT_BASEADDRESS, c); }

} // extern "C"

void testIp() {

  uartIsrQueue = xQueueCreate(512, 1); // TODO check

  tcpip_init(myInitDone, nullptr);

  ip4_addr_t slip_addr = {PP_HTONL(LWIP_MAKEU32(10, 0, 0, 32))},
             slip_mask = {PP_HTONL(LWIP_MAKEU32(255, 255, 255, 0))},
             slip_gw = {PP_HTONL(LWIP_MAKEU32(10, 0, 0, 1))};

  netifapi_netif_add(&netif, &slip_addr, &slip_mask, &slip_gw, NULL,
                     slipif_init, netif_input);

  netifapi_netif_set_default(&netif);
  // should be done by driver, which does not do it, so we do it here
  netifapi_netif_set_link_up(&netif);
  netifapi_netif_set_up(&netif);

  vTaskDelay(pdMS_TO_TICKS(2000));

  /* Install the UART Interrupt handler. */
  BaseType_t xStatus =
      XScuGic_Connect(&xInterruptController, UART_INT_NR,
                      (Xil_ExceptionHandler)handleUARTInt, nullptr);
  configASSERT(xStatus == XST_SUCCESS);
  (void)xStatus; /* Remove compiler warning if configASSERT() is not defined. */

  // Set trigger level to 62 of 64 bytes, giving interrupt some time to react
  XUartPs_WriteReg(STDIN_BASEADDRESS, XUARTPS_RXWM_OFFSET, 62);

  // Setting the rx timeout to n*4 -1 bits
  XUartPs_WriteReg(STDIN_BASEADDRESS, XUARTPS_RXTOUT_OFFSET, 50);

  // enable UART Interrupts
  u32 mask = XUARTPS_IXR_RTRIG | XUARTPS_IXR_RXOVR | XUARTPS_IXR_RXFULL |
             XUARTPS_IXR_TOUT;
  /* Write the mask to the IER Register */
  XUartPs_WriteReg(STDIN_BASEADDRESS, XUARTPS_IER_OFFSET, mask);
  /* Write the inverse of the Mask to the IDR register */
  XUartPs_WriteReg(STDIN_BASEADDRESS, XUARTPS_IDR_OFFSET, (~mask));

  /* Enable the interrupt for the UART1 in the interrupt controller. */
  XScuGic_Enable(&xInterruptController, UART_INT_NR);

  // Start task to forwad packets from ISR to IP Task
  xTaskCreate(forwardPackets, /* The function that implements the task. */
              "slip forward", /* The text name assigned to the task - for debug
                              only as it is not used by the kernel. */
              1024,    /* The size of the stack to allocate to the task. */
              nullptr, /* The parameter passed to the task - not used in this
                          simple case. */
              4,       /* The priority assigned to the task. */
              nullptr);

  tftp_init_server(&tftp);

  puts("socket");

  int serverSocket = socket(AF_INET, SOCK_DGRAM, 0);
  if (serverSocket == -1) {
    puts("socket failed");
    return;
  }

  sockaddr_in serverAddr;

  memset(&serverAddr, 0, sizeof(serverAddr));

  serverAddr.sin_family = AF_INET;
  serverAddr.sin_port = htons(0xcafe);
  serverAddr.sin_addr.s_addr = inet_addr("10.0.0.13");

  uint8_t data[] = {1, 2, 3, 4, 5, 6, 7};

  puts("send");

  sendto(serverSocket, data, sizeof(data), 0, (sockaddr *)&serverAddr,
         sizeof(serverAddr));
  puts("send done");
}