clc;
clear;
n=5; %input('請輸入節點數:n=');
b=5; %input('請輸入支路數:b=');
jd=0.00001; %input(' 請輸入精度:jd=');
sb=5; %input(' 請輸入平衡節點號sb=');
c=load('zlsj.txt');                                                 %支路數據形成的矩陣 第一列支路號 第二列支路始端 第三列支路末端 第四列支路電阻 
                                                                           %第五列支路電抗 第六列支路導納一半 第七列變比 第八列方向標記
d=load('jdsj.txt');                                               %節點數據形成的矩陣 第一列節電號 第二列節點類型:1--PQ 2--PV 3--平衡 第三列注入有功 
                                                                           %第四列注入無功 第五列消耗有功 第六列消耗無功 第七初始電壓實部 第八列初始電壓虛部 
for ii=1:b
    B1(ii,1)=c(ii,1);                            %支路號
    B1(ii,2)=c(ii,2);                            %支路始端
    B1(ii,3)=c(ii,3);                            %支路末端
    B1(ii,4)=c(ii,4)+i*c(ii,5);                  %支路阻抗 
    B1(ii,5)=c(ii,6);                            %支路導納一半
    B1(ii,6)=c(ii,7);                            %變比
    B1(ii,7)=c(ii,8);                            %方向標記（首端低壓為0，反之為1）
end

%---------------------------------------------以下求導納矩陣
Y=zeros(n,n);
for ii=1:b
    if B1(ii,7)==0
       p=B1(ii,2);q=B1(ii,3);
    else
       p=B1(ii,3);q=B1(ii,2);
    end
    Y(p,q)=Y(p,q)-1./(B1(ii,4)*B1(ii,6)); 
    Y(q,p)=Y(p,q);
    Y(p,p)=Y(p,p)+1./B1(ii,4)+i*B1(ii,5);                                  %以前錯誤虛部忘記乘以i
    Y(q,q)=Y(q,q)+1./(B1(ii,4)*B1(ii,6)^2)+i*B1(ii,5);
end
disp('導納矩陣如下:');
disp(Y);

%--------------------------------------以下求ΔP,ΔQ或ΔP,ΔV^2
G=real(Y);
B=imag(Y);

for ii=1:n
    e(1,ii)=d(ii,7);                                   %電壓實部
    f(1,ii)=d(ii,8);                                   %電壓虛部
    V(1,ii)=sqrt(e(1,ii)^2+f(1,ii)^2);                 %電壓幅值
    angle(1,ii)=atan(f(1,ii)/e(1,ii))*180/pi;          %電壓相角（角度）
end   

Ps=zeros(1,n);
Qs=zeros(1,n);
DP=zeros(1,n);
DQ=zeros(1,n);
ee=zeros(100,n);
ff=zeros(100,n);

for j=1:n
     Ps(1,j)=d(j,3)+d(j,5);
     Qs(1,j)=d(j,4)+d(j,6);         
end

cs=0;
pre=1;

while pre>jd    
    cs=cs+1;   
      A1=zeros(1,n);
      A2=zeros(1,n);
      for ii=1:n
          for j=1:n   
              A1(1,ii)=A1(1,ii)+G(ii,j)*e(1,j)-B(ii,j)*f(1,j);
              A2(1,ii)=A2(1,ii)+G(ii,j)*f(1,j)+B(ii,j)*e(1,j);
          end
          if d(ii,2)==1                                                    %PQ節點
             DP(1,ii)=Ps(1,ii)-e(1,ii)*A1(1,ii)-f(1,ii)*A2(1,ii);
             DQ(1,ii)=Qs(1,ii)-f(1,ii)*A1(1,ii)+e(1,ii)*A2(1,ii);
          else                                                              %PV及平衡節點
                 DP(1,ii)=Ps(1,ii)-e(1,ii)*A1(1,ii)-f(1,ii)*A2(1,ii);
                 DV(1,ii)=d(ii,7)^2+d(ii,8)^2-(e(1,ii)^2+f(1,ii)^2);
          end
      end
      
