/*
 * ModbusRtu.cpp
 *
 *  Created on: 5 июл. 2020 г.
 *      Author: LeonidTitov
 */

#include "../common/Crc.hpp"

#include "ModbusRtu.hh"

// Ожидание запроса
void driver::modbus::ModBusPdu::processReq() {

    if ( mAdu.getFrm ( deqFrmReq ) ) {

        // В случаи нового запроса полный сброс состояния
        mMapTaskRd.clear();
        mMapTaskWr.clear();

        deqFrmRes.clear();

        mExcCode = NoException;

        mIsBroadcast = !deqFrmReq[0];// Флаг широковещательного запроса
        mFuncCode = deqFrmReq[1];// Сохраняем код запроса

        const uint16_t shift = getDataTblShift ( (FuncCode)mFuncCode );

        switch ( mFuncCode ) {
        case ReadCoil:
        case ReadDiscreteInput:
        case ReadHoldingReg:
        case ReadInputReg: {

            uint16_t addrBeg = shift + ( deqFrmReq[2] << 8 ) + deqFrmReq[3];
            uint16_t addrEnd = addrBeg + ( deqFrmReq[4] << 8 ) + deqFrmReq[5];

            if( addrEnd - addrBeg > getLimitQuantity( (FuncCode)mFuncCode )  ) {
                mExcCode = IllegalDataValue;
                break;
            }

            for ( uint16_t addrReg = addrBeg; addrReg < addrEnd; addrReg++ ) {

                mMapTaskRd.insert ( make_pair( addrReg, 0 ) );

            }

        } break;
        case WriteSingleCoil:
        case WriteSingleReg: {

            uint16_t addr = shift + ( deqFrmReq[2] << 8 ) + deqFrmReq[3];
            uint16_t value = ( deqFrmReq[4] << 8 ) + deqFrmReq[5];

            mMapTaskWr.insert( make_pair( addr, value ) );

        } break;
        case WriteMultipleCoil: {

            const size_t valShift = 7;          // Смещение данных в пакете запроса

            uint16_t addrBeg = shift + ( deqFrmReq[2] << 8 ) + deqFrmReq[3];
            uint16_t addrEnd = addrBeg + ( deqFrmReq[4] << 8 ) + deqFrmReq[5];

            if( addrEnd - addrBeg > getLimitQuantity( (FuncCode)mFuncCode )  ) {
                mExcCode = IllegalDataValue;
                break;
            }

            const uint16_t bitOn = 0xFF00;      // Индикатор включенного бита
            const uint16_t bitOff = 0x0000;     // Индикатор выключенного бита

            for( uint16_t iBit = 0, addrReg = addrBeg;
                                            addrReg < addrEnd; addrReg++, iBit++ ) {

                // Вычисление дискретного значения
                uint16_t value = deqFrmReq[valShift + iBit / 8] &
                                                    (1u << iBit % 8) ? bitOn: bitOff;
                mMapTaskWr.insert( make_pair( addrReg, value ) );

            }

        } break;
        case WriteMultipleReg: {

            const size_t valShift = 7;          // Смещение данных в пакете запроса

            uint16_t addrBeg = shift + ( deqFrmReq[2] << 8 ) + deqFrmReq[3];
            uint16_t addrEnd = addrBeg + ( deqFrmReq[4] << 8 ) + deqFrmReq[5];

            if( addrEnd - addrBeg > getLimitQuantity( (FuncCode)mFuncCode )  ) {
                mExcCode = IllegalDataValue;
                break;
            }

            for( uint16_t iVal = valShift, addrReg = addrBeg;
                                        addrReg < addrEnd; addrReg++, iVal += 2 ) {

                // Вычисление аналогового значения
                uint16_t value = ( deqFrmReq[iVal] << 8 ) + deqFrmReq[iVal + 1];
                mMapTaskWr.insert( make_pair( addrReg, value ) );

            }

        } break;
        default: {

            // Не поддерживаемый код операции
            mExcCode = IllegalFunc;

        }
        }

        // Установка признака ошибки
        if ( mExcCode != NoException ) {
            mFuncCode += 0x80;
        } else
            packet_counter++;

    }

}

