%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%a
% Simulate the combined system with NCO tracking in the RTO 
% layer and SOC in the lower layer.
% The unexpected disturbance is set in the file "d3.m" in 
% the parent directory.
% Please run runSimpleCSTR in the parent directory first
% 
%
% Johannes Jaeschke
% 2010
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all; close all; clc;

%% Define Parameters
par.F	= 1;		% holdup/min
par.CAin = 1;		% mol/l
par.CBin = 0;		% mol/l
par.Hrx = -5000; 	% kg/mol
par.rho = 1;		% kg/l
par.tau = 1;		% min	
par.E1	= 1e4;		% cal/mole
par.E2 = 15000;		% cal/mole
par.C1 = 5000;		% 1/s
par.C2 = 1e6;		% 1/s
par.R = 1.987;		% cal/(mol K)
par.Cp = 1000;		% cal/(kgK)
par.u.Ti0 = 424.2;	% K , input
par.pTi = 1.657e-3;	% heating costs
par.pCB = 2.009;	% price of Product
%% Initial solves
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%% solve the ODE system for a test %%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
opts = odeset('RelTol',1e-9,'Abstol',1e-9);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Implementation of NCO Tracking 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% and we update k once steady state has been reached after the gradient has
% experimentally obtained. To do this we run experiments on the dynamic model.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% NCO-Tracking parameters:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

H=-1;                       % Initial Hessian k=0
beta =1.8
pertu = -0.025;                % perturbation for obtaining the gradient
tsample = 25; % sample time default:10
tsamplep =tsample; % time for perturbation run
par.tsample=tsample;
par.tsamplep=tsamplep
tspert = [0 tsample];       % (for gradient calculations) 
tall=[];xall=[];uall=[];deluall=[];csSetAll=[];
% set initial value of csSet:
% define control parameters from Vidar Alstads Thesis
par.u.csSet=0; par.Kc=2.89*3/2.89; par.taui=1% min
Hsoc       = [-0.7689    0.6394    0.0046];
xnom    = [0.4978    0.5022      426.6998]';

% read disturbance d3 form shared file
run('../d3.m')

x0=[1 0 398 0]'
for k= 1:250/5*2
	if ~rem(k,1)
		pertu=-pertu;
	end

% for getting the procedure started %%%%%%%%%%%%%%%%%%%%%%%%%%
	if k==1 
		delu = 0;
		tall=[];
		xall=[];
		par.u.csSet = delu;
		csSetAll(end+1)=par.u.csSet
		objall=[]
		tstart=0;
	else 
		delu = -beta*H(k-1)*gradobj(k-1); % define the update step in u
    		par.u.csSet = par.u.csSet+delu;
    		csSetAll(end+1)=par.u.csSet;
	end
	if k==2 %to help startup
		par.u.csSet=0;
    		csSetAll(end+1)=par.u.csSet;
	end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	%% NCO tracking for the current interval
	
	%% 1. Adaption step
	ts=tstart+[0 tsample];
	% simulate the time to next sampling time.
    	[t,x] =ode15s(@(t,x) dynModel(t,x,par),ts,x0,opts);

	% The simulation is run in a way, that the last time point in the 
	% simulation is the starting point for the next. In order to avoid 
    	% simulating the same time instant twice, we have to cut the last point
    	% and declare it as initial point for the next simulation:
    	x0pert=x(end,:);		% starting point for the next iteration k
    	x=x(1:end-1,:);			% remove last point
    	tstartpert= t(end);
    	t=t(1:end-1);

    	
	% calculate value of the objective funcion at the end of this simulation run
	% backcalculate inputs
	y_1       = [x(:,1)-xnom(1) x(:,2)-xnom(2) x(:,3)-xnom(3)] ; % measurements as deviation variables
	cs_1     = Hsoc*y_1';
	e_1	= cs_1'-par.u.csSet'; %control error
	eint_1	= x(:,4);
	TiSocU_1=par.u.Ti0-par.Kc*e_1-(par.Kc/par.taui)*eint_1;
	
	par.u.Ti = TiSocU_1(end); % Process input at steady state
    	obj(k) = objfun(x0pert,par);

	
	%% 2. Gradient calculation step%%%%%%%%%%%%%%%%%%%%%%%%%
    	par.u.csSet = par.u.csSet+pertu;	% do a little perturbation
    	csSetAll(end+1)= par.u.csSet;		% save it for the record
    	
	% simulate the time to next steady state.
    	tspert=tstartpert+[0 tsamplep];
    	
	[tpert,xpert] =ode15s(@(t,x) dynModel(t,x,par),tspert,x0pert,opts);
    	x0=xpert(end,:); 		%starting point for the next iteration k
    	xpert=xpert(1:end-1,:);		% reduced x-vector	
    	tstart=tpert(end);		% reduced time vector
    	tpert =tpert(1:end-1);
    	
	% backcalculate inputs
	y_2       = [xpert(:,1)-xnom(1) xpert(:,2)-xnom(2) xpert(:,3)-xnom(3)] ; %write measurements as difference variables from the nominal values
	cs_2     = Hsoc*y_2';
	e_2	= cs_2'-par.u.csSet'; %control error
	eint_2	= xpert(:,4);
	TiSocU_2=par.u.Ti0-par.Kc*e_2-(par.Kc/par.taui)*eint_2;
	
    	% use last point for gradient calculation
	par.u.Ti= TiSocU_2(end); % Process input at steady state
    	objpert(k) = objfun(x0,par);
    	gradobj(k) = (obj(k)-objpert(k))/(pertu);
    	
    	%save the measurements at ent points
    	yend(k,:)=x0;
	% Constant hessian
      	H(k+1)=-0.06;


	tadd=[t;tpert];
	tall((end+1):numel(tall)+numel(tadd),1)=tadd;
	xadd=[x;xpert];
	xallsize=size(xall);
	xaddsize=size(xadd);
	xall((end+1):xallsize(1)+xaddsize(1),:)=xadd;
	
	deluall(end+1)=delu;
	objall(end+1:end+2)=[obj(k);objpert(k)];
	uadd=[TiSocU_1;TiSocU_2];
	uaddsize=size(uadd);
	uallsize=size(uall);
	uall((end+1):uallsize(1)+uaddsize(1),:)=uadd;

	% to see where about the simulation is 
	if ~rem(k,25)
		k
		tspert(end)
	end
	
