#include "ConnectionConfig.h"
#include "ModbusAdapter.h"

#include <cstring>  /// Added for memset functionality

ModbusAdapter::ModbusAdapter()
    : m_modbus( nullptr )
{
    this->InitBuffers();
}

ModbusAdapter::~ModbusAdapter()
{

}

bool ModbusAdapter::ModbusConnect( const ConnectionConfig &conncfg )
{
    if( m_connected )
    {
        this->ModbusDisconnect();   // Will already set m_connected
    }

    m_connType = conncfg.getType();

    switch( m_connType )
    {
        case ConnectionType::CT_SERIAL:
            m_connected = this->ModbusConnectRTU( conncfg.getPort(), conncfg.getBaudRate(), conncfg.getParity(), conncfg.getDataBits(), conncfg.getStopBits(), conncfg.getTimeOut(), MODBUS_RTU_RTS_NONE );
            break;

        case ConnectionType::CT_TCP:
            m_connected = this->ModbusConnectTCP( conncfg.getIpAddress(), conncfg.getTcpPort(), 10 );
            break;

        default:
            // throw a sensible message or return an errorcode.
            break;
    }
    return m_connected;
}

bool ModbusAdapter::ModbusDisconnect()
{
    if( m_modbus != nullptr )
    {
        if( m_connected )
        {
            modbus_close( m_modbus );
            modbus_free( m_modbus );
        }
        // Clean up after ourselves.
        m_modbus = nullptr;
    }

    m_connected = false;
    return true;
}

modbusData ModbusAdapter::ModbusReadData( int slaveId, int functionCode, int startAddress, int noOfItems )
{
    if( m_modbus == nullptr )
    {
        // No context
        return modbusData();
    }

    int resultValue = -1;
    bool is16Bit = false;

    modbus_set_slave( m_modbus, slaveId );

    // Request data from modbus.
    switch( functionCode )
    {
        case MODBUS_FC_READ_COILS:
            resultValue = modbus_read_bits( m_modbus, startAddress, noOfItems, m_dest );
            break;
        case MODBUS_FC_READ_DISCRETE_INPUTS:
            resultValue = modbus_read_input_bits( m_modbus, startAddress, noOfItems, m_dest );
            break;
        case MODBUS_FC_READ_HOLDING_REGISTERS:
            resultValue = modbus_read_registers( m_modbus, startAddress, noOfItems, m_dest16 );
            break;
        case MODBUS_FC_READ_INPUT_REGISTERS:
            resultValue = modbus_read_input_registers( m_modbus, startAddress, noOfItems, m_dest16 );
            break;

        default:
            break;

    }

    // Read the databuffers
    if( resultValue != noOfItems )
    {
        return modbusData();
    }

    modbusData resultData;
    for( int index = 0; index < noOfItems; ++index )
    {
        resultData.push_back( (is16Bit ? static_cast<uint16_t>(m_dest16[index]) : static_cast<uint16_t>(m_dest[index])) );
    }

    modbus_flush( m_modbus );   // flush data
    this->ClearBuffers();

    return resultData;
}

modbusData ModbusAdapter::ModbusReadHoldReg( int slaveId, int startAddress, int noOfItems )
{
    if( m_modbus == nullptr )
    {
        return modbusData();
    }

    int resultValue = -1;       /// Return value from read functions

    // -- Start Critical Section --- ?? //

    modbus_set_slave( m_modbus, slaveId );
    /// Request data from modbus
    resultValue = modbus_read_registers( m_modbus, startAddress, noOfItems, m_dest16 );

    /// Read the databuffers
    if( resultValue != noOfItems )
    {
        return modbusData();
    }

    modbusData resultData;
    for( int index = 0; index < noOfItems; ++index )
    {
        resultData.push_back( static_cast<uint16_t>(m_dest16[index]) );
    }

    modbus_flush( m_modbus );
    this->ClearBuffers();

    // -- End Critical Section --- ?? //

    return resultData;
}

