/* 
 * AUV Battery Management Server (BatMan)
 *
 *
 *              ++
 *              ||
 *              ||
 *   --     .----++-------------+----------O---------+-----------------+---.
 *    )\  /                     |                    |                 |    \
 *    >-<|                      |        MBARI       |                 |    |
 *   ( /  \                     |                    |                 |    /
 *   --     `-------------------+--------------------+-----------------+---'
 *
 * 
 *
 *
 * Copyright MBARI 2016 
 */

#include "board.h"
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "iap_driver.h"
#include "uart.h"
#include "smbus.h"
#include "srv_ui.h"

#include "emac_support.h"


xSemaphoreHandle bat_semphr;
char strData[2100];

APP_CFG serial0_app;
APP_CFG serial3_app;
APP_CFG control_app;
APP_CFG battery_app;

SERIAL_CFG serial0_cfg;
SERIAL_CFG serial3_cfg;
CONTROL_CFG control_cfg;
BATTERY_CFG battery_cfg;

TASK_CFG maintask, serialtask;

/*
 * network config
 *
 * Some parameters like IP addr, etc. are set in external c-files and we want
 * to be able to set them from flash at boot so they can't be local defines.
 */

#define	TCP_BUFSIZE		256

uint8_t mac_addr[] 		= { 0x0c, 0x1d, 0x12, 0xe0, 0x1f, 0x10 };
char tcp_srv_ip[]  		= "134.89.13.201"; 	/* IP address */
char tcp_srv_gw[]  		= "134.89.12.1";	/* gateway */
char tcp_srv_nm[] 		= "255.255.254.0"; 	/* netmask */
const uint16_t base_port = 10000;

#define WATCHDOG_ON		0
bool wdt_running = false;

/* global variables */
TASK *to_ts_comm;
TASK *to_ts_sercomm;
xTaskHandle to_ts_battery = NULL;

FLASH_CFG flash_cfg;

/* functions in embTCP stack library */
extern void uart_init(int);
extern int32_t nichestack_init(uint32_t);
extern void embTCP_tick_hook(void);

/* functions in emac_support */
extern void msleep(uint32_t);
extern int get_mac_address(int iface, uint8_t *mac_addr);
extern void check_phy(void);

/* local functions */
static void commTask(void *parm);
int tcp_srv_init(uint16_t port);
void tcp_srv_recv(void);
int tcp_srv_respond_term(SOCKTYPE sock, char *terminated_str);
int tcp_srv_respond(SOCKTYPE sock, char *bufp, int bytestosend);
void init_wdt(uint32_t timeout);
int send_to_uart(void *arg, int sockidx, int len);
void decode_ip(char *ip, char *ipstr);
int load_flash_config(void);
void store_flash_config(void);

/*
 * The embTCP stack requires a write_leds(int) function to display
 * internal status. We don't have enough LEDs for this so we just
 * do nothing.
 */ 
void write_leds(int leds) {
}


void
init_wdt(uint32_t timeout) {

	uint32_t wdtFreq;
	/*	Initialize WWDT and event router */
	Chip_WWDT_Init(LPC_WWDT);
	Chip_WWDT_SelClockSource(LPC_WWDT, WWDT_CLKSRC_WATCHDOG_PCLK);
	wdtFreq = Chip_Clock_GetPeripheralClockRate(SYSCTL_PCLK_WDT) / 4;

	/* Set watchdog feed time constant to timeout */
	Chip_WWDT_SetTimeOut(LPC_WWDT, wdtFreq * timeout);

	/* Configure WWDT to reset on timeout */
	Chip_WWDT_SetOption(LPC_WWDT, WWDT_WDMOD_WDRESET);

	/* Clear watchdog warning and timeout interrupts */
	Chip_WWDT_ClearStatusFlag(LPC_WWDT, WWDT_WDMOD_WDTOF | WWDT_WDMOD_WDINT);

	/* Start watchdog */
	Chip_WWDT_Start(LPC_WWDT);

	/* Enable watchdog interrupt */
	NVIC_ClearPendingIRQ(WDT_IRQn);
	NVIC_EnableIRQ(WDT_IRQn);
	wdt_running = true;
}


