####These are a set of functions for perfoming Savitsky Golay algorithm.
####Note that we have modified the function so that it does not return NAs 
####This is done by maintaining the width, but shifting the zero point as needed.


savgol.coef<-function(x,  degree, wfun=NULL ,  ...)
{
  ##This is a support function that returns the weights
  xx <- outer(x, 0:degree, "^")
  ##1,x,x^2 ...
  if(is.null(wfun)) weights <- rep(1, length(x)) else weights <- wfun(x, ...)
  twxx <- t(weights * xx)
  ## x[i,j]*xx[i] column major order = diag(weights)%*%xx 
  xxtxx <- solve(twxx %*% xx) %*% twxx
  ## least squares y~Ax+e ; e~N(0,1/weights)
  return(xxtxx)
}

weight.fun<- function(x, weights)
  {
###passthrough function
    if(length(weights) != length(x) | any(weights <= 0))
      stop("Invalid Weights")
    else return(weights)
  }
##exponential weightings 
exp.fun<- function(x, range = 1, power = 1)
{
  exp( - (abs(x/range)^power))
}


savgol.coef.inc<-function(n=c(2,2),degree=2,save=NULL, inc=1, ...)
  {
###This function calls savgol.coef
    x<-inc*seq(-n[1],n[2])
    y<-savgol.coef(x=x,degree=degree, ...=...)
    attributes(y)<-c(attributes(y),list(n=n,inc=inc,degree=degree, other=list(...)))
    if(!is.null(save)) assign(save,y,where=0)
    y
  }

savgol.best<-function(x=use.best,field="Wmax",...)
{
xt<-x$Shour
yt<-x[[field]]
cnames<-c(paste(field,"S",sep=""),paste("D",field,"Dt",sep=""))
idx<-split(1:nrow(x),x$Sid)
tmp<-savgol.field(x=xt,y=yt,index=idx,colNames=cnames,...)
cbind(x,tmp)
}


savgol.field<-function(x,y,index,n=c(3,2),degree=3,colNames=c("WmaxS","DWmaxDt"),...)
    {
    if(any(n<0)) stop("Please use non negative values for window")
    nc<-length(colNames)
    if(nc == 0) stop("Please provide column names")
    nr<-length(x)
    if(nr != length(y)) stop("x and y series lengths do not agree")
    #x and y are the field values
    #index is a list of position vectors for each Storm Id
    #This is a wrapper function for savgol
    deriv=(1:nc)-1
    if(degree < nc-1) stop("Degree of polynomial must be as large as maximum derivative order")
    if(degree == sum(n)) warning("Exact fit, no smoothing")
    if(degree > sum(n)) stop("Indeterminite form degree to large")
    z<-matrix(0,nr,nc)
    colnames(z)<-colNames

    for(i in seq(along=index))
        {
        obs<-index[[i]]
        yy<-y[obs]
        xx<-x[obs]
        lobs<-length(obs)
        if(lobs>sum(n))
            {
            z[obs,]<-savgol.ireg(x=xx,y=yy,n=n,deriv=deriv,degree=degree,...)
            }
        else
            { ###Here we perform symmetric differences for first and second derivatives.
            ###
            z[obs,1]<-yy
            if(nc > 1 && lobs >1)
                {
                if(lobs==2)
                    z[obs,2]<-(yy[2]-yy[1])/(xx[2]-xx[1])
                else
                    {
                    z[obs,2]<-c((yy[2]-yy[1])/(xx[2]-xx[1]),(yy[3:lobs]-yy[1:(lobs-2)])/(xx[3:lobs]-xx[1:(lobs-2)]),(yy[lobs]-yy[lobs-1])/(xx[lobs]-xx[lobs-1]))
                    if(nc > 2)
                      z[obs,3]<-c(0,2*diff(diff(yy)/diff(xx))/(xx[3:lobs]-xx[1:(lobs-2)]),0)
                    }
                }
            }
        }
    return(z)
    }


