# Gibbs sampling loop - for Random Regression model
#
# Model: y = Mu_t + Sum(1:3)a_it z_t + Sum(1:3)p_it z_t + e

nrr2 = nrr*nrr
niter = 10
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
HGVi = ginv(HGV)
HPEi = ginv(HPEV)


# Initialize solution vectors and accumulation arrays
sanm = matrix(data=c(0),nrow=nanimp,ncol=nrr)
sape = sanm
mut = rep(0,49)
dday = as.numeric(levels(as.factor(day)))
dayfac = factor(day)
resid = 3
VG = matrix(data=c(0),nrow=niter,ncol=nrr2)
VP = VG
VR = rep(0,niter)

for(iter in 1:niter){

HGVi = HGVi*resid
HPEi = HPEi*resid

# means by day  Mu_t effects
rhs = obs - (diag(sanm[aid,] %*% t(cov))) - (diag(sape[aid,] %*% t(cov)))

mut = tapply(rhs,dayfac,mean)
mxx = tapply(rhs,dayfac,length)
n = length(mut)
v = rnorm(n,0,1)*sqrt(resid/mxx)
mut = mut + v

# 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)
soln = dgi %*% xx
# Must add GS to soln
dgih = chol(dgi)
zz = rnorm(nrr,0,1)
gsadd = t(dgih) %*% zz
sanm[kprev,]=soln + gsadd
   }
   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)
soln = dgi %*% xx
# Must add GS to soln
dgih = chol(dgi)
zz = rnorm(nrr,0,1)
gsadd = t(dgih) %*% zz
sanm[kprev,]=soln + gsadd

# Estimate Genetic covariance matrix, nrr by nrr

MS = sanm[aaid,] - 0.5*(sanm[ssid,] + sanm[ddid,])
MS
SS = (t(MS) %*% diag(bii[aaid]) %*% MS)
SS = SS + HGV*nanim
ndf = nanim+nanim
SS; ndf;
UV = riwish(ndf,SS)
UV 
HGVi = ginv(UV)
VG[iter,]=UV

#  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)
soln = dgi %*% xx
# Must add GS to soln
dgih = chol(dgi)
zz = rnorm(nrr,0,1)
gsadd = t(dgih) %*% zz
sape[i,]=soln + gsadd
}

# Estimate PE RR covariance matrix
SS = (t(sape) %*% sape) + nanim*HPEV
SS 
ndf = nanim + nanim
UV = riwish(ndf,SS)
UV
HPEi = ginv(UV)
HPEi
VP[iter,]=UV

# Estimate the residual variance
rhs = obs - mut[dayfac] - 
  (diag(sanm[aid,] %*% t(cov)))-(diag(sape[aid,] %*% t(cov)))
xd = as.numeric(rhs)
SS = (t(xd) %*% xd) 
SS
dg = rchisq(1,trec)
resid = as.numeric(SS/dg)
VR[iter]=resid

 
}