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 = 30.05;
[ref, t, N] = make_hdd_reference(max_time, Ts_whole_register, 1e-2, 42);


%% Running a Simulation
% System Poles - in continuse domain
L_poles = [-1/Ts_whole_register/5 + 0.01i, -1/Ts_whole_register/5-0.01i];
K_poles = [-10/Ts_whole_register/5 + 0.01i, -10/Ts_whole_register/5-0.01i];

% 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    = exp(K_poles*Ts_whole_register);
opts.L    = exp(L_poles*Ts_whole_register);
% opts.K    = [0.7+1e-4i 0.7-1e-4i];
% opts.L    = [0.1+0.01i 0.1-0.01i];
opts.r    = ref;
opts.res  = 3*4;
opts.plotting = false;

[x_hist_fs, y_hist_fs, u_hist_fs, x_hat_hist_fs] = solve_full_step(opts);

% ADC delay, with sub-steps
num_sub_steps = 3;
[ref_third, t_third, N_third] = make_hdd_reference(max_time, Ts_third_register, 1e-2, 42);
opts.K    = exp(K_poles*Ts_third_register);
opts.L    = exp(L_poles*Ts_third_register);
% opts.K    = [0.7+1e-4i 0.7-1e-4i];
% opts.L    = [0.1+0.01i 0.1-0.01i];
opts.debug= false;
opts.sys  = sys_third_register;
opts.sub_steps = num_sub_steps;
opts.r    = ref_third;
opts.N    = N_third/num_sub_steps;
opts.res  = 3*4;
opts.plotting = false;

[x_hist_ss, y_hist_ss, u_hist_ss, x_hat_hist_ss] = solve_sub_step(opts);

%% Analysis
%RMSE - Overall Error Energy
solNames = {'Full Sample','Sub-Sample'};
xHists   = {x_hist_fs, x_hist_ss};   % cell array of your x_hist outputs
rHists   = {ref, ref_third};   % cell array of your x_hist outputs

nSolvers = numel(xHists);
rmseVals  = zeros(1,nSolvers);

for i = 1:nSolvers
  % extract the i-th solver’s first-state history
  xi = xHists{i}(1,:);
  ri = rHists{i}(1,:);

  % compute RMSE
  err      = xi - ri;
  mse      = mean(err.^2);
  rmseVals(i) = sqrt(mse);

  % print with the solver’s name
  fprintf('  %s RMSE = %.8f\n', solNames{i}, rmseVals(i));
end

%% ITAE – Averaged over Multiple Runs
step_time       = 1e-2;     % step time for HDD head
max_time_itae   = step_time;
nRuns           = 10;       % number of repetitions
solNames        = {'Full Sample','Sub-Sample'};
nSolvers        = numel(solNames);

% Preallocate a matrix to hold ITAE values for each solver & run
itaeVals = zeros(nSolvers, nRuns);

for j = 1:nRuns
  seedVal = j;            % ensure reproducible, paired RNG state
  rng(seedVal);           % set the seed

  %— Full-Step Solver —%
  [ref_fs, t_fs, N_fs] = make_hdd_reference( ...
    max_time_itae, Ts_whole_register, step_time, seedVal);
  opts_fs = struct();
  opts_fs.sys      = sys_whole_register;
  opts_fs.x0       = [0; 0];
  opts_fs.N        = N_fs;
  opts_fs.K        = exp(K_poles*Ts_whole_register);
  opts_fs.L        = exp(L_poles*Ts_whole_register);
  opts_fs.r        = ref_fs;
  opts_fs.res      = 3*4;
  opts_fs.plotting = false;    % turn off plots for batch runs
  [x_hist_fs, ~, ~, ~] = solve_full_step(opts_fs);

  err_fs = x_hist_fs(1,:) - ref_fs(1,:);
  % Discrete‐time ITAE: sum over t * |error|
  itaeVals(1,j) = sum(t_fs .* abs(err_fs));

  %— Sub-Sample Solver —%
  rng(seedVal);           % reset to same seed
  [ref_ss, t_ss, N_ss] = make_hdd_reference( ...
    max_time_itae, Ts_third_register, step_time, seedVal);
  opts_ss = struct();
  opts_ss.sys       = sys_third_register;
  opts_ss.x0        = [0; 0];
  opts_ss.N         = N_ss/num_sub_steps;
  opts_ss.K         = exp(K_poles*Ts_third_register);
  opts_ss.L         = exp(L_poles*Ts_third_register);
  opts_ss.sub_steps = num_sub_steps;
  opts_ss.res       = 3*4;
  opts_ss.r         = ref_ss;
  opts_ss.plotting  = false;    % disable plotting
  [x_hist_ss, ~, ~, ~] = solve_sub_step(opts_ss);

  err_ss = x_hist_ss(1,:) - ref_ss(1,:);
  itaeVals(2,j) = sum(t_ss .* abs(err_ss));
end

% Compute the average ITAE for each solver
avgItae = mean(itaeVals, 2);

% Display the results
fprintf('\nAverage ITAE over %d runs (simulation time = %.4g s):\n', ...
  nRuns, max_time_itae);
for i = 1:nSolvers
  fprintf('  %s: %.6e\n', solNames{i}, avgItae(i));
end