#include "transmission_interface.hpp"
#include "hardware/i2c.h"

#include <string.h>

#include "pinConfig.hpp"

GobotRPC_TI_Hardware_RP2040_I2C * g_GobotRPC_TI_Hardware_RP2040_I2C;

GobotRPC_TI_Hardware_RP2040_I2C::GobotRPC_TI_Hardware_RP2040_I2C(UBaseType_t core, i2c_inst_t *i2c, QueueHandle_t revCIInstructionQueue) {
    this->core = core;
    this->i2c = i2c0;
    this->revCIInstructionQueue = revCIInstructionQueue;

    g_GobotRPC_TI_Hardware_RP2040_I2C = this;

    i2c_init(i2c, GOBOTRPC_TI_CLOCK_SPEED);
    gpio_set_function(GOBOTRPC_TI_I2C_SDA, GPIO_FUNC_I2C);
    gpio_set_function(GOBOTRPC_TI_I2C_SCL, GPIO_FUNC_I2C);

    if(!GOBOTRPC_TI_EXTERNAL_PULLUP) {
        gpio_pull_up(GOBOTRPC_TI_I2C_SDA);
        gpio_pull_up(GOBOTRPC_TI_I2C_SCL);
    }

    for(int i=0; i<GOBOTRPC_TI_INT_NUM; i++) {
        intAddressMap[i] = 0xffffffff;
        gpio_init(GOBOTRPC_TI_INT_PIN_START + i);
        gpio_set_dir(GOBOTRPC_TI_INT_PIN_START + i, GPIO_IN);
        //gpio_pull_down(GOBOTRPC_TI_INT_PIN_START + i);
        gpio_set_irq_enabled_with_callback(GOBOTRPC_TI_INT_PIN_START + i, GPIO_IRQ_EDGE_RISE, true, intPin_ISR);
    }

    initTasks();

    //gpio_init(GOBOTRPC_TI_COMBINED_INT_PIN);
    //gpio_set_dir(GOBOTRPC_TI_COMBINED_INT_PIN, GPIO_IN);
    //gpio_pull_down(GOBOTRPC_TI_COMBINED_INT_PIN);

}

static void i2cIntTaskFn(void * pvParameters) {
    GobotRPC_TI_Hardware_RP2040_I2C * hw = (GobotRPC_TI_Hardware_RP2040_I2C *)pvParameters;
    hw->manualTriggerIntTask();
}

static void revCIInstructionTaskFn(void * pvParameters) {
    GobotRPC_TI_Hardware_RP2040_I2C * hw = (GobotRPC_TI_Hardware_RP2040_I2C *)pvParameters;
    hw->revCIInstructionTask();
}

void GobotRPC_TI_Hardware_RP2040_I2C::initTasks() {
    i2cMutex = xSemaphoreCreateMutex();
    i2cRXSemaphore = xSemaphoreCreateBinary();

    pushPackageCB = NULL;
    pullPackageCB = NULL;
    pushPackageCBArgs = NULL;
    pullPackageCBArgs = NULL;

    volatile int a = xPortGetFreeHeapSize();

    xTaskCreateAffinitySet(i2cRxTaskFn, "i2c Rx Task", 4096, this, 3, core, &i2cRxTaskHandle);
    xTaskCreateAffinitySet(i2cTxTaskFn, "i2c Tx Task", 4096, this, 3, core, &i2cTxTaskHandle);
    xTaskCreateAffinitySet(i2cIntTaskFn, "i2c Int Task", 4096, this, 3, core, &i2cIntTaskHandle);
    xTaskCreateAffinitySet(revCIInstructionTaskFn, "Rev CI Instruction Task", 4096, this, 3, core, &revCIInstructionTaskHandle);
}

void GobotRPC_TI_Hardware_RP2040_I2C::registerPullPackageCB(callback_pullPackage cb, void *args) {
    pullPackageCB = cb;
    pullPackageCBArgs = args;
}

void GobotRPC_TI_Hardware_RP2040_I2C::registerPushPackageCB(callback_pushPackage cb, void *args) {
    pushPackageCB = cb;
    pushPackageCBArgs = args;
}

void GobotRPC_TI_Hardware_RP2040_I2C::registerPushCIInstructionCB(callback_pushCIInstruction cb, void *args) {
    pushCIInstructionCB = cb;
    pushCIInstructionCBArgs = args;
}

void GobotRPC_TI_Hardware_RP2040_I2C::setAddrMap(uint32_t addr, int intNum) {
    intAddressMap[intNum] = addr;
}

