#include <WiFi.h>
#include <PubSubClient.h>
#include "SSD1306.h" // alias for `#include "SSD1306Wire.h"

// jfs Wemos lolin32
// jfs Heltec WiFi kit 32 (weisses Board)
//define HELTEC
#define DEBUG

// Initialize the OLED display using Wire library
#ifdef HELTEC
SSD1306  display(0x3c, 4, 15);
#else
SSD1306  display(0x3c, 5, 4);
#endif


const char* mqttServer = "192.168.2.71";
const int mqttPort = 1883;
const char* clientID = "esp_32_relay";
const char* channelName = "/Urs/";

WiFiClient MQTTclient;
PubSubClient client(MQTTclient);

const int relay = 25;

void callback(char* topic, byte* payload, unsigned int length) {
  String payload_buff;
  for (int i=0;i<length;i++) {
    payload_buff = payload_buff+String((char)payload[i]);
    if (payload_buff[0] == '1'){
      Serial.print("on ");
      digitalWrite(relay, LOW);
    }
    if (payload_buff[0] == '0'){
      Serial.print("off ");
      digitalWrite(relay, HIGH);
    }
  }
  Serial.println(payload_buff); // Print out messages.
}
long lastReconnectAttempt = 0;

boolean reconnect() {
  if (client.connect(clientID)) {
    client.subscribe(channelName); // Subscribe to channel.
  }
  return client.connected();
}

unsigned long previousMillis = 0; // Letzter Update-Zeitstempel
const long interval = 1000;       // Sende-Intervall in ms

//// WIFI
const int mxSize=4;
String ssids[mxSize] ={"Home","pipanet","FRITZ!Box Gastzugang","WLAN-DE8245"};
String ssidp[mxSize] = {"2Drittel%13579","passatvr6","praxis123","4955065570896956"};
boolean conok =false;

void netfound(int i){
     display.clear();
     display.setColor(BLACK);
     display.fillRect(0, 0, 128, 10);
     display.setColor(WHITE);
     display.drawString(0,0,String(i));
     display.drawString(20,0,"networks found");
     display.display(); 
}

boolean init_wifi(){
    boolean ok = false;
    WiFi.mode(WIFI_STA);
    WiFi.disconnect();
    delay(100);
    int n = WiFi.scanNetworks();
    Serial.println("scan done");
    if (n == 0) {
        Serial.println("no networks found");
        netfound(0);
    } else {
        Serial.print(n);
        Serial.println(" networks found");
        netfound(n);        
        for (int i = 0; i < n; ++i) {
            for (int p=0;p<mxSize;p++){
              if (WiFi.SSID(i).equals(ssids[p])){
                String pp = ssidp[p];
                String ss = WiFi.SSID(i);
                WiFi.begin(ss.c_str(), pp.c_str());
                i=n;
                ok = true;
              }
            }
            delay(10);
        }
    }     
    return ok;
}  
///// WIFI



void setup() {
  // test
  adcAttachPin(37);
  analogSetClockDiv(1); // 1338mS
  analogSetPinAttenuation(37,ADC_0db);
  pinMode(relay,OUTPUT);
  digitalWrite(relay,HIGH);
  
  // put your setup code here, to run once:
#ifdef HELTEC
  pinMode(16,OUTPUT); digitalWrite(16, LOW); delay(50); digitalWrite(16, HIGH);
#endif 

  Serial.begin(115200); 
  display.init();
  display.flipScreenVertically();
  display.clear();
  display.drawString(0, 0, "Starting...");
  display.display();
  while (!init_wifi()){
    delay(200);  
  }
  display.drawString(0, 10, "Connecting to WiFi...");
  display.display();
  while (WiFi.status() != WL_CONNECTED) {
      delay(500);
      Serial.print(".");
  }
  display.drawString(0, 20, "\nWiFi connected, IP address: "); 
  display.drawString(0, 30, WiFi.localIP().toString());
  client.setServer(mqttServer, mqttPort); // Connect to PubNub.
  client.setCallback(callback);
  lastReconnectAttempt = 0;
  display.display();
  display.init();
  }

void loop() {
    if (!client.connected()) {
    long now = millis();
    if (now - lastReconnectAttempt > 5000) { // Try to reconnect.
      lastReconnectAttempt = now;
      if (reconnect()) { // Attempt to reconnect.
        lastReconnectAttempt = 0;
      }
    }
  } else { // Connected.
    client.loop();
    publishit(); // Publish message.
    delay(100);
  }
}

void publishit() {
  char buffer[10];
  double volt2 = ReadVoltage(36)*127000/27000;
  double volt1 = ReadVoltage0dB(37);
  //Serial.print(volt2,1);
  //Serial.print("   ");
  //Serial.println(volt1);
  dtostrf(volt1, 6, 2, buffer);
  client.publish("/ESP_32_AD/V1",buffer); // Publish message.
  dtostrf(volt2, 6, 2, buffer);
  client.publish("/ESP_32_AD/V2",buffer); // Publish message.
  display.clear();
  display.drawString(0,20,"Volt 1 "+String(volt1));
  display.drawString(0,30,"Volt 2 "+String(volt2));
  display.display();
  delay(2000); 
}

// test
double ReadVoltage0dB(byte pin){
  double reading = analogRead(pin); // Reference voltage is 3v3 so maximum reading is 3v3 = 4095 in range 0 to 4095
  if(reading < 1 || reading > 4095) return 0;
  return reading/4095;
}
double ReadVoltage(byte pin){
  double reading = analogRead(pin); // Reference voltage is 3v3 so maximum reading is 3v3 = 4095 in range 0 to 4095
  if(reading < 1 || reading > 4095) return 0;
  // return -0.000000000009824 * pow(reading,3) + 0.000000016557283 * pow(reading,2) + 0.000854596860691 * reading + 0.065440348345433;
  return -0.000000000000016 * pow(reading,4) + 0.000000000118171 * pow(reading,3)- 0.000000301211691 * pow(reading,2)+ 0.001109019271794 * reading + 0.034143524634089;
} // Added an improved polynomial, use either, comment out as required