/* EthernetLink and Ethernet strategies contributed by Fred Larsen

  This EthernetLink class transfers packets of data over
  TCP/IP Ethernet connections.

  It may accept incoming connections and send ACK back for each incoming
  package, it may connect to another object in another device and deliver a
  packet and get an ACK back, or it may do both -- connect to another device
  for delivering packages and accept incoming connections and packages for
  bidirectional transfer.

  When establishing connections, the target must be registered with the add_node
  function for setting the IP and port along with a unique id that is used for
  referring to the target in all other functions.

  An EthernetLink may work in multiple ways when bidirectional:

  1. One-to-one connection with another EthernetLink. In this mode, using the
     keep_connection is recommended because it reduces network activity and
     speeds things up dramatically. In this mode, there are three options:

    A. When running on a firewall-free network one socket can be created in each
       direction, allowing for bidirectional "full-duplex" (as far as that goes
       for single-threaded code), with data being buffered in each direction. It
       requires each side to know about each other's address, but delivers
       higher throughput than a single_socket approach. An EthernetLink object
       in this mode has one node added by add_node, and both sides calls
       start_listening to accept incoming connections. If there are multiple
       objects on one device, each has to listen to a unique port number
       (specified in the start_listening function call).

    B. Set single_socket to mave a master-slave scenario where one side, the
       initiator, creates a socket connection and sends any packages it may
       have and then receives any package waiting to be sendt in the opposite
       direction. This requires the initiator to know where to connect, but not
       the other way around. This is suitable for connections through the
       Internet because a firewall opening only has to be opened in one place.
       An EthernetLink object in this mode has one node added by add_node
       (the initiator side), or calls the start_listening function to start
       listening (the receiver side). If there are multiple receiving objects
       on one device, each has to listen to a unique port number
       (specified in the start_listening function call).

    C. Set single_initiate_direction to have one device create two sockets to
       the other device, using one for each packet direction. This is a variant
       of B) that is firewall friendly and more efficient, but uses one more
       socket, and cannot be run on the simplest Ethernet cards with only one
       available socket. This mode has 3-4 times the speed of B) and does not
       do polling so it will not generate network traffic when idle.

  2. If running a one-to-many or many-to-many scenario the keep_connection
     should be deactivated because then one incoming socket would block others
     from connecting. Not using keep_connection drops the transfer speed some, though.

    A. When running on a firewall-free network each packet is delivered through
       a dedicated connection created for delivering that package. After
       delivery and ACK the socket is closed. All devices can send to and
       receive from each other.

    B. Using the single_socket approach will use a master-slave like approach
       where only the initiators need to know the address of the receiver(s),
       for easier firewall traversal. After the two-way transfer of packets on
       the same connection, the connection will be closed so that the receiver
       is ready for another connection. This requires only _one firewall
       opening_ for bidirectional transfer with 255 others.

  Limitations for the different modes when using the W5100 based Ethernet shield
  with a 4 socket limit:

  1A. Two EthernetLink objects can be used for bidirectional communication
      with two sites.

  1B. Four EthernetLink objects can be used for bidirectional communication
      with four sites.

  2A. As the sockets are only used temporarily each object can accept
      connections from 255 others, and deliver packets to 255 others. Up to
      two objects can be used in one device, if multiple listening ports are
      needed. Each object can have 1 to 255 nodes added by add_node, and can
      deliver to all. There can be up to three objects in each device if needed.
      One socket must remain free for outgoing connections.
  2B. As 2A, each receiver can receive packets from and send packets to 255
      initiators, using a _single listening port_. There can be four receiver
      objects in on device, or three receiver objects and an unlimited number
      of initiator objects that will be able to send to and receive from to up
      to 255 sites each.

  Limitations for the different modes when using the ENC28J60 based Ethernet
  shield with a 1 socket limit:

  1A. Not available.
  1B. One EthernetLink object can be used for fast bidirectional communication
      with one site.
  1C. Not available.
  2A. Not available.
  2B. One receiver can receive from and deliver packets to unlimited initiators,
      _or_ unlimited initator objects can send to and receive from to up to
      255 sites each

  Note: To use the ENC28J60 shield instead of the WIZ5100 shield, include
  <UIPEthernet.h> before EthernetLink.h.

  PJON and EthernetLink
  The EthernetLink can be used standalone for simple delivery between two
  devices. But the EthernetLink is also used as a tool by the EthernetTCP strategy
  of PJON. A site consisting of one or more buses of devices communicating
  through PJON, wired and/or wirelessly, can be connected seamlessly with
  multiple other similar sites in other places of the world, communicating
  through Internet with minimal firewall configuration.

  NOTE: The W5100 full-size Ethernet shields come in multiple variants. If you
        get a problem where the card is not starting after plugging in the
        power, check if there is small resistor network behind the Ethernet
        outlet, with one resistor having printed "511" on it. If so, try
        another brand. Even startup delays do not fix this, but it can be solved
        with a capacitor+resistor, search for it.

  NOTE: If needing single_socket functionality with ACK (polling mode),
        define ETCP_SINGLE_SOCKET_WITH_ACK. The program size has been reduced by
        only including this when needed.

        The same goes for ETCP_SINGLE_DIRECTION. It is not included by default
        to reduce program size. Define this when needed.
        */

#pragma once

#ifdef ARDUINO
#include "../../interfaces/ARDUINO/TCPHelper_ARDUINO.h"
#else
#include "../../interfaces/LINUX/TCPHelper_POSIX.h"
#ifndef F
#define F(x) (x)
#endif
#define Serial DummyPrint
struct DummyPrint {
	static void print(const char *s)
	{
		printf("%s", s);
	}
	static void print(int n)
	{
		printf("%d", n);
	}
	static void println(const char *s)
	{
		printf("%s\n", s);
	}
	static void println(int n)
	{
		printf("%d\n", n);
	}
} DummyPrint;
#endif

