Tiempo=15000;
id1=Isource1(Tiempo,1);
iq1=Isource1(Tiempo,2);
id2=Isource2(Tiempo,1);
iq2=Isource2(Tiempo,2);
ed1=Vsource1(Tiempo,1);
eq1=Vsource1(Tiempo,2);
ed2=Vsource2(Tiempo,1);
eq2=Vsource2(Tiempo,2);
s=tf('s');

kp=2*pi*0.0001;%0.976*5.6955e-06%
%2*pi*0.2373*3.5714e-06 critical damping
kq=0.016;%5.4212e-04%2*pi*5e-4/10;
wf=2*pi;
%% Parámetros derivados de la linealización 

md1=-eq1/(ed1^2+eq1^2);
md2=-eq2/(ed2^2+eq2^2);
mq1=ed1/(ed1^2+eq1^2);
mq2=ed2/(ed2^2+eq2^2);
nd1=ed1/(sqrt(ed1^2+eq1^2));
nd2=ed2/(sqrt(ed2^2+eq2^2));
nq1=eq1/(sqrt(ed1^2+eq1^2));
nq2=eq2/(sqrt(ed2^2+eq2^2));

M1=[-wf 0 0;
    nq1/(md1*nq1-mq1*nd1) (mq1*nd1*wf)/(md1*nq1-mq1*nd1) (mq1*nq1*wf)/(md1*nq1-mq1*nd1);
    nd1/(mq1*nd1-md1*nq1) (md1*nd1*wf)/(mq1*nd1-md1*nq1) (md1*nq1*wf)/(mq1*nd1-md1*nq1)];
M2=[-wf 0 0;
    nq2/(md2*nq2-mq2*nd2) (mq2*nd2*wf)/(md2*nq2-mq2*nd2) (mq2*nq2*wf)/(md2*nq2-mq2*nd2);
    nd1/(mq1*nd1-md1*nq1) (md2*nd2*wf)/(mq2*nd2-md2*nq2) (md2*nq2*wf)/(mq2*nd2-md2*nq2)];
Ms=[M1 [0 0 0;0 0 0;0 0 0];[0 0 0;0 0 0;0 0 0] M2];
C1=[-kp*wf 0;0 (kq*mq1*wf)/(md1*nq1-mq1*nd1);0 (kq*md1*wf)/(mq1*nd1-md1*nq1)];
C2=[-kp*wf 0;0 (kq*mq2*wf)/(md2*nq2-mq2*nd2);0 (kq*md2*wf)/(mq2*nd2-md2*nq2)];
Cs=[C1 [0 0;0 0;0 0];[0 0;0 0;0 0] C2];
    
%% Parámetros
f1=fre(Tiempo,1);
F1=f1;
w=2*pi*f1;
L=190e-3;
Rl=10.1;
RLoad=230;
%% Nodal admitance matrix
YLESS1=1/(Rl+w*L*1i);
YLESS2=1/(Rl+w*L*1i);
YLOAD=1/RLoad;
Y1=(YLESS1*(YLOAD+YLESS2))/(YLESS1+(YLOAD+YLESS2));
Y2=((YLESS2*(YLOAD+YLESS1))/(YLESS2+(YLOAD+YLESS1)))*YLESS1/(YLOAD+YLESS1);
Y3=((YLESS1*(YLOAD+YLESS2))/(YLESS1+(YLOAD+YLESS2)))*YLESS2/(YLOAD+YLESS2);
Y4=(YLESS2*(YLOAD+YLESS1))/(YLESS2+(YLOAD+YLESS1));
YA=[Y1 -Y2;-Y3 Y4];
Ys1=[real(YA(1,1)) -imag(YA(1,1)) real(YA(1,2)) -imag(YA(1,2));imag(YA(1,1)) real(YA(1,1)) imag(YA(1,2)) real(YA(1,2));real(YA(2,1)) -imag(YA(2,1)) real(YA(2,2)) -imag(YA(2,2));imag(YA(2,1)) real(YA(2,1)) imag(YA(2,2)) real(YA(2,2))]


%% Verificación 
E=[ed1;eq1;ed2;eq2];
Iess2=Ys1*E;
%%  

Is1=[id1 iq1;iq1 -id1];
Is2=[id2 iq2;iq2 -id2];
Is=(3/2)*[Is1 [0 0;0 0];[0 0;0 0] Is2];

Es1=[ed1 eq1;-eq1 ed1];
Es2=[ed2 eq2;-eq2 ed2];
Es=(3/2)*[Es1 [0 0;0 0];[0 0;0 0] Es2];
Ks=[0 1 0 0 0 0;0 0 1 0 0 0;0 0 0 0 1 0;0 0 0 0 0 1];
A=Ms+Cs*(Is+Es*Ys1)*Ks;
B=eye(6);
C=[1 0 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0];
G=C*inv(s*eye(6)-A)*B;
tf=G(1,1)
p=eig(A);
%figure
for k = 1:1:6
    plot(real(p(k)),imag(p(k)),'*')
    hold on
    k = k + 1;
end
sgrid