clear all;
close all;

b=0.7;
x1=[0,0,0,0,0]';
x2=[0,0,0,0,0]';

c1=zeros(5,3000);c2=zeros(5,3000);
q1=0;q2=0;


u1_1=0;u1_2=0;u2_1=0;u2_2=0;y1_1=0;y2_1=0;
xc1=[0,0,0]';xc2=[0,0,0]';
error1_1=0;
error1_2=0;
error1=0;
error2_1=0;
error2_2=0;
error2=0;

kp1=0.02;
ki1=0.03;
kd1=0.005;
kp1_1=kp1;
kd1_1=kd1;
ki1_1=ki1;
kp2=0.02;
ki2=0.03;
kd2=0.005;
kp2_1=kp2;
kd2_1=kd2;
ki2_1=ki2;

xitekp=0.25
xitekd=0.25;
xiteki=0.25;

ts=0.001;
for k=1:1:2000
   time(k)=k*ts;
   r1(k)=1.0*sign(sin(2*pi*k*ts));
   r2(k)=-1.0*sign(sin(2*pi*k*ts));
  
   a(k)=1.2*(1-0.8*exp(-0.1*k));
   y1(k)=a(k)*y1_1/(1+y1_1^2)+u1_1+0.2*u2_2;
   y2(k)=a(k)*y2_1/(1+y2_1^2)+0.8*u2_1+0.7*u1_2;
 
   if k==1
       q1=q1+1; q2=q2+1;        % Category number q 
       c1(:,q1)=x1;c2(:,q2)=x2;
       s1(q1)=y1(k);s2(q2)=y2(k);
       ct1(q1)=1;ct2(q2)=1; 
       w1(q1)=s1(q1)/ct1(q1);w2(q2)=s2(q2)/ct2(q2);
   else
       min1=9999;min2=9999;
         for j=1:1:q1
               a1=norm(x1-c1(:,j));
                 if a1<min1
                    min1=a1;
                    v1=j;
                 end
         end
         for j=1:1:q2
               a2=norm(x2-c2(:,j));
                 if a2<min2
                    min2=a2;
                    v2=j;
                 end
         end
           if min1>b
               q1=q1+1;
               c1(:,q1)=x1;
               s1(q1)=y1(k);
               ct1(q1)=1; 
               w1(q1)=s1(q1)/ct1(q1);
           else
               s1(v1)=s1(v1)+y1(k);
               ct1(v1)=ct1(v1)+1;
               w1(v1)=s1(v1)/ct1(v1);
           end
           if min2>b
               q2=q2+1;
               c2(:,q2)=x2;
               s2(q2)=y2(k);
               ct2(q2)=1; 
               w2(q2)=s2(q2)/ct2(q2);
           else
               s2(v2)=s2(v2)+y2(k);
               ct2(v2)=ct2(v2)+1;
               w2(v2)=s2(v2)/ct2(v2);
           end
    end
    m1=0;n1=0;t1=0;p1=0;
    for i=1:1:q1
        m1=m1+exp(-norm(x1-c1(:,i))^2/b^2);
        n1=n1+w1(i)*exp(-norm(x1-c1(:,i))^2/b^2);
        t1=t1+w1(i)*exp(-norm(x1-c1(:,i))^2/b^2)*c1(1,i);
        p1=p1+exp(-norm(x1-c1(:,i))^2/b^2)*c1(1,i);     
    end
    ym1(k)=n1/m1;
    m2=0;n2=0;t2=0;p2=0;
    for i=1:1:q2
        m2=m2+exp(-norm(x2-c2(:,i))^2/b^2);
        n2=n2+w2(i)*exp(-norm(x2-c2(:,i))^2/b^2);
        t2=t2+w2(i)*exp(-norm(x2-c2(:,i))^2/b^2)*c2(1,i);
        p2=p2+exp(-norm(x2-c2(:,i))^2/b^2)*c2(1,i);     
    end
    ym2(k)=n2/m2;
   
   %Jacobian&
   dy1(k)=2*(m1*t1-n1*p1)/m1^2*b^2;
   dy2(k)=2*(m2*t2-n2*p2)/m2^2*b^2;
   
   error1(k)=r1(k)-y1(k);
   error2(k)=r2(k)-y2(k);
   
   kp1(k)=kp1_1+xitekp*error1(k)*dy1(k)*xc1(1);
   kd1(k)=kd1_1+xitekd*error1(k)*dy1(k)*xc1(2);
   ki1(k)=ki1_1+xiteki*error1(k)*dy1(k)*xc1(3);
   kp2(k)=kp2_1+xitekp*error2(k)*dy2(k)*xc2(1);
   kd2(k)=kd2_1+xitekd*error2(k)*dy2(k)*xc2(2);
   ki2(k)=ki2_1+xiteki*error2(k)*dy2(k)*xc2(3);
   
   if kp1(k)<0
       kp1(k)=0;
   end
   if kd1(k)<0
       kd1(k)=0;
   end
   if ki1(k)<0
       ki1(k)=0;
   end
   if kp2(k)<0
       kp2(k)=0;
   end
   if kd2(k)<0
       kd2(k)=0;
   end
   if ki2(k)<0
       ki2(k)=0;
   end
   
   u1(k)=u1_1+kp1(k)*xc1(1)+kd1(k)*xc1(2)+ki1(k)*xc1(3);
   u2(k)=u2_1+kp2(k)*xc2(1)+kd2(k)*xc2(2)+ki2(k)*xc2(3)
   
   if u1(k)>=10
       u1(k)=10;
   end
   if u1(k)<=-10
       u1(k)=-10;
   end
   if u2(k)>=10
       u2(k)=10;
   end
   if u2(k)<=-10
       u2(k)=-10;
   end
   
   u1_2=u1_1;u2_2=u2_1;
   u1_1=u1(k);u2_1=u2(k);
   y1_1=y1(k);y2_1=y2(k);
   
   x1(1)=u1_1;x1(2)=u1_2;x1(3)=u2_1;x1(4)=u2_2;x1(5)=y1_1;  
   x2(1)=u2_1;x2(2)=u2_2;x2(3)=u1_1;x1(4)=u1_2;x1(5)=y2_1;  
  
   xc1(1)=error1(k)-error1_1;
   xc1(2)=error1(k)-2*error1_1+error1_2;
   xc1(3)=error1(k);
   xc2(1)=error2(k)-error2_1;
   xc2(2)=error2(k)-2*error2_1+error2_2;
   xc2(3)=error2(k);
   
   error1_2=error1_1;
   error1_1=error1(k);
   error2_2=error2_1;
   error2_1=error2(k);
   
   kp1_1=kp1(k);
   kd1_1=kd1(k);
   ki1_1=ki1(k);
   kp2_1=kp2(k);
   kd2_1=kd2(k);
   ki2_1=ki2(k);
