#include "modbus.h"

static uint8_t buf_ptr = 0;
static unsigned char rx_buf_ptr = 0;
static unsigned char rx_buf[128];
static unsigned char tx_buf[128];

static uint16_t addr_buf_1 = 0, addr_buf_2 = 0;
static uint16_t regs_to_read = 0;
static uint16_t CRC16 = 0;
static unsigned char crc_buf[300];
static unsigned char modbus_id = 0;
static uint16_t reg_wr_data = 0;
static uint16_t modbus_reg_addr;


static uint16_t holding_registers[MAX_HOLDING_REGISTERS];
static uint16_t input_registers[MAX_INPUT_REGISTERS];

static unsigned char rx_flag = 0;
static unsigned char timer_state = 0;


static uint16_t dev_id = DEFAULT_DEVICE_MODBUS_ID;
static uint16_t com_dev_id = 247;

static void (*ModbusUartSendByte)(USART_TypeDef*, unsigned char);
static void (*ModbusTimEnable)(TIM_TypeDef*);
static void (*ModbusTimDisable)(TIM_TypeDef*);
static int32_t (*ModbusTcpSendByte)(uint8_t, unsigned char*, uint16_t);
static void (*ModbusRIRPoll)(void);
static void (*ModbusRHRPoll)(void);
static void (*ModbusWSRPoll)(uint16_t, uint16_t);

#ifdef CURRENT_OPERATION_MODE_UART
void modbus_connect_callback_send_byte(void (*s)(USART_TypeDef*, unsigned char)) {
    ModbusUartSendByte = s;
}
void modbus_connect_callback_tim_en(void (*s)(TIM_TypeDef*)) {
    ModbusTimEnable = s;
}
void modbus_connect_callback_tim_dis(void (*s)(TIM_TypeDef*)) {
    ModbusTimDisable = s;
}
#endif

#ifdef CURRENT_OPERATION_MODE_TCP
void modbus_connect_callback_send_buf_tcp(int32_t (*s)(uint8_t, unsigned char*, uint16_t)) {
    ModbusTcpSendByte = s;
}
#endif

void modbus_connect_callback_rir_poll(void (*s)(void)) {
    ModbusRIRPoll = s;
}

void modbus_connect_callback_rhr_poll(void (*s)(void)) {
    ModbusRHRPoll = s;
}

void modbus_connect_callback_wsr_poll(void (*s)(uint16_t, uint16_t)) {
    ModbusWSRPoll = s;
}

static uint16_t modbus_CRC16(unsigned char buf[], uint16_t len)
{
    /// ������ crc16

    uint16_t crc = 0xFFFF;
    //U8 crc_lsb, crc_msb;
    for (uint16_t pos = 0; pos < len; pos++) {
        crc ^= (uint16_t)buf[pos];          // XOR byte into least sig. byte of crc
        for (int i = 8; i != 0; i--) {    // Loop over each bit
            if ((crc & 0x0001) != 0) {      // If the LSB is set
                crc >>= 1;                // Shift right and XOR 0xA001
                crc ^= 0xA001;
            }
            else                          // Else LSB is not set
                crc >>= 1;                  // Just shift right
        }
    }

    // Note, this number has low and high bytes swapped, 
    // so use it accordingly (or swap bytes)
    // swapping bytes
    crc = ((crc<<8)&0xff00)|((crc>>8)&0x00ff);

    return crc;
}

void modbus_wsr_answer() {
    crc_buf[0] = (modbus_id == dev_id) ? dev_id : com_dev_id;
    crc_buf[1] = (unsigned char) MODBUS_WSR_CMD;
    crc_buf[2] = (unsigned char) (modbus_reg_addr >> 8);
    crc_buf[3] = (unsigned char) (modbus_reg_addr & 0xff);
    crc_buf[4] = (unsigned char) (reg_wr_data >> 8);
    crc_buf[5] = (unsigned char) (reg_wr_data & 0xff);
    
    CRC16 = modbus_CRC16(crc_buf, 6);
    
    #ifdef CURRENT_OPERATION_MODE_UART
	    MODBUS_TX_EN;
	    ModbusUartSendByte(Modbus_UART, (modbus_id == dev_id) ? dev_id : com_dev_id);
	    ModbusUartSendByte(Modbus_UART, (unsigned char)MODBUS_WSR_CMD);
	    ModbusUartSendByte(Modbus_UART, modbus_reg_addr >> 8);
	    ModbusUartSendByte(Modbus_UART, modbus_reg_addr & 0xff);
	    ModbusUartSendByte(Modbus_UART, reg_wr_data >> 8);
	    ModbusUartSendByte(Modbus_UART, reg_wr_data & 0xff);
    
	    ModbusUartSendByte(Modbus_UART, CRC16 >> 8);
	    ModbusUartSendByte(Modbus_UART, CRC16 & 0xff);
	    MODBUS_TX_DIS;
    #endif
    #ifdef CURRENT_OPERATION_MODE_TCP
	    tx_buf[0] = (modbus_id == dev_id) ? dev_id : com_dev_id;
	    tx_buf[1] = MODBUS_WSR_CMD;
	    tx_buf[2] = modbus_reg_addr >> 8;
	    tx_buf[3] = modbus_reg_addr & 0xff;
	    tx_buf[4] = reg_wr_data >> 8;
	    tx_buf[5] = reg_wr_data & 0xff;
	    tx_buf[6] = CRC16 >> 8;
	    tx_buf[7] = CRC16 & 0xff;

	    ModbusTcpSendByte(DEFAULT_TCP_MODBUS_SOCKET, tx_buf, 8);
	#endif
}

