## multiplicative model for Genotype, environment and missing parents ###
set.seed(100)
library(MASS)
start=Sys.time()
k=1
i1=1
#### define variable  ####
Case.T=rep(0,k)
Case2.T=rep(0,k)
bgstat.h=rep(0,k)
bgstat.b=rep(0,k)
bgestat.h=rep(0,k)
bgestat.b=rep(0,k)
beta.bar2=matrix(0,k,2)
varb.bar2=matrix(0,k,4)
bbgeg2=rep(0,k)

bgstat.b2=rep(0,k)
bgestat.b2=rep(0,k)
Trio.tg=rep(0,k)
Trio.ge=rep(0,k)
rbar.ge=rep(0,k)
rbar.g=rep(0,k)
varb.ge=rep(0,k)
varb.g=rep(0,k)
CC.GGE=rep(0,k)
hbgeg=rep(0,k)
bbgeg=rep(0,k)
Tdf.2=rep(0,k)
trio.b=matrix(0,k,2)
trio.v=matrix(0,k,4)
beta.hat=matrix(0,k,2)
beta.bar=matrix(0,k,2)
varb.hat=matrix(0,k,4)
varb.bar=matrix(0,k,4)
r.rhat=matrix(0,k,2)
r.rbar=matrix(0,k,2)

cff1=matrix(0,k,2)
cff2=matrix(0,k,2)
cff3=matrix(0,k,2)

varr.hat=matrix(0,k,2)
varr.bar=matrix(0,k,2)

n=300
n3=600
q=0.7

full.D=matrix(0,n,7)
full2.D=matrix(0,n3,7)
full3.c=matrix(0,(n3+n),5)
b.g=0
b.ge=0
b.e=0.3

################### generate genotype and environment data for one-stage and two-stage #######
m1=1

while (m1<=n){
### stratification ####
u1=runif(1,0,1)
if(u1>q){p.Af=0.3;p.Am=0.3;b0=rnorm(1,log(0.01),1);E=rbinom(1,1,0.2)}
if(u1<=q){p.Af=0.4;p.Am=0.4;b0=rnorm(1,log(0.05),1);E=rbinom(1,1,0.4)}

p.af=1-p.Af
{
gf=runif(1,0,1);
gf[gf>0 & gf<=p.Af^2]=2;
gf[p.Af^2<gf&gf<=(p.Af^2+2*p.af*p.Af)]=1;
gf[gf>(p.Af^2+2*p.af*p.Af)& gf<1]=0
}
p.am=1-p.Am
{
gm=runif(1,0,1);
gm[gm>0 & gm<=p.Am^2]=2;
gm[p.Am^2<gm & gm<=(p.Am^2+2*p.am*p.Am)]=1;
gm[gm>(p.Am^2+2*p.am*p.Am)& gm<1]=0
}
if (gf==0 & gm==0) g0=0
if ((gf==0 & gm==1)|(gf==1 & gm==0)) g0=rbinom(1,1,0.5)
if ((gf==0 & gm==2)|(gf==2 & gm==0)) g0=1
if (gf==1 & gm==1) {g0=runif(1,0,1);
g0[0.25>=g0 & g0>0]=0 ;
g0[g0>0.25& g0<=0.75]=1; 
g0[g0>0.75& g0<1]=2}
if (gf==2 & gm==2) g0=2
if ((gf==1 & gm==2)|(gf==2 & gm==1)) {g0=runif(1,0,1);
g0[0.5>=g0 & g0>0]=1;
g0[g0>0.5& g0<1]=2 }

p3<-exp(b0+2*b.g+b.e*E+2*b.ge*E)
if (g0==0 & p3<1){D=rbinom(1,1,exp(b0+b.e*E))}
if (g0==1 & p3<1){D=rbinom(1,1,exp(b0+b.g+b.e*E+b.ge*E))}
if (g0==2 & p3<1){D=rbinom(1,1,exp(b0+2*b.g+b.e*E+2*b.ge*E))}

if (D==1 & p3<1){
if(u1>0.5){
if (gf==0){Rf=rbinom(1,1,0.1)}
if (gf==1){Rf=rbinom(1,1,0.1)}
if (gf==2){Rf=rbinom(1,1,0.3)}

if (gm==0){Rm=rbinom(1,1,0.1)}
if (gm==1){Rm=rbinom(1,1,0.1)}
if (gm==2){Rm=rbinom(1,1,0.3)}
}
if(u1<=0.5){
if (gf==0){Rf=rbinom(1,1,0.2)}
if (gf==1){Rf=rbinom(1,1,0.2)}
if (gf==2){Rf=rbinom(1,1,0.4)}

if (gm==0){Rm=rbinom(1,1,0.2)}
if (gm==1){Rm=rbinom(1,1,0.2)}
if (gm==2){Rm=rbinom(1,1,0.4)}

}

full.D[m1,]=cbind(D,g0,gf,gm,E,Rf,Rm);m1=m1+1 

}
 }

