/* **********************************************************************
 * *	                                                                *
 * *     Routines for Real Time Modeling of N-Dimensional Vectors       *
 * *                                                                    *
 * **********************************************************************
 * File: libs/user/rtvec.c
 *
 *                        COPYRIGHT (c) 2002  BY
 *         SIGMET INCORPORATED, WESTFORD MASSACHUSETTS, U.S.A.
 * 
 * THIS SOFTWARE IS FURNISHED UNDER A LICENSE AND MAY BE USED AND  COPIED
 * ONLY  IN  ACCORDANCE WITH  THE  TERMS  OF  SUCH  LICENSE  AND WITH THE
 * INCLUSION OF THE ABOVE COPYRIGHT NOTICE.  THIS SOFTWARE  OR  ANY OTHER
 * COPIES  THEREOF MAY NOT BE PROVIDED OR OTHERWISE MADE AVAILABLE TO ANY
 * OTHER PERSON.  NO TITLE TO AND OWNERSHIP OF  THE  SOFTWARE  IS  HEREBY
 * TRANSFERED.
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <math.h>

#include "sigtypes.h"
#include "user_lib.h"
#include "himath_lib.h"

/* *******************************
 * *                             *
 * *  Visible User Entry Points  *
 * *                             *
 * *******************************
 *
 * Summary of entry points:
 *
 *    rtvecInitFirst: Initialize a realtime vector history structure
 *       rtvecAttach: Attach a process or thread set to a history structure
 *  rtvecInsertPoint: Insert a data vector for a given binary time
 *       rtvecLookup: Compute the data vector at a given binary time
 */

/* ------------------------------
 * Initialize the history structures which make up a realtime vector
 * model.  This routine should be called just once on startup prior to
 * any other use of the area, and with the same NSLOTS and VECDIM
 * values that were used with RTVECALLOCATE to allocate the area.
 *
 * The calling program should also fill in iSlotWidthMS and iModulos[]
 * (for any elements of the data vector which are angles) after doing
 * these inits.
 */
void rtvecInitFirst
( volatile struct rtvec_info *info_a, SINT4 nSlots_a, SINT4 iVecDim_a )
{
  SINT4  nSlots = MAX( 3, MIN( RTVECMAXSLOTS, nSlots_a  ) ) ;
  SINT4 iVecDim = MAX( 1, MIN( RTVECMAXDIM  , iVecDim_a ) ) ;
  SINT4 iSlot, iSlotSize = sizeof(struct rtvec_dhdr) + (iVecDim * sizeof(FLT8) ) ;

  memset( (void *)info_a, 0, sizeof(struct rtvec_info) ) ;
  info_a->nSlots  = nSlots ;
  info_a->iVecDim = iVecDim ;
  info_a->iSlotWidthMS = 50 ;	/* default only */

  for( iSlot=0 ; iSlot < nSlots ; iSlot++ ) {
    struct rtvec_dhdr *pDHdr = (void *)((iSlot * iSlotSize) + (char *)&info_a[1]) ;
    FLT8 *pData = (void *)( &pDHdr[1] ) ; SINT4 iDim ;

    memset( pDHdr, 0, sizeof(struct rtvec_dhdr) ) ;
    for( iDim=0 ; iDim < iVecDim ; iDim++ ) *pData++ = 0.0 ;
  }
}

/* ------------------------------
 * Attach the current process (or set of threads) to a given RTVEC
 * model area.  We assume that the area has already been initialized
 * and is sitting in memory waiting to be used.  The structure that is
 * filled in here is the only one that should be used for subsequent
 * access to the (possibly shared) model area.
 *
 * The lock/unlock routines will be used whenever this particular
 * process (or set of threads) will be accessing the modeling area.
 * It is assumed that multiple processes will all cooperate to attach
 * to a given model area using the same lock/unlock handlers.
 */
void rtvecAttach
( struct rtvec *rtvec_a, volatile struct rtvec_info *info_a,
  void (*pLock_a)(void), void (*pUnLock_a)(void) )
{
  rtvec_a->info    = (void *)info_a ;
  rtvec_a->pLock   = (void *)pLock_a ;
  rtvec_a->pUnLock = (void *)pUnLock_a ;
}

/* ------------------------------
 * Insert another data point into a realtime model.
 */
#define DHDR(SLOT) ( (struct rtvec_dhdr *) \
  (((SLOT) * iSlotSize) + (char *)&info[1]) )

