/*
 * BTSR.c
 *
 *  Created on: 1 Dec 2020
 *      Author: avi
 */

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

#include <ti/sysbios/hal/Hwi.h>
#include <xdc/runtime/Error.h>
#include <xdc/runtime/System.h>

#include <inc/hw_ints.h>

#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "driverlib/gpio.h"
#include "driverlib/pin_map.h"
#include "driverlib/rom.h"
#include "driverlib/rom_map.h"
#include "driverlib/sysctl.h"
#include "driverlib/interrupt.h"
#include "utils/uartstdio.h"
#include "driverlib/uart.h"
#include "Modules/Control/MillisecTask.h"
#include <DataDef.h>
#include <Drivers/I2C_Communication/I2C_Task.h>
#include "Drivers/FPGA/FPGA_Comm.h"
#include "Drivers/Uart_Comm/BTSR/BTSR.h"



static uint32_t GlobalClk;
uint8_t BTSR_RX_Buff[Max_BTSR_RX_Bytes];
uint16_t  TX_Timeout = 0xFFFF;//6250
uint16_t  RX_Timeout = 0xFFFF;//1

BTSR_t BTSR[MaxUFeeders];

typedef enum
{
    BTSR_UART_CONFIG_PAR_1 = (UART_CONFIG_WLEN_8 |UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_ONE),
    BTSR_UART_CONFIG_PAR_0 = (UART_CONFIG_WLEN_8 |UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_ZERO),
}UAR_CFG;
UAR_CFG BTSR_UART_CONFIG;

uint8_t txlen = 0;
uint8_t txcounter = 0, rxcounter = 0;
uint8_t BTSR_TX_Buff[Max_BTSR_TX_Bytes];


uint8_t RX_Int_Counter = 0, RT_Int_Counter = 0, TX_Int_Counter = 0;

uint8_t UART4_RS485_Direction(bool TX_OR_RX)//TX or RX
{
    ROM_GPIOPinWrite(GPIO_PORTR_BASE, GPIO_PIN_0, !(TX_OR_RX));

    return OK;
}
void U4TX()
{
    UARTIntEnable(UART4_BASE, UART_INT_TX);
    ROM_UARTCharPutNonBlocking(UART4_BASE,BTSR_TX_Buff[txcounter]);
}
void U4RX()
{
    UARTIntEnable(UART4_BASE, UART_INT_TX);
    BTSR_RX_Buff[rxcounter] = ROM_UARTCharGetNonBlocking(UART4_BASE);//Receive data
    rxcounter++;
}

void uart4_intHandler()
{
    //Get interrupt flags
    unsigned long status = UARTIntStatus(UART4_BASE, 1);

    //Clear flags
    UARTIntClear(UART4_BASE, status);

    //Treat both of these interrupts the same
    if(status & UART_INT_RX)
    {
        RX_Int_Counter++;
        U4RX();
    }
    else
    if(status & UART_INT_RT) /// Receive Timeout Interrupt
    {
        RT_Int_Counter++;
        U4RX();
    }
    if(status & UART_INT_TX)
    {
        UARTIntDisable(UART4_BASE, UART_INT_TX);

        TX_Int_Counter++;

        BTSR_Change_Comm_Mode(TX_NOT_FIRST_BYTE);
        txcounter++;
        if(txcounter >= txlen )
         {
            txcounter = 0;
            rxcounter = 0;

            BTSR_Change_Comm_Mode(RX);
            UARTIntEnable(UART4_BASE, UART_INT_RT | UART_INT_RX);

         }
        else
        {
            U4TX();
        }

    }

}




void InitConsole_BTSR_UART4(int32_t ui32SysClock)
{

    ROM_IntMasterDisable();

    GlobalClk = ui32SysClock;

    //uint32_t ui32SysClock = MAP_SysCtlClockFreqSet((SYSCTL_XTAL_25MHZ | SYSCTL_OSC_MAIN | SYSCTL_USE_PLL | SYSCTL_CFG_VCO_480), SYS_CLK_FREQ);

    SysCtlPeripheralEnable(SYSCTL_PERIPH_UART4);
    //
    // Enable GPIO port A which is used for UART4 pins.
    //
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);

    SysCtlDelay(3);

    //
    // Configure the pin muxing for UART4 functions on port A0 and A1.
    // This step is not necessary if your part does not support pin muxing.
    //
    MAP_GPIOPinConfigure(GPIO_PA2_U4RX);
    MAP_GPIOPinConfigure(GPIO_PA3_U4TX);

    //
    // Enable UART4 so that we can configure the clock.
    //
    SysCtlPeripheralEnable(SYSCTL_PERIPH_UART4);

    SysCtlDelay(3);

    //
    // Use the internal 16MHz oscillator as the UART clock source.
    //
    UARTClockSourceSet(UART4_BASE, UART_CLOCK_SYSTEM);//UART_CLOCK_PIOSC);

    //
    // Select the alternate (UART) function for these pins.
    //
    MAP_GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_2 | GPIO_PIN_3);

    //
    // Initialize the UART for console I/O.
    //
    UARTFIFOEnable(UART4_BASE);

    MAP_UARTConfigSetExpClk(UART4_BASE, ui32SysClock, 57600,(UART_CONFIG_WLEN_7 |UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_ONE));//BTSR protocol
    UART4_RS485_Direction(TX);

    //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
    //Set UART FIFO level to 1/2
    //UARTFIFOLevelSet(UART4_BASE, UART_FIFO_TX7_8, UART_FIFO_RX1_8);
