/* Copyright (c) 1993, 1994 The MathWorks, Inc.
 * All Rights Reserved
 *
 * File    : simstruc.c
 *
 * Abstract:
 *      Creation and deletion of the SimStruct model data structure.
 *
 *      Each instance of an S-function (a model or user defined S-function) 
 *      has a unique "SimStruct" (simulation structure) associated with it.
 *
 *      This file contains routines for creating, destroying, 
 *      and initializing the simulation structure.  The simulation 
 *      structure is defined in simstruc.h.  This file also uses many
 *      of the access methods defined in simstruc.h.
 *
 */


#include <stdlib.h>               /* for free   */
#include <string.h>		  /* for memset */
#include <math.h>		  /* for fabs */

#include "simstruc.h"


#ifndef PUBLIC
# define PUBLIC
# define PRIVATE static
#endif



/*=================*
 * Local functions *
 *=================*/

#ifndef MATLAB_MEX_FILE

/* Function: ssInitSampleTimeCounters =========================================
 * Abstract:
 *      Initialize the counters which are used to determine when sample
 *      hits occur.
 */
PRIVATE
#ifdef __STDC__
void ssInitSampleTimeCounters(SimStruct *S)
#else
void ssInitSampleTimeCounters(S)
     SimStruct *S;
#endif
{
  int n = ssGetNumSampleTimes(S);
  int i;
  
  for (i = 0; i < n; i++) {
    /*
     * Note that the sample and offset times must be integer multiples of 
     * the step-size for the simulation to work.
     */
    ssSetSampleCount
      (S, i, (int)(ssGetSampleTimeEvent(S, i) / ssGetStepSize(S) + 0.5));

    ssSetOffsetCount
      (S, i, (int)(ssGetOffsetTimeEvent(S, i) / ssGetStepSize(S) + 0.5));

    ssSetCounter(S, i, ssGetSampleCount(S, i) - ssGetOffsetCount(S, i));
  }
}  /* end ssInitSampleTimeCounters */
#endif


/*===================*
 * Visible functions *
 *===================*/

/* Function: ssCreateSimStruct ================================================
 * Abstract:
 *      Create the simulation structure.
 *      Initialize all internal structures to NULL to be malloced later.
 */
PUBLIC 
#ifdef __STDC__
SimStruct *ssCreateSimStruct(SimStruct *parentSS)
#else
SimStruct *ssCreateSimStruct(parentSS)
     SimStruct *parentSS;
#endif
{
  SimStruct *S;
    
# ifdef MATLAB_MEX_FILE
    S = (SimStruct *) mxCalloc(1,sizeof(SimStruct));
# else
    S = (SimStruct *) malloc(sizeof(SimStruct));
# endif

  if (S == NULL) {
    return(NULL);  /* error allocating memory */
  }
    
  /*
   * Initialize the structure
   */
    
  memset((char *)S, 0, sizeof(SimStruct));
    
  ssSetVersion(S, SIMSTRUCT_VERSION);
  ssSetParentSS(S, parentSS);

  if (parentSS != NULL) {
    ssSetRootSS(S, ssGetRootSS(parentSS));
  } else {
    ssSetRootSS(S, S);
  }
  return(S);
} /* end ssCreateSimStruct */



/* Function: ssDestroySimStruct ===============================================
 * Abstract:
 *      Destroy the simulation structure.  Free all allocated memory.
 */
PUBLIC
#ifdef __STDC__
void ssDestroySimStruct(SimStruct *S)
#else
void ssDestroySimStruct(S)
     SimStruct *S;
#endif
{
# define FREE(p)   if (p) free(p)
    
  int i;
    
  /*
   * Destroy the simulation structures for all associated
   * S-Functions
   */
    
  for (i = 0; i < ssGetNumSFunctions(S); i++)
    ssDestroySimStruct(ssGetSFunction(S,i));

  FREE(ssGetSFunctions(S));
  FREE(ssGetArgPtr(S));
  FREE(ssGetU(S));		/* first location of the fields malloc */
  FREE(ssGetIWork(S));
  FREE(ssGetPWork(S));
  FREE(S);
    
# undef FREE
} /* end ssDestroySimStruct */