int
send_to_uart(void *arg, int sockidx, int len)
{
	APP_CFG *app = (APP_CFG *)arg;
	int sent, free, n;
	SERIAL_CFG *ext = (SERIAL_CFG *)app->ext_app_cfg;
	
	n = sent = 0;
	while (sent < len) {
		app->tcp_buffer += n;
		free = RingBuffer_GetFree(&ext->txring);
		if (free < 32) {
			n = 0;
			task_sleep(25);
			continue;
		}
		if (free > len - sent) {
			free = len - sent;
		}
		n = Chip_UART_SendRB(ext->uart, &ext->txring,
		  app->tcp_buffer, free);
		sent += n;
	}
	ext->tx_bytes += sent;
	return (0);
}


void
recv_from_uart(APP_CFG *app)
{
	int i, n;
	int loops = 0;
	int len = 0;
	const int chunksz = 256;
	int rounds = 0;
	uint8_t bytes[chunksz + 1];
	uint8_t *byteptr = bytes;
	portTickType start, dur;
	SERIAL_CFG *ext = (SERIAL_CFG *)app->ext_app_cfg;

	/*
	 * We are trying to avoid sending TCP packets for every single UART byte
	 * we receive. So cycle over Chip_UART_ReadRB() until we have enough bytes
	 * to send out (set by chunksz). However, we don't want to cause
	 * unnecessary delays so we have a timeout of 2 ticks (10-20ms) for sending
	 * out whatever we have.
	 */
	start = xTaskGetTickCount();
	while (1) {
		loops++;
		n = Chip_UART_ReadRB(ext->uart, &ext->rxring, byteptr, chunksz-len);
		len += n;
		ext->rx_bytes += n;

		/* nothing here */
		if (!len) break;

		/* not enough bytes so try again if we didn't wait too long already */
		dur = xTaskGetTickCount() - start;
		if (len < chunksz && dur < 2) {
			byteptr += n;
			continue;
		}
		bytes[len] = 0;
		/* send serial data to all connected sockets */
		for (i = 0; i < MAX_APP_CLIENTS; i++) {
			if (!app->clientsocks[i]) continue;
			tcp_srv_respond(app->clientsocks[i], (char*)bytes, len);
		}

		/* we sent out all the data or we spent too much time in this loop */
		if (len < chunksz || ++rounds % 5 == 0) break;

		/* otherwise reset counters and start over */
		len = 0;
		byteptr = bytes;
		start = xTaskGetTickCount();
	}
}


int
handle_ctl_requests(void *arg, int sockidx, int len)
{
	
	APP_CFG *app = (APP_CFG *)arg;
	CONTROL_CFG *ctlcfg = (CONTROL_CFG *)app->ext_app_cfg;
	SOCKTYPE sock = app->clientsocks[sockidx];
	char response[32];

	int ret;
	
	ret = parse_ctl_cmd(app, sockidx, len);
	if (ret) return (ret);
	
	if (!ctlcfg->sockapp[sockidx]) ctlcfg->sockapp[sockidx] = app;

	/* show prompt based on what app we are configuring */
	sprintf(response, "%s> ", ctlcfg->sockapp[sockidx]->name);
	tcp_srv_respond_term(sock, response);

	return ret;
}

int
handle_battery_requests(void *arg, int sockidx, int len)
{
	
	APP_CFG *app = (APP_CFG *)arg;
	SOCKTYPE sock = app->clientsocks[sockidx];
	char response[32];

	int ret;
	
	ret = parse_battery_cmd(app, sockidx, len);
	if (ret) return (ret);
	
	sprintf(response, "battery> ");
	tcp_srv_respond_term(sock, response);

	return ret;
}

