#include <string.h>
#include <stdlib.h>

#include "board.h"
#include "app_cfg.h"
#include "uart.h"
#include "smbus.h"
#include "FreeRTOS.h"
#include "semphr.h"
#include "utils.h"
#include "bl_shared.h"

#include "FreeRTOS_Sockets.h"


/* global variables from main source */
extern char str_data[];
extern xSemaphoreHandle bat_semphr;

extern FLASH_CFG flash_cfg;
extern bool wdt_running;
extern uint8_t ucMACAddress[];
extern char tcp_srv_ip[];
extern char tcp_srv_gw[];
extern char tcp_srv_nm[];

extern const char version[];

/* global variables from smbus.c */
extern bool bat_data_ready;
extern BATTERY_DATA batteryData[];
extern BATTERY_STATE bat_state;

extern Socket_t tcp_srv_init(uint16_t port);
extern int tcp_srv_respond_term(Socket_t sock, char *terminated_str);
extern int tcp_srv_respond(Socket_t sock, char *bufp, int bytestosend);
extern void init_wdt(uint32_t timeout);
extern void store_flash_config(void);


static char
*get_cmd(char *buf, int len)
{
	char *cmd = NULL;
	char *sp;
	
	for (sp = buf; sp - buf < len; sp++) {
		if (*sp <= ' ') {
			if (cmd) {
				/* trim trailing whitespace */
				*sp = '\0';
				break;
			} else {
				/* trim leading whitespace */
				continue;
			}
		}
		if (!cmd) cmd = sp;
	}
	return (cmd);
}

/* parsing helper to find first argument after cmd, trims whitespaces */
static char
*get_argument(char *buf, int len)
{
	char *arg = NULL;
	char *sp;
	bool fspc = false;
	
	for (sp = buf; sp - buf < len; sp++) {
		/* find first white space that separates cmd from argument */
		if (*sp > ' ' && !fspc) continue;
		else fspc = true;
		/* ignore leading white spaces*/
		if (*sp <= ' ') {
			/* delete trailing whitespace */
			if (arg) {
				*sp = '\0';
				break;
			}
			continue;
		}
		/* found beginning of argument */
		if (!arg) arg = sp;
	}
	return arg;
}


/* decodes IP address from in_addr data structure in 'ip' */
void
decode_ip(char *ip, char *ipstr)
{
	uint16_t port = (ip[0] << 8) + ip[1];
	sprintf(ipstr, "%d.%d.%d.%d:%u", ip[2],ip[3],ip[4],ip[5], port);
	return;
}

/* parses IP address from string and validates it */
int
parse_ip_octet(char *param, char **ipstr)
{
	char *sp;
	bool fspc = false, end = false;
	int i, octet, dots = 0;
	
	char tmp[4] = {0, 0, 0, 0};
	bool error = false;
		
	if (strlen(param) < 7) {
		return (-1);
	}
	
	*ipstr = NULL;
	fspc = false;
	for (sp = param, i = 0; sp - param < strlen(param); sp++, i++) {
		/* ignore leading spaces */
		if (*sp <= ' ' && !fspc) continue;
		if (!*ipstr) {
			*ipstr = sp;
			fspc = true;
		}
		/* trim trailing white space */
		if (*sp <= ' ') {
			*sp = '\0';
			end = true;
		};
		/* octet with more than 3 digits */
		if (i > 3) {
			return (-1);
		}
		if (*sp == '.' || end) {
			/* no digit between two dots */
			if (i < 1) {
				return (-1);
			}
			octet = atoi(tmp);
			/* atoi() returns 0 on invalid number so check if digit is 0 */
			if (octet == 0 && strncmp(tmp, "0", 3) != 0) {
				return (-1);
			}
			if (octet > 256 || octet < 0) {
				return (-1);
			}
			if (end) break;

			dots++;
			i = -1;
			memset(tmp, 0, 3);
			continue;
		}
		tmp[i] = *sp;
	}
		
	if (dots != 3 || error) {
		return (-1);
	}
	
	return (0);

}


