clear ;

max_row_idx = 162 ;
max_clm_idx = 162 ;
numBlocks = 4 ;
ugx = 1e-10 ;
ucx = 1e-15 ;

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 ;

%%%%%%%%%%%%%%% EDGE ASSIGNMENTS %%%%%%%%%%%%%%%%%%%%%%
numEdges = ( ( max_row_idx - 1 ) * max_row_idx ) * 2 ;

bbBoundBox = mapBB ( max_clm_idx , numBlocks ) ;

edgeBB = sparse(numEdges,1) ;
sigEdgeCong = sparse ( numEdges , 1 ) ;
PGEdgeCong = sparse ( numEdges , 1 ) ;
numEdgesPerBB = ( ( max_clm_idx / sqrt(numBlocks) - 1 ) * ( max_clm_idx / sqrt(numBlocks) ) )* 2;

%% assign each edge a bbid
%% edgeBB ( matlabeid + 1 )
%% BB assignments
%% 4->5->6 
%% 1->2->3

%% Vertical
for i=1:max_row_idx
    for j=1:max_clm_idx-1
        gx1 = i ;
        gy1 = j ;
        gx2 = i ;
        gy2 = j + 1;
        bb=0;
        for k=1:numBlocks
            if (gy1 >= bbBoundBox(k).minRow & gx1 >= bbBoundBox(k).minClm & gy2 <= bbBoundBox(k).maxRow & gx2 <= bbBoundBox(k).maxClm)
                bb = k ;
            end
        end
        g1 = matlabGxGy_to_matlabGid ( gx1, gy1, max_clm_idx ) ; 
        g2 = matlabGxGy_to_matlabGid ( gx2, gy2, max_clm_idx ) ;
        
        eid = matlabGid_to_matlabEdge ( g1, g2, max_clm_idx ) ;
        
        edgeBB(eid) = bb ;
        
    end
end

% %% HORIZONTAL
for i=1:max_row_idx
    for j=1:max_clm_idx-1
        gy1 = i ;
        gx1 = j ;
        gy2 = i ;
        gx2 = j+1 ;
        bb=0;
        for k=1:numBlocks
            if (gy1 >= bbBoundBox(k).minRow & gx1 >= bbBoundBox(k).minClm & gy2 <= bbBoundBox(k).maxRow & gx2 <= bbBoundBox(k).maxClm)
                bb = k ;  
            end
        end
        g1 = matlabGxGy_to_matlabGid ( gx1, gy1, max_clm_idx ) ; 
        g2 = matlabGxGy_to_matlabGid ( gx2, gy2, max_clm_idx ) ;
        
        eid = matlabGid_to_matlabEdge ( g1, g2, max_clm_idx ) ;
        
        edgeBB(eid) = bb ;
        
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Build Insertion Matrix %%%%%%%%%%%%%%%%%%%%

XgH_BaseGrid.x = sparse(N0,N0);
XgV_BaseGrid.x = sparse(N0,N0);

XgH_nom.x=sparse(N0,N0);
XgV_nom.x=sparse(N0,N0);
 

for i=1:maxLevel
        XgSH(i).x=sparse(N0,N0);
        XgSV(i).x=sparse(N0,N0);
        XgSH_rout(i).x = sparse(N0,N0) ;
end

% Build Base Grid Connections
 
%% East
for i=1:bb_max_clm_idx-1
    gx1=i;
    gy1=1;
    gx2=i+1;
    gy2=1;
    XgH_BaseGrid.x=set_loc(gx1,gy1,gx2,gy2,bb_max_row_idx,XgH_BaseGrid.x); 
    gx1=i;
    gy1=bb_max_clm_idx;
    gx2=i+1;
    gy2=bb_max_clm_idx;
    XgH_BaseGrid.x=set_loc(gx1,gy1,gx2,gy2,bb_max_row_idx,XgH_BaseGrid.x); 
end

%North
for i=1:bb_max_row_idx-1
    gx1=1;
    gy1=i;
    gx2=1;
    gy2=i+1;
    XgH_BaseGrid.x=set_loc(gx1,gy1,gx2,gy2,bb_max_row_idx,XgH_BaseGrid.x); 
    gx1=bb_max_row_idx;
    gy1=i;
    gx2=bb_max_row_idx;
    gy2=i+1;
    XgH_BaseGrid.x=set_loc(gx1,gy1,gx2,gy2,bb_max_row_idx,XgH_BaseGrid.x); 
end

%Vertical Base insertion matrix

gx1=1;
gy1=1;
XgV_BaseGrid.x = set_loc(gx1,gy1,gx1,gy1,bb_max_row_idx,XgV_BaseGrid.x);
gx1=1;
gy1=bb_max_row_idx;
XgV_BaseGrid.x = set_loc(gx1,gy1,gx1,gy1,bb_max_row_idx,XgV_BaseGrid.x);
gx1=bb_max_clm_idx;
gy1=1;
XgV_BaseGrid.x = set_loc(gx1,gy1,gx1,gy1,bb_max_row_idx,XgV_BaseGrid.x);
gx1=bb_max_clm_idx;
gy1=bb_max_row_idx;
XgV_BaseGrid.x = set_loc(gx1,gy1,gx1,gy1,bb_max_row_idx,XgV_BaseGrid.x);