static void
commTask(void *parm) {

	TASK_CFG *task = (TASK_CFG *)parm;
	APP_CFG *app;
	fd_set read_fd_set;
	struct sockaddr_in client;
	int len, i, j;
	SOCKTYPE newsock;
	int recv_toggle;
	char response[32];
	
	/* wait for tcp stack to be ready */
	while (!iniche_net_ready) task_sleep(250);

	FD_ZERO (&task->active_fds);
	
	/* create listeners for all apps in task */
	for (i = 0; i < task->appnum; i++) {
		app = task->apps[i];
		app->master_sock = tcp_srv_init(app->tcp_port);
		FD_SET (app->master_sock, &task->active_fds);	
	}
	
	if (task->serial) {
		recv_toggle += 1;
	}

	recv_toggle = 0;
	while (1) {
		/* the serial task checks for any bytes in the UART queue to send out */
		if (task->serial) {
			for (i = 0; i < task->appnum; i++) {
				recv_from_uart(task->apps[i]);
			}
			if (recv_toggle++ % 5 != 0) continue;
		} else {
			/* watchdog is fed by control/battery task */
			if (WATCHDOG_ON) Chip_WWDT_Feed(LPC_WWDT);
			/* check for link status changes and update EMAC if necessary */
			check_phy();
		}

		read_fd_set = task->active_fds;
		if (t_select(&read_fd_set, NULL, NULL, 0) < 0) {
			panic ("select");
		}
		
		/* cycle through configured apps and check for incoming requests */ 
		for (i = 0; i < task->appnum; i++) {
			app = task->apps[i];

			/* check for requests on established connectiosn */
			for (j = 0; j < MAX_APP_CLIENTS; j++) {
				if (!app->clientsocks[j] || !FD_ISSET(app->clientsocks[j],
				  &read_fd_set))
					continue;

				/* Data arriving on an already-connected socket. */
				len = t_recv(app->clientsocks[j], app->tcp_buffer, TCP_BUFSIZE, 0);
				if (len <= 0 || app->request_handler(app, j, len) < 0) {
					/* Read error (-1) or EOF (0) */
					t_socketclose(app->clientsocks[j]);
					FD_CLR (app->clientsocks[j], &task->active_fds);
					app->clientsocks[j] = 0;
					continue;
				}

			if (app->app_type != SERIAL_APP) taskYIELD();
			}

			/* check for incoming new connections on master socks */
			if (!app->master_sock || !FD_ISSET(app->master_sock, &read_fd_set))
				continue;

			len = sizeof (client);
			newsock = t_accept (app->master_sock, (struct sockaddr *) &client,
			  &len);
			if (newsock < 0) {
				perror ("accept");
				exit (EXIT_FAILURE);
			}
			if (app->app_type != SERIAL_APP) {
				sprintf(response, "\r\n\nConnected to %s \r\n%s> ",
				  app->name, app->name);
				tcp_srv_respond_term(newsock, response);
			}
			for (j = 0; j < MAX_APP_CLIENTS; j++) {
				if (!app->clientsocks[j]) {
					FD_SET (newsock, &task->active_fds);
					app->clientsocks[j] = newsock;
					break;
				}
			}
			if (j >= MAX_APP_CLIENTS) {
				tcp_srv_respond_term(newsock,
				  "\r\nToo many active connections, closing!\r\n\n");
				t_socketclose(newsock);
			}			
			if (app->app_type != SERIAL_APP) taskYIELD();
		}
		if (app->app_type != SERIAL_APP) task_sleep(50);
	}
}


static void
batteryTask(void *parm) {
	
	init_batteries();

	for (;;) {
		
		Board_LED_Set(0, false);
		
		if (xSemaphoreTake(bat_semphr, 500 / portTICK_RATE_MS) == pdFALSE) {			
			continue;
		}

		update_battery_status();
		
		xSemaphoreGive(bat_semphr);	

		if (WATCHDOG_ON) Chip_WWDT_Feed(LPC_WWDT);
		chgwd_check();

		/* sleep for 5s */
		Board_LED_Set(0, true);
		vTaskDelay( 5000 / portTICK_RATE_MS);
	}
}	
	

