clear all
clc
%nominal k1 and k2
dn=[0.65 0.014];

%nominal optimal
[Jn,un,yn,hess]=getOptimum(dn);

%measurement indices
idmeassig=[1 2 3 ];
idmeasnec=[1 2 3];


%get gains for various methods
[Ksig,Kmy,Kmu,Knecy,Knecu,Kvini,Ksigy,Ksigu,Hsoc,Hnco,Kcsoc,Hmary,Hmary2,Gd,G]=getSensitivities_v2(idmeassig,idmeasnec);


%% Run simulation


for n=[0]

for kiter=4

close all
clear J1termvec J2termsvec Jmaryvec Jnecvec Jnomvec Joptvec Jsocvec Jncovec

%inital values
unec=un;
umary=un;
usig1=un;
usig2=un;
usoc=un;
unco=un;

%PI tuning parameters for SOC
%  Kc=1.*[1/500  0;0 1/300];
Kc=1*Kcsoc;
% Kc=0.5*[1/0.8  0;0 1/-0.4];
err=[0;0];
cs=Hsoc*[yn(idmeassig);un];


%NCO tracking
Knco=-0.5*inv(hess);

%define disturbances and noise
%autoregressive white noise model param
noisesize=0.0051*n;
noisesizeu=0.0051*0;
noiseyprev=[0;0;0];
dnoiseyprev=[0;0;0];
b1=0.5;
a1=0.5;
driftnoise=0.01*yn*0;

switch kiter
    case 1
    %d1
            d=dn*1.2;
            dstr='d1';
    case 2
            %d2
            d=dn*0.8;
            dstr='d2';
    case 3
            %d3
            d(1)=dn(1)*1.2;
            d(2)=dn(2)*0.8;
            dstr='d3';
    case 4
        %d4
        d(1)=dn(1)*0.8;
        d(2)=dn(2)*1.2;
        dstr='d4';

           
    case 5
        d(1)=0.3;
        d(2)=0.001;
        dstr='d5';
     case 6
        d(1)=3;
        d(2)=0.1;
        dstr='d6';
        
     case 7
        d(1)=0.3;
        d(2)=0.1;
        dstr='d7';
     case 8
        d(1)=3;
        d(2)=0.001;
        dstr='d8';
        
     case 9
        %d4
        d(1)=dn(1)*1.5;
        d(2)=dn(2)*1.5;
        dstr='d9';
        
     case 10
        %d4
        d(1)=dn(1)*0.5;
        d(2)=dn(2)*0.5;
        dstr='d10';
        
        case 11
        %d4
        d(1)=dn(1)*1.5;
        d(2)=dn(2)*0.5;
        dstr='d11';
        
        case 12
        %d4
        d(1)=dn(1)*0.5;
        d(2)=dn(2)*1.5;
        dstr='d12';
        
    otherwise
        error('not defined')
end
%  d(1)=dn(1)*1.5;
%    d=[3  0.1];
%input disturbance (unmodelled)
inputdist=un*0.0;

%optimal solution for the perturbed system 
[Jopt,uopt,yopt]=getOptimum(d);

Nite=1000;

%noise-to-signal ratio of the cost variation
thetad=[-0.0166 0.1812;0.1812 -2.1059];
thetan=[-0.1441 0.0119;0.0119 10.2615];
deltad=d-dn;
E=[];


noisevec=[];
dnoisevec=[];

ynomvec=[];
ynecvec=[];
ymaryvec=[];

errvec=[];
errvec2=[];
errvec_nec=[];

