aboutsummaryrefslogtreecommitdiffstats
path: root/Software/Visual_Studio/MachineStudio/Modules/Tango.MachineStudio.Logging/ViewModels/EventsViewVM.cs
blob: 2c3886ea4fb6a07bfcd82b2e35e60ef89eedad77 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Tango.BL;
using Tango.BL.Entities;
using Tango.Core.Commands;
using Tango.MachineStudio.Common.EventLogging;
using Tango.MachineStudio.Common.Messages;
using Tango.MachineStudio.Common.Notifications;
using Tango.MachineStudio.Common.StudioApplication;
using Tango.MachineStudio.Logging.Navigation;
using Tango.MachineStudio.Logging.Views;
using Tango.SharedUI;
using System.Data.Entity;

namespace Tango.MachineStudio.Logging.ViewModels
{
    public class EventsViewVM : ViewModel
    {
        private INotificationProvider _notification;
        private IStudioApplicationManager _application;
        private IEventLogger _eventLogger;
        private ObservableCollection<MachinesEvent> _realTimeEvents;
        private LoggingNavigationManager _navigation;
        private bool _dialog_shown;
        private ObservablesContext _db;
        private List<MachinesEvent> _history_events;

        private Machine _selectedMachine;
        public Machine SelectedMachine
        {
            get { return _selectedMachine; }
            set
            {
                if (_selectedMachine != value)
                {
                    _selectedMachine = value; RaisePropertyChangedAuto(); OnSelectedMachineChanged();
                }
            }
        }

        private ObservableCollection<MachinesEvent> _events;
        public ObservableCollection<MachinesEvent> Events
        {
            get { return _events; }
            set { _events = value; RaisePropertyChangedAuto(); }
        }

        private MachinesEvent _selectedEvent;
        public MachinesEvent SelectedEvent
        {
            get { return _selectedEvent; }
            set { _selectedEvent = value; RaisePropertyChangedAuto(); OnSelectedEventChanged(); }
        }

        private ObservableCollection<DateTime> _dates;
        public ObservableCollection<DateTime> Dates
        {
            get { return _dates; }
            set { _dates = value; RaisePropertyChangedAuto(); }
        }

        private DateTime _selectedDate;
        public DateTime SelectedDate
        {
            get { return _selectedDate; }
            set { _selectedDate = value; RaisePropertyChangedAuto(); OnSelectedDateChanged(); }
        }

        private DateTime _minDate;
        public DateTime MinDate
        {
            get { return _minDate; }
            set { _minDate = value; RaisePropertyChangedAuto(); }
        }

        private DateTime _maxDate;
        public DateTime MaxDate
        {
            get { return _maxDate; }
            set { _maxDate = value; RaisePropertyChangedAuto(); }
        }

        private bool _isRealTime;
        public bool IsRealTime
        {
            get { return _isRealTime; }
            set { _isRealTime = value; RaisePropertyChangedAuto(); OnSelectedDateChanged(); }
        }

        private TimelineViewVM _timelineViewVM;

        public TimelineViewVM TimelineViewVM
        {
            get { return _timelineViewVM; }
            set { _timelineViewVM = value; RaisePropertyChangedAuto(); }
        }

        public RelayCommand<MachinesEvent> DisplayTimelineCommand { get; set; }

        public RelayCommand NavigateToEventsCommand { get; set; }

        public RelayCommand NavigateToHomeCommand { get; set; }

        public EventsViewVM(INotificationProvider notification, IEventLogger eventLogger, IStudioApplicationManager application, LoggingNavigationManager navigation)
        {
            TimelineViewVM = new TimelineViewVM(notification);

            _navigation = navigation;
            _application = application;
            _notification = notification;
            _eventLogger = eventLogger;
            _realTimeEvents = new ObservableCollection<MachinesEvent>();
            _eventLogger.NewLog += _eventLogger_NewLog;

            DisplayTimelineCommand = new RelayCommand<MachinesEvent>(DisplayTimeline);
            NavigateToEventsCommand = new RelayCommand(() => _navigation.NavigateTo(LoggingNavigationView.EventsView));
            NavigateToHomeCommand = new RelayCommand(() => _navigation.NavigateTo(LoggingNavigationView.HomeView));
        }