/* setup tcp socket for server */
int 
tcp_srv_init(uint16_t port)
{
	struct sockaddr_in me;
	
	SOCKTYPE elisten_sock;

	/* open TCP socket */
	me.sin_family = AF_INET;
	me.sin_addr.s_addr = INADDR_ANY;
	me.sin_port = htons(port);

	if (((elisten_sock = t_socket(AF_INET, SOCK_STREAM, 0)) ==
	  INVALID_SOCKET) || 
	  (t_bind(elisten_sock, (struct sockaddr *)&me, sizeof(me)) != 0) ||
	  (t_listen(elisten_sock, 3) != 0)) {
		return (-1);
	}
	/* put listen socket into non-blocking mode */
	t_setsockopt(elisten_sock, 0, SO_NBIO, NULL, 0); 
	return (elisten_sock);
}


/*
 * Wrapper for tcp_srv_respond() to pass in strings without a length.
 * Use ONLY! for 0-terminated strings!!!
 */
int
tcp_srv_respond_term(SOCKTYPE sock, char *terminated_str)
{
	return tcp_srv_respond(sock, terminated_str, strlen(terminated_str));
}


/* send response to client */
int
tcp_srv_respond(SOCKTYPE sock, char *bufp, int bytestosend)
{   
   int bytessent;
   int err;

   while (bytestosend) {
      bytessent = t_send(sock, bufp, bytestosend, 0);
      if (bytessent < 0) {
         err = t_errno(sock);
         if (err == EWOULDBLOCK) {
            taskYIELD();
            continue;
         }
         printf("TCP echo server, err=%d while sending reply\n", err);
         return (err);
      } else if (bytessent < bytestosend) {
         /* Only part of the data was written */
         bufp += bytessent;
         bytestosend -= bytessent;
         taskYIELD();       /* Let other tasks run before we try again */
         continue;
      } else {
         break;     /* All bytes sent  */
	  }
   }
   return (0);
}

int
load_flash_config(void)
{
	void *fptr = (void *) FLASH_CFG_LOCATION;
	int i;
	uint16_t sum = 1;
	/* sync in-ram copy */
	memcpy(&flash_cfg, fptr, FLASH_CFG_SIZE);
	
	/* verify data is valid */
	for (i = 0; i < sizeof(FLASH_CFG); i++) {
		sum = (sum + ((uint8_t *)&flash_cfg)[i]) % 256;
	}
	sum = (sum - flash_cfg.chksum) % 256;
	sum &= 0xff;
	
	if (sum != flash_cfg.chksum) {
		/* flash location is not a valid config */
		memset(&flash_cfg, 0, FLASH_CFG_SIZE);
		return (1);
	}
	return (0);
}

void
store_flash_config(void)
{
	int i, ret;
	uint16_t sum = 1;
	void *fptr = (void *) FLASH_CFG_LOCATION;
	
	flash_cfg.chksum = 0;
	/* calculate checksum before storing data */
	for (i = 0; i < sizeof(FLASH_CFG); i++) {
		sum = (sum + ((uint8_t *)&flash_cfg)[i]) % 256;
	}
	flash_cfg.chksum = sum;
		
	(void) iap_prepare_sector(29, 29);
	(void) iap_erase_sector(29, 29);
	(void) iap_prepare_sector(29, 29);
    ret = iap_copy_ram_to_flash(&flash_cfg, fptr, FLASH_CFG_SIZE);
}


