%% Calculation of SRC

%This system of equations  solves for Kc and each of the slopes alpha_i




syms a1 a2 a3  Kc dv1 dv2 dv3 
%ai refers to the slopes
%transfer functions
% 1 = AC
% 2 = HW
% 3 = EH

v1max   = ACmax;
v2max   = HWmax;
v3max   = EHmax;


%Kcp = [ -1.3333 0.8667 1.3333];
Kcp = [ Kc_ac  Kc_hw  Kc_eh]; 
eq1 = a1*Kc-Kcp(1);eq2 = a2*Kc-Kcp(2);eq3 = a3*Kc-Kcp(3);
eq5 = a1-v1max/dv1;eq6 = a2-v2max/dv2; eq7 = a3-v3max/dv3;
eq9 = abs(dv1)+abs(dv2)+abs(dv3)-1;

eq = [eq1;eq2;eq3;eq5;eq6;eq7;eq9];


var = [a1;a2;a3;Kc;dv1;dv2;dv3];

S=solve(eq,var);

a       = double([S.a1; S.a2; S.a3]); % slopes
Kc_SRC  = abs(double(S.Kc));                    % controller gain
dv      = double([abs(S.dv1); abs(S.dv2); abs(S.dv3)]); % delta v


alphaAC = -abs(a(1));
alphaHW = abs(a(2));
alphaEH = abs(a(3));

u0AC = 0; %v1max;
u0HW = 0; %-dv(1)*(a(2));
u0EH = -abs((dv(2))*(alphaEH)); %-(dv(1)+dv(2))*tan(a(3));


tauISRC =min([tauI_hw,tauI_eh, tauI_ac]);

