#include "ModbusRepository.h"
#include <QByteArray>
#include <QBuffer>
#include <QDataStream>

CModbusRepository::CModbusRepository()
{
}

int CModbusRepository::AddHRDataMap(quint16 StartAddress, quint16 Length)
{
    for(int i= 0; i < mHoldingRegisters.size(); i++)
    {
        if((StartAddress >= mHoldingRegisters.at(i).mStartAddress && StartAddress <= mHoldingRegisters.at(i).mEndAddress) ||
           (StartAddress + Length - 1 >= mHoldingRegisters.at(i).mStartAddress && StartAddress + Length - 1 <= mHoldingRegisters.at(i).mEndAddress))
        {
            return RET_ERROR;   //The data map overlaps an existing map.
        }
    }

    CHRDataMap NewHRMap(StartAddress,Length);

    mHoldingRegisters.append(NewHRMap);

    qDebug("Created a new HR data map at address: %d with size %d",StartAddress,Length);

    return RET_OK;
}

bool CModbusRepository::IsHRValid(quint16 StartAddress, quint16 Length, int *index)
{
    for(int i= 0; i < mHoldingRegisters.size(); i++)
    {
        if((StartAddress >= mHoldingRegisters.at(i).mStartAddress && StartAddress + Length - 1 <= mHoldingRegisters.at(i).mEndAddress))
        {
            if(index != 0)
            {
                *index = i;
            }
            return true;
        }
    }

    return false;
}

QByteArray CModbusRepository::GetHRData(quint16 StartAddress, quint16 Length, bool *OK)
{
    QByteArray Data;
    int RegisterIndex = 0;

    if(IsHRValid(StartAddress,Length, &RegisterIndex) == false)
    {
        if(OK != 0)
        {
            *OK = false;
        }
    }
    else
    {
        int DataIndex = StartAddress - mHoldingRegisters.at(RegisterIndex).mStartAddress;
        for(int i = 0; i < Length; i++)
        {
            quint16 CurReg = mHoldingRegisters.at(RegisterIndex).mRegistersData.at(DataIndex++);
            quint8 HighByte, LowByte;

            LowByte = CurReg & 0x00FF;
            HighByte = (CurReg >> 8) & 0x00FF;
            Data.append(HighByte);
            Data.append(LowByte);
        }

        if(OK != 0)
        {
            *OK = true;
        }
    }

    return Data;
}

int CModbusRepository::WriteHRData(quint16 StartAddress, quint16 Length, QByteArray Data)
{
    int RegisterIndex;

    if(IsHRValid(StartAddress,Length, &RegisterIndex) == false)
    {
        return RET_ERROR;
    }

    if(Length*2 != Data.size())
    {
        return RET_ERROR;
    }

    int DataIndex = StartAddress - mHoldingRegisters.at(RegisterIndex).mStartAddress;

    for(int i = 0; i < Length; i++)
    {
        quint8 HighByte = Data.at(i*2);
        quint8 LowByte = Data.at(i*2 + 1);
        qint16 Word = HighByte;
        Word <<= 8;
        Word += LowByte;
        Word = Word &0xFFFF;

        mHoldingRegisters[RegisterIndex].mRegistersData[DataIndex++] = Word;
    }

    return RET_OK;
}

int CModbusRepository::WriteSingleReg(quint16 Address, quint16 Value)
{
    int RegisterIndex;

    if(IsHRValid(Address,1, &RegisterIndex) == false)
    {
        return RET_ERROR;
    }

    int DataIndex = Address - mHoldingRegisters.at(RegisterIndex).mStartAddress;
    mHoldingRegisters[RegisterIndex].mRegistersData[DataIndex] = Value;
    return RET_OK;
}

quint16 CModbusRepository::GetSingleReg(quint16 Address, bool *OK)
{
    int RegisterIndex;

    if(IsHRValid(Address,1, &RegisterIndex) == false)
    {
        if(OK != 0)
        {
            *OK = false;
        }
        return 0;
    }

    if(OK != 0)
    {
        *OK = true;
    }

    int DataIndex = Address - mHoldingRegisters.at(RegisterIndex).mStartAddress;
    return mHoldingRegisters[RegisterIndex].mRegistersData[DataIndex];
}

QList<qint16> CModbusRepository::GetRegs(quint16 StartAddress, quint16 Length, bool *OK)
{
    int RegisterIndex;

    if(IsHRValid(StartAddress,Length, &RegisterIndex) == false)
    {
        if(OK != 0)
        {
            *OK = false;
        }
        return QList<qint16>();
    }

    if(OK != 0)
    {
        *OK = true;
    }

    int DataIndex = StartAddress - mHoldingRegisters.at(RegisterIndex).mStartAddress;
    return mHoldingRegisters[RegisterIndex].mRegistersData.mid(DataIndex,Length);
}

CHRDataMap::CHRDataMap(quint16 StartAddress, quint16 Length)
{
    mStartAddress = StartAddress;
    mLength = Length;
    for(int i = 0; i < Length; i++)
    {
        mRegistersData.append(0);
    }


    /*
    mRegistersData[0] = 0x1234;
    mRegistersData[1] = 0x4567;
    mRegistersData[2] = 0x8911;
    mRegistersData[3] = 9873;
    mRegistersData[4] = 0xABCD;*/


    mEndAddress = StartAddress + Length - 1;
}

int CModbusRepository::WriteMultipleRegs(quint16 StartAddress, QList<qint16> Data)
{
    int RegisterIndex;

    if(IsHRValid(StartAddress,Data.size(), &RegisterIndex) == false)
    {
        return RET_ERROR;
    }

    //mMutex.lockForWrite();
    int DataIndex = StartAddress - mHoldingRegisters.at(RegisterIndex).mStartAddress;
    for(int i = 0; i < Data.size(); i++)
    {
        mHoldingRegisters[RegisterIndex].mRegistersData[DataIndex++] = Data.at(i);
    }
 //   mMutex.unlock();

  //  emit RepoChanged(StartAddress,Data.size());
    return RET_OK;
}