#include <stdbool.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>

#include "inc/hw_memmap.h"
#include "driverlib/gpio.h"
#include "driverlib/pin_map.h"
#include "driverlib/ssi.h"
#include "driverlib/sysctl.h"
#include "utils/uartstdio.h"

//#include "graphics_adapter.h"
#include "drivers/FPGA/Moters_Driver/L6470.h"
#include "PMR/Hardware/HardwareMotor.pb-c.h"

//#include "drivers/FPGA/FPGA_Comm.h"

#include <driverlib/rom.h>
#include <driverlib/rom_map.h>
#include <DataDef.h>
#include "drivers/Motors/Motor.h"

void temp_init_spi2();
void temp_setup();

void Avi_test_get_speed();
//uint32_t Get_Param_Status(byte param);
uint32_t Get_and_Clear_Status();

uint32_t Get_Param(byte param);
unsigned long MaxSpdCalc(float stepsPerSec);

extern unsigned long Run_Value ;
extern unsigned long Mov_Value ;
extern unsigned long Pos_Value;
extern bool Direction ;
extern unsigned long Time_2_Change_Direction ;
extern bool Display_Tx_ON_LCD;
extern bool Display_Rx_on_LCD;
extern unsigned long Init_MicroStep ;
extern unsigned long Init_Acc;
extern unsigned long Init_Dec;
extern bool TestBool_1;
extern bool TestBool_2;
extern bool TestBool_3;
extern bool TestBool_4;
extern bool TestBool_5;
extern unsigned long TestUint32_1;
extern unsigned long TestUint32_2;
extern unsigned long TestUint32_3;
extern unsigned long TestUint32_4;
extern unsigned long TestUint32_5;
extern unsigned long TestUint32_6;

extern unsigned char Stop_Command;

extern int Global_EVB_Motor_Id;

//#define SPI_EVA_LCD_ENABLED


void SPI2_Init()
{
#ifdef EVALUATION_BOARD

    #if defined(TARGET_IS_TM4C129_RA0) ||                                         \
    defined(TARGET_IS_TM4C129_RA1) ||                                         \
    defined(TARGET_IS_TM4C129_RA2)
        uint32_t ui32SysClock;
    #endif



    //
    // Set the clocking to run directly from the external crystal/oscillator.
    // TODO: The SYSCTL_XTAL_ value must be changed to match the value of the
    // crystal on your board.
    //
    #if defined(TARGET_IS_TM4C129_RA0) ||                                         \
    defined(TARGET_IS_TM4C129_RA1) ||                                         \
    defined(TARGET_IS_TM4C129_RA2)
        /*ui32SysClock = SysCtlClockFreqSet((SYSCTL_XTAL_25MHZ |
                                       SYSCTL_OSC_MAIN |
                                       SYSCTL_USE_OSC), 25000000);*/
        ui32SysClock = MAP_SysCtlClockFreqSet((SYSCTL_XTAL_25MHZ |
                                               SYSCTL_OSC_MAIN | SYSCTL_USE_PLL |
                                               SYSCTL_CFG_VCO_480), 120000000);
    #else
        SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN |
                   SYSCTL_XTAL_16MHZ);
    #endif
#ifdef SPI_EVA_LCD_ENABLED
    writeLine("SSI:");
    writeLine("  Mode: SPI");
    writeLine("  Data: 8-bit");
