#include <Container.h>
#include <DataDef.h>
#include <Drivers/FPGA/Motors_Driver/L6470.h>
#include <PMR/Stubs/StubMotorInitRequest.pb-c.h>
#include <PMR/Stubs/StubMotorInitResponse.pb-c.h>
#include <PMR/Stubs/StubMotorRunRequest.pb-c.h>
#include <PMR/Stubs/StubMotorRunResponse.pb-c.h>
#include <PMR/Stubs/StubMotorStopRequest.pb-c.h>
#include <PMR/Stubs/StubMotorStopResponse.pb-c.h>
#include <PMR/Stubs/StubMotorStatusRequest.pb-c.h>
#include <PMR/Stubs/StubMotorStatusResponse.pb-c.h>
#include <PMR/Stubs/StubMotorSpeedResponse.pb-c.h>
#include <PMR/Stubs/StubMotorSpeedRequest.pb-c.h>
#include <PMR/Stubs/StubMotorPositionRequest.pb-c.h>
#include <PMR/Stubs/StubMotorPositionResponse.pb-c.h>
#include <PMR/Stubs/StubMotorMovRequest.pb-c.h>
#include <PMR/Stubs/StubMotorMovResponse.pb-c.h>
#include <PMR/Stubs/StubMotorRequest.pb-c.h>
#include <PMR/Stubs/StubMotorResponse.pb-c.h>
#include "PMR/Hardware/HardwareMotor.pb-c.h"
#include "PMR/Hardware/HardwareMotorType.pb-c.h"

#include <stdbool.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_uart.h"

#include "Drivers/USB_Communication/USBCDCD.h"

#include "MessageContainer.pb-c.h"

#include "Stub_Status.h"
#include "drivers/FPGA/FPGA.h"
#include "drivers/SPI/SPI_Comm.h"

#include "driverlib/ssi.h"
#include "drivers/SPI/SPI_Comm.h"
#include "Modules/Thread/Thread_ex.h"
#include "Modules/Ids/Ids_ex.h"


#include "Modules/thread/thread.h"
#include "Drivers/Motors/Motor.h"

#include "drivers/FPGA/FPGA_SPI_Comm.h"


extern unsigned long Run_Value ;
extern unsigned long Pos_Value ;
extern bool Direction ;
STOP_TYPE_ENUM Stop_Command;

int Global_EVB_Motor_Id;


