/********************************************************
   PID simulated 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/359088752027305985
//
//  Based on
//  Wokwi https://wokwi.com/projects/357374218559137793
//  Wokwi https://wokwi.com/projects/356437164264235009

#include "PID_v1.h" // https://github.com/br3ttb/Arduino-PID-Library
// local copy of .h and .cpp are tweaked to expose the integral per
// https://github.com/br3ttb/Arduino-PID-Library/pull/133
#define USE_HACK   // access the PID.outputSum variable
// For 1D heat transfer model:
#include <BasicLinearAlgebra.h> // https://github.com/tomstewart89/BasicLinearAlgebra


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

//Specify the links and initial tuning parameters
//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
//double Kp = 255, Ki = 0.001, Kd = 0; // works reasonably with sim heater block
//double Kp = 255, Ki = .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);

// expose rod temperature vector to other routines
const int n_ele = 5;
const int ele_sensor = 1; // 0 for sensor close to heater, easiest
                              // n_ele -1 for furthest, most lag, hardest
static BLA::Matrix<n_ele> x; //initial

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 SETPOINT_PIN = A1;   // Analog pin for Setpoint Potentiometer
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


#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);

void setup()
{
  Serial.begin(115200);
  Serial.println(__FILE__);
  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(PWM_PIN,OUTPUT);
  pinMode(FAN_PIN,OUTPUT);
  Setpoint = 0;
  //turn the PID on
  myPID.SetMode(AUTOMATIC);
  if (INPUT_PIN > 0) {
    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
  if (INPUT_PIN > 0 ) {
    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()==MANUAL)
  {
    //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

  }

  Setpoint = analogRead(SETPOINT_PIN) / 4; // Read setpoint from potentiometer
  if (digitalRead(OVERRIDE_PIN) == LOW) mySetIntegral(&myPID, 0); // integral override
  if (digitalRead(AUTOMATIC_PIN) == HIGH !=  myPID.GetMode() == AUTOMATIC) {
    myPID.SetMode(digitalRead(AUTOMATIC_PIN) == HIGH ? AUTOMATIC : MANUAL);
  }
  static uint32_t lastButton = 0;
  if (myPID.GetMode() == MANUAL && millis() - lastButton > 250) {
    float incValue = pow(10.0, map(analogRead(A2),0,1023,-3,1));
    if (digitalRead(PLUS_PIN) == LOW) {
      Output += incValue;
      lastButton = millis();
    }
    if (digitalRead(MINUS_PIN) == LOW) {
      Output -= incValue;
      lastButton = millis();
    }
  }

  report();
  reportLCD();

}

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:");
#if defined(USE_HACK)
    Serial.print(myPID.outputSum);
#endif
    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, 3);
    lcd.print("  ");
    lcd.setCursor(0, 1);
    lcd.print("SP:");
    lcd.print(Setpoint, 3);
    lcd.print(myPID.GetMode() == AUTOMATIC ? " Automatic " : " Manual   ");
    lcd.print("  ");
    lcd.setCursor(0, 3);
    lcd.print("Int:");
#if defined(USE_HACK)
    lcd.print(myPID.outputSum, 4);
#endif
    lcd.print(' ');
    lcd.println();
  }
}



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.0f; // *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 (some lag)
    //T = x(0); // element with heater (least lag)
  }
  return T;

}


void  mySetIntegral(PID * ptrPID, double value ) {
  // per
  ptrPID->SetMode(MANUAL);
  Output = value;
  ptrPID->SetMode(AUTOMATIC);
}