#include <string.h>
#include "gobot_rpc.hpp"
#include "sm.hpp"
#include "protocol_spec.hpp"

#include "freertos/FreeRTOS.h"

GobotRPCReciver::GobotRPCReciver(MCP2521 * can_interface) {
    this->can_interface = can_interface;
    this->state_mashine = GobotRPCStateMashine();

    can_interface->register_rx0_handler(onRX0_GoboRPC, can_interface);
}


void onRX0_GoboRPC(void *arg) {
    GobotRPCReciver *rpc_socket = (GobotRPCReciver *)arg;
    rpc_socket->onRX();
}

void GobotRPCReciver::onRX() {
    rx_info info = can_interface->get_rx_id(MCP2521_RX_BUFFER::RXB0);
    uint8_t data[8];

    can_interface->read_rx_buf(MCP2521_RX_BUFFER::RXB0, MCP2521_BUFFER_TYPE::DATA, data, info.length);

    RPCFrame frame;
    memcpy(((char *)data) + 1, (char *)frame.data, 7);
    frame.header = (RPCHeader)(data[0]);

    SMResult smResult;
    state_mashine.submitFrame(&frame, &smResult);

    if(smResult.done) {
        
    }
}

void GobotRPCReciver::registerOnDoneFrameHandler(GobotRPCHandler_t handler, void * arg) {
    onDoneFrameHandler = handler;
    argOnDoneFrameHandler = arg;
}