#
# function to evaluate the likelihood for univariate and
# bivariate QTL linkage analysis
#
ln <- function(x) {
   if (x==0) {
     x
   }else{
     log(x)
   }
}

qtl.lik <- function(q2,h2,c2,e2,n=1) {
   S0 <- qtl.cov(q2,h2,c2,e2, 0.0)  
   S1 <- qtl.cov(q2,h2,c2,e2, 0.5)  
   S2 <- qtl.cov(q2,h2,c2,e2, 1.0)
   InvS <- solve(S1)
   logdet <- ln(det(S1))
   lam0 <- logdet + sum(diag(S0 %*% InvS)) - ln(det(S0)) - 2 
   lam2 <- logdet + sum(diag(S2 %*% InvS)) - ln(det(S2)) - 2 
   lam <- 0.25 * n * (lam0 + lam2)
   return(S0=S0, S1=S1, S2=S2, lam=lam, contrib=c(lam0,lam2), n=n)
}

qtl.cov <- function (q2,h2,c2,e2, ibd=0.5, rel=0.5)  {
   matrix(c( q2+h2+c2+e2, ibd*q2+rel*h2+c2,
             ibd*q2+rel*h2+c2, q2+h2+c2+e2),
          nr=2, nc=2)
}

corners <- function (q2,h2,c2,e2,p=0.10,n=1) {
   require(mvtnorm)
   S0 <- qtl.cov(q2,h2,c2,e2, 0.0)  
   S1 <- qtl.cov(q2,h2,c2,e2, 0.5)  
   S2 <- qtl.cov(q2,h2,c2,e2, 1.0)

   z<-qnorm(p,lower=F)
   p10 <- 0.25*  pmvnorm(lower=c(z,z), upper=c(Inf,Inf), 
                         mean=c(0,0),
                         sigma=S0)[1]
   p11 <- 0.50*  pmvnorm(lower=c(z,z), upper=c(Inf,Inf), 
                         mean=c(0,0),
                         sigma=S1)[1]
   p12 <- 0.25*  pmvnorm(lower=c(z,z), upper=c(Inf,Inf), 
                         mean=c(0,0),
                         sigma=S2)[1]
   p00 <- 0.25*  pmvnorm(lower=c(-Inf,z), upper=c(-z,Inf), 
                         mean=c(0,0),
                         sigma=S0)[1]
   p01 <- 0.50*  pmvnorm(lower=c(-Inf,z), upper=c(-z,Inf), 
                         mean=c(0,0),
                         sigma=S1)[1]
   p02 <- 0.25*  pmvnorm(lower=c(-Inf,z), upper=c(-z,Inf), 
                         mean=c(0,0),
                         sigma=S2)[1]
   corner11<-(p12+0.5*p11)/(p10+p11+p12)
   corner10<-(p02+0.5*p01)/(p00+p01+p02)

   n0<-n*(p10+p11+p12)
   n1<-2*n*(p00+p01+p02)
   n2<-n*(p10+p11+p12)
   con<-round(n0+n2)
   dis<-round(n1)

   edac<-(n2*corner11 + n0*corner11)/(n2+n0) - corner10
   se<-0.5*sqrt((n0+n1+n2)/(2*n1*(n2+n0)))
   stat<-(edac/se)^2
   c(con,dis,corner11, corner10, edac, se, stat)
}

biv.qtl.lik <- function(qx,hx,cx,ex, qy,hy,cy,ey, rg, rc, re, n=1) {
   S0 <- biv.qtl.cov(qx,hx,cx,ex, qy,hy,cy,ey, rg, rc, re, 0.0)  
   S1 <- biv.qtl.cov(qx,hx,cx,ex, qy,hy,cy,ey, rg, rc, re, 0.5)  
   S2 <- biv.qtl.cov(qx,hx,cx,ex, qy,hy,cy,ey, rg, rc, re, 1.0)
   InvS <- solve(S1)
   logdet <- ln(det(S1))
   lam0 <- logdet + sum(diag(S0 %*% InvS)) - ln(det(S0)) - 4 
   lam2 <- logdet + sum(diag(S2 %*% InvS)) - ln(det(S2)) - 4 
   lam <- 0.25 * n * (lam0 + lam2)
   return(S0=S0, S1=S1, S2=S2, lam=lam, contrib=c(lam0,lam2))
}