//
//    //Register interrupt handler
//    UARTIntRegister(UART4_BASE, uart4_intHandler);
//
    //
        // Configure and enable UART interrupts.
        //
        ROM_UARTIntClear(UART4_BASE, ROM_UARTIntStatus(UART4_BASE, false));
        //Declare HWI dynamically  "TI-RTOS Kernel User's Guide" Page. 167 or Section 8.2.1
        Hwi_Handle Uart4Hwi;
        Error_Block eb;
        Error_init(&eb);
        Uart4Hwi = Hwi_create(INT_UART4, uart4_intHandler, NULL, &eb);
        if (Uart4Hwi == NULL)
        {
        System_abort("Uart4Hwi create failed");
        }

        UARTTxIntModeSet(UART4_BASE, UART_TXINT_MODE_EOT);//generate a TX interrupt when the last character has completed transmission

        //Enable RX and RT interrupts
    UARTIntEnable(UART4_BASE, /*UART_INT_RT | UART_INT_RX | */UART_INT_TX);
    //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

//#define UART4_CTL_R             (*((volatile uint32_t *)0x40010030))
//
//    UART4_CTL_R |= UART_CTL_EOT;// End of Transmission

    ROM_UARTEnable(UART4_BASE);
    //
    // Enable interrupts now that the application is ready to start.
    //
    ROM_IntEnable(INT_UART4);



    ROM_IntMasterEnable();
}


unsigned long UARTStatus()
{
    return(HWREG(UART4_BASE + UART_O_FR));
}

bool BTSR_Change_Comm_Mode(uint8_t Mode)
{

    if(Mode == TX_FIRST_BYTE)
    {
        UART4_RS485_Direction(TX);

        Task_sleep (eOneMillisecond);//Doesn't work without it - maybe to change to TX after RX
        if(BTSR_UART_CONFIG != BTSR_UART_CONFIG_PAR_1)
        {
            BTSR_UART_CONFIG = BTSR_UART_CONFIG_PAR_1;
            MAP_UARTConfigSetExpClk(UART4_BASE, GlobalClk, 57600,BTSR_UART_CONFIG_PAR_1);
        }
    }
    else if(Mode == TX_NOT_FIRST_BYTE)
    {
        UART4_RS485_Direction(TX);

        //Task_sleep (eOneMillisecond);
        if(BTSR_UART_CONFIG != BTSR_UART_CONFIG_PAR_0)
        {
            BTSR_UART_CONFIG = BTSR_UART_CONFIG_PAR_0;
            MAP_UARTConfigSetExpClk(UART4_BASE, GlobalClk, 57600,BTSR_UART_CONFIG_PAR_0);
        }
    }
    else if(Mode == RX)
    {
        UART4_RS485_Direction(RX);

        //Task_sleep (eOneMillisecond);
        if(BTSR_UART_CONFIG != BTSR_UART_CONFIG_PAR_0)
        {
            BTSR_UART_CONFIG = BTSR_UART_CONFIG_PAR_0;
            MAP_UARTConfigSetExpClk(UART4_BASE, GlobalClk, 57600,BTSR_UART_CONFIG_PAR_0);
        }
    }
    return OK;
}

int Uart4TransmitResult = 0;

uint8_t BTSR_Auto_Identify(uint8_t BTSR_Id)//AUTOMATIC IDENTIFICATION PROCEDURE (IDENTIFY)
{
    uint8_t i = 0;
    uint8_t FUNCTION_STRING_FD = 0xFD;
    uint8_t TX_Timeout = 10;
    uint8_t RX_Timeout = 10;

    BTSR_Change_Comm_Mode(TX_FIRST_BYTE);
    Task_sleep (eOneMillisecond);

    for(i = 0 ;i < 3; i++)
    {

        Uart4TransmitResult |=ROM_UARTCharPutNonBlocking(UART4_BASE,FUNCTION_STRING_FD);
        Task_sleep (eOneMillisecond);
    }

    while(UARTBusy(UART4_BASE) == true)//true if the UART is transmitting, false if all transmissions are complete
    {
        if(TX_Timeout)
        {
            TX_Timeout--;
            Task_sleep (eOneMillisecond);
        }
        else
            return ERROR;
    }

    BTSR_Change_Comm_Mode(RX);

    while(UARTCharsAvail(UART4_BASE) == false);//true if there is data in the receive FIFO or false if there is no data in the receive FIFO
    {
        if(RX_Timeout)
        {
            RX_Timeout--;
            Task_sleep (eOneMillisecond);
        }
        else
            return ERROR;
    }
    BTSR_RX_Buff[0] = ROM_UARTCharGetNonBlocking(UART4_BASE);//Receive data

    if(BTSR_RX_Buff[0] == 0xFD)
    {
        BTSR_Change_Comm_Mode(TX_FIRST_BYTE);
        Task_sleep (eOneMillisecond);
        Uart4TransmitResult |=ROM_UARTCharPutNonBlocking(UART4_BASE,BTSR_Id);
        return OK; //Need to press ENTER in BTSR!
    }
    else
    {
        return ERROR;
    }
}

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

