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

#include "protocol.hpp"

#include "pico/stdlib.h"
#include "hardware/watchdog.h"

#include "FreeRTOSConfig.h"
#include "FreeRTOS.h"
#include "task.h"


GobotRPC_CI::GobotRPC_CI(I_GobotRPC_CI_Hardware *hardware, UBaseType_t core, QueueHandle_t ciInstructionQueue, QueueHandle_t ciInstructionReverseQueue) {
    this->ciInstructionQueue = ciInstructionQueue;
    this->ciInstructionReverseQueue = ciInstructionReverseQueue;
    
    this->hardware = hardware;
    this->hardware->registerCB_RxData(GobotRPC_CI_rxData_cb, this);

    this->cb_TxPacket = NULL;
    this->cb_TxPacket_args = NULL;
    this->cb_SetAddressMap = NULL;
    this->cb_SetAddressMap_args = NULL;

    xTaskCreateAffinitySet(GobotRPC_CI_heartBeatTaskFn, "Heartbeat Task", 2048, this, 2, core, &this->heartBeatTaskHandle);
    xTaskCreateAffinitySet(GobotRPC_CI_txCIInstructionTaskFn, "Tx CI Instruction Task", 2048, this, 3, core, &this->txCIInstructionTaskHandle);
}

// Rx Side
void GobotRPC_CI_rxData_cb(void * args, char *data, size_t len) {
    GobotRPC_CI *gobotRPC = (GobotRPC_CI *)args;
    gobotRPC->onRxData(data, len);
}

void GobotRPC_CI::onRxData(char *data, size_t len) {
    GobotRPC_CI_CMD cmd = (GobotRPC_CI_CMD)data[0];

    switch (cmd) {
        case TX_CI_PACKET:
            if(this->cb_TxPacket != NULL) {
                uint32_t addr = (data[2] << 24) | (data[3] << 16) | (data[4] << 8) | data[5];
                this->cb_TxPacket(this->cb_TxPacket_args, data+6, len-6, addr);
            }
            break;

        case RESET_CI_PACKET:
            //softwareReset();
            break;

        case SET_ADDR_PORT_MAP: {
            if(this->cb_SetAddressMap != NULL) {
                uint32_t addr = (data[2] << 24) | (data[3] << 16) | (data[4] << 8) | data[5];
                uint8_t port = data[6];
                this->cb_SetAddressMap(cb_SetAddressMap_args, addr, port);
            }
            break;
        }

        default:
            break;
    }
}

void GobotRPC_CI::registerCB_TxPacket(callback_TxPacket cb, void *args) {
    this->cb_TxPacket = cb;
    this->cb_TxPacket_args = args;
}

void GobotRPC_CI::registerCB_SetAddress(callback_SetAddress cb, void *args) {
    this->cb_SetAddressMap = cb;
    this->cb_SetAddressMap_args = args;
}

void GobotRPC_CI::send_RxPacket(char *data, size_t len, uint32_t addr) {
    data[0] = RX_CI_PACKET;
    data[1] = len + CI_RX_PACKAGE_DATA_OFFSET;

    data[2] = (addr >> 24) & 0xff;
    data[3] = (addr >> 16) & 0xff;
    data[4] = (addr >> 8) & 0xff;
    data[5] = addr & 0xff;

    this->hardware->send(data, len+6);
}

// Heartbeat Task

void GobotRPC_CI_heartBeatTaskFn(void *args) {
    GobotRPC_CI *gobotRPC = (GobotRPC_CI *)args;
    gobotRPC->heartBeartTaskFn();
}

void GobotRPC_CI::heartBeartTaskFn() {
    char heartBeatPacket[] = {HEARTBEAT, 0x02};
    while(1) {
        this->hardware->send(heartBeatPacket, 2);
        vTaskDelay(GOBOTRPC_HEARTBEAT_INTERVAL / portTICK_PERIOD_MS);
    }
}