clc; clear all; close all;

format shorteng

function s = eng(x, prec)

    if nargin < 2
        prec = 3;
    end

    if x == 0
        s = sprintf(sprintf("%%.%df", prec), 0);
        return;
    end

    e = 3*floor(log10(abs(x))/3);
    m = x/10^e;

    fmt = sprintf("%%.%dfe%%+03d", prec);
    s = sprintf(fmt, m, e);

end

function print_cosine_form(pole, residue_value)

    sigma = real(pole);
    omega = imag(pole);

    amplitude = 2*abs(residue_value);
    phase = angle(residue_value);

    fprintf("\nComplex pair\n");
    fprintf("------------\n");
    fprintf("p = %s + %sj\n", ...
            eng(sigma), eng(omega));

    fprintf("k = %s + %sj\n", ...
            eng(real(residue_value)), ...
            eng(imag(residue_value)));

    fprintf("\nTime domain\n");
    fprintf("-----------\n");
    fprintf("%s * exp(%st) * cos(%st %+0.3f)\n\n", ...
            eng(amplitude), ...
            eng(sigma), ...
            eng(omega), ...
            phase);
end

% Respuesta al escalón

fprintf("------------------\n");
fprintf("Heaviside response\n");
fprintf("------------------\n");

num = [1 250];
den1 = [1 500];
den2 = [1 4000^2/9000 4000^2];
den3 = [1 0];
den = conv(conv(den1, den2), den3);

[r, p] = residue(num, den)

k1 = r(3); p1 = p(3);
k2 = r(1); p2 = p(1);
k3 = r(2); p3 = p(2);
k4 = r(4); p4 = p(4);

print_cosine_form(p2, k2)

% Respuesta a seno

w0 = 2*pi*250; % Aproximadamente 250Hz
fprintf("\n---------------------------------------\n");
fprintf("Sine function response with f=%.1f Hz\n", w0/(2*pi));
fprintf("---------------------------------------\n");

num = w0*[1 250];
den1 = [1 500];
den2 = [1 4000^2/9000 4000^2];
den3 = [1 0 w0^2];
den = conv(conv(den1, den2), den3);

[r, p] = residue(num, den)

k1 = r(5); p1 = p(5);
k2 = r(3); p2 = p(3);
k3 = r(4); p3 = p(4);
k4 = r(1); p4 = p(1);
k5 = r(2); p5 = p(2);

print_cosine_form(p2, k2)
print_cosine_form(p4, k4)