'********************************************************************
'* SMOOTHING AN ARRAY OF N ORDINATES Y's (ASCENDING ORDER ABCISSAS) *
'*           using Savitzky-Golay filter coefficients               *
'* ---------------------------------------------------------------- *
'* Description:                                                     *
'* This program uses the routine SAVGOL for smoothing an array of   *
'* given ordinates (y's) that are in order of increasing abscissas  *
'* (x's), but without using the abscissas themselves supposed to be *
'* equally spaced. It first calculates the filter coefficients and  *
'* then applies the numerical filter to low-pass filter the data.   *
'* The user-specified parameter are: nl, nr and m  to enter "the    *
'* amount of smoothing", given roughly as the number of points over *
'* which the data should be smoothed.                               *
'* ---------------------------------------------------------------- *
'* SAMPLE RUN:                                                      *
'* Input data file contains:                                        *
'* 1024                                                             *
'* 0.00000000000000E+0000 7.50000000000000E-0001                    *
'* 9.21288168207468E-0003 7.77368637910513E-0001                    *
'* 1.84257633641494E-0002 8.34466556277221E-0001                    *
'* 2.76386450462383E-0002 9.03071871110114E-0001                    *
'* 3.68515267282987E-0002 9.92958153417021E-0001                    *
'* 4.60644084103592E-0002 1.09195646826811E+0000                    *
'* 5.52772900924197E-0002 1.15230452277865E+0000                    *
'* 6.44901717745370E-0002 1.06763022290215E+0000                    *
'* 7.37030534565974E-0002 1.34541171127239E+0000                    *
'* 8.29159351386579E-0002 1.48611048393104E+0000                    *
'* 9.21288168207184E-0002 1.09349703210864E+0000                    *
'* 1.01341698502779E-0001 1.72386602840743E+0000                    *
'* 1.10554580184839E-0001 1.14317464708984E+0000                    *
'* ---------------------- ----------------------                    *
'* 9.37871355209791E+0000 2.43969819122867E+0001                    *
'* 9.38792643377383E+0000 2.42468007203424E+0001                    *
'* 9.39713931544975E+0000 2.42436619192304E+0001                    *
'* 9.40635219712567E+0000 2.42829449073179E+0001                    *
'* 9.41556507880159E+0000 2.42980085689633E+0001                    *
'* 9.42477796047751E+0000 2.43119449022633E+0001                    *
'*                                                                  *
'* Output file contains (here nl=nr=5, m=4):                        *
'*                                                                  *
'*       Time          Y        Smoothed Y                          *
'* ----------------------------------------                         *
'*     0.000000     0.750000     0.750000                           *
'*     0.009213     0.777369     0.777369                           *
'*     0.018426     0.834467     0.834467                           *
'*     0.027639     0.903072     0.903072                           *
'*     0.036852     0.992958     0.992958                           *
'*     0.046064     1.091956     1.028790                           *
'*     0.055277     1.152305     1.162248                           *
'*     0.064490     1.067630     1.173897                           *
'*     0.073703     1.345412     1.281085                           *
'*     0.082916     1.486110     1.351451                           *
'*     0.092129     1.093497     1.426208                           *
'*     0.101342     1.723866     1.348183                           *
'*     0.110555     1.143175     1.332568                           *
'*     --------     --------     --------                           *
'*     9.341862    24.185782    24.167925                           *
'*     9.351075    24.135968    24.154880                           *
'*     9.360288    24.252124    24.215144                           *
'*     9.369501    24.183153    24.268185                           *
'*     9.378714    24.396982    24.279420                           *
'*     9.387926    24.246801    24.246801                           *
'*     9.397139    24.243662    24.243662                           *
'*     9.406352    24.282945    24.282945                           *
'*     9.415565    24.298009    24.298009                           *
'*     9.424778    24.311945    24.311945                           *
'*                                                                  *
'* Note: the last nr points are unchanged.                          *
'* ---------------------------------------------------------------- *
'* Reference:  "Numerical Recipes By W.H. Press, B. P. Flannery,    *
'*              S.A. Teukolsky and W.T. Vetterling, Cambridge       *
'*              University Press, 1986 - 1992" [BIBLI 08].          *
'*                                                                  *
'*                              Basic Release By J-P Moreau, Paris  *
'*                                       (www.jpmoreau.fr)          *
'********************************************************************
'PROGRAM tsavgol

DEFINT I-N

NMAX = 2048: NP = 50: MMAX = 6