%--------------------------------------------------以下是求所有節點雅可比矩陣
     K=zeros(2*n,2*n+3); 
     H=zeros(n);
     N=zeros(n);
     J=zeros(n);
     L=zeros(n);
     R=zeros(n);
     S=zeros(n);
     
      for ii=1:n
          if d(ii,2)==1
             for j=1:n
                 if ii~=j
                    H(ii,j)=-G(ii,j)*e(1,ii)-B(ii,j)*f(1,ii);
                    N(ii,j)=B(ii,j)*e(1,ii)-G(ii,j)*f(1,ii);
                    J(ii,j)=B(ii,j)*e(1,ii)-G(ii,j)*f(1,ii);
                    L(ii,j)=G(ii,j)*e(1,ii)+B(ii,j)*f(1,ii);
                 else
                     H(ii,j)=-G(ii,j)*e(1,ii)-B(ii,j)*f(1,ii)-A1(1,ii);
                     N(ii,j)=B(ii,j)*e(1,ii)-G(ii,j)*f(1,ii)-A2(1,ii);
                     J(ii,j)=B(ii,j)*e(1,ii)-G(ii,j)*f(1,ii)+A2(1,ii);
                     L(ii,j)=G(ii,j)*e(1,ii)+B(ii,j)*f(1,ii)-A1(1,ii);
                 end
                 p=2*ii-1;q=2*j-1;r=p+1;s=q+1;t=2*n+1;
                 K(p,q)=J(ii,j);K(p,s)=L(ii,j);K(p,t)=DQ(1,ii);
                 K(r,q)=H(ii,j);K(r,s)=N(ii,j);K(r,t)=DP(1,ii);
             end 
         else     
              for j=1:n
                  if ii~=j
                     H(ii,j)=-G(ii,j)*e(1,ii)-B(ii,j)*f(1,ii);
                     N(ii,j)=B(ii,j)*e(1,ii)-G(ii,j)*f(1,ii);
                     R(ii,j)=0;
                     S(ii,j)=0;
                  else
                      H(ii,j)=-G(ii,j)*e(1,ii)-B(ii,j)*f(1,ii)-A1(1,ii);
                      N(ii,j)=B(ii,j)*e(1,ii)-G(ii,j)*f(1,ii)-A2(1,ii);
                      R(ii,j)=-2*e(1,ii);
                      S(ii,j)=-2*f(1,ii);
                  end
                  p=2*ii-1;q=2*j-1;r=p+1;s=q+1;t=2*n+1;
                  K(p,q)=R(ii,j);K(p,s)=S(ii,j);K(p,t)=DV(1,ii);
                  K(r,q)=H(ii,j);K(r,s)=N(ii,j);K(r,t)=DP(1,ii);
              end
          end
          K(2*ii-1,2*n+2)=d(ii,1);K(2*ii-1,2*n+3)=d(ii,2);
          K(2*ii,2*n+2)=d(ii,1);K(2*ii,2*n+3)=d(ii,2);
      end
      
%-----------------------------------------------以下是求除去平衡節點的雅可比矩陣
      KL=zeros(2*n-2,2*n+3);                                               %KL為行除去平衡節點后形成的矩陣
      for ii=1:2*n
          if K(ii,2*n+2)==sb
              break
          end
      end
              for q=1:ii-1
                  for j=1:2*n+3
                      KL(q,j)=K(q,j);
                  end
              end
              for p=ii:2*n-2
                  for j=1:2*n+3
                      KL(p,j)=K(p+2,j);
                  end
              end            
      
      KK=zeros(2*n-2,2*n+1);                                               %KK為行和列除去平衡節點后形成的矩陣
      for j=1:2*n
          if K(j,2*n+2)==sb
              break
          end
      end
              for q=1:j-1
                  for ii=1:2*n-2
                      KK(ii,q)=KL(ii,q);
                  end
              end
              for p=j:2*n+1
                  for ii=1:2*n-2
                      KK(ii,p)=KL(ii,p+2);
                  end
              end
                 
      zd(1,cs)=KK(1,2*n-1);
      for ii=1:2*n-2    
          if KK(ii,2*n-1)>zd(1,cs)
             zd(1,cs)=KK(ii,2*n-1);
          end 
      end 
      