#endif

        // The SSI2 peripheral must be enabled for use.
        //
        SysCtlPeripheralReset(SYSCTL_PERIPH_SSI2);
        SysCtlPeripheralEnable(SYSCTL_PERIPH_SSI2);

        //
        // For this example SSI2 is used with PortG[7:4].  GPIO port G needs to be
        // enabled so these pins can be used.
        //
        SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG);



        /* Configure pad settings */  // AVI
        GPIOPadConfigSet(GPIO_PORTG_BASE,
                GPIO_PIN_7 | GPIO_PIN_5,
                GPIO_STRENGTH_4MA, GPIO_PIN_TYPE_STD);


        GPIOPadConfigSet(GPIO_PORTG_BASE,
                GPIO_PIN_4,
                GPIO_STRENGTH_4MA, GPIO_PIN_TYPE_STD_WPU);

        GPIOPadConfigSet(GPIO_PORTG_BASE,
                GPIO_PIN_6,
                GPIO_STRENGTH_4MA, GPIO_PIN_TYPE_STD);


        //
        // Configure the pin muxing for SSI2 functions on port G4, G5, G6 and G7.
        // This step is not necessary if your part does not support pin muxing.


        #define GPIO_PG4_SSI2TX GPIO_PG4_SSI2XDAT1 // AVI
        #define GPIO_PG5_SSI2RX GPIO_PG5_SSI2XDAT0 // AVI

        //
        GPIOPinConfigure(GPIO_PG7_SSI2CLK);
        GPIOPinConfigure(GPIO_PG6_SSI2FSS);
        GPIOPinConfigure(GPIO_PG5_SSI2RX);
        GPIOPinConfigure(GPIO_PG4_SSI2TX);

        //
        // Configure the GPIO settings for the SSI pins.  This function also gives
        // control of these pins to the SSI hardware.  Consult the data sheet to
        // see which functions are allocated per pin.
        // The pins are assigned as follows:
        //      PG4 - SSI2Tx
        //      PG5 - SSI2Rx
        //      PG6 - SSI2Fss
        //      PG7 - SSI2CLK
        //
        GPIOPinTypeSSI(GPIO_PORTG_BASE, GPIO_PIN_7 | GPIO_PIN_6 | GPIO_PIN_5 |
                GPIO_PIN_4);

        //
        // Configure and enable the SSI2 port for SPI Master mode.
        //
        #if defined(TARGET_IS_TM4C129_RA0) ||                                         \
        defined(TARGET_IS_TM4C129_RA1) ||                                         \
        defined(TARGET_IS_TM4C129_RA2)
            SSIConfigSetExpClk(SSI2_BASE, ui32SysClock, SSI_FRF_MOTO_MODE_0,
                           SSI_MODE_MASTER, 1000000, 8);
        #else
            SSIConfigSetExpClk(SSI2_BASE, SysCtlClockGet(), SSI_FRF_MOTO_MODE_0,
                           SSI_MODE_MASTER, 1000000, 8);
        #endif

        SSIAdvModeSet(SSI2_BASE,SSI_ADV_MODE_READ_WRITE);//data is written to and read from the slave // AVI
        //
        // Enable the SSI2 module.
        //
        SSIEnable(SSI2_BASE);
        //----------------------------------------------------------------
#endif
}

void init_BUSY_Pin(void)
{
#ifdef EVALUATION_BOARD
    MAP_GPIOPinTypeGPIOInput(GPIO_PORTG_BASE, GPIO_PIN_0); // Enable pin for GPIOInput
    SysCtlDelay(10000);
    GPIOPadConfigSet(GPIO_PORTG_BASE,GPIO_PIN_0,GPIO_STRENGTH_2MA,GPIO_PIN_TYPE_STD_WPU );//Configure GPIO PULL UP resistors. NOTE: does not work if ROM_GPIOPinTypeGPIOInput() isnt called before.
#endif
}

bool Check_SPI_Busy(void)
{
#ifdef EVALUATION_BOARD

    if(ROM_GPIOPinRead(GPIO_PORTG_BASE, GPIO_PIN_0) == GPIO_PIN_0)
        return NOTBUSY;
    else
        return BUSY;
#else
    return NOTBUSY;
#endif
}

bool Polling_SPI_Busy(void)
{
#ifdef EVALUATION_BOARD
    uint32_t timeout = 10000; // todo - check this value
    char temp;

    do
    {
        timeout--;
        temp = ROM_GPIOPinRead(GPIO_PORTG_BASE, GPIO_PIN_0);
     } while(( temp != GPIO_PIN_0) && (timeout));

    if(temp != GPIO_PIN_0)
        return BUSY;//After time out
    else
        return NOTBUSY;
#else
    return NOTBUSY;
#endif
}

void SPI_TX(unsigned int SSI_BASE,uint32_t *pui32DataTx, uint32_t *pui32DataRx, unsigned int Num_SSI_Data)
{
    uint32_t ui32Index;
    //
    // Read any residual data from the SSI port.  This makes sure the receive
    // FIFOs are empty, so we don't read any unwanted junk.  This is done here
    // because the SPI SSI mode is full-duplex, which allows you to send and
    // receive at the same time.  The SSIDataGetNonBlocking function returns
    // "true" when data was returned, and "false" when no data was returned.
    // The "non-blocking" function checks if there is any data in the receive
    // FIFO and does not "hang" if there isn't.
    //
    while(SSIDataGetNonBlocking(SSI_BASE, &pui32DataRx[0]))
    {
    }


    //
    // Display indication that the SSI is transmitting data.
    //
    //UARTprintf("Sent:\n  ");
#ifdef SPI_EVA_LCD_ENABLED
    writeLine("Sent: ");
#endif
    //
    // Send 3 bytes of data.
    //

    for(ui32Index = 0; ui32Index < Num_SSI_Data; ui32Index++)
    {
        #ifdef SPI_EVA_LCD_ENABLED
#ifdef SPI_EVA_LCD_ENABLED
            // Display the data that SSI is transferring.
            writeFloat (pui32DataTx[ui32Index]);
            writeString("   ");
#endif
        #endif


        //
        // Send the data using the "blocking" put function.  This function
        // will wait until there is room in the send FIFO before returning.
        // This allows you to assure that all the data you send makes it into
        // the send FIFO.
        //
        SSIDataPut(SSI_BASE, pui32DataTx[ui32Index]);
    }

    //
    // Wait until SSI2 is done transferring all the data in the transmit FIFO.
    //
//    while(SSIBusy(SSI_BASE))
//    {
//    }

    uint32_t timeout = SSI_SPI_TIMEOUT;

    while(SSIBusy(SSI_BASE))
    {
       timeout--;
        if(timeout == 0)
        {
            return ;
        }
    }
}

