#include "Arduino.h"
#include "bodytemp.h"

Adafruit_MLX90614 mlx = Adafruit_MLX90614();

double bodytemp_ambient=0;
double bodytemp_object=0;

bool body_present=false;

const double bodytemp_activate_temperature=30; //°C min object temperature to detect human
const double bodytemp_activate_mindiff_ambient=3; //°C object temperature has to be at least this amount above ambient
const double bodytemp_deactivate_maxdiff_ambient=2; //°C maximum difference between object and ambient temperature to not detect human anymore
const unsigned long bodytemp_deacitvate_mintime=1000; //minimum time in ms between activate and deactivate

void initBodytemp() {
  Wire.begin(PIN_SDA,PIN_SCL);
  if (!mlx.begin(0x5A,&Wire)) {
    Serial.println("Error connecting to MLX sensor. Check wiring.");
    while (1);
  };
}

void printBodytempDebug() {
  Serial.print("min:20,max:40,");
  Serial.print("Ambient:");Serial.print(bodytemp_ambient);
  Serial.print(",Object:");Serial.print(bodytemp_object);
  Serial.print(",Presence:");Serial.print(body_present?39:21);

  
}

bool checkBodypresence(unsigned long millis) {
  static unsigned long last_read_bodytemp=0;
  if (millis-last_read_bodytemp > BODYTEMP_READ_MIN_INTERVAL)
  {
    last_read_bodytemp = millis;

    bodytemp_ambient = mlx.readAmbientTempC();
    bodytemp_object = mlx.readObjectTempC();

    static unsigned long last_change_bodypresent=0;
    if (millis-last_change_bodypresent > bodytemp_deacitvate_mintime) {}
    if (!body_present) { //is currently not present
      if (bodytemp_object >= bodytemp_activate_temperature && (bodytemp_object-bodytemp_ambient) >= bodytemp_activate_mindiff_ambient) {
        body_present=true;
        last_change_bodypresent=millis;
      }
    }else{ //was present last time
      if((bodytemp_object-bodytemp_ambient) <= bodytemp_deactivate_maxdiff_ambient) {
        body_present=false;
        last_change_bodypresent=millis;
      }
    }
  }
  return body_present;
}