DIM ysignal(NMAX), ysave(NMAX)
DIM c(NP)
DIM index(NP)

  FOR i = 1 TO NMAX
    ysignal(i) = 0!
  NEXT i

  'open input and output files
  OPEN "smooth.dat" FOR INPUT AS #1
  OPEN "tsavgol.lst" FOR OUTPUT AS #2

  'read number of input ysignal points in input file
  INPUT #1, ndata

  'read ndata couples T(i), Y(i) in input data file
  FOR i = 1 TO ndata
    INPUT #1, temp, ysignal(i)
    IF i = 1 THEN tbegin = temp
    IF i = ndata THEN tend = temp
  NEXT i
  CLOSE #1

  FOR i = 1 TO NMAX
    ysave(i) = ysignal(i)   'save unsmoothed ysignal
  NEXT i

  nl = 5: nr = 5: m = 4    'see savgol
  
' seek shift index for given case nl, nr, m (see savgol)
  index(1) = 0
' example: case nl=nr=5
' index(2)=-1; index(3)=-2; index(4)=-3; index(5)=-4; index(6)=-5
  j = 3
  FOR i = 2 TO nl + 1
    index(i) = i - j
    j = j + 2
  NEXT i
' index(7)= 5; index(8)= 4; index(9)= 3; index(10)=2; index(11)=1
  j = 2
  FOR i = nl + 2 TO nl + nr + 1
    index(i) = i - j
    j = j + 2
  NEXT i

' calculate Savitzky-Golay filter coefficients.
  np1 = nl + nr + 1: ld = 0
  GOSUB 1000 'call savgol(c,npl,nl,nr,ld,m)

  CLS
  PRINT
  PRINT " Number of left points .......: "; nl
  PRINT " Number of right points ......: "; nr
  PRINT " Order of smoothing polynomial: "; m
  PRINT
  PRINT " Savitzky-Golay Filter Coefficients:"

  F$ = "#####.######"
  F1$ = "###.######"

  FOR i = 1 TO nl + nr + 1
    PRINT USING F1$; c(i);
  NEXT i
  PRINT

' Apply filter to input data
  FOR i = 1 TO ndata - nr
    ysignal(i) = 0!
    FOR j = 1 TO nl + nr + 1
      IF i + index(j) > 0 THEN 'skip left points that do not exist
        ysignal(i) = ysignal(i) + c(j) * ysave(i + index(j))
      END IF
    NEXT j
  NEXT i

' write results to output file
  dt = (tend - tbegin) / (ndata - 1)
  t = tbegin - dt
  PRINT #2, "      Time          Y       Smoothed Y "
  PRINT #2, "---------------------------------------"
  FOR i = 1 TO ndata
    t = t + dt
    PRINT #2, USING F$; t;
    PRINT #2, USING F$; ysave(i);
    PRINT #2, USING F$; ysignal(i)
  NEXT i

  CLOSE #2
  PRINT
  PRINT " Results in file tsavgol.lst."
  PRINT

END 'of main program


500 'Function IMin(ia,ib)
  IF ia <= ib THEN
    IMin = ia
  ELSE
    IMin = ib
  END IF
RETURN

600 'Function Power(x, n)
'Integer power n of x (n>=0)
  result = 1!
  IF N = 0 THEN
    Power = result
    RETURN
  ELSE
    FOR i = 1 TO N
      result = x * result
    NEXT i
  END IF
  Power = result
RETURN


1000 'Subroutine savgol(c, np1, nl, nr, ld, m)
'--------------------------------------------------------------------------------------------
'USES lubksb,ludcmp given below.
'Returns in c(np), in wrap-around order (see reference) consistent with the argument respns
'in routine convlv, a set of Savitzky-Golay filter coefficients. nl is the number of leftward
'(past) data points used, while nr is the number of rightward (future) data points, making
'the total number of data points used nl +nr+1. ld is the order of the derivative desired
'(e.g., ld = 0 for smoothed function). m is the order of the smoothing polynomial, also
'equal to the highest conserved moment; usual values are m = 2 or m = 4.
'--------------------------------------------------------------------------------------------
'id:integer
DIM INDX(MMAX + 1)
DIM A(MMAX + 1, MMAX + 1)
DIM B(MMAX + 1)

  IF NP < nl + nr + 1 OR nl < 0 OR nr < 0 OR ld > m OR m > MMAX OR nl + nr < m THEN
    PRINT " Bad args in savgol."
    RETURN
  END IF

  FOR i = 1 TO MMAX + 1
    FOR j = 1 TO MMAX + 1
      A(i, j) = 0!
    NEXT j
    B(i) = 0!
    INDX(i) = 0
  NEXT i

  FOR ipj = 0 TO 2 * m      'Set up the normal equations of the desired leastsquares fit.
    sum = 0!
    IF ipj = 0 THEN sum = 1!
    FOR k = 1 TO nr
      x = 1! * k: N = ipj: GOSUB 600
      sum = sum + Power
    NEXT k
    FOR k = 1 TO nl
      x = -1! * k: N = ipj: GOSUB 600
      sum = sum + Power
    NEXT k
    ia = ipj: ib = 2 * m - ipj: GOSUB 500
    mm = IMin
    imj = -mm
    WHILE imj <= mm
      A(1 + (ipj + imj) / 2, 1 + (ipj - imj) / 2) = sum
      imj = imj + 2
    WEND
  NEXT ipj

  GOSUB 2000 'call ludcmp(a,m+1,indx,id,icode)      Solve them: LU decomposition
 
  FOR j = 1 TO m + 1
    B(j) = 0!
  NEXT j

  B(ld + 1) = 1!'Right-hand side vector is unit vector, depending on which derivative we want

  GOSUB 3000 'call lubksb(a,m+1,MMAX+1,indx,b)  Backsubstitute, giving one row of the inverse matrix
  
  FOR kk = 1 TO np1                  'Zero the output array (it may be bigger than the number
    c(kk) = 0!                       'of coefficients
  NEXT kk

  FOR k = -nl TO nr                  'Each Savitzky-Golay coefficient is the dot product
    sum = B(1)                       'of powers of an integer with the inverse matrix row
    fac = 1!
    FOR mm = 1 TO m
      fac = fac * k
      sum = sum + B(mm + 1) * fac
    NEXT mm
    kk = ((np1 - k) MOD np1) + 1     'Store in wrap-around order
    c(kk) = sum
  NEXT k