void rtvecInsertPoint
( struct rtvec *rtvec_a, UINT4 iBtime_a, const FLT8 fData_a[] )
{
  struct rtvec_info *info = rtvec_a->info ; struct rtvec_dhdr *pDHdr ;
  SINT4 iDataSlot, iSlotSize = sizeof(struct rtvec_dhdr) +
    ( info->iVecDim * sizeof(FLT8) ) ;

  /* Begin by locking the structure if we need to, and checking the
   * most recent data header.
   */
  if( rtvec_a->pLock ) ( (void(*)(void))rtvec_a->pLock )() ;

  pDHdr = DHDR( info->iTopSlot ) ;
  if( ! pDHdr->lValidData ) {
    /* If nothing has been inserted yet, then simply add this new data
     * point.  This is how the model gets started from scratch.
     */
    iDataSlot = info->iTopSlot ;

  } else {
    SINT4 iTimeHop = (SINT4)(iBtime_a - pDHdr->iBtime) ;
    if( abs(iTimeHop) > RTVECMAXTIMESPAN ) {
      /* If the time of this new data is not within the maximum time
       * span of the model, then invalidate the entire model history
       * and start fresh with this new point.
       */
      for( iDataSlot=0 ; iDataSlot < info->nSlots ; iDataSlot++ ) {
	DHDR( iDataSlot )->lValidData = FALSE ;
      }
      iDataSlot = info->iTopSlot = 0 ;

    } else if( iTimeHop < 0 ) {
      /* If the time of this new data is earlier than the most recent
       * history point, then throw this sample away entirely.  For
       * now, we insist that data be inserted in strictly forward
       * order.
       */
      goto exit ;

    } else {
      /* Otherwise we have a valid time to insert.  Check whether the
       * time of the previous data point is within the slot width of
       * the point we are about to add.  If so, then just overwrite
       * the top slot; otherwise bump to a new slot.
       */
      UINT4 iTick1 = (     iBtime_a / info->iSlotWidthMS) ;
      UINT4 iTick2 = (pDHdr->iBtime / info->iSlotWidthMS) ;
      if( iTick1 == iTick2 ) {
	iDataSlot = info->iTopSlot ;
      } else {
	iDataSlot = ( info->iTopSlot + 1 ) % info->nSlots ;
      }
    }
  }
  /* Move the data vector into the new slot, update the time, and flag
   * that valid data are stored here.
   */
  pDHdr = DHDR( iDataSlot ) ;
  {
    FLT8 *pData = (void *)( &pDHdr[1] ) ; SINT4 iDim ;
    for( iDim=0 ; iDim < info->iVecDim ; iDim++ ) *pData++ = fData_a[ iDim ] ;
  }
  pDHdr->iBtime     = iBtime_a ;
  pDHdr->lValidData = TRUE ;

  /* Update the top pointer to the most recent slot, and optionally
   * unlock on the way out.
   */
  info->iTopSlot = iDataSlot ;

 exit:
  if( rtvec_a->pUnLock ) ( (void(*)(void))rtvec_a->pUnLock )() ;
}
/* =======================================================
 * Add a fixed offset to all data in the history because we
 * know that a data correction was just made
 */
void rtvecJumpData
( struct rtvec *rtvec_a, const FLT8 fData_a[] )
{
  struct rtvec_info *info = rtvec_a->info ; struct rtvec_dhdr *pDHdr ;
  SINT4 iDataSlot, iSlotSize = sizeof(struct rtvec_dhdr) +
    ( info->iVecDim * sizeof(FLT8) ) ;
  
  /* Begin by locking the structure if we need to, and checking the
   * most recent data header.
   */
  if( rtvec_a->pLock ) ( (void(*)(void))rtvec_a->pLock )() ;
  
  for (iDataSlot=0; iDataSlot<info->nSlots; iDataSlot++)
    {
      pDHdr = DHDR( iDataSlot ) ;
      if( pDHdr->lValidData )
	{
	  FLT8 *pData = (void *)( &pDHdr[1] ) ; SINT4 iDim ;
	  for( iDim=0 ; iDim < info->iVecDim ; iDim++ )
	    {
	      *pData++ += fData_a[ iDim ] ;
	      switch( info->iModulos[ iDim ] ) {
	      case RTVEC_MOD_UBIN:
		if ((*pData)>=360.0)      *pData -= 360.0;
		else if ((*pData)<0.0)    *pData += 360.0;
		break ;
	      case RTVEC_MOD_SBIN:
		if ((*pData)>=180.0)      *pData -= 360.0;
		else if ((*pData)<-180.0) *pData += 360.0;
		break ;
	      case RTVEC_MOD_EBIN:
		if ((*pData)>=270.0)      *pData -= 360.0;
		else if ((*pData)<-90.0)  *pData += 360.0;
		break ;
	      }
	    }
	}
      
    }
  if( rtvec_a->pUnLock ) ( (void(*)(void))rtvec_a->pUnLock )() ;
}

