#include "ModemSocket.h"
#include "ModemMessage.h"
#include <stdio.h>
#include <time.h>
#include <signal.h>

#ifndef _MSC_VER // QNX
#include <unix.h>
#include <netdb.h>
#endif

#define SOCKET_PORT 2391

#define SOCKET_KEEPALIVE (-2134)
#define SOCKET_MODEMPACKET 30333


ModemSocket::ModemSocket(const char* pcIPAddress, long approxSpeed) : ModemFork("ModemSocket")
{
    debug = 1;

	_approxSpeed = approxSpeed;
	unsigned long ip = INADDR_ANY;
    ip = inet_addr(pcIPAddress);
	_ipAddress.s_addr = ip;

#ifndef _MSC_VER // QNX
    // Apparently the SIGPIPE signal might be killing QNX during sends!!!
    signal(SIGPIPE, SIG_IGN);
#endif

    _cSocket = INVALID_SOCKET;
    _lSocket = INVALID_SOCKET;

#ifndef _MSC_VER // QNX
    _cPort = htons(SOCKET_PORT);
	_lPort = htons(SOCKET_PORT + 1);
#else // Windows
	_cPort = htons(SOCKET_PORT + 1);
	_lPort = htons(SOCKET_PORT);
#endif

	_fuzzyCon = 0;
	_keepAliveCount = 1000;

//	bSetupSocketConnect();
}


ModemSocket::~ModemSocket() {
    shutdown(_cSocket, SD_BOTH);
    closesocket(_cSocket);
    _cSocket = INVALID_SOCKET;

    shutdown(_lSocket, SD_BOTH);
    closesocket(_lSocket);
    _lSocket = INVALID_SOCKET;

	_fuzzyCon = 0;
}


void ModemSocket::initialize()
{
    shutdown(_cSocket, SD_BOTH);
    closesocket(_cSocket);
    _cSocket = INVALID_SOCKET;

#ifndef _MSC_VER // QNX
	_ipAddress.s_addr = INADDR_ANY;
#endif

	_fuzzyCon = 0;
}

bool ModemSocket::bSocketConnected() const
{
	//&& (_lSocket != INVALID_SOCKET)
	return ((_cSocket != INVALID_SOCKET)  && (_fuzzyCon > 0));
}

int ModemSocket::iGetMaxTransitBytes()
{
    // Subtract out the length of the sent data.
    return iMAX_BLOB_SIZE - sizeof(int);
}


int ModemSocket::iReadFromModem(char* buffer, int maxBytes)
{

    // This call should block until the read is done or fails.

    // Do we even have a connection?
    if (_lSocket == INVALID_SOCKET) {
    	if (!bSetupSocketListen()) return 0;
    }

	Assert(_lSocket != INVALID_SOCKET);

    // Make sure socket is non-blocking.
    unsigned long temp = 1;
    if (ioctlsocket(_lSocket, FIONBIO, &temp)) return 0;

    char data[iMAX_BLOB_SIZE];

    int err, len = sizeof(struct sockaddr);
	struct sockaddr_in from;


	int i_recv = recvfrom(_lSocket, data, iMAX_BLOB_SIZE, 0,(struct sockaddr *)&from, &len);

    switch (i_recv) {
    case (-1):
		err = WSAGetLastError();
		//if (err == WSAEWOULDBLOCK) return 0; // "normal" error for non-blocking
		if (err != WSAEWOULDBLOCK) parseWSAError(err);
		return 0;
    case (0):
		// connection has been gracefully closed.
		parseWSAError(WSAGetLastError());
		initialize();
		return 0;
    default:
#ifndef _MSC_VER // QNX
		if (_cSocket == INVALID_SOCKET) {
			_ipAddress.s_addr = from.sin_addr.s_addr;
			bSetupSocketConnect();
		}
#endif
		//fprintf(stderr,"i_recv = %d\n",i_recv);
		int totalLen = 0;

		const char * pData = data;
		while (i_recv > (pData - data)) {
			// there may be several of our messages here,
			// we need to scan through the data, strip header information,
			// and copy the message content into the output buffer

			// check the header code
			int i_code = 0;
			pData = ::pcRead(pData,&i_code);
			if (i_code == SOCKET_KEEPALIVE) {
				if (from.sin_addr.s_addr == _ipAddress.s_addr) _fuzzyCon = 500;
				continue;
			}
			if (i_code != SOCKET_MODEMPACKET) {
				parseWSAError(666);
				// re-establish sync
				while (i_recv > (pData - data)) {
					pData-=3;
					pData = ::pcRead(pData,&i_code);
					if (i_code == SOCKET_MODEMPACKET) break;
				}
				if (i_recv <= (pData - data)) {
						continue;
					}
			}

			// get the message length
			int iMyLen = -1;
			pData = ::pcRead(pData, &iMyLen);
			// Is the length reasonable?
			if ((iMyLen < 0) || (iMyLen > iMAX_BLOB_SIZE) || ((pData + iMyLen - data) > i_recv)) {
				parseWSAError(667);
				continue;
			}

			// check the terminator code (negative header code)
			::pcRead(pData + iMyLen,&i_code);
			if (i_code != -SOCKET_MODEMPACKET) {
				parseWSAError(668);
				continue;
			}

			// Ok...
			if ((totalLen + iMyLen) < maxBytes){
				memcpy(&(buffer[totalLen]), pData, iMyLen);
				totalLen += iMyLen;
				pData += iMyLen + 4; // 4=terminator
				if (from.sin_addr.s_addr == _ipAddress.s_addr) _fuzzyCon = 500;
			} else {
				break;
			}
		}
		return totalLen;
    }
}

