#include "delta_modbus/delta_modbus_tcp.h"
#include <boost/exception/all.hpp>

DELTA_NAMESPACE::PLC::PLC(std::string ip_address, int port, int id)
 : mb_(nullptr) 
{
    mb_ = boost::make_shared<modbus>(ip_address, port);
    mb_->modbus_set_slave_id(id);
}

DELTA_NAMESPACE::PLC::~PLC()
{
    mb_ = nullptr;
}

void DELTA_NAMESPACE::PLC::connect(void) {
    ROS_INFO("Delta PLC is connecting.......");
    try
    {
        if(!mb_->modbus_connect()) {
            ROS_ERROR("Delta PLC connection is error");
            error_num = NOT_CONNECT_TO_PLC;
        } else {
            ros::Duration(0.5).sleep();
            error_num = PLC_RUN_OK;
            ROS_INFO("Delta PLC is Connected");
        } 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (const std::exception& e) 
    {
        ROS_ERROR_STREAM(e.what());
    } 
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    }   
}

void DELTA_NAMESPACE::PLC::close()
{
    try
    {
        mb_->modbus_close();
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (const std::exception& e) 
    {
        ROS_ERROR_STREAM(e.what());
    } 
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    }   
}

bool DELTA_NAMESPACE::PLC::checkConnected()
{
    try
    {
        return mb_->_connected;
    }
    catch (const std::exception& e) 
    {
        ROS_ERROR_STREAM(e.what());
    } 
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    }   
}

int DELTA_NAMESPACE::PLC::writeM(int M_number, const bool status) {
    try {
        if(M_number < M0_NUM || M_number > M4095_NUM) { throw AddressIsNotExistException(M_number); }
        if(M_number >= M0_NUM && M_number <= M1535_NUM) {
            return mb_->modbus_write_coil(M0_HEX + M_number, status);
        }
        else if(M_number >= M1536_NUM && M_number <= M4095_NUM) {
            return mb_->modbus_write_coil(M1536_HEX + M_number, status);
        }
    }    
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("delta::writeM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("delta::writeM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    }   
    return 0;
}