void Stub_MotorInitRequest(MessageContainer* requestContainer)
{
    //uint32_t status = FAILED;

    MessageContainer responseContainer;

    StubMotorInitRequest* request = stub_motor_init_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
#ifdef EVALUATION_BOARD
    //SPI_Control(INIT);

    SPI2_Init();
    init_BUSY_Pin();
    //while(SSIBusy(SSI2_BASE)){};
    //init_BUSY_Pin();

#else

    MotorsInit();
    SysCtlDelay(20000);
    //MotorConfig(request->motor_id, &MotorConfig);

#endif

//  MotorsConfigMessage already calls setup!!!!!!!!!
     MotorDriverConfigStruc MotorDriverConfig;
    MotorDriverConfig.maxchangeslope = request->acc;
    MotorDriverConfig.maxchangeslope = request->dec;
    MotorDriverConfig.maxfrequency = request->max_speed;

    MotorDriverConfig.microstep = request->micro_steps;
//    else
//        MotorDriverConfig.Microstep = 2;
    //MotorDriverConfig.HasConfigWord = false;


    MotorDriverConfig.configword =  request->config;
    MotorDriverConfig.kvalhold =  request->kval_hold;
    MotorDriverConfig.kvalrun =   request->kval_run;
    MotorDriverConfig.kvalacc =    request->kval_acc;
    MotorDriverConfig.kvaldec =    request->kval_dec;
    MotorDriverConfig.stslp =      request->st_slp;
    MotorDriverConfig.intspd =     request->int_spd;
    MotorDriverConfig.fnslpacc =  request->fn_slp_acc;
    MotorDriverConfig.fnslpdec =  request->fn_slp_dec;


#ifdef EVALUATION_BOARD
    Global_EVB_Motor_Id = request->motor_id;
    setup(&MotorDriverConfig);
#else
    TimerMotors_t MotorId =(TimerMotors_t) request->motor_id;

    MotorConfig(MotorId, &MotorDriverConfig); //------------------------------ to open !!!!!!!!!!!!!!!!!!!!!!1
#endif

    //setup();

    //init_BUSY_Pin();
    //while(SSIBusy(SSI2_BASE)){};
    //ResetPos();
    //while(SSIBusy(SSI2_BASE)){};

    //Run_Value = request->run_value;
    //Mov_Value = request->mov_value;
    //Direction = request->direction;


    //writeLine("L6470 Request: ");

    //writeFloat(request->run_value);

    //status = FPGA_ReadVersion((unsigned char)request->fpgaid, &Version, &Year, &Month, &Day);
    //SPI_Control();

    //temp_init_spi2();



/*
    HardwareMotor MotorConfig;

    HardwareMotorType Motor_Id  = (HardwareMotorType)request->motor_id;
    MotorConfig.hardwaremotortype = Motor_Id;
    MotorConfig.minfrequency = 0;
    MotorConfig.maxfrequency = request->max_speed;
    MotorConfig.microstep = request->micro_steps;
    MotorsConfigMessage(&MotorConfig);
*/




    StubMotorInitResponse response =   STUB_MOTOR_INIT_RESPONSE__INIT;

   // status_response(status,&response.status, &response.statusword ,&response.has_statusword);


    response.motor_id = request->motor_id;
    response.has_motor_id = true;
    response.status = 0;

    responseContainer = createContainer(MESSAGE_TYPE__StubMotorInitResponse, requestContainer->token, true, &response, &stub_motor_init_response__pack, &stub_motor_init_response__get_packed_size);


    //free(request);
    //-------------------------------------------------------------------------------------------
    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    free(responseContainer.data.data);
    SendChars((char*)container_buffer, container_size);
    //free(container_buffer);
    //free(requestContainer);
    stub_motor_init_request__free_unpacked(request,NULL);
}


void Stub_MotorRunRequest(MessageContainer* requestContainer)
{
    //uint32_t status = FAILED;

#ifdef EVALUATION_BOARD
    uint32_t read_status;
#endif

    MessageContainer responseContainer;

    StubMotorRunRequest* request = stub_motor_run_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

   float  temp =  (float)request->speed;
    Run_Value = /*SpdCalc*/(temp);
    Direction = request->direction;



#ifdef EVALUATION_BOARD
    //SPI_Control(SKIP_INIT);
    Mot_Run();

    //writeLine("L6470 Request: ");

    //writeFloat(request->run_value);

    //status = FPGA_ReadVersion((unsigned char)request->fpgaid, &Version, &Year, &Month, &Day);
#else
    TimerMotors_t Motor_Id  = (TimerMotors_t)request->motor_id; //to remove warning

    //FPGA_GetBusy();

    if (MotorDriverResponse[Motor_Id].DriverType == CombinrdMotDriver)
    {
        Direction = Direction^1;
    }

    MotorSetDirection(Motor_Id,Direction);
    MotorSetSpeed(Motor_Id, Run_Value);
    if ((Motor_Id >= HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1)&&(Motor_Id <= HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8))
    {
        CurrentDispenserSpeed[Motor_Id-HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1] = Run_Value;
        if(request->direction == 0)
            CurrentDispenserSpeed[Motor_Id-HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1] *= (-1);

    }

#endif


    StubMotorRunResponse response =   STUB_MOTOR_RUN_RESPONSE__INIT;

   // status_response(status,&response.status, &response.statusword ,&response.has_statusword);


    response.motor_id = request->motor_id;
    response.has_motor_id = true;
#ifdef EVALUATION_BOARD
    read_status = Get_and_Clear_Status();

    response.speed = (double)(CurrentSpdCalc(Get_Param(x_SPEED)));
    response.has_speed = true;

    response.status_reg = read_status;
    response.has_status_reg = true;

    response.direction = ((read_status & x_STATUS_DIR)>>4);
    response.has_direction = true;

    response.step_loss_a = ((read_status & x_STATUS_STEP_LOSS_A)>>13);
    response.has_step_loss_a = true;
    response.step_loss_b = ((read_status & x_STATUS_STEP_LOSS_B)>>14);
    response.has_step_loss_b = true;


    response.mot_status = ((read_status & x_STATUS_MOT_STATUS)>>5);
    response.has_mot_status = true;

    response.busy = ((read_status & x_STATUS_BUSY)>>1);
    response.has_busy = true;
#endif

    responseContainer = createContainer(MESSAGE_TYPE__StubMotorRunResponse, requestContainer->token, true, &response, &stub_motor_run_response__pack, &stub_motor_run_response__get_packed_size);

    //free(request);
    //-------------------------------------------------------------------------------------------
    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    free(responseContainer.data.data);
    SendChars((char*)container_buffer, container_size);
    //free(container_buffer);
    //free(requestContainer);
    stub_motor_run_request__free_unpacked(request,NULL);
}


