/*
 * ThreadLoad.c
 *
 *  Created on: Jan 16, 2019
 *      Author: shlomo
 */
#include <DataDef.h>
#include "include.h"
#include "thread_ex.h"

#include "Common/SWUpdate/FileSystem.h"

#include "StateMachines/Printing/PrintingSTM.h"

#include "Modules/thread/thread.h"
#include "Modules/Control/control.h"
#include "Modules/General/process.h"
#include "Modules/control/pidalgo.h"

#include "Modules/heaters/heaters_ex.h"
#include "Modules/General/buttons.h"
#include "StateMachines/Initialization/InitSequence.h"

#include "PMR/Hardware/HardwareMotor.pb-c.h"
#include "PMR/Hardware/HardwareMotorType.pb-c.h"
#include "PMR/Hardware/HardwareDancerType.pb-c.h"
#include "PMR/Printing/ProcessParameters.pb-c.h"

#include "drivers/Flash_Memory/fatfs/ff.h"
#include "drivers/I2C_Communication/DAC/Blower.h"
#include "drivers/SSI_Comm/Dancer/Dancer.h"
#include "drivers/motors/motor.h"
#include "drivers/Flash_ram/MCU_E2Prom.h"
#include "Drivers/I2C_Communication/I2C.h"
#include <Drivers/I2C_Communication/WHS_Card/D_Max5805_ADC_Blower/WHS_Blower.h>
#include <Drivers/I2C_Communication/I2C_Task.h>
#include "drivers/FPGA/FPGA_SPI_Comm.h"
#include "drivers/FPGA/Full_Vme/FPGA_Programming_Up.h"

#include "PMR/Stubs/StubHeatingTestRequest.pb-c.h"
#include "PMR/Stubs/StubHeatingTestResponse.pb-c.h"
#include "PMR/Stubs/StubHeatingTestPollRequest.pb-c.h"
#include "PMR/Stubs/StubHeatingTestPollResponse.pb-c.h"
#include "PMR/ThreadLoading/StartThreadLoadingRequest.pb-c.h"
#include "PMR/ThreadLoading/StartThreadLoadingResponse.pb-c.h"
#include "PMR/ThreadLoading/ContinueThreadLoadingRequest.pb-c.h"
#include "PMR/ThreadLoading/ContinueThreadLoadingResponse.pb-c.h"
#include "PMR/ThreadLoading/TryThreadLoadingRequest.pb-c.h"
#include "PMR/ThreadLoading/TryThreadLoadingResponse.pb-c.h"
#include "PMR/ThreadLoading/StopThreadLoadingRequest.pb-c.h"
#include "PMR/ThreadLoading/StopThreadLoadingResponse.pb-c.h"

//#include <stdint.h>
//#include <stdbool.h>
    static char* LoadStagesStr[15] = {
    /*00*/ "Initialization",
    /*01*/ "Heating stop",
    /*02*/ "Rockers opening",
    /*03*/ "Drier unloading",
    /*04*/ "Lids opening",
    /*05*/ "Dancers opening",
    /*06*/ "Initial tension building",
    /*07*/ "Rockers closing",
    /*08*/ "Dancers closing",
    /*09*/ "Lids closing",
    /*10*/ "Feeder jogging",
    /*11*/ "Drier loading",
    /*12*/ "Heating resuming",
    /*13*/ "Thread jogging" ,
    /*14*/ "END"};

