/**************************************************************************/
/*  Example: Non-blocking Heating Control with PID, Soft-Start,           */
/*  and Thermocouple read only when Heater is OFF                         */
/**************************************************************************/

#include <PID_v1_bc.h>

/**************************************************************************/
/* Pin Assignments                                                        */
/**************************************************************************/
const uint8_t TEMP_SENSOR_PIN  = A0;    // e.g. thermocouple amplifier analog out
const uint8_t HEATER_DRIVE_PIN = 9;     // PWM pin driving an NPN transistor base

/**************************************************************************/
/* PID Parameters (tune for your setup)                                   */
/**************************************************************************/
double Kp = 2.0;   // Proportional Gain
double Ki = 5.0;   // Integral Gain
double Kd = 1.0;   // Derivative Gain

// PID working variables
double Setpoint = 380.0;  // Desired temperature (example: 100°C)
double Input    = 0.0;    // Measured temperature
double Output   = 0.0;    // PID calculation result (0-255 for analogWrite)

/**************************************************************************/
/* PID Object                                                             */
/**************************************************************************/
// Use DIRECT if raising Output raises temperature; otherwise REVERSE.
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

/**************************************************************************/
/* Soft-Start / Inrush Limiting                                          */
/**************************************************************************/
// Max step size in PWM counts per second
const int MAX_RAMP_STEP_PER_SEC = 100;  

// Variables for ramp calculation
int currentPWM              = 0;     // current heater PWM
unsigned long lastRampTime  = 0;     // timestamp for ramp calculations

/**************************************************************************/
/* Non-blocking intervals                                                 */
/**************************************************************************/
// We want to run a PID cycle every 500 ms (example).
// We'll also define a small "heater off" window to get a clean thermocouple reading.
const unsigned long PID_SAMPLE_INTERVAL_MS     = 500;
const unsigned long HEATER_OFF_MEASURE_WINDOW  = 50;  // ms to wait after turning heater off

/**************************************************************************/
/* State Machine to handle "heater off -> read sensor -> heater on"       */
/**************************************************************************/
enum MeasurementState {
  IDLE,
  HEATER_OFF_WAIT,     // turning heater off, wait for noise to settle
  HEATER_READ_SENSOR,  // read thermocouple
};

MeasurementState measureState = IDLE;

unsigned long stateChangeTime = 0;   // track when we changed states

/**************************************************************************/
/* Setup()                                                                */
/**************************************************************************/
void setup()
{
  // Initialize serial for debugging
  Serial.begin(115200);
  Serial.println("Heating Control with PID + Soft-Start + Off-Only Sensor Reads");

  // Set the heater drive pin as output
  pinMode(HEATER_DRIVE_PIN, OUTPUT);
  digitalWrite(HEATER_DRIVE_PIN, LOW); // ensure it’s off at startup

  // Configure PID
  myPID.SetSampleTime(PID_SAMPLE_INTERVAL_MS);  // run compute every X ms
  myPID.SetOutputLimits(0, 255);                // PWM range for analogWrite
  myPID.SetMode(AUTOMATIC);                     // Start the PID in Automatic mode

  lastRampTime  = millis();
  stateChangeTime = millis();
}

/**************************************************************************/
/* Loop() - Non-blocking main loop                                        */
/**************************************************************************/
void loop()
{
  unsigned long now = millis();

  // State machine controlling the "heater off -> read sensor" sequence
  switch (measureState)
  {
    /**********************************************************************
     *  IDLE: The heater runs normally (using last known PWM).
     *  When it's time for a new PID sample, we switch to HEATER_OFF_WAIT.
     **********************************************************************/
    case IDLE:
    {
      // If it's time for a new PID cycle...
      static unsigned long lastPIDTime = 0;
      if (now - lastPIDTime >= PID_SAMPLE_INTERVAL_MS)
      {
        lastPIDTime = now;
        // Turn off heater to reduce noise
        analogWrite(HEATER_DRIVE_PIN, 0); 
        currentPWM = 0; // keep our "tracking" variable in sync
        stateChangeTime = now;
        measureState = HEATER_OFF_WAIT;

        Serial.println("Turning heater off to read thermocouple...");
      }
    }
    break;

    /**********************************************************************
     *  HEATER_OFF_WAIT: Wait a small window for EMI to settle,
     *  so our thermocouple reading is cleaner.
     **********************************************************************/
    case HEATER_OFF_WAIT:
    {
      if (now - stateChangeTime >= HEATER_OFF_MEASURE_WINDOW)
      {
        // Enough time has passed -> read sensor next
        measureState = HEATER_READ_SENSOR;
      }
    }
    break;

    /**********************************************************************
     *  HEATER_READ_SENSOR: Now read the thermocouple, run PID,
     *  then apply soft-start to re-enable the heater smoothly.
     **********************************************************************/
    case HEATER_READ_SENSOR:
    {
      // 1) Read the temperature (heater is off at the moment).
      Input = readTemperature();

      // 2) Run the PID calculation
      myPID.Compute();  // -> Output

      // 3) Soft-start ramp from currentPWM to PID's Output
      currentPWM = applySoftStart(currentPWM, (int)Output);

      // 4) Turn the heater back on at 'currentPWM'
      analogWrite(HEATER_DRIVE_PIN, currentPWM);

      // 5) Debug info
      Serial.print("Temp(C)="); Serial.print(Input);
      Serial.print("  PIDOut="); Serial.print(Output);
      Serial.print("  RampPWM="); Serial.println(currentPWM);

      // 6) Go back to idle until next sampling
      measureState = IDLE;
    }
    break;
  }

  // You can place other non-blocking tasks here...
}

/**************************************************************************/
/* readTemperature()                                                      */
/* Replace with your specific thermocouple code or amplifier reading.     */
/**************************************************************************/
double readTemperature()
{
  // Example for an analog thermocouple amplifier output going into A0:
  // Suppose 0..5V => 0..1023 ADC, and let's assume 10 mV/°C amplifier scale
  // This is purely an example – use your actual conversion formula!

  int raw = analogRead(TEMP_SENSOR_PIN);
  double millivolts = (raw * 5000.0) / 1023.0;  // scale 0..1023 -> 0..5000 mV
  double temperatureC = millivolts / 10.0;      // 10 mV/°C => 1°C per 10 mV

  return temperatureC;
}

/**************************************************************************/
/* applySoftStart(...)                                                    */
/* Limits how fast we ramp up/down PWM to avoid inrush. Non-blocking.     */
/**************************************************************************/
int applySoftStart(int currentVal, int targetVal)
{
  unsigned long now = millis();
  unsigned long elapsed = now - lastRampTime; 

  // stepAllowed = (MAX_RAMP_STEP_PER_SEC) * (elapsed/1000) 
  float stepAllowed = (MAX_RAMP_STEP_PER_SEC / 1000.0) * elapsed;
  int step = (int)stepAllowed;
  // If some time has passed, allow at least 1 step
  if (step == 0 && elapsed > 0) step = 1;

  if (targetVal > currentVal)
  {
    // Ramp up
    currentVal += step;
    if (currentVal > targetVal) {
      currentVal = targetVal;
    }
  }
  else if (targetVal < currentVal)
  {
    // Ramp down
    currentVal -= step;
    if (currentVal < targetVal) {
      currentVal = targetVal;
    }
  }

  lastRampTime = now;
  return currentVal;
}