// Constants
#ifndef PJON_ACK
#define PJON_ACK                           6
#endif

// Internal constants
#ifndef PJON_FAIL
#define PJON_FAIL                      0x100
#endif

#ifndef ETCP_MAX_REMOTE_NODES
#define ETCP_MAX_REMOTE_NODES             10
#endif

#define ETCP_DEFAULT_PORT                 7000

/* The maximum packet size to be transferred, this protects again buffer overflow.
   Set this to a size that is guaranteed to be available in RAM during runtime,
   depending on the hardware and software. */
#ifndef ETCP_MAX_PACKET_SIZE
#define ETCP_MAX_PACKET_SIZE             300
#endif


/* If an incoming packet has not arrived for some time, disconnect the socket so
   it will be reconnected on demand. The timeout is in ms.
   The reason for this is that in some cases an idle socket may have gotten
   disconnected without it being detected, unless trying to write to it.
   So we could be waiting for data that never arrives. */
#ifndef ETCP_IDLE_TIMEOUT
#define ETCP_IDLE_TIMEOUT 30000ul
#endif

// Magic number to verify that we are aligned with telegram start and end
#define ETCP_HEADER               0x18ABC427ul
#define ETCP_FOOTER               0x9ABE8873ul
#define ETCP_SINGLE_SOCKET_HEADER 0x4E92AC90ul
#define ETCP_SINGLE_SOCKET_FOOTER 0x7BB1E3F4ul
#define ETCP_CONNECTION_HEADER_A      0xFEDFED67ul  // Primary socket, packets in initiated direction
#define ETCP_CONNECTION_HEADER_A_ACK  0xFEDFED68ul  // Same, but request ACK for all packets
#define ETCP_CONNECTION_HEADER_B      0xFEDFED77ul  // Reverse socket, packets in reverse direction

/* UIPEthernet library used for the ENC28J60 based Ethernet shields has the
   correct return value from the read call, while the standard Ethernet library
   does not follow the standard! */
#if defined(ARDUINO) && !defined(UIPETHERNET_H)
#define ETCP_ERROR_READ                   0
#else
#define ETCP_ERROR_READ                  -1
#endif

typedef void (*link_receiver)(
    uint8_t id,
    const uint8_t *payload,
    uint16_t length,
    void *callback_object
);

typedef void (*link_error)(
    uint8_t code,
    uint8_t data
);

class TmpBuffer
{
	uint8_t *buf = NULL;
	uint16_t len = 0;
public:
	TmpBuffer(uint16_t size)
	{
		len = size;
		buf = new uint8_t[size];
	}
	~TmpBuffer()
	{
		if (buf) {
			delete buf;
		}
	}
	uint8_t* operator()()
	{
		return buf;
	}
	uint16_t size()
	{
		return len;
	}
};

class EthernetLink
{
private:
	// ********* Dynamic members ************

	TCPHelperServer *_server = NULL;

	// Connection for writing outgoing packets
	TCPHelperClient _client_out;

	// Connection for reading incoming packets
	TCPHelperClient _client_in;

	// The id of the remove device/node that we have connected to
	int16_t _current_device = -1;

	// When a socket is received, the connection header specifies if ACKs are wanted
	bool _ack_requested = false;

	// Remember the connection statistics
	uint32_t _connection_time = 0;
	uint32_t _connection_count = 0;
	uint32_t _last_receive_time = 0;

	// ********* Configuration ************

	link_receiver _receiver = NULL;
	link_error    _error = NULL;
	void *_callback_object = NULL;

	// Local node
	uint8_t  _local_id = 0;
	uint8_t  _local_ip[4];
	uint16_t _local_port = ETCP_DEFAULT_PORT;

	// Remote nodes
	uint8_t  _remote_node_count = 0;
	uint8_t  _remote_id[ETCP_MAX_REMOTE_NODES];
	uint8_t  _remote_ip[ETCP_MAX_REMOTE_NODES][4];
	uint16_t _remote_port[ETCP_MAX_REMOTE_NODES];

	// Keep sockets permanently open instead of reconnecting for each transfer
	bool _keep_connection = false;

	// Do bidirectional transfer on a single socket
	bool _single_socket = false;

	// Request an immediate ACK for eack packet delivery to ensure guaranteed delivery
	bool _request_ack = false;

	// To avoid deadlocks while connecting (if receiver tries to connect back simultanously),
	// receive and discard packets while doing non-blocking connect. This should resolve deadlocks,
	// and the missing ACK should make the packet being resent later.
	bool _receive_and_discard = false;

	// With single_socket = false, there is one socket for each packet direction.
	// Normally the sockets are initiated from the side sending the packet.
	// By setting initiate_both_sockets_in_same direction, both sockets can be
	// initiated from one of the devices, to simplify firewall setup, or for letting
	// only one of the devices have a static IP address.
	// This should only be used with _keep_connection = true, and is meant for permanent
	// one-to-one links.
	bool _initiate_both_sockets_in_same_direction = false;
	// Whether to be the side initiating both sockets or not
	bool _initiator = true;

public:

	void init()
	{
		memset(_local_ip, 0, 4);
	};


	int16_t find_remote_node(uint8_t id)
	{
		for(uint8_t i = 0; i < _remote_node_count; i++)
			if(_remote_id[i] == id) {
				return i;
			}
		return -1;
	};


