/**********************************************************
 *
 *  MEX file for the tcpip toolbox.
 */
#define VERSION "Version  2.0.5 + PTBMods by Mario Kleiner - 2012-09-10"

/*
 * %   This file(s) is part of the tcp_udp_ip toolbox (C) Peter Rydesater et al.
 * %   et al.  1998-2003 for running in MATLAB(R) as scripts and/or plug-ins.
 * %
 * %   This program is free software; you can redistribute it and/or modify
 * %   it under the terms of the GNU General Public License as published by
 * %   the Free Software Foundation; either version 2 of the License, or
 * %   (at your option) any later version.
 * %
 * %   This program is distributed in the hope that it will be useful,
 * %   but WITHOUT ANY WARRANTY; without even the implied warranty of
 * %   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * %   GNU General Public License for more details.
 * %
 * %   You should have received a copy of the GNU General Public License
 * %   along with this program; if not, write to the Free Software
 * %   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 * %
 * %   In addition, as a SPECIAL EXCEPTION, Peter Rydesater, SWEDEN,
 * %   gives permission to link the code of this program with any library,
 * %   and distribute linked combinations of it. You must obey the GNU
 * %   General Public License in all respects for all of the code in the
 * %   tcp_udp_ip toolbox and any . If you modify any source file included,
 * %   you may extend this exception to your version of the file, but you are
 * %   not obligated to do so.  If you do not wish to do so, delete this exception
 * %   statement from your version. This exception makes it possible to use
 * %   pnet.c (.dll) as a plug-in as it is intended and let it be (dynamical)
 * %   linked to MATLAB(R) or any compiled stand alone application.
 *
 *
 *  Notes for Unix implementation
 *  Compile this with:
 *
 *  mex -g -v -largeArrayDims -DMEX_DOUBLE_HANDLE pnet.c
 *
 *  On Linux + gcc 4.6+ + Matlab use:
 *
 *  mex -O -g -v CFLAGS='$CFLAGS -fPIC -fexceptions' -largeArrayDims -DMEX_DOUBLE_HANDLE pnet.c
 *
 *  Notes for Windows implementation
 *
 *  When using LCC, compile this with:
 *  mex -O pnet.c {MATLAB_INSTALL_DIR}\sys\lcc\lib\wsock32.lib -DWIN32
 *
 *  When using Visual C++, compile this with:
 *  mex -O pnet.c ws2_32.lib winmm.lib -DWIN32 -largeArrayDims -DMEX_DOUBLE_HANDLE
 *
 *
 *  == Main Authour ==           == Windows support ==      == Earlie/Basic UDP support ==
 *  Peter Rydesater              Mario Bergeron             Mike Medeiros at 23-Jan-2001.
 *                               LYRtech
 *  Ostersund, Sweden            Quebec, Canada
 *  +46 70 560 68 16
 *  Peter.Rydesater@mh.se        Mario.Bergeron@lyrtech.com
 *
 *
 *  SE MORE:  http://www.rydesater.com
 *
 **********************************************************/

/******* GENERAL DEFINES *********/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <ctype.h>

/******* WINDOWS ONLY DEFINES *********/
#ifdef WIN32
#define IFWINDOWS(dothis) dothis
#define IFUNIX(dothis)
//#include <windows.h>
#include <winsock2.h>
#define close(s) closesocket(s)
#define nonblockingsocket(s) {unsigned long ctl = 1;ioctlsocket( s, FIONBIO, &ctl );}
#define s_errno WSAGetLastError()
#define EWOULDBLOCK WSAEWOULDBLOCK
#define usleep(a) Sleep((a)/1000)
#define MSG_NOSIGNAL 0
#define DEFAULT_USLEEP        1000        /* MK: Changed from 10 msecs to 1 msec == 1000 microsecs. for lower latency. Can't go lower than 1 msec on Windoze.  :-( */

/******* NON WINDOWS DEFINES *********/
#else
#define IFWINDOWS(dothis)
#define IFUNIX(dothis) dothis

#include <errno.h>
#define s_errno errno // ?? Is this OK ??
#include <netdb.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <fcntl.h>
#include <time.h>
#include <sys/time.h>

#define nonblockingsocket(s)  fcntl(s,F_SETFL,O_NONBLOCK)
#define DEFAULT_USLEEP        500        /* MK: Changed from 10 msecs to 0.5 msec == 500 microsecs. for lower latency. Should not be a problem on good OS/X and Linux :-) */
#endif

#ifndef INADDR_NONE
#define INADDR_NONE (-1)
#endif

// Do this hack cause SIGPIPE that kills matlab on any platform???
#ifndef MSG_NOSIGNAL
#define MSG_NOSIGNAL 0
#endif

/* Include header file for matlab mex file functionality */
#include "mex.h"

/********** DEFINES related to pnet own functionality *****************/
/*   Set debuging on/off   */
#define debug_view_con_status(X)   // __debug_view_con_status(X)

#define MAX_CON         100       /* Maximum number of simultanius tcpip connections.*/
#define NAMEBUFF_LEN    100
#define MINBUFFSIZE     1000      /* readbuffer will shrink to this size if datalength is smaller. */
#define CBNAMELEN       30

#define CON_FREE         -1       /* Find a new free struct in struct array */

#define BACKLOG           50       /* How many pending connections queue will hold */

#define double_inf            HUGE_VAL
#define DEFAULT_WRITETIMEOUT  double_inf
#define DEFAULT_READTIMEOUT   double_inf
#define DEFAULT_INPUT_SIZE    50000

/* Different status of a con_info struct handles a file descriptor    */
#define STATUS_FREE       -1
#define STATUS_NOCONNECT   0    // Disconnected pipe that is note closed
#define STATUS_TCP_SOCKET  1
#define STATUS_IO_OK       5    // Used for IS_... test
#define STATUS_UDP_CLIENT  6
#define STATUS_UDP_SERVER  8
#define STATUS_CONNECT     10   // Used for IS_... test
#define STATUS_TCP_CLIENT  11
#define STATUS_TCP_SERVER  12
#define STATUS_UDP_CLIENT_CONNECT 18
#define STATUS_UDP_SERVER_CONNECT 19

#define IS_STATUS_IO_OK(x) ((x)>=STATUS_IO_OK)
#define IS_STATUS_CONNECTED(x) ((x)>=STATUS_CONNECT)
#define IS_STATUS_UDP_NO_CON(x) ((x)==STATUS_UDP_CLIENT || (x)==STATUS_UDP_SERVER)
#define IS_STATUS_TCP_CONNECTED(x) ((x)==STATUS_TCP_CLIENT || (x)==STATUS_TCP_SERVER)

typedef struct
{
    char *ptr;       /* Pointer to buffert. */
    int len;         /* Length of allocated buffert. */
    int pos;         /* Length used of buffer for data storage.*/
} io_buff;

/* Structure that hold all information about a tcpip connection. */
typedef struct
{
    int fid;
    double writetimeout;
    double readtimeout;
    struct sockaddr_in remote_addr;
    io_buff write;
    io_buff read;
    int status;       /* STATUS_... FREE, NOCONNECT, SERVER, CLIENT ... */
    char callback[CBNAMELEN+1];
} con_info;