int
parse_battery_cmd(APP_CFG *app, int sockidx, int len)
{
	Socket_t sock = app->clientsocks[sockidx];
	char *srv_inbuf = app->tcp_buffer;
	int battery;
	float fval1, fval2, fval3;
	uint16_t *voltages;
	uint8_t types[2];

	srv_inbuf[len] = 0;
	
	/*
	 * There seems to be an issue when we send an EOF command in a cygwin
	 * telnet session in that it floods the server with EOT messages.
	 * So we just close the socket in this case since this is what has been
	 * requested by the client anyway.
	 */
	if (len >= 2 && (uint8_t)srv_inbuf[0] == 0xff &&
	  (uint8_t)srv_inbuf[1] == 0xec) {
		return (-1);
	}

	/* ignore all other telnet commands */
	if (len >= 1 && (uint8_t)srv_inbuf[0] == 0xff) {
		return (1);
	}

	/* we've got a customer, check if we're ready to return battery stats */
	if (len == 7) {
		if (!strncmp(srv_inbuf,"HILFE", 5)) {
			char *str_ptr = str_data;
			str_ptr += sprintf(str_ptr," @99AF  ...  Turn off packs, disable all switches  \r\n");
			str_ptr += sprintf(str_ptr," @99AC  ...  Enable charge switch\r\n");
			str_ptr += sprintf(str_ptr," @99AD  ...  Enable discharge switch A (main)\r\n");
			str_ptr += sprintf(str_ptr," @99BD  ...  Enable discharge switch B (AUX)\r\n");
			str_ptr += sprintf(str_ptr," @99BF  ...  Disable discharge switch B (AUX)\r\n");
			str_ptr += sprintf(str_ptr," @99CD  ...  Enable discharge switch C (AUX2)\r\n");
			str_ptr += sprintf(str_ptr," @99CF  ...  Disable discharge switch C (AUX2)\r\n");
			str_ptr += sprintf(str_ptr," @??PF  ...  Turn off pack ?? (00-54)\r\n");
			str_ptr += sprintf(str_ptr," @??PN  ...  Turn on pack ?? (00-54)\r\n");
			str_ptr += sprintf(str_ptr," @99RQ  ...  Request status\r\n");
			str_ptr += sprintf(str_ptr," @??CV  ...  Get cell voltages for pack ?? (3.4Ah packs only)\r\n");
			str_ptr += sprintf(str_ptr," @99SH  ...  Put packs into shutdown mode \r\n");
			str_ptr += sprintf(str_ptr," WDOFF  ...  Disable charge watchdog\r\n");
			str_ptr += sprintf(str_ptr," WDACT  ...  Enable charge watchdog\r\n");
			str_ptr += sprintf(str_ptr," WDRST  ...  Reset watchdog\r\n");
			tcp_srv_respond(sock, str_data, str_ptr-str_data);

		} else if (!bat_data_ready) {
			tcp_srv_respond_term(sock, "Battery data not ready!\n");
			return (0);

		} else if (!strncmp(srv_inbuf,"@99SH", 5)) {
			if (xSemaphoreTake(bat_semphr,
			  10000 / portTICK_RATE_MS) == pdFALSE) {
				tcp_srv_respond_term(sock, "Can't take battery semaphore!\r\n");
				return (0);
			}
			tcp_srv_respond_term(sock, "Shutting packs down ...");
			Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, CHG_ENABLE_PIN);
			Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, DISCH_ENABLE_PIN);
			Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, AUX2_ENABLE_PIN);
			Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, AUX_ENABLE_PIN);
			shutdown_packs();
			xSemaphoreGive(bat_semphr);
			tcp_srv_respond_term(sock, " done\r\n");
			
		} else if (!strncmp(srv_inbuf,"WDRST", 5)) {
			chgwd_reset();
			tcp_srv_respond_term(sock, "Charge watchdog reset.\r\n");
			
		} else if (!strncmp(srv_inbuf,"WDOFF", 5)) {
			chgwd_control(false);
			tcp_srv_respond_term(sock, "Charge watchdog disabled.\r\n");
			
		} else if (!strncmp(srv_inbuf,"WDACT", 5)) {
			chgwd_control(true);
			chgwd_reset();
			tcp_srv_respond_term(sock, "Charge watchdog activated.\r\n");
			
		} else if (!strncmp(srv_inbuf,"@99AC", 5)) {
			tcp_srv_respond_term(sock, "Setting batteries to charge ...");
			Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, CHG_ENABLE_PIN);
			Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, DISCH_ENABLE_PIN);

			if (xSemaphoreTake(bat_semphr,
			  10000 / portTICK_RATE_MS) == pdFALSE) {
				tcp_srv_respond_term(sock, "Can't take battery semaphore!\r\n");
				return (0);
			}
			allPacks(1);
			xSemaphoreGive(bat_semphr);

			utils_task_sleep(250);
			Chip_GPIO_SetPinOutHigh(LPC_GPIO, 0, CHG_ENABLE_PIN);
			bat_state = BATT_CHG;
			chgwd_reset();
			tcp_srv_respond_term(sock, " done\r\n");

		} else if (!strncmp(srv_inbuf,"@99AD", 5)) {
			tcp_srv_respond_term(sock, "Setting batteries to discharge ...");
			Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, CHG_ENABLE_PIN);
			Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, DISCH_ENABLE_PIN);

			if (xSemaphoreTake(bat_semphr,
			  10000 / portTICK_RATE_MS) == pdFALSE) {
				tcp_srv_respond_term(sock, "Can't take battery semaphore!\r\n");
				return (0);
			}
			allPacks(1);
			xSemaphoreGive(bat_semphr);

			utils_task_sleep(250);
			Chip_GPIO_SetPinOutHigh(LPC_GPIO, 0, DISCH_ENABLE_PIN);
			bat_state = BATT_DISCH;
			tcp_srv_respond_term(sock, " done\r\n");

		} else if (!strncmp(srv_inbuf,"@99BD", 5)) {
			tcp_srv_respond_term(sock, "Enabling AUX ...");
			Chip_GPIO_SetPinOutHigh(LPC_GPIO, 0, AUX_ENABLE_PIN);
			//bat_state = BATT_DISCH;
			tcp_srv_respond_term(sock, " done\r\n");

		} else if (!strncmp(srv_inbuf,"@99BF", 5)) {
			tcp_srv_respond_term(sock, "Disabling AUX ...");
			Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, AUX_ENABLE_PIN);
			//bat_state = BATT_DISCH;
			tcp_srv_respond_term(sock, " done\r\n");

