#include "UBus.h"

#include <PN532_HSU.h>
#include <PN532.h>

PN532_HSU pn532hsu(Serial1);
PN532 nfc(pn532hsu);

#include <FastLED.h>
#define NUM_LEDS 10
#define DATA_PIN 12
CRGB leds[NUM_LEDS];

void UBus::begin() {
    RS485Class::begin(115200);

    Serial.print("UBus starting up on address ");
    Serial.println(addr);

    if (addr >= 0x80) {
        Serial.println("**** This device is unprovisioned! Disabling responder. ****");
    }

    // Peripherals
    FastLED.addLeds<WS2811, DATA_PIN, RGB>(leds, NUM_LEDS);
    FastLED.show();

    // PN532 UART needs a pullup
    pinMode(11, INPUT_PULLUP);
    pinMode(12, INPUT_PULLUP);

    nfc.begin();
    uint32_t versiondata = nfc.getFirmwareVersion();

    // Got ok data, print it out!
    Serial.print("Found chip PN5"); Serial.println((versiondata>>24) & 0xFF, HEX);
    Serial.print("Firmware ver. "); Serial.print((versiondata>>16) & 0xFF, DEC);
    Serial.print('.'); Serial.println((versiondata>>8) & 0xFF, DEC);

    nfc.setPassiveActivationRetries(0xFF);
    nfc.SAMConfig();
    // Blink on bootup
    identify_ts = 1;

    receive();
}

void UBus::poll() {
    if (identify_ts != 0) {
        if (identify_ts + 200 > millis()) {
            digitalWrite(13, false);
        } else {
            identify_ts = 0;
            digitalWrite(13, true);
        }
    }

    while(available() > 0) parse(read());

    uint8_t uid[] = { 0, 0, 0, 0, 0, 0, 0 };	// Buffer to store the returned UID
    uint8_t uidLength;				// Length of the UID (4 or 7 bytes depending on ISO14443A card type)

    // Wait for an ISO14443A type cards (Mifare, etc.).  When one is found
    // 'uid' will be populated with the UID, and uidLength will indicate
    // if the uid is 4 bytes (Mifare Classic) or 7 bytes (Mifare Ultralight)
    bool success = nfc.readPassiveTargetID(PN532_MIFARE_ISO14443A, &uid[0], &uidLength);

    if (success) {
        for (uint8_t i=0; i < uidLength; i++)
        {
            Serial.print(uid[i], HEX);
            Serial.print(" ");
        }
        Serial.println("");
    }
}

// Encodes a single byte and writes it into am output buffer
void UBus::sendByte(uint8_t b) {
    if (b == HDLC_START || b == HDLC_ESC) {
        write(HDLC_ESC);
        write(b ^ 0x20);
    } else {
        write(b);
    }
}

// Builds and sends a response message
void UBus::send(uint8_t msg_type, uint8_t* buf, size_t size) {
    beginTransmission();
    uint16_t crc = CRC16_CCITT_INIT_VAL;
    write(HDLC_START);

    sendByte(addr | 0x80); crc = _crc_ccitt_update(crc, addr | 0x80);
    sendByte(msg_type); crc = _crc_ccitt_update(crc, msg_type);

    for (size_t i = 0; i < size; i++) {
        sendByte(buf[i]); crc = _crc_ccitt_update(crc, buf[i]);
    }

    sendByte(crc & 0xff);
    sendByte(crc >> 8);
    write(HDLC_START);
    endTransmission();
}

// Handle message residing currently in the buffer
void UBus::handleRequest() {
    uint8_t target = buffer[0];
    uint8_t msg_type = buffer[1];

    uint8_t* payload = buffer + 2;
    uint8_t payload_len = buf_pos - 4;
    uint8_t resp[16];

    // Serial.println(msg_type);

    switch (msg_type) {
        case SYS_StatusPoll:
        resp[0] = 0x00;
        sendResponse(msg_type, resp, 1);
        break;

        case SYS_Identify:
        identify_ts = millis();
        sendResponse(msg_type, resp, 0);
        break;

        case SYS_SupportedCommands:
        resp[0] = 0x00;
        resp[1] = 0x01;
        resp[2] = 0x02;

        sendResponse(msg_type, resp, 3);
        // SYS_StatusPoll, SYS_Identify, SYS_SupportedCommands,
        // ...
        break;

        case GPIO_Write:
        for (uint8_t i = 0; i < payload_len / 2; i++) {
            pinMode(payload[2 * i], OUTPUT);
            digitalWrite(payload[2 * i], payload[2 * i + 1]);
        }

        resp[0] = 0x00;
        sendResponse(msg_type, resp, 1);
        break;

        case GPIO_Read:
        for (uint8_t i = 0; i < payload_len; i++) {
            pinMode(payload[i], INPUT_PULLUP);
            resp[i] = digitalRead(payload[i]);
        }
        sendResponse(msg_type, resp, payload_len);
        break;

        case GPIO_WS2812:
        // TODO memcpy?
        for (uint8_t i = 0; i < payload_len / 3; i++) {
            leds[i].r = payload[3*i];
            leds[i].g = payload[3*i + 1];
            leds[i].b = payload[3*i + 2];
        }
        FastLED.show();
        sendResponse(msg_type, {}, 0);
        break;
    }
}

// Sends a response while also handling delayed response in case of broadcast
// messages
void UBus::sendResponse(uint8_t msg_type, uint8_t buf[], size_t size) {
    if (buffer[0] == 0xff) {
        uint32_t _broadcast_timeslot = 20;
        uint32_t offset = 0; //  millis() - _hdlc_start;
        if (_broadcast_timeslot * addr > offset) {
            delay(_broadcast_timeslot * addr - offset); // (millis() - _hdlc_start));
            send(msg_type, buf, size);
        }
    } else {
        send(msg_type, buf, size);
    }
}

void UBus::parse(uint8_t c) {
    if (c == HDLC_ESC) {
        escape = true;
        return;
    }

    if (c == HDLC_START) {
        escape = false;
        // Serial.println(millis() - _hdlc_start);
        // 1 byte address, 1 byte msg_type, 2 bytes checksum
        if (buf_pos >= 4) {
            if (checksum == 0) {
                // frame finish - verified
                // digitalWrite(13, !digitalRead(13));
                if ((buffer[0] == 0xff || buffer[0] == addr) && addr != 0xff) {
                    handleRequest();
                }
            } else {
                Serial.println("Invalid frame: ");
                for (int i = 0; i < buf_pos; i++) { Serial.print(buffer[i], HEX); Serial.print(" "); }
                Serial.println(checksum);
                // frame invalid
            }
        }

        buf_pos = 0;
        checksum = CRC16_CCITT_INIT_VAL;

        return;
    }

    if (buf_pos < HDLC_BUF_SIZE) {
        // Serial.println(c);
        buffer[buf_pos] = escape ? c ^ 0x20 : c;
        checksum = _crc_ccitt_update(checksum, buffer[buf_pos]);
        buf_pos += 1;
    }

    escape = false;
}