/** Level 2 Simulink S-function for UDP communication with VILLASnode
 *
 * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
 * @copyright 2017, Institute for Automation of Complex Power Systems, EONERC
 * @license GNU General Public License (version 3)
 *
 * VILLASnode
 *
 * 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 3 of the License, or
 * 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, see <http://www.gnu.org/licenses/>.
 *********************************************************************************/

#define S_FUNCTION_NAME udp_client
#define S_FUNCTION_LEVEL 2

#define MAXLEN 65536

#include <math.h>
#include "simstruc.h"


#if defined(_WIN32)
  #include <winsock2.h>
#elif defined(__linux__) || defined(__MACH__)
  #include <sys/types.h>
  #include <sys/socket.h>
#endif

typedef struct SInfoTag {
#if defined(_WIN32)
	SOCKET socket;
#elif defined(__linux__) || defined(__MACH__)
    int socket;
#endif

	struct sockaddr_in remote;
    struct sockaddr_in local
} SInfo;

/* Create Structure Info */
void CreateStructure(SimStruct *S)
{
	SInfo *info;
	void **PWork = ssGetPWork(S);

	info = (SInfo *) malloc(sizeof(SInfo));
	PWork[0] = (void *) info;
}

/* Get Structure Info */
SInfo *GetStructure(SimStruct *S)
{
	void **PWork = ssGetPWork(S);
	return (SInfo *) PWork[0];
}

/* Get Structure Info */
void DeleteStructure(SimStruct *S)
{
	SInfo *info = GetStructure(S);
	free(info);
}

/* mdlCheckParameters, check parameters, this routine is called later from mdlInitializeSizes */
#define MDL_CHECK_PARAMETERS
static void mdlCheckParameters(SimStruct *S)
{
    /* Basic check : All parameters must be real positive vectors */
    double *dPort, *dNumOfInputs; 
	char hostname[256];
	int iPort, iNumOfInputs;

	mxGetString(ssGetSFcnParam(S,0), hostname, 200);

    /* Check number of elements in second parameter: */
    if ( mxGetNumberOfElements(ssGetSFcnParam(S,1)) != 1 ) {
        ssSetErrorStatus(S,"The parameter must be a 1 elements vector");
        return;
    }

    /* Check port number */
	dPort=mxGetPr(ssGetSFcnParam(S,1));
	iPort = (int)floor((*dPort)+0.5);
	if ( (iPort < 1025) || (iPort > 65535) ) {
        ssSetErrorStatus(S,"Port # must be from 1025 to 65535");
        return;
	}

    /* Check number of inputs*/
    dNumOfInputs = mxGetPr(ssGetSFcnParam(S,2));
	iNumOfInputs = (int)floor((*dNumOfInputs)+0.5);
    if ( (iNumOfInputs < 1) || (iNumOfInputs > MAXLEN) ) { 
		ssSetErrorStatus(S, "Buffer Size Limit MAXLEN exceeded"); 
		return; 
	}

}

/* mdlInitializeSizes - initialize the sizes array */
static void mdlInitializeSizes(SimStruct *S)
{

	int iBufSize;
    
	ssSetNumSFcnParams(S,4);                          /* number of expected parameters        */

    /* Check the number of parameters and then calls mdlCheckParameters to see if they are ok */
    if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
        mdlCheckParameters(S);
        if (ssGetErrorStatus(S) != NULL)
            return;
    }
    else
        return;

	iBufSize = (int) floor ((*mxGetPr(ssGetSFcnParam(S,2)))+0.5);

    ssSetNumContStates(S, 0);                           /* number of continuous states          */
    ssSetNumDiscStates(S, 0);                          /* number of discrete states            */

    if (!ssSetNumInputPorts(S, 2))                     /* number of input ports                */
        return;

    ssSetInputPortWidth(S, 0, iBufSize);                /* first input port width               */
    ssSetInputPortDirectFeedThrough(S, 0, 1);           /* first port direct feedthrough flag   */
	ssSetInputPortDataType(S, 0, SS_UINT8);			  /* first input port data type           */

    ssSetInputPortWidth(S, 1, 1);                       /* second input port width              */
    ssSetInputPortDirectFeedThrough(S, 1, 1);           /* second port direct feedthrough flag  */
	ssSetInputPortDataType(S, 1, SS_UINT32);			  /* second input port data type          */

    if (!ssSetNumOutputPorts(S, 0))                    /* number of output ports               */
        return;
   
    ssSetNumSampleTimes(S,0);                         /* number of sample times               */

    ssSetNumRWork(S,0);                               /* number real work vector elements     */
    ssSetNumIWork(S,0);                               /* number int_T work vector elements    */
    ssSetNumPWork(S,2);                               /* number ptr work vector elements      */
    ssSetNumModes(S,0);                               /* number mode work vector elements     */
    ssSetNumNonsampledZCs(S,0);                       /* number of nonsampled zero crossing   */
}

