########### Main Function #################
##SVD of (W+lambda)^{-1}B
solvebeta<-function(lambda1,lambda2,SSw,SSb)
 { temp<-eigen(SSw,symmetric=T)
   Uw<-temp$vectors
   diagD<-temp$values+lambda2
   value<-1/sqrt(diagD)
   root<-Uw%*%diag(value)%*%t(Uw)
   SSb<-SSb+lambda1*diag(1,nrow=nrow(SSb),ncol=ncol(SSb))
   m<-root%*%SSb%*%root
   temp1<-eigen(m,symmetric=T)
   beta<-root%*%temp1$vectors
   d<-temp1$values
   return(list(beta=beta, rootw=root,d=d))
 }

###########  GCV  ######################
n<-14 #num of fams
sel<-split(sample(1:n), rep(1:2,7))$'1'

nid<-id(pheno[,1]) # families id from 1 to 14
mfix<-rep(0, 14) # family size
for(i in 1:14)
 mfix[i]<-sum(nid==i)

a<-0

### split families ####
sel2<-function(sel)
 {temp<-vector("numeric",14)
  t<-vector("numeric", (14-length(sel)))
  temp[sel]<-1
  j<-1
  for (i in 1:14)
    if (temp[i]==0) {t[j]<-i; j<-j+1}
  return(t)
}    


cv_diffb<-function(lambda1,lambda2,ssb_comp,ssw_comp,ssb_pred,ssw_pred) 
 {  beta<-solvebeta(lambda1,lambda2,ssw_comp,ssb_comp)$beta
    value<-beta[,1]%*%ssb_pred%*%beta[,1]/(beta[,1]%*%ssw_pred%*%beta[,1])
    return(list(lambda1=lambda1,lambda2=lambda2,value=value))
}


CV<-function(N,lambda1,lambda2,pheno,column.select,npredfam)
 { avg<-0; avg2<-0
   #ssall<-pheno.pcnew(c(1:14))
   #ssw_all<-ssall$SSw
   #ssb_all<-ssall$SSb
   #cov_beta<-cov(beta)
   avg<-array(dim=c(N,length(lambda1),length(lambda2))); #avg2<-avg  
   for (i in 1:N)
       {   
        sel<-sample(1:14, size=npredfam)
        comp<-sel2(sel)
        ss_pred<-pheno.pcnew(sel)
        ssb_pred<-ss_pred$SSb
        ssw_pred<-ss_pred$SSw
        ss_comp<-pheno.pcnew(comp)
        ssb_comp<-ss_comp$SSb
        ssw_comp<-ss_comp$SSw 
        for (j in 1:length(lambda1))
         for (k in 1:length(lambda2))  
        {
        avg[i,j,k]<-cv_diffb(lambda1[j],lambda2[k],ssb_comp,ssw_comp,ssb_pred,ssw_pred)$value
        }          
      print(i) 
       } 
    return(list(diffb=avg, sameb=avg2))
}

****speed up SS:
nfam<-14
colm<-gt150[1:150,1]
fammean<-matrix(0,nfam,length(colm)); 
famssw<-matrix(0,14*length(colm),length(colm))
k<-150

for(i in 1: nfam)  
  {
   if (i==1) 
    {fammean[i,]<-apply(pheno[1:mfix[i],colm],2,mean) 
     j<-mfix[i]}
   else 
     {fammean[i,]<-apply(pheno[(j+1):(j+mfix[i]),colm],2,mean)
      j<-j+mfix[i]}
  }

for(i in 1:nfam)
  {if (i==1)
    {famssw[((i-1)*k+1):(i*k),]<-pheno.pc(pheno[1:mfix[i],],gt150[1:150,1])$SSw
      j<-mfix[i]}
   else
    {famssw[((i-1)*k+1):(i*k),]<-pheno.pc(pheno[(j+1):(j+mfix[i]),],gt150[1:150,1])$SSw
     j<-j+mfix[i]}
   }

#when need to change column.select re-calculate fammean and famssw

pheno.pcnew<-function(id1)
  {   k<-length(colm)
      tmean<-t(as.matrix(mfix[id1]))%*%fammean[id1,]/sum(mfix[id1])   #y..
      SSb<-0; SSw<-0
      for (i in 1:length(id1))
        {
         SSb<-SSb+as.matrix(fammean[i,]-t(tmean))%*%t(as.matrix(fammean[i,]-t(tmean)))*mfix[i]   
         SSw<-SSw+famssw[((id1[i]-1)*k+1):(id1[i]*k),]
         }
      return(list(SSw=SSw,SSb=SSb)) 
  }


id<-function(id)
{n<-length(id)
 nid<-rep(0, n)
 mark<-id[1]
 index<-1
 for (i in 1:n)
   { if (id[i]==mark) nid[i]<-index 
     else {index<-index+1; mark<-id[i+1]; nid[i]<-index}
    }
return(nid)
}