## Copyright (C) 2018 Jens Ahrensfeld
## 
## This program is free software; you can redistribute it and/or modify it
## under the terms of the GNU General Public License as published by
## the Free Software Foundation; either version 3 of the License, or
## (at your option) any later version.
## 
## This program is distributed in the hope that it will be useful,
## but WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
## GNU General Public License for more details.
## 
## You should have received a copy of the GNU General Public License
## along with this program.  If not, see <http://www.gnu.org/licenses/>.

## Author: Jens Ahrensfeld <ahrensfeld@w2ess001vm>
## Created: 2018-02-08

function brew_eval(varargin)
%
% brew_eval('M', 20.0, 'cont_ctrl', 1, 'kp', 2.0, 'ki', 0.02, 'kd', 400, 'T', 1800, 'mass_kleak', 0.5, 'P_q', 100, 'mass_delay', 60, 'dt', 1.00, [30 50 70]);

WITH_RATE_CONTROLLER = 1;
WITH_HYST = 0.0;

params = struct ('cont_ctrl',0, 'C',4.19e3, 'M',10, 'mass_kleak',0.5, 'mass_delay',50, 'P_min', 0, 'P_max',3000, 'P_q',100, 'T',100, 'dt',1.0, 'Td',10.0, 'theta_lock', 0.5, 'theta_amb',20.0, 'kp',10, 'ki',0.04, 'kd',500, 'pid_kleak',0.001, 'k_noise', 0.0, 'heatrate', 1.0);
units = struct ('cont_ctrl', '', 'C', 'J/(kg*K)', 'M', 'kg', 'mass_kleak', '', 'mass_delay', 's', 'P_min', 'W', 'P_max', 'W', 'P_q','W', 'T', 's', 'dt', 's', 'Td', 's', 'theta_lock', '°C', 'theta_amb', '°C', 'kp', '', 'ki', '', 'kd', '', 'pid_kleak', '', 'k_noise', '', 'heatrate', '°C/min');

% Parse parameters
names = fieldnames(params);
varargin
for k=1:nargin-1,
	for n=1:length(names),
		if strcmpi(varargin{k}, names{n})
			params.(names{n}) = varargin{k+1};
		end
	end
end
	
print_parameters(params)

s_mass = 0;
s_pid = [];
s_pid_rate = [];
theta_ist = params.theta_amb;
theta_last = params.theta_amb;
thetas = varargin{nargin}
heatRate = 0;
heatRateMeasureInterval = 1.0; % seconds
heatRateMeasureCount = 0; % seconds
n = 0;
nSteps = params.P_max / params.P_q;

N = 2*fix(length(thetas)*params.T/params.dt);

P_cont = 0;
y_rate = 0;
y = 0;

stbl_rate_z = [];
stbl_hold_z = [];

ovenState = 'HOLD';
controllerState = 'TRK';

curr = struct('kp', 0, 'ki', 0, 'kd', 0);
curr_rate = struct('kp', 0, 'ki', 0, 'kd', 0);