void modbus_rir_answer() {
    buf_ptr = 0;
    addr_buf_1 = modbus_reg_addr;
    addr_buf_2 = addr_buf_1;
    
    crc_buf[0] = (modbus_id == dev_id) ? dev_id : com_dev_id;
    crc_buf[1] = MODBUS_RIR_CMD;
    crc_buf[2] = regs_to_read * 2;
    
    unsigned char cnt = CRC_COUNT_OFFSET;
    
    for (int i = 0; i < regs_to_read; i++) {
        crc_buf[cnt++] = (unsigned char)(input_registers[addr_buf_1] >> 8);
        crc_buf[cnt++] = (unsigned char)(input_registers[addr_buf_1] & 0xff);
        ++addr_buf_1;
    }
    
    CRC16 = modbus_CRC16(crc_buf, regs_to_read * 2 + CRC_COUNT_OFFSET);
    
    
    #ifdef CURRENT_OPERATION_MODE_UART
	    MODBUS_TX_EN;
	    ModbusUartSendByte(Modbus_UART, (modbus_id == dev_id) ? dev_id : com_dev_id);
	    ModbusUartSendByte(Modbus_UART, (unsigned char) MODBUS_RIR_CMD);
	    ModbusUartSendByte(Modbus_UART, regs_to_read * 2);
    
	    for (int i = 0; i < regs_to_read; i++) {
		ModbusUartSendByte(Modbus_UART, (unsigned char)(input_registers[addr_buf_2] >> 8));
		ModbusUartSendByte(Modbus_UART, (unsigned char)(input_registers[addr_buf_2] & 0xff));
		++addr_buf_2;
	    }
	    ModbusUartSendByte(Modbus_UART, (unsigned char)(CRC16 >> 8));
	    ModbusUartSendByte(Modbus_UART, (unsigned char)(CRC16 & 0xff));
	    MODBUS_TX_DIS;
    #endif
    #ifdef CURRENT_OPERATION_MODE_TCP
	    tx_buf[buf_ptr++] = (modbus_id == dev_id) ? dev_id : com_dev_id;
	    tx_buf[buf_ptr++] = (unsigned char) MODBUS_RIR_CMD;
	    tx_buf[buf_ptr++] = regs_to_read * 2;

	    for (int i = 0; i < regs_to_read; i++) {
		tx_buf[buf_ptr++] = (unsigned char)(input_registers[addr_buf_2] >> 8);
		tx_buf[buf_ptr++] = (unsigned char)(input_registers[addr_buf_2] & 0xff);
		++addr_buf_2;
	    }
	    tx_buf[buf_ptr++] = CRC16 >> 8;
	    tx_buf[buf_ptr++] = CRC16 & 0xff;
	    ModbusTcpSendByte(DEFAULT_TCP_MODBUS_SOCKET, tx_buf, buf_ptr);
	#endif

}

