Polynomial num; // Numerator polynomial in z

Polynomial den; // Denominator polynomial in z

double Ts; // Sample time (seconds)

// ============ Constructors ============

/// Default: H(z) = 1, Ts = 1

DiscreteTransferFunction() : num({1}), den({1}), Ts(1.0) {}

/// From polynomials and sample time

DiscreteTransferFunction(const Polynomial& n, const Polynomial& d, double ts = 1.0)

: num(n), den(d), Ts(ts) {

if (den.isZero()) throw std::runtime_error("Denominator cannot be zero");

}

/// From coefficient vectors

DiscreteTransferFunction(

const std::vector<double>& n,

const std::vector<double>& d,

double ts = 1.0

) : num(n), den(d), Ts(ts) {

if (den.isZero()) throw std::runtime_error("Denominator cannot be zero");

}

/// From initializer lists

DiscreteTransferFunction(

std::initializer_list<double> n,

std::initializer_list<double> d,

double ts = 1.0

) : num(n), den(d), Ts(ts) {

if (den.isZero()) throw std::runtime_error("Denominator cannot be zero");

}

// ============ Properties ============

/// Order of the system

int order() const { return den.degree(); }

/// DC gain: H(1)

double dcgain() const {

double d1 = den(1.0);

if (std::abs(d1) < 1e-15) return std::numeric_limits<double>::infinity();

return num(1.0) / d1;

}

/// Sample frequency (rad/s)

double omega_s() const { return 2 * M_PI / Ts; }

/// Nyquist frequency (rad/s)

double omega_nyquist() const { return M_PI / Ts; }

// ============ Poles and Zeros ============

/// Get poles (roots of denominator in z-plane)

std::vector<std::complex<double>> poles() const {

return den.roots();

}

/// Get zeros (roots of numerator in z-plane)

std::vector<std::complex<double>> zeros() const {

return num.roots();

}

// ============ Stability ============

/// Check if system is stable (all poles inside unit circle)

bool isStable() const {

auto p = poles();

for (const auto& pole : p) {

if (std::abs(pole) >= 1.0) return false;

}

return true;

}

// ============ Frequency Response ============

/// Evaluate at z

std::complex<double> eval(std::complex<double> z) const {

return num(z) / den(z);

}

/// Evaluate at z = e^(jωT) (frequency response)

std::complex<double> freqresp(double omega) const {

std::complex<double> z = std::polar(1.0, omega * Ts);

return eval(z);

}

/// Magnitude at frequency ω (linear)

double mag(double omega) const {

return std::abs(freqresp(omega));

}

/// Magnitude in dB

double mag_dB(double omega) const {

double m = mag(omega);

if (m < 1e-20) return -400;

return 20.0 * std::log10(m);

}

/// Phase in radians

double phase(double omega) const {

return std::arg(freqresp(omega));

}

/// Phase in degrees

double phase_deg(double omega) const {

return phase(omega) * 180.0 / M_PI;

}

// ============ Arithmetic ============

DiscreteTransferFunction operator*(const DiscreteTransferFunction& other) const {

if (std::abs(Ts - other.Ts) > 1e-10) {

throw std::runtime_error("Sample times must match for series connection");

}

return DiscreteTransferFunction(num * other.num, den * other.den, Ts);

}

DiscreteTransferFunction operator+(const DiscreteTransferFunction& other) const {

if (std::abs(Ts - other.Ts) > 1e-10) {

throw std::runtime_error("Sample times must match for parallel connection");

}

Polynomial newNum = num * other.den + other.num * den;

Polynomial newDen = den * other.den;

return DiscreteTransferFunction(newNum, newDen, Ts);

}

DiscreteTransferFunction operator*(double K) const {

return DiscreteTransferFunction(num * K, den, Ts);

}

// ============ String Representation ============

std::string toString() const {

std::ostringstream oss;

std::string numStr = num.toString("z");

std::string denStr = den.toString("z");

size_t width = std::max(numStr.length(), denStr.length());

size_t numPad = (width - numStr.length()) / 2;

oss << std::string(numPad, ' ') << numStr << "\n";

oss << std::string(width, '-') << "\n";

size_t denPad = (width - denStr.length()) / 2;

oss << std::string(denPad, ' ') << denStr;

oss << "\n\nTs = " << Ts << " s";

return oss.str();

}

int n = P.degree();

double k = 2.0 / Ts;

