%% Inverted pendulum control with nonlinear feadback and output selection
%  
%  File: ccs_invpend_LQR_nonlin_control_szed2002.m
%  Directory: 4_gyujtemegy/11_CCS/_2_nonlin-pannon/invpend_nonlin_control
%  Author: Peter Polcz (ppolcz@gmail.com) 
%  
%  Created on 2018. July 30.
%
% Inherited from
%  
%  File: pcz_nonlinearControl.m (2014)

%%
% Automatically generated stuff

global SCOPE_DEPTH VERBOSE LATEX_EQNR 
SCOPE_DEPTH = 0;
VERBOSE = 1;
LATEX_EQNR = 0;

try c = evalin('caller','persist'); catch; c = []; end
persist = Persist(mfilename('fullpath'), c); clear c; 
persist.backup();
%clear persist

%% Control design

visualization = 0;
use_kalman_instead_of_simple_state_estimator = 1;
full_state_estimation = 1;

% constants:
M = 1.5;
m = 0.2;
b = 0;
g = 9.81;
l = 1;
Ts = 0.016;
dim = 4;

T0 = 0;
T1 = 10;
N = (T1 - T0) / Ts;

% by my own calculations:
A = [0  1              0                    0; 
     0  -4*b/(m+4*M)   3*g*m/(m+4*M)        0; 
     0  0              0                    1; 
     0  -3*b/(m+4*M)/l  3*g*(m+M)/l/(m+4*M)  0];

B = [0; 
     4/(m+4*M); 
     0; 
     3/(m+4*M)/l];

C = [1 0 0 0; 
     0 0 1 0];

D = [0; 0];

Q = diag([1, 1, 1, 1]);

% generating state space system
sysc = ss(A,B,C,D);
[K,~,~] = lqr(A,B,Q,0.5, 0);
pcz_debugw(K)

% discretization - only A and B needed to be updated
sysd = c2d(sysc, Ts);
A = sysd.A;
B = sysd.B;

% lqr feedback gain generation
[K,~,~] = dlqr(A, B, Q, 0.5, 0);
pcz_debugw(K)

% a Matlab szimulacio valtozoi
sx = zeros(4,N);
su = zeros(1,N);

%%
% Download the Simulink model: <sim invpend_simulation.slx>
%
% *Main model*
% 
% <img fig/sim.png>
% 
% *Alpha-Beta System* submodel
% 
% <img fig/sim_ab.png>
% 
% *Nonlinear system* submodel
% 
% <img fig/sim_ab_nonlin.png>

open_system invpend_simulation
print -dpng fig/sim.png -sinvpend_simulation
print -dpng fig/sim_ab.png '-sinvpend_simulation/Alpha-Beta System'
print -dpng fig/sim_ab_nonlin.png '-sinvpend_simulation/Alpha-Beta System/Nonlinear system'

%% Plot

t = S_x.Time';

l = 1;
cl = 1;
cw = 0.1;

T0 = t(1);
T1 = t(end);
fps = 15;
Ts = 1/fps;
nr_samples = (T1-T0) * fps;
t_ = linspace(T0, T1, nr_samples);

Data = interp1(S_x.Time, S_x.Data, t_');
x = Data(:,1)';
fi = Data(:,3)';
ref = interp1(S_r.Time, squeeze(S_r.Data), t_');
% 2018.07.30. (július 30, hétfő), 09:56 [korrigálás: squeeze]

x_init = x;
x1 = x_init - l*sin(fi);
y1 = l*cos(fi);

% angle of the pendulum: phi 
figure, subplot(212);
plot(t_, fi), title('Angle of pendulum'), grid on;

% chart's position
subplot(211)
plot(t_, [x' ref]), title('Chart''s position'), grid on;
legend('chart''s position', 'reference input', 'actual time step');

max_fi = max(fi);
min_fi = min(fi);
max_x = max(x);
min_x = min(x);

in_Time = 64;

return

%% Video [TODO]

figure('Position', [ 378 , 103 , 1312 , 821 ], 'Color', [1 1 1]);

t = S_x.Time';

l = 1;
cl = 1;
cw = 0.1;

T0 = t(1);
T1 = t(end);
fps = 15;
Ts = 1/fps;
nr_samples = (T1-T0) * fps;
t_ = linspace(T0, T1, nr_samples);

Data = interp1(S_x.Time, S_x.Data, t_');
x = Data(:,1)';
fi = Data(:,3)';
ref = interp1(S_r.Time, squeeze(S_r.Data), t_');
% 2018.07.30. (július 30, hétfő), 09:56 [korrigálás: squeeze]

x_init = x;
x1 = x_init - l*sin(fi);
y1 = l*cos(fi);

% angle of the pendulum: phi 
subplot(221), hold on;
plot(t_, fi), title('Angle of pendulum'), grid on;

% chart's position
subplot(222), hold on;
plot(t_, [x' ref]), title('Chart''s position'), grid on;

max_fi = max(fi);
min_fi = min(fi);
max_x = max(x);
min_x = min(x);

subplot(2,2,[3 4])
Movie = moviein(length(fi));
for kk = 1:nr_samples
    tic

    % angle of the pendulum: phi 
    subplot(221);
    plot(t_, fi), title('Angle of pendulum'), grid on, hold on;
    plot([t_(kk), t_(kk)], [min_fi, max_fi], 'r'), hold off;

    % chart's position
    subplot(222);
    plot(t_, x), title('Chart''s position'), grid on, hold on;
    plot([t_(kk), t_(kk)], [min_x, max_x], 'r'), hold off;

    subplot(2,2,[3 4]), 
    plot([x_init(kk)-cl/2, x_init(kk)-cl/2, x_init(kk)+cl/2, x_init(kk)+cl/2, x_init(kk)-cl/2], [-cw/2, cw/2, cw/2, -cw/2, -cw/2])
    hold on;
    plot([x_init(kk); x1(kk)],[0; y1(kk)],'b-'),
    plot(x1(kk), y1(kk), 'o'), title('Simulation');
    axis equal, 
%     axis([110 140 -2 2]), grid on;
    axis([-20.2 20.2 -2 2]), grid on;
    hold off;
    pause(max(Ts-toc,0));
end
