/*
 * DiagnosticsHoming.c

 *
 *  Created on: July 29 2018
 *      Author: shlomo
 */
#include <DataDef.h>
#include "include.h"

#include <inc/hw_ints.h>
#include <Container.h>

#include "diagnostics.h"
#include "drivers/Motors/Motor.h"

#include  <PMR/Diagnostics/DispenserHomingRequest.pb-c.h>
#include  <PMR/Diagnostics/DispenserHomingResponse.pb-c.h>
#include  <PMR/Diagnostics/DispenserAbortHomingRequest.pb-c.h>
#include  <PMR/Diagnostics/DispenserAbortHomingResponse.pb-c.h>

#include  <PMR/Diagnostics/MotorHomingRequest.pb-c.h>
#include  <PMR/Diagnostics/MotorHomingResponse.pb-c.h>
#include  <PMR/Diagnostics/MotorAbortHomingRequest.pb-c.h>
#include  <PMR/Diagnostics/MotorAbortHomingResponse.pb-c.h>

#include "Modules/Control/control.h"

#include "drivers/Motors/Motor.h"
#include "drivers/Valves/Valve.h"
#include "Drivers/I2C_Communication/DAC/blower.h"
#include "drivers/Flash_ram/MCU_E2Prom.h"
#include <Drivers/SSI_Comm/Dancer/Dancer.h>

#include "ids/ids_ex.h"
#include "General/process.h"
#include "Thread/Thread_ex.h"

#include "StateMachines/Printing/PrintingSTM.h"

char *HomingToken[NUM_OF_MOTORS]={0};
int HomingCounter[NUM_OF_MOTORS];
uint32_t HomingControlId[NUM_OF_MOTORS];

uint32_t Diagnostics_Dryer_UnLoading(void);
uint32_t Diagnostics_Dryer_Loading(void);
int kval_upper_value;

void HomingStopReporting(void)
{
    int i;
    for (i = 0; i< NUM_OF_MOTORS;i++)
    {
        HomingToken[i] = NULL;
    }
    LOG_ERROR(0,"HomingStopReporting");
}

/********************************************************************************
 * Motor Homing
 ********************************************************************************/
int32_t keepkvalright,keepkvalleft;
uint32_t MotorHomingProgressReport(uint32_t deviceID, uint32_t ReadValue)
{
    if (HomingToken[deviceID] == NULL)
        return OK;

    MessageContainer responseContainer;
    MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT;
    bool last = false;
    response.has_progress = true;
    response.has_maxprogress = true;
    response.maxprogress = 1000;
    if (HomingCounter[deviceID]>=240)
    {
        last = true;
        SafeRemoveControlCallback(HomingControlId[deviceID],MotorHomingProgressReport);
        ReportWithPackageFilter(DiagnosticsFilter,"MotorHomingProgressReport stopped 1000",__FILE__,__LINE__,deviceID,RpMessage,HomingCounter[deviceID],0);
        HomingControlId[deviceID] = 0xff;
    }
    response.progress = HomingCounter[deviceID]++;
    ReportWithPackageFilter(DiagnosticsFilter,"MotorHomingProgressReport",__FILE__,__LINE__,deviceID,RpMessage,HomingCounter[deviceID],0);
    responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[deviceID], last, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
    responseContainer.has_continuous = true;
    responseContainer.continuous = true;
    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    my_free(responseContainer.data.data);
    SendChars(container_buffer, container_size);

    return OK;

}
bool resetPullerDancer = false;
bool resetWinderDancer = false;
uint32_t MotorHomingRequestCallback(uint32_t deviceID, uint32_t ReadValue)
{
    MessageContainer responseContainer;
    MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT;

    if(deviceID == HARDWARE_MOTOR_TYPE__MOTO_RLOADING)
    {
        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, keepkvalright);
    }
    if(deviceID == HARDWARE_MOTOR_TYPE__MOTO_LLOADING)
    {
        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, keepkvalleft);
    }
    if (resetPullerDancer == true)
    {
        resetPullerDancer = false;
        MCU_E2PromProgram(EEPROM_PULLER_TENSION_POSITION,1);
    }
    if (resetWinderDancer == true)
    {
        resetWinderDancer = false;
        MCU_E2PromProgram(EEPROM_WINDER_TENSION_POSITION,1);
    }
    if (HomingControlId[deviceID] != 0xff)
    {
        ReportWithPackageFilter(DiagnosticsFilter,"MotorHomingProgressReport stopped",__FILE__,__LINE__,deviceID,RpMessage,HomingCounter[deviceID],0);
        RemoveControlCallback(HomingControlId[deviceID],MotorHomingProgressReport);
        HomingControlId[deviceID] = 0xff;
    }
    /*if (deviceID == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM)
    {
        MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,NumberOfCycles);//it takes two cycles to identify a stop of the arm
    }*/
    if (HomingToken[deviceID] == NULL)
        return OK;
    responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[deviceID], true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
    responseContainer.has_continuous = true;
    responseContainer.continuous = true;
    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    my_free(responseContainer.data.data);
    SendChars(container_buffer, container_size);
    if (HomingToken[deviceID])
        my_free(HomingToken[deviceID]);
    HomingToken[deviceID] = NULL;