void Stub_MotorStopRequest(MessageContainer* requestContainer)
{
    //uint32_t status = FAILED;

    MessageContainer responseContainer;

    StubMotorStopRequest* request = stub_motor_stop_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    
    Stop_Command = (STOP_TYPE_ENUM)request->stop_cmd;

    TimerMotors_t Motor_Id  = (TimerMotors_t)request->motor_id; //to remove warning
#ifdef EVALUATION_BOARD
  uint32_t read_status;

 //SPI_Control(SKIP_INIT);
    Mot_Stop();
#else
    MotorStop(Motor_Id, Stop_Command);
#endif
    if ((Motor_Id >= HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1)&&(Motor_Id <= HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_8))
    {
        CurrentDispenserSpeed[Motor_Id-HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1] = 0;
    }

    //status = FPGA_ReadVersion((unsigned char)request->fpgaid, &Version, &Year, &Month, &Day);



    StubMotorStopResponse response =   STUB_MOTOR_STOP_RESPONSE__INIT;

   // status_response(status,&response.status, &response.statusword ,&response.has_statusword);


    response.motor_id = request->motor_id;
    response.has_motor_id = true;
#ifdef EVALUATION_BOARD
    read_status = Get_and_Clear_Status();

    response.mot_status = ((read_status & x_STATUS_MOT_STATUS)>>5);
    response.has_mot_status = true;

    response.hiz = (read_status & x_STATUS_HIZ);
    response.has_hiz = true;

    response.busy = ((read_status & x_STATUS_BUSY)>>1);
    response.has_busy = true;
#endif
    responseContainer = createContainer(MESSAGE_TYPE__StubMotorStopResponse, requestContainer->token, true, &response, &stub_motor_stop_response__pack, &stub_motor_stop_response__get_packed_size);


    //free(request);
    //-------------------------------------------------------------------------------------------
    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    free(responseContainer.data.data);
    SendChars((char*)container_buffer, container_size);
    //free(container_buffer);
    //free(requestContainer);
    stub_motor_stop_request__free_unpacked(request,NULL);
}



