#include "modbus_rtu/modbus_rtu.h"

/** 
 * @brief Constructor 
 * @param[in] poly      The poly number to caculartion CRC16
 * @param[in] devfile   Example /dev/tty* 
 * @param[in] baud      Number of transmitted bits  per a second
 * @param[in] parity    The parity check bit {Even, None , Old }
 * @param[in] data_bit  Number of bits in a transmission frame 
 * @param[in] stop_bit  End bit
 */
modbus_rtu::modbus_rtu(uint16_t poly, const char* devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit) : _poly(poly), rs485(devfile, baud, parity, data_bit, stop_bit){
    err = false;
    err_no = 0;
    error_msg = "";
    ROS_INFO("Set POLY value is 0x%x", _poly);
    if(setCharacterNumToRead(5) < 0) {setConfigFaild();}
}      

/** 
 * Destructor.
 */                       
modbus_rtu::~modbus_rtu(){}

/**
 * Read Holding Registers
 * MODBUS FUNCTION 0x03
 * @param[in] slave_id   The id on modbus device
 * @param[in] address    Reference Address
 * @param[in] amount     Amount of Registers to Read
 * @param[in] buffer     Buffer to Store Data Read from Registers
 */
int modbus_rtu::modbus_read_holding_registers(uint8_t slave_id, int address, int amount, uint16_t *buffer){
    if(_connected){
        if(address > WORD_BITS || amount > DATA_BITS){
            set_bad_input();
            return EX_BAD_DATA;
        }
        modbus_read(slave_id, address, amount, READ_HOLDING_REGS);
        //if(setCharacterNumToRead(5 + amount * 2) < 0) {setConfigFaild(); return BAD_CONFIG;}
        uint8_t to_rec[MAX_MSG_LENGTH];
        memset(to_rec,'\0',MAX_MSG_LENGTH);
        ssize_t k = 0;
        k = modbus_receive(to_rec);
        if (k == -1 || k == 65535) {return BAD_CON;}
        if(k == 5 + amount * 2) {
            if(!checkCRC16(to_rec,k)){
                ROS_ERROR("modbus_read_holding_registers");
                set_bad_input();
                return EX_BAD_DATA;
            }
            modbuserror_handle(to_rec, READ_HOLDING_REGS);
            if(err) return err_no;
            for(int i = 0; i < to_rec[2u]/2; i++){
                buffer[i] = ((uint16_t)to_rec[3u + 2u * i]) << 8u;
                buffer[i] |= ((uint16_t)to_rec[4u + 2u * i] & 0x00FFu);
            }
            return 0;
        } else {
                ROS_ERROR("receive message is not correct %d ", (int)k);
                for(int i = 0; i < k; i++) printf("0x%x ",to_rec[i]);
                printf("\n");
                set_bad_input();
                return EX_BAD_DATA;
            }
    } else {
        return BAD_CON;
    }
}

/**
 * Read Input Registers
 * MODBUS FUNCTION 0x04
 * @param[in] slave_id    The id on modbus device 
 * @param[in] address     Reference Address
 * @param[in] amount      Amount of Registers to Read
 * @param[in] buffer      Buffer to Store Data Read from Registers
 */
int modbus_rtu::modbus_read_input_registers(uint8_t slave_id, int address, int amount, uint16_t *buffer){
    if(_connected){
        if(amount > DATA_BITS || address > WORD_BITS){
            set_bad_input();
            return EX_BAD_DATA;           
        }
        modbus_read(slave_id, address, amount, READ_INPUT_REGS);
        //if(setCharacterNumToRead(5 + amount * 2) < 0) {setConfigFaild(); return BAD_CONFIG;}
        uint8_t to_rec[MAX_MSG_LENGTH];
        memset(to_rec,'\0',MAX_MSG_LENGTH);
        ssize_t k = 0;
        k = modbus_receive(to_rec);
        if (k == -1 || k == 65535) {return BAD_CON;}
        if(k > 0){
            if(!checkCRC16(to_rec,k)) {
                ROS_ERROR("modbus_read_input_registers");
                set_bad_input();
                return EX_BAD_DATA;
            }
            modbuserror_handle(to_rec, READ_INPUT_REGS);
            if(err) return err_no; 
            for(int i = 0; i < to_rec[2u]/2; i++){
                buffer[i] = ((uint16_t)to_rec[3u + 2u * i]) << 8u;
                buffer[i] |= ((uint16_t)to_rec[4u + 2u * i] & 0x00FFu);
            }
            return 0;
        } else {
                ROS_ERROR("Slave no response !!");
                set_bad_input();
                return EX_BAD_DATA;
            }
    } else {
        return BAD_CON;
    }
}

