// Device ID (grafana label)
#define DEV_ID "humi"

// ESP32 with Wifi
#define DEVICE "ESP32"
#include <WiFiMulti.h>
WiFiMulti wifiMulti;
#define WIFI_SSID "cazicthule2"
#define WIFI_PASSWORD "kalamazoomi"

// InfluxDB for grafana
#include <InfluxDbClient.h>
#define INFLUXDB_URL "http://192.168.1.6:8086"
#define INFLUXDB_DB_NAME "ifdb"
InfluxDBClient client(INFLUXDB_URL, INFLUXDB_DB_NAME);
Point sensor(DEV_ID);

// BME280 - TPH Temperature Pressure Humidity
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BME280.h>
Adafruit_BME280 bme;

const int HUMID_PIN_A = 16; // Mosfet A for ultrasonic himidifier
const int HUMID_PIN_B = 23; // Mosfet B (unused for now)

const int PID_PIN_KP = 33; //orange
const int PID_PIN_KI = 32; //brown
const int PID_PIN_KD = 34; //green\

int potKp;
int potKi;
int potKd;

const float Kp_Range = 2;
const float Ki_Range = 0.2;
const float Kd_Range = 2;

// TODO add buttons to control these targets
int VPD_mode = 1; // VPD -> 1; RH -> 0
float VPD_target = 0.9; // Vapor Pressure Deficit target
float RH_target = 65; // Relative Humidity target
float setpoint;

float temp_meas=18;
float humid_meas=50;

// used to revert outlandish measurements
float humid_sane=humid_meas;
float temp_sane=temp_meas;

float Psat(float T){
  // Calculate the Saturation Vapor Pressure using Arden Buck equation
  // Psat = 0.61121 exp( (18.678 - T/234.5) * (T/(257.14+T)) )
  return 0.61121 * pow(2.7182818284, (18.678 - T/234.5) * (T/(257.14+T)));
}

float VPD(float T, float RH){
  // Calculate the Vapor Pressure Deficit in kPa
  return Psat(T) * (1-RH/100.);
}

float RH(float VPDtarget, float T){
  // Compute the required %RH to hit VPD_target
  return 100. * ( 1 - VPDtarget/Psat(T) );
}

double readPot(int pin){
  double pot_val;
  double mag = 0.5;
  pot_val = (double)analogRead(pin);
  if(pot_val<1000) {
    mag=0.0;
  }
  else{
    if (pot_val > 3000){
      mag=1.0;
    }
    else{
      mag = (pot_val-1000.)/(3000.-1000.);
    }
  }
  return(mag);
}



// PID Controller
#include <PID_v1.h>
//Declare PID Variables
double PID_Setpoint, PID_Input, PID_Output;
unsigned long PID_now;
double PID_Kp=0.05, PID_Ki=5, PID_Kd=0;

//Specify the links and initial tuning parameters
PID myPID(&PID_Input, &PID_Output, &PID_Setpoint, PID_Kp, PID_Ki, PID_Kd, P_ON_M, DIRECT);

// PID timer for interval between turning device on/off 10000=10seconds
int PID_WindowSize = 10000;

int PID_RESTART(){
  float out=PID_Output;
  myPID.SetMode(AUTOMATIC);
  myPID.SetOutputLimits(0.0, 1.0);  // Forces minimum up to 0.0
  myPID.SetOutputLimits(-1.0, 0.0);  // Forces maximum down to 0.0
  myPID.SetOutputLimits(1, PID_WindowSize-1); // reset the output range
  if (0<out && out<PID_WindowSize){
    PID_Output=out;
    return(1);
  }
  else{
    PID_Output=0.25*PID_WindowSize;
    return(0);
  }
}



void setup() {

  pinMode(HUMID_PIN_A, OUTPUT);
  pinMode(HUMID_PIN_B, OUTPUT);

  pinMode(PID_PIN_KP, INPUT);
  pinMode(PID_PIN_KI, INPUT);
  pinMode(PID_PIN_KD, INPUT);

  Serial.begin(115200);

  // BME setup
  if (!bme.begin(0x76)) {
    Serial.println("Could not find a valid BME280 sensor, check wiring!");
    while (1);
  }

  // PID setup
  //initialize the variables we're linked to (Input and Setpoint)
  PID_Setpoint = RH(VPD_target,bme.readTemperature());
  //tell the PID to range between 0 and the full window size
  myPID.SetOutputLimits(1, PID_WindowSize-1);
  //turn the PID on
  myPID.SetMode(AUTOMATIC);
  PID_Output=0.25*(PID_WindowSize);
  
  // WiFi setup
  Serial.println("Connecting to WiFi");
  WiFi.mode(WIFI_STA);
  wifiMulti.addAP(WIFI_SSID, WIFI_PASSWORD);
  while (wifiMulti.run() != WL_CONNECTED) {
    Serial.print(".");
    delay(500);
  }
  Serial.println();
  // Check server connection
  if (client.validateConnection()) {
    Serial.print("Connected to InfluxDB: ");
    Serial.println(client.getServerUrl());
  } else {
    Serial.print("InfluxDB connection failed: ");
    Serial.println(client.getLastErrorMessage());
  }
}

