/*                                Copyright 1996
**                                      by
**                         The Board of Trustees of the
**                       Leland Stanford Junior University.
**                              All rights reserved.
**
**
**         Work supported by the U.S. Department of Energy under contract
**       DE-AC03-76SF00515.
**
**                               Disclaimer Notice
**
**        The items furnished herewith were developed under the sponsorship
**   of the U.S. Government.  Neither the U.S., nor the U.S. D.O.E., nor the
**   Leland Stanford Junior University, nor their employees, makes any war-
**   ranty, express or implied, or assumes any liability or responsibility
**   for accuracy, completeness or usefulness of any information, apparatus,
**   product or process disclosed, or represents that its use will not in-
**   fringe privately-owned rights.  Mention of any product, its manufactur-
**   er, or suppliers shall not, nor is it intended to, imply approval, dis-
**   approval, or fitness for any particular use.  The U.S. and the Univer-
**   sity at all times retain the right to use and disseminate the furnished
**   items for any purpose whatsoever.                       Notice 91 02 01
*/
/*=============================================================================

  Abs:  Test Accelerator Structure Processing

  Name: subAS.c
	   subASprocInitTRS1,2,3 - Processing Init for Stns 1,2,3
           subASprocTRS1,2,3     - Processing      for Stns 1,2,3
	   subASprocInit         - Processing Init (File Scope)
           subASproc             - Processing      (File Scope)
           subASmissing          - Missing Pulse Check
           subASstatus           - Processing Status and Counts
           subASlatch            - Latch and Reset Logic
	   subAScount            - Trip Counter
           subASstatEnergy       - Individual Energy Statuses
	   subASinit             - General Initialization
           subASlostEnergy       - Lost Energy
           subASnorm             - Normalization Calculation

  Proto: not required and not used.  All functions called by the
         subroutine record get passed one argument:

         psub                       Pointer to the subroutine record data.
          Use:  pointer               In general, all functions read the
          Type: struct subRecord *    input fields (a to l) and write to
          Acc:  read/write            the val field.
          Mech: reference

         All functions return a long integer.  0 = OK, -1 = ERROR.
         The subroutine record ignores the status returned by the Init
         routines.  For the calculation routines, the record status (STAT) is
         set to SOFT_ALARM (unless it is already set to LINK_ALARM due to
         severity maximization) and the severity (SEVR) is set to psub->brsv
         (BRSV - set by the user in the database though it is expected to
          be invalid).

  Auth: 2-Apr-1998, Ron Chestnut
  Rev:  19-Jun-2001, Stephanie Allison

-------------------------------------------------------------------------------

  Mod:  

=============================================================================*/

#include   "vxWorks.h"    /* OK,ERROR             */
#include   "subRecord.h"  /* for struct subRecord */
#include   "dbAccess.h"   /* for database access  */
#include   "dbFldTypes.h" /* DBR_DOUBLE           */
#include   "stdio.h"      /* for printf           */

/*
 * Prototypes for file-scope routines
 */
static long subASprocInit(struct subRecord *psub, int idx);
static long subASproc    (struct subRecord *psub, int idx);

#define AS_DBADDR_LIM(x,j) \
  AS_DBADDR(x,wrn_addr_as[idx][j],"HIGH"); \
  AS_DBADDR(x,err_addr_as[idx][j],"HIHI") 

#define AS_DBGET_LIM(j, wrn_lvl, err_lvl) \
  wrn_lvl = *((float *)(wrn_addr_as[idx][j].pfield)); \
  err_lvl = *((float *)(err_addr_as[idx][j].pfield))

#define AS_DBADDR(x,y,f) \
  sprintf(pvName_c,"TRS%d:%s:ENERGY.%s",idx+1,x,f); \
  iss = dbNameToAddr(pvName_c, &y); \
  if (iss) {printf("%s not found \n",pvName_c);iret=iss;}

#define NUMBER_OF_STATIONS 3
#define NUMBER_TO_CHECK    8
#define NUMBER_OF_VALS     10    /* NUMBER_TO_CHECK + 2 */

/*
 * Values for processing status
 */
#define AS_OKOK     0
#define AS_WARNING  1 
#define AS_ERROR    2
#define AS_INVALID  3
#define AS_MISSING  4
#define AS_DISABLED 5
#define AS_WARN2ERR 6
#define AS_REBOOT   7

