/*
modified on Apr 10, 2021
Modified by MehranMaleki from Arduino Examples
Home
*/

// this is using the HiLetgo 5V 4 Channel Relay Shield
// pinout is apparently 7,6,5,4

#define USE_SHT30

#ifdef USE_SHT30

#include "SHT85.h"
#define SHT85_ADDRESS         0x44

SHT85 sht30;

#else
#include <Wire.h>
#include "DFRobot_SHT20.h"

TwoWire twowire;
DFRobot_SHT20 sht20(&twowire);
#endif

const int PIN_RELAY4 = 4;
const int PIN_RELAY3 = 5;

void fan(bool state) {
  int last_state = digitalRead(PIN_RELAY3);
  digitalWrite(PIN_RELAY3, state);
  if (last_state != state) {
    Serial.print(F("f:"));
    Serial.println(state);
  }
}
void humidifier(bool state) {
  int last_state = digitalRead(PIN_RELAY4);
  digitalWrite(PIN_RELAY4, state);
  if (last_state != state) {
    Serial.print(F("h:"));
    Serial.println(state);
  }
}

constexpr int sample_sz = 20;
struct {
  int sample_time_millis = 250;
  unsigned long last_sample_time = 0;
  float humd = 0.0f, temp = 0.0f;
  int volt1, volt2; // corresponds to humdifier and fan state respectively

  int manual_mode_ts_millis = 0;
  int manual_mode_timeout_millis = 5000; // how long to wait to time out from manual mode
  bool manual_mode = false;

  struct {
    // simple ring buffer to allow
    float humd[sample_sz];
    int towrite = 0;
    bool started = false;

    void add(float h) {
      if (!started) {
        for (auto& val: humd) {
          val = h;
        }
        started = true;
      } else {
        humd[towrite] = h;
      }
      towrite = (towrite + 1) % sample_sz;
    }
    float readLast(int n) {
      if (n >= sample_sz) {
        n = sample_sz - 1;
      }
      if (n < 0) {
        n = 0;
      }
      int idx = towrite - 1 - n;
      while (idx < 0) {
        idx += sample_sz;
      }
      return humd[idx];
    }
  } samples;

  struct {
    float upper = 0.87f;
    float lower = 0.83f;
  } thresh;

  int fan_on_secs = 10;
  int fan_cycle_secs = 5*60;

  void readSamples() {
    last_sample_time = millis();
    /*
#ifdef USE_SHT30
    sht30.read();
    humd = sht30.getHumidity();
    temp = sht30.getTemperature();
#else
    humd = sht20.readHumidity();         // Read Humidity
    temp = sht20.readTemperature();      // Read Temperature
#endif
    */
    volt1 = digitalRead(PIN_RELAY4);
    volt2 = digitalRead(PIN_RELAY3);

    samples.add(humd);
  }

  float feedforward_coeff = 50;
  void controlHumidifier() {
    // TODO maybe use linear regression instead of just the last point
    const int prev_idx = (samples.towrite - 2 + sample_sz) % sample_sz;
    const float cur_h = humd;
    const float prev_h = samples.readLast(1);
    const float slope = (cur_h - prev_h) * feedforward_coeff;
    const float comp_h = cur_h + slope;
    if (cur_h < thresh.lower) {
      humidifier(true);
    }
    if (cur_h > thresh.upper) {
      humidifier(false);
    }
  }

  void controlFan() {
    unsigned long time_secs = millis() / 1000;
    if ((time_secs % fan_cycle_secs) < fan_on_secs) {
      fan(true);
    } else {
      fan(false);
    }
  }
} cur;

void setup()
{
    pinMode(PIN_RELAY4, OUTPUT);
    pinMode(PIN_RELAY3, OUTPUT);
    pinMode(7, OUTPUT);
    digitalWrite(7, LOW);
    
    Serial.begin(115200);
#ifdef USE_SHT30 
    Wire.begin();
    sht30.begin(SHT85_ADDRESS);
    Wire.setClock(10000);
#else
    twowire.setClock(10000);
    sht20.initSHT20();                         // Init SHT20 Sensor
#endif
    delay(100);
    //sht20.checkSHT20();                        // Check SHT20 Sensor
    cur.readSamples();
}

void printStatus() {
    Serial.print(F("s:"));
    Serial.print(cur.humd);
    Serial.print(F(":"));
    Serial.print(cur.temp);
    Serial.print(F(":"));
    Serial.print(cur.volt1);
    Serial.print(F(":"));
    Serial.print(cur.volt2);
    Serial.println(F(""));
}

// this would be nice with AVR supported it..
struct Introspect {
public:
  const char *name;
  void* val;
  enum Ty {
    kInt,
    kFloat,
    kBool
  };
  Ty ty;

