//
// configurablefirmata_esp32
//
// D. Yi @ 2023 5 30
//

#define WIRELESS

#if defined(WIRELESS)
//wi-fi
#include <WiFi.h>

//network!
const char* ssid     = "esp1";
const char* password = "1111100000";
const int NETWORK_PORT = 27016;
// IPAddress local_IP(192, 168, 137, 111);
// IPAddress gateway(192, 168, 137, 1);
// IPAddress subnet(255, 255, 255, 0);

//wi-fi event handlers : re-connect automatically
void WiFiStationConnected(WiFiEvent_t event, WiFiEventInfo_t info){
  Serial.println("Connected to AP successfully!");
}

void WiFiGotIP(WiFiEvent_t event, WiFiEventInfo_t info){
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
  digitalWrite(2, LOW);
}

void WiFiStationDisconnected(WiFiEvent_t event, WiFiEventInfo_t info){
  Serial.println("Disconnected from WiFi access point");
  Serial.print("WiFi lost connection. Reason: ");
  Serial.println(info.wifi_sta_disconnected.reason);
  Serial.println("Trying to Reconnect");
  WiFi.begin(ssid, password);
  digitalWrite(2, HIGH);
}
#endif

//firmata over wi-fi
#include <ConfigurableFirmata.h>
#if defined(WIRELESS)
#include "utility/WiFiClientStream.h"
#include "utility/WiFiServerStream.h"
WiFiServerStream serverStream(NETWORK_PORT);

//firmware update over the air
#include <ArduinoOTA.h>
#endif

//handle digitalWrite
byte previousPORT[TOTAL_PORTS];
void digitalWriteCallback(byte port, int value)
{
  byte i;
  byte currentPinValue, previousPinValue;

  if (port < TOTAL_PORTS && value != previousPORT[port]) {
    for (i = 0; i < 8; i++) {
      currentPinValue = (byte) value & (1 << i);
      previousPinValue = previousPORT[port] & (1 << i);
      if (currentPinValue != previousPinValue) {
        digitalWrite(i + (port * 8), currentPinValue);
      }
    }
    previousPORT[port] = value;
  }
}

////
void setup()
{
  //pinmodes
  //digital inputs
  pinMode(23, INPUT_PULLUP);
  pinMode(22, INPUT_PULLUP);
  pinMode(21, INPUT_PULLUP);
  pinMode(19, INPUT_PULLUP);

  //digital outputs
  pinMode(2, OUTPUT);    //build-in LED && also used as wifi indicator!
#if defined(WIRELESS)
  digitalWrite(2, HIGH);  //HIGH ==> LED ON ==> 'wifi. disconnected.'
#else
  digitalWrite(2, LOW);  //LOW ==> LED OFF
#endif

  //serial (for debug & info)
  Serial.begin(57600);

#if defined(WIRELESS)
  // delete old config
  WiFi.disconnect(true);

  delay(1000);

  WiFi.onEvent(WiFiStationConnected, WiFiEvent_t::ARDUINO_EVENT_WIFI_STA_CONNECTED);
  WiFi.onEvent(WiFiGotIP, WiFiEvent_t::ARDUINO_EVENT_WIFI_STA_GOT_IP);
  WiFi.onEvent(WiFiStationDisconnected, WiFiEvent_t::ARDUINO_EVENT_WIFI_STA_DISCONNECTED);

  /* Remove WiFi event
  Serial.print("WiFi Event ID: ");
  Serial.println(eventID);
  WiFi.removeEvent(eventID);*/

  WiFi.begin(ssid, password);

  Serial.println();
  Serial.println();
  Serial.println("Wait for WiFi... ");
#endif

  //firmata
  Firmata.disableBlinkVersion();
  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
#if defined(WIRELESS)
  Firmata.begin(serverStream);
#else
  Firmata.begin(Serial);
#endif

#if defined(WIRELESS)
  //ota
  ArduinoOTA.onStart([]() {
    Serial.println("Start");
  });
  ArduinoOTA.onEnd([]() {
    Serial.println("\nEnd");
  });
  ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) {
    Serial.printf("Progress: %u%%\r", (progress / (total / 100)));
  });
  ArduinoOTA.onError([](ota_error_t error) {
    Serial.printf("Error[%u]: ", error);
    if (error == OTA_AUTH_ERROR) Serial.println("Auth Failed");
    else if (error == OTA_BEGIN_ERROR) Serial.println("Begin Failed");
    else if (error == OTA_CONNECT_ERROR) Serial.println("Connect Failed");
    else if (error == OTA_RECEIVE_ERROR) Serial.println("Receive Failed");
    else if (error == OTA_END_ERROR) Serial.println("End Failed");
  });
  ArduinoOTA.begin();
#endif
}

void loop()
{
  // loop frequency = 50hz (20 ms)
  static unsigned long last = 0;
  unsigned long now = millis();
  if (now > last + 20) {
    last = now;

    //
    static int counter = 0;
    counter++;
    if (counter == 1000) counter = 0;

    //digital
    //==> https://randomnerdtutorials.com/esp8266-pinout-reference-gpios/
    byte port = 0;

    static byte port0_prev = 0;
    port = 0x00;
    port |= (digitalRead(23) ? 0x01 : 0x00);
    port |= (digitalRead(22) ? 0x02 : 0x00);
    port |= (digitalRead(21) ? 0x04 : 0x00);
    port |= (digitalRead(19) ? 0x08 : 0x00);
    port |= (0 ? 0x10 : 0x00);
    port |= (0 ? 0x20 : 0x00);
    port |= (0 ? 0x40 : 0x00);
    port |= (0 ? 0x80 : 0x00);
    if (port0_prev != port) Firmata.sendDigitalPort(0, port);
    port0_prev = port;

    //send out alive counter every 1sec. (50 counts x 20ms == 1 sec.)
    if (counter % 50 == 0) {
      static int alivecounter = 0;
      Firmata.sendAnalog(0, alivecounter);
      alivecounter++;
      if (alivecounter == 1000) alivecounter = 0;
    }

    //analog reads
    Firmata.sendAnalog(1, analogRead(36));
    Firmata.sendAnalog(2, analogRead(39));
    Firmata.sendAnalog(3, analogRead(34));
    Firmata.sendAnalog(4, analogRead(35));

    //touch reads
    Firmata.sendAnalog(5, touchRead(32)); //touch 9
    Firmata.sendAnalog(6, touchRead(33)); //touch 8
    Firmata.sendAnalog(7, touchRead(27)); //touch 7
    Firmata.sendAnalog(8, touchRead(13)); //touch 4
  }

  //process firmata stream
  while (Firmata.available()) {
    Firmata.processInput();
  }

#if defined(WIRELESS)
  //firmata over wi-fi
  serverStream.maintain();

  //
  ArduinoOTA.handle();
#endif

  //for wdt
  yield();
}