.packageName <- "cwhmath"
### Rational Transfer Function objects for S-PLUS.
### Author:  Henrik Aalborg Nielsen, IMM, DTU (han@imm.dtu.dk)

rtf <- function(A=1, B=1, delay=0, unit.sg=T, stability.check=T)
{
  ## Creates and checks a rational transfer-function object
  ##
  ## y_t = H(q) x_t, where H(q) = q^-delay B(q^-1) / A(q^-1)
  ##
  ## If unit.sq is TRUE (default) the coefs. of B(q^-1) is multiplied
  ## with a factor making the stationary gain one.
  ##
  ## A(q^-1) = 1   - a_1 q^-1 - ... - a_na q^-na
  ## B(q^-1) = b_0 + b_1 q^-1 + ... + b_nb q^-nb
  ##
  ## Note that '-' is used in A() and '+' in B(), these are specified as:
  ##
  ## A = c(1,   a_1, .... , a_na)
  ## b = c(b_0, b_1, .... , b_nb)
  ##
  ## If stability.check is TRUE (default) the function will stop if any
  ## poles of A() is outside the unit circle.
  ##
  ## VALUE:  A list with elements:
  ## call:    Image of the call
  ## stable:  Logical indicating if the transfer-function is stable
  ## sg:      Stationary gain (only if stable is true)
  ## n.init:  Number of initial values needed
  ## A:       A
  ## B:       B
  ## delay:   delay

  call <- match.call()
  
  if(A[1] != 1) stop("A[1] must be 1")

  # Stability  
  if(length(A) == 1)
    stable <- T
  else {
    tmp <- A
    tmp[-1] <- - A[-1]  # Def. of signs on params
    stable <- all(abs(polyroot(rev(tmp))) < 1)
  }
  if(stability.check && !stable)
    stop("unstable transfer-function")
  if(!stable && unit.sg)
    stop("unstable system: not possible to make the stationary gain one")
  
  ## Stationary gain
  if(stable) {
    if(length(A) == 1)
      sg <- sum(B)
    else
      sg <- sum(B) / (1 + sum(-A[2:length(A)]))
  }
  else
    sg <- NA
  
  if(unit.sg) {
    B <- B/sg
    sg <- 1
  }

  ## Number of initial values needed
  n.init <- length(A) - 1

  ## Return value
  z <- list(call=call, stable=stable, sg=sg, n.init=n.init,
            A=A, B=B, delay=delay)
  return(structure(z, class="rtf"))
}


rtf.filter <- function(x, rtfobj, init)
{
  ## Filter x using a rational transfer function object (rtfobj) as created by rtf().
  ## If initialization is needed 'init' is supplied to the recursive filter (the first).
  ##
  ## Note that:
  ##
  ## * 'init' is multiplied with the stationary gain of the recursive filter before
  ##   it is applied, i.e. replaced by init/A(1).
  ##
  ## * First the series is filtered trough 1/A(q^-1), and the initialization is in
  ##   terms of the output of this filter.  Furthermore, 'init' is used to calculate
  ##   the first value of the filtered series, i.e. 'init' corresponds to times
  ##   0, -1, -2, ...
  ##
  ## * The causal convolution filter cannot return values for time <= length(B) - 1,
  ##   since it do not use initialization.
  ##
  ## * Since the recursive filter is run first (an no missing i allowed in x)
  ##   the bug in filter() when the series starts with NA will not become active.

  if(!rtfobj$stable)
    warning("unstable filter")
  
  if(any(is.na(x)))
    stop("missing values in x not allowed")
  
  ## z_t = 1 / A(q^-1) x_t 
  if(rtfobj$n.init > 0) {
    if(missing(init))
      init <- x[1:rtfobj$n.init]
    init <- init / (1 + sum(-rtfobj$A[2:length(rtfobj$A)]))
    x <- filter(x=x, filter=rtfobj$A[2:length(rtfobj$A)], method="rec", init=init)
  }
  
  ## B(q^-1) z_t = B(q^-1) / A(q^-1) x_t
  x <- filter(x=x, filter=rtfobj$B, method="con", sides=1) 

  ## Delay (H(q) x_t)
  x <- as.numeric(x)
  x <- c(rep(NA, rtfobj$delay), x[(1+rtfobj$delay):length(x)])
  
  ## Return filtered series
  return(as.numeric(x))
}