	int16_t read_bytes(
	    TCPHelperClient &client,
	    uint8_t *contents,
	    uint16_t length,
	    uint16_t timeout_ms = 2000
	)
	{
		int32_t total_bytes_read = 0, bytes_read = ETCP_ERROR_READ;
		uint32_t start_ms = PJON_MILLIS();
		/* NOTE: The Arduino standard recv/read functions returns
		   -1 if no data waiting
		    0 if socket closed
		    This is the opposite of POSIX. */
		do {
#ifdef HAS_ETHERNETUDP // Avoid using blocking read until there is data present
			int16_t avail = 0;
			while(
			    client.connected() &&
			    (avail = client.available()) <= 0 &&
			    (uint32_t)(PJON_MILLIS() - start_ms) < min(1000, timeout_ms)
			) {
				PJON_DELAY_MICROSECONDS(250);
			}
			if (avail <= 0) {
				continue;
			}
#endif

			bytes_read = client.read(
			                 &contents[total_bytes_read],
			                 length - total_bytes_read
			             );

			if(bytes_read > 0) {
				total_bytes_read += bytes_read;
			}
		} while(
		    bytes_read != ETCP_ERROR_READ &&
		    total_bytes_read < length &&
		    (uint32_t)(PJON_MILLIS() - start_ms) < timeout_ms
		);

		if(bytes_read == ETCP_ERROR_READ) {
			stop(client); // Lost connection
#ifdef ETCP_ERROR_PRINT
			Serial.println(F("Read failed, closing cl"));
#endif
		}
		return total_bytes_read;
	};


	// Read a package from a connected client (incoming or outgoing socket) and send ACK
	uint16_t receive(TCPHelperClient &client, bool wait)
	{
		uint16_t return_value = PJON_FAIL;
		uint32_t start_ms = PJON_MILLIS(), avail;
		if (wait) {
			while(
			    client.connected() &&
			    (avail = client.available()) <= 0 &&
			    (uint32_t)(PJON_MILLIS() - start_ms) < 1000
			) {
				PJON_DELAY_MICROSECONDS(250);
			}
		} else {
			avail = client.connected() ? client.available() : 0;
		}

		if(avail > 0) {
#ifdef ETCP_DEBUG_PRINT
			Serial.println(F("Recv from cl"));
#endif

			// Locate and read encapsulation header (4 bytes magic number)
			bool ok = read_until_header(client, ETCP_HEADER);
#ifdef ETCP_DEBUG_PRINT
			Serial.print(F("Read header, stat "));
			Serial.println(ok);
#endif

			// Read sender device id (1 byte) and length of contents (4 bytes)
			int16_t bytes_read = 0;
			uint8_t sender_id = 0;
			uint32_t content_length = 0;
			if(ok) {
				uint8_t buf[5];
				bytes_read = read_bytes(client, buf, 5);
				if(bytes_read != 5) {
					ok = false;
				} else {
					memcpy(&sender_id, buf, 1);
					memcpy(&content_length, &buf[1], 4);
					content_length = ntohl(content_length);
					if(content_length == 0) {
						ok = 0;
					}
				}
			}

			// Protect against too large packets
			if (content_length > ETCP_MAX_PACKET_SIZE) {
				return PJON_FAIL;
			}

			// Read contents and footer
			TmpBuffer buf(content_length);
			if(ok) {
				bytes_read = read_bytes(client, (uint8_t*)buf(), content_length);
				if((uint32_t)bytes_read != content_length) {
					ok = false;
				}
			}

			// Read footer (4 bytes magic number)
			if(ok) {
				uint32_t foot = 0;
				bytes_read = read_bytes(client, (uint8_t*) &foot, 4);
				if(bytes_read != 4 || foot != htonl(ETCP_FOOTER)) {
					ok = false;
				}
			}

#ifdef ETCP_DEBUG_PRINT
			Serial.print(F("Stat rec: "));
			Serial.print(ok);
			Serial.print(" len: ");
			Serial.println(content_length);
#else
#if defined(ETCP_ERROR_PRINT)
			if (!ok) {
				Serial.print(F("FAIL rec: "));
				Serial.println(bytes_read);
			}
#endif
#endif

			return_value = ok ? PJON_ACK : PJON_FAIL;
			if (ok) {
				_last_receive_time = PJON_MILLIS();
			}
			if (_ack_requested && !_receive_and_discard) {
				// Write PJON_ACK
				int8_t acklen = 0;
				if(ok) {
					uint16_t r = htons(return_value);
					acklen = client.write((uint8_t*) &r, 2);
					client.flush();
				}

#ifdef ETCP_DEBUG_PRINT
				Serial.print("Sent ");
				Serial.print(ok ? "PJON_ACK: " : "PJON_FAIL: ");
				Serial.println(acklen);
#else
				(void)acklen; // Avoid "set but not used" warning
#if defined(ETCP_ERROR_PRINT)
				if (!ok) {
					Serial.println("FAILURE sending ACK");
				}
#endif
#endif
			}

			// Call receiver callback function
			if(ok && !_receive_and_discard && content_length > 0) {
				_receiver(sender_id, (uint8_t*)buf(), content_length, _callback_object);
			}

			if (!ok) {
				disconnect_in();
			}
		}
		return return_value;
	};


