/* $Date:  $
 * $Revision: 0.99 $
 * $Author: EZ $
 *
 * File: dactest.c  
 *
 * Abstract:
 *      S-Function device driver for DAC of the MiniPmac Board      
 *
 * Copyright (c) 
 */


#define S_FUNCTION_NAME	ECPDSPDriver
#define S_FUNCTION_LEVEL 2

#include  <stdio.h>
#include  <stdlib.h>     /* malloc(), free(), strtoul() */
#include "simstruc.h"    /* the simstruct access macros */


/*=========================================================================*
 * Number of S-function Parameters and macros to access from the SimStruct *
 *=========================================================================*/
#define NUM_PARAMS                 (4)
#define BASE_ADDRESS_PARAM         (ssGetSFcnParam(S,0))
#define SAMPLE_TIME_PARAM          (ssGetSFcnParam(S,1))
#define TIMEOUT_PERIOD             (ssGetSFcnParam(S,2))
#define ACCESS_HW_PARAM            (ssGetSFcnParam(S,3))


/*==================================================*
 * Macros to access the S-function parameter values *
 *==================================================*/
#define SAMPLE_TIME           ((real_T) mxGetPr(SAMPLE_TIME_PARAM)[0])
#define DAC_CHANNELS          2
#define ENC_CHANNELS          5
#define pmac_timeout_cnt      ((uint_T) mxGetPr(TIMEOUT_PERIOD)[0])

/*========================================================*
 * (Hardware Specific) Macros pertaining to the I/O board *
 *========================================================*/

#if defined(MATLAB_MEX_FILE)
# define ACCESS_HW                  0
#endif

#if defined(RT)
# define ACCESS_HW                  (mxGetPr(ACCESS_HW_PARAM)[0] != 0.0)
#define DOINIT
#endif


#include "ECPDSPDriver.h"


/*======================*
 * Miscellaneous macros *
 *======================*/
#define BASE_ADDR_PARAM_STRLEN     (128)

/*======================*
 * S-function User Data *
 *======================*/
typedef struct {
    uint_T    baseAddr;
	int       encoderoffsets[4];
} DACInfo;


/*====================*
 * S-function methods *
 *====================*/


/*=============================================== 
 * Function: mdlInitializeSizes 
 *===============================================
 */
static void mdlInitializeSizes(SimStruct *S)
{
	ssSetNumSFcnParams(S, NUM_PARAMS);
	if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
		if (ssGetErrorStatus(S) != NULL) {
          return;   
       }
    } else {
       return; /* Parameter mismatch will be reported by Simulink */
    }

    /* None of this s-functions's parameters are tunable during simulation */
    {
        int_T i;
        for (i=0; i < NUM_PARAMS; i++) {
            ssSetSFcnParamNotTunable(S, i);
        }
    }

    ssSetNumSampleTimes  ( S, 1);
    ssSetNumInputPorts   ( S, 1);
	ssSetInputPortWidth  ( S, 0, DAC_CHANNELS);
// DAC outputs directly to the port
	ssSetNumOutputPorts  ( S, 1);
	ssSetOutputPortWidth ( S, 0, ENC_CHANNELS);
	 
}	

/* Function: mdlInitializeSampleTimes =========================================
 *
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);
}



#define MDL_START  /* Change to #undef to remove function */
#if defined(MDL_START)
/* Function: mdlStart =========================================================
 *
 */