void driver::modbus::ModBusPdu::processResp() {

    if ( ( mFuncCode != DummyCode
            && mMapTaskRd.empty()
            && mMapTaskWr.empty() ) || mFuncCode > 0x80 ) {

        deqFrmRes.clear();
        deqFrmRes.push_back( mFuncCode );// Начало ответного пакета код операции

        switch ( mFuncCode ) {
            case ReadCoil:
            case ReadDiscreteInput: {

                const size_t num_results = mMapTaskRes.size();
                const size_t nrBytes = num_results ? ( ( num_results - 1 ) / 8 ) + 1 : 0;

                deqFrmRes.push_back( nrBytes );// Сохраняем кол-во байт данных

                for ( size_t i = 0; i < nrBytes; i ++ ) {

                    deqFrmRes.push_back( 0 );// Предустановка off значений

                }

                const uint16_t bitOn = 0xFF00;// Индикатор включенного бита
                const size_t valShift = 2;// Смещение данных в пакете ответа

                // Фиксируем полученные данные
                size_t iBit = 0;
                for( MapPolyAlloc::const_iterator it = mMapTaskRes.begin(); it != mMapTaskRes.end(); it++, iBit++ ) {

                    deqFrmRes[valShift + iBit / 8] |=
                                            it->second == bitOn ? (1u << iBit % 8) : 0;

                }

            } break;
            case ReadHoldingReg:
            case ReadInputReg: {

                const size_t nrBytes = mMapTaskRes.size() * 2;

                // Сохраняем кол-во байт данных
                deqFrmRes.push_back( nrBytes );

                // Фиксируем полученные данные
                for( MapPolyAlloc::const_iterator it = mMapTaskRes.begin(); it != mMapTaskRes.end(); it++ ) {

                    deqFrmRes.push_back( it->second >> 8 );         // HiByte
                    deqFrmRes.push_back( it->second & 0xFF );       // LoByte

                }

            } break;
            case WriteSingleCoil:
            case WriteSingleReg: {

                uint16_t shift = getDataTblShift( (FuncCode)mFuncCode );

                // Адрес первого регистра
                uint16_t addrBeg = mMapTaskRes.begin()->first - shift;
                deqFrmRes.push_back( addrBeg >> 8 );           // HiByte
                deqFrmRes.push_back( addrBeg & 0xFF );         // LoByte

                // Записанное значение регистра
                uint16_t value = mMapTaskRes.begin()->second;
                deqFrmRes.push_back( value >> 8 );             // HiByte
                deqFrmRes.push_back( value & 0xFF );           // LoByte

            } break;
            case WriteMultipleCoil:
            case WriteMultipleReg: {

                uint16_t shift = getDataTblShift( (FuncCode)mFuncCode );

                // Адрес первого регистра
                uint16_t addrBeg = mMapTaskRes.begin()->first - shift;
                deqFrmRes.push_back( addrBeg >> 8 );       // HiByte
                deqFrmRes.push_back( addrBeg & 0xFF );     // LoByte

                // Количество записанных регистров
                uint16_t nrReg = mMapTaskRes.size();
                deqFrmRes.push_back( nrReg >> 8 );         // HiByte
                deqFrmRes.push_back( nrReg & 0xFF );       // LoByte

            } break;
            default: {

                // Обработка исключительных ситуаций
                deqFrmRes.push_back( mExcCode );

            } break;
        }

        // Отправка пакета
        if( !mIsBroadcast && !mAdu.putFrm( deqFrmRes ) ) {

            // TODO Обработка отказа отправки

        }

        mFuncCode = DummyCode;// Запрос завершен

        //todo: Добавил очистку мапы после использования.
        mMapTaskRes.clear();

    }

}

bool driver::modbus::ModBusAdu::getFrm( DequePolyAlloc & deqFrm ) {

    if( !deqFrm.empty() ) {
        deqFrm.clear();
    }

    static char modbus_frame_word = 0;

    while (mIUartPort.receive(&modbus_frame_word, 1)) {
        deqFrm.push_front(modbus_frame_word);
    }

    if( deqFrm.size() > 2 ) {
        ModbusCrc::crc_t crcTmp;
        DequePolyAlloc::const_iterator itFrm = deqFrm.begin();

        const std::size_t szFrmData = deqFrm.size() - 2;

        for ( std::size_t i = 0; i < szFrmData; i++, itFrm++ ) {
            crcTmp = common::crc::crcCalc<ModbusCrc>(&itFrm[0], 1, crcTmp);
        }

        const ModbusCrc::res_t received_crc_low = *itFrm;
        itFrm++;
        const ModbusCrc::res_t received_crc_high = *itFrm;
        const ModbusCrc::res_t received_crc =
            (received_crc_high << 8) | (received_crc_low & 0xFF);

        if((received_crc == (ModbusCrc::res_t) crcTmp)
                && ((mAddr == deqFrm.front()) || !deqFrm.front()))
        {
            //Удаляем данные CRC
            deqFrm.pop_back();
            deqFrm.pop_back();
            crc_error_flag = false;
        } else {
            deqFrm.clear ();
            crc_error_flag = true;
        }
    }

    return !deqFrm.empty();

}

bool driver::modbus::ModBusAdu::putFrm( DequePolyAlloc & deqFrm ) {

    deqFrm.push_front(mAddr);// Вставка адреса устройства

    ModbusCrc::crc_t crcTmp;
    VecPolyAlloc vecFrm(mpMemRes);// Выходной буфер
    DequePolyAlloc::const_iterator itFrm = deqFrm.begin();

    while (itFrm != deqFrm.end()) {
        // Вставка данных фрэйма
        crcTmp = common::crc::crcCalc<ModbusCrc>(&itFrm[0], 1, crcTmp);
        vecFrm.push_back(*itFrm);
        itFrm++;
    }

    const ModbusCrc::res_t output_crc = crcTmp;

    // Вставка контрольной суммы
    vecFrm.push_back(output_crc & 0xFF);
    vecFrm.push_back(output_crc >> 8);

    return mIUartPort.send(&vecFrm.front(), vecFrm.size());

}

