using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using Tango.Core.Helpers;
using Tango.BL.Entities;
using Tango.Logging;
using Tango.MachineStudio.Common.Modules;
using Tango.MachineStudio.Common.Navigation;
using Tango.MachineStudio.Common.Notifications;
using Tango.MachineStudio.Common.StudioApplication;
using Tango.SharedUI;
using Tango.BL;
using Tango.MachineStudio.Common.EventLogging;
using Tango.BL.Enumerations;
using Tango.MachineStudio.UI.TFS;
using Tango.MachineStudio.Common;
using Tango.MachineStudio.Common.Web;
using Tango.Core.DI;
using Tango.Settings;
using Tango.Core;

namespace Tango.MachineStudio.UI.ViewModels
{
    /// <summary>
    /// Represents the Machine Studio loading view, view model.
    /// </summary>
    /// <seealso cref="Tango.SharedUI.ViewModel" />
    public class LoadingViewVM : StudioViewModel
    {
        private INotificationProvider _notificationProvider;
        private TeamFoundationServiceExtendedClient _tfs;
        private INavigationManager _navigationManager;
        private IStudioModuleLoader _studioModuleLoader;
        private IEventLogger _eventLogger;
        private LogManager logManager = LogManager.Default;
        public IStudioApplicationManager ApplicationManager { get; set; }

        private bool _isLoading;
        public bool IsLoading
        {
            get { return _isLoading; }
            set { _isLoading = value; RaisePropertyChangedAuto(); }
        }

