//
// Created by magino on 20.5.2019.
//
#include <cmath>
#include <open62541/plugin/log_stdout.h>
#include "communication/ModbusTcpClient.h"
#include "core/UaObjects.h"

ModbusTcpClient::ModbusTcpClient(ModbusTcpClientCfg cfg): _config(cfg) {
    _modbus = modbus_new_tcp(_config.ipAddress.c_str(), _config.port);
    modbus_set_slave(_modbus, 2);
    // Set infinite reconnect on communication fail
    modbus_set_error_recovery(_modbus, MODBUS_ERROR_RECOVERY_LINK);
    // TODO get endian setting from cfg file ?
    _endian = EEndian::LittleEndian;
    // TODO temporary ... min/max address should be loaded from S7NodeTree
    _startAddress = _config.minAddress;
    _endAddress = _config.maxAddress;

    _buffer = new uint16_t[GetAddressDiff() / 2];
}

void ModbusTcpClient::Reconnect(volatile bool* running) {
    int status;
    while(*running && !_isConnected) {
        status = modbus_connect(_modbus);
        if(status != -1) {
            _isConnected = true;
        } else std::cout << modbus_strerror(errno) << std::endl;
        sleep(1);
    }
}

void ModbusTcpClient::GetData() {
    _lock.lock();
    if(modbus_read_registers(_modbus, _startAddress, GetAddressDiff() / 2, _buffer) == -1)
        std::cout << modbus_strerror(errno) << std::endl;
    _lock.unlock();
}

void ModbusTcpClient::StopComm() {
    modbus_close(_modbus);
    modbus_free(_modbus);
}

int ModbusTcpClient::GetModbusRegisterPos(int pos) { return pos < 2 ? 0 : (pos - _startAddress) / 2; }
int ModbusTcpClient::GetCycleTime() { return _config.cycleTime; }

UA_Variant ModbusTcpClient::ReadValue(void *params) {
    S7CsvRowModel* rowModel = static_cast<S7CsvRowModel*>(params);
    void* value =  UA_new(&rowModel->uaDataType);

    switch (rowModel->dataType) {
        case SiemensDataType::BOOL : {
            UA_Boolean boolVal = GetBoolAt(rowModel->offset);
            value = &boolVal;
            break;
        }
        case SiemensDataType::BYTE: case SiemensDataType::USINT: {
            int offset = (int)rowModel->offset;
            UA_Byte byteVal = (offset%2==0) ? MODBUS_GET_HIGH_BYTE(_buffer[GetModbusRegisterPos(offset)]) : MODBUS_GET_LOW_BYTE(_buffer[GetModbusRegisterPos(offset)]);
            value = &byteVal;
            break;
        }
        case SiemensDataType::SINT: {
            int offset = (int)rowModel->offset;
            UA_SByte sbyteVal = (offset%2==0) ? MODBUS_GET_HIGH_BYTE(_buffer[GetModbusRegisterPos(offset)]) : MODBUS_GET_LOW_BYTE(_buffer[GetModbusRegisterPos(offset)]);
            value = &sbyteVal;
            break;
        }
        case SiemensDataType::INT: {
            UA_Int16 int16Val = (UA_Int16)_buffer[GetModbusRegisterPos((int)rowModel->offset)];
            value = &int16Val;
            break;
        }
        case SiemensDataType::UINT: case SiemensDataType::WORD: {
            UA_UInt16 uint16Val = _buffer[GetModbusRegisterPos((int)rowModel->offset)];
            value = &uint16Val;
            break;
        }
        case SiemensDataType::DINT: {
            UA_Int32 int32Val = MODBUS_GET_INT32_FROM_INT16(_buffer, GetModbusRegisterPos((int)rowModel->offset));
            value = &int32Val;
            break;
        }
        case SiemensDataType::UDINT: case SiemensDataType::DWORD: {
            UA_UInt32 uInt32Val = MODBUS_GET_INT32_FROM_INT16(_buffer, GetModbusRegisterPos((int)rowModel->offset));
            value = &uInt32Val;
            break;
        }
        case SiemensDataType::LINT: {
            UA_Int64 int64Val = MODBUS_GET_INT64_FROM_INT16(_buffer, GetModbusRegisterPos((int)rowModel->offset));
            value = &int64Val;
            break;
        }
        case SiemensDataType::ULINT: case SiemensDataType::LWORD: {
            UA_UInt64 uInt64Val = MODBUS_GET_INT64_FROM_INT16(_buffer, GetModbusRegisterPos((int)rowModel->offset));
            value = &uInt64Val;
            break;
        }
        case SiemensDataType::REAL: {
            int offset = GetModbusRegisterPos((int)rowModel->offset);
            uint16_t mirror[2];
            mirror[1]= *(_buffer + offset);
            mirror[0]= *(_buffer + offset + 1);
            UA_Float floatVal = modbus_get_float(mirror);
            // badc
            value = &floatVal;
            break;
        }
        case SiemensDataType::LREAL: {
            //TODO Double
            UA_Double doubleVal;
            value = &doubleVal;
            break;
        }
        case SiemensDataType::CHAR: {
            int offset = (int)rowModel->offset;
            uint16_t val = _buffer[GetModbusRegisterPos(offset)];
            char byteArray[2];
            byteArray[0] = (offset%2==0) ? MODBUS_GET_HIGH_BYTE(val) : MODBUS_GET_LOW_BYTE(val);
            byteArray[1] = '\0';
            UA_String charVal = UA_String_fromChars(byteArray);

            value = &charVal;
            break;
        }
        case SiemensDataType::STRING: {
            UA_String stringVal = GetString((int)rowModel->offset);
            value = &stringVal;
            break;
        }
    }

    UA_Variant variant;
    UA_Variant_setScalarCopy(&variant, value, &(rowModel->uaDataType));
    return variant;
}
#include <math.h>

