clear all;
clc
randn('seed',1); 
rand('seed',1);
dt = 0.01; %sampling time [s]
t0 = 0; %starting time
t1 = 20; %ending time [s]
T = (0:dt:t1); %time vector
N = length(T); % no. samples

% input experiment
Uexp=(prbs1(N,1,60)*pi/18)-0;  

%dv=0.001; dw=0.001; 
dv=0; dw=0; 
% storing matrices
X=zeros(N,6); Y=zeros(N,2); U=zeros(N,2);
R=zeros(N,1);
v=randn(N,6)*dv; w=randn(N,1)*dw;  %No noise, gives perfekt ID model
 
% settings for mfdelta pi pid tuning
L=5; J=5; n=2; delta=2.6; zeta=1;
%wc=0.4;
m=2;

for i=1:2
    i
    randn('seed',1); 
    rand('seed',1);v=randn(N,5)*dv; w=randn(N,1)*dw;  %No noise, gives perfekt ID model
    vu=0;bprbs=1;
    x=zeros(6,1); x(1)=1; y_old=0;
    z=0; r=pi/180*4; u=0.2;
for k = 1:N
    k
    tk = T(k);
    y=x(2);
    if(bprbs==0)
        if k<900
            u=0;
        else
            e=r-y;
            u=Kp*e-KpTdh*(y-y_old)+z;
            z=z+KpTih*e;
        end
	elseif(bprbs==1)
        if k<200
            u=0;
        elseif k<600
            u=Uexp(k); 
        else 
            Yid=Y(200:end,i); Uid=U(200:end,i);
        if i==1
            [Kp,Ti,Td]=pidtun(Yid,Uid,dt,m);z=-Kp*r;
        elseif i==2
            [Kp,Ti,Td]=pidTuner1(Yid,Uid,dt,m);z=-Kp*r;
        end
        KpTdh=Kp*Td/dt;
        KpTih=Kp*dt/Ti; 
        bprbs=0;
        end
    end
    % store last value
    y_old=y; 
    u_old=u;  
	if tk>14
        u=u+pi/180*30;
    end
    % store data 
    Y(k,i)=y; U(k,i)=u; R(k)=r;
    % discretise the model
    [Time,sol] = ode45(@miss,[tk tk+dt],x',[],u);
    % update model with last element
	x = sol(end,:)'+v(k)';
end
end
figure(1)
subplot(211),plot(1:N,Y(:,1)*180/pi,'r',1:N,Y(:,2)*180/pi,'b--',1:N,R*180/pi,'k--'),title('$y$: Angle of attack [deg]','Interpreter','latex'),ylabel('$y$','Interpreter','latex'),grid,t=legend('pidtun','pidTuner1','reference');set(t,'Interpreter','latex'),xlim([0,N])
subplot(212),plot(1:N,U(:,1)*180/pi,'r',1:N,U(:,2)*180/pi,'b--'),title('$u$: Rudder deflection [deg]','Interpreter','latex'),grid,ylabel('$u$','Interpreter','latex'),xlabel('Time, [samples]','Interpreter','latex'),xlim([0,N])

str1 = 'PRBS start';
text(2.2,30,str1,'Interpreter','latex')
annotation('arrow','X',[0.207142857142857 0.207142857142857],'Y',[0.403174603174604 0.303174603174604]);

str2 = 'PRBS stop';
text(6.2,30,str2,'Interpreter','latex')
annotation('arrow','X',[0.361904761904761 0.361904761904761],'Y',[0.404761904761906 0.304761904761906]);

str3 = 'Control on';
text(9.2,23,str3,'Interpreter','latex')
annotation('arrow','X',[0.479761904761903 0.479761904761903],'Y',[0.406349206349211 0.306349206349211]);

a=900; % O step at 0.5 sec
b=1400; % input step at 3.5 sec

disp('pidtun')
IAE_vy_d=trapz(T(a:b), abs(R(a:b)-Y(a:b,1)))
IAE_vu_d=trapz(T(b:end), abs(R(b:end)-Y(b:end,1)))
%IAE_d=IAE_vy_d+IAE_vu_d
TV_vy_d=sum(abs(diff(U(a:b,1))))
TV_vu_d=sum(abs(diff(U(b:end,1))))
disp('pidTuner1')
IAE_vy_s=trapz(T(a:b), abs(R(a:b)-Y(a:b,2)))
IAE_vu_s=trapz(T(b:end), abs(R(b:end)-Y(b:end,2)))
%IAE_s=IAE_vy_s+IAE_vu_s
TV_vy_s=sum(abs(diff(U(a:b,2))))
TV_vu_s=sum(abs(diff(U(b:end,2))))