#include <Homie.h>
#include <ArduinoOTA.h>
#include <Arduino.h>
#include <SoftwareSerial.h>

// Timeout, wann die Verbindung als "zurückgesetzt" angesehen werden soll und wieder auf das erste Byte gewartet wird
#define TIMEOUT 500

#define FW_NAME "esp-videoswitcher"
#define FW_VERSION "1.0.0"

SoftwareSerial swSer(D2, D1);
int currentnumber = 0;
int inputbytes[4];
int commandbytes[4];
String output = "";
String output2 = "";

HomieNode switchNode("switch", "switch");

bool switchHandler(const HomieRange& range, const String& value) {
  Homie.getLogger() << "switch " << ": " << value << endl;
  int localByte = 0;

  int lastspace = 0;
  int nextspace = 0;
  while (nextspace < value.length()-1)
  {
    // Nur solange wir noch Platz für Inputbytes haben... :)
    if (localByte <= 3) {
      nextspace = value.indexOf(' ', lastspace);
      commandbytes[localByte] = (value.substring(lastspace,nextspace-1)).toInt(); // TEST for sanity
      localByte++;
    }
  }
  
  if (localByte ==4)
  {
      // Nur wenn wirklich 4 Werte übernommen wurden
      switchNode.setProperty("switch").send(value);
      // Sende an das Gerät
      swSer.write(commandbytes[0]);
      swSer.write(commandbytes[1]);
      swSer.write(commandbytes[2]);
      swSer.write(commandbytes[3]);
  }
  else
  {
      commandbytes[0] = 0;
      commandbytes[1] = 0;
      commandbytes[2] = 0;
      commandbytes[3] = 0;
      switchNode.setProperty("switch").send("ERROR");
  }
  return true;
}

void setup() {
  Serial.begin(115200);
  swSer.begin(9600);
  Serial.println("\nKramer VS-162AV");

  Homie_setFirmware(FW_NAME, FW_VERSION);
  Homie_setBrand(FW_NAME);

  switchNode.advertise("switch").settable(switchHandler);

  Homie.setup();

  ArduinoOTA.setHostname(Homie.getConfiguration().deviceId);
  ArduinoOTA.begin();
}

void loop() {
  Homie.loop();
  ArduinoOTA.handle();

  while (swSer.available() > 0) {
    int r1 = swSer.read();
    Serial.print(currentnumber);
    Serial.print(": ");
    Serial.print(r1);
    Serial.print(" ");
    inputbytes[currentnumber] = r1;
    
    output += String(r1, DEC);
    output += " ";
    if (currentnumber == 3) {
      switchNode.setProperty("data").send(output);
      output2 = "";
      output2 += "OUTPUT ";
      output2 += String(inputbytes[2]-128, DEC);
      output2 += " INPUT ";
      output2 += String(inputbytes[1]-128, DEC);
      switchNode.setProperty("switchevent").send(output2);
      switchNode.setProperty("input").send(String(inputbytes[1]-128, DEC));
      switchNode.setProperty("output").send(String(inputbytes[2]-128, DEC));      

      currentnumber = 0;
      inputbytes[0] = 0;
      inputbytes[1] = 0;
      inputbytes[2] = 0;
      inputbytes[3] = 0;
      output = "";

    } else {
      Serial.print(">");
      Serial.print(currentnumber);
      Serial.print("<");
      currentnumber++;
    }
  }
}