######## the EM algorithm and helper functions
# for parameter estimation with splines or trigonometric functions in the state process
# of an HMM with step lengths and turning angles




## computing the forward and backward probabilities (see Zucchini et al. 2016)
alphaBeta <- function(step, angle, tod, trackID, ID, N, stepShape, stepRate, anglemean, anglecon, Gamma, deltas) {
  T <- length(step)
  lalpha <- lbeta <- matrix(NA, N, T)
  stepprobs <- matrix(NA, nrow = T, ncol = N)
  stepprobs[, 1] <- dgamma(step, stepShape[1], rate = stepRate[1])
  stepprobs[, 2] <- dgamma(step, stepShape[2], rate = stepRate[2])
  stepprobs[is.na(step), ] <- 1 # for missing values in step lengths, both states are equally likely

  angleprobs <- matrix(NA, nrow = T, ncol = N)
  angleprobs[, 1] <- dvonmises(circular(angle), circular(anglemean[1]), anglecon[1])
  angleprobs[, 2] <- dvonmises(circular(angle), circular(anglemean[2]), anglecon[2])
  angleprobs[is.na(angle), ] <- 1 # for missing values in angles, both states are equally likely
  
  allprobs <- stepprobs * angleprobs # contemporaneous conditional independence of steps and angles

  start_i <- 0
  end_i <- 0
  for (i in (unique(trackID))) {
    T_i <- sum(trackID == i)
    start_i <- end_i + 1
    end_i <- end_i + T_i
    foo <- deltas[tod[start_i],] * allprobs[start_i, ]
    sumfoo <- sum(foo)
    lscale <- log(sumfoo)
    foo <- foo / sumfoo
    lalpha[, start_i] <- log(foo) + lscale
    for (t in (start_i + 1):end_i) {
      z <- tod[t - 1]
      foo <- foo %*% Gamma[, , z] * allprobs[t, ]
      sumfoo <- sum(foo)
      lscale <- lscale + log(sumfoo)
      foo <- foo / sumfoo
      lalpha[, t] <- log(foo) + lscale
    }
    lbeta[, end_i] <- rep(0, N)
    foo <- rep(1 / N, N)
    lscale <- log(N)
    for (t in (end_i - 1):start_i) {
      z <- tod[t]
      foo <- Gamma[, , z] %*% (allprobs[t + 1, ] * foo)
      lbeta[, t] <- log(foo) + lscale
      sumfoo <- sum(foo)
      foo <- foo / sumfoo
      lscale <- lscale + log(sumfoo)
    }
  }
  list(la = lalpha, lb = lbeta, allprobs = allprobs)
}

## get the weighted logl of the vonmises distr
wlogl_vonmises <- function(theta, weights, x) {
  mean <- Arg(theta[1] + (0 + 1i) * theta[2])
  con <- sqrt(theta[1]^2 + theta[2]^2)
  logl <- sum(weights * log(dvonmises(circular(x), circular(mean), kappa = con)))
  return(-logl)
}
## get the weighted logl of the gamma distr
wlogl_gamma <- function(theta, weights, x) {
  shape <- exp(theta[1]) # lower bound 0
  rate <- exp(theta[2]) # lower bound 0
  probs <- dgamma(x, shape, rate = rate)
  logl <- sum(weights * log(probs))
  return(-logl)
}

## get the switching probabilities v (see Zucchini et al. 2016)
get_switchprobs <- function(t, la, lb, tod, Gamma, lallprobs, llh, j, k) {
  exp(la[j, (t - 1)] + log(Gamma[j, k, tod[t - 1]]) + lallprobs[t, k] + lb[k, t] - llh)
}