rtf.impulse <- function(rtfobj, lag.max, plot.it=T,
                        nzero=2, type="h",
                        xlab="Lag", ylab="Impulse Response", ... )
{
  ## Impulse response of rtfobj (one like the one created by rtf()),
  ## i.e. the response on a unit impule corresponding to index 1 of
  ## the output.
  init <- rep(0, rtfobj$n.init)
  x <- rep(0, length(rtfobj$B) - 1 + 2*rtfobj$delay)
  x <- c(x, 1, rep(0, lag.max))
  y <- rtf.filter(x=x, rtfobj=rtfobj, init=init)
  idx <- length(y) - rtfobj$delay - (lag.max:0)
  y <- y[idx]
  names(y) <- as.character(0:lag.max)
  if(!plot.it)
    return(y)
  plot(c(rep(0,nzero),0:lag.max), c(rep(0,nzero),y),
       type=type, xlab=xlab, ylab=ylab, ...)
  abline(h=0)
  invisible(return(y))
}


rtf.step <- function(rtfobj, lag.max, plot.it=T,
                     nzero=2, type="h",
                     xlab="Lag", ylab="Step Response", ylim, ... )
{
  ## Step response of rtfobj (one like the one created by rtf())
  ## i.e. the response on a unit step corresponding to index 1 of
  ## the output.
  init <- rep(0, rtfobj$n.init)
  x <- rep(0, length(rtfobj$B) - 1 + 2*rtfobj$delay)
  x <- c(x, rep(1, lag.max+1))
  y <- rtf.filter(x=x, rtfobj=rtfobj, init=init)
  idx <- length(y) - rtfobj$delay - (lag.max:0)
  y <- y[idx]
  names(y) <- as.character(0:lag.max)
  if(!plot.it)
    return(y)
  if(missing(ylim))
    ylim <- sort(range(c(0,rtfobj$sg,y)))
  plot(c(rep(0,nzero),0:lag.max), c(rep(0,nzero),y),
       type=type, xlab=xlab, ylab=ylab, ylim=ylim, ...)
  abline(h=0)
  if(rtfobj$stable)
    abline(h=rtfobj$sg, lty=2)
  invisible(return(y))
}


print.rtf <- function(x, ..., digits)
{
  rtfobj <- x
  cat("Rational transfer function (rtf) object.\n")
  cat("Call:\n")
  print(rtfobj$call)
  cat(paste(ifelse(rtfobj$stable, "Stable", "UNSTABLE"),
            "filter with definition (coef. rounded in display):\n"))
  if(missing(digits))
    digits <- options()$digits
  digits <- max(3, digits-3) 
  A <- signif(rtfobj$A, digits=digits)
  B <- signif(rtfobj$B, digits=digits)
  delay <- rtfobj$delay
  if(length(A) == 1)
    A.poly <- paste(A)
  else {
    A.poly <- c(1, paste(A[-1], " q^-", 1:(length(A)-1), sep=""))
    A.poly <- paste(A.poly, collapse=" - ")
  }
  if(length(B) == 1)
    B.poly <- paste(B)
  else {
    B.poly <- c(B[1], paste(B[-1], " q^-", 1:(length(B)-1), sep=""))
    B.poly <- paste(B.poly, collapse=" + ")
  }
  if(delay != 0) 
    delay.txt <- paste("q^-", delay, sep="")
  else
    delay.txt <- ""
  empty.space <- paste(rep(" ", nchar(delay.txt)), collapse="")
  cat("\n")
  cat("\t", empty.space, B.poly, "\n")
  cat("\t", delay.txt,
      paste(rep("-", max(nchar(B.poly), nchar(A.poly))), collapse=""), "\n")
  cat("\t", empty.space, A.poly, "\n")
  cat("\n")  
  cat(paste("Stationary gain:", rtfobj$sg), "\n")
  cat(paste("Number of initial values needed for filtering:", rtfobj$n.init), "\n")
  invisible(return(rtfobj))
}