  Introspect(): name(nullptr), val(nullptr), ty(Ty::kInt) {}
  constexpr Introspect(const char* n, int& v): name(n), val(reinterpret_cast<void*>(&v)), ty(Ty::kInt) {}
  constexpr Introspect(const char* n, float& v): name(n), val(reinterpret_cast<void*>(&v)), ty(Ty::kFloat) {}
  constexpr Introspect(const char* n, bool& v): name(n), val(reinterpret_cast<void*>(&v)), ty(Ty::kBool) {}

  void readVal() const {
    switch (ty) {
      case kInt: {
        const int* v = val;
        Serial.print(*v);
        break;
      }
      case kFloat: {
        const float* v = val;
        Serial.print(*v);
        break;
      }
      case kBool: {
        const bool* v = val;
        int val = *v;
        Serial.print(val);
        break;
      }
    }
  }
  bool writeVal(const char* buf, int len) {
    switch (ty) {
      case kInt: {
        bool isnegative = false;
        bool hasdigit = false;
        int tmp = 0;
        for (int i = 0; i < len; i++) {
          if (i == 0 && buf[i] == '-') {
            isnegative = true;
          } else if (buf[i] >= '0' && buf[i] <= '9') {
            tmp = 10*tmp + (buf[i] - '0');
            hasdigit = true;
          } else {
            return false;
          }
        }

        if (!hasdigit) {
          return false;
        }
        if (isnegative) {
          tmp = -tmp;
        }
        int* v = reinterpret_cast<int*>(val);
        *v = tmp;
        return true;
      }
      case kFloat: {
        float tmp = 0.0f;
        float factor = 0.1f;
        int exponent = 0;
        bool is_negative = false, has_digit = false;
        bool using_exponent = false, has_exponent = false;
        bool exp_is_negative = false;

        enum State {
          kPredecimal,
          kPostdecimal, // after the decimal point
          kExponent, // after the e
          kExponentPostsign, // after the +/-
        };
        State state = kPredecimal;

        for (int i = 0; i < len; i++) {
          switch (state) {
            case kPredecimal:
              if (i == 0 && buf[i] == '-') {
                is_negative = true;
              } else if (buf[i] >= '0' && buf[i] <= '9') {
                has_digit = true;
                tmp = 10.0f * tmp + (buf[i] - '0');
              } else if (buf[i] == '.') {
                state = kPostdecimal;
              } else if (buf[i] == 'e') {
                state = kExponent;
                using_exponent = true;
              } else {
                return false;
              }
              break;
            case kPostdecimal:
              if (buf[i] >= '0' && buf[i] <= '9') {
                has_digit = true;
                tmp = tmp + factor * (buf[i] - '0');
                factor *= 0.1f;
              } else if (buf[i] == 'e') {
                state = kExponent;
                using_exponent = true;
              } else {
                return false;
              }
              break;
            case kExponent:
              if (buf[i] == '+') {
                state = kExponentPostsign;
              } else if (buf[i] == '-') {
                exp_is_negative = true;
                state = kExponentPostsign;
              } else if (buf[i] >= '0' && buf[i] <= '9') {
                has_exponent = true;
                exponent = (buf[i] - '0');
                state = kExponentPostsign;
              } else {
                return false;
              }
              break;
            case kExponentPostsign:
              if (buf[i] >= '0' && buf[i] <= '9') {
                has_exponent = true;
                exponent = 10*exponent + (buf[i] - '0');
              } else {
                return false;
              }
              break;
          }
        }

        if (!has_digit) return false;
        if (using_exponent && !has_exponent) return false;

        if (is_negative) {
          tmp = -tmp;
        }
        if (has_exponent) {
          if (exp_is_negative) {
            for (int i = 0; i < exponent; i++) {
              tmp *= 0.1f;
            }
          } else {
            for (int i = 0; i < exponent; i++) {
              tmp *= 10.f;
            }
          }
        }

        float* v = reinterpret_cast<float*>(val);
        *v = tmp;
        return true;
      }
      case kBool: {
        if (len < 1) {
          return false;
        }
        bool* v = reinterpret_cast<bool*>(val);
        *v = (buf[0] != '0');
        return true;
      }
    }
  }

};

#define LIST_PROPS(x) \
  x(sample_time_millis, cur.sample_time_millis) \
  x(manual_mode_timeout_millis, cur.manual_mode_timeout_millis) \
  x(manual_mode, cur.manual_mode) \
  x(thresh_upper, cur.thresh.upper) \
  x(thresh_lower, cur.thresh.lower) \
  x(fan_on_secs, cur.fan_on_secs) \
  x(fan_cycle_secs, cur.fan_cycle_secs) \
  x(feedforward_coeff, cur.feedforward_coeff) \

#define DEFINE_STRS(n, var) const char n##_str[] PROGMEM = #n;
#define DEFINE_INTRO(n, var) {n##_str, var},

