#include <Arduino.h>
#include <ArduinoSort.h>

// ######## EC
#define EC_PIN_ADC 4
#define EC_PIN_FREQ 5
#define EC_PWM_CH 0
#define EC_RESOLUTION 8
#define EC_FREQUENCY 5000

#define EC_ARRAY_SIZE 1024
uint16_t ec_array[EC_ARRAY_SIZE];
uint16_t ec_array_pos=0;
unsigned long last_read_ec=0;
#define EC_READ_INTERVAL 1

// ######## Temperature
#include <OneWire.h>
#include <DallasTemperature.h>

//first address: 28FF6C1C7216058B
//second address: 

#define ONE_WIRE_BUS 18 //GPIO pin
#define TEMPERATURE_PRECISION 12 //max is 12
#define READINTERVAL_DS18B20 1000 //ms

// Setup a oneWire instance to communicate with any OneWire devices (not just Maxim/Dallas temperature ICs)
OneWire oneWire(ONE_WIRE_BUS);

// Pass our oneWire reference to Dallas Temperature.
DallasTemperature sensors(&oneWire);


#define TEMPMEAN_SIZE 16
uint16_t tempCmean_pos=0;
// arrays to hold device addresses
DeviceAddress thermometerReservoir={0x28,0xFF,0x30,0xBA,0x85,0x16,0x03,0xB5};
float tempC_reservoir;
float tempCmean_reservoir[TEMPMEAN_SIZE];

DeviceAddress thermometerAir={0x28,0xFF,0x6C,0x1C,0x72,0x16,0x05,0x8B};
float tempC_air;
float tempCmean_air[TEMPMEAN_SIZE];


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

#define WATERLEVELMEAN_SIZE 32
float waterlevelMean[WATERLEVELMEAN_SIZE];
uint16_t waterlevelMean_pos=0;


// ######## Flow Rate
#define FLOW_PIN 19
uint16_t flow_counter=0; //maximum counts/s measured with Eden 128 Pump was 171
void IRAM_ATTR isr_flow();
unsigned long last_read_flow=0;
#define READINTERVAL_FLOW 1000
float flow_factor=7.5; //F=7.5*flowrate[L/min]
float flow;

uint32_t flow_counter_sum=0;


unsigned long last_print=0;




float getMean(uint16_t *parray,uint16_t psize);
float getMeanf(float *parray,uint16_t psize);
uint16_t getMin(uint16_t *parray, uint16_t psize);
uint16_t getMax(uint16_t *parray, uint16_t psize);
float getMaxf(float *parray,uint16_t psize);
float getMinf(float *parray, uint16_t psize);
bool isValueArrayOK(uint16_t *parray,uint16_t psize, uint16_t pcheck);
bool isValueArrayOKf(float *parray,uint16_t psize, float pcheck);
float getFilteredf(float *parray,uint16_t psize, uint16_t pcutOff);


void printAddress(DeviceAddress deviceAddress);
void printTemperature(DeviceAddress deviceAddress);
void printResolution(DeviceAddress deviceAddress);
void printData(DeviceAddress deviceAddress);




void setup() {
  Serial.begin(115200);
  pinMode(EC_PIN_ADC,INPUT);

  ledcSetup(EC_PWM_CH, EC_FREQUENCY, EC_RESOLUTION);
  ledcAttachPin(EC_PIN_FREQ, EC_PWM_CH);
  ledcWrite(EC_PWM_CH, 127);

  //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[i]=-1; //-1 is also timeout value
    
  }

  //initialize mean array
  for (uint16_t i=0;i<TEMPMEAN_SIZE;i++) {
    tempCmean_reservoir[i]=-127;
    tempCmean_air[i]=-127;
  }

  sensors.begin();
  delay(1000);

  Serial.print("Locating devices...");
  Serial.print("Found ");
  Serial.print(sensors.getDeviceCount(), DEC);
  Serial.println(" devices.");

  delay(1000);

  Serial.print("Parasite power is: ");
  if (sensors.isParasitePowerMode()) Serial.println("ON");
  else Serial.println("OFF");

  delay(1000);


  //Just search for devices. Only needed when connecting a new sensor to find the address
  oneWire.reset_search();
  
  for (uint8_t i=0;i<sensors.getDeviceCount();i++){
    DeviceAddress _addr;
    if (!oneWire.search(_addr)) {
      Serial.print("Error: Device not found");
    }else{
      Serial.print("Found device. Address:");
      printAddress(_addr);
    }
    Serial.println();
      
  }

  sensors.setResolution(thermometerReservoir, TEMPERATURE_PRECISION);
  sensors.setResolution(thermometerAir, TEMPERATURE_PRECISION);
  

  pinMode(FLOW_PIN, INPUT_PULLUP);
	attachInterrupt(FLOW_PIN, isr_flow, CHANGE);

  Serial.println("Setup finished");
  delay(500);
}

