#include <Arduino.h>

// Параметры ПИД-регулятора
float Kp = 2.0f;
float Ki = 0.5f;
float Kd = 0.1f;

// Параметры модели объекта
float tau = 10.0f;
float Kplant = 2.5f;
float disturbanceAmp = 50.0f;

const unsigned long dt_ms = 100;
const float dt_s = dt_ms / 1000.0f;

float plantState = 20.0f;
const float ambient = 20.0f;

// Переменные ПИД-регулятора
float integrator = 0.0f;
float prevMeasured = 0.0f;

// Для фильтрации производной
float derivativeFilter = 0.0f;
const float N = 15.0f; // Коэффициент фильтрации производной

// Для автотюнинга
bool autoTuneMode = false;
unsigned long autoTuneStartTime = 0;
float initialPV = 0.0f;

// Для логгирования
unsigned long startTime = 0;

static uint32_t rng_state;

void seed_rng() {
    uint32_t s = analogRead(A1);
    if (s == 0) s = 0x1234567;
    rng_state = s;
}

static inline float rand_signed() {
    rng_state ^= rng_state << 11;
    rng_state ^= rng_state >> 17;
    rng_state ^= rng_state << 5;
    float v = (rng_state & 0xFFFFFFF) / 2147483647.0f;
    return v * 2.0f - 1.0f;
}

int readRawADC() {
    return analogRead(A0);
}

float readSetpointFromADC(int raw) {
    float denom = (raw > 2000) ? 4095.0f : 1023.0f;
    return (raw / denom) * 50.0f; // масштаб 0..50°C
}

// Базовая версия ПИД с жестким ограничением
float pidComputeBasic(float setpoint, float measured) {
    float error = setpoint - measured;
    float P = Kp * error;

    integrator += error * dt_s;
    float I = Ki * integrator;
    
    // Ограничение интеграла (anti-windup)
    if (I > 100.0f) { I = 100.0f; integrator = I / Ki; }
    if (I < -100.0f) { I = -100.0f; integrator = I / Ki; }

    float derivative = (measured - prevMeasured) / dt_s;
    float D = -Kd * derivative;
    prevMeasured = measured;

    float out = P + I + D;
    if (out > 100.0f) out = 100.0f;
    if (out < -100.0f) out = -100.0f;
    return out;
}

// Улучшенная версия с anti-windup по обратной связи
float pidComputeImproved(float setpoint, float measured) {
    float error = setpoint - measured;
    float P = Kp * error;

    // Интегральная составляющая
    integrator += error * dt_s;
    float I = Ki * integrator;

    // Производная составляющая
    float derivative = (measured - prevMeasured) / dt_s;
    float D = -Kd * derivative;

    // НЕограниченный выход
    float u_unsat = P + I + D;
    
    // Ограниченный выход
    float u_sat = u_unsat;
    if (u_sat > 100.0f) u_sat = 100.0f;
    if (u_sat < -100.0f) u_sat = -100.0f;

    // Anti-windup по обратной связи
    if (Ki > 0.001f) { // Избегаем деления на ноль
        integrator += (u_sat - u_unsat) / Ki * 0.1f; // Мягкая коррекция
    }

    prevMeasured = measured;
    return u_sat;
}

// Версия с фильтрованной производной
float pidComputeFiltered(float setpoint, float measured) {
    float error = setpoint - measured;
    float P = Kp * error;

    // Интегральная составляющая
    integrator += error * dt_s;
    float I = Ki * integrator;
    if (I > 100.0f) { I = 100.0f; integrator = I / Ki; }
    if (I < -100.0f) { I = -100.0f; integrator = I / Ki; }

    // Фильтрованная производная
    float derivative = (measured - prevMeasured) / dt_s;
    derivativeFilter = (N * derivativeFilter + derivative) / (N + 1.0f);
    float D = -Kd * derivativeFilter;

    float out = P + I + D;
    if (out > 100.0f) out = 100.0f;
    if (out < -100.0f) out = -100.0f;

    prevMeasured = measured;
    return out;
}

void updatePlant(float u) {
    float disturbance = rand_signed() * disturbanceAmp;
    float dy = - (plantState - ambient) / tau + Kplant * (u / 100.0f) + disturbance * 0.1f;
    plantState += dy * dt_s;
}

