/* BSD Socket API Example

   This example code is in the Public Domain (or CC0 licensed, at your option.)

   Unless required by applicable law or agreed to in writing, this
   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.
*/

#include "platform.h"
#if PL_CONFIG_USE_UDP_SERVER
#include "udp_server.h"
#include <string.h>
#include <sys/param.h>
#include <stdlib.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "esp_netif.h"

#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include <lwip/netdb.h>

#include "Shell.h"
#include "McuUtility.h"
#include "McuLog.h"

#define CONFIG_EXAMPLE_IPV4
#define TAG "UDP_SERVER"

static TaskHandle_t taskHandle = NULL; /* udp server task handle */

static int SendToSocket(int sock, const char *msg, const struct sockaddr *to, socklen_t tolen) {
  return sendto(sock, msg, McuUtility_strlen((char*)msg), 0, to, tolen);
}

void HandleIncomingUdpMessage(const char *rxMsg, int sock, struct sockaddr *source_addr_p, socklen_t source_addr_len) {
	unsigned char response[128];
	int err;
	unsigned char espMsg[McuShell_DEFAULT_SHELL_BUFFER_SIZE] = "";
	const char espMsgPrefix[] = "@esp:";
	const char espMsgPostfix[] = "!";

	ESP_LOGI(TAG, "handling incoming message %s", rxMsg);
	McuUtility_strcpy(response, sizeof(response), (unsigned char*)"OK"); /* default response */
	/* check framing */
	if (McuUtility_strncmp(rxMsg, espMsgPrefix, sizeof(espMsgPrefix)-1)==0) { /* check prefix */
		size_t strLen = McuUtility_strlen(rxMsg);
		if (rxMsg[strLen-1]=='!') {
			/* send to ESP32 shell */
			for(int i = 0; i < strLen-McuUtility_strlen(espMsgPrefix)-McuUtility_strlen(espMsgPostfix); i++){
				espMsg[i] = rxMsg[i + McuUtility_strlen(espMsgPrefix)];
			}
			SHELL_SendToESPAndGetResponse(espMsg, response, sizeof(response));
		} else {
			McuUtility_strcpy(response, sizeof(response), (unsigned char*)"!missing!");
		}
	}
	/* send back response */
	ESP_LOGI(TAG, "Sending back response");
	err = SendToSocket(sock, (const char*)response, source_addr_p, source_addr_len);
	if (err < 0) {
		ESP_LOGE(TAG, "Error occurred during sending: errno %d", errno);
	}
}





static void udp_server_task(void *pvParameters) {
  char rx_buffer[128];
  char addr_str[128];
  int addr_family;
  int ip_protocol;

  vTaskSuspend(NULL); /* UDP_Server_Start() will wake me up */
  for(;;) {
#ifdef CONFIG_EXAMPLE_IPV4
    struct sockaddr_in dest_addr;

    dest_addr.sin_addr.s_addr = htonl(INADDR_ANY); /** 0.0.0.0 */
    dest_addr.sin_family = AF_INET;
    dest_addr.sin_port = htons(UDP_SERVER_PORT);
    addr_family = AF_INET;
    ip_protocol = IPPROTO_IP;
    inet_ntoa_r(dest_addr.sin_addr, addr_str, sizeof(addr_str) - 1);
#else // IPV6
    struct sockaddr_in6 dest_addr;
    bzero(&dest_addr.sin6_addr.un, sizeof(dest_addr.sin6_addr.un));
    dest_addr.sin6_family = AF_INET6;
    dest_addr.sin6_port = htons(UDP_SERVER_PORT);PO
    addr_family = AF_INET6;
    ip_protocol = IPPROTO_IPV6;
    inet6_ntoa_r(dest_addr.sin6_addr, addr_str, sizeof(addr_str) - 1);
#endif

    int sock = socket(addr_family, SOCK_DGRAM, ip_protocol);
    if (sock < 0) {
      McuLog_error("Unable to create socket: errno %d", errno);
      break;
    }
    McuLog_info("Socket created");

    int err = bind(sock, (struct sockaddr *)&dest_addr, sizeof(dest_addr));
    if (err < 0) {
      McuLog_error("Socket unable to bind: errno %d", errno);
    }
    McuLog_info("Socket bound, port %d", UDP_SERVER_PORT);
    while (1) {
      McuLog_info("Waiting for data on port %d", UDP_SERVER_PORT);
      struct sockaddr_in6 source_addr; /* Large enough for both IPv4 or IPv6 */
      socklen_t socklen = sizeof(source_addr);

      /* receive data (blocking): */
      int len = recvfrom(sock, rx_buffer, sizeof(rx_buffer)-1, 0, (struct sockaddr *)&source_addr, &socklen);

      /* Error occurred during receiving */
      if (len < 0) {
        McuLog_error("recvfrom failed: errno %d", errno);
        break;
      } else { /* Data received */
        /* Get the sender's ip address as string */
        if (source_addr.sin6_family == PF_INET) {
          inet_ntoa_r(((struct sockaddr_in *)&source_addr)->sin_addr.s_addr, addr_str, sizeof(addr_str) - 1);
        } else if (source_addr.sin6_family == PF_INET6) {
          inet6_ntoa_r(source_addr.sin6_addr, addr_str, sizeof(addr_str) - 1);
        }
        rx_buffer[len] = '\0'; /* Null-terminate whatever we received and treat like a string... */
        McuLog_info("Received %d bytes from %s:\n%s", len, addr_str, rx_buffer);

        /* \TODO Need to handle messages and send them to the robot */
        HandleIncomingUdpMessage(rx_buffer, sock, (struct sockaddr *)&source_addr, socklen);
        /* send back response */
//        unsigned char test_response[128];
//        int err;
//
//        McuLog_info("Sending back response");
//        McuUtility_strcpy(test_response, sizeof(test_response), (unsigned char*)"OK"); /* default response */
//        if (McuUtility_strncmp(rx_buffer, "test", sizeof("test")-1)==0) { /* hard-coded command */
//          McuUtility_strcpy(test_response, sizeof(test_response), (unsigned char*)"test_response");
//        }
//        err = SendToSocket(sock, (const char*)test_response, (struct sockaddr *)&source_addr, sizeof(source_addr));
//        if (err < 0) {
//          McuLog_error("Error occurred during sending: errno %d", errno);
//        }
      } /* if */
    } /* while */
    if (sock != -1) {
      McuLog_error("Shutting down socket and restarting...");
      shutdown(sock, 0);
      close(sock);
    }
  } /* for */
  vTaskDelete(NULL);
}

void UDP_Server_Start(void) {
  if (taskHandle!=NULL) {
    vTaskResume(taskHandle);
  }
}

void UDP_Server_Stop(void) {
  if (taskHandle!=NULL) {
    vTaskSuspend(taskHandle);
  }
}

void UDP_Server_Init(void) {
  BaseType_t res;

  res = xTaskCreate(udp_server_task, "udp_server", (16*1024)/sizeof(StackType_t), NULL, tskIDLE_PRIORITY+5, &taskHandle);
  if (res==pdPASS) {
    McuLog_info("created UDP server task");
  } else {
    McuLog_error("failed creating UDP server task!");
  }
}

#endif /* PL_CONFIG_USE_UDP_SERVER */
