/*
 * File    : offsetkompensation.c
 *
 * Abstract: Contains non-inlined S-function for torque compensation
 *
 * Changes:  CS021003 created
 *
 */

#define 	S_FUNCTION_LEVEL 	2
#undef 		S_FUNCTION_NAME
#define 	S_FUNCTION_NAME 	momentenkompensation

#include 	<stddef.h>
#include 	<stdlib.h>

#include 	"simstruc.h" 

#ifdef 		MATLAB_MEX_FILE
#include 	"mex.h"
#endif


/* Input Arguments */
#define NUMBER_OF_ARGS          (1)
#define SAMP_TIME_ARG          	ssGetSFcnParam(S, 0)   /* Sample time [sec] */

#define NO_I_WORKS              (1)
#define COMP_FLAG_LAST			    (0)

#define NO_R_WORKS              (2)
#define OFFSET_VALUE_LAST       (0)  
#define TIME_1                  (1)

#define NO_INPORTS              (2)
#define NO_OUTPORTS             (2)

#define FLAG_COMP_OFF            0
//#define FLAG_COMP_ON             1
#define FLAG_COMP_ONA            1
#define FLAG_COMP_ONB            2

static char_T msg[256];


static void mdlInitializeSizes(SimStruct *S)
{
	int i;

  ssSetNumSFcnParams(S, NUMBER_OF_ARGS);
  if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
    sprintf(msg, "Wrong number of input arguments passed.\n%d arguments are expected\n", NUMBER_OF_ARGS);
    ssSetErrorStatus(S, msg);
    return;
  }

  ssSetNumContStates(S, 0);
  ssSetNumDiscStates(S, 0);

	ssSetNumInputPorts(S, NO_INPORTS);
	for (i=0; i<NO_INPORTS; i++) {
		ssSetInputPortWidth(S, i, 1);
    ssSetInputPortDirectFeedThrough(S, i, 1);
	}

	ssSetNumOutputPorts(S, NO_OUTPORTS);
	for (i=0; i<NO_OUTPORTS; i++) {
		ssSetOutputPortWidth(S, i, 1);
	}

  ssSetNumSampleTimes(S, 1);

  ssSetNumRWork(S, NO_R_WORKS);
  ssSetNumIWork(S, NO_I_WORKS);
  ssSetNumPWork(S, 0);

  ssSetNumModes(S, 0);
  ssSetNumNonsampledZCs(S, 0);

	ssSetSFcnParamNotTunable(S, 0);

  ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE | SS_OPTION_PLACE_ASAP);
}

static void mdlInitializeSampleTimes(SimStruct *S)
{
  ssSetSampleTime(S, 0, mxGetPr(SAMP_TIME_ARG)[0]);
  ssSetOffsetTime(S, 0, 0.0);
}

#define MDL_START
static void mdlStart(SimStruct *S)
{
  real_T 	*RWork = ssGetRWork(S);
  int_T 	*IWork = ssGetIWork(S);

  /* Set workspace values */	
  IWork[COMP_FLAG_LAST]    = FLAG_COMP_OFF;
	RWork[OFFSET_VALUE_LAST] = 0.0;
}

static void mdlOutputs(SimStruct *S, int_T tid)
{
	real_T              *y;
	InputRealPtrsType 	uPtrs;
	real_T 				      *RWork = ssGetRWork(S);
	int_T 				      *IWork = ssGetIWork(S);
	
  /* Input signals */
	int_T				        comp_flag;            /* Compensation flag [-] */
	real_T				      raw_value;            /* Raw signal value [Unit] */

  /* Parameter */
	real_T              dT;                   /* Sample rate [msec] */

	/* Output signals */
	real_T              signal_value;         /* Compensated value [Unit] */

  /* Workspace variables */
	int_T               comp_flag_last;       /* Compensation flag in last step [-] */
  real_T              offset_value_last;    /* Offset value in last step [Unit] */

  /* Local variables */
  real_T              time_1;               /* Aktuelle Zeit letzten Schritt [sec] */ 
	real_T              offset_value;         /* Actual offset value [Unit] */

 	/* Get parameter */
  dT = mxGetPr(SAMP_TIME_ARG)[0];  
  
	/* Read input signals */
	uPtrs = ssGetInputPortRealSignalPtrs(S, 0);
	comp_flag = (int_T) *uPtrs[0]; 

	uPtrs = ssGetInputPortRealSignalPtrs(S, 1);
	raw_value = *uPtrs[0];    

	/* Get values from last step */
  comp_flag_last    = IWork[COMP_FLAG_LAST];
  offset_value_last = RWork[OFFSET_VALUE_LAST];
  time_1            = RWork[TIME_1];

  /* Check if offset compensation has to be done */
	if (comp_flag == FLAG_COMP_OFF)
	{
	  time_1 = 0.0;

    offset_value = 0.0;
 	}
  else if ((comp_flag == FLAG_COMP_ONA || comp_flag == FLAG_COMP_ONB)
            && comp_flag_last != FLAG_COMP_ONA || comp_flag_last != FLAG_COMP_ONB)
	{
    /* Interne Zeit berechnen */
	  time_1 += dT;
      offset_value = raw_value;
	}


  	
	/* Compensate signal value */
  signal_value = offset_value;

  /* Output signal values */
	y = ssGetOutputPortSignal(S, 0);
  y[0] = signal_value;

  y = ssGetOutputPortSignal(S, 1);
	y[0] = time_1;

	/* Set values for next step */
  IWork[COMP_FLAG_LAST]    = comp_flag;    
  RWork[OFFSET_VALUE_LAST] = offset_value;
  RWork[TIME_1]            = time_1;
}

static void mdlTerminate(SimStruct *S)
{   


}

#ifdef MATLAB_MEX_FILE  /* Is this file being compiled as a MEX-file? */
#include "simulink.c"   /* Mex glue */
#else
#include "cg_sfun.h"    /* Code generation glue */
#endif