// Test to adjust the K parameters at runtime and plot the setpoint and PID values.

// This simulates a 20W heater block driven by the PID
// Wokwi https://wokwi.com/projects/356437164264235009

#include "PID_RT.h"
// https://github.com/RobTillaart/PID_RT

constexpr byte PWM_PIN = 9;  // NANO PWM servo pin

int output_power = 0;
float input_temp = 0;
float Kp = 0.1;
float Ki = 0.1;
float Kd = 0.1;

constexpr byte numChars = 64;
char readCstring[numChars];
boolean finished_reading_serial = false;

PID_RT PID;

void setup()
{
  Serial.begin(115200);
  Serial.println(__FILE__);

  PID.setPoint(125);
  PID.setOutputRange(0, 255);  // PWM range
  PID.setInterval(50);
  PID.setK(0.1, 0.1, 0.1);
  PID.start();
}


void loop()
{
  check_and_read_1_char_serial();
  if (finished_reading_serial == true) {
    Serial.print("Recieved:");
    Serial.println(readCstring);
    update_variables_from_serial();
    update_PID();
    finished_reading_serial = false;
  }
  float heaterWatts = ((int)output_power)*20.0/255; // 20W heater
  float blockTemp = simPlant(heaterWatts); // simulate heating
  input_temp = blockTemp;

  //Kp=10;

  //PID.setKp(Kp);
  //PID.setKi(Ki);
  //PID.setKd(Kd);
  //PID.setK(2, 0, 0);
  if (PID.compute(input_temp))
  {
    output_power = PID.getOutput();
  }
  report();
}

void check_and_read_1_char_serial()
// Respeta lo que dice:
// https://forum.arduino.cc/t/serial-input-basics-updated/382007/2
// .
{
  static byte i = 0;
  char endMarker = '\n';      // endMarker = New Line (NL). En Matlab es el LF (line feed).
  char recievedChar;
  while (Serial.available() > 0 && finished_reading_serial == false) {
        recievedChar = Serial.read();
        if (recievedChar != endMarker) {
            readCstring[i] = recievedChar;
            i++;
            if (i >= numChars) {
                i = numChars - 1;
                Serial.println("Error, buffer overflow.");
            }
        }
        else {
            readCstring[i] = '\0'; // terminate the string
            i = 0;
            finished_reading_serial = true;
        }
  }
}

void update_variables_from_serial()
{
  if(readCstring[0] == 'K') {
    switch(readCstring[1]) {
      case 'p':
        Kp = atof(readCstring + 2);
        break;
      case 'i':
        Ki = atof(readCstring + 2);
        break;
      case 'd':
        Kd = atof(readCstring + 2);
        break;
      default:
        Serial.println("Error: not Kp, Ki, or Kd.");
        break;
    }
  }
  else {
    Serial.println("Error: no K.");
  }
  //finished_reading_serial = false;
}

void update_PID() {  
  PID.setK(Kp, Ki, Kd);
  Serial.print("Kp=");
  Serial.print(PID.getKp());
  Serial.print(" , Ki=");
  Serial.print(PID.getKi());
  Serial.print(" , Kd=");
  Serial.println(PID.getKd());
}

void report(void){
  static uint32_t last = 0;
  const int interval = 1000;
  if (millis() - last > interval){
    last += interval;
//    Serial.print(millis()/1000.0); 
    Serial.print("SP = ");
    Serial.print(PID.getSetPoint());
    Serial.print(", In temp = ");
    Serial.print(input_temp);
    Serial.print(", Out power = ");
    Serial.println(output_power);
  }
}

float simPlant(float Q){ // 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
   float h = 5 ; // W/m2K thermal convection coefficient for Al passive
   float Cps = 0.89; // J/g°C
   float area = 1e-4; // m2 area for convection
   float mass = 10 ; // g
   float Tamb = 25; // °C
   static float T = Tamb; // °C
   static uint32_t last = 0;
   uint32_t interval = 100; // ms

   if(millis() - last >= interval){
     last += interval;
     T = T + Q*interval/1000/mass/Cps - (T-Tamb)*area*h;
   }
   return T;
}