#include <stdio.h>
#include <stdbool.h>

typedef struct st {
    float x1;
    float x2;
} state;

// rownania rozniczkowe
state inert2_diffs (state current, float u) {
    state diffs;
    // parametry ukladu dynamicznego
    float k = 3.0;
    float T1 = 2.0;
    float T2 = 3.0;

    float mult = T1 * T2;
    diffs.x1 = current.x2;
    diffs.x2 = -(T1 + T2) / mult * current.x2 - 1 / mult * current.x1 + k / mult * u;
    return diffs;
}

// calkowanie rownan rozniczkowych z uzyciem metody Eulera
state euler_inert2 (state previous, float u, float h) {
    state next;
    state diffs = inert2_diffs (previous, u);
    next.x1 = previous.x1 + h * diffs.x1;
    next.x2 = previous.x2 + h * diffs.x2;
    return next;
};

// symulacja ukladu otwartego
void open_loop (float t_sim, float h, float u) {
    FILE *f = fopen("open.txt", "w");
    float t = 0.0;
    state s;
    // warunek poczatkowy
    s.x1 = 0.0;
    s.x2 = 0.0;

    while (t <= t_sim) {
        fprintf(f, "%.4f;%.4f;%.4f\n", t, s.x1, s.x2);
        s = euler_inert2 (s, u, h);
        t += h;
    }
    fclose (f);
}

// implementacja cyfrowego regulatora PID
float pid (float e, float h) {
    float val = 0.0;
    // nastawy regulatora
    float Kp = 2.9635;
    float Ti = 4.8004;
    float Td = 0.7152;
    // wartosc maksymalna i minimalna sterowania
    float max_u = 2.0;
    float min_u = -2.0;
    // wartosci sterowania z poszczegolnych czlonow
    float up = 0.0, ui = 0.0, ud = 0.0;
    // calka uchybu
    static float sum_e = 0.0;
    // ostatnia wartosc uchybu
    static bool ud_enable = false;
    static float last_e = 0.0;

    // czlon proporcjonalny
    up = Kp * e;
    // czlon calkujacy
    sum_e += e;
    ui = sum_e * h * Kp / Ti;
    // czlon rozniczkujacy
    if (ud_enable)
        ud = (e - last_e) / h * Kp * Td;
    ud_enable = true;
    last_e = e;
    // sterowanie z regulatora
    val = up + ui + ud;
    // ograniczenie wartosci sterujacej i wylaczenie calki
    if (val > max_u) {
        val = max_u;
        sum_e -= e;
    } else if (val < min_u) {
        val = min_u;
        sum_e -= e;
    }

    return val;
}

// symulacja ukladu regulacji PID
void closed_loop (float t_sim, float h, float w) {
    FILE *f = fopen("closed.txt", "w");
    float t = 0.0;
    float e, u = 0.0;
    state s;
    // warunek poczatkowy
    s.x1 = 0.0;
    s.x2 = 0.0;

    while (t <= t_sim) {
        fprintf(f, "%.4f;%.4f;%.4f;%.4f;%.4f\n", t, s.x1, s.x2, u, w);
        e = w - s.x1;
        u = pid (e, h);
        s = euler_inert2 (s, u, h);
        t += h;
    }
    fclose (f);
}

int main () {
    float h = 0.01; // krok symulacji
    float t_sim = 20.0f; // czas symulacji
    float u = 1.0; // wartosc sterowania / wartosc zadana

    open_loop (t_sim, h, u);
    closed_loop (t_sim, h, u);

    return 0;
}