void ModbusAdapter::ModBusWriteData( int slaveId, int functionCode, int startAddress, int noOfItems, std::vector<int>values )
{
    if( m_modbus == nullptr )
    {
        // No modbus context. Sensible log-line and exit.
        return;
    }

    // ------------------------------------------------
    // Create 8 bits databuffer
    auto * data8 = new uint8_t[noOfItems];
    for( int index = 0; index < noOfItems; ++index )
    {
        data8[index] = values[index];
    }

    // Create same buffer for 16 bits data
    auto * data16 = new uint8_t[noOfItems];
    for( int index = 0; index < noOfItems; ++index )
    {
        data16[index] = values[index];
    }
    // ------------------------------------------------

    int resultValue = -1;

    modbus_set_slave( m_modbus, slaveId );

    // Request data from modbus
    switch( functionCode )
    {
        case MODBUS_FC_WRITE_SINGLE_COIL:
            resultValue = modbus_write_bit( m_modbus, startAddress, values[0] );
            noOfItems = 1;
            break;
        case MODBUS_FC_WRITE_SINGLE_REGISTER:
            resultValue = modbus_write_register( m_modbus, startAddress, values[0] );
            noOfItems = 1;
            break;
        case MODBUS_FC_WRITE_MULTIPLE_COILS:

            resultValue = modbus_write_bits( m_modbus, startAddress, noOfItems, data8 );
            break;
        case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
            resultValue = modbus_write_bits( m_modbus, startAddress, noOfItems, data16 );
            break;
    }

    delete[]  data8;
    delete[] data16;
    modbus_flush(m_modbus); // Flush data.

    if( resultValue != noOfItems )
    {
        // Create a log line that writing the register failed. Maybe increment ErrorCounter(s)
    }
}

bool ModbusAdapter::isConnected() const
{
    return m_connected;
}

std::string ModbusAdapter::ErrorString( int errnum ) const
{
    switch(errnum)
    {
        case EINVAL:
            return "Protocol context is NULL";
            break;
        case ETIMEDOUT:
            return "Timeout";
            break;
        case ECONNRESET:
            return "Connection reset";
            break;
        case ECONNREFUSED:
            return "Connection refused";
            break;
        case EPIPE:
            return "Socket error";
            break;
        default:
            return modbus_strerror(errno);
    }
}

/* ============= PRIVATE METHODS ============= */
bool ModbusAdapter::ModbusConnectRTU( const std::string &serialport, int baud, char parity, int dataBits, int stopBits, int RTS, int timeOut )
{
    m_modbus = modbus_new_rtu( serialport.c_str(), baud, parity, dataBits, stopBits, RTS );

#ifdef LIB_MODBUS_DEBUG_OUTPUT
    // Do sensible logging through PRIVA_LOG
    m_modbus_set_debug( m_modbus, 1 );
#endif
    if( m_modbus == nullptr )
    {
        // We can stop here. Nothing to be done as we don't have a valid context
        // Log to PRIVA_LOG
        m_connected = false;
        return m_connected;
    }
    else if( m_modbus && modbus_connect( m_modbus ) == -1 )
    {
        // We could not connect to the selected serial port.
        // We can stop here. Nothing to be done as we don't have a valid connection
        modbus_free( m_modbus );
        m_connected = false;
        return m_connected;
    }

    m_connected = true;

    // Set recovery mode
    modbus_set_error_recovery( m_modbus, MODBUS_ERROR_RECOVERY_PROTOCOL );

    // Set the response timeout
    modbus_set_response_timeout( m_modbus, timeOut, 0 );

    return m_connected;
}

bool ModbusAdapter::ModbusConnectTCP( const std::string &ip, int port, int timeOut )
{
    if( ip.empty() )
    {
        // Nothing to be done. Set an Logline to PRIVA_LOG and exit.
        return false;
    }

    m_modbus = modbus_new_tcp( ip.c_str(), port );

#ifdef LIB_MODBUS_DEBUG_OUTPUT
    // Do sensible logging through PRIVA_LOG
    m_modbus_set_debug( m_modbus, 1 );
#endif

    if( m_modbus == nullptr )
    {
        // We can stop here. Nothing to be done as we don't have a valid context
        // Log to PRIVA_LOG
        return false;
    }
    else if( m_modbus && modbus_connect( m_modbus ) == -1 )
    {
        // We could not connect to the selected serial port.
        // We can stop here. Nothing to be done as we don't have a valid connection
        modbus_free( m_modbus );
        m_connected = false;
        return m_connected;
    }

    m_connected = true;

    // Set recovery mode
    modbus_set_error_recovery( m_modbus, MODBUS_ERROR_RECOVERY_PROTOCOL );

    // Set the response timeout
    modbus_set_response_timeout( m_modbus, timeOut, 0 );

    return m_connected;
}

void ModbusAdapter::InitBuffers()
{
    // Setup memory for Data.
    m_dest = (uint8_t *)malloc(2000 + sizeof(uint8_t));
    m_dest16 = (uint16_t *)malloc(125 * sizeof(uint16_t));

    this->ClearBuffers();
}

void ModbusAdapter::ClearBuffers()
{
    memset(m_dest, 0, 2000 * sizeof(uint8_t));
    memset(m_dest16, 0, 125 * sizeof(uint16_t));
}