return OK;

}
uint32_t LoadArmRounds;

uint32_t D_numberOfSteps = 0;
float D_numberOfCycles = 0;
uint32_t D_DrierPrevLocation = 0;


uint32_t MotorHomingRequestFunc(MessageContainer* requestContainer)
{
    uint32_t status = FAILED;
    MessageContainer responseContainer;

    MotorHomingRequest* request = motor_homing_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT;

    LoadArmRounds = (int)dryerbufferlength;

    if (Is_PP_Machine())
        kval_upper_value = 28;
    else
        kval_upper_value = 70;


    int direction;
    TimerMotors_t MotorId = (TimerMotors_t)request->motortype;
    if (LoadArmRounds <= 2)
            LoadArmRounds = 30;

    int speed = request->speed;
    if (speed == 0) speed = 150;
    if (MotorId <= NUM_OF_MOTORS)
    {
        if ((JobIsActive() == false)&&(isMotorConfigured(MotorId) == true))
        {
            HomingToken[MotorId] = my_malloc(36);
            ustrncpy (HomingToken[MotorId], requestContainer->token,36);
            if (request->direction == MOTOR_DIRECTION__Backward) //opening the loading system
            {
                if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID)
                {
                    if (MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].DriverType == CombinrdMotDriver)//reverse direction for powerstep
                    {
                        direction = DRIER_LID_CLOSE;
                    }
                    else
                    {
                        direction = DRIER_LID_OPEN;
                    }
                    ReportWithPackageFilter(DiagnosticsFilter,"MotorHomingRequestFunc Dryer lid open",__FILE__,MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].DriverType,DRIER_LID_OPEN,RpMessage,GPI_LS_DRYER_LID_OPEN,0);
                    MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, direction, GPI_LS_DRYER_LID_OPEN, MotorHomingRequestCallback,10000);
                    status = OK;
                }
                else
                {
                    if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM)
                    {
                        Diagnostics_Dryer_UnLoading();
                        status = OK;
                    }
                    if ( Motor_Id_to_LS_IdDown[MotorId] != MAX_GPI)
                    {
                        if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_LDANCER2)
                        {
                            resetPullerDancer = true;
                        }
                        if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_LDANCER1)
                        {
                            resetWinderDancer = true;
                        }
                        if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_RLOADING)
                        {
                            keepkvalright = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun;
                            MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, kval_upper_value);
                        }
                        if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_LLOADING)
                        {
                            keepkvalleft = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].kvalrun;
                            MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, kval_upper_value);
                        }
                        MotorMovetoLimitSwitch (MotorId,1-MotorsCfg[MotorId].directionthreadwize, speed,  Motor_Id_to_LS_IdUp[MotorId], MotorHomingRequestCallback,30000);
                        status = OK;
                    }

                }
            }
            else // closing the system into normal work
            {
                if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID)
                {
                    if (MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].DriverType == CombinrdMotDriver) //reverse direction for powerstep
                    {
                        direction = DRIER_LID_OPEN;
                    }
                    else
                    {
                        direction = DRIER_LID_CLOSE;
                    }
                    ReportWithPackageFilter(DiagnosticsFilter,"MotorHomingRequestFunc Dryer lid close",__FILE__,MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].DriverType,DRIER_LID_CLOSE,RpMessage,GPI_LS_DRYER_LID_CLOSED,0);
                    MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, direction, GPI_LS_DRYER_LID_CLOSED, MotorHomingRequestCallback,10000);
                    status = OK;
                }
                else
                {
                    if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM)
                    {
                        Diagnostics_Dryer_Loading();
                        status = OK;
                    }
                    if ( Motor_Id_to_LS_IdUp[MotorId] != MAX_GPI)
                    {
                        if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_RLOADING)
                        {
                            keepkvalright = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun;
                            MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, kval_upper_value);
                        }
                        if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_LLOADING)
                        {
                            keepkvalleft = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].kvalrun;
                            MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, kval_upper_value);
                        }
                        MotorMovetoLimitSwitch (MotorId,MotorsCfg[MotorId].directionthreadwize, speed,  Motor_Id_to_LS_IdDown[MotorId], MotorHomingRequestCallback,30000);
                        status = OK;
                    }
                }
            }
        }
    }
    if (status == OK)
    {
        HomingControlId[MotorId] = AddControlCallback(NULL, MotorHomingProgressReport,  2*eOneSecond, TemplateDataReadCBFunction,MotorId, 0, 0 );
        HomingCounter[MotorId] = 0;
        ReportWithPackageFilter(DiagnosticsFilter,"MotorHomingProgressReport started",__FILE__,__LINE__,MotorId,RpMessage,HomingControlId[MotorId],0);
    }
    else
    {
        responseContainer.has_error = true;
        responseContainer.error = ERROR_CODE__INVALID_PROCESS_ID;
        responseContainer.errormessage = "Job Active or incorrect motor ID";
        responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse,  requestContainer->token, true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
        responseContainer.has_continuous = true;
        responseContainer.continuous = true;
        uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
        size_t container_size = message_container__pack(&responseContainer, container_buffer);
        my_free(responseContainer.data.data);
        SendChars(container_buffer, container_size);
    }

