#include <qlibs.h> /*you can get this from the library manager*/

/*
PID Controller example using a simulated process
The controller is initially configured with non-optimal gains. 
The user can activate autotune by pressing the button. Once pressed,
auto-tune will work for a short period of time. During this activation 
period, the red LED L will remain on until the auto-tune process ends. 
The controller gains will be applied automatically.
NOTE: Please activate the graph view icon, to watch the dynamic response
*/

/*
Process transfer function:
         1.5
G(s) = --------
        3s + 1 
*/
constexpr real_t dt = 0.05f;    // Time step

continuousTF<1> processTransferFunction= {
    { 0.0f, 1.5f },
    { 3.0f, 1.0f },
};
continuousSystem process( processTransferFunction, dt ); // Simulated process

constexpr int setPointPin = A0; // select the input pin for the potentiometer that will drive the setPoint
constexpr int ledPin = 13;      // the number of the LED pin
constexpr int buttonPin = 2;    // the number of the pushbutton pin (To enable autoTune)
real_t yt = 0.0f;                                        // process output
pidController controller;                                // PID controller
pidAutoTuning at;                                        // AutoTune instance 

volatile bool enableAutoTunning = false;

void ButtonPress() {
  enableAutoTunning = true;
}

void setup() {
  // Setup the hardware
  Serial.begin(9600);
  pinMode(buttonPin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(buttonPin), ButtonPress, RISING );
  pinMode(ledPin, OUTPUT);
  // Setup the PID controller (With initial gains)
  controller.setup( 1.0f, 1.0f, 0.0f , dt ); /*Kc, Ki, Kd, dt*/
  controller.setSaturation( 0.0f, 100.0f );
  controller.bindAutoTuning( at );
  controller.setAutoTuningControllerType( pidType::PID_TYPE_PI );
} 

void loop() {
  if ( enableAutoTunning ) { 
    controller.enableAutoTuning( 100 ); //activate auto-tuning
    digitalWrite(ledPin, 1);
    enableAutoTunning = false;
  }
  if ( controller.isAutoTuningComplete() ) { //auto-tuning complete!
    digitalWrite(ledPin, 0);
  }

  /*Get the Set-Point (Scaling to 0-100%)*/
  real_t wt = map( analogRead( setPointPin ), 0, 1023, 0.0f, 100.0f );
  real_t ut = controller.control( wt, yt ); 
  real_t noise = random(-100,100)/250.0;
  yt = process.excite( ut ) + noise;
  /*enable graph window to see the control loop response*/
  Serial.print( ut );
  Serial.print( "," );
  Serial.print( wt );
  Serial.print( "," );
  Serial.println( yt );
  delay( dt*1000 ); /*wait the time-step*/
}