#ifndef _WATERLEVEL_H_
#define _WATERLEVEL_H_


#include <HCSR04.h>
#define HCSR04_PIN_ECHO 17
#define HCSR04_PIN_TRIGGER 16
#define HCSR04_TIMEOUT 5000 //default is 100000 (uS)
#define READINTERVAL_HCSR04 500

#define WATERLEVELMEAN_SIZE 32
float waterlevelMean_array[WATERLEVELMEAN_SIZE];
uint16_t waterlevelMean_array_pos=0;
#define WATERLEVEL_UNAVAILABLE -1
float waterlevel=WATERLEVEL_UNAVAILABLE;

uint16_t waterlevel_failcounter=0;
#define WATERLEVEL_MAXFAILS 15 //maximum counter value
#define WATERLEVEL_FAILTHRESHOLD 10 //if failcounter is greater or equal this value waterlevel will not be valid


float waterlevel_distanceToVolume(float distance);


void waterlevel_setup() {

  //HCSR04.begin(HCSR04_PIN_TRIGGER, HCSR04_PIN_ECHO);
  HCSR04.begin(HCSR04_PIN_TRIGGER, HCSR04_PIN_ECHO,HCSR04_TIMEOUT, HCSR04.eUltraSonicUnlock_t::unlockSkip);
  for (uint16_t i=0;i<WATERLEVELMEAN_SIZE;i++) {
    waterlevelMean_array[i]=-1; //-1 is also timeout value
    
  }
}

void waterlevel_loop(unsigned long loopmillis) {

  static unsigned long last_read_hcsr04;
  if (loopmillis>=last_read_hcsr04+READINTERVAL_HCSR04) {
    last_read_hcsr04=loopmillis;
    float temperature=20.0;
    if (tempCmean_air!=DEVICE_DISCONNECTED_C) { //sensor ok
      temperature=tempCmean_air;
    }
    
    double* distances = HCSR04.measureDistanceMm(temperature);
    double distance=distances[0];
    //Serial.print("Distance reading:"); Serial.println(distance);

    if (distance!=WATERLEVEL_UNAVAILABLE) { //successful
      waterlevelMean_array[waterlevelMean_array_pos]=distance;
      waterlevelMean_array_pos++;
      waterlevelMean_array_pos%=WATERLEVELMEAN_SIZE;
      if (waterlevel_failcounter>0) { //reduce failcounter if sucessfull
        waterlevel_failcounter--;
      }
    }else{
      if (waterlevel_failcounter<WATERLEVEL_MAXFAILS) {
        waterlevel_failcounter++;
      }
      
    }

    
    if (isValueArrayOKf(waterlevelMean_array,WATERLEVELMEAN_SIZE,WATERLEVEL_UNAVAILABLE)){
      float _distance=getFilteredf(waterlevelMean_array,WATERLEVELMEAN_SIZE,8);

      waterlevel=waterlevel_distanceToVolume(_distance);

      //float _meanWaterlevel=getMeanf(waterlevelMean,WATERLEVELMEAN_SIZE);
      //Serial.print("\t Dist="); Serial.print(_filteredWaterlevel); Serial.print("mm"); Serial.print("(+- "); Serial.print((getMaxf(waterlevelMean,WATERLEVELMEAN_SIZE)-getMinf(waterlevelMean,WATERLEVELMEAN_SIZE))/2.0); Serial.print(")"); Serial.print(" [mean="); Serial.print(_meanWaterlevel); Serial.print("]"); 
    }

    if (waterlevel_failcounter>=WATERLEVEL_FAILTHRESHOLD) { //too many failed readings
      waterlevel=-1;
      /*if (debug) {
        Serial.print("Waterlevel Failcounter="); Serial.println(waterlevel_failcounter);
      }*/
    }

  }
}

float waterlevel_distanceToVolume(float distance){
  return distance;
}

#endif