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

using namespace qlibs;
/*
PID Controller example using Smith Predictor for a simulated process with delay
NOTE: Please activate the graph view icon, to watch the dynamic response
*/

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

continuousTF<1> processTransferFunction= {
    { 0.0f, 1.5f },
    { 3.0f, 1.0f },
};

continuousTF<1> modelTransferFunction= { //process model aproximation
    { 0.0f, 1.51f },
    { 2.98f, 1.0f },
};

continuousSystem process( processTransferFunction, dt ); // Simulated process
continuousSystem model( modelTransferFunction, dt );   // process model aproximation
transportDelay< 2.5_td(dt) > processDelay;
transportDelay< 2.5_td(dt) > modelDelay;
smithPredictor smith( model, modelDelay );
pidController controller;

void setup() {
  Serial.begin(9600);
  controller.setup( 5.0_kc + 7.0_ki + 0.05_kd , dt );
  controller.setSaturation( 0.0f, 100.0f );
  Serial.println(qlibs::product::caption);
}

real_t getSetPoint() {
  /*Get the Set-Point by reading A0 (Scaling to 0-100%)*/
  return map( analogRead( A0 ), 0, 1023, 0.0f, 100.0f );
}

real_t simulateProcess(real_t ut ) {
  real_t noise = random(-100,100)/250.0;
  return  processDelay( process( ut ) ) + noise;
}

void graphResponse() {}
template<typename T, typename... Rest>
void graphResponse(T first, Rest... rest) {
    Serial.print(first);
    if (sizeof...(rest) > 0) {
        Serial.print( "," );
        graphResponse(rest...);
    }
    else {
      Serial.println();
    }
}

void loop() {
  real_t wt = getSetPoint();
  real_t ut = controller( wt, smith.getPrediction() ); 
  real_t yt = simulateProcess( ut );
  smith.updatePrediction( ut, yt );
  graphResponse( ut, wt, yt ); /* enable graph window to see the control loop response */
  delay( dt*1000 ); /*wait the time-step*/ 
}