#include <Arduino.h>
#include <Adafruit_MAX31865.h>
#include "thermoControl.h"


void thermoControl::begin(int pinRTD) {
  RTD = new Adafruit_MAX31865(pinRTD);
  r_ref = 430.0;
  r_nominal = 100.0;
  outMax = 100;
  outMin = 10;
  OpMode = OFF;
  SampleInterval = 100;
  lastTime = millis()-SampleInterval;
  output_pwm = 0;
  control_temp = 0;
  hysteresis = 1.0;
  max_pwr_threshold = 5.0;
}

void thermoControl::begin(int pinRTD, double temp, double duty, double mpt, double hyst) {
  RTD = new Adafruit_MAX31865(pinRTD);
  r_ref = 430.0;
  r_nominal = 100.0;
  outMax = 100;
  outMin = 10;
  OpMode = OFF;
  SampleInterval = 100;
  lastTime = millis()-SampleInterval;

  output_pwm = duty;
  control_temp = temp;
  hysteresis = hyst;
  max_pwr_threshold = mpt;
}

bool thermoControl::Compute() {
  unsigned long now = millis();
  unsigned long timeChange = (now - lastTime);

  double temp = RTD->temperature(r_nominal, r_ref);

  if(timeChange>=SampleInterval && OpMode == AUTOMATIC) {
    double output;
    double error = control_temp - temp;

    if (error >= max_pwr_threshold) {
      output = outMax;
    } else if (error > hysteresis) {
      output = 100 * error / max_pwr_threshold;
      output = max(output, outMin);
      output = min(output, outMax);
    } else {
      output = 0;
    }
    output_pwm = output;
    lastTime = now;

    return true;
  } else {
    return false;
  }
}

double thermoControl::Power() {
  return output_pwm;
}

void thermoControl::Power(double pwr) {
  output_pwm = pwr;
}

double thermoControl::Setpoint() {
  return control_temp;
}

void thermoControl::Setpoint(double temp) {
  control_temp = temp;
}

void thermoControl::SampleTime(int NewSampleTime) {
  if (NewSampleTime > 0) {
    SampleInterval = (unsigned long)NewSampleTime;
  }
}

void thermoControl::PowerLimits(double Max, double Min) {
  if(Min >= Max) return;
  outMax = Max;
  outMin = Min;
}

void thermoControl::Hysteresis(double hys) {
  hysteresis = hys;
}

void thermoControl::ThreshPWR(double thresh) {
  max_pwr_threshold = thresh;
}
    
void thermoControl::Mode(modes newMode) {
  OpMode = newMode;
}

modes thermoControl::Mode() {
  return OpMode;
}

modes thermoControl::CycleMode() {
  if (OpMode + 1 == OVERFLOW) {
    OpMode = (modes)(0);
  } else {
    OpMode = (modes)(OpMode + 1);
  }
  return OpMode;
}