%% Uze Base Grid Information to build sensitivity for level 1
gx1=(bb_max_clm_idx-1)/2 + 1;
gy1=1;
gx2=gx1;
gy2=(bb_max_row_idx);
XgSH(1).x = set_loc(gx1,gy1,gx2,gy2,bb_max_row_idx,XgSH(1).x);

gy1=(bb_max_clm_idx-1)/2 + 1;
gx1=1;
gy2=gy1;
gx2=bb_max_row_idx;
XgSH(1).x = set_loc(gx1,gy1,gx2,gy2,bb_max_row_idx,XgSH(1).x);

gx1=(bb_max_row_idx-1)/2 + 1;
gy1=1;
XgSV(1).x = set_loc(gx1,gy1,gx1,gy1,bb_max_row_idx,XgSV(1).x);

gx1=1;
gy1=(bb_max_row_idx-1)/2 + 1;
XgSV(1).x = set_loc(gx1,gy1,gx1,gy1,bb_max_row_idx,XgSV(1).x);

gx1=bb_max_row_idx;
gy1=(bb_max_row_idx-1)/2 + 1;
XgSV(1).x = set_loc(gx1,gy1,gx1,gy1,bb_max_row_idx,XgSV(1).x);

gx1=(bb_max_row_idx-1)/2 + 1;
gy1=bb_max_row_idx;
XgSV(1).x = set_loc(gx1,gy1,gx1,gy1,bb_max_row_idx,XgSV(1).x);

%%% Build insertion matrix for routing

for i=1:bb_max_clm_idx-1
    gx1 = (bb_max_clm_idx-1)/2 + 1 ;
    gy1 =  i ;
    gx2 = gx1 ;
    gy2 = i+1 ;
    XgSH_rout(1).x = set_loc1(gx1,gy1,gx2,gy2,bb_max_row_idx,XgSH_rout(1).x) ;

    gy1 = (bb_max_clm_idx-1)/2 + 1 ;
    gx1 = i ;
    gy2 = gy1 ; 
    gx2 = i+1  ;
    XgSH_rout(1).x = set_loc1(gx1,gy1,gx2,gy2,bb_max_row_idx,XgSH_rout(1).x) ;
end

for i=2:maxLevel
    iter=2^(i-1);  %% # iterations on a row
    idx=1;  
    tileSize = (bb_max_clm_idx-1)/iter;
    for j=1:iter
        startRow=tileSize*(j-1)+1;
        endRow = startRow+tileSize;
        startClm=1;
        
        for k=1:iter
            endClm=startClm+tileSize;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% SENSITIVITIES %%%%%%%%%%%%         
            tmp=sparse(N0,N0);
            gx1=(endClm-startClm)/2 + startClm;
            gy1=startRow;
            gx2=gx1;
            gy2=endRow;
            XgSH(i).x = set_loc(gx1,gy1,gx2,gy2,bb_max_row_idx,XgSH(i).x) ;
            
            gx1 = startClm;
            gy1 = (endRow-startRow)/2 + startRow;
            gx2 = endClm;
            gy2 = gy1;
            XgSH(i).x = set_loc(gx1,gy1,gx2,gy2,bb_max_row_idx,XgSH(i).x) ;

            tmp=sparse(N0,N0);
            gx1=(endClm-startClm)/2 + startClm;
            gy1=startRow;
            XgSV(i).x = set_loc(gx1,gy1,gx1,gy1,bb_max_row_idx,XgSV(i).x) ;
            
            gx1=(endClm-startClm)/2 + startClm;
            gy1=endRow;
            XgSV(i).x = set_loc(gx1,gy1,gx1,gy1,bb_max_row_idx,XgSV(i).x) ;
            
            gx1=startClm;
            gy1=(endRow-startRow)/2 + startRow;
            XgSV(i).x = set_loc(gx1,gy1,gx1,gy1,bb_max_row_idx,XgSV(i).x) ;
            
            gx1=endClm;
            gy1=(endRow-startRow)/2 + startRow;
            XgSV(i).x = set_loc(gx1,gy1,gx1,gy1,bb_max_row_idx,XgSV(i).x) ;
            
            %% build insertion for routing
            for mm=startRow:endRow-1
                gx1=(endClm-startClm)/2 + startClm ;
                gy1 = mm ;
                gx2 = gx1 ; 
                gy2 = mm+1 ; 
                XgSH_rout(i).x = set_loc1(gx1,gy1,gx2,gy2,bb_max_row_idx,XgSH_rout(i).x) ;
            end
            
            for nn=startClm:endClm-1
                gx1 = nn ;
                gy1 = (endRow-startRow)/2 + startRow;
                gx2 = nn+1 ;
                gy2 = gy1 ;
                XgSH_rout(i).x = set_loc1(gx1,gy1,gx2,gy2,bb_max_row_idx,XgSH_rout(i).x) ;
            end
            
            startClm=endClm;
        end
    end
end