plot.rtf <- function(x, ..., lag.max)
{
  ## Graphical representation of an object of class 'rtf'.  If lag.max is not specified reasonable things happens.  '...' are passed to rtf.impulse and rtf.step.

  oldpar <- par()
  on.exit(par(oldpar))
  par(mfrow=c(2,2))

  zeros <- polyroot(rev(x$B))
  tmp <- x$A
  tmp[-1] <- - x$A[-1]  
  poles <- polyroot(rev(tmp))  # Bug in calc. of poles for ploting fixed Wed Sep 5 2001

  par(pty="s")

  plot(complex(modulus=1, argument=seq(0,2*pi,length=200)),
       type="l",
       xlab="Re", ylab="Im",
       main="Zeros",
       xlim=c(-1,1)*max(1, abs(zeros)),
       ylim=c(-1,1)*max(1, abs(zeros)))
  abline(h=0,v=0,lty=2)
  if(length(zeros) > 0)
    points(zeros, pch=1)
  else
    text(0, 0, "No Zeros")
  
  plot(complex(modulus=1, argument=seq(0,2*pi,length=200)),
       type="l",
       xlab="Re", ylab="Im",
       main="Poles",
       xlim=c(-1,1)*max(1, abs(poles)),
       ylim=c(-1,1)*max(1, abs(poles)))
  abline(h=0,v=0,lty=2)
  if(length(poles) > 0)
    points(poles, pch=1)
  else
    text(0, 0, "No Poles")

  par(pty="m")

  if(missing(lag.max))
    lag.max <- ceiling(3 / (1 - max(abs(poles)))) + x$delay
  if(is.na(lag.max))
    lag.max <- 1
  lag.max <- lag.max + ceiling(1.5*length(x$B))

  if(x$stable) {
    rtf.impulse(x, lag.max=lag.max, ...)
    rtf.step(x, lag.max=lag.max, ...)
  }
  else
    warning("Unstable filter impulse and step response not plotted")
            
  invisible(return())
  
}
adaptsim <- function(f,a,b,tol=.Machine$double.eps,trace=FALSE,...) {
#adaptsim  Numerically evaluate integral using adaptive
#   Simpson rule.
#
#   adaptsim(f,a,b) approximates the integral of
#   f(x) from A to B to machine precision. The
#   function f must return a vector of output values if
#   given a vector of input values.
#
#   adaptsim(f,a,b,tol) integrates to a relative
#   error of TOL.
#
#   adaptsim(f,a,b,tol,trace) displays the left
#   end point of the current interval, the interval
#   length, and the partial integral.
#
#   adaptsim(f,a,b,tol,trace,p1,p2,...) allows
#   coefficients p1, ... to be passed directly to the
#   function f:  G <- f(x,p1,p2,...). To use default values
#   for tol or trace, one may pass the empty matrix ([]).
#
#   See also adaptsimstp.
#
#   Walter Gander, 08/03/98
#   Reference: Gander, Computermathematik, Birkhaeuser, 1992.

  assign("termination2", FALSE, env = .GlobalEnv)
  tol <- max(tol, .Machine$double.eps)
  x <- c(a, (a+b)/2, b);
  y <- f(x, ...);
  fa <- y[1]; fm <- y[2]; fb <- y[3];
  yy <- f(a+c(0.9501, 0.2311, 0.6068, 0.4860, 0.8913)*(b-a), ...)
  is <- (b - a)/8*(sum(y)+sum(yy));
  if (is == 0) is <- b-a
  is <- is*tol/.Machine$double.eps;
  Q <- adaptsimstp(f,a,b,fa,fm,fb,is,trace,...);
  attr(Q,"termination2") <- termination2
  Q
}