# function to create trigonometric functions of the time of day
todtrans <- function(x = 1:24){
  sin1 <- sin(2 * pi * x * 1 / 24) # sine of transformed xvar
  cos1 <- cos(2 * pi * x * 1 / 24) # cosine of transformed xvar
  sin2 <- sin(2 * pi * x * 2 / 24) 
  cos2 <- cos(2 * pi * x * 2 / 24) 
  sin3 <- sin(2 * pi * x * 3 / 24) 
  cos3 <- cos(2 * pi * x * 3 / 24) 
  sin4 <- sin(2 * pi * x * 4 / 24) 
  cos4 <- cos(2 * pi * x * 4 / 24) 
  sin5 <- sin(2 * pi * x * 5 / 24) 
  cos5 <- cos(2 * pi * x * 5 / 24) 
  sin6 <- sin(2 * pi * x * 6 / 24) 
  cos6 <- cos(2 * pi * x * 6 / 24) 
  sin7 <- sin(2 * pi * x * 7 / 24) 
  cos7 <- cos(2 * pi * x * 7 / 24) 
  return(list(var_x = x,
              sin_x1 = sin1,
              cos_x1 = cos1,
              sin_x2 = sin2,
              cos_x2 = cos2,
              sin_x3 = sin3,
              cos_x3 = cos3,
              sin_x4 = sin4,
              cos_x4 = cos4,
              sin_x5 = sin5,
              cos_x5 = cos5,
              sin_x6 = sin6,
              cos_x6 = cos6,
              sin_x7 = sin7,
              cos_x7 = cos7))
}

