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

% Input experiment
Uexp=(prbs1(N,1,60)*pi/18)+pi/180*2;  

%dv=0.001; dw=0.001;
dv=0; dw=0; 

% storing matrices
X=zeros(N,5); Y=zeros(N,2); U=zeros(N,2);
R=zeros(N,1);
v=randn(N,5)*dv; w=randn(N,1)*dw;  %No noise, gives perfekt ID model
 
% settings for pidtun tuning
L=5;m=2;
% Loop
%Nn=5;

for i=1:2
	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;y_old=0; y1old=0;
    r=pi/180*-4; u=0; 
    x=zeros(5,1);
    z=0;
for k = 1:N
    k
    tk = T(k);
    y=x(5)+w(k);   
	if(bprbs==0)
        if k<1200
            u=0;
        else
            e=r-y;
            u=Kp*e+z-KpTdh*(y-y_old);
            z=z+KpTih*e;       
        end
        
	elseif(bprbs==1)
        if k<400
            u=0;
        elseif k<900
            u=Uexp(k); 
        else 
            Yid=Y(400:end,i); Uid=U(400:end,i);
        if i==1
           [Kp,Ti,Td]=pidtun(Yid,Uid,dt,m);z=-Kp*r-0.10;
        elseif i==2
            [Kp,Ti,Td]=pidTuner1(Yid,Uid,dt,m);z=-Kp*r-0.03;
        end
        KpTdh=Kp*Td/dt;
        KpTih=Kp*dt/Ti; 
        bprbs=0; 
        end
	end
    % store last value
    y_old=y; 
    u_old=u;
    
   if tk>18
        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(@DSRV,[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$: Pitch angle [deg]','Interpreter','latex'),ylabel('$y$','Interpreter','latex'),grid,t=legend('pidtun','pidTuner1','reference');set(t,'Interpreter','latex'),ylim([-7,3]),xlim([1,N])
subplot(212),plot(1:N,U(:,1)*180/pi,'r',1:N,U(:,2)*180/pi,'b--'),title('$u$: Stern plane [deg]','Interpreter','latex'),grid,ylabel('$u$','Interpreter','latex'),xlabel('Time, [samples]','Interpreter','latex'),ylim([-25,30]),xlim([1,N])
 
str1 = 'PRBS start';
text(4,30,str1,'Interpreter','latex')
annotation('arrow','X',[0.207142857142857 0.207142857142857],'Y',[0.403174603174604 0.303174603174604]);

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

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

a=1200; % O step at 0.5 sec
b=1800; % 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))))