%BP based PID Control
clear all;
clc;
t=cputime;
load best_pop

S=3;    %Signal type

IN=4;H=6;OUT=3; %NN Structure
wi=best_pop{1};
wo=best_pop{2};

y_1=0;
u_1=0;

Ih=zeros(H,1); 
Oh=zeros(H,1); 
Io=zeros(OUT,1); 
Oo=zeros(OUT,1); 

error_2=0;
error_1=0;

for k=1:10000
    time(k)=k;

    if S==1
        rin(k)=1;
    end

    if S==2
        rin(k)=sin(1*2*pi*k*0.001);
    end

    if S==3
       if k<=50
         rin(k)=0.75;
       else
         rin(k)=0.25;
       end
    end

    if S==4
       if k<=50
         rin(k)=1;
       elseif k<=100
         rin(k)=0;
       elseif k<=150
         rin(k)=0.9;
       elseif k<=200
         rin(k)=0;
       elseif k<=250
         rin(k)=0.8;
       elseif k<=300 
         rin(k)=0;
       elseif k<=350
         rin(k)=0.7;
       elseif k<=400 
         rin(k)=0;
       elseif k<=450
         rin(k)=0.6;
       elseif k<=500 
         rin(k)=0;
       elseif k<=550
         rin(k)=0.5;
       elseif k<=600 
         rin(k)=0;
       elseif k<=650
         rin(k)=0.4;
       elseif k<=700 
         rin(k)=0;
       elseif k<=750
         rin(k)=0.3;
       elseif k<=800 
         rin(k)=0;
       elseif k<=850
         rin(k)=0.2;
       elseif k<=900 
         rin(k)=0;
       elseif k<=950
         rin(k)=0.1;
       elseif k<=1000 
         rin(k)=0;
       end
    end  

    a(k)=1;%.2*(1-0.8*exp(-0.1*k));
    yout(k)=a(k)*y_1/(1+y_1^2)+u_1;

    %Restricting the output of controller
    if yout(k)>=2     
      yout(k)=2;
    elseif yout(k)<=-2
      yout(k)=-2;
    end   
    error(k)=rin(k)-yout(k);
    
    xi=[error(k)-error_1,error(k),error(k)-2*error_1+error_2,1];
    Ih=xi*wi';
    for j=1:H
      if Ih(j)>5
          Oh(j)=1;
      elseif  Ih(j)<-5   
          Oh(j)=-1;
      else    
         Oh(j)=(exp(Ih(j))-exp(-Ih(j)))/(exp(Ih(j))+exp(-Ih(j)));  
      end   
    end
    Io=wo*Oh;
    for j=1:OUT
      if Io(j)>5
          Oo(j)=1;
      elseif  Io(j)<-5   
          Oo(j)=0;
      else    
         Oo(j)=exp(Io(j))/(exp(Io(j))+exp(-Io(j))); 
      end
    end
    
    Kpid=[Oo(1),Oo(2),Oo(3)];
    epid=[error(k)-error_1;error(k);error(k)-2*error_1+error_2]; 

    du(k)=Kpid*epid;
    u(k)=u_1+du(k);

    u_1=u(k);
    y_1=yout(k);

    error_2=error_1;
    error_1=error(k);
end

figure(2);
plot(time,rin,'r',time,yout,'b');grid on;
xlabel('time(s)');ylabel('rin(red),yout(blue)');

e=cputime-t