adaptsimstp <- function(f,a,b,fa,fm,fb,is,trace,...) {
#adaptsimstp  Recursive function used by adaptsim.
#
#   q <- adaptsimstp('f',a,b,fa,fm,fb,is,trace) tries to
#   approximate the integral of f(x) from A to B to
#   an appropriate relative error. The argument 'f' is
#   a string containing the name of f. The remaining
#   arguments are generated by adaptsim or by recursion.
#
#   See also adaptsim.
#
#   Walter Gander, 08/03/98

  m <- (a + b)/2; h <- (b - a)/4;
  x <- c(a + h, b - h);
  y <- f(x, ...);
  fml <- y[1]; fmr <- y[2];
  i1 <- h/1.5 * (fa + 4*fm + fb);
  i2 <- h/3 * (fa + 4*(fml + fmr) + 2*fm + fb);
  i1 <- (16*i2 - i1)/15;
  if ((is + (i1-i2) == is) | (m <= a) | (b <= m)) {
    if (((m <= a) | (b <= m)) & (!eval(termination2))) {
       warning('Interval contains no more machine number. \nRequired tolerance may not be met.');
       assign(termination2, TRUE, env = .GlobalEnv)
    }
    Q <- i1;
    if (trace) cat(a, b-a, Q,"\n");
    }
  else  Q <- adaptsimstp (f,a,m,fa,fml,fm,is,trace,...) +
        adaptsimstp (f,m,b,fm,fmr,fb,is,trace,...);
  attr(Q,"termination2") <- termination2;
  Q
}


adaptlob <- function (f,a,b,tol=.Machine$double.eps,trace=FALSE,...) {

#   See also adaptlobstp.

#   Walter Gautschi, 08/03/98
#   Reference: Gander, Computermathematik, Birkhaeuser, 1992.

  assign("termination2", FALSE, env = .GlobalEnv)
  tol <- max(tol, .Machine$double.eps)
  m <- (a+b)/2; h <- (b-a)/2;
  alpha <- sqrt(2/3); beta <- 1/sqrt(5);
  x1 <- .942882415695480; x2 <- .641853342345781;
  x3 <- .236383199662150;
  x <- c(a, m-x1*h, m-alpha*h, m-x2*h, m-beta*h, m-x3*h, m,
         m+x3*h, m+beta*h, m+x2*h, m+alpha*h, m+x1*h, b);
  y <- f(x, ...);
  fa <- y[1]; fb <- y[13];
  i2 <- (h/6)*(y[1]+y[13]+5*(y[5]+y[9]));
  i1 <- (h/1470)*(77*(y[1]+y[13])+432*(y[3]+y[11])+625*(y[5]+y[9])+672*y[7]);
  is <- h*(.0158271919734802*(y[1]+y[13])+.0942738402188500
     *(y[2]+y[12])+.155071987336585*(y[3]+y[11])+
     .188821573960182*(y[4]+y[10])+.199773405226859
     *(y[5]+y[9])+.224926465333340*(y[6]+y[8])
     +.242611071901408*y[7]);
  s <- sign(is)
  if (is == 0) is <- 1
  erri1 <- abs(i1-is);
  erri2 <- abs(i2-is);
  R <- 1; if (erri2 != 0) R <- erri1/erri2
  if ((R>0 & R<1)) tol <- tol/R;
  is <- s*abs(is)*tol/.Machine$double.eps;
  if(is == 0) is <- b-a;
  Q <- adaptlobstp(f,a,b,fa,fb,is,trace,...);
  attr(Q,"termination2") <- termination2
  Q
}

