

function [ Gg, Cg, Bg, Lg, Vtg, GgSBB_tot , CgSBB_tot , GXconn, CXconn] = init ( XgH_BaseGrid , XgV_BaseGrid , XgSH , XgSV , SH )

global s0 p0 layer maxLevel numBlocks max_row_idx max_clm_idx q0 
global ugH ucH  ugV ucV  ugHX ucHX  ugVX ucVX  ugx ucx ucV_toGnd

bb_max_row_idx = max_row_idx / sqrt ( numBlocks ) ; 
bb_max_clm_idx = max_clm_idx / sqrt ( numBlocks ) ;
N0 = bb_max_row_idx * bb_max_clm_idx ;
display('ininit')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%     ALL RC TO GND

XgV_nom.x = XgV_BaseGrid.x ;
for i=1:maxLevel  
        XgV_nom.x = ( XgV_nom.x | XgSV(i).x ) ;
end

UXgV_BaseGrid.x = ugH * XgV_nom.x ;
UXcV_BaseGrid.x = ucH * XgV_nom.x ;
display('afterchk')
% UXgV_sens.x = ugVX * XgV_nom.x ;
% UXcV_sens.x = ucVX * XgV_nom.x ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    BASEGRID HORZ

XgH_BaseGrid1.x = set_diagG ( N0 , XgH_BaseGrid.x , 0 ) ;

UXgH_BaseGrid.x = ugH * XgH_BaseGrid1.x ;
UXcH_BaseGrid.x = ucH * XgH_BaseGrid1.x ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%      BASEGRID SENS CONNECTONS 
for j=1:maxLevel
     XgSH1(j).x = set_diagG ( N0 , XgSH(j).x , 0 ) ;
end

for i=1:numBlocks
    for j=1:maxLevel
       UXgSH(i,j).x =  (ugH / ((bb_max_row_idx-1)/(2^(j-1))))  * XgSH1(j).x ;
       UXcSH(i,j).x =  (ucH / ((bb_max_row_idx-1)/(2^(j-1))))  * XgSH1(j).x ;
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INITIALIZED SENSITIVITIES

for i=1:numBlocks
    for j=1:maxLevel
       idx = (i-1)*(maxLevel)+j ;
       UXgSH_sens(i,j).x =  SH ( idx , 1 ) * (ugHX / ((bb_max_row_idx-1)/(2^(j-1))))  * XgSH1(j).x ;
       UXcSH_sens(i,j).x =  SH ( idx , 1 ) * (ucHX / ((bb_max_row_idx-1)/(2^(j-1))))  * XgSH1(j).x ;
    end
end

for i=1:numBlocks
    for j=1:maxLevel
        idx = (i-1)*(maxLevel)+j ;
        UXgSV_sens(i,j).x = SH ( idx , 1 ) * ( ugVX ) * XgSV(j).x ;
        UXcSV_sens(i,j).x = SH ( idx , 1 ) * ( ucVX ) * XgSV(j).x ;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:numBlocks
    ugH_nom(i).x = UXgV_BaseGrid.x + UXgH_BaseGrid.x ;
    ucH_nom(i).x = UXcV_BaseGrid.x + UXcH_BaseGrid.x ;
end
% 
% for i=1:numBlocks
%     for j=1:maxLevel
%         ugH_nom(i).x = ugH_nom(i).x + UXgSH(i,j).x ;
%         ucH_nom(i).x = ucH_nom(i).x + UXcSH(i,j).x ;
%     end
% end

for i=1:numBlocks
    for j=1:maxLevel
        ugH_nom(i).x = ugH_nom(i).x + UXgSH(i,j).x + UXgSH_sens(i,j).x - UXgSV_sens(i,j).x;
        ucH_nom(i).x = ucH_nom(i).x + UXcSH(i,j).x + UXcSH_sens(i,j).x - UXgSV_sens(i,j).x;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Add in E, W, N, S Connections