/////////////////////////////////////////////////////////////////////////////////////////////////
/* Some global variables */
int        gret_args=0;         /* Global variable that holds number of matlab return argumens returned */
int            gnlhs;           /* number of expected outputs */
mxArray       **gplhs;          /* array of pointers to output arguments */
int            gnrhs;           /* number of inputs */
const mxArray  **gprhs;         /* array of pointers to input arguments */

/* Global list with static length of connections structures holding info about current connection */
con_info con[MAX_CON];
int con_index=0;                   /* Current index possition for list of handlers */
unsigned long mex_call_counter=0;  /* Counter that counts how many calls that have been done to pnet */

/***********************************************************************/
void Print_Start_Message(){
    mexPrintf("\n===============================================================================\n"
    "Loaded pnet MEX-file for the tcp/udp/ip-toolbox Compiled @ "
    __DATE__ " " __TIME__  "\n"
    VERSION "\n"
    "Copyright (C) Peter Rydesater, Sweden, et al. , 1998 - 2003\n"
    "GNU General Public License, se license.txt for full license notis.\n"
    "You are allowed to (dynamicaly) link this file with non-free code. \n\n"
    "   http://www.rydesater.com \n\n"
    "===============================================================================\n\n"
    );
}

/***********************************************************************/
int myoptstrcmp(const char *s1,const char *s2)
{
    int val;
    while( (val= toupper(*s1) - toupper(*s2))==0 ){
        if(*s1==0 || *s2==0) return 0;
        s1++;
        s2++;
        while(*s1=='_') s1++;
        while(*s2=='_') s2++;
    }
    return val;
}

/********************************************************************/
/* A "wrapper" function for memory allocation. Most for debuging /tracing purpose */
void *myrealloc(char *ptr,int newsize)
{
    ptr=realloc(ptr,newsize);
    if(ptr==NULL && newsize>0)
        mexErrMsgTxt("Internal out of memory!");
    return ptr;
}

/********************************************************************/
/* A "wrapper" function for memory allocation. Most for debuging /tracing purpose */
void newbuffsize(io_buff *buff,int newsize)
{
    //    fprintf(stderr,"NEWSIZE:%d\n",newsize);
    if(newsize==-1){
        free(buff->ptr);
        buff->ptr=NULL;
        return;
    }
    if(newsize<buff->pos)
        newsize=buff->pos;
    if(newsize<256)
        newsize=256;
    if(newsize>buff->len){   // Grow....
        //    fprintf(stderr,"NEWSIZE UP %d -> %d\n",buff->len,newsize*2);
        buff->ptr=myrealloc(buff->ptr,newsize*2);
        buff->len=newsize*2;
    }else if(newsize*4 < buff->len){ // Decrease...
        //    fprintf(stderr,"NEWSIZE DOWN %d -> %d\n",buff->len,newsize*2);
        buff->ptr=myrealloc(buff->ptr,newsize*2);
        buff->len=newsize*2;
    }
}

/********************************************************************/
mxClassID str2classid(const char *str)
{
    if(myoptstrcmp("CHAR",str)==0)        return mxCHAR_CLASS;
    if(myoptstrcmp("DOUBLE",str)==0)    return mxDOUBLE_CLASS;
    if(myoptstrcmp("SINGLE",str)==0)    return mxSINGLE_CLASS;
    if(myoptstrcmp("INT8",str)==0)        return mxINT8_CLASS;
    if(myoptstrcmp("UINT8",str)==0)        return mxUINT8_CLASS;
    if(myoptstrcmp("INT16",str)==0)        return mxINT16_CLASS;
    if(myoptstrcmp("UINT16",str)==0)    return mxUINT16_CLASS;
    if(myoptstrcmp("INT32",str)==0)        return mxINT32_CLASS;
    if(myoptstrcmp("UINT32",str)==0)    return mxUINT32_CLASS;
    return mxCHAR_CLASS; // Default to char;
}

/********************************************************************/
mxClassID classid2size(const mxClassID id)
{
    if(id==mxCHAR_CLASS)    return 1;
    if(id==mxDOUBLE_CLASS)    return sizeof(double);
    if(id==mxSINGLE_CLASS)    return sizeof(float);
    if(id==mxINT8_CLASS)    return 1;
    if(id==mxUINT8_CLASS)    return 1;
    if(id==mxINT16_CLASS)    return 2;
    if(id==mxUINT16_CLASS)    return 2;
    if(id==mxINT32_CLASS)    return 4;
    if(id==mxUINT32_CLASS)    return 4;
    mexErrMsgTxt("Non supported datatype!");
    return 0;
}

/* Windows implementation of perror() function */
#ifdef WIN32
/********************************************************************/
/*
 * void perror(const char *context )
 * {
 *    int wsa_err;
 *    wsa_err = WSAGetLastError();
 *    mexPrintf( "[%s]: WSA error: ", context );
 *    switch ( wsa_err )
 *    {
 *    case WSANOTINITIALISED: mexPrintf( "WSANOTINITIALISED\n" ); break;
 *    case WSAENETDOWN:       mexPrintf( "WSAENETDOWN      \n" ); break;
 *    case WSAEADDRINUSE:     mexPrintf( "WSAEADDRINUSE    \n" ); break;
 *    case WSAEACCES:         mexPrintf( "WSAEACCES        \n" ); break;
 *    case WSAEINTR:          mexPrintf( "WSAEINTR         \n" ); break;
 *    case WSAEINPROGRESS:    mexPrintf( "WSAEINPROGRESS   \n" ); break;
 *    case WSAEALREADY:       mexPrintf( "WSAEALREADY      \n" ); break;
 *    case WSAEADDRNOTAVAIL:  mexPrintf( "WSAEADDRNOTAVAIL \n" ); break;
 *    case WSAEAFNOSUPPORT:   mexPrintf( "WSAEAFNOSUPPORT  \n" ); break;
 *    case WSAEFAULT:         mexPrintf( "WSAEFAULT        \n" ); break;
 *    case WSAENETRESET:      mexPrintf( "WSAENETRESET     \n" ); break;
 *    case WSAENOBUFS:        mexPrintf( "WSAENOBUFS       \n" ); break;
 *    case WSAENOTSOCK:       mexPrintf( "WSAENOTSOCK      \n" ); break;
 *    case WSAEOPNOTSUPP:     mexPrintf( "WSAEOPNOTSUPP    \n" ); break;
 *    case WSAESHUTDOWN:      mexPrintf( "WSAESHUTDOWN     \n" ); break;
 *    case WSAEWOULDBLOCK:    mexPrintf( "WSAEWOULDBLOCK   \n" ); break;
 *    case WSAEMSGSIZE:       mexPrintf( "WSAEMSGSIZE      \n" ); break;
 *    case WSAEHOSTUNREACH:   mexPrintf( "WSAEHOSTUNREACH  \n" ); break;
 *    case WSAEINVAL:         mexPrintf( "WSAEINVAL        \n" ); break;
 *
 *    case WSAECONNREFUSED:   mexPrintf( "WSAECONNREFUSED  \n" ); break;
 *    case WSAECONNABORTED:   mexPrintf( "WSAECONNABORTED  \n" ); break;
 *    case WSAECONNRESET:     mexPrintf( "WSAECONNRESET    \n" ); break;
 *    case WSAEISCONN:        mexPrintf( "WSAEISCONN       \n" ); break;
 *    case WSAENOTCONN:       mexPrintf( "WSAENOTCONN      \n" ); break;
 *    case WSAETIMEDOUT:      mexPrintf( "WSAETIMEDOUT     \n" ); break;
 *    default:                mexPrintf( "Unknown(%d) error!\n", wsa_err ); break;
 *    }
 *    return;
 * }
 **/