m2=1

while (m2<=n3){
### stratification ####
u1=runif(1,0,1)
if(u1>q){p.Af=0.3;p.Am=0.3;b0=rnorm(1,log(0.01),1);E=rbinom(1,1,0.2)}
if(u1<=q){p.Af=0.4;p.Am=0.4;b0=rnorm(1,log(0.05),1);E=rbinom(1,1,0.4)}

p.af=1-p.Af
{
gf=runif(1,0,1);
gf[gf>0 & gf<=p.Af^2]=2;
gf[p.Af^2<gf&gf<=(p.Af^2+2*p.af*p.Af)]=1;
gf[gf>(p.Af^2+2*p.af*p.Af)& gf<1]=0
}
p.am=1-p.Am
{
gm=runif(1,0,1);
gm[gm>0 & gm<=p.Am^2]=2;
gm[p.Am^2<gm & gm<=(p.Am^2+2*p.am*p.Am)]=1;
gm[gm>(p.Am^2+2*p.am*p.Am)& gm<1]=0
}
if (gf==0 & gm==0) g0=0
if ((gf==0 & gm==1)|(gf==1 & gm==0)) g0=rbinom(1,1,0.5)
if ((gf==0 & gm==2)|(gf==2 & gm==0)) g0=1
if (gf==1 & gm==1) {g0=runif(1,0,1);
g0[0.25>=g0 & g0>0]=0 ;
g0[g0>0.25& g0<=0.75]=1; 
g0[g0>0.75& g0<1]=2}
if (gf==2 & gm==2) g0=2
if ((gf==1 & gm==2)|(gf==2 & gm==1)) {g0=runif(1,0,1);
g0[0.5>=g0 & g0>0]=1;
g0[g0>0.5& g0<1]=2 }

p3<-exp(b0+2*b.g+b.e*E+2*b.ge*E)
if (g0==0 & p3<1){D=rbinom(1,1,exp(b0+b.e*E))}
if (g0==1 & p3<1){D=rbinom(1,1,exp(b0+b.g+b.e*E+b.ge*E))}
if (g0==2 & p3<1){D=rbinom(1,1,exp(b0+2*b.g+b.e*E+2*b.ge*E))}

if (D==1 & p3<1){
if(u1>0.5){
if (gf==0){Rf=rbinom(1,1,0.1)}
if (gf==1){Rf=rbinom(1,1,0.1)}
if (gf==2){Rf=rbinom(1,1,0.3)}

if (gm==0){Rm=rbinom(1,1,0.1)}
if (gm==1){Rm=rbinom(1,1,0.1)}
if (gm==2){Rm=rbinom(1,1,0.3)}
}
if(u1<=0.5){
if (gf==0){Rf=rbinom(1,1,0.2)}
if (gf==1){Rf=rbinom(1,1,0.2)}
if (gf==2){Rf=rbinom(1,1,0.4)}

if (gm==0){Rm=rbinom(1,1,0.2)}
if (gm==1){Rm=rbinom(1,1,0.2)}
if (gm==2){Rm=rbinom(1,1,0.4)}


}

full2.D[m2,]=cbind(D,g0,gf,gm,E,Rf,Rm);m2=m2+1 

}
 }

m3=1

while (m3<=(n3+n)){
### stratification ####
u1=runif(1,0,1)
if(u1>(1-q)){p.Af=0.3;p.Am=0.3;b0=rnorm(1,log(0.01),1);E=rbinom(1,1,0.2)}
if(u1<=(1-q)){p.Af=0.4;p.Am=0.4;b0=rnorm(1,log(0.05),1);E=rbinom(1,1,0.4)}

p.af=1-p.Af
{
gf=runif(1,0,1);
gf[gf>0 & gf<=p.Af^2]=2;
gf[p.Af^2<gf&gf<=(p.Af^2+2*p.af*p.Af)]=1;
gf[gf>(p.Af^2+2*p.af*p.Af)& gf<1]=0
}
p.am=1-p.Am
{
gm=runif(1,0,1);
gm[gm>0 & gm<=p.Am^2]=2;
gm[p.Am^2<gm & gm<=(p.Am^2+2*p.am*p.Am)]=1;
gm[gm>(p.Am^2+2*p.am*p.Am)& gm<1]=0
}
if (gf==0 & gm==0) g0=0
if ((gf==0 & gm==1)|(gf==1 & gm==0)) g0=rbinom(1,1,0.5)
if ((gf==0 & gm==2)|(gf==2 & gm==0)) g0=1
if (gf==1 & gm==1) {g0=runif(1,0,1);
g0[0.25>=g0 & g0>0]=0 ;
g0[g0>0.25& g0<=0.75]=1; 
g0[g0>0.75& g0<1]=2}
if (gf==2 & gm==2) g0=2
if ((gf==1 & gm==2)|(gf==2 & gm==1)) {g0=runif(1,0,1);
g0[0.5>=g0 & g0>0]=1;
g0[g0>0.5& g0<1]=2 }

p3<-exp(b0+2*b.g+b.e*E+2*b.ge*E)
if (g0==0 & p3<1){D=rbinom(1,1,exp(b0+b.e*E))}
if (g0==1 & p3<1){D=rbinom(1,1,exp(b0+b.g+b.e*E+b.ge*E))}
if (g0==2 & p3<1){D=rbinom(1,1,exp(b0+2*b.g+b.e*E+2*b.ge*E))}

if (D==0 & p3<1){
full3.c[m3,]=cbind(D,g0,gf,gm,E);m3=m3+1 
}
 }

