function[gc]=lagbode(g,K,PM,GM)
%This function make the phase lag compensation, input is tf,Kp(or Kv,Ka) 
%phase margin and gain margin which expect.(Kc=1) in this func if have no K. 
%gc=lagbode(g,K,PM,GM)
%-------------------------------------------------------|%
%|(C)2005 Bui Trung Hieu				|%
%|Website: www.khvt.com					|%
%|Email: matlab@khvt.com				|%
%-------------------------------------------------------|%

if (nargin==3)
    Kc=1;
%This code to find Kc
elseif (nargin==4)
    Kc=Kc_error(g,K);
end

g1=Kc*g;
[gm1,Pm,wcg1,wcp1]=margin(g1);
clear gm1 wcp1 wcg1

%Phase angle adder
theta=2;
max_ang=30;
%Step 3


while(theta<max_ang)
    phi=(-180+PM+theta);
    
%Find freq value which satisfies relation: Phi1((w'c))=-180+PM+theta    

    [mag,phase,w]=bode(g1);
    clear mag
    phase=phase(:);
    w=w(:);

    t=find((phase>phi-10)&(phase<phi+20));
    [vltemp,degree]=min(abs(phase(t)-phi));
  if (degree>=1)
        clear vltemp phase
        wc=w(min(t)+degree-1);    
        clear w degree t    
        w=abs(wc-0.5):0.001:(0.5+wc);
        w=w(:);        
        [mag,phase]=bode(g1,w);    
        phase=phase(:);    
        [value,degree]=min(abs(phase-phi));
        wc=w(degree);    
        clear value w
%Find anpha
        anpha=1/abs(freqresp(g1,wc));
%Caculate time constant T
        T=ceil(12/(wc*anpha));
        clear wc
%Phase lag compensation
        gc=Kc*tf([anpha*T 1],[T 1]);    
%Step 7
        gkq=g*gc;     
        [gmc,pmc,wgc,wpc]=margin(gkq);    
        gmc=20*log10(gmc);
        if ((gmc>GM)&(pmc>PM))
            clf;
            hold on
            bode(gc);
            bode(g);
            bode(gkq,'r')
            legend('Phase lag compensation','Org Tranfer Function','Compensated Tranfer Function');
            margin(gkq);
            pause();
            clf;        
            if(g.iodelay==0)
                clf;
                hold on;
                step(feedback(gkq,1),feedback(g,1));
                grid on
                legend('Compensated Tranfer Function','Org Tranfer Function');
                pause();
                close();
            end
            break
        else           
            theta=theta+1;
            if (theta==max_ang)
                Result=sprintf('Bai toan vo nghiem')
                gc=nan;
            end
        end
  else 
        clear gc gmc pmc wgc wpc gkq
       theta=theta+1;
       if (theta==max_ang)
           gc=nan;
           Result=sprintf('Bai toan vo nghiem')
       end
  end
end
%End