/********************************************************
   QuickPID simulated 1D heater Example
   Reading simulated analog input 0 to control analog PWM output 3
 ********************************************************/
//  This simulates a 5W heater block driven by the PID
//  Vary the setpoint with the Pot, and watch the heater drive the temperature up
//
// Simulation at https://wokwi.com/projects/359818835102393345
//
//  Based on
//  PID_v1 sim at https://wokwi.com/projects/359088752027305985
//  Wokwi https://wokwi.com/projects/357374218559137793
//  Wokwi https://wokwi.com/projects/356437164264235009

//#include "PID_v1.h" // https://github.com/br3ttb/Arduino-PID-Library
#define HACK_QPID_PE_MULT 0
#include "QuickPID.h" // https://github.com/Dlloydev/Arduino-PID-Library
// For 1D heat transfer model:
#include <BasicLinearAlgebra.h> // https://github.com/tomstewart89/BasicLinearAlgebra

#include <sTune.h> // https://github.com/Dlloydev/sTune

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

//Specify the links and initial tuning parameters
float Kp = 23.202, Ki = 0.056, Kd = 0.222; // ZN_PID for ele_sensor=1

//double Kp = 20, Ki = .01, Kd = 10; // works reasonably with sim heater block fo 220deg
//double Kp = 25.5, Ki = 0.001, Kd = 0; // +/-10°proportional band
//float Kp = 255, Ki = 0.001, Kd = 0; // works reasonably with sim heater block
//double Kp = 255, Ki = 0.0, Kd = 0; // +/-1° proportional band works reasonably with sim heater block
//double Kp = 10000, Ki = 0.0, Kd = 0.0; // bang-bang
//double Kp = 2, Ki = 0.0, Kd = 0.0; // P-only
//double Kp = 2, Ki = 5, Kd = 1; // commonly used defaults

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

//Specify PID links
QuickPID myPID(&Input, &Output, &Setpoint);


// expose temperature vector to other routines
const int n_ele = 5;
static BLA::Matrix<n_ele> x; //initial
const int ele_sensor = 0;  // 0..(n_ele-1)

const int PWM_PIN = 3;  // UNO PWM pin for Output
const int FAN_PIN = 5;  // UNO PWM pin for Fan Output

const int INPUT_PIN = -1; // Analog pin for Input (set <0 for simulation)
const int AUTOMATIC_PIN = 8; // Pin for controlling manual/auto mode, NO
const int OVERRIDE_PIN = 12; // Pin for integral override, NO
const int PLUS_PIN = 4; // Pin for integral override, NO
const int MINUS_PIN = 7; // Pin for integral override, NO
const int STUNE_PIN = 11; // Pin for initiating sTune
const int SHIFT_PIN = 2;

#include <LiquidCrystal_I2C.h>

#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   4

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

enum MODES {MANUAL_MODE = 0, AUTO_MODE = 1, TUNE_MODE = 2} mode = MANUAL_MODE;
const char * modes[] = {"Manual", "Auto  ", "Tuning"};
float incValue; // for editing params
uint16_t control_time = 50000U; // us on/off cycle for softpwm

sTune tuner = sTune(&Input, &Output, tuner.ZN_PID, tuner.direct5T, tuner.printALL);
/*                                         ZN_PID           directIP     serialOFF
                                           DampedOsc_PID    direct5T     printALL
                                           NoOvershoot_PID  reverseIP    printSUMMARY
                                           CohenCoon_PID    reverse5T    printDEBUG
                                           Mixed_PID
                                           ZN_PI
                                           DampedOsc_PI
                                           NoOvershoot_PI
                                           CohenCoon_PI
                                           Mixed_PI
*/


//  end of globals



