   function [ Ksig,Kmy,Kmu,Knecy,Knecu,Kvini,Ksigy,Ksigu,Hsoc,Hnco,Kcsoc,Hmary,Hmary2,Gd,G ] = getSensitivities(idmeas,idmeasnec)


%     idmeas=[1 2 3];
%    idmeasnec=[1 2 3]
% 


%nominal parameters
k1=0.65;
k2=0.014;
dn=[k1; k2];

%nominal input and measurements
[Jn,un,yn,Hess]=getOptimum(dn);
Hess

cain=2;
cbin=1.5;
V=500;
R=[1 0;0 1];

%% based on perturbation first --- for debug only:
%computing Gu and Gd
delta=0.001;
noise=zeros(5,1);
%  %--compute G
%   [ ~,~,y1] = plant(un+[1;0]*delta,dn,noise);
%   [ ~,~,y2] = plant(un-[1;0]*delta,dn,noise);
%   g1=(y1-y2)/(2*delta);
%
%
%   [ ~,~,y1] = plant(un+[0;1]*delta,dn,noise);
%   [ ~,~,y2] = plant(un-[0;1]*delta,dn,noise);
%   g2=(y1-y2)/(2*delta);
%
%   Gpert=[g1 g2];
%
%   %--Gd
%   [ ~,~,y1] = plant(un,dn+[1;0]*delta,noise);
%   [ ~,~,y2] = plant(un,dn-[1;0]*delta,noise);
%   g1=(y1-y2)/(2*delta);
%
%  [ ~,~,y1] = plant(un,dn+[0;1]*delta,noise);
%   [ ~,~,y2] = plant(un,dn-[0;1]*delta,noise);
%   g2=(y1-y2)/(2*delta);
%
%   Gdpert=[g1 g2];
%

%optimal sensitivity
delta=1e-8
[~,u1,y1,~]=getOptimum(dn+delta*[1;0]);
[~,u2,y2,~]=getOptimum(dn-delta*[1;0]);
f1=(y1-y2)/(2*delta);
fu1=(u1-u2)/(2*delta);

delta=1e-8;
[~,u1,y1,~]=getOptimum(dn+delta*[0;1]);
[~,u2,y2,~]=getOptimum(dn-delta*[0;1]);
f2=(y1-y2)/(2*delta);
fu2=(u1-u2)/(2*delta);
Fpert=[f1 f2];
Fupert=[fu1 fu2];


%gradient:
delta=1e-6;
dn=dn;
[ J1,~,~] = plant(un+[1;0]*delta,dn,noise);
[ J2,~,~] = plant(un-[1;0]*delta,dn,noise);
g1=(J1-J2)/(2*delta);

delta=0.5e-5;
[ J1,~,~] = plant(un+[0;1]*delta,dn,noise);
[ J2,~,~] = plant(un-[0;1]*delta,dn,noise);
g2=(J1-J2)/(2*delta);

gradpert=[g1 g2];


% % % %%Jud
% % Jud=[];
% % 
% % for j=1:2
% %     gradi=[];
% %     for i=1:2
% %         
% %         delta=1e-6;
% %         d=dn;
% %         d(j)=dn(j)*(1+(1-2*rem(1,i))*delta);
% %         
% %         [ J1,~,~] = plant(un+[1;0]*delta,d,noise);
% %         [ J2,~,~] = plant(un-[1;0]*delta,d,noise);
% %         g1=(J1-J2)/(2*delta);
% %         
% %         delta=0.5e-5;
% %         [ J1,~,~] = plant(un+[0;1]*delta,d,noise);
% %         [ J2,~,~] = plant(un-[0;1]*delta,d,noise);
% %         g2=(J1-J2)/(2*delta);
% %         
% %         gradpert=[g1 g2];
% %         
% %         gradi=[gradi ; gradpert];
% %     end
% %     Judj=(gradi(1,:)-gradi(2,:))/(2*delta);
% %     Jud=[Jud; Judj ];
% %     
% % end
% % 
% % Judpert=Jud;



%%  =====================symbolic computations
syms cas cbs ccs
syms Fas Fbs
syms k1s k2s
cas=cas;
cbs=cbs;
ccs=ccs;
Fas=Fas;
Fbs=Fbs;
k1s=k1s;
k2s=k2s;


f1=Fas*cain-(Fas+Fbs)*cas-k1s*cas*cbs*V;
f2=Fbs*cbin-(Fas+Fbs)*cbs-k1s*cas*cbs*V-2*k2s*cbs^2*V;
f3=-(Fas+Fbs)*ccs+k1s*cas*cbs*V;

f=[f1;f2;f3];

%measurement equation
C=eye(3,3);

x=[cas;cbs;ccs];
u=[Fas;Fbs];
d=[k1s ;k2s];
obj=(Fas+Fbs)*ccs*(Fas+Fbs)*ccs/(Fas*cain)-0.5*[Fas Fbs]*R*[Fas;Fbs];
obj=-obj;

Jx=jacobian(obj,x);
Ju=jacobian(obj,u);
Fx=jacobian(f,x);
Fu=jacobian(f,u);
Fd=jacobian(f,d);