%%%%%%%%%%%%%%  ASSIGN EDGES A SENSITIVITY LEVEL %%%%%%%%%%%%%%%%

routEdgeLevel = sparse ( numEdges , 1 ) ;
  for kk=1:maxLevel

    [aa,bb,cc]=find (XgSH_rout(1,kk).x) ;
    [mm,nn]=size(aa) ;
    for i=1:mm
       gid1 = aa(i) ;
       gid2 = bb(i) ;
       if(gid1~=gid2)

           %%%%%%%%%%%%%%  INITIAL GRID 00 %%%%%%%%%%%%%%%%%

           [ gx1, gy1 ] = matlabGid_to_matlabGxGy ( gid1 , bb_max_clm_idx ) ; 
           [ gx2, gy2 ] = matlabGid_to_matlabGxGy ( gid2 , bb_max_clm_idx ) ;

           for j=1:sqrt(numBlocks)
           %   display('move up vertically')
               tgx1 = gx1 ; 
               tgy1 = gy1 + (j-1)*bb_max_clm_idx ; 
               tgx2 = gx2 ; 
               tgy2 = gy2 + (j-1)*bb_max_clm_idx ; 

               gid1 = matlabGxGy_to_matlabGid ( tgx1 , tgy1 , max_clm_idx ) ;
               gid2 = matlabGxGy_to_matlabGid ( tgx2 , tgy2 , max_clm_idx ) ;
               eid = matlabGid_to_matlabEdge ( gid1, gid2, max_clm_idx ) ;

               routEdgeLevel ( eid , 1 ) = kk ; 

        %% move across horizontally
                for k=1:sqrt(numBlocks)-1

                    tgx1 = gx1  + (k)*bb_max_clm_idx ;
                    tgy1 = tgy1  ;
                    tgx2 = gx2 + (k)*bb_max_clm_idx ;
                    tgy2 = tgy2 ;

                    gid1 = matlabGxGy_to_matlabGid ( tgx1 , tgy1 , max_clm_idx ) ;
                    gid2 = matlabGxGy_to_matlabGid ( tgx2 , tgy2 , max_clm_idx ) ;
                    eid = matlabGid_to_matlabEdge ( gid1, gid2, max_clm_idx ) ;

                    routEdgeLevel ( eid , 1 ) = kk ;  

                end
           end
       end
    end
  end


 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%     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 + 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( max_row_idx , max_clm_idx , ugx, ucx) ;
display('after buildbbxcon')


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



for i=1:numBlocks
    Gg1(i) = Gg(i).xq ;
    Cg1(i) = Cg(i).xq ;
    Bg1(i) = Bg(i).xq ;
    Lg1(i) = Lg(i).xq ;
    GgSBB1(i) = GgSBB_tot(i).xq ;
    CgSBB1(i) = CgSBB_tot(i).xq ;
    GXconn1(i) = 
end



    
dlmwrite ( 'edgeBB.out' , edgeBB ) ;
dlmwrite ( 'routEdgeLevel.out', routEdgeLevel ) ;
dlmwrite ( 'bb1g.out', Gg1(1) ) ;
dlmwrite ( 'bb2g.out', Gg1(2) ) ;
dlmwrite ( 'bb3g.out', Gg1(3) ) ;
dlmwrite ( 'bb4g.out', Gg1(4) ) ;

dlmwrite ( 'bb1c.out', Cg1(1) ) ;
dlmwrite ( 'bb2c.out', Cg1(2) ) ;
dlmwrite ( 'bb3c.out', Cg1(3) ) ;
dlmwrite ( 'bb4c.out', Cg1(4) ) ;

dlmwrite ( 'bb1b.out', Bg1(1) ) ;
dlmwrite ( 'bb2b.out', Bg1(2) ) ;
dlmwrite ( 'bb3b.out', Bg1(3) ) ;
dlmwrite ( 'bb4b.out', Bg1(4) ) ;

dlmwrite ( 'bb1l.out', Lg1(1) ) ;
dlmwrite ( 'bb2l.out', Lg1(2) ) ;
dlmwrite ( 'bb3l.out', Lg1(3) ) ;
dlmwrite ( 'bb4l.out', Lg1(4) ) ;


dlmwrite ( 'GgSBB1.out', GgSBB1(1) ) ;
dlmwrite ( 'GgSBB2.out', GgSBB1(2) ) ;
dlmwrite ( 'GgSBB3.out', GgSBB1(3) ) ;
dlmwrite ( 'GgSBB4.out', GgSBB1(4) ) ;

dlmwrite ( '





for ii=1:2
display ( 'tri')
[ GgTri, CgTri, BgTri ] = tri ( Gg, Cg, Bg, Vtg, GgSBB_tot, CgSBB_tot, GXconn, CXconn , SH) ;
display ( 'sim1' )
[ sensTot, xout1 ] = sim1( GgTri, CgTri, BgTri, sigEdgeCong, edgeBB );
display ( 'sim2' )
[xout] = sim2 ( xout1 , Lg, delta ) ;

delta  = step * sensTot / norm ( sensTot )
 
SH =  SH  + delta
VPinit=max(max(xout))
plotGraph ( xout ) ;

end