#define AS_NOGO     1
#define AS_GO       0

/* Define the DB address for all warning and error limits. */ 

static struct dbAddr wrn_addr_as[NUMBER_OF_STATIONS][NUMBER_TO_CHECK];
static struct dbAddr err_addr_as[NUMBER_OF_STATIONS][NUMBER_TO_CHECK];

long subASprocInitTRS1(struct subRecord *psub)
{
  return subASprocInit(psub,0);
}
long subASprocInitTRS2(struct subRecord *psub)
{
  return subASprocInit(psub,1);
}
long subASprocInitTRS3(struct subRecord *psub)
{
  return subASprocInit(psub,2);
}
long subASprocTRS1(struct subRecord *psub)
{
  return subASproc(psub,0);
}
long subASprocTRS2(struct subRecord *psub)
{
  return subASproc(psub,1);
}
long subASprocTRS3(struct subRecord *psub)
{
  return subASproc(psub,2);
}
static long subASprocInit(struct subRecord *psub, int idx)
{
    int iss;
    int iret = OK;
    char pvName_c[PVNAME_STRINGSZ+FLDNAME_SZ];
 
    /* Check for valid station ID */

    if (idx >= NUMBER_OF_STATIONS) return(ERROR);

    /* At INIT time, find the db_addr structures for the HIHI/HIGH values  */
    /* The order must be the same as the inputs to subASproc (see below)   */

    AS_DBADDR_LIM("AS1:INPREFL",0);
    AS_DBADDR_LIM("AS1:INPFRWD",1);
    AS_DBADDR_LIM("AS1:LDFRWD" ,2);
    AS_DBADDR_LIM("AS2:INPREFL",3);
    AS_DBADDR_LIM("AS2:INPFRWD",4);
    AS_DBADDR_LIM("AS2:LDFRWD" ,5);
    AS_DBADDR_LIM("AS1:LOST"   ,6);
    AS_DBADDR_LIM("AS2:LOST"   ,7);

    return(iret);
}

static long subASproc(struct subRecord *psub, int idx)
{
  /*
   * Note:  Order of A through H must be the same as
   *        the order defined in subProcInit above.
   * Inputs:  A = AS1 Input Reflected Energy 
   *          B = AS1 Input Forward   Energy
   *          C = AS1 Load  Forward   Energy
   *          D = AS2 Input Reflected Energy
   *          E = AS2 Input Forward   Energy
   *          F = AS2 Load  Forward   Energy
   *          G = AS1 Lost Energy (from subASlostEnergy)
   *          H = AS2 Lost Energy (from subASlostEnergy)
   *          I = Spare
   *          J = Normalization Factor (from subASnorm)
   *          K = Bit Mask of Channels not to check
   * Outputs: L = Bit Mask of Channels with warnings or errors
   *          VAL = Error Level (0=ok, 1=warning, 2=error)
   */

  double *val_a = &psub->a;
  double  err_lvl;
  double  wrn_lvl;
  int     err_mask = 0;
  int     wrn_mask = 0;
  int     chk_mask = ~((int)psub->k); /* Channels to check */
  int     mask;
  int     i;

  /* Check for problems */

  for (i = 0, mask = 1; i < NUMBER_TO_CHECK; i++, mask <<= 1) 
  {
    /*
    **   First check if the channel is enabled for checking.
    **   Check for the error level exceeded and then the warning level.
    */
    AS_DBGET_LIM(i,wrn_lvl,err_lvl); /* Get current limits */
    if (chk_mask & mask)
    {
      if      ((val_a[i] > err_lvl) && (val_a[i] > (err_lvl * psub->j)))
      {
	err_mask |= mask;
      }
      else if ((val_a[i] > wrn_lvl) && (val_a[i] > (wrn_lvl * psub->j)))
      {
	wrn_mask |= mask;
      }
    }
  }

  /* Output value and bit mask. */

  if      (err_mask > 0)
  {
    psub->l   = err_mask;
    psub->val = AS_ERROR;
  }
  else if (wrn_mask > 0)
  {
    psub->l   = wrn_mask;
    psub->val = AS_WARNING;
  }
  else
  {
    psub->l   = 0;
    psub->val = AS_OKOK;
  }
  return(OK);
}