#ifdef X41CELL
		} else if (!strncmp(srv_inbuf,"@99CD", 5)) {
			tcp_srv_respond_term(sock, "Enabling AUX2 ...");
			Chip_GPIO_SetPinOutHigh(LPC_GPIO, 0, AUX2_ENABLE_PIN);
			//bat_state = BATT_DISCH;
			tcp_srv_respond_term(sock, " done\r\n");

		} else if (!strncmp(srv_inbuf,"@99CF", 5)) {
			tcp_srv_respond_term(sock, "Disabling AUX2 ...");
			Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, AUX2_ENABLE_PIN);
			//bat_state = BATT_DISCH;
			tcp_srv_respond_term(sock, " done\r\n");
#endif

		} else if (!strncmp(srv_inbuf,"@99AF", 5)) {
			tcp_srv_respond_term(sock, "Turning all batteries off ...");
			Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, CHG_ENABLE_PIN);
			Chip_GPIO_SetPinOutLow(LPC_GPIO, 0, DISCH_ENABLE_PIN);

			if (xSemaphoreTake(bat_semphr,
			  10000 / portTICK_RATE_MS) == pdFALSE) {
				tcp_srv_respond_term(sock, "Can't take battery semaphore!\r\n");
				return (0);
			}
			allPacks(0);
			xSemaphoreGive(bat_semphr);
			
			bat_state = BATT_OFF;
			tcp_srv_respond_term(sock, " done\r\n");

		} else if (!strncmp(srv_inbuf+3,"PF", 2)) {
			int pack;
			char out[32];
			sscanf(srv_inbuf, "@%2dPF", &pack);
			if (pack >= get_bat_count() || pack < 0) {
				tcp_srv_respond_term(sock, "invalid pack number\r\n");
				return (0);
			}
			batteryData[pack].flags = PACK_OFF;
			sprintf(out, "turning off pack %d\n", pack);
			tcp_srv_respond_term(sock, out);

		} else if (!strncmp(srv_inbuf+3,"PN", 2)) {
			int pack;
			char out[32];
			sscanf(srv_inbuf, "@%2dPN", &pack);
			if (pack >= get_bat_count() || pack < 0) {
				tcp_srv_respond_term(sock, "invalid pack number\r\n");
				return (0);
			}
			batteryData[pack].flags = PACK_ON;
			sprintf(out, "turning on pack %d\n", pack);
			tcp_srv_respond_term(sock, out);

		} else if (!strncmp(srv_inbuf,"@99RQ", 5)) {
			char *strPtr = str_data;
			tcp_srv_respond_term(sock, "querying battery status ... \r\n");
			if (xSemaphoreTake(bat_semphr,
			  6000 / portTICK_RATE_MS) == pdFALSE) {
				tcp_srv_respond_term(sock, "No access to battery status!\r\n");
				return (0);
			}
			types[0] = types[1] = 0;
			for (battery = 0; battery < get_bat_count(); battery++) {
				char states[] = {'#','*','$','.'};
				*strPtr++ = states[batteryData[battery].state];
				//batteryData[battery].state ? (*strPtr++ = '*') : (*strPtr++ = '#');
				strPtr += sprintf(strPtr,"%02d:%04d,%04d,%05d,%04d,%03d,%04X\r\n", battery, batteryData[battery].serialNo, batteryData[battery].current, batteryData[battery].stackVolt,
					batteryData[battery].capacity_mAh, batteryData[battery].cycleCnt, batteryData[battery].status);
				if (batteryData[battery].type == BATTYPE_29)
					types[0]++;
				else if (batteryData[battery].type == BATTYPE_34)
					types[1]++;
					
			}
			xSemaphoreGive(bat_semphr);
			fval1 = (float)get_battery_temp() * .0625F;
			fval2 = (float)get_humidity_temp() * .010072F - 40;
			fval3 = (float)get_humidity() * .006104F;
			voltages = get_voltages();

			strPtr += sprintf(strPtr,"Env:%3dC,%3dC,%3d%%RH\r\n",
			  (int)(fval1*10.0), (int)(fval2*10.0),(int)(fval3*10.0));
			strPtr += sprintf(strPtr,"Meas1:%04d,%04d,%04d,%04d,%04d\r\n",
			  voltages[0],voltages[1],voltages[2],voltages[3],voltages[4]);
			strPtr += sprintf(strPtr,"Meas2:%3d,%3d,%3d,%3d,%3d\r\n",
			  (int)(0.092294 * voltages[0]),  // (10 * 3.15V / 4096 steps) * ((100 kOhm + 9.09kOhm) / 9.09kOhm) = 0.088632 dV/step
			  (int)(0.092294 * voltages[1]),
			  (int)(0.099976 * voltages[2]),  // (10 * 3.15V / 4096 steps) / ( 1/13000 * 1000 Ohm) = 0.099976 dA/step
			  (int)(0.099976 * voltages[3]),
			  (int)(0.199951 * voltages[4])); // (10 * 3.15V / 4096 steps) / ( 1/13000 * 500 Ohm) =  0.199951 dA/step

			strPtr += sprintf(strPtr,"Packtype: 29<%02d>,34<%02d>\r\n",
			  types[0], types[1]);
			  
			strPtr += sprintf(strPtr,"CHG: %d, ",
			  Chip_GPIO_ReadValue(LPC_GPIO, 0) & 1<<CHG_ENABLE_PIN ? 1 : 0);
			strPtr += sprintf(strPtr,"DISCH: %d, ",
			  Chip_GPIO_ReadValue(LPC_GPIO, 0) & 1<<DISCH_ENABLE_PIN ? 1 : 0);
#ifdef X41CELL
			strPtr += sprintf(strPtr,"AUX: %d, ",
			  Chip_GPIO_ReadValue(LPC_GPIO, 0) & 1<<AUX_ENABLE_PIN ? 1 : 0);
			strPtr += sprintf(strPtr,"AUX2: %d\r\n",
			  Chip_GPIO_ReadValue(LPC_GPIO, 0) & 1<<AUX2_ENABLE_PIN ? 1 : 0);
#else
			strPtr += sprintf(strPtr,"AUX: %d\r\n",
			  Chip_GPIO_ReadValue(LPC_GPIO, 0) & 1<<AUX_ENABLE_PIN ? 1 : 0);
#endif
			strPtr += sprintf(strPtr,"SMBUSerr:%lu (%lu unrecoverable)\r\n",
			  get_smbus_errors(), get_unrec_smbus_errors());

			tcp_srv_respond(sock, str_data, strPtr-str_data);

		} else if (!strncmp(srv_inbuf+3,"CV", 2)) {
			int pack, count;
			CELL_VOLTAGES *cv;
			char *strPtr = str_data;

			sscanf(srv_inbuf, "@%2dCV", &pack);
			if (pack == 99) {
				pack = -1;
				count = get_bat_count();
			} else {
				count = 1;
			}
			if (pack >= get_bat_count() || pack < -1) {
				tcp_srv_respond_term(sock, "invalid pack number\r\n");
				return (0);
			}

			tcp_srv_respond_term(sock, "querying battery cell voltages ... \r\n");
			if (xSemaphoreTake(bat_semphr,
			  6000 / portTICK_RATE_MS) == pdFALSE) {
				tcp_srv_respond_term(sock, "No access to battery status!\r\n");
				return (0);
			}
			cv = get_cell_voltages(pack);
			if (pack == -1) pack = 0;
			for (battery = pack; battery < pack + count; battery++) {
				strPtr += sprintf(strPtr,
				  "%02d:%04d,%04d,%04d,%04d,%04d,%04d,%04d,%04d\r\n",
				  battery, cv[battery].cell1,cv[battery].cell2,
				  cv[battery].cell3,cv[battery].cell4,cv[battery].cell5,
				  cv[battery].cell6,cv[battery].cell7,cv[battery].cell8);
			}
			xSemaphoreGive(bat_semphr);
			tcp_srv_respond(sock, str_data, strPtr-str_data);
		} else if (!strncmp(srv_inbuf,"STATS", 5)) {
			int pack, count = 0;
			uint16_t minvolt = UINT16_MAX, maxvolt = 0;
			int totcur = 0, avgvolt = 0, avgcap = 0;
			float power = 0;

			if (xSemaphoreTake(bat_semphr,
			  6000 / portTICK_RATE_MS) == pdFALSE) {
				tcp_srv_respond_term(sock, "No access to battery status!\r\n");
				return (0);
			}
			for (pack = 0; pack < get_bat_count(); pack++) {
				if (batteryData[pack].stackVolt < minvolt)
					minvolt = batteryData[pack].stackVolt;
				if (batteryData[pack].stackVolt > maxvolt)
					maxvolt = batteryData[pack].stackVolt;
				if (batteryData[pack].stackVolt > 0) {
					avgvolt += batteryData[pack].stackVolt;
					power += (float)batteryData[pack].stackVolt * \
					  batteryData[pack].current / 1000000;
					count++;
				}
				if (batteryData[pack].capacity_mAh > 0) {
					avgcap += batteryData[pack].capacity_mAh;
				}
				totcur += batteryData[pack].current;
			}

			xSemaphoreGive(bat_semphr);
			sprintf(str_data,
			  "min voltage:    %6.2fV\r\n" \
			  "max voltage:    %6.2fV\r\n" \
			  "avg voltage:    %6.2fV\r\n" \
			  "avg capacity:   %6.2fAh\r\n" \
			  "total current:  %6.2fA\r\n" \
			  "total power:   %7.2fW\r\n\n" \
			  "ambient temp:   %6.1f*C\r\n" \
			  "humidity:       %6.1f%%\r\n",
			  (float)minvolt / 1000, (float)maxvolt / 1000,
			  (float)avgvolt / count / 1000, (float)avgcap / count / 1000,
			  (float)totcur/1000, power,
			  (float)get_humidity_temp() * .010072F - 40,
			  (float)get_humidity() * .006104F);
			tcp_srv_respond_term(sock, str_data);
		}

	}
	
	return (0);
}

