#include "transmission_interface.hpp"
#include "ci/base.hpp"

void i2cRxTaskFn(void * args) {
    GobotRPC_TI_Hardware_RP2040_I2C * hw = (GobotRPC_TI_Hardware_RP2040_I2C *)args;
    hw->i2cRxTask();
}

void GobotRPC_TI_Hardware_RP2040_I2C::i2cRxTask() {
    while (1) {
        xSemaphoreTake(i2cRXSemaphore, portMAX_DELAY);

        uint32_t done_mask = 0xffffffff;

        uint32_t pinStates = readIntPins();

        do {
            for(int i=0; i<GOBOTRPC_TI_INT_NUM; i++) {
                if((pinStates & done_mask) & (1 << i)) {
                    done_mask &= ~(1 << i);
            

                    uint32_t addr = intAddressMap[i];

                    if(addr > 0xff)
                        continue;

                    GoRPCPackage_Transport pkg;

                    xSemaphoreTake(i2cMutex, portMAX_DELAY);
                    bool read_res = readI2C(&pkg, addr);
                    xSemaphoreGive(i2cMutex);

                    pkg.addr |= (i) << 8;

                    if(pushPackageCB != NULL)
                        pushPackageCB(&pkg, pushPackageCBArgs);
            
                    break;
                }
            }
            
            pinStates = readIntPins();
        } while(pinStates & done_mask);
    }
}

bool GobotRPC_TI_Hardware_RP2040_I2C::readI2C(GoRPCPackage_Transport * pkg, uint32_t addr) {
    unsigned int res;
    //uint res = i2c_read_blocking(i2c, addr, (uint8_t *)pkg->data, 2, true);

    i2c_read_blocking(i2c, addr, (uint8_t *)pkg->data + CI_RX_PACKAGE_DATA_OFFSET, 2, true);


    size_t len = pkg->data[CI_RX_PACKAGE_DATA_OFFSET + 1];

    res = i2c_read_blocking(i2c, addr, ((uint8_t *)pkg->data) + CI_RX_PACKAGE_DATA_OFFSET, len, false);
    pkg->len = len;
    pkg->addr = addr;

    return true;
}