long subASmissing(struct subRecord *psub)
{
  /*
   * Check for missing pulses.
   * Inputs:  A = Data acquisition counter
   *          B = Severity of data acquisition counter
   *              (0, 1, 2 = OK; 3 = INVALID)
   *          C = Maximum counter value
   *          D = Number of allowed delta pulses
   *          E = Previous data acquisition counter
   *          F = First time flag (0 = first time, 1 = not first time)
   *          G = Flag to clear total number of missing pulses
   *              (0 = clear, 1 = no clear)
   * Outputs: K = Delta pulses
   *          L = Total number of missing pulses
   *          VAL = Error Level (0=ok,2=error,3=invalid,7=reboot)
   */

  /* Clear counter on request */
  if (psub->g < 0.5)
  {
    psub->g = 1;
    psub->l = 0;
  }
  /* See if delta pulses cannot be determined */
  if (psub->b > 2.5)
  {
    psub->f = 0;
    psub->k = 0;
    psub->val = AS_INVALID;
  }
  /* Find delta pulses - first check for first time through (reboot) */
  else if (psub->f < 0.5)
  {
    psub->f = 1;
    psub->k = 0;
    psub->val = AS_REBOOT;
  }
  /* Check for too many missing pulses */
  else
  {
    psub->k = psub->a - psub->e;
    if (psub->k < 0) psub->k += psub->c;  /* Check for rollover */
    /* Check for invalid number of pulses */
    if (psub->k < 0.5)
    {
      psub->val = AS_INVALID;
    }
    else if (psub->k > psub->d)
    {
      psub->l += psub->k - psub->d;
      psub->val = AS_ERROR;
    }
    else
    {
      psub->val = AS_OKOK;
    }
  }
  /* Update previous value */
  psub->e = psub->a;

  return(OK);
}

long subASstatus(struct subRecord *psub)
{
  /*
   * Determine processing status and keep counts.
   * Inputs:  A = Error Level (from subASproc) (0=ok, 1=warning, 2=error)
   *          B = Number of good cycles required after one warning
   *          C = Error Level Severity (0, 1, 2 = OK; 3 = INVALID)
   *          D = Missing pulses error (from subASmissing)
   *          E = Flag to clear number of invalid/error/warning cycles
   *              (0 = clear, 1 = no clear)
   * Outputs: H = Number of         cycles
   *          I = Number of invalid cycles
   *          J = Number of error   cycles
   *          K = Number of good    cycles since the last warning or error
   *          L = Number of warning cycles
   *          VAL = Error Level (0=ok,1=warning,2=error,3=invalid,4=missing,
   *                             6=warning-to-error,7=reboot)
   */

  /* Update internal counter */
  psub->h++;

  /* Clear counters on request */
  if (psub->e < 0.5)
  {
    psub->e = 1;
    psub->l = 0;
    psub->j = 0;
    psub->i = 0;
  }
  /* 
  ** Check for invalid data or processing problems 
  */
  if (psub->c > 2.5)
  {
    psub->val = AS_INVALID;
    psub->k   = 0;
    psub->i++;
  }
  /*  
  ** Check for a trip or too many warnings.
  ** Two warnings within 30 cycles is interpreted as an ERROR.
  */
  else if (psub->a == AS_ERROR)
  {
    psub->val = AS_ERROR;
    psub->k   = 0;
    psub->j++;
  }
  else if ((psub->a == AS_WARNING) && (psub->k < psub->b))
  {
    psub->val = AS_WARN2ERR;
    psub->k   = 0;
    psub->j++;
  }
  /*  
  ** Check for a reboot.
  */
  else if (psub->d == AS_REBOOT)
  {
    psub->val = AS_REBOOT;
    psub->k   = 0;
  }
  /*  
  ** Check for missing data.
  */
  else if (psub->d > AS_WARNING)
  {
    psub->val = AS_MISSING;
    psub->k   = 0;
  }
  /*
  ** Check for a warning.
  */
  else if (psub->a == AS_WARNING)
  {
    psub->val = AS_WARNING;
    psub->k   = 0;
    psub->l++;
  }
  /*
  ** All must be OK.
  */
  else
  {
    psub->val = AS_OKOK;
    psub->k++;
  }
  return(OK);
}