/*    typedef enum
    {
        00 THREAD_LOAD_INIT,
        01 THREAD_LOAD_REDUCE_HEAT,  //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
        02 THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE
        03 THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,//USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY
        04 THREAD_LOAD_OPEN_COVERS,  //OPEN DYEING HEAD COVER AND DRYER LID
        05 THREAD_LOAD_LIFT_DANCERS,
        06 THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND
        07 THREAD_LOAD_CLOSE_ROCKERS,
        08 THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD
        09 THREAD_LOAD_CLOSE_LIDS,
        10 THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION
        11 THREAD_LOAD_DRYER_LOADING, //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40
                                    //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER
        12 THREAD_LOAD_RESUME_HEATING,
        13 THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
        14 THREAD_LOAD_END
    }THREAD_LOAD_STAGES_ENUM;*/
    THREAD_LOAD_STAGES_ENUM LoadStages = THREAD_LOAD_INIT;
    ProcessParameters ProcessParametersClear,ProcessParametersRecover;

    uint32_t LoadStatus = OK;
    char LoadErrorMsg[100];
    int32_t keepmicrostep;
    int32_t keepkvalrun,keepkvalhold,kval_upperValue;
    uint8_t CallbackCounter = 0;
    uint8_t TimeoutsCounter = 0;
    bool SecondTry = false,TryAgain = false;
    int MessageState = 0; //0 - none, 1 = start, 2 = continue

    uint32_t UnloadingStart = 0;
    uint8_t NumberOfDrierLoaderCycles = 0;
    uint32_t status = OK;
    uint32_t LoadingControlId = 0xFF,PullerControlId = 0xFF,WinderControlId = 0xFF, SpeedTControlId = 0xFF;
    uint32_t previousPosition = 0, currentPosition = 0,Tinitialpos = 0xFFFF;
    uint32_t LoadCounter = 0;
    double TotalLoadedLen = 0.0;
    double LengthCalculationMultiplier;
    float numberOfSteps = 0;
    float numberOfCycles = 0;
    double DrierPrevLocation = 0;
    FPGA_GPI_ENUM screw[2] = {GPI_LS_SCREW_RIGHT,GPI_LS_SCREW_LEFT};
    int Screw_Dir = false;
    bool InitCalled = false;
    uint32_t LoadArmRounds;

    uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM LoadStages);
    uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue);
    uint32_t ThreadLoadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue);
    uint32_t Thread_Load_Dryer_UnLoading(void);
    uint32_t ThreadLoadingReport(void);
    bool ThreadLoaded(void);
    bool ThreadLoadingActive(void)
    {
        if ((LoadStages > THREAD_LOAD_INIT)&&(LoadStages < THREAD_LOAD_END))
            return true;
        else
            return false;
    }
    uint32_t Thread_Load_Init(THREAD_LOAD_STAGES_ENUM Stage)
    {
        //void* buffer = NULL;
        //uint32_t Bytes = 0;
        //FRESULT Fresult = FR_OK;
        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Init");
        load.color = BLINK;
        LoadStatus = OK;
        //LoadArmRounds = 0xFF;
        //EEPROM_STORAGE_DRYER_CYCLES
        MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds);

        Report("Read_Dryer_ENC_Position()",__FILE__,__LINE__,0,RpWarning,Read_Dryer_ENC_Position(),0);

        Report("MotorMovetoEncoderPosition",__FILE__,__LINE__,LoadArmRounds,RpWarning,0,0);

        Report("Thread_Load_Init",__FILE__,__LINE__,LoadArmRounds,RpMessage,LoadArmRounds,0);
        StopInitSequence();
        memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters));
        //NumberOfDrierLoaderCycles = loadLoadArmParameters();
        if (Stage == THREAD_LOAD_INIT)
        {
            LoadStages++;
            ThreadLoadStateMachine(LoadStages);
        }
        else
        {
            Report("Thread_Load_Init another state ",__FILE__,__LINE__,Stage,RpMessage,LoadArmRounds,0);
        }
        InitCalled = true;

        return OK;
    }
    uint32_t Thread_Load_Reduce_Heat(void)
    {

        if (Is_PP_Machine())
            kval_upperValue = 40;
        else
            kval_upperValue = 70;

        //Heaters Off, Dryer Blower Off, Blower Low,
        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Reduce_Heat");
        memcpy (&ProcessParametersClear,&ProcessParametersKeep,sizeof(ProcessParameters));
        ProcessParametersClear.dryerzone1temp = 0;
        ProcessParametersClear.dryerzone2temp = 0;
        ProcessParametersClear.dryerzone3temp = 0;
        ProcessParametersClear.mixertemp      = 0;
        ProcessParametersClear.headzone1temp  = 0;
        ProcessParametersClear.headzone2temp  = 0;
        ProcessParametersClear.headzone3temp  = 0;
        ProcessParametersClear.headzone4temp  = 0;
        ProcessParametersClear.headzone5temp  = 0;
        ProcessParametersClear.headzone6temp  = 0;
        ProcessParametersClear.headzone7temp  = 0;
        ProcessParametersClear.headzone8temp  = 0;
        ProcessParametersClear.headzone9temp  = 0;
        ProcessParametersClear.headzone10temp  = 0;
        ProcessParametersClear.headzone11temp  = 0;
        ProcessParametersClear.headzone12temp  = 0;
        ProcessParametersClear.rblowertemp  = 0;
        ProcessParametersClear.lblowertemp  = 0;
        ProcessParametersClear.rblowerflow  = 0;
        ProcessParametersClear.lblowerflow  = 0;
        ProcessParametersClear.dyeingspeed = 40;
        ProcessParametersClear.dryerbufferlength = ProcessParametersKeep.dryerbufferlength;
        ProcessParametersClear.pullertension = ProcessParametersKeep.pullertension;
        ProcessParametersClear.windertension = ProcessParametersKeep.windertension;

        if (HandleProcessParameters(&ProcessParametersClear,false)!= OK)
        {
            LOG_ERROR (LoadStages, "Thread_Load_Reduce_Heat failed");
            strcpy(LoadErrorMsg,"Thread_Load_Reduce_Heat failed");
            LoadStatus = ERROR;
            TryAgain = true;
            //ThreadLoadButton(LoadStages);
        }
        else
        {
            if(Head_Type > HEAD_TYPE_SYLKO_WITHOUT_CARD)
            {
                WHS_Set_SetPoint_Q_value(headairflow/2);
            }
            else
            {
                Turn_the_Blower_On();//Turn on with the Default_Voltage
                if (BlowerCfg.heatingvoltage)
                    Control_Voltage_To_Blower(BlowerCfg.heatingvoltage);
                else
                    Control_Voltage_To_Blower(BlowerCfg.voltage-500);
            }
            Control_Dryer_Fan(STOP,75);//use START or STOP,  0 - 100%
            LoadStages++;
            ThreadLoadStateMachine(LoadStages);
        }
        return OK;
    }
    uint32_t Thread_Load_Set_Load_Arm_To_Stopper_Callback(uint32_t deviceID, uint32_t BusyFlag)
    {

        /*if(PullerControlId != 0xFF)
        {
            MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz);
            RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction );
            PullerControlId = 0xFF;
        }*/
        if(WinderControlId != 0xFF)
        {
            MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz);
            RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction );
            WinderControlId = 0xFF;
        }

        MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
        /*Task_sleep(5)
        MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
        MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/

        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz);
//move to exact location?
//        Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
        CallbackCounter = 0;
        Report("Thread_Load_Set_Load_Arm_To_Stopper time",__FILE__,msec_millisecondCounter - UnloadingStart,msec_millisecondCounter,RpMessage,UnloadingStart,0);

        Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
        NumberOfDrierLoaderCycles=0;
        //storeLoadArmParameters();
        LoadStages++;
        SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);  //set this point as the spool home

        ThreadLoadStateMachine(LoadStages);
        return OK;
    }
    uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue)
    {
        Report("Thread_Load_HomingCallback",__FILE__,MotorId,LoadStages,RpMessage,CallbackCounter,0);
//        REPORT_MSG(MotorId, "Thread_Load_HomingCallback Motor Id");
        if ((LoadStages == THREAD_LOAD_CLOSE_ROCKERS)||(LoadStages == THREAD_LOAD_LIFT_ROCKERS))
        {
            if((MotorId == HARDWARE_MOTOR_TYPE__MOTO_RLOADING)||(MotorId == HARDWARE_MOTOR_TYPE__MOTO_LLOADING))
            {
                if(LoadStages == THREAD_LOAD_CLOSE_ROCKERS)
                {
                    MotorMove(MotorId,MotorsCfg[MotorId].directionthreadwize,400);
			        Report("Move rockers further down",__FILE__,__LINE__,MotorId,RpMessage,400,0);
                    Task_sleep(200);
                }
                MotorSetMicroStep(MotorId, keepmicrostep);
                MotorSetKvalRun(MotorId, keepkvalrun);
            }
        }

        if (CallbackCounter)
        {
            CallbackCounter--;
            if (ReadValue != LIMIT)
            {
                //returned with a timeout
                TimeoutsCounter++;
            }
            if (CallbackCounter == 0)
            {
                if (TimeoutsCounter)
                {
                    TimeoutsCounter = 0;
                    load.color = fastBILNK;
                    usnprintf(LoadErrorMsg, 100, "Stage %s - %s timeout",LoadStagesStr[LoadStages], MotorStr[MotorId]);
                    Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0);

                    LoadStatus = ERROR;
                    ThreadLoadingReport();
                    TryAgain = true;
                    //ThreadLoadButton(LoadStages);
                }
                else
                {
                    LoadStages++;
                    LoadStatus = OK;
                    if (LoadStages == THREAD_LOAD_CLOSE_DANCERS)
                    {
                        MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, keepkvalhold);
                    }
                    if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this stage we should wait for user call
                    {
                        ThreadLoadStateMachine(LoadStages);
                    }
                    else
                    {
                        load.color = colorON;
                        ThreadLoadingReport();
                    }
                }
            }
        }
        else
            LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0");
        return OK;
    }
    uint32_t Thread_Load_Open_Covers(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Open_Covers");
        //Open Dyeing Head Cover And Dryer Lid
        //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2,
        //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4,
        //DeActivateHeadMagnet();
        if (Head_Type != HEAD_TYPE_STAPLE_SPUN)
        {
            CallbackCounter++;
            MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,10000);
        }
        CallbackCounter++;
        //MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
        int direction;
        if (MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].DriverType == CombinrdMotDriver)//reverse direction for powerstep
        {
            direction = DRIER_LID_CLOSE;
        }
        else
        {
            direction = DRIER_LID_OPEN;
        }
        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, direction, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
        if (Head_Type == HEAD_TYPE_SYLKO)
        {
            Report("Thread_Load_Lift_actuators",__FILE__,__LINE__,LOW,RpMessage,false,0);
            Trigger_Head_Actuators_Control(ACTOT, LOW,true);
            Trigger_Head_Actuators_Control(ACTIN, LOW,true);
        }
        return OK;
    }
    uint32_t Thread_Load_Lift_Dancers(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Lift_Dancers");
        /*keepkvalhold = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].kvalhold;
        MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 40);
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,15000);*/
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000);
        status |= MCU_E2PromProgram(EEPROM_WINDER_TENSION_POSITION,1);
        if (Is_PP_Machine() == true) //PP machine - new LTFU
        {
            CallbackCounter++;
            MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, 500,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2], Thread_Load_HomingCallback,25000);
            status |= MCU_E2PromProgram(EEPROM_PULLER_TENSION_POSITION,1);

        }
        return OK;
    }
    uint32_t Thread_Load_Lift_Rockers(void)
    //Machine Is Ready. Send Message, Start Timer To Close Lids, Wait For Operator Response
    {
        keepmicrostep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep;
        keepkvalrun = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun;
        if (keepkvalrun>=25)
            keepkvalrun = 25;

        Report("Read_Dryer_ENC_Position()",__FILE__,__LINE__,0,RpWarning,Read_Dryer_ENC_Position(),0);

        Report("Thread_Load_Lift_Rockers",__FILE__,__LINE__,keepkvalrun,RpMessage,keepmicrostep,0);

        Report("Thread_Load_Lift_right dancer",__FILE__,__LINE__,0,RpMessage,0,0);
        keepkvalhold = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].kvalhold;
        MotorSetKvalHold(HARDWARE_MOTOR_TYPE__MOTO_RDANCER, 40);
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 22,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,15000);

        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Lift_Rockers");
        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 8);
        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 8);
        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, kval_upperValue);
        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, kval_upperValue);
        Task_sleep(10);
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 250,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,35000);
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 250,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,35000);

        return OK;
    }
    uint32_t Thread_Load_Initial_Tension(void) //Check Spool Presence, run Winder Until Break Sensor Is Identifieing Movement For A Second
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Initial_Tension");
        load.color = BLINK;
        if (FPGA_Read_limit_Switches(GPI_SW_SPOOL_EXISTS)==LIMIT)
        {
            REPORT_MSG(LIMIT, "No cone in winder");
            //return ERROR;
        }
        CallbackCounter++;
        MotorMovetoBreakSensor (HARDWARE_MOTOR_TYPE__MOTO_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize, 500,  Thread_Load_HomingCallback,10000);

        return OK;

    }
    uint32_t Thread_Load_Close_Rockers(void)
    {
        keepmicrostep = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].microstep;
        keepkvalrun = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].kvalrun;
        if (keepkvalrun>=25)
            keepkvalrun = 25;
        Report("Thread_Load_Close_Rockers",__FILE__,__LINE__,keepkvalrun,RpMessage,keepmicrostep,0);

        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 8);
        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 8);
        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, kval_upperValue);
        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, kval_upperValue);

        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Rockers");
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 250,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,25000);
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 250,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,25000);

        return OK;
    }
    uint32_t Thread_Load_Close_Dancers(void)
    //Send Dancer Motors To Preset Location, Check That The Dancers Are On The Thread
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Dancers");
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RDANCER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDANCER].directionthreadwize, 15,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RDANCER], Thread_Load_HomingCallback,10000);
        CallbackCounter++;
        MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, 500);

