#include "sTune.h"
#include <QuickPID.h>

class PID_Controller {
  private:
    sTune *tuner;
    QuickPID *myPID;

    boolean controllBit = false;
    uint32_t settleTimeSec = 10;
    uint32_t testTimeSec = 1000;  // runPid interval = testTimeSec / samples
    uint16_t samples = 500;
    float inputSpan = 15;
    float outputSpan = 2500; // relay controll window
    float outputStart = 15;
    float outputStep = 5000; // how long the relay should be turned on
    float tempLimit = 35;
    float Input, Output, Setpoint = 80, Kp, Ki, Kd;
  public:
    void begin() {
      tuner = new sTune(&Input, &Output, tuner->ZN_PID, tuner->directIP, tuner->printSUMMARY);
      myPID = new QuickPID(&Input, &Output, &Setpoint);
      //myPID->SetProportionalMode(QuickPID::pMode::pOnError);
      tuner->Configure(inputSpan, outputSpan, outputStart, outputStep, testTimeSec, settleTimeSec, samples);
      tuner->SetEmergencyStop(tempLimit);
    }

    void setNewSetPoint(float newSetTemperature){
      Setpoint = newSetTemperature;
      inputSpan = Setpoint - Input;
      tuner->Configure(
            inputSpan,
            outputSpan,
            outputStart,
            outputStep,
            testTimeSec,
            settleTimeSec,
            samples
      );
    }

    boolean run(float temperature, float setTemperature) {
      Setpoint = setTemperature;
      tuner->softDigit(controllBit, Input, Output, Setpoint, outputSpan, 1);
      switch (tuner->Run()) {
        case tuner->sample:
          Input = temperature;
          tuner->plotter(Input, Output * 0.1, Setpoint, 1, 3);
          break;

        case tuner->tunings:    // active just once when sTune is done
          tuner->GetAutoTunings(&Kp, &Ki, &Kd); // sketch variables updated by sTune
          myPID->SetOutputLimits(0, outputSpan);
          myPID->SetSampleTimeUs(outputSpan * 1000 - 1);
          myPID->SetMode(myPID->Control::automatic); // the PID is turned on
          myPID->SetProportionalMode(myPID->pMode::pOnMeas);
          myPID->SetAntiWindupMode(myPID->iAwMode::iAwClamp);
          myPID->SetTunings(Kp, Ki, Kd); // update PID with the new tunings

          //tuner->GetAutoTunings(&Kp, &Ki, &Kd);
          //myPID->SetTunings(Kp, Ki, Kd);
          Serial.printf("[PID]- Updating with new tunings: Kp: %0.2f, Ki: %0.2f, Kd: %0.2f\n", Kp, Ki, Kd);
          break;

        case tuner->runPid:
          Input = temperature;
          myPID->Compute();
          tuner->plotter(Input, Output, Setpoint, 0.1f, 3);
          break;
      }
      return controllBit;
    }
};
PID_Controller pid;

class Thermostat{
  private:
    const float MIN_TEMP = 0;
    const float MAX_TEMP = 35;

    // For simulation purposes only
    // I suspect that the temperature cooling more quickly than be hotter
    const int TEMP_INCREASE_INTERVAL = 10000;
    const int TEMP_DECREASE_INTERVAL = 7000;

    long lastIncrease     = 0;
    long lastDecrease     = 0;
    float temperature     = 15;
    float setTemperature  = 20;

  public:
    // Increasing the temperature for simulation every TEMP_INCREASE_INTERVAL only.
    void increaseTemp(){
      if(millis() - lastIncrease >= TEMP_INCREASE_INTERVAL){
        lastIncrease = millis();
        temperature++;
        Serial.printf("[THERMOSTAT] - Temperature increased to %0.2f c°\n",temperature);
      }
    }

    // Decreasing the temperature for simulation every TEMP_DECREASE_INTERVAL only.
    void decreaseTemp(){
      if(millis() - lastDecrease >= TEMP_DECREASE_INTERVAL){
        lastDecrease = millis();
        temperature--;
        temperature = constrain(temperature,MIN_TEMP,MAX_TEMP);
        Serial.printf("[THERMOSTAT] - Temperature decreased to %0.2f c°\n",temperature);
      }
    }

    // Used to set the set temperature.
    void setTemp(float setTemp){
      setTemperature = setTemp;
      pid.setNewSetPoint(setTemperature);
    }

    // Used to get the measured temperature.
    float getTemp(){
      return temperature;
    }
    
    // Used to get the setted temperature.
    float getSetTemp(){
      return setTemperature;
    }
};
Thermostat thermostat;

class Heater{
  private:
    // VALVE_OPEN_TIME can vary between 0 and 600 sec
    const int VALVE_OPEN_TIME = 5000;

    // last started time. used to measure the valve open time
    long startedMS = 0;
    // Status indicator for the heating process.
    boolean heating = false;
    // Status indicator for the valves.
    boolean isValveOpen = false;
  public:

    // Starting the heating process.
    // Will open the valves
    // start the boiler ( digitalWrite(BOILER_START_PIN,HIGH) )
    // start the circulation
    void start(){
      if(heating){ return; }
      heating = true;
      startedMS = millis();
      Serial.println("[HEATER] - Started.");
    }

    // Stopping the heating process.
    // Will stop the boiler ( digitalWrite(BOILER_START_PIN,LOW) )
    // start the post circulation
    // and close the valves if the circulation ended.
    void stop(){
      if(!heating){return;}
      // Should we wait here for the valve to open?
      if(!isValveOpen){return;}
      heating = false;
      Serial.println("[HEATER] - Stopped.");
    }

    // Check if the heating process is on or not.
    boolean isHeating(){
      return heating;
    }

    // Controlling the heating process. Simulating the valve open time.
    // Temperature does not increase if the valves are not opened since the hot water should flow somehow.
    void run(){
      if(heating){
        if( millis() - startedMS >= VALVE_OPEN_TIME ){
          thermostat.increaseTemp();
          isValveOpen = true;
        }
      }else{
        thermostat.decreaseTemp();
        isValveOpen = false;
      }
    }
};
Heater heater;


void setup() {
  Serial.begin(115200);
  pid.begin();
  thermostat.setTemp(17.0);
}

void loop() {
  boolean shouldHeat = pid.run( thermostat.getTemp(), thermostat.getSetTemp() );
  if(shouldHeat){
    heater.start();
  }else{
    heater.stop();
  }
  heater.run();
  delay(10);
}