// Result: P(s) with s=(2/T)*(z-1)/(z+1) = sum_i a_i * (k*(z-1))^i / (z+1)^i

// Numerator = sum_i a_i * k^i * (z-1)^i * (z+1)^(n-i)

// Denominator = (z+1)^n

// Build (z-1)^i and (z+1)^i powers

std::vector<Polynomial> z_minus_1_pow(n + 1);

std::vector<Polynomial> z_plus_1_pow(n + 1);

z_minus_1_pow[0] = Polynomial({1.0}); // (z-1)^0 = 1

z_plus_1_pow[0] = Polynomial({1.0}); // (z+1)^0 = 1

Polynomial z_minus_1({1.0, -1.0}); // z - 1

Polynomial z_plus_1({1.0, 1.0}); // z + 1

for (int i = 1; i <= n; ++i) {

z_minus_1_pow[i] = z_minus_1_pow[i-1] * z_minus_1;

z_plus_1_pow[i] = z_plus_1_pow[i-1] * z_plus_1;

}

// Build numerator: sum_i a_i * k^i * (z-1)^i * (z+1)^(n-i)

Polynomial num_result({0.0});

double ki = 1.0; // k^i

for (int i = 0; i <= n; ++i) {

// Coefficient a_i (from highest to lowest power in original polynomial)

// P.coeffs[0] is coefficient of s^n, P.coeffs[n] is coefficient of s^0

double a_i = P.coeffs[n - i]; // coefficient of s^i

// Term: a_i * k^i * (z-1)^i * (z+1)^(n-i)

Polynomial term = z_minus_1_pow[i] * z_plus_1_pow[n - i];

term = term * (a_i * ki);

num_result = num_result + term;

ki *= k;

}

// Denominator: (z+1)^n

Polynomial den_result = z_plus_1_pow[n];

return std::make_pair(num_result, den_result);

// Apply Tustin substitution to both numerator and denominator

std::pair<Polynomial, Polynomial> num_result = tustin_substitute(Gc.num, Ts);

std::pair<Polynomial, Polynomial> den_result = tustin_substitute(Gc.den, Ts);

Polynomial num_z_num = num_result.first;

Polynomial num_z_den = num_result.second;

Polynomial den_z_num = den_result.first;

Polynomial den_z_den = den_result.second;

// H(z) = [num_z_num/num_z_den] / [den_z_num/den_z_den]

// = (num_z_num * den_z_den) / (den_z_num * num_z_den)

Polynomial final_num = num_z_num * den_z_den;

Polynomial final_den = den_z_num * num_z_den;

// Normalize so leading coefficient of denominator is 1

double scale = final_den.coeffs[0];

if (std::abs(scale) > 1e-15) {

for (double& c : final_num.coeffs) c /= scale;

for (double& c : final_den.coeffs) c /= scale;

}

// Remove near-zero leading coefficients

while (final_num.coeffs.size() > 1 && std::abs(final_num.coeffs.front()) < 1e-15) {

final_num.coeffs.erase(final_num.coeffs.begin());

}

while (final_den.coeffs.size() > 1 && std::abs(final_den.coeffs.front()) < 1e-15) {

final_den.coeffs.erase(final_den.coeffs.begin());

}

return DiscreteTransferFunction(final_num, final_den, Ts);

// For first-order: G(s) = K / (τs + 1)

if (Gc.order() == 1) {

double K = Gc.dcgain();

double tau = -1.0 / Gc.poles()[0].real();

double alpha = std::exp(-Ts / tau);

double b0 = K * (1 - alpha);

return DiscreteTransferFunction({b0}, {1, -alpha}, Ts);

}

// For second-order systems

