#include "ModbusInterface.h"
#include "ModbusInterface.h"
#include <iostream>
#include <easylogging++.h>

void ModbusInterface::assignSlave() {
    my_modbus->addSlave(slaveAddressID);
    my_slave = my_modbus->slavePtr(slaveAddressID);
}

ModbusInterface::~ModbusInterface() {
    if(my_modbus != nullptr){
        my_modbus->close();
        delete my_modbus;
    }
}

bool ModbusInterface::openConnection() {
    modbus_init();
    //if(!(my_modbus->isOpen()))
    open = my_modbus->open();
    if(!open){
        LOG(ERROR) << "Couldn't open Modbus " << getConnectionType() <<
                      " connection at " << device;
        LOG(ERROR) << my_modbus->lastError();
    }
    return open;
}

void ModbusInterface::disconnect() {
    if(my_modbus != nullptr)
        my_modbus->close();
}

bool ModbusInterface::readBit(ParameterSpecification &param) {
    if(!param.connection->getIsOpen()){
        param.error = true;
        return false;
    }
    bool buf;
    if (my_slave->readCoil(param.address, buf) != 1) {
        param.error = true;
        return 0;
    }
    else{
        param.readedBytes.push_back(buf);
        param.error = false;
        return buf;
    }
}


bool ModbusInterface::readBits(ParameterSpecification &param) {
    if(!param.connection->getIsOpen()){
        param.error = true;
        return false;
    }
    bool bufBool[param.length*8];
    int ans = my_slave->readCoils(param.address, bufBool, param.length*8);
    if (ans != param.length*8) {
        param.error = true;
        return 0;
    }
    else{
        //Big Endian decryption
        for(unsigned int i = 0; i < param.length; i++){
            uint8_t byte = 0;
            for(unsigned int j = 0; j < 8; j++){
                byte += bufBool[i*8 +j] << j;
            }
            param.readedBytes.push_back(byte);
        }
        param.error = false;
        return true;
    }
}

uint8_t ModbusInterface::readByte(ParameterSpecification &param) {
    if(!param.connection->getIsOpen()){
        param.error = true;
        return false;
    }
    uint16_t buf;
    if (my_slave->readRegister(param.address, buf) != 1) {
        param.error = true;
        return 0;
    }
    else{
        param.readedBytes.push_back(buf & 0x00FF);
        param.error = false;
        return static_cast<uint8_t>(buf & 0x00FF);
    }
}

uint16_t ModbusInterface::readRegister(ParameterSpecification &param) {
    if(!param.connection->getIsOpen()){
        param.error = true;
        return false;
    }
    uint16_t buf;
    if (my_slave->readRegister(param.address, buf) != 1) {
        param.error = true;
        return 0;
    }
    else{
        param.readedBytes.push_back(buf);
        param.error = false;
        return buf;
    }
}

uint32_t ModbusInterface::readDRegister(ParameterSpecification &param) {
    if(!param.connection->getIsOpen()){
        param.error = true;
        return false;
    }
    uint16_t buf[2];

    if (my_slave->readRegisters(param.address, buf, 2) != 2) {
        param.error = true;
        return 0;
    }
    else{
        param.readedBytes.push_back(*buf);
        param.readedBytes.push_back(*(buf+1));
        param.error = false;
        return _MODBUS_GET_INT32_FROM_INT16(buf, 0);
    }
}

uint64_t ModbusInterface::readQRegister(ParameterSpecification &param) {
    if(!param.connection->getIsOpen()){
        param.error = true;
        return false;
    }

    uint16_t buf[4];
    int ans = my_slave->readRegisters(param.address, buf, 4);
    if (ans != 4) {
        param.error = true;
        return 0;
    }
    else{
        param.readedBytes.push_back(*buf);
        param.readedBytes.push_back(*(buf+1));
        param.readedBytes.push_back(*(buf+2));
        param.readedBytes.push_back(*(buf+3));
        param.error = false;
        return _MODBUS_GET_INT64_FROM_INT16(buf, 0);
    }
}

bool ModbusInterface::writeBit(const uint16_t address, bool bit)
{
    if(my_slave->writeCoil(address, bit) != 1){
        return false;
	}
	else
		return true;
}

bool ModbusInterface::writeByte(const uint16_t address, uint8_t byte)
{
    bool buf[8];
    for(int i = 0; i < 8; i++){
        buf[i] = byte & 0x01;
        byte = byte > 1;
    }

    if(my_slave->writeCoils(address, buf, 8) != 1){
        return false;
    }
    else
        return true;
}

bool ModbusInterface::writeRegister(const uint16_t address, uint16_t word)
{
    if(my_slave->writeRegister(address, word) != 1){
        return false;
    }
    else
        return true;
}

bool ModbusInterface::writeDRegister(const uint16_t address, uint32_t dword)
{
    uint16_t data[2];
    data[0] = (dword & 0x0000FFFF);
    data[0] = (dword & 0xFFFF0000)>>16;

    if(my_slave->writeRegisters(address, data, 2) != 1){
        return false;
    }
    else
        return true;
}

bool ModbusInterface::writeQRegister(const uint16_t address, uint64_t qword)
{
	uint16_t data[4];
	
	data[0] = (qword & 0x0000'0000'0000'FFFF);
	data[1] = (qword & 0x0000'0000'FFFF'0000) >> 16;
	data[2] = (qword & 0x0000'FFFF'0000'0000) >> 32;
	data[3] = (qword & 0xFFFF'0000'0000'0000) >> 48;

    if(my_slave->writeRegisters(address, data, 4) != 1){
        return false;
    }
    else
        return true;
}