#include "transmission_interface.hpp"

void i2cTxTaskFn(void * args) {
    GobotRPC_TI_Hardware_RP2040_I2C * hw = (GobotRPC_TI_Hardware_RP2040_I2C *)args;
    hw->i2cTxTask();
}

void GobotRPC_TI_Hardware_RP2040_I2C::i2cTxTask() {
    while (1) {
        GoRPCPackage_Transport pkg;

        if(pullPackageCB == NULL) {
            vTaskDelay(pdMS_TO_TICKS(100));
            continue;
        }
        
        pullPackageCB(&pkg, pullPackageCBArgs);

        xSemaphoreTake(i2cMutex, portMAX_DELAY);
        unsigned int res = i2c_write_blocking(i2c, pkg.addr & 0x7f, (uint8_t * )(pkg.data), pkg.len, false);        
        xSemaphoreGive(i2cMutex);

        if(res == PICO_ERROR_GENERIC) {
            raiseTranmissionError(false, pkg.addr);
        } else {
            raiseTransmissionSuceess(pkg.addr);
        }
    }
}