#ifndef _WATERLEVEL_H_
#define _WATERLEVEL_H_

#include <Wire.h>
#include <VL6180X.h>


VL6180X sensor;
// To try different scaling factors, change the following define.
// Valid scaling factors are 1, 2, or 3.
#define SCALING 1


#define READINTERVAL_WATERLEVEL 200

#define WATERLEVELMEAN_SIZE 32
#define WATERLEVELMEAN_FILTER_CUTOFF 8 //max value is around WATERLEVELMEAN_SIZE/2
float waterlevelMean_array[WATERLEVELMEAN_SIZE];
uint16_t waterlevelMean_array_pos=0;
#define WATERLEVEL_UNAVAILABLE -1
float waterlevel=WATERLEVEL_UNAVAILABLE;  //distance from floor to water surface [mm]
float watervolume=WATERLEVEL_UNAVAILABLE; //calculated Volume in Reservoir


//Calibration
float waterlevel_calib_offset_measured=86; //Sollwert
float waterlevel_calib_offset_sensor=78; //Istwert
//raw reading is 78mm, ruler reads 86mm. VL8160 sensor is 169mm above bottom of reservoir.

float waterlevel_calib_reservoirArea=27*36.5; //area in cm^2


float waterlevel_heightToVolume(float distance);


mqttValueTiming timing_waterlevel;

void waterlevel_setup() {

  timing_waterlevel.minchange=0.0;
  timing_waterlevel.maxchange=3.0;
  timing_waterlevel.mintime=30*000;
  timing_waterlevel.maxtime=60*60*1000;

  Wire.begin();

  sensor.init();
  sensor.configureDefault();
  sensor.setScaling(SCALING);
  sensor.setTimeout(500);

  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_waterlevel;
  if (loopmillis>=last_read_waterlevel+READINTERVAL_WATERLEVEL) {
    last_read_waterlevel=loopmillis;
    
    
    uint16_t distance=sensor.readRangeSingleMillimeters();
    
    //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 (isValueArrayOKf(waterlevelMean_array,WATERLEVELMEAN_SIZE,WATERLEVEL_UNAVAILABLE)){
      float _filteredDistance=getFilteredf(waterlevelMean_array,WATERLEVELMEAN_SIZE,WATERLEVELMEAN_FILTER_CUTOFF);
      

      //Invert distance and offset
      waterlevel=(waterlevel_calib_offset_sensor+waterlevel_calib_offset_measured)-_filteredDistance;
      watervolume=waterlevel_heightToVolume(waterlevel);

      //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("]"); 
    }


  }
}

float waterlevel_heightToVolume(float distance){
  return waterlevel_calib_reservoirArea/100 * distance/100; //area[cm^2] in dm^2 * height in dm = dm^3= L
}

#endif