#include "node_interface_hardware.hpp"
#include "pico/stdlib.h"
#include "hardware/i2c.h"
#include <string.h>

GobotRPC_NI_Hardware_RP2040_I2C * g_gobotrpc_ni_hardware_rp2040_i2c;
i2c_inst_t * g_i2c_inst;
bool g_transfer_in_progress = false;

static void finish_transfer(BaseType_t * xHigherPriorityTaskWoken) {
    if(g_transfer_in_progress) {
        GobotRPC_NI_Hardware_RP2040_I2C * gobotrpc_ni = g_gobotrpc_ni_hardware_rp2040_i2c;
        g_transfer_in_progress = false;
        gobotrpc_ni->onI2CIRQ(I2C_SLAVE_FINISH, xHigherPriorityTaskWoken);
    }
}

static void i2c_isr() {
    GobotRPC_NI_Hardware_RP2040_I2C * gobotrpc_ni = g_gobotrpc_ni_hardware_rp2040_i2c;
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;

    i2c_hw_t * hw = i2c_get_hw(g_i2c_inst);
    uint32_t intr_stat = hw->intr_stat;

    if (intr_stat == 0) {
        return;
    }

    if (intr_stat & I2C_IC_INTR_STAT_R_TX_ABRT_BITS) {
        hw->clr_tx_abrt;
        finish_transfer(&xHigherPriorityTaskWoken);
    }

    if (intr_stat & I2C_IC_INTR_STAT_R_START_DET_BITS) {
        hw->clr_start_det;
        finish_transfer(&xHigherPriorityTaskWoken);
    }

    if (intr_stat & I2C_IC_INTR_STAT_R_STOP_DET_BITS) {
        hw->clr_stop_det;
        finish_transfer(&xHigherPriorityTaskWoken);
    }

    if (intr_stat & I2C_IC_INTR_STAT_R_RX_FULL_BITS) {
        g_transfer_in_progress = true;
        gobotrpc_ni->onI2CIRQ(I2C_SLAVE_RECEIVE, &xHigherPriorityTaskWoken);
    }

    if (intr_stat & I2C_IC_INTR_STAT_R_RD_REQ_BITS) {
        hw->clr_rd_req;
        g_transfer_in_progress = true;
        gobotrpc_ni->onI2CIRQ(I2C_SLAVE_REQUEST, &xHigherPriorityTaskWoken);
    }

    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

static void preTxTaskFn(void *pvParameters) {
    GobotRPC_NI_Hardware_RP2040_I2C * hw = (GobotRPC_NI_Hardware_RP2040_I2C *)(pvParameters);
    hw->preTxTask();
};

GobotRPC_NI_Hardware_RP2040_I2C::GobotRPC_NI_Hardware_RP2040_I2C(
    xQueueHandle TXQueue, xQueueHandle RXQueue, uint core,
    i2c_inst_t * i2c_inst,
    uint8_t i2cAddress,
    uint i2cSDA_PIN, uint i2cSCL_PIN, uint int_PIN
) {
    g_gobotrpc_ni_hardware_rp2040_i2c = this;
    g_i2c_inst = i2c_inst;

    this->core = core;

    this->readState = I2C_READ_STAGE_FIRST;
    this->writeState = I2C_WRITE_STATE_FIRST;

    this->isrRXQueue = RXQueue;
    this->isrPreTxQueue = TXQueue;
    this->isrTXQueue = xQueueCreate(3, sizeof(GobotRPC_NI_Package_Transport));

    xTaskCreateAffinitySet(
        preTxTaskFn,
        "GobotRPC_NI Hardware RP2040 I2C PreTx Task",
        2048,
        this,
        4,
        this->core,
        &this->preTxTaskHandle
    );

    this->i2cSDA_PIN = i2cSDA_PIN;
    this->i2cSCL_PIN = i2cSCL_PIN;
    this->int_PIN = int_PIN;

    this->i2cAddress = i2cAddress;
    this->i2c_inst = i2c_inst;

    memset(&rxPackage.data, 0, sizeof(GobotRPC_NI_Package_Transport::data));
    memset(&txPackage.data, 0, sizeof(GobotRPC_NI_Package_Transport::data));

    gpio_init(i2cSDA_PIN);
    gpio_init(i2cSCL_PIN);
    gpio_set_function(i2cSDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(i2cSCL_PIN, GPIO_FUNC_I2C);

    gpio_init(int_PIN);
    gpio_set_dir(int_PIN, GPIO_OUT);
    gpio_put(int_PIN, 0);

    // Note: The I2C slave does clock stretching implicitly after a RD_REQ, while the Tx FIFO is empty.
    // There is also an option to enable clock stretching while the Rx FIFO is full, but we leave it
    // disabled since the Rx FIFO should never fill up (unless slave->handler() is way too slow).
    i2c_set_slave_mode(i2c_inst, true, i2cAddress);

    i2c_hw_t *hw = i2c_get_hw(i2c_inst);
    // unmask necessary interrupts
    hw->intr_mask = I2C_IC_INTR_MASK_M_RX_FULL_BITS |\
        I2C_IC_INTR_MASK_M_RD_REQ_BITS |\
        I2C_IC_RAW_INTR_STAT_TX_ABRT_BITS |\
        I2C_IC_INTR_MASK_M_STOP_DET_BITS |\ 
        I2C_IC_INTR_MASK_M_START_DET_BITS;

    // enable interrupt for current core
    unsigned int intNum = i2c_inst == i2c0 ? I2C0_IRQ : I2C1_IRQ;
    irq_set_exclusive_handler(intNum, i2c_isr);
    irq_set_enabled(intNum, true);
}