void SPI_RX(unsigned int SSI_Bsae, uint32_t* RxBuf, uint32_t NumOfWords)
{

/*
    uint32_t ui32Index;
#define DUMMY_BYTE                  0x00

    NumOfWords = 2;

    writeLine("Starting Read Operations...");

    for(ui32Index=0;ui32Index<NumOfWords-1;ui32Index++)
    {
        SSIDataPut(SSI_Bsae,DUMMY_BYTE);
        SSIDataGet(SSI_Bsae,&RxBuf[ui32Index]);
    }
    SSIAdvDataPutFrameEnd(SSI_Bsae,DUMMY_BYTE);
    SSIDataGet(SSI_Bsae,&RxBuf[NumOfWords-1]);


    writeFloat (RxBuf[0]);
    writeFloat (RxBuf[1]);
//
    writeLine("Read Completed...");

*/

    uint32_t ui32Index;
#ifdef SPI_EVA_LCD_ENABLED
    // Display indication that the SSI is receiving data.
    writeLine("Received: ");
#endif
    //
    // Receive 3 bytes of data.

//    while(SSIBusy(SSI_Bsae))
//    {
//    }
    //
    unsigned char Num_SSI_Data = 1;
    uint32_t pui32DataRx[1];

    for(ui32Index = 0; ui32Index < Num_SSI_Data; ui32Index++)
    {
        //
        // Receive the data using the "blocking" Get function. This function
        // will wait until there is data in the receive FIFO before returning.
        //
        SSIDataGet(SSI_Bsae, &pui32DataRx[ui32Index]);

        //#ifdef SPI_EVA_LCD_ENABLED
//        writeLine("SPI_RX: ");
//        writeFloat (pui32DataRx[ui32Index]);
//        writeString("   ");
        //#endif


        //
        // Since we are using 8-bit data, mask off the MSB.
        //
        pui32DataRx[ui32Index] &= 0x00FF;

        //
        // Display the data that SSI2 received.
        //
        //UARTprintf("'%c' ", pui32DataRx[ui32Index]);

        //temp[0] = pui32DataRx[ui32Index];
        //writeString(temp);
//#ifdef SPI_EVA_LCD_ENABLED
//        writeFloat (pui32DataRx[ui32Index]);
//        writeString("   ");
//#endif
    }

    RxBuf[0] = pui32DataRx[0];

    //
    // Return no errors
    //


}

///////////////

//void setup(MotorDriverConfigStruc *MotorConfig)
//{
//
//}

