#include "modelbahn.h"
#include "interface.h"
#include "interface-z21.h"

//
// command and datasets for communication
static unsigned char TX_X_SET_TRACK_POWER_ON[] { 0x07, 0x00, 0x40, 0x00, 0x21, 0x81, 0xa0 };
static unsigned char TX_X_SET_TRACK_POWER_OFF[] { 0x07, 0x00, 0x40, 0x00, 0x21, 0x80, 0xa1 };

//
// values to comapare for data
static unsigned char RX_SYSTEMSTATE_DATACHANGED[] { 0x14, 0x00, 0x84, 0x00 };
static unsigned char RX_GET_SERIAL_NUMBER[] { 0x08, 0x00, 0x10, 0x00 };
static unsigned char RX_LOCONET_Z21_TX[] { 0x00, 0xa1, 0x00 };	// loconet dynamic length
static unsigned char RX_X_GET_TURNOUT_INFO[] { 0x09, 0x00, 0x40, 0x00, 0x43 };
static unsigned char RX_RMBUS_DATACHANGED[] { 0x0F, 0x00, 0x80, 0x00 };

//
// locomotive values
#define Z21_IDX_SETLOCO_DRIVE_SFMT   5
#define Z21_IDX_SETLOCO_DRIVE_ADRH   6
#define Z21_IDX_SETLOCO_DRIVE_ADRL   7
#define Z21_IDX_SETLOCO_DRIVE_STEP   8
#define Z21_IDX_SETLOCO_DRIVE_CHK    9

//
// turnout values
#define Z21_IDX_SETTURNOUT_ADRH   5
#define Z21_IDX_SETTURNOUT_ADRL   6
#define Z21_IDX_SETTURNOUT_MODE   7
#define Z21_IDX_SETTURNOUT_CHK    8

InterfaceZ21::InterfaceZ21 () {
	status_connected = false;
	status_poweron = false;
	status_programmingmode = false;
	status_shortcircuit = false;
	status_emergencystop = false;

	send_logon = false;

	memset (rmsensors, 0x0, INTF_Z21_RMSENSOR_GROUPS * INTF_Z21_RMSENSOR_BYTES);
	rmsensorinit = 0;

	serial = "";
	hostname = "";
	timeout = time(NULL);
	rmgetdatatimeout = time(NULL) - INTF_Z21_RMGETDATA_TIMEOUT;
};

InterfaceZ21::~InterfaceZ21() {
	Disconnect();
};


void InterfaceZ21::Connect (string destination) {
	debug (DEBUG_INFO | DEBUG_IFACE, "%s:%d Connect to: %s", __FILE__, __LINE__, destination.c_str());

	if (status_connected) Disconnect();
	hostname = destination;

	if (udp.Listen(0) == 0) {
		debug (DEBUG_ERROR | DEBUG_IFACE, "%s:%d Error could not bind UDP socket (%s)",
				__FILE__, __LINE__, strerror(errno));
		return;
	}
	udp.SetBlocked(1);
	timeout = time(NULL);
	send_SET_BROADCASTFLAGS();
	send_GET_SERIAL_NUMBER();
	status_connected = true;

	for (int i = 0; i < INTF_Z21_LOCONET_MAXADDR; i++) {
		loconet_map[i] = { 0 , 0, 0, 0, 0};
	}
};



//
// if connected, poweroff track and logoff from Z21 controller
void InterfaceZ21::Disconnect() {
	if (status_connected) {
		PowerOnOff(0);
		send_LOGOFF();
	}
	udp.Close();

	status_connected = false;
	status_poweron = false;
};