uint8_t BTSR_Calculate_CheckSum(uint8_t *Buf, uint8_t size_without_Checksum)
{
    uint8_t i = 0;
    uint16_t Sum = 0;

    for(i=0;i<(size_without_Checksum);i++)
    {
        Sum += Buf[i];
    }
    Buf[i] = Sum & 0xFF;
    Buf[i+1] = Sum >> 8;

    return OK;
}

uint8_t Check_Received_CheckSum(uint8_t CODE_STRING, uint8_t Function_Code, uint8_t *Buf, uint8_t size_include_Checksum)//Function_Code + CODE_STRING needed only for FUNCTION_STRING = 0x33
{
    uint8_t Status = OK;
    uint8_t i = 0;
    uint16_t Sum = 0;

    //uint8_t temp[10];

    if(size_include_Checksum > 2) // No checkSum for <=2
    {
        for(i = 0; i < (size_include_Checksum - 2); i++)
        {
            Sum += Buf[i];//Sum Of DATA_STRING
        }

//        for(i = 0; i < size_include_Checksum ; i++)//debug
//        {
//            temp[i] = Buf[i];
//        }

        if(size_include_Checksum == 6)  //FUNCTION_STRING = 0x33
        {
            //Sum Of DATA_STRING, Device Code, Function Code.
            Sum += Function_Code + CODE_STRING;
        }

        if( (Buf[size_include_Checksum - 2] != (Sum & 0xFF)) ||
            (Buf[size_include_Checksum - 1] != (Sum >> 8))      )
        {
            Status = ERROR;
            ReportWithPackageFilter(ThreadFilter,"BTSR Get Wrong Checksum",__FILE__,__LINE__,(int)CODE_STRING,RpError,(int)Function_Code,0);
        }
    }
    return Status;
}



//APPLICATION AND YARN TYPOLOGY SET
typedef union
{
    struct
    {
        uint8_t  Code;
        uint8_t  Function;
        uint8_t Data[2];
        uint8_t Check[2];
    }String;
    uint8_t Buf[6];
}BTSR_TYPOLOGY_SET;

BTSR_TYPOLOGY_SET Typology_Set;

uint8_t BTSR_Send_Buf(uint8_t *Buf, uint8_t size)
{
    int TxResult = 0;
    uint8_t i;

    txlen = size;
    for(i=0;i<size;i++)
        BTSR_TX_Buff[i]  = Buf[i];

    UARTIntEnable(UART4_BASE, UART_INT_TX);
    BTSR_Change_Comm_Mode(TX_FIRST_BYTE);
    //Task_sleep (eOneMillisecond);
    TxResult |=ROM_UARTCharPutNonBlocking(UART4_BASE,Buf[0]);

    return TxResult;
}

uint8_t BTSR_Send_Buf_3xADD1(uint8_t *Buf, uint8_t size)//Only for commands with 3 bytes with Address 1 (0x06/0xFD)
{
    int TxResult = 0;
    uint8_t i = 0;

    if((Buf[4] != 0x06) || (size < 3))
        return ERROR;

    BTSR_Change_Comm_Mode(TX_FIRST_BYTE);
    Task_sleep (eOneMillisecond);

    for(i = 0; i < 3; i++)
    {
        TxResult |=ROM_UARTCharPutNonBlocking(UART4_BASE,Buf[i]);
        Task_sleep (eOneMillisecond);
    }

    BTSR_Change_Comm_Mode(TX_NOT_FIRST_BYTE);
    Task_sleep (eOneMillisecond);

    for(; i < size; i++)
    {
        TxResult |=ROM_UARTCharPutNonBlocking(UART4_BASE,Buf[i]);
    }

    while(HWREG(UART4_BASE + UART_O_FR) & UART_FR_BUSY)//UARTFR
    {
        if(TX_Timeout)
        {
            TX_Timeout--;
        }
        else
            return ERROR;
    }
    UART4_RS485_Direction(RX);

    return TxResult;
}