void loop() {
  unsigned long loopmillis=millis();

  bool flag_print=false;

  

  if (loopmillis>last_read_ec+EC_READ_INTERVAL) {
    last_read_ec=loopmillis;
    ec_array_pos++;
    flag_print= ec_array_pos==EC_ARRAY_SIZE;
    ec_array_pos%=EC_ARRAY_SIZE;
    ec_array[ec_array_pos]=analogRead(EC_PIN_ADC);

    //Serial.print(ec_array[ec_array_pos]); Serial.print(" ");
  }

  
  static unsigned long last_read_ds18b20;
  static bool flag_requestTemperatures=false;
  if (loopmillis>last_read_ds18b20+READINTERVAL_DS18B20) { 
    if (loopmillis>last_read_ds18b20+READINTERVAL_DS18B20*10) { //timeout
      Serial.println("Warn: Request Temperatures Timeout!");
      flag_requestTemperatures=false;
    }
    if (!flag_requestTemperatures) {
      sensors.requestTemperatures(); //this takes ~600ms
      flag_requestTemperatures=true;
    }
    if (sensors.isConversionComplete()) {
      flag_requestTemperatures=false;
      last_read_ds18b20=loopmillis;
      
      tempC_reservoir = sensors.getTempC(thermometerReservoir);
      if (tempC_reservoir == DEVICE_DISCONNECTED_C)
      {
        Serial.print(" Error reading: ");  printAddress(thermometerReservoir);
      }else{
        tempCmean_reservoir[tempCmean_pos]=tempC_reservoir;
      }

      tempC_air = sensors.getTempC(thermometerAir);
      if (tempC_air == DEVICE_DISCONNECTED_C)
      {
        Serial.print(" Error reading: ");  printAddress(thermometerReservoir);
      }else{
        tempCmean_air[tempCmean_pos]=tempC_air;
      }

      tempCmean_pos++;
      tempCmean_pos%=TEMPMEAN_SIZE;
    }
    
  }

  


  static unsigned long last_read_hcsr04;
  if (loopmillis>=last_read_hcsr04+READINTERVAL_HCSR04) {
    last_read_hcsr04=loopmillis;
    float temperature=20.0;
    if (tempC_air!=DEVICE_DISCONNECTED_C && isValueArrayOKf(tempCmean_air,TEMPMEAN_SIZE,DEVICE_DISCONNECTED_C)) { //sensor ok
      temperature=getMeanf(tempCmean_air,TEMPMEAN_SIZE);
    }
    
    double* distances = HCSR04.measureDistanceMm(temperature);

    waterlevelMean[waterlevelMean_pos]=distances[0];
    waterlevelMean_pos++;
    waterlevelMean_pos%=WATERLEVELMEAN_SIZE;
  }


  static uint16_t _last_flowconter; //for debugging
  if (loopmillis>=last_read_flow+READINTERVAL_FLOW) {
    flow=flow_counter*1000.0/(loopmillis-last_read_flow)/2.0; //Frequency [Hz]
    flow/=flow_factor; //[L/min]
    _last_flowconter=flow_counter; //for debugging
  
    flow_counter=0;
    last_read_flow=loopmillis;  
  }

  
  if (loopmillis>last_print+500) { 
    last_print=loopmillis;

    if (isValueArrayOK(ec_array,EC_ARRAY_SIZE,0))
    {
      Serial.print("EC=");
      Serial.print(getMean(ec_array,EC_ARRAY_SIZE),3);
      Serial.print(" count (+- ");  Serial.print( (getMax(ec_array,EC_ARRAY_SIZE) - getMin(ec_array,EC_ARRAY_SIZE))/2.0);  Serial.print(")");
    }else{
      Serial.print("Waiting for EC");
    }

    if (isValueArrayOKf(tempCmean_reservoir,TEMPMEAN_SIZE,DEVICE_DISCONNECTED_C)){
      Serial.print("\t Treservoir="); Serial.print(getMeanf(tempCmean_reservoir,TEMPMEAN_SIZE)); Serial.print("\t Tair="); Serial.print(getMeanf(tempCmean_air,TEMPMEAN_SIZE));
    }else{
      Serial.print("\t waiting for temperature");
    }

    
    if (isValueArrayOKf(waterlevelMean,WATERLEVELMEAN_SIZE,-1.0)){
      float _max=getMaxf(waterlevelMean,WATERLEVELMEAN_SIZE);
      float _min=getMinf(waterlevelMean,WATERLEVELMEAN_SIZE);

      float _filteredWaterlevel=getFilteredf(waterlevelMean,WATERLEVELMEAN_SIZE,8);
      float _meanWaterlevel=getMeanf(waterlevelMean,WATERLEVELMEAN_SIZE);
      

      Serial.print("\t Dist="); Serial.print(_filteredWaterlevel); Serial.print("mm"); Serial.print("(+- "); Serial.print((_max-_min)/2.0); Serial.print(")"); Serial.print(" [mean="); Serial.print(_meanWaterlevel); Serial.print("]");
      
    }else{
      Serial.print("\t waiting for distance");
    }

    Serial.print("\t Flow="); Serial.print(flow,2); Serial.print(" ("); Serial.print(_last_flowconter); Serial.print(")");
    Serial.print("\t Flowsum="); Serial.print(flow_counter_sum);
    
    

    Serial.println();   
    
  }
  
}