UA_Boolean ModbusTcpClient::GetBoolAt(float offset) {
    int pos = (int)offset;
    //int bit = (offset - pos)*10;
    float tmp = (offset - pos)*10;
    int bit = rint(tmp);
    // TODO check (bit < 0 && bit > 7) && pos is in min and max address
    // TODO what if startAddress is odd -> S7 data in data block starts always from even number!
    int a = GetModbusRegisterPos(pos);
    uint16_t val = _buffer[a];

    if(_endian == EEndian::LittleEndian && pos % 2 == 0)
        bit += 8;
    auto out = (val & ( 1 << bit )) >> bit != 0;
    return out;
}

UA_String ModbusTcpClient::GetString(int pos) {
    int startRegister = GetModbusRegisterPos(pos);
    uint16_t metaData = _buffer[startRegister];
    int actualLength = MODBUS_GET_LOW_BYTE(metaData);
    char byteArray[actualLength+1];

    int j = 0;
    int other = GetModbusRegisterPos(pos + 2 + actualLength);
    for(int i = startRegister + 1; i <= other; ++i) {
        byteArray[j] = MODBUS_GET_HIGH_BYTE(_buffer[i]);
        if( i == other && (actualLength % 2 != 0)) break;
        byteArray[j+1] = MODBUS_GET_LOW_BYTE(_buffer[i]);
        j+=2;
    }
    byteArray[actualLength] = '\0';
    return UA_String_fromChars(byteArray);
}