for k=1:Nite
    
    %generate normal distribution noise
     dnoisey=noisesize*yn.*randn(3,1);  
     
     %autoregressive white noise 
     noisey=dnoisey+b1*dnoiseyprev+a1*noiseyprev+driftnoise;
     dnoiseyprev=dnoisey;
     noiseyprev=noisey;
     noisevec=[noisevec noisey];
     dnoisevec=[dnoisevec dnoisey];
     
     
     %input disturbance
     noiseu=noisesizeu*un.*randn(2,1)+inputdist;
     %measurement noise on the input
     noise_mu=noiseu;
     noise=[noisey;noiseu];
     
    
     %nominal and optimal inputs
     [Jnomvec(k),uk,yk] = plant(un,d,noise);    
     [Joptvec(k),~,~] = plant(uopt,d,noise*0);
     
     %2-terms sigurd
     [J2termsvec(k),uk,yk] = plant(usig2(:,k),d,noise);
     usig2(:,k+1)=un+Ksigy*(yk(idmeassig)-yn(idmeassig))+Ksigu*(usig2(:,k)-un);
          
     %1-term sigurd
     [J1termvec(k),uk,yk] = plant(usig1(:,k),d,noise);
     usig1(:,k+1)=un+Ksig*(yk(idmeassig)-yn(idmeassig));
     
      %Maryam estimator
      [Jmaryvec(k),uk,yk] = plant(umary(:,k),d,noise);       
      umary(:,k+1)=un+Kmy*(yk(idmeasnec)-yn(idmeasnec))+Kmu*(umary(:,k)-un);

      ymaryvec=[ ymaryvec yk];

%      ddest=Hmary*[yk(idmeasnec)-yn;uk-un];
%      ddest2=Hmary2*[yk(idmeasnec)-yn;uk-un];
%      derr1=norm(d'-dn'-ddest);
%      derr2=norm(d'-dn'-ddest2);
      
      
     %NEC 
     [Jnecvec(k),uk,yknec] = plant(unec(:,k),d,noise);
     ynecvec=[ynecvec yknec];
     Hnec=[pinv(Gd)  -pinv(Gd)*G];
%      ddest_nec=Hnec*[yk(idmeasnec)-yn;uk-un];
%      derr_nec=norm(d'-dn'-ddest_nec);  
     unec(:,k+1)=un+Knecy*(yknec(idmeasnec)-yn(idmeasnec))+Knecu*(unec(:,k)-un);
          
     
     %SOC
     [Jsocvec(k),uk,yk] = plant(usoc(:,k),d,noise);
     c=Hsoc*([yk(idmeassig);uk]);
     err(:,k)=cs-c;      
     usoc(:,k+1)=usoc(:,k)+Kc*err(:,k);
     errprev=err;
     
     
     %NCO
     [Jncovec(k),uk,yk] = plant(unco(:,k),d,noise);
     gk(:,k)=Hnco*([yk(idmeasnec)-yn(idmeasnec);uk-un]);
     unco(:,k+1)=unco(:,k)+Knco*gk(:,k);
     
     
     
%       errvec=[errvec;derr1];
%       errvec2=[errvec2;derr2];
%       errvec_nec=[errvec_nec;derr_nec];
     
end
filename=strcat('new_everything_noise_n_',num2str(n),dstr)
save(filename)


%%
clc
%       load('everything_small_noised3')
%         load('new_everything_noise_n_4d9')
    load(filename)

% 'marry 1 (meas noise)'
% mean(errvec)
% 'marry 2 (input dist)'
% mean(errvec2)
% 'Gd inverse'
% mean(errvec_nec)