//        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, 500,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1], Thread_Load_HomingCallback,25000);
        MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER1, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].directionthreadwize, ((int)windertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].microstep), Thread_Load_HomingCallback,20000);
        status |= MCU_E2PromProgram(EEPROM_WINDER_TENSION_POSITION,(int)windertension);
        if (Is_PP_Machine() == true) //PP machine - new LTFU
        {
            CallbackCounter++;
            MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, 500);
            MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_LDANCER2, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].directionthreadwize, ((int)pullertension*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER2].microstep), Thread_Load_HomingCallback,20000);
            status |= MCU_E2PromProgram(EEPROM_PULLER_TENSION_POSITION,(int)pullertension);
        }

        return OK;
    }
    uint32_t Thread_Load_Close_Lids(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Close_Lids");
        //Close Dyeing Head Cover And Dryer Lid
        //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2,
        //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4,
        if (Head_Type != HEAD_TYPE_STAPLE_SPUN)
        {
            CallbackCounter++;
            MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DH_LID], Thread_Load_HomingCallback,10000);
        }
        CallbackCounter++;
//        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].directionthreadwize, 200,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
        int direction;
        if (MotorDriverResponse[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID].DriverType == CombinrdMotDriver)//reverse direction for powerstep
        {
            direction = DRIER_LID_OPEN;
        }
        else
        {
            direction = DRIER_LID_CLOSE;
        }
        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, direction, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
        if (Head_Type == HEAD_TYPE_SYLKO)
        {
            Report("Thread_Load_Close_actuators",__FILE__,__LINE__,LOW,RpMessage,true,0);
            Trigger_Head_Actuators_Control(ACTOT, LOW,false);
            Trigger_Head_Actuators_Control(ACTIN, LOW,false);
        }
        return OK;
    }
    uint32_t Thread_Load_Resume_Heating(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Resume_Heating");
        ActivateHeadMagnet();

        if (HandleProcessParameters(&ProcessParametersRecover,false)!= OK)
        {
            usnprintf(LoadErrorMsg, 100, "Stage %s error",LoadStagesStr[LoadStages]);
            Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0);
        }
        else
        {
            Control_Dryer_Fan(START,75);//use START or STOP,  0 - 100%
            LoadStages++;
            ThreadLoadStateMachine(LoadStages);
        }
        return OK;
    }
    uint32_t Thread_Load_Jog_Feeder_To_Middle_Point_Callback(uint32_t deviceID, uint32_t Condition)
    {
        if (Condition == true)
        {
            CallbackCounter--;
            Report("Thread_Load_Jog_Feeder_To_Middle_Point_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
            //storeLoadArmParameters();
            LoadStages++;
            ThreadLoadStateMachine(LoadStages);
        }
        else
        {
            CallbackCounter--;
            Report("Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
            //storeLoadArmParameters();
            if (SecondTry == true)
            {
                LoadStages++;
                ThreadLoadStateMachine(LoadStages);
                LoadStatus = OK;
            }
            else
            {
                TimeoutsCounter = 0;
                load.color = fastBILNK;
                usnprintf(LoadErrorMsg, 100, "Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!");
                Report("Thread_Load_Jog_Feeder_To_Middle_Point TimeOut!",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
                LoadStatus = ERROR;
                ThreadLoadingReport();
                TryAgain = true;
                //ThreadLoadButton(LoadStages);
            }
        }

        return OK;
    }
    uint32_t Thread_Load_Jog_Feeder_To_Middle_Point(void)
    //Jog The Feeder Motor Until The Feeder Dancer Is At Middle Position
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Jog_Feeder_To_Middle_Point");
        CallbackCounter++;
        MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 400, FEEDER_DANCER ,true, Thread_Load_Jog_Feeder_To_Middle_Point_Callback,6000);
        return OK;
    }
    uint32_t Thread_Load_Dryer_MovetoEncoderPosition_Callback(uint32_t MotorId, uint32_t ReadValue)
    {
        Report("Thread_Load_Dryer_MovetoEncoderPosition_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
        MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200);
        if(LoadingControlId != 0xFF)
        {
            MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
            RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction );
            LoadingControlId = 0xFF;
        }
        if (SpeedTControlId != 0xFF)
        {
           RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction );
           SpeedTControlId = 0xFF;
        }
        Report("Total Feeder load",__FILE__,__LINE__,dryerbufferCentimeters,RpMessage,TotalLoadedLen,0);

        if (CallbackCounter)
        {
            CallbackCounter--;
            if (CallbackCounter == 0)
            {
                LoadStatus = OK;

                LoadStages++;
                if (LoadStages != THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call
                {
                    ThreadLoadStateMachine(LoadStages);
                }
            }
        }
        else
            LOG_ERROR(MotorId, "Thread_Load_HomingCallback Callback is already 0");

        return OK;
    }
    uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue)
    {
        numberOfCycles++;

        Report("Thread_Load_Dryer_Loading_Callback",__FILE__,(int)TotalLoadedLen,(int)DrierPrevLocation,RpMessage,ReadValue,0);

        if ((fabs (TotalLoadedLen  -DrierPrevLocation)>50  )&&(ReadValue == NOTBUSY))
        {
            DrierPrevLocation = TotalLoadedLen ;
            //Report("Thread_Load_Dryer_Loading_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
            //Report("Thread_Load_Dryer_Loading_Callback details",__FILE__,(int)(TotalLoadedLen),numberOfCycles,RpMessage,CallbackCounter,0);
            if (LoadStages != THREAD_LOAD_DRYER_LOADING)
            {
                return OK;
            }
            if (numberOfCycles<LoadArmRounds)
            {
                MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
                                       numberOfSteps,    Thread_Load_Dryer_Loading_Callback, 10000);
            }
            else
            {
                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Dryer_MovetoEncoderPosition_Callback,30000);
                MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
                Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
            }
        }
        else
        {
            TryAgain = true;
            SecondTry = false;
            Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
            MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
            TimeoutsCounter = 0;
            CallbackCounter = 0;
            load.color = fastBILNK;
            usnprintf(LoadErrorMsg, 100, "Stage %s - Drier load arm timeout",LoadStagesStr[LoadStages]);
            Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0);
            LoadStatus = ERROR;
            MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 200);
            if(LoadingControlId != 0xFF)
            {
                MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
                RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction );
                LoadingControlId = 0xFF;
            }
            if (SpeedTControlId != 0xFF)
            {
               RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction );
               SpeedTControlId = 0xFF;
            }
            Report("Total Feeder load",__FILE__,__LINE__,dryerbufferCentimeters,RpMessage,TotalLoadedLen,0);

            ThreadLoadingReport();
            //ThreadLoadButton(LoadStages);
        }
        return OK;
    }

    uint32_t Thread_Load_Dryer_Loading(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Dryer_Loading");

        int CurrentlyLoaded = 0;
        if (dryerbufferlength)
            LoadArmRounds = (int)dryerbufferlength;
        if (LoadArmRounds <= 2)
            LoadArmRounds = 30;
        //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
        SetOriginMotorSpeed(19);
//        OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000;
//        CurrentControlledSpeed[FEEDER_MOTOR] = 1000;

        MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&CurrentlyLoaded);
        Report("Thread_Load_Dryer_Loading cycles",__FILE__,LoadArmRounds,CurrentlyLoaded,RpMessage,LoadArmRounds-CurrentlyLoaded,0);
        if (CurrentlyLoaded)
        {
            LoadArmRounds -= CurrentlyLoaded;
        }
        ThreadMotorIdToMotorId[FEEDER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_RDRIVING ;
        ThreadMotorIdToMotorId[DRYER_MOTOR] = HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING;

        MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&DrierPrevLocation);
        numberOfCycles = 0;
        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;
