# Solve MME - for multiple trait model
# Iterate until level of convergence reached
ccc = 100
ntr2 = ntrait*ntrait
oscg = scg*0        # Previous round solutions
osanm = sanm*0      # Previous round solutions
osage = sage*0      # Previous round solutions

while(ccc > 0.00001){

# 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)
sage[i,] = dgi %*% xx
}

# 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){
dg = matrix(data=dgg[i,],byrow=TRUE,nrow=ntrait,ncol=ntrait)
xx = arhs[i,]
dgi = ginv(dg)
scg[i,] = dgi %*% xx
}

# 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]

  if(ka != kprev){
# Obtain solutions for one animal, kprev 
dg = matrix(data=dgg[kprev,],byrow=TRUE,nrow=ntrait,ncol=ntrait)
dg = dg + GGi*adiag[kprev]
xx = arhs[kprev,]
dgi = ginv(dg)
sanm[kprev,] = dgi %*% xx   }

   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
dg = matrix(data=dgg[kprev,],byrow=TRUE,nrow=ntrait,ncol=ntrait)
dg = dg + GGi*adiag[kprev]
xx = arhs[kprev,]
dgi = ginv(dg)
sanm[kprev, ] = dgi %*% xx

# Compute convergence criteria
a1 = sum(diag(sage %*% t(sage)))
a2 = sum(diag(scg %*% t(scg)))
a3 = sum(diag(sanm %*% t(sanm)))
ccd = a1 + a2 + a3
diff = sage - osage
d1 = sum(diag(diff %*% t(diff)))
diff = scg - oscg
d2 = sum(diag(diff %*% t(diff)))
diff = sanm - osanm
d3 = sum(diag(diff %*% t(diff)))
ccn = d1 + d2 + d3
ccc = ccn/ccd
osage = sage
oscg = scg
osanm = sanm
 
}