% figure(3)
% subplot(211)
% plot(noisevec')
% title('noise')
% subplot(212)
% plot(dnoisevec')
% title('noise derivative')

%noise-to-signal ratio
 rho=abs(mean(E)/(deltad*thetad*deltad'))
% 
%  figure(2)
%  subplot(211)
%  plot(err')
%  ylabel('c=Hy')
%  subplot(212)

% ylabel('gradient estimate gk')
% 
% print -depsc grad_estimate

%========plot responses=============
itervec=1:Nite;

%worst case loss
% Jnomworstloss=max((Joptvec-Jnomvec)./Joptvec)*100
%

Jnomavg=mean(Jnomvec);
Joptavg=mean(Joptvec);
Jnecavg=mean(Jnecvec);
Jmaryavg=mean(Jmaryvec);

%Normalize cost with respect to the optimal
Jnomvec=Jnomvec./Joptvec;
Jnecvec=Jnecvec./Joptvec;
Jmaryvec=Jmaryvec./Joptvec;
J1termvec=J1termvec./Joptvec;
J2termsvec=J2termsvec./Joptvec;
Jsocvec=Jsocvec./Joptvec;
Jncovec=Jncovec./Joptvec;
Joptvec=Joptvec./Joptvec;
 



 
Lossavg_nom=sum(1-Jnomvec)
Lossavg_nec=sum(1-Jnecvec)
Lossavg_mary=sum(1-Jmaryvec)

aux.lossnom=Lossavg_nom;
aux.lossnec=Lossavg_nec;
aux.lossmary=Lossavg_mary;
structname=strcat('struct',filename);
save(structname,'aux')

% Lossavg_Sig1term=sum(1-J1termvec)
% Lossavg_Sig2terms=sum(1-J2termsvec)
% Lossavg_soc=sum(1-Jsocvec)
% Lossavg_nco=sum(1-Jncovec)
% close all
% 
% 
ynm=repmat(yn,1,1000);
figure(1)
subplot(311)
plot(itervec,ymaryvec(1:2,:),'g','LineWidth',2)
hold on
plot(itervec,ynecvec(1:2,:),'r','LineWidth',2)
hold on
set(gca,'FontSize',16)
% xlabel('Iteration')
ylabel('$c_A$ $and$ $c_B, [mol,l]$','interpreter','latex')
 xlim([0 100])
 ylim([0.05 0.075])
% plot(itervec,J1termvec,'b--')
% hold on
% plot(itervec,J2termsvec,'b')
% hold on
% plot(itervec,Jmaryvec,'g')
% hold on
% plot(itervec,Jnecvec,'r')
% % hold on
% % plot(itervec,Jsocvec,'b-*')
% hold on
% plot(itervec,Jncovec,'r--')
% 
% set(gca,'FontSize',16)
% 
% xlabel('Iteration')
% ylabel('Normalized cost')
% % legend('Open loop','Optimal','1 term sig','2 terms sig','Mary 2 terms','NEC','SOC','NCO')
% 
 subplot(312)
usig1v=usig1(:,1:end-1);
usig2v=usig2(:,1:end-1);
umaryv=umary(:,1:end-1);
unecv=unec(:,1:end-1);
unomvec=repmat(un,1,Nite);
uoptvec=repmat(uopt,1,Nite);
usocv=usoc(:,1:end-1);
uncov=unco(:,1:end-1);
% 
plot(itervec,unomvec,'k--','LineWidth',2)
hold on
plot(itervec,uoptvec,'b-.','LineWidth',2)
 hold on
% plot(itervec,usig1v,'b--')
% hold on
% plot(itervec,usig2v,'b')
% hold on
plot(itervec,umaryv,'g','LineWidth',2)
hold on
plot(itervec,unecv,'r','LineWidth',2)
hold on
% plot(itervec,usocv,'b-*')
% hold on
% plot(itervec,uncov,'r--')
hold on
set(gca,'FontSize',16)
% xlabel('Iteration')
ylabel('$u, [l/min]$','interpreter','latex')
ylim([0.5 0.82 ])
 xlim([0 100])


subplot(313)
plot([0 1000],[1 1],'b-.','LineWidth',2)
hold on
plot(itervec,Jmaryvec,'g','LineWidth',2)
hold on
plot(itervec,Jnecvec,'r','LineWidth',2)
hold on
plot(itervec,Jnomvec,'k--','LineWidth',2)
set(gca,'FontSize',16)
xlabel('Iteration \#','interpreter','latex')
ylabel('$Normalized$ $J$','interpreter','latex')
% set(gca,'YTick',[0.998771795635371 1.000])

 xlim([0 100])
 ylim([0.998 1.0006])
% tilefigs
% figure(1)
% figurefilename=strcat('performance_compare_manyruns_',dstr);
 print -depsc zero_noise_case4

end


end