/*
Copyright (C) 2001-2002 Christian Bottelier <c.bottelier@iradis.org>

    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., 675 Mass Ave, Cambridge, MA 02139, USA.
*/


/*
 *       An S-function for reading data usig one of
 *       the libC supported DIOM-DLs
 */


#define S_FUNCTION_NAME  libGCGateway
#define S_FUNCTION_LEVEL 2

#define N_STRING 512

#define WIN32

#include "simstruc.h"

#include <windows.h>
#include <libGC/diom.h>
#include <libGC/dl.h>
#include <libGC/configfile.h>


static int  g_iNumInputs = 0;
static int* g_aiInputs = NULL;



int parseInputSection(CFILE *file, int iInput, char* sInput)
{
    // Try to open the input definition section
    if (cf_openSection(file, sInput) == 0) {
        printf("Input section '%s' not found\n", sInput);
    	cf_closeFile(file);
    	return -1;
    }

    // Parse all line in the display section
    char sBuffer[512];
    while (cf_readline(file, sBuffer, sizeof sBuffer) != NULL) {
		char *sValue = cf_splitValuePair(sBuffer, " \t");
		if (*sBuffer == '\000' || *sValue == '\000') continue;

        if (strcasecmp(sBuffer, "variable") == 0) {
			g_aiInputs[iInput] = (int)diom_loadVariable(sValue);
			if (g_aiInputs[iInput] == -1) {
				printf("Variable '%s' not known\n", sValue);
				cf_closeFile(file);
				return -1;
			}
		} else {
            printf("Unknown keyword: %s\n", sBuffer);
		    cf_closeFile(file);
		    return -1;
		}
    }

    if (g_aiInputs[iInput] == -1) {
		printf("Variable for input '%s' not defined\n", sInput);
		cf_closeFile(file);
		return -1;
	}

    return 0;
}




int parseConfigFile(const char *sConfigFileName)
{
	int i;

    // Try to open the configuration file
    CFILE *file = cf_openFile(sConfigFileName);
    if (file == NULL) return -1;

    // Try to open the A340GC section
    if (cf_openSection(file, "S-Function") == 0) {
        printf("S-Function section not found");
    	cf_closeFile(file);
    	return -1;
    }

    // Parse all line in the A340GC section
    char sBuffer[512];
    while (cf_readline(file, sBuffer, sizeof sBuffer) != NULL) {
		char *sValue = cf_splitValuePair(sBuffer, " \t");
		if (*sBuffer == '\000' || *sValue == '\000') continue;

		if (strcasecmp(sBuffer, "inputs") == 0) {
			if (cf_str2int(sValue, &g_iNumInputs) == 0) {
				cf_closeFile(file);
				return -1;
			}
		} else {
			printf("Parse error unrecognized symbol : %s\n", sBuffer);
			cf_closeFile(file);
			return -1;
		}
    }


	if (g_iNumInputs == 0) {
		printf("No inputs defined\n");
		cf_closeFile(file);
		return -1;
	}

	// Get the memory for the pool of inputs
    g_aiInputs = (int*)malloc(sizeof(int)*g_iNumInputs);
    memset(g_aiInputs, -1, sizeof(int)*g_iNumInputs);

	// Load input map
    for (i=0; i < g_iNumInputs; ++i) {
		char sInput[32];
		snprintf(sInput, sizeof(sInput), "input.%i", i);
		if (parseInputSection(file, i, sInput) == -1) return -1;
	}

    cf_closeFile(file);
    return 0;
}



/* Function: mdlInitializeSizes ===============================================
 * Abstract:
 *   Setup sizes of the various vectors.
 */
static void mdlInitializeSizes(SimStruct *S)
{
    char_T sConfigFile[N_STRING];

	// Report our presence
    printf("Simulink libGC gateway version 0.0\n");

	// Let simulink know we want 1 parameter specifiying the config file
    ssSetNumSFcnParams(S, 1);
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
        return; /* Parameter mismatch will be reported by Simulink */
    }

    // Get the config file name from the parameter box
    mxGetString(ssGetSFcnParam(S,0), sConfigFile, N_STRING);
    printf("Configfile = %s\n", sConfigFile);

	// Initialize the libGC DIOM
    if (!diom_init(sConfigFile, ".")) {
        printf("Failed to initialize Data Input Output Module:\n %s\n %s\n",
	       	diom_errorString(), dlerror());

	    ssSetErrorStatus(S, "Failed to init libGC");
        return;
    }
    printf("The DIOM is initialized\n");


	if (parseConfigFile(sConfigFile) == -1) {
	    ssSetErrorStatus(S, "Failed to parse configuration");
	    return;
	}


    // Define the number of input ports
	printf("Defining %i input(s) on S-Function block\n", g_iNumInputs);
    if (!ssSetNumInputPorts(S, g_iNumInputs)) return;

	int i;
	for (i=0; i < g_iNumInputs; ++i) {
	    // Set the width of each input port
	    ssSetInputPortWidth(S, i, 1);
	}


	// Define the number of output ports
    if (!ssSetNumOutputPorts(S, 0)) return;


	// Define the sample time behaviour
    ssSetNumSampleTimes(S, 1);

    // No exceptions will be generated
    ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
}


/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *    Specifiy that we inherit our sample time from the driving block.
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
	// Take the same sampling time as the block(s) that drive
	// our inputs
    ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);
}


/* Function: mdlOutputs =======================================================
 * Abstract:
 *    y = 2*u
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{
    InputRealPtrsType uPtrs;
    double dValue;

	int i;
	for (i=0; i < g_iNumInputs; ++i) {
		// Get the input
	    uPtrs  = ssGetInputPortRealSignalPtrs(S,i);
		dValue = *uPtrs[0];

		// Propagate the variable on the network
		diom_setValueFloat(g_aiInputs[i], (float)dValue);
	}

	Sleep(1);
}


/* Function: mdlTerminate =====================================================
 * Abstract:
 *    No termination needed, but we are required to have this routine.
 */
static void mdlTerminate(SimStruct *S)
{
}


#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