display('before buildbbxconn')
[GXconn, CXconn] = buildBBXconn() ;
display('after buildbbxcon')
% for j=1:numBlocks
%     for k=1:numBlocks
%         Xconn(j,k).x = sparse( N0 , N0) ;
%     end
% end
% 
% %% East , West
% 
% max_Grow_idx = sqrt(numBlocks) ;
% max_Gclm_idx = max_Grow_idx ;
% 
% % build gidTable 
% gidGTable=[];
% gidGTable=buildGidTable(gidGTable,max_Grow_idx);
% 
% gidTable = [];
% gidTable = buildGidTable(gidTable,bb_max_row_idx) ;
% 
% %% East West Big Blocks
% for j=1:max_Grow_idx-1
%         for k=1:max_Gclm_idx
%         %% East West little blocks
%          tmp=sparse(N0,N0);
%          tmp1=sparse(N0,N0);
%          for m=1:max_row_idx
%              for n=1:bb_max_clm_idx
%                 gx1 = bb_max_clm_idx ;
%                 gy1 = n ;
%                 gx2 = 1 ;
%                 gy2 = n ;
%                 tmp = set_loc1( gx1, gy1, gx2, gy2, bb_max_row_idx, tmp ) ;
%                 tmp1 = set_loc1( gx2 , gy2 , gx1 , gy1 , bb_max_row_idx , tmp1 ) ;
%              end
%          end
%         gid1=getGid(j,k,max_Grow_idx);
%         gid2=getGid(j+1,k,max_Grow_idx);
%         Xconn(gid1,gid2).x = Xconn(gid1,gid2).x + tmp;
%         Xconn(gid2,gid1).x = Xconn(gid2,gid1).x + tmp1;
%         end
% 
% end
% % North South Big Blocks
% for j=1:max_Grow_idx
%         for k=1:max_Gclm_idx-1
%         %% North South little blocks
%         tmp=sparse(N0,N0);
%         tmp1=sparse(N0,N0); 
%          for m=1:bb_max_row_idx
%              for n=1:bb_max_clm_idx
%                 gx1 = n;
%                 gy1 = bb_max_row_idx;
%                 gx2 = n ;
%                 gy2 = 1 ;
%                 tmp = set_loc1( gx1 , gy1, gx2, gy2, bb_max_row_idx, tmp ) ;
%                 tmp1 = set_loc1( gx2 , gy2 , gx1 , gy1 , bb_max_row_idx , tmp1 ) ;
%              end
%          end
%         gid1=getGid(j,k,max_Grow_idx);
%         gid2=getGid(j,k+1,max_Grow_idx);
%         Xconn(gid1,gid2).x = Xconn(gid1,gid2).x + tmp;
%         Xconn(gid2,gid1).x = Xconn(gid2,gid1).x + tmp1;
%         end
% end

%%%%%%%%%%%%%%%%%%%%%%%%%  Account for Big Blocks on Diag %%%%%%%%%%%%%%

% for i=1:numBlocks
%     ugH_nom_BB(i).x = ugH_nom(i).x ;
%     ucH_nom_BB(i).x = ucH_nom(i).x ;
% end
% 
% for i=1:numBlocks   
%     for j=1:numBlocks
%         if(j~=i)  
%             %%%%%%%% On Each little Row %%%%%%%%%
%             for k=1:N0
%                 tot=0;
%                 for m=1:N0
%                     if ( Xconn(i,j).x(k,m)~=0 )
%                         tot = tot + 1;
%                     end
%                 end
%              	ugH_nom_BB(i).x(k,k) = ugH_nom_BB(i).x(k,k) + ugx * tot ;
%                 ucH_nom_BB(i).x(k,k) = ucH_nom_BB(i).x(k,k) + ucx * tot ;
%             end
%         end
%         
%     end
% end

for i=1:numBlocks
    ugH_nom_BB(i).x = ugH_nom(i).x + GXconn(i).x ;
    ucH_nom_BB(i).x = ucH_nom(i).x + CXconn(i).x ;
end

v=1e-14*ones(N0,1) ;
for i=1:numBlocks
    ugH_nom_BB(i).x = ugH_nom_BB(i).x + diag (v,0) ;
    ucH_nom_BB(i).x = ucH_nom_BB(i).x + diag( v,0) ;
