#include "IOBoard.h"
#include <SPI.h>
#include "ArduinoConfig.h"
#include "helper/SimpleRingBuffer.h"
#include "helper/DleEncoder.h"
#include "helper/crc_ccitt.h"

SimpleRingBuffer ringBuffer(RING_BUFFER_SIZE, true);

uint8_t rawData[2 * RING_BUFFER_SIZE];
size_t rawDataSize = 0;

#define PACKET_COMMAND_LENGTH       1
#define PACKET_ADDRESS_LENGTH       1
#define PACKET_SIZE_FIELD_LENGTH    2

#define PACKET_HEADER_LENGTH        4

namespace IOBoard {

void handleNewData() {
    ringBuffer.readData(rawData, sizeof(rawData), true, &rawDataSize);

    if (rawDataSize == 0) {
        return;
    }

    //look for STX
    size_t firstSTXinRawData = 0;
    while ((firstSTXinRawData < rawDataSize)
            && (rawData[firstSTXinRawData] != DleEncoder::STX)) {
        Serial.println(rawData[firstSTXinRawData]);
        firstSTXinRawData++;
    }

    if (rawData[firstSTXinRawData] != DleEncoder::STX) {
        //there is no STX in our data, throw it away...
        Serial.println("NO STX");
        ringBuffer.deleteData(rawDataSize);
        return;
    }

    uint8_t packet[MAX_PACKET_LENGTH];
    size_t packetLen;

    size_t readSize;

    ReturnValue_t result = DleEncoder::decode(rawData + firstSTXinRawData,
            rawDataSize - firstSTXinRawData, &readSize, packet, sizeof(packet),
            &packetLen);

    size_t toDelete = firstSTXinRawData;
    if (result == HasReturnvaluesIF::RETURN_OK) {
        handlePacket(packet, packetLen);

        // after handling the packet, we can delete it from the raw stream,
        // it has been copied to packet
        toDelete += readSize;
    }

    //remove Data which was processed
    ringBuffer.deleteData(toDelete);

}

void handlePacket(uint8_t *packet, size_t packetLen) {
    /* Paket layout is:
      ------------------------------
     |byte     |  field             |
     |         |                    |
     |------------------------------|
     |1        | 8 bit command      |
     |------------------------------|
     |1        | 8 bit address      |
     |------------------------------|
     |2        | 16bit length       |
     |------------------------------|
     |length   | <length> byte data |
     |------------------------------|
     |2        | 16 bit crc         |
     |------------------------------|
      ------------------------------
    */

    uint16_t crc = Calculate_CRC(packet, packetLen);

    if (crc != 0) {
        Serial.println("-AI- Invalid packet checksum detected!");
        return;
    }

    uint16_t payloadLen = (packet[2] << 8) | packet[3];

    if (payloadLen != packetLen - 6) {
        Serial.println("-AI- Invalid packet length detected!");
        return;
    }

    uint8_t command = packet[0];
    uint8_t address = packet[1];

    switch (command) {
    case COMMAND_TRANSFER_SPI:
        transferSPI(address, packet + PACKET_HEADER_LENGTH, payloadLen);
        //echo the data back, no need to change the header fields, they are the same
        //checksum will be written by sendReply()
        //check reply:
        Serial.println("Data response check: ");
        for(size_t i =0; i< packetLen; i++){
            Serial.print("packet nr ");Serial.print(i);Serial.print(" ");
            Serial.println(packet[i]);
        }
        sendReply(packet, packetLen);
        break;
    default:
        Serial.println("invalid command");
        break;
    }

}

void transferSPI(uint8_t address, uint8_t *data, size_t datalen) {
    SPI.beginTransaction(SPISettings(14000000, MSBFIRST, SPI_MODE3));
    // The specified address is the bit where the last bit is port 0
    // and the first bit is port 7. It is inverted because the SPI protocol
    // requires the slave select to be driven low.
    CS_PORT = ~address;
    SPI.transfer(data, datalen);
    // some MGM stuff. This delay might not be needed for other devices.
    delay(100);
    // Pull the slave select high again.
    CS_PORT = 0xff;
    SPI.endTransaction();
}

/**
 * Encode and send the data, last two bytes of data are assumed to be for the checksum
 * and will be overwritten with it
 */
void sendReply(uint8_t *data, size_t len) {
    uint16_t crc = Calculate_CRC(data, len - 2);
    data[len - 2] = crc >> 8;
    data[len - 1] = crc;
    //we're being conservative here
    //TODO move this to global so other protocols can use it too
    uint8_t buffer[2 * len + 2];
    buffer[0] = DleEncoder::STX;
    size_t writtenLen;
    ReturnValue_t result = DleEncoder::encode(data, len, buffer, sizeof(buffer),
            &writtenLen, true);
    if (result != HasReturnvaluesIF::RETURN_OK) {
        return;
    }
    Serial.write(buffer, writtenLen);
}


//TODO check if this is thread safe by arduino
// I think it is, see HardwareSerial::available()
void serialEvent() {
    //Serial.println(ringBuffer.availableWriteSpace());
    //uint8_t i = 0;

    while (Serial.available()>0) {
        uint8_t byte = Serial.read();
        ringBuffer.writeData(&byte, 1);
    }
    //Serial.println(ringBuffer.availableWriteSpace());
}
}