void ModbusTcpClient::WriteValue(UA_Variant data, void* params) {
    S7CsvRowModel* rowModel = static_cast<S7CsvRowModel*>(params);

    switch (data.type->typeIndex) {
        case UA_TYPES_BOOLEAN: {
            UA_Boolean boolVal = *static_cast<UA_Boolean*>(data.data);
            WriteBoolAt(boolVal, rowModel->offset);
            return;
        }
        case UA_TYPES_BYTE: {
            UA_Byte byteVal = *static_cast<UA_Byte*>(data.data);
            WriteByteAt(byteVal, (int)rowModel->offset);
            return;
        }
        case UA_TYPES_SBYTE: {
            UA_SByte sbyteVal = *static_cast<UA_SByte*>(data.data);
            WriteByteAt(sbyteVal, (int)rowModel->offset);
            return;
        }
        case UA_TYPES_INT16: {
            UA_Int16 int16Val = *static_cast<UA_Int16*>(data.data);
            WriteInt16At(int16Val, (int)rowModel->offset);
            return;
        }
        case UA_TYPES_UINT16: {
            UA_UInt16 uint16Val = *static_cast<UA_UInt16*>(data.data);
            WriteUInt16At(uint16Val, (int)rowModel->offset);
            return;
        }
        case UA_TYPES_INT32: {
            UA_Int32 int32Val = *static_cast<UA_Int32*>(data.data);
            WriteInt32At(int32Val, (int)rowModel->offset);
            return;
        }
        case UA_TYPES_UINT32: {
            UA_UInt32 uint32Val = *static_cast<UA_UInt32*>(data.data);
            WriteUInt32At(uint32Val, (int)rowModel->offset);
            return;
        }
        case UA_TYPES_INT64: {
            UA_Int64 int64Val = *static_cast<UA_Int64*>(data.data);
            WriteInt64At(int64Val, (int)rowModel->offset);
            return;
        }
        case UA_TYPES_UINT64: {
            UA_UInt64 uint64Val = *static_cast<UA_UInt64*>(data.data);
            WriteUInt64At(uint64Val, (int)rowModel->offset);
            return;
        }
        case UA_TYPES_FLOAT: {
            UA_Float floatVal = *static_cast<UA_Float*>(data.data);
            WriteFloatAt(floatVal, (int)rowModel->offset);
            return;
        }
        case UA_TYPES_DOUBLE: {
            UA_Double doubleVal = *static_cast<UA_Double*>(data.data);
            // TODO double write
            return;
        }
        case UA_TYPES_STRING: {
            UA_String stringVal = *static_cast<UA_String*>(data.data);
            WriteStringAt(stringVal, (int)rowModel->offset);
            return;
        }
    }
}

void ModbusTcpClient::WriteBoolAt(UA_Boolean val, float offset) {
    int pos = (int)offset;
    //int bit = (offset - pos)*10;
    float tmp = (offset - pos)*10;
    int bit = rint(tmp);

    if(_endian == EEndian::LittleEndian && pos % 2 == 0)
        bit += 8;

    _lock.lock();
    uint16_t bool_register = _buffer[GetModbusRegisterPos(pos)];

    if(val) {
        bool_register |= (1 << bit); // set bit
    } else {
        bool_register &= ~(1 << bit); // clear bit
    }

    // Write register
    int status = modbus_write_register(_modbus, GetModbusRegisterPos(pos), bool_register);
    _lock.unlock();

    if(status == -1) std::cout << modbus_strerror(errno) << std::endl;
}

void ModbusTcpClient::WriteByteAt(UA_Byte val, int offset) {
    uint16_t mbReg = _buffer[GetModbusRegisterPos(offset)];

    mbReg = offset%2!=0 ? (mbReg&0xFF00)|(uint8_t)val : (mbReg&0x00FF)|((uint8_t)val<<8);
    _lock.lock();
    int status = modbus_write_register(_modbus, GetModbusRegisterPos(offset), mbReg);
    _lock.unlock();
    if(status == -1) std::cout << modbus_strerror(errno) << std::endl;
}

void ModbusTcpClient::WriteSByteAt(UA_SByte val, int offset) {
    uint16_t mbReg = _buffer[GetModbusRegisterPos(offset)];
    mbReg = offset%2!=0 ? (mbReg&0xFF00)|(int8_t)val : (mbReg&0x00FF)|((int8_t)val<<8);
    _lock.lock();
    int status = modbus_write_register(_modbus, GetModbusRegisterPos(offset), mbReg);
    _lock.unlock();
    if(status == -1) std::cout << modbus_strerror(errno) << std::endl;
}

void ModbusTcpClient::WriteInt16At(UA_Int16 val, int pos) {
    _lock.lock();
    int status = modbus_write_register(_modbus, GetModbusRegisterPos(pos), val);
    _lock.unlock();
    if(status == -1) std::cout << modbus_strerror(errno) << std::endl;
}

void ModbusTcpClient::WriteUInt16At(UA_UInt16 val, int pos) {
    _lock.lock();
    int status = modbus_write_register(_modbus, GetModbusRegisterPos(pos), val);
    _lock.unlock();
    if(status == -1) std::cout << modbus_strerror(errno) << std::endl;
}

