#include <Patrix.h>
#include "sensors/Dallas.h"
#include "sensors/DallasSensor.h"
#include "ArduPID.h"
#include <Arduino.h>

#define CONTROL_GPIO          4
#define CONTROL_PWM_BITS      10

Dallas dallas(2);

DallasSensor sensor(dallas, 0xAA0121125E4A7528, "sensor", 0.5, 5, 60); // TODO wrong address

ArduPID pid;

double temperatureTarget = 31;

double temperatureCurrent = NAN;

double temperatureMaxOvershoot = 5;

double heaterPWM = 0;

double proportional = 1;

double integral = 0;

double derivative = 0;

bool setDouble(const char *name, double *destinationPtr, double min, double max);

void patrixSetup() {
  analogWriteResolution(CONTROL_PWM_BITS);

  pid.begin(&temperatureCurrent, &heaterPWM, &temperatureTarget, proportional, integral, derivative);
  pid.setOutputLimits(0, pow(10, CONTROL_PWM_BITS) - 1);
  pid.start();
}


void patrixLoop() {
  dallas.loop();
  sensor.loop();

  temperatureCurrent = sensor.getLastValue();
  if (!isnan(temperatureCurrent)) {
    pid.compute();
  } else {
    heaterPWM = 0;
  }

  const bool emergencyCutOff = heaterPWM > 0 && temperatureCurrent > temperatureTarget + temperatureMaxOvershoot;
  if (emergencyCutOff) {
    heaterPWM = 0;
  }
  analogWrite(CONTROL_GPIO, (int) round(heaterPWM));

  static unsigned long lastDebug = 0;
  unsigned long now = millis();
  if (now - lastDebug >= 1000) {
    lastDebug = now;
    debug("p: %f | i: %f | d: %f | current: %4.1f | target: %4.1f | pwm: %3d%%", proportional, integral, derivative, temperatureCurrent, temperatureTarget, heaterPWM);
    if (emergencyCutOff) {
      error("[EMERGENCY CUTOFF] temperatureCurrent (=%4.1f) > temperatureTarget + %4.1f (=%4.1f) [EMERGENCY CUTOFF]", temperatureCurrent, temperatureMaxOvershoot, temperatureTarget);
    }
  }
}

bool patrix_command(char *cmd) {
  const char *first = strtok(cmd, " ");
  if (strcmp(first, "target") == 0) {
    return setDouble("target", &temperatureTarget, -10, 80);
  } else if (strcmp(first, "p") == 0) {
    return setDouble("proportional", &proportional, NAN, NAN);
  } else if (strcmp(first, "i") == 0) {
    return setDouble("integral", &integral, NAN, NAN);
  } else if (strcmp(first, "p") == 0) {
    return setDouble("derivative", &derivative, NAN, NAN);
  }
  return false;
}

bool setDouble(const char *name, double *destinationPtr, double min, double max) {
  const char *valueStr = strtok(nullptr, nullptr);
  if (valueStr == nullptr) {
    error("Missing value for \"%s\"", name);
    return false;
  }
  double value = strtod(valueStr, nullptr);
  if (isnan(value)) {
    error("Failed to parse double for \"%s\": %s", name, valueStr);
    return false;
  }
  if ((!isnan(min) && value < min) || (!isnan(max) && value > max)) {
    error("Value out of range for \"%s\" [%f..%f]: %f", name, min, max, value);
    return false;
  }
  *destinationPtr = value;
  info("Value for \"%s\" set to: %f", name, value);
  return true;
}