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;

static uint32_t rng_state;

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

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

void setup() {
  Serial.begin(115200);
  seed_rng();
}

int readRawADC() {
  return analogRead(A0);  // Потенциометр на пине A0
}

float readSetpointFromADC(int raw) {
  return (raw / 1023.0f) * 100.0f; // Преобразуем 0-1023 в 0-100%
}

float pidCompute(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 < 0.0f) { I = 0.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 < 0.0f) out = 0.0f;
  return out;
}

void updatePlant(float u) {
  // Случайные помехи
  float disturbance = randf_signed() * disturbanceAmp;
  
  // Модель растения: инерционное звено + помехи
  float target = ambient + Kplant * u;
  float dy = (target - plantState) / tau + disturbance * 0.01f;
  plantState += dy * dt_s;
  
  if (plantState < 0.0f) plantState = 0.0f;
}

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

  int raw = readRawADC();
  float sp = readSetpointFromADC(raw);  // Setpoint от потенциометра
  float meas = plantState;              // Текущее значение
  float u = pidCompute(sp, meas);       // Управляющее воздействие
  updatePlant(u);

  // Для Serial Plotter
  Serial.print("Setpoint:");
  Serial.print(sp);
  Serial.print(" Actual:");
  Serial.print(meas);
  Serial.print(" Power:");
  Serial.println(u);
}