float getMean(uint16_t *parray,uint16_t psize) {
  double mean=0;
  for (uint16_t i=0;i<psize;i++) {
    mean+=parray[i];
  }

  return mean/psize;
}
float getMeanf(float *parray,uint16_t psize) {
  double mean=0;
  for (uint16_t i=0;i<psize;i++) {
    mean+=parray[i];
  }

  return mean/psize;
}

bool isValueArrayOK(uint16_t *parray,uint16_t psize, uint16_t pcheck) { //check if array has error values
  for (uint16_t i=0;i<psize;i++) {
    if (parray[i]==pcheck){
      return false;
    }
  }
  return true;
}
bool isValueArrayOKf(float *parray,uint16_t psize, float pcheck) { //check if array has error values
  for (uint16_t i=0;i<psize;i++) {
    if (parray[i]==pcheck){
      return false;
    }
  }
  return true;
}




uint16_t getMin(uint16_t *parray, uint16_t psize) {
  uint16_t min=65535;
  for (uint16_t i=0;i<psize;i++) {
    if (parray[i]<min) {
      min=parray[i];
    }
  }

  return min;
}

uint16_t getMax(uint16_t *parray,uint16_t psize) {
  uint16_t max=0;
  for (uint16_t i=0;i<psize;i++) {
    if (parray[i]>max) {
      max=parray[i];
    }
  }

  return max;
}

float getMinf(float *parray, uint16_t psize) {
  float min=65535;
  for (uint16_t i=0;i<psize;i++) {
    if (parray[i]<min) {
      min=parray[i];
    }
  }

  return min;
}

float getMaxf(float *parray,uint16_t psize) {
  float max=0;
  for (uint16_t i=0;i<psize;i++) {
    if (parray[i]>max) {
      max=parray[i];
    }
  }

  return max;
}




void printAddress(DeviceAddress deviceAddress)
{
  for (uint8_t i = 0; i < 8; i++)
  {
    // zero pad the address if necessary
    if (deviceAddress[i] < 16) Serial.print("0");
    Serial.print(deviceAddress[i], HEX);
  }
}


void IRAM_ATTR isr_flow() {
	flow_counter++;
  flow_counter_sum++;
}

float getFilteredf(float *parray,uint16_t psize, uint16_t pcutOff) {
  //cuts off lowest and highest pcutOff values from array, then returns the mean of the psize-2*pcutOff center values.
  //pcutOff < psize/2

  float _copy[psize];
  std::copy(parray,parray + psize, _copy);
  sortArray(_copy,psize);

  double mean=0;
  for (uint16_t i=pcutOff;i<psize-pcutOff;i++) {
    mean+=_copy[i];
  }

  return mean/(psize-2*pcutOff);
}