return OK;
}
uint32_t MotorAbortHomingRequestFunc(MessageContainer* requestContainer)
{

    MessageContainer responseContainer;

    MotorAbortHomingRequest* request = motor_abort_homing_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    MotorAbortHomingResponse response = MOTOR_ABORT_HOMING_RESPONSE__INIT;

    TimerMotors_t MotorId = (TimerMotors_t)request->motortype;
    MotorAbortMovetoLimitSwitch(MotorId);
    MotorHomingRequestCallback(MotorId,0);
    if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_LDANCER2)
    {
        resetPullerDancer = false;
    }
    if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_LDANCER1)
    {
        resetWinderDancer = false;
    }
    if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_RLOADING)
    {
        if (keepkvalright)
            MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, keepkvalright);
    }
    if(MotorId == HARDWARE_MOTOR_TYPE__MOTO_LLOADING)
    {
        if (keepkvalleft)
            MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, keepkvalleft);
    }

    responseContainer = createContainer(MESSAGE_TYPE__MotorAbortHomingResponse,  requestContainer->token, false, &response, &motor_abort_homing_response__pack, &motor_abort_homing_response__get_packed_size);
    responseContainer.continuous = false;
    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    my_free(responseContainer.data.data);
    SendChars(container_buffer, container_size);

return OK;
}

/********************************************************************************
 * Dispenser Homing
 ********************************************************************************/
