#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 sensorA;
#define PIN_VL53L0X_XSHUT_A 19
// 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 waterlevelAMean_array[WATERLEVELMEAN_SIZE];
uint16_t waterlevelAMean_array_pos=0;
float waterlevelA=WATERLEVEL_UNAVAILABLE;  
float watervolumeA=WATERLEVEL_UNAVAILABLE; 


//Calibration
float waterlevelA_calib_offset=500.0; //c
float waterlevelA_calib_factor=-1.0; //m


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

uint16_t distanceA_unsuccessful_count=0;

// +++++++++++++++ VL53L0X +++++++++++++++
VL53L0X sensorB;

#define PIN_VL53L0X_XSHUT_B 23
// 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 waterlevelBMean_array[WATERLEVELMEAN_SIZE];
uint16_t waterlevelBMean_array_pos=0;
float waterlevelB=WATERLEVEL_UNAVAILABLE;  //distance from floor to water surface [mm]
float watervolumeB=WATERLEVEL_UNAVAILABLE; //calculated Volume in Reservoir


//Calibration
float waterlevelB_calib_offset=273.0; //c
float waterlevelB_calib_factor=-1.0; //m


float waterlevelB_calib_reservoirArea=56.5*36.5; //area in cm^2

uint16_t distanceB_unsuccessful_count=0;



float waterlevelA_heightToVolume(float distance);
float waterlevelB_heightToVolume(float distance);


mqttValueTiming timing_waterlevelA;
mqttValueTiming timing_waterlevelB;

void waterlevel_shutdownSensors() {
  pinMode(PIN_VL53L0X_XSHUT_A, OUTPUT);
  digitalWrite(PIN_VL53L0X_XSHUT_A, LOW); //pull to GND

  pinMode(PIN_VL53L0X_XSHUT_B, OUTPUT);
  digitalWrite(PIN_VL53L0X_XSHUT_B, LOW); //pull to GND
}

void waterlevel_enableSensor(uint8_t sensorid) {
  switch (sensorid){
    case 0:
      pinMode(PIN_VL53L0X_XSHUT_A, INPUT); //Enable Sensor A
      break;
    case 1:
      pinMode(PIN_VL53L0X_XSHUT_B, INPUT); //Enable Sensor B
      break;
  }
}

void waterlevel_setup() {


  waterlevel_shutdownSensors();
  delay(100);


  
  /*
  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_waterlevelA.minchange=0.0;
  timing_waterlevelA.maxchange=3.0;
  timing_waterlevelA.mintime=30*000;
  timing_waterlevelA.maxtime=60*60*1000;

  timing_waterlevelB.minchange=0.0;
  timing_waterlevelB.maxchange=0.5;
  timing_waterlevelB.mintime=10*000;
  timing_waterlevelB.maxtime=60*60*1000;

  waterlevel_enableSensor(1); //1==B    //Enable Sensor B first, to change its address
  delay(50);

  Wire.begin(21,22);
  Serial.print("I2C Clock Speed=");
  Serial.println(Wire.getClock());

  delay(100);


  //Initialize SensorB first
  sensorB.setTimeout(2000);
  if (!sensorB.init())
  {
    Serial.println("Failed to detect and initialize sensorB!");
    publishInfo("error/waterlevel","Failed to detect and initialize sensorB");
    delay(1000);
  }

  Serial.println("set addr 0x2A");
  sensorB.setAddress(0x2A); //change address
  Serial.println("conf Default");


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

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

  

  // Stop driving this sensor's XSHUT low. This should allow the carrier
  // board to pull it high. (We do NOT want to drive XSHUT high since it is
  // not level shifted.) Then wait a bit for the sensor to start up.
  waterlevel_enableSensor(0);
  delay(50);

  //Initialize Sensor A after SensorB's address was changed
  sensorA.setTimeout(2000);
  if (!sensorA.init())
  {
    Serial.println("Failed to detect and initialize sensorA!");
    publishInfo("error/waterlevel","Failed to detect and initialize sensorA");
    delay(1000);
  }


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

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






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

void waterlevel_loop(unsigned long loopmillis) {
  static uint8_t waterlevel_loop_select=0;

  switch(waterlevel_loop_select)
  {
    case 0: 
    // ############ A

    static unsigned long last_read_waterlevelA;
    if (loopmillis>=last_read_waterlevelA+READINTERVAL_WATERLEVEL) {
      last_read_waterlevelA=loopmillis;
      
      
      uint16_t distance=sensorA.readRangeSingleMillimeters(); //error=65535
      
      //Serial.print("Distance reading A="); Serial.print(distance);Serial.println();

      
      if (distance!=WATERLEVEL_UNAVAILABLE && distance!=65535) { //successful
        waterlevelAMean_array[waterlevelAMean_array_pos]=distance;
        waterlevelAMean_array_pos++;
        waterlevelAMean_array_pos%=WATERLEVELMEAN_SIZE;
        distanceA_unsuccessful_count=0;
      }else{
        distanceA_unsuccessful_count++;
        if (distanceA_unsuccessful_count%20==0) {
          String _text="Distance A unsuccessful count=";
          _text.concat(distanceA_unsuccessful_count);
          _text.concat(" distance=");
          _text.concat(distance);
          publishInfo("error/waterlevel",_text);
        }
      }


      if (isValueArrayOKf(waterlevelAMean_array,WATERLEVELMEAN_SIZE,WATERLEVEL_UNAVAILABLE)){
        float _filteredDistance=getFilteredf(waterlevelAMean_array,WATERLEVELMEAN_SIZE,WATERLEVELMEAN_FILTER_CUTOFF);
        //Serial.print("Filtered reading A="); Serial.print(_filteredDistance);Serial.println();

        //Invert distance and offset
        waterlevelA=constrain(waterlevelA_calib_offset+waterlevelA_calib_factor*_filteredDistance,0,1000);
        watervolumeA=waterlevelA_heightToVolume(waterlevelA);

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

  case 1:

    // ############ B

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


      if (distance!=WATERLEVEL_UNAVAILABLE && distance!=65535) { //successful
        waterlevelBMean_array[waterlevelBMean_array_pos]=distance;
        waterlevelBMean_array_pos++;
        waterlevelBMean_array_pos%=WATERLEVELMEAN_SIZE;
        distanceB_unsuccessful_count=0;
      }else{
        distanceB_unsuccessful_count++;
        if (distanceB_unsuccessful_count%20==0) {
          String _text="Distance B unsuccessful count=";
          _text.concat(distanceB_unsuccessful_count);
          _text.concat(" distance=");
          _text.concat(distance);
          publishInfo("error/waterlevel",_text);
        }
      }


      if (isValueArrayOKf(waterlevelBMean_array,WATERLEVELMEAN_SIZE,WATERLEVEL_UNAVAILABLE)){
        float _filteredDistance=getFilteredf(waterlevelBMean_array,WATERLEVELMEAN_SIZE,WATERLEVELMEAN_FILTER_CUTOFF);
        
        
        //Invert distance and offset
        waterlevelB=constrain(waterlevelB_calib_offset+waterlevelB_calib_factor*_filteredDistance,0,1000);
        watervolumeB=waterlevelB_heightToVolume(waterlevelB);

        //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("]"); 
      }
      
      waterlevel_loop_select=0;
      break;
    }

  }
}

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


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

#endif