# Gibbs sampling loop - for multiple trait model
ntr2 = ntrait*ntrait
niter=1000
VG = matrix(data=c(0),nrow=niter,ncol=ntr2)
VC = VG
VR = VG

for(iter in 1:niter){
iter = iter + 1
# AGE EFFECTS SOLUTIONS
rhs = yy - scg[cgfac,] - sanm[anfac, ]
arhs = sage*0
dgg = matrix(data=c(0),nrow=length(nage),ncol=ntr2)
for(i in 1:nrec){
   y1 = yy[i,]
   ri = rinv(y1, RR2)
    r = rhs[i,]
  r[is.na(r)]=0
 adt = ri %*% r
 mlv = agefac[i]
 arhs[mlv, ] = arhs[mlv, ] + t(adt)
 dgg[mlv, ] = dgg[mlv, ] + ri  }

arhs
dgg
nlage=length(nage)
for(i in 1:nlage){
dg1 = matrix(data=dgg[i,],byrow=TRUE,nrow=ntrait,ncol=ntrait)
xx = arhs[i,]
dgi = ginv(dg1)
soln = dgi %*% xx
# Must add GS to soln
dgih = chol(dgi)
zz = rnorm(ntrait,0,1)
gsadd = t(dgih) %*% zz
sage[i,]=soln + gsadd
}

# CONTEMPORARY GROUP SOLUTIONS AND COVARIANCE MATRIX
rhs = yy - sage[agefac,] - sanm[anfac, ]
arhs = scg*0
dgg = matrix(data=c(0),nrow=length(ncg),ncol=ntr2)
for(i in 1:nrec){
   y1 = yy[i,]
   ri = rinv(y1, RR2)
    r = rhs[i,]
  r[is.na(r)]=0
 adt = ri %*% r
 mlv = cgfac[i]
 arhs[mlv, ] = arhs[mlv, ] + t(adt)
 dgg[mlv, ] = dgg[mlv, ] + ri }
#  add CGi inverse matrix to ddd matrices
nlcg = length(ncg)
for(i in 1:nlcg){
	dgg[i, ] = dgg[i, ] + CGi
   }

arhs
dgg
for(i in 1:nlcg){
dg2 = matrix(data=dgg[i,],byrow=TRUE,nrow=ntrait,ncol=ntrait)
xx = arhs[i,]
dgi = ginv(dg2)
soln = dgi %*% xx
# Must add GS to soln
dgih = chol(dgi)
zz = rnorm(ntrait,0,1)
gsadd = t(dgih) %*% zz
scg[i,]=soln + gsadd
}

# Compute sum of squares of solutions, estimate covariance matrix
SS = (t(scg) %*% scg) + nanim*CG2
SS 
ndf = nlcg + nanim
CGnew = riwish(ndf,SS)
CGi = ginv(CGnew)
VC[iter,]=CGnew

# ANIMAL GENETIC SOLUTIONS (random factor in model
#  Must account for relationships among animals
rhs = yy - scg[cgfac,] - sage[agefac,] 
rhs
k2=b2$aaid

arhs = sanm*0
dgg = matrix(data=c(0),nrow=nanim,ncol=ntr2)
for(i in 1:nrec){
   y1 = yy[i,]
   ri = rinv(y1, RR2)
    r = rhs[i,]
  r[is.na(r)]=0
 adt = ri %*% r
 mlv = k2[i]
 arhs[mlv, ] = arhs[mlv, ] + t(adt)
 dgg[mlv, ] = dgg[mlv, ] + ri
}

ij = kord[1]
kprev = aaa[ij]
for(j in 1:nped) {     # Use coded pedigree list
   i = kord[j]
   ka = aaa[i]
   ks = sss[i]
   kd = ddd[i]
    ka; ks; kd; kprev; cods[i];
  if(ka != kprev){
# Obtain solutions for one animal, kprev 
dg1 = matrix(data=dgg[kprev,],byrow=TRUE,nrow=ntrait,ncol=ntrait)
dg1 = dg1 + GGi*adiag[kprev]
xx = arhs[kprev,]
dgi = ginv(dg1)
soln = dgi %*% xx
# Must add GS to soln
dgih = chol(dgi)
zz = rnorm(ntrait,0,1)
gsadd = t(dgih) %*% zz
sanm[kprev,]=soln + gsadd    }

   kprev = ka

   if(cods[i]==0){   gag = bii[ka]*GGi
     arhs[ka, ]=arhs[ka, ] + 0.5*(gag %*% (sanm[ks,]+sanm[kd,]))
        } else
        { gag = bii[ks]*GGi
     arhs[ka, ] = arhs[ka, ] + gag %*% (0.5*sanm[ks,]-0.25*sanm[kd,]) }

 }
#  Take care of last animal
dg2 = matrix(data=dgg[kprev,],byrow=TRUE,nrow=ntrait,ncol=ntrait)
dg2 = dg2 + GGi*adiag[kprev]
xx = arhs[kprev,]
dgi = ginv(dg2)
soln = dgi %*% xx
# Must add GS to soln
dgih = chol(dgi)
zz = rnorm(ntrait,0,1)
gsadd = t(dgih) %*% zz
sanm[kprev,]=soln + gsadd
sanm
# Estimate additive genetic covariance matrix
MS = sanm[aaid,] - 0.5*(sanm[ssid,] + sanm[ddid,])
MS

SS = (t(MS) %*% diag(bii[aaid]) %*% MS)
SS = SS + nanim*GG2
ndf = nanim + nanim
UV = riwish(ndf,SS)
UV 
GGnew = UV
GGi = ginv(GGnew)
VG[iter, ]=GGnew

# Estimate the residual variance
rhs = yy - scg[cgfac,  ]-sanm[anfac, ]-sage[agefac, ]

# Must estimate the missing residuals
# Must add GS to predicted missing ones
arhs = rhs*0
for(i in 1:nrec){
   y1 = yy[i,]
   ri = rinv(y1, RR2)
    r = rhs[i,]
  r[is.na(r)]=0
 adt = (RR2 %*% ri) %*% r  # new predicted residuals
 vri = rvar(ri, RR2)
 dgh = LRShalf(vri)
 zz = rnorm(ntrait,0,1)
 gsadd = dgh %*% zz
 arhs[i, ] = adt + gsadd
}

SS = (t(arhs) %*% arhs) + nanim*RR2
ndf = nrec+nanim
RR2 = riwish(ndf,SS)
VR[iter, ] = RR2

# Samples save in VC, VG, and VR
}