uint32_t DispenserHomingProgressReport(uint32_t deviceID, uint32_t ReadValue)
{
    MessageContainer responseContainer;
    DispenserHomingResponse response = DISPENSER_HOMING_RESPONSE__INIT;

    uint8_t MotorId = deviceID+HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;
    if (HomingToken[deviceID] == NULL)
        return OK;

    bool last = false;
    response.has_progress = true;
    response.has_maxprogress = true;
    response.maxprogress = 5800000;
    response.progress = IDS_Dispenser_Data[deviceID].consumedinnanolitter;

    responseContainer = createContainer(MESSAGE_TYPE__DispenserHomingResponse, HomingToken[MotorId], last, &response, &dispenser_homing_response__pack, &dispenser_homing_response__get_packed_size);
    ReportWithPackageFilter(DiagnosticsFilter,"DispenserHomingProgressReport",__FILE__,__LINE__,deviceID,RpMessage,IDS_Dispenser_Data[deviceID].consumedinnanolitter,0);
    responseContainer.has_continuous = true;
    responseContainer.continuous = true;
    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    my_free(responseContainer.data.data);
    SendChars(container_buffer, container_size);

    return OK;

}
uint32_t DispenserHomingRequestCallback(uint32_t deviceID, uint32_t ReadValue)
{
    MessageContainer responseContainer;
    DispenserHomingResponse response = DISPENSER_HOMING_RESPONSE__INIT;

    uint8_t MotorId = deviceID+HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;
    if (HomingControlId[MotorId] != 0xff)
    {
        RemoveControlCallback(HomingControlId[MotorId],DispenserHomingProgressReport);
        HomingControlId[MotorId] = 0xff;
        ReportWithPackageFilter(DiagnosticsFilter,"DispenserHomingProgressReport stopped",__FILE__,__LINE__,deviceID,RpMessage,HomingControlId[MotorId],0);
    }

    //close dry air valve in the dispenser
    //Valve_Set((Valves_t) (DispenserId+VALVE_2W_MID_AIR_8), Atm_MidTank_OFF);
    //MotorSetMicroStep(deviceID, MotorsCfg[deviceID].microstep);

    if (HomingToken[deviceID] == NULL)
        return OK;
    responseContainer = createContainer(MESSAGE_TYPE__DispenserHomingResponse, HomingToken[MotorId], true, &response, &dispenser_homing_response__pack, &dispenser_homing_response__get_packed_size);
    responseContainer.has_continuous = true;
    responseContainer.continuous = true;
    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    my_free(responseContainer.data.data);
    SendChars(container_buffer, container_size);
    if (HomingToken[deviceID])
        my_free(HomingToken[deviceID]);
    HomingToken[deviceID] = NULL;

return OK;

}
uint32_t DispenserHomingRequestFunc(MessageContainer* requestContainer)
{

    uint32_t status = OK;

    MessageContainer responseContainer;

    DispenserHomingRequest* request = dispenser_homing_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    DispenserHomingResponse response = DISPENSER_HOMING_RESPONSE__INIT;

    TimerMotors_t MotorId = (request->index)+HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;

    HomingToken[MotorId] = my_malloc(36);
    ustrncpy (HomingToken[MotorId], requestContainer->token,36);
    int speed = request->speed;
    if (speed == 0) speed = 500;

    responseContainer = createContainer(MESSAGE_TYPE__DispenserHomingResponse,  requestContainer->token, false, &response, &dispenser_homing_response__pack, &dispenser_homing_response__get_packed_size);
    responseContainer.has_continuous = true;
    responseContainer.continuous = true;
    if (MotorId <= NUM_OF_MOTORS)
    {
        if ((JobIsActive())&&(DispenserUsedInJob[request->index] == true))
        {
            responseContainer.error = ERROR_CODE__GENERAL_ERROR;
            responseContainer.errormessage = "Dispenser active in job";
        }
        else if (isMotorConfigured(MotorId) == true)
        {
            if (request->direction == MOTOR_DIRECTION__Backward) //dispenser homing
                status = IDS_HomeDispenser(request->index,speed,DispenserHomingRequestCallback);
            else //empty dispenser -
                status = IDS_EmptyDispenser(request->index,speed,DispenserHomingRequestCallback);//return ERROR;
            if (status == OK)
            {
                HomingControlId[MotorId] = AddControlCallback(NULL, DispenserHomingProgressReport,  2*eOneSecond, TemplateDataReadCBFunction,request->index, 0, 0 );
                ReportWithPackageFilter(DiagnosticsFilter,"DispenserHomingProgressReport started",__FILE__,__LINE__,request->index,RpMessage,HomingControlId[MotorId],0);
            }
        }
        else
        {
            responseContainer.error = ERROR_CODE__GENERAL_ERROR;
            responseContainer.errormessage = "Invalid Id or dispenser not configured";
        }
    }
    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    my_free(responseContainer.data.data);
    //USBCDCD_sendData(container_buffer, container_size,10);
    SendChars(container_buffer, container_size);

return OK;
}
uint32_t DispenserAbortHomingRequestFunc(MessageContainer* requestContainer)
{

    MessageContainer responseContainer;

    DispenserAbortHomingRequest* request = dispenser_abort_homing_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    DispenserAbortHomingResponse response = DISPENSER_ABORT_HOMING_RESPONSE__INIT;

    IDS_StopHomeDispenser(request->index);

    TimerMotors_t MotorId = (request->index)+HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;
    if (HomingControlId[MotorId] != 0xff)
    {
        RemoveControlCallback(HomingControlId[MotorId],DispenserHomingProgressReport);
        HomingControlId[MotorId] = 0xff;
        ReportWithPackageFilter(DiagnosticsFilter,"DispenserHomingProgressReport stopped",__FILE__,__LINE__,request->index,RpMessage,HomingControlId[MotorId],0);
    }
    /*
    TimerMotors_t MotorId = (request->index)+HARDWARE_MOTOR_TYPE__MOTO_DISPENSER_1;
    MotorAbortMovetoLimitSwitch(MotorId);
    //close dry air valve in the dispenser
    Valve_Set((Valves_t) ((request->index)+VALVE_2W_MID_AIR_8), Atm_MidTank_OFF);
*/
    responseContainer = createContainer(MESSAGE_TYPE__DispenserAbortHomingResponse,  requestContainer->token, false, &response, &dispenser_abort_homing_response__pack, &dispenser_abort_homing_response__get_packed_size);
    responseContainer.continuous = false;
    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    my_free(responseContainer.data.data);
    SendChars(container_buffer, container_size);

return OK;
}
/********************************************************************************
 * Drier Loading Arm Homing
 ********************************************************************************/