        private void _eventLogger_NewLog(object sender, MachinesEvent machineEvent)
        {
            InvokeUI(() =>
            {
                _realTimeEvents.Insert(0, machineEvent);
            });
        }

        private async void OnSelectedMachineChanged()
        {
            if (SelectedMachine != null)
            {
                using (_notification.PushTaskItem("Loading machine events..."))
                {
                    try
                    {
                        await Task.Factory.StartNew(() =>
                        {
                            _db = ObservablesContext.CreateDefault();

                            _db.EventTypes.Load();

                            DateTime now = DateTime.UtcNow.AddMonths(-1);

                            _history_events = _db.MachinesEvents.Where(x => x.MachineGuid == SelectedMachine.Guid && x.DateTime > now).Include(x => x.User).Include(x => x.User.Contact).Include(x => x.Machine).ToList();

                            Dates = new ObservableCollection<DateTime>();

                            foreach (var day in _history_events.GroupBy(x => x.DateTime.DayOfYear).Select(x => x.First().DateTime).OrderByDescending(x => x))
                            {
                                Dates.Add(day);
                            }

                            if (Dates.Count > 0)
                            {
                                MinDate = Dates.Min();
                                MaxDate = Dates.Max();
                            }

                            SelectedDate = Dates.FirstOrDefault();
                        });
                    }
                    catch (Exception ex)
                    {
                        LogManager.Log(ex, "Error loading machine events.");
                        _notification.ShowError($"An error occurred while trying to load the selected machine events.\n{ex.FlattenMessage()}");
                    }
                }
            }
        }

        private void OnSelectedDateChanged()
        {
            if (SelectedDate != null && SelectedMachine != null)
            {
                if (IsRealTime)
                {
                    Events = _realTimeEvents;
                }
                else if (_history_events != null)
                {
                    Events = _history_events.Where(x => x.DateTime.DayOfYear == SelectedDate.Date.DayOfYear).OrderByDescending(x => x.DateTime).ToObservableCollection();
                }
            }
        }

        private void OnSelectedEventChanged()
        {
            if (SelectedEvent != null && SelectedEvent.Type != BL.Enumerations.EventTypes.APPLICATION_STARTED && !_dialog_shown)
            {
                _dialog_shown = true;
                _notification.ShowModalDialog<EventDetailsViewVM, EventDetailsView>(new EventDetailsViewVM(SelectedEvent), (x) =>
                {

                }, () =>
                {
                    _dialog_shown = false;
                });
            }
        }