end

%%%%%%%%%%%%%%%%%%%%%%%%  Reduce All the BBs  %%%%%%%

for i=1:numBlocks
    Gg(i).xq = sparse( layer*q0 , layer*q0) ;
    Cg(i).xq = sparse( layer*q0 , layer*q0) ;
    Bg(i).xq = sparse( layer*q0 , p0) ;
    Lg(i).xq = sparse( layer*q0 , p0) ;
    Vtg(i).x = sparse( layer*N0, layer*q0) ;
end

for m=1:numBlocks
    
    Gx = ugH_nom_BB(m).x ;
    Cx = ucH_nom_BB(m).x ;
    portLoc=[1];
    B0 = sparse(portLoc(1:end),1:p0,1,N0,p0);
    L0=B0 ;

%   prima algorithm
    tm1=clock;

    q= ceil(q0/p0) ;

    disp(sprintf('reducing by PRIMA (to size %d by %d) ... ...\n',q*p0,q*p0));

    Vi(1).V= (Gx+s0*Cx)\B0;
    [Vi(1).V,R] = qr(Vi(1).V,0);
    for j=1:q-1
        W = -(Gx+s0*Cx)\(Cx*Vi(j).V);
        for i=1:j
            Hi(i,j).H = Vi(i).V.'*W;
            W = W - Vi(i).V*Hi(i,j).H;
        end
        for i=1:j
            Rho = Vi(i).V.'*W;
            W = W - Vi(i).V*Rho;
            Hi(i,j).H = Hi(i,j).H + Rho;
        end;
        [Vi(j+1).V,Hi(i+1,j).H]=qr(W,0);
    end; 
    Vt = []; 
    for i=1:q
        Vt=[Vt,Vi(i).V];
    end

    q0=q*p0 ;

    Gq = Vt.'*Gx*Vt; 
    Cq = Vt.'*Cx*Vt;
    Bq = Vt.'*B0;
    Lq = Vt.'*L0;  

    GXsVtot.x = (-1)*ugVX*XgV_nom.x ;
    CXsVtot.x = (-1)*ucVX*XgV_nom.x ;

        for j=1:maxLevel
            idx = (m-1)*(maxLevel)+j ;
            UXgSVsens(j).x = SH ( idx , 1 ) * ( ugVX ) * XgSV(j).x ;
            UXcSVsens(j).x = SH ( idx , 1 ) * ( ucVX ) * XgSV(j).x ;
        end


        for j=1:maxLevel
            GXsVtot.x = GXsVtot.x + UXgSVsens(j).x ;
            CXsVtot.x = CXsVtot.x + UXcSVsens(j).x ;
        end
        
    GXsVtot.xq = Vt.'*GXsVtot.x*Vt; 
    CXsVtot.xq = Vt.'*CXsVtot.x*Vt ;

for k=1:layer

    Gg(m).xq((k-1)*q0+1:(k-1)*q0+q0,(k-1)*q0+1:(k-1)*q0+q0)=Gq ;
    Cg(m).xq((k-1)*q0+1:(k-1)*q0+q0,(k-1)*q0+1:(k-1)*q0+q0)=Cq;
    Bg(m).xq((k-1)*q0+1:(k-1)*q0+q0,:)=Bq;
    Lg(m).xq((k-1)*q0+1:(k-1)*q0+q0,:)=Lq;
        if (k<layer)
                Gg(m).xq((k-1)*q0+q0+1:(k-1)*q0+2*q0,(k-1)*q0+1:(k-1)*q0+q0)= GXsVtot.xq ;
                Gg(m).xq((k-1)*q0+1:(k-1)*q0+q0,(k-1)*q0+q0+1:(k-1)*q0+2*q0)= GXsVtot.xq.';
                Cg(m).xq((k-1)*q0+q0+1:(k-1)*q0+2*q0,(k-1)*q0+1:(k-1)*q0+q0)= CXsVtot.xq;
                Cg(m).xq((k-1)*q0+1:(k-1)*q0+q0,(k-1)*q0+q0+1:(k-1)*q0+2*q0)= CXsVtot.xq.';
        end 
    Vtg(m).x((k-1)*N0+1:(k-1)*N0+N0,(k-1)*q0+1:(k-1)*q0+q0)=Vt;
