#include "lwip/api.h"
#include "lwip/priv/sockets_priv.h"
#include "lwip/tcpip.h"

#include "sys/socket.h"

#include "FreeRTOS.h"
#include "task.h"

#include "xethernet.h"

#include "hardware/interfaces.h"
#include <unistd.h>

// Those three are a hack, but a quite performant one
struct lwip_sock *get_socket(int fd); // only works with a patched lwip
extern QueueHandle_t uart0_receive_queue;
extern QueueHandle_t uart1_receive_queue;

void udp_echo_thread(void *_) {
  vTaskDelay(5000 * portTICK_RATE_MS);

  int sock = socket(AF_INET, SOCK_DGRAM, 0);
  if (sock == -1) {
    xil_printf("no sock\n");
  }

  struct sockaddr_in our_addr;
  memset(&our_addr, 0, sizeof(our_addr));

  our_addr.sin_family = AF_INET;
  our_addr.sin_port = htons(8100);

  int ret = inet_aton("192.168.1.10", &our_addr.sin_addr);
  if (ret == 0) {
    xil_printf("no addr");
  }

  ret = bind(sock, (struct sockaddr *)&our_addr, sizeof(our_addr));
  if (ret == -1) {
    xil_printf("no bind");
  }

  struct sockaddr_in peer_addr;
  memset(&peer_addr, 0, sizeof(peer_addr));

  peer_addr.sin_family = AF_INET;
  peer_addr.sin_port = htons(8100);

  ret = inet_aton("192.168.1.5", &peer_addr.sin_addr);
  if (ret == 0) {
    xil_printf("no addr");
  }

  int uart0_fd = hw_device_open("uart0", 5);
  int uart1_fd = hw_device_open("uart1", 5);

  // char buffer[] = {'1','2','3','4'};
  // ret = sendto(sock, buffer, sizeof(buffer), 0, (struct sockaddr *)
  // &peer_addr, sizeof(peer_addr)); if (ret <= 0) {
  //   xil_printf("no send");
  // }

  uint8_t rec_buffer[1024];

  QueueSetHandle_t listening_set = xQueueCreateSet(100);
  if (listening_set == NULL) {
    xil_printf("no set");
  }

  struct lwip_sock *raw_socket = get_socket(sock);

  sys_mbox_t queue_id = raw_socket->conn->recvmbox;

  xQueueAddToSet(queue_id, listening_set);
  xQueueAddToSet(uart0_receive_queue, listening_set);
  xQueueAddToSet(uart1_receive_queue, listening_set);

  while (1) {
    QueueSetMemberHandle_t readable =
        xQueueSelectFromSet(listening_set, portMAX_DELAY);

    if (readable == queue_id) {
      socklen_t peer_len = sizeof(peer_addr);
      ret = lwip_recvfrom(sock, rec_buffer, sizeof(rec_buffer), 0,
                          (struct sockaddr *)&peer_addr, &peer_len);
      if (peer_len > sizeof(peer_addr)) {
        xil_printf("invalid peer");
        continue;
      }
      if (ret < 0) {
        continue;
      }
      uint16_t port = ntohs(peer_addr.sin_port);
      switch (port) {
      case 8100:
        // xil_printf("udp rec 8100 len: %i\n", ret);
        write(uart0_fd, rec_buffer, ret);
        break;
      case 8101:
        xil_printf("udp rec 8101 len: %i\n", ret);
        write(uart1_fd, rec_buffer, ret);
        break;
      default:
        xil_printf("invalid port %i\n", port);
        break;
      }
    } else if (readable == uart1_receive_queue) {
      ret = read(uart1_fd, rec_buffer, sizeof(rec_buffer));
      // Do not send empty packets
      if (ret <= 0) {
        continue;
      }
      xil_printf("uart got %i\n", ret);
      peer_addr.sin_port = htons(8101);

      // TODO sending 1 byte gives invalid UDP checksum
      lwip_sendto(sock, rec_buffer, ret, 0, (struct sockaddr *)&peer_addr,
                  sizeof(peer_addr));
    } else if (readable == uart0_receive_queue) {
      ret = read(uart0_fd, rec_buffer, sizeof(rec_buffer));
      // xil_printf("uart got %i\n", ret);

      // Do not send empty packets
      if (ret <= 0) {
        continue;
      }
      peer_addr.sin_port = htons(8100);

      // TODO sending 1 byte gives invalid UDP checksum
      lwip_sendto(sock, rec_buffer, ret, 0, (struct sockaddr *)&peer_addr,
                  sizeof(peer_addr));
    } else {
      xil_printf("invalid handle");
    }
  }

  xQueueRemoveFromSet(queue_id, listening_set);

  while (1) {
    ret = recv(sock, rec_buffer, sizeof(rec_buffer), 0);
    xil_printf("received %i\n", ret);
    vTaskDelay(100 * portTICK_RATE_MS);
  }
}

int testEth() {
  tcpip_init(NULL, NULL);
  xethernet_init();
  sys_thread_new("echo", udp_echo_thread, 0, 1024, DEFAULT_THREAD_PRIO);
  vTaskStartScheduler();
}