static void
main_task_setup(TASK_CFG *maintask, TASK_CFG *serialtask) {
	
	
	int id;
	bool flash_valid = false;
	char *srv_inbuf, *serial_inbuf;

	/* alloc TCP recv buffers for main and serial tasks */
	if ((srv_inbuf = (char *)pvPortMalloc(TCP_BUFSIZE)) == NULL) panic(NULL);
	if ((serial_inbuf = (char *)pvPortMalloc(TCP_BUFSIZE)) == NULL) panic(NULL);
	memset(srv_inbuf, 0, TCP_BUFSIZE);
	memset(serial_inbuf, 0, TCP_BUFSIZE);
	
	/* zero task and app cnfiguration structs */
	memset(maintask, 0, sizeof(TASK_CFG));
	memset(serialtask, 0, sizeof(TASK_CFG));

	memset(&control_app, 0 , sizeof(control_app));
	memset(&serial0_app, 0 , sizeof(serial0_app));
	memset(&serial3_app, 0 , sizeof(serial3_app));
	memset(&battery_app, 0 , sizeof(battery_app));

	memset(&serial0_cfg, 0, sizeof(serial0_cfg));
	memset(&serial3_cfg, 0, sizeof(serial3_cfg));
	memset(&control_cfg, 0, sizeof(control_cfg));

	/* load network config from flash */
	if (load_flash_config() == 0) {
		memcpy(mac_addr, flash_cfg.macaddr, sizeof(mac_addr));
		strcpy(tcp_srv_ip, flash_cfg.ipstr);
		strcpy(tcp_srv_gw, flash_cfg.gwstr);
		strcpy(tcp_srv_nm, flash_cfg.nmstr);
		flash_valid = true;
	} else {
		/* there are no valid values in flash, store defaults for next time */
		memcpy(flash_cfg.macaddr, mac_addr, sizeof(mac_addr));
		strcpy(flash_cfg.ipstr, tcp_srv_ip);
		strcpy(flash_cfg.gwstr, tcp_srv_gw);
		strcpy(flash_cfg.nmstr, tcp_srv_nm);
	}

	/* setup for control app */
	id = 0;
	control_app.id = id;
	strcpy(control_app.name, "control");
	control_app.app_type = CTL_APP;
	control_app.request_handler = &handle_ctl_requests;
	control_app.tcp_buffer = srv_inbuf;
	control_app.parent_task = maintask;
	control_app.ext_app_cfg = &control_cfg;
	if (flash_valid && flash_cfg.app_cfgs[id].port > 0) {
		control_app.tcp_port = flash_cfg.app_cfgs[id].port;
	} else {
		flash_valid = false;
		control_app.tcp_port = base_port + id;
		flash_cfg.app_cfgs[id].port = base_port + id;
	}
	
	control_cfg.apps[0] = &serial0_app;
	control_cfg.apps[1] = &serial3_app;
	control_cfg.apps[2] = &control_app;
	control_cfg.apps[3] = &battery_app;
	control_cfg.appnum = 4;

	
	/* setup for serial app on UART0 */
	id = 1;
	serial0_app.id = id;
	strcpy(serial0_app.name, "uart0");
	serial0_app.app_type = SERIAL_APP;
	serial0_app.request_handler = send_to_uart;
	serial0_app.tcp_buffer = serial_inbuf;
	serial0_app.parent_task = serialtask;
	serial0_app.ext_app_cfg = &serial0_cfg;
	if (flash_valid && flash_cfg.app_cfgs[id].port > 0) {
		serial0_app.tcp_port = flash_cfg.app_cfgs[id].port;
	} else {
		flash_valid = false;
		serial0_app.tcp_port = base_port + id;
		flash_cfg.app_cfgs[id].port = base_port + id;
	}

	setup_uart0(&serial0_cfg, flash_cfg.app_cfgs[id].baud,
	  flash_cfg.app_cfgs[id].mode);


	/* setup for serial app on UART3 */
	id = 2;
	serial3_app.id = id;
	strcpy(serial3_app.name, "uart3");
	serial3_app.app_type = SERIAL_APP;
	serial3_app.request_handler = send_to_uart;
	serial3_app.tcp_buffer = serial_inbuf;
	serial3_app.parent_task = serialtask;
	serial3_app.ext_app_cfg = &serial3_cfg;
	if (flash_valid) {
		serial3_app.tcp_port = flash_cfg.app_cfgs[id].port;
	} else {
		serial3_app.tcp_port = base_port + id;
		flash_cfg.app_cfgs[id].port = base_port + id;
	}
	setup_uart3(&serial3_cfg, flash_cfg.app_cfgs[id].baud,
	  flash_cfg.app_cfgs[id].mode);
	
	
	/* setup for battery app */
	id = 3;
	battery_app.id = id;
	strcpy(battery_app.name, "battery");
	battery_app.app_type = BAT_APP;
	battery_app.request_handler = &handle_battery_requests;
	battery_app.tcp_buffer = srv_inbuf;
	battery_app.parent_task = maintask;
	if (flash_valid && flash_cfg.app_cfgs[id].port > 0) {
		battery_app.tcp_port = flash_cfg.app_cfgs[id].port;
	} else {
		flash_valid = false;
		battery_app.tcp_port = base_port + id;
		flash_cfg.app_cfgs[id].port = base_port + id;
	}
	

	/* if we had to use default values for anything, store new values */
	if (!flash_valid) {
		store_flash_config();
	}
	
	/* assign apps to tasks */
	maintask->appnum = 2;
	maintask->apps[0] = &control_app;
	maintask->apps[1] = &battery_app;
	
	serialtask->serial = true;
	serialtask->appnum = 2;
	serialtask->apps[0] = &serial0_app;
	serialtask->apps[1] = &serial3_app;
	
	return;
}