void modbus_rhr_answer() {
    buf_ptr = 0;
    addr_buf_1 = modbus_reg_addr;
    addr_buf_2 = addr_buf_1;
    
    crc_buf[0] = (modbus_id == dev_id) ? dev_id : com_dev_id;
    crc_buf[1] = MODBUS_RHR_CMD;
    crc_buf[2] = regs_to_read * 2;
    
    unsigned char cnt = CRC_COUNT_OFFSET;
    
    for (int i = 0; i < regs_to_read; i++) {
        crc_buf[cnt++] = (unsigned char)(holding_registers[addr_buf_1] >> 8);
        crc_buf[cnt++] = (unsigned char)(holding_registers[addr_buf_1] & 0xff);
        ++addr_buf_1;
    }
    
    CRC16 = modbus_CRC16(crc_buf, regs_to_read * 2 + CRC_COUNT_OFFSET);
    
    
    #ifdef CURRENT_OPERATION_MODE_UART
	    MODBUS_TX_EN;
	    ModbusUartSendByte(Modbus_UART, (modbus_id == dev_id) ? dev_id : com_dev_id);
	    ModbusUartSendByte(Modbus_UART, (unsigned char) MODBUS_RHR_CMD);
	    ModbusUartSendByte(Modbus_UART, regs_to_read * 2);
    
	    for (int i = 0; i < regs_to_read; i++) {
		ModbusUartSendByte(Modbus_UART, (unsigned char)(holding_registers[addr_buf_2] >> 8));
		ModbusUartSendByte(Modbus_UART, (unsigned char)(holding_registers[addr_buf_2] & 0xff));
		++addr_buf_2;
	    }
	    ModbusUartSendByte(Modbus_UART, (unsigned char)(CRC16 >> 8));
	    ModbusUartSendByte(Modbus_UART, (unsigned char)(CRC16 & 0xff));
	    MODBUS_TX_DIS;
    #endif
    #ifdef CURRENT_OPERATION_MODE_TCP
            tx_buf[buf_ptr++] = (modbus_id == dev_id) ? dev_id : com_dev_id;
            tx_buf[buf_ptr++] = (unsigned char) MODBUS_RHR_CMD;
            tx_buf[buf_ptr++] = regs_to_read * 2;

            for (int i = 0; i < regs_to_read; i++) {
                tx_buf[buf_ptr++] = (unsigned char)(holding_registers[addr_buf_2] >> 8);
                tx_buf[buf_ptr++] = (unsigned char)(holding_registers[addr_buf_2] & 0xff);
                ++addr_buf_2;
            }
            tx_buf[buf_ptr++] = CRC16 >> 8;
            tx_buf[buf_ptr++] = CRC16 & 0xff;
            ModbusTcpSendByte(DEFAULT_TCP_MODBUS_SOCKET, tx_buf, buf_ptr);
	#endif
}

static unsigned char modbus_rx_CRC_check(unsigned char modbus_cmd) {
    /// ���������� ������� CRC ��� �������� � ��������� � �����������  
    uint16_t CRC16_calc = 0;  // ���������� ����������� �����
    unsigned char ans = 0;

    modbus_reg_addr = (uint16_t)((rx_buf[2] << 8) | rx_buf[3]); // get starting reg addr

    crc_buf[0] = (modbus_id == dev_id) ? dev_id : com_dev_id;
    crc_buf[2] = (unsigned char)(modbus_reg_addr >> 8);
    crc_buf[3] = (unsigned char)(modbus_reg_addr & 0x00ff);


    switch(modbus_cmd) {
        case MODBUS_WSR_CMD:
            reg_wr_data	= (rx_buf[4] << 8) | rx_buf[5]; // get data to write into reg

            crc_buf[1] = (unsigned char) MODBUS_WSR_CMD;
            crc_buf[4] = (unsigned char)(reg_wr_data >> 8);
            crc_buf[5] = (unsigned char)(reg_wr_data & 0x00ff);
        break;
        //----
        case MODBUS_RHR_CMD:
            regs_to_read = (rx_buf[4] << 8) | rx_buf[5]; // get number of regs to read

            crc_buf[1] = (unsigned char) MODBUS_RHR_CMD;
            crc_buf[4] = (unsigned char)(regs_to_read >> 8);
            crc_buf[5] = (unsigned char)(regs_to_read & 0x00ff);
        break;
        //----
        case MODBUS_RIR_CMD:
            regs_to_read = (rx_buf[4] << 8) | rx_buf[5]; // get number of regs to read

            crc_buf[1] = (unsigned char) MODBUS_RIR_CMD;
            crc_buf[4] = (unsigned char)(regs_to_read >> 8);
            crc_buf[5] = (unsigned char)(regs_to_read & 0x00ff);
        break;
        //----
        default: break;
    }

    CRC16 = (rx_buf[6] << 8) | rx_buf[7]; // get CRC16 from rx msg
    CRC16_calc = modbus_CRC16(crc_buf,6); // calc CRC16  

    if(CRC16_calc == CRC16) 
        ans = modbus_cmd;

    return ans;
}