void setup(MotorDriverConfigStruc *MotorConfig)
//void setup()
{

    unsigned long read_status, NOP;

    //  // Standard serial port initialization for debugging.
    //Serial.begin(9600);
    //Serial.setTimeout(50);
    //delay(500);

    //if (GetParam(x_CONFIG) == 0x2E88) // the default value of the CONFIG register
        // The following function calls are for this demo application-
        //  you will need to adjust them for your particular
        //  application, and you may need to configure additional
        //  registers.

        // First, let's set the step mode register:
        //   - x_SYNC_EN controls whether the BUSY/SYNC pin reflects
        //      the step frequency or the BUSY status of the chip. We
        //      want it to be the BUSY status.
        //   - x_STEP_SEL_x is the microstepping rate- we'll go full
        //      step.
        //   - x_SYNC_SEL_x is the ratio of (micro)steps to toggles
        //      on the BUSY/SYNC pin (when that pin is used for SYNC).
        //      Make it 1:1, despite not using that pin.


        //Step mode can only be changed when bridges are disabled:
        HardHiZ();
        read_status = Get_Param(x_STATUS);

        uint32_t timeout = HIZ_TIMEOUT;

        while((read_status & x_STATUS_HIZ) != x_STATUS_HIZ)
        {
            NOP++;

           timeout--;
            if(timeout == 0)
            {
                return ;
            }

        }

        //while(SSIBusy(SSI2_BASE)){};
        timeout = SSI_SPI_TIMEOUT;

        while(SSIBusy(SSI2_BASE))
        {
           timeout--;
            if(timeout == 0)
            {
                return ;
            }
        }

        if(MotorConfig->microstep !=0)
        {
            SetParam(x_STEP_MODE,
                !x_SYNC_EN |
                MotorConfig->microstep |
                x_SYNC_SEL_1);
        }
        else
        {


                if( Global_EVB_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RLOADING)//Roker
                {
                    SetParam(x_STEP_MODE,
                        !x_SYNC_EN |
                    x_STEP_SEL_1_2 | //for QSH2818-32-07-006 roker
                    x_SYNC_SEL_1);
                }
                else //if( Global_EVB_Motor_Id == MOTOR_RDRIVING)fidder
                {
                    SetParam(x_STEP_MODE,
                        !x_SYNC_EN |
                    x_STEP_SEL_1 |//for QSH4218-51-10-049 fidder
                    x_SYNC_SEL_1);
                }


        }

    // Configure the MAX_SPEED register- this is the maximum number
    //  of (micro)steps per second allowed. You'll want to mess
    //  around with your desired application to see how far you can
    //  push it before the motor starts to slip. The ACTUAL
    //  parameter passed to this function is in steps/tick;
    //  MaxSpdCalc() will convert a number of steps/s into an
    //  appropriate value for this function. Note that for any move
    //  or goto type function where no speed is specified, this
    //  value will be used.
    //while(SSIBusy(SSI2_BASE)){};
        timeout = SSI_SPI_TIMEOUT;

        while(SSIBusy(SSI2_BASE))
        {
           timeout--;
            if(timeout == 0)
            {
                return ;
            }
        }

    if(MotorConfig->maxfrequency !=0)
    {
        SetParam(x_MAX_SPEED, MaxSpdCalc(MotorConfig->maxfrequency));
    }
    else
    {
        SetParam(x_MAX_SPEED, MaxSpdCalc(50000));
    }
/*    if(request->set_max_speed)
    {
        SetParam(x_MAX_SPEED, MaxSpdCalc(request->max_speed));
    }
    else
    {
        SetParam(x_MAX_SPEED, MaxSpdCalc(2500));
    }
*/

    // Configure the FS_SPD register- this is the speed at which the
    //  driver ceases microstepping and goes to full stepping.
    //  FSCalc() converts a value in steps/s to a value suitable for
    //  this register; to disable full-step switching, you can pass
    //  0x3FF to this register.
    //while(SSIBusy(SSI2_BASE)){};
    timeout = SSI_SPI_TIMEOUT;

    while(SSIBusy(SSI2_BASE))
    {
       timeout--;
        if(timeout == 0)
        {
            return ;
        }
    }

    SetParam(x_FS_SPD, FSCalc(0x3FF));//h3FF (max.) the system always works in microstepping mode

    //STALL threshold
   // while(SSIBusy(SSI2_BASE)){};
    timeout = SSI_SPI_TIMEOUT;

    while(SSIBusy(SSI2_BASE))
    {
       timeout--;
        if(timeout == 0)
        {
            return ;
        }
    }

    //RMS=sqrt((I*I)*dutycycle)
    SetParam(x_STALL_TH, 0x1F);//1A Reset Value 0x40 (2.03A) ---------------------


    // Configure the acceleration rate, in steps/tick/tick. There is
    //  also a DEC register; both of them have a function (AccCalc()
    //  and DecCalc() respectively) that convert from steps/s/s into
    //  the appropriate value for the register. Writing ACC to 0xfff
    //  sets the acceleration and deceleration to 'infinite' (or as
    //  near as the driver can manage). If ACC is set to 0xfff, DEC
    //  is ignored. To get infinite deceleration without infinite
    //  acceleration, only hard stop will work.

   //SoftStop(); // ACC + DEC writable only when motor is stopped
    //SysCtlDelay(10000000);
    //while(SSIBusy(SSI2_BASE)){};

    timeout = SSI_SPI_TIMEOUT;

    while(SSIBusy(SSI2_BASE))
    {
       timeout--;
        if(timeout == 0)
        {
            return ;
        }
    }

    HardHiZ();
    read_status = Get_Param(x_STATUS);
    while((read_status & x_STATUS_HIZ) != x_STATUS_HIZ)
    {
        SysCtlDelay(1000000);
        read_status = Get_Param(x_STATUS);
    }
    // ACC + DEC writable only when motor is stopped

    if(MotorConfig->maxchangeslope != 0)
        SetParam(x_ACC, MotorConfig->maxchangeslope);//roll-over after 0x7F - 7 bit ??? (should be 12 bit) AVI
    else
        SetParam(x_ACC, 0x0FF);
/*    if(request->set_acc)
    {
        SetParam(x_ACC, request->acc);//roll-over after 0x7F - 7 bit ??? (should be 12 bit) AVI
    }
    else
    {
        SetParam(x_ACC, 0x8A);//roll-over after 0x7F - 7 bit ??? (should be 12 bit) AVI
    }*/

    //while(SSIBusy(SSI2_BASE)){};
     timeout = SSI_SPI_TIMEOUT;

    while(SSIBusy(SSI2_BASE))
    {
       timeout--;
        if(timeout == 0)
        {
            return ;
        }
    }

    if(MotorConfig->maxchangeslope != 0)
        SetParam(x_DEC, MotorConfig->maxchangeslope);//roll-over after 0x7F - 7 bit ??? (should be 12 bit) AVI
    else
        SetParam(x_DEC, 0x0FF);


/*    if(request->set_dec)
    {
        SetParam(x_DEC, request->dec);//roll-over after 0x7F - 7 bit ??? (should be 12 bit) AVI
    }
    else
    {
        SetParam(x_DEC, 0x8A);//roll-over after 0x7F - 7 bit ??? (should be 12 bit) AVI
    }
*/

    if(MotorConfig->configword!=0)
    {
        //while(SSIBusy(SSI2_BASE)){};
        timeout = SSI_SPI_TIMEOUT;

        while(SSIBusy(SSI2_BASE))
        {
           timeout--;
            if(timeout == 0)
            {
                return ;
            }
        }

        SetParam(x_CONFIG, MotorConfig->configword);//roll-over after 0x7F - 7 bit ??? (should be 12 bit) AVI
    }

    // Configure the overcurrent detection threshold. The constants
    //  for this are defined in the L6470.h file.
    //while(SSIBusy(SSI2_BASE)){};
    timeout = SSI_SPI_TIMEOUT;

        while(SSIBusy(SSI2_BASE))
        {
           timeout--;
            if(timeout == 0)
            {
                return ;
            }
        }
	if(MotorConfig->overcurrentthreshold!=0)
    {
        SetParam(x_OCD_TH, MotorConfig->overcurrentthreshold);
    }
	else
    	SetParam(x_OCD_TH, x_OCD_TH_3000mA);

    // Set up the CONFIG register as follows:
    //  PWM frequency divisor = 1
    //  PWM frequency multiplier = 2 (62.5kHz PWM frequency)
    //  Slew rate is 290V/us
    //  Do NOT shut down bridges on overcurrent
    //  Disable motor voltage compensation
    //  Hard stop on switch low
    //  16MHz internal oscillator, nothing on output

    //Get_Param(x_STATUS); //High impedance state
    //while(SSIBusy(SSI2_BASE)){};
    //GetStatus();
    //SysCtlDelay(10000000);
    //while(SSIBusy(SSI2_BASE)){};

    timeout = SSI_SPI_TIMEOUT;

    while(SSIBusy(SSI2_BASE))
    {
       timeout--;
        if(timeout == 0)
        {
            return ;
        }
    }

    HardHiZ();
    read_status = Get_Param(x_STATUS);
    while((read_status & x_STATUS_HIZ) != x_STATUS_HIZ)
    {
        SysCtlDelay(1000000);
        read_status = Get_Param(x_STATUS);
    }

    //writable only when outputs are in high impedance:


    if( Global_EVB_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RLOADING)//Roker
    {
        //for QSH2818-32-07-006 roker
        SetParam(x_CONFIG,
        x_CONFIG_PWM_MUL_1 |
        x_CONFIG_PWM_DIV_1 |

        x_CONFIG_SR_180V_us |
        x_CONFIG_OC_SD_ENABLE |
        x_CONFIG_VS_COMP_DISABLE |

        /*
        //------------------------ AVI TEST
        x_CONFIG_VS_COMP_ENABLE |
        x_CONFIG_OC_SD_DISABLE |
        x_CONFIG_SR_530V_us |
        //----------------------------
         */

        x_CONFIG_SW_HARD_STOP |
        x_CONFIG_INT_16MHZ);


    }
    else //if( Global_EVB_Motor_Id == MOTOR_RDRIVING)fidder
    {
        SetParam(x_CONFIG,
        //for QSH4218-51-10-049 fidder
        x_CONFIG_PWM_MUL_2 |
        x_CONFIG_PWM_DIV_1 |

        x_CONFIG_SR_180V_us |
        x_CONFIG_OC_SD_ENABLE |
        x_CONFIG_VS_COMP_DISABLE |

        /*
        //------------------------ AVI TEST
        x_CONFIG_VS_COMP_ENABLE |
        x_CONFIG_OC_SD_DISABLE |
        x_CONFIG_SR_530V_us |
        //----------------------------
         */

        x_CONFIG_SW_HARD_STOP |
        x_CONFIG_INT_16MHZ);

    }




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

    timeout = SSI_SPI_TIMEOUT;

    while(SSIBusy(SSI2_BASE))
    {
       timeout--;
        if(timeout == 0)
        {
            return ;
        }
    }

    SetParam(x_ALARM_EN,0xFF);
    //----------->>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>     Special Section <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

    // Configure the RUN KVAL. This defines the duty cycle of the
    //  PWM of the bridges during running. 0xFF means that they are
    //  essentially NOT PWMed during run; this MAY result in more
    //  power being dissipated than you actually need for the task.
    //  Setting this value too low may result in failure to turn.
    //  There are ACC, DEC, and HOLD KVAL registers as well; you may
    //  need to play with those values to get acceptable performance
    //  for a given application.


    if( Global_EVB_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RLOADING)//Roker
    {
            //for QSH2818-32-07-006 roker
            SetParam(x_KVAL_HOLD, 0x28);
            SetParam(x_KVAL_RUN, 0x5F);
            SetParam(x_KVAL_ACC, 0x5F);
            SetParam(x_KVAL_DEC, 0x5F);
            SetParam(x_ST_SLP, 0x06);
            SetParam(x_INT_SPD, 0x44B8);
            SetParam(x_FN_SLP_ACC, 0x14);
            SetParam(x_FN_SLP_DEC, 0x14);
    }
    else //if( Global_EVB_Motor_Id == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)feeder
    {
        //for QSH4218-51-10-049 feeder
        SetParam(x_KVAL_HOLD, 0x35);
        SetParam(x_KVAL_RUN, 0x7F);
        SetParam(x_KVAL_ACC, 0x7F);
        SetParam(x_KVAL_DEC, 0x7F);
        SetParam(x_ST_SLP, 0x20);
        SetParam(x_INT_SPD, 0x1A13);
        SetParam(x_FN_SLP_ACC, 0x50);
        SetParam(x_FN_SLP_DEC, 0x50);
    }



    //https://www.youtube.com/watch?v=8C7qdjPbhlg MIN 2:20

    //<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

    // Calling GetStatus() clears the UVLO bit in the status
    //  register, which is set by default on power-up. The driver
    //  may not run without that bit cleared by this read operation.
    GetStatus();

    // Now we're going to set up a counter to track pulses from an
    //  encoder, to verify against the expected values.
    //TCCR1A = 0;  // No waveform generation stuff.
    //TCCR1B = B00000110; // Clock on falling edge, T1 pin.
    //TCNT1 = 0;   // Clear the count.

}

