#include "node_interface_hardware.hpp"
#include "hardware/irq.h"

void GobotRPC_NI_Hardware_RP2040_I2C::onI2CIRQ(I2C_SLAVE_EVENT event, BaseType_t * xHigherPriorityTaskWoken) {

    switch (event) {
    case I2C_SLAVE_RECEIVE: { // I2C Write from Master
        char data = i2c_read_byte_raw(i2c_inst);

        switch (writeState) {
        case I2C_WRITE_STATE_FIRST:
            writeState = I2C_WRITE_STATE_LENGTH;
            rxPackage.index = 0;
            rxPackage.length = 0;
            rxPackage.data[rxPackage.index++] = data;
            break;

        case I2C_WRITE_STATE_LENGTH:
            rxPackage.data[rxPackage.index++] = data;
            rxPackage.length = data;
            writeState = I2C_WRITE_STATE_DATA;
            break;
        
        case I2C_WRITE_STATE_DATA:
            rxPackage.data[rxPackage.index++] = data;
            
            if(rxPackage.index == rxPackage.length) {
                xQueueSendFromISR(isrRXQueue, &rxPackage, xHigherPriorityTaskWoken);
                writeState = I2C_WRITE_STATE_FIRST;
            } else {
                writeState = I2C_WRITE_STATE_DATA;
            }

            break;

        default:
            break;
        }

        break;
    }

    case I2C_SLAVE_REQUEST: { 
        // I2C Read from Master
        //if(xQueueIsQueueEmptyFromISR(isrTXQueue) == pdTRUE) {
        //    readState = I2C_READ_STAGE_INVALID;
        //}

        switch (readState) {
        case I2C_READ_STAGE_INVALID:
            i2c_write_byte_raw(i2c_inst, 0x01);
            break;
        
        case I2C_READ_STAGE_FIRST:
            xQueueReceiveFromISR(isrTXQueue, &txPackage, xHigherPriorityTaskWoken);
            txPackage.index = 0;
            i2c_write_byte_raw(i2c_inst, txPackage.data[txPackage.index++]);
            readState = I2C_READ_STAGE_WIP;
            break;

        case I2C_READ_STAGE_WIP:
            i2c_write_byte_raw(i2c_inst, txPackage.data[txPackage.index++]);
            
            if(txPackage.index == txPackage.length) {
                readState = I2C_READ_STAGE_FIRST;
                gpio_put(int_PIN, 0);
            }

            break;

        default:
            break;
        }

        break;
    }

    case I2C_SLAVE_FINISH:
        writeState = I2C_WRITE_STATE_FIRST;
        
        rxPackage.index = 0;
        txPackage.index = 0;
        break;

    default:
        break;
    }
}

void GobotRPC_NI_Hardware_RP2040_I2C::preTxTask() {
    GobotRPC_NI_Package_Transport pkg;

    while (1) {
        xQueueReceive(isrPreTxQueue, &pkg, portMAX_DELAY);
        xQueueSend(isrTXQueue, &pkg, portMAX_DELAY);
        vTaskDelay(5 / portTICK_PERIOD_MS);
        gpio_put(int_PIN, 1);
    }
}