# EM algorithm for trigonometric functions in state process
EM_trigon <- function(tod, step, angle, trackID, ID, N,
                      stepShape, stepRate,
                      anglemean, anglecon, Gamma,
                      TrigPairs = 1, maxiter = 100, tol = 0.001) {
  T <- length(step)
  
  deltas <- matrix(1 / N, nrow = 24, ncol = 2) # starting value for the initial state probabilities
  
  llh_total_prev <- 0 # this variable will be used to compare likelihood values between two iterations
  
  for (iter in 1:maxiter) {
    ###### E-step: get the expected values of the state sequence ######
    # get the forward and backward probabilities
    fb <- alphaBeta(
      step = step, angle = angle, tod = tod,
      stepShape = stepShape, stepRate = stepRate,
      anglemean = anglemean, anglecon = anglecon,
      trackID = trackID, ID = ID, N = N,
      Gamma = Gamma, deltas = deltas
    )
    la <- fb$la # log-forward probabilities
    lb <- fb$lb # log-backward probabilities
    lallprobs <- log(fb$allprobs)
    # using la and lb, get state probs w and switching probs v for each observed track
    w <- matrix(NA, N, T) # matrix for the state probs
    v <- array(NA, dim = c(N, N, T), dimnames = list(paste("st ", 1:N), paste("st ", 1:N))) # array for switching probs
    start_i <- end_i <- llh_total <- 0
    for (i in (unique(trackID))) { # loop over each track 
      T_i <- sum(trackID == i)
      start_i <- end_i + 1
      end_i <- end_i + T_i
      c <- max(la[, end_i]) # constant for numerical stability
      llh <- c + log(sum(exp(la[, end_i] - c))) # likelihood of this track
      llh_total <- sum(llh_total, llh) # overall likelihood
      
      t_i <- start_i:end_i
      w[, t_i] <- exp(la[, t_i] + lb[, t_i] - llh) # state probabilities (see Zucchini et al.)
      
      t_i1 <- (start_i + 1):end_i
      for (j in 1:N) {
        for (k in 1:N) {
          v[j, k, t_i1] <- sapply(t_i1, get_switchprobs, # switching probabilities
                                  la = la, lb = lb, tod = tod, Gamma = Gamma,
                                  lallprobs = lallprobs, llh = llh, j = j, k = k
          )
        }
      }
    }
    
    # criterion: differences between likelihoods of iterations
    crit <- abs(llh_total - llh_total_prev)
    print(paste("Convergence criterion:", crit, "Log likelihood:", llh_total))
    if (crit < tol) {
      return(list(
        stepShape = stepShape, stepRate = stepRate, anglemean = anglemean,
        anglecon = anglecon, mllk = llh_total, mods = mget(ls(pattern = "gammod")),
        deltas = deltas
      ))
    }
    
    ###### M-step: ######
    
    # update parameters of the t.p.m. 
    mid_t <- which(!is.na(v[1, 1, ]), arr.ind = TRUE) # define time points within tracks
    var_x <- rep(tod[mid_t], N)
    var_x <- var_x - 1
    var_x[var_x == 0] <- 24
    trigon_x <- todtrans(x = var_x)
    sin_x1 <- trigon_x$sin_x1
    cos_x1 <- trigon_x$cos_x1
    sin_x2 <- trigon_x$sin_x2
    cos_x2 <- trigon_x$cos_x2
    sin_x3 <- trigon_x$sin_x3
    cos_x3 <- trigon_x$cos_x3
    sin_x4 <- trigon_x$sin_x4
    cos_x4 <- trigon_x$cos_x4
    sin_x5 <- trigon_x$sin_x5
    cos_x5 <- trigon_x$cos_x5
    sin_x6 <- trigon_x$sin_x6
    cos_x6 <- trigon_x$cos_x6
    sin_x7 <- trigon_x$sin_x7
    cos_x7 <- trigon_x$cos_x7
    
    
    ID_mod <- rep(as.factor(ID[mid_t]), N)
    sw <- rep(c(1, 0), each = length(mid_t)) # switch events (1), and remain events (0) in one target variable
    w1 <- c(v[1, 2, mid_t], v[1, 1, mid_t]) # weights for switching from state 1->2 and for staying (1->1)
    w2 <- c(v[2, 1, mid_t], v[2, 2, mid_t]) # weights for switching from state 2->1 and for staying (2->2)
    
    if (TrigPairs == 0) {
      formulaTrigPairs <- "sw ~ 1"
    }
    if (TrigPairs == 1) {
      formulaTrigPairs <- "sw ~ sin_x1 + cos_x1"
    }
    if (TrigPairs == 2) {
      formulaTrigPairs <- "sw ~ sin_x1 + cos_x1 + sin_x2 + cos_x2"
    }
    if (TrigPairs == 3) {
      formulaTrigPairs <- "sw ~ sin_x1 + cos_x1 + sin_x2 + cos_x2 + sin_x3 + cos_x3"
    }
    if (TrigPairs == 4) {
      formulaTrigPairs <- "sw ~ sin_x1 + cos_x1 + sin_x2 + cos_x2 + sin_x3 + cos_x3 +
                                sin_x4 + cos_x4"
    }
    if (TrigPairs == 5) {
      formulaTrigPairs <- "sw ~ sin_x1 + cos_x1 + sin_x2 + cos_x2 + sin_x3 + cos_x3 +
                                sin_x4 + cos_x4 + sin_x5 + cos_x5"
    }
    if (TrigPairs == 6) {
      formulaTrigPairs <- "sw ~ sin_x1 + cos_x1 + sin_x2 + cos_x2 + sin_x3 + cos_x3 +
                                sin_x4 + cos_x4 + sin_x5 + cos_x5 + sin_x6 + cos_x6"
    }
    if (TrigPairs == 7) {
      formulaTrigPairs <- "sw ~ sin_x1 + cos_x1 + sin_x2 + cos_x2 + sin_x3 + cos_x3 +
                                sin_x4 + cos_x4 + sin_x5 + cos_x5 + sin_x6 + cos_x6 +
                                sin_x7 + cos_x7"
    }
    gammod1 <- gam(as.formula(formulaTrigPairs),
                   weights = w1, family = binomial(link = "logit")
    ) 
    gammod2 <- gam(as.formula(formulaTrigPairs),
                   weights = w2, family = binomial(link = "logit")
    ) 
    
    newdata_trigon24 <- todtrans()
    
    if (TrigPairs > 0) {
      probs12 <- predict.gam(gammod1, newdata_trigon24,
                             type = "response"
      )
      probs21 <- predict.gam(gammod2, newdata_trigon24,
                             type = "response"
      )
    }
    
    if (TrigPairs == 0) {
      probs12 <- fitted(gammod1)[1:24]
      probs21 <- fitted(gammod2)[1:24]
    }
    
    # using the results of the GAMs, define the t.p.m. (for each hour of the day)
    Gamma[1, 1, ] <- (1 - probs12)
    Gamma[1, 2, ] <- probs12
    Gamma[2, 1, ] <- probs21
    Gamma[2, 2, ] <- (1 - probs21) 
    
    # update stationary distribution
    Gamma_star <- Gamma
    if (TrigPairs >0){ # not for K = 0 (no periodic stationarity, just stationarity):
    for (t in 1:24){
      # loopi determines which gamma matrices will be multiplied for which t
      loopi <- c(1:24,1:24)[seq(t+1,24+t-1)]
      for (k in loopi){
        Gamma_star[, , t] <- Gamma_star[, , t]%*%Gamma[,,k]
      }
    }
    }
    # get new "stationary" distributions
    for (t in 1:24) {
      deltas[t, ] <- solve(t(diag(N) - Gamma_star[, , t] + 1), c(1, 1))
    }
    
    # update the parameters of the state-dependent distributions
    
    step_x <- step[!is.na(step)] # for parameter estimation I remove NA values
    angle_x <- angle[!is.na(angle)] # for parameter estimation I remove NA values
    for (j in 1:N) { # separately for each state
      # get parameter estimates for the gamma distribution for the step lengths, dependent on state j
      u_j_step <- w[j, ][!is.na(step)] # weight: for each time point the probability of state j being active
      theta0 <- c(log(stepShape[j]), log(stepRate[j])) # unconstrained parameters (starting values for nlm)
      gamma_mod <- nlm(wlogl_gamma, p = theta0, weights = u_j_step, x = step_x)
      stepShape[j] <- exp(gamma_mod$estimate[1])
      stepRate[j] <- exp(gamma_mod$estimate[2])
      
      
      # get parameter estimates for the vonmises distribution for the turning angles, dependent on state j
      u_j_angle <- w[j, ][!is.na(angle)]
      # for nlm, unconstrained parameters are needed. transformations:
      x <- anglecon[j] * cos(anglemean[j])
      y <- anglecon[j] * sin(anglemean[j])
      theta0 <- c(x, y)
      vonMises_mod <- nlm(wlogl_vonmises, p = theta0, weights = u_j_angle, x = angle_x)
      anglemean[j] <- Arg(vonMises_mod$estimate[1] + (0 + 1i) * vonMises_mod$estimate[2])
      anglecon[j] <- sqrt(vonMises_mod$estimate[1]^2 + vonMises_mod$estimate[2]^2)
    }
    
    llh_total_prev <- llh_total
  }
  print(paste("No convergence after", maxiter, "iterations"))
}



