#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 "drivers/FPGA/FPGA_Comm.h"

#include <driverlib/rom.h>
#include <driverlib/rom_map.h>
#include <PMR/Stubs/StubMotorInitRequest.pb-c.h>
#include <Stubs_Handler/DataDef.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;

//#define SPI_EVA_LCD_ENABLED


void SPI2_Init()
{
    #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);
        //----------------------------------------------------------------

}

void init_BUSY_Pin(void)
{
    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.
}

bool Check_SPI_Busy(void)
{

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

bool Polling_SPI_Busy(void)
{
    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;
}

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))
    {
    }
}

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

//        writeLine("SPI_RX: ");
//        writeFloat (pui32DataRx[ui32Index]);
//        writeString("   ");


        //
        // 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(StubMotorInitRequest* request)
//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);
        while((read_status & x_STATUS_HIZ) != x_STATUS_HIZ)
        {
            NOP++;
        }

        while(SSIBusy(SSI2_BASE)){};

        if(request->set_micro_steps)
        {
            SetParam(x_STEP_MODE,
                !x_SYNC_EN |
                request->micro_steps |
                x_SYNC_SEL_1);
        }
        else
        {
            SetParam(x_STEP_MODE,
                !x_SYNC_EN |
                x_STEP_SEL_1_8 |
                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)){};

    if(request->set_max_speed)
    {
        SetParam(x_MAX_SPEED, MaxSpdCalc(request->max_speed));
    }
    else
    {
        SetParam(x_MAX_SPEED, MaxSpdCalc(1120));
    }


    // 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)){};
    SetParam(x_FS_SPD, FSCalc(0x3FF));

    //STALL threshold
    while(SSIBusy(SSI2_BASE)){};
    //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)){};

    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(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)){};

    if(request->set_dec)
    {
        SetParam(x_ACC, 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
    }



    // Configure the overcurrent detection threshold. The constants
    //  for this are defined in the L6470.h file.
    while(SSIBusy(SSI2_BASE)){};
    SetParam(x_OCD_TH, x_OCD_TH_1500mA);

    // 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)){};

    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:
    SetParam(x_CONFIG,
        x_CONFIG_PWM_DIV_1 |
        x_CONFIG_PWM_MUL_2 |
        x_CONFIG_SR_180V_us |
        x_CONFIG_OC_SD_ENABLE |
        x_CONFIG_VS_COMP_DISABLE |
        x_CONFIG_SW_HARD_STOP |
        x_CONFIG_INT_16MHZ);

    while(SSIBusy(SSI2_BASE)){};

    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.
    SetParam(x_KVAL_HOLD, /*0x0B);// */ 0x05);
    SetParam(x_KVAL_RUN, /*0x4B);// */ 0x23);
    SetParam(x_KVAL_ACC, /*0x4B);// */ 0x23);
    SetParam(x_KVAL_DEC, /*0x4B);// */ 0x23);
    SetParam(x_ST_SLP, 0x18);
    SetParam(x_INT_SPD, /*0x1A13);// */ 0x3550);
    SetParam(x_FN_SLP_ACC, /*0x48);// */ 0x23);
    SetParam(x_FN_SLP_DEC, /*0x48);// */ 0x23);

    //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)){};
    Move(Direction,Pos_Value );
}



void Mot_Stop()
{
    while(SSIBusy(SSI2_BASE)){};
    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();
   // writeLine("Received: ");
    //while(1)
    {

         ResetPos();
        //for(i=0;i<0x3FFFFF;i++)
//        {
//            Run(FWD,i );
//            HardStop();
//            writeLine("Tx ");
//            writeFloat(i);
//            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)){};
                        //Avi_test_get_speed(); //OK !!!
            Read_Speed = Get_Param(x_SPEED);
/*            writeLine("Read_Speed: ");
            writeFloat(Read_Speed);*/

            while(SSIBusy(SSI2_BASE)){};
                        //SysCtlDelay(17500000);
                        Read_pos = Get_Param(x_ABS_POS);
                        //writeLine("Read_pos: ");
                        //writeFloat(Read_pos);
                        while(SSIBusy(SSI2_BASE)){};

/*                        Read_ELpos = Get_Param(x_EL_POS);
                        writeLine("Read_EL_pos: ");
                        writeFloat(Read_ELpos);
                        while(SSIBusy(SSI2_BASE)){};*/


            //writeLine("Tx ");
            //writeFloat(Run_Value);
            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);
/*
            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;
            }
            */
            //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();

            if((read_status &  x_STATUS_UVLO)==0)
               //writeLine("Undervoltage");

            Get_Param(x_STEP_MODE);

            /*
            //speed =  Get_Param(x_SPEED);
            speed =  GetParam(x_SPEED);
            if(speed & 0xfffff == Run_Value )
                writeLine(" XXXXXXX ");
            writeLine(" Speed: ");
            writeFloat(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);
                writeLine("x_ACC: ");
                writeFloat(Read_Acc);


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

               Read_Dec = Get_Param(x_DEC);
                writeLine("x_ACC: ");
                writeFloat(Read_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);
        writeLine("x_KVAL_ACC: ");
        writeFloat(Read_KvalAcc);

        Read_KvalDec = Get_Param(x_KVAL_DEC);
        writeLine("x_KVAL_DEC: ");
        writeFloat(Read_KvalDec);
*/

 /*      while (1)
       {
           //Run(Direction,Run_Value );

           writeLine("Tx ");
           writeFloat(Run_Value);

           SysCtlDelay(7500000);
           Read_Speed = Get_Param(x_SPEED);
           writeLine("Speed: ");
           writeFloat(Read_Speed);

           SpeedStepPerSec = MaxSpdCalc(Read_Speed);

           writeLine("Speed (Step Per Sec): ");
           writeFloat(SpeedStepPerSec);
       }
*/

/*

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

         //writeLine("Tx ");
        // writeFloat(i);
        // writeLine("Rx ");
        //delay(5);
        //SysCtlDelay(7500000); // need #include "driverlib/sysctl.h"
        //ReadSpeed =  GetParam(x_SPEED);

        //writeFloat(ReadSpeed);


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

   //writeLine("Transfer_tx: ");
    //writeFloat(pui32DataRx[0]);

    data_out = pui32DataRx[0];

    return data_out;

}


byte Write_Byte(uint8_t WByte)
{
    uint32_t RByte;

    SSIDataPut(SSI2_BASE, WByte);

    while(SSIBusy(SSI2_BASE))
    {
    }

    while(Check_SPI_Busy() == BUSY)
    {
    }

    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;
}