clear;
close all;

E=10;Vref=15;Vd=0.3;
rs1=0.5;rs2=0.285;
Lr=6e-6;Cr=3.3e-5;Co=3.3e-4;R=50;
fs=1e+4;Ts=1/fs;D=0.0972;
alfa=-rs2/(2*Lr);Wr1=(1/(Lr*Cr)-alfa^2)^0.5;
Wr2=(1/(Lr*Cr))^0.5;
Wr3=((Cr+Co)/(Lr*Cr*Co)-rs1^2/(4*Lr^2))^0.5;Tr=2*pi/Wr3;

n=100;
temp=0;
for i=1:2501
    k=(i-1)/1000+0.5
    Vc=0;Vo=0;
    dx=[0.001;0.002];
    d0=norm(dx);
    e=1/d0*dx;
    le=0;
    for j=1:n
        U1=Vc;U2=Vo;
        
        %狀態1,t=t0-t1
        dn=D-k.*(Vo-Vref);
        if dn<0
            hn=0;
        end
        if dn>pi/(Wr1*Ts)
            hn=pi/(Wr1*Ts);
        end
        if dn>=0&dn<=pi/(Wr1*Ts)
            syms Vo;
            hn=vpa(D-k*(Vo-Vref);
        end
        syms Vc Vo;
        A11=Vc+Vd-E;A12=-alfa/Wr1*A11;
        t1=hn*Ts;
        Vc1=exp(alfa*t1)*(A11*cos(Wr1*t1)+A12*sin(Wr1*t1))+E-Vd;
        iL1=1/(Lr*Wr1)*exp(alfa*t1)*sin(Wr1*t1);
        
        %狀態2，t=t1-t2
        A21=Vc1+2*Vd;A22=1/(Cr*Wr2)*iL1;
        Vc2=A21*cos(atan(A22/A21))+A22*sin(atan(A22/A21))-2*Vd;
        iL2=0;
        
        %狀態3，t=t2-t3
        Vc3=Vc2;
        Vo3=Vo*exp(-Ts/(2*R*Co));
        iL3=0;
        
        %狀態4，t=t3-t4
        A=[-rs1/Lr,-1/Lr,1/Lr;1/Cr,0,0;-1/Co,0,-1/(R*Co)];
        b=[1/Lr;0;0];
        x3=[iL3;Vc3;Vo3];
        x4=expm(0.5*Tr.*A)*x3+(expm(0.5*Tr.*A)-eye(3))*inv(A)*b*(Vd-E);
        
        %狀態5，t=t4-t5
        Vc5=x4(2);
        Vo5=x4(3)*exp(-(Ts-Tr)/(2*R*Co));
        iL5=0;
        
        Vn=[Vc5;Vo5];
        J=jacobian(Vn,[Vc,Vo]);
        Vc=U1;Vo=U2;
        Jq=subs(vpa(J,6));
        vec=Jq*e;
        d=norm(vec);
        e=1/d*vec;
        le=le+1/n*log(d);
        Vc=subs(Vc5);Vo=subs(Vo5);
    end
    lle(i)=le;
    if temp==0&lle(i)>0
        l=k,temp=1;
    end
end
k=0.5:0.001:3;
plot(k,zeros(1,length(k)),'k');
hold on; 
plot(k,lle,'k');