#include <ConfigurableFirmata.h>
#include <vector>

const char* ssid     = "nolink2";
const char* password = "2222277777";
const int NETWORK_PORT = 27016;

//// A UDP instance to let us send and receive packets over UDP
//WiFiUDP Udp;
//const IPAddress outIp(192,168,43,13);        // remote IP (not needed for receive)
//const unsigned int outPort = 9999;          // remote port (not needed for receive)
//const unsigned int localPort = 8888;        // local port to listen for UDP packets (here's where we send the packets)


#include <WiFi.h>
#include "utility/WiFiClientStream.h"
#include "utility/WiFiServerStream.h"
WiFiServerStream serverStream(NETWORK_PORT);

std::vector<int> pins = {16, 15, 17, 18, 20, 19, 42, 41, 40, 39, 38, 37, 36, 35, 34, 33}; //16
//2 ==> flipping autonomously 1/0 without grounding! (maybe grounded by something else???)
//1, 3, 4, 5, 6, 7, 8, ==> flipping 1/0 ... while grounded manually.

//i have a feeling ... this relates to 'touch sensing'... this precise flipping might be from cap. charge/discharge cycles?

void setup()
{
  //pinmodes
  for (int idx = 0; idx < pins.size(); idx++) {
    pinMode(pins[idx], INPUT_PULLUP);
  }

  //serial
  Serial.begin(115200);

  //wifi
  WiFi.mode(WIFI_STA);
  // Connect to WiFi network
  Serial.println();
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("");

  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
  Serial.println("Mac address: " + WiFi.macAddress());

  //firmata
  Firmata.disableBlinkVersion();
  Firmata.begin(serverStream);

//  Firmata.begin(115200);
//  Firmata.setFirmwareNameAndVersion("ConfigurableFirmata", FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
}

void loop()
{
  for (int idx = 0; idx < pins.size(); idx++) {
    Firmata.sendAnalog(idx, digitalRead(pins[idx])*1023);
  }

  serverStream.maintain();
}


// [NOTE]
// because firmata analog way has limits of max. channels <= 16. you can use digital way.. like below.
// but this not yet confirmed whether it is working or not.
//

// #include <ConfigurableFirmata.h>

// int pins_port0[8] = {17, 18, 2, 3, 4, 5, 6, 7};
// int pins_port1[8] = {8, 9, 10, 11, 12, 24, 25, 26};
// int pins_port2[8] = {27, 28, 29, 30, 31, 32, 33, 33};

// void setup()
// {
//   for (int idx = 0; idx < 8; idx++) pinMode(pins_port0[idx], INPUT_PULLUP);
//   for (int idx = 0; idx < 8; idx++) pinMode(pins_port1[idx], INPUT_PULLUP);
//   for (int idx = 0; idx < 8; idx++) pinMode(pins_port2[idx], INPUT_PULLUP);

//   Firmata.setFirmwareNameAndVersion("ConfigurableFirmata", FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
//   Firmata.begin(115200);
// }

// void loop()
// {
//   byte port = 0;

//   static byte port0_prev = 0;
//   port = 0x00;
//   for (int idx = 0; idx < 8; idx++) port |= (digitalRead(pins_port0[idx]) ? 0x01<<idx : 0x00);
//   if (port0_prev != port) Firmata.sendDigitalPort(0, port);
//   port0_prev = port;

//   static byte port1_prev = 0;
//   port = 0x00;
//   for (int idx = 0; idx < 8; idx++) port |= (digitalRead(pins_port1[idx]) ? 0x01<<idx : 0x00);
//   if (port1_prev != port) Firmata.sendDigitalPort(1, port);
//   port1_prev = port;

//   static byte port2_prev = 0;
//   port = 0x00;
//   for (int idx = 0; idx < 8; idx++) port |= (digitalRead(pins_port2[idx]) ? 0x01<<idx : 0x00);
//   if (port2_prev != port) Firmata.sendDigitalPort(2, port);
//   port2_prev = port;

//   delay(20);
// }