sub.D=full.D[((full.D[,6]+full.D[,7])==0),]
sub.ms=full.D[((full.D[,6]+full.D[,7])==1),]

E.trio=sub.D[,5]
E.case=sub.ms[,5]
n1=length(E.trio)
n2=length(sub.ms[,5])
n.p=n-n1-n2
SS.g=rep(0,n1)
SS.ge=rep(0,n1)
SS.g2=rep(0,n2)
SS.ge2=rep(0,n2)
################### (CPG) family data likelihood and score ########
E1=E.trio
E2=sub.ms[,5]
LL1= function(p) {
L<-rep(0,n1)
L2<-rep(0,n2)
 for(i in 1:n1){
 lh<-0
 RR1=exp(p[1]+p[2]*E1[i])
 RR2=exp(2*p[1]+2*p[2]*E1[i])

 if((sub.D[i,3]==1 & sub.D[i,4]==2 &sub.D[i,2]==2 )|(sub.D[i,3]==2 & sub.D[i,4]==1 &sub.D[i,2]==2)) {lh=log(RR2/(RR1+RR2))}
 if((sub.D[i,3]==1 & sub.D[i,4]==2 &sub.D[i,2]==1 )|(sub.D[i,3]==2 & sub.D[i,4]==1 &sub.D[i,2]==1)){lh=log(RR1/(RR1+RR2))}
 if(sub.D[i,3]==1 & sub.D[i,4]==1 &sub.D[i,2]==2 ){lh=log(RR2/(1+2*RR1+RR2))}
 if(sub.D[i,3]==1 & sub.D[i,4]==1 &sub.D[i,2]==1 ) {lh=log(2*RR1/(1+2*RR1+RR2))}
 if(sub.D[i,3]==1 & sub.D[i,4]==1 &sub.D[i,2]==0 ) {lh=log(1/(1+2*RR1+RR2))}
 if((sub.D[i,3]==1 & sub.D[i,4]==0 &sub.D[i,2]==1 )|(sub.D[i,3]==0 & sub.D[i,4]==1 &sub.D[i,2]==1)) {lh=log(RR1/(1+RR1))}
 if((sub.D[i,3]==1 & sub.D[i,4]==0 &sub.D[i,2]==0 )|(sub.D[i,3]==0 & sub.D[i,4]==1 &sub.D[i,2]==0)){lh=log(1/(1+RR1))}
 L[i]<-lh
 }
 for(i in 1:n2 ){
 lh2<-0
 if (E2[i]==0){
 pp1=exp(p[3])
 pp2=exp(p[1]+p[2]*E2[i])
 pp3=exp(2*p[1]+2*p[2]*E2[i])*(1-exp(p[3]))
 }
 if (E2[i]==1){
 pp1=exp(p[4])
 pp2=exp(p[1]+p[2]*E2[i])
 pp3=exp(2*p[1]+2*p[2]*E2[i])*(1-exp(p[4]))
 }
 if((sub.ms[i,7]==0 & sub.ms[i,4]==1 & sub.ms[i,2]==0)|(sub.ms[i,6]==0 & sub.ms[i,3]==1 & sub.ms[i,2]==0)){
 lh2=log(pp1/(pp1+pp2+pp3))}
 if((sub.ms[i,7]==0 & sub.ms[i,4]==1 & sub.ms[i,2]==1)|(sub.ms[i,6]==0 & sub.ms[i,3]==1 & sub.ms[i,2]==1)){
 lh2=log(pp2/(pp1+pp2+pp3))}
 if((sub.ms[i,7]==0 & sub.ms[i,4]==1 & sub.ms[i,2]==2)|(sub.ms[i,6]==0 & sub.ms[i,3]==1 & sub.ms[i,2]==2)){
 lh2=log(pp3/(pp1+pp2+pp3))}
 L2[i]<-lh2
 }
 
-(sum(L)+sum(L2))
}
LL2= function(p) {
L<-rep(0,n1)

 for(i in 1:n1){
 lh<-0
 RR1=exp(p[1]+p[2]*E1[i])
 RR2=exp(2*p[1]+2*p[2]*E1[i])

 if((sub.D[i,3]==1 & sub.D[i,4]==2 &sub.D[i,2]==2 )|(sub.D[i,3]==2 & sub.D[i,4]==1 &sub.D[i,2]==2)) {lh=log(RR2/(RR1+RR2))}
 if((sub.D[i,3]==1 & sub.D[i,4]==2 &sub.D[i,2]==1 )|(sub.D[i,3]==2 & sub.D[i,4]==1 &sub.D[i,2]==1)){lh=log(RR1/(RR1+RR2))}
 if(sub.D[i,3]==1 & sub.D[i,4]==1 &sub.D[i,2]==2 ){lh=log(RR2/(1+2*RR1+RR2))}
 if(sub.D[i,3]==1 & sub.D[i,4]==1 &sub.D[i,2]==1 ) {lh=log(2*RR1/(1+2*RR1+RR2))}
 if(sub.D[i,3]==1 & sub.D[i,4]==1 &sub.D[i,2]==0 ) {lh=log(1/(1+2*RR1+RR2))}
 if((sub.D[i,3]==1 & sub.D[i,4]==0 &sub.D[i,2]==1 )|(sub.D[i,3]==0 & sub.D[i,4]==1 &sub.D[i,2]==1)) {lh=log(RR1/(1+RR1))}
 if((sub.D[i,3]==1 & sub.D[i,4]==0 &sub.D[i,2]==0 )|(sub.D[i,3]==0 & sub.D[i,4]==1 &sub.D[i,2]==0)){lh=log(1/(1+RR1))}
 L[i]<-lh
 }

 
-(sum(L))
}