void driver::modbus::ModBusPdu::setExceptCode( ExceptCode code ) {

    mExcCode = code;
    mFuncCode |= 0x80;

}

bool driver::modbus::ModBusPdu::isException() const {

    return mExcCode != NoException;

}

uint16_t driver::modbus::ModBusPdu::getLimitQuantity( FuncCode code ) const {

    uint16_t limit = 0;

    switch( code ) {

    case ReadDiscreteInput:
    case WriteSingleCoil:
    case WriteMultipleCoil:
    case ReadCoil: {

        limit = 0x7D0;

    } break;

    case ReadInputReg:
    case WriteSingleReg:
    case WriteMultipleReg:
    case ReadHoldingReg: {

        limit = 0x7D;

    } break;

    default: break;
    }

    return limit;

}

uint16_t driver::modbus::ModBusPdu::getPacketCounter() const {

    return packet_counter;

}

driver::modbus::ModBusPdu::ExceptCode driver::modbus::ModBusPdu::getExceptCode() const {

    return mExcCode;

}

uint16_t driver::modbus::ModBusPdu::getDataTblShift( FuncCode code ) const {

    uint16_t shift = 0;

    switch ( code ) {

    case WriteSingleCoil:
    case WriteMultipleCoil:
    case ReadCoil: {

        // Чтение дискретного вывода (DO)
        shift = DataTableBegin::DO;

    } break;

    case ReadDiscreteInput: {

        // Чтение дискретного вывода (DI)
        shift = DataTableBegin::DI;

    } break;

    case WriteSingleReg:
    case WriteMultipleReg:
    case ReadHoldingReg: {

        // Чтение аналогового вывода (AO)
        shift = DataTableBegin::AO;

    } break;

    case ReadInputReg: {

        // Чтение аналогового вывода (AI)
        shift = DataTableBegin::AI;

    } break;

    default: {

        shift = 0;

    } break;

    }

    return shift;

}

void driver::modbus::ModBusDispatch::add( uint16_t nReg, IModBusDispatch & disp ) {

    mBase.insert( make_pair( nReg, &disp ) );

}

void driver::modbus::ModBusDispatch::del( uint16_t nReg ) {

    mBase.erase( nReg );

}

void driver::modbus::ModBusDispatch::process() {

    // Получение запросов
    mPdu.processReq();

    dispatch( TaskRead, mPdu.mMapTaskRd  );
    dispatch( TaskWrite, mPdu.mMapTaskWr  );

    mPdu.processResp();

}

void driver::modbus::ModBusDispatch::dispatch( TaskType typeOp,
                                               MapPolyAlloc & mapTasks ) {

    if( !mapTasks.empty() ) {

        // Список регистров закончивших действия
        DequeU16PolyAlloc deqTaskDone( mPdu.mAdu.mpMemRes );

        for(    MapPolyAlloc::iterator it = mapTasks.begin();
                it != mapTasks.end();
                it++ ) {

            const uint16_t nReg = it->first;

            if( mBase.count( nReg ) ) {

                const IModBusDispatch::Status sts =
                        typeOp == TaskRead ?    mBase[nReg]->read( it->second ) :
                                                mBase[nReg]->write( it->second );

                switch ( sts ) {
                case IModBusDispatch::Done: {
                    // Сохраняем результат
                    mPdu.mMapTaskRes.insert( *it );
                    deqTaskDone.push_back( nReg );

                } break;
                case IModBusDispatch::Failure: {

                    mPdu.setExceptCode( ModBusPdu::ServerDeviceFail );

                } break;
                case IModBusDispatch::Pending: {

                    mPdu.setExceptCode( ModBusPdu::ServerDeviceBusy );

                } break;
                default: {

                } break;
                }
            } else {
                // Ошибка, такой регист не зарегистрирован
                mPdu.setExceptCode( ModBusPdu::IllegalDataAddr );

            }

            if ( mPdu.isException() )
                break;

        }

        // Удаляем выполненные задачи из списка
        for(    DequeU16PolyAlloc::const_iterator it = deqTaskDone.begin();
                it != deqTaskDone.end();
                it++ ) {

            mapTasks.erase( *it );

        }
    }
}

void driver::modbus::ModBusRtu::process() {

    if (mPort.isRecieptionError()) {
        mPort.reset();
    }

    mDisp.process();

}

void driver::modbus::ModBusRtu::add(uint16_t nReg, IModBusDispatch &disp) {

    mDisp.add( nReg, disp );

}

void driver::modbus::ModBusRtu::del(uint16_t nReg) {

    mDisp.del( nReg );

}

bool driver::modbus::ModBusAdu::isCrcError() const {

    return crc_error_flag;

}

bool driver::modbus::ModBusRtu::isCrcError() const {

    return mAdu.isCrcError();

}

bool driver::modbus::ModBusRtu::isException() const {

    return mPdu.isException();

}

float driver::modbus::ModBusRtu::getPacketCounter() const {

    return mPdu.getPacketCounter();

}