uint8_t BTSR_RECEIVE_Buf(uint8_t *Buf, uint8_t size) //to test with BTSR_Set_Parmeters(0x01,SHOES,ALL_YARNS);
{
    uint8_t i = 0;

    BTSR_Change_Comm_Mode(RX);

    while(UARTCharsAvail(UART4_BASE) == false)//true if there is data in the receive FIFO or false if there is no data in the receive FIFO
    {
        if(RX_Timeout)
        {
            RX_Timeout--;
        }
        else
            return ERROR;
    }

    memset (BTSR_RX_Buff,0,sizeof(BTSR_RX_Buff));

    i=0;
    while(UARTCharsAvail(UART4_BASE) == true)
    {
        BTSR_RX_Buff[i] = ROM_UARTCharGetNonBlocking(UART4_BASE);//Receive data
        i++;
    }
//    for(i=0;i<4;i++)
//    {
//        BTSR_RX_Buff[i] = ROM_UARTCharGetNonBlocking(UART4_BASE);//Receive data
//    }

    if(size == i)
        return OK;//as expected
    return ERROR;
}

uint8_t BTSR_Set_Parmeters(uint8_t BTSR_Id, uint8_t Application, uint8_t Yarn_Type )
{
    //uint16_t Sum = 0;
    //uint8_t i = 0;

    //BTSR_RX_Buff[0] = 0;

    Typology_Set.String.Code = BTSR_Id;
    Typology_Set.String.Function = 0x3F;
    Typology_Set.String.Data[0] = Application;
    Typology_Set.String.Data[1] = Yarn_Type;
    BTSR_Calculate_CheckSum(Typology_Set.Buf,(sizeof(Typology_Set.Buf))- 2);
    /*
    for(i=0;i<(sizeof(Typology_Set) - sizeof(Typology_Set.String.Check));i++)
    {
        Sum += Typology_Set.Buf[i];
    }
    Typology_Set.String.Check[0] = Sum & 0xFF;
    Typology_Set.String.Check[1] = Sum >> 8;
*/


    BTSR_Send_Buf(Typology_Set.Buf, sizeof(Typology_Set));

    //BTSR_RECEIVE_Buf(BTSR_RX_Buff, 1);

/*
    if(BTSR_RX_Buff[0] == 0xFB)
        return OK;
    else
        return ERROR;
        */
    return BTSR_RX_Buff[0];
}


//------------------------------------------------------------------------


typedef union
{
    struct
    {
        uint8_t Data[2];
        uint8_t Check[2];
    }String;
    uint8_t Buf[4];
}BTSR_2_2;

typedef union
{
    struct
    {
        uint8_t  Code;
        uint8_t  Function;
        uint8_t Check[2];
    }String;
    uint8_t Buf[4];
}BTSR_1_1_2;

BTSR_1_1_2 Status_Read_Command;//ALARM AND ENABLING STATUS READ
BTSR_2_2   Status_Read_Response;

uint8_t BTSR_Read_Status(uint8_t BTSR_Id, uint8_t Application, uint8_t Yarn_Type )
{
    uint16_t Sum = 0;
    uint8_t i = 0;

    Typology_Set.String.Code = BTSR_Id;
    Typology_Set.String.Function = 0x1F;
    for(i=0;i<(sizeof(Typology_Set) - sizeof(Typology_Set.String.Check));i++)
    {
        Sum += Typology_Set.Buf[i];
    }
    Typology_Set.String.Check[0] = Sum & 0xFF;
    Typology_Set.String.Check[1] = Sum >> 8;

    BTSR_Send_Buf(Typology_Set.Buf, sizeof(Typology_Set));

    BTSR_RECEIVE_Buf(BTSR_RX_Buff, 6);

    for(i=0;i<(sizeof(Status_Read_Response) - sizeof(Status_Read_Response.String.Check));i++)
    {
        Sum += BTSR_RX_Buff[i];
    }
    if( (BTSR_RX_Buff[sizeof(Status_Read_Response) - 2] == Sum & 0xFF) &&
        (BTSR_RX_Buff[sizeof(Status_Read_Response) - 1] == Sum >> 8)      )
    {
        for(i=0;i<(sizeof(Status_Read_Response) - sizeof(Status_Read_Response.String.Check));i++)
        {
            Status_Read_Response.Buf[i] = BTSR_RX_Buff[i];
        }
        return OK;
    }
    else
        return ERROR;
}

//----------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------



typedef union
{
    struct
    {
        uint8_t CODE_STRING    ;
        uint8_t FUNCTION_STRING;

        //DATA_STRING
        uint16_t WORK_TENSION  ;//1° - 2°
        uint16_t OUTPUT_TENSION;//3° - 4°
        uint16_t RELAX_YARN    ;//5° - 6°
        uint8_t INC_DEC_TENSION;//7°
        uint8_t INC_DEC_STRETCH;//8°
        uint16_t TENSION_ERROR ;//9° -  10°
        uint16_t TIME_ALARM    ;//11° - 12°
        uint8_t RELAX_SPEED    ;//13°
        uint8_t APPLICATION    ;//14°
        uint8_t YARN_TYPE      ;//15°
        uint8_t SPARE[4]       ;//16°
        uint8_t ALARM_ENABLING ;//20°
        uint8_t ABILITAZIONE   ;//21°

        uint16_t CHECKSUM      ;
    }Byte;
uint8_t Buf[25];
}BTSR_ADVANCED_STYLE_LOAD;

BTSR_ADVANCED_STYLE_LOAD BTSR_AdvStyle_Load;