out.nf <- try(nlm(LL1,p=c(0,0,-1,-1), hessian = TRUE,steptol = 1e-4, iterlim = 1000),TRUE)
out2.nf <- nlm(LL2,p=c(0,0), hessian = TRUE,steptol = 1e-4, iterlim = 1000)

if (!is.null(out.nf$estimate)){
b.nf=out.nf$estimate

ck1=as.numeric(is.nan(det(out.nf$hessian)))
ck2=det(out.nf$hessian)
if (ck1==0 & ck2!=0 & ck2!=-Inf & ck2!=Inf  ){

bnf.v=ginv(out.nf$hessian)
b2.nf=out2.nf$estimate
bnf2.v=ginv(out2.nf$hessian)
E=E.trio
for(i in 1:n1){

phi1=exp(b.nf[1]+b.nf[2]*E[i])
phi2=exp(2*b.nf[1]+2*b.nf[2]*E[i])

if (sub.D[i,3]==2 & sub.D[i,4]==2 ){S.g=0;S.ge=0}
if ((sub.D[i,3]==2 & sub.D[i,4]==1 & sub.D[i,2]==2)|(sub.D[i,3]==1 & sub.D[i,4]==2 & sub.D[i,2]==2)) {S.g=1/(1+phi1);S.ge=E[i]*S.g}
if ((sub.D[i,3]==2 & sub.D[i,4]==1 & sub.D[i,2]==1)|(sub.D[i,3]==1 & sub.D[i,4]==2 & sub.D[i,2]==1)) {S.g=-phi1/(1+phi1);S.ge=E[i]*S.g}
if ((sub.D[i,3]==2 & sub.D[i,4]==0 )|(sub.D[i,3]==0 & sub.D[i,4]==2 )) {S.g=0;S.ge=0}
if (sub.D[i,3]==1 & sub.D[i,4]==1 &sub.D[i,2]==2 ){S.g=2/(1+phi1);S.ge=E[i]*S.g }
if (sub.D[i,3]==1 & sub.D[i,4]==1 &sub.D[i,2]==1 ) {S.g=-(phi1-1)/(1+phi1);S.ge=E[i]*S.g }
if (sub.D[i,3]==1 & sub.D[i,4]==1 &sub.D[i,2]==0 ) {S.g=-2*phi1/(1+phi1);S.ge=E[i]*S.g }
if ((sub.D[i,3]==1 & sub.D[i,4]==0 &sub.D[i,2]==1 )|(sub.D[i,3]==0 & sub.D[i,4]==1 &sub.D[i,2]==1)) {S.g=1/(1+phi1);S.ge=E[i]*S.g }
if ((sub.D[i,3]==1 & sub.D[i,4]==0 &sub.D[i,2]==0 )|(sub.D[i,3]==0 & sub.D[i,4]==1 &sub.D[i,2]==0)){S.g=-phi1/(1+phi1);S.ge=E[i]*S.g }
if ((sub.D[i,3]==0 & sub.D[i,4]==0 )) {S.g=0;S.ge=0}
SS.g[i]=S.g
SS.ge[i]=S.ge
}

    SSr1=rep(0,n2)
    SSrr1=rep(0,n2)
    SSr2=rep(0,n2)
    SSrr2=rep(0,n2)
    SSr3=rep(0,n2)
    SSrr3=rep(0,n2)
    SSg1=rep(0,n2)
    SSge1=rep(0,n2)
    SSrb1=rep(0,n2)
    SSrbe1=rep(0,n2)

    SSg2=rep(0,n2)
    SSge2=rep(0,n2)
    SSrb2=rep(0,n2)
    SSrbe2=rep(0,n2)

    SSg3=rep(0,n2)
    SSge3=rep(0,n2)
    SSrb3=rep(0,n2)
    SSrbe3=rep(0,n2)
    
##############################  adjust CPG Score function #################################
  
 for(i in 1:n2 ){
    bg=b.nf[1]
    bge=b.nf[2]
    if(E2[i]==0){ r=b.nf[3]}
    if(E2[i]==1){ r=b.nf[4]}
    Sr1=0
    Srr1=0
    Sr2=0
    Srr2=0
    Sr3=0
    Srr3=0
    Sg1=0
    Sge1=0
    Srb1=0
    Srbe1=0

    Sg2=0
    Sge2=0
    Srb2=0
    Srbe2=0

    Sg3=0
    Sge3=0
    Srb3=0
    Srbe3=0

 if((sub.ms[i,7]==0 & sub.ms[i,4]==1 & sub.ms[i,2]==0)|(sub.ms[i,6]==0 & sub.ms[i,3]==1 & sub.ms[i,2]==0)){

    Sr1=exp(bg+bge*E2[i])*(1+exp(bg+bge*E2[i]))/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))  
    Srr1=-exp(bg+bge*E2[i])*(1+exp(bg+bge*E2[i]))*(exp(r)-exp(2*bg+2*bge*E2[i]+r))/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))^2
    Sg1=-exp(bg+bge*E2[i])*(1+2*exp(bg+bge*E2[i])-2*exp(bg+bge*E2[i]+r))/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))
    Sge1=-E2[i]*exp(bg+bge*E2[i])*(1+2*exp(bg+bge*E2[i])-2*exp(bg+bge*E2[i]+r))/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))
    Srb1=(exp(3*bg+3*bge*E2[i]+r)+exp(bg+bge*E2[i]+r)+2*exp(2*bg+2*bge*E2[i]+r))/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))^2
    Srbe1=E2[i]*(exp(3*bg+3*bge*E2[i]+r)+exp(bg+bge*E2[i]+r)+2*exp(2*bg+2*bge*E2[i]+r))/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))^2
    }

 if((sub.ms[i,7]==0 & sub.ms[i,4]==1 & sub.ms[i,2]==1)|(sub.ms[i,6]==0 & sub.ms[i,3]==1 & sub.ms[i,2]==1)){

    Sr2=-(exp(r)-exp(2*bg+2*bge*E2[i]+r))/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))
    Srr2=-exp(bg+bge*E2[i])*(1+exp(bg+bge*E2[i]))*(exp(r)-exp(2*bg+2*bge*E2[i]+r))/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))^2
    Sg2=(exp(r)-exp(2*bg+2*bge*E2[i])+exp(2*bg+2*bge*E2[i]+r))/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))
    Sge2=E2[i]*(exp(r)-exp(2*bg+2*bge*E2[i])+exp(2*bg+2*bge*E2[i]+r))/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))
    Srb2=exp(bg+bge*E2[i])*(exp(r)+2*exp(bg+bge*E2[i]+r)+exp(2*bg+2*bge*E2[i]+r))/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))^2
    Srbe2=E2[i]*exp(bg+bge*E2[i])*(exp(r)+2*exp(bg+bge*E2[i]+r)+exp(2*bg+2*bge*E2[i]+r))/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))^2
    }

 if((sub.ms[i,7]==0 & sub.ms[i,4]==1 & sub.ms[i,2]==2)|(sub.ms[i,6]==0 & sub.ms[i,3]==1 & sub.ms[i,2]==2)){

    Sr3=(exp(3*bg+3*bge*E2[i]+r)+exp(2*bg+2*bge*E2[i]+r))*exp(-2*bg-2*bge*E2[i])/((exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))*(-1+exp(r)))    
    Srr3=-(exp(3*bg+3*bge*E2[i]+r)+exp(2*bg+2*bge*E2[i]+r))*(exp(-bg-bge*E2[i])+1+exp(-2*bg-2*bge*E2[i]+2*r)-exp(2*r))/((exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))^2*(-1+exp(r))^2)
    Sg3=(2*exp(r)+exp(bg+bge*E2[i]))/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))
    Sge3=(2*exp(r)+exp(bg+bge*E2[i]))*E2[i]/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))
    Srb3=exp(bg+bge*E2[i])*(exp(r)+2*exp(bg+bge*E2[i]+r)+exp(2*bg+2*bge*E2[i]+r))/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))^2
    Srbe3=E2[i]*exp(bg+bge*E2[i])*(exp(r)+2*exp(bg+bge*E2[i]+r)+exp(2*bg+2*bge*E2[i]+r))/(exp(r)+exp(bg+bge*E2[i])+exp(2*bg+2*bge*E2[i])-exp(2*bg+2*bge*E2[i]+r))^2
    }
    SSr1[i]=Sr1
    SSrr1[i]=Srr1
    SSr2[i]=Sr2
    SSrr2[i]=Srr2
    SSr3[i]=Sr3
    SSrr3[i]=Srr3

    SSg1[i]=Sg1
    SSge1[i]=Sge1
    SSrb1[i]=Srb1
    SSrbe1[i]=Srbe1

    SSg2[i]=Sg2
    SSge2[i]=Sge2
    SSrb2[i]=Srb2
    SSrbe2[i]=Srbe2

    SSg3[i]=Sg3
    SSge3[i]=Sge3
    SSrb3[i]=Srb3
    SSrbe3[i]=Srbe3
   
 }
   
 SSrr=c(SSrr1,SSrr2,SSrr3)
 SSr=c(SSr1,SSr2,SSr3)
 Srr.iv=solve(sum(SSrr))
 SSSr=sum(SSr)
 for(i in 1:n2 ){
 Sbg=0;Sbge=0
 if((sub.ms[i,7]==0 & sub.ms[i,4]==1 & sub.ms[i,2]==0)|(sub.ms[i,6]==0 & sub.ms[i,3]==1 & sub.ms[i,2]==0)){
 Sbg=SSg1[i]-SSrb1[i]*Srr.iv*SSSr;Sbge=SSge1[i]-SSrbe1[i]*Srr.iv*SSSr}
 if((sub.ms[i,7]==0 & sub.ms[i,4]==1 & sub.ms[i,2]==1)|(sub.ms[i,6]==0 & sub.ms[i,3]==1 & sub.ms[i,2]==1)){
 Sbg=SSg2[i]-SSrb2[i]*Srr.iv*SSSr;Sbge=SSge2[i]-SSrbe2[i]*Srr.iv*SSSr}
 if((sub.ms[i,7]==0 & sub.ms[i,4]==1 & sub.ms[i,2]==2)|(sub.ms[i,6]==0 & sub.ms[i,3]==1 & sub.ms[i,2]==2)){
 Sbg=SSg3[i]-SSrb3[i]*Srr.iv*SSSr;Sbge=SSge3[i]-SSrbe3[i]*Srr.iv*SSSr}
 SS.g2[i]=Sbg
 SS.ge2[i]=Sbge
 }


