%% parameterek
v = 1;      % mm/ms = m/s, speed
L = 310;    % mm, distance between wheels
d = 110;    % mm, distance between LSAs
Ts = 1;     % ms, sampling time for simulation
p_max = 3.5+7*11; % mm, maximum measured position

parse_constants;

%% szakasz
P = tf([-L*v -v*v],[L+d 0 0]);
A = [0 v; 0 0];
B = [-v*L/(L+d); -v/(L+d)];

%% szabalyzo beallitasai
% szabalyzo kivalasztasa (1-PID, 2-P, 3-spec, 4-PD, 5-allapotteres)
%select = 5;

% 1 - PID szabalyzo hangolasa
%A_pid  = -0.0005;
Ti_pid = 1;
Td_pid = L/v;
Tc_pid = L/v;
numd_pid = conv( [Ti_pid Ts-Ti_pid], [Td_pid Ts-Td_pid] );
dend_pid = conv( [Ti_pid -Ti_pid],   [Tc_pid Ts-Tc_pid] );

% 2 - P szabalyzo hangolasa
%gain = -0.015;
% gain = -4/(L+d); % a hivatalosan ajanlott

% 3 - intuitiv szabalyzo hangolasa
%Ai = -0.6;
%Ai = (3/2)^2*(gain/p_max^2); felhasznalva a P szab. eredmenyeit

% 4 - PD szabalyzo hangolasa
% A_pd  = -0.009;
Td_pd = 930;%L/v*10;
Tc_pd = 310;%L/v*1;
numd_pd = [Td_pd Ts-Td_pd];
dend_pd = [Tc_pd Ts-Tc_pd];

% 5 - allapotteres szabalyozo automatikusan hangolva
% state controller
ksi   = 0.7;     % ne legyen tulloves
w0    = 1/100;   % szabalyzasi idoallando 1/ms-ban
poles = [-w0*ksi+1i*w0*sqrt(1-ksi^2); -w0*ksi-1i*w0*sqrt(1-ksi^2)];
% K = acker(A,B,poles);

%% fix kormanyszogek
% cSwapCurve = sin(tIn/5000*2*pi) .* sin(tIn/5000*pi); % TODO ez meg nem jo!

%% korlatok
deg2cnt = (180+140) / ((atan(310/1570)+atan(310/1670))*180/pi);
max_TIM_CCR = 180;
min_TIM_CCR = -180;
max_real_angle = (max_TIM_CCR-708+320)/deg2cnt;    % deg
min_real_angle = (min_TIM_CCR-708+320)/deg2cnt;    % deg
max_virt_angle = 180/pi*atan((L+d)/L*tan(pi/180*max_real_angle));    % deg
min_virt_angle = 180/pi*atan((L+d)/L*tan(pi/180*min_real_angle));    % deg

%% ellenorzes
% plot([-p_max:p_max], gain*[-p_max:p_max]*180/pi, ...
%      [-p_max:p_max], Ai*tan([-p_max:p_max]/(p_max*2.5/pi))*180/pi, ...
%      [-p_max:p_max], max_virt_angle, ...
%      [-p_max:p_max], min_virt_angle)
 
% plot([-p_max:p_max], gain*[-p_max:p_max]*180/pi, ...
%      [-p_max:p_max], Ai*[-p_max:p_max].^3*180/pi,
%      [-p_max:p_max], max_virt_angle, ...
%      [-p_max:p_max], min_virt_angle) 

% PID
numc_pid  = conv( [Ti_pid 1], [Td_pid 1] );
denc_pid  = conv( [Ti_pid 0], [Tc_pid 1] );
C1_c      = tf(A_pid*numc_pid, denc_pid);
C1_d      = c2d(C1_c, Ts, 'zoh');
C1_direct = tf(A_pid*numd_pid, dend_pid, Ts);

% PD
numc_pd   = [Td_pd 1];
denc_pd   = [Tc_pd 1];
C2_c      = tf(A_pd*numc_pd, denc_pd);
C2_d      = c2d(C2_c, Ts, 'zoh');
C2_direct = tf(A_pd*numd_pd, dend_pd, Ts);

%% a segedszamitasok torlese
clear denc_pid numc_pid denc_pd numc_pd
clear C1_d C1_direct C2_d C2_direct
clear ksi w0 poles A B
clear max_TIM_CCR min_TIM_CCR max_real_angle min_real_angle max_virt_angle min_virt_angle