#ifdef ESP8266
#include <ESP8266WiFi.h>
#else
#include <WiFi.h>
#endif
#include <WiFiUdp.h>

const char* ssid = "XXXXXXXX";
const char* password = "YYYYYYYYYY";

unsigned int multicastPort = 9522;  // local port to listen on
IPAddress multicastIP(239, 12, 255, 254);

// Initialize WiFi
static void initWiFi() {
    WiFi.mode(WIFI_STA);
    WiFi.begin(ssid, password);
    Serial.print("Connecting to WiFi ..");
    while (WiFi.status() != WL_CONNECTED) {
        Serial.print('.');
        delay(500);
    }
    Serial.println(WiFi.localIP());
}

WiFiUDP Udp;

void setup() {
    Serial.begin(115200);
    Serial.println("Connecting...");

    initWiFi();

    Udp.begin(multicastPort);
#ifdef ESP8266
    Udp.beginMulticast(WiFi.localIP(), multicastIP, multicastPort);
#else
    Udp.beginMulticast(multicastIP, multicastPort);
#endif
}

void loop() {
    uint8_t buffer[1024];
    int packetSize = Udp.parsePacket();
    if (packetSize) {
        int rSize = Udp.read(buffer, 1024);
        if (buffer[0] != 'S' || buffer[1] != 'M' || buffer[2] != 'A') {
            Serial.println("Not an SMA packet?");
            return;
        }
        uint16_t grouplen;
        uint16_t grouptag;
        uint8_t* offset = buffer + 4;
        do {
            grouplen = (offset[0] << 8) + offset[1];
            grouptag = (offset[2] << 8) + offset[3];
            for (int i = 0; i < 8; i++) {
                Serial.print(offset[i], HEX);
                Serial.print(' ');
            }
            Serial.println();
            offset += 4;
            if (grouplen == 0xffff) return;
            if (grouptag == 0x02A0 && grouplen == 4) {
                // Not sure why SMA has this...
                uint32_t defaultGroup = (offset[0] << 24) +
                                        (offset[1] << 16) +
                                        (offset[2] << 8) +
                                        offset[3];
                offset += 4;
                Serial.print("defaultGroup=");
                Serial.println(defaultGroup);
            } else if (grouptag == 0x0010) {
                uint8_t* endOfGroup = offset + grouplen;
                uint16_t protocolID = (offset[0] << 8) + offset[1];
                offset += 2;
                uint16_t susyID = (offset[0] << 8) + offset[1];
                offset += 2;
                uint32_t serial = (offset[0] << 24) + (offset[1] << 16) + (offset[2] << 8) + offset[3];
                offset += 4;
                uint32_t timestamp = (offset[0] << 24) + (offset[1] << 16) + (offset[2] << 8) + offset[3];
                offset += 4;
                Serial.print("protocolID=");
                Serial.print(protocolID);
                Serial.print(", susyID=");
                Serial.print(susyID);
                Serial.print(", serial=");
                Serial.print(serial);
                Serial.print(", timestamp=");
                Serial.println(timestamp);
                while (offset < endOfGroup) {
                    uint8_t kanal = offset[0];
                    uint8_t index = offset[1];
                    uint8_t art = offset[2];
                    uint8_t tarif = offset[3];
                    offset += 4;

                    Serial.print(kanal);
                    Serial.print(':');
                    Serial.print(index);
                    Serial.print('.');
                    Serial.print(art);
                    Serial.print('.');
                    Serial.print(tarif);
                    Serial.print('=');
                    if (art == 8) {
                    // TODO: hier passt noch was nicht, entweder beim berechnen von "data" oder beim println() später.
                        uint64_t data = ((uint64_t)offset[0] << 56) +
                                        ((uint64_t)offset[1] << 48) +
                                        ((uint64_t)offset[2] << 40) +
                                        ((uint64_t)offset[3] << 32) +
                                        ((uint64_t)offset[4] << 24) +
                                        ((uint64_t)offset[5] << 16) +
                                        ((uint64_t)offset[6] << 8) +
                                        offset[7];
                        offset += 8;
                        Serial.println((double)data);
                    } else if (art == 4) {
                        uint32_t data = (offset[0] << 24) +
                                        (offset[1] << 16) +
                                        (offset[2] << 8) +
                                        offset[3];
                        offset += 4;
                        Serial.println(data);
                    } else {
                        // Strange
                        offset += art;
                        Serial.println("Strange measurement skipped");
                    }
                }

            } else if (grouptag == 0) {
                // end marker
                offset += grouplen;
            } else {
                Serial.print("unhandled group ");
                Serial.print(grouptag);
                Serial.print(" with len=");
                Serial.println(grouplen);
                offset += grouplen;
            }

        } while (grouplen > 0 && offset + 4 < buffer + rSize);
    }
}