subGr=full.D[,2]
subGr.c=full3.c[,2][1:n]
subGr.c2=full3.c[,2]
subEr=full.D[,5]
subEr.c=full3.c[,5][1:n]
subEr.c2=full3.c[,5]
sub.stat=c(rep(1,n),rep(0,n))
sub.stat2=c(rep(1,n),rep(0,(n+n3)))
####
G.cc=c(subGr,subGr.c)
E.cc=c(subEr,subEr.c)
GE.cc=G.cc*E.cc

G.cc2=c(subGr,subGr.c2)
E.cc2=c(subEr,subEr.c2)
GE.cc2=G.cc2*E.cc2
####
G.rb=c(full.D[,2],full2.D[,2],full3.c[,2])
E.rb=c(full.D[,5],full2.D[,5],full3.c[,5])
GE.rb=G.rb*E.rb
status=c(rep(1,(n+n3)),rep(0,(n+n3)))
fix.rb=glm(status~G.rb+E.rb+GE.rb,family=binomial)
rr.bar=summary(fix.rb)
r.bar=as.matrix(fix.rb$coefficients)
rco.v=as.matrix(rr.bar$coefficients[,2])^2
var.cc=rr.bar$cov.scaled
rbar.ge[i1]=r.bar[4]
rbar.g[i1]=r.bar[2]
varb.ge[i1]=rco.v[4]
varb.g[i1]=rco.v[2]
CC.GGE[i1]=(r.bar[c(2,4)])%*%solve(var.cc[c(2,4),c(2,4)])%*%r.bar[c(2,4)]