/**
 * Write Single Register
 * @brief FUCTION 0x06
 * @param[in] slave_id      The id on modbus device 
 * @param[in] address       Reference Address
 * @param[in] value         Value to Be Written to Register
 */
int modbus_rtu::modbus_write_register(uint8_t slave_id, int address, const uint16_t& value) {
    if(_connected){
        if(address > WORD_BITS){
            set_bad_input();
            return EX_BAD_DATA;
        }
        modbus_write(slave_id, address, 1, WRITE_SINGLE_HOLDING_REG, &value);
        //if(setCharacterNumToRead(8) < 0 ) {setConfigFaild(); return BAD_CONFIG;}
        uint8_t to_rec[MAX_MSG_LENGTH];
        memset(to_rec,'\0',MAX_MSG_LENGTH);
        ssize_t k = 0;
        k = modbus_receive(to_rec);
        if (k == -1 || k == 65535) {return BAD_CON;}
        if(k == 8){ 
            // for(int i = 0; i < k; i++) ROS_INFO("0x%x",to_rec[i]);
            if(!checkCRC16(to_rec, k)) {
                ROS_ERROR("modbus_write_register");
                set_bad_input();
                return EX_BAD_DATA;
            }
            modbuserror_handle(to_rec, WRITE_SINGLE_HOLDING_REG);
            if(err) return err_no;
            return 0;
        } else {
                ROS_ERROR("Slave no response !!");
                set_bad_input();
                return EX_BAD_DATA;
            }
    } else {
        return BAD_CON;
    }
}

/**
 * Write Multiple Registers
 * @brief MODBUS FUNCION 0x10
 * @param[in] slave_id      The id on modbus device
 * @param[in] address   Reference Address
 * @param[in] amount    Amount of Value to Write
 * @param[in] value     Values to Be Written to the Registers
 */
int modbus_rtu::modbus_write_registers(uint8_t slave_id, int address, int amount, const uint16_t *value){
    if(_connected){
        if(amount > DATA_BITS || address > WORD_BITS){
            set_bad_input();
            return EX_BAD_DATA;
        }
        modbus_write(slave_id, address, amount, WRITE_MULTIPLE_HOLDING_REGS, value);
        //if(setCharacterNumToRead(8) < 0) {setConfigFaild(); return BAD_CONFIG;}
        uint8_t to_rec[MAX_MSG_LENGTH];
        memset(to_rec,'\0',MAX_MSG_LENGTH);
        ssize_t k = 0;
        k = modbus_receive(to_rec);
        if (k == -1 || k == 65535) {return BAD_CON;}
        if(k == 8) { 
            if(!checkCRC16(to_rec, k)){
                ROS_ERROR("modbus_write_registers");
                set_bad_input();
                return EX_BAD_DATA;
            }
            modbuserror_handle(to_rec, WRITE_MULTIPLE_HOLDING_REGS);
            if(err) return err_no;
            return 0;
        } else {
                ROS_ERROR("Slave no response !!");
                set_bad_input();
                return EX_BAD_DATA;
            }
    } else {
        return BAD_CON;
    }
}

/**
 * @brief Mb_calcul_crc : compute the crc of a packet and put it at the end
 * @param[in] msg         Message to send 
 * @param[in] length      The length of message to send
 * @param[in] poly        The poly number to caculartion CRC16
 * @return CRC            Result is CRC value 16 bit
 */
uint16_t modbus_rtu::getCRC16(uint8_t *to_send, uint16_t length){
    /*
        EX: Ban đầu CRC = 1111 1111 1111 1111 chuyển sang Hex là FFFF
        Chọn data_p là 54 hay 0101 0100(1 byte) là số cần tính. 
        Chọn số poly =  A001h hay 1010 000 000 0001 
        (Poly là một số mà bạn sử dụng làm phép tính số CRC cơ sở của mình.)

        + Bước 1: Dịch CRC và data_p sang phải 1 bit 
        data_p = 54, là 0101 0100 trở thành 0010 1010 
        CRC = 1111 1111 1111 1111 trở thành 0111 1111 1111 1111
        
        + Bước 2: Kiểm tra BIT ngoài cùng bên phải của Dữ liệu và so sánh nó với một trong các CRC
        NẾU chúng bằng nhau, dịch chuyển CRC sang phải 1 bit 
        NẾU chúng không phải, dịch chuyển CRC sang phải 1 bit VÀ cộng thêm số Poly một lần nữa.
        Thực hiện bước 2 đúng 8 lần vì 1 byte có 8 bit.

        +Bước 3: Bước 1 và 2 sẽ được lăp lại theo số lượng data_p.
    */
    unsigned char i;
	unsigned int data;
	unsigned int crc = 0xffff;
    do {
	    for (i=0, data=(unsigned int)0xff & *to_send++; i < 8; i++, data >>= 1) {
	        if ((crc & 0x0001) ^ (data & 0x0001))
	            crc = (crc >> 1) ^ _poly;
	        else  crc >>= 1;
	    }
	} while (--length);
	return (crc);
}