void ModbusTcpClient::WriteInt32At(UA_Int32 val, int pos) {
    _lock.lock();
    uint16_t tmp[2];
    MODBUS_SET_INT32_TO_INT16(tmp,0,val);
    int status = modbus_write_registers(_modbus, GetModbusRegisterPos(pos), 2, tmp);

    if(status == -1) std::cout << modbus_strerror(errno) << std::endl;
    _lock.unlock();
}

void ModbusTcpClient::WriteUInt32At(UA_UInt32 val, int pos) {
    _lock.lock();
    uint16_t tmp[2];
    MODBUS_SET_INT32_TO_INT16(tmp,0,val);
    int status = modbus_write_registers(_modbus, GetModbusRegisterPos(pos), 2, tmp);
    if(status == -1) std::cout << modbus_strerror(errno) << std::endl;
    _lock.unlock();
}

void ModbusTcpClient::WriteInt64At(UA_Int64 val, int pos) {
    uint16_t tmp[4];
    MODBUS_SET_INT64_TO_INT16(tmp, 0, val);
    _lock.lock();
    int status = modbus_write_registers(_modbus, GetModbusRegisterPos(pos), 4, tmp);
    _lock.unlock();
    if(status == -1) std::cout << modbus_strerror(errno) << std::endl;
}

void ModbusTcpClient::WriteUInt64At(UA_UInt64 val, int pos) {
    uint16_t tmp[4];
    MODBUS_SET_INT64_TO_INT16(tmp, 0, val);
    _lock.lock();
    int status = modbus_write_registers(_modbus, GetModbusRegisterPos(pos), 4, tmp);
    _lock.unlock();
    if(status == -1) std::cout << modbus_strerror(errno) << std::endl;
}

void ModbusTcpClient::WriteFloatAt(UA_Float val, int pos) {
    uint16_t tmp[2];
    //modbus_set_float(val, tmp);
    //modbus_set_float_cdab(val, tmp);
    //modbus_set_float_abcd(val,tmp);
    modbus_set_float_dcba(val,tmp);
    _lock.lock();
    int status = modbus_write_registers(_modbus, GetModbusRegisterPos(pos), 2, tmp);
    _lock.unlock();
    if(status == -1) std::cout << modbus_strerror(errno) << std::endl;
}

void ModbusTcpClient::WriteDoubleAt(UA_Double val, int pos) {
    uint16_t tmp[4];
    // TODO set double
    _lock.lock();
    int status = modbus_write_registers(_modbus, GetModbusRegisterPos(pos), 4, tmp);
    _lock.unlock();
    if(status == -1) std::cout << modbus_strerror(errno) << std::endl;
}

void ModbusTcpClient::WriteStringAt(UA_String val, int pos) {
    int startRegister = GetModbusRegisterPos(pos);
    uint16_t metaData = _buffer[startRegister];
    int totalLength = MODBUS_GET_HIGH_BYTE(metaData);

    // val can be longer than max string length!!
    int valLength;
    if (val.length > totalLength) {
        valLength = totalLength;
        UA_LOG_WARNING(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND,
                       "The string you typed is too long (%zu)! Shortening to %d.", val.length, totalLength);
    } else valLength = val.length;


    int tmpLength = (int) ceil((double) valLength / 2) + 1;
    uint16_t tmp[tmpLength];

    tmp[0] = (totalLength << 8) | valLength;
    int j = 0;
    for (int i = 1; i < tmpLength; ++i) {
        if (i == tmpLength - 1 && totalLength % 2 != 0 && totalLength == valLength) {
            tmp[i] = (val.data[j] << 8) | (_buffer[startRegister + tmpLength - 1] & 0x00FF);
        } else {
            tmp[i] = (val.data[j] << 8) | val.data[j + 1];
        }
        j += 2;
    }

    _lock.lock();
    int status = modbus_write_registers(_modbus, GetModbusRegisterPos(pos), tmpLength, tmp);
    if (status == -1) std::cout << modbus_strerror(errno) << std::endl;
    _lock.unlock();
}