/* Sets up system hardware */
static void prvSetupHardware(void)
{
	SystemCoreClockUpdate();
	Board_Init();
	smbus_setup();

	/*
	 * Initialize embTCP debug UART.
	 * I have no idea what this does (internal embTCP library function) or
	 * if it clobbers our own UART setup. Would have to try with a serial
	 * cable attached. Disabled for now.
	 */
	uart_init(1);
}


int
main(void)
{
	int32_t  err;
	char  buf[32];
	
	prvSetupHardware();
	main_task_setup(&maintask, &serialtask);
	
	vSemaphoreCreateBinary(bat_semphr);

	err = nichestack_init(0);
	if (err < 0)
	{
		sprintf(&buf[0], "nichestack_init error %d", err);
		panic(buf);
	}
	
	/* TCP comm task */
	TK_CREATE(&commTask, "ts_comm", 1024, &maintask, 2, &to_ts_comm);
	TK_CREATE(&commTask, "ts_sercomm", 1024, &serialtask, 2, &to_ts_sercomm);

	/* battery query task */
	xTaskCreate(batteryTask, (signed char *)"ts_battery",
	  configMINIMAL_STACK_SIZE, NULL, 2, &to_ts_battery);

	/* enable watchdog */
	if (WATCHDOG_ON) init_wdt(120);
	
	TK_START_OS();
	
	/* should never get here */
	panic("exit");
	return (1);
}



/*----------------- FreeRTOS Application Hooks ----------------------------*/

/* FUNCTION: vApplicationTickHook()
 *
 * Called by the FreeRTOS timer interrupt function.
 */
void
vApplicationTickHook(void)
{
   /* DO NOT remove this function call if you are using embTCP. */
   embTCP_tick_hook();        /* increment the embTCP 'cticks' counter */
}


#if (configUSE_IDLE_HOOK == 1)

/* FUNCTION: vApplicationIdleHook()
 *
 * Called from the "idle" task.
 *
 * PARAMS: none
 *
 * RETURN: none
 */
void
vApplicationIdleHook(void)
{
   const unsigned long ulMSToSleep = 5;

   /* Sleep to reduce CPU load, but don't sleep indefinitely in case
    * there are tasks waiting to be terminated by the idle task.
    */
   Sleep(ulMSToSleep);
}

#endif

#if (configUSE_MALLOC_FAILED_HOOK == 1)

/* FUNCITON: vApplicationMallocFailedHook()
 *
 * Called by FreeRTOS when a memory allocation request fails.
 *
 * PARAMS: none
 *
 * RETURN: none
 */
void
vApplicationMallocFailedHook(void)
{
   panic("malloc");
}

#endif

#if (configCHECK_FOR_STACK_OVERFLOW > 0)

/* FUNCTION: vApplicationStackOverflowHook()
 *
 * Called if the OS detects a task stack overflow.
 *
 * PARAM1: xTaskHandle        current FreeRTOS task
 * PARAM2: char *             name of current task
 *
 * RETURN: none
 */
void
vApplicationStackOverflowHook(xTaskHandle curTCB, char *name)
{
	panic("stack overflow");
}

#endif

/* FUNCTION: vAssertCalled()
 *
 * Called if "configAssert(x)" is defined in FreeRTOSConfig.h.
 *
 * PARAMS: none
 *
 * RETURN: none
 */
void
vAssertCalled(void)
{
	panic("assert");
}