/* Function: ssIncrementT =====================================================
 * Abstract:
 *      Increment the Time, T, field in the root SimStruct in a numerically 
 *	sound manner.  Since Tcount is a flint (floating point integer), it 
 *	can be incremented with no round-off error.  This technique is used in 
 *	conjunction with fixed-step-size integration algorithms only.
 */
PUBLIC
#ifdef __STDC__
void ssIncrementT(SimStruct *S)
#else
void ssIncrementT(S)
     SimStruct *S;
#endif
{

  S->Tcount++;
  ssSetT(S, S->Tcount * ssGetStepSize(S));

}  /* END ssIncrementT */



/* Function: ssUpdatePresentTimeEvents ========================================
 * Abstract:
 *      Updates the time events for the SimStruct.  This function *does not* 
 *      update the child S-Functions of the SimStruct, as it is called as 
 *      part of the ssCallmdlUpdate macro.
 */
PUBLIC 
#ifdef __STDC__
void ssUpdatePresentTimeEvents(SimStruct *S, int tid)
#else
void ssUpdatePresentTimeEvents(S, tid)
     SimStruct *S;
     int tid;
#endif
{
# ifndef MATLAB_MEX_FILE
#   ifdef  MULTITASKING
    {
      double event;

      event = ssGetPresentTimeEvent(S,tid) + ssGetSampleTimeEvent(S,tid);
      ssSetPresentTimeEvent(S, tid, event);
    }
#   else
    {
      int    i;
      double event;

      for (i = 0; i < ssGetNumSampleTimes(S); i++) {
	event = ssGetPresentTimeEvent(S,i) + ssGetSampleTimeEvent(S,i);
	if (ssIsSampleHitEvent(S, i, tid))
	  ssSetPresentTimeEvent(S, i, event);
      }
    }
#   endif
# endif
} /* end ssUpdatePresentTimeEvents */




#ifndef MATLAB_MEX_FILE

/* Function: ssInitSimStruct ==================================================
 * Abstract:
 *      Sets up the simulation structure and initializes the I/O and state 
 *      vectors.  Returns 0 upon success and -1 on an error.
 */
PUBLIC
#ifdef __STDC__
int ssInitSimStruct(SimStruct *S)
# else
int ssInitSimStruct(S)
     SimStruct *S;