/**
 * @brief Mb_calcul_crc : compute the crc of a packet and put it at the end
 * @param[in] msg         Message to send 
 * @param[in] length      The length of message to send
 * @param[in] poly        The poly number to caculartion CRC16
 * @return bool           Result is CRC value true/false
 */
bool modbus_rtu::checkCRC16(uint8_t *to_send, uint16_t length){
    uint16_t to_check;
    to_check = getCRC16(to_send,length - 2);
    for(int i = 0; i < length; i++) printf("0x%x ",to_send[i]);
    printf("length(%d) crc16(0x%x 0x%x) 0x%x 0x%x ", (int)length, to_check & 0x00FFu, to_check >> 8u, to_send[length - 2], to_send[length - 1]);
    printf("\n");
    return to_send[length - 2] == (uint8_t)(to_check & 0x00FFu) && to_send[length - 1] == (uint8_t)(to_check >> 8u);
}

/*
 * Modbus Request Builder
 * @param[in] slave_id   The id on modbus device
 * @param[in] to_send   Message Buffer to Be Sent
 * @param[in] address   Reference Address
 * @param[in] func      Modbus Functional Code
 */
void modbus_rtu::modbus_build_request(uint8_t slave_id, uint8_t *to_send, int address, uint8_t func) const {
    to_send[0] = slave_id;
    to_send[1] = func;
    to_send[2] = (uint8_t) (address >> 8u);
    to_send[3] = (uint8_t) (address & 0x00FFu);
}

/*
 * Write Request Builder and Sender
 * @param[in] slave_id   The id on modbus device
 * @param[in] address   Reference Address
 * @param[in] amount    Amount of data to be Written
 * @param[in] func      Modbus Functional Code
 * @param[in] value     Data to Be Written
 */
ssize_t modbus_rtu::modbus_write(uint8_t slave_id, int address, uint amount, int func, const uint16_t *value) {
    ssize_t result = 0;
    if(func == WRITE_SINGLE_COIL || func == WRITE_SINGLE_HOLDING_REG){
        uint8_t to_send[8];
        modbus_build_request(slave_id, to_send, address, func);
        to_send[4] = (uint8_t) (value[0] >> 8u);
        to_send[5] = (uint8_t) (value[0] & 0x00FFu);
        uint16_t crc = getCRC16(to_send,6);
        to_send[6] = (uint8_t) (crc & 0x00FFu);
        to_send[7] = (uint8_t) (crc >> 8u);
        // for(int i = 0; i < 8; i++) ROS_INFO("0x%x", to_send[i]);
        result = modbus_send(to_send,8);
    } else if(func == WRITE_MULTIPLE_HOLDING_REGS){
        uint8_t to_send[9 + 2 * amount];
        modbus_build_request(slave_id,to_send, address, func);
        to_send[4] = (uint8_t)(amount >> 8u);
        to_send[5] = (uint8_t)(amount & 0x00FFu);
        to_send[6] = (uint8_t)(amount * 2);
        for(int i = 0; i < amount; i++){
            to_send[7 + 2 * i] = (uint8_t)(value[i] >> 8u);
            to_send[8 + 2 * i] = (uint8_t)(value[i] & 0x00FFu);
        }
        uint16_t crc = getCRC16(to_send, 9 + 2 * amount - 2);
        to_send[9 + 2 * amount - 2] = (uint8_t)(crc & 0x00FFu);
        to_send[9 + 2 * amount - 1] = (uint8_t)(crc >> 8u);
        //for(int i = 0; i < 9 + 2 * amount; i++) ROS_INFO("0x%x", to_send[i]);
        result = modbus_send(to_send, 9 + 2 * amount);
    }
    return result;
}
/*
 * Read Request Builder and Sender
 * @param[in] slave_id   The id on modbus device
 * @param[in] address   Reference Address
 * @param[in] amount    Amount of Data to Read
 * @param[in] func      Modbus Functional Code
 */