#################### logistic score and hessian matrix##############

fix.r=glm(sub.stat~G.cc+E.cc+GE.cc,family=binomial)
rr.hat=summary(fix.r)
d2.2=solve(rr.hat$cov.scaled)
r.hat=as.matrix(fix.r$coefficients)
pr.hat=exp(r.hat[1]+r.hat[2]*G.cc+r.hat[3]*E.cc+r.hat[4]*GE.cc)/(1+exp(r.hat[1]+r.hat[2]*G.cc+r.hat[3]*E.cc+r.hat[4]*GE.cc))
z=cbind(rep(1,(2*n)),G.cc,E.cc,GE.cc)
d2=t(z)%*%(pr.hat*(1-pr.hat)*z)
c22=t(z)%*%((sub.stat-pr.hat)^2*z)
### 2 beta_bar

fix.r2=glm(sub.stat2~G.cc2+E.cc2+GE.cc2,family=binomial)
rr.hat2=summary(fix.r2)
r.hat2=as.matrix(fix.r2$coefficients)
pr.hat2=exp(r.hat2[1]+r.hat2[2]*G.cc2+r.hat2[3]*E.cc2+r.hat2[4]*GE.cc2)/(1+exp(r.hat2[1]+r.hat2[2]*G.cc2+r.hat2[3]*E.cc2+r.hat2[4]*GE.cc2))
z2=cbind(rep(1,(2*n+n3)),G.cc2,E.cc2,GE.cc2)
d2.c2=t(z2)%*%(pr.hat2*(1-pr.hat2)*z2)
c22.c2=t(z2)%*%((sub.stat2-pr.hat2)^2*z2)