long subASlatch(struct subRecord *psub)
{
  /*
   * Determine latch state and reset faults as appropriate.  Set permit.
   * Inputs:  A = Error Level (from subASstatus)
   *          B = Reset (0 = reset, 1 = noreset)
   *          C = Number of good cycles since the last warning or error
   *              (from subASstatus)
   *          D = Number of good cycles to auto-clear warning latch
   *          E = Bit Mask of Channels with warnings or errors (from subASproc)
   *          F = Number of cycles to wait before fault waveform capture
   * Outputs: G = Number of cycles after fault before fault waveform capture
   *          H = Update Fault Waveform Capture (0 = no update, 1 = update)
   *          I = Update Fault (0 = no update, 1 = update)
   *          J = Latched Bit Mask of Chans with warnings or errors
   *          K = Latched Bit Mask of Chans with faults
   *          L = Permit (0 = GO, 1 = NOGO)
   *          VAL = Latched Error Level
   */

  /* Assume no permit and no fault update. */
  psub->l = AS_NOGO;
  psub->h = 0;
  psub->i = 0;

  /* Update data capture counter if needed. */
  if (psub->g < psub->f)
  {
    psub->g++;
    if (psub->g >= psub->f) psub->h = 1;
  }

  /* Are we OK now? 
   * Don't permit if the previous error hasn't yet been cleared */
  if (psub->a == AS_OKOK)
  {
    /* Deal with reset request first.  Don't allow reset until
       data from last fault is captured. */
    if ((psub->val == AS_OKOK) || ((psub->b < 0.5) && (psub->g >= psub->f)))
    {
      psub->b   = 1;
      psub->g   = psub->f;
      psub->j   = 0;
      psub->k   = 0;
      psub->l   = AS_GO;
      psub->val = AS_OKOK;
    }
    else if (psub->val == AS_WARNING)
    {
      psub->l = AS_GO;
      /* Automatically reset the latched status if there's been
         enough good cycles */
      if (psub->c > psub->d)
      {
	psub->j   = 0;
	psub->k   = 0;
	psub->val = AS_OKOK;
      }
    }
  }
  /* Are we just in a warning state?
   * Latch it if it hasn't already been latched */
  else if (psub->a == AS_WARNING)
  {
    /* Deal with reset request first */
    if ((psub->val == AS_OKOK) || ((psub->b < 0.5) && (psub->g >= psub->f)))
    {
      psub->b   = 1;
      psub->g   = psub->f;
      psub->j   = psub->e;
      psub->k   = 0;
      psub->l   = AS_GO;
      psub->val = AS_WARNING;
    }
    else if (psub->val == AS_WARNING)
    { 
      psub->j   = (int)psub->j | (int)psub->e;
      psub->l   = AS_GO;
    }
  }
  /* In some error state.  Latch if we haven't already. */
  else if ((psub->val == AS_OKOK) || (psub->val == AS_WARNING)) 
  {
    /* First time through logic - do not count this fault or capture data */
    if (psub->b < 0.5)
    {
      psub->b = 1;
      psub->g = psub->f;
      psub->h = 0;
    }
    /* Set flag to count this fault and initialize data capture counter.
       And capture data now if no post-fault data is wanted. */
    else
    {
      psub->i = 1;
      psub->g = 0;
      if (psub->f == 0) psub->h = 1;
    }
    psub->j   = psub->e;
    psub->k   = psub->e;
    psub->val = psub->a;
  }
  /* In some error state.  Check for a reset. */
  else if ((psub->b < 0.5) && (psub->g >= psub->f))
  {
    psub->b   = 1;
    psub->g   = psub->f;
    psub->j   = psub->e;
    psub->k   = psub->e;
    psub->val = psub->a;
  }
  /* Update error bit mask in case a new channel has tripped */
  else
  {
      psub->j = (int)psub->j | (int)psub->e;
  }
  return(OK);
}