	bool connect(uint8_t id)
	{
		// Locate the node's IP address and port number
		int16_t pos = find_remote_node(id);

		// Determine if to disconnect
		bool disconnect = !_keep_connection,
		     reverse = _initiate_both_sockets_in_same_direction && _initiator;

		// Break existing connection if not connected to the wanted server
		if(!disconnect && _client_out && _current_device != id) {
			// Connected, but to the wrong device
#ifdef ETCP_DEBUG_PRINT
			Serial.println(F("Switch conn to another srv"));
#endif
			disconnect = true;
			_current_device = -1;
		}

		// Check if established sockets have been disconnected
		if (!disconnect && _client_out && !_client_out.connected()) {
			disconnect = true;
		}
#ifdef ETCP_SINGLE_DIRECTION
		if (!disconnect && reverse && _client_in && !_client_in.connected()) {
			disconnect = true;
		}
		if (!disconnect && reverse && (!_client_in || !_client_out)) {
			disconnect = true;
		}
		if (!disconnect && reverse && _client_in && got_receive_timeout()) {
#ifdef ETCP_ERROR_PRINT
			Serial.println(F("Receive timeout, disconn."));
#endif
			disconnect = true;
		}
#endif
		if (disconnect) {
			disconnect_out();
		}
		// NOTE: From this point we can avoid using the expensive connected() call

		bool connected = _client_out;
#ifdef ETCP_DEBUG_PRINT
		if (!connected) {
			Serial.print(F("Conn to srv pos="));
			Serial.println(pos);
		}
#endif

		if(pos < 0) {
			return false;
		}

		// Try to connect to server if not already connected
		bool did_connect = false;
		if(!connected) {
#ifdef ETCP_DEBUG_PRINT
			Serial.println("Conn..");
#endif
#ifdef HAS_ETHERNETUDP // Arduino, otherwise POSIX
			connected = _client_out.connect(_remote_ip[pos], _remote_port[pos]);
#else
			// Use non-blocking calls, and receive and discard incoming packets
			if (_client_out.prepare_connect(_remote_ip[pos], _remote_port[pos])) {
				int8_t status = 0;
				uint32_t start = PJON_MILLIS();
				do {
					if (!_initiate_both_sockets_in_same_direction && !_single_socket && _server) {
						TCPHelperClient client = _server->available();
						if (client) {
							client.stop();
						}
					}
					status = _client_out.try_connect();
					if (status == 0) {
						PJON_DELAY_MICROSECONDS(PJON_RANDOM(250));    // Avoid misusing CPU while waiting
					}
				} while (status == 0 && (uint32_t)(PJON_MILLIS()-start)<4000);
				connected = (status == 1);
			}
#endif

#ifdef ETCP_DEBUG_PRINT
			Serial.println(connected ? "Conn to srv" : "Failed conn to srv");
#endif
			if(connected) {
				// Adjust connection header A to include ACK request flag
				uint32_t conn_header = htonl(_request_ack ? ETCP_CONNECTION_HEADER_A_ACK :
				                             ETCP_CONNECTION_HEADER_A);
				if (_client_out.write((uint8_t*) &conn_header, 4) != 4) {
					connected = false;
#ifdef ETCP_ERROR_PRINT
					Serial.println(F("Disconn. failed write connhead"));
#endif
				}
			}
			if (!connected) {
				stop(_client_out);
			} else {
				did_connect = true;
			}
		}

		bool connected_rev = false;
#ifdef ETCP_SINGLE_DIRECTION
		if (connected && reverse) {
			connected_rev = _client_in;
			if (!connected_rev) {
#ifdef ETCP_DEBUG_PRINT
				Serial.println("Conn rev..");
#endif
				uint32_t start = PJON_MILLIS();
				do {
					connected_rev = _client_in.connect(_remote_ip[pos], _remote_port[pos]);
				} while (!connected_rev && (uint32_t)(PJON_MILLIS()-start) < 2000);
#ifdef ETCP_DEBUG_PRINT
				Serial.println(connected_rev ? F("Conn rev to srv") : F("Failed rev conn to srv"));
#endif
				if(connected_rev) {
					uint32_t conn_header = htonl(ETCP_CONNECTION_HEADER_B);
					if (_client_in.write((uint8_t*) &conn_header, 4) != 4) {
						connected_rev = false;
#ifdef ETCP_ERROR_PRINT
						Serial.println(F("Disconn rev. failed write connhead"));
#endif
					}
				}
				if (connected_rev) {
					// ACK active on both sockets or none in this mode
					_ack_requested = _request_ack;
					did_connect = true;
					_last_receive_time = PJON_MILLIS(); // Count the connection as a receive action
				}
			}
		}
#endif

		if (did_connect) { // Gather a litte connection information
			_connection_time = PJON_MILLIS();
			_connection_count++;
			if (_single_socket) {
				_ack_requested = _request_ack;    // Same mode in both directions
			}
		}

		if (!connected || (reverse && !connected_rev)) {
#ifdef ETCP_ERROR_PRINT
			Serial.print(F("Fail conn, closing "));
			Serial.print(connected);
			Serial.println(connected_rev);
#endif
			disconnect_out();
			_current_device = -1;
			PJON_DELAY(10);  // Slow down if failure
			return false; // Server is unreachable or busy
		} else {
			_current_device = id;    // Remember who we are connected to
		}

		return true;
	};


	void stop(TCPHelperClient &client)
	{
		client.stop();
	};

	void disconnect_in()
	{
#ifdef ETCP_DEBUG_PRINT
		if (_client_in || (_initiate_both_sockets_in_same_direction && _client_out)) {
			Serial.println(F("Disconn in&rev"));
		}
#endif
#ifdef ETCP_SINGLE_DIRECTION
		if (_initiate_both_sockets_in_same_direction && _client_out) {
			stop(_client_out);
		}
#endif
		if (_client_in) {
			stop(_client_in);
		}
	}


	void disconnect_out()
	{
#ifdef ETCP_DEBUG_PRINT
		if (_client_out || (_initiate_both_sockets_in_same_direction && _client_in)) {
			Serial.println(F("Disconn out&rev"));
		}
#endif
		if (_client_out) {
			stop(_client_out);
		}
#ifdef ETCP_SINGLE_DIRECTION
		if (_initiate_both_sockets_in_same_direction && _client_in) {
			stop(_client_in);
		}
#endif
	}