typedef union
{
    struct
    {
        uint8_t CODE_STRING      ;
        uint8_t FUNCTION_STRING  ;

        //DATA_STRING
        uint16_t OUTPUT_TENSION  ;
        uint16_t WORK_TENSION    ;
        uint16_t RELAX_YARN      ;
        uint8_t  INC_DEC_TENSION ;
        uint8_t  INC_DEC_STRETCH ;
        uint32_t METER_START     ;
        uint32_t INC_DEC_METER   ;
        uint32_t TARGET          ;
        uint16_t TENSION_ERROR   ;
        uint16_t ALARM_TIME      ;
        uint8_t  SPARE1[2]       ;
        uint8_t  ALARM_ENABLING  ;
        uint8_t  TARGET_ENABLING ;
        uint8_t  RELAX_YARN_SPEED;
        uint8_t  SPARE2[6]       ;

        uint16_t CHECKSUM        ;
    }Byte;
uint8_t Buf[39];
}BTSR_ADVANCED_TENSION_PROGRAMMING;

BTSR_ADVANCED_TENSION_PROGRAMMING BTSR_Adv_Tension_Prog;

void BTSR_Configuration_Set(UFeeder_Index UFeeder_i)//15
{
    memset(BTSR_TX_Buff, 0, Max_BTSR_TX_Bytes);

    BTSR_TX_Buff[0] = BTSR[UFeeder_i].ID;
    BTSR_TX_Buff[1] = 0x36;
    BTSR_TX_Buff[2] = BTSR[UFeeder_i].DirectionCCW << 7;
    BTSR_TX_Buff[3] = 0x01;//YYT OFF, UNICO OFF
    BTSR_TX_Buff[4] = BTSR[UFeeder_i].Flip_Display << 6;
    BTSR_TX_Buff[5] = BTSR[UFeeder_i].Lock_Keyboard_Menue;// ‘1’ EDITOR LOCKED

    BTSR_Calculate_CheckSum(BTSR_TX_Buff,18);

    BTSR_Send_Buf(BTSR_TX_Buff, 20);

}

void BTSR_Advanced_Style_Load(UFeeder_Index UFeeder_i)
{
    //memset(BTSR_AdvStyle_Load.Buf, 0, Size);
    /*
    uint8_t Size = sizeof(BTSR_AdvStyle_Load.Buf);

    memset(BTSR_AdvStyle_Load.Buf, 0, Size);
/
    BTSR_AdvStyle_Load.Byte.CODE_STRING = UFeeder_ID;
    BTSR_AdvStyle_Load.Byte.FUNCTION_STRING = 0x4B;

    BTSR_AdvStyle_Load.Byte.WORK_TENSION = Tension;
    BTSR_AdvStyle_Load.Byte.TENSION_ERROR = Tension_Error;
    BTSR_AdvStyle_Load.Byte.TIME_ALARM = Time_Alarm;
    BTSR_AdvStyle_Load.Byte.APPLICATION = Application;
    BTSR_AdvStyle_Load.Byte.YARN_TYPE = YarnType;
    BTSR_AdvStyle_Load.Byte.ALARM_ENABLING = 0x03; // OUTPUT TENSION ENABLED + TENSION ERROR ENABLED
*/
    //BTSR_Calculate_CheckSum(BTSR_AdvStyle_Load.Buf,Size - 2);

    memset(BTSR_TX_Buff, 0, Max_BTSR_TX_Bytes);

    BTSR_TX_Buff[0] = BTSR[UFeeder_i].ID;
    BTSR_TX_Buff[1] = 0x4B;

    BTSR_TX_Buff[2] = BTSR[UFeeder_i].Tension & 0XFF;//1
    BTSR_TX_Buff[3] = BTSR[UFeeder_i].Tension >> 8;//2

    BTSR_TX_Buff[10] = BTSR[UFeeder_i].Tension_Err & 0XFF;//9
    BTSR_TX_Buff[11] = BTSR[UFeeder_i].Tension_Err >> 8;//10
    BTSR_TX_Buff[12] = BTSR[UFeeder_i].Alarm_Time & 0XFF;//11
    BTSR_TX_Buff[13] = BTSR[UFeeder_i].Alarm_Time >> 8;//12


    BTSR_TX_Buff[15] = BTSR[UFeeder_i].Application;//14
    BTSR_TX_Buff[16] = BTSR[UFeeder_i].Yarn_Type;//15

    BTSR_Calculate_CheckSum(BTSR_TX_Buff,23);

    BTSR_Send_Buf(BTSR_TX_Buff,25);

}