EM <- function(tod, step, angle, trackID, ID, N,
               stepShape, stepRate,
               anglemean, anglecon, Gamma, maxiter = 100, tol = 0.001) {
 
  T <- length(step)

  deltas <- matrix(1 / N, nrow = 24, ncol = 2) # starting value for the initial state probabilities

  llh_total_prev <- 0

  for (iter in 1:maxiter) {

    ###### E-step: get the expected values of the state sequence ######
    # get the forward and backward probabilities
    fb <- alphaBeta(
      step = step, angle = angle, tod = tod,
      stepShape = stepShape, stepRate = stepRate,
      anglemean = anglemean, anglecon = anglecon,
      trackID = trackID, ID = ID, N = N,
      Gamma = Gamma, deltas = deltas
    )
    la <- fb$la # log-forward probabilities
    lb <- fb$lb # log-backward probabilities
    lallprobs <- log(fb$allprobs)
    # using la and lb, get state probs w and switching probs v for each observed track
    w <- matrix(NA, N, T) # matrix for the state probs
    v <- array(NA, dim = c(N, N, T), dimnames = list(paste("st ", 1:N), paste("st ", 1:N))) # array for switching probs
    start_i <- end_i <- llh_total <- 0
    for (i in (unique(trackID))) { # loop over all tracks
      T_i <- sum(trackID == i)
      start_i <- end_i + 1
      end_i <- end_i + T_i
      c <- max(la[, end_i]) # constant for numerical stability
      llh <- c + log(sum(exp(la[, end_i] - c))) # likelihood of this track
      llh_total <- sum(llh_total, llh) # overall likelihood

      t_i <- start_i:end_i
      w[, t_i] <- exp(la[, t_i] + lb[, t_i] - llh) # state probabilities (see Zucchini et al.)

      t_i1 <- (start_i + 1):end_i
      for (j in 1:N) {
        for (k in 1:N) {
          v[j, k, t_i1] <- sapply(t_i1, get_switchprobs, # switching probabilities
            la = la, lb = lb, tod = tod, Gamma = Gamma,
            lallprobs = lallprobs, llh = llh, j = j, k = k
          )
        }
      }
    }

    # criterion: differences between likelihoods of iterations
    crit <- abs(llh_total - llh_total_prev)
    print(paste("Convergence criterion:", crit, "Log likelihood:", llh_total))
    if (crit < tol) {
      return(list(
        stepShape = stepShape, stepRate = stepRate, anglemean = anglemean,
        anglecon = anglecon, mllk = llh_total, mods = mget(ls(pattern = "gammod")),
        deltas = deltas
      ))
    }

    ###### M-step: ######
    
    # update parameters of the t.p.m. 
    mid_t <- which(!is.na(v[1, 1, ]), arr.ind = TRUE) # define time points within tracks
    var_x <- rep(tod[mid_t], N)
    var_x <- var_x - 1
    var_x[var_x == 0] <- 24
    ID_mod <- rep(as.factor(ID[mid_t]), N)

    sw <- rep(c(1, 0), each = length(mid_t)) # switch events (1), and remain events (0) in one target variable
    w1 <- c(v[1, 2, mid_t], v[1, 1, mid_t]) # weights for switching from state 1->2 and for staying (1->1)
    w2 <- c(v[2, 1, mid_t], v[2, 2, mid_t]) # weights for switching from state 2->1 and for staying (2->2)
    gammod1 <- gam(sw ~ s(var_x, bs = "cp"), knots=list(var_x=c(0,24)), # cyclic P-spline
      weights = w1, family = binomial(link = "logit")
    ) # weights
    gammod2 <- gam(sw ~ s(var_x, bs = "cp"), knots=list(var_x=c(0,24)), # cyclic P-spline
      weights = w2, family = binomial(link = "logit")
    ) # weights
    probs12 <- predict.gam(gammod1, list(var_x = 1:24), type = "response")
    probs21 <- predict.gam(gammod2, list(var_x = 1:24), type = "response")
    # using the results of the GAMs, define the t.p.m. (for each hour of the day)

    Gamma[1, 1, ] <- (1 - probs12)
    Gamma[1, 2, ] <- probs12
    Gamma[2, 1, ] <- probs21
    Gamma[2, 2, ] <- (1 - probs21) # Gamma.next

    # update stationary distribution
    Gamma_star <- Gamma
    for (t in 1:24){
      # loopi determines which gamma matrices will be multiplied for which t
      loopi <- c(1:24,1:24)[seq(t+1,24+t-1)]
      for (k in loopi){
        Gamma_star[, , t] <- Gamma_star[, , t]%*%Gamma[,,k]
      }
    }
    # get new "stationary" distributions
    for (t in 1:24) {
      deltas[t, ] <- solve(t(diag(N) - Gamma_star[, , t] + 1), c(1, 1))
    }
    
    # update the parameters of the state-dependent distributions

    step_x <- step[!is.na(step)] # for parameter estimation I remove NA values
    angle_x <- angle[!is.na(angle)] # for parameter estimation I remove NA values
    for (j in 1:N) { # separately for each state
      # get parameter estimates for the gamma distribution for the step lengths, dependent on state j
      u_j_step <- w[j, ][!is.na(step)] # weight: for each time point the probability of state j being active
      theta0 <- c(log(stepShape[j]), log(stepRate[j])) # unconstrained parameters (starting values for nlm)
      gamma_mod <- nlm(wlogl_gamma, p = theta0, weights = u_j_step, x = step_x)
      stepShape[j] <- exp(gamma_mod$estimate[1]) # stepShape.next
      stepRate[j]  <- exp(gamma_mod$estimate[2]) # stepRate.next


      # get parameter estimates for the vonmises distribution for the turning angles, dependent on state j
      u_j_angle <- w[j, ][!is.na(angle)]
      # for nlm, unconstrained parameters are needed. transformations:
      x <- anglecon[j] * cos(anglemean[j])
      y <- anglecon[j] * sin(anglemean[j])
      theta0 <- c(x, y)
      vonMises_mod <- nlm(wlogl_vonmises, p = theta0, weights = u_j_angle, x = angle_x)
      anglemean[j] <- Arg(vonMises_mod$estimate[1] + (0 + 1i) * vonMises_mod$estimate[2]) # anglemean.next
      anglecon[j]  <- sqrt(vonMises_mod$estimate[1]^2 + vonMises_mod$estimate[2]^2) # anglecon.next
    }

    llh_total_prev <- llh_total
  }
  print(paste("No convergence after", maxiter, "iterations"))
}