# endif
{
  unsigned mallocDoubleSize, mallocIntSize, mallocVoidSize;
  double *doublePtr = NULL;
  int *intPtr = NULL;
  void **voidPtr = NULL;
  int scopeVectorLength;
    
  /*
   * compute the size of the scope vector
   */

  scopeVectorLength = 0;
  if (ssGetNumScopes(S) != 0) {
    int i, noScopes;
    ModelScopeInfo *scopes;
    
    noScopes = ssGetNumScopes(S);
    scopes = ssCallmdlScopeInfo(S);
    for (i = 0; i < noScopes; i++)
      scopeVectorLength += scopes[i].inputWidth;
  }
  
  /*
   * obtain the total dynamic size of the simulation structure
   * to be allocated.
   */
  mallocDoubleSize =
    (ssGetNumInputs(S) +
     ssGetNumOutputs(S) +
     ssGetNumTotalStates(S) +
     ssGetNumContStates(S) +
     ssGetNumRWork(S) +
     ssGetNumBlockIO(S) +
     scopeVectorLength +
     ssGetNumContStates(S) +
     ssGetNumContStates(S) +
     ssGetNumContStates(S) +
     ssGetNumContStates(S) +
     ssGetNumContStates(S) +
     ssGetNumContStates(S) +
     ssGetNumContStates(S) +
     ssGetNumContStates(S) +
     ssGetNumOutputs(S) +
     ssGetNumBlockParams(S) +
     ssGetNumSampleTimes(S)*4) * sizeof(double);
  
  mallocIntSize = 
    (ssGetNumIWork(S) +
     ssGetNumSampleTimes(S)*ssGetNumSampleTimes(S) +
     ssGetNumSampleTimes(S)*5) * sizeof(int);
  
  mallocVoidSize =
    ssGetNumPWork(S) * sizeof(void *);
  
  /*
   * create a dynamic simulation structure and have the model initialize
   * everything to zero.
   */
  
  if ((mallocDoubleSize != 0) && 
      (doublePtr = (double *) malloc(mallocDoubleSize)) == NULL)
    return(-1);
  
  if ((mallocIntSize != 0) && 
      (intPtr = (int *) malloc(mallocIntSize)) == NULL)
    return(-1);
  
  if ((mallocVoidSize != 0) && 
      (voidPtr = (void **) malloc(mallocVoidSize)) == NULL)
    return(-1);
  
  if (mallocDoubleSize > 0) {
    memset((char *) doublePtr, 0, mallocDoubleSize);  /* initialize to zero (NULL) */
  }

  if (mallocIntSize > 0) {
    memset((char *) intPtr,    0, mallocIntSize);     /* initialize to zero (NULL) */
  }

  if (mallocVoidSize > 0) {
    memset((char *) voidPtr,   0, mallocVoidSize);    /* initialize to zero (NULL) */
  }

  /*
   * initialize simulation vectors pointers
   */
  ssSetU(S,doublePtr);          doublePtr += ssGetNumInputs(S); 
  ssSetY(S,doublePtr);          doublePtr += ssGetNumOutputs(S);
  ssSetX(S,doublePtr);          doublePtr += ssGetNumTotalStates(S);
  ssSetdX(S,doublePtr);         doublePtr += ssGetNumContStates(S);
  ssSetRWork(S,doublePtr);      doublePtr += ssGetNumRWork(S);
  ssSetIWork(S,intPtr);         intPtr += ssGetNumIWork(S);
  ssSetPWork(S,voidPtr);        voidPtr += ssGetNumPWork(S);
  ssSetBlockIO(S,doublePtr);    doublePtr += ssGetNumBlockIO(S);
  ssSetScopes(S,doublePtr);     doublePtr += scopeVectorLength;
  
  /*
   * initialize temporary storage pointers
   */
  ssSetTempStore0(S,doublePtr);   doublePtr += ssGetNumContStates(S);
  ssSetTempStore1(S,doublePtr);   doublePtr += ssGetNumContStates(S);
  ssSetTempStore2(S,doublePtr);   doublePtr += ssGetNumContStates(S);
  ssSetTempStore3(S,doublePtr);   doublePtr += ssGetNumContStates(S);
  ssSetTempStore4(S,doublePtr);   doublePtr += ssGetNumContStates(S);
  ssSetTempStore5(S,doublePtr);   doublePtr += ssGetNumContStates(S);
  ssSetTempStore6(S,doublePtr);   doublePtr += ssGetNumContStates(S);
  ssSetTempX(S,doublePtr);        doublePtr += ssGetNumContStates(S);
  ssSetTempY(S,doublePtr);        doublePtr += ssGetNumOutputs(S);
  
  /*
   * initialize pointers and set the default model parameters.
   */
  if (ssGetNumBlockParams(S) > 0) {
    double *defaultParams;
    double *params;
    int i;
    
    params = doublePtr;	doublePtr += ssGetNumBlockParams(S);
    
  defaultParams = ssGetDefaultParam(S);
    memcpy((char *)params, (char *)defaultParams, ssGetNumBlockParams(S)*sizeof(double));
    ssSetBlockParam(S, params);
    
    /*
     * fix the input arg pointers, they are originally pointing to
     * a location within the DefaultParameters array
     */
    for (i = 0; i < ssGetNumSFunctions(S); i++) {
      SimStruct *s;
      int j;
      
      s = ssGetSFunction(S, i);
      for (j = 0; j < ssGetNumInputArgs(s); j++) {
	Matrix *m;
	
	/*
	 * this input arg matrix is pointing to a location within
	 * the default parameters.  This needs to be adjusted to be
	 * pointing to a location within the newly allocated
	 * parameter vector.
	 */
	m = ssGetArg(s, j);
	ssSetArg (s, j, &params[m - defaultParams]);
      }
    }
  }
  
  /*
   * initialize the sample times for models with discrete time components
   */
  ssSetSampleTimeEventPtr(S,doublePtr);  doublePtr += ssGetNumSampleTimes(S);
  ssSetOffsetTimeEventPtr(S,doublePtr);  doublePtr += ssGetNumSampleTimes(S);
  ssSetNextTimeEventPtr(S,doublePtr);    doublePtr += ssGetNumSampleTimes(S);
  ssSetPresentTimeEventPtr(S,doublePtr); doublePtr += ssGetNumSampleTimes(S);
  ssSetSampleHitEventPtr(S,intPtr);      intPtr += ssGetNumSampleTimes(S);
  ssSetCounterPtr(S,intPtr);             intPtr += ssGetNumSampleTimes(S);
  ssSetSampleCountPtr(S,intPtr);         intPtr += ssGetNumSampleTimes(S);
  ssSetOffsetCountPtr(S,intPtr);         intPtr += ssGetNumSampleTimes(S);
  ssSetSampleTimeTaskIDPtr(S,intPtr);	   intPtr += ssGetNumSampleTimes(S);
  ssSetPerTaskSampleHitsPtr(S,intPtr);
  
  ssCallmdlInitializeSampleTimes(S);


  /*
   * Set the first element in the sample hit event array.  This is needed
   * for the case of 1 discrete sample time, since the routine to update
   * discrete events is only called if there are more than 1 sample time.
   */
  if (ssGetNumSampleTimes(S) == 1 && ssGetSampleTimeEvent(S,0) != 0.0)
    ssSetSampleHitEvent(S,0,1);

#ifdef NRT
  /*
   * Initialize present time events as used in NRT timing.
   */
  {
      int i;
      for (i = 0; i < ssGetNumSampleTimes(S); i++) {
	  ssSetPresentTimeEvent(S, i, -1e30);
      }
  }
#endif
  
  return(0);
}  /* end ssInitSimStruct */