	bool accept()
	{
		// Determine if to disconnect
		bool disconnect = !_keep_connection;
#ifdef ETCP_SINGLE_DIRECTION
		bool reverse = _initiate_both_sockets_in_same_direction && !_initiator;
#endif
		if (!disconnect && _client_in && !_client_in.connected()) {
			disconnect = true;
		}
#ifdef ETCP_SINGLE_DIRECTION
		if (!disconnect && reverse && _client_out && !_client_out.connected()) {
			disconnect = true;
		}
		if (!disconnect && reverse && (!_client_in || !_client_out)) {
			disconnect = true;
		}
#endif
		if (!disconnect && _client_in && got_receive_timeout()) {
#ifdef ETCP_ERROR_PRINT
			Serial.println(F("Receive timeout, disconn."));
#endif
			disconnect = true;
		}
		if (disconnect) {
			disconnect_in();
		}
		// NOTE: From this point we can avoid using the expensive connected() call

		// Accept new incoming connection
		bool did_connect = false, connected = _client_in;
		if (!connected) {
			_client_in = _server->available();
			connected = _client_in;
#ifdef ETCP_DEBUG_PRINT
			if(connected) {
				Serial.println(F("Accepted"));
			}
#endif
			if (connected) {
				uint32_t connection_header = 0;
				bool header_ok = false;
				if (read_bytes(_client_in, (uint8_t*) &connection_header, 4) == 4) {
					if (connection_header == htonl(ETCP_CONNECTION_HEADER_A)) {
						header_ok = true;
						_ack_requested = false;
					} else if (connection_header == htonl(ETCP_CONNECTION_HEADER_A_ACK)) {
						header_ok = true;
						_ack_requested = true;
					}
				}
				if (header_ok) {
					did_connect = true;
				} else {
					disconnect_in();
					connected = false;
#ifdef ETCP_ERROR_PRINT
					Serial.println(F("Disconn. no connhead"));
#endif
				}
			}
		}

		// Accept reverse connection if relevant
#ifdef ETCP_SINGLE_DIRECTION
		if (connected && reverse && !_client_out) {
#ifdef ETCP_DEBUG_PRINT
			Serial.println(F("Lst rev"));
#endif
			// ACK active on both sockets or none in this mode
			_request_ack = _ack_requested;

			bool connected_reverse = false;
			uint32_t start = PJON_MILLIS();
			do {
				_client_out = _server->available();
			} while (!_client_out && (uint32_t)(PJON_MILLIS()-start) < 2000);

			if(_client_out) {
#ifdef ETCP_DEBUG_PRINT
				Serial.println("Accept rev OK");
#endif
				uint32_t connection_header = 0;
				if (read_bytes(_client_out, (uint8_t*) &connection_header, 4) == 4 &&
				        connection_header == htonl(ETCP_CONNECTION_HEADER_B)) {
					connected_reverse = true;
				} else {
#ifdef ETCP_ERROR_PRINT
					Serial.println(F("Disconn. rev no connhead"));
#endif
				}
			} else {
#ifdef ETCP_ERROR_PRINT
				Serial.println(F("Accept rev timed out"));
#endif
			}

			if (connected_reverse) {
				did_connect = true;
			} else {
#ifdef ETCP_ERROR_PRINT
				Serial.print(F("Fail accept, closing, "));
				Serial.print(connected);
				Serial.println(connected_reverse);
#endif
				connected = false;
				disconnect_in();
			}
		}
#endif

		if (did_connect) {
			_connection_time = PJON_MILLIS();
			_connection_count++;
			_last_receive_time = PJON_MILLIS(); // Count the connection as a receive action
		}
		return connected;
	};


	void disconnect_out_if_needed(uint16_t result)
	{
		//bool connected = _client_out.connected();
		if(_client_out && (result == PJON_FAIL || !_keep_connection)) {
			stop(_client_out);
#ifdef ETCP_DEBUG_PRINT
			Serial.print("Disconn outcl. OK=");
			Serial.println(result == PJON_ACK);
#endif
		}
	};

	bool got_receive_timeout()
	{
		return (uint32_t)(PJON_MILLIS() - _last_receive_time) > ETCP_IDLE_TIMEOUT;
	}

	bool disconnect_in_if_needed()
	{
		if(_client_in && !_keep_connection) {
#ifdef ETCP_DEBUG_PRINT
			Serial.println("Disc. inclient.");
#endif
			stop(_client_in);
		}
		return true;
	};


	uint16_t send(
	    TCPHelperClient &client,
	    uint8_t id,
	    const uint8_t *packet,
	    uint16_t length
	)
	{
		// Assume we are connected. Try to deliver the package
		uint32_t head = htonl(ETCP_HEADER), foot = htonl(ETCP_FOOTER), len = htonl(length);
		uint8_t buf[9];
		memcpy(buf, &head, 4);
		memcpy(&buf[4], &id, 1);
		memcpy(&buf[5], &len, 4);

		if (!_single_socket && _client_in) if (_client_in.available() > 0) {
				return PJON_BUSY;
			}

#ifdef HAS_ETHERNETUDP
		bool ok = client.write((uint8_t*) buf, 9) == 9;
		if(ok) {
			ok = client.write((uint8_t*) packet, length) == length;
		}
		if(ok) {
			ok = client.write((uint8_t*) &foot, 4) == 4;
		}
#else
		// On a POSIX capable device we expect to have enough memory to collect all into one buffer
		// so that it will not be sent as 3 separate packets when TCP_NODELAY is active.
		TmpBuffer totalbuf(9+length+4);
		memcpy(totalbuf(), buf, 9);
		memcpy(&(totalbuf()[9]), packet, length);
		memcpy(&(totalbuf()[9+length]), &foot, 4);
		bool ok = client.write((uint8_t*)totalbuf(), 9+length+4) == (9+length+4);
#endif
		if(ok) {
			client.flush();
		}
#ifdef ETCP_DEBUG_PRINT
		Serial.print("Write stat: ");
		Serial.println(ok);
#endif

		uint16_t result = PJON_FAIL;
		if (_request_ack) {
			// Read ACK
			if(ok) {
				uint16_t code = 0;
				int16_t status = read_bytes(client, (uint8_t*) &code, 2);
				code = ntohs(code);
				if(status == 2 && code == PJON_ACK) {
					result = code;
				} else {
					ok = false;
				}
			}
#ifdef ETCP_DEBUG_PRINT
			Serial.print("PJON_ACK stat: ");
			Serial.print(result == PJON_ACK);
			Serial.println(ok ? " OK" : " FAIL");
#endif
		} else {
			result = ok ? PJON_ACK : PJON_FAIL;
		}
		return result;  // PJON_FAIL, PJON_ACK
	};


