#include <PID_v1.h>
#include <PID_AutoTune_v0.h>

// Simulated process variables
double Setpoint, ProcessVariable, ControllerOutput;

// PID parameters
double Kp = 2.0, Ki = 5.0, Kd = 1.0;

// PID controller
PID myPID(&ProcessVariable, &ControllerOutput, &Setpoint, Kp, Ki, Kd, DIRECT);

// PID autotune
PID_ATune aTune(&ProcessVariable, &ControllerOutput);

boolean tuning = false;

// Simulation parameters
double processGain = 1.0;
double processTimeConstant = 10.0; // Time constant of the process
double dt = 1.0; // Time step in seconds

unsigned long lastTime;

void setup()
{
  Serial.begin(9600);

  // Initial setpoint
  Setpoint = 50.0;
  ProcessVariable = 0.0;

  // Initialize PID controller
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(0, 100); // Assuming controller output between 0 and 100
  myPID.SetSampleTime(dt * 1000); // Convert dt to milliseconds

  // Autotune parameters
  aTune.SetOutputStep(20);
  aTune.SetControlType(1); // 0=PI, 1=PID
  aTune.SetLookbackSec(10);
  aTune.SetNoiseBand(0.5);

  lastTime = millis();

  Serial.println("PID Simulation with Autotune");
  Serial.println("Type 't' and press Enter to start autotune");
}

void loop()
{
  unsigned long now = millis();
  if (now - lastTime >= dt * 1000)
  {
    lastTime = now;

    if (tuning)
    {
      // Autotuning
      byte val = aTune.Runtime();
      if (val != 0)
      {
        // Autotune finished
        tuning = false;

        // Retrieve tuned parameters
        Kp = aTune.GetKp();
        Ki = aTune.GetKi();
        Kd = aTune.GetKd();

        myPID.SetTunings(Kp, Ki, Kd);
        myPID.SetMode(AUTOMATIC); // Return to automatic control

        Serial.println("AutoTune Complete!");
        Serial.print("Kp: "); Serial.println(Kp);
        Serial.print("Ki: "); Serial.println(Ki);
        Serial.print("Kd: "); Serial.println(Kd);
      }
    }
    else
    {
      // PID control
      myPID.Compute();
    }

    // Simulate process
    simulateProcess();

    // Print variables
    Serial.print("Setpoint: "); Serial.print(Setpoint);
    Serial.print(" | ProcessVariable: "); Serial.print(ProcessVariable);
    Serial.print(" | ControllerOutput: "); Serial.println(ControllerOutput);
  }

  // Check for user input to start autotune
  if (Serial.available())
  {
    char ch = Serial.read();
    if (ch == 't' || ch == 'T')
    {
      startAutoTune();
    }
  }
}

void simulateProcess()
{
  // Simple first-order lag process simulation
  double delta = (ControllerOutput - ProcessVariable) * (dt / processTimeConstant);
  ProcessVariable += delta;
}

void startAutoTune()
{
  if (!tuning)
  {
    tuning = true;
    // Set the controller to manual mode during tuning
    myPID.SetMode(MANUAL);

    // Initialize autotune
    aTune.Cancel(); // Reset autotune in case it's already running
    aTune.SetNoiseBand(0.5);
    aTune.SetOutputStep(20);
    aTune.SetLookbackSec(10);
    aTune.SetControlType(1); // PID control

    Serial.println("AutoTune Started");
  }
}