void BTSR_Advanced_Tension_Prog(UFeeder_Index UFeeder_i )
{

    memset(BTSR_TX_Buff, 0, Max_BTSR_TX_Bytes);

    BTSR_TX_Buff[0] = BTSR[UFeeder_i].ID;
    BTSR_TX_Buff[1] = 0x39;

    BTSR_TX_Buff[4] = BTSR[UFeeder_i].Tension & 0XFF;//3
    BTSR_TX_Buff[5] = BTSR[UFeeder_i].Tension >> 8;//4

    BTSR_TX_Buff[22] = BTSR[UFeeder_i].Tension_Err & 0XFF;//21
    BTSR_TX_Buff[23] = BTSR[UFeeder_i].Tension_Err >> 8;//22

    BTSR_TX_Buff[24] = BTSR[UFeeder_i].Alarm_Time & 0XFF;//23
    BTSR_TX_Buff[25] = BTSR[UFeeder_i].Alarm_Time >> 8;//24

    BTSR_TX_Buff[28] = 0x01;//27  ‘1’ TENSION ERROR ENABLED

    BTSR_Calculate_CheckSum(BTSR_TX_Buff,37);

    BTSR_Send_Buf(BTSR_TX_Buff,39);
}

void BTSR_Application_Yarn_Type_Set(UFeeder_Index UFeeder_i)
{
    //APPLICATION_AND_YARN_TYPOLOGY_SET_27
    memset(BTSR_TX_Buff, 0, Max_BTSR_TX_Bytes);

    BTSR_TX_Buff[0] = BTSR[UFeeder_i].ID;
    BTSR_TX_Buff[1] = 0x3F;
    BTSR_TX_Buff[2] = BTSR[UFeeder_i].Application;
    BTSR_TX_Buff[3] = BTSR[UFeeder_i].Yarn_Type;

    BTSR_Calculate_CheckSum(BTSR_TX_Buff,4);

    BTSR_Send_Buf(BTSR_TX_Buff, 6);
}


void BTSR_Status_Reset(UFeeder_Index UFeeder_i)//35
{
    memset(BTSR_TX_Buff, 0, Max_BTSR_TX_Bytes);

    BTSR_TX_Buff[0] = BTSR[UFeeder_i].ID;
    BTSR_TX_Buff[1] = 0x27;

    BTSR_TX_Buff[2] = 0x07;//RESET_0
    BTSR_TX_Buff[3] = 0x00;//RESET_1
    BTSR_TX_Buff[4] = 0x00;//SPARE
    BTSR_TX_Buff[5] = 0x00;//SPARE

    BTSR_Calculate_CheckSum(BTSR_TX_Buff,6);

    BTSR_Send_Buf(BTSR_TX_Buff, 8);

}

void BTSR_Errors_Counter_Reset(UFeeder_Index UFeeder_i)//36
{
    memset(BTSR_TX_Buff, 0, Max_BTSR_TX_Bytes);

    BTSR_TX_Buff[0] = BTSR[UFeeder_i].ID;
    BTSR_TX_Buff[1] = 0x29;
    BTSR_TX_Buff[2] = 0x00;//00 - RESETS ALL THE COUNTERS

    BTSR_Calculate_CheckSum(BTSR_TX_Buff,3);

    BTSR_Send_Buf(BTSR_TX_Buff, 5);

}

void BTSR_Meters_Total_Counter_Reset(UFeeder_Index UFeeder_i , uint8_t Resolution)//37/39/41
{
    memset(BTSR_TX_Buff, 0, Max_BTSR_TX_Bytes);

    BTSR_TX_Buff[0] = BTSR[UFeeder_i].ID;

    if(Resolution == LOW)
        BTSR_TX_Buff[1] = 0x06;
    else
    if(Resolution == HIGH)
        BTSR_TX_Buff[1] = 0x1E;
    else
    if(Resolution == HIGHEST)
        BTSR_TX_Buff[1] = 0x48;

    BTSR_Calculate_CheckSum(BTSR_TX_Buff,2);

    BTSR_Send_Buf(BTSR_TX_Buff, 4);
}

void BTSR_Info_Read(UFeeder_Index UFeeder_i)
{
    memset(BTSR_TX_Buff, 0, Max_BTSR_TX_Bytes);

    BTSR_TX_Buff[0] = BTSR[UFeeder_i].ID;
    BTSR_TX_Buff[1] = 0x3E;

    BTSR_Calculate_CheckSum(BTSR_TX_Buff,2);

    BTSR_Send_Buf(BTSR_TX_Buff, 4);

}

void BTSR_Meters_Total_Counter_Read(UFeeder_Index UFeeder_i , uint8_t Resolution)//37/39/41
{
    memset(BTSR_TX_Buff, 0, Max_BTSR_TX_Bytes);

    BTSR_TX_Buff[0] = BTSR[UFeeder_i].ID;

    if(Resolution == LOW)
        BTSR_TX_Buff[1] = 0x16;
    else
    if(Resolution == HIGH)
        BTSR_TX_Buff[1] = 0x1D;
    else
    if(Resolution == HIGHEST)
        BTSR_TX_Buff[1] = 0x47;

    BTSR_Calculate_CheckSum(BTSR_TX_Buff,2);

    BTSR_Send_Buf(BTSR_TX_Buff, 4);
}