void Mot_Run()
{
    //while(SSIBusy(SSI2_BASE)){};
    Run(Direction,Run_Value );
    //Run_tx_test(Direction,Run_Value );
}

void Mot_Mov()
{
    //while(SSIBusy(SSI2_BASE)){};
    uint32_t timeout = SSI_SPI_TIMEOUT;

    while(SSIBusy(SSI2_BASE))
    {
       timeout--;
        if(timeout == 0)
        {
            return ;
        }
    }
    Move(Direction,Pos_Value );
}



void Mot_Stop()
{
    //while(SSIBusy(SSI2_BASE)){};
    uint32_t timeout = SSI_SPI_TIMEOUT;

    while(SSIBusy(SSI2_BASE))
    {
       timeout--;
        if(timeout == 0)
        {
            return ;
        }
    }
    switch(Stop_Command)
    {
        case 0:
                SoftHiZ();
            break;
        case 1:
                HardHiZ();
            break;
        case 2:
                SoftStop();
            break;
        case 3:
                HardStop();
            break;
        default:
            break;
    }
}
/*
void loop()
{
    GetStatus(); // Clear the error

    //unsigned long i;
    //unsigned long ReadSpeed;
//    ResetPos();
//    SoftStop();
//    SysCtlDelay(17500000);

//    HardStop();
    //while(1)
    {

         ResetPos();
        //for(i=0;i<0x3FFFFF;i++)
//        {
//            Run(FWD,i );
//            HardStop();
//            SysCtlDelay(5000000);
//        }
        //i  = 100000;

         unsigned long Read_Speed,
                       //Read_MaxSpeed,
                       //Read_Acc,
                       //Read_Dec,
                       //speed,
                       Read_pos,
                       //Pos_Value,
                       //Read_ELpos,
                       read_status;
        //unsigned long SpeedStepPerSec,
        //               MaxSpeedStepPerSec;

         //Direction = FWD;
         //Run_Value = 0x10625; // 1000 step/sec
         //Pos_Value = 0x111111; // 1000 step/sec
         unsigned k = 1;

         //SetParam(x_MAX_SPEED, MaxSpdCalc(1100)); // >1000 step/sec
         //SetParam(x_MAX_SPEED, 0x10680);  // >1000 step/sec

        while(k)
        {

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

            uint32_t timeout = SSI_SPI_TIMEOUT;

            while(SSIBusy(SSI2_BASE))
            {
               timeout--;
                if(timeout == 0)
                {
                    return ;
                }
            }

                        //Avi_test_get_speed(); //OK !!!
            Read_Speed = Get_Param(x_SPEED);

            //while(SSIBusy(SSI2_BASE)){};
            timeout = SSI_SPI_TIMEOUT;

                        while(SSIBusy(SSI2_BASE))
                        {
                           timeout--;
                            if(timeout == 0)
                            {
                                return ;
                            }
                        }
                        //SysCtlDelay(17500000);
                        Read_pos = Get_Param(x_ABS_POS);
                        timeout = SSI_SPI_TIMEOUT;

                                    while(SSIBusy(SSI2_BASE))
                                    {
                                       timeout--;
                                        if(timeout == 0)
                                        {
                                            return ;
                                        }
                                    }

///                       Read_ELpos = Get_Param(x_EL_POS);
//                        while(SSIBusy(SSI2_BASE)){};


            Run(Direction,Run_Value );

            switch(Stop_Command)
            {
                case 1:
                        HardStop();
                    break;
                case 2:
                        SoftStop();
                    break;
                case 3:
                        HardHiZ();
                    break;
                case 4:
                        SoftHiZ();
                    break;
                default:
                    break;
            }

///           if(k>5)
//                Move(Direction,Pos_Value );
//            else
//                //GoTo_DIR(Direction,Pos_Value );
//                GoTo(Pos_Value );

            read_status = Get_Param(x_STATUS);
///
//#ifdef SPI_EVA_LCD_ENABLED
//            switch((read_status & x_STATUS_MOT_STATUS)>>5)
//            {
//            case 0://Stopped
//                    writeLine("-");
//                break;
//            case 1://Acceleration
//                if(read_status & x_STATUS_DIR)
//                    writeLine(">>>");
//                else
//                    writeLine("<<<");
//                break;
//            case 2://Deceleration
//                if(read_status & x_STATUS_DIR)
//                    writeLine(">");
//                else
//                    writeLine("<");
//                break;
//            case 3://Constant speed
//                if(read_status & x_STATUS_DIR)
//                    //writeLine(">>");
//                else
//                    writeLine("<<");
//                break;
//            }
//#endif
//
            //read_status = GetStatus();
            //read_status = Get_Param_Status( x_GET_STATUS); // The GetStatus command resets the STATUS register warning flags.
            read_status = Get_and_Clear_Status();


            Get_Param(x_STEP_MODE);

//
//            //speed =  Get_Param(x_SPEED);
//            speed =  GetParam(x_SPEED);
//
            //SysCtlDelay(17500000);
            ///while(SSIBusy(SSI2_BASE)){};
            //Avi_test_get_speed(); //OK !!!
           // Get_Param(x_SPEED);

           // while(SSIBusy(SSI2_BASE)){};
            //SysCtlDelay(17500000);
          //  Get_Param(x_ABS_POS);
            //SysCtlDelay(17500000);
            //GetParam(x_SPEED); // wrong value;


            //unsigned long read_status;
///
//            if(k<2)
//            {
//                HardHiZ();
//                   read_status = Get_Param(x_STATUS);
//                   while((read_status & x_STATUS_HIZ) != x_STATUS_HIZ)
//                   {
//                       read_status = Get_Param(x_STATUS);
//                       SysCtlDelay(1000000);
//                   }
//
//
//                Read_Acc = Get_Param(x_ACC);
//
//
//               // while(SSIBusy(SSI2_BASE)){};
//
//               Read_Dec = Get_Param(x_DEC);
//
//                while(SSIBusy(SSI2_BASE)){};
//
//                Get_Param(x_CONFIG);
//                }
            //Get_Param(x_ALARM_EN);
            k--;
        }





//
//        Read_MaxSpeed = Get_Param(x_MAX_SPEED);
//        writeLine("Max Speed: ");
//        writeFloat(Read_MaxSpeed);
//
//        MaxSpeedStepPerSec = MaxSpdCalc(Read_MaxSpeed);
//
//        writeLine("Max Speed (Step Per Sec): ");
//        writeFloat(MaxSpeedStepPerSec);



//
//        Read_KvalAcc = Get_Param(x_KVAL_ACC);
//
//        Read_KvalDec = Get_Param(x_KVAL_DEC);
//

//       while (1)
//       {
//           //Run(Direction,Run_Value );
//
//
//           SysCtlDelay(7500000);
//           Read_Speed = Get_Param(x_SPEED);
//
//           SpeedStepPerSec = MaxSpdCalc(Read_Speed);
//
//       }
//

///
//
//        //for(i=0xFFFFF;i>0;i--)
//        //{
//
//         //Move(FWD, 25600);
//
//         //Move(FWD, 25600);
//
//         Run(FWD, 0xFFFFF);
//        //while (digitalRead(Get_BUSY()) == LOW);  // Until the movement completes, the
//        //while(L6470_BUSY == LOW){}                                    //  BUSYN pin will be low.
//         //SetParam(x_MAX_SPEED, MaxSpdCalc(1));  // Change the Speed
//        // SetParam(x_MIN_SPEED, MinSpdCalc(300));
//
//        //delay(5);
//        //SysCtlDelay(7500000); // need #include "driverlib/sysctl.h"
//        //ReadSpeed =  GetParam(x_SPEED);
//
//
//
//         //SysCtlDelay(17500000);
//         SysCtlDelay(10000000); // need #include "driverlib/sysctl.h"
//         //SoftStop();
//        // HardStop();
////        }
////        //SetParam(x_MAX_SPEED, MaxSpdCalc(300));  // Change the Speed
////        //delay(5);
////       // for(i=0;i<10;i++)
////        {
////           //Move(REV, 25600);
//           //Move(REV, 25600);
//
//         Run(REV, 0xFFFFF);
//
//           //SetParam(x_MAX_SPEED, MaxSpdCalc(30));  // Change the Speed
//           SysCtlDelay(10000000);
//           //SoftStop();
////        //while (digitalRead(Get_BUSY()) == LOW);  // Until the movement completes, the
////        //while (*BUSY_Reg == LOW){}                                    //  BUSYN pin will be low.
////        //SetParam(x_MAX_SPEED, MaxSpdCalc(50));  // Change the Speed
////        //delay(5);
//          // HardStop();
////        }
//
//        //GoHome();
//
//       // SysCtlDelay(17500000);
//      //for(i=0;i<10;i++);
//
    }
}
*/
///////////////
/*
int SPI_Control(bool Init)
{
    if(Init == INIT)
    {
        SPI2_Init();
        init_BUSY_Pin();
        //setup();
        setup(NULL);
    }
    loop();

    return(0);
}
*/
byte Transfer_tx(byte data/*, byte data_out*/ )
{
    #define NUM_SSI_DATA            1
    byte data_out;

    uint32_t pui32DataTx[NUM_SSI_DATA];
    uint32_t pui32DataRx[NUM_SSI_DATA];

    pui32DataTx[0] = data;
    //pui32DataRx[0] = data_out;

    SPI_TX(SSI2_BASE, &pui32DataTx, &pui32DataRx, NUM_SSI_DATA );

    SPI_RX(SSI2_BASE, &pui32DataRx, NUM_SSI_DATA );


    data_out = pui32DataRx[0];

    return data_out;

}