/* mdlInitializeSampleTimes - initialize the sample times array */
static void mdlInitializeSampleTimes(SimStruct *S)
{
	double *dSampleTime = mxGetPr(ssGetSFcnParam(S, 3));
    
	/* Set things up to run with inherited sample time */
    ssSetSampleTime(S, 0, dSampleTime[0]);
    ssSetOffsetTime(S, 0, 0);
}

/* start the client - called inside mdlStart */
void StartUDPClient(SimStruct *S, SInfo *info)
{
    struct {
        char host[256];
        int iport;
        double *dport;
    } local, remote;

	mxGetString(ssGetSFcnParam(S,0), local.host, 255);
    mxGetString(ssGetSFcnParam(S,1), remote.host, 255);

    local.dport = mxGetPr(ssGetSFcnParam(S,1));
    remote.dport = mxGetPr(ssGetSFcnParam(S,1));

	local.iport  = (int) floor((*lcaol.dport)  + 0.5);
    remote.iport = (int) floor((*remote.dport) + 0.5);
    
	struct hostent *hent;
	unsigned long srv_addr;
    
	/* Clear the structure so that we don't have garbage around */
	memset((void *) &(info->local),  0, sizeof(info->local));
    memset((void *) &(info->remote), 0, sizeof(info->remote));

    /* Try to find the IP address for the hostname */
 	if ((hent = gethostbyname(hostname)) != NULL) {
        memcpy((char *) &srv_addr, (char *) hent->h_addr_list[0], hent->h_length);
        
        /* Fill in IP address */
        info->remote.sin_addr.s_addr = srv_addr;
 	} /* If hostname does not exist try as IP address */
    else if ((info->cli_sin.sin_addr.s_addr = inet_addr(hostname)) == INADDR_NONE) {
        /* If cannot convert IP address than it is an error */
        printf("Bad hostname.\n");
        goto err;
    }

	/* AF means Address Family - same as Protocol Family for now */
	info->cli_sin.sin_family = AF_INET;

	/* Fill in port number in address (careful of byte-ordering) */
	info->cli_sin.sin_port = htons(serv_port);

	cli_sock = socket(AF_INET, SOCK_DGRAM, 0);
	if (cli_sock == INVALID_SOCKET) {
        printf("Cannot create socket.\n");
        goto err;
	}

    return cli_sock;

err:
	info->connState = csError;
    return SOCKET_ERROR;

	if(info->connState == csConnected) {
        unsigned long ctl = 1;
        ioctlsocket(info->socket, FIONBIO, &ctl );
    }
}

/* mdlStart - initialize work vectors */
#define MDL_START
static void mdlStart(SimStruct *S)
{
	WSADATA wsa_data;
	int status;
	SInfo *info;

	/* get buffer size */ 
	int iBufSize = (int)floor((*mxGetPr(ssGetSFcnParam(S,2)))+0.5);

	/* retrieve pointer to pointers work vector */
	void **PWork = ssGetPWork(S);

	/* allocate buffer */
	unsigned char *buffer;
    buffer = malloc(iBufSize*sizeof(unsigned char));

	/* check if memory allocation was ok */
	if (buffer==NULL)  {
        ssSetErrorStatus(S,"Error in mdlStart : could not allocate memory");
        return;
    }

    /* store pointers in PWork so they can be accessed later */
	PWork[1] = (void*) buffer;

	/* Activate the Winsock DLL */
	if ((status = WSAStartup(MAKEWORD(2,2),&wsa_data)) != 0) {
		printf("%d is the WSA startup error\n",status);
		exit(1);
	}

	CreateStructure(S);
	info = GetStructure(S);
	StartUDPClient(S, info);
}

/* mdlOutputs - compute the outputs */
static void mdlOutputs(SimStruct *S, int_T tid)
{
	/* input ports */
	uint8_T **u = (uint8_T**) ssGetInputPortSignalPtrs(S,0);
	uint32_T **dataLen = (uint8_T**) ssGetInputPortSignalPtrs(S,1);

	int iBufSize = (int)floor((*mxGetPr(ssGetSFcnParam(S,2)))+0.5);

	int i, ret;
	SInfo *info = GetStructure(S);
	int length = (int) floor(*dataLen[0]+0.5);
	int sock_error;
    
	/* retrieve pointer to pointers work vector */
	void **PWork = ssGetPWork(S);

	/* assign buffer pointer */
	unsigned char *buffer;
    buffer = PWork[1];

	ret = sendto(info->socket, buffer, length, 0, &(info->remote), sizeof(info->remote));
    	if (ret < 0)
}

/* mdlTerminate - called when the simulation is terminated */
static void mdlTerminate(SimStruct *S) 
{
	void **P = ssGetPWork(S);
	SInfo *info = GetStructure(S);

	/* Deallocate Buffer */
	free(P[1]);

	/* Deallocate Socket */
	closesocket(info->socket);
	DeleteStructure(S);
    WSACleanup();
}

/* Trailer information to set everything up for simulink usage */
#ifdef  MATLAB_MEX_FILE                      /* Is this file being compiled as a MEX-file?   */
#include "simulink.c"                        /* MEX-file interface mechanism                 */
#else
#include "cg_sfun.h"                         /* Code generation registration function        */
#endif