#endif

/********************************************************************/
/*Makes byte swapping, or not depending on the mode argument        */
void byteswapdata(char *ptr,const int elements,const int elementsize,int mode)
{
    // MODE=0 Do nothing, MODE=1 Swap, MODE=2 network byte order, MODE=3 Intel byte order.
    #ifndef SWAPDATA
    #define SWAPDATA(a,b) { a^=b; b^=a; a^=b; }
    #endif
    // A little smart ckeck of byte the machine byte order.
    const int ordertest=1;
    const char *is_intel_order=(const char *)(&ordertest);
    //    fprintf(stderr,"SWAP FUNCTION...E:%d SI:%d\n",elements,elementsize);
    if(elementsize<2) return;
    if(is_intel_order[0]==1 && mode==2)   mode=1;
    if(is_intel_order[0]==0 && mode==3)   mode=1;
    if(mode==1){
        int e;
        //    fprintf(stderr,"SWAP DATA\n");
        switch(elementsize){
            case 2: for(e=0;e<elements*elementsize;e+=elementsize)
                SWAPDATA(ptr[e],ptr[e+1]) break;
            case 3: for(e=0;e<elements*elementsize;e+=elementsize)
                SWAPDATA(ptr[e],ptr[e+2])  break;
            case 4: for(e=0;e<elements*elementsize;e+=elementsize)
                SWAPDATA(ptr[e],ptr[e+3]) SWAPDATA(ptr[e+1],ptr[e+2])  break;
            case 5: for(e=0;e<elements*elementsize;e+=elementsize)
                SWAPDATA(ptr[e],ptr[e+4]) SWAPDATA(ptr[e+1],ptr[e+3])  break;
            case 6: for(e=0;e<elements*elementsize;e+=elementsize)
                SWAPDATA(ptr[e],ptr[e+5]) SWAPDATA(ptr[e+1],ptr[e+4]) SWAPDATA(ptr[e+2],ptr[e+3])  break;
            case 7: for(e=0;e<elements*elementsize;e+=elementsize)
                SWAPDATA(ptr[e],ptr[e+6]) SWAPDATA(ptr[e+1],ptr[e+5]) SWAPDATA(ptr[e+2],ptr[e+4])  break;
            case 8: for(e=0;e<elements*elementsize;e+=elementsize)
                SWAPDATA(ptr[e],ptr[e+7]) SWAPDATA(ptr[e+1],ptr[e+6]) SWAPDATA(ptr[e+2],ptr[e+5]) SWAPDATA(ptr[e+3],ptr[e+4])  break;
        }
    }
}

/********************************************************************/
/*Makes byte swapping, or not depending on the mode argument        */
void byteswapcopy(char *dest,char *src,const int elements,const int elementsize,int mode)
{
    // MODE=0 Do nothing, MODE=1 Swap, MODE=2 network byte order, MODE=2 Intel byte order.
    // A little smart ckeck of byte the machine byte order.
    const int ordertest=1;
    const char *is_intel_order=(const char *)(&ordertest);
    if(is_intel_order[0]==1 && mode==2)   mode=1;
    if(is_intel_order[0]==0 && mode==3)   mode=1;
    //    fprintf(stderr,"SWAP COPY E:%d SI:%d SWAP:%d\n",elements,elementsize,mode);
    if(mode==1){
        int e,n;
        //    fprintf(stderr,"SWAP COPY\n");
        for(e=0;e<elements;e++){
            char *dp=&dest[e*elementsize];
            char *sp=&src[e*elementsize];
            for(n=0;n<elementsize;n++){
                dp[n]=sp[elementsize-1-n];
                //        fprintf(stderr,"E:%d/%d N:%d/%d\n",e,elements,n,elementsize);
            }
        }
        //    fprintf(stderr,"SWAP COPY END\n");
    }
    else
        memmove(dest,src,elements*elementsize);
}

/********************************************************************/
/* DEBUGING FUNCTION */
void __debug_view_con_status(char *str)
{
    int a;
    mexPrintf("%s\n",str);
    for(a=0;a<5;a++)
    {
        mexPrintf("[%02d] FID:%02d STATUS:%02d WRT.POS:%d RD.POS:%d ",a,con[a].fid,con[a].status,con[a].write.pos,con[a].read.pos);
        if(con[a].read.ptr)
            mexPrintf("RD: %02x %02x %02x %02x %02x %02x %02x %02x ",
                      (int)con[a].read.ptr[0],(int)con[a].read.ptr[1],(int)con[a].read.ptr[2],(int)con[a].read.ptr[3],
                      (int)con[a].read.ptr[4],(int)con[a].read.ptr[5],(int)con[a].read.ptr[6],(int)con[a].read.ptr[7]);
            if(con[a].write.ptr)
                mexPrintf("WR: %02x %02x %02x %02x %02x %02x %02x %02x ",
                          (int)con[a].write.ptr[0],(int)con[a].write.ptr[1],(int)con[a].write.ptr[2],(int)con[a].write.ptr[3],
                          (int)con[a].write.ptr[4],(int)con[a].write.ptr[5],(int)con[a].write.ptr[6],(int)con[a].write.ptr[7]);
                if(a==con_index)
                    mexPrintf("<--\n");
                else
                    mexPrintf("\n");
    }
    mexPrintf("--------------------\n");
}

#ifdef WIN32
/********************************************************************/
/* Portable time function using matlabs NOW                         */
double my_now(){
    double sec;

    sec = ((double) timeGetTime()) / 1000.0;
    return(sec);

    /*
     *    double dotimenow;
     *    static double lastdotime;
     *    mxArray *plhs[1]={NULL};
     *    mxArray *prhs[1]={NULL};
     *    mexCallMATLAB(1,plhs,0,prhs,"now");
     *    sec=mxGetScalar(plhs[0])*60*60*24; //Return time as sec from 1970
     *    dotimenow=floor(sec*10)/10; // Do calls every 1/10 Sec
     *    mxDestroyArray(plhs[0]);
     *    if(lastdotime!=dotimenow){  //Call drawnow once evry X second
     *        int ret=mexCallMATLAB(0,plhs,0,prhs,"drawnow");
     *        //mexPrintf("wait... drawnow returns: %d\n",ret);
     *        lastdotime= dotimenow;
}
return sec;
*/
}
#else
double my_now()
{
    struct timeval tv;
    double sec;
    gettimeofday(&tv, NULL);
    sec = (double) tv.tv_sec + ((double) tv.tv_usec) / (double) 1e6;
    return(sec);
}
#endif