        private String _status;
        public String Status
        {
            get { return _status; }
            set { _status = value; RaisePropertyChangedAuto(); }
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="LoadingViewVM"/> class.
        /// </summary>
        /// <param name="navigationManager">The navigation manager.</param>
        /// <param name="studioModuleLoader">The studio module loader.</param>
        /// <param name="notificationProvider">The notification provider.</param>
        public LoadingViewVM(IStudioApplicationManager applicationManager, INavigationManager navigationManager, IStudioModuleLoader studioModuleLoader, INotificationProvider notificationProvider, IEventLogger eventLogger, TeamFoundationServiceExtendedClient teamFoundationClient)
        {
            Status = "Loading, please wait...";
            _tfs = teamFoundationClient;
            _eventLogger = eventLogger;
            ApplicationManager = applicationManager;
            _navigationManager = navigationManager;
            _studioModuleLoader = studioModuleLoader;
            _notificationProvider = pre { line-height: 125%; }
td.linenos .normal { color: inherit; background-color: transparent; padding-left: 5px; padding-right: 5px; }
span.linenos { color: inherit; background-color: transparent; padding-left: 5px; padding-right: 5px; }
td.linenos .special { color: #000000; background-color: #ffffc0; padding-left: 5px; padding-right: 5px; }
span.linenos.special { color: #000000; background-color: #ffffc0; padding-left: 5px; padding-right: 5px; }
.highlight .hll { background-color: #ffffcc }
.highlight .c { color: #888888 } /* Comment */
.highlight .err { color: #a61717; background-color: #e3d2d2 } /* Error */
.highlight .k { color: #008800; font-weight: bold } /* Keyword */
.highlight .ch { color: #888888 } /* Comment.Hashbang */
.highlight .cm { color: #888888 } /* Comment.Multiline */
.highlight .cp { color: #cc0000; font-weight: bold } /* Comment.Preproc */
.highlight .cpf { color: #888888 } /* Comment.PreprocFile */
.highlight .c1 { color: #888888 } /* Comment.Single */
.highlight .cs { color: #cc0000; font-weight: bold; background-color: #fff0f0 } /* Comment.Special */
.highlight .gd { color: #000000; background-color: #ffdddd } /* Generic.Deleted */
.highlight .ge { font-style: italic } /* Generic.Emph */
.highlight .ges { font-weight: bold; font-style: italic } /* Generic.EmphStrong */
.highlight .gr { color: #aa0000 } /* Generic.Error */
.highlight .gh { color: #333333 } /* Generic.Heading */
.highlight .gi { color: #000000; background-color: #ddffdd } /* Generic.Inserted */
.highlight .go { color: #888888 } /* Generic.Output */
.highlight .gp { color: #555555 } /* Generic.Prompt */
.highlight .gs { font-weight: bold } /* Generic.Strong */
.highlight .gu { color: #666666 } /* Generic.Subheading */
.highlight .gt { color: #aa0000 } /* Generic.Traceback */
.highlight .kc { color: #008800; font-weight: bold } /* Keyword.Constant */
.highlight .kd { color: #008800; font-weight: bold } /* Keyword.Declaration */
.highlight .kn { color: #008800; font-weight: bold } /* Keyword.Namespace */
.highlight .kp { color: #008800 } /* Keyword.Pseudo */
.highlight .kr { color: #008800; font-weight: bold } /* Keyword.Reserved */
.highlight .kt { color: #888888; font-weight: bold } /* Keyword.Type */
.highlight .m { color: #0000DD; font-weight: bold } /* Literal.Number */
.highlight .s { color: #dd2200; background-color: #fff0f0 } /* Literal.String */
.highlight .na { color: #336699 } /* Name.Attribute */
.highlight .nb { color: #003388 } /* Name.Builtin */
.highlight .nc { color: #bb0066; font-weight: bold } /* Name.Class */
.highlight .no { color: #003366; font-weight: bold } /* Name.Constant */
.highlight .nd { color: #555555 } /* Name.Decorator */
.highlight .ne { color: #bb0066; font-weight: bold } /* Name.Exception */
.highlight .nf { color: #0066bb; font-weight: bold } /* Name.Function */
.highlight .nl { color: #336699; font-style: italic } /* Name.Label */
.highlight .nn { color: #bb0066; font-weight: bold } /* Name.Namespace */
.highlight .py { color: #336699; font-weight: bold } /* Name.Property */
.highlight .nt { color: #bb0066; font-weight: bold } /* Name.Tag */
.highlight .nv { color: #336699 } /* Name.Variable */
.highlight .ow { color: #008800 } /* Operator.Word */
.highlight .w { color: #bbbbbb } /* Text.Whitespace */
.highlight .mb { color: #0000DD; font-weight: bold } /* Literal.Number.Bin */
.highlight .mf { color: #0000DD; font-weight: bold } /* Literal.Number.Float */
.highlight .mh { color: #0000DD; font-weight: bold } /* Literal.Number.Hex */
.highlight .mi { color: #0000DD; font-weight: bold } /* Literal.Number.Integer */
.highlight .mo { color: #0000DD; font-weight: bold } /* Literal.Number.Oct */
.highlight .sa { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Affix */
.highlight .sb { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Backtick */
.highlight .sc { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Char */
.highlight .dl { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Delimiter */
.highlight .sd { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Doc */
.highlight .s2 { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Double */
.highlight .se { color: #0044dd; background-color: #fff0f0 } /* Literal.String.Escape */
.highlight .sh { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Heredoc */
.highlight .si { color: #3333bb; background-color: #fff0f0 } /* Literal.String.Interpol */
.highlight .sx { color: #22bb22; background-color: #f0fff0 } /* Literal.String.Other */
.highlight .sr { color: #008800; background-color: #fff0ff } /* Literal.String.Regex */
.highlight .s1 { color: #dd2200; background-color: #fff0f0 } /* 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 "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 "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 <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_CENTER_HEAD_ROCKERS,  //puthead cleaning rockers to middle position
        4 THREAD_LOAD_OPEN_COVERS,  //OPEN DYEING HEAD COVER AND DRYER LID
        5 THREAD_LOAD_LIFT_DANCERS,
        6 THREAD_LOAD_LIFT_ROCKERS, //MACHINE IS READY. SEND MESSAGE, START TIMER TO CLOSE LIDS, WAIT FOR OPERATOR RESPONSE
        7 THREAD_LOAD_INITIAL_TENSION,//CHECK SPOOL PRESENCERUN WINDER UNTIL BREAK SENSOR IS IDENTIFIEING MOVEMENT FOR A SECOND
        8 THREAD_LOAD_CLOSE_ROCKERS,
        9 THREAD_LOAD_CLOSE_DANCERS, //SEND DANCER MOTORS TO PRESET LOCATION, CHECK THAT THE DANCERS ARE ON THE THREAD
        10 THREAD_LOAD_CLOSE_LIDS,
        11 THREAD_LOAD_RESUME_HEATING,
        12 THREAD_LOAD_JOG_FEEDER_TO_MIDDLE_POINT,//JOG THE FEEDER MOTOR UNTIL THE FEEDER DANCER IS AT MIDDLE POSITION
        13 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
        14 THREAD_LOAD_JOG_THREAD, //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
        15 THREAD_LOAD_END
    }THREAD_LOAD_STAGES_ENUM;*/
    THREAD_LOAD_STAGES_ENUM LoadStages = THREAD_LOAD_INIT;
    ProcessParameters ProcessParametersClear,ProcessParametersRecover;
    uint8_t NumberOfDrierLoaderCycles = 0;
    uint32_t status = OK;
    uint32_t ControlId = 0xFF;
    uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM LoadStages);
    uint32_t ThreadLoadControlCBFunction(uint32_t index, uint32_t ReadValue);

    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
    uint32_t Thread_Load_Init(void)
    {
        void* buffer = NULL;
        uint32_t Bytes = 0;
        FRESULT Fresult = FR_OK;
        REPORT_MSG(LoadStages, "Thread Load State Machine step");

        LoadArmInfo.LoadArmBackLash = 10;
        LoadArmInfo.LoadArmRounds = 0xFF;
        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.LoadArmBackLash,RpMessage,LoadArmInfo.LoadArmRounds,0);
        memcpy (&ProcessParametersRecover,&ProcessParametersKeep,sizeof(ProcessParameters));
        //NumberOfDrierLoaderCycles = loadLoadArmParameters();
        LoadStages++;
        ThreadLoadStateMachine(LoadStages);

        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;
        if (HandleProcessParameters(&ProcessParametersClear)!= OK)
        {
            LOG_ERROR (LoadStages, "Thread_Load_Init failed");
        }
        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,NumberOfDrierLoaderCycles,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)
    {
//        Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
        Report("Thread_Load_Set_Load_Arm_To_Stopper_Callback",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,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/2, 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;
    }
    uint8_t CallbackCounter = 0;
    uint8_t TimeoutsCounter = 0;
    uint32_t Thread_Load_HomingCallback(uint32_t MotorId, uint32_t ReadValue)
    {
        Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
        REPORT_MSG(MotorId, "Thread_Load_HomingCallback Motor Id");
        if (CallbackCounter)
        {
            CallbackCounter--;
            if (ReadValue != LIMIT)
            {
                //returned with a timeout
                TimeoutsCounter++;
            }
            if (CallbackCounter == 0)
            {
                if (TimeoutsCounter)
                {
                    LOG_ERROR(LoadStages,"Load sequence timeout");
                    TimeoutsCounter = 0;
                }
                else
                {
                    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_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, 200,  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, 200,  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,4000);
        return OK;
    }
    uint32_t Thread_Load_Lift_Dancers(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        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,20000);

        return OK;
    }
    uint32_t Thread_Load_Lift_Rockers(void)
    //Machine Is Ready. Send Message, Start Timer To Close Lids, Wait For Operator Response
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 300,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,12000);
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 300,  Motor_Id_to_LS_IdUp[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,12000);

        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");
        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)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_RLOADING,1-MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_RLOADING].directionthreadwize, 300,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_RLOADING], Thread_Load_HomingCallback,12000);
        CallbackCounter++;
        MotorMovetoLimitSwitch (HARDWARE_MOTOR_TYPE__MOTO_LLOADING,MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_LLOADING].directionthreadwize, 300,  Motor_Id_to_LS_IdDown[HARDWARE_MOTOR_TYPE__MOTO_LLOADING], Thread_Load_HomingCallback,12000);

        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,4000);
        CallbackCounter++;
        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,10000);

        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,4000);

        return OK;
    }
    uint32_t Thread_Load_Resume_Heating(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        ActivateHeadMagnet();

        if (HandleProcessParameters(&ProcessParametersRecover)!= 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, 200, FEEDER_DANCER ,true, Thread_Load_HomingCallback,10000);
        return OK;
    }

    uint32_t Thread_Load_Dryer_Loading_Callback(uint32_t MotorId, uint32_t ReadValue)
    {
        Report("Thread Load State Machine Callback.",__FILE__,__LINE__,LoadStages,RpMessage,NumberOfDrierLoaderCycles,0);
        REPORT_MSG(MotorId, "Thread_Load_Dryer_Loading_Callback Motor Id");
        if(ControlId != 0xFF)
        {
            MotorStop(ThreadMotorIdToMotorId[FEEDER_MOTOR],Hard_Hiz);
            RemoveControlCallback(ControlId, ThreadLoadControlCBFunction );
            ControlId = 0xFF;
        }
        if (CallbackCounter)
        {
            CallbackCounter--;
            if (CallbackCounter == 0)
            {
                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(void)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        LoadArmInfo.LoadArmRounds = 20;
        uint32_t numberOfSteps = 0;
        //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;

        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[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);

        ControlId = AddControlCallback(ThreadLoadControlCBFunction, eOneMillisecond,Control_Read_Dancer_Position,(IfTypeThread*0x100+FEEDER_MOTOR),FEEDER_DANCER,FEEDER_MOTOR);
        CallbackCounter++;
        MotorMoveWithCallback (HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM, MotorsCfg[HARDWARE_MOTOR_TYPE__MOTO_DRYER_LOADARM].directionthreadwize,
                               numberOfSteps,    Thread_Load_Dryer_Loading_Callback, 100000);

        //Keep Notation How Many Rotations In The Dryer
        //LoadArmInfo.LoadArmBackLash = 0;
        //LoadArmInfo.LoadArmRounds = 0xFF;
        FileWrite(&LoadArmInfo, sizeof(LoadArmInfo),LoadArmPath);
        return OK;
    }
    uint32_t Thread_Load_Jog_ThreadStop(uint32_t index, uint32_t ReadValue)
    {
        REPORT_MSG(LoadStages, "Thread Load State Machine step");
        SafeRemoveControlCallback(ControlId, Thread_Load_Jog_ThreadStop );
        ControlId = 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);
        ControlId = AddControlCallback(Thread_Load_Jog_ThreadStop, eOneSecond*5,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");
        return OK;
    }

    uint32_t ThreadLoadStateMachine( THREAD_LOAD_STAGES_ENUM ReadValue)
    {
        REPORT_MSG(ReadValue,"ThreadLoadStateMachine");
        switch (ReadValue)
        {
        case THREAD_LOAD_INIT:
            //LoadStages++;
            Thread_Load_Init();
            break;
        case THREAD_LOAD_REDUCE_HEAT:  //HEATERS OFF, DRYER BLOWER OFF, BLOWER LOW,
            //LoadStages++;
            Thread_Load_Reduce_Heat();
            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();
            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_RESUME_HEATING:
            //LoadStages++;
            Thread_Load_Resume_Heating();
            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_JOG_THREAD: //JOG THREAD SHORTLY TO MAKE SURE SPOOL IS RUNNING. REPORT END OF LOADING
            //LoadStages++;
            Thread_Load_Jog_Thread();
            break;
        case THREAD_LOAD_END:
            Thread_Load_End();
            break;
        default:
            LOG_ERROR(LoadStages,"ERROR IN LOAD STATE MACHINE");
            break;
    }
    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;

    DancerId = FEEDER_DANCER;//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 (IfIndex>>8 != IfTypeThread)
    {
        LOG_ERROR (IfIndex, "Wrong  Interface type");
        return 0xFFFFFFFF;
    }
    index = IfIndex&0xFF;
    /*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 = AdvancedPIDAlgorithmCalculation((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 (abs(calculated_speed-CurrentControlledSpeed[index])> MotorControlConfig[index].m_ingnoreValue)
    {
        CurrentControlledSpeed[index] = calculated_speed;
        MotorSetSpeed(ThreadMotorIdToMotorId[index], calculated_speed);
    }
 return OK;
}
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);
}