ssize_t modbus_rtu::modbus_read(uint8_t slave_id, int address, uint amount, int func){
    uint8_t to_send[8];
    modbus_build_request(slave_id, to_send, address, func);
    to_send[4] = (uint8_t) (amount >> 8u);
    to_send[5] = (uint8_t) (amount & 0x00FFu);
    uint16_t crc = getCRC16(to_send,6);
    to_send[6] = (uint8_t) (crc & 0x00FFu);
    to_send[7] = (uint8_t) (crc >> 8u);
    return modbus_send(to_send, 8);
}

/*
 * Data Sender
 * @param[in] to_send Request to Be Sent to Server
 * @param[in] length  Length of the Request
 * @return        Size of the request
 */
ssize_t modbus_rtu::modbus_send(uint8_t *to_send, uint16_t length){
    struct stat sb;
    if(stat(ctx._port, &sb) < 0) reconnect();
    std::cout << std::endl; 
    return sendMsgs(to_send, length);
}

/*
 * Data Receiver
 * @param[in] buffer Buffer to Store the Data Retrieved
 * @return       Size of Incoming Data
 */
ssize_t modbus_rtu::modbus_receive(uint8_t *buffer) {
    struct timeval timeout;
    timeout.tv_sec = 2;
	timeout.tv_usec = 0;
    fd_set set;
    FD_ZERO(&set); /* clear the set */
	FD_SET(ctx.s, &set); /* add our file descriptor to the set */
    int rv = select(ctx.s + 1, &set, NULL, NULL, &timeout);
    if(rv == -1){
        err = true;
        _connected = false;
        error_msg = "Bad connection select";
        return BAD_CON;
    } else if(rv == 0) {
        err = true;
        _connected = false;
     	error_msg = "Bad connection timeout";  /* an timeout occured */
        return BAD_CON;
    } else
        return receiveMsgs(buffer);
}

/*
 * Error Code Handler
 * @param[in] msg   Message Received from the Server
 * @param[in] func  Modbus Functional Code
 */
void modbus_rtu::modbuserror_handle(const uint8_t *msg, int func){
    if(msg[1] == func + 0x80){
        err = true;
        err_no = 1;
        switch(msg[2]){
        case EX_PARAM_NOT_EXIT:
            error_msg = "The parameter number does not exits";
            break;
        case EX_ACCESS_TO_PARAM:
            error_msg = "There is no write access to the parameter";
            break;
        case EX_PARAM_LIMITS:
            error_msg = "The data value exceeds the parameter limits";
            break;
        case EX_SUB_INDEX_NO_EXIT:
            error_msg = "The sub-index in use does not exits";
            break;
        case EX_PARAM_NOT_TYPE:
            error_msg = "The parameter is not of the array type";
            break;
        case EX_TYPE_NO_MATCH:
            error_msg = "The data type does not match the parameter called";
            break;
        case EX_ONLY_RESET:
            error_msg = "Only reset";
            break;
        case EX_NOT_CHANGEABLE:
            error_msg = "Not changeable";
            break;
        case EX_NO_WRITE_ACCESS:
            error_msg = "No write address";
            break;
        case EX_NO_POSSIBLE_PRE_MODE:
            error_msg = "Data change in the parameter called is not possible in the present mode";
            break;
        case EX_OTHER_ERROR:
            error_msg = "Other error";
            break;      
        case EX_INVALID_DATA_ADDR:
            error_msg = "Invalid data address";
            break; 
        case EX_INVALID_MSGS_LENGTH:
            error_msg = "Invalid message length";
            break; 
        case EX_INVALID_LENGTH_VALUE:
            error_msg = "Invalid data length or value";
            break; 
        case EX_INVALID_FUNC_CODE:
            error_msg = "Invalid function code";
            break; 
        case EX_NO_BUS_ACCESS:
            error_msg = "There is no bus access";
            break; 
        case EX_DATA_NOT_POSSIBLE:
            error_msg = "Data change is not possible because factory set-up is selected";
            break;  
        }
    }
    err = false;
    error_msg = "NO ERR";
}

/*
 * Set Bad Data lenght or Address
 */
void modbus_rtu::set_bad_input(void) {
    err = true;
    error_msg = "Bad Data lenght or Address";
}

/*
 * Set Bad connection
 */
void modbus_rtu::set_bad_con(void) {
    err = true;
    _connected = false;
    error_msg = "Bad connection";
}

/**
 * Set config faild
 */
void modbus_rtu::setConfigFaild(void){
    err = true;
    _connected = false;
    error_msg = "Driver config failded";
}