	/* Do ACKed bidirectional transfer of packets over a single socket connection by
	   using a master-slave mode where the master connects and delivers packets
	   or a placeholder, then reads packets or placeholder back before closing
	   the connection (unless letting it stay open). */
#ifdef ETCP_SINGLE_SOCKET_WITH_ACK
	uint16_t single_socket_transfer(
	    TCPHelperClient &client,
	    int16_t id,
	    bool master,
	    const uint8_t *contents,
	    uint16_t length
	)
	{
		if(master) { // Creating outgoing connections
			// Connect or check that we are already connected to the correct server
			bool connected = connect(id);
#ifdef ETCP_DEBUG_PRINT
			//Serial.println(connected ? "Out conn" : "No out conn");
#endif
			if(!connected) {
				return PJON_FAIL;
			}

			// Send singlesocket header and number of outgoing packets
			bool ok = true;
			uint32_t head = htonl(ETCP_SINGLE_SOCKET_HEADER);
			uint8_t numpackets_out = length > 0 ? 1 : 0;
			uint8_t buf[5];
			memcpy(buf, &head, 4);
			memcpy(&buf[4], &numpackets_out, 1);
			if(ok) {
				ok = client.write((uint8_t*) &buf, 5) == 5;
			}
			if(ok) {
				client.flush();
			}

			// Send the packet and read PJON_ACK
			if(ok && numpackets_out > 0) {
				ok = send(client, id, contents, length) == PJON_ACK;
#ifdef ETCP_DEBUG_PRINT
				Serial.print(F("++++Sent p, ok="));
				Serial.println(ok);
#endif
			}

			// Read number of incoming messages
			uint8_t numpackets_in = 0;
			if(ok) {
				ok = read_bytes(client, &numpackets_in, 1) == 1;
			}
#ifdef ETCP_DEBUG_PRINT
			if (!ok || numpackets_in > 0) {
				Serial.print("Read np_in: ");
				Serial.print(numpackets_in);
				Serial.println(ok ? " OK" : " FAIL");
			}
#endif

			// Read incoming packages if any
			for(uint8_t i = 0; ok && i < numpackets_in; i++) {
				while(client.available() < 1 && client.connected()) ;
				ok = receive(client, true) == PJON_ACK;
#ifdef ETCP_DEBUG_PRINT
				Serial.print(F("------->Read p "));
				Serial.print(i);
				Serial.print(F(", ok="));
				Serial.println(ok);
#endif
			}

			// Write singlesocket footer for the whole thing
			uint32_t foot = htonl(ETCP_SINGLE_SOCKET_FOOTER);
			if(ok) {
				ok = client.write((uint8_t*) &foot, 4) == 4;
			}
			if(ok) {
				client.flush();
			}
#ifdef ETCP_DEBUG_PRINT
			//        Serial.print("Sent ss foot, ok=");
			//        Serial.println(ok);
#endif

			// Disconnect
			uint16_t result = ok ? PJON_ACK : PJON_FAIL;
			disconnect_out_if_needed(result);
#ifdef ETCP_DEBUG_PRINT
			if (numpackets_in > 0 || numpackets_out > 0) {
				Serial.print("INOUTm ");
				Serial.print(numpackets_in);
				Serial.print(numpackets_out);
				Serial.println(ok ? " OK" : " FAIL");
			}
#endif

			// Return PJON_ACK if successfully sent or received a packet
			return contents == NULL ? (numpackets_in > 0 ? result : PJON_FAIL) : result;

		} else { // Receiving incoming connections and packets and request
			if (client && got_receive_timeout()) {
#ifdef ETCP_ERROR_PRINT
				Serial.println(F("Receive timeout, disconn."));
#endif
				stop(client);
			}

			// Wait for and accept connection
			bool connected = accept();
#ifdef ETCP_DEBUG_PRINT
			//Serial.println(connected ? "In conn" : "No in conn");
#endif
			if(!connected) {
				return PJON_FAIL;
			}

			// Read singlesocket header
			bool ok = read_until_header(client, ETCP_SINGLE_SOCKET_HEADER);
			if (ok) {
				_last_receive_time = PJON_MILLIS();
			}
#ifdef ETCP_DEBUG_PRINT
			//Serial.print("Read ss head, ok=");
			//Serial.println(ok);
#endif

			// Read number of incoming packets
			uint8_t numpackets_in = 0;
			if(ok) {
				ok = read_bytes(client, (uint8_t*) &numpackets_in, 1) == 1;
			}
#ifdef ETCP_DEBUG_PRINT
			if (!ok || numpackets_in > 0) {
				Serial.print("Read np_in: ");
				Serial.print(numpackets_in);
				Serial.println(ok ? " OK" : " FAIL");
			}
#endif

			// Read incoming packets if any, send ACK for each
			for(uint8_t i = 0; ok && i < numpackets_in; i++) {
				while(client.available() < 1 && client.connected()) ;
				ok = receive(client, true) == PJON_ACK;
#ifdef ETCP_DEBUG_PRINT
				Serial.print(F("Read p "));
				Serial.print(i);
				Serial.print(F(", ok="));
				Serial.println(ok);
#endif
			}

			// Write number of outgoing packets
			uint8_t numpackets_out = length > 0 ? 1 : 0;
			if(ok) {
				ok = client.write((uint8_t*) &numpackets_out, 1) == 1;
			}
			if(ok) {
				client.flush();
			}

			// Write outgoing packets if any
			if(ok && numpackets_out > 0) {
				ok = send(client, id, contents, length) == PJON_ACK;
#ifdef ETCP_DEBUG_PRINT
				Serial.print(F("Sent p, ok="));
				Serial.println(ok);
#endif
			}

			// Read singlesocket footer
			if(ok) {
				uint32_t foot = 0;
				ok = read_bytes(client, (uint8_t*) &foot, 4) == 4;
				if(foot != htonl(ETCP_SINGLE_SOCKET_FOOTER)) {
					ok = 0;
				}
#ifdef ETCP_DEBUG_PRINT
				//Serial.print(F("Read ss foot, ok="));
				//Serial.println(ok);
#endif
			}

			// Disconnect
			disconnect_in_if_needed();

#ifdef ETCP_DEBUG_PRINT
			if (numpackets_in > 0 || numpackets_out > 0) {
				Serial.print("INOUT ");
				Serial.print(numpackets_in);
				Serial.print(numpackets_out);
				Serial.println(ok ? " OK" : " FAIL");
			}
#endif

			if (!ok) {
				stop(client);
#ifdef ETCP_ERROR_PRINT
				Serial.print(F("Failure, disconnecting."));
#endif
			}

			// Return PJON_ACK if successfully sent or received a packet
			uint16_t result = ok ? PJON_ACK : PJON_FAIL;
			return contents == NULL ? (numpackets_in > 0 ? result : PJON_FAIL) : result;
		}
		return PJON_FAIL;
	};
#endif