KI_SRC=Kc_SRC/tauISRC;
% % %Calculate delta u
% % dub=ones(MVs,1);
% % usum=0;
% % for i=1:MVs
% %  dub(i)=vmax/abs(a2(i));
% %  usum=usum+dub(i);
% % end
% 
% % Crossing points at zero --------
% %for the first two - negative gain
% c0=ones(MVs,1);
% c0(1)=du(1);
% c0(2)=du(1)+du(2);
% %then the gain becomes positive
% c0(3)=c0(2);
% c0(4)=c0(3)+du(3);
% 
% %Find bias
% uio=ones(MVs,1);
% uio(1)=vmax;
% uio(2)=vmax-c0(1)*a(2);
% uio(3)=-c0(2)*a(3);
% %uio(4)=-c100(3)*a(4);
% uio(4)=vmax-a(4);
% 
% %Crossing point at 100
% c100=ones(MVs,1);
% for i=1:MVs
%  c100(i)=(vmax-uio(i))/a(i);
% end
% 
% save('SRCvalues_Kc_alphaV3')
% 
% 
% %Generate v for each MV
% 
% v=ones(length(u),MVs);
% for i=1:MVs
%  v(:,i)=a(i)*u+uio(i);
% end
% 
% %u at v=50
% ulabel=ones(MVs,1);
% vlabel=50*ones(MVs,1);
% for i=1:MVs
%  ulabel(i)=(50-uio(i))/a(i)+5;
% end
% 
% split0=[c100(1), c100(2), c0(2), c100(3), c100(4)];
% split=split0-split0(3)*ones(1,length(split0)); %shifted so that the middle value is when CW and HW meet
% save('SRCvalues_Kc_alpha_delay')
% %% Plot 
% 
% %This is to draw the slopes and label them
% %u at v=50
% ulabel=ones(MVs,1);
% slopea=ones(MVs,1);
% slopeb=ones(MVs,1);
% slopec=ones(MVs,1);
% delta=3;
% 
% height=50;
% heightslope=height+5.5;
% vlabel=height*ones(MVs,1);
% for i=1:MVs
%  ulabel(i)=(height-uio(i))/a(i);
%  slopea(i)=ulabel(i);
%  slopec(i)=(heightslope-uio(i))/a(i);
% end
% 
% ulabel(1)=ulabel(1)-5;
% ulabel(2)=ulabel(2)-5;
% ulabel(3)=ulabel(3)+2;
% ulabel(4)=ulabel(4)+2;
% 
% slopeb(1)=ulabel(1)+delta;
% slopeb(2)=ulabel(2)+delta;
% slopeb(3)=ulabel(3)-delta;
% slopeb(4)=ulabel(4)-delta;
% 
% slope1=ones(MVs,2);
% slope2=slope1;
% slopey1=ones(MVs,2);
% slopey2=ones(MVs,2);
% for i=1:MVs  %These are the x,y values for the figure of the slope
%  slope1(i,1)=slopea(i);
%  slope1(i,2)=slopec(i);
%  slope2(i,1)=slopec(i);
%  slope2(i,2)=slopec(i);
%  slopey1(i,1)=height; 
%  slopey1(i,2)=height;
%  slopey2(i,1)=height;
%  slopey2(i,2)=heightslope;
% end
% 
% 
% 
% 
% set(0,'DefaultTextFontName','Times',...
% 'DefaultTextFontSize',24,...
% 'DefaultAxesFontName','Times',...
% 'DefaultAxesFontSize',24,...
% 'DefaultLineLineWidth',3,...
% 'DefaultLineMarkerSize',12)
% 
% ymin=0;
% ymax=1;
% Linewidth_light=1.5;
% Linewidth=3;
% 
% figure
% plot(u,v(:,1),'Color',[0.3010    0.7450    0.9330],'LineWidth',Linewidth)
% hold on
% plot(u,v(:,2),'Color',[0    0.4470    0.7410],'LineWidth',Linewidth)
% plot(u,v(:,3),'Color', [0.9290    0.6940    0.1250],'LineWidth',Linewidth)
% plot(u,v(:,4),'r','LineWidth',Linewidth)
% %plot(u,100*ones(length(u),MVs),'k--')
% %plot(u,100*zeros(length(u),MVs),'k--')
% plot(ones(length(u),MVs)*c0(1),linspace(ymin,ymax),'Color',[0.3010    0.7450    0.9330], 'LineStyle',':','LineWidth',Linewidth_light) %crossing at zero for first line %grey [169/255,169/255,169/255]
% plot(ones(length(u),MVs)*c0(2),linspace(ymin,ymax),'Color',[0    0.4470    0.7410], 'LineStyle',':','LineWidth',Linewidth_light)
% plot(ones(length(u),MVs)*c100(3),linspace(ymin,ymax),'Color',[0.9290    0.6940    0.1250], 'LineStyle',':','LineWidth',Linewidth_light) %crossing at 100 for the third line
% plot(ones(length(u),MVs)*c100(4),linspace(ymin,ymax),'Color',[169/255,169/255,169/255], 'LineStyle',':','LineWidth',Linewidth_light) %crossing at 100 for the third line
% legend('boxoff')
% for i=1:MVs  %These are the values in u for height
%     plot(slope1(i,:),slopey1(i,:),'k');
%     plot(slope2(i,:),slopey2(i,:),'k');
% end
% legend('AC','CW','HW','EH','Location','SE');
% xlabel('Control action, $u$')
% ylabel('Valve opening, $v_i$')
% ylim([ymin ymax])
% xlim([0 umax])
% %labels for Kuv
% str={'K_{u,AC}','K_{u,CW}','K_{u,HW}', 'K_{u,EH}'};
% 
% text(ulabel,vlabel,str)
% 
% 
% % %Bias
% % uio=ones(MVs,1);
% % uio(1)=ymax;
% % uio(2)=ymax-c0(1)*Kuv(2);
% % uio(3)=-c0(2)*Kuv(3);
% % %uio(4)=-c100(3)*Kuv(4);
% % uio(4)=100-Kuv(4)*100;
% % 
% % %Crossing point at zero 
% % c0=ones(MVs,1);
% % for i=1:MVs
% %  c0(i)=-uio(i)/Kuv(i);
% % end