end
kall = 1:k;




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Visualization

f1=figure;
plot((1:2*kall(end))/2*(tsample+tsamplep),-objall,'LineWidth',2)
ylabel('objective','FontSize',15)
set(gca,'FontSize',15)

f2=figure;
subplot(2,1,1)
plot(tall,xall(:,1),'b',tall,xall(:,2),'r','LineWidth',2)
xlabel('time [min]','FontSize',15)
ylabel('C_A, C_B  [mole/l]','FontSize',15)
legend('C_A','C_B','Location','SouthEast')
set(gca,'FontSize',15)
subplot(2,1,2)
plot(tall,xall(:,3),'b','LineWidth',2)
xlabel('time [min]','FontSize',15)
ylabel('Reactor temperature [K]','FontSize',15)
set(gca,'FontSize',15)

f3=figure;
plot(tall,uall,'LineWidth',2)
ylabel('Ti','FontSize',15)
xlabel('time [min]','FontSize',15)
set(gca,'FontSize',15)

f4=figure;
plot((1:size(csSetAll,2))*25,csSetAll,'LineWidth',2)
ylabel('setpoint','FontSize',15)
set(gca,'FontSize',15)
axis([0 5000 -1.5 1.5])

f5=figure;
plot((1:size(gradobj,2))*50, gradobj,'LineWidth',2)
ylabel('gradient','FontSize',15)
set(gca,'FontSize',15)
axis auto

save Combi objall tall uall
uallcombi=uall
combobj=objall;
clear objall;
load ../NCOalone

f6=figure
plot((1:200)*25,-combobj,'m', (1:500)*10, -objall,'b','LineWidth',2)
ylabel('profit','FontSize',15)
legend('Combined System','NCO tracking','Location','North')
set(gca,'FontSize',15)
axis([0 5000 0 1.4])

f7=figure
plot(tall,uallcombi,'m','LineWidth',2)
%axis([0 5000 410 430])
xlabel('time [min]','FontSize',15)
ylabel('Input u NCO','FontSize',15)
axis([0 5000 380 480])
set(gca,'FontSize',15)
hold on
stairs((1:size(uall,2))*10,uall,'LineWidth',2)
legend('Combined System','NCO tracking','Location','North')
plot(tall,uallcombi,'m','LineWidth',2)



print(f1,'-r80', '-depsc2','ProfitCombined.eps')
print(f2,'-r80', '-depsc2','StatexCombined')
print(f3,'-r80', '-depsc2','InputsCombined.eps')
print(f4,'-r80', '-depsc2','SOCsetpoints.eps')
print(f5,'-r80', '-depsc2','GradientCombined.eps')
print(f6,'-r80', '-depsc2','ProfitComparisonCombinedNCOSOC.eps')
print(f7,'-r80', '-depsc2','CombinedInputAndNCOComparison.eps')