if (Gc.order() == 2) {

auto p = Gc.poles();

double K = Gc.dcgain();

double p1_real = p[0].real();

double p1_imag = p[0].imag();

double p2_real = p[1].real();

double p2_imag = p[1].imag();

// Check if underdamped (complex conjugate poles)

if (std::abs(p1_imag) > 1e-10) {

double sigma = p1_real;

double omega_d = std::abs(p1_imag);

double alpha = std::exp(sigma * Ts);

double cos_wd = std::cos(omega_d * Ts);

double sin_wd = std::sin(omega_d * Ts);

// Denominator: (z - e^(σT)e^(jω_dT))(z - e^(σT)e^(-jω_dT))

// = z² - 2αcos(ω_dT)z + α²

double a1 = -2 * alpha * cos_wd;

double a2 = alpha * alpha;

// Numerator from step response matching

// Using partial fractions and inverse z-transform

double omega_n2 = p1_real * p1_real + p1_imag * p1_imag; // ω_n²

double zeta_wn = -p1_real; // ζω_n

double b0 = K * (1 - alpha * (cos_wd + zeta_wn / omega_d * sin_wd));

double b1 = K * (alpha * alpha - alpha * (cos_wd - zeta_wn / omega_d * sin_wd));

return DiscreteTransferFunction({b0, b1}, {1, a1, a2}, Ts);

}

// Overdamped (two distinct real poles)

else if (std::abs(p1_real - p2_real) > 1e-10) {

double p1 = p1_real; // First real pole

double p2 = p2_real; // Second real pole

double e1 = std::exp(p1 * Ts); // e^(p1*T)

double e2 = std::exp(p2 * Ts); // e^(p2*T)

// Denominator: (z - e1)(z - e2) = z² - (e1+e2)z + e1*e2

double a1 = -(e1 + e2);

double a2 = e1 * e2;

// Numerator from partial fractions:

// G(s)/s = A/s + B/(s-p1) + C/(s-p2)

// where A = K, B = K*p2/(p1*(p1-p2)), C = K*p1/(p2*(p2-p1))

double A = K;

double B = K * p2 / (p1 * (p1 - p2));

double C = K * p1 / (p2 * (p2 - p1));

// Z{1-z^-1} * Z{A + B*e^(p1*t) + C*e^(p2*t)}

// = A*(1-z^-1)*z/(z-1) + B*(1-z^-1)*z/(z-e1) + C*(1-z^-1)*z/(z-e2)

// = A + B*(z-1)/(z-e1) + C*(z-1)/(z-e2)

double b0 = A + B + C - A; // Coefficient of z in numerator / leading den coeff

double b1 = A * (e1 + e2 - 2) + B * (1 - e2) + C * (1 - e1);

// Simplified form using step response invariance

b0 = K * (1 + (p2 * e1 - p1 * e2) / (p1 - p2) - p1 * p2 / (p1 * p2));

b1 = K * (e1 * e2 - 1 + (p1 * e2 - p2 * e1) / (p1 - p2));

// Alternative: Direct computation from step response

// At t=0: y(0) = 0

// At t=T: y(T) = K * (1 - p2/(p2-p1)*e^(p1*T) + p1/(p2-p1)*e^(p2*T))

double y_T = K * (1 - p2/(p2-p1)*e1 + p1/(p2-p1)*e2);

double y_2T = K * (1 - p2/(p2-p1)*e1*e1 + p1/(p2-p1)*e2*e2);

// H(z) = b0 + b1*z^-1 = (b0*z + b1) / (z² + a1*z + a2)

// From step response: y[1] = b0, y[2] = -a1*y[1] - a2*y[0] + b0 + b1

b0 = y_T;

b1 = y_2T + a1 * y_T - y_T; // y[2] = y_T + b1 - a1*b0

return DiscreteTransferFunction({b0, b1}, {1, a1, a2}, Ts);

}

// Critically damped (repeated real pole)

else {

double p = p1_real; // Repeated pole

double e_pT = std::exp(p * Ts);

// Denominator: (z - e^(pT))² = z² - 2e^(pT)z + e^(2pT)

double a1 = -2 * e_pT;

double a2 = e_pT * e_pT;

// G(s) = K*ω_n² / (s+ω_n)² where ω_n = -p

double omega_n = -p;

// Step response: y(t) = K * (1 - (1 + ω_n*t) * e^(-ω_n*t))

double y_T = K * (1 - (1 + omega_n * Ts) * e_pT);

double y_2T = K * (1 - (1 + omega_n * 2 * Ts) * e_pT * e_pT);

double b0 = y_T;

double b1 = y_2T - (1 + a1) * y_T;

return DiscreteTransferFunction({b0, b1}, {1, a1, a2}, Ts);

}

}

// For higher order or systems with zeros: use step-invariant approximation

// Simulate continuous step response and fit discrete model