/*******************************************************************************/
/* Checks that given index is valid index and set current index, "con_index"   */
/* to that point. If index is CON_FREE (-1) then is search done for a free one */
/* Returns 1 On success. 0 on error                                            */
int move_con(int idx)
{
    if(idx>=MAX_CON)
        mexErrMsgTxt("Unvalid value of handler!\n");
    if(idx>=0)
    {
        con_index=idx;
        if(con[con_index].status==STATUS_FREE)
            mexErrMsgTxt("No valid handler! already closed?");
        return 1;
    }
    debug_view_con_status("START MOVE");
    if(idx==CON_FREE)    /* Move con_index until it find a free non used struct */
    {
        for(con_index=0;con_index<MAX_CON;con_index++)
        {
            debug_view_con_status("STEP MOVE");
            if(con[con_index].status==STATUS_FREE)
                return 1;
        }
        mexErrMsgTxt("To many open connection! Forgot to close old connections?");
    }
    if(idx<0)
        mexErrMsgTxt("Unvalid value of handler!");
    return 0;
}

/**********************************************************************************************/
/* Returns true if specified argument exist                                                  */
int my_mexIsInputArgOK(const int argno)
{
    //    fprintf(stderr,"IS_INPUT_ARG_OK NO:%d of %d\n",argno,gnrhs);        // DEBUG
    if(gnrhs>argno)
        return 1;
    return 0;
}

/******************************************************************************************************/
/* Returns specified input argument as scalar. Global and error tolerant replacement for mxGetScalar */
const mxArray *my_mexInputArg(const int argno)
{
    //    fprintf(stderr,"GET_INPUT_ARG NO:%d\n",argno);                      // DEBUG
    if(!my_mexIsInputArgOK(argno))
        mexErrMsgTxt("Missing input argument.");
    return gprhs[argno];
}

/****************************************************************************************************************/
/* Returns pointer to static buffer (max 80 chars) holding a string in argument argno Global and error tolerant */
const char *my_mexInputOptionString(const int argno)
{
    static char buff[80+1];
    buff[0]=0;
    if(my_mexIsInputArgOK(argno))
        if(mxIsChar(my_mexInputArg(argno)))
            mxGetString(my_mexInputArg(argno),buff,80);
        return buff;
}

/****************************************************************************************************************/
/* Returns pointer to static buffer (max 80 chars) holding the first string with non numeric,nonempty contents  */
const char *my_mexFindInputString(int argno)
{
    const char *str;
    static char buff[1];
    buff[0]=0;
    while(my_mexIsInputArgOK(argno)){
        str=my_mexInputOptionString(argno);
        if(!isdigit(str[0]) && str[0]!=0 )
            return str;
        argno++;
    }
    return buff;
}

/*********************************************************************/
/* Returns true if if finds option opt at argument argno or later    */
int my_mexFindInputOption(int argno,const char *opt)
{
    char buff[80+1];
    while(my_mexIsInputArgOK(argno))
    {
        buff[0]=0;
        mxGetString(my_mexInputArg(argno),buff,80);
        if(myoptstrcmp(buff,opt)==0)
            return 1;
        argno++;
    }
    return 0;
}

/***********************************************************************************************************/
/* Returns specified input arguments cell as scalar. Global and error tolerant replacement for mxGetScalar */
double my_mexInputCell(const int argno,int cell_no)
{
    if(!mxIsDouble(my_mexInputArg(argno)))
        mexErrMsgTxt("Argument has wrong datatype.");
    if(cell_no>=mxGetNumberOfElements(my_mexInputArg(argno)))
        mexErrMsgTxt("Argument has wrong number of cells");
    return mxGetPr(my_mexInputArg(argno))[cell_no];
}

/******************************************************************************************************/
/* Returns specified input argument as scalar or a scalar product of its elements.                    */
/* Global and error tolerant replacement for mxGetScalar                                              */
int my_mexInputSize(const int argno)
{
    if(!my_mexIsInputArgOK(argno))
        return DEFAULT_INPUT_SIZE;
    if(mxIsChar(my_mexInputArg(argno))){
        const int ch=(my_mexInputOptionString(argno)[0]);
        if(!isdigit(ch))
            return DEFAULT_INPUT_SIZE;
        else
            return atoi(my_mexInputOptionString(argno));
    }
    if(mxGetNumberOfElements(my_mexInputArg(argno))>1){
        int val=1,n=0,el=mxGetNumberOfElements(my_mexInputArg(argno));
        for(n=0;n<el;n++)
            val*=(int)my_mexInputCell(argno,n);
        return val;
    }else
        return (int)mxGetScalar(my_mexInputArg(argno));
}

/******************************************************************************************************/
/* Returns specified input argument as scalar. Global and error tolerant replacement for mxGetScalar */
double my_mexInputScalar(const int argno)
{
    if(mxIsChar(my_mexInputArg(argno)))
        return atof(my_mexInputOptionString(argno));
    else
        return mxGetScalar(my_mexInputArg(argno));
}

/********************************************************************/
/* Copys a matlab Char array to a char * string buffer              */
int my_mexInputArray2Buff(const int argno,io_buff *buff)
{
    mxClassID id=mxGetClassID(my_mexInputArg(argno));
    int si=classid2size(id);
    int len=mxGetNumberOfElements(my_mexInputArg(argno));
    int swap=2;
    if(my_mexFindInputOption(argno+1,"NATIVE"))  swap=0;
    if(my_mexFindInputOption(argno+1,"SWAP"))    swap=1;
    if(my_mexFindInputOption(argno+1,"NETWORK")) swap=2;
    if(my_mexFindInputOption(argno+1,"INTEL"))   swap=3;

    newbuffsize(buff,buff->pos+len*si);

    if(id==mxCHAR_CLASS){
        mxChar *ptr = (mxChar *)mxGetData(gprhs[argno]);
        int a;
        for(a=0;a<len;a++)
            buff->ptr[buff->pos++]=(char)(unsigned char)ptr[a];
    }else{
        char *ptr = (char *)mxGetData(gprhs[argno]);
        byteswapcopy(&buff->ptr[buff->pos],ptr,len,si,swap);
        buff->pos+=(len*si);
    }
    return len;
}

/*******************************************************************************/
/* Puts double scalar in matlab variable in the array of return argument for mex*/
void my_mexReturnValue(double val)
{
    if((gret_args>gnlhs) && (gret_args>1))
        return;
    gplhs[gret_args]=mxCreateDoubleMatrix(1,1,mxREAL);
    if(gplhs[gret_args]==NULL)
        mexErrMsgTxt("Matrix creation error! Lack of memory?");
    else
    {
        *mxGetPr(gplhs[gret_args])=val;
        gret_args++;
    }
    //    mexPrintf("DEBUG MEX RETURN VALUE:%g\n",val);  // DEBUG
}