err0_hold = 0;
err0_rate = 0;
a_err = 0.5;
P_max = 0;
heatRate_soll = 0;
for k=1:length(thetas)
	theta_soll = thetas(k);
	hold_theta = 0;
	P_max = 0;
	printf('%d: Target temperatur %d °C\n', k, theta_soll);
	while n < N,
		err1 = theta_ist - theta_soll;
		err1_rate = heatRate - heatRate_soll;
		err0_hold = (1-a_err)*err0_hold + a_err*err1;
		err0_rate = (1-a_err)*err0_rate + a_err*err1_rate;
		[err_rate_stbl, stbl_rate_z] = isStable(stbl_rate_z, err0_rate, 0.05);
		[err_hold_stbl, stbl_hold_z] = isStable(stbl_hold_z, err0_hold, 0.05);
		ovenState_n = ovenState;
		controllerState_n  = controllerState;
		switch ovenState
			case {'HEAT'}
				heatRate_soll = params.heatrate;
				curr_rate.kp = 4.0;
				curr_rate.ki = 0.02;
				curr_rate.kd = 4.0;
				P_max = 0.5*P_max + 0.5*P_cont;
				switch controllerState
					case {'ACQ'}
						if err_rate_stbl && (abs(err0_rate) < 0.1)
							controllerState_n = 'TRK';
						end
					case {'TRK'}
						curr_rate.kp = 8.0;
						curr_rate.ki = 0.03;
						curr_rate.kd = 8.0;
						if abs(err0_rate) > 0.5
							controllerState_n = 'ACQ';
						end
				endswitch
				if abs(err0_hold) < 1.0
					ovenState_n = 'HOLD';
					controllerState_n = 'ACQ';
				end
			case {'HOLD'}
				heatRate_soll = 0;
				curr.kp = params.kp;
				curr.ki = params.ki;
				curr.kd = params.kd;
				switch controllerState
					case {'ACQ'}
						if err_hold_stbl && (abs(err0_hold) < 0.1)
							controllerState_n = 'TRK';
						end
					case {'TRK'}
						curr.kp = params.kp;
						curr.ki = params.ki;
						curr.kd = params.kd;
						if abs(err0_hold) > 0.2
							controllerState_n = 'ACQ';
						end
				endswitch
				if err0_hold < -1.0
					if WITH_RATE_CONTROLLER
						ovenState_n = 'HEAT';
					end
					controllerState_n = 'ACQ';
				end
		endswitch
		if strcmp(controllerState, controllerState_n) == 0 || strcmp(ovenState, ovenState_n) == 0
			printf('%d: State %s::%s -> %s::%s\n', n, ovenState, controllerState, ovenState_n, controllerState_n);
		end
		ovenState = ovenState_n;
		controllerState = controllerState_n;

		% Hold controller
		if strcmp(ovenState, 'HEAT')
			[y, yc, s_pid] = jpid(s_pid, curr_rate.kp, curr_rate.ki*params.dt, curr_rate.kd/params.dt, 1-params.pid_kleak, -err0_rate);
			P_cont = max(params.P_min, min(params.P_max, params.P_max*y));
			s_pid.y_max = 80;
		else
			[y, yc, s_pid] = jpid(s_pid, curr.kp, curr.ki*params.dt, curr.kd/params.dt, 1-params.pid_kleak, -err0_hold);
			P_cont = max(params.P_min, min(P_max, params.P_max*y));
			s_pid.y_max = 80;
		end

		P_cont_q = fix(P_cont/params.P_q + 0.5)*params.P_q;
		if params.cont_ctrl
			P = P_cont_q;
		else
			duty = mod(n*params.dt/params.Td*params.P_max, params.P_max);
			heat = (duty < P_cont_q);
			P = params.P_max * heat;
		end
		[theta, s_mass] = mass(s_mass, params.dt, params.C, params.M, params.mass_kleak, P, params.mass_delay, theta_ist-params.theta_amb);
		theta_ist = params.theta_amb + theta + params.k_noise*randn()/sqrt(12);
		
		if heatRateMeasureCount <= 0
			heatRate = 60*(theta_ist-theta_last)/heatRateMeasureInterval;
			heatRateMeasureCount = heatRateMeasureInterval;
			theta_last = theta_ist;
		end	
		heatRateMeasureCount = heatRateMeasureCount - params.dt;
		n = n + 1;
		theta_ist = theta_ist;
		heatRate_(n) = heatRate;
		theta_(n) = theta_ist;
		err_(n) = err0_hold;
		p_(n) = P;
		pc_(n) = P_cont;
		pcq_(n) = P_cont_q;
		y_(n) = y;
		y_rate_(n) = y_rate;
		err_rate_(n) = err0_rate;
		
		if hold_theta > 0
			hold_theta = hold_theta - 1;
			if hold_theta == 0
				break;
			end
		elseif abs(err0_hold) < params.theta_lock
			hold_theta = params.T/params.dt;
		end
	end
end
close all;
t = (0:n-1)*params.dt;

subplot(3,1,1)
plot(t, theta_); grid; xlabel('t/s'); ylabel('Theta/°'); title('Temperature');
subplot(3,1,2)
plot(t, p_, 'b-', t, pcq_, 'g-', t, pc_, 'r-'); grid; xlabel('t/s'); ylabel('P/W'); title('Power Control'); legend('P','y','P_{cont}');
subplot(3,1,3)
plot(t, err_, t, err_rate_); grid; xlabel('t/s'); ylabel('err/K'); title('Error'); legend('Error_{Hold}', 'Error_{Rate}');

figure;
subplot(3,1,1)
plot(t, heatRate_); grid; xlabel('t/s'); ylabel('Theta/°'); title('Heat_{rate}');
subplot(3,1,2)
plot(t, y_rate_); grid; xlabel('t/s'); ylabel('Theta/°'); title('y_{rate}');
subplot(3,1,3)
plot(t, err_rate_); grid; xlabel('t/s'); ylabel('Theta/°'); title('Error_{rate}');

function [theta, s] = mass(si, dt, C, M, L, P, Td, dTheta)
s = si;
if ~isstruct(si)
	alpha = 1.0;
	if Td > 0
		alpha = dt/Td;
	end
	s = struct('e', 0, 'a', alpha, 'x', 0, 'gain', 0.8);
end

s.e = s.e * (1-((L*(0+dTheta))*dt)/(M*C));
s.x = (1-s.a)*s.x + s.gain*s.a*P*dt;
s.e = s.e + s.x;
theta = s.e/(M*C);
endfunction

function [y, z] = isStable(zi, v, maxErr)
z = zi;
if ~isstruct(zi)
	z = struct('v', 0, 'cnt', 3, 'cnt_reload', 3);
end

if z.cnt > 0
	z.cnt = z.cnt - 1;
endif

if abs(v - z.v) <= maxErr
	z.cnt = z.cnt_reload;
endif

y = (z.cnt == 0);
endfunction

function print_parameters(params)
align = 32;
names = fieldnames(params);
for n=1:length(names),
	fprintf('%s', names{n});
	remain = align - length(names{n});
	if remain < 0
		error('Invalid variable length!');
	else
		for j=1:remain
			fprintf(' ');
		end
		fprintf('= %g %s\n', params.(names{n}), units.(names{n}));
	end
end
endfunction

endfunction