if (Gc.order() >= 1) {

int n = Gc.order();

// Simulate step response at sample times using numerical integration

std::vector<double> t_samples, y_samples;

double t = 0;

double dt = Ts / 100.0; // Fine integration step

// Simple state for numerical integration (Euler method for simulation)

// This is approximate but works for generating ZOH equivalent

std::vector<double> y_step;

y_step.push_back(0); // y[0] = 0

// Use small-step integration to get y(kT) values

for (int k = 1; k <= n + 2; ++k) {

double target_t = k * Ts;

double y_current = 0;

// Integrate from 0 to target_t using step input

// This is a crude simulation - for production use proper ODE solver

int steps = static_cast<int>(target_t / dt);

double t_int = 0;

// For low-order systems, use analytical step response if possible

if (n <= 2) {

// Already handled above

}

// Approximate using DC gain and dominant pole

auto poles_vec = Gc.poles();

std::complex<double> dom_pole = poles_vec[0];

for (const auto& pp : poles_vec) {

if (pp.real() > dom_pole.real()) dom_pole = pp;

}

double tau_dom = -1.0 / dom_pole.real();

y_current = Gc.dcgain() * (1 - std::exp(-target_t / tau_dom));

y_step.push_back(y_current);

}

// Build discrete TF from step response samples

// This is a simplified approach - in practice, use exact ZOH formulas

// Fall back to Tustin for robustness

return c2d_tustin(Gc, Ts);

}

// Fallback to Tustin for unsupported cases

return c2d_tustin(Gc, Ts);

// Frequency range: 0 to Nyquist

std::vector<double> omega;

double w_nyq = H.omega_nyquist();

for (double w = w_nyq / 1000; w <= w_nyq; w *= 1.03) {

omega.push_back(w);

}

std::vector<double> mag, phase_raw;

for (double w : omega) {

mag.push_back(H.mag_dB(w));

phase_raw.push_back(H.phase_deg(w));

}

// Unwrap phase

std::vector<double> phase = phase_raw;

for (size_t i = 1; i < phase.size(); ++i) {

while (phase[i] - phase[i-1] > 180) phase[i] -= 360;

while (phase[i] - phase[i-1] < -180) phase[i] += 360;

}

figure(800, 600);

layout(2, 1);

subplot(2, 1, 1);

plot(omega, mag, "-", opts({{"color", "#1f77b4"}, {"linewidth", "2"}}));

xscale("log");

axhline(0, opts({{"color", "black"}, {"linestyle", "-"}, {"linewidth", "0.5"}}));

axhline(-3, opts({{"color", "gray"}, {"linestyle", ":"}, {"linewidth", "0.8"}}));

axvline(w_nyq, opts({{"color", "red"}, {"linestyle", "--"}, {"linewidth", "1"}}));

text(w_nyq * 0.7, 5, "Nyquist", opts({{"fontsize", "9"}, {"color", "red"}}));

ylabel("Magnitude (dB)");

grid(true);

subplot(2, 1, 2);

plot(omega, phase, "-", opts({{"color", "#1f77b4"}, {"linewidth", "2"}}));

xscale("log");

axhline(-180, opts({{"color", "gray"}, {"linestyle", ":"}, {"linewidth", "0.5"}}));

axvline(w_nyq, opts({{"color", "red"}, {"linestyle", "--"}, {"linewidth", "1"}}));

xlabel("Frequency (rad/s)");

ylabel("Phase (deg)");

grid(true);

std::ostringstream title_str;

title_str << "Discrete Bode Diagram (Ts = " << H.Ts << " s)";

suptitle(title_str.str());

auto p = H.poles();

auto z = H.zeros();

std::vector<double> poles_re, poles_im;

std::vector<double> zeros_re, zeros_im;

for (const auto& pole : p) {

poles_re.push_back(pole.real());

poles_im.push_back(pole.imag());

}

for (const auto& zero : z) {

zeros_re.push_back(zero.real());

zeros_im.push_back(zero.imag());

}

figure(700, 700);

// Unit circle (stability boundary)

std::vector<double> circle_x, circle_y;

for (int i = 0; i <= 360; ++i) {

double theta = i * M_PI / 180.0;

circle_x.push_back(std::cos(theta));

circle_y.push_back(std::sin(theta));

}

plot(circle_x, circle_y, "-", opts({

{"color", "gray"},

{"linewidth", "1.5"},

{"linestyle", "--"}

}));

// Fill unstable region (outside unit circle)

// Shade stable region (inside)

fill_between(

std::vector<double>{-1.5, 1.5},

std::vector<double>{-1.5, -1.5},

std::vector<double>{1.5, 1.5},

opts({{"color", "red"}, {"alpha", "0.05"}})

);