/*******************************************************************************/
/* Puts double matrix in matlab variable in the array of return argument for mex*/
void my_mexReturnMatrix(int rows, int cols, double *vals)
{
    double *pr;
    if((gret_args>gnlhs) && (gret_args>1))
        return;
    gplhs[gret_args] = mxCreateDoubleMatrix( rows, cols, mxREAL);
    if( gplhs[gret_args] == NULL)
        mexErrMsgTxt("Matrix creation error");
    pr = (double *)mxGetPr(gplhs[gret_args]);
    memcpy(pr,vals,rows*cols*sizeof(double));
    gret_args++;
}

/********************************************************************************/
/* Puts string as matlab char char variable in array of return argument for mex */
void my_mexReturnString(const char *str)
{
    if(gret_args>gnlhs && gret_args>1)
        return;
    gplhs[gret_args]=mxCreateString(str);
    if(gplhs[gret_args]==NULL)
        mexErrMsgTxt("String creation error! Lack of memory?");
    gret_args++;
}

/******************************************************************************/
/* Puts data from buffer into next MATLAB return Array                        */
void my_mexReturnArrayFromBuff(const int argno,io_buff *buff,const int line)
{
    const int maxelements=my_mexInputSize(argno);
    const mxClassID id=str2classid(my_mexInputOptionString(argno+1));
    mwSize dims[20]={0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0, 0,0,0,0,0 };
    const int si=classid2size(id);
    int returnelements= ( (buff->pos/si)< maxelements )?(buff->pos/si):maxelements;
    int deleteelements=returnelements;
    int swap=2;
    int return_no_dims= my_mexIsInputArgOK(argno) && !mxIsChar(my_mexInputArg(argno))?mxGetNumberOfElements(my_mexInputArg(argno)):1;

    if(my_mexFindInputOption(argno+1,"NATIVE"))  swap=0;
    if(my_mexFindInputOption(argno+1,"SWAP"))    swap=1;
    if(my_mexFindInputOption(argno+1,"NETWORK")) swap=2;
    if(my_mexFindInputOption(argno+1,"INTEL"))   swap=3;
    if(return_no_dims>20)
        mexErrMsgTxt("To many dimentions to return.");
    debug_view_con_status("GET_ARRAY");
    if(line){
        int n=-7;
        if(id!=mxCHAR_CLASS && return_no_dims)
            mexErrMsgTxt("'READLINE' works only with datatype char and non fixed blocksize");
        for(n=0;n<returnelements;n++)
            if(buff->ptr[n]=='\n')
                break;
            if(n==maxelements)                             // If no new-line found inside limit...
                deleteelements=returnelements=maxelements; // ...return first part of splited line.
                else if(n==returnelements)                     // If new-line not recived inside limit...
                    deleteelements=returnelements=0;           // ...return empty string, and delete nothing.
                    else if(n>0 && buff->ptr[n-1]=='\r')           // If(*3) new-line, return line of char but not nl chars.
                        deleteelements=2+(returnelements=n-1);
                    else
                        deleteelements=1+(returnelements=n);
                    return_no_dims=1;
    }
    if(return_no_dims>1){                 // If shape of return argument is specified.....
        if(returnelements==maxelements){        // ...then only accept correct shape.
            int n;
            for(n=0;n<return_no_dims;n++)
                dims[n]=(mwSize) my_mexInputCell(argno,n);
        }
    }else if(returnelements>0){           // else... Just return row of available elements
        dims[0] = 1;
        dims[1] = (mwSize) returnelements;
        return_no_dims=2;
    }
    if(! (gret_args>gnlhs && gret_args>1) ){
        /* Create a 2-Dimensional character mxArray.*/
        if( dims[0]==0)
            gplhs[gret_args] = mxCreateNumericArray(0,dims,id,mxREAL);
        else
            gplhs[gret_args] = mxCreateNumericArray(return_no_dims,dims,id,mxREAL);
        if(gplhs[gret_args] == NULL)
            mexErrMsgTxt("Could not create return array.");
        if(dims[0]!=0){
            if(id==mxCHAR_CLASS){
                int i;
                mxChar *p=(mxChar *)mxGetData(gplhs[gret_args]);
                for(i=0;i<returnelements;i++)
                    p[i]=buff->ptr[i];
            }else{
                char *p=(char *)mxGetData(gplhs[gret_args]);
                byteswapcopy(p,buff->ptr,returnelements,si,swap);
            }
        }
        gret_args++;
    }
    //    debug_view_con_status("GET_ARRAY NSTAN KLAR");
    // Delete from read buffer if not "VIEW" option and dims filled
    if(my_mexFindInputOption(argno+1,"VIEW")==0 && deleteelements>0 ){
        buff->pos-=deleteelements*si;
        memmove(buff->ptr,&buff->ptr[deleteelements*si],buff->pos);
        newbuffsize(buff,buff->pos);
    }
    // mexPrintf("DEBUG MEX RETURN ARRAY OF:%d\n",returnelements);  // DEBUG

    //    fprintf(stderr,"DIMS:[%d %d] DEL:%d RET:%d SI:%d POS:%d LEN:%d PTR:%08X\n",
    //        dims[0],dims[1],deleteelements,returnelements,si,buff->pos,buff->len,buff->ptr);
}


/**********************************************************************/
int ipv4_lookup(const char *hostname,int port)
{
    struct in_addr addr;    /* my address information */
    /* Try IP address */
    addr.s_addr=inet_addr(hostname);
    if (addr.s_addr==INADDR_NONE){
        /*Can't resolve host string as dot notation IP number...
         *      try lookup IP from hostname */
        struct hostent *he;
        //    fprintf(stderr,"Trying nameserverlookup:%s\n",hostname);
        he=gethostbyname(hostname);
        if(he==NULL){
            mexPrintf("\nUNKNOWN HOST:%s\n",hostname);
            return -1;  /* Can not lookup hostname */
        }
        addr = *((struct in_addr *)he->h_addr);
    }
    con[con_index].remote_addr.sin_family=AF_INET;
    con[con_index].remote_addr.sin_port=htons(port);
    con[con_index].remote_addr.sin_addr=addr;
    memset(&con[con_index].remote_addr.sin_zero, 0,8);
    return 0;
}


