#include <PID_v1.h>
#include <sTune.h>
#include "SevSeg.h"
#include <ezButton.h>
#include <M2M_LM75A.h>
#include <EEPROM.h>
#include <math.h>

#define INPUT_TIMEOUT_MS 5000
#define MAXIMUM_SAFE_TEMP 29.0f
#define SETPOINT_EEPROM_ADDRESS 0x42
#define WINDOW_SIZE_MS 2000

#define INPUT_PIN 0
#define RELAY_PIN 14

SevSeg sevseg; //Instantiate a seven segment controller object
volatile unsigned long windowStartTime, now, loops;
float Input, Output, Setpoint, Kp, Ki, Kd;
double input, kp=2, ki=1, kd=1, output, setpoint;
PID myPID(&input, &output, &setpoint, 0, 0, 0, DIRECT);
sTune tuner = sTune(&Input, &Output, tuner.NoOvershoot_PID, tuner.directIP, tuner.printSUMMARY);
unsigned long inputSettingTimeout;
ezButton upButton(A2, INPUT_PULLUP), downButton(A1, INPUT_PULLUP);
M2M_LM75A lm75a;
char serialBuffer[128], inputBuf[16], setpointBuf[16], outputBuf[16];

void setup() {
  //7-segment display setup
  byte numDigits = 4;
  byte digitPins[] = {10, 11, 12, 13};
  byte segmentPins[] = {2, 3, 4, 5, 6, 7, 8, 9};
  bool resistorsOnSegments = false; // 'false' means resistors are on digit pins
  byte hardwareConfig = COMMON_CATHODE; // See README.md for options
  bool updateWithDelays = false; // Default 'false' is Recommended
  bool leadingZeros = false; // Use 'true' if you'd like to keep the leading zeros
  bool disableDecPoint = false; // Use 'true' if your decimal point doesn't exist or isn't connected
  sevseg.begin(hardwareConfig, numDigits, digitPins, segmentPins, resistorsOnSegments,
  updateWithDelays, leadingZeros, disableDecPoint);
  sevseg.setBrightness(50);

  //PID setup
  windowStartTime = millis();
  //initialize the variables we're linked to

  double savedSetpoint;
  EEPROM.get(SETPOINT_EEPROM_ADDRESS, savedSetpoint);
  if (1) {//(isnan(savedSetpoint)) {
    //ideal kombucha temp celsius
    Setpoint =  25.0f;
  } else {
    Setpoint = savedSetpoint;
  }
  //tell the PID to range between 0 and the full window size
  myPID.SetOutputLimits(0, WINDOW_SIZE_MS);
  //turn the PID on
  myPID.SetMode(MANUAL);
  tuner.Configure(
    25.0f,
    WINDOW_SIZE_MS * 1.0f,
    400.0f,
    50.0f,
    30,
    1,
    50);

  //ambient temp in Celsius
  Input = input = 10.0f;
  lm75a.begin();

  inputSettingTimeout = 0;
  Serial.begin(9600);
}

void loop() {
  //Input = lm75a.getTemperature();
  switch (tuner.Run()) {
    case tuner.sample: // active once per sample during test
      //Input = analogRead(inputPin) * 0.322265625 - 50.0; // get degC (using 3.3v AREF)
      //tuner.plotter(Input, Output, Setpoint, WINDOW_SIZE_MS * 1.0f, 3); // output scale 0.1, plot every 3rd sample
      break;

    case tuner.tunings: // active just once when sTune is done
      tuner.GetAutoTunings(&Kp, &Ki, &Kd); // sketch variables updated by sTune
      myPID.SetOutputLimits(0, WINDOW_SIZE_MS);
      myPID.SetSampleTime(WINDOW_SIZE_MS - 1);
      output = WINDOW_SIZE_MS/2, kp = Kp, ki = Ki, kd = Kd, Output = output;
      myPID.SetMode(AUTOMATIC); // the PID is turned on
      myPID.SetTunings(kp, ki, kd); // update PID with the new tunings
      break;

    case tuner.runPid: // active once per sample after tunings
      //input = analogRead(inputPin) * 0.322265625 - 50.0; // get degC (using 3.3v AREF)
      myPID.Compute();
      Output = output, input = Input;
      //tuner.plotter(Input, Output, Setpoint, 0.1f, 3);
      break;
  }

  upButton.loop();
  downButton.loop();
  now = millis();
  if ((upButton.isPressed() || downButton.isPressed())) {
    inputSettingTimeout = now + INPUT_TIMEOUT_MS;
      if (upButton.isPressed()) {
        Setpoint += 0.5f;
      } else if (downButton.isPressed()) {
        Setpoint -= 0.5f;
    }
  }
  if (now > inputSettingTimeout) {
    if (inputSettingTimeout > 0) {
      EEPROM.put(SETPOINT_EEPROM_ADDRESS, Setpoint);
    }
    inputSettingTimeout = 0;
    sevseg.setNumberF(Input, 2);
  } else {
    if (now / 200 % 2 == 0) {
      sevseg.blank();
    } else {
      sevseg.setNumberF(Setpoint, 2);
    }
  }
  sevseg.refreshDisplay();

  /************************************************
   * turn the output pin on/off based on pid output
   ************************************************/
  if (now - windowStartTime > WINDOW_SIZE_MS)
  { //time to shift the Relay Window
    windowStartTime += WINDOW_SIZE_MS;
  }
  if (Output < now - windowStartTime || Input > MAXIMUM_SAFE_TEMP) { 
    digitalWrite(RELAY_PIN, LOW);
    Input -= 0.001f;
  } else {
    digitalWrite(RELAY_PIN, HIGH);
    Input += 0.005f;
  }

  if (++loops % 500 == 0) {
    dtostrf(Input, 5, 2, inputBuf);
    dtostrf(Output, 6, 2, outputBuf);
    dtostrf(Setpoint, 5, 2, setpointBuf);
    sprintf(serialBuffer,
      "{ Input: \"%s\", Output: \"%s\", Setpoint: \"%s\", millis(): \"%lu\", loops: \"%lu\" }",
      inputBuf,
      outputBuf,
      setpointBuf,
      now,
      loops);
    Serial.println(serialBuffer);
  }
}