void loop() {

// If measured RH is too low

  PID_now = millis();

  // This loop runs every 10seconds (windowsize)
  //loop for device on time (deltat < windowsize) 10000 = 10seconds between power cycles
  while ( millis() - PID_now < PID_WindowSize ){
    // Measure Temperature and Humidity (TODO do some error checking)
    temp_meas = bme.readTemperature(); // Celcius
    
    humid_meas = bme.readHumidity(); // Relative Humidity (%)
  
    // Calculate the setpoint RH from the target VPD and current temperature
    setpoint=(RH(VPD_target,temp_meas)); 

    if (humid_meas<0.9*setpoint){
      //reset PID (only in automatic mode)
      //myPID.SetOutputLimits(0.0, 1.0);  // Forces minimum up to 0.0
      //myPID.SetOutputLimits(-1.0, 0.0);  // Forces maximum down to 0.0
      //myPID.SetOutputLimits(1, PID_WindowSize-1);
      // turn humi full on
      PID_Output=PID_Output+PID_WindowSize*0.01;
      myPID.SetMode(MANUAL);
      digitalWrite(HUMID_PIN_A, HIGH);

      
      // wait windowsize time to try again
      delay(PID_WindowSize);
    }
    else if (humid_meas>1.1*setpoint) {
      //reset PID (only in automatic mode)
      //myPID.SetOutputLimits(0.0, 1.0);  // Forces minimum up to 0.0
      //myPID.SetOutputLimits(-1.0, 0.0);  // Forces maximum down to 0.0
      //myPID.SetOutputLimits(1, PID_WindowSize-1);
      // turn humi full off 
      PID_Output=abs(PID_Output-PID_WindowSize*0.01);
      myPID.SetMode(MANUAL);
      digitalWrite(HUMID_PIN_A, LOW);

      
      // wait windowsize time to try again
      delay(PID_WindowSize);
    }
    else{
      // PID control
      myPID.SetMode(AUTOMATIC);

      //PID input: the current RH value
      PID_Input = humid_meas;
      //PID Setpoint: calculate the required RH to hit target VPD
      PID_Setpoint = setpoint;

      myPID.Compute();

      //Serial.println(PID_WindowSize);
      //Serial.println(PID_Output);  // duty cycle is PID_Output divided by PID_WindowSize
      //Serial.println(PID_Setpoint);
      //Serial.println(PID_Input);

      // trigger off for cycle
      if ( millis() - PID_now > PID_Output ){
       digitalWrite(HUMID_PIN_A, LOW);
      }
      //trigger on for cycle
      else{
        digitalWrite(HUMID_PIN_A, HIGH);
      }
      delay(200);
    }
  }

  //read PID from pots
  PID_Kp= readPot(PID_PIN_KP)*Kp_Range+1.0;
  PID_Ki= readPot(PID_PIN_KI)*Ki_Range;
  PID_Kd= 2.0;

 
  //print PID
  Serial.print("  KP:");
  Serial.print(PID_Kp);
  Serial.print("  KI:");
  Serial.print(PID_Ki);
  Serial.print("  KD:");
  Serial.println(PID_Kd);

  // update PID
  myPID.SetTunings(PID_Kp, PID_Ki, PID_Kd);

  
  // Write values and triggers into influxdb
  sensor.clearFields();
  sensor.addField("VPD",VPD(temp_meas,humid_meas));
  sensor.addField("VPDtarget",VPD_target);
  sensor.addField("Temperature",(temp_meas*9/5.) + 32.);
  sensor.addField("Humidity",humid_meas);
  sensor.addField("Humidity_target",RH(VPD_target,temp_meas));
  sensor.addField("Duty",PID_Output/(float)PID_WindowSize);
  sensor.addField("PID_Output",PID_Output);
  sensor.addField("PID_Kp",PID_Kp);
  sensor.addField("PID_Ki",PID_Ki);
  sensor.addField("PID_Kd",PID_Kd);
  
  // Send fields to InfluxDB
  Serial.print("Writing: ");
  Serial.println(client.pointToLineProtocol(sensor));
  // If no Wifi signal, try to reconnect it
  if (wifiMulti.run() != WL_CONNECTED) {
    Serial.println("Wifi connection lost");
  }
  
  // Write point
  if (!client.writePoint(sensor)) {
    Serial.print("InfluxDB write failed: ");
    Serial.println(client.getLastErrorMessage());
  }

  // modify window size to tune mister to 30% load
  // need to collect the "average load" over a long period
  // if mean(load) < 0.25 and variance is low ; PID_WindowSize increases by 10%
  // if mean(load) > 0.60 and variance is low ; PID_WindowSize decreases by 10%
  
}