void setup()
{
  Serial.begin(115200);
  Serial.println(__FILE__);
  //apply PID gains
  myPID.SetTunings(Kp, Ki, Kd);
  myPID.SetOutputLimits(-4, 255); // -4 for fan to increase cooling
  pinMode(OVERRIDE_PIN, INPUT_PULLUP);
  pinMode(AUTOMATIC_PIN, INPUT_PULLUP);
  pinMode(MINUS_PIN, INPUT_PULLUP);
  pinMode(PLUS_PIN, INPUT_PULLUP);
  pinMode(SHIFT_PIN, INPUT_PULLUP);

  pinMode(PWM_PIN, OUTPUT);
  pinMode(FAN_PIN, OUTPUT);
  pinMode(STUNE_PIN, INPUT_PULLUP);

  Setpoint = 50;
  incValue = 1;

  //turn the PID on
  if (digitalRead(AUTOMATIC_PIN) == HIGH) {
    myPID.SetMode(myPID.Control::automatic);
    mode = AUTO_MODE;
  } else {
    myPID.SetMode(myPID.Control::manual);
    mode = MANUAL_MODE;
  }
  myPID.SetMode(digitalRead(AUTOMATIC_PIN) == HIGH ? myPID.Control::automatic : myPID.Control::manual);
  if (INPUT_PIN > 0) { // Actual pin or simulated heater?
    Input = analogRead(INPUT_PIN);
  } else {
    Input = simPlant1D(0.0, 1.0); // simulate heating
  }
  lcd.init();
  lcd.backlight();

  Serial.println("Setpoint Input Output Integral");
}

void loop()
{
  // gather Input from INPUT_PIN or simulated block

  float heaterWatts = Output * 5.0 / 255; // 5W heater
  //float heaterWatts = (digitalRead(PWM_PIN)==HIGH) * 5.0 ; // 5W heater

  if (INPUT_PIN > 0 ) { // Actual pin or simulated heater?
    Input = analogRead(INPUT_PIN);
  } else {
    float blockTemp = simPlant1D(heaterWatts, Output > 0 ? 1 : 1 - Output); // simulate heating
    Input = blockTemp;   // read input from simulated heater block
  }

  if (myPID.Compute() || myPID.GetMode() == (uint8_t)myPID.Control::automatic);
  {
    //Output = (int)Output; // Recognize that the output as used is integer
    analogWrite(PWM_PIN, Output >= 0 ? Output : 0); // Heater
    analogWrite(FAN_PIN, Output < 0 ? -Output * 255 / 4.0 : 0); // Fan
  }
  // duty cycle or analogWrite above
  //softPwm(PWM_PIN,Output >= 0 ? Output : 0,control_time);

  doButtons();
  static uint32_t lastButton = 0;
  if (myPID.GetMode() == (uint8_t)myPID.Control::manual && millis() - lastButton > 250) {
    if (digitalRead(STUNE_PIN) == LOW) { // initiate sTune
      mode = 2; // stune
      lcd.clear();
      lcd.setCursor(0, 2);
      lcd.print("sTune: see serial");
      delay(1000);
      sTuneRunner();
      mode = MANUAL_MODE;

    }
  }

  report();
  reportLCD();

}

int softPwm(byte pin, float val, uint32_t control_time_us) {
  static uint32_t last = 0;
  uint32_t now = micros();
  if (now - last >= control_time_us) {
    if (val > 0)digitalWrite(pin, HIGH);
    last = now;
  }
  if (now - last >= (val * control_time_us) / 255) {
    digitalWrite(pin, LOW);
  }
  return now - last < (val * control_time_us) / 255;

}

void doButtons(void) {
  uint32_t now = millis();
  static uint32_t shiftMs = 0, minusMs = 0, plusMs = 0;
  static byte lastShift = HIGH, lastMinus = HIGH, lastPlus = HIGH;
  byte shift = digitalRead(SHIFT_PIN);
  byte minus = digitalRead(MINUS_PIN);
  byte plus = digitalRead(PLUS_PIN);

  if (digitalRead(OVERRIDE_PIN) == LOW) mySetIntegral(&myPID, 0); // integral override
  if ((digitalRead(AUTOMATIC_PIN) == HIGH !=  mode == AUTO_MODE) && mode != TUNE_MODE) {
    switch (digitalRead(AUTOMATIC_PIN)) {
      case HIGH:
        mode = AUTO_MODE;
        myPID.SetMode(myPID.Control::automatic);
        break;
      case LOW:
        mode = MANUAL_MODE;
        myPID.SetMode(myPID.Control::manual);
        break;
    }
  }
  if (plus == LOW && now - plusMs > 250) {
    if (mode == MANUAL_MODE) {
      Output += incValue;
    } else if (mode == AUTO_MODE) {
      Setpoint += incValue;
    }
    plusMs = now;
  }

  if (minus == LOW && now - minusMs > 250) {
    if (mode == MANUAL_MODE) {
      Output -= incValue;
    } else if (mode == AUTO_MODE) {
      Setpoint -= incValue;
    }
    minusMs = now;
  }

  if (shift == LOW && now - shiftMs > 250) {
    incValue *= 10;
    if (incValue > 100) {
      incValue = 0.01;
    }
    shiftMs = now;
  }


}

