close all
% The quick brown fox jumps over the lazy dog. The dog stays blissfully asleep. :)
% Dane Sabo
% ME 2046 Final Project Code

%% System Setup
% Continuous System
J = 0.01;   %kgm^2
C = 0.004;  %Nm/(rad/s)
K = 10;     %Nm/rad
K_i = 0.05; %Nm/rad

F = [0 1; -K/J -C/J];
G = [0; K_i/J];

G_disturb = [0; 1/J];

C = [1 0];
D = 0;

sys_cont = ss(F, G, C, D);

% Digital System Conversion
Ts_whole_register = 1/15e3; %s
sys_whole_register = c2d(sys_cont, Ts_whole_register, 'zoh');


% Assume a 12-bit SAR ADC with bits 0-3 in first, bits 4-7 in 2nd, 8-11 in 3rd
Ts_third_register = Ts_whole_register/3;
sys_third_register = c2d(sys_cont, Ts_third_register, 'zoh');

% Create a Reference Signal
max_time = 1e-2;
[ref, t, N] = make_hdd_reference(max_time, Ts_whole_register, 1e-2, 42);

[ref_third, t_third, N_third] = make_hdd_reference(9*Ts_third_register, Ts_third_register, 1e-2, 42);

%% Running a Simulation
% ADC Delay, no sub-steps - Setting Up Simulation
opts      = struct();      % start empty
opts.sys  = sys_whole_register;
opts.x0   = [0; 0];
opts.N    = N;
opts.K    = [0.7+1e-4i 0.7-1e-4i];
opts.L    = [0.1+0.01i 0.1-0.01i];
opts.r    = ref;
opts.plotting = false;

[x_hist, y_hist, u_hist, x_hat_hist] = solve_full_step(opts);

% ADC delay, with sub-steps
opts.K    = [0.7+1e-4i 0.7-1e-4i];
opts.L    = [0.4+0.01i 0.4-0.01i];
opts.sys  = sys_third_register;
opts.sub_steps = 3;
opts.r    = ref_third;
opts.N    = N_third/3;
opts.res  = 12;
opts.plotting = true;

[x_hist, y_hist, u_hist, x_hat_hist] = solve_sub_step(opts);