uint32_t Drier_Center_read = 0;
uint32_t Diagnostics_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag)
{
    MessageContainer responseContainer;
    MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT;
    uint32_t temp = Read_Dryer_ENC_Position();
    int angle;

    angle = Calculate_Arm_Distance(Drier_Center_read,temp);

    ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Set_Load_Arm_To_Stopper time",__FILE__,__LINE__,msec_millisecondCounter,RpMessage,angle,0);
    if (HomingControlId[deviceID] != 0xff)
    {
        ReportWithPackageFilter(DiagnosticsFilter,"MotorHomingProgressReport stopped",__FILE__,__LINE__,deviceID,RpMessage,HomingCounter[deviceID],0);
        RemoveControlCallback(HomingControlId[deviceID],MotorHomingProgressReport);
        HomingControlId[deviceID] = 0xff;
    }

    //NumberOfDrierLoaderCycles=0;
    //storeLoadArmParameters();
    //SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);  //set this point as the spool home
    ThreadLoadingRestartReport();
    responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[deviceID], true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
    responseContainer.has_continuous = true;
    responseContainer.continuous = true;
    if (abs (angle)>20) // arm not at center position
    {
        responseContainer.has_error = true;
        responseContainer.error = ERROR_CODE__GENERAL_ERROR;
    }
    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    my_free(responseContainer.data.data);
    SendChars(container_buffer, container_size);
    if (HomingToken[deviceID])
        my_free(HomingToken[deviceID]);
    HomingToken[deviceID] = NULL;

    return OK;
}