static void mdlStart(SimStruct *S)
{
	int counter;
    DACInfo *dacInfo = ssGetUserData(S);
    char_T baseAddrStr[BASE_ADDR_PARAM_STRLEN];
    uint_T baseAddr;

    /* Initialize diInfo (pointer saved in the user data) */
    mxGetString(BASE_ADDRESS_PARAM, baseAddrStr, BASE_ADDR_PARAM_STRLEN);
    baseAddr = (uint_T) strtoul(baseAddrStr, NULL, 0);
    if (dacInfo != NULL) {
        free(dacInfo);
    }
    if ((dacInfo = malloc(sizeof(DACInfo))) == NULL) {
        ssSetErrorStatus(S,"Memory Allocation Error\n");
        return;
    }
    dacInfo->baseAddr   = baseAddr;
		

    if (ACCESS_HW) {
		// do initialiazation here ...

		for (counter =0; counter<4; counter++)
		{
			dacInfo->encoderoffsets[counter] = 0;//RetVal;
		}

		ssSetUserData(S, (void*) dacInfo);
    } else if (ssGetSimMode(S) == SS_SIMMODE_NORMAL) {
        printf("MINIPMAC: Hardware Access Disabled\n");
    }
}
#endif /* MDL_START */

/* Function: mdlOutputs =======================================================
 *
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{
	DACInfo            *dacInfo  = ssGetUserData(S);
	InputRealPtrsType  uPtrs  = ssGetInputPortRealSignalPtrs(S,0);
	uint_T             baseAddr = dacInfo->baseAddr;
	real_T			 value;
	real_T             *y = ssGetOutputPortRealSignal(S,0);
	int i=0, j=0, k=0; 
	char_T Response[255];
	long EncVal[5];
	char_T SingEncVal[255];
	long Dac1Value=0,Dac2Value=0;  
	int NumOfDacs = 0;

	if (ACCESS_HW) 
	{
		// write to the dacs 
		for (i = 0; i<DAC_CHANNELS; i++)
		{
			value = *uPtrs[i];
			if(i==0) Dac1Value = value;
			if(i==1) Dac2Value = value;
			NumOfDacs++;
		}

		i=0;
		j=0;
		k=0;
		PMACReadEncsWriteToDacs(baseAddr,Response,NumOfDacs, Dac1Value,Dac2Value,pmac_timeout_cnt);
		for(i=0 ; i<strlen(Response); i++)
		{
			if(Response[i] == CARRIAGE_RETURN || Response[i] == NULL_CHR)
			{
				SingEncVal[j] = NULL_CHR;
				if(k != 0)
					EncVal[k-1] = strtol(SingEncVal, NULL, 0);
				SingEncVal[0] = NULL_CHR;
				k++;
				j=0;
			}
			else
			{
				SingEncVal[j] = Response[i];
				j++;
			}
		} 

		j=0;
		for (j=0; j<ENC_CHANNELS; j++)
		{
			y[j] = EncVal[j];
		}
	}
	else
	{
		for (j=0; j<ENC_CHANNELS; j++)
		{
			y[j] = 0;
		}
	}
}



/* Function: mdlTerminate =====================================================
 *
 */
static void mdlTerminate(SimStruct *S)
{
   DACInfo *dacInfo = ssGetUserData(S);
	
	if (ACCESS_HW && dacInfo) 
	{
		uint_T baseAddr = dacInfo->baseAddr;

		PMACSetDacsToZero(baseAddr,pmac_timeout_cnt);
	}

 	// free(dacInfo);  this is the command that is bad
    ssSetUserData(S,NULL); 
}