void report(void)
{
  static uint32_t last = 0;
  const int interval = 250;
  if (millis() - last > interval) {
    last += interval;
    //    Serial.print(millis()/1000.0);
    Serial.print("SP:"); Serial.print(Setpoint);
    Serial.print(" PV:");
    Serial.print(Input);
    Serial.print(" CV:");
    Serial.print(Output);
    Serial.print(" Int:");
    Serial.print(myPID.outputSum);
    Serial.print(' ');
    for (int i = 0; i < n_ele; ++i) {
      Serial.print("x_i");
      Serial.print(i);
      Serial.print(':');
      Serial.print(x(i), 3);
      Serial.print(' ');
    }

    Serial.println();
  }
}

void reportLCD(void)
{
  static uint32_t last = 0;
  const int interval = 250;
  if (millis() - last > interval) {
    last += interval;
    //    Serial.print(millis()/1000.0);
    // lcd.clear();

    lcd.setCursor(0, 0);
    lcd.print("PV:");
    lcd.print(Input, 3);
    lcd.print(" CV:");
    lcd.print(Output, 2);
    lcd.print(" ");
    lcd.setCursor(0, 1);
    lcd.print("SP:");
    lcd.print(Setpoint, 2);
    lcd.print("  ");
    lcd.print(modes[mode]);
    lcd.print("  ");
    lcd.setCursor(0, 3);
    lcd.print("Int:");
    lcd.print(myPID.outputSum, 4);
    lcd.print("  +");
    lcd.print(incValue, incValue >= 1 ? 0 : incValue >= 0.1 ? 1 : 2);
    lcd.print(" ");
  }
}



float simPlant1D(float Q, float hfactor) { // heat input in W (or J/s)
  // simulate a 1x1x2cm aluminum block with a heater and passive ambient cooling
  // float C = 237; // W/mK thermal conduction coefficient for Al
  // Using a 1D heat diffusion rod model

  // Still 0D  See https://wokwi.com/projects/359193529297155073 for
  // what needs to go in here
  using namespace BLA;

  const int n_force = 2;
  // setup for three-element 1D heat transfer model
  // expose temperature vector to other routines
  // move these to global if you want other routines to see them:
  // const int n_ele = 5;
  // static BLA::Matrix<n_ele> x; //initial
  static BLA::Matrix<n_force> u; // Forcings at ends Watts
  static BLA::Matrix<n_ele, 2> G;  // map from forcings to elements, dT=f(W)
  static BLA::Matrix<n_ele, n_ele> Fa ; // transition matrix


  float h = 5  ; // W/m2K thermal convection coefficient for Al passive
  float Cps = 0.89; // J/g°C
  float C_tcond = 237; // W/mK thermal conduction coefficient for Al
  float Tamb = 25; // °C
  float rho = 2.710e6; // kg/m^3 density // Al: 2,710kg/m3
  float rod_dia = 0.02; // m
  float rod_len = 0.10; //m
  float area = pow(rod_dia, 2) * PI / 4 + 0 * rod_len / 3 * PI * rod_dia; // m2 area for convection
  float mass = rod_len * pow(rod_dia, 2) * PI / 4 * rho ; // g

  static float T = Tamb; // °C
  static uint32_t last = 0;
  uint32_t interval = 10; // ms
  float dt = interval / 1000.0;
  static bool oneShot = true;
  if (millis() - last >= interval) {
    last += interval;

    if (oneShot == true) {
      oneShot = false;
      for (int i = 0; i < n_ele; ++i) {
        x(i) = Tamb;
        if (i > 0) { // conduction to the left
          Fa(i, i - 1) = C_tcond * pow(rod_dia, 2) * PI / 4 / (rod_len / n_ele) / (mass / n_ele);
          Fa(i, i) -= Fa(i, i - 1);
        }
        if (i < n_ele - 1) { // conduction to the right
          Fa(i, i + 1) = C_tcond * pow(rod_dia, 2) * PI / 4 / (rod_len / n_ele) / (mass / n_ele);
          Fa(i, i) -= Fa(i, i + 1);
        }
        Fa = Fa * 1; // *20 extra conductive
        G(0, 0) = Cps;
        G(n_ele - 1, 1) = Cps;
        u(0) = 0;
        u(n_force - 1) = 0;
        Serial << "Fa:" << Fa << '\n';
        Serial << "G:" << G << '\n';
        Serial.print("Oneshot");
      }
    } // end oneShot
    // 0-dimensional heat transfer
    //T = T + Q * interval / 1000 / mass / Cps - (T - Tamb) * area * h;
    // 1-D explicit:
    u(0) = ((Tamb - x(0)) * area * h * hfactor) + Q;
    u(1) = (Tamb - x(2)) * area * h * hfactor;

    x += (Fa * x + G * u) * dt ;

    //T = x(n_ele - 1); // end element (most lag) Harder to control
    T = x(ele_sensor); // element with heater (least lag)
  }
  return T;

}