void BTSR_Alarm_and_Enabling_Status_Read(UFeeder_Index UFeeder_i)//30
{
    memset(BTSR_TX_Buff, 0, Max_BTSR_TX_Bytes);

    BTSR_TX_Buff[0] = BTSR[UFeeder_i].ID;
    BTSR_TX_Buff[1] = 0x1C;

    BTSR_Calculate_CheckSum(BTSR_TX_Buff,2);

    BTSR_Send_Buf(BTSR_TX_Buff, 4);
}



typedef union
{
    struct
    {
        uint32_t Uint32;
        uint16_t CheckSum;
    }Data;
    uint8_t Buf[6];
}BTSR_METERS;

BTSR_METERS BTSR_Meters;


#define Get_Set_resp 5
#define Set_Command 6

#define Get_Read_resp 7
#define Read_Command 8

uint8_t BTSR_State_Machine(UFeeder_Index UFeeder_i)
{
    uint8_t status = OK;
    uint8_t i,j;
    float temp = 0.0;

    //BTSR_Init();

    for(i = (MAX_BTSR_READ + 1); i < MAX_BTSR_SET; i++)
    {
        if(BTSR[UFeeder_i].Command[i] == Get_Set_resp)
        {
            if((i >= DEVICE_CALIBRATION_11) && (i <= RESETS_ALL_THE_COUNTERS_36) && (BTSR_RX_Buff[0] != BTSR_RISP_OK))
            {
                    status = ERROR;
                    BTSR[UFeeder_i].Command[i] = BTSR_RX_Buff[0];
                    //print response + command ID
            }
            else if( (i == METERS_TOTAL_COUNTER_RESET_38_40_42)        &&
                     ( ((BTSR[UFeeder_i].Length_Resolution == LOW)     && (BTSR_RX_Buff[1] != 0x06)) ||
                       ((BTSR[UFeeder_i].Length_Resolution == HIGH)    && (BTSR_RX_Buff[1] != 0x1E)) ||
                       ((BTSR[UFeeder_i].Length_Resolution == HIGHEST) && (BTSR_RX_Buff[1] != 0x48))   ) )
            {
                    status = ERROR;
                    BTSR[UFeeder_i].Command[i] = BTSR_RX_Buff[1];
                    //print response + command ID
            }
            else//print response + command ID
                BTSR[UFeeder_i].Command[i] = BTSR_RX_Buff[0];//OK;
        }

        if(BTSR[UFeeder_i].Command[i] == Set_Command)
        {
            switch(i)
            {
                case CONFIGURATION_SET_15:
                    BTSR_Configuration_Set(UFeeder_i);
                break;
                case ADVANCED_TENSION_PROGRAMMING_21:
                    BTSR_Advanced_Tension_Prog(UFeeder_i);
                break;
                case APPLICATION_AND_YARN_TYPOLOGY_SET_27:
                    BTSR_Application_Yarn_Type_Set(UFeeder_i);
                break;
                case ALARM_STATUS_RESET_35:
                    BTSR_Status_Reset(UFeeder_i);
                break;
                case RESETS_ALL_THE_COUNTERS_36:
                    BTSR_Errors_Counter_Reset(UFeeder_i);
                break;
                case METERS_TOTAL_COUNTER_RESET_38_40_42:
                    BTSR_Meters_Total_Counter_Reset(UFeeder_i, BTSR[UFeeder_i].Length_Resolution);
                break;
            }
            BTSR[UFeeder_i].Command[i] = Get_Set_resp;
            return status;//Only one command in cycle
        }
    }


    for(i = 0; i < MAX_BTSR_READ; i++)
    {
        if(BTSR[UFeeder_i].Command[i] == Get_Read_resp)
        {
            // Add CRC check
            if(i == ALARM_AND_ENABLING_STATUS_READ_30)
            {
                for(j = 0; j < 6; j++)
                    BTSR[UFeeder_i].Status.Byte[j] = BTSR_RX_Buff[j];
            }
            else
            if(i == DEVICE_INFORMATION_READ_10)
            {
                for(j = 0; j < 12; j++)
                    BTSR[UFeeder_i].Info.Buf[j] = BTSR_RX_Buff[j];
                status |= Check_Received_CheckSum(BTSR[UFeeder_i].ID, 0x3E, BTSR[UFeeder_i].Info.Buf, 12);
                ReportWithPackageFilter(ThreadFilter,"BTSR VERSION",__FILE__,UFeeder_i,(BTSR[UFeeder_i].Info.Byte.BOARD_HW_VERSION << 8) | BTSR[UFeeder_i].Info.Byte.BOOTLOADER_SW_VERSION,RpMessage,(BTSR[UFeeder_i].Info.Byte.SW_VERSION << 8) | BTSR[UFeeder_i].Info.Byte.SW_UNDER_VERSION,0);
            }
            else
            if(METER_TOTAL_COUNTER_READ_37_39_41)
            {
                for(j = 0; j < 6; j++)
                    BTSR_Meters.Buf[j] = BTSR_RX_Buff[j];
                status |= Check_Received_CheckSum(BTSR[UFeeder_i].ID, 0x16, BTSR_Meters.Buf, 6);

                temp  = (float)(BTSR_Meters.Data.Uint32) * PI * BTSR[UFeeder_i].Radius_Meter;
                if(BTSR[UFeeder_i].Length_Resolution == HIGH)
                    temp /=12.0;
                else
                if(BTSR[UFeeder_i].Length_Resolution == HIGHEST)
                    temp /=750.0;

                BTSR[UFeeder_i].LengthInMeter  = temp;
            }

            memset(BTSR_RX_Buff, 0, Max_BTSR_RX_Bytes);
            BTSR[UFeeder_i].Command[i] = BTSR_RISP_OK;
        }
        //else//?

        if(BTSR[UFeeder_i].Command[i] == Read_Command)
        {
            switch(i)
            {
                case ALARM_AND_ENABLING_STATUS_READ_30:
                    BTSR_Alarm_and_Enabling_Status_Read(UFeeder_i);
                break;
                case DEVICE_INFORMATION_READ_10:
                    BTSR_Info_Read(UFeeder_i);
                break;
                case METER_TOTAL_COUNTER_READ_37_39_41:
                    BTSR_Meters_Total_Counter_Read(UFeeder_i, BTSR[UFeeder_i].Length_Resolution);
                break;
            }

            BTSR[UFeeder_i].Command[i] = Get_Read_resp;
            return status;//Only one command in cycle
        }
    }

        return status;
}

