#include "transmission_interface.hpp"

#include "FreeRTOS.h"
#include "queue.h"

void GobotRPC_TI_Hardware_RP2040_I2C::raiseTranmissionError(bool rx, uint32_t addr) {
    CI_Instruction_Transport ciInstruction;
    ciInstruction.type = CI_INSTRUCTION_SEND_TRANMISSION_ERROR;
    ciInstruction.data[0] = rx;
    ciInstruction.data[1] = addr & 0xff;
    ciInstruction.data[2] = (addr >> 8) & 0xff;
    ciInstruction.data[3] = (addr >> 16) & 0xff;
    ciInstruction.data[4] = (addr >> 24) & 0xff;
    
    this->pushCIInstructionCB(&ciInstruction, pushCIInstructionCBArgs);
}

void GobotRPC_TI_Hardware_RP2040_I2C::raiseTransmissionSuceess(uint32_t addr) {
    CI_Instruction_Transport ciInstruction;
    ciInstruction.type = CI_INSTRUCTION_SEND_TRANMISSION_SUCCESS;
    ciInstruction.data[0] = addr & 0xff;
    ciInstruction.data[1] = (addr >> 8) & 0xff;
    ciInstruction.data[2] = (addr >> 16) & 0xff;
    ciInstruction.data[3] = (addr >> 24) & 0xff;
    
    this->pushCIInstructionCB(&ciInstruction, pushCIInstructionCBArgs);
}

void GobotRPC_TI_Hardware_RP2040_I2C::raiseInfoReset() {
    CI_Instruction_Transport ciInstruction;
    ciInstruction.type = CI_INSTRUCTION_SEND_INFO_RESET;
    
    this->pushCIInstructionCB(&ciInstruction, pushCIInstructionCBArgs);
}

void GobotRPC_TI_Hardware_RP2040_I2C::revCIInstructionTask() {
    CI_Instruction_Transport ciInstruction;
    while(1) {
        xQueueReceive(revCIInstructionQueue, &ciInstruction, portMAX_DELAY);
        
        switch (ciInstruction.type) {
            case CI_INSTRUCTION_REV_SEND_SET_ADDR_MAP: {
                uint32_t addr = ciInstruction.data[0] |\
                    (ciInstruction.data[1] << 8) |\ 
                    (ciInstruction.data[2] << 16) |\
                    (ciInstruction.data[3] << 24);
                uint8_t port = ciInstruction.data[4];
                this->setAddrMap(addr, port);
                break;
            }
        }
    }
}