####### adjust logistic regression Score####

SS.hat=((sub.stat2-pr.hat2)*z2)
I.hat=t(z2)%*%(pr.hat2*(1-pr.hat2)*z2)
sg=SS.hat[,2]-I.hat[1,2]*I.hat[1,1]^(-1)*SS.hat[,1]
se=SS.hat[,3]-I.hat[1,3]*I.hat[1,1]^(-1)*SS.hat[,1]
sge=SS.hat[,4]-I.hat[1,4]*I.hat[1,1]^(-1)*SS.hat[,1]
SShat.adj1=cbind(sg,se,sge)
SShat.adj=rbind(SShat.adj1[((full.D[,6]+full.D[,7])==0),],SShat.adj1[((full.D[,6]+full.D[,7])==1),],SShat.adj1[((full.D[,6]+full.D[,7])==2),])
I.22=I.hat[2:4,2:4]-(as.matrix(I.hat[2:4,1])*I.hat[1,1]^(-1))%*%I.hat[1,2:4]

pr.bar=exp(r.bar[1]+r.bar[2]*G.rb+r.bar[3]*E.rb+r.bar[4]*GE.rb)/(1+exp(r.bar[1]+r.bar[2]*G.rb+r.bar[3]*E.rb+r.bar[4]*GE.rb))
zb2=cbind(rep(1,(n+n3)*2),G.rb,E.rb,GE.rb)
SS.bar=((status-pr.bar)*zb2)
I.bar=t(zb2)%*%(pr.bar*(1-pr.bar)*zb2)
sgb=SS.bar[,2]-I.bar[1,2]*I.bar[1,1]^(-1)*SS.bar[,1]
seb=SS.bar[,3]-I.bar[1,3]*I.bar[1,1]^(-1)*SS.bar[,1]
sgeb=SS.bar[,4]-I.bar[1,4]*I.bar[1,1]^(-1)*SS.bar[,1]
SSbar.adj1=cbind(sgb,seb,sgeb)
SSbar.adj=rbind(SSbar.adj1[((full.D[,6]+full.D[,7])==0),],SSbar.adj1[((full.D[,6]+full.D[,7])==1),],SSbar.adj1[((full.D[,6]+full.D[,7])==2),])

I.33=I.bar[2:4,2:4]-(as.matrix(I.bar[2:4,1])*I.bar[1,1]^(-1))%*%I.bar[1,2:4]



########## Two-stage analysis ######
d11.iv=ginv(out.nf$hessian)[1:2,1:2]
c11=t(cbind(c(SS.g,SS.g2,rep(0,n.p)),c(SS.ge,SS.ge2,rep(0,n.p))))%*%cbind(c(SS.g,SS.g2,rep(0,n.p)),c(SS.ge,SS.ge2,rep(0,n.p)))
ssg=cbind(c(SS.g,SS.g2,rep(0,n.p)),c(SS.ge,SS.ge2,rep(0,n.p)))

cov12=d11.iv%*%(t(ssg)%*%(SShat.adj[1:n,]))%*%solve(I.22)
cov13=d11.iv%*%(t(ssg)%*%(SSbar.adj[1:n,]))%*%solve(I.33)
cov11=d11.iv%*%t(ssg)%*%(ssg)%*%d11.iv
cov22=solve(I.22)%*%t(SShat.adj)%*%SShat.adj%*%solve(I.22)
cov33=solve(I.33)%*%t(SSbar.adj)%*%SSbar.adj%*%solve(I.33)
cov23=solve(I.22)%*%t(rbind(SShat.adj[1:n,],matrix(0,n3,3),SShat.adj[(n+1):(2*n+n3),]))%*%SSbar.adj%*%solve(I.33)

bg2=b.nf[1:2]-(cov12-cov13)%*%ginv(cov22+cov33-2*cov23)%*%(r.hat-r.bar)[2:4]
Q=(cov12-cov13)%*%t(ginv(cov22+cov33-2*cov23))
varb2.bar=d11.iv+Q%*%cov22%*%t(Q)+Q%*%cov33%*%t(Q)-2*cov12%*%t(Q)+2*cov13%*%t(Q)-2*Q%*%cov23%*%t(Q)

beta.bar2[i1,]=bg2
varb.bar2[i1,]=varb2.bar