savgol.reg.fit<-function(A,y,deriv=c(0,1))
  {
    ##Given the model, fit the data y, return a matrix with one column per degree
####model is A
    rd<-range(deriv)
    if(rd[1]<0 | rd[2] >= nrow(A)) stop("Invalid derivative values")
    user<-deriv+1
    n<-attr(A,"n")
    nl<-sum(n)+1
    ny<-length(y)
    if(ny< nl)
      {
        out<-matrix(NA,ny,length(deriv))
      }
    else
      {
        out<-apply(A[user,,drop=F],1,function(x,y) filter(y,rev(x),sides=1),y=y)
        if(n[2]>0)
          {
            n12<-1:n[2] #The old series lags the requried series by n[2] there will be ast least n[2] NA in front lag convoultion
            z<-out[n12,] #just gets the old NA
            out<-out[-n12,] #removes the NA
            out<-rbind(out,z) #puts new NA at end.. if we need future values to estimate function.
          }
        deg.deriv<-matrix(gamma(user),nrow(out),ncol(out),byrow=T)
        ##Since Y=a0+a1*x+a2*x^2 ... then y'(0)=a1 y"(0)=2*a2, y^n(0)=an*gamma(n+1)
        out<-out*deg.deriv
      }
    colnames(out)<-paste("D",deriv,sep=".")
    rownames(out)<-names(y)
    out
  }

savgol.reg<-function(y,n=c(2,2),degree=2,inc=1,x=NULL,use.old=T,print.warning=T,save=T,deriv=c(0,1),...)
  {
    ##The first argument
    ##This function pefroms a fit.
    ##y is the response
    ##n is the left and right filter lengths
    ##degree is the filter degree
    ##inc is the increment
    ##If save is true model fom savgol.coef.inc will be saved in frame zero as .savgo.nl.nr.degree
    ##If use.old is true model from savgol.coef.inc will be retrieved, the attributes will be compard
    ##the x argument is not used, but we eat any "x" around so as not to pass it to savgol.coef.inc
    ##This is a finite length window algorithm, it returns smooths estimates of the function values evaluated at
    ##The local points. The resulting data may be interpolated using loess spline technique applied to the derivative of interest.
    ##
    if(use.old || save)
      name<-paste(".savgol",n[1],n[2],degree,sep=".")      
    if(use.old)
      {
        if(exists(pos=1,name))
          {
            A<-get(pos=1,name)
            new.attr<-list(n=n,inc=inc,degree=degree,other=list(...))
            A.attr<-attributes(A)[c("n","inc","degree","other")]
            compare.all<-all.equal(new.attr,A.attr)
            if(is.character(compare.all))
              {
                if(print.warning) warning(paste("Model attributes mismatch\n",compare.all))
                use.old<-F
              }
          }
        else
          {
            if(print.warning) warning(paste("Model cannot be found"))
            use.old<-F
          }
       
      }
    if(!use.old)
      A<-savgol.coef.inc(n=n,inc=inc,degree=degree,save=name,...=...)
    return(savgol.reg.fit(y=y,A=A,deriv=deriv))
  }

ssa.matrix<-function(x,n, left=NULL,right=NULL)
{
  ##copied from scom.q (see scom.q for details)
  ##also listed in c:/research/bayes/extreme/atlantic.ssc
    lx <- length(x)
    if(lx <  n)
        stop("n must be smaller than length of x")
    lx1 <- lx + 1
    xx <- rep(x, n + 1)[1:(lx1 * n)]
    dim(xx) <- c(lx1, n)
    xx <- xx[1:(lx1 - n),,drop=F]
    nn<-nrow(xx)
    mm<-1:nn
    if(!is.null(left) && left >0)
    mm<-c(rep(1,left),mm)
    if(!is.null(right) && right > 0)
    mm<-c(mm,rep(nn,right))
    xx[mm,]
}
savgol.ireg.fit<-function(x,n,degree,deriv, wfun,  ...)
  {
#This function does a single fit, supports savgol.ireg function 
    A<-savgol.coef(x=x[,1],degree=degree,wfun=wfun, ...=...)
    rd<-range(deriv)
    if(rd[1]<0 | rd[2] >= nrow(A)) stop("Invalid derivative values")
    user<-deriv+1    
    A[user,,drop=F]%*%x[,2]*gamma(user)
  }
