#include "modbus.h"

/** @ingroup util_crc16
    Processor-independent CRC-16 calculation.

    Polynomial: x^16 + x^15 + x^2 + 1 (0xA001)<br>
    Initial value: 0xFFFF

    This CRC is normally used in disk-drive controllers.

    @param uint16_t crc (0x0000..0xFFFF)
    @param uint8_t a (0x00..0xFF)
    @return calculated CRC (0x0000..0xFFFF)
*/
uint16_t crc16_update(uint16_t crc, uint8_t a)
{
    int i;
    crc ^= a;
    for (i = 0; i < 8; ++i) {
	if (crc & 1)
	    crc = (crc >> 1) ^ 0xA001;
	else
	    crc = (crc >> 1);
    }

    return crc;
}

void ClearResponseBuffer(modbus_slave_device *device)
{
    memset(device->response_buffer, 0, sizeof(device->response_buffer));
}

MB_StatusTypeDef ReadHoldingRegisters(modbus_slave_device *device, uint16_t reg_addr, int read_num)
{
    return modbus_transaction(read_holding_regs, device, reg_addr, read_num, 0);
}

MB_StatusTypeDef ReadInputRegisters(modbus_slave_device *device, uint16_t reg_addr, int read_num)
{
    return modbus_transaction(read_input_regs, device, reg_addr, read_num, 0);
}

MB_StatusTypeDef WriteSingleRegister(modbus_slave_device *device, uint16_t reg_addr, uint16_t write_val)
{
    return modbus_transaction(write_single_reg, device, reg_addr, 1, write_val);
}

MB_StatusTypeDef modbus_transaction(uint8_t func, modbus_slave_device *device,
				    uint16_t rw_addr, int read_num, uint16_t write_value)
{
    uint16_t adu[256], adu_currsize=0;
    MB_StatusTypeDef MB_status = MB_OK;
    
    /* Add the values that are relevant to all (supported) functions to the buffer */
    adu[adu_currsize++] = device->slave_id;
    adu[adu_currsize++] = func;
    adu[adu_currsize++] = highByte(rw_addr);
    adu[adu_currsize++] = lowByte(rw_addr);

    /* Put data in the buffer based on function code */
    switch (func) {
    case read_holding_regs:
    case read_input_regs:
	adu[adu_currsize++] = highByte(read_num);
	adu[adu_currsize++] = lowByte(read_num);
	break;
    case write_single_reg:
	adu[adu_currsize++] = highByte(write_value);
	adu[adu_currsize++] = lowByte(write_value);
    }


    /* Calculate and append the CRC16 to the buffer */
    uint16_t crc = 0xffff;
    for (int i=0; i < adu_currsize; i++) {
	crc = crc16_update(crc, adu[i]);
    }
    
    adu[adu_currsize++] = lowByte(crc);
    adu[adu_currsize++] = highByte(crc);
    adu[adu_currsize] = 0;
    
    /* Send data using HAL UART  */
    for (uint8_t i=0; i < adu_currsize; i++) {
	MB_status = MB_UART_Tx(&device->modbus_uart, &adu[i], (uint8_t*)1, &device->modbus_timeout);
    }
    
    
    /* Reset the ADU currsize pointer in preperation of data RX */
    adu_currsize = 0;

    int rx_curr_bytesize = 8;
    while (rx_curr_bytesize > 0 && MB_status == MB_OK) {
	MB_status = MB_UART_Rx(&device->modbus_uart, &adu[adu_currsize++],
				 (uint8_t*)1, &device->modbus_timeout);
	if (MB_status == MB_OK) {	    
	    rx_curr_bytesize--;
	    if (adu_currsize == 5) {
		if (adu[0] != device->slave_id) {
		    /* Return invalid slave ID error */
		    MB_status = MB_SLAVE_ID_ERR;
		    break;
		}
		if ((adu[1] & 0x7f) != func) {
		    /* Return invalid function error */
		    MB_status = MB_FUNC_ERR;
		    break;
		}

		/* Need to read n more bytes based on the function code */
		switch (adu[1]) {
		case read_holding_regs:
		case read_input_regs:
		    rx_curr_bytesize = adu[2];
		    break;
		case write_single_reg:
		    rx_curr_bytesize = 3;
		    break;
		}
	    }
	}
    }

    if (adu_currsize > 5 && MB_status == MB_OK) {
	crc = 0xffff;
	for (int i=0; i < adu_currsize - 2; i++) {
	    crc = crc16_update(crc, adu[i]);	    
	}
	if((lowByte(crc) != adu[adu_currsize - 2] ||
	    highByte(crc) != adu[adu_currsize - 1])) {
	    /* Invalid CRC error */
	    MB_status = MB_CRC_ERR;
	}
    }

    /* For read operations: disassemble ADU into words */
    if (MB_status == MB_OK) {
	switch (adu[1]) {
	case read_input_regs:
	case read_holding_regs:
	    for (int i=0; i < (adu[2] >> 1); i++) {
		if (i < MAX_BUFSIZE) {
		    device->response_buffer[i] = word(adu[2*i+3], adu[2*i+4]);
		}
		device->rbuf_len = i;
	    }
	    break;
	}
    }    
    
    return MB_status;
}