#  This is the iteration on data code
#  ccc is the convergence criterion, can be made
#  more strict by reducing the number in the while statement
resid=700
gi = ginv(g)*resid
ratpe = resid/pvar
ccc = 100
while(ccc > 0.00001){

# New Contemporary group solutions (Fixed Factor in Model)
rhs = y - sanm[anfac] - smat[damfac] - smpe[damfac]
scg = tapply(rhs,cgfac,mean)

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

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
   sanm[kprev] = anew
   }
   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

# Maternal genetic solutions
# Must account for relationships among animals
rhs = y - scg[cgfac] - sanm[anfac] - smpe[damfac]
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
   }
   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

#  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] }
arhs
dd = mm + ratpe
smpe = arhs/dd
smpe

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