diff --git a/ME_2046/Project/final_scripts/final.m b/ME_2046/Project/final_scripts/final.m
index 9a6374e..4eefa41 100644
--- a/ME_2046/Project/final_scripts/final.m
+++ b/ME_2046/Project/final_scripts/final.m
@@ -33,7 +33,6 @@ sys_third_register = c2d(sys_cont, Ts_third_register, 'zoh');
 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
@@ -44,18 +43,20 @@ 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;
+opts.plotting = true;
 
 [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];
+[ref_third, t_third, N_third] = make_hdd_reference(12*Ts_third_register, Ts_third_register, 1e-2, 42);
+opts.K    = [0.99+1e-4i 0.99-1e-4i];
+opts.L    = [0.2+0.01i 0.2-0.01i];
+opts.debug= true;
 opts.sys  = sys_third_register;
 opts.sub_steps = 3;
 opts.r    = ref_third;
 opts.N    = N_third/3;
-opts.res  = 12;
+opts.res  = 48;
 opts.plotting = true;
 
 [x_hist, y_hist, u_hist, x_hat_hist] = solve_sub_step(opts);
diff --git a/ME_2046/Project/final_scripts/solve_sub_step.m b/ME_2046/Project/final_scripts/solve_sub_step.m
index 8469b42..bf6a11e 100644
--- a/ME_2046/Project/final_scripts/solve_sub_step.m
+++ b/ME_2046/Project/final_scripts/solve_sub_step.m
@@ -89,7 +89,6 @@ end
 
 % ----------------‑‑ Simulation loop ------------------------------------
 for k = (1:J:(N-1)*J)%N-1
-  k
   x_hist(:,k+1)     = A*x_hist(:,k+0) + B*u_hist(k+0);
   y_hist(:,k+0)     = C*x_hist(:,k+0);
 
@@ -103,11 +102,11 @@ for k = (1:J:(N-1)*J)%N-1
     B*u_hist(k+0) + ...
     L*( ADC_output(1) - C*x_hat_hist(:,k+0));
 
-  u_hist(k+1)       = K*(r(:,k+1)-x_hat_hist(:,k+1));
+  u_hist(k+2)       = K*(r(:,k+1)-x_hat_hist(:,k+1));
 
   %SECOND SUBSTEP
   x_hist(:,k+2)     = A*x_hist(:,k+1) + B*u_hist(k+1);
-  y_hist(:,k+1)   = C*x_hist(:,k+1);
+  y_hist(:,k+2)   = C*x_hist(:,k+1);
 
   x_hat_hist(:,k+2) = A*(A*x_hat_hist(:,k+0) + ...
     B*u_hist(k+0) + ...
@@ -119,7 +118,7 @@ for k = (1:J:(N-1)*J)%N-1
 
   %THIRD SUBSTEP
   x_hist(:,k+3)     = A*x_hist(:,k+2) + B*u_hist(k+2);
-  y_hist(:,k+2)   = C*x_hist(:,k+2);
+  y_hist(:,k+3)   = C*x_hist(:,k+2);
 
   x_hat_hist(:,k+3) = A*(A*(A*x_hat_hist(:,k+0) + ...
     B*u_hist(k+0) + ...
@@ -130,20 +129,55 @@ for k = (1:J:(N-1)*J)%N-1
     B*u_hist(k+2);
 
   u_hist(k+3)       = K*(r(:,k+3)-x_hat_hist(:,k+3));
+  %── DEBUG PRINTS (only if opts.debug) ────────────────────────────
+  if isfield(opts,'debug') && opts.debug
 
+    fprintf("\n=== Macro‐step k = %d ===\n", k);
+
+    %— Sensor / ADC —
+    fprintf("-- Sensor + ADC --\n");
+    fprintf(" sensor_value = [%s]  (y_hist(:,%d)*2^%d)\n", ...
+      num2str(sensor_value.', ' %.4f'), k, res);
+    [mADC,nADC] = size(ADC_output);
+    fprintf(" ADC_output   = %d×%d matrix:\n", mADC, nADC);
+    for row = 1:mADC
+      fprintf("   row %d: [%s]\n", row, num2str(ADC_output(row,:), ' %.4f'));
+    end
+
+    %— Microstep 1 (@ k+1) —
+    fprintf("\n-- Microstep 1 (t = k+1) --\n");
+    fprintf(" r(:,%d)     = [%s]\n", k+1, num2str(r(:,k+1).', ' %.4f'));
+    fprintf(" x_hist(:,%d)     = [%s]\n", k+1, num2str(x_hist(:,k+1).', ' %.4f'));
+    fprintf(" x_hat_hist(:,%d) = [%s]\n", k+1, num2str(x_hat_hist(:,k+1).', ' %.4f'));
+    fprintf(" y_hist(:,%d)     = [%s]\n", k+1, num2str(y_hist(:,k+1).', ' %.4f'));
+    fprintf(" u_hist(%d)       = %.4f\n", k+1, u_hist(k+1));
+
+    %— Microstep 2 (@ k+2) —
+    fprintf("\n-- Microstep 2 (t = k+2) --\n");
+    fprintf(" r(:,%d)     = [%s]\n", k+1, num2str(r(:,k+2).', ' %.4f'));
+    fprintf(" x_hist(:,%d)     = [%s]\n", k+2, num2str(x_hist(:,k+2).', ' %.4f'));
+    fprintf(" x_hat_hist(:,%d) = [%s]\n", k+2, num2str(x_hat_hist(:,k+2).', ' %.4f'));
+    fprintf(" y_hist(:,%d)     = [%s]\n", k+2, num2str(y_hist(:,k+2).', ' %.4f'));
+    fprintf(" u_hist(%d)       = %.4f\n", k+2, u_hist(k+2));
+
+    %— Microstep 3 (@ k+3) —
+    fprintf("\n-- Microstep 3 (t = k+3) --\n");
+    fprintf(" r(:,%d)     = [%s]\n", k+1, num2str(r(:,k+3).', ' %.4f'));
+    fprintf(" x_hist(:,%d)     = [%s]\n", k+3, num2str(x_hist(:,k+3).', ' %.4f'));
+    fprintf(" x_hat_hist(:,%d) = [%s]\n", k+3, num2str(x_hat_hist(:,k+3).', ' %.4f'));
+    fprintf(" y_hist(:,%d)     = [%s]\n", k+3, num2str(y_hist(:,k+3).', ' %.4f'));
+    fprintf(" u_hist(%d)       = %.4f\n", k+3, u_hist(k+3));
+
+  end  % if debugend
 end
 
-x_hist
-x_hat_hist
-y_hist
-u_hist
-
 
 % ----------------‑‑ Plot results ---------------------------------------
 
 if plotting
   figure;
   time = (0:N*J)*Ts;
+
   for i = 1:nx
     subplot(nx+1,2,i);
     plot(time,x_hat_hist(i,:),'-xr', time,x_hist(i,:),'-ob');
@@ -152,8 +186,13 @@ if plotting
     if i==nx, xlabel('Time (s)'); end
     legend('x_{hat}', 'x')
   end
+
   subplot(nx+1,2,3);
-  stairs(time(1:N), r(1,1:N), "Color", "#22FF22")
+  hold on;
+  stairs(time(1:N), r(1,1:N), "Color", "#00FF80")
+  stairs(time(1:N), r(2,1:N), "Color", "#88aa00")
+  hold off;
+
   ylabel('Position Demanded');
   xlabel('Time (s)');