/* ------------------------------
 * Get the vector approximation at a given binary time.
 * Return flags indicating the situation.
 */
UINT4 rtvecLookupPoint
( struct rtvec *rtvec_a, UINT4 iBtime_a, FLT8 fData_a[] )
{
  struct rtvec_info *info = rtvec_a->info ; struct rtvec_dhdr *pDHdr ;
  SINT4 ii, iSlots[4], iDim, iDataSlot ; UINT4 iBtimes[4] ; FLT8 *pValues[4] ;
  SINT4 iSlotSize = sizeof(struct rtvec_dhdr) + ( info->iVecDim * sizeof(FLT8) ) ;
  FLT4 fRtWgt, fSkWgt ; UINT1 lInterpolate ;
  UINT4 iResultFlags=0;

  /* Begin by locking the structure if we need to, and checking the
   * most recent data header.
   */
  if( rtvec_a->pLock ) ( (void(*)(void))rtvec_a->pLock )() ;

  pDHdr = DHDR( info->iTopSlot ) ;
  if( ! pDHdr->lValidData ) {
    /* If there are no data at all, then just return zeros
     */
    for( iDim=0 ; iDim < info->iVecDim ; iDim++ ) fData_a[iDim] = 0.0 ;
    iResultFlags |= RTVEC_LOOKUP_EMPTY;
    goto exit ;

  } else if( ((SINT4)(iBtime_a - pDHdr->iBtime)) >= 0 ) {
    /* If the data time we seek is past the most recently inserted
     * data point, then we know we have to extrapolate off the end.
     */
    iDataSlot = info->iTopSlot ;
    iResultFlags |= RTVEC_LOOKUP_EXTRAPOLATE;

  } else {
    /* Otherwise we need to do a (binary) search on the available data
     * points to determine where we are in the history.  The data slot
     * that we find will be the first one whose time is at or after
     * the desired lookup time.
     */
    SINT4 iOffset, iTrialBit = ipower_of_two( info->nSlots ) ;
    for( iOffset = 0 ; iTrialBit ; iTrialBit >>= 1 ) {
      SINT4 iTrialSlot = ( iOffset | iTrialBit ) ;
      if( iTrialSlot < info->nSlots ) {
	pDHdr = DHDR( (info->iTopSlot + (info->nSlots - iTrialSlot)) % info->nSlots ) ;
	if( pDHdr->lValidData ) {
	  if( ((SINT4)(iBtime_a - pDHdr->iBtime)) <= 0 ) iOffset = iTrialSlot ;
	}
      }
    }
    iDataSlot = (info->iTopSlot + (info->nSlots - iOffset)) % info->nSlots ;
  }
  /* Try to look one slot ahead of our starting point and two slots
   * behind.  This gives the default four points that will be used for
   * weighted smoothing.
   */
  if( iDataSlot == info->iTopSlot ) iSlots[3] =  iDataSlot ;
  else                              iSlots[3] = (iDataSlot + 1) % info->nSlots ;

  for( ii=2 ; ii >= 0 ; ii-- ) {
    iSlots[ii] = (iSlots[ii+1] + (info->nSlots - 1)) % info->nSlots ;

    if( (         iSlots[ii] == info->iTopSlot ) ||
	( ! DHDR( iSlots[ii] )->lValidData     ) ) {

      /* If we could not find all four points, then the model is still
       * being filled in from scratch and we simply return the data
       * from the starting slot that was located.
       */
      FLT8 *pData = (void *)( &DHDR(iDataSlot)[1] ) ;
      for( iDim=0 ; iDim < info->iVecDim ; iDim++ ) fData_a[iDim] = *pData++ ;
      iResultFlags |= RTVEC_LOOKUP_SHORT;
      goto exit ;
    }
  }
  /* We now have the four history points that will contribute to the
   * data estimate for the desired binary time.  The two middle points
   * always have a weight of 1.0.  The weights of the edge points will
   * be determined by how the times are placed, but they will always
   * sum to one.  Begin by prefetching some slot information.
   */
  for( ii=0 ; ii < 4 ; ii++ ) {
    pDHdr = DHDR( iSlots[ii] ) ;
    pValues[ ii ] = (void *)&pDHdr[1] ;
    iBtimes[ ii ] = pDHdr->iBtime ;
  }
  /* The weight of the righthand edge point is biased down whenever it
   * is closer to its predecessor than the average spacing of the last
   * few points.  This downplays rapidly inserted recent data which
   * have not yet really contributed any new information.  Limit this
   * effect as soon as the new point's spacing is 75% or more of the
   * average.
   */
  fRtWgt = 1.0 ;
  if( ( iSlots[3] == info->iTopSlot) &&
      (iBtimes[2] != iBtimes[0]    ) ) {
    fRtWgt = ( ( 2.0 * (FLT4)(SINT4)( iBtimes[3] - iBtimes[2] )) /
	       (       (FLT4)(SINT4)( iBtimes[2] - iBtimes[0] )) ) ;
    fRtWgt = MAX( 0.0, MIN( 1.0, (fRtWgt / 0.75) ) ) ;
  }
  /* Determine relative interpolation weight of the right three points
   * relative to the left three points according to the exact location
   * of the desired binary time.  This gives us a smooth transition in
   * output values as the time varies between slots.
   */
  fSkWgt = 1.0 ; lInterpolate = FALSE ;
  if( (               iBtimes[2] != iBtimes[1]      ) &&
      ( (ii = (SINT4)(iBtimes[2]  - iBtime_a)) >= 0 ) ) {
    FLT4 fWgt = ((FLT4)ii) / (FLT4)(SINT4)(iBtimes[2] - iBtimes[1]) ;
    fWgt = MAX( 0.0, MIN( 1.0, fWgt ) ) ;
    fSkWgt *= ( 1.0 - fWgt ) ; lInterpolate = TRUE ;
  }
  /* Calculate the output value for each dimension of the modelled
   * data vector.  Compute moments from the four weighted (Time,Value)
   * data points using relative offset units (to prevent wraparounds).
   */
  for( iDim=0 ; iDim < info->iVecDim ; iDim++ ) {
    FLT4 fXSum = 0.0, fYSum = 0.0, fXXSum = 0.0, fXYSum = 0.0 ;
    FLT8 fBaseValue = pValues[0][iDim], fMeans[2], fYVal ;
    UINT4 iBaseTime = iBtimes[0] ;

    /* Macro to accumulate a single weighted data point into the least
     * squares statistics that will be used for a linear fit.
     */
#define ACCUMDATA( SLOT, WEIGHT ) {					\
      FLT8 fX = (SINT4)( iBtimes[SLOT] - iBaseTime  ) ;			\
      FLT8 fY = (  pValues[SLOT][iDim] - fBaseValue ) ;			\
      if( info->iModulos[ iDim ] != RTVEC_MOD_NONE ) {			\
        fY = fmod( fY, 360.0 ) ;					\
        if( fY < -180.0 ) fY += 360.0 ;					\
        if( fY >  180.0 ) fY -= 360.0 ;					\
      }									\
      fXSum += (WEIGHT) * fX ; fXXSum += (WEIGHT) * SQUARE(fX) ;	\
      fYSum += (WEIGHT) * fY ; fXYSum += (WEIGHT) * fX * fY ;		\
    }
    /* Macro to evaluate the current least squares line at a desired
     * BTIME and return the RESULT.
     */
#define EVALPOINT( RESULT, BTIME ) {				\
      FLT4 fSlope = ( ((3.0 * fXYSum) - (fXSum * fYSum)) /		\
		      ((3.0 * fXXSum) - SQUARE(fXSum)  ) ) ;		\
      FLT4 fXVal = (SINT4)( (BTIME) - iBaseTime ) ;			\
      RESULT = (fYSum / 3.0) + (fSlope * (fXVal - (fXSum / 3.0))) ;	\
    }
    /* Fit a line to the first three points using equal weights for
     * each one.  Note that we do not have to explicitly accumulate
     * the first point since it's coordinates are (0,0), and the total
     * weight is known to be 3.0.  If we're going to interpolate then
     * evaluate this line at the time of the middle point to get the
     * smoothed value there.
     */
    ACCUMDATA( 1, 1.0 ) ; ACCUMDATA( 2, 1.0 ) ;
    if( lInterpolate ) EVALPOINT( fMeans[0], iBtimes[1] ) ;

    /* Fit a line to the last three points by simply adding a weighted
     * version of the third.  For interpolation, get a second smoothed
     * value and use the connecting line segment to compute the output
     * value.  For extrapolation, simply extend the final fitted line.
     */
    ACCUMDATA( 3, fRtWgt ) ;
    if( lInterpolate ) {
      EVALPOINT( fMeans[1], iBtimes[2] ) ;
      fYVal = ((1.0 - fSkWgt) * fMeans[0]) + (fSkWgt * fMeans[1]) ;
    } else {
      EVALPOINT( fYVal, iBtime_a ) ;
    }
    /* Restore the base value that was originally removed, and convert
     * back to folded output units.
     */
    fYVal += fBaseValue ;

    if( info->iModulos[ iDim ] == RTVEC_MOD_NONE ) {
      fData_a[ iDim ] = fYVal ;

    } else {
      fYVal = fmod( fYVal, 360.0 ) ;
      if( fYVal <    0.0 ) fYVal += 360.0 ; /* Get the floating angle strictly */
      if( fYVal <    0.0 ) fYVal  =   0.0 ; /*   in the range [0, 360), not */
      if( fYVal >= 360.0 ) fYVal  =   0.0 ; /*   fuzzed slightly beyond. */

      switch( info->iModulos[ iDim ] ) {
      case RTVEC_MOD_UBIN:
	fData_a[ iDim ] = fYVal ; break ;
      case RTVEC_MOD_SBIN:
	fData_a[ iDim ] = (fYVal <= 180.0) ? fYVal : (fYVal - 360.0) ; break ;
      case RTVEC_MOD_EBIN:
	fData_a[ iDim ] = (fYVal <= 270.0) ? fYVal : (fYVal - 360.0) ; break ;
      }
    }
  }
  /* Optionally unlock the data structure on the way out.
   */
 exit:
  if( rtvec_a->pUnLock ) ( (void(*)(void))rtvec_a->pUnLock )() ;

  return iResultFlags;
}