/**********************************************************************/
/* Writes from specified position (pointer) in buffer of spec. length */
int writedata()
{
    const double timeoutat=my_now()+con[con_index].writetimeout;
    int   len=con[con_index].write.pos;
    const char *ptr=con[con_index].write.ptr;
    const int  fid=con[con_index].fid;
    int sentlen=0;
    int retval=0;
    int lastsize=1000000;
    if(con[con_index].status<STATUS_IO_OK)
        return 0;
    //    if( !IS_STATUS_TCP_CONNECTED(con[con_index].status)  && len>65534 )   // TODO: Ta bort!
    //    len=65534;
    while(sentlen<len)
    {
        if(lastsize<1000)
            usleep(DEFAULT_USLEEP);
        if(IS_STATUS_UDP_NO_CON(con[con_index].status))
            retval = sendto(fid,&ptr[sentlen],len-sentlen,MSG_NOSIGNAL,
                            (struct sockaddr *)&con[con_index].remote_addr,sizeof(struct sockaddr));
            else
                retval=send(fid,&ptr[sentlen],len-sentlen,MSG_NOSIGNAL);
            lastsize=retval>0?retval:0;
        sentlen+=lastsize;
        /*    if( retval==0){
         *        mexPrintf("\nREMOTE HOST DISCONNECTED\n");
         *        con[con_index].status=STATUS_NOCONNECT;
         *        break;
    }*/
        if(retval<0 && s_errno!=EWOULDBLOCK
            //       IFWINDOWS( && s_errno!=WSAECONNRESET  )           // DEBUG: REMOVE THIS LINE?
        ){
            con[con_index].status=STATUS_NOCONNECT;
            perror( "sendto() / send()" );
            mexPrintf("\nREMOTE HOST DISCONNECTED\n");
            break;
        }
        if( !IS_STATUS_TCP_CONNECTED(con[con_index].status) && sentlen>0 )
            break;
        if(timeoutat<=my_now())
            break;
    }
    con[con_index].write.pos-=sentlen;
    memmove(con[con_index].write.ptr, &con[con_index].write.ptr[sentlen], con[con_index].write.pos);
    newbuffsize(&con[con_index].write,con[con_index].write.pos);
    return sentlen;
}

/********************************************************************/
/* Init current record with values                                  */
void init_con(int fid,int status)
{
    memset(&con[con_index],0,sizeof(con_info));
    con[con_index].fid=fid;
    con[con_index].status=status;
    con[con_index].writetimeout=DEFAULT_WRITETIMEOUT;
    con[con_index].readtimeout=DEFAULT_READTIMEOUT;
}

/********************************************************************/
/* Close con struct                                                 */
void close_con()
{
    if(con[con_index].fid>=0)
        close(con[con_index].fid);
    else
        mexWarnMsgTxt("Closing already closed connection!");
    newbuffsize(&con[con_index].write,-1);
    newbuffsize(&con[con_index].read,-1);
    init_con(-1,STATUS_FREE);
}

/*******************************************************************      */
/* Function to close all still open tcpip connections */
int closeall(void)
{
    int flag=0;
    for(con_index=0;con_index<MAX_CON;con_index++)
        if(con[con_index].fid>=0) {  /* Already closed?? */
            close_con();
            flag=1;
        }
        return flag;
}

/****************************************************************************/
/* This function is called on unloading of mex-file                         */
void CleanUpMex(void)
{
    if(closeall()) /* close all still open connections...*/
        mexWarnMsgTxt("Unloading mex file. Unclosed tcp/udp/ip connections will be closed!");
    IFWINDOWS(   WSACleanup();  );
}

/********************************************************************/
/* Read to fill input buffer with specified length from UDP or TCP network*/
int read2buff(const int len,int newline,int noblock)
{
    const double timeoutat=my_now()+con[con_index].readtimeout;
    int retval=-1;

    if(len<con[con_index].read.pos)              /* If enouth in buffer then return */
        return len;
    if(0==IS_STATUS_IO_OK(con[con_index].status))/* If not read/write fid (broken pipe) then exit.*/
        if(len<con[con_index].read.pos)
            return len;
        else
            return con[con_index].read.pos;

        /* Resize readbuffer to needed size */
        if(con[con_index].read.len<len)
            newbuffsize(&con[con_index].read,len);

        while(1){
            int readlen=len-con[con_index].read.pos;
            //    mexPrintf("DEBUG: READLINE: readlen:%d\n",readlen);
            if(readlen>0){
                if(IS_STATUS_CONNECTED(con[con_index].status))
                    retval=recv(con[con_index].fid,&con[con_index].read.ptr[con[con_index].read.pos],readlen ,MSG_NOSIGNAL);
                else{
                    struct sockaddr_in my_addr;
                    int fromlen=sizeof(my_addr);

                    // Copy 0.0.0.0 adress and 0 port to remote_addr as init-value.
                    memset(&my_addr,0,sizeof(my_addr));
                    con[con_index].remote_addr.sin_addr = my_addr.sin_addr;
                    con[con_index].remote_addr.sin_port = my_addr.sin_port;
                    retval=recvfrom(con[con_index].fid,&con[con_index].read.ptr[con[con_index].read.pos],
                                    readlen,MSG_NOSIGNAL,(struct sockaddr *)&my_addr, &fromlen);
                    if (retval>0){
                        con[con_index].remote_addr.sin_addr = my_addr.sin_addr;
                        con[con_index].remote_addr.sin_port = htons((unsigned short int)ntohs(my_addr.sin_port));
                    }
                }
                if( retval==0){
                    mexPrintf("\nREMOTE HOST DISCONNECTED\n");
                    con[con_index].status=STATUS_NOCONNECT;
                    break;
                }
                if(retval<0 && s_errno!=EWOULDBLOCK
                    //           IFWINDOWS( && s_errno!=WSAECONNRESET )// DEBUG: REMOVE THIS LINE?
                ) {
                    con[con_index].status=STATUS_NOCONNECT;
                    perror( "recvfrom() or recv()" );
                    break;
                }
            }
            //    fprintf(stderr,"RET:%d/%d ",retval,s_errno);
            readlen=retval>0?retval:0;
            con[con_index].read.pos+=readlen;
            if( !IS_STATUS_TCP_CONNECTED(con[con_index].status) && con[con_index].read.pos>0 )
                break;
            if( con[con_index].read.pos>=len )
                break;
            if(noblock || timeoutat<=my_now())
                break;
            if(newline){
                int n;
                for(n=0;n<con[con_index].read.pos;n++)
                    if(con[con_index].read.ptr[n]=='\n')
                        return con[con_index].read.pos;
            }
            if(readlen<1000)
                usleep(DEFAULT_USLEEP);
        }
        return con[con_index].read.pos;
}

/***************************************************************************/   // BORT???
/* Read specified length & type from UDP or TCP network to input buffer    */
int readtype2buff(int len,mxClassID datatype,int newline,int noblock)
{
    const int si=classid2size(datatype);
    return read2buff(len*si,newline,noblock)/si;
}

/********************************************************************/
/* Function Creating a tcpip connection and returns handler number  */
int tcp_connect(const char *hostname,const int port)
{
    if(ipv4_lookup(hostname,port)==-1)
        return -1;
    con[con_index].fid=socket(AF_INET, SOCK_STREAM, 0);
    if(con[con_index].fid== CON_FREE){
        /*Can't open socket */
        close_con();
        return -1;
    }
    if(connect(con[con_index].fid,(struct sockaddr *)&con[con_index].remote_addr,sizeof(struct sockaddr)) == -1){
        /*Can't connect to remote host. */
        close_con();
        return -1;
    }
    con[con_index].status=STATUS_TCP_CLIENT;
    nonblockingsocket(con[con_index].fid); /* Non blocking read! */
    return con_index;
}

/*******************************************************************
 * Function Creating a TCP server socket
 * or a connectionless UDP client socket.
 */