///////////////////////////////////////////////////////////////////////
//
//
// check if we are connected and for timeouts.
// read and go through incoming data
int InterfaceZ21::Loop(string interfacename) {
	time_t curtime = time(NULL);
	string source;
	int inlen;
	int i;
	int update = false;

	if (status_connected) {
		//
		// Z21 timed out? close all connection
		if ((timeout + 2 * INTF_Z21_TIMEOUT) < curtime) {
			debug (DEBUG_ERROR | DEBUG_IFACE, "%s:%d connection timed out (%d)", __FILE__, __LINE__, curtime-(timeout + 2 * INTF_Z21_TIMEOUT));
			Disconnect();
			update = true;
		}
		else if (send_logon == false && (timeout + INTF_Z21_TIMEOUT) < curtime) {
			send_logon = true;
			send_GET_SERIAL_NUMBER();
			send_SET_BROADCASTFLAGS();
			timeout = time(NULL);
			update = true;
		}

		//
		// rmgetdatatimeout?
		if (curtime - rmgetdatatimeout > INTF_Z21_RMGETDATA_TIMEOUT) {
			rmgetdatatimeout = curtime;
			send_RM_GETDATA(0);
			send_RM_GETDATA(1);
		}
	}


	//
	// check for incoming data
	if (status_connected) {
		if ((inlen = udp.ReadTimeout(&source, inbuffer, INTF_Z21_INBUFFER, 10)) > 0) {
			//
			// got some data

			//
			// check for LAN_SYSTEMSTATE_DATACHANGED
			if (memcmp (inbuffer, RX_GET_SERIAL_NUMBER, sizeof(RX_GET_SERIAL_NUMBER)) == 0) {
				// got serial number... ignore this for now
				send_logon = false;
			}

			//
			// check for LAN_X_TURNOUT_INFO
			if (memcmp (inbuffer, RX_X_GET_TURNOUT_INFO, sizeof(RX_X_GET_TURNOUT_INFO)) == 0) {
				// got turnout information
				int addr = 0;

				addr = ((unsigned char)inbuffer[Z21_IDX_SETTURNOUT_ADRH] << 8) + (unsigned char)inbuffer[Z21_IDX_SETTURNOUT_ADRL];

				if (inbuffer[Z21_IDX_SETTURNOUT_MODE] == 2)
					server->TurnoutAddrMode(interfacename, addr, 1);
				else if (inbuffer[Z21_IDX_SETTURNOUT_MODE] == 1)
					server->TurnoutAddrMode(interfacename, addr, 0);
			}



			//
			// check for LAN_SYSTEMSTATE_DATACHANGED
			else if (memcmp (inbuffer, RX_SYSTEMSTATE_DATACHANGED, sizeof(RX_SYSTEMSTATE_DATACHANGED)) == 0) {
				int cs = (unsigned char) inbuffer[16];
				int csex = (unsigned char) inbuffer[17];

				bool old_poweron = status_poweron;
				bool old_shortcircuit = status_shortcircuit;
				bool old_programmingmode = status_programmingmode;
				bool old_connected = status_connected;
				bool old_emergencystop = status_emergencystop;

				status_emergencystop = (cs & INTF_Z21_CS_EmergencyStop);
				status_poweron = (cs & INTF_Z21_CS_TrackVoltageOff);
				status_shortcircuit = (cs & INTF_Z21_CS_ShortCircuit);
				status_programmingmode = (cs & INTF_Z21_CS_ProgModeActive);

				debug (0, "%s:%d cs:%d csex:%d", __FILE__, __LINE__, cs, csex);

				update = true;
				//if (	old_poweron != status_poweron ||
				//		old_shortcircuit != status_shortcircuit ||
				//		old_programmingmode != status_programmingmode ||
				//		old_emergencystop != status_emergencystop) update = true;
			}

			//
			// LOCONET
			//Got some Data:08:00:a1:00:a1:02:00:5c:
			//Got some Data:08:00:a1:00:a0:02:55:08:
			else if (memcmp (inbuffer+1, RX_LOCONET_Z21_TX, sizeof (RX_LOCONET_Z21_TX)) == 0) {
				int loconetopc = (unsigned char)inbuffer[4];
				printf ("%s:%d Got some LOCONET (%x) Data:", __FILE__, __LINE__, loconetopc);
				for (i = 0; i < inlen; i++) {
					int z = (unsigned char) inbuffer[i];
					printf ("i:%d %02x:", i, z);
				}
				printf ("\n");

				//
				//
				if (loconetopc == 0xa0) {
					//
					// speed
					unsigned int inloc = inbuffer[5];
					unsigned int inspeed = inbuffer[6];

					if (inloc >= 0 && inloc <= INTF_Z21_LOCONET_MAXADDR) {
						if (loconet_last.addr) {
							debug (0, "%s:%d loconet_lastaddr:%d", __FILE__, __LINE__, loconet_last.addr);
							loconet_map[inloc].addr = loconet_last.addr;
						}

						debug (0, "%s:%d Loc:%d  Speed:%d\n", __FILE__, __LINE__, loconet_map[inloc].addr, inspeed);
						if (loconet_map[inloc].addr > 0) {
							int speed = inspeed;
							server->LocomotiveAddrSpeed(interfacename, loconet_map[inloc].addr, speed);
						}
					}
					loconet_last.addr = 0;
				}

				if (loconetopc == 0xa1) {
					//
					// speed
					unsigned int inloc = inbuffer[5];
					unsigned int infunc = inbuffer[6];

					if (inloc >= 0 && inloc <= INTF_Z21_LOCONET_MAXADDR) {
						if (loconet_last.addr) loconet_map[inloc] = loconet_last;
						debug (0, "%s:%d Loc:%d  Function:%d\n", __FILE__, __LINE__, loconet_map[inloc].addr, infunc);
					}
					loconet_last.addr = 0;
				}
			}

			//
			// check for LAM_RMBUS_DATACHANGED
			else if (memcmp (inbuffer, RX_RMBUS_DATACHANGED, sizeof(RX_RMBUS_DATACHANGED)) == 0) {
				int group = (unsigned char)inbuffer[4];
				int idx, bit;

				printf ("LAN_RMBUS_DATA_CHANGE[%s] ", interfacename.c_str());
				for (idx = 0; idx < INTF_Z21_RMSENSOR_BYTES * INTF_Z21_RMSENSOR_GROUPS; idx++) {
					printf ("%x ", (unsigned char) inbuffer[4+idx]);
				}
				printf ("\n");

				// groupindex (first or last 10 bytes)
				unsigned char rmsold[INTF_Z21_RMSENSOR_BYTES * INTF_Z21_RMSENSOR_GROUPS];
				memcpy (rmsold, rmsensors, INTF_Z21_RMSENSOR_BYTES * INTF_Z21_RMSENSOR_GROUPS);

				if (group < INTF_Z21_RMSENSOR_GROUPS) {
					memcpy (rmsensors+(group * INTF_Z21_RMSENSOR_BYTES), inbuffer+5, INTF_Z21_RMSENSOR_BYTES);

					// check for changes
					for (idx = 0; idx < INTF_Z21_RMSENSOR_GROUPS*INTF_Z21_RMSENSOR_BYTES; idx++) {
						if (rmsold[idx]^rmsensors[idx]) {
							for (i = 0; i < 8; i++)	if (rmsensorinit || (rmsold[idx] & 1 << i) != (rmsensors[idx] & 1 << i)) {
									debug (0, "Sendor Data Changed: %s[%d]", interfacename.c_str(), idx*8+i);
									if (rmsensors[idx] & 1 << i)	server->SensorAddrChange(interfacename, idx*8+i, 1);
									else 							server->SensorAddrChange(interfacename, idx*8+i, 0);
								}
						}
					}
					rmsensorinit = 0;
				}
			}

			else {
				printf ("InterfaceZ21(%s) Got some Data:", interfacename.c_str());
				for (i = 0; i < inlen; i++) {
					int z = (unsigned char) inbuffer[i];
					printf ("%02x:", z);
				}
				printf ("\n");
			}
		}
		else if (inlen < 0) {
			if (errno != EAGAIN && errno != EWOULDBLOCK) {
				debug (DEBUG_ERROR | DEBUG_IFACE, "%s:%d error on reading (%s)",
						__FILE__, __LINE__, strerror(errno));
			}
		}
	}

	return update;
};