end

figure(1);
subplot(211);
plot(time,r1,time,y1);
xlabel('time(s)'); ylabel('輸入 r1  , 實際輸出 y1');
axis([0 3 -1.5 1.5]);
legend('r1','y1');
subplot(212);
plot(time,r2,time,y2);
xlabel('time(s)'); ylabel('輸入 r2  , 實際輸出 y2');
axis([0 3 -1.5 1.5]);
legend('r2','y2');
figure(2);
subplot(211);
plot(time,y1,time,ym1);
xlabel('time(s)'); ylabel('實際輸出 y1 , 辨識輸出 ym1');
axis([0 3 -1.5 1.5]);
legend('r1','ym1');
subplot(212);
plot(time,y2,time,ym2);
xlabel('time(s)'); ylabel('實際輸出 y2 , 辨識輸出 ym2');
axis([0 3 -1.5 1.5]);
legend('y2','ym2');
figure(3);
subplot(211);
plot(time,dy1);
xlabel('time(s)'); ylabel('dy1');
subplot(212);
plot(time,dy2);
xlabel('time(s)'); ylabel('dy2');
axis([0 3 -1.5 1.5]);
figure(4);
subplot(311);
plot(time,kp1);
xlabel('time(s)'); ylabel('kp1');
subplot(312);
plot(time,ki1);
xlabel('time(s)'); ylabel('ki1');
subplot(313);
plot(time,kd1);
xlabel('time(s)'); ylabel('kd1');
figure(5);
subplot(311);
plot(time,kp2);
xlabel('time(s)'); ylabel('kp2');
subplot(312);
plot(time,ki2);
xlabel('time(s)'); ylabel('ki2');
subplot(313);
plot(time,kd2);
xlabel('time(s)'); ylabel('kd2');