int ModemSocket::iWriteToModem(char* buffer, int nBytes)
{

    // Are we connected?
    if (_cSocket == INVALID_SOCKET) {
		if (!bSetupSocketConnect()) return 0;
    }

    Assert(_cSocket != INVALID_SOCKET);

    // If we get this far, we are OK to send!!!
    // Make the packet to send. (needs major abstracting)
    int packetSize = nBytes + 3*sizeof(int);
    Assert (packetSize < iMAX_BLOB_SIZE);
    char data[iMAX_BLOB_SIZE];
    char * pData = data;
    //memset(data, 0, packetSize);

    // First, the transmit code.
    int i_code = SOCKET_MODEMPACKET;
    pData = ::pcWrite(pData, i_code);
    // Write the buffer length.
    pData = ::pcWrite(pData, nBytes);
    // And finally, the data
    memcpy(pData, buffer, nBytes);
	pData+=nBytes;
	// And the terminator (negative header code)
    pData = ::pcWrite(pData, -i_code);

	Assert((pData - data) == packetSize);

    // Make sure socket is non-blocking!
    unsigned long temp = 1;
    if (ioctlsocket(_cSocket, FIONBIO, &temp)) return 0;

    int iResult = send(_cSocket, data, packetSize, 0);

    // Check result
    if (iResult == SOCKET_ERROR) {
		int err = WSAGetLastError();
		if (err == WSAEWOULDBLOCK) return 0; // "normal" error for non-blocking
			parseWSAError(WSAGetLastError());
			initialize();
			return 0;
		}

		if (iResult < packetSize) {
		fprintf(stderr,"Socket: incomplete transfer\n");
		// incomplete transfer
		return 0;
    }

    return nBytes;
}


bool ModemSocket::bSetupSocket()
{
    // vehicle listens... topside connects
	if (bSetupSocketConnect()) {
		Assert(_cSocket != INVALID_SOCKET);
		return true;
    }
	/*
#ifndef _MSC_VER // QNX
    if (bSetupSocketListen()) {
	Assert(_lSocket != INVALID_SOCKET);
	return true;
    }
#else // Windows
    if (bSetupSocketConnect() && bSetupSocketListen()) {
	Assert(_cSocket != INVALID_SOCKET);
	Assert(_lSocket != INVALID_SOCKET);
	return true;
    }
#endif
	*/
    return false;
}


bool ModemSocket::bSetupSocketListen()
{
    debug_t debug = True;
    if (_lSocket == INVALID_SOCKET) {
		// Create listening socket
		_lSocket = socket(PF_INET, SOCK_DGRAM, 0);
		if (_lSocket == INVALID_SOCKET) return false;

		// Make the socket reusable.
		bool b = 1;
		if (setsockopt(_lSocket, SOL_SOCKET, SO_REUSEADDR, (char*)&b, sizeof(bool))) return false;

		// Make socket non-blocking.
		unsigned long temp=1;
		if (ioctlsocket(_lSocket, FIONBIO, &temp)) return false;

		// "Binding";
		sockaddr_in name;
		//memset(&name,0,sizeof(sockaddr_in));
		name.sin_family = PF_INET;
		name.sin_port = _lPort;

		if (_ipAddress.s_addr == htonl(INADDR_ANY)) {
			name.sin_addr.s_addr = htonl(INADDR_ANY);
		} else {
			
			char * buf1 = strdup(inet_ntoa(_ipAddress));
			int s1, s2, s3, s4;
			sscanf(buf1,"%d.%d.%d.%d",&s1,&s2,&s3,&s4);
			delete buf1;

			char * buf2 = new char[64];
			if (gethostname(buf2,64) != 0) {
				delete[] buf2;
				parseWSAError(WSAGetLastError());
				closesocket(_lSocket);
				_lSocket= INVALID_SOCKET;
				return false;
			}

			hostent * he = gethostbyname(buf2);
			delete[] buf2;

			int i = 0;
			int t1, t2, t3, t4;
			sockaddr_in temp;
			while(he->h_addr_list[i] != NULL){
				temp.sin_addr.s_addr = *((unsigned long*) he->h_addr_list[i]);
				char * buf3 = strdup(inet_ntoa(temp.sin_addr));
				sscanf(buf3,"%d.%d.%d.%d",&t1,&t2,&t3,&t4);
				delete buf3;
				if ((s1 == t1) && (s2 == t2) && (s3 == t3)) break; // interface in same subnet
				i++;
			}
			name.sin_addr.s_addr = temp.sin_addr.s_addr;
		}
		if (bind(_lSocket, (sockaddr*)&name, sizeof(sockaddr_in))) {
			parseWSAError(WSAGetLastError());
			closesocket(_lSocket);
			_lSocket= INVALID_SOCKET;
			return false;
		}
		
		//fprintf(stderr,"Set up listening socket\n");
    }
    return true;
}


