#include "udp2lcm.h"

void robotCtrlInit(robot_control_t *robotCtrlData, int64_t utime,
                   int8_t commandid, int8_t robotid, int8_t ndparams,
                   int8_t niparams, int8_t nsparams, int64_t nbparams) {
    if (robotCtrlData == NULL) {
        fprintf(stderr, "Error: Invalid robot control data\n");
        return;
    }
    robotCtrlData->utime = utime;
    robotCtrlData->commandid = commandid;
    robotCtrlData->robotid = robotid;
    robotCtrlData->ndparams = ndparams;
    robotCtrlData->dparams = NULL;
    robotCtrlData->niparams = niparams;
    robotCtrlData->iparams = NULL;
    robotCtrlData->nsparams = nsparams;
    robotCtrlData->sparams = NULL;
    robotCtrlData->nbparams = nbparams;
    robotCtrlData->bparams = NULL;
    if (ndparams > 0) {
        robotCtrlData->dparams = (double *)malloc(ndparams * sizeof(double));
        memset(robotCtrlData->dparams, 0, ndparams * sizeof(double));
    }
    if (niparams > 0) {
        robotCtrlData->iparams = (int8_t *)malloc(niparams * sizeof(int8_t));
        memset(robotCtrlData->iparams, 0, niparams * sizeof(int8_t));
    }
    if (nsparams > 0) {
        robotCtrlData->sparams = (char **)malloc(nsparams * sizeof(char *));
        memset(robotCtrlData->sparams, 0, nsparams * sizeof(char *));
    }
    if (nbparams > 0) {
        robotCtrlData->bparams = (uint8_t *)malloc(nbparams * sizeof(uint8_t));
        memset(robotCtrlData->bparams, 0, nbparams * sizeof(uint8_t));
    }
}

void freeRobotCtrl(robot_control_t *robotCtrlData) {
    if (robotCtrlData == NULL) {
        fprintf(stderr, "Error: Invalid robot control data\n");
        return;
    }
    if (robotCtrlData->dparams != NULL) {
        free(robotCtrlData->dparams);
        robotCtrlData->dparams = NULL;
    }
    if (robotCtrlData->iparams != NULL) {
        free(robotCtrlData->iparams);
        robotCtrlData->iparams = NULL;
    }
    if (robotCtrlData->sparams != NULL) {
        free(robotCtrlData->sparams);
        robotCtrlData->sparams = NULL;
    }
    if (robotCtrlData->bparams != NULL) {
        free(robotCtrlData->bparams);
        robotCtrlData->bparams = NULL;
    }
}