void Stub_MotorStatusRequest(MessageContainer* requestContainer)
{
    uint32_t read_status;

    MessageContainer responseContainer;

    StubMotorStatusRequest* request = stub_motor_status_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    
//    if(request->clearstatus)
//  {
//
//        #ifdef EVALUATION_BOARD
//            read_status = Get_and_Clear_Status();
//        #else
//            MotorGetStatusFromFPGA();
//            //deley
//            SysCtlDelay(10000);
//            read_status =  MotorGetStatusFromFPGA_Res();
//
//        #endif
//  }
//  else
//  {
//      read_status = Get_Param(x_STATUS);
//  }

    if(request->clearstatus)
           {

                #ifdef EVALUATION_BOARD
                    read_status = Get_and_Clear_Status();
                #else
                    TimerMotors_t Motor_Id  = (TimerMotors_t)request->motor_id; //to remove warning
                    MotorGetStatusFromFPGA(Motor_Id);
                    //deley TODO
                    SysCtlDelay(10000);
                    read_status =  Fpga_Spi[(TimerMotors_t)request->motor_id].RX_MISO;//>>8;//MotorGetStatusFromFPGA_Res(Motor_Id); TODO >>8 ???

                #endif
           }
           else
           {
               #ifdef EVALUATION_BOARD
                   read_status = Get_Param(x_STATUS);// Function only for evaluation board !!!!!
                   //if needed use Motor_Get_Param_Horizontal_Printing_powerstep01_rev10.cs
               #endif
           }



    StubMotorStatusResponse response =   STUB_MOTOR_STATUS_RESPONSE__INIT;

    response.motor_id = request->motor_id;
    response.has_motor_id = true;

    response.sck_mod = ((read_status & x_STATUS_SCK_MOD)>>15);
    response.has_sck_mod = true;

    response.step_loss_b = ((read_status & x_STATUS_STEP_LOSS_B)>>14);
    response.has_step_loss_b = true;

    response.step_loss_a = ((read_status & x_STATUS_STEP_LOSS_A)>>13);
    response.has_step_loss_a = true;

    response.ocd = ((read_status & x_STATUS_OCD)>>12);
    response.has_ocd = true;

    response.th_sd = ((read_status & x_STATUS_TH_SD)>>11);
    response.has_th_sd = true;

    response.th_wrn = ((read_status & x_STATUS_TH_WRN)>>10);
    response.has_th_wrn = true;

    response.uvlo = ((read_status & x_STATUS_UVLO)>>9);
    response.has_uvlo = true;

    response.wrong_cmd = ((read_status & x_STATUS_WRONG_CMD)>>8);
    response.has_wrong_cmd = true;

    response.notperf_cmd = ((read_status & x_STATUS_NOTPERF_CMD)>>7);
    response.has_notperf_cmd = true;

    response.mot_status = ((read_status & x_STATUS_MOT_STATUS)>>5);
    response.has_mot_status = true;

    response.dir = ((read_status & x_STATUS_DIR)>>4);
    response.has_dir = true;

    response.sw_evn = ((read_status & x_STATUS_SW_EVN)>>3);
    response.has_sw_evn = true;

    response.sw_f = ((read_status & x_STATUS_SW_F)>>2);
    response.has_sw_f = true;

    response.busy = ((read_status & x_STATUS_BUSY)>>1);
    response.has_busy = true;

    response.hiz = (read_status & x_STATUS_HIZ);
    response.has_hiz = true;

    responseContainer = createContainer(MESSAGE_TYPE__StubMotorStatusResponse, requestContainer->token, true, &response, &stub_motor_status_response__pack, &stub_motor_status_response__get_packed_size);


    //free(request);
    //-------------------------------------------------------------------------------------------
    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    free(responseContainer.data.data);
    SendChars((char*)container_buffer, container_size);
    //free(container_buffer);
    //free(requestContainer);
    stub_motor_status_request__free_unpacked(request,NULL);
}


