# Gibbs sampling loop - for maternal effects model
iter = 0
rvar=700
pvar=66
resid = 700
AA = c(0)
AM = AA
MM = AA
PEV = AA
RV = AA
VRhd2 = c(0)
VRhm2 = c(0)
VRdmc = c(0)


while(iter < 1000){
iter = iter + 1

# New Contemporary group solutions (Fixed Factor in Model)

rhs = y - sanm[anfac] - smat[damfac] - smpe[damfac]
rhs

scg = tapply(rhs,cgfac,mean)
scg
dd = as.numeric(tapply(rhs,cgfac,length))
n = length(scg)
v = rnorm(n,0,1)*sqrt(resid/dd)
scg = scg + v

# New animal direct genetic solutions (random factor in model
#  Must account for relationships among animals
rhs = y - scg[cgfac] - smat[damfac] - smpe[damfac]
rhs
arhs = rep(0,nanim+1)
k2=b2$aaid
arhs[k2]=rhs
arhs

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]
  if(ka != kprev){
   x = zz[kprev] + gi[1,1]*adiag[kprev]
   anew = arhs[kprev]/x
     v = rnorm(1)*sqrt(resid/x)
   sanm[kprev] = anew + v   }

   kprev = ka

   if(cods[i]==0){    dd = bii[ka]*gi[1,1]
         dm = bii[ka]*gi[1,2]
         arhs[ka]=arhs[ka] + 0.5*dd*(sanm[ks]+sanm[kd])
         arhs[ka]=arhs[ka] + dm*((smat[ks]+smat[kd])*0.5-smat[ka])
        } else
        { dd = bii[ks]*gi[1,1]
          dm = bii[ks]*gi[1,2]
         arhs[ka] = arhs[ka] + dd*(0.5*sanm[ks]-0.25*sanm[kd]) 
         arhs[ka] = arhs[ka] + dm*(0.5*smat[ks]-0.25*(smat[ka]+smat[kd])) }
 }

#  Take care of last animal
   x = zz[kprev] + gi[1,1]*adiag[kprev]
   anew = arhs[kprev]/x
   sanm[kprev] = anew + (rnorm(1)*sqrt(resid/x))
sanm

# Maternal genetic solutions
# Must account for relationships among animals
rhs = y - scg[cgfac] - sanm[anfac] - smpe[damfac]
rhs
arhs = rep(0,nanim+1)
k2=b2$ddid
for(i in 1:length(k2)){
   kd = k2[i]
   arhs[kd] = arhs[kd] + rhs[i] }

arhs

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]

  if(ka != kprev){
   x = mm[kprev] + gi[2,2]*adiag[kprev]
   anew = arhs[kprev]/x
   smat[kprev] = anew + (rnorm(1)*sqrt(resid/x))   }

   kprev = ka

   if(cods[i]==0){
         dd = bii[ka]*gi[2,2]
         dm = bii[ka]*gi[1,2]
         arhs[ka]=arhs[ka] + 0.5*dd*(smat[ks]+smat[kd])
         arhs[ka]=arhs[ka] + dm*((sanm[ks]+sanm[kd])*0.5-sanm[ka])
        } else
        { dd = bii[ks]*gi[2,2]
          dm = bii[ks]*gi[1,2]
         arhs[ka] = arhs[ka] + dd*(0.5*smat[ks]-0.25*smat[kd]) 
         arhs[ka] = arhs[ka] + dm*(0.5*sanm[ks]-0.25*(sanm[ka]+sanm[kd])) }
 }
#  Take care of last animal
   x = mm[kprev] + gi[2,2]*adiag[kprev]
   anew = arhs[kprev]/x
   smat[kprev] = anew + (rnorm(1)*sqrt(resid/x))
smat

# Estimate direct and maternal covariance matrix
u = cbind(sanm,smat)

MS = u[aaid,] - 0.5*(u[ssid,] + u[ddid,])
MS

SS = (t(MS) %*% diag(bii[aaid]) %*% MS)
SS = SS + nanim*g
ndf = nanim + nanim
UV = riwish(ndf,SS)
UV 
AA = c(AA,UV[1,1])
AM = c(AM,UV[1,2])
MM = c(MM,UV[2,2])

#  New Maternal PE solutions
# Must account for relationships among animals
rhs = y - scg[cgfac] - sanm[anfac] - smat[damfac]
rhs
arhs = rep(0,nanim)
k2=b2$ddid
for(i in 1:length(k2)){
   kd = k2[i]
   arhs[kd] = arhs[kd] + rhs[i] }

dd = mm + ratpe
dd
v = rnorm(nanim,0,1)*sqrt(resid/dd)
smpe = (arhs/dd) 
smpe[damfac] = smpe[damfac]+v[damfac]
smpe

#  Estimate variance
S = as.numeric(t(smpe) %*% smpe) + nanim*pvar
n = length(levels(as.factor(damfac)))
ndf = n + nanim
xd = rchisq(1,ndf)
mpevar = S/xd
PEV = c(PEV, mpevar)

# Estimate the residual variance
rhs = y - scg[cgfac]-sanm[anfac]-smat[damfac]-smpe[damfac]
S = as.numeric(t(rhs) %*% rhs) + nanim*rvar
ndf = length(rhs) + nanim
xd = rchisq(1,ndf)
resid = S/xd
RV = c(RV, resid)

# Form new ratios, etc
gi = resid * ginv(UV)
resid
gi

mpevar
ratpe = resid/mpevar
vy = UV[1,1]+UV[1,2]+UV[2,2]+mpevar+resid
hd2 = UV[1,1]/vy
VRhd2 = c(VRhd2, hd2)
hm2 = UV[2,2]/vy
VRhm2 = c(VRhm2, hm2)
cor = UV[1,2]/sqrt(UV[1,1]*UV[2,2])
VRdmc = c(VRdmc, cor)  }