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

/*
    This piece will simulate the room temperature
*/
class SimulatedRoomTemp {
  private:
    float tempIncreaseAmount = 0.9,
          tempDecreaseAmount = 0.1;

    long lastTempIncreaseMS = 0,
         lastTempDecreaseMS = 0;

    int tempIncreaseInterval = 3000,
        tempDecreaseInterval = 10000;

    float temperature = 15.0;

    void printTemp() {
      Serial.printf("[TEMPERATURE] - %0.2fc°\n", temperature);
    }
  public:
    // Returns the current temperature
    float getTemp() {
      return temperature;
    }
    // Increase the temperature with the defined parameters.
    void increase() {
      if ( millis() - lastTempIncreaseMS >= tempIncreaseInterval ) {
        lastTempIncreaseMS = millis();
        temperature = temperature + tempIncreaseAmount;
        printTemp();
      }
    }
    // Decrease the temperature with the defined parameters.
    void decrease() {
      if ( millis() - lastTempDecreaseMS >= tempDecreaseInterval ) {
        lastTempDecreaseMS = millis();
        temperature = temperature - tempDecreaseAmount;
        printTemp();
      }
    }
};
SimulatedRoomTemp roomTemp;

/*
    Boiler class will simulate the boiler start/stop
*/
class Boiler {
  private:
    const int heatingIndicatorPIN = 22;
    boolean started = false;
  public:

    void begin() {
      pinMode(heatingIndicatorPIN, OUTPUT);
    }

    boolean isStarted() {
      return started;
    }

    void start() {
      if (started) {
        return;
      }
      digitalWrite(heatingIndicatorPIN, HIGH);
      Serial.println("[BOILER] - Starting.");
      started = true;
    }

    void stop() {
      if (!started) {
        return;
      }
      digitalWrite(heatingIndicatorPIN, LOW);
      Serial.println("[BOILER] - Stopping.");
      started = false;
    }
};

/*
    Thermostat class will simulate the thermostat
*/
class Thermostat {
  private:
    float setTemp = 23.0;
    float measuredTemp = 0;
    float hysteresis = 0.3;
  public:
    void begin() {
      measuredTemp = roomTemp.getTemp();
      Serial.printf(
        "\n[THERMOSTAT INFO]\n- Measured temp: %0.2fc°\n- Set temp: %0.2fc°\n- Hysteresis: %0.2fc°\n[THERMOSTAT INFO]\n\n",
        measuredTemp,
        setTemp,
        hysteresis
      );
    }

    // Returns true if we should heat or not, based on the set parameters
    boolean shouldHeat() {
      measuredTemp = roomTemp.getTemp();
      if ( measuredTemp >= setTemp ) {
        return false;
      } else if ( measuredTemp <= (setTemp + hysteresis) ) {
        return true;
      }
      return false;
    }

    float getSetTemp(){
      return setTemp;
    }

    float getTemp() {
      return roomTemp.getTemp();
    }
};

/*
    Valve class will simulate the valve open and close
*/
#define VALVE_CLOSED_STATE 0
#define VALVE_OPENING_STATE 1
#define VALVE_OPEN_STATE 2
class Valve {
  private:
    const int output = 12;
    long openStartMS = 0;
    int openTime = 10000; // In millisec
    int state = VALVE_CLOSED_STATE;
  public:

    void begin() {
      pinMode(output, OUTPUT);
    }

    // Will start to open the valve.
    // Open time can take up from 0 to 600 seconds
    void open() {
      if ( state != VALVE_CLOSED_STATE ) {
        return;
      }
      state = VALVE_OPENING_STATE;
      openStartMS = millis();
      Serial.println("[VALVE] - Opening.");
    }

    // Start to close the valves
    // Closing time probably equal with the open time but we don't care for now.
    void close() {
      if ( state == VALVE_CLOSED_STATE ) {
        return;
      }
      state = VALVE_CLOSED_STATE;
      digitalWrite(output, LOW);
      Serial.println("[VALVE] - Closing.");
    }

    // Checks the time if the valves are opened or not.
    void run() {
      if ( state == VALVE_OPENING_STATE ) {
        if ( millis() - openStartMS >= openTime ) {
          state = VALVE_OPEN_STATE;
          digitalWrite(output, HIGH);
          Serial.println("[VALVE] - Fully opened.");
        }
      }
    }

    boolean isOpen() {
      return (state == VALVE_OPEN_STATE ? true : false);
    }
};

/*
    Pump class will simulate the pump start, stop and post circulation
*/
#define PUMP_STOPPED_STATE 0
#define PUMP_CIRCULATING_STATE 1
#define PUMP_POST_CIRCULATING_STATE 2
class Pump {
  private:
    const int output = 14;
    const int postCirculationOutput = 27;
    long postCirculationStartMS = 0;
    int postCirculationTime = 15000; // In millisec
    int state = PUMP_STOPPED_STATE;
  public:

    void begin() {
      pinMode(output, OUTPUT);
      pinMode(postCirculationOutput, OUTPUT);
    }

