///////////////////////////////////////////////////////////////////////////////// 
//
// test-fc16.cc is part of TestModbus-Server.
//
///////////////////////////////////////////////////////////////////////////////// 

#include "tcp.h"
#include <stdio.h>
#include <stdlib.h>

#define SIZE_BUFFER 4096

void swapbytes (unsigned char *p1, unsigned char *p2) {
	unsigned char tmp;
	tmp = *p2;
	*p2 = *p1;
	*p1 = tmp;
}

void swapwords (unsigned char *p1, unsigned char *p2) {
	uint16_t tmp;

	memcpy (&tmp, p2, 2);
	memcpy (p2, p1, 2);
	memcpy (p1, &tmp, 2);
}


int main (int argc, char **argv) {
	unsigned char data[] = {	0x00,0x70,0x00,0x00 , 0x00,0x0B,0x01,0x10 ,
								0x00,0x04,0x00,0x02 , 0x04,0xC0,0x02,0x7B ,
								0xA5 };

	char buffer[SIZE_BUFFER];
	int pos;
	TCP tcp;
	int port;
	int i,c;
	int regstart;
	int regcnt;
	uint8_t ui8;
	uint16_t ui16;
	float f;

	memset (buffer, 0x00, SIZE_BUFFER);

	if (argc < 6) {
		printf ("%d\n", argc);
		printf("test-fc16 server port register NOSWAP|WSWAP|BSWAP|BWSWAP values...\n");
		printf("test-fc16 localhost 502 16 NOSWAP 25.0 0.54 1000.5\n");
		printf (" NOSWAP          no swapping words\n");
		printf (" WordSWAP        swap the 16bit words\n");
		printf (" ByteSWAP        swap the Bytes\n");
		printf (" WordByteSWAP    swap the bytes and the words\n");
		return 0;
	}

	port = atoi (argv[2]);
	printf ("%d connecting to: %s:%d\n", argc, argv[1], port);
	if (tcp.Connect(argv[1], port) != 1) {
		printf ("could not connect.\n");
		return -1;
	}

	if (argc == 3) {
		tcp.Write((char*)data, sizeof(data));
	}
	else if (argc > 5) {
		regstart = atoi (argv[3]);
		regcnt = argc-5;

		printf ("writing %d floats, offset %d\n", regcnt, regstart);

		// construct modbus protocol
		pos = 0;

		// transact id
		ui16 = 0;
		memcpy (buffer+pos, &ui16, sizeof(ui16));
		pos += sizeof(ui16);

		// proitocol id
		ui16 = 0;
		memcpy (buffer+pos, &ui16, sizeof(ui16));
		pos += sizeof(ui16);

		// lenght
		ui16 = htons(3 + regcnt*4);
		memcpy (buffer+pos, &ui16, sizeof(ui16));
		pos += sizeof(ui16);

		// device id
		ui8 = 1;
		memcpy (buffer+pos, &ui8, sizeof(ui8));
		pos += sizeof(ui8);

		// function code
		ui8 = 16;
		memcpy (buffer+pos, &ui8, sizeof(ui8));
		pos += sizeof(ui8);

		// starting address
		ui16 = htons((uint16_t)regstart);
		memcpy (buffer+pos, &ui16, sizeof(ui16));
		pos += sizeof(ui16);

		// number of registers
		ui16 = htons((uint16_t)regcnt*2);
		memcpy (buffer+pos, &ui16, sizeof(ui16));
		pos += sizeof(ui16);

		// number of bytes
		ui8 = regcnt*4;
		memcpy (buffer+pos, &ui8, sizeof(ui8));
		pos += sizeof(ui8);

		for (i = 0; i < regcnt; i++) {
			f = (float) atof(argv[i+5]);
			printf ("i: %d  string:%s   f:%f\n", i, argv[i+5], f);

			memcpy (&ui16, &f, sizeof(ui16));
			ui16 = htons(ui16);
			memcpy (buffer+pos, &ui16, sizeof(ui16));
			pos += sizeof(ui16);

			memcpy (&ui16, ((char*)&f +2), sizeof(ui16));
			ui16 = htons(ui16);
			memcpy (buffer+pos, &ui16, sizeof(ui16));
			pos += sizeof(ui16);

			if (strstr (argv[4], "Word") != NULL) swapwords((unsigned char*)buffer+pos-4, (unsigned char*)buffer+pos-2);
			if (strstr (argv[4], "Byte") != NULL) {
				swapbytes((unsigned char*)buffer+pos-4, (unsigned char*)buffer+pos-3);
				swapbytes((unsigned char*)buffer+pos-2, (unsigned char*)buffer+pos-1);
			}
		}

		printf ("Wrote %d (out of %d) bytes\n", tcp.Write((char*)buffer, pos), pos);
	}

	pos = tcp.ReadTimeout(buffer, SIZE_BUFFER, 1000);
	printf ("Got %d bytes.\n", pos);

	tcp.Close();

	return 0;
};