long subAScount(struct subRecord *psub)
{
  /*
   * Determine total fault count and count for each energy value.
   * Inputs:  K = First-Time Latched Bit Mask of Chans with warnings or errors 
   *              (from subASlatch)
   *          L = Flag to reset number of latched trips (0 = no reset,
   *                                                     1 = reset)
   * Outputs: Number of trips:
   *          A = AS1 Input Reflected Energy 
   *          B = AS1 Input Forward   Energy
   *          C = AS1 Load  Forward   Energy
   *          D = AS2 Input Reflected Energy
   *          E = AS2 Input Forward   Energy
   *          F = AS2 Load  Forward   Energy
   *          G = AS1 Lost Energy
   *          H = AS2 Lost Energy
   *          VAL = Total Number of Latched Trips
   */

  double *val_a = &psub->a;
  int     chn_mask = (int)psub->k; /* Mask of chans with warnings or errors */
  int     mask;
  int     i;

  /* Update count for each energy */
  for (i = 0, mask = 1; i < NUMBER_TO_CHECK; i++, mask <<= 1) 
  {
    /*
    **   Don't update if this is a counter reset request.
    **   Otherwise, update only if this is channel has faulted.
    **   
    */
    if (psub->l > 0.5)        val_a[i] = 0;
    else if (chn_mask & mask) val_a[i]++;
  }
  /* Update total count. */
  if (psub->l > 0.5)
  {
    psub->l   = 0;
    psub->val = 0;
  }
  else
  {
    psub->val++;
  }
  return(OK);
}

long subASstatEnergy(struct subRecord *psub)
{
  /*
   * Determine status for each energy value.
   * Inputs:  J = Error Level (from subASstatus or subASlatch)
   *              (0=ok,1=warning,2=error,3=invalid,4=missing)
   *          K = Bit Mask of Channels with warnings or errors 
   *              (from subASproc or subASlatch)
   *          L = Bit Mask of Channels not to check
   * Outputs: Error Levels:
   *          A = AS1 Input Reflected Energy 
   *          B = AS1 Input Forward   Energy
   *          C = AS1 Load  Forward   Energy
   *          D = AS2 Input Reflected Energy
   *          E = AS2 Input Forward   Energy
   *          F = AS2 Load  Forward   Energy
   *          G = AS1 Lost Energy
   *          H = AS2 Lost Energy
   *          VAL = Error Level (same as J)
   */

  double *val_a = &psub->a;
  int     chn_mask = (int)psub->k; /* Mask of chans with warnings or errors */
  int     chk_mask = (int)psub->l; /* Disabled channel mask */
  int     mask;
  int     i;

  /* Update status for each energy */
  psub->val = psub->j;
  for (i = 0, mask = 1; i < NUMBER_TO_CHECK; i++, mask <<= 1) 
  {
    /*
    **   First check if the channel is disabled for checking.
    */
    if (chk_mask & mask)
    {
      val_a[i] = AS_DISABLED;
    }
    /*
    **   Next check for error or warning and set energy status only
    **   if it is included in the mask.
    */
    else if ((psub->val == AS_ERROR) || (psub->val == AS_WARNING) ||
	     (psub->val == AS_WARN2ERR))
    {
      if (chn_mask & mask) val_a[i] = psub->val;
      else                 val_a[i] = AS_OKOK;
    }
    /*
    **   For all other errors (OK, INVALID, MISSING, REBOOT), set individual
    **   status to overall status.
    */
    else
    {
      val_a[i] = psub->val;
    }
  }
  return(OK);
}

long subASinit(struct subRecord *psub)
{
  /*
   * General purpose initialization required since all subroutine records
   * require a non-NULL init routine even if no initialization is required.  
   * Note that most subroutines in this file use this routine as an init
   * routine.  If init logic is needed for a specific subroutine, create a 
   * new routine for it - don't modify this one.
   */
  return(OK);
}

long subASlostEnergy(struct subRecord *psub)
{
  /*
   * Calculate lost energy (frwd energy - (C * load energy)).
   * Inputs:  A = Forward Energy
   *          B = Load    Energy
   *          C = Load Energy Multiplier
   * Outputs: VAL = Lost Energy
   */
  psub->val = psub->a - (psub->c * psub->b);
  return(OK);
}

long subASnorm(struct subRecord *psub)
{
  /*
   * Calculate normalization factor used on limits during structure
   * processing.
   * Inputs:  A = Offset
   *          B = Slope
   *          C = Forward Energy
   * Outputs: VAL = Normalization Factor (dimensionless)
   */
  psub->val = psub->a + (psub->b * psub->c);
  return(OK);
}