# Viterbi algorithm for global state decoding (see Zucchini et al.)
Viterbi <- function(tod, step, angle, trackID, ID, N,
                    stepShape, stepRate,
                    anglemean, anglecon, Gamma, deltas) {
  T <- length(step)

  stepprobs <- matrix(NA, nrow = T, ncol = N)
  stepprobs[, 1] <- dgamma(step, stepShape[1], rate = stepRate[1])
  stepprobs[, 2] <- dgamma(step, stepShape[2], rate = stepRate[2])
  stepprobs[is.na(step), ] <- 1 # for missing values in step lengths, both states are equally likely

  angleprobs <- matrix(NA, nrow = T, ncol = N)
  angleprobs[, 1] <- dvonmises(circular(angle), circular(anglemean[1]), anglecon[1])
  angleprobs[, 2] <- dvonmises(circular(angle), circular(anglemean[2]), anglecon[2])
  angleprobs[is.na(angle), ] <- 1 # for missing values in angles, both states are equally likely
  allprobs <- stepprobs * angleprobs # contemporaneous conditional independence of steps and angles

  yi <- matrix(0, T, N)
  iv <- numeric(T)

  start_i <- 0
  end_i <- 0
  for (i in 1:length(unique(trackID))) {
    T_i <- sum(trackID == unique(trackID)[i])
    start_i <- end_i + 1
    end_i <- end_i + T_i
    foo <- deltas[tod[start_i],] * allprobs[start_i, ]
    yi[start_i, ] <- foo / sum(foo)
    for (t in (start_i + 1):end_i) {
      z <- tod[t - 1]
      foo <- apply(yi[t - 1, ] * Gamma[, , z], 2, max) * allprobs[t, ]
      yi[t, ] <- foo / sum(foo)
    }
    iv[end_i] <- which.max(yi[end_i, ])
    for (t in (end_i - 1):start_i) {
      z <- tod[t]
      iv[t] <- which.max(Gamma[, , z][, iv[t + 1]] * yi[t, ])
    }
  }
  return(iv)
}


