#include "ci/base.hpp"

#include "ci/instructions.hpp"

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

void GobotRPC_CI_txCIInstructionTaskFn(void *args) {
    GobotRPC_CI *gobotRPC = (GobotRPC_CI *)args;
    gobotRPC->txCIInstructionTask();
}

void GobotRPC_CI::txCIInstructionTask() {
    while(1) {
        CI_Instruction_Transport ciInstruction;
        xQueueReceive(ciInstructionQueue, &ciInstruction, portMAX_DELAY);

        switch (ciInstruction.type) {
            case CI_INSTRUCTION_SEND_TRANMISSION_ERROR: {
                uint32_t addr = ciInstruction.data[1] | (ciInstruction.data[2] << 8) | (ciInstruction.data[3] << 16) | (ciInstruction.data[4] << 24);
                uint8_t rx = ciInstruction.data[0];
                send_ErrorTransmission(rx, addr);
                break;
            }

            case CI_INSTRUCTION_SEND_TRANMISSION_SUCCESS: {
                uint32_t addr = ciInstruction.data[0] \
                    | (ciInstruction.data[1] << 8) \
                    | (ciInstruction.data[2] << 16) \
                    | (ciInstruction.data[3] << 24);
                send_SuccessTransmission(addr);
                break;
            }

            case CI_INSTRUCTION_SEND_INFO_RESET:
                send_InfoReset();
                break;
        }
    }
}

void GobotRPC_CI::send_ErrorTransmission(bool rx, uint64_t addr) {
    char errorPacket[7];

    errorPacket[0] = ERROR_TRANMISSION;
    errorPacket[1] = 7;
    errorPacket[2] = rx ? 0x01 : 0x00;
    errorPacket[3] = (addr >> 24) & 0xff;
    errorPacket[4] = (addr >> 16) & 0xff;
    errorPacket[5] = (addr >> 8) & 0xff;
    errorPacket[6] = addr & 0xff;

    this->hardware->send(errorPacket, 7);
}

void GobotRPC_CI::send_SuccessTransmission(uint64_t addr) {
    char successPacket[6];

    successPacket[0] = SUCESS_TRANMISSION;
    successPacket[1] = 6;
    successPacket[2] = (addr >> 24) & 0xff;
    successPacket[3] = (addr >> 16) & 0xff;
    successPacket[4] = (addr >> 8) & 0xff;
    successPacket[5] = addr & 0xff;

    this->hardware->send(successPacket, 6);
}

void GobotRPC_CI::send_InfoReset() {
    char resetPacket[2];

    resetPacket[0] = RESET_INFO_CI_PACKET;
    resetPacket[1] = 2;

    this->hardware->send(resetPacket, 2);
}