
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%        FORMATION OF A MATRIX with PSS

TAGmod=sparse([zeros(ngen) zeros(ngen) zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)
              diag(Ag21)  zeros(ngen)  diag(Ag23)    diag(Ag24)   diag(Ag25)   diag(Ag26)    zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)
              zeros(ngen) zeros(ngen) zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)
              zeros(ngen) zeros(ngen) zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)
              zeros(ngen) zeros(ngen) zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)
              zeros(ngen) zeros(ngen) zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)
              zeros(ngen) zeros(ngen) zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)
              zeros(ngen) zeros(ngen) zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)
              zeros(ngen) zeros(ngen) zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)
              zeros(ngen) zeros(ngen) zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)
              zeros(ngen) zeros(ngen) zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)
              zeros(ngen) zeros(ngen) zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)
              zeros(ngen) zeros(ngen) zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)
              zeros(ngen) zeros(ngen) zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)
              zeros(ngen) zeros(ngen) zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)
              zeros(ngen) zeros(ngen) zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)  zeros(ngen)   zeros(ngen)  zeros(ngen)]);    
              
 TAG=TAGmod(ii,ii);
 clear TAGmod;
 %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 TBGmod=sparse([zeros(ngen)      zeros(ngen)
              diag(Bg21rect)   diag(Bg22rect)
              zeros(ngen)      zeros(ngen)
              zeros(ngen)      zeros(ngen)
              zeros(ngen)      zeros(ngen)
              zeros(ngen)      zeros(ngen)
              zeros(ngen)      zeros(ngen)
              zeros(ngen)      zeros(ngen)
              zeros(ngen)      zeros(ngen)
              zeros(ngen)      zeros(ngen)
              zeros(ngen)      zeros(ngen)
              zeros(ngen)      zeros(ngen)
              zeros(ngen)      zeros(ngen)
              zeros(ngen)      zeros(ngen)
              zeros(ngen)      zeros(ngen)
              zeros(ngen)      zeros(ngen) ]);
           
           
  TBG=TBGmod(ii,kk);
  clear TBGmod; 
  %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  TCGp2=CG;
  
 if(npss~=0)
   TAGp2=([TAG+sparse(zeros(size(EG*PS1'*DPS*PS1*FG))) sparse(zeros(size(EG*PS1'*CPS))); BPS*PS1*FG APS]);
   TBGp2=([TBG; sparse(zeros(size(APS,1),2*ngen))]);
   TEGp2=([EG; sparse(zeros(size(APS,1),ngen))]); %padded zeros to EG suitable.
 else
   TAGp2=TAG;
   TBGp2=TBG;
   TEGp2=EG;
 end
 
if(npss1~=0)  
TAGp2=([TAGp2+sparse(zeros(size(TEGp2*PS2'*DPSS*PS2m*FGd))) sparse(zeros(size(TEGp2*PS2'*CPSS))); BPSS*PS2m*FGd APSS]);
TBGp2=([TBGp2; sparse(zeros(size(APSS,1),2*ngen))]);
TEGp2=([TEGp2; sparse(zeros(size(APSS,1),ngen))]); %padded zeros to EG suitable.
end


TEATd=-TAGp2-TBGp2*PG'*YDQdi*PG*TCGp2; 
 %~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 H2_diag = 2*diag(H);
 TEATD2=H2_diag*TEATd([2:16:16*ngen],:);

APSS2=[];
BPSS2=[];
CPSS2=[];
DPSS2=[];

for k=1:size(power_pss,1)
[NP1,DP1]=series([-Kpower_pss(k)],[Td1_power(k) 1],[Tw_power(k) 0],[Tw_power(k) 1]);
[APSS2d,BPSS2d,CPSS2d,DPSS2d]=tf2ss(NP1,DP1);
[APSS2,BPSS2,CPSS2,DPSS2]=append(APSS2,BPSS2,CPSS2,DPSS2,APSS2d,BPSS2d,CPSS2d,DPSS2d);
end
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
APSS2=sparse(APSS2);
BPSS2=sparse(BPSS2);
CPSS2=sparse(CPSS2);
DPSS2=sparse(DPSS2);
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
for k=1:size(power_pss,1)
    for p=1:ngen
        if (power_pss(k,1)==gen(p,1))
            temp(k)=p;
        end
    end
end

PS3=sparse(zeros(size(power_pss,1),ngen));
PS3(1:(size(power_pss,1)),temp)=(eye(size(power_pss,1)));
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
AG=([AG+TEGp2*PS3'*DPSS2*PS3*TEATD2 TEGp2*PS3'*CPSS2; BPSS2*PS3*TEATD2 APSS2]);
BG=([BG; sparse(zeros(size(APSS2,1),2*ngen))]);
CG=([CG  sparse(zeros(size(APSS2,1),2*ngen)')]);
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
clear C1 D1;
clear A1 B1;
clear temp;