        private void DisplayTimeline(MachinesEvent ev)
        {
            var events = Events.OrderBy(x => x.DateTime).SkipWhile(x => x != ev).Skip(1).TakeWhile(x => x.DateTime > ev.DateTime && x.Type != BL.Enumerations.EventTypes.APPLICATION_STARTED).ToObservableCollection();
            events.Insert(0, ev);

            TimelineViewVM.Initialize(events.ToList());

            _navigation.NavigateTo(LoggingNavigationView.TimelineWrapperView);
        }
    }
}
/* Literal.String.Single */ .highlight .ss { color: #aa6600; background-color: #fff0f0 } /* Literal.String.Symbol */ .highlight .bp { color: #003388 } /* Name.Builtin.Pseudo */ .highlight .fm { color: #0066bb; font-weight: bold } /* Name.Function.Magic */ .highlight .vc { color: #336699 } /* Name.Variable.Class */ .highlight .vg { color: #dd7700 } /* Name.Variable.Global */ .highlight .vi { color: #3333bb } /* Name.Variable.Instance */ .highlight .vm { color: #336699 } /* Name.Variable.Magic */ .highlight .il { color: #0000DD; font-weight: bold } /* Literal.Number.Integer.Long */
/*
 * 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 "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/StopThreadLoadingRequest.pb-c.h"
#include "PMR/ThreadLoading/StopThreadLoadingResponse.pb-c.h"

//#include <stdint.h>
//#include <stdbool.h>

/*    typedef enum
    {
        0 THREAD_LOAD_INIT,
        1 THREAD_LOAD_REDUCE_HEAT,                           //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
        2 THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION,        //USE NOTATION HOW MANY ROTATIONS IN THE DRYER, OR CHECK AGAINST STOPPER. MOVE SLOWLY
        3 THREAD_LOAD_OPEN_COVERS,                           //OPEN DYEING HEAD COVER AND DRYER LID
        4 THREAD_LOAD_LIFT_DANCERS,
        5 THREAD_LOAD_LIFT_ROCKERS,                          //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE
        6 THREAD_LOAD_INITIAL_TENSION,                       //CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND
        7 THREAD_LOAD_CLOSE_ROCKERS,
        8 THREAD_LOAD_CLOSE_DANCERS,                         //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD
        9  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;
    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;
    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);

    typedef struct
    {
        uint32_t LoadArmRounds;
        uint32_t LoadArmBackLash;
    }LoadArmInfoStruc;
    LoadArmInfoStruc LoadArmInfo;
    //char LoadArmPath[50] = "0://SysInfo//LoadArm.cfg";

    //RUN MOTOR TO SWITCH WITH TIMEOUT
    //RUN MOTOR TO BREAK SENSOR
    //RUN MOTOR TO DANCER LOCATION (POOLER GOES TO THE OTHER DIRECTION)

    //RUN MOTOR A FULL CYCLE
    //RUN A MOTOR NUMBER OF STEPS
    //RUN CONTROL FOR A SINGLE DANCER
    bool ThreadLoadingActive(void)
    {
        if ((LoadStages > THREAD_LOAD_INIT)&&(LoadStages < THREAD_LOAD_END))
            return true;
        else
            return false;
    }
    bool InitCalled = 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");
        load.color = BLINK;
        LoadStatus = OK;
        LoadArmInfo.LoadArmBackLash = MCU_E2PromDryerBacklashRead();
        REPORT_MSG(LoadArmInfo.LoadArmBackLash, "Thread Load MCU_E2PromDryerBacklashRead");
        if (LoadArmInfo.LoadArmBackLash > 10)
            LoadArmInfo.LoadArmBackLash = 5;
        //LoadArmInfo.LoadArmRounds = 0xFF;
        //EEPROM_STORAGE_DRYER_CYCLES
        MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmInfo.LoadArmRounds);


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

        //if (LoadArmInfo.LoadArmRounds <= 2)
        //        LoadArmInfo.LoadArmRounds = 20;
        Report("MotorMovetoEncoderPosition",__FILE__,__LINE__,LoadArmInfo.LoadArmRounds,RpWarning,LoadArmInfo.LoadArmBackLash,0);

        /*Fresult = f_mkdir(SW_INFO_DIR);
        if ((Fresult == FR_OK)||(Fresult == FR_EXIST))
        {

            Fresult = FileRead(LoadArmPath, &Bytes, &buffer);
            if (Fresult == FR_OK)
            {
               if (Bytes>=sizeof(LoadArmInfo))
                   memcpy (&LoadArmInfo,buffer,sizeof(LoadArmInfo));
               free (buffer);
            }
        }*/

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

        return OK;
    }
    uint32_t Thread_Load_Reduce_Heat(void)
    {

        //Heaters Off, Dryer Blower Off, Blower Low,
        REPORT_MSG(LoadStages, "Thread Load State Machine step ");
        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.dyeingspeed = 40;
        ProcessParametersClear.dryerbufferlength = ProcessParametersKeep.dryerbufferlength;
        if (HandleProcessParameters(&ProcessParametersClear,false)!= OK)
        {
            LOG_ERROR (LoadStages, "Thread_Load_Init failed");
            strcpy(LoadErrorMsg,"Thread_Load_Init failed");
            LoadStatus = ERROR;
            TryAgain = true;
        }
        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_Start_Position_Callback(uint32_t deviceID, uint32_t BusyFlag)
    {
       // Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);

        /`*NumberOfDrierLoaderCycles--;
        if (NumberOfDrierLoaderCycles)
        {
            Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
            status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize),
                                            MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000);
        }
        else*`/
        {
            Report("Thread_Load_Set_Load_Arm_To_Start_Position_Callback",__FILE__,__LINE__,LoadStages,RpMessage,CallbackCounter,0);
            //storeLoadArmParameters();
            SetMotHome(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);  //set this point as the spool home
            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_Set_Load_Arm_To_Start_Position(void)
    {
        uint32_t numberOfSteps = 0;
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        if (LoadArmInfo.LoadArmRounds != 0xFF)
        {
            //Start Feeder Pid, Rotate Loading Arm Counter Thread Direction X Circles According To Rml. Feeder Speed Is 40
            numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.LoadArmRounds*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
            Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,numberOfSteps,RpMessage,NumberOfDrierLoaderCycles,0);
            NumberOfDrierLoaderCycles = LoadArmInfo.LoadArmRounds;
            status |= MotorMoveWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, (1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize),
                                            numberOfSteps, Thread_Load_Set_Load_Arm_To_Start_Position_Callback,1000);
        }
        else //number of circles is not known - compare to position of the motor to verify location at the stopper
        {
            Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
            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*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep, Thread_Load_Set_Load_Arm_To_Stopper_Callback,LoadArmInfo.LoadArmBackLash,1000);

        }
        //Use Notation How Many Rotations In The Drier, Or Check Against Stopper. Move Slowly
        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!");
                LoadStatus = ERROR;
                ThreadLoadingReport();
                TryAgain = true;
            }
        }

        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)
                {
                    LOG_ERROR(LoadStages,"Load sequence timeout");
                    TimeoutsCounter = 0;
                    load.color = fastBILNK;
                    usnprintf(LoadErrorMsg, 100, "Load sequence timeout %d motor %d",LoadStages, MotorId);
                    LoadStatus = ERROR;
                    ThreadLoadingReport();
                    TryAgain = true;
                }
                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_Center_Head_Rockers(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD].directionthreadwize, 80,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANHEAD], Thread_Load_HomingCallback,10000);
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH].directionthreadwize, 80,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DH_CLEANMECH], Thread_Load_HomingCallback,10000);
        return OK;
    }*/
    uint32_t Thread_Load_Open_Covers(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        //Open Dyeing Head Cover And Dryer Lid
        //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2,
        //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4,
        //DeActivateHeadMagnet();
        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);
        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 0, Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
