LV_weight_partial<-function(V,Xl,dtemp,h,q,Y,grad=T){
  N<-length(Y)
  if(is.vector(V)){k<-1}
  else{k<-length(V[1,])}
  Xlv<-Xl%*%V
  d<-dtemp-((Xlv^2)%*%rep(1,k))
  w<-exp(-0.5*(d/h)^2)#dnorm(d/h)/dnorm(0)
  w<-matrix(w,N,q)
  wn<-apply(w,2,sum)#new
  w<-apply(w,2,column_normalize)
  mY<-t(w)%*%Y
  sig<-t(w)%*%(Y^2)-(mY)^2
  W<-diag(kronecker(t(wn),rep(1,N)))##new
  if(grad==T){
    grad<-mat.or.vec(dim,k)
    tmp1<-(kronecker(sig,rep(1,N))-(as.vector(kronecker(rep(1,q),Y))-kronecker(mY,rep(1,N)))^2)
    if(k==1){
      grad_d<- -2*Xl*as.vector(Xlv)
      grad<-(1/h^2)*(1/sum(wn))*t(grad_d*as.vector(d)*as.vector(w)*as.vector(W))%*%tmp1 #new
      # wn_grad<-(-1/h^2)*t(grad_d*as.vector(d)*as.vector(w))%*%kronecker(diag(rep(1,q)),rep(1,N))
      # grad<- wn_grad%*%(sig-rep(var1[2],q))/(sum(wn))+grad
    }
    else{
      for (j in 1:(k)){
        grad_d<- -2*Xl*as.vector(Xlv[,j])
        grad[,j]<- (1/h^2)*(1/sum(wn))*t(grad_d*as.vector(d)*as.vector(w)*as.vector(W))%*%tmp1#new
        # wn_grad<-(-1/h^2)*t(grad_d*as.vector(d)*as.vector(w))%*%kronecker(diag(rep(1,q)),rep(1,N))
        # grad[,j]<- wn_grad%*%(sig-rep(var1[2],q))/(sum(wn))+grad
      }
    }
    ret<-list(t(wn)%*%sig/sum(wn),sig,grad)#new
    names(ret)<-c('var','sig','grad')
  }
  else{
    ret<-list(t(wn)%*%sig/sum(wn),sig)#new
    names(ret)<-c('var','sig')
  }
  
  return(ret)
}
######
LV_weight_full<-function(V,Xl,dtemp,h,q,Y,grad=T){
  N<-length(Y)
  if(is.vector(V)){k<-1}
  else{k<-length(V[1,])}
  Xlv<-Xl%*%V
  d<-dtemp-((Xlv^2)%*%rep(1,k))
  w<-exp(-0.5*(d/h)^2)#dnorm(d/h)/dnorm(0)
  w<-matrix(w,N,q)
  wn<-apply(w,2,sum)#new
  w<-apply(w,2,column_normalize)
  mY<-t(w)%*%Y
  sig<-t(w)%*%(Y^2)-(mY)^2
  W<-(kronecker(t(wn),rep(1,N)))#new or ###diag(kronecker(t(wn),rep(1,N)))
  var<-t(wn)%*%sig/sum(wn)#new
  if(grad==T){
    grad<-mat.or.vec(dim,k)
    tmp1<-(kronecker(sig,rep(1,N))-(as.vector(kronecker(rep(1,q),Y))-kronecker(mY,rep(1,N)))^2)
    if(k==1){
      grad_d<- -2*Xl*as.vector(Xlv)
      grad<-(1/h^2)*(1/sum(wn))*t(grad_d*as.vector(d)*as.vector(w)*as.vector(W))%*%tmp1#new
      wn_grad<-(-1/h^2)*t(grad_d*as.vector(d)*as.vector(w))%*%kronecker(diag(rep(1,q)),rep(1,N))#new
      grad<- wn_grad%*%(sig-rep(var,q))/(sum(wn))+grad#new
    }
    else{
      for (j in 1:(k)){
        grad_d<- -2*Xl*as.vector(Xlv[,j])
        grad[,j]<- (1/h^2)*(1/sum(wn))*t(grad_d*as.vector(d)*as.vector(w)*as.vector(W))%*%tmp1#new
        wn_grad<-(-1/h^2)*t(grad_d*as.vector(d)*as.vector(w))%*%kronecker(diag(rep(1,q)),rep(1,N))#new
        grad[,j]<- wn_grad%*%(sig-rep(var,q))/(sum(wn))+grad[,j]#new
      }
    }
    ret<-list(var,sig,grad)
    names(ret)<-c('var','sig','grad')
  }
  else{
    ret<-list(var,sig)
    names(ret)<-c('var','sig')
  }
  
  return(ret)
}
####
stiefl_weight_partial_opt<-function(dat,h=NULL,k,k0=30,p=1,maxit=50,nObs=sqrt(length(dat[,1])),lambda_0=1,tol=10^(-3),sclack_para=0){
  Y<-dat[,1]
  X<-dat[,-1]
  N<-length(Y)
  dim<-length(X[1,])
  if(p<1){
    S<-est_varmat(X)
    tmp1<-q_ind(X,S,p)
    q<-tmp1$q
    ind<-tmp1$ind
  }
  else{
    q<-N
    ind<-1:N
  }
  Xl<-(kronecker(rep(1,q),X)-kronecker(X[ind,],rep(1,N)))
  dtemp<-apply(Xl,1,norm2)
  if(is.null(h)){
    S<-est_varmat(X)
    tr<-var_tr(S)
    h<-choose_h_2(dim,k,N,nObs,tr)
  }
  best<-exp(10000)
  Vend<-mat.or.vec(dim,k)
  sig<-mat.or.vec(q,1)
  for(u in 1:k0){
    Vnew<-Vold<-stiefl_startval(dim,k)
    #print(Vold)
    #print(LV(Vold,Xl,dtemp,h,q,Y)$var)
    Lnew<-Lold<-exp(10000)
    lambda<-lambda_0
    err<-10
    count<-0
    count2<-0
    while(err>tol&count<maxit){
      #print(Vold)
      tmp2<-LV_weight_partial(Vold,Xl,dtemp,h,q,Y)
      G<-tmp2$grad
      Lold<-tmp2$var
      W<-G%*%t(Vold)-Vold%*%t(G)
      stepsize<-lambda#/(2*sqrt(count+1))
      Vnew<-solve(diag(1,dim)+stepsize*W)%*%(diag(1,dim)-stepsize*W)%*%Vold
      # print(Vnew)
      tmp3<-LV_weight_partial(Vnew,Xl,dtemp,h,q,Y,grad=F)
      Lnew<-tmp3$var
      err<-sqrt(sum((Vold%*%t(Vold)-Vnew%*%t(Vnew))^2))/sqrt(2*k)#sqrt(sum(tmp3$grad^2))/(dim*k)#
      #print(err)
      if(((Lnew-Lold)/Lold) > sclack_para){#/(count+1)^(0.5)
        lambda=lambda/2
        err<-10
        count2<-count2+1
        count<-count-1
        Vnew<-Vold #!!!!!
        
      }
      Vold<-Vnew
      count<-count+1
      #print(count)
    }
    if(best>Lnew){
      best<-Lnew
      Vend<-Vnew
      sig<-tmp3$sig
    }
  }
  ret<-list(Vend,best,sig,count,h,count2)
  names(ret)<-c('est_base','var','aov_dat','count','h','count2') 
  return(ret)
}
###########
stiefl_weight_full_opt<-function(dat,h=NULL,k,k0=30,p=1,maxit=50,nObs=sqrt(length(dat[,1])),lambda_0=1,tol=10^(-3),sclack_para=0){
  Y<-dat[,1]
  X<-dat[,-1]
  N<-length(Y)
  dim<-length(X[1,])
  if(p<1){
    S<-est_varmat(X)
    tmp1<-q_ind(X,S,p)
    q<-tmp1$q
    ind<-tmp1$ind
  }
  else{
    q<-N
    ind<-1:N
  }
  Xl<-(kronecker(rep(1,q),X)-kronecker(X[ind,],rep(1,N)))
  dtemp<-apply(Xl,1,norm2)
  if(is.null(h)){
    S<-est_varmat(X)
    tr<-var_tr(S)
    h<-choose_h_2(dim,k,N,nObs,tr)
  }
  best<-exp(10000)
  Vend<-mat.or.vec(dim,k)
  sig<-mat.or.vec(q,1)
  for(u in 1:k0){
    Vnew<-Vold<-stiefl_startval(dim,k)
    #print(Vold)
    #print(LV(Vold,Xl,dtemp,h,q,Y)$var)
    Lnew<-Lold<-exp(10000)
    lambda<-lambda_0
    err<-10
    count<-0
    count2<-0
    while(err>tol&count<maxit){
      #print(Vold)
      tmp2<-LV_weight_full(Vold,Xl,dtemp,h,q,Y)
      G<-tmp2$grad
      Lold<-tmp2$var
      W<-G%*%t(Vold)-Vold%*%t(G)
      stepsize<-lambda#/(2*sqrt(count+1))
      Vnew<-solve(diag(1,dim)+stepsize*W)%*%(diag(1,dim)-stepsize*W)%*%Vold
      # print(Vnew)
      tmp3<-LV_weight_full(Vnew,Xl,dtemp,h,q,Y,grad=F)
      Lnew<-tmp3$var
      err<-sqrt(sum((Vold%*%t(Vold)-Vnew%*%t(Vnew))^2))/sqrt(2*k)#sqrt(sum(tmp3$grad^2))/(dim*k)#
      #print(err)
      if(((Lnew-Lold)/Lold) > sclack_para){#/(count+1)^(0.5)
        lambda=lambda/2
        err<-10
        count2<-count2+1
        count<-count-1
        Vnew<-Vold #!!!!!
        
      }
      Vold<-Vnew
      count<-count+1
      #print(count)
    }
    if(best>Lnew){
      best<-Lnew
      Vend<-Vnew
      sig<-tmp3$sig
    }
  }
  ret<-list(Vend,best,sig,count,h,count2)
  names(ret)<-c('est_base','var','aov_dat','count','h','count2') 
  return(ret)
}