#
#  Coding for solving MME for animal model
#

while(ccc > 0.00001){
# New Contemporary group solutions (Random Factor in Model)
#  Must Add variance ratio, solutions should sum to zero
rhs = y - sage[agefac] - (bhat * cov) - sanm[anfac]
rhs
scg = tapply(rhs,cgfac,sum)
dd = as.numeric(tapply(rhs,cgfac,length))
dd = dd + ratcg
dd
scg
scg = scg/dd
scg = scg - mean(scg)  # Force solutions to sum to zero
scg

# New Age Factor solutions (Fixed factor in Model)
rhs = y - scg[cgfac] - (bhat*cov) - sanm[anfac]
rhs
sage = tapply(rhs,agefac,mean)
sage

# New marker regression coefficient
rhs = y - scg[cgfac] - sage[agefac] - sanm[anfac]
bhat = xx %*% covt %*% as.matrix(rhs)
bhat

# New animal genetic solutions (random factor in model
#  Must account for relationships among animals
rhs = y - scg[cgfac] - sage[agefac] - (bhat*cov)
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] + ratan*adiag[kprev]
   anew = arhs[kprev]/x
   sanm[kprev] = anew   }

   kprev = ka

   if(cods[i]==0){
         d = bii[ka]*ratan
         arhs[ka]=arhs[ka] + 0.5*d*(sanm[ks]+sanm[kd])
        }  else
        { d = bii[ks]*ratan
         arhs[ka] = arhs[ka] + d*(0.5*sanm[ks]-0.25*sanm[kd]) }
 }
#  Take care of last animal
   x = zz[kprev] + ratan*adiag[kprev]
   anew = arhs[kprev]/x
   sanm[kprev] = anew
sanm

# Convergence criterion 
znew = c(scg, sage, bhat, sanm)
diff = znew - zold
ccd = t(diff) %*% diff
ccs = t(znew) %*% znew
ccc = as.numeric(ccd)/as.numeric(ccs)
ccc
zold = znew }

#