void Stub_MotorSpeedRequest(MessageContainer* requestContainer)
{

    MessageContainer responseContainer;

    StubMotorSpeedRequest* request = stub_motor_speed_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    StubMotorSpeedResponse response =   STUB_MOTOR_SPEED_RESPONSE__INIT;

    response.motor_id = request->motor_id;
    response.has_motor_id = true;

    ///////////////////////////////////////////////////////////////////
    #ifdef EVALUATION_BOARD
    //response.speed = Get_Param(x_SPEED);
    response.speed = (double)(CurrentSpdCalc(Get_Param(x_SPEED)));
    #else

        TimerMotors_t Motor_Id = (TimerMotors_t)request->motor_id;//to remove warning
        MotorGetSpeedFromFPGA(Motor_Id);
        //deley TODO
        SysCtlDelay(10000);
        //response.speed = (double)(CurrentSpdCalc(MotorGetSpeedFromFPGA_Res(Motor_Id)));
        response.speed = (double)(MotorGetSpeedFromFPGA_Res(Motor_Id));

    #endif
    //////////////////////////////////////////////////////////////////

    //response.speed = Get_Param(x_SPEED);
    //response.speed = (double)(CurrentSpdCalc(Get_Param(x_SPEED)));
    response.has_speed = true;

    responseContainer = createContainer(MESSAGE_TYPE__StubMotorSpeedResponse, requestContainer->token, true, &response, &stub_motor_speed_response__pack, &stub_motor_speed_response__get_packed_size);

    //free(request);
    //-------------------------------------------------------------------------------------------
    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    free(responseContainer.data.data);
    SendChars((char*)container_buffer, container_size);
    //free(container_buffer);
    //free(requestContainer);
    stub_motor_speed_request__free_unpacked(request,NULL);
}


void Stub_MotorPositionRequest(MessageContainer* requestContainer)
{

    MessageContainer responseContainer;

    StubMotorPositionRequest* request = stub_motor_position_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    StubMotorPositionResponse response =   STUB_MOTOR_POSITION_RESPONSE__INIT;

    response.motor_id = request->motor_id;
    response.has_motor_id = true;
    #ifdef EVALUATION_BOARD
        response.position = Get_Param(x_ABS_POS);

    #else
        TimerMotors_t Motor_Id  = (TimerMotors_t)request->motor_id; //to remove warning
        MotorGetPositionFromFPGA(Motor_Id);
        //deley TODO
        SysCtlDelay(10000);
        response.position = Fpga_Spi[(TimerMotors_t)request->motor_id].RX_MISO;//MotorGetPositionFromFPGA_Res(Motor_Id);

    #endif
    response.has_position = true;

    responseContainer = createContainer(MESSAGE_TYPE__StubMotorPositionResponse, requestContainer->token, true, &response, &stub_motor_position_response__pack, &stub_motor_position_response__get_packed_size);

    //free(request);
    //-------------------------------------------------------------------------------------------
    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    free(responseContainer.data.data);
    SendChars((char*)container_buffer, container_size);
    //free(container_buffer);
    //free(requestContainer);
    stub_motor_position_request__free_unpacked(request,NULL);
}


