/*
 * WeinbusSlave.cpp
 *
 *      Author: Aleksey Gerasimenko
 *      gerasimenko.aleksey.n@gmail.com
 */

#include "WEINBUS/WeinbusSlave.h"

namespace WEINBUS
{
//CONSTRUCTOR
WeinbusSlave::WeinbusSlave(MODBUSRTU::ModbusRTUCRC& crc):
        //outputCoils(),
        //inputCoils(),
        outputRegisters(),
        inputRegisters(),
        //
        rxStack(),
        txStack(),
        rxLength(0),
        txLength(0),
        //
        m_function(0),
        m_start_address(0),
        m_length(0),
        m_value(0),
        m_counter(0),
        m_counter_length(0),
        m_point(0),
        m_cursor(0),
        m_byte_length(0),
        m_byte_counter(0),
        m_aux_register(),
        //
        m_crc(crc)
        //
{}//CONSTRUCTOR


void WeinbusSlave::execute()
{
    if(rxLength != 0)
    {
        m_function = rxStack[1];

        switch(m_function)
        {
        case MODBUSRTU_FUNCTION_BROADCAST:                  { _function_broadcast();                    break;}
        case MODBUSRTU_FUNCTION_READ_COIL_STATUS:           { _function_read_coil_status();             break;}
        case MODBUSRTU_FUNCTION_READ_INPUT_STATUS:          { _function_read_input_status();            break;}
        case MODBUSRTU_FUNCTION_READ_HOLDING_REGISTERS:     { _function_read_holding_registers();       break;}
        case MODBUSRTU_FUNCTION_READ_INPUT_REGISTERS:       { _function_read_input_registers();         break;}
        case MODBUSRTU_FUNCTION_FORCE_SINGLE_COIL:          { _function_force_single_coil();            break;}
        case MODBUSRTU_FUNCTION_PRESET_SINGLE_REGISTER:     { _function_preset_single_register();       break;}
        case MODBUSRTU_FUNCTION_FORCE_MULTIPLE_COILS:       { _function_force_multiple_coils();         break;}
        case MODBUSRTU_FUNCTION_PRESET_MULTIPLE_REGISTERS:  { _function_preset_multiple_registers();    break;}
        default:{ txLength = 0;}

        //
        }//switch
        //
    }//if
    //
}//
//
void WeinbusSlave::_function_broadcast()
{
    // Function 0x001 broadcast
    //
    txLength = 0;
    rxLength = 0;
    //
}//
//
void WeinbusSlave::_function_read_coil_status()
{
    // Function 0x01 read coil, table is "outputCoils"
    //
    txLength = 0;
    rxLength = 0;
    //
}//
//
void WeinbusSlave::_function_read_input_status()
{
    // Function 0x02 read input status
    //
    txLength = 0;
    rxLength = 0;
    //
}//
//
void WeinbusSlave::_function_read_holding_registers()
{
    // Function 0x03 read register, table is "outputRegisters"

    // construct start address
    m_start_address.byte.bt1 = rxStack[2];
    m_start_address.byte.bt0 = rxStack[3];

    // construct 16-bit register quantity
    m_length.byte.bt1 = rxStack[4];
    m_length.byte.bt0 = rxStack[5];
    m_counter_length = m_length.all;
    m_counter = 0;


    // check address range
//    if((outputRegisters.address_range(m_start_address.all))&
//            (outputRegisters.address_range(m_start_address.all + m_length.all - (uint16_t)1)))
    if(outputRegisters.address_range(m_start_address.all))
    {
        // CONSTRUCT ANSWER
        //1. Node ID
        txStack[0] = rxStack[0];
        //2. Function code
        txStack[1] = rxStack[1];
        //3. Byte number - should be counted
        txStack[2] = 0 ;
        m_byte_counter = 0;
        //4. Fill fields with values
        m_point = 3;

        m_cursor = outputRegisters.get_cursor(m_start_address.all);

        //for(m_counter = 0; m_counter < m_length.all; m_counter++)
        while(m_counter < m_counter_length)
        {
            m_aux_register = outputRegisters.get_register_cursor(m_cursor);
            switch(m_aux_register.get_type())
            {
            case DATA_UINT16:
            {
                m_aux_register.read((uint16_t&)m_value.u16);
                txStack[m_point] = m_value.byte.bt1;
                m_point++;
                txStack[m_point] = m_value.byte.bt0;
                m_point++;
                m_counter++;
                m_cursor++;
                m_byte_counter += 2;
                break;
            }
            case DATA_INT16:
            {
                m_aux_register.read((int16_t&)m_value.i16);
                txStack[m_point] = m_value.byte.bt1;
                m_point++;
                txStack[m_point] = m_value.byte.bt0;
                m_point++;
                m_counter++;
                m_cursor++;
                m_byte_counter += 2;
                break;
            }
            case DATA_UINT32:
            {
                m_aux_register.read((uint32_t&)m_value.u32);
                txStack[m_point] = m_value.byte.bt1;
                m_point++;
                txStack[m_point] = m_value.byte.bt0;
                m_point++;
                txStack[m_point] = m_value.byte.bt3;
                m_point++;
                txStack[m_point] = m_value.byte.bt2;
                m_point++;
                m_counter += 2;
                m_cursor++;
                m_byte_counter += 4;
                break;
            }
            case DATA_INT32:
            {
                m_aux_register.read((int32_t&)m_value.i32);
                txStack[m_point] = m_value.byte.bt1;
                m_point++;
                txStack[m_point] = m_value.byte.bt0;
                m_point++;
                txStack[m_point] = m_value.byte.bt3;
                m_point++;
                txStack[m_point] = m_value.byte.bt2;
                m_point++;
                m_counter += 2;
                m_cursor++;
                m_byte_counter += 4;
                break;
            }
            case DATA_FLOAT:
            {
                m_aux_register.read((float&)m_value.f);
                txStack[m_point] = m_value.byte.bt1;
                m_point++;
                txStack[m_point] = m_value.byte.bt0;
                m_point++;
                txStack[m_point] = m_value.byte.bt3;
                m_point++;
                txStack[m_point] = m_value.byte.bt2;
                m_point++;
                m_counter += 2;
                m_cursor++;
                m_byte_counter += 4;
                break;
            }
            default:{}
            }
            //
        }//for
        //
        //5. Byte number
        txStack[2] = m_byte_counter;
        //6. CRC
        m_value.all = m_crc.calculate(txStack, m_point);
        txStack[m_point] = m_value.byte.bt0;
        m_point++;
        txStack[m_point] = m_value.byte.bt1;
        m_point++;
        //
        //6. Finish
        txLength = m_point;
        rxLength = 0;
        //
    }
    else
    {
        // address wrong
        txLength = 0;
        rxLength = 0;
        //
    }//if else
    //
}//
//
void WeinbusSlave::_function_read_input_registers()
{
    // Function 0x04 read input register
    //
    txLength = 0;
    rxLength = 0;
    //
}//
//
void WeinbusSlave::_function_force_single_coil()
{
    // Function 0x05 write single coil, table is "inputCoils"
    //
    txLength = 0;
    rxLength = 0;
    //
}//
//
void WeinbusSlave::_function_preset_single_register()
{
    // Function 0x06 write single register, table is "inputRegisters"
    //
    txLength = 0;
    rxLength = 0;
    //
}//
//
void WeinbusSlave::_function_force_multiple_coils()
{
    // Function 0x0F write multiple coil, table is "inputCoils"`
    //
    txLength = 0;
    rxLength = 0;
    //
}//
//
void WeinbusSlave::_function_preset_multiple_registers()
{
    // Function 0x10 write multiple register, table is "inputRegisters"

    // construct start address
    m_start_address.byte.bt1 = rxStack[2];
    m_start_address.byte.bt0 = rxStack[3];

    // construct 16-bit register quantity
    m_length.byte.bt1 = rxStack[4];
    m_length.byte.bt0 = rxStack[5];
    m_counter_length = m_length.all;
    m_counter = 0;

    // construct byte length
    m_byte_length = rxStack[6];
    m_byte_counter = 0;


    // check address range
//    if((outputRegisters.address_range(m_start_address.all))&
//            (outputRegisters.address_range(m_start_address.all + m_length.all - (uint16_t)1)))
    if(outputRegisters.address_range(m_start_address.all))
    {

        m_cursor = inputRegisters.get_cursor(m_start_address.all);
        m_point = 7;
        while(m_byte_counter < m_byte_length)
        {
            m_aux_register = inputRegisters.get_register_cursor(m_cursor);
            switch(m_aux_register.get_type())
            {
                case DATA_UINT16:
                {
                    m_value.byte.bt1 = rxStack[m_point];
                    m_point++;
                    m_value.byte.bt0 = rxStack[m_point];
                    m_point++;
                    m_byte_counter += 2;
                    m_aux_register.write((uint16_t)m_value.u16);
                    m_cursor++;
                    break;
                }
                case DATA_INT16:
                {
                    m_value.byte.bt1 = rxStack[m_point];
                    m_point++;
                    m_value.byte.bt0 = rxStack[m_point];
                    m_point++;
                    m_byte_counter += 2;
                    m_aux_register.write((int16_t)m_value.i16);
                    m_cursor++;
                    break;
                }
                case DATA_UINT32:
                {
                    m_value.byte.bt1 = rxStack[m_point];
                    m_point++;
                    m_value.byte.bt0 = rxStack[m_point];
                    m_point++;
                    m_value.byte.bt3 = rxStack[m_point];
                    m_point++;
                    m_value.byte.bt2 = rxStack[m_point];
                    m_point++;
                    m_byte_counter += 4;
                    m_aux_register.write((uint32_t)m_value.u32);
                    m_cursor++;
                    break;
                }
                case DATA_INT32:
                {
                    m_value.byte.bt1 = rxStack[m_point];
                    m_point++;
                    m_value.byte.bt0 = rxStack[m_point];
                    m_point++;
                    m_value.byte.bt3 = rxStack[m_point];
                    m_point++;
                    m_value.byte.bt2 = rxStack[m_point];
                    m_point++;
                    m_byte_counter += 4;
                    m_aux_register.write((int32_t)m_value.i32);
                    m_cursor++;
                    break;
                }
                case DATA_FLOAT:
                {
                    m_value.byte.bt1 = rxStack[m_point];
                    m_point++;
                    m_value.byte.bt0 = rxStack[m_point];
                    m_point++;
                    m_value.byte.bt3 = rxStack[m_point];
                    m_point++;
                    m_value.byte.bt2 = rxStack[m_point];
                    m_point++;
                    m_byte_counter += 4;
                    m_aux_register.write((float)m_value.f);
                    m_cursor++;
                    break;
                }
                default:{}
            }//switch
            //
        }//while

        // CONSTRUCT ANSWER
        //1. Node ID
        txStack[0] = rxStack[0];
        //2. Function code
        txStack[1] = rxStack[1];
        //3. Address h-byte
        txStack[2] = m_start_address.byte.bt1;
        //4. Address l-byte
        txStack[3] = m_start_address.byte.bt0;
        //5. Quantity h-byte
        txStack[4] = m_length.byte.bt1;
        //6. Quantity l-byte
        txStack[5] = m_length.byte.bt0;
        //7. CRC
        m_value.all = m_crc.calculate(txStack, 6);
        txStack[6] = m_value.byte.bt0;
        txStack[7] = m_value.byte.bt1;
        //
        //8. Finish
        txLength = 8;
        rxLength = 0;
        //
    }
    else
    {
        // address wrong
        txLength = 0;
        rxLength = 0;
        //
    }//if else
    //
}//
//
//


} /* namespace WEINBUS */