int tcp_udp_socket(int port,int dgram_f)
{
    int sockfd;
    struct sockaddr_in my_addr;    /* my address information */
    const int on=1;

    #ifndef WIN32
    #ifndef IPTOS_LOWDELAY
    #define IPTOS_LOWDELAY          0x10
    #endif
    int tos = IPTOS_LOWDELAY;
    #endif

    if(dgram_f)
        sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    else
        sockfd = socket(AF_INET, SOCK_STREAM, 0);
    if(sockfd==-1)
        return -1;
    my_addr.sin_family = AF_INET;         /* host byte order */
    my_addr.sin_port = htons(port);       /* short, network byte order */
    my_addr.sin_addr.s_addr = INADDR_ANY; /* auto-fill with my IP */
    memset(&(my_addr.sin_zero),0, 8);        /* zero the rest of the struct */
    setsockopt(sockfd,SOL_SOCKET,SO_REUSEADDR,(const char *)&on,sizeof(on));
    if(bind(sockfd, (struct sockaddr *)&my_addr, sizeof(struct sockaddr))== -1)
    {
        close(sockfd);
        return -1;
    }
    listen(sockfd,BACKLOG);
    nonblockingsocket(sockfd);

    /* Try to enable low-latency send/receive operations on socket: */
    #ifndef WIN32
    if (-1 == setsockopt(sockfd, IPPROTO_IP, IP_TOS, &tos, sizeof(tos))) {
        mexPrintf("pnet: Warning: Could not enable low-latency mode on socket! [%s]\n", strerror(errno));
    }
    #endif

    return sockfd;
}

/*****************************************************************/
/* Listen to socket and returns connection if there is one...
 *   else it returns -1 */
int tcpiplisten(int noblock)
{
    const double timeoutat=my_now()+con[con_index].readtimeout;
    int new_fd;
    const int sock_fd= con[con_index].fid;
    int sin_size = sizeof(struct sockaddr_in);
    move_con(CON_FREE);        /* Find a new free con struct for the new connection.... */
    while(1){
        if ((new_fd = accept(sock_fd, (struct sockaddr *)&con[con_index].remote_addr,&sin_size)) > -1)
            break;
        if(noblock || timeoutat<=my_now())
            return -1;
        usleep(DEFAULT_USLEEP);
    }
    nonblockingsocket(new_fd); /* Non blocking read! */
    setsockopt(new_fd,SOL_SOCKET,SO_KEEPALIVE,(void *)1,0); /* realy needed? */
    con[con_index].fid=new_fd;
    con[con_index].status=STATUS_TCP_SERVER;
    return con_index;
}

/*****************************************************************/
/* Make a UDP socket be a "connected" UDP socket                 */
int udp_connect(const char *hostname,int port)
{
    if(con[con_index].status < STATUS_UDP_CLIENT)
        mexErrMsgTxt("Must pass UDP client or server handler!");
    if(ipv4_lookup(hostname,port)==-1)
        return -1;
    if(connect(con[con_index].fid,(struct sockaddr *)&con[con_index].remote_addr,sizeof(struct sockaddr)) == -1)
        return -1; /*Can't connect to remote host. */
        if(con[con_index].status == STATUS_UDP_CLIENT)
            con[con_index].status = STATUS_UDP_CLIENT_CONNECT;
        else
            con[con_index].status = STATUS_UDP_SERVER_CONNECT;
        nonblockingsocket(con[con_index].fid); /* Non blocking read! */
        return con[con_index].status;
}