void  mySetIntegral(QuickPID * ptrPID, float value ) {
  // per
  uint8_t saveMode = ptrPID->GetMode();
  ptrPID->SetMode(myPID.Control::manual);
  Output = value;
  ptrPID->SetMode(saveMode);
}


void sTuneRunner() { // blocking
  float inputSpan = 255, outputSpan = 255;
  float outputStart = Output;
  float outputStep = Output + 2;
  float testTimeSec = 300;
  float settleTimeSec = 10;
  float samples = 300;
  float tempLimit = 255;
  tuner.Configure(inputSpan, outputSpan, outputStart, outputStep, testTimeSec, settleTimeSec, samples);
  tuner.SetEmergencyStop(tempLimit);
  int armed = true;
  while (armed) {
    float outputSpan = 255;
    int relayPin = PWM_PIN;
    float heaterWatts = Output * 5.0 / 255; // 5W heater

    Input = simPlant1D(heaterWatts, Output > 0 ? 1 : 1 - Output); // simulate heating

    // float sTune::softPwm(const uint8_t relayPin, float input, float output, float setpoint, uint32_t windowSize, uint8_t debounce)
    tuner.softPwm(relayPin, Input, Output, 0, outputSpan, 1);
    int junk = tuner.Run();
    switch (junk) { // does the step
      case tuner.sample: // active once per sample during test
        // Serial.print(".");
        // void sTune::plotter(float input, float output, float setpoint, float outputScale, uint8_t everyNth) {

        //tuner.plotter(Input, Output, Setpoint, 1, 3); // ... outScale, every nth
        reportLCD();
        //report();
        break;

      case tuner.tunings: // active just once when sTune is done
        Serial.println("sTune Tunings");
        // Update values from sTun results
        tuner.GetAutoTunings(&Kp, &Ki, &Kd); // sketch variables updated by sTune
        myPID.SetTunings(Kp, Ki, Kd); // update PID with the new tunings
        lcd.setCursor(0, 2);
        lcd.print("Kp:");
        lcd.print(Kp, 2);
        lcd.print(" Ki:");
        lcd.print(Ki, 3);

        Output = 0; // turn off
        tuner.SetTuningMethod(tuner.TuningMethod::DampedOsc_PID);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::NoOvershoot_PID);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::CohenCoon_PID);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::Mixed_PID);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::ZN_PI);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::DampedOsc_PI);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::NoOvershoot_PI);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::CohenCoon_PI);
        tuner.printTunings();
        tuner.SetTuningMethod(tuner.TuningMethod::Mixed_PI);
        tuner.printTunings();

        //        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
        armed = false;
        break;

      case tuner.runPid: // active once per sample after tunings
        Serial.println("sTune Run");
        armed = false; //end function
        break;
      default:
        // Serial.print("default tuner.???");
        ;
    }
  }
}

float rnorm(float mean, float stdev) {
  // https://en.wikipedia.org/wiki/Box%E2%80%93Muller_transform
  static byte state = 0;
  static float F1,F2;
  switch (state) {
    case 0:
      F1 = sqrt(-2 * log(random()));
      F2 = 2 * PI * random();
      state = 1;
      return mean + stdev * F1 * cos(F2);
      break;
    case 1:
      return mean + stdev * F1 * sin(F2);
      state = 0;
      break;
  }
}