RETURN


'***************************************************************
'* Given an N x N matrix A, this routine replaces it by the LU *
'* decomposition of a rowwise permutation of itself. A and N   *
'* are input. INDX is an output vector which records the row   *
'* permutation effected by the partial pivoting; D is output   *
'* as -1 or 1, depending on whether the number of row inter-   *
'* changes was even or odd, respectively. This routine is used *
'* in combination with LUBKSB to solve linear equations or to  *
'* invert a matrix. Return code is 1, if matrix is singular.   *
'***************************************************************
2000 'Subroutine LUDCMP(A, N, INDX, ID, ICODE)

 NMAX = 100: TINY = 1E-12

 DIM VV(NMAX)
  
 ID = 1: ICODE = 0: N = m + 1

 FOR i = 1 TO N
   AMAX = 0!
   FOR j = 1 TO N
     IF ABS(A(i, j)) > AMAX THEN
       AMAX = ABS(A(i, j))
     END IF
   NEXT j
   IF AMAX < TINY THEN
     ICODE = 1
     RETURN
   END IF
   VV(i) = 1! / AMAX
 NEXT i

 FOR j = 1 TO N
   FOR i = 1 TO j - 1
     sum = A(i, j)
     FOR k = 1 TO i - 1
       sum = sum - A(i, k) * A(k, j)
     NEXT k
     A(i, j) = sum
   NEXT i
   AMAX = 0!
   FOR i = j TO N
     sum = A(i, j)
     FOR k = 1 TO j - 1
       sum = sum - A(i, k) * A(k, j)
     NEXT k
     A(i, j) = sum
     DUM = VV(i) * ABS(sum)
     IF DUM >= AMAX THEN
       IMAX = i
       AMAX = DUM
     END IF
   NEXT i
   
   IF j <> IMAX THEN
     FOR k = 1 TO N
       DUM = A(IMAX, k)
       A(IMAX, k) = A(j, k)
       A(j, k) = DUM
     NEXT k
     ID = -ID
     VV(IMAX) = VV(j)
   END IF

   INDX(j) = IMAX
   IF ABS(A(j, j)) < TINY THEN
     A(j, j) = TINY
   END IF

   IF j <> N THEN
     DUM = 1! / A(j, j)
     FOR i = j + 1 TO N
       A(i, j) = A(i, j) * DUM
     NEXT i
   END IF
 NEXT j

RETURN


'******************************************************************
'* Solves the set of N linear equations A . X = B.  Here A is     *
'* input, not as the matrix A but rather as its LU decomposition, *
'* determined by the routine LUDCMP. INDX is input as the permuta-*
'* tion vector returned by LUDCMP. B is input as the right-hand   *
'* side vector B, and returns with the solution vector X. A, N and*
'* INDX are not modified by this routine and can be used for suc- *
'* cessive calls with different right-hand sides. This routine is *
'* also efficient for plain matrix inversion.                     *
'******************************************************************
3000 'Subroutine LUBKSB(A, N, INDX, B)

 II = 0

 FOR i = 1 TO N
   LL = INDX(i)
   sum = B(LL)
   B(LL) = B(i)
   IF II <> 0 THEN
     FOR j = II TO i - 1
       sum = sum - A(i, j) * B(j)
     NEXT j
   ELSEIF sum <> 0! THEN
     II = i
   END IF
   B(i) = sum
 NEXT i

 FOR i = N TO 1 STEP -1
   sum = B(i)
   IF i < N THEN
     FOR j = i + 1 TO N
       sum = sum - A(i, j) * B(j)
     NEXT j
   END IF
   B(i) = sum / A(i, i)
 NEXT i

RETURN

'end of file tsavgol.bas