#include <SPI.h>
#include "api/Compat.h"
#include "common.hpp"
#include <WiFiNINA.h>


uint8_t buffer[0x8000];
const long SERIAL_TIMEOUT = 6000;  // ms
SPISettings spiSettings(100, MSBFIRST, SPI_MODE0);  // max freq [Hz], bit order, mode
// lowest frequency seems to be 2000 Hz

/* void maskedWrite(uint32_t mask, uint32_t bits) { */
/*   for (uint32_t i = 0; i < 32; i++) { */
/*       if (mask & (1 << i)) { */
/*           digitalWrite(i, bits & (1 << i)); */
/*       } */
/*   } */
/* } */ 
void blinkLED(unsigned n=5, unsigned delay_=200) { 
    for (unsigned i = 0; i < n; i++) {
        digitalWrite(LED_BUILTIN, HIGH);
        delay(delay_);
        digitalWrite(LED_BUILTIN, LOW);
        delay(delay_);
    }
}


void sendErrorMSG(const String& msg1, int i1, const String& msg2="", int i2=-2, int mode=DEC) {
    Serial.write(ControlBytes::PRINT);
    Serial.print(msg1);
    Serial.print(i1, mode);
    Serial.print(msg2);
    if (i2 != -2)
        Serial.print(i2, mode);
    Serial.println("");
}


uint8_t receiveControlByte(const String& fname) {
    uint8_t recByte;
    // not using Serial.read() because it doesnt use the set serial timeout
    if (Serial.readBytes(&recByte, 1) != 1) {
        Serial.write(ControlBytes::PRINT);
        Serial.print(fname);
        Serial.print(": failed to receive control byte");
        return 0xff;
    }
    return recByte;
}


buffer_t receiveSize() {
    // receive size to read or write
    buffer_t size = 0;
    for (unsigned i = 0; i < sizeof(buffer_t); i++) {
        // not using Serial.read() because it doesnt use the set serial timeout
        uint8_t receivedByte = 0;
        if (Serial.readBytes(&receivedByte, 1) != 1) {
            sendErrorMSG("receiveSize: could not read bufferSize from Serial, failed at byte nr. ", i+1);
            return 0;
        }
        size |= (receivedByte << i * 8);
    }
    if (size > MAX_BUFFER_SIZE) {
        sendErrorMSG("Received bufferSize=", size, " is larger than MAX_BUFFER_SIZE=", MAX_BUFFER_SIZE, HEX);
        return 0;
    }
    return size;
}


void write() {
    buffer_t size = receiveSize();
    if (size == 0) {
        return;
    }
    int receivedBytes = Serial.readBytes(buffer, size);
    if (receivedBytes != size) {
        sendErrorMSG("write: Received buffer of size=", receivedBytes, ", expected size=", size, HEX);
        return;
    }
    uint8_t ctrlByte = receiveControlByte("write");
    if (ctrlByte != ControlBytes::WRITE) {
        sendErrorMSG("write: Did not receive control byte write: byte=", ctrlByte);
        return;
    }
    /* // debug */
    /* Serial.write(ControlBytes::PRINT); */
    /* Serial.print("BufSize="); */
    /* Serial.print(size, HEX); */
    /* Serial.print(", buffer="); */
    /* for (unsigned i = 0; i < size; i++) { */
    /*     Serial.print(buffer[i], HEX); */
    /*     Serial.print(","); */
    /* } */
    /* Serial.println("."); */
    // transfer the size in LE
    SPI.transfer(static_cast<uint8_t>(size));
    SPI.transfer(static_cast<uint8_t>(size >> 8));

    SPI.transfer(buffer, size);
    Serial.write(ControlBytes::WRITE);
}



void setup() {
    pinMode(LEDR, OUTPUT);
    pinMode(LEDG, OUTPUT);
    pinMode(LEDB, OUTPUT);
    digitalWrite(LEDR, LOW);
    digitalWrite(LEDG, HIGH);
    digitalWrite(LEDB, HIGH);


    Serial.begin(9600);
    // wait until available
    /* while (!Serial); { */
    /*     delay(100); */
    /* } */
    /* digitalWrite(LED_BUILTIN, LOW); */
    // empty buffer
    while (Serial.read() != -1);
    Serial.setTimeout(SERIAL_TIMEOUT);

    SPI.begin();
    SPI.beginTransaction(spiSettings);

    digitalWrite(LEDR, LOW);
    digitalWrite(LEDG, LOW);
    digitalWrite(LEDB, LOW);
}


void loop() {
    if (!Serial)  {
        delay(100);
        digitalWrite(LEDG, LOW);
        digitalWrite(LEDB, HIGH);
        return;
    }
    else {
        digitalWrite(LEDG, HIGH);
        digitalWrite(LEDB, LOW);
    }

    int ctrlByte = Serial.read();
    if (ctrlByte == -1)  {
        delay(100);
        return;
    }
    /* blinkLED(3, 50); */
    switch(ctrlByte) {
        case ControlBytes::READY: {
            Serial.write(ControlBytes::READY);
            break;
        }
        case ControlBytes::WRITE: {
            digitalWrite(LEDG, LOW);
            digitalWrite(LEDR, HIGH);
            read();
            digitalWrite(LEDR, LOW);
            digitalWrite(LEDG, HIGH);
            break;
        }
        case ControlBytes::READ: {
            digitalWrite(LEDG, LOW);
            digitalWrite(LEDR, HIGH);
            write();
            digitalWrite(LEDR, LOW);
            digitalWrite(LEDG, HIGH);
            break;
        }
        default: {
            digitalWrite(LEDR, HIGH);
            digitalWrite(LEDG, LOW);
            digitalWrite(LEDB, HIGH);
            sendErrorMSG("loop: received invalid ControlByte: byte=", ctrlByte);
            break;
        }
    }
}