/*  
  Theengs OpenMQTTGateway - We Unite Sensors in One Open-Source Interface

   Act as a gateway between your 433mhz, infrared IR, BLE, LoRa signal and one interface like an MQTT broker  
   Send and receiving command by MQTT
 
    HC SR-04 reading Addon
  
    Copyright: (c)Florian ROBERT
    
    Contributors:
    - 1technophile
    - mpember
  
    This file is part of OpenMQTTGateway.
    
    OpenMQTTGateway is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    OpenMQTTGateway is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#include "User_config.h"

#ifdef ZsensorHCSR04
#  include "TheengsCommon.h"
#  include "config_HCSR04.h"

unsigned long timeHCSR04 = 0;

void setupHCSR04() {
  THEENGS_LOG_NOTICE(F("HCSR04 trigger pin: %d" CR), HCSR04_TRI_GPIO);
  THEENGS_LOG_NOTICE(F("HCSR04 echo pin: %d" CR), HCSR04_ECH_GPIO);
  pinMode(HCSR04_TRI_GPIO, OUTPUT); // declare HC SR-04 trigger GPIO as output
  pinMode(HCSR04_ECH_GPIO, INPUT); // declare HC SR-04 echo GPIO as input
}

void MeasureDistance() {
  if (millis() > (timeHCSR04 + TimeBetweenReadingHCSR04)) {
    timeHCSR04 = millis();
    THEENGS_LOG_TRACE(F("Creating HCSR04 buffer" CR));
    StaticJsonDocument<JSON_MSG_BUFFER> HCSR04dataBuffer;
    JsonObject HCSR04data = HCSR04dataBuffer.to<JsonObject>();
    digitalWrite(HCSR04_TRI_GPIO, LOW);
    delayMicroseconds(2);
    digitalWrite(HCSR04_TRI_GPIO, HIGH);
    delayMicroseconds(10);
    digitalWrite(HCSR04_TRI_GPIO, LOW);
    unsigned long duration = pulseIn(HCSR04_ECH_GPIO, HIGH);
    if (isnan(duration)) {
      THEENGS_LOG_ERROR(F("Failed to read from HC SR04 sensor!" CR));
    } else {
      static unsigned int distance = 99999;
      unsigned int d = duration / 58.2;
      HCSR04data["distance"] = (int)d;
      if (d > distance) {
        HCSR04data["direction"] = "away";
        THEENGS_LOG_TRACE(F("HC SR04 Distance changed" CR));
      } else if (d < distance) {
        HCSR04data["direction"] = "towards";
        THEENGS_LOG_TRACE(F("HC SR04 Distance changed" CR));
      } else if (HCSR04_always) {
        HCSR04data["direction"] = "static";
        THEENGS_LOG_TRACE(F("HC SR04 Distance hasn't changed" CR));
      }
      distance = d;
      enqueueJsonObject(HCSR04data);
    }
  }
}
#endif