void Stub_MotorMovRequest(MessageContainer* requestContainer)
{

    bool GoTo_Flag = false;

#ifdef EVALUATION_BOARD
    uint32_t read_status;
#endif
    MessageContainer responseContainer;

    StubMotorMovRequest* request = stub_motor_mov_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    if(request->position & 0x800000)
    {
        request->position =  request->position & ~0x800000;
        GoTo_Flag = true;
    }

    Pos_Value = request->position;
    Direction = request->direction;



    MotorGetnBusyFromFPGA(); //temporary -

    #ifdef EVALUATION_BOARD
        Mot_Mov();
    #else
        TimerMotors_t Motor_Id  = (TimerMotors_t)request->motor_id; //to remove warning

    if (MotorDriverResponse[Motor_Id].DriverType == CombinrdMotDriver)
    {
       Direction = Direction^1;
    }

    if(GoTo_Flag)
    {
       MotorGoTo(Motor_Id, Pos_Value );
    }
    else
    {
        MotorMove(Motor_Id, Direction, Pos_Value );
    }
    #endif

    StubMotorMovResponse response =   STUB_MOTOR_MOV_RESPONSE__INIT;

    response.motor_id = request->motor_id;
    response.has_motor_id = true;

    #ifdef EVALUATION_BOARD
        response.position = Get_Param(x_ABS_POS);
        response.has_position = true;

        while(SSIBusy(SSI2_BASE)){};

        read_status = Get_and_Clear_Status();


        response.status_reg = read_status;
        response.has_status_reg = true;

        response.direction = ((read_status & x_STATUS_DIR)>>4);
        response.has_direction = true;

        response.step_loss_a = ((read_status & x_STATUS_STEP_LOSS_A)>>13);
        response.has_step_loss_a = true;
        response.step_loss_b = ((read_status & x_STATUS_STEP_LOSS_B)>>14);
        response.has_step_loss_b = true;


        response.mot_status = ((read_status & x_STATUS_MOT_STATUS)>>5);
        response.has_mot_status = true;
    #endif

    responseContainer = createContainer(MESSAGE_TYPE__StubMotorMovResponse, requestContainer->token, true, &response, &stub_motor_mov_response__pack, &stub_motor_mov_response__get_packed_size);

    //free(request);
    //-------------------------------------------------------------------------------------------
    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    free(responseContainer.data.data);
    SendChars((char*)container_buffer, container_size);
    //free(container_buffer);
    //free(requestContainer);
    stub_motor_mov_request__free_unpacked(request,NULL);
}

void Stub_MotorRequest(MessageContainer* requestContainer)
{

    MessageContainer responseContainer;

    StubMotorRequest* request = stub_motor_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    StubMotorResponse response =   STUB_MOTOR_RESPONSE__INIT;

#ifdef EVALUATION_BOARD

    //uint32_t read_status;
    uint32_t temp = 0;
    uint32_t rx = 0;

    unsigned char N_Get = 0;


    N_Get =  (unsigned char)request->n_bytes_to_get;

    switch(request->n_bytes_to_set)
    {
        case 4:
            temp = Write_Byte((byte)((request->data_to_send >> 24)&0xFF));
            rx |= (temp & 0xFF) ;
            N_Get = N_Get-1;
        case 3:
            rx = rx << 8;
            temp = Write_Byte((byte)((request->data_to_send >> 16)&0xFF));
            rx |= (temp & 0xFF);
            N_Get = N_Get-1;
        case 2:
            rx = rx << 8;
            temp = Write_Byte((byte)((request->data_to_send >> 8 )&0xFF));
            rx |= (temp & 0xFF);
            N_Get = N_Get-1;
        case 1:
        default:
            rx = rx << 8;
            temp = Write_Byte((byte)((request->data_to_send      )&0xFF));
            rx |= temp & 0xFF;
            N_Get = N_Get-1;
    }

    while (N_Get)
    {
        rx = rx << 8;
        temp = Write_Byte((uint8_t)(0x00));
        rx |= temp & 0xFF;
        N_Get = N_Get-1;
    }

    response.has_recived_data = true;
    response.recived_data = rx;
#else
    TimerMotors_t Motor_Id = (TimerMotors_t)request->motor_id;
    MotorSendFPGARequest(Motor_Id,request->data_to_send, request->n_bytes_to_set);
    SysCtlDelay(2000);
    MotorGetFPGAResponse(Motor_Id,&response.recived_data);
    response.has_recived_data = true;

#endif
    response.has_motor_id = true;
    response.motor_id = request->motor_id;

    responseContainer = createContainer(MESSAGE_TYPE__StubMotorResponse, requestContainer->token, true, &response, &stub_motor_response__pack, &stub_motor_response__get_packed_size);

    //free(request);
    //-------------------------------------------------------------------------------------------
    uint8_t* container_buffer = malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    free(responseContainer.data.data);
    SendChars((char*)container_buffer, container_size);
    //free(container_buffer);
    //free(requestContainer);
    stub_motor_request__free_unpacked(request,NULL);
}