static bool
is_cmd(char *cmd, char *cmpstr, int mincmp) {
	
	if (strlen(cmd) < mincmp) return (false);
	if (strlen(cmd) > strlen(cmpstr)) return (false);
	return (strncmp(cmpstr, cmd, strlen(cmd)) == 0);
}

int
parse_ctl_cmd(APP_CFG *app, int sockidx, int len)
{

	CONTROL_CFG *ctlcfg = (CONTROL_CFG *)app->ext_app_cfg;
	APP_CFG *current_app = ctlcfg->sockapp[sockidx];
	Socket_t sock = app->clientsocks[sockidx];
	char *srv_inbuf = app->tcp_buffer;
	char *strPtr, *arg, *cmd, *argstart;
	int i, j, arglen;
	bool control_mode, serial_mode, battery_mode;

	/*
	 * There seems to be an issue when we send an EOF command in a cygwin
	 * telnet session in that it floods the server with EOT messages.
	 * So we just close the socket in this case since this is what has been
	 * requested by the client anyway.
	 */
	if (len >= 2 && (uint8_t)srv_inbuf[0] == 0xff &&
	  (uint8_t)srv_inbuf[1] == 0xec) {
		return (-1);
	}

	/* ignore all other telnet commands */
	if (len >= 1 && (uint8_t)srv_inbuf[0] == 0xff) {
		return (1);
	}

	/*
	 * this is important for some of the string parsing to work correctly since
	 * the inbuffer is not 0 terminated
	 */
	srv_inbuf[len] = 0;
	
	/* default current_app to control app */
	if (!current_app) {
		ctlcfg->sockapp[sockidx] = current_app = app;
	}

	/* some cmds are only valid for specific apps */
	control_mode = ctlcfg->sockapp[sockidx]->app_type == CTL_APP;
	serial_mode = ctlcfg->sockapp[sockidx]->app_type == SERIAL_APP;
	battery_mode = ctlcfg->sockapp[sockidx]->app_type == BAT_APP;
	
	
	cmd = get_cmd(srv_inbuf, len);
	if (!cmd) {
		return (0);	
	}
	argstart = cmd + strlen(cmd);
	arglen = (srv_inbuf + len - argstart);

	/* help cmd */
	if (is_cmd(cmd, "help", 0)) {
		strPtr = str_data;
		strPtr += sprintf(strPtr, "\r\n  %s commands:\r\n\n",
		  current_app->name);

		if (serial_mode) {
			strPtr += sprintf(strPtr, "baud <value>    set baud rate to <value> bps\r\n");
			strPtr += sprintf(strPtr, "mode <value>    set mode: <wordlen><parity><stopbits> (e.g. 8N1)\r\n");
			strPtr += sprintf(strPtr, "break           send break signal\r\n");
			strPtr += sprintf(strPtr, "cache <value>   set cache mode <value> (0: timed, 1: newline)\r\n");
			strPtr += sprintf(strPtr, "status          show UART app status\r\n");
		} else if (battery_mode) {
			strPtr += sprintf(strPtr, "status          show battery app status\r\n");
		} else {
			strPtr += sprintf(strPtr, "app             show apps\r\n");	
			strPtr += sprintf(strPtr, "app <value>     manage app <value>\r\n");	
			strPtr += sprintf(strPtr, "ip <value>      set IP address (requires MCU reset)\r\n");
			strPtr += sprintf(strPtr, "nm <value>      set IP netmask (requires MCU reset)\r\n");
			strPtr += sprintf(strPtr, "gw <value>      set gateway address (requires MCU reset)\r\n");
			strPtr += sprintf(strPtr, "mac <value>     set MAC address (requires MCU reset)\r\n");
			strPtr += sprintf(strPtr, "status          show network status\r\n");
			strPtr += sprintf(strPtr, "clients         show connected clients\r\n");
			strPtr += sprintf(strPtr, "reset           reset the MCU\r\n");
			strPtr += sprintf(strPtr, "version         show version\r\n");
		}
		strPtr += sprintf(strPtr, "port <value>    set TCP port (disconnects clients)\r\n");	
		strPtr += sprintf(strPtr, "exit            leave menu\r\n");	
		strPtr += sprintf(strPtr, "\r\n");
		
		tcp_srv_respond_term(sock, str_data);
		return (0);
	}
	  
	/* app cmd */
	if (control_mode && is_cmd(cmd, "app", 0)) {
		arg = get_argument(argstart, arglen);
		/* no arguments given, list apps */
		if (!arg) {
			char type[8];
			strPtr = str_data;
			
			strPtr += sprintf(strPtr, "App (type)\r\n\n");
			
			for (i = 0; i < ctlcfg->appnum; i ++) {
				if (ctlcfg->apps[i]->app_type == SERIAL_APP)
					strcpy(type, "serial");
				else if (ctlcfg->apps[i]->app_type == BAT_APP)
					strcpy(type, "battery");
				else
					strcpy(type, "control");
				strPtr += sprintf(strPtr, "  %s (%s)\r\n",
				  ctlcfg->apps[i]->name, type);
			}
			tcp_srv_respond_term(sock, str_data);
			return (0);
		}
		/* parse arg and select app to configure */
		for (i = 0; i < ctlcfg->appnum; i ++) {
			if (strcmp(arg, ctlcfg->apps[i]->name) == 0) {
				ctlcfg->sockapp[sockidx] = ctlcfg->apps[i];
				return (0);
			}
		}
		sprintf(str_data, "App '%s' is not defined!\r\n", arg);
		tcp_srv_respond_term(sock, str_data);
		return (0);
	}
	
	/* reset mcu cmd */
	if (control_mode && is_cmd(cmd, "reset", 0)) {
		tcp_srv_respond_term(sock, "resetting MCU ...\r\n");
		utils_task_sleep(250);
		utils_socket_close(sock);
		if (wdt_running) {
			Chip_WWDT_SetTimeOut(LPC_WWDT, 1);
			Chip_WWDT_Feed(LPC_WWDT);
		} else {
			init_wdt(0);
		}
		while(1);
	}
	
	/* client cmd */
	if (control_mode && is_cmd(cmd, "clients", 0)) {
		struct freertos_sockaddr name;
		char peer[24];
		char info[12];
		int id;
		Socket_t csock;

		strPtr = str_data;
		str_data[0] = 0;
		for (i = 0; i < ctlcfg->appnum; i++) {
			for (j = 0; j < MAX_APP_CLIENTS; j++) {
				id = (i + 1) * 10 + j;
				csock = ctlcfg->apps[i]->clientsocks[j];
				if (!csock) continue;
				
				FreeRTOS_GetRemoteAddress(csock, &name);
				FreeRTOS_inet_ntoa(name.sin_addr, peer);
				
				if (csock == sock)
					strcpy(info, "(you)");
				else
					sprintf(info, "(%s)", ctlcfg->apps[i]->name);
				
				strPtr += sprintf(strPtr, "  [%d] %s %s\r\n", id, peer, info);
			}
		}
		tcp_srv_respond_term(sock, str_data);
		return (0);
	}
	
	/* port cmd */
	if (is_cmd(cmd, "port", 0)) {
		int port;
		
		arg = get_argument(argstart, arglen);
		if (!arg) {
			tcp_srv_respond_term(sock, "Argument required!\r\n");
			return (0);
		}
		port = atoi(arg);
		
		if (port < 1 || port > 0xffff) {
			sprintf(str_data, "Invalid port '%s'!\r\n", arg);
			tcp_srv_respond_term(sock, str_data);
			return (0);
		}

		if (port == current_app->tcp_port) {
			sprintf(str_data, "Continuing using old port '%d'\r\n", port);
			tcp_srv_respond_term(sock, str_data);
			return (0);
		}
		
		for (i = 0; i < ctlcfg->appnum; i++) {
			if (ctlcfg->apps[i]->tcp_port == port) {
				sprintf(str_data, "Port '%d' is already taken by '%s'!\r\n",
				  port, ctlcfg->apps[i]->name);
				tcp_srv_respond_term(sock, str_data);
				return (0);
			}
		}
		
		sprintf(str_data, "Setting port to %d, clients will be "
		  "disconnected.\r\n", port);
		tcp_srv_respond_term(sock, str_data);
		for (i = 0; i < MAX_APP_CLIENTS; i++) {
			if (!current_app->clientsocks[i]) continue;
			
			utils_socket_close(current_app->clientsocks[i]);
			FreeRTOS_FD_CLR(current_app->clientsocks[i], current_app->parent_task->active_fds, eSELECT_READ | eSELECT_EXCEPT);
			current_app->clientsocks[i] = 0;
		}
		utils_task_sleep(100);
		utils_socket_close(current_app->master_sock);
		FreeRTOS_FD_CLR(current_app->master_sock, current_app->parent_task->active_fds, eSELECT_READ | eSELECT_EXCEPT);
		
		current_app->tcp_port = port;
		current_app->master_sock = tcp_srv_init((uint16_t)port);
		FreeRTOS_FD_SET(current_app->master_sock, current_app->parent_task->active_fds, eSELECT_READ | eSELECT_EXCEPT);


		/* store new port value in flash */
		flash_cfg.app_cfgs[current_app->id].port = port;
		store_flash_config();

		return (0);
	}
	
	/* ip cmd */
	if (control_mode && is_cmd(cmd, "ip", 0)) {
		char *ipstr = NULL;
		
		arg = get_argument(argstart, arglen);
		if (!arg) {
			tcp_srv_respond_term(sock, "Argument required!\r\n");
			return (0);
		}
		if (parse_ip_octet(arg, &ipstr) == -1) {
			tcp_srv_respond_term(sock, "Not a valid IP address!\r\n");
			return (0);
		}
		strcpy(flash_cfg.ipstr, ipstr);
		store_flash_config();
		sprintf(str_data, "New IP address '%s' active after reset!\r\n", ipstr);
		tcp_srv_respond_term(sock, str_data);
		return (0);
	}

	/* gw cmd */
	if (control_mode && is_cmd(cmd, "gw", 0)) {
		char *ipstr = NULL;
		
		arg = get_argument(argstart, arglen);
		if (!arg) {
			tcp_srv_respond_term(sock, "Argument required!\r\n");
			return (0);
		}
		if (parse_ip_octet(arg, &ipstr) == -1) {
			tcp_srv_respond_term(sock, "Not a valid IP address!\r\n");
			return (0);
		}
		strcpy(flash_cfg.gwstr, ipstr);
		store_flash_config();
		sprintf(str_data, "New GW IP address '%s' active after reset!\r\n", ipstr);
		tcp_srv_respond_term(sock, str_data);
		return (0);
	}

	/* nm cmd */
	if (control_mode && is_cmd(cmd, "nm", 0)) {
		char *ipstr = NULL;
		
		arg = get_argument(argstart, arglen);
		if (!arg) {
			tcp_srv_respond_term(sock, "Argument required!\r\n");
			return (0);
		}
		if (parse_ip_octet(arg, &ipstr) == -1) {
			tcp_srv_respond_term(sock, "Not a valid netmask!\r\n");
			return (0);
		}
		strcpy(flash_cfg.nmstr, ipstr);
		store_flash_config();
		sprintf(str_data, "New netmask '%s' active after reset!\r\n", ipstr);
		tcp_srv_respond_term(sock, str_data);
		return (0);
	}
	
	/* mac cmd */
	if (control_mode && is_cmd(cmd, "mac", 0)) {
		int res;
		bool error = false;
		uint8_t mac[6];
		
		arg = get_argument(argstart, arglen);
		if (!arg) {
			tcp_srv_respond_term(sock, "Argument required!\r\n");
			return (0);
		}
		if (strlen(arg) == 17) {
			for (i = 0; i < 6; i++) {
				char c = *(arg + i*3 + 2);
				if (i < 5 && !(c == '-' || c == ':')) {
					error = true;
					break;
				}
				*(arg + i*3 + 2) = 0;
				res = strtol(arg + i*3, NULL, 16);
				if (res < 0 || res > 255) {
					error = true;
					break;
				}
				mac[i] = res;
			}
		} else {
			error = true;
		}
		if (error) {
			tcp_srv_respond_term(sock, "Not a valid MAC address!\r\n"
			"(format: XX:XX:XX:XX:XX:XX)\r\n");
			return (0);
		}

		memcpy(flash_cfg.macaddr, mac, 6);
		store_flash_config();
		sprintf(str_data, "New MAC address is active after reset!\r\n");
		tcp_srv_respond_term(sock, str_data);
		
		return (0);
	}

	/* network status cmd */
	if (control_mode && is_cmd(cmd, "status", 0)) {
		
		strPtr = str_data;
		
		strPtr += sprintf(strPtr, "Port:        %d\r\n", current_app->tcp_port); 
		strPtr += sprintf(strPtr, "IP address:  %s ", tcp_srv_ip); 
		if (strcmp(flash_cfg.ipstr, tcp_srv_ip) != 0)
			strPtr += sprintf(strPtr, "(%s)\r\n", flash_cfg.ipstr);
		else
			strPtr += sprintf(strPtr, "\r\n");
		
		strPtr += sprintf(strPtr, "netmask:     %s ", tcp_srv_nm); 
		if (strcmp(flash_cfg.nmstr, tcp_srv_nm) != 0)
			strPtr += sprintf(strPtr, "(%s)\r\n", flash_cfg.nmstr);
		else
			strPtr += sprintf(strPtr, "\r\n");

		strPtr += sprintf(strPtr, "gateway:     %s ", tcp_srv_gw); 
		if (strcmp(flash_cfg.gwstr, tcp_srv_gw) != 0)
			strPtr += sprintf(strPtr, "(%s)\r\n", flash_cfg.gwstr);
		else
			strPtr += sprintf(strPtr, "\r\n");
		
		strPtr += sprintf(strPtr, "MAC address: %02x:%02x:%02x:%02x:%02x:%02x ",
		  ucMACAddress[0], ucMACAddress[1], ucMACAddress[2], ucMACAddress[3],
		  ucMACAddress[4], ucMACAddress[5]); 
		if (memcmp(flash_cfg.macaddr, ucMACAddress, 6) != 0)
			strPtr += sprintf(strPtr, "(%02x:%02x:%02x:%02x:%02x:%02x)\r\n",
			  flash_cfg.macaddr[0], flash_cfg.macaddr[1], flash_cfg.macaddr[2],
			  flash_cfg.macaddr[3], flash_cfg.macaddr[4], flash_cfg.macaddr[5]); 
		else
			strPtr += sprintf(strPtr, "\r\n");
		
		tcp_srv_respond_term(sock, str_data);

		return (0);
	}

	/* version cmd */
	if (control_mode && is_cmd(cmd, "version", 0)) {
		strPtr = str_data;
		strPtr += sprintf(strPtr, "BatMan:     %s\r\n", version);
		if (!strncmp(bl_shared->magic, "BOLO", 4)) {
			sprintf(strPtr, "Bootloader: %s\r\n", bl_shared->version);
		} else {
			sprintf(strPtr, "Bootloader: n/a\r\n");
		}
		tcp_srv_respond_term(sock, str_data);
		return (0);
	}

	/* send break cmd */
	if (serial_mode && is_cmd(cmd, "break", 2)) {
	
		SERIAL_CFG *scfg = current_app->ext_app_cfg;
		tcp_srv_respond_term(sock, "sending break ...\r\n");
		send_break(scfg->uart_num);
		return (0);
	}

	/* set uart speed */
	if (serial_mode && is_cmd(cmd, "baud", 2)) {
		int baud ;
		strPtr = str_data;
		
		arg = get_argument(argstart, arglen);
		if (!arg) {
			tcp_srv_respond_term(sock, "Argument required!\r\n");
			return (0);
		}
		baud = atoi(arg);
		if (baud <= 0) {
			sprintf(strPtr, "Invalid baud rate '%s'! Most be a positive "
			  "integer.\r\n\n", arg);
		} else if (set_uart_config(current_app->ext_app_cfg, baud, NULL,
		  strPtr, false) == 0) {
			sprintf(strPtr, "Setting baud rate to %d.\r\n\n", baud);

			if (flash_cfg.app_cfgs[current_app->id].baud != baud) {
				flash_cfg.app_cfgs[current_app->id].baud = baud;
				store_flash_config();
			}
		}
		tcp_srv_respond_term(sock, str_data);
		return (0);
	}

	/* set uart mode */
	if (serial_mode && is_cmd(cmd, "mode", 0)) {

		strPtr = str_data;
		arg = get_argument(argstart, arglen);
		
		if (!arg) {
			tcp_srv_respond_term(sock, "Argument required!\r\n");
			return (0);
		}
		
		if (set_uart_config(current_app->ext_app_cfg, -1, arg, strPtr, false) == 0) {
			sprintf(strPtr, "Setting mode to %c%c%c.\r\n\n",
			  arg[0], arg[1], arg[2]);

			if (strncmp(flash_cfg.app_cfgs[current_app->id].mode, arg, 3) != 0) {
				strncpy(flash_cfg.app_cfgs[current_app->id].mode, arg, 3);
				store_flash_config();
			}
		}
		tcp_srv_respond_term(sock, str_data);
		return (0);
	}

	/* serial cache mode */
	if (serial_mode && is_cmd(cmd, "cache", 0)) {
		uint8_t cache, oldflags;
		SERIAL_CFG *scfg = current_app->ext_app_cfg;

		strPtr = str_data;

		arg = get_argument(argstart, arglen);
		if (!arg) {
			tcp_srv_respond_term(sock, "Argument required!\r\n");
			return (0);
		}
		if (strlen(arg) > 1 || (*arg != '1' && *arg != '0')) {
			sprintf(strPtr, "Invalid cache mode '%s'! Most be 0 or 1.\r\n\n", arg);
		} else {
			sprintf(strPtr, "Setting cache mode to %c.\r\n\n", *arg);
			if (*arg == '0') {
				flash_cfg.app_cfgs[current_app->id].flags &= ~FLASH_CFG_SERIAL_CACHE_MODE;
				scfg->cache_mode = CACHE_TYPE_TIME;
			} else {
				flash_cfg.app_cfgs[current_app->id].flags |= FLASH_CFG_SERIAL_CACHE_MODE;
				scfg->cache_mode = CACHE_TYPE_NEWLINE;
			}
			store_flash_config();
		}
		tcp_srv_respond_term(sock, str_data);
		return (0);
	}

	/* uart status cmd */
	if (serial_mode && is_cmd(cmd, "status", 0)) {
		SERIAL_CFG *scfg = current_app->ext_app_cfg;
		strPtr = str_data;
		
		strPtr += sprintf(strPtr, "\r\nPort:           %d\r\n",
		  current_app->tcp_port);
		strPtr += sprintf(strPtr, "UART settings:  %lu %s\r\n",
		  scfg->baud, scfg->mode);
		if (scfg->cache_mode == CACHE_TYPE_TIME)
			strPtr += sprintf(strPtr, "Cache mode:     timed\r\n");
		else
			strPtr += sprintf(strPtr, "Cache mode:     newline\r\n");

		strPtr += sprintf(strPtr, "TX total bytes: %llu\r\n", scfg->tx_bytes);
		strPtr += sprintf(strPtr, "RX total bytes: %llu\r\n", scfg->rx_bytes);
		strPtr += sprintf(strPtr, "RX lost bytes:  %lu \r\n\n", scfg->rx_lost);

		tcp_srv_respond_term(sock, str_data);
		
		return (0);
	}

	/* battery status cmd */
	if (battery_mode && is_cmd(cmd, "status", 0)) {
		strPtr = str_data;
		
		strPtr += sprintf(strPtr, "\r\nPort:           %d\r\n\n",
		  current_app->tcp_port);

		tcp_srv_respond_term(sock, str_data);
		
		return (0);
	}

/*
	if (control_mode && len >= 4 && strncmp(srv_inbuf, "k", 1) == 0) {
		char *sp = srv_inbuf + 1;
		bool fspc = false;
		int client = 0;
		
		while (sp - srv_inbuf < len) {
			if (*(sp++) > ' ' && !fspc) continue;
			else fspc = true;
			if (*sp <= ' ') continue;
			// array index starts at 0, client list at 1
			client = atoi(sp) + 1;
			invcmd = false;
			break;
		}
		sprintf(strPtr, "Kill command currently not supported!\r\n\n");
		invcmd = true;
		if (!invcmd) {
			if (client < 2 || client > SOCK_COUNT+2) {
				sprintf(strPtr, "Invalid client number!\r\n\n");
			} else if (client == sock_idx) {
				sprintf(strPtr, "Use 'exit' to kill yourself!\r\n\n");
			} else if (!socks[client]) {
				sprintf(strPtr, "Client (%d) does not exist!\r\n\n", client-1);
			} else {
				sprintf(strPtr, "Killed client (%d)\r\n\n", client-1);
				t_socketclose(socks[client]);
				FD_CLR(socks[client], afds);
				socks[client] = 0;
			}
		}
	}
*/
	/* exit cmd */
	if (is_cmd(cmd, "exit", 0)) {
		ctlcfg->sockapp[sockidx] = 0;
		/* exiting control app means disconnect */
		if (current_app == app)	return (-1);
		return (0);
	}
	
	tcp_srv_respond_term(sock, "say what?\r\n");

	return (0);

}