    // Starting the pumps.
    // Pumps should run if the boiler is heating the water
    // It must spread the hot water across the room
    void start() {
      if ( state != PUMP_STOPPED_STATE ) {
        return;
      }
      state = PUMP_CIRCULATING_STATE;
      digitalWrite(output, HIGH);
      Serial.println("[PUMP] - Starting.");
    }
    // Force stop the pump
    void stop() {
      if ( state == PUMP_STOPPED_STATE ) {
        return;
      }
      state = PUMP_STOPPED_STATE;
      digitalWrite(output, LOW);
      Serial.println("[PUMP] - Stopped.");
    }
    // Starting the post circulation
    // It is required to empty the hot water from the boiler
    void startPostCirculation() {
      if ( state == PUMP_POST_CIRCULATING_STATE || state == PUMP_STOPPED_STATE ) {
        return;
      }
      state = PUMP_POST_CIRCULATING_STATE;
      postCirculationStartMS = millis();
      digitalWrite(postCirculationOutput, HIGH);
      Serial.println("[PUMP] - Starting post circulation.");
    }

    // Returns true if the post circulation is in progress
    boolean isPostCirculation() {
      return (state == PUMP_POST_CIRCULATING_STATE ? true : false);
    }

    // Checks the post circulation progress end
    void run() {
      if ( state == PUMP_POST_CIRCULATING_STATE ) {
        if ( millis() - postCirculationStartMS >= postCirculationTime ) {
          Serial.println("[PUMP] - Post circulation end.");
          digitalWrite(postCirculationOutput, LOW);
          stop();
        }
      }
    }
};

/*
  Auto tune PID implementation
*/
class PID {
  private:
    boolean shouldStartHeat = false;

    uint32_t settleTimeSec = 1;     // cool down time for the input temperature to stabilize
    uint32_t testTimeSec = 1000;    // runPid interval = testTimeSec / samples
    const uint16_t samples = 1000;  // sample amount
    const float inputSpan = 200;    // starter input temperature
    const float outputSpan = 10000;  // how often we should turn on
    float outputStart = 150;         // starter computed output ?
    float outputStep = 10000;        // how long should we turn on
    float tempLimit = 35.0;         // maximum temperature

    float Input,      // measured temperature
          Output = 0, // computed output
          Setpoint,   // set temperature
          Kp,
          Ki,
          Kd;

    sTune *tuner;
    QuickPID *myPID;
  public:

    void setSetPoint( float setTemp ){
      Setpoint = setTemp;
    }

    boolean shouldHeat( float temp ) {
      Input = temp;
      return shouldStartHeat;
    }

    void begin( float temp, float setTemp ) {
      Input = temp;
      Setpoint = setTemp;
      tuner = new sTune(&Input, &Output, tuner->ZN_PID, tuner->directIP, tuner->printSUMMARY);
      myPID = new QuickPID(&Input, &Output, &Setpoint);
      tuner->Configure(inputSpan, outputSpan, outputStart, outputStep, testTimeSec, settleTimeSec, samples);
      tuner->SetEmergencyStop(tempLimit);
    }

    void run() {
      float optimumOutput = tuner->softDigit(shouldStartHeat, Input, Output, 0, outputSpan, 1);
      switch (tuner->Run()) {
        case tuner->sample: // active once per sample during test
          tuner->plotter(Input, Output, Setpoint, 0.5f, 1); // output scale 0.5, plot every 3rd sample
          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 * 0.1);
          myPID->SetSampleTimeUs((outputSpan - 1) * 1000);
          Output = outputStep;
          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
          break;

        case tuner->runPid: // active once per sample after tunings
          myPID->Compute();
          tuner->plotter(Input, optimumOutput, Setpoint, 0.5f, 1);
          break;
      }
    }
};


class Heater {
  private:
    Thermostat myThermostat;
    Boiler myBoiler;
    Valve myValve;
    Pump myPump;
    PID myPID;

    boolean usePID      = true;
    boolean shouldHeat  = false;
    boolean heatEnd     = false;
    boolean postCirculationStarted = false;

    void checkHeating() {
      if (shouldHeat) {
        heatEnd = false;
        if ( !myValve.isOpen()) {
          myValve.open();
        } else {
          myBoiler.start();
          myPump.start();
          roomTemp.increase();
        }
      } else if (!heatEnd) {
        if (!postCirculationStarted) {
          myPump.startPostCirculation();
          postCirculationStarted = true;
        }
        if (!myPump.isPostCirculation()) {
          myPump.stop();
          myBoiler.stop();
          myValve.close();
          heatEnd = true;
          postCirculationStarted = false;
          return;
        }
      } else {
        roomTemp.decrease();
      }
    }

  public:

    void begin() {
      myThermostat.begin();
      myBoiler.begin();
      myValve.begin();
      myPump.begin();
      if (usePID) {
        myPID.begin( myThermostat.getTemp(), myThermostat.getSetTemp() );
      }
    }

    void run() {
      // Getting a controll bit from either the thermostat or the pid
      if (!usePID) {
        shouldHeat = myThermostat.shouldHeat();
      } else {
        shouldHeat = myPID.shouldHeat( myThermostat.getTemp() );
      }
      myValve.run();
      myPump.run();
      checkHeating();
      if (usePID) {
        myPID.run();
      }
    }

};
Heater heater;


void setup() {
  Serial.begin(115200);
  Serial.println("Starting heat controll simulation.");
  heater.begin();
}

void loop() {
  delay(10);
  heater.run();
}