//        numberOfSteps -= 100;
        //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*3.3*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
        Report("Thread Load State Machine step.",__FILE__,LoadStages,numberOfSteps,RpMessage,(int)(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius),0);
        MotorControlConfig[FEEDER_MOTOR].m_params.MAX = 1;
        MotorControlConfig[FEEDER_MOTOR].m_params.MIN = MotorsControl[FEEDER_MOTOR].outputproportionalpowerlimit*-1;
        MotorControlConfig[FEEDER_MOTOR].m_params.Kd = MotorsControl[FEEDER_MOTOR].derivativetime;
        MotorControlConfig[FEEDER_MOTOR].m_params.Kp = MotorsControl[FEEDER_MOTOR].proportionalgain;
        MotorControlConfig[FEEDER_MOTOR].m_params.Ki = MotorsControl[FEEDER_MOTOR].integraltime;
        MotorControlConfig[FEEDER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[FEEDER_MOTOR].setpointramprateorsoftstartramp;
        MotorControlConfig[FEEDER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[FEEDER_MOTOR].outputonoffhysteresisvalue;
        MotorControlConfig[FEEDER_MOTOR].m_params.epsilon = MotorsControl[FEEDER_MOTOR].epsilon;
        MotorControlConfig[FEEDER_MOTOR].m_params.dt = MotorsControl[FEEDER_MOTOR].controloutputtype;
        MotorControlConfig[FEEDER_MOTOR].m_ingnoreValue =  MotorsControl[FEEDER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
        MotorControlConfig[FEEDER_MOTOR].m_calculatedError = 0;
        MotorControlConfig[FEEDER_MOTOR].m_integral = 0;
        MotorControlConfig[FEEDER_MOTOR].m_isEnabled = true;
        MotorControlConfig[FEEDER_MOTOR].m_isReady = true;
        MotorControlConfig[FEEDER_MOTOR].m_mesuredParam = 0;
        MotorControlConfig[FEEDER_MOTOR].m_preError = 0;
        MotorControlConfig[FEEDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage
        MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize);

        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

        LoadingControlId = AddControlCallback(NULL,ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
        Report("AddControlCallback feeder",__FILE__,__LINE__,LoadingControlId,RpMessage,IfTypeThread*0x100+FEEDER_MOTOR,0);
        if (SpeedTControlId != 0xFF)
        {
            RemoveControlCallback(SpeedTControlId,ThreadLoadLengthCBFunction);
            SpeedTControlId = 0xFF;
        }
        //SetMotHome(ThreadMotorIdToMotorId[Motor_i]);
        LengthCalculationMultiplier = (MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].pulleyradius*2*PI)/(MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].pulseperround*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].microstep);
        TotalLoadedLen = 0;
        DrierPrevLocation = 200; //initial safe value
        SpeedTControlId = AddControlCallback("Thread length",ThreadLoadLengthCBFunction, eHundredMillisecond,MotorGetPositionFromFPGA,(IfTypeThread*0x100+HARDWARE_MOTOR_TYPE__MOTO_RDRIVING),HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,0);
        Tinitialpos = 0xFFFF;previousPosition = 0; currentPosition = 0;
        CallbackCounter++;
        Report("Thread_Load_Dryer_Loading",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
        MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
                               numberOfSteps-800,    Thread_Load_Dryer_Loading_Callback, 10000);
        return OK;
    }
    uint32_t Thread_Load_Jog_ThreadStop(uint32_t index, uint32_t ReadValue)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Jog_ThreadStop");
        SafeRemoveControlCallback(LoadingControlId, Thread_Load_Jog_ThreadStop );
        LoadingControlId = 0xFF;
        ThreadAbortJoggingFunc();
        LoadStages++;
        ThreadLoadStateMachine(LoadStages);
        return OK;
    }
    uint32_t Thread_Load_Jog_Thread(void)
    //Jog Thread Shortly To Make Sure Spool Is Running. Report End Of Loading
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step Thread_Load_Jog_Thread");
        ThreadJoggingFunc(20);
        LoadingControlId = AddControlCallback("Load jog",Thread_Load_Jog_ThreadStop, eOneSecond*25,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
        return OK;
    }
    uint32_t Thread_Load_End(void)
    {
        REPORT_MSG(LoadStages,"Loading Ended");
        ThreadLoadingReport();
        if(LoadingControlId != 0xFF)
        {
            MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
            RemoveControlCallback(LoadingControlId, ThreadLoadControlCBFunction );
            LoadingControlId = 0xFF;
        }
        if(PullerControlId != 0xFF)
        {
            MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz);
            RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction );
            PullerControlId = 0xFF;
        }
        if(WinderControlId != 0xFF)
        {
            MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz);
            RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction );
            WinderControlId = 0xFF;
        }
        if (SpeedTControlId != 0xFF)
        {
           RemoveControlCallback(SpeedTControlId, ThreadLoadLengthCBFunction );
           SpeedTControlId = 0xFF;
        }

        MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);
        MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_RLOADING);
        MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_LLOADING);
        MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_RDANCER);
        MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_LDANCER1);
        MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID);
        MotorStopAction(HARDWARE_MOTOR_TYPE__MOTO_DH_LID);
        MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);
        if (LoadStages!=THREAD_LOAD_END)
        {
            usnprintf(LoadErrorMsg, 100, "Load sequence stopped by user on stage %d",LoadStages);
            LoadStatus = ERROR;
        }

        LoadStages = THREAD_LOAD_INIT;
        load.color = colorON;
        return OK;
    }
    uint32_t Thread_Load_switchCallback(uint32_t deviceID, uint32_t BusyFlag)
    {
        if ((LoadStages != THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION)||(LoadStatus == ERROR))
        {
            return OK;
        }
        Screw_Dir = 1-Screw_Dir;
        // movement of the screw while unloading the thread from the dryer to the spool
        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,13000);

        return OK;
    }
    uint32_t Thread_Load_Dryer_UnLoading_Callback(uint32_t MotorId, uint32_t ReadValue)
    {
        numberOfCycles++;
        uint32_t temp = Read_Dryer_ENC_Position();
        Report("Thread_Load_Dryer_UnLoading_Callback",__FILE__,ReadValue,temp,RpMessage,DrierPrevLocation,0);
        //Report("Thread_Load_Dryer_UnLoading_Callback details",__FILE__,(int)(TotalLoadedLen),numberOfCycles,RpMessage,CallbackCounter,0);
        if (LoadStages != THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION)
        {
            Report("Thread_Load_Dryer_UnLoading_Callback wrong state",__FILE__,__LINE__,LoadStages,RpMessage,DrierPrevLocation,0);
            return OK;
        }
        if ((abs (temp -DrierPrevLocation)>1000  )&&(ReadValue == NOTBUSY)) // OK - take another round
        {
            DrierPrevLocation = temp;
            Report("Thread_Load_Dryer_UnLoading cycles",__FILE__,numberOfCycles,LoadArmRounds,RpMessage,0,0);
            if (numberOfCycles<LoadArmRounds)
            {
                MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
                                       numberOfSteps,    Thread_Load_Dryer_UnLoading_Callback, 10000);
            }
            else //done enough cycles, go to the center point
            {
                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,30000);
                MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,0);
                Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
            }
        }
        else  //timeout or no movement
        {
            Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
            MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmRounds-numberOfCycles);
            TimeoutsCounter = 0;
            CallbackCounter = 0;
            /*if(PullerControlId != 0xFF)
            {
                MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz);
                RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction );
                PullerControlId = 0xFF;
            }*/
            if(WinderControlId != 0xFF)
            {
                MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz);
                RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction );
                WinderControlId = 0xFF;
            }

            MotorAbortMovetoLimitSwitch(HARDWARE_MOTOR_TYPE__MOTO_SCREW);
            /*Task_sleep(5)
            MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
            MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);*/

            MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,Hard_Hiz);
            MotorStop(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Hard_Stop);
            load.color = fastBILNK;
            usnprintf(LoadErrorMsg, 100, "Stage %s - Drier load arm timeout",LoadStagesStr[LoadStages]);
            Report(LoadErrorMsg,__FILE__,__LINE__,LoadStages,RpWarning,TimeoutsCounter,0);
            Report("Drier unloading timeout(1) or no movement",__FILE__,temp,DrierPrevLocation,RpWarning,ReadValue,0);
            LoadStatus = ERROR;
            TryAgain = true;
            ThreadLoadingReport();
            //ThreadLoadButton(LoadStages);
        }
        return OK;
    }
    uint32_t Thread_Load_Dryer_UnLoading(void)
    {
        uint32_t temp;
        REPORT_MSG(LoadStages, "Thread Load State Machine step Dryer Unloading");
        //LoadArmRounds = 0;
        //uint32_t numberOfSteps = 0;
        //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
        SetOriginMotorSpeed(22);
//        OriginalMotorSpd_2PPS[FEEDER_MOTOR] = 1000;
//        CurrentControlledSpeed[FEEDER_MOTOR] = 1000;

// Rockers are up already, so puller handling is not needed
        //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;
        /*MotorControlConfig[POOLER_MOTOR].m_params.MAX = 1;
        MotorControlConfig[POOLER_MOTOR].m_params.MIN = MotorsControl[POOLER_MOTOR].outputproportionalpowerlimit*-1;
        MotorControlConfig[POOLER_MOTOR].m_params.Kd = MotorsControl[POOLER_MOTOR].derivativetime;
        MotorControlConfig[POOLER_MOTOR].m_params.Kp = MotorsControl[POOLER_MOTOR].proportionalgain;
        MotorControlConfig[POOLER_MOTOR].m_params.Ki = 0;//MotorsControl[POOLER_MOTOR].integraltime;
        MotorControlConfig[POOLER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[POOLER_MOTOR].setpointramprateorsoftstartramp;
        MotorControlConfig[POOLER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[POOLER_MOTOR].outputonoffhysteresisvalue;
        MotorControlConfig[POOLER_MOTOR].m_params.epsilon = MotorsControl[POOLER_MOTOR].epsilon;
        MotorControlConfig[POOLER_MOTOR].m_params.dt = MotorsControl[POOLER_MOTOR].controloutputtype;
        MotorControlConfig[POOLER_MOTOR].m_ingnoreValue =  MotorsControl[POOLER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
        MotorControlConfig[POOLER_MOTOR].m_calculatedError = 0;
        MotorControlConfig[POOLER_MOTOR].m_integral = 0;
        MotorControlConfig[POOLER_MOTOR].m_isEnabled = true;
        MotorControlConfig[POOLER_MOTOR].m_isReady = true;
        MotorControlConfig[POOLER_MOTOR].m_mesuredParam = 0;
        MotorControlConfig[POOLER_MOTOR].m_preError = 0;
        MotorControlConfig[POOLER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage
        MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_LDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDRIVING].directionthreadwize);

        PullerControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+POOLER_MOTOR),POOLER_DANCER,POOLER_MOTOR);
        Report("AddControlCallback Puller",__FILE__,__LINE__,PullerControlId,RpMessage,IfTypeThread*0x100+POOLER_MOTOR,0);*/
////////////////////////
        MotorControlConfig[WINDER_MOTOR].m_params.MAX = 1;
        MotorControlConfig[WINDER_MOTOR].m_params.MIN = MotorsControl[WINDER_MOTOR].outputproportionalpowerlimit*-1;
        MotorControlConfig[WINDER_MOTOR].m_params.Kd = MotorsControl[WINDER_MOTOR].derivativetime;
        MotorControlConfig[WINDER_MOTOR].m_params.Kp = MotorsControl[WINDER_MOTOR].proportionalgain;
        MotorControlConfig[WINDER_MOTOR].m_params.Ki = 0;//MotorsControl[WINDER_MOTOR].integraltime;
        MotorControlConfig[WINDER_MOTOR].m_params.IntegralErrorMultiplier = MotorsControl[WINDER_MOTOR].setpointramprateorsoftstartramp;
        MotorControlConfig[WINDER_MOTOR].m_params.ProportionalErrorMultiplier = MotorsControl[WINDER_MOTOR].outputonoffhysteresisvalue;
        MotorControlConfig[WINDER_MOTOR].m_params.epsilon = MotorsControl[WINDER_MOTOR].epsilon;
        MotorControlConfig[WINDER_MOTOR].m_params.dt = MotorsControl[WINDER_MOTOR].controloutputtype;
        MotorControlConfig[WINDER_MOTOR].m_ingnoreValue =  MotorsControl[WINDER_MOTOR].sensorcorrectionadjustment; // the minimal change required to change the motor speed in pulses
        MotorControlConfig[WINDER_MOTOR].m_calculatedError = 0;
        MotorControlConfig[WINDER_MOTOR].m_integral = 0;
        MotorControlConfig[WINDER_MOTOR].m_isEnabled = true;
        MotorControlConfig[WINDER_MOTOR].m_isReady = true;
        MotorControlConfig[WINDER_MOTOR].m_mesuredParam = 0;
        MotorControlConfig[WINDER_MOTOR].m_preError = 0;
        MotorControlConfig[WINDER_MOTOR].m_SetParam = 0;//need to update SetParams on presegment stage
        MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_WINDER,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_WINDER].directionthreadwize);

        WinderControlId = AddControlCallback(NULL,ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+WINDER_MOTOR),WINDER_DANCER,WINDER_MOTOR);
        Report("AddControlCallback Winder",__FILE__,__LINE__,WinderControlId,RpMessage,IfTypeThread*0x100+WINDER_MOTOR,0);

