#ifndef _WATERLEVEL_H_
#define _WATERLEVEL_H_

#include <Wire.h>
#include <VL53L0X.h> //pololu/VL53L0X@^1.3.1





// +++++++++++++++ Common Parameters ++++++++++

#define READINTERVAL_WATERLEVEL 500
#define WATERLEVELMEAN_SIZE 16
#define WATERLEVELMEAN_FILTER_CUTOFF 4 //max value is around WATERLEVELMEAN_SIZE/2


#define WATERLEVEL_UNAVAILABLE -1 //-1 is also timeout value 


// +++++++++++++++ VL53L0X +++++++++++++++
VL53L0X tofsensor;

// Uncomment this line to use long range mode. This
// increases the sensitivity of the sensor and extends its
// potential range, but increases the likelihood of getting
// an inaccurate reading because of reflections from objects
// other than the intended target. It works best in dark
// conditions.

//#define LONG_RANGE

// Uncomment ONE of these two lines to get
// - higher speed at the cost of lower accuracy OR
// - higher accuracy at the cost of lower speed

//#define HIGH_SPEED
#define HIGH_ACCURACY




float waterlevelMean_array[WATERLEVELMEAN_SIZE];
uint16_t waterlevelMean_array_pos=0;
float waterlevel=WATERLEVEL_UNAVAILABLE;  
float watervolume=WATERLEVEL_UNAVAILABLE; 
#define DISTANCE_UNAVAILABLE 65535
uint16_t distance=DISTANCE_UNAVAILABLE;



//Calibration
float waterlevel_calib_offset=500.0; //c
float waterlevel_calib_factor=-1.0; //m


float waterlevel_calib_reservoirArea=20*20*3.1416; //area in cm^2. barrel diameter inside is 400mm

uint16_t distance_unsuccessful_count=0;

bool tofenabled=true;


float waterlevel_heightToVolume(float distance);


mqttValueTiming timing_waterlevel;

void waterlevel_setup() {


  
  /*
  Wire.begin();

  byte error, address;
  int nDevices;
 
  delay(500);
  Serial.println("Scanning...");
 
  nDevices = 0;
  for(address = 1; address < 127; address++ )
  {
    // The i2c_scanner uses the return value of
    // the Write.endTransmisstion to see if
    // a device did acknowledge to the address.
    Wire.beginTransmission(address);
    error = Wire.endTransmission();
 
    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16)
        Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");
 
      nDevices++;
    }
    else if (error==4)
    {
      Serial.print("Unknown error at address 0x");
      if (address<16)
        Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  */
  

  

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


  for (uint16_t i=0;i<WATERLEVELMEAN_SIZE;i++) {
    waterlevelMean_array[i]=WATERLEVEL_UNAVAILABLE; //-1 is also timeout value 
  }

  delay(50);


  tofsensor.setTimeout(2000);
  if (!tofsensor.init())
  {
    Serial.println("Failed to detect and initialize tofsensor!");
    publishInfo("error/waterlevel","Failed to detect and initialize tofsensor");
    delay(1000);
    tofenabled=false;
    return;
  }


    
  #if defined LONG_RANGE
    // lower the return signal rate limit (default is 0.25 MCPS)
    tofsensor.setSignalRateLimit(0.1);
    // increase laser pulse periods (defaults are 14 and 10 PCLKs)
    tofsensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
    tofsensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);
  #endif

  #if defined HIGH_SPEED
    // reduce timing budget to 20 ms (default is about 33 ms)
    tofsensor.setMeasurementTimingBudget(20000);
  #elif defined HIGH_ACCURACY
    // increase timing budget to 200 ms
    tofsensor.setMeasurementTimingBudget(200000);
  #endif


}

void waterlevel_loop(unsigned long loopmillis) {
  if (!tofenabled) {
    return;
  }

  static unsigned long last_read_waterlevelB;
  if (loopmillis>=last_read_waterlevelB+READINTERVAL_WATERLEVEL) {
    last_read_waterlevelB=loopmillis;
    
    
    distance=tofsensor.readRangeSingleMillimeters(); //out of range =255
    
    //Serial.print("Distance reading               B="); Serial.print(distance);Serial.println();


    if (distance!=DISTANCE_UNAVAILABLE) { //successful
      waterlevelMean_array[waterlevelMean_array_pos]=distance;
      waterlevelMean_array_pos++;
      waterlevelMean_array_pos%=WATERLEVELMEAN_SIZE;
      distance_unsuccessful_count=0;
    }else{
      distance_unsuccessful_count++;
      if (distance_unsuccessful_count%20==0) {
        String _text="Distance unsuccessful count=";
        _text.concat(distance_unsuccessful_count);
        _text.concat(" distance=");
        _text.concat(distance);
        publishInfo("error/waterlevel",_text);
      }
    }


    if (isValueArrayOKf(waterlevelMean_array,WATERLEVELMEAN_SIZE,WATERLEVEL_UNAVAILABLE)){
      float _filteredDistance=getFilteredf(waterlevelMean_array,WATERLEVELMEAN_SIZE,WATERLEVELMEAN_FILTER_CUTOFF);
      
      
      //Invert distance and offset
      waterlevel=constrain(waterlevel_calib_offset+waterlevel_calib_factor*_filteredDistance,0,1000);
      watervolume=waterlevel_heightToVolume(waterlevel);

      //Serial.print("Filtered reading               B="); Serial.print(_filteredDistance); Serial.print(" fixed="); Serial.println(waterlevelB); Serial.println();


      //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("]"); 
    }else{
      waterlevel=WATERLEVEL_UNAVAILABLE;
    }
  }
    
      
}

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