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

using namespace qlibs;

/*
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
*/

constexpr real_t dt = 0.05f;    // Time step
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;

real_t win[ 6 ];
real_t weighs[ 6 ];
smootherALNF filter;

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.0_kc + 1.0_ki , dt ); /*Kc, Ki, Kd, dt*/
  controller.setSaturation( 0.0f, 100.0f );
  controller.bindAutoTuning( at );
  controller.setAutoTuningControllerType( pidType::PID_TYPE_PI );
  controller.setAutoTuningParameters( 0.95f, 0.75f, 0.9898f, 100.0 );
  filter.setup(0.01,win,weighs);
  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 ) {
  static integrator h( dt, 0.0f );
  h.setSaturation( 0.0f, 100.0f );
  constexpr real_t Kv = 0.45f;
  constexpr real_t At = 1.0f;
  constexpr real_t g = 9.8f;
  constexpr real_t Ao = 1.0f;
  const real_t b = Kv/At;
  const real_t a = Ao*ffmath::sqrt(2*g)/At;

  real_t noise = random(-100,100)/250.0;
  real_t dh = b*ut - a*ffmath::sqrt( h() );
  return h( dh ) + 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() {
  if ( enableAutoTunning ) { 
    controller.enableAutoTuning( 100 ); //activate auto-tuning
    digitalWrite(ledPin, 1);
    tone(12, 160);
    enableAutoTunning = false;
  }
  if ( controller.isAutoTuningComplete() ) { //auto-tuning complete!
    digitalWrite(ledPin, 0);
    noTone(12);
  }

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