////////////////////////
        MotorSetDirection(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING].directionthreadwize);
        MotorSetSpeed(HARDWARE_MOTOR_TYPE__MOTO_DRYER_DRIVING, OriginalMotorSpd_2PPS[DRYER_MOTOR]);

        CallbackCounter++;
        //MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
        //                       numberOfSteps,    Thread_Load_Dryer_Loading_Callback, 100000);
        //Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmRounds,0);
        //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
//shlomo
        UnloadingStart = msec_millisecondCounter;
        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;
        numberOfCycles = 0;

        if (SecondTry == true)
        {
            MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds);
            if (LoadArmRounds == 0) //prev trial stopped
            {
                LoadArmRounds = (int)dryerbufferlength;
            }
            //LoadArmRounds = 30;
        }
        else
        {
            MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmRounds);
            if (LoadArmRounds == 0) //prev trial stopped
            {
                LoadArmRounds = (int)dryerbufferlength;
            }
        }
        MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp);
        DrierPrevLocation = temp;
        Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,UnloadingStart,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, Thread_Load_Set_Load_Arm_To_Stopper_Callback,0,1000);
        MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
                               numberOfSteps-800,    Thread_Load_Dryer_UnLoading_Callback, 10000);

        Screw_Dir = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize;

        // movement of the screw while unloading the thread from the dryer to the spool
        status = MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_SCREW,Screw_Dir, 80, screw[Screw_Dir], Thread_Load_switchCallback,20000);

        //Keep Notation How Many Rotations In The Dryer
        //LoadArmRounds = (int)dryerbufferlength;
        load.color = BLINK;
        return OK;
    }

    uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue)
    {
        REPORT_MSG(ReadValue,"ThreadLoadStateMachine");
        //ThreadLoaded();
        ThreadLoadingReport();

        if (LoadStages!=ReadValue)
            LoadStages = ReadValue;
        switch (ReadValue)
        {
        case THREAD_LOAD_INIT:
            //LoadStages++;
            MessageState = 1;

            Thread_Load_Init(ReadValue);
            break;
        case THREAD_LOAD_REDUCE_HEAT:  //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
            //LoadStages++;
            MessageState = 1;
            if (InitCalled == false)
            {
                REPORT_MSG(ReadValue,"Thread_Load_Init called from 1");
                Thread_Load_Init(ReadValue);
            }
            else
            {
                REPORT_MSG(ReadValue,"Thread_Load_Init called from 0");
            }
            Thread_Load_Reduce_Heat();
            InitCalled = false;
            break;
        case THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION://USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINS STOPPER. MOVE SLOWLY
            //LoadStages++;
            //Thread_Load_Set_Load_Arm_To_Start_Position();
            Thread_Load_Dryer_UnLoading();
            break;
        /*case THREAD_LOAD_CENTER_HEAD_ROCKERS:
            //LoadStages++;
            Thread_Load_Center_Head_Rockers();
            break;*/
        case THREAD_LOAD_OPEN_COVERS:  //OPEN DYEING HEAD COVER AND DRYER LID
            //LoadStages++;
            Thread_Load_Open_Covers();
            break;
        case THREAD_LOAD_LIFT_DANCERS:
            //LoadStages++;
            Thread_Load_Lift_Dancers();
            break;
        case THREAD_LOAD_LIFT_ROCKERS: //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE
            //LoadStages++;
            Thread_Load_Lift_Rockers();
            break;
        case THREAD_LOAD_INITIAL_TENSION://CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND
            //LoadStages++;
            Thread_Load_Initial_Tension();
            break;
        case THREAD_LOAD_CLOSE_ROCKERS:
            //LoadStages++;
            Thread_Load_Close_Rockers();
            break;
        case THREAD_LOAD_CLOSE_DANCERS: //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD
            //LoadStages++;
            Thread_Load_Close_Dancers();
            break;
        case THREAD_LOAD_CLOSE_LIDS:
            //LoadStages++;
            Thread_Load_Close_Lids();
            break;
        case THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT://JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION
            //LoadStages++;
            Thread_Load_Jog_Feeder_To_Middle_Point();
            break;
        case THREAD_LOAD_DRYER_LOADING: //START FEEDER PID, ROTATE LOADING ARM COUNTER THREAD DIRECTION X CIRCLES ACCORDING TO RML. FEEDER SPEED IS 40
                                    //KEEP NOTATION HOW MANY ROTATIONS IN THE DRYER
            //LoadStages++;
            Thread_Load_Dryer_Loading();
            break;
        case THREAD_LOAD_RESUME_HEATING:
            //LoadStages++;
            Thread_Load_Resume_Heating();
            break;
        case THREAD_LOAD_JOG_THREAD: //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
            //LoadStages++;
            Thread_Load_Jog_Thread();
            ThreadLoadingReport();
            break;
        case THREAD_LOAD_END:
            LoadStages = THREAD_LOAD_END;
            Thread_Load_End();
            break;
        default:
            LOG_ERROR(LoadStages,"ERROR IN LOAD STATE MACHINE");
            break;
    }

    return OK;
}
uint32_t ThreadLoadButton(THREAD_LOAD_STAGES_ENUM ReadValue)
{
    if (JobIsActive()||(MachineReadyForHeating == false)||(FPGABurningActive==true))
    {
        Report("ThreadLoadButton called on job",__FILE__,__LINE__,LoadStages,RpMessage,ReadValue,0);
        return ERROR;
    }
    if (CallbackCounter)
    {
        Report("ThreadLoadButton called when the loading is active",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
        return ERROR;
    }
    Report("ThreadLoadButton",__FILE__,__LINE__,LoadStages,RpMessage,ReadValue,0);
    Report("ThreadLoadButton params",__FILE__,LoadStatus,TryAgain,RpMessage,SecondTry,0);
    ThreadLoadingReport();
    if (LoadStatus == ERROR)
    {
        if ((SecondTry == false)&&(TryAgain == true))
        {
            LoadStatus = OK;
            SecondTry = true;
            Report("Calling State machine",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0);
            ThreadLoadStateMachine(LoadStages);
        }
        else if (SecondTry == true)
        {
            SecondTry = false;
            Report("Calling State machine",__FILE__,LoadStages,LoadStatus,RpMessage,SecondTry,0);
            ThreadLoadStateMachine(LoadStages+1);
        }
    }
    else
    {
        Report("Calling State machine status OK",__FILE__,ReadValue,LoadStatus,RpMessage,SecondTry,0);
        ThreadLoadStateMachine(LoadStages);
    }
    return OK;
}

uint32_t ThreadLoadLengthCBFunction(uint32_t IfIndex, uint32_t ReadValue)
{
    uint32_t positionDiff = 0,prevprev;
    double length = 0.0;
    char lenstr[150];
    static int counter = 0;
    int index = MAX_THREAD_MOTORS_NUM;
    if (IfIndex>>8 != IfTypeThread)
    {
        LOG_ERROR (IfIndex, "Wrong  Interface type");
        return 0xFFFFFFFF;
    }
    index = IfIndex&0xFF;
//    if (CurrentRequestedLength == 0.0)
//        return OK;
    if (index != HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)
    {
        LOG_ERROR (IfIndex, "Wrong Motor");
        return 0xFFFFFFFF;
    }
    currentPosition = MotorGetPosition(HARDWARE_MOTOR_TYPE__MOTO_RDRIVING);
    if (currentPosition != 0)
    {
        if (Tinitialpos == 0xFFFF)
        {
            previousPosition = currentPosition;
            Tinitialpos = 0;
        }
        prevprev = previousPosition;
        if (Extended_Motor_Param[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING] == true) //powerstep driver reverses the direction
            positionDiff = Control_Delta_Position_Pass(previousPosition,currentPosition);
        else
            positionDiff = Control_Delta_Position_Pass(currentPosition,previousPosition);
        //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
        previousPosition = currentPosition;

        // total length = (position diff / full cycle) * pulley perimeter
        //(positionDiff/pulseperround)*((2*PI*motor_Radius)

        //positionDiff = positionDiff / MotorsCfg[ThreadMotorIdToMotorId[index]].microstep;
        length = (double)(positionDiff)*LengthCalculationMultiplier;

        if (counter++ % 50 == 0)
        {
            usnprintf(lenstr, 100, " length %d, diff 0x%x, pos 0x%x prev 0x%x TotalLoadedLen %d",(int)length*100,(int)positionDiff,previousPosition,prevprev, (int)TotalLoadedLen);
//            SendJobProgress(0.0,0,false, lenstr);
            ReportWithPackageFilter(ThreadFilter,lenstr,__FILE__,__LINE__,(int)TotalLoadedLen,RpWarning,(int)TotalLoadedLen,0);
//            length = 0;

        }

    }
    TotalLoadedLen += length;

    return OK;
}
uint32_t ThreadLoadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
{
    //read value is the dancer angle
    int i;
    int DancerId;
    int32_t TranslatedReadValue, avreageSampleValue = 0;
    int index=MAX_THREAD_MOTORS_NUM;
    //double tempcalcspeed = 0;
    uint32_t calculated_speed;
    double NormalizedError;

    if (IfIndex>>8 != IfTypeThread)
    {
        LOG_ERROR (IfIndex, "Wrong  Interface type");
        return 0xFFFFFFFF;
    }
    index = IfIndex&0xFF;

    DancerId = ThreadMotorIdToDancerId[index];
    if (ReadValue < 10)
    {
        Report("Dancer value read too small.",__FILE__,__LINE__,DancerId,RpError,ReadValue,0);
        return OK;
    }
    if (ReadValue == 0x3FFF)
    {
        return OK;
    }
    TranslatedReadValue = ReadValue - DancersCfg[DancerId].zeropoint;
    if (index == POOLER_MOTOR)
    {
        //pooler dancer is right sided: data is opposite
        TranslatedReadValue = (-1*TranslatedReadValue);
    }
    //TranslatedReadValue = 0;//test
    MotorSamples[index][MotorSamplePointer[index]] = TranslatedReadValue;//(-1 * TranslatedReadValue);
    MotorSamplePointer[index]++;
    if (MotorSamplePointer[index] >= MotorsControl[index].pvinputfilterfactormode)
        MotorSamplePointer[index] = 0;
    for (i=0;i<MotorsControl[index].pvinputfilterfactormode;i++)
        avreageSampleValue += MotorSamples[index][i];
    avreageSampleValue = avreageSampleValue / MotorsControl[index].pvinputfilterfactormode;


    NormalizedError = avreageSampleValue*NormalizedErrorCoEfficient[index];
    MotorControlConfig[index].m_mesuredParam = NormalizedError;
    MotorControlConfig[index].m_calculatedError = PIDAlgorithmCalculation((float)MotorControlConfig[index].m_SetParam , (float)MotorControlConfig[index].m_mesuredParam,
                                                                          &MotorControlConfig[index].m_params,   &MotorControlConfig[index].m_preError, &MotorControlConfig[index].m_integral);
    if (index != FEEDER_MOTOR) //feeder unit handles errors opposite to left unit
    {
        MotorControlConfig[index].m_calculatedError = (-1*MotorControlConfig[index].m_calculatedError);
    }
    calculated_speed = (1-MotorControlConfig[index].m_calculatedError)*OriginalMotorSpd_2PPS[index];
    if (fabs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
    {
        CurrentControlledSpeed[index] = calculated_speed;
        MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
    }
   /*

     if (index == POOLER_MOTOR)
    {
        if ((TranslatedReadValue)<(-1200))
        {
            if(PullerControlId != 0xFF)
            {
                Report("Puller stoped",__FILE__,__LINE__,ReadValue,RpWarning,(int)DancersCfg[DancerId].zeropoint,0);
                MotorStop(ThreadMotorIdToMotorId[POOLER_MOTOR],Hard_Hiz);
                RemoveControlCallback(PullerControlId, ThreadLoadControlCBFunction );
                PullerControlId = 0xFF;
            }
        }

    }
    if (index == WINDER_MOTOR)
    {
        if ((TranslatedReadValue)<(-1200))
        {
            if(WinderControlId != 0xFF)
            {
                Report("Winder stoped",__FILE__,__LINE__,ReadValue,RpWarning,(int)DancersCfg[DancerId].zeropoint,0);
                MotorStop(ThreadMotorIdToMotorId[WINDER_MOTOR],Hard_Hiz);
                RemoveControlCallback(WinderControlId, ThreadLoadControlCBFunction );
                WinderControlId = 0xFF;
            }

        }

    }*/
/*    LoadCounter++;
    if ((LoadCounter % 5001) == 0)
    {
          Report("MotorSpeedUpdated",__FILE__,index,abs(TranslatedReadValue),RpWarning,(int)CurrentControlledSpeed[index],0);
    }*/
 return OK;
}
bool ThreadLoaded(void)
{
   /* uint32_t ReadValue;

    int32_t PoolerValue,WinderValue,FeederValue;
    //this function tests if there is a thread in the machine. all dancers are in lowermost position then there is no thread.

    ReadValue = Control_Read_Dancer_Position(FEEDER_DANCER,0);
    FeederValue = ReadValue - DancersCfg[FEEDER_DANCER].zeropoint;
    Report("Feeder out",__FILE__,abs(FeederValue),ReadValue,RpWarning,(int)DancersCfg[FEEDER_DANCER].zeropoint,0);
    ReadValue = Control_Read_Dancer_Position(WINDER_DANCER,0);
    WinderValue = ReadValue - DancersCfg[WINDER_DANCER].zeropoint;
    Report("Winder out",__FILE__,abs(WinderValue),ReadValue,RpWarning,(int)DancersCfg[WINDER_DANCER].zeropoint,0);
    ReadValue = Control_Read_Dancer_Position(POOLER_DANCER,0);
    PoolerValue = ReadValue - DancersCfg[POOLER_DANCER].zeropoint;
        //pooler dancer is right sided: data is opposite
    PoolerValue = (-1*PoolerValue);
    Report("Puller out",__FILE__,abs(PoolerValue),ReadValue,RpWarning,(int)DancersCfg[POOLER_DANCER].zeropoint,0);

    if (((PoolerValue)<(-1200))&&((WinderValue)<(-1200))&&((FeederValue)<(-1200)))
    {
        Report("Thread is out",__FILE__,(-1*PoolerValue),(-1*WinderValue),RpWarning,(-1*FeederValue),0);
        return true;
    }
    else*/
        return false;
}
void ThreadLoadRequest(MessageContainer* requestContainer)
{
//#ifdef DEBUG_TEST_FUNCTIONS
    MessageContainer responseContainer;
    uint8_t* container_buffer;
    //uint32_t status = OK;

    StubHeatingTestRequest* request = stub_heating_test_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
    StubHeatingTestResponse response = STUB_HEATING_TEST_RESPONSE__INIT;

    responseContainer = createContainer(MESSAGE_TYPE__StubHeatingTestResponse, requestContainer->token, false, &response, &stub_heating_test_response__pack, &stub_heating_test_response__get_packed_size);
    container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));


    int LoadStage = (int)request->dryerzone1temp;
    LoadStages = LoadStage;
    ThreadLoadStateMachine(LoadStage);

    /*if (status)
    {
        responseContainer.has_error = true;
        responseContainer.error = ERROR_CODE__JOB_UNSPECIFIED_ERROR;
    }*/
    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);
    stub_heating_test_request__free_unpacked(request,NULL);