/*
const Introspect introspect_vals[] PROGMEM = {
  {("cur_sample_time_millis"), cur.cur_sample_time_millis},
  {("manual_mode_timeout_millis"), cur.manual_mode_timeout_millis},
  {("manual_mode"), cur.manual_mode},

  {("thresh.upper"), cur.thresh.upper},
  {("thresh.lower"), cur.thresh.lower},

  {("fan_on_secs"), cur.fan_on_secs},
  {("fan_cycle_secs"), cur.fan_cycle_secs},

  {("feedforward_coeff"), cur.feedforward_coeff},
};
*/
LIST_PROPS(DEFINE_STRS)

const Introspect introspect_vals[] PROGMEM = {
  LIST_PROPS(DEFINE_INTRO)
};

bool match(const char* name, int len, Introspect& intro);
bool match(const char* name, int len, Introspect& intro) {
  for (int i = 0; i < sizeof(introspect_vals)/sizeof(introspect_vals[0]); i++) {
    memcpy_P((void*)&intro, (void*)&introspect_vals[i], sizeof(intro));
    int intro_len = strlen_P(intro.name);
    if (intro_len != len) {
      continue;
    }

    if (strncmp_P(name, intro.name, len) == 0) {
      return true;
    }
  }
  return nullptr;
}

void dumpIntrospect() {
  char buf[40];
  Introspect intro;
  for (int i = 0; i < sizeof(introspect_vals)/sizeof(introspect_vals[0]); i++) {
    memcpy_P((void*)&intro, (void*)&introspect_vals[i], sizeof(intro));
    Serial.print(F("d:"));
    strncpy_P(buf, intro.name, sizeof(buf));
    Serial.print(buf);
    Serial.print(F(":"));
    switch (intro.ty) {
      case Introspect::kInt:
        Serial.print(F("i"));
        break;
      case Introspect::kFloat:
        Serial.print(F("f"));
        break;
      case Introspect::kBool:
        Serial.print(F("b"));
        break;
    }
    Serial.println(F(""));
  }
    Serial.println(F("d:done"));
  return nullptr;
}


void loop()
{
  unsigned long now = millis();
  if (now > (cur.manual_mode_ts_millis + cur.manual_mode_timeout_millis)) {
    cur.manual_mode = false;
  }

  if ((now - cur.last_sample_time) >= cur.sample_time_millis) {
    cur.last_sample_time = now;

    cur.readSamples();
    if (!cur.manual_mode) {
      cur.controlHumidifier();
      cur.controlFan();
    }
    printStatus();
  }
  if (Serial.available()) {
    const int c = Serial.read();
    if (c == 's') {
      printStatus();
    } else if (c == 'r') {
      char buf[40];

      int len = Serial.readBytesUntil('\n', buf, sizeof(buf) - 1);
      buf[len] = 0;

      if (len == 0) {
        Serial.println(F("r:"));
        Serial.print(buf+1);
        Serial.println(F(":invalid name"));
        return;
      }

      Introspect tmp;
      if (!match(buf+1, len-1, tmp)) {
        Serial.println(F("r:"));
        Serial.print(buf+1);
        Serial.print(",");
        Serial.print(len);
        Serial.println(F(":no match"));
        return;
      }
      Serial.print(F("r:"));
      Serial.print(buf+1);
      Serial.print(F(":"));
      tmp.readVal();
      Serial.println(F(""));
    } else if (c == 'w') {
      char buf[40];

      int len = Serial.readBytesUntil('\n', buf, sizeof(buf) - 1);
      buf[len] = 0;

      if (len == 0) {
        Serial.println(F("w:no name"));
        return;
      }

      int colon_idx = -1;
      for (int i = 1; i < len; i++) {
        if (buf[i] == ':') {
          colon_idx = i;
          break;
        }
      }

      if (colon_idx == -1) {
        Serial.println(F("w:invalid command"));
        return;
      }
      buf[colon_idx] = 0;

      Introspect tmp;
      if (!match(buf+1, colon_idx-1, tmp)) {
        Serial.println(F("w:no match"));
        return;
      }
      if (!tmp.writeVal(buf + colon_idx + 1, len - colon_idx - 1)) {
        Serial.println(F("w:invalid value"));
        return;
      }

      Serial.print(F("w:"));
      Serial.print(buf+1);
      Serial.print(F(":"));
      tmp.readVal();
      Serial.println(F(""));
    } else if (c == 'H') humidifier(true);
    else if (c == 'h') humidifier(false);
    else if (c == 'F') fan(true);
    else if (c == 'f') fan(false);
    else if (c == 'M') {
      cur.manual_mode = true;
      cur.manual_mode_ts_millis = millis();
    } else if (c == 'm') cur.manual_mode = false;
    else if (c == 'd') dumpIntrospect();
  }
}