#include "simple_daq.h"

// Send commands to socket
void cmd_send (char* Buffer, int Socket)
{
	int i;
	unsigned short CMD_Buffer[512];
	unsigned char CMD_Str[16];
	unsigned short CMD_Num = 1;
	unsigned int val, num;

	if (strncmp (Buffer, "r\n", 2) == 0)
	{
		CMD_Buffer[0] = htons (CMD_Start);
		printf ("# Start Run ->\n");
		printf ("# 0x%.4X\n", ntohs (CMD_Buffer[0]));
	}
	
	else if (strncmp (Buffer, "s\n", 2) == 0)
	{
		CMD_Buffer[0] = htons (CMD_Stop);
		printf ("# Stop Run ->\n");
		printf ("# 0x%.4X\n", ntohs (CMD_Buffer[0]));
	}

	else if (strncmp (Buffer, "sz\n", 3) == 0)
	{
		CMD_Buffer[0] = 0x0000;
		printf ("# Send 0x0000 ->\n");
		printf ("# 0x%.4X\n", ntohs (CMD_Buffer[0]));
	}

	else if (strncmp (Buffer, "t\n", 2) == 0)
	{
		CMD_Buffer[0] = htons (CMD_Trigger);
		printf ("# Trigger ->\n");
		printf ("# 0x%.4X\n", ntohs (CMD_Buffer[0]));
	}
	
    else if (strncmp (Buffer, "de\n", 2) == 0)
	{
		CMD_Buffer[0] = htons (CMD_DENABLE);
		printf ("# domino wave enabled ->\n");
		printf ("# 0x%.4X\n", ntohs (CMD_Buffer[0]));
	}
	
    else if (strncmp (Buffer, "dd\n", 2) == 0)
	{
		CMD_Buffer[0] = htons (CMD_DDISABLE);
		printf ("# domino wave disabled ->\n");
		printf ("# 0x%.4X\n", ntohs (CMD_Buffer[0]));
	}
	
    else if (strncmp (Buffer, "dr\n", 2) == 0)
	{
		CMD_Buffer[0] = htons (CMD_DWRITE_RUN);
		printf ("# DWRITE HIGH->\n");
		printf ("# 0x%.4X\n", ntohs (CMD_Buffer[0]));
	}
	
    else if (strncmp (Buffer, "ds\n", 2) == 0)
	{
		CMD_Buffer[0] = htons (CMD_DWRITE_STOP);
		printf ("# DWRITE LOW ->\n");
		printf ("# 0x%.4X\n", ntohs (CMD_Buffer[0]));
	}
	
	else if (strncmp (Buffer, "tc\n", 3) == 0)
	{
		CMD_Buffer[0] = htons (CMD_Trigger_C);
		printf ("# Continuous Trigger ->\n");
		printf ("# 0x%.4X\n", ntohs (CMD_Buffer[0]));
	}
	
	else if (strncmp (Buffer, "ts\n", 3) == 0)
	{
		CMD_Buffer[0] = htons (CMD_Trigger_S);
		printf ("# Stop Trigger ->\n");
		printf ("# 0x%.4X\n", ntohs (CMD_Buffer[0]));
	}
	
	// Set ROI value
	else if (strncmp (Buffer, "sr ", 3) == 0)
	{
		CMD_Num = 0;
		if (sscanf (Buffer, "%3s %i %i", CMD_Str, &num, &val) == 3)
		{
			if ((num >= 0) & (num <= MAX_ROINUM) & (val >= 0) & (val <= MAX_ROIVAL))
			{
				printf ("# Set ROI %d to %d ->\n", num, val);
				CMD_Buffer[0] = htons (CMD_Write | (BADDR_ROI + num));
				CMD_Buffer[1] = htons (val);
				printf ("# 0x%.4X 0x%.4X\n", ntohs (CMD_Buffer[0]), ntohs (CMD_Buffer[1]));
				CMD_Num = 2;
			}
			else
			{
				printf ("Out of range\n");
				printf ("Usage: sr NUM VAL: Set ROI NUM (0-%d) to VAL (0-%d)\n", MAX_ROINUM, MAX_ROIVAL);
			}
		}
		else
		{
			printf ("Usage: sr NUM VAL: Set ROI NUM (0-%d) to VAL (0-%d)\n", MAX_ROINUM, MAX_ROIVAL);
		}
	}
	
	// Set all ROIs
	else if (strncmp (Buffer, "sra ", 4) == 0)
	{
		CMD_Num = 0;
		if (sscanf (Buffer, "%4s %i", CMD_Str, &val) == 2)
		{
			if ((val >= 0) & (val <= MAX_ROIVAL))
			{
				printf ("# Set all ROIs to %d ->\n", val);
				for (i = 0; i < (MAX_ROINUM + 1); i++)
				{
					CMD_Buffer[i * 2] = htons (CMD_Write | (BADDR_ROI + i));
					CMD_Buffer[(i * 2) + 1] = htons (val);
					CMD_Num += 2;
				}
				for (i = 0; i < ((MAX_ROINUM + 1) * 2); i += 8)
				{
					printf ("# 0x%.4X 0x%.4X    0x%.4X 0x%.4X    0x%.4X 0x%.4X    0x%.4X 0x%.4X\n",
							ntohs (CMD_Buffer[i]), ntohs (CMD_Buffer[i + 1]), 
							ntohs (CMD_Buffer[i + 2]), ntohs (CMD_Buffer[i + 3]),
							ntohs (CMD_Buffer[i + 4]), ntohs (CMD_Buffer[i + 5]),
							ntohs (CMD_Buffer[i + 6]), ntohs (CMD_Buffer[i + 7]));
				}
			}
			else
			{
				printf ("Out of range\n");
				printf ("Usage: sra VAL: Set all ROIs to VAL (0-%d)\n", MAX_ROIVAL);
			}
		}
		else
		{
			printf ("Usage: sra VAL: Set all ROIs to VAL (0-%d)\n", MAX_ROIVAL);
		}
	}

	// Set DAC value
	else if (strncmp (Buffer, "sd ", 3) == 0)
	{
		CMD_Num = 0;
		if (sscanf (Buffer, "%3s %i %i", CMD_Str, &num, &val) == 3)
		{
			if ((num >= 0) & (num <= MAX_DACNUM) & (val >= 0) & (val <= MAX_DACVAL))
			{
				printf ("# Set DAC %d to %d ->\n", num, val);
				CMD_Buffer[0] = htons (CMD_Write | (BADDR_DAC + num));
				CMD_Buffer[1] = htons (val);
				printf ("# 0x%.4X 0x%.4X\n", ntohs (CMD_Buffer[0]), ntohs (CMD_Buffer[1]));
				CMD_Num = 2;
			}
			else
			{
				printf ("Out of range\n");
				printf ("Usage: sd NUM VAL: Set DAC NUM (0-%d) to VAL (0-%d)\n", MAX_DACNUM, MAX_DACVAL);
			}
		}
		else
		{
			printf ("Usage: sd NUM VAL: Set DAC NUM (0-%d) to VAL (0-%d)\n", MAX_DACNUM, MAX_DACVAL);
		}
	}

	// Set address to value
	else if (strncmp (Buffer, "sa ", 3) == 0)
	{
		CMD_Num = 0;
		if (sscanf (Buffer, "%4s %i %i", CMD_Str, &num, &val) == 3)
		{
			if ((num >= 0) & (num <= MAX_ADDR) & (val >= 0) & (val <= MAX_VAL))
			{
				printf ("# Set ADDR %d to %d ->\n", num, val);
				CMD_Buffer[0] = htons (CMD_Write | num);
				CMD_Buffer[1] = htons (val);
				printf ("# 0x%.4X 0x%.4X\n", ntohs (CMD_Buffer[0]), ntohs (CMD_Buffer[1]));
				CMD_Num = 2;
			}
			else
			{
				printf ("Out of range\n");
				printf ("Usage: sa NUM VAL: Set addr NUM (0-%d) to VAL (0-%d)\n", MAX_ADDR, MAX_VAL);
			}
		}
		else
		{
			printf ("Usage: raw NUM VAL: Set addr NUM (0-%d) to VAL (0-%d)\n", MAX_ADDR, MAX_VAL);
		}
	}

	// Send value
	else if (strncmp (Buffer, "sv ", 3) == 0)
	{
		CMD_Num = 0;
		if (sscanf (Buffer, "%4s %i", CMD_Str, &val) == 2)
		{
			if ((val >= 0) & (val <= MAX_VAL))
			{
				printf ("# Send %d ->\n", val);
				CMD_Buffer[0] = htons (val);
				printf ("# 0x%.4X\n", ntohs (CMD_Buffer[0]));
				CMD_Num = 1;
			}
			else
			{
				printf ("Out of range\n");
				printf ("Usage: sv VAL: Send VAL (0-%d)\n", MAX_VAL);
			}
		}
		else
		{
			printf ("Usage: sv VAL: Send VAL (0-%d)\n", MAX_VAL);
		}
	}

	else if (strncmp (Buffer, "q\n", 2) == 0)
	{
		// Exit program
		printf ("# Quit\n");
		exit_program (EXIT_SUCCESS);
	}
	
	// Anything else... , print help
	else
	{
		printf ("Commands:\n");
		printf ("    r:          Start Run -- TODO\n");
		printf ("    s:          Stop Run  -- TODO\n");
		printf ("    t:          Single Trigger\n\n");
		printf ("    de:         DENABLE HIGH\n");
		printf ("    dd:         DENABLE LOW\n");
		printf ("    dr:         DWRITE HIGH\n");
		printf ("    ds:         DWRITE LOW\n\n");
		printf ("    tc:         Continuous Trigger\n");
		printf ("    ts:         Stop Trigger\n");
		printf ("    sr NUM VAL: Set ROI NUM (0-35) to VAL (0-1024)\n");
		printf ("    sra VAL:    Set all ROIs to VAL (0-1024)\n");
		printf ("    sd NUM VAL: Set DAC NUM (0-7) to VAL (0-65535)\n");
		printf ("    q:          Quit\n");
		printf ("\n");
		printf ("    Only for debugging:\n");
		printf ("        sa NUM VAL: Set addr NUM (0-255) to VAL (0-65535)\n");
		printf ("        sv VAL:     Send VAL\n");
		printf ("        sz:         Send 0x0000\n");
		printf ("\n");

		CMD_Num = 0;
	}
	
	// Send commands
	if (CMD_Num > 0)
	{
		if (write (Socket, CMD_Buffer, CMD_Num * sizeof (short)) < 1)
		{
			printf ("Error: could not write to socket\n");
		}
	}
}