unsigned char modbus_get_poll(void)  
{
    /// update modbus regs and vars, send answer to master
    unsigned char rx_cmd_code = 0;        // ??? ??????????? ???????rx_cmd_code = 0;
    // state 1 and 2, transmit end, rx buf has > 7 bytes ?
    //
    if ( rx_flag == 1 ) {
        if( rx_buf_ptr > 7 ) { 
            modbus_id = rx_buf[0];      // get device ID from master msg
            
            if((modbus_id == dev_id) || (modbus_id == com_dev_id)) {
                switch(rx_buf[1]) {   
                    case MODBUS_RHR_CMD:                // ???? ??????? - ?????? R/W ?????????
                        if(modbus_rx_CRC_check(MODBUS_RHR_CMD) == MODBUS_RHR_CMD) {
                            rx_cmd_code = MODBUS_RHR_CMD;
                        }
                    break;
                    ////-------------------------------------------------------------------
                    case MODBUS_WSR_CMD:                // ???? ??????? - ?????? Read-only ?????????
                        if(modbus_rx_CRC_check(MODBUS_WSR_CMD) == MODBUS_WSR_CMD) {
                            rx_cmd_code = MODBUS_WSR_CMD;
                            holding_reg_write(modbus_reg_addr, reg_wr_data); // !!!! 
                        }
                    break;
                    ////-------------------------------------------------------------------
                    case MODBUS_RIR_CMD: 
                        if(modbus_rx_CRC_check(MODBUS_RIR_CMD) == MODBUS_RIR_CMD) {
                            rx_cmd_code = MODBUS_RIR_CMD;
                        }
                    break;
                } // switch(rx_buf[1])
                modbus_reset();
            } else {
                modbus_reset();
            }// if dev_id
        } else { modbus_reset(); } //  if(rx_buf_pos > 7)
    }

    return rx_cmd_code; 
}

void modbus_reset(void)
{ 
    for(int i = 0; i < 128; i++) 
        rx_buf[i] = 0;

    rx_buf_ptr = 0;
    rx_flag = 0;
}

void holding_reg_write(uint16_t red_addr, uint16_t value) {
    holding_registers[red_addr] = value;
}

void holding_reg_read(uint16_t red_addr, uint16_t *usr_var_ptr) {
    *usr_var_ptr = holding_registers[red_addr];
}

void input_reg_read(uint16_t red_addr, uint16_t *usr_var_ptr) {
    *usr_var_ptr = input_registers[red_addr];
}
  
void input_reg_write(uint16_t reg_addr, uint16_t value) {
    input_registers[reg_addr] = value;
}

uint16_t get_wr_reg_addr(void) {
    return modbus_reg_addr;
}

uint16_t get_wr_reg_val(void) {
    return reg_wr_data;
}

unsigned char get_modbus_id(void) {
    return dev_id;
}

void set_modbus_id(unsigned char newID)
{
    dev_id = newID;
    modbus_reset();
}

unsigned char get_modbus_broadcast_id(void) {
    return com_dev_id;
}

void set_modbus_broadcast_id(unsigned char newID)
{
    com_dev_id = newID;
    modbus_reset();
}

static uint16_t RegisterAddr  = 0;
static uint16_t RegisterValue = 0;

void modbus_poll(void) {
    uint8_t mb_get_poll = modbus_get_poll();
    switch(mb_get_poll) { 
        //////////////////////////// READING INPUTS  ////////////////////////// 
        case MODBUS_RIR_CMD:
            MODBUS_LED_ON;
            ModbusRIRPoll();
            modbus_rir_answer();
            MODBUS_LED_OFF;
        break;
        //////////////////////////// READING HOLDING ////////////////////////// 
        case MODBUS_RHR_CMD:
            MODBUS_LED_ON;
            ModbusRHRPoll();
	    modbus_rhr_answer(); 
            MODBUS_LED_OFF;
        break;
        //////////////////////////// WRITING HOLDING ////////////////////////// 
        case MODBUS_WSR_CMD:
            MODBUS_LED_ON;
            modbus_wsr_answer();
        
            RegisterAddr  = get_wr_reg_addr();  // get address
            RegisterValue = get_wr_reg_val();   // get the new value

            ModbusWSRPoll(RegisterAddr, RegisterValue);
            MODBUS_LED_OFF;			// LED toggle
       break;
    } // switch
}
//======================= UART HANDLERS =========================//
#ifdef CURRENT_OPERATION_MODE_UART
void Modbus_GetByte(unsigned char byte) {
    if (rx_buf_ptr < 128) {
        rx_buf[rx_buf_ptr++] = byte;
    }
    
    if (timer_state == 0) {
        timer_state = 1;
        ModbusTimEnable(Modbus_TIM);
    }
}

void Modbus_TIM_Handler(void) {
    ModbusTimDisable(Modbus_TIM);
    timer_state = 0;
    rx_flag = 1;
}
#endif


//======================== TCP HANDLERS ========================//
#ifdef CURRENT_OPERATION_MODE_TCP
void Modbus_TCP_Handler(unsigned char* buf, uint8_t size) {
    for (rx_buf_ptr = 0; rx_buf_ptr < size; rx_buf_ptr++) {
    	rx_buf[rx_buf_ptr] = buf[rx_buf_ptr];
    }

    rx_flag = 1;
}
#endif