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


%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
APS1=[];
BPS1=[];
CPS1=[];
DPS1=[];
APS2=[];
BPS2=[];
CPS2=[];
DPS2=[];
APS=[];
BPS=[];
CPS=[];
DPS=[];
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
if(sum(xpss_nt)~=0)
 for k=1:size(xpss_nt,2)
  [NPSS1,DPSS1]=series([Tw_slip_g(xpss_nt(k))*Kslip_pss_g(xpss_nt(k)) 0],[Tw_slip_g(xpss_nt(k)) 1],[T1_slip_g(xpss_nt(k)) 1],[T2_slip_g(xpss_nt(k)) 1]);
  if (Tmd_slip_nt==0)
      [NPSS1,DPSS1]=series(NPSS1,DPSS1,[1],[Td1_slip_g(xpss_nt(k)) 1]);
  end    
  [A1,B1,C1,D1]=tf2ss(NPSS1,DPSS1);
  [APS1,BPS1,CPS1,DPS1]=append(APS1,BPS1,CPS1,DPS1,A1,B1,C1,D1);
  end
end
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
if(sum(xpss_t)~=0)
 for k=1:size(xpss_t,2)
  [NPSS2,DPSS2]=series([Tw_slip_g(xpss_t(k))*Kslip_pss_g(xpss_t(k)) 0],[Tw_slip_g(xpss_t(k)) 1],[T1_slip_g(xpss_t(k)) 1],[T2_slip_g(xpss_t(k)) 1]);
  [NPSS2,DPSS2]=series(NPSS2,DPSS2,[a0_slip_g(xpss_t(k))],[1 a1_slip_g(xpss_t(k)) a0_slip_g(xpss_t(k))]);
  if (Tmd_slip_t==0)
      [NPSS2,DPSS2]=series(NPSS2,DPSS2,[1],[Td1_slip_g(xpss_t(k)) 1]);
  end 
  [A2,B2,C2,D2]=tf2ss(NPSS2,DPSS2);
  [APS2,BPS2,CPS2,DPS2]=append(APS2,BPS2,CPS2,DPS2,A2,B2,C2,D2);
  end
end
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
[APS,BPS,CPS,DPS]=append(APS1,BPS1,CPS1,DPS1,APS2,BPS2,CPS2,DPS2);
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
APS=sparse(APS);
BPS=sparse(BPS);
CPS=sparse(CPS);
DPS=sparse(DPS);
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
SL_x_pss=[xpss_nt xpss_t]';
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
for k=1:size(slip_pss,1)
    for p=1:ngen
        if (SL_x_pss(k,1)==gen(p,1))
            temp(k)=p;
        end
    end
end

PS1=sparse(zeros(size(slip_pss,1),ngen));
PS1(1:(size(slip_pss,1)),temp)=(eye(size(slip_pss,1)));
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
% PS1 matrix is used to adjust the dimension of DPS 
AG=([AG+EG*PS1'*DPS*PS1*FG EG*PS1'*CPS; BPS*PS1*FG APS]);
BG=([BG; sparse(zeros(size(APS,1),2*ngen))]);
CG=([CG  sparse(zeros(size(APS,1),2*ngen)')]);
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
clear C1 D1;
clear A1 B1;
%clear APS BPS;
%clear CPS DPS;
clear temp;
%end
