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

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

#define SIZE_BUFFER 4096
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 < 3) {
		printf("test-fc15 server port register values.[01010101]..\n");
		printf("test-fc15 localhost 502 16 1111100000001010000\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 > 4) {
		regstart = atoi (argv[3]);
		regcnt = strlen(argv[4]);

		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(4 + regcnt/8);
		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 = 15;
		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);
		memcpy (buffer+pos, &ui16, sizeof(ui16));
		pos += sizeof(ui16);

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

		buffer[pos] = 0;
		for (i = 0; i < regcnt; i++) {
			if (i > 0 && i % 8 == 0) {
				pos++;
				buffer[pos] = 0;
			}
			if (argv[4][i] == '1')	buffer[pos] |= (1 << i%8);
		}
		pos++;

		tcp.Write((char*)buffer, pos);
	}
	tcp.Close();

	return 0;
};