// https://wokwi.com/projects/384972541717315585
// is
// https://github.com/br3ttb/Arduino-PID-Library/blob/master/examples/PID_RelayOutput/PID_RelayOutput.ino
// adapted to https://forum.arduino.cc/t/time-spent-in-a-function/1202863/17?u=davex

/********************************************************
   PID RelayOutput Example
   Same as basic example, except that this time, the output
   is going to a digital pin which (we presume) is controlling
   a relay.  the pid is designed to Output an analog value,
   but the relay can only be On/Off.

     to connect them together we use "time proportioning
   control"  it's essentially a really slow version of PWM.
   first we decide on a window size (5000mS say.) we then
   set the pid to adjust its output between 0 and that window
   size.  lastly, we add some logic that translates the PID
   output into "Relay On Time" with the remainder of the
   window being "Relay Off Time"
 ********************************************************/

//#include <PID_v1.h> // https://github.com/br3ttb/Arduino-PID-Library
#include <PID_v1_bc.h> // https://github.com/drf5n/Arduino-PID-Library_bc

#define PIN_INPUT A0 // setpoint Pot or temperature
#define RELAY_PIN 8 // active low relay pin

bool powerOn = false;  // flag for

static float temp = 25, Tamb = 20;  // °C
static float noisyTemp = temp;

//Define Variables we'll be connecting to
double Setpoint, Input, Output;

//unsigned long WindowSize = 5 * 60000UL; // 5 minutes
//unsigned long WindowSize = 1 * 60000UL; // 1 minutes
//unsigned long WindowSize = 5 * 60 * 1000UL; //
unsigned long WindowSize = 10 * 1000UL; //

unsigned long windowStartTime;

//Specify the links and initial tuning parameters
// per https://blog.opticontrols.com/archives/697 level PID
// slope0 = -0.01434 deg/sec
// slope1 = 0.17184 deg/sec
// deltaCO = 10000 ms/10000ms
// ri = (0.17184 - -0.01434)/10000 = 1.8618e-05 deg/sec/msOn
// dt = 1
// so for 
/* To calculate tuning constants for a PI controller:

Kc = 0.9 / (SM x ri x td)
Ti = 3.33 x SM x td
Td = 0

And for a PID controller:
Kc = 1.2 / (SM x ri x td)
Ti = 2 x SM x td
Td = td / 2

Kc = 1.2/(3* 1.8618e-05 *1) = 21484.58
Ti = 3.33 * 3 *1 -> Ki = Kc/Ti = 2148.458
Td = 1/2         -> Kd = Kc*tD =  10742.29

Try SM = 30 rather than 3
*/

//double Kp = WindowSize / 20.0, Ki = WindowSize * 0 / 10.0, Kd =  WindowSize / 10 ; //+-10deg prop zone
double Kp = 2148.4, Ki = 21.48, Kd =  1074.2 ; //level tuning

PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

void setup()
{
  windowStartTime = millis();

  //initialize the variables we're linked to
  Setpoint = 100;

  //tell the PID to range between 0 and the full window size
  myPID.SetOutputLimits(0, WindowSize);

  //tell the PID to sample at 1/20 the adjustment rate:
  myPID.SetSampleTime(WindowSize / 100);


  //turn the PID on
  myPID.SetMode(AUTOMATIC);
  pinMode(RELAY_PIN, OUTPUT);

  Serial.begin(115200);

}

void loop()
{

  Input = noisyTemp;

  Setpoint = analogRead(PIN_INPUT) * 100.0 / 1023;
  myPID.Compute();

  /************************************************
     turn the output pin on/off based on pid output
   ************************************************/
  if (millis() - windowStartTime > WindowSize)
  { //time to shift the Relay Window
    windowStartTime += WindowSize;
  }
  if (Output < millis() - windowStartTime) {
    digitalWrite(RELAY_PIN, HIGH);
    powerOn = false;
  }
  else {
    digitalWrite(RELAY_PIN, LOW);
    powerOn = true;
  }
  simHeater();
  report();
  //reportSlope(true);
}

void report(void) {
  const unsigned long interval = 500;
  static unsigned long last = 0;
  static float lastTemp = 0;
  unsigned long now = millis();
  if (millis() - last < interval) return;
  Serial.print("Input:");
  Serial.print(Input);
  Serial.print(" Setpoint:");
  Serial.print(Setpoint);
  Serial.print(" Error:");
  Serial.print(Input - Setpoint);
  Serial.print(" OutputMs:");
  Serial.print(Output);
  Serial.print(" Power:");
  Serial.print(powerOn > 0 ? "On" : "Off");
  Serial.print(" window:");
  Serial.print(now - windowStartTime);
  Serial.print(" Temp:");
  Serial.print( noisyTemp, 6);
  Serial.print(" Slope:");
  Serial.print((noisyTemp - lastTemp) * 1000 / interval, 5);
  Serial.println("");

  lastTemp = noisyTemp;
  last += interval;
}

void reportSlope(int slopeNotTemp) {
  const unsigned long interval = 500;
  static unsigned long last = 0;
  static float lastTemp = 0;
  unsigned long now = millis();
  if (millis() - last < interval) return;
  if (slopeNotTemp) {
    Serial.print(" Slope:");
    Serial.print((noisyTemp - lastTemp) * 1000 / interval, 5);
    Serial.println("");
  } else {
    Serial.print(" Temp:");
    Serial.print( noisyTemp, 6);
    Serial.println();
  }
  lastTemp = noisyTemp;
  last += interval;
}



float simHeater() {
  // periodically simulate the heat transfer into and out of
  // a heater
  const float mass = 2; // kg of water
  const float heater = 1500; // W of heater
  const float  A = 0.3 * 0.6; // m^2 of Area
  const float  h = 5;  // W/(m^2*K) of convection, See https://www.engineeringtoolbox.com/overall-heat-transfer-coefficients-d_284.html
  const float Cp = 4186; // JK/kg
  constexpr float deadTime = 1.0; // sec
  constexpr unsigned long dt = deadTime * 1000; // in millis

  const unsigned long interval = 500;
  static unsigned long last = 0;
  static unsigned long lastPowerChangeMs = -dt;
  static int laggedPowerOn = powerOn;  // Add some deadtime
  unsigned long now = millis();
  if (now - last < interval) return; // abort early
  last += interval;
  // heater

  // lag by deadtime // not quite right
  // this should lag observations, but still apply heat to
  // hidden unlagged process. It currently ignores the pre-debounced
  // heating
  static int savedPowerOn = powerOn;
  if (savedPowerOn != powerOn) {
    lastPowerChangeMs = now;
    savedPowerOn = powerOn;
  }
  if (laggedPowerOn != powerOn && now - lastPowerChangeMs > dt) {
    laggedPowerOn = powerOn;
  }

  if (laggedPowerOn) {
    temp += heater * interval / 1000.0 / (mass * Cp);
  }
  // convection
  temp -= (temp - Tamb) * A * h / (mass * Cp);
  noisyTemp = temp + (random(500) - random(500)) / 500000.0;
  return temp;
}