#ifdef Use_Head_Card
        HeadCard_Actuators_Control(ACTOT, LOW,true);
#endif
        return OK;
    }
    uint32_t Thread_Load_Lift_Dancers(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        /*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);

        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");
        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 8);
        MotorSetMicroStep(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 8);
        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_RLOADING, 70);
        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 70);
        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");
        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, 70);
        MotorSetKvalRun(HARDWARE_MOTOR_TYPE__MOTO_LLOADING, 70);

        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        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");
        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, (5000*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LDANCER1].microstep), Thread_Load_HomingCallback,20000);

        return OK;
    }
    uint32_t Thread_Load_Close_Lids(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        //Close Dyeing Head Cover And Dryer Lid
        //HARDWARE_MOTOR_TYPE__MOTO_DH_LID = 2,
        //HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID = 4,
        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);
        MotorGotoWithCallback(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID, 2, Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LID], Thread_Load_HomingCallback,10000);
#ifdef Use_Head_Card
        HeadCard_Actuators_Control(ACTOT, LOW,false);
#endif
        return OK;
    }
    uint32_t Thread_Load_Resume_Heating(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        ActivateHeadMagnet();

        if (HandleProcessParameters(&ProcessParametersRecover,false)!= OK)
        {
            LOG_ERROR (LoadStages, "Thread_Load_Resume_Heating failed");
        }
        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(void)
    //Jog The Feeder Motor Until The Feeder Dancer Is At Middle Position
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        CallbackCounter++;
        MotorMovetoDancerPosition (HARDWARE_MOTOR_TYPE__MOTO_RDRIVING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RDRIVING].directionthreadwize, 100, FEEDER_DANCER ,true, Thread_Load_Jog_Feeder_To_Middle_Point_Callback,4000);
        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;
    }
    float numberOfSteps = 0;
    float numberOfCycles = 0;
    double DrierPrevLocation = 0;
    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<LoadArmInfo.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,10000);
                MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
                Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0);
            }
        }
        else
        {
            LOG_ERROR(LoadStages,"Load sequence timeout - drier loading");
            TryAgain = true;
            SecondTry = false;
            Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0);
            MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
            TimeoutsCounter = 0;
            load.color = fastBILNK;
            usnprintf(LoadErrorMsg, 100, "Load sequence timeout %d motor %d",LoadStages, HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);
            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();
        }
        return OK;
    }

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

        if (dryerbufferlength)
            LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
        if (LoadArmInfo.LoadArmRounds <= 2)
            LoadArmInfo.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_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmInfo.LoadArmRounds);
        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/**LoadArmInfo.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/4*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius);

        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-400,    Thread_Load_Dryer_Loading_Callback, 10000);
//shlomo
        //Keep Notation How Many Rotations In The Dryer
        //LoadArmInfo.LoadArmBackLash = 0;
        //LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
        //FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath,BIOS_WAIT_FOREVER);
        return OK;
    }
    uint32_t Thread_Load_Jog_ThreadStop(uint32_t index, uint32_t ReadValue)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        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");
        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;
    }
    FPGA_GPI_ENUM screw[2] = {GPI_LS_SCREW_LEFT,GPI_LS_SCREW_RIGHT};
    int Screw_Dir = false;

    uint32_t Thread_Load_switchCallback(uint32_t deviceID, uint32_t BusyFlag)
    {
        if (LoadStages != THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION)
        {
            return OK;
        }
        Screw_Dir = 1-Screw_Dir;
        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 ((abs (temp -DrierPrevLocation)>1000  )&&(ReadValue == NOTBUSY))
        {
            DrierPrevLocation = temp;
            if (LoadStages != THREAD_LOAD_SET_LOAD_ARM_TO_START_POSITION)
            {
                return OK;
            }
            if (numberOfCycles<LoadArmInfo.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
            {
                MotorMovetoEncoderPosition(HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM,Thread_Load_Set_Load_Arm_To_Stopper_Callback,10000);
                MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmInfo.LoadArmRounds-numberOfCycles);
                Report("Store Number of cycles in drier",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0);
            }
        }
        else
        {
            LOG_ERROR(LoadStages,"Load sequence timeout - drier Unloading");
            Report("Store Number of cycles in drier - halted",__FILE__,__LINE__,numberOfCycles,RpMessage,LoadArmInfo.LoadArmRounds,0);
            MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,LoadArmInfo.LoadArmRounds-(numberOfCycles-2));//it takes two cycles to identify a stop of the arm
            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);
            load.color = fastBILNK;
            usnprintf(LoadErrorMsg, 100, "Load sequence timeout %d motor %d",LoadStages, HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM);
            LoadStatus = ERROR;
            TryAgain = true;
            ThreadLoadingReport();
        }
        return OK;
    }
    uint32_t Thread_Load_Dryer_UnLoading(void)
    {
        uint32_t temp;
        REPORT_MSG(LoadStages, "Thread UnLoad State Machine step");
        //LoadArmInfo.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;

        //numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround*LoadArmInfo.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,LoadArmInfo.LoadArmRounds,0);
        //MCU_E2PromProgram(EEPROM_STORAGE_DRYER_CYCLES,numberOfCycles);
//shlomo
        UnloadingStart = msec_millisecondCounter;
        numberOfSteps = MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/**LoadArmInfo.LoadArmRounds*/*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].microstep*MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulleyradius;
        numberOfCycles = 0;

        MCU_E2PromRead(EEPROM_STORAGE_DRYER_CENTER,&temp);
        MCU_E2PromRead(EEPROM_STORAGE_DRYER_CYCLES,&LoadArmInfo.LoadArmRounds);
        DrierPrevLocation = temp;
        Report("Thread_Load_Set_Load_Arm_To_Start_Position",__FILE__,UnloadingStart,DrierPrevLocation,RpMessage, LoadArmInfo.LoadArmRounds,0);
        MotorSetMaxSpeed (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].pulseperround/4*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,LoadArmInfo.LoadArmBackLash,1000);
        MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
                               numberOfSteps-400,    Thread_Load_Dryer_UnLoading_Callback, 10000);

        Screw_Dir = 1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_SCREW].directionthreadwize;
        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
        //LoadArmInfo.LoadArmRounds = (int)dryerbufferlength;
        load.color = BLINK;
        return OK;
    }

    uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue)
    {
        REPORT_MSG(ReadValue,"ThreadLoadStateMachine");
        ThreadLoaded();
        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())
    {
        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);
    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
        {
            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;
        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__,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__,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__,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.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 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;
        LoadArmInfo.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();
    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;

}