byte Write_Byte(uint8_t WByte)
{
    uint32_t RByte;

    uint16_t TimeOut = 0;

    SSIDataPut(SSI2_BASE, WByte);

//    while(SSIBusy(SSI2_BASE))
//    {
//    }

    uint32_t timeout = SSI_SPI_TIMEOUT;

    while(SSIBusy(SSI2_BASE))
    {
       timeout--;
        if(timeout == 0)
        {
            return ERROR;
        }
    }

   while(Check_SPI_Busy() == BUSY)  //TODO
   {
       TimeOut = TimeOut++;
   }

    SSIDataGet(SSI2_BASE, &RByte);

    return (RByte & 0xff);

}

uint32_t Get_Param(byte param)//OK
{
    uint32_t temp = 0;
    uint32_t rx = 0;
    //while(SSIBusy(SSI2_BASE)){};
   // while(Check_SPI_Busy() == BUSY){};
    /* Send GetParam operation code to dSPIN */
    temp = Write_Byte((uint8_t)x_GET_PARAM | (uint8_t)param);
    /* MSB which should be 0 */
    rx |= (temp & 0xFF) << 24;
    switch (param)
    {
        case x_ABS_POS: ;
        case x_MARK: ;
        case x_SPEED:
            //while(SSIBusy(SSI2_BASE)){};
            //while(Check_SPI_Busy() == BUSY){};
            temp = Write_Byte((uint8_t)(0x00));
            rx |= (temp & 0xFF) << 16;
        case x_EL_POS: ;
        case x_ACC: ;
        case x_DEC: ;
        case x_MAX_SPEED: ;
        case x_MIN_SPEED: ;
        case x_FS_SPD: ;
        case x_INT_SPD: ;
        case x_CONFIG: ;
        case x_STATUS:
            //while(SSIBusy(SSI2_BASE)){};
            //while(Check_SPI_Busy() == BUSY){};
            temp = Write_Byte((uint8_t)(0x00));
            rx |= (temp & 0xFF) << 8;
        default:
            //while(SSIBusy(SSI2_BASE)){};
            //while(Check_SPI_Busy() == BUSY){};
            temp = Write_Byte((uint8_t)(0x00));
            rx |= temp & 0xFF;
    }

    return rx;
}
/*
void Avi_test_get_speed()//  Working as expected read the speed
{

    unsigned char i;

    uint32_t pui32DataRx[10];

    while(SSIDataGetNonBlocking(SSI2_BASE, &pui32DataRx[0]))
    {
    }

    SSIDataPut(SSI2_BASE, 0x24);//get speed

    while(SSIBusy(SSI2_BASE))
    {
    }

    writeLine("Received: ");


    for(i = 0; i < 4; i++)
    {
        if(i>0)
            SSIDataPut(SSI2_BASE, 0x00);

        while(SSIBusy(SSI2_BASE))
        {
        }

        SSIDataGet(SSI2_BASE, &pui32DataRx[i]);

        pui32DataRx[i] &= 0x00FF;

        writeFloat (pui32DataRx[i]);
        writeString("   ");
    }

}

*/
uint32_t Get_and_Clear_Status()// Tested OK AVI
{
    uint32_t temp = 0;
    uint32_t rx = 0;

    /* Send GetParam operation code to dSPIN */
    temp = Write_Byte(x_GET_STATUS);
    /* MSB which should be 0 */
    rx |= (temp & 0xFF) << 24;

    temp = Write_Byte((uint8_t)(0x00));
    rx |= (temp & 0xFF) << 8;

    temp = Write_Byte((uint8_t)(0x00));
    rx |= temp & 0xFF;

    return rx;
}