/*uint32_t Diagnostics_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue)
{
    bool direction;

    D_numberOfCycles++;
    uint32_t temp = Read_Dryer_ENC_Position();
    ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_UnLoading_Callback",__FILE__,ReadValue,temp,RpMessage,D_DrierPrevLocation,0);
    //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_UnLoading_Callback details",__FILE__,(int)(TotalLoadedLen),D_numberOfCycles,RpMessage,CallbackCounter,0);
    //if ((AccumulatedArmMovement>8000  )&&(ReadValue == NOTBUSY)) // OK - take another round
    if (ReadValue == NOTBUSY) // OK - take another round
    {
        ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_UnLoading cycles",__FILE__,D_numberOfCycles,LoadArmRounds,RpMessage,0,0);
        MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0);
        ReportWithPackageFilter(DiagnosticsFilter,"Store Number of cycles in drier",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0);
        if (abs(temp -Drier_Center_read)<200)
        {
            ReportWithPackageFilter(DiagnosticsFilter,"drier center proximity",__FILE__,temp,Drier_Center_read,RpMessage,abs(temp -Drier_Center_read),0);
            if (temp<Drier_Center_read)
                direction = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
            else
                direction = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize;
            MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Diagnostics_Set_Load_Arm_To_Stopper_Callback,3000,direction,10);

        }
        else
            MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);

    }
    else  //timeout or no movement
    {
        ReportWithPackageFilter(DiagnosticsFilter,"Unloading drier - halted",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0);
        //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-(D_numberOfCycles-2));//it takes two cycles to identify a stop of the arm

        ReportWithPackageFilter(DiagnosticsFilter,"Dryer unloading timeout(1) or no movement",__FILE__,temp,D_DrierPrevLocation,RpWarning,ReadValue,0);
        Diagnostics_Set_Load_Arm_To_Stopper_Callback(MotorId, ReadValue);
    }
    return OK;
}*/
uint32_t Diagnostics_Dryer_UnLoading(void)
{
    D_DrierPrevLocation = Read_Dryer_ENC_Position();
    MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&Drier_Center_read);
    LoadingArmReset(Diagnostics_Set_Load_Arm_To_Stopper_Callback,300000);
    /*uint32_t temp;

    MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds);
    if (LoadArmRounds == 0) //prev trial stopped
    {
        LoadArmRounds = (int)dryerbufferlength;
    }
    if (LoadArmRounds <= 2)
        MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds);

    D_numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
    D_numberOfCycles = 0;

    MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp);
    Drier_Center_read = temp;
    D_DrierPrevLocation = temp;
    ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,D_DrierPrevLocation,RpMessage, LoadArmRounds,0);
    MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius);

//        status |= MotorMoveToStopper(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize),
//                                        MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4, Diagnostics_Set_Load_Arm_To_Stopper_Callback,0,1000);
    //MotorRunWithCallback
    MotorRunWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
                          MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius,    Diagnostics_Dryer_UnLoading_Callback, 300000);
*/
    return OK;
}
uint32_t Diagnostics_Dryer_MovetoEncoderPosition_Callback(uint32_t MotorId, uint32_t ReadValue)
{
    MessageContainer responseContainer;
    MotorHomingResponse response = MOTOR_HOMING_RESPONSE__INIT;
    ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_MovetoEncoderPosition_Callback time",__FILE__,__LINE__,msec_millisecondCounter,RpMessage,0,0);
    if (HomingControlId[MotorId] != 0xff)
    {
        ReportWithPackageFilter(DiagnosticsFilter,"MotorHomingProgressReport stopped",__FILE__,__LINE__,MotorId,RpMessage,HomingCounter[MotorId],0);
        RemoveControlCallback(HomingControlId[MotorId],MotorHomingProgressReport);
        HomingControlId[MotorId] = 0xff;
    }

    ReportWithPackageFilter(DiagnosticsFilter,"Store angle of arm drier - ended",__FILE__,__LINE__,(int)(Calculate_Arm_Angle(Drier_Center_read,ReadValue)*360),RpMessage,D_numberOfCycles,0);
    //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_MovetoEncoderPosition_Callback",__FILE__,__LINE__,0,RpMessage,CallbackCounter,0);
    MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200);
    responseContainer = createContainer(MESSAGE_TYPE__MotorHomingResponse, HomingToken[MotorId], true, &response, &motor_homing_response__pack, &motor_homing_response__get_packed_size);
    responseContainer.has_continuous = true;
    responseContainer.continuous = true;
    uint8_t* container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    my_free(responseContainer.data.data);
    SendChars(container_buffer, container_size);
    if (HomingToken[MotorId])
        my_free(HomingToken[MotorId]);
    HomingToken[MotorId] = NULL;

    return OK;
}