%------------------------------------------以下是高斯消去法求e,f
      for ii=1:2*n-2
          for j=ii+1:2*n-1
              KK(ii,j)=KK(ii,j)/KK(ii,ii);
         end
          KK(ii,ii)=1;
          for p=ii+1:2*n-2
              for j=ii+1:2*n-1
                  KK(p,j)=KK(p,j)-KK(ii,j)*KK(p,ii);
              end
              KK(p,ii)=0;
          end
      end                                                                  %求出了上三角矩陣  
      
%-------------------------------------------以下是回代過程      
      X=zeros(1,2*n-2);DE=zeros(1,n-1);DF=zeros(1,n-1);
      for ii=2*n-2:-1:1
          if ii~=2*n-2
             X(1,ii)=KK(ii,2*n-1);
             for j=2*n-2:-1:ii+1
                 X(1,ii)=X(1,ii)-KK(ii,j)*X(1,j);
             end
          else
              X(1,2*n-2)=KK(2*n-2,2*n-1);
          end
      end                                                                  %回代結束
     pree=0;
       for ii=1:2*n-2
          if abs(X(1,ii))>pree
              pree=abs(X(1,ii));
          end
       end
       pre=pree;                                                           %判斷是否收斂
       
%---------------------------------------------以下是求修正值       
       for ii=1:n-1
           DE(1,ii)=X(1,2*ii-1);
           DF(1,ii)=X(1,2*ii);                                             %除去平衡節點的電壓實部、虛部的修正值
       end
       for ii=1:n
          if ii==sb
             for j=n:-1:ii+1
                  DE(1,j)=DE(1,j-1);
                  DF(1,j)=DF(1,j-1);
             end
             DE(1,ii)=0;
             DF(1,ii)=0;
          end
       end                                                                 %加上平衡節點后的電壓實部、虛部修正值  
       
%-----------------------------------------------以下是修正后的新的值       
      for ii=1:n               
          if cs~=1
             ee(cs,ii)=ee(cs-1,ii)-DE(1,ii);
             e(1,ii)=ee(cs,ii);
             ff(cs,ii)=ff(cs-1,ii)-DF(1,ii);
             f(1,ii)=ff(cs,ii);
          else
              ee(cs,ii)=e(1,ii)-DE(1,ii);
              e(1,ii)=ee(cs,ii);
              ff(cs,ii)=f(1,ii)-DF(1,ii);
              f(1,ii)=ff(cs,ii);
          end
             V(cs,ii)=sqrt(ee(cs,ii)^2+ff(cs,ii)^2);
             angle(cs,ii)=atan(ff(cs,ii)/ee(cs,ii))*180/pi;
      end
end
%----------------------------------------------以下是求各點有功功率、無功功率
for ii=1:n
    A1(1,ii)=0;
    A2(1,ii)=0;
    for j=1:n
       A1(1,ii)=A1(1,ii)+G(ii,j)*e(1,j)-B(ii,j)*f(1,j);
       A2(1,ii)=A2(1,ii)+G(ii,j)*f(1,j)+B(ii,j)*e(1,j);
    end
    PP(1,ii)=e(1,ii)*A1(1,ii)+f(1,ii)*A2(1,ii);
    QQ(1,ii)=f(1,ii)*A1(1,ii)-e(1,ii)*A2(1,ii);
end
CH=zeros(n,5);                                                             %CH為各節點潮流。第一列為節點號，第二列為電壓幅值，
                                                                           %第三列為電壓角度，第四列為有功功率，第五列為無功功率
for ii=1:n
    CH(ii,1)=ii;
    CH(ii,2)=V(cs,ii);
    CH(ii,3)=angle(cs,ii);
    CH(ii,4)=PP(1,ii);
    CH(ii,5)=QQ(1,ii);
end

disp('迭代次數:')
disp(cs);
disp('電壓實部:')
disp(ee(1:cs,n));
disp('電壓虛部:')
disp(ff(1:cs,n));
disp('電壓數量值:')
disp(V);
disp('電壓角度值:')
disp(angle);
disp('各節點潮流')
disp(CH)
figure(1);
plot(zd);
xlabel('diedaicishu');
ylabel('dianyawucha');