	/* Read until a specific 4 byte value is found.
	   This will resync if stream position is lost. */

	bool read_until_header(TCPHelperClient &client, uint32_t header)
	{
		header = htonl(header); // Network byte order
		uint32_t head = 0;
		int8_t bytes_read = 0;
		bytes_read = (uint8_t)read_bytes(client, (uint8_t*) &head, 4);
		if(bytes_read != 4 || head != header) {
			// Did not get header. Lost position in stream?
			do {
				/* Try to resync if we lost position in the stream
				        (throw avay all until ETCP_HEADER found) */
				head = head >> 8;
				// Make space for 8 bits to be read into the most significant byte
				bytes_read = (uint8_t)read_bytes(client, &((uint8_t*) &head)[3], 1);
				if(bytes_read != 1) {
					break;
				}
			} while(head != header);
		}
		return head == header;
	};

public:

	EthernetLink()
	{
		init();
	};


	EthernetLink(uint8_t id)
	{
		init();
		set_id(id);
	};

#ifndef ARDUINO
	// Destructor not needed on Arduino, uses a lot of program space
	~EthernetLink()
	{
		if (_server) {
			delete _server;
		}
	}
#endif

	int16_t add_node(
	    uint8_t remote_id,
	    const uint8_t remote_ip[],
	    uint16_t port_number = ETCP_DEFAULT_PORT
	)
	{
		if (_remote_node_count == ETCP_MAX_REMOTE_NODES) {
			return -1;
		}
		_remote_id[_remote_node_count] = remote_id;
		memcpy(_remote_ip[_remote_node_count], remote_ip, 4);
		_remote_port[_remote_node_count] = port_number;
		_remote_node_count++;
		return _remote_node_count - 1;
	};


	/* This function must be called after configuration before running,
	   for ALL configurations EXCEPT when initiator in single_socket
	   and single_initiate_direction modes.
	   In single_socket and single_initiate_direction modes, calling this
	   function sets it as receiver, NOT initiator. */

	void start_listening(uint16_t port_number = ETCP_DEFAULT_PORT)
	{
		_local_port = port_number;
		_initiator = false; // If we are listening, we are not the initiator
		if(_server == NULL && (!_initiate_both_sockets_in_same_direction || !_initiator)) {
#ifdef ETCP_DEBUG_PRINT
			Serial.print("Lst on port ");
			Serial.println(port_number);
#endif
			_server = new TCPHelperServer(port_number);
			_server->begin();
		}
	};


	/* Whether to keep outgoing connection live until we need connect to
	   another EthernetLink node */

	void keep_connection(bool keep)
	{
		_keep_connection = keep;
	};


	/* Whether to do bidirectional data transfer on a single socket or use one
	   socket for each direction. Single socket transfer is slower but more
	   firewall-friendly. Client connects to server, and packets are exchanged in
	   both directions on the same socket. If using this, _only one_ of the sides
	   should call start_listening, and that side will be receiver with the other
	   initiator (establishing connections) */
#ifdef ETCP_SINGLE_SOCKET_WITH_ACK
	void single_socket(bool single_socket)
	{
		_single_socket = single_socket;
		keep_connection(true);
		request_ack(true);
	};
#endif