biv.qtl.cov <- function(qx,hx,cx,ex, qy,hy,cy,ey, rg, rc, re, ibd) {
   matrix(c(qx^2+hx^2+cx^2+ex^2, qx*qy+rg*hx*hy+rc*cx*cy+re*ex*ey, 
                 hx^2/2 + cx^2 + ibd*qx^2,       rg*hx*hy/2 + rc*cx*cy + ibd*qx*qy,
            qx*qy+rg*hx*hy+rc*cx*cy+re*ex*ey, qy^2+hy^2+cy^2+ey^2,          
                 rg*hx*hy/2+rc*cx*cy + ibd*qx*qy,    hy^2/2+cy^2+ibd*qy^2,
            hx^2/2 + cx^2 + ibd*qx^2,      rg*hx*hy/2+rc*cx*cy+ibd*qx*qy,    
                 qx^2+hx^2+cx^2+ex^2,          qx*qy+rg*hx*hy+rc*cx*cy+re*ex*ey,
            rg*hx*hy/2+rc*cx*cy+ibd*qx*qy,   hy^2/2+cy^2+ibd*qy^2,         
                 qx*qy+rg* hx*hy+rc*cx*cy+re*ex*ey, qy^2+hy^2+cy^2+ey^2), 
            nr=4, nc=4)                                                
}

grid.lik.ge <- function(qx=1/sqrt(3),hx=1/sqrt(3),cx=0,ex=1/sqrt(3),
                        qy=1/sqrt(3),hy=1/sqrt(3),cy=0,ey=1/sqrt(3), 
                        rg=c(0), rc=c(0), re=c(0), lik.fun=biv.qtl.lik) {
  n.rg<-length(rg) 
  n.re<-length(re) 
  lik.data <- matrix(0,nr=n.rg, nc=n.re)
  for(i in 1:n.rg) for(j in 1:n.re) {
    lik.data[i,j]<-lik.fun(qx,hx,cx,ex,qy,hy,cy,ey,rg[i],rc[1],re[j])$lam
  }
  return(rg=rg, re=re, lam=lik.data)
}
grid.lik.ce <- function(qx=1/sqrt(3),hx=0,cx=1/sqrt(3),ex=1/sqrt(3),
                        qy=1/sqrt(3),hy=0,cy=1/sqrt(3),ey=1/sqrt(3), 
                        rg=c(0), rc=c(0), re=c(0), lik.fun=biv.qtl.lik) {
  n.rc<-length(rc) 
  n.re<-length(re) 
  lik.data <- matrix(0,nr=n.rc, nc=n.re)
  for(i in 1:n.rc) for(j in 1:n.re) {
    lik.data[i,j]<-lik.fun(qx,hx,cx,ex,qy,hy,cy,ey,rg[1],rc[i],re[j])$lam
  }
  return(rc=rc, re=re, lam=lik.data)
}
plot.grid <- function () {
  x<-grid.lik.ge(rg=seq(-0.95,0.95,0.05),re=seq(-0.95,0.95,0.05))
  persp(x$rg,x$re,x$lam,
        theta=135,xlab="genetic r",ylab="env r",zlab="lambda",
        ticktype="detailed",
        main=paste("Noncentrality parameter, bivariate QTL VC model",
                   "VA=1/3, VQ=1/3, VE=1/3",sep="\n"))
}
plot.grid2 <- function () {
  x<-grid.lik.ce(rc=seq(-0.95,0.95,0.05),re=seq(-0.95,0.95,0.05))
  persp(x$rc,x$re,x$lam,
        theta=135,xlab="residual r",ylab="env r",zlab=expression(lambda),
        ticktype="detailed",
        main=paste("Noncentrality parameter, bivariate QTL VC model",
                   "VC=1/3, VQ=1/3, VE=1/3",sep="\n"))
}