adaptlobstp <- function(f,a,b,fa,fb,is,trace,...) {
#adaptlobstp  recursive function used by adaptlob.
#
#   Q  <-  adaptlobstp('f',a,b,fa,fb,is,trace) tries to
#   approximate the integral of f(x) from A to B to
#   an appropriate relative error. The remaining
#   arguments are generated by adaptlob or by recursion.
#
#   See also ADAPTLOB.

#   Walter Gautschi, 08/03/98

  h <- (b-a)/2; m <- (a+b)/2;
  alpha <- sqrt(2/3); beta <- 1/sqrt(5);
  mll <- m-alpha*h; ml <- m-beta*h; mr <- m+beta*h; mrr <- m+alpha*h;
  x <- c(mll,ml,m,mr,mrr);
  y <- f(x, ...);
  fmll <- y[1]; fml <- y[2]; fm <- y[3]; fmr <- y[4]; fmrr <- y[5];
  i2 <- (h/6)*(fa+fb+5*(fml+fmr));
  i1 <- (h/1470)*(77*(fa+fb)+432*(fmll+fmrr)+625*(fml+fmr)+672*fm);
  if ((is + (i1-i2) == is) | (m <= a) | (b <= m)) {
    if (((m <= a) | (b <= m)) & (!eval(termination2))) {
       warning('Interval contains no more machine number. \nRequired tolerance may not be met.');
       assign("termination2", TRUE, env = .GlobalEnv)
    }
    Q <- i1;
    if (trace) cat(a, b-a, Q,"\n");
    }
  else
    Q <- adaptlobstp(f,a,mll,fa,fmll,is,trace,...)+
      adaptlobstp(f,mll,ml,fmll,fml,is,trace,...)+
      adaptlobstp(f,ml,m,fml,fm,is,trace,...)+
      adaptlobstp(f,m,mr,fm,fmr,is,trace,...)+
      adaptlobstp(f,mr,mrr,fmr,fmrr,is,trace,...)+
      adaptlobstp(f,mrr,b,fmrr,fb,is,trace,...);
  attr(Q,"termination2") <- termination2;
  Q
}
ellipse <- function(k, m, A = NULL, cn = NULL, a = NULL, b = NULL, phi = NULL)
{
  if (missing(A)) { # convert to call with A
    warning("not yet tested")
    A <- rotm(2,1,2,phi) %*% matrix(c(a,0,0,b), 2, 2)
  }
  else {#A <- a; cn <- b  quick fix 2004-09-03
      }
  k <- max(k, 4)
  r <- A[1, 2]/sqrt(A[1, 1] * A[2, 2])
  Q <- matrix(0, 2, 2)			 # construct matrix Q for
  Q[1, 1] <-  sqrt(A[1, 1] * (1+r)/2)    # transformation of circle
  Q[1, 2] <- -sqrt(A[1, 1] * (1-r)/2)	 # to ellipse
  Q[2, 1] <-  sqrt(A[2, 2] * (1+r)/2)
  Q[1, 1] <-  sqrt(A[2, 2] * (1-r)/2)
  alpha <- seq(0, by = (2 * pi)/k, length = k)	   # define angles
  Z <- cbind(cos(alpha), sin(alpha))     # points on unit circle
  X <- t(m + cn * Q %*% t(Z))            # coordinates of points on ellipse
  X
}					 # end of procedure ellipse