	/* With single_socket = false, there is one socket for each packet direction.
	  Normally the sockets are initiated from the side sending the packet.
	  By setting initiate_both_sockets_in_same direction, both sockets can be
	  initiated from one of the devices, to simplify firewall setup, or for letting
	  only one of the devices have a static IP address.
	  This should only be used with _keep_connection = true, and is meant for permanent
	  one-to-one links. */
#ifdef ETCP_SINGLE_DIRECTION
	void single_initiate_direction(bool single_initiate_direction)
	{
		_initiate_both_sockets_in_same_direction = single_initiate_direction;
		keep_connection(true);
		request_ack(true);
	};
#endif

	/* Request an explicit immediate ACK for each packet being sent.
	   This make it possible with guearanteed delivery.
	   Without ACK, a packet may be lost before a dead socket is discovered,
	   but for some shemes this is acceptable. */

	void request_ack(bool request_ack)
	{
		_request_ack = request_ack;
	}

	/* Some connection statistics */
	uint32_t get_connection_time() const
	{
		return _connection_time;
	}
	uint32_t get_connection_count() const
	{
		return _connection_count;
	}


	// Keep trying to send for a maximum duration

	uint16_t send_with_duration(
	    uint8_t id,
	    const uint8_t *packet,
	    uint16_t length,
	    uint32_t duration_us
	)
	{
		uint32_t start = PJON_MICROS();
		uint16_t result = PJON_FAIL;
		do {
			result = send(id, packet, length);
		} while(
		    result != PJON_ACK &&
		    (uint32_t)(PJON_MICROS() - start) <= duration_us
		);
		return result;
	};


	/* In single-socket mode and acting as initiator, connect and check for
	   incoming packets from a specific device */

	uint16_t poll_receive(uint8_t remote_id)
	{
		/* Create connection if needed but only poll for incoming packet
		   without delivering any */
		if(_single_socket) {
			if(!_server) {
#ifdef ETCP_SINGLE_SOCKET_WITH_ACK
				return single_socket_transfer(_client_out, remote_id, true, NULL, 0);
#else
				(void)remote_id; // Avoid "unused parameter" warning
				return PJON_FAIL;
#endif
			}
		} else { // Just do an ordinary receive without using the id
			return receive();
		}
		return PJON_FAIL;
	};


	uint8_t get_id() const
	{
		return _local_id;
	};


	// Overridden functions below -----------------------------------------------

	// Connect to a server if needed, then read incoming package and send ACK

	uint16_t receive()
	{
		if(_server == NULL) { // Not listening for incoming connections
#if defined(ETCP_SINGLE_SOCKET_WITH_ACK) || defined(ETCP_SINGLE_DIRECTION)
			int16_t remote_id = _remote_node_count == 1 ? _remote_id[0] : -1;
#endif
			if(_single_socket) { // Single-socket mode.
				/* Only read from already established outgoing socket, or create
				connection if there is only one remote node configured (no doubt about
				which node to connect to). */
#ifdef ETCP_SINGLE_SOCKET_WITH_ACK
				return single_socket_transfer(_client_out, remote_id, true, NULL, 0);
#else
				return PJON_FAIL;
#endif
			}
#ifdef ETCP_SINGLE_DIRECTION
			else if (_initiate_both_sockets_in_same_direction && _initiator) {
				connect(remote_id);
				uint16_t result = receive(_client_in, false);
				disconnect_out_if_needed(PJON_ACK); // Do not disconnect if nothing to read
				disconnect_in_if_needed();
				return result;
			}
#endif
		} else {
			// Accept new incoming connection if connection has been lost
			if(_single_socket) {
#ifdef ETCP_SINGLE_SOCKET_WITH_ACK
				return single_socket_transfer(_client_in, -1, false, NULL, 0);
#else
				return PJON_FAIL;
#endif
			} else {
				// Accept incoming connection(s)
				if(!accept()) {
					return PJON_FAIL;
				}

				uint16_t result = receive(_client_in, !_keep_connection);

				disconnect_in_if_needed();
				return result;
			}
		}
		return PJON_FAIL;
	};


	uint16_t receive(uint32_t duration_us)
	{
		uint32_t start = PJON_MICROS();
		uint16_t result = PJON_FAIL;
		do {
			result = receive();
			if (result != PJON_ACK) {
				PJON_DELAY_MICROSECONDS(250);
			}
		} while(
		    result != PJON_ACK &&
		    (uint32_t)(PJON_MICROS() - start) <= duration_us
		);
		return result;
	};


	uint16_t send(
	    uint8_t id,
	    const uint8_t *packet,
	    uint16_t length,
	    uint32_t = 0 // timing_us
	)
	{
		// Special algorithm for single-socket transfers
		if(_single_socket)
#ifdef ETCP_SINGLE_SOCKET_WITH_ACK
			return single_socket_transfer(
			           _server ? _client_in : _client_out,
			           id,
			           _server ? false : true,
			           packet,
			           length
			       );
#else
			return PJON_FAIL;
#endif

		// Connect or check that we are already connected to the correct server
		bool connected = false;
#ifdef ETCP_SINGLE_DIRECTION
		if (_initiate_both_sockets_in_same_direction && !_initiator) {
			connected = accept();
		} else
#endif
			connected = connect(id);

		// Send the packet and read PJON_ACK
		uint16_t result = PJON_FAIL;
		if(connected) {
			result = send(_client_out, id, packet, length);
		}

		// Disconnect
		disconnect_out_if_needed(result);

		return result;
	};


	uint8_t device_id()
	{
		return _local_id;
	};


	uint8_t acquire_id()   // Not supported yet
	{
		return 0;
	};


	void set_error(link_error e)
	{
		_error = e;
	};


	void set_id(uint8_t id)
	{
		_local_id = id;
	};


	void set_receiver(link_receiver r, void *callback_object)
	{
		_receiver = r;
		_callback_object = callback_object;
	};


	void update() { };
};

#undef Serial
