#include <WiFi.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

#include "EasyPCF8574.h"

EasyPCF8574 pcf_A(0x20,0); //PCF address, initial value


//// WIFI
const int mxSize=4;
String ssids[mxSize] ={"GAST","pipanet","FRITZ!Box Gastzugang","WLAN-DE8245"};
String ssidp[mxSize] = {"passatvr6","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;
}  

const int PWM_FREQ = 5000;             // freq limits depend on resolution
    const byte PWM_CHANNEL = 0;            // channels 0-15
    const byte PWM_RES = 16;                //resolution 1-16 bits
    const byte PWM_PIN = 39;


void setup() {
  Serial.begin(230400); 
  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());
  //server.begin(); // server starten
  display.display();
  ledcAttachPin(PWM_PIN, PWM_CHANNEL); // assign pin to channel
  ledcSetup(PWM_CHANNEL, PWM_FREQ, PWM_RES);

  i2c_scanner();
  delay(1000);
  if (!pcf_A.startI2C(5,4)){
        Serial.println("Not started. Check pin and address.");
        while (true);
    }
  test_pcf8574();
}

int duty_cycle=0;

void generator(){
  if (duty_cycle < 2550){
    if (duty_cycle%5==0){
      int i = duty_cycle; 
      ledcWrite(PWM_CHANNEL, i);
    }
    duty_cycle += 1;
  }
  else {
    duty_cycle=0;
  }
  
}
void loop() {
  generator();
  double volt2 = ReadVoltage(36);
  //double volt2 = ReadVoltage0dB(36);
  //Serial.print(volt2,1);
  //Serial.print("   ");
  Serial.println(volt2);
  display.clear();
  display.drawString(0, 10,"IP "+ WiFi.localIP().toString());
  display.drawString(0,20,"Volt 36 "+String(volt2));
  //display.drawString(0,30,"Volt 14 "+String(volt2));
  display.display();
  delay(300);
}
void test_pcf8574(){
    Serial.println(" Starting cycle:");
    Serial.println("Initial value (as specified):");
    Serial.println(pcf_A.getPCFValue());
    oldFunc(pcf_A.getPCFValue());
    delay(5000);
    
    Serial.println("Setting value to (39):");
    //00100111
    pcf_A.setFullValue(0xFF); //like pcf address :)
    Serial.println("Byte now is:");
    Serial.println(pcf_A.getPCFValue());
        oldFunc(pcf_A.getPCFValue());
    delay(5000);

    Serial.println("Checking bit 2 status:"); //bits starts in 0, from right to left
    Serial.println(pcf_A.getBitValue(2));
    Serial.println("Byte now is:");
    Serial.println(pcf_A.getPCFValue());
        oldFunc(pcf_A.getPCFValue());
    delay(5000);

    Serial.println("Inverting  bit 2 value");
    pcf_A.setInvertBit(2);
    Serial.println(pcf_A.getBitValue(2));
    Serial.println("Byte now is:");
    Serial.println(pcf_A.getPCFValue());
        oldFunc(pcf_A.getPCFValue());
    delay(5000);

    Serial.println("Set bit 2 up:");
    pcf_A.setUpBit(2);
    Serial.println(pcf_A.getBitValue(2));
    Serial.println("Byte now is:");
    Serial.println(pcf_A.getPCFValue());
        oldFunc(pcf_A.getPCFValue());
    delay(5000);

    Serial.println("Set bit 2 down");
    pcf_A.setDownBit(2);
    Serial.println(pcf_A.getBitValue(2));
    Serial.println("Byte now is:");
    Serial.println(pcf_A.getPCFValue());
        oldFunc(pcf_A.getPCFValue());
    delay(5000);

    Serial.println("Done.");
  
}
void i2c_scanner(){
   byte error, address;
   int nDevices;
  for(address = 1; address < 127; address++ ) {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();
    if (error == 0) {
      nDevices++;
      Serial.print("Device found @ 0x");
      if (address<16) Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");
      }
    else if (error==4) 
    {
      Serial.print("Unknow error @ 0x");
      if (address<16) Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0) {
    Serial.println("No I2C devices found\n");    
    #if OLED_DISPLAY
        display.drawString(0, 23 , "No I2C devices found");
        display.display();
    #endif
  } else {
    Serial.println("done\n");
  }
}

void oldFunc(int var) {
  for (int i = 0; i < 16; i++)  {
    Serial.print(((var >> i) & 1) == 1 ? "1" : "0");
  }
  Serial.println();
}

void newFunc(int var) {
  for (unsigned int test = 0x8000; test; test >>= 1) {
    Serial.write(var  & test ? '1' : '0');
  }
  Serial.println();
}

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*3.3/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