// Функция автотюнинга
void autoTune() {
    if (!autoTuneMode) {
        autoTuneMode = true;
        autoTuneStartTime = millis();
        initialPV = plantState;
        Serial.println("Auto-tuning started... Applying step input.");
        
        // Применяем ступенчатое воздействие
        float u_step = 50.0f;
        updatePlant(u_step);
        
        // Сохраняем текущие параметры
        float savedKp = Kp, savedKi = Ki, savedKd = Kd;
        
        // Ждем реакцию и измеряем параметры
        bool foundTheta = false, foundTau = false;
        float theta = 0.0f, tau_meas = 0.0f;
        float K = 0.0f;
        
        unsigned long stepTime = millis();
        while (!foundTau && (millis() - stepTime < 30000)) {
            float currentPV = plantState;
            float currentTime = (millis() - stepTime) / 1000.0f;
            
            // Определение запаздывания (θ)
            if (!foundTheta && currentPV > initialPV + 2.0f) {
                theta = currentTime;
                foundTheta = true;
                Serial.print("Theta: "); Serial.println(theta, 3);
            }
            
            // Определение 63% от установившегося значения
            if (foundTheta && !foundTau && currentPV > initialPV + 0.63 * (currentPV - initialPV)) {
                tau_meas = currentTime - theta;
                foundTau = true;
                K = (currentPV - initialPV) / u_step;
                
                Serial.print("Tau: "); Serial.println(tau_meas, 3);
                Serial.print("K: "); Serial.println(K, 3);
                
                // Расчет параметров по методу Чиена-Хроудера
                Kp = (1.2f * tau_meas) / (K * theta);
                Ki = Kp / (2.0f * theta);
                Kd = Kp * 0.5f * theta;
                
                Serial.println("Auto-tuning completed!");
                Serial.print("New Kp: "); Serial.println(Kp, 3);
                Serial.print("New Ki: "); Serial.println(Ki, 3);
                Serial.print("New Kd: "); Serial.println(Kd, 3);
            }
            
            updatePlant(u_step);
            delay(100);
        }
        
        if (!foundTau) {
            Serial.println("Auto-tuning failed: timeout");
            Kp = savedKp; Ki = savedKi; Kd = savedKd;
        }
        
        autoTuneMode = false;
    }
}

void setup() {
    Serial.begin(115200);
    seed_rng();
    startTime = millis();
    
    // Заголовок CSV
    Serial.println("Time(ms),Setpoint,ProcessValue,ControlOutput,Kp,Ki,Kd");
    
    Serial.println("PID Controller with Auto-tuning");
    Serial.println("Commands: 'a' - auto-tune, 'p' - P-only, 'i' - PI, 'f' - filtered");
    Serial.println("'1' - basic PID, '2' - improved anti-windup, '3' - filtered derivative");
}

void loop() {
    static unsigned long last = 0;
    unsigned long now = millis();
    if (now - last < dt_ms) return;
    last = now;

    // Обработка команд с Serial
    if (Serial.available()) {
        char cmd = Serial.read();
        switch (cmd) {
            case 'a': autoTune(); break;
            case 'p': Kp = 2.0f; Ki = 0.0f; Kd = 0.0f; Serial.println("P-only mode"); break;
            case 'i': Kp = 2.0f; Ki = 0.5f; Kd = 0.0f; Serial.println("PI mode"); break;
            case 'f': Kp = 2.0f; Ki = 0.5f; Kd = 0.1f; Serial.println("PID with filtering"); break;
            case '1': Serial.println("Basic PID"); break;
            case '2': Serial.println("Improved anti-windup"); break;
            case '3': Serial.println("Filtered derivative"); break;
        }
    }

    int raw = readRawADC();
    float sp = readSetpointFromADC(raw);
    float meas = plantState;
    
    float u;
    // Выбор алгоритма ПИД
    char mode = '1'; // Можно изменить на '2' или '3' для тестирования
    switch (mode) {
        case '2': u = pidComputeImproved(sp, meas); break;
        case '3': u = pidComputeFiltered(sp, meas); break;
        default: u = pidComputeBasic(sp, meas); break;
    }
    
    updatePlant(u);
    
    // Логгирование в CSV формате
    Serial.print(now - startTime);
    Serial.print(",");
    Serial.print(sp, 3);
    Serial.print(",");
    Serial.print(meas, 3);
    Serial.print(",");
    Serial.print(u, 3);
    Serial.print(",");
    Serial.print(Kp, 3);
    Serial.print(",");
    Serial.print(Ki, 3);
    Serial.print(",");
    Serial.println(Kd, 3);
}