end

end

%%%%%%%%%%   SENSISITIVIES  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i=1:maxLevel
    XgSH_tot(i).x = sparse ( layer*N0 , layer*N0 ) ;
    XgSV_tot(i).x = sparse ( layer*N0 , layer*N0 ) ;
end

for i=1:maxLevel
        for k=1:layer
            if (k<layer)
                XgSV_tot(i).x ( (k-1)*N0 + 1 : (k-1)*N0 + N0 , (k)*N0 + 1 : (k)*N0 + N0 ) = XgSV(i).x;
                XgSV_tot(i).x ( (k)*N0 + 1 : (k)*N0 + N0 , (k-1)*N0 + 1 : (k-1)*N0 + N0 ) = XgSV(i).x;
            end
            %XgSV_tot(i).x( (k-1)*N0+1 : (k-1)*N0+N0, (k-1)*N0+1 : (k-1)*N0+N0) = XgSV(i).x * (-1) ;
            XgSV_tot(i).x( (k-1)*N0+1 : (k-1)*N0+N0, (k-1)*N0+1 : (k-1)*N0+N0) = XgSV(i).x * (-1.2) ;
            XgSH_tot(i).x( (k-1)*N0+1 : (k-1)*N0+N0, (k-1)*N0+1 : (k-1)*N0+N0) = XgSH(i).x;
        end
        XgSH_tot(i).x = set_diagG ( layer*N0 , XgSH_tot(i).x , 0 ) ;

        GgSHBB(i).x = (ugHX / ((bb_max_row_idx-1)/(2^(i-1))))* XgSH_tot(i).x ;
        CgSHBB(i).x = (ucHX / ((bb_max_row_idx-1)/(2^(i-1))))* XgSH_tot(i).x ;
        GgSVBB(i).x = (ugVX) * XgSV_tot(i).x ;
        CgSVBB(i).x = (ucVX) * XgSV_tot(i).x ;
        
        GgSBB_tot(i).x = GgSHBB(i).x + GgSVBB(i).x ;
        CgSBB_tot(i).x = CgSHBB(i).x + CgSVBB(i).x ;
        
end


%%%%%%%%%%% Augment Xconn Matrix to add sensitivities

for i=1:numBlocks
    for j=1:numBlocks
        Xconn_aug(i,j).x = sparse ( N0*layer*(maxLevel+1) , N0*layer*(maxLevel+1) ) ;
    end
end

for i=1:numBlocks
    for j=1:numBlocks
            for k=1:layer*(maxLevel+1)
                Xconn_aug(i,j).x ( (k-1)*N0 + 1 : (k-1)*N0 + N0 , (k-1)*N0 + 1 : (k-1)*N0 + N0  ) = Xconn(i,j).x  ;
            end
    end

end

%%%%%%%%%%% Augment Vt Matrix to add sensitivities %%%%%%%%%%%%%%%

for i=1:numBlocks
  Vtg_aug(i).x = sparse( (maxLevel+1)*layer*N0 , (maxLevel+1)*layer*q0 ) ; 
	for j=1:maxLevel+1
  		Vtg_aug(i).x ( (j-1)*layer*N0 + 1 : (j-1)*layer*N0 + layer*N0 , (j-1)*layer*q0 + 1 : (j-1)*layer*q0 + layer*q0 ) = Vtg(i).x ;
	end
end

%%%%%%%%%%%  Reduce Xconn matrices individually   %%%%%%%%%%%%%%%%%%%%%%%%
for i=1:numBlocks
    for j=1:numBlocks
        Xconn(i,j).xq = Vtg_aug(i).x' * Xconn_aug(i,j).x *  Vtg_aug(j).x;
        GXconn(i,j).xq = ugx * Xconn(i,j).xq ;
        CXconn(i,j).xq = ucx * Xconn(i,j).xq ;
    end
end