uint32_t Diagnostics_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue)
{
    D_numberOfCycles++;
    int angle;
    uint32_t temp = Read_Dryer_ENC_Position();

    angle = Calculate_Arm_Distance(D_DrierPrevLocation,temp);
    ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading_Callback",__FILE__,(int)angle,(int)ReadValue,RpMessage,temp,0);

    MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,D_numberOfCycles);
    if ((abs(angle)<400  )&&(ReadValue == NOTBUSY)) // OK - take another round
    {
        D_DrierPrevLocation = temp;
        ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading_Callback",__FILE__,ReadValue,temp,RpMessage,angle,0);
        //ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading_Callback details",__FILE__,(int)(TotalLoadedLen),numberOfCycles,RpMessage,CallbackCounter,0);
        if (D_numberOfCycles<LoadArmRounds)
        {
            MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
                                   D_numberOfSteps,    Diagnostics_Dryer_Loading_Callback, 10000);
        }
        else
        {
            MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Diagnostics_Dryer_MovetoEncoderPosition_Callback,15000,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,40);
            ReportWithPackageFilter(DiagnosticsFilter,"Store Number of cycles in drier",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0);
        }
    }
    else
    {
        ReportWithPackageFilter(DiagnosticsFilter,"Loading drier - halted",__FILE__,__LINE__,D_numberOfCycles,RpMessage,LoadArmRounds,0);
        ReportWithPackageFilter(DiagnosticsFilter,"Store angle of arm drier - halted",__FILE__,__LINE__,(int)(Calculate_Arm_Angle(Drier_Center_read,ReadValue)*360),RpMessage,D_numberOfCycles,0);
        MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200);
        Diagnostics_Dryer_MovetoEncoderPosition_Callback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,0);
    }
    return OK;
}

uint32_t Diagnostics_Dryer_Loading(void)
{
    //float Calc_angle;
    if (dryerbufferlength)
        LoadArmRounds = (int)dryerbufferlength;
    MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&D_DrierPrevLocation);

    //MCU_E2PromProgram(EEPROM_DRIER_LOADING_ARM_ANGLE,Calc_angle);
    ReportWithPackageFilter(DiagnosticsFilter,"Diagnostics_Dryer_Loading request: current ",__FILE__,D_DrierPrevLocation,D_numberOfCycles,RpMessage,LoadArmRounds,0);
    Drier_Center_read = D_DrierPrevLocation;
    D_numberOfCycles = 0;
    D_numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
    D_numberOfSteps -= 100;
    MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/6*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius);
    //6 seconds per round

    //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
    MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
                           D_numberOfSteps,    Diagnostics_Dryer_Loading_Callback, 10000);
    return OK;
}