/* *****************************
 * *                           *
 * *  StandAlone Test Utility  *
 * *                           *
 * *****************************
 */
#ifdef SELFTEST

#define NSLOTS   20		/* Number of slots in history */
#define VECDIM    1		/* Dimension of modelled vector */
#define NPLOTS    3		/* Number of plots (pruned end data) */

int main( int argc_a, char **argv_a )
{
  RTVECALLOCATE( NSLOTS, VECDIM ) rthist[NPLOTS] ; struct rtvec rtvec[NPLOTS] ;
  SINT4 iPlot, ii, ij ; FLT8 fValue ;
  struct { UINT4 iBtime ; FLT4 fValue ; } data[] = {
    {   0,1 },{ 100,1 },{ 200,1 },{ 300,0 },{ 400,1 },{ 500,2 },{ 600,2 },{ 700,2 }
  } ;
#define DATASLOTS ( sizeof(data) / 8 )

  /* Initialize the realtime models that will generate each of the
   * plots, and insert the appropriate amount of canned data.
   */
  for( iPlot=0 ; iPlot < NPLOTS ; iPlot++ ) {
    rtvecInitFirst( &rthist[iPlot].info, NSLOTS, VECDIM ) ;
    rthist[iPlot].info.iSlotWidthMS = 1 ;

    rtvecAttach( &rtvec[iPlot], &rthist[iPlot].info, NULL, NULL ) ;

    for( ii=0 ; ii < (DATASLOTS - iPlot) ; ii++ ) {
      fValue = data[ii].fValue ;
      rtvecInsertPoint( &rtvec[iPlot], data[ii].iBtime, &fValue ) ;
    }
  }
  /* Generate an array of lookup points to plot by scanning across the
   * interval of valid data.  Write one coordinate pair per line in a
   * format suitable for GNUPLOT.
   */
  for( ii=0 ; ii < (3*data[DATASLOTS-1].iBtime)/2 ; ii++ ) {
    fprintf( stdout, "%5d", ii ) ;
    for( iPlot=0 ; iPlot < NPLOTS ; iPlot++ ) {
      rtvecLookupPoint( &rtvec[iPlot], ii, &fValue ) ;
      fprintf( stdout, "%10.3f", fValue ) ;
    }
    for( ij=0 ; ij < DATASLOTS ; ij++ ) {
      if( ii == data[ij].iBtime ) {
	fprintf( stdout, "%10.3f", data[ij].fValue ) ; break ;
      }
    }
    fprintf( stdout, "\n" ) ;
  }

  return(0) ;
}
#endif /* #ifdef SELFTEST */