//
// send_SET_BROADCASTFLAGS();
void InterfaceZ21::send_SET_BROADCASTFLAGS() {
	unsigned char buffer[] = { 0x08, 0x00, 0x50, 0x00, 0x0F, 0x01, 0x00, 0x03 };
	udp.Write(hostname, (char*)buffer, sizeof (buffer));
}

//
// send_GET_SERIAL_NUMBER
void InterfaceZ21::send_GET_SERIAL_NUMBER() {
	unsigned char buffer[] = { 0x04, 0x00, 0x10, 0x00 };
	udp.Write(hostname, (char*)buffer, sizeof (buffer));
}


//
// send_RM_GETDATA
void InterfaceZ21::send_RM_GETDATA(int group) {
	unsigned char buffer[] = { 0x05, 0x00, 0x81, 0x00, 0x00 };
	if (group < INTF_Z21_RMSENSOR_GROUPS) {
		buffer[4] = (unsigned char)group;
		udp.Write(hostname, (char*)buffer, sizeof (buffer));
	}
}


//
// send_LOGOFF
void InterfaceZ21::send_LOGOFF() {
	if (status_connected == false) return;

	unsigned char buffer[] = { 0x04, 0x00, 0x30, 0x00 };
	udp.Write(hostname, (char*)buffer, sizeof (buffer));
}