// Axes

axhline(0, opts({{"color", "black"}, {"linewidth", "0.8"}}));

axvline(0, opts({{"color", "black"}, {"linewidth", "0.8"}}));

// Plot zeros (o)

if (!zeros_re.empty()) {

scatter(zeros_re, zeros_im, opts({

{"s", "60"}, {"color", "blue"}, {"marker", "o"}

}));

}

// Plot poles (x)

if (!poles_re.empty()) {

scatter(poles_re, poles_im, opts({

{"s", "60"}, {"color", "red"}, {"marker", "x"}

}));

}

xlabel("Real Axis");

ylabel("Imaginary Axis");

title("Discrete Pole-Zero Map (z-plane)");

grid(true);

xlim(-1.5, 1.5);

ylim(-1.5, 1.5);

// Simulate step response using difference equation

// y[k] = -a1*y[k-1] - a2*y[k-2] - ... + b0*u[k] + b1*u[k-1] + ...

int n_num = H.num.degree() + 1;

int n_den = H.den.degree() + 1;

std::vector<double> a = H.den.coeffs; // Denominator coeffs

std::vector<double> b = H.num.coeffs; // Numerator coeffs

// Normalize by a[0]

double a0 = a[0];

for (double& ai : a) ai /= a0;

for (double& bi : b) bi /= a0;

std::vector<double> y(num_samples, 0); // Output

std::vector<double> u(num_samples, 1); // Step input (all 1s)

// Simulation

for (int k = 0; k < num_samples; ++k) {

double yk = 0;

// Add input terms

for (int i = 0; i < n_num && i <= k; ++i) {

yk += b[i] * u[k - i];

}

// Subtract output feedback terms

for (int i = 1; i < n_den && i <= k; ++i) {

yk -= a[i] * y[k - i];

}

y[k] = yk;

}

// Time vector

std::vector<double> t(num_samples);

for (int k = 0; k < num_samples; ++k) {

t[k] = k * H.Ts;

}

figure(800, 500);

// Stem plot (discrete)

for (int k = 0; k < num_samples; ++k) {

std::vector<double> stem_x = {t[k], t[k]};

std::vector<double> stem_y = {0, y[k]};

plot(stem_x, stem_y, "-", opts({{"color", "#1f77b4"}, {"linewidth", "1"}}));

}

scatter(t, y, opts({{"s", "20"}, {"color", "#1f77b4"}}));

// Steady-state reference

double ss = H.dcgain();

if (std::isfinite(ss)) {

axhline(ss, opts({{"color", "gray"}, {"linestyle", "--"}, {"linewidth", "1"}}));

}

axhline(0, opts({{"color", "black"}, {"linestyle", "-"}, {"linewidth", "0.5"}}));

xlabel("Time (s)");

ylabel("Amplitude");

std::ostringstream title_str;

title_str << "Discrete Step Response (Ts = " << H.Ts << " s)";

title(title_str.str());

grid(true);

std::cout << "======== Discrete System Info ========" << std::endl;

std::cout << "\nTransfer Function H(z):" << std::endl;

std::cout << H.toString() << std::endl;

std::cout << "\nProperties:" << std::endl;

std::cout << " Order: " << H.order() << std::endl;

std::cout << " Sample Time: " << H.Ts << " s" << std::endl;

std::cout << " Sample Freq: " << H.omega_s() << " rad/s" << std::endl;

std::cout << " Nyquist Freq: " << H.omega_nyquist() << " rad/s" << std::endl;

std::cout << " DC Gain: " << H.dcgain() << std::endl;

std::cout << " Stable: " << (H.isStable() ? "Yes" : "No") << std::endl;

auto p = H.poles();

std::cout << "\nPoles:" << std::endl;

for (size_t i = 0; i < p.size(); ++i) {

double mag = std::abs(p[i]);

std::cout << " z" << (i+1) << " = " << std::fixed << std::setprecision(4)

<< p[i].real();

if (std::abs(p[i].imag()) > 1e-10) {

std::cout << (p[i].imag() >= 0 ? " + " : " - ") << std::abs(p[i].imag()) << "j";

}

std::cout << " |z| = " << mag << (mag >= 1 ? " (UNSTABLE)" : "") << std::endl;

}

std::cout << "======================================" << std::endl;