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

GobotRPCStateMashine::GobotRPCStateMashine() {
    memset(requestDataBuffer, 0, REQUEST_DATA_BUFFER_SIZE*sizeof(uint8_t)*SLOTS);
    memset(responseDataBuffer, 0, RESPONSE_DATA_BUFFER_SIZE*sizeof(uint8_t)*SLOTS);
    memset(errorDataBuffer, 0, RESPONSE_DATA_BUFFER_SIZE*sizeof(uint8_t)*SLOTS);

    memset(requestSegmentArrivedFlags, 0, sizeof(uint8_t)*SLOTS);
    memset(responseSegmentArrivedFlags, 0, sizeof(uint8_t)*SLOTS);
    memset(errorSegmentArrivedFlags, 0, sizeof(uint8_t)*SLOTS);

    slotCounter = 0;
    rpc_number_map = GobotRPCNumberMap();
}

void GobotRPCStateMashine::registerRPC(RpcNum rpc_num) {
    rpc_number_map.set(rpc_num, slotCounter);
    slotCounter = (slotCounter + 1) % SLOTS;
}

void GobotRPCStateMashine::submitFrame(RPCFrame * frame, SMResult * result) {
    RpcNum rpcNum = (RpcNum)(frame->header.rpc_num);

    result->done = false;
    result->rpc_num = rpcNum;

    if(rpcNum == RpcNum::INVALID)
        return;

    size_t slot = rpc_number_map.get(rpcNum);
    if (slot == 0xffffffff)
        return;

    uint8_t segNum = frame->header.rpc_segement;

    if(frame->header.error) {
        memcpy(result->data, frame->data, 7);
        errorSegmentArrivedFlags[slot] |= (1 << segNum);

        result->done = true;
        result->type = RPCPackageType::ERROR;

    } else if(frame->header.response) {
        memcpy(responseDataBuffer[slot] + segNum, frame->data, 7);
        responseSegmentArrivedFlags[slot] |= (1 << segNum);
        
        uint8_t mask = SEGMENT_MASK_RESPONSE[(int)rpcNum];

        if(((responseSegmentArrivedFlags[slot] & mask) ^ mask) == 0) {
            responseSegmentArrivedFlags[slot] = 0;

            result->done = true;
            memcpy(result->data, responseDataBuffer[slot], 7*4);
            result->type = RPCPackageType::RESPONSE;
        }

    } else {
        memcpy(requestDataBuffer[slot] + segNum, frame->data, 7);
        requestSegmentArrivedFlags[slot] |= (1 << segNum);
        
        uint8_t mask = SEGMENT_MASK_RESPONSE[(int)rpcNum];
        
        if(((requestSegmentArrivedFlags[slot] & mask) ^ mask) == 0) {
            requestSegmentArrivedFlags[slot] = 0;

            result->done = true;
            memcpy(result->data, requestDataBuffer[slot], 7*4);
            result->type = RPCPackageType::REQUEST;
        }
    }

}