# ordinary pseudo residuals code with full conditional distributions as explained in Zucchini et al., chap.5 and 6
PseudoRes <- function(data,mod,N){
  stepShape = mod$stepShape
  stepRate = mod$stepRate
  anglemean = mod$anglemean
  anglecon = mod$anglecon
  Gamma = getGamma(mod = mod, cycle = 24)
  deltas = getDelta(Gamma, cycle = 24)
  
  tod = data$tod
  step = data$step
  angle = data$angle 
  trackID = data$ID
  ID = data$animalID 
  
  T <- dim(data)[1]
  
  fb <- alphaBeta(
    step = step, angle = angle, tod = tod,
    stepShape = stepShape, stepRate = stepRate,
    anglemean = anglemean, anglecon = anglecon,
    trackID = trackID, ID = ID, N = N,
    Gamma = Gamma, deltas = deltas
  )
  la <- fb$la # log-forward probabilities
  lb <- fb$lb # log-backward probabilities
  lafact <- apply(la, 2, max) # constant to avoid numerical underflow
  lbfact <- apply(lb, 2, max) # constant to avoid numerical underflow
  
  # percentiles of each observation under N component distributions for both steps and  turns
  Pstep <- Pangle <- matrix(NA,T,N)
  angle[which(angle==pi)] <- NA
  ind.angle <- which(!is.na(angle))
  for (j in 1:N){
    Pstep[, j] <- pgamma(step, shape=stepShape[j], rate=stepRate[j])
    for (i in ind.angle){
      Pangle[i, j] <- integrate(dvonmises, circular(-pi), circular(angle[i]), circular(anglemean[j]), anglecon[j])$value
    }
  }
  
  URes.step <- URes.angle <- rep(NA,T)
  Res.step <- Res.angle <- rep(NA,T)
  URes.step[1] <- deltas[tod[1],]%*%Pstep[1,]
  URes.angle[1] <- deltas[tod[1],]%*%Pangle[1,]
  for (t in 2:T){
    alpha <- exp(la[,t-1]-lafact[t-1]) # forward probs, minus constant to avoid numerical underflow
    beta <- exp(lb[,t]-lbfact[t]) # backward probs, minus constant to avoid numerical underflow
    foo <- alpha %*% Gamma[,,tod[t]] * beta # "d" in Zucchini book, chap.5
    foo <- foo/sum(foo) # "w" in Zucchini book, chap.5
    URes.step[t] <- foo%*%Pstep[t,] # uniformly distributed pseudo residuals
    URes.angle[t] <- foo%*%Pangle[t,] # uniformly distributed pseudo residuals
  }
  Res.step <- qnorm(URes.step) # normally distributed pseudo residuals
  Res.angle <- qnorm(URes.angle) # normally distributed pseudo residuals
  return(list(res.step=Res.step,res.angle=Res.angle))
}