/* Function: ssInitSampleTimeTaskIDs ==========================================
 * Abstract:
 *      Called to initialize the SampleTimeTaskIDs mapping array which is
 *      used to map all event (or tid's) to the root simstruct event's.
 *
 *      Also, if not real time (NRR defined), then we initialize sample
 *      time counters.
 */

#define CLASH_PERCENTAGE  1e-6

PUBLIC 
#ifdef __STDC__
void ssInitSampleTimeTaskIDs(SimStruct *S)
#else
void ssInitSampleTimeTaskIDs(S)
     SimStruct *S;
#endif
{
    SimStruct *rootSS = ssGetRootSS(S);
    int       N = ssGetNumSampleTimes(rootSS);
    int       n = ssGetNumSampleTimes(S);
    int i, j;
    int firstDiscIndex = ssGetSampleTimeEvent(rootSS, 0) == 0.0;
    double Tol;
    
    if (N <= 1) {
	return;
    }
    
    Tol = ssGetSampleTimeEvent(rootSS, firstDiscIndex) * (CLASH_PERCENTAGE/100.0);
    
# ifndef NRT
    if (S == rootSS) {
	ssInitSampleTimeCounters(S);  /* counters only used in real-time */
    }
# endif

    for (i = 0; i < n; i++) {
	double st = ssGetSampleTimeEvent(S, i);
	double of = ssGetOffsetTimeEvent(S, i);
	for (j = 0; j < N; j++) {
	    double root_st = ssGetSampleTimeEvent(rootSS, j);
	    double root_of = ssGetOffsetTimeEvent(rootSS, j);
	    if ((fabs(root_st - st) < Tol) && (fabs(root_of - of) < Tol)) {
		ssSetSampleTimeTaskID(S, i, j);
		break;
	    }
	}
    }


    /* Recurse for nested S-Functions */
    for (i = 0; i < ssGetNumSFunctions(S); i++) {
	ssInitSampleTimeTaskIDs(ssGetSFunction(S,i));
    }
}
#endif /* !MATLAB_MEX_FILE */



/* EOF: simstruc.c */