//#else
//    LOG_ERROR (-1, "Heating Control not on debug");
//    return ERROR;
//#endif
}
void ThreadLoadPollRequest(MessageContainer* requestContainer)
{
    uint8_t* container_buffer;

    StubHeatingTestPollRequest* request = stub_heating_test_poll_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);

    StubHeatingTestPollResponse response = STUB_HEATING_TEST_POLL_RESPONSE__INIT;
    MessageContainer responseContainer;

    response.has_heater1percentage = true;
    response.heater1percentage = LoadStages;
    responseContainer = createContainer(MESSAGE_TYPE__StubHeatingTestPollResponse, requestContainer->token, false, &response, &stub_heating_test_poll_response__pack, &stub_heating_test_poll_response__get_packed_size);
    container_buffer = my_malloc(message_container__get_packed_size(&responseContainer));


/*
    {
        responseContainer.has_error = true;
        responseContainer.error = ERROR_CODE__JOB_UNSPECIFIED_ERROR;
    }*/
    size_t container_size = message_container__pack(&responseContainer, container_buffer);
    my_free(responseContainer.data.data);
    SendChars(container_buffer, container_size);
    stub_heating_test_poll_request__free_unpacked(request,NULL);
}
char ThreadLoadingToken[36+1] = {0};
char DefaultErrSrt[] = "OK";
uint32_t ThreadLoadingReport(void)
{
    MessageContainer responseContainer;
    bool last = false;
    StartThreadLoadingResponse response = START_THREAD_LOADING_RESPONSE__INIT;

    if (ThreadLoadingToken[0] == 0)
        return OK;

    if (MessageState == 0)
    {
        response.has_state = true;
        response.state = THREAD_LOADING_STATE__None;
        response.errorreason = DefaultErrSrt;

    }
    else if (MessageState == 1)
    {
        response.has_state = true;
        if (LoadStatus == OK)
        {
            response.state = THREAD_LOADING_STATE__Preparing;
            if (LoadStages == THREAD_LOAD_INITIAL_TENSION)
            {
                response.state = THREAD_LOADING_STATE__ReadyForLoading;
                response.errorreason = DefaultErrSrt;
            }
        }
        else
        {
            response.state = THREAD_LOADING_STATE__PreparationError;
            response.errorreason = LoadErrorMsg;
        }
    }
    else if (MessageState == 2)
    {
        response.has_state = true;
        if (LoadStatus == OK)
        {
            response.state = THREAD_LOADING_STATE__Finalizing;
            if (LoadStages > THREAD_LOAD_JOG_THREAD)
            {
                response.state = THREAD_LOADING_STATE__Completed;
                response.errorreason = DefaultErrSrt;
                //last = true;
            }
        }
        else
        {
            response.state = THREAD_LOADING_STATE__FinalizationError;
            response.errorreason = LoadErrorMsg;
        }
    }
    else return ERROR;
    Report("ThreadLoadingReport",__FILE__,MessageState,response.state,RpWarning,(int)LoadStages,0);

    //-------------------------------------------------------------------------------------------
    responseContainer = createContainer(MESSAGE_TYPE__StartThreadLoadingResponse, ThreadLoadingToken, last, &response, &start_thread_loading_response__pack, &start_thread_loading_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((char*)container_buffer, container_size);

    return OK;

}

uint32_t TryThreadLoadingFunc(MessageContainer* requestContainer)
{
    TryThreadLoadingRequest *request = continue_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
    TryThreadLoadingResponse Cresponse = TRY_THREAD_LOADING_RESPONSE__INIT;
    MessageContainer responseContainer;
    MessageState = 2;
    ThreadLoadingReport();
    Report("TryThreadLoadingFunc",__FILE__,__LINE__,MESSAGE_TYPE__TryThreadLoadingResponse,RpWarning,(int)LoadStages,0);

    responseContainer = createContainer(MESSAGE_TYPE__TryThreadLoadingResponse, requestContainer->token, true, &Cresponse, &try_thread_loading_response__pack, &try_thread_loading_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((char*)container_buffer, container_size);

    ThreadLoadButton(LoadStages);
    return OK;
}
uint32_t StartThreadLoadingFunc(MessageContainer* requestContainer)
{
    StartThreadLoadingRequest *request = start_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
    ustrncpy (ThreadLoadingToken, requestContainer->token,36);
    //MessageState = 1;
    Report("StartThreadLoadingFunc",__FILE__,__LINE__,0,RpWarning,(int)LoadStages,0);
    ThreadLoadingReport();
    return OK;

}

uint32_t ContinueThreadLoadingFunc(MessageContainer* requestContainer)
{
    ContinueThreadLoadingRequest *request = continue_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
    ContinueThreadLoadingResponse Cresponse = CONTINUE_THREAD_LOADING_RESPONSE__INIT;
    MessageContainer responseContainer;
    MessageState = 2;
    ThreadLoadingReport();
    if (request->processparameters)
    {
        dryerbufferlength = request->processparameters->dryerbufferlength;
        LoadArmRounds = (int)(request->processparameters->dryerbufferlength);

    }
    Report("ContinueThreadLoadingFunc",__FILE__,__LINE__,(int)(request->processparameters->dryerbufferlength),RpWarning,(int)LoadStages,0);

    responseContainer = createContainer(MESSAGE_TYPE__ContinueThreadLoadingResponse, requestContainer->token, true, &Cresponse, &continue_thread_loading_response__pack, &continue_thread_loading_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((char*)container_buffer, container_size);
    if (LoadStages == THREAD_LOAD_INITIAL_TENSION) //on this satge we should wait for user call
    {
        ThreadLoadStateMachine(LoadStages);
    }
    return OK;

}
uint32_t StopThreadLoadingFunc(MessageContainer* requestContainer)
{
    StopThreadLoadingRequest *request = stop_thread_loading_request__unpack(NULL, requestContainer->data.len, requestContainer->data.data);
    StopThreadLoadingResponse Cresponse = STOP_THREAD_LOADING_RESPONSE__INIT;
    MessageContainer responseContainer;
    MessageState = 2;
    Thread_Load_End();
    MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
    Report("StopThreadLoadingFunc",__FILE__,__LINE__,0xFF,RpWarning,(int)LoadStages,0);

    responseContainer = createContainer(MESSAGE_TYPE__StopThreadLoadingResponse, requestContainer->token, true, &Cresponse, &stop_thread_loading_response__pack, &stop_thread_loading_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((char*)container_buffer, container_size);
    return OK;

}