/*****************************************************************/
/*                                                               */
/*    ----Main function that is called from matlab--------       */
/*                                                               */
void mexFunction(
    int           nlhs,           /* number of expected outputs */
    mxArray       *plhs[],        /* array of pointers to output arguments */
    int           nrhs,           /* number of inputs */
    const mxArray *prhs[]         /* array of pointers to input arguments */
)
{
    char fun[80+1];
    /* Initialization on first call to the mex file */
    if(mex_call_counter==0)
    {
        #ifdef WIN32
        WORD wVersionRequested;
        WSADATA wsaData;
        int wsa_err;
        wVersionRequested = MAKEWORD( 2, 0 );
        wsa_err = WSAStartup( wVersionRequested, &wsaData );
        if (wsa_err)
            mexErrMsgTxt("Error starting WINSOCK32.");
        #endif
        Print_Start_Message();

        mexAtExit(CleanUpMex);
        /* Init all connecttions to to free */
        for(con_index=0;con_index<MAX_CON;con_index++)
            init_con(-1,STATUS_FREE);
        con_index=0;
    }
    mex_call_counter++;
    debug_view_con_status("ENTER_MEX");

    /* GLOBAL IN-OUT ARGUMENTS */
    gnlhs=nlhs;       /* number of expected outputs */
    gplhs=plhs;       /* array of pointers to output arguments */
    gnrhs=nrhs;       /* number of inputs */
    gprhs=prhs;       /* array of pointers to input arguments */
    gret_args=0;      /* No return argumens returned */


    if(mxIsChar(my_mexInputArg(0))){
        /* GET FIRST ARGUMENT -- The "function" name */
        strncpy(fun,my_mexInputOptionString(0),80);
        //    mexPrintf("DEBUG MEX(1):[%d] %s\n",con_index,fun);   // DEBUG

        /* Find of the function name corresponds to a non connection associated function */
        if(myoptstrcmp(fun,"CLOSEALL")==0){
            closeall();
            return;
        }
        if(myoptstrcmp(fun,"TCPCONNECT")==0){
            const int port=(int)my_mexInputScalar(2);
            const char *hostname=my_mexInputOptionString(1);
            move_con(STATUS_FREE);
            my_mexReturnValue(tcp_connect(hostname,port));
            return;
        }
        if(myoptstrcmp(fun,"TCPSOCKET")==0){
            const int fd=tcp_udp_socket((int)my_mexInputScalar(1), 0);
            if(fd>=0){
                move_con(STATUS_FREE);
                init_con(fd,STATUS_TCP_SOCKET);
                my_mexReturnValue(con_index);
            }else
                my_mexReturnValue(-1);
            return;
        }
        if(myoptstrcmp(fun,"UDPSOCKET")==0){
            const int fd=tcp_udp_socket((int)my_mexInputScalar(1), 1);
            if(fd>=0){
                move_con(STATUS_FREE);
                init_con(fd,STATUS_UDP_CLIENT);
                my_mexReturnValue(con_index);
            }else
                my_mexReturnValue(-1);
            return;
        }
        if(myoptstrcmp(fun,"DOCALLBACKS")==0){
            return;
        }
    }
    /* Get connection handler and suppose that it is a connection assosiated function */
    /* Find given handel */
    //       if(strncasecmp(fun,"DEF",3)!=0)
    if(move_con((int)my_mexInputScalar(0))==0)
        mexErrMsgTxt("Unknown connection handler");
    strncpy(fun,my_mexInputOptionString(1),80);
    //   mexPrintf("DEBUG MEX(2):[%d] %s\n",con_index,fun);   // DEBUG
    debug_view_con_status("CON_MOVED!!");

    if(myoptstrcmp(fun,"CLOSE")==0){
        close_con();
        return;
    }
    if(myoptstrcmp(fun,"TCPLISTEN")==0){
        if(con[con_index].status!=STATUS_TCP_SOCKET)
            mexErrMsgTxt("Invalid socket for LISTEN, Already open, or UDP?...");
        my_mexReturnValue(tcpiplisten(my_mexFindInputOption(2,"noblock")));
        return;
    }
    /* MATLAB called with status = UDP_CONNECT(fid, ip, port) */
    if(myoptstrcmp(fun,"UDPCONNECT")==0){
        const int port=(int)my_mexInputScalar(3);
        const char *hostname=my_mexInputOptionString(2);
        my_mexReturnValue(udp_connect(hostname,port));
        return;
    }
    if(myoptstrcmp(fun,"WRITE")==0){
        my_mexInputArray2Buff(2,&con[con_index].write);
        if(IS_STATUS_TCP_CONNECTED(con[con_index].status))
            writedata();
        return;
    }
    if(myoptstrcmp(fun,"PRINTF")==0){
        mxArray *plhs[1]={NULL};
        if(gnrhs<3) return;
        mexCallMATLAB(1,plhs, gnrhs-2, (mxArray **)&(gprhs[2]),"sprintf");
        gprhs=(const mxArray **)plhs; gnrhs=1; // HACK: Move return arg from sprintf to input arg of this mex.
        my_mexInputArray2Buff(0,&con[con_index].write);
        if(IS_STATUS_TCP_CONNECTED(con[con_index].status))
            writedata();
        return;
    }
    if(myoptstrcmp(fun,"READ")==0){
        if(IS_STATUS_TCP_CONNECTED(con[con_index].status))
            readtype2buff( (int)my_mexInputSize(2),str2classid(my_mexFindInputString(2)),0,my_mexFindInputOption(2,"noblock"));
        my_mexReturnArrayFromBuff(2,&con[con_index].read,0);
        return;
    }
    if(myoptstrcmp(fun,"READLINE")==0){
        //    mexPrintf("DEBUG: READLINE....\n");
        if(IS_STATUS_TCP_CONNECTED(con[con_index].status))
            read2buff(my_mexInputSize(2),1,my_mexFindInputOption(2,"noblock"));
        my_mexReturnArrayFromBuff(2,&con[con_index].read,1);
        //    mexPrintf("DEBUG: READLINE END\n");
        return;
    }
    if(myoptstrcmp(fun,"READTOFILE")==0){
        FILE *f=NULL;
        const int blocklen=my_mexInputSize(2+1);
        int readlen=blocklen;
        int writelen=0;
        if(IS_STATUS_TCP_CONNECTED(con[con_index].status))
            readlen= read2buff(blocklen,0,(int)my_mexFindInputOption(2+1,"noblock"));
        if(readlen>0)
            f=fopen(my_mexInputOptionString(2),(int)my_mexFindInputOption(2+1,"append")?"ab":"wb");
        if(f){
            writelen=fwrite(con[con_index].read.ptr,1,readlen,f);
            fclose(f);
        }
        // Delete from read buffer if not "VIEW" option and dims filled
        if(my_mexFindInputOption(2+1,"VIEW")==0 ){
            con[con_index].read.pos-=writelen;
            memmove(con[con_index].read.ptr,&con[con_index].read.ptr[writelen],con[con_index].read.pos);
            newbuffsize(&con[con_index].read,con[con_index].read.pos);
        }
        my_mexReturnValue(writelen);
        return;
    }
    if(myoptstrcmp(fun,"WRITEFROMFILE")==0){
        int len=1024*1024*1024;int start=0;
        FILE *f=fopen(my_mexInputOptionString(2),"rb");
        if(f==NULL)
            my_mexReturnValue(-1);
        if(my_mexIsInputArgOK(2+1))
            start=(int)my_mexInputScalar(2+1);
        if(my_mexIsInputArgOK(2+2))
            len=(int)my_mexInputScalar(2+2);
        else{
            fseek(f,0,SEEK_END);
            len=ftell(f)-start;
        }
        fseek(f,start,SEEK_SET);
        newbuffsize(&con[con_index].write,con[con_index].write.pos+len);
        len=fread(&con[con_index].write.ptr[con[con_index].write.pos],1,len,f);
        con[con_index].write.pos+=len;
        fclose(f);
        if(IS_STATUS_TCP_CONNECTED(con[con_index].status))
            writedata();
        if(len<0)
            my_mexReturnValue(-1);
        else
            my_mexReturnValue(len);
        return;
    }
    if(myoptstrcmp(fun,"READPACKET")==0){
        con[con_index].read.pos=0;
        my_mexReturnValue(readtype2buff(my_mexInputSize(2),str2classid(my_mexInputOptionString(2+1)),0,
                                        my_mexFindInputOption(2+1,"noblock")));
        return;
    }
    if(myoptstrcmp(fun,"WRITEPACKET")==0){
        if(IS_STATUS_UDP_NO_CON(con[con_index].status))
            ipv4_lookup(my_mexInputOptionString(2),my_mexInputScalar(3));
        my_mexReturnValue(writedata());
        con[con_index].write.pos=0;
        return;
    }
    if(myoptstrcmp(fun,"STATUS")==0){
        my_mexReturnValue(con[con_index].status);
        return;
    }
    if(myoptstrcmp(fun,"GETHOST")==0){
        int n;
        double ip_bytes[4] = {0, 0, 0, 0};
        const unsigned char *ipnr=(const unsigned char *)&con[con_index].remote_addr.sin_addr;
        for(n=0;n<4;n++)
            ip_bytes[n] = (double)ipnr[n];
        my_mexReturnMatrix(1,4,ip_bytes);
        my_mexReturnValue((int)ntohs(con[con_index].remote_addr.sin_port));
        return;
    }
    if(myoptstrcmp(fun,"SETCALLBACK")==0){
        strncpy(con[con_index].callback, my_mexInputOptionString(2), CBNAMELEN);
        return;
    }
    if(myoptstrcmp(fun,"GETCALLBACK")==0){
        my_mexReturnString(con[con_index].callback);
        return;
    }
    if(myoptstrcmp(fun,"SETWRITETIMEOUT")==0){
        con[con_index].writetimeout=my_mexInputScalar(2);
        return;
    }
    if(myoptstrcmp(fun,"SETREADTIMEOUT")==0){
        con[con_index].readtimeout=my_mexInputScalar(2);
        return;
    }
    if(myoptstrcmp(fun,"debug")==0){
        mexPrintf("     FID:%d\n",con[con_index].fid);
        mexPrintf("  STATUS:%d\n",con[con_index].status);
        mexPrintf("WRITE  TO:%g\n",con[con_index].writetimeout);
        mexPrintf("WRITE PTR:%p\n",con[con_index].write.ptr);
        mexPrintf("WRITE POS:%d\n",con[con_index].write.pos);
        mexPrintf("WRITE LEN:%d\n",con[con_index].write.len);
        mexPrintf("READ  TO:%g\n",con[con_index].readtimeout);
        mexPrintf("READ PTR:%p\n",con[con_index].read.ptr);
        mexPrintf("READ POS:%d\n",con[con_index].read.pos);
        mexPrintf("READ LEN:%d\n",con[con_index].read.len);
        return;
    }
    mexErrMsgTxt("Unknown 'function name' in argument.");
}