Hu=zeros(2);
Hx=eye(3);

%sensitivities
Gd=-Hx*inv(Fx)*Fd;
G=-Hx*inv(Fx)*Fu;
grad=(Ju-Jx*inv(Fx)*Fu);
Jud=jacobian(grad,d);
Juu=jacobian(grad,u);
Hess


%nominal parameters
Fas=un(1);
Fbs=un(2);
cas=yn(1);
cbs=yn(2);
ccs=yn(3);
k1s=k1;
k2s=k2;

%convert to double
Jud=subs(subs(Jud));
Juu=subs(subs(Juu));
Gd=subs(subs(Gd));
G=subs(subs(G));
grad=subs(subs(grad))
gradpert

%  Juu
%  Hess
%% ================ Exact local method=================
%includes u in the measurements ym=[y;u]
nu=2;
ny=3;
nd=2;
Gy=[G  ; eye(nu,nu) ];
Gdy=[Gd;zeros(nd,nu)];

%Optimal sensitivity F=G*Fu+Gd, Fu=-Juu^-1*Jud
Fu=-inv(Hess)*Jud;
F=Gy*Fu+Gdy;

F=[Fpert;Fupert];

%compute H matrix using the exact local method
Juu=full(Hess);


%disturbance magnitude (expected)
Wd=1*diag([0.9 ;   0.02]);


%measurement noise magnitude (expected)
Wny=diag( [zeros(3,1)+0.0051*yn ] );
Wnu=diag([zeros(2,1)+0.0051]);
Wn=blkdiag(Wny,Wnu);

Y=[F*Wd  Wn];
Hexact=((Y*Y')^(-1)*Gy)';
scalefactor=max(max(Hexact));
Hexact=Hexact./scalefactor

%computing loss
Md=-Juu^(0.5)*(Hexact*Gy)^-1*Hexact*F*Wd;
Mn=-Juu^(0.5)*(Hexact*Gy)^-1*Hexact*Wn;
M=[Md Mn];
z=M;
Loss=1/2*max(svd(M))^2
H1=Hexact(:,1:ny);
H2=Hexact(:,ny+1:end);
Hexact=[H1(:,idmeas) H2];



%% Computing null space
idmeas2=[1 2  3];
Hnull=null(F')';
Hnull=Hnull([2 3],[idmeas2  ny+1  ny+2 ])

ganhossoc=Hnull*Gy([idmeas2  ny+1  ny+2 ],:)
ga=ganhossoc.*pinv(ganhossoc).'
Kcsoc=inv(ganhossoc)
 

H1=Hnull(:,1:ny);
% H2=Hnull(:,ny+1:end);
% H2=Hnull()
%1 term sigurd gain 
H1=-H1(:,idmeas2);
Ksig=(H2)\H1;


%using nullspace method instead of exact local method
Hsoc=Hnull(:,[idmeas 4 5] );

%% =========== applying Maryams method===============

%weighting matrices (diagonal containing standard deviations)
Wu=diag([0.0017  0.0025]);

% dn+50%dn lies inside the 95% probability region
Wd=diag(dn*0.5/2);

%measurement noise magnitude (expected)
Wny=diag( [zeros(3,1)+0.0051*yn ] );
Wnu=diag([zeros(2,1)+0.0051*un]);
Wn=blkdiag(Wny,Wnu);


Gx=Gy;
Gdx=Gdy;

Gdy=eye(2);
Guy=zeros(2);

Y1=[Guy*Wu Gdy*Wd zeros(2,5)];
X1=[Gx*Wu Gdx*Wd Wn];
% X1inv=inv(X1'*X1)*X1';
Hmary=Y1*pinv(X1)


%computing Hmary assuming input disturbance (instead of measurement noise on the input)
Y2=[zeros(2,2) Wd  zeros(2,5)];
X2=[Gx*Wu  Gdx*Wd  [Wny;zeros(2,3)]   [G*Wnu;zeros(2,2)]];
Hmary2=Y2*pinv(X2);

% H1=Hmary(:,1:ny);
H1=Hmary2(:,idmeasnec);
H2=Hmary2(:,ny+1:end);

% H1=pinv(Gd(idmeasnec,:));
% H2=pinv(Gd(idmeasnec,:))*G(idmeasnec,:);
Kmy=Fupert*H1;
Kmu=-Fupert*H2;

%% %========================NEC gains======================
Knecy=Fupert*pinv(Gd(idmeasnec,:))
Knecu=-Fupert*pinv(Gd(idmeasnec,:))*G(idmeasnec,:)
Kvini=Fupert*pinv(Fpert(idmeas,:));

Ksig
%Gains 2 terms sigurd
I=eye(2);
Ksigy=(I-Knecu)*Ksig;
Ksigu=Knecu;


%%NCO tracking gains
Hnco=[Jud*pinv(Gd(idmeasnec,:))   Juu-Jud*pinv(Gd(idmeasnec,:))*G(idmeasnec,:)];

Juu
Jud