bool ModemSocket::bSetupSocketConnect()
{
	if (_ipAddress.s_addr == INADDR_ANY) return false;

    if (_cSocket == INVALID_SOCKET) {
		// Create socket.
		_cSocket = socket(PF_INET, SOCK_DGRAM, 0);
		if (_cSocket == INVALID_SOCKET)	return false;

		// Make socket reusable.
		bool b = 1;
		if (setsockopt(_cSocket, SOL_SOCKET, SO_REUSEADDR, (char*)&b, sizeof(bool))) return false;

		// Make socket non-blocking.
		unsigned long temp=1;
		if (ioctlsocket(_cSocket, FIONBIO, &temp)) return false;
    }

    // Build the descriptor for the remote socket.
    sockaddr_in name;
	memset(&name,0,sizeof(sockaddr_in));
    name.sin_family = PF_INET;
    name.sin_port = _cPort;
    name.sin_addr.s_addr = _ipAddress.s_addr;
//	bzero(&(name.sin_zero), 8);

	// Make the UDP socket connection.
	fprintf(stderr,"connecting\n");
	if (connect(_cSocket, (sockaddr*)&name, sizeof(sockaddr_in))) {
		int err = WSAGetLastError();
		// This is a crazy number of errors to ignore, but has to do with QNX and Windows socket buggitude
		if ((err == WSAEINPROGRESS) ||
			(err == WSAEALREADY)    ||
			(err == WSAEWOULDBLOCK) ||
			(err == WSAEINVAL)) return false; // "normal" for non-blocking sockets
		parseWSAError(err);
		initialize();
		return false;
	}
    return true;
}

void ModemSocket::setTargetIP(const char* pcIP)
{
    unsigned long ip = INADDR_ANY;
    ip = inet_addr(pcIP);
    _ipAddress.s_addr = ip;
}

void ModemSocket::setPort(unsigned int port)
{
   // _port = port;
}

bool ModemSocket::bHasConnectionChild()
{
	_keepAliveCount++;
	if (_keepAliveCount > 100) {
		_keepAliveCount = 0;
		if (_cSocket == INVALID_SOCKET) bSetupSocketConnect();
		if (_cSocket != INVALID_SOCKET) {
			// Send keepalive
			int i_code = SOCKET_KEEPALIVE;
			int i_send = send(_cSocket, (char*)&i_code, sizeof(int), 0);
			if (i_send == SOCKET_ERROR) {
				int err = WSAGetLastError();
				if (err != WSAEWOULDBLOCK) {
					parseWSAError(err);
					initialize();
				}
			}
		}
	}

	_fuzzyCon--;
	if (_fuzzyCon < -200) initialize();

	return bSocketConnected();
}

int ModemSocket::getBytesPerSecondChild()
{
    // In an ideal world, this call would actually calculate the recent connection speed.
    // However, we are just going to return a pretty big number, relying on ethernet to
    //  outpace the other hardware.
    if (_approxSpeed) return _approxSpeed / (8+1);
	else return 10000;
}


void ModemSocket::parseWSAError( int err )
{
	int ie;
	switch (err) {
		case (WSAEFAULT) :
						ie = 1;
			break;
		case (WSAEHOSTUNREACH) :
						ie = 2;
			break;
		case (WSAEINPROGRESS) :
						ie = 3;
			break;
		case (WSAENOBUFS) :
						ie = 4;
			break;
		case (WSAENOTCONN) :
						ie = 5;
			break;
		case (WSAENOTSOCK) :
						ie = 6;
			break;
		case (WSAEWOULDBLOCK) :    // this is normal during non-blocking connects/receives
						ie = 75;
			break;
			//case (WSANOTINITIALISED) :
				//		ie = 7;
			//	break;
		case (WSAEINVAL) :
						ie = 8;
			break;
		case (WSAEALREADY) :
						ie = 9;
			break;
		case (WSAEISCONN) :
						ie = 10;
			break;
	default:
	    ie = err;
		}
	fprintf(stderr,"socket error = %d\n",ie);
}