savgol.ireg<-function(x,y,n=c(2,2),degree=2,inc=NULL,wfun=NULL,deriv=c(0,1),...)
  {
    #`#this function assumes that the spacing on x is irregular. This amounts to a fit by  
    ##some` usefull functions
    ##inc set to null to eat it.
    ln<-sum(n)+1
    lx<-length(x)
    if(lx < ln)
      {
        out<-matrix(NA,lx,length(deriv))
      }
    else
      {
        xxraw<-ssa.matrix(x,ln,n[1],n[2]) #get corresponding x value
        left<-1:n[1]
        right<-1:n[2]
        lx<-length(x)
        xx<-xxraw-x #this gives differences for moving window xx[
        yy<-ssa.matrix(y,ln,n[1],n[2]) #get corresponding y values
        zz<-array(c(xx,yy),c(dim(xx),2)) #create three dimensional array
        zz<-aperm(zz,c(2,3,1)) #third dimension is for each value of x, zz[,,2] is a matrix with x column in first row y column in second
        out<-apply(zz,3,savgol.ireg.fit,degree=degree,wfun=wfun,deriv=deriv,...=...) #matrix
#       out<-do.call("cbind",c(rep(list(NA),n[1]),list(out),rep(list(NA),n[2]))) #add columns to center x0 values over correct positions
        out<-t(out) #now rows are columns
      }
    colnames(out)<-paste("D",deriv,sep=".")
    out
  }

savgol<-function(y,n=c(2,2),degree=2,deriv=c(0,1),inc=1,x=NULL, wfun=NULL, tol=10^{-7} ,...)
  {
    #This is a warper function for the Savitzky-Golay algorithm. It calls either savgol.ireg for irregularly spaced
    #time series or it calls savgol.fit and savglo.coef for regularly spaced coefficients.
    #Since calculating the coefficients can be redundant, the model option is provided for a saved model.
    #this function computes a matrix for the Savitsky-Golay smoothing filter and smooths the values for y
    #stored allows us to resuse old smoothing matrices if diff(x) is the same.
    #If coef is not null then we 
    #if x is provided then it fits  sum(a[j,i]*(x[i+j]-x[j])^k to y[i+j] for i=-n[1] to i=n[1] and k=0 to degree
    #note we offset, but this is taken care of in the filter function. 
    #if y is provided it returns a matrix of fitted y values 1,2,..,k derivatives,note that the first n[1] and last n[2]
    # Do easy case first

    ###first we decide which model we are going to use...

    if(!is.null(x))
      {
        dx<-diff(x)
        if(all(abs(dx-dx[1]) < tol))
          inc<-mean(dx)
        else
          inc<-NULL
      }
    else
      if(is.null(inc)) inc<-1
      else
        if(inc<=0) stop("Invalid increment")
    if(!is.null(inc))
      sav.fun<-savgol.reg
    else
      sav.fun<-savgol.ireg
      sav.fun(x=x,y=y,n=n,degree=degree,inc=inc, wfun=wfun,deriv=deriv, ...)
  }
        
add.rates<-function(x=best.track,deriv=c(0,1,2),n=c(2,2),degree=3,idfield="Sid",timefield="days",datafield="Wmax",...)
  {
    ##This function adds a rate column  and a smoothed data column to the data set using symmetric differences
                                        ##Since fitting the data to a smooth spline would give instantaneous estimates we do not do that here.
                                        ##See work in c:/southcom/funs/scom.q
   ##Note that initial and final rates will be zero.
   ##We assume of course that the value occurs at the center point, but may not always be the case. 
    index<-split(1:nrow(x),x[,idfield])
    out<-matrix(0,nrow(x),length(deriv))
    xx<-x[,timefield]
    yy<-x[,datafield]
    lx<-length(index)
    for(i in 1:lx)
      {
        old<-i>1
        if(!(i%%50)) print("\n", format(1:lx,lx)[i]) else print(".")
        ii<-index[[ii]]
        out[ii,]<-savgol(x=xx[ii],y=yy[ii],n=n,degree=degree,deriv=deriv, save=T, use.old=old)
      }
    out
    ##        o.index<-sapply(index,I)
    ##        int.rates<-lapply(index,function(x,
    ##                                         
    ##   \
    ##                       int.ratesMS<-int.rates*.5144 #in m/(sec*days)
    ##
  }