int DELTA_NAMESPACE::PLC::mulWriteM(int start_number, int end_number, bool *status) {
    try {
        if(start_number >= end_number)                          { throw CompareException(); }
        if(start_number < M0_NUM || start_number > M4095_NUM)   { throw AddressIsNotExistException(start_number); }
        if(end_number < M0_NUM || end_number > M4095_NUM)       { throw AddressIsNotExistException(end_number); }
       
        if(start_number >= M0_NUM && end_number <= M1535_NUM) {
            int amount = end_number - start_number + 1;
            bool value[amount];
            for(int i = 0; i < amount; i++) value[i] = status[i];
            return mb_->modbus_write_coils(M0_HEX + start_number, amount,value);
        }
        else if(start_number >= M1536_NUM && end_number <= M4095_NUM) {
            int amount = end_number - start_number + 1;
            bool value[amount];
            for(int i = 0; i < amount; i++) value[i] = status[start_number + i];
            return mb_->modbus_write_coils(M1536_HEX + start_number - M1535_NUM, amount, value);
        }
        else {
            int amount1,amount2;
            amount1 = M1535_NUM - start_number + 1;
            amount2 = end_number - M1536_NUM + 1;
            bool value1[amount1], value2[amount2];
            for(int i = 0; i < amount1; i++) value1[i] = status[i];
            for(int i = 0; i < amount2; i++) value2[i] = status[amount1 + i];
            int n1,n2;
            n1 = mb_->modbus_write_coils(M0_HEX + start_number,amount1,value1);
            n2 = mb_->modbus_write_coils(M1536_HEX, amount2,value2);
            return n1*n2;
        }
    }
    catch(CompareException & e) {
        ROS_ERROR_STREAM("delta::mulWriteM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON;
    } 
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("delta::mulWriteM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception & e) {
        ROS_ERROR_STREAM("delta::mulWriteM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::setM(int M_number) {
    try {
        if(M_number < M0_NUM || M_number > M4095_NUM) { throw AddressIsNotExistException(M_number); }
        if(M_number >= M0_NUM && M_number <= M1535_NUM){
            return mb_->modbus_write_coil(M0_HEX + M_number, ON);
        }
        else if(M_number >= M1536_NUM && M_number <= M4095_NUM) {
            return mb_->modbus_write_coil(M1536_HEX + M_number, ON);
        }
    }
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::setM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::setM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::resetM(int M_number) {
    try {
        if(M_number < M0_NUM || M_number > M4095_NUM) { throw AddressIsNotExistException(M_number); }
        if(M_number >= M0_NUM && M_number <= M1535_NUM){
            return mb_->modbus_write_coil(M0_HEX + M_number, OFF);
        }
        else if(M_number >= M1536_NUM && M_number <= M4095_NUM) {
            return mb_->modbus_write_coil(M1536_HEX + M_number, OFF);
        }
    }
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::setM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::setM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::mulSetM(int start_number, int end_number) {
    try {
        if(start_number >= end_number)                         { throw CompareException(); }
        if(start_number < M0_NUM || start_number > M4095_NUM)  { throw AddressIsNotExistException(start_number); }
        if(end_number < M0_NUM || end_number > M4095_NUM)      { throw AddressIsNotExistException(end_number); }
        
        if(start_number >= M0_NUM && end_number <= M1535_NUM) {
            int amount = end_number - start_number + 1;
            bool value[amount];
            for(int i = 0; i < amount; i++) value[i] = ON;
            return mb_->modbus_write_coils(M0_HEX + start_number, amount,value);
        }
        else if(start_number >= M1536_NUM && end_number <= M4095_NUM) {
            int amount = end_number - start_number + 1;
            bool value[amount];
            for(int i = 0; i < amount; i++) value[i] = ON;
            return mb_->modbus_write_coils(M1536_HEX + start_number - M1535_NUM, amount, value);
        }
        else {
            int amount1,amount2;
            amount1 = M1535_NUM - start_number + 1;
            amount2 = end_number - M1536_NUM + 1;
            bool value1[amount1], value2[amount2];
            for(int i = 0; i < amount1; i++) value1[i] = ON;
            for(int i = 0; i < amount2; i++) value2[i] = ON;
            int n1,n2;
            n1 = mb_->modbus_write_coils(M0_HEX + start_number,amount1,value1);
            n2 = mb_->modbus_write_coils(M1536_HEX, amount2,value2);
            return n1*n2;
        }
    }
    catch(CompareException & e) {
        ROS_ERROR_STREAM("PLC::mulSetM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON;
    } 
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::mulSetM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::mulSetM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::mulResetM(int start_number, int end_number) {
    try {
        if(start_number >= end_number)                         { throw CompareException(); }
        if(start_number < M0_NUM || start_number > M4095_NUM)  { throw AddressIsNotExistException(start_number); }
        if(end_number < M0_NUM || end_number > M4095_NUM)      { throw AddressIsNotExistException(end_number); }
        
        if(start_number >= M0_NUM && end_number <= M1535_NUM) {
            int amount = end_number - start_number + 1;
            bool value[amount];
            for(int i = 0; i < amount; i++) value[i] = OFF;
            return mb_->modbus_write_coils(M0_HEX + start_number, amount,value);
        }
        else if(start_number >= M1536_NUM && end_number <= M4095_NUM) {
            int amount = end_number - start_number + 1;
            bool value[amount];
            for(int i = 0; i < amount; i++) value[i] = OFF;
            return mb_->modbus_write_coils(M1536_HEX + start_number - M1535_NUM, amount, value);
        }
        else {
            int amount1,amount2;
            amount1 = M1535_NUM - start_number + 1;
            amount2 = end_number - M1536_NUM + 1;
            bool value1[amount1], value2[amount2];
            for(int i = 0; i < amount1; i++) value1[i] = OFF;
            for(int i = 0; i < amount2; i++) value2[i] = OFF;
            int n1,n2;
            n1 = mb_->modbus_write_coils(M0_HEX + start_number,amount1,value1);
            n2 = mb_->modbus_write_coils(M1536_HEX, amount2,value2);
            return n1*n2;
        }
    }
    catch(CompareException & e) {
        ROS_ERROR_STREAM("PLC::mulSetM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON;
    } 
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::mulSetM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::mulSetM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::getM(int M_number, bool &buffer) {
    try {
        if(M_number < M0_NUM || M_number > M4095_NUM) { throw AddressIsNotExistException(M_number); }
        
        if(M_number >= M0_NUM && M_number <= M1535_NUM)
        {
            return mb_->modbus_read_coil(M0_HEX + M_number, buffer);
        }
        else if(M_number >= M1536_NUM && M_number <= M4095_NUM){
            return mb_->modbus_read_coil(M1536_HEX + M_number, buffer);
        }
    }
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::getM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::getM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    } 
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::mulGetM(int start_number, int end_number, bool* buffer) {
    try {
        if(start_number >= end_number)                          { throw CompareException(); }
        if(start_number < M0_NUM || start_number > M4095_NUM)  { throw AddressIsNotExistException(start_number); }
        if(end_number < M0_NUM || end_number > M4095_NUM)      { throw AddressIsNotExistException(end_number); }
        
        if(start_number >= M0_NUM && end_number <= M1535_NUM) {
            int amount = end_number - start_number + 1;
            return mb_->modbus_read_coils(M0_HEX + start_number, amount,buffer);
        }
        else if(start_number >= M1536_NUM && end_number <= M4095_NUM) {
            int amount = end_number - start_number + 1;
            return mb_->modbus_read_coils(M1536_HEX + start_number - M1535_NUM, amount,buffer);
        }
        else {
            int amount1,amount2;
            amount1 = M1535_NUM - start_number + 1;
            amount2 = end_number - M1536_NUM + 1;
            bool value1[amount1], value2[amount2];
            int n1,n2;
            n1 = mb_->modbus_read_coils(M0_HEX + start_number,amount1,value1);
            n2 = mb_->modbus_read_coils(M1536_HEX, amount2,value2);
            for(int i = 0; i < amount1; i++) buffer[i] = value1[i];
            for(int i = 0; i < amount2; i++) buffer[amount1 + i] = value2[i];
            return n1*n2;
        }
    }
    catch(CompareException & e) {
        ROS_ERROR_STREAM("PLC::mulGetM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON;
    } 
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::mulGetM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::mulGetM: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::setY(int Y_number) {
    try {
        if(Y_number < Y0_NUM || Y_number > Y377_NUM) { throw AddressIsNotExistException(Y_number); }
        return mb_->modbus_write_coil(Y0_HEX + Y_number, ON);
    }
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::setY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::setY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    } 
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0; 
}

int DELTA_NAMESPACE::PLC::resetY(int Y_number) {
    try {
        if(Y_number < Y0_NUM || Y_number > Y377_NUM) { throw AddressIsNotExistException(Y_number); }
        return mb_->modbus_write_coil(Y0_HEX + Y_number, OFF);
    }
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::setY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::setY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }  
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::mulWriteY(int start_number, int end_number, bool *buffer) {

    try {
        if(start_number >= end_number)                        { throw CompareException(); }
        if(start_number < Y0_NUM || start_number > Y377_NUM)  { throw AddressIsNotExistException(start_number); }
        if(end_number < Y0_NUM || end_number > Y377_NUM)      { throw AddressIsNotExistException(end_number); }
        int amount = end_number - start_number + 1;
        return mb_->modbus_write_coils(Y0_HEX + start_number, amount, buffer);
    }
    catch(CompareException & e) {
        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON;
    } 
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::mulSetY(int start_number, int end_number) {
    try {
        if(start_number >= end_number)                        { throw CompareException(); }
        if(start_number < Y0_NUM || start_number > Y377_NUM)  { throw AddressIsNotExistException(start_number); }
        if(end_number < Y0_NUM || end_number > Y377_NUM)      { throw AddressIsNotExistException(end_number); }
        int amount = end_number - start_number + 1;
        bool value[amount];
        for(int i = 0; i < amount; i++) value[i] = ON;
        return mb_->modbus_write_coils(Y0_HEX + start_number, amount, value);
    }
    catch(CompareException & e) {
        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON;
    } 
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::mulResetY(int start_number, int end_number) {
    try {
        if(start_number >= end_number)                        { throw CompareException(); }
        if(start_number < Y0_NUM || start_number > Y377_NUM)  { throw AddressIsNotExistException(start_number); }
        if(end_number < Y0_NUM || end_number > Y377_NUM)      { throw AddressIsNotExistException(end_number); }
        int amount = end_number - start_number + 1;
        bool value[amount];
        for(int i = 0; i < amount; i++) value[i] = OFF;
        return mb_->modbus_write_coils(Y0_HEX + start_number, amount, value);
    }
    catch(CompareException & e) {
        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON;
    } 
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::mulSetY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::mulGetY(int start_number, int end_number, bool *buffer) {
    try {
        if(start_number >= end_number)                        { throw CompareException(); }
        if(start_number < Y0_NUM || start_number > Y377_NUM)  { throw AddressIsNotExistException(start_number); }
        if(end_number < Y0_NUM || end_number > Y377_NUM)      { throw AddressIsNotExistException(end_number); }
        int amount = end_number - start_number + 1;
        return mb_->modbus_read_coils(Y0_HEX + start_number, amount, buffer);
    }
    catch(CompareException & e) {
        ROS_ERROR_STREAM("PLC::mulGetY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON;
    } 
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::mulGetY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::mulGetY: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }  
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::getD(int D_number, uint16_t &buffer) {
    try 
    {
        if(D_number < D0_NUM || D_number > D4095_NUM) 
            throw AddressIsNotExistException(D_number); 
        return mb_->modbus_read_holding_register(D0_HEX + D_number, buffer);
    }
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::getD: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::getD: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::mulGetD(int start_number, int end_number, uint16_t *buffer) {
    try {
        if(start_number >= end_number)                         { throw CompareException(); }
        if(start_number < D0_NUM || start_number > D4095_NUM)  { throw AddressIsNotExistException(start_number); }
        if(end_number < D0_NUM || end_number > D4095_NUM)      { throw AddressIsNotExistException(end_number); }
        int amount = end_number - start_number + 1;
        return mb_->modbus_read_holding_registers(D0_HEX + start_number, amount, buffer);
    }
    catch(CompareException & e) {
        ROS_ERROR_STREAM("PLC::mulGetD: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON;
    } 
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::mulGetD: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::mulGetD: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::setD(int D_number, int16_t &buffer) {
    try {
        if(D_number < D0_NUM || D_number > D4095_NUM) { throw AddressIsNotExistException(D_number); }
        return mb_->modbus_write_register(D0_HEX + D_number, buffer);
    }
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::setD: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::setD: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::mulSetD(int start_number, int end_number, uint16_t *buffer) {
    try {
        if(start_number >= end_number)                         { throw CompareException(); }
        if(start_number < D0_NUM || start_number > D4095_NUM)  { throw AddressIsNotExistException(start_number); }
        if(end_number < D0_NUM || end_number > D4095_NUM)      { throw AddressIsNotExistException(end_number); }
        int amount = end_number - start_number + 1;
        return mb_->modbus_write_registers(D0_HEX + start_number, amount, buffer);
    }
    catch(CompareException & e) {
        ROS_ERROR_STREAM("PLC::mulSetD: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON;
    } 
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::mulSetD: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::mulSetD: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::getX(int X_number, bool &buffer) {
    try {
        if(X_number < X0_NUM || X_number > X377_NUM) { throw AddressIsNotExistException(X_number); }
        mb_->modbus_read_input_bit(X0_HEX + X_number, buffer);
    }
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::getX: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::getX: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    return 0;
}

/*
 * Read X bit
 * @param start_number  Begin number X
 * @param end_number    End number X
 * @param buffer        Status of X
 */
int DELTA_NAMESPACE::PLC::mulGetX(int start_number, int end_number, bool* buffer) {
    try {
        if(start_number >= end_number)                          { throw CompareException(); }
        if(start_number < X0_NUM || start_number > X377_NUM)    { throw AddressIsNotExistException(start_number); }
        if(end_number < X0_NUM || end_number > X377_NUM)        { throw AddressIsNotExistException(end_number); }
        int amount = end_number - start_number + 1;
        mb_->modbus_read_input_bits(X0_HEX + start_number, amount, buffer);
    }
    catch(CompareException & e) {
        ROS_ERROR_STREAM("PLC::mulGetX: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON;
    } 
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::mulGetX: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::mulGetX: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}

int DELTA_NAMESPACE::PLC::getC(int C_number, uint16_t &buffer) {
    try {
        if(C_number < C0_NUM || C_number > C199_NUM) { throw AddressIsNotExistException(C_number); }
        mb_->modbus_read_holding_register(C0_HEX + C_number, buffer);
    }
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::getC: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::getC: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}
   
int DELTA_NAMESPACE::PLC::mulGetC(int start_number, int end_number, uint16_t *buffer) {
    try {
        if(start_number >= end_number)                          { throw CompareException(); }
        if(start_number < C0_NUM || start_number > C199_NUM)    { throw AddressIsNotExistException(start_number); }
        if(end_number < C0_NUM || end_number > C199_NUM)        { throw AddressIsNotExistException(end_number); }
        int amount = end_number - start_number + 1;
        mb_->modbus_read_holding_registers(C0_HEX + start_number, amount, buffer);
    }
    catch(CompareException & e) {
        ROS_ERROR_STREAM("PLC::mulGetC: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON;
    } 
    catch(AddressIsNotExistException & e) {
        ROS_ERROR_STREAM("PLC::mulGetC: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    }
    catch(std::exception& e) {
        ROS_ERROR_STREAM("PLC::mulGetC: " << e.what());
        ros::Duration(TIME_ERROR).sleep();
        return BAD_CON; 
    } 
    catch(const boost::exception& e)
    {
        ROS_ERROR_STREAM(boost::diagnostic_information(e));
    }
    catch (...) 
    {
        ROS_ERROR_STREAM("Caught unknown exception");
    } 
    return 0;
}