void BTSR_Read_Errors_Status(UFeeder_Index UFeeder_i)//Every 1 second. 3 seconds debouce for on/off
{
    BTSR[UFeeder_i].Command[ALARM_AND_ENABLING_STATUS_READ_30] = Read_Command;
}

void BTSR_Read_Length(UFeeder_Index UFeeder_i, uint8_t Length_Resolution)//Every 100msec during thread movement
{
    BTSR[UFeeder_i].Length_Resolution = Length_Resolution;

    BTSR[UFeeder_i].Command[METER_TOTAL_COUNTER_READ_37_39_41] = Set_Command;
}

void BTSR_Reset_Length(UFeeder_Index UFeeder_i, uint8_t Length_Resolution)//before job
{
    BTSR[UFeeder_i].Length_Resolution = Length_Resolution;

    BTSR[UFeeder_i].Command[METERS_TOTAL_COUNTER_RESET_38_40_42] = Set_Command;
}

void BTSR_Reset_Status(UFeeder_Index UFeeder_i)//
{

    BTSR[UFeeder_i].Command[ALARM_STATUS_RESET_35] = Set_Command;
}

void BTSR_Reset_Counters(UFeeder_Index UFeeder_i)//
{

    BTSR[UFeeder_i].Command[RESETS_ALL_THE_COUNTERS_36] = Set_Command;
}

void BTSR_RML_Settings(UFeeder_Index UFeeder_i, BTSR_APPLICATION Application, BTSR_YARN_TYPE Yarn_Type, uint16_t Tension, uint16_t Tension_Err, uint16_t Alarm_Time )
{
    BTSR[UFeeder_i].Application = Application;
    BTSR[UFeeder_i].Yarn_Type = Yarn_Type;
    BTSR[UFeeder_i].Tension = Tension;
    BTSR[UFeeder_i].Tension_Err = Tension_Err;
    BTSR[UFeeder_i].Alarm_Time = Alarm_Time;

    BTSR[UFeeder_i].Command[ADVANCED_TENSION_PROGRAMMING_21] = Set_Command;
    BTSR[UFeeder_i].Command[APPLICATION_AND_YARN_TYPOLOGY_SET_27] = Set_Command;
}



void BTSR_Init()
{
    UFeeder_Index i = RUFeeder1;

    for(i = RUFeeder1; i < MaxUFeeders; i++)
   {
        BTSR[i].ID = 0x01 + i;
        BTSR[i].Flip_Display = true;
        BTSR[i].DirectionCCW = true;
        BTSR[i].Radius_Meter = BTSR_ROLLER_RADIUS;
        BTSR[i].Lock_Keyboard_Menue = false;

        BTSR[i].Command[CONFIGURATION_SET_15] = Set_Command;
        BTSR[i].Command[DEVICE_INFORMATION_READ_10] = Read_Command;

        BTSR_Reset_Status(i);
        BTSR_Reset_Counters(i);
        BTSR_Reset_Length(i,LOW);
        BTSR_Reset_Length(i,HIGH);
        BTSR_Reset_Length(i,HIGHEST);
   }

#ifdef FORCE_BTSR_CARD_0023 // only for testing on the table
    //Only for testing -------------------------------------------------------------------------------
    BTSR_RML_Settings(RUFeeder1, SHOES, ALL_YARNS3, 1000, 10, 5);//Test!!! ---  need to move to RML
    //BTSR_Reset_Counters(RUFeeder1);
    //BTSR_Reset_Status(RUFeeder1);
    //BTSR_Reset_Length(RUFeeder1, HIGHEST);
    //BTSR_Read_Length(RUFeeder1, HIGHEST);
    BTSR_Read_Errors_Status(RUFeeder1);
    //------------------------------------------------------------------------------------------------
#endif
}