###  Wald statistic #########
Rob.vnf=d11.iv
bbgeg2[i1]=t(bg2)%*%ginv(varb2.bar)%*%(bg2)
bgstat.b2[i1]=(bg2[1])^2/varb2.bar[1,1]
bgestat.b2[i1]=(bg2[2])^2/varb2.bar[2,2]
Case.T[i1]=(r.bar[2])^2/rco.v[2]
Case2.T[i1]=(r.bar[4])^2/rco.v[4]
Trio.tg[i1]=(b2.nf[1])^2/bnf2.v[1,1]
Trio.ge[i1]=(b2.nf[2])^2/bnf2.v[2,2]
bgstat.h[i1]=(b.nf[1])^2/Rob.vnf[1,1]
bgestat.h[i1]=(b.nf[2])^2/Rob.vnf[2,2]

bz1=as.matrix(c(b.nf[1],b.nf[2]))
vbz1=Rob.vnf

hbgeg[i1]=t(bz1)%*%ginv(vbz1)%*%(bz1)
Tdf.2[i1]=t(b2.nf)%*%ginv(bnf2.v)%*%(b2.nf)


}
}

Sys.time()-start
###  Wald Test #########
t.c=Case.T[Case.T>qchisq(0.95,1)]
t2.c=Case2.T[Case2.T>qchisq(0.95,1)]
ttrio.g=Trio.tg[Trio.tg>qchisq(0.95,1)]
ttrio.ge=Trio.ge[Trio.ge>qchisq(0.95,1)]
ct1=bgstat.h[bgstat.h>qchisq(0.95,1)]
ct2=bgstat.b[bgstat.b>qchisq(0.95,1)]
ct2.2=bgstat.b2[bgstat.b2>qchisq(0.95,1)]
ct3=bgestat.h[bgestat.h>qchisq(0.95,1)]
ct4=bgestat.b[bgestat.b>qchisq(0.95,1)]
ct4.2=bgestat.b2[bgestat.b2>qchisq(0.95,1)]
ct5=hbgeg[hbgeg>qchisq(0.95,2)]
ct6=bbgeg[bbgeg>qchisq(0.95,2)]
ct6.2=bbgeg2[bbgeg2>qchisq(0.95,2)]
ct7=Tdf.2[Tdf.2>qchisq(0.95,2)]
cc=CC.GGE[CC.GGE>qchisq(0.95,2)]

tt.c=length(t.c)/k
tt2.c=length(t2.c)/k
tt.g=length(ttrio.g)/k
tt.ge=length(ttrio.ge)/k
hbg.t=length(ct1)/k
bbg.t=length(ct2)/k
bbg.t2=length(ct2.2)/k
hbge.t=length(ct3)/k
bbge.t=length(ct4)/k
bbge.t2=length(ct4.2)/k
ihbgge.t=length(ct5)/k
ibbgge.t=length(ct6)/k
ibbgge.t2=length(ct6.2)/k
tdf2=length(ct7)/k
cc.t=length(cc)/k

### output #############
report1=matrix(c("","beta0","betaG","betaE","betaGE","true",b0,b.g,b.e,b.ge,
"CPG.beta","",mean(trio.b[,1]),"",mean(trio.b[,2]),
"beta.hat","",mean(beta.hat[,1]),"",mean(beta.hat[,2]),
"beta.bar","",mean(beta.bar2[,1]),"",mean(beta.bar2[,2]),
"case-control","",mean(rbar.g),"",mean(rbar.ge),
#"Simu var.CPG.beta","",var(trio.b[,1]),"",var(trio.b[,2]),
#"Simu var.beta.hat","",var(beta.hat[,1]),"",var(beta.hat[,2]),
#"Simu var.beta.bar","",var(beta.bar2[,1]),"",var(beta.bar2[,2]),
#"Simu var.case-control","",var(rbar.g),"",var(rbar.ge),
"Var.CPG.beta","",mean(trio.v[,1]),"",mean(trio.v[,4]),
"Var.beta.hat","",mean(varb.hat[,1]),"",mean(varb.hat[,4]),
"Var.beta.bar","",mean(varb.bar2[,1]),"",mean(varb.bar2[,4]),
"Var.case-control","",mean(varb.g),"",mean(varb.ge),
"Test_CPG","b.g=0",tt.g,"b.ge=0",tt.ge,
"test hat","b.g.hat=0",hbg.t,"b.ge.hat=0",hbge.t,
"test bar","b.g.bar=0",bbg.t2,"b.ge.bar=0",bbge.t2,
"Case-control G","","","Test G=0",tt.c,
"Case-control GE","","","Test GE=0",tt2.c,
"2df_TGE","G.GE.CPG",tdf2,"","",
"2df_TGE","G.GE.hat",ihbgge.t,"","",
"2df_TGE","G.GE.bar",ibbgge.t2,"","",
"2DF","CC.GGE 2df",cc.t,"",""
),5,19)

print(t(report1))