//
// Set Locomotive Speed
void InterfaceZ21::SetLocoSpeed(Locomotive *l, int step) {
	unsigned char buffer[] = { 0x0A, 0x00, 0x40, 0x00, 0xE4, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00 };

	debug (0, "%s:%d InterfaceZ21::SetLocoSpeed step:%d", __FILE__, __LINE__, step);

	//
	// reverse?
	if (step < 0) {
		l->flags |= LOCO_F_REVERSE;
	}
	else if (step > 0) {
		l->flags &= ~LOCO_F_REVERSE;
	}
	if (l->flags & LOCO_F_REVERSE) buffer[Z21_IDX_SETLOCO_DRIVE_STEP] = 0x80;
	else buffer[Z21_IDX_SETLOCO_DRIVE_STEP] = 0x00;

	//
	// setup loc addr
	if (l->addr >= 128) buffer[Z21_IDX_SETLOCO_DRIVE_ADRH] = 0xC0;
	else buffer[Z21_IDX_SETLOCO_DRIVE_ADRH] = 0;
	buffer[Z21_IDX_SETLOCO_DRIVE_ADRL] = (unsigned char) (l->addr & 0xFF);
	buffer[Z21_IDX_SETLOCO_DRIVE_ADRH] |= (unsigned char) (l->addr >> 8);

	//
	// setup steps and selected step
	if (l->steps <= 14) {
		buffer[Z21_IDX_SETLOCO_DRIVE_SFMT] = 0x10;
		if (step == 0) 	buffer[Z21_IDX_SETLOCO_DRIVE_STEP] |= 0;
		else			buffer[Z21_IDX_SETLOCO_DRIVE_STEP] |= (abs(step)+1);
	}
	else if (l->steps <= 28) {
		buffer[Z21_IDX_SETLOCO_DRIVE_SFMT] = 0x12;
		if (step == 0) 	buffer[Z21_IDX_SETLOCO_DRIVE_STEP] |= 0;
		else {
			buffer[Z21_IDX_SETLOCO_DRIVE_STEP] |= ((abs(step)>>1)+1);
			if (abs(step) & 0x01) buffer[Z21_IDX_SETLOCO_DRIVE_STEP] |= 0x10;
		}
	}
	else if (l->steps <= 128) {
		buffer[Z21_IDX_SETLOCO_DRIVE_SFMT] = 0x13;
		if (step == 0) 	buffer[Z21_IDX_SETLOCO_DRIVE_STEP] |= 0;
		else			buffer[Z21_IDX_SETLOCO_DRIVE_STEP] |= (abs(step)+1);
	}

	//
	// XOR Byte
	for (int i = 4; i < sizeof (buffer)-1; i++)
		buffer[Z21_IDX_SETLOCO_DRIVE_CHK] = (unsigned char)buffer[Z21_IDX_SETLOCO_DRIVE_CHK] xor (unsigned char)buffer[i];

	printf ("Send Data:");
	for (int i = 0; i < sizeof (buffer); i++) {
		int z = (unsigned char) buffer[i];
		printf ("%02x:", z);
	}
	printf ("\n");
	udp.Write(hostname, (char*)buffer, sizeof (buffer));
	loconet_last.addr = l->addr;
	loconet_last.dir = l->flags & LOCO_F_REVERSE;
	loconet_last.speed = l->speed;
	loconet_last.maxspeed = l->vmax;
	loconet_last.steps = l->steps;
};


//
// Set Locomotive Function
void InterfaceZ21::SetLocoFunction(Locomotive *l, int func, int value) {
	debug (0, "%s:%d InterfaceZ21::SetLocoFunction", __FILE__, __LINE__);
};


//
// Set Turnout
void InterfaceZ21::SetTurnout(Turnout *t, int activate, int motoractive) {
	unsigned char buffer[] = { 0x09, 0x00, 0x40, 0x00, 0x53, 0x00, 0x00, 0x00, 0x00 };

	debug (0, "%s:%d InterfaceZ21::SetTurnout (a:%d, m:%d)", __FILE__, __LINE__, activate, motoractive);

	//
	// setup turnout addr
	buffer[Z21_IDX_SETTURNOUT_ADRL] = (unsigned char) (t->addr & 0xFF);
	buffer[Z21_IDX_SETTURNOUT_ADRH] |= (unsigned char) (t->addr >> 8);

	//
	// setup steps and selected step
	buffer[Z21_IDX_SETTURNOUT_MODE] = 0x80;
	if (activate)  		buffer[Z21_IDX_SETTURNOUT_MODE] |= 1;
	if (motoractive)  	buffer[Z21_IDX_SETTURNOUT_MODE] |= 8;

	//
	// XOR Byte
	for (int i = 4; i < sizeof (buffer)-1; i++)
		buffer[Z21_IDX_SETTURNOUT_CHK] = (unsigned char)buffer[Z21_IDX_SETTURNOUT_CHK] xor (unsigned char)buffer[i];

	printf ("Send Data:");
	for (int i = 0; i < sizeof (buffer); i++) {
		int z = (unsigned char) buffer[i];
		printf ("%02x:", z);
	}
	printf ("\n");
	udp.Write(hostname, (char*)buffer, sizeof (buffer));
};


//
// power on off
// send_X_SET_TRACK_POWER_ON();   send_X_SET_TRACK_POWER_ON();
void InterfaceZ21::PowerOnOff(int onoff) {
	if (status_connected == false) return;

	if (onoff) {
		udp.Write(hostname, (char*)TX_X_SET_TRACK_POWER_ON, sizeof (TX_X_SET_TRACK_POWER_ON));
	}
	else {
		udp.Write(hostname, (char*)TX_X_SET_TRACK_POWER_OFF, sizeof (TX_X_SET_TRACK_POWER_OFF));
	}
};