hpdUniv <- function(para.smp, parai0, alpha=0.05, hpd=TRUE, 
                    quantile.prob=NULL){
  N <- length(para.smp)
  id.para <- order(para.smp)
  para.smp.sort <- para.smp[id.para]
  if(hpd){
    seg <- round(N*(1-alpha))
    N.ws.possible <- round(N*alpha/2)
    if(N.ws.possible < 1){
      N.ws.possible <- 1
    }
    hpd.possible <- matrix(0, N.ws.possible, 4)
    colnames(hpd.possible) <- c("hpdL", "hpdU", "hpdLength", "cover")
    set1 <- 1:N.ws.possible
    set2 <- set1 + seg
    # print(cbind(set1, set2))
    set2[set2 > N] <- N
    hpd.possible[, 1:2] <- cbind(para.smp.sort[set1], para.smp.sort[set2])
    hpd.possible[, 3] <- hpd.possible[,2] - hpd.possible[,1]
    hpd.possible[, 4] <- as.numeric((parai0 >= hpd.possible[, 1]) & ((parai0 <= hpd.possible[, 2])))
    cv1.set <- as.matrix(hpd.possible)
    cv1.min <- which.min(cv1.set[,3])
    hpd.final <- cv1.set[cv1.min, ] 
    CI <- hpd.final
  } else{
    id1 <- round(N*(alpha/2))
    if(id1==0L){
      id1 <- 1
    }
    seg <- round(N*(1-alpha))
    id2 <- seg + id1
    if(id2 > N){
      id2 <- N
    }
    IL <- para.smp.sort[id2] - para.smp.sort[id1]
    cv <- as.numeric((parai0 >= para.smp.sort[id1]) & ((parai0 <= para.smp.sort[id2])))
    CI <- matrix(c(para.smp.sort[id1], para.smp.sort[id2], IL, cv), 1, 4)
    if(!is.null(quantile.prob)){
      stopifnot(length(quantile.prob)==2L)
      yci <- quantile(para.smp.sort, probs=quantile.prob)
      IL <- yci[2] - yci[1]
      cv <- as.numeric((parai0 >=  yci[1]) & ((parai0 <= yci[2])))
      CI <- matrix(c(yci[1], yci[2], IL, cv), 1, 4)
    }
  }
  CI
}


CIscoref <- function(testMat, para0, alpha=0.05){
  d <- length(para0)
  N <- dim(testMat)[1] #here N is the simulated data sets number
  # Id0.list <- apply(testMat[, (1:d)*4], 2, function(x) which(x==0, arr.ind = TRUE))
  Iscore <- NULL
  for(i in 1:d){
    para0i <- para0[i]
    Idcur <- (1:4) + (i-1)*4
    w1 <- (2/alpha)*(testMat[, Idcur[1]] - para0i)*as.numeric(testMat[, Idcur[1]] > para0i)
    w2 <- (2/alpha)*(para0i - testMat[, Idcur[2]])*as.numeric(testMat[, Idcur[2]] < para0i)
    temp <- sum(testMat[, Idcur[3]] + w1 + w2 )/N
    Iscore <- c(Iscore, temp)
  }
  Iscore
}

