%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Main file to simulate the CSTR 
% from Vidar Alstads PhD thesis, page 61-63
% we use NCO tracking and self-optimizing control 

% 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

% Unexpected disturbances
run('./d3.m')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 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 = 0.3%2               	% default = 0.3
beta = 0.4%2               	% default = 0.4
pertu =1                	% perturbation for obtaining the gradient


par.u.Ti = par.u.Ti0;     	% initial input
opts = odeset('RelTol',1e-8,'Abstol',1e-8);
x0 = [1; 0; 398];	  	% initial state

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% For computing the initial gradient k=0
tsample = 10; % sample time default:10
tsamplep =tsample;          	% time for perturbation run
par.tsample=tsample;
par.tsamplep=tsamplep
tspert = [0 tsample];       	% (for gradient calculations) 
[ta,x0a] =ode23(@(t,x) dynModel(t,x,par),tspert,x0,opts);


par.u.Ti =par.u.Ti+pertu;   	% perturbe input
% use last point as initial point and for evaluation of the objective function
obj0 = objfun(x0a(end,:),par)
% integrate perturbed model
[tb,x0pert] =ode23(@(t,x) dynModel(t,x,par),[tspert(end) tspert(end)+tsample],x0a(end,:),opts);
% use last point for gradient calculation
obj0pert = objfun(x0pert(end,:),par)
gradobj0=(obj0-obj0pert(end,:)')/pertu;

par.u.Ti = par.u.Ti - beta*H*gradobj0;



%% Do The NCO-Tracking procedure

tall=[];
xall=[];
deluall=[];
uall=[];
Objtotal=[];




for k= 1:250
    if ~rem(k,4)
        pertu=-pertu;

    end

	if k==1
		delu = -beta*H(k)*gradobj0;
		tall=[];
		xall=[];
		par.u.Ti = par.u.Ti+delu;
		uall(end+1)=par.u.Ti;
		objall=[]
		tstart=0;
        par.u.Ti=par.u.Ti0
    else
        delu = -beta*H(k-1)*gradobj(k-1); % define the update step in u
        par.u.Ti = par.u.Ti+delu;
        uall(end+1) =par.u.Ti;
	end
	ts=tstart+[0 tsample];
	% simulate the time to next sampling time.
	%[t,x] =ode23(@(t,x) dynModel(t,x,par),ts,x0,opts);
    [t,x] =ode45(@(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,:);			% reduced x-vector
	tstartpert= t(end);
	t=t(1:end-1);
	Obj1=objfun(x,par);

	% calculate value of the objective funcion at the end of this simulation run
	obj(k) = objfun(x0pert,par);

    
    % Gradient calculation
    par.u.Ti = par.u.Ti+pertu;	% do a little perturbation
    uall(end+1)=par.u.Ti;               % save it for the record

	% simulate the time to next steady state.
	tspert=tstartpert+[0 tsamplep];
    	[tpert,xpert] =ode45(@(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);
	Obj2=objfun(xpert,par);


	% use last point for gradient calculation
	objpert(k) = objfun(x0,par);
    	gradobj(k) = (obj(k)-objpert(k))/(pertu);

 	%save the measurements at ent points
	yend(k,:)=x0;


        H(k+1)=-4500; %  constant Hessian

	% book keeping
	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)];


	Objadd=[Obj1;Obj2];
	Objtotal(end+1:xallsize(1)+xaddsize(1),:)=Objadd;
	
	% to see where about the simulation is 
	if ~rem(k,25)
		k
        tspert(end)
	end
	
end
kall = 1:k;
%objNCO=objall;



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Now we do SOC as a comparison
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Optimize steady state model to find F
%
%% nominal optimal point
X0 = [1; 0; 398;420]; % initial estimation
A=[];b=[];Aeq=[];beq=[];lb=[];ub=[];
Optopts = optimset('TolFun',1e-6,'TolX',1e-6);
%% calculate nominal optimum
XNomOpt = fmincon(@(X)objfunFmincon(X,par),X0,A,b,Aeq,beq,lb,ub,@(X)nonlincon(X,par),Optopts) 

% define control parameters from Vidar Alstads Thesis
par.Kc=2.89;
par.taui=1% min 

% Simulate System
X0soc=[XNomOpt(1:3);0]
[tsoc,xsoc] =ode15s(@(t,x) dynModelSOC(t,x,par),[0 tall(end)],X0soc,opts);


ktime=floor(tsoc/(tsample+tsamplep)+1);
%nall=n(ktime)';


%backcalculating input:
Hsoc   = [-0.7689	0.6394	0.0046];  %from F=dyopt/dd	

y       = [xsoc(:,1)-X0soc(1) xsoc(:,2)-X0soc(2) xsoc(:,3)-X0soc(3)] ; %write measurements as difference variables from the nominal values
cs      = Hsoc*y';
e	= cs'-0; %control error
eint	= xsoc(:,4);

TiSoc=par.u.Ti0-par.Kc*e-(par.Kc/par.taui)*eint;%+nall;
%Cost when using SOC
Jsoc = -objfunSOC(xsoc,par,TiSoc);

%Disturbance Profile
i=1;
for ti=0:1:tall(end)
	[CAallin(i) CBallin(i)]=scenario(ti,par);
	i=i+1;
end
tiall=0:1:tall(end);






%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Visualization
f1=figure;
subplot(2,1,1)
	plot(tsoc,xsoc(:,1),'b',tsoc,xsoc(:,2),'r','LineWidth',2)
	xlabel('time [min]','FontSize',15)
	ylabel('Concentrations SOC','FontSize',15)
	legend('C_A','C_B', 'Location', 'North')
	axis([0 5000 0.2 1.2])
    set(gca,'FontSize',15)
subplot(2,1,2)
	plot(tsoc,xsoc(:,3),'LineWidth',2)
	xlabel('time [min]','FontSize',15)
	ylabel('Reactor temp. [K]','FontSize',15)
	axis([0 5000 390 480])
    set(gca,'FontSize',15)


f2=figure;
subplot(2,1,1)
	plot(tsoc,TiSoc,'LineWidth',2)
	xlabel('time [min]','FontSize',15)
	ylabel('Input Ti_{SOC}','FontSize',15)
	axis([0 5000 410 435])
    set(gca,'FontSize',15)
subplot(2,1,2)
	plot(tsoc,cs,'LineWidth',2)
	xlabel('time [min]','FontSize',15)
	ylabel('cs_SOC','FontSize',15)
    set(gca,'FontSize',15)


f3=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','North')
	axis([0 5000 0.2 1.2])
    set(gca,'FontSize',15)

subplot(2,1,2)
	plot(tall,xall(:,3),'b','LineWidth',2)
	xlabel('time [min]','FontSize',15)
	ylabel('Reactor temp. [K]','FontSize',15)
	axis([0 5000 390 480])
    set(gca,'FontSize',15)




f4=figure;
subplot(2,1,1)
	stairs((1:(2*kall(end)))/2*(tsample+tsamplep),uall,'LineWidth',2)
	xlabel('time [min]','FontSize',15)
	ylabel('Input u','FontSize',15)
	axis([0 5000 395 510])
    set(gca,'FontSize',15)
subplot(2,1,2)
	plot(kall*(tsample+tsamplep),gradobj,'LineWidth', 2)
	xlabel('time [min]','FontSize',15)
	ylabel('gradient','FontSize',15)
	axis([0 5000 -4e-2 4e-2])
    set(gca,'FontSize',15)

f5=figure;
	plot(tall,-Objtotal,tsoc,Jsoc,'r','LineWidth',2)
	xlabel('time [min]','FontSize',15)
	ylabel('profit','FontSize',15)
	legend('NCO profit', 'SOC profit','Location','SouthEast')
    	axis([0 5000 0 1.4])
        set(gca,'FontSize',15)
    
    
f6=figure;
    plot((1:2*kall(end))/2*(tsample+tsamplep),-objall,'LineWidth',2)
    xlabel('time [min]','FontSize',15)
    ylabel('NCO Profit','FontSize',15)
    axis([0 5000 0 1.4])
    set(gca,'FontSize',15)
f7=figure;
    plot(tsoc,Jsoc ,'LineWidth',2)
    xlabel('time [min]','FontSize',15)
    ylabel('SOC Profit','FontSize',15)
    axis([0 5000 0.2 2])
    set(gca,'FontSize',15)
    
f8 = figure;
	plot((1:(2*kall(end)))/2*(tsample+tsamplep),uall,'b', tsoc,TiSoc,'r','LineWidth',2)
	hold on
	xlabel('time [min]','FontSize',15)
	ylabel('Input Ti','FontSize',15)
	legend('NCO tracking','self-optimizing control','location','SouthEast')
	axis([0 5000 390 470])
    set(gca,'FontSize',15)
    
f9 = figure;
    plot(tiall,CAallin,tiall,CBallin,'LineWidth', 3)
 	xlabel('time [min]','FontSize',15)
 	ylabel('Disturbances','FontSize',15)
 	legend('CAin', 'CBin','Location','South')
    set(gca,'FontSize',15)

f10=figure;
	plot(tall,-Objtotal,'b','LineWidth',2)
	xlabel('time [min]','FontSize',15)
	ylabel('profit','FontSize',15)
	legend('NCO profit','Location','SouthEast')
    axis([0 5000 0 2])
    set(gca,'FontSize',15)
    
print(f1,'-r80', '-depsc2','SOCconcentrations.eps')
print(f2,'-r80', '-depsc2','SOCInputs.eps')
print(f3,'-r80', '-depsc2','NCOconcentrations.eps')
print(f4,'-r80', '-depsc2','NCOInputs.eps')
print(f5,'-r80', '-depsc2','ProfitComparison.eps')
print(f6,'-r80', '-depsc2','NCOSOCProfit.eps')
print(f7,'-r80', '-depsc2','SOCProfit.eps')
print(f8,'-r80', '-depsc2','SOCNCOInputs.eps')
print(f9,'-r80', '-depsc2','Scenario.eps')
print(f10,'-r80', '-depsc2','NCOProfit.eps')
    
print(f1,'-r80', '-dpdf','SOCconcentrations.pdf')
print(f2,'-r80', '-dpdf','SOCInputs.pdf')
print(f3,'-r80', '-dpdf','NCOconcentrations.pdf')
print(f4,'-r80', '-dpdf','NCOInputs.pdf')
print(f5,'-r80', '-dpdf','ProfitComparison.pdf')
print(f6,'-r80', '-dpdf','NCOSOCProfit.pdf')
print(f7,'-r80', '-dpdf','SOCProfit.pdf')
print(f8,'-r80', '-dpdf','SOCNCOInputs.pdf')
print(f9,'-r80', '-dpdf','Scenario.pdf')
print(f10,'-r80', '-dpdf','NCOProfit.pdf')

save NCOalone objall  uall   