conf.ellipse <- function(k, m, A, df, level = 0.95)
{
  k <- max(k, 4)
	d <- sqrt(diag(A))
	dfvec <- c(2, df)
	phase <- acos(A[1, 2]/(d[1] * d[2]))
	angles <- seq( - (pi), pi, len = k)
	mult <- sqrt(dfvec[1] * qf(level, dfvec[1], dfvec[2]))
	xpts <- m[1] + d[1] * mult * cos(angles)
	ypts <- m[2] + d[2] * mult * cos(angles + phase)
	cbind(xpts, ypts)
}
frac <- function(x,d) {  # fractional part
  res <- abs(x-trunc(x))
  if (!missing(d)) res <- round(10^d*res)
  res
}
is.constant <-  function(x) {
  if (is.factor(x)) (length(attributes(x)$levels)==1) && (!any(is.na(as.character(x))))
  else (is.numeric(x) && !any(is.na(x)) && identical(min(x), max(x)))
}
lengths.angle <- function(x,y=NULL) {
  if (!is.null(y)) {
    x <- cbind(x,y)
  }
  w <- t(x) %*% x
  a <- sqrt(w[1,1])
  b <- sqrt(w[2,2])
  c <- if (abs(a*b) <= .Machine$double.eps) 0 else acos(w[1,2]/(a*b))
  list(lx=a, ly=b, angle=c, angleDeg=c*180/pi)
}
my.var <- function(x, y=NULL) {  ## omit rows with NA
#   if (!missing(y)) x <- cbind(x,y)
#   res <- var(x[rowSums(is.na(x)) == 0,])
#   if (dim(res)[1]==2) res <- res[1,2]
#   else  dimnames(res) <- list(names(x)[1],names(x)[1])
#   res
  if (!missing(y)) x <- cbind(x,y)
  var(x, y, na.rm = TRUE, use = "complete")
}
my.cor <- function(x, y=NULL) {  ## omit rows with NA
#   if (!missing(y)) x <- cbind(x,y)
#   res <- cor(x[rowSums(is.na(x)) == 0,])
#   if (dim(res)[1]==2) res <- res[1,2]
#   else  dimnames(res) <- list(dimnames(x)[[2]],dimnames(x)[[2]])
#   res
  if (!missing(y)) x <- cbind(x,y)
  cor(x, y, use = "complete")
}
normalize <- function(x) {
  x/rep(sqrt(drop(apply(x,2, function(x) sum(x^2)))),rep(nrow(x),ncol(x)))
}
num.ident <- function(x,y) {
  x==y | is.nan(x) & is.nan(y) | is.na(x) & !is.nan(x) & is.na(y) & !is.nan(y)
}
persp2 <- function(d,axis,r) {
  # perspective along axis axi from point (axi=r,0,0)
  xy <- setdiff(1:3,axis)
  rl <- r/(r-d[,axis])
  res <- d
  res[,xy[1]] <- res[,xy[1]]*rl
  res[,xy[2]] <- res[,xy[2]]*rl
  res
}
pointfit <- function(xi,x) {  # row vectors
  xiq <- apply(xi,2,mean)
  xq  <- apply(x ,2,mean)
  A   <- sweep(x ,2,xq )
  B   <- sweep(xi,2,xiq)
  sv  <- La.svd(t(A) %*% B)
  Q   <- t(sv$vt) %*% t(sv$u)
#  f   <- mean((B/(A %*% t(Q))))
  lf  <- log(B/(A %*% t(Q)))
  lf  <- lf[is.finite(lf)]
  f   <- exp(mean(lf))
  Qf   <- Q*f
  list(Q = Qf, tr = as.vector(xiq - xq %*% Qf), factor = f, res = B - A %*% t(Qf))
}

"rec.prot" <- function(x, eps = .Machine$double.eps)
{
        index <- abs(x/max(abs(x))) > eps
        result <- x
        result[index] <- 1/result[index]
        result[!index] <- 0
        result
}

rotangle <- function(Q) {
  n <- nrow(Q)
  theta <- NULL
  for (i in 1:(n-1)) {
    for (j in (i+1):n) {
      if (Q[j,i] != 0) {
        thk <- atan2(-Q[j,i],Q[i,i])
        theta <- c(thk,theta)
        c <- cos(thk);  s <- sin(thk)
        R <- diag(n)
        R[i,i] <- R[j,j] <- c
        R[i,j] <- -s; R[j,i] <- s
        Q <- R %*% Q
      }
      else
        theta <- c(0,theta)
    }
  }
  if (min(diag(Q)) < 0) stop("Reflection(s) occurred!")
  theta
}
rotm <- function(n,x,y,phi) {
  res <- diag(n)
  ss  <- sin(phi)
  cc  <- cos(phi)
  res[x,x] <- res[y,y] <- cc
  res[x,y] <- ss
  res[y,x] <- -ss
  res
}
seqm <- function(from, to, by=1) {
  if ((to-from)*by < 0) return(NULL)
  else return(seq(from, to, by))
}
signp <- function(x) ifelse(x>=0,1,-1)
whole.number <- function(x) all((x %% 1) == 0)