int PMACReadEncsWriteToDacs(uint_T addr,char *Response,int NumOfDacs, long Dac1Value,long Dac2Value,uint_T timeout_cnt)//real_T Enc1Val, real_T Enc2Val, real_T Enc3Val, real_T Enc4Val, real_T ErrFlag,uint_T timeout_cnt)
{
	int Stat = 0;
	char_T sValue[255];
	char_T sVar[255];
	char_T sVars[255];
	int TheNumOfDacs = NumOfDacs;

	if(TheNumOfDacs > 0)
	{
		ltoa(Dac1Value,sValue,10);
		strcpy(sVar,"M6=");
		strcat(sVar,sValue);
		strcpy(sVars,sVar);
		TheNumOfDacs=TheNumOfDacs-1;
	}
	if(TheNumOfDacs > 0)
	{
		ltoa(Dac2Value,sValue,10);
		strcpy(sVar,"M7=");
		strcat(sVar,sValue);
		strcat(sVars,sVar);
	}
	if(NumOfDacs > 0)
	{
		strcat(sVars,"M0..5");
		if(flush_port(addr,timeout_cnt) != COMM_ERROR)
		{
			write_line_port(addr, sVars, &Stat,timeout_cnt);
			read_line_port(addr, Response, &Stat,timeout_cnt);
		}
	}
	else
	{
		if(flush_port(addr,timeout_cnt) != COMM_ERROR)
		{
			write_line_port(addr, "m0..5", &Stat,timeout_cnt);
			read_line_port(addr, Response, &Stat,timeout_cnt);
		}
	}
	return strlen(Response);
}


int PMACSetDacsToZero(uint_T addr,uint_T timeout_cnt)
{
	int Stat = 0;
	flush_port(addr,timeout_cnt); 
	write_line_port(addr, "M6=0M7=0", &Stat,timeout_cnt);
	return 0;
}

int write_char_port(uint_T addr, char outchar,uint_T timeout_cnt)
{
	unsigned long timeout=0;
	while(timeout++ < timeout_cnt){
		if(PMAC_RDY_TO_RCV(addr)){
			outp(PMAC_DATA(addr),outchar);
			return COMM_OK;
		}
	}
	return COMM_TIMEOUT;
}

int write_line_port(uint_T addr, char *buffer, int *status,uint_T timeout_cnt)
{
	unsigned int nc=0;
	int result=COMM_OK;

	while(buffer[nc]!=NULL_CHR && nc<255 && result==COMM_OK)
		result=write_char_port(addr, buffer[nc++],timeout_cnt);

	if(result==COMM_OK)
		result=write_char_port(addr, CARRIAGE_RETURN,timeout_cnt);

	*status=result;
	return nc;
}

int read_line_port(uint_T addr, char *buffer, int *status,uint_T timeout_cnt)
{
	unsigned long timeout=0;
	int nc=0;


	while(timeout<timeout_cnt && nc<256){
		if(!PMAC_RDY_TO_SEND(addr)){
			timeout++;
			continue;
		}

		buffer[nc] = inp(PMAC_DATA(addr));
		timeout=0;

		if(buffer[nc] == ACK_CHR){
			*status = COMM_EOT;
			break;
		}else if(buffer[nc] == CARRIAGE_RETURN){
			nc++;
		}else if(buffer[nc] > 0x7E){
			*status=COMM_ERROR;
			break;
		}else if(buffer[nc] == CTRL_G)
			*status=COMM_ERROR;
		else
			nc++;

		if(*status==COMM_ERROR && !PMAC_RDY_TO_SEND(addr))
			break;

	}

	if (timeout>=timeout_cnt)
		*status=COMM_TIMEOUT;

	buffer[nc] = NULL_CHR;
	return nc;
}
   /*flush_port() clears out any garbage in pmac command buffer by writing Ctrl-X and
    *then reads few characters back to clear out data buffer
    */
int flush_port(uint_T addr,uint_T timeout_cnt)
{
	int i=0, nchr=0;
	unsigned int temp;
	

	write_char_port(addr, CTRL_X,timeout_cnt);

	while(i++<timeout_cnt && nchr<5){
		if(!PMAC_RDY_TO_SEND(addr)){
			continue;
		}else
			nchr++;

		temp = inp(PMAC_DATA(addr));
		if(temp == CTRL_X)
			break;
		else if(temp == 0xFF)
			return COMM_ERROR;
	}
	return 0;
}

/*=============================*
 * Required S-function trailer *
 *=============================*/
#ifdef MATLAB_MEX_FILE
#include "simulink.c"     
#else
#include "cg_sfun.h"
#endif

/* EOF: MiniPmac.c */
