# Iterations to solve MME - for Random Regression model
#
# Model: y = Mu_t + Sum(1:3)a_it z_t + Sum(1:3)p_it z_t + e

iter = 0
nrr2 = nrr*nrr
ccc = 100.0
dayzz = (2*(day - 1)/48) - 1
cov = matrix(data=c(0),nrow=trec,ncol=nrr)
#  set up covariates
for(i in 1:trec){
    x = dayzz[i]
    p = Legdre(x)[1:3]
    cov[i,] = p }
trec
cov

resid = 3
HGVi = ginv(HGV)*resid
HPEi = ginv(HPEV)*resid


# Initialize solution vectors and accumulation arrays
sanm = matrix(data=c(0),nrow=nanimp,ncol=nrr)
sape = sanm
mut = rep(0,49)
omut = mut
osape = sape
osanm = sanm
dday = as.numeric(levels(as.factor(day)))
dayfac = factor(day)

ccc=100.0
while(ccc > 0.00001){
iter = iter + 1
# means by day  Mu_t effects
rhs = obs - (diag(sanm[aid,] %*% t(cov))) - (diag(sape[aid,] %*% t(cov)))

mut = tapply(rhs,dayfac,mean)

# ANIMAL GENETIC SOLUTIONS (random factor in model
#  Must account for relationships among animals
rhs = obs - mut[dayfac] - (diag(sape[aid,] %*% t(cov)))

k2=rrdat2$aid
arhs = sanm*0
qqq = matrix(data=c(0),nrow=nanimp,ncol=nrr2)  # diagonal blocks for animals
for(i in 1:trec){
   y1 = as.numeric(rhs[i])
   ri = cov[i,]
   mlv = k2[i]
  arhs[mlv, ] = arhs[mlv, ] + ri*y1
  qqq[mlv, ] = qqq[mlv, ] + ri %*% t(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=qqq[kprev,],byrow=TRUE,nrow=nrr,ncol=nrr)
dg = dg + HGVi*adiag[kprev]
xx = arhs[kprev,]
dgi = ginv(dg)
sanm[kprev,] = dgi %*% xx
   }
   kprev = ka
   if(cods[i]==0){ gag = bii[ka]*HGVi
     arhs[ka, ]=arhs[ka, ] + 0.5*(gag %*% (sanm[ks,]+sanm[kd,]))
        } else
        { gag = bii[ks]*HGVi
     arhs[ka, ] = arhs[ka, ] + gag %*% (0.5*sanm[ks,]-0.25*sanm[kd,]) }
 }
#  Take care of last animal
dg = matrix(data=qqq[kprev,],byrow=TRUE,nrow=nrr,ncol=nrr)
dg = dg + HGVi*adiag[kprev]
xx = arhs[kprev,]
dgi = ginv(dg)
sanm[kprev,] = dgi %*% xx

#  Animal Permanent Environmental RR effects

rhs = obs - mut[dayfac] - (diag(sanm[aid,] %*% t(cov)))
k2=rrdat2$aid
arhs = sape*0
for(i in 1:trec){
   y1 = as.numeric(rhs[i])
   ri = cov[i,]
   mlv = k2[i]
  arhs[mlv, ] = arhs[mlv, ] + ri*y1
}
for(i in 1:nanim) {     
# Obtain solutions for one animal,  
dg = matrix(data=qqq[i,],byrow=TRUE,nrow=nrr,ncol=nrr)
dg = dg + HPEi
xx = arhs[i,]
dgi = ginv(dg)
sape[i,] = dgi %*% xx
}

# Compute convergence criteria
a1 = sum(diag(sape %*% t(sape)))
a2 = as.numeric(t(mut) %*% mut)
a3 = sum(diag(sanm %*% t(sanm)))
ccd = a1 + a2 + a3
diff = sape - osape
d1 = sum(diag(diff %*% t(diff)))
xd = mut - omut
d2 = as.numeric(t(xd) %*% xd)
diff = sanm - osanm
d3 = sum(diag(diff %*% t(diff)))
ccn = d1 + d2 + d3
ccc = ccn/ccd
osape = sape
omut = mut
osanm = sanm

}