package uk.co.wingpath.modbus;

import java.io.*;
import java.util.*;
import uk.co.wingpath.util.*;

/**
* This class implements the {@link ModbusRequestHandler} interface by
* interrogating/updating a {@link ModbusModel}.
*/
public class ModbusSlave
    implements ModbusRequestHandler
{
    private final ModbusModel model;
    private int slaveId;
    private final boolean isMonitor;

    private int maxPdu = Modbus.MAX_IMP_PDU_SIZE;
    protected boolean enforceCountLimits = false;
    protected boolean strictChecking = false;
    private boolean allowLongMessages = false;
    private DeviceId deviceId = null;
    private PacketType packetType = null;
    private byte [] cmd17Data;

    private int busMessageCount = 0;
    private int slaveCommErrorCount = 0;
    private int slaveExceptionErrorCount = 0;
    private int slaveMessageCount = 0;
    private int slaveNoResponseCount = 0;
    private int commEventCounter = 0;
    private boolean listenOnlyMode = false;

    /**
    * Constructs a {@code ModbusSlave} using the supplied {@code ModbusModel}
    * and slave ID.
    * @param model the Modbus model.
    * @param slaveId the slave ID.
    * @param isMonitor whether the slave is being used for monitoring.
    * If {@code true}, writes are allowed to registers that
    * are not flagged as writable, and special commands that write to
    * input registers and discrete inputs are supported.
    */
    public ModbusSlave (ModbusModel model, int slaveId, boolean isMonitor)
    {
        this.model = model;
        this.slaveId = slaveId;
        this.isMonitor = isMonitor;
        try
        {
            String str = "Wingpath";
            cmd17Data = str.getBytes ("UTF-8");
        }
        catch (UnsupportedEncodingException e)
        {
            cmd17Data = new byte [0];
        }
    }

    /**
    * Constructs a {@code ModbusSlave} using the supplied {@code ModbusModel}
    * and slave ID. Writes are only allowed to registers that are flagged as
    * writable.
    * @param model the Modbus model.
    * @param slaveId the slave ID.
    */
    public ModbusSlave (ModbusModel model, int slaveId)
    {
        this (model, slaveId, false);
    }

    /**
    * Sets the device identification.
    * Default value is {@code null};
    * @param deviceId device identification.
    */
    public void setDeviceId (DeviceId deviceId)
    {
        this.deviceId = deviceId;
    }

    /**
    * Sets the data for command 17 - Report Slave ID.
    * Default value is "Wingpath" in UTF-8.
    * @param data data for command 17.
    */
    public void setCmd17Data (Numeric.Value [] data)
    {
        cmd17Data = Numeric.toByteArray (data);
    }

    /**
    * Sets the packet type.
    * The packet type is used only to support the 'Change ASCII Input Delimiter'
    * command.
    * Default value is {@code null}.
    * @param packetType the packet type.
    */
    public void setPacketType (PacketType packetType)
    {
        this.packetType = packetType;
    }

    /**
    * Returns the Modbus model.
    * @return the model.
    */
    public ModbusModel getModel ()
    {
        return model;
    }

    /** Sets the slave ID.
    * @param slaveId the new slave ID.
    */
    public void setSlaveId (int slaveId)
    {
        this.slaveId = slaveId;
    }

    /**
    * Gets the slave ID.
    * @return the slave ID.
    */
    public int getSlaveId ()
    { 
        return slaveId;
    }

    /**
    * Sets the maximum PDU size.
    * Default value is 253 bytes.
    * @param maxPdu maximum PDU size in bytes.
    */
    public void setMaxPdu (int maxPdu)
    {
        this.maxPdu = maxPdu;
    }

    /**
    * Sets the "enforce count limit" state.
    * Default value is {@code false}.
    * @param enforceCountLimits value for "enforce count limits" state.
    */
    public void setEnforceCountLimits (boolean enforceCountLimits)
    {
        this.enforceCountLimits = enforceCountLimits;
    }

    /**
    * Sets the "strict checking" state.
    * Default value is {@code false}.
    * If set to {@code true}, various checks will be done to ensure that
    * received messages strictly conform to the Modbus standard.
    * @param strictChecking value for "strict checking" state.
    */
    public void setStrictChecking (boolean strictChecking)
    {
        this.strictChecking = strictChecking;
    }

    /**
    * Sets the "allow long messages" state.
    * Default value is {@code false}.
    * If set to {@code true}, messages with more than 255 bytes of data
    * will be allowed:
    * The byte count in received messages will be ignored, and the byte
    * count in sent messages will be set to 0 if more than 255 bytes of data
    * are being sent.
    * @param allowLongMessages value for "allow long messages" state.
    */
    public void setAllowLongMessages (boolean allowLongMessages)
    {
        this.allowLongMessages = allowLongMessages;
    }

    /**
    * Checks whether the supplied {@code ModbusRequest} contains the
    * correct slave ID for this slave.
    * @param request the Modbus request.
    * @throws ModbusException if the slave ID is incorrect.
    */
    protected void checkSlaveId (ModbusRequest request)
        throws ModbusException
    {
        int id = request.getSlaveId ();
        if (id != slaveId && slaveId != 0)
        {
            throw new ModbusException (Modbus.ERROR_PATH_UNAVAILABLE,
                "S001",
                "Wrong slave ID: " + id + " instead of " + slaveId);
        }
    }

    /**
    * Checks the request size, and gets the number of data bytes in the
    * request.
    * If long messages are allowed, the supplied byte count is ignored and
    * the number of data bytes is calculated as the request length minus
    * the supplied overhead.
    * If long messages are not allowed, the supplied byte count is returned.
    * If strict checking is enabled, the request length is checked to be
    * exactly the supplied overhead plus the supplied byte count.
    * If strict checking is disabled and long messages are not allowed,
    * the request length is checked to at least the supplied overhead plus
    * the supplied byte count.
    * @param request the request message.
    * @param overhead the number of bytes used in the request for fields
    * other than data.
    * @param byteCount the data byte count extracted from the message, or 0 if
    * the request does not contain a byte count.
    * @return the number of data bytes in the request.
    */
    protected int checkSize (ModbusRequest request, int overhead, int byteCount)
        throws ModbusException
    {
        if (allowLongMessages)
        {
            request.checkMinSize (overhead);
            return request.size () - overhead;
        }
        if (strictChecking)
            request.checkSize (overhead + byteCount);
        else
            request.checkMinSize (overhead + byteCount);
        return byteCount;
    }

    /**
    * Increments the communications error count.
    */
    public void incrementCommErrorCount ()
    {
        slaveCommErrorCount++;
    }

    /**
    * Handles a Modbus request message and returns a response message.
    * <p>The action appropriate to the request function code is taken.
    * If it is a read request, the response is constructed using values
    * from the Modbus model.
    * If it is a write request, the values are extracted from the request
    * and stored in the Modbus model.
    * If it is a diagnostic request, it is handled by this method.
    * <p>Errors are reported by throwing an exception, not by returning
    * an error response message.
    * @param request the request message.
    * @return the response message.
    * @throws ModbusException if an error occurs.
    */
    public ModbusResponse handleRequest (ModbusRequest request)
        throws ModbusException
    {
        busMessageCount++;
        int id = request.getSlaveId ();
        if (id != 0)
            checkSlaveId (request);
        slaveMessageCount++;
        if (id == 0)
            slaveNoResponseCount++;

        commEventCounter++;

        int func = request.getFunctionCode ();
        if (listenOnlyMode && func != Modbus.FUNC_DIAGNOSTICS)
        {
            throw new ModbusException (Modbus.ERROR_TARGET_FAILED_TO_RESPOND,
                "S002", "In listen-only mode");
        }

        try
        {
            switch (func)
            {
            case Modbus.FUNC_READ_HOLDING_REGISTERS:
                return handleReadHoldingRegisters (request);

            case Modbus.FUNC_READ_INPUT_REGISTERS:
                return handleReadInputRegisters (request);

            case Modbus.FUNC_WRITE_MULTIPLE_REGISTERS:
                return handleWriteMultipleRegisters (request);

            case Modbus.FUNC_WRITE_SINGLE_REGISTER:
                return handleWriteSingleRegister (request);

            case Modbus.FUNC_MASK_WRITE_REGISTER:
                return handleMaskWriteRegister (request);

            case Modbus.FUNC_READ_COILS:
                return handleReadCoils (request);

            case Modbus.FUNC_READ_DISCRETE_INPUTS:
                return handleReadDiscreteInputs (request);

            case Modbus.FUNC_WRITE_MULTIPLE_COILS:
                return handleWriteCoils (request);

            case Modbus.FUNC_WRITE_SINGLE_COIL:
                return handleWriteCoil (request);

            case Modbus.FUNC_READ_WRITE_MULTIPLE_REGISTERS:
                return handleReadWriteRegisters (request);

            case Modbus.FUNC_READ_EXCEPTION_STATUS:
                return handleReadExceptionStatus (request);

            case Modbus.FUNC_DIAGNOSTICS:
                return handleDiagnostics (request);

            case Modbus.FUNC_GET_COMM_EVENT_COUNTER:
                return handleGetCommEventCounter (request);

            case Modbus.FUNC_REPORT_SLAVE_ID:
                return handleReportSlaveID (request);

            case Modbus.FUNC_ENCAPSULATED_INTERFACE_TRANSPORT:
                return handleEncapsulatedInterfaceTransport (request);

            case Modbus.FUNC_WRITE_INPUT_REGISTERS:
                if (isMonitor)
                    return handleWriteInputRegisters (request);
                break;

            case Modbus.FUNC_WRITE_DISCRETE_INPUTS:
                if (isMonitor)
                    return handleWriteDiscreteInputs (request);
                break;
            }

            throw new ModbusException (Modbus.ERROR_ILLEGAL_FUNCTION,
                "S003", "Function " + func + " not supported");
        }
        catch (ModbusException e)
        {
            commEventCounter--;
            if (listenOnlyMode)
            {
                throw new ModbusException (
                    Modbus.ERROR_TARGET_FAILED_TO_RESPOND,
                    "S002", "In listen-only mode");
            }
            slaveExceptionErrorCount++;
            throw e;
        }
    }

    private ModbusResponse handleReadHoldingRegisters (ModbusRequest request)
        throws ModbusException
    {
        checkSlaveId (request);
        checkSize (request, 4, 0);

        int address = request.getInt (0);
        int count = request.getInt (2);
        if (strictChecking && count == 0)
            Modbus.dataError ("S008", "Invalid count (0)");

        AddressMap.Map space = model.getAddressMap ().getHoldingRegisterMap ();
        int addr = model.fromHoldingRegister (address);
        byte [] data = model.pack (addr,
            model.getNumValues (addr, count, space), space, true,
            slaveId, false);
        if (!allowLongMessages && data.length > 255)
        {
            Modbus.dataError ("S009", "Invalid count (" + count +
                ") - response byte count (" + data.length +
                ") would not fit in a byte");
        }
        if (data.length + 2 > maxPdu)
        {
            Modbus.dataError ("S010", "Invalid count (" + count +
                ") - response would exceed max PDU size (" +
                maxPdu + " bytes)");
        }
        if (enforceCountLimits && data.length > 125 * 2)
        {
            Modbus.dataError ("S011", "Invalid count (" + count +
                ") - response would exceed count limit (125 * 2 bytes)");
        }
        MessageBuilder body = new MessageBuilder ();
        body.addByte (data.length > 255 ? 0 : data.length);
        body.addData (data);
        ModbusResponse reply = request.response (body.getData ());
        return reply;
    }

    private ModbusResponse handleReadInputRegisters (ModbusRequest request)
        throws ModbusException
    {
        checkSlaveId (request);
        checkSize (request, 4, 0);

        int address = request.getInt (0);
        int count = request.getInt (2);
        if (strictChecking && count == 0)
            Modbus.dataError ("S008", "Invalid count (0)");

        AddressMap.Map space = model.getAddressMap ().getInputRegisterMap ();
        int addr = model.fromInputRegister (address);
        byte [] data = model.pack (addr,
            model.getNumValues (addr, count, space), space, true,
            slaveId, false);
        if (!allowLongMessages && data.length > 255)
        {
            Modbus.dataError ("S009", "Invalid count (" + count +
                ") - response byte count (" + data.length +
                ") would not fit in a byte");
        }
        if (data.length + 2 > maxPdu)
        {
            Modbus.dataError ("S010", "Invalid count (" + count +
                ") - response would exceed max PDU size (" +
                maxPdu + " bytes)");
        }
        if (enforceCountLimits && data.length > 125 * 2)
        {
            Modbus.dataError ("S011", "Invalid count (" + count +
                ") - response would exceed count limit (125 * 2 bytes)");
        }
        MessageBuilder body = new MessageBuilder ();
        body.addByte (data.length > 255 ? 0 : data.length);
        body.addData (data);
        ModbusResponse reply = request.response (body.getData ());
        return reply;
    }

    private ModbusResponse handleWriteMultipleRegisters (ModbusRequest request)
        throws ModbusException
    {
        request.checkMinSize (5);
        int address = request.getInt (0);
        int count = request.getInt (2);
        if (strictChecking && count == 0)
            Modbus.dataError ("S008", "Invalid count (0)");
        int size = checkSize (request, 5, request.getByte (4));
        byte [] data = request.getData (5, size);
        AddressMap.Map space = model.getAddressMap ().getHoldingRegisterMap ();
        int addr = model.fromHoldingRegister (address);
        int nvalues = model.getNumValues (addr, count, space);
        if (strictChecking &&
            model.getTotalBytes (addr, nvalues, space) != size)
        {
            Modbus.dataError ("S012", "Count (" + count +
                ") does not match number of data bytes (" + size + ")");
        }
        if (enforceCountLimits && size > 123 * 2)
        {
            Modbus.dataError ("S013", "Number of data bytes (" + size +
                ") exceeds count limit (123 * 2 bytes)");
        }
        if (!isMonitor)
            model.checkWritable (addr, nvalues, space);
        model.unpack (data, addr, nvalues, space, strictChecking,
            slaveId, true);

        MessageBuilder body = new MessageBuilder ();
        body.addInt (address);
        body.addInt (count);
        ModbusResponse reply = request.response (body.getData ());
        return reply;
    }

    private ModbusResponse handleWriteSingleRegister (ModbusRequest request)
        throws ModbusException
    {
        request.checkMinSize (3);
        int address = request.getInt (0);
        byte [] data = request.getData (2, request.size () - 2);
        AddressMap.Map space = model.getAddressMap ().getHoldingRegisterMap ();
        int addr = model.fromHoldingRegister (address);
        if (!isMonitor)
            model.checkWritable (addr, 1, space);
        model.unpack (data, addr, 1, space, strictChecking, slaveId, true);

        MessageBuilder body = new MessageBuilder ();
        body.addInt (address);
        body.addData (data);
        ModbusResponse reply = request.response (body.getData ());
        return reply;
    }

    private ModbusResponse handleMaskWriteRegister (ModbusRequest request)
        throws ModbusException
    {
        request.checkMinSize (4);
        int address = request.getInt (0);
        AddressMap.Map space = model.getAddressMap ().getHoldingRegisterMap ();
        int addr = model.fromHoldingRegister (address);
        if (!isMonitor)
            model.checkWritable (addr, 1, space);
        if (model.isFloat (addr))
        {
            Modbus.addressError ("S005",
                "Can't mask float value (register " + addr + ")");
        }
        int size = model.getValueSize (addr);
        if (size == 0)
        {
            Modbus.addressError ("S006",
                "Can't mask discrete value (register " + addr + ")");
        }
        checkSize (request, 2 + size * 2, 0);
        byte [] data = request.getData (2, size * 2);
        long andMask = model.unpackLongValue (size, data, 0, 0);
        long orMask = model.unpackLongValue (size, data, size, 0);
        model.setValue (addr,
            (model.getLongValue (addr) & andMask) | (orMask & ~andMask));

        MessageBuilder body = new MessageBuilder ();
        body.addInt (address);
        body.addData (data);
        ModbusResponse reply = request.response (body.getData ());
        return reply;
    }

    private ModbusResponse handleReadCoils (ModbusRequest request)
        throws ModbusException
    {
        checkSlaveId (request);
        checkSize (request, 4, 0);

        int address = request.getInt (0);
        int nbits = request.getInt (2);
        if (strictChecking && nbits == 0)
            Modbus.dataError ("S008", "Invalid count (0)");

        int nbytes = (nbits + 7) / 8;
        if (!allowLongMessages && nbytes > 255)
        {
            Modbus.dataError ("S009", "Invalid count (" + nbits +
                ") - response byte count (" + nbytes +
                ") would not fit in a byte");
        }
        if (nbytes + 2 > maxPdu)
        {
            Modbus.dataError ("S010", "Invalid count (" + nbits +
                ") - response would exceed max PDU size (" +
                maxPdu + " bytes)");
        }
        if (enforceCountLimits && nbits > 2000)
        {
            Modbus.dataError ("S014", "Count (" + nbits +
                ") exceeds count limit (2000)");
        }
        AddressMap.Map space = model.getAddressMap ().getCoilMap ();
        space.checkInRange (address, nbits);
        int valueSize = model.getCoilValueSize ();
        ModelAddress ma = model.fromCoil (address);
        int addr = ma.getAddress ();
        int shift = ma.getShift ();
        int multiplier = valueSize == 0 ? 1 : valueSize * 8;
        int nvalues = (nbits + shift + multiplier - 1) / multiplier;
        byte [] shdata = model.pack (addr, nvalues, space, true,
            slaveId, false);
        if (model.bitReverseCoils ())
            Bytes.reverseBits (shdata);
        if (model.byteSwapCoils ())
            Bytes.byteSwap (shdata);
        byte [] data = new byte [nbytes];
        shiftRight (data, shdata, shift);
        byte [] mask = new byte [nbytes];
        setMask (mask, nbits);
        Bytes.and (data, mask);
        MessageBuilder body = new MessageBuilder ();
        body.addByte (nbytes > 255 ? 0 : nbytes);
        body.addData (data);
        ModbusResponse reply = request.response (body.getData ());
        return reply;
    }

    private ModbusResponse handleReadDiscreteInputs (ModbusRequest request)
        throws ModbusException
    {
        checkSlaveId (request);
        checkSize (request, 4, 0);

        int address = request.getInt (0);
        int nbits = request.getInt (2);
        if (strictChecking && nbits == 0)
            Modbus.dataError ("S008", "Invalid count (0)");

        int nbytes = (nbits + 7) / 8;
        if (!allowLongMessages && nbytes > 255)
        {
            Modbus.dataError ("S009", "Invalid count (" + nbits +
                ") - response byte count (" + nbytes +
                ") would not fit in a byte");
        }
        if (nbytes + 2 > maxPdu)
        {
            Modbus.dataError ("S010", "Invalid count (" + nbits +
                ") - response would exceed max PDU size (" +
                maxPdu + " bytes)");
        }
        if (enforceCountLimits && nbits > 2000)
        {
            Modbus.dataError ("S014", "Count (" + nbits +
                ") exceeds count limit (2000)");
        }
        AddressMap.Map space = model.getAddressMap ().getDiscreteInputMap ();
        space.checkInRange (address, nbits);
        int valueSize = model.getDiscreteInputValueSize ();
        ModelAddress ma = model.fromDiscreteInput (address);
        int addr = ma.getAddress ();
        int shift = ma.getShift ();
        int multiplier = valueSize == 0 ? 1 : valueSize * 8;
        int nvalues = (nbits + shift + multiplier - 1) / multiplier;
        byte [] shdata = model.pack (addr, nvalues, space, true,
            slaveId, false);
        if (model.bitReverseDiscreteInputs ())
            Bytes.reverseBits (shdata);
        if (model.byteSwapDiscreteInputs ())
            Bytes.byteSwap (shdata);
        byte [] data = new byte [nbytes];
        shiftRight (data, shdata, shift);
        byte [] mask = new byte [nbytes];
        setMask (mask, nbits);
        Bytes.and (data, mask);
        MessageBuilder body = new MessageBuilder ();
        body.addByte (nbytes > 255 ? 0 : nbytes);
        body.addData (data);
        ModbusResponse reply = request.response (body.getData ());
        return reply;
    }

    private ModbusResponse handleWriteCoils (ModbusRequest request)
        throws ModbusException
    {
        request.checkMinSize (5);
        int address = request.getInt (0);
        int nbits = request.getInt (2);
        if (strictChecking && nbits == 0)
            Modbus.dataError ("S008", "Invalid count (0)");
        int nbytes = checkSize (request, 5, (nbits + 7) / 8);
        if (strictChecking && nbytes != request.getByte (4))
        {
            Modbus.dataError ("S015", "Number of coils (" + nbits +
                ") does not match byte count (" + nbytes + ")");
        }
        if (enforceCountLimits && nbits > 1968)
        {
            Modbus.dataError ("S014", "Count (" + nbits +
                ") exceeds count limit (1968)");
        }
        byte [] data = request.getData (5, nbytes);
        AddressMap.Map space = model.getAddressMap ().getCoilMap ();
        space.checkInRange (address, nbits);
        int valueSize = model.getCoilValueSize ();
        ModelAddress ma = model.fromCoil (address);
        int addr = ma.getAddress ();
        int shift = ma.getShift ();
        int multiplier = valueSize == 0 ? 1 : valueSize * 8;
        int nvalues = (nbits + shift + multiplier - 1) / multiplier;
        if (!isMonitor)
            model.checkWritable (addr, nvalues, space);
        int size = valueSize == 0 ? (nvalues + 7) / 8 : nvalues * valueSize;
        byte [] newData = new byte [size];
        shiftLeft (newData, data, shift);
        byte [] mask = new byte [nbytes];
        setMask (mask, nbits);
        byte [] shmask = new byte [size];
        shiftLeft (shmask, mask, shift);
        if (model.bitReverseCoils ())
        {
            Bytes.reverseBits (newData);
            Bytes.reverseBits (shmask);
        }
        if (model.byteSwapCoils ())
        {
            Bytes.byteSwap (newData);
            Bytes.byteSwap (shmask);
        }
        byte [] oldData = model.pack (addr, nvalues, space, false,
            slaveId, true);
        assert oldData.length == size;
        Bytes.andNot (oldData, shmask);
        Bytes.or (newData, oldData);
        model.unpack (newData, addr, nvalues, space, strictChecking,
            slaveId, true);

        MessageBuilder body = new MessageBuilder ();
        body.addInt (address);
        body.addInt (nbits);
        ModbusResponse reply = request.response (body.getData ());
        return reply;
    }

    private ModbusResponse handleWriteCoil (ModbusRequest request)
        throws ModbusException
    {
        checkSize (request, 4, 0);
        int address = request.getInt (0);
        int value = request.getInt (2);
        if (strictChecking && value != 0 && value != 0xff00)
            Modbus.dataError ("S016", "Value should be 0000 or FF00");
        boolean on = value != 0;
        AddressMap.Map space = model.getAddressMap ().getCoilMap ();
        space.checkInRange (address, 1);
        int valueSize = model.getCoilValueSize ();
        ModelAddress ma = model.fromCoil (address);
        int addr = ma.getAddress ();
        int shift = ma.getShift ();
        if (!isMonitor)
            model.checkWritable (addr, 1, space);
        byte [] mask = new byte [1];
        mask [0] = 1;
        int size = valueSize == 0 ? 1 : valueSize;
        byte [] shmask = new byte [size];
        shiftLeft (shmask, mask, shift);
        if (model.bitReverseCoils ())
            Bytes.reverseBits (shmask);
        if (model.byteSwapCoils ())
            Bytes.byteSwap (shmask);
        byte [] data = model.pack (addr, 1, space, false, slaveId, true);
        assert data.length == size;
        if (on)
            Bytes.or (data, shmask);
        else
            Bytes.andNot (data, shmask);
        model.unpack (data, addr, 1, space, strictChecking, slaveId, true);

        MessageBuilder body = new MessageBuilder ();
        body.addInt (address);
        body.addInt (value);
        ModbusResponse reply = request.response (body.getData ());
        return reply;
    }

    private ModbusResponse handleReadWriteRegisters (ModbusRequest request)
        throws ModbusException
    {
        checkSlaveId (request);
        request.checkMinSize (9);

        int readAddress = request.getInt (0);
        int readCount = request.getInt (2);
        if (strictChecking && readCount == 0)
            Modbus.dataError ("S017", "Invalid read count (0)");
        int writeAddress = request.getInt (4);
        int writeCount = request.getInt (6);
        if (strictChecking && writeCount == 0)
            Modbus.dataError ("S018", "Invalid write count (0)");
        int size = checkSize (request, 9, request.getByte (8));
        byte [] writeData = request.getData (9, size);

        // Check the read for errors.

        AddressMap.Map space = model.getAddressMap ().getHoldingRegisterMap ();
        int readAddr = model.fromHoldingRegister (readAddress);
        int len = model.getTotalBytes (readAddr,
            model.getNumValues (readAddr, readCount, space), space);
        if (!allowLongMessages && len > 255)
        {
            Modbus.dataError ("S019",
                "Invalid read count (" + readCount +
                ") - response byte count (" + len +
                ") would not fit in a byte");
        }
        if (len + 2 > maxPdu)
        {
            Modbus.dataError ("S020",
                "Invalid read count (" + readCount +
                ") - response would exceed max PDU size (" +
                maxPdu + " bytes)");
        }
        if (enforceCountLimits && len > 125 * 2)
        {
            Modbus.dataError ("S021",
                "Invalid read count (" + readCount +
                ") - response would exceed count limit (125 * 2 bytes)");
        }

        // Do the write (latest spec says it must be done before read).

        int writeAddr = model.fromHoldingRegister (writeAddress);
        int nvalues = model.getNumValues (writeAddr, writeCount, space);
        if (strictChecking &&
            model.getTotalBytes (writeAddr, nvalues, space) != size)
        {
            Modbus.dataError ("S022",
                "Write count (" + writeCount +
                ") does not match number of data bytes (" + size + ")");
        }
        if (enforceCountLimits && size > 121 * 2)
        {
            Modbus.dataError ("S023",
                "Number of bytes to write (" + size +
                ") exceeds count limit (121 * 2 bytes)");
        }
        if (!isMonitor)
            model.checkWritable (writeAddr, nvalues, space);
        model.unpack (writeData, writeAddr, nvalues, space, strictChecking,
            slaveId, true);

        // If this is a monitor-mode slave, don't do the read.
        // It wouldn't do any harm, but it isn't needed and it would show
        // in the register logging.
        if (isMonitor)
            return null;

        // Do the read.

        byte [] readData = model.pack (readAddr,
            model.getNumValues (readAddr, readCount, space), space, true,
            slaveId, false);

        // Construct the response.

        MessageBuilder body = new MessageBuilder ();
        body.addByte (readData.length > 255 ? 0 : readData.length);
        body.addData (readData);
        ModbusResponse reply = request.response (body.getData ());
        return reply;
    }

    /**
    * Handles a Read Exception Status request
    * Constructs a response using coils 0-7, if defined.
    *  @param request the request message
    * @return the response message
    */
    private ModbusResponse handleReadExceptionStatus (ModbusRequest request)
        throws ModbusException
    {
        checkSlaveId (request);
        checkSize (request, 0, 0);
        int addr = model.getExceptionStatusRegister ();
        if (addr < 0)
            Modbus.addressError ("S025", "No exception status register");
        AddressMap.Map space = model.getAddressMap ().getAddressSpace (addr);
        if (space == null)
            Modbus.addressError ("S025", "No exception status register");
        MessageBuilder body = new MessageBuilder ();
        int size;
        try
        {
            size = model.getValueSize (addr);
        }
        catch (ModbusException e)
        {
            Modbus.addressError ("S025", "No exception status register");
            throw new AssertionError ("Unreachable");
        }
        byte [] data = model.pack (addr, size == 0 ? 8 : 1, space, true,
            slaveId, false);
        if (size == 0 && model.getBitReverse ())
            Bytes.reverseBits (data);
        body.addByte (data [data.length - 1]);
        return request.response (body.getData ());
    }

    private ModbusResponse handleGetCommEventCounter (ModbusRequest request)
        throws ModbusException
    {
        checkSlaveId (request);
        checkSize (request, 0, 0);
        commEventCounter--;
        MessageBuilder body = new MessageBuilder ();
        body.addInt (0);
        body.addInt (commEventCounter);
        ModbusResponse reply = request.response (body.getData ());
        return reply;
    }

    private ModbusResponse handleReportSlaveID (ModbusRequest request)
        throws ModbusException
    {
        checkSlaveId (request);
        checkSize (request, 0, 0);
        MessageBuilder body = new MessageBuilder ();
        int len = cmd17Data.length;
        if (len > maxPdu - 2)
            len = maxPdu - 2;
        body.addByte (len);
        body.addData (cmd17Data, 0, len);
        return request.response (body.getData ());
    }

    private void resetCounters ()
    {
        busMessageCount = 0;
        slaveCommErrorCount = 0;
        slaveExceptionErrorCount = 0;
        slaveMessageCount = 0;
        slaveNoResponseCount = 0;
        commEventCounter = 0;
    }

    private ModbusResponse handleDiagnostics (ModbusRequest request)
        throws ModbusException
    {
        checkSlaveId (request);
        request.checkMinSize (2);
        int subfunc = request.getInt (0);

        if (listenOnlyMode)
        {
            if (subfunc == Modbus.DIAG_RESTART_COMM_OPTION)
            {
                resetCounters ();
                listenOnlyMode = false;
            }
            throw new ModbusException (Modbus.ERROR_TARGET_FAILED_TO_RESPOND,
                "S002", "In listen-only mode");
        }

        if (subfunc == Modbus.DIAG_RETURN_QUERY_DATA)
        {
            ModbusResponse reply = request.response (request.getData ());
            return reply;
        }

        checkSize (request, 4, 0);
        int requestData = request.getInt (2);
        int replyData = requestData;

        switch (subfunc)
        {
        case Modbus.DIAG_RETURN_BUS_MESSAGE_COUNT:
            replyData = busMessageCount;
            break;

        case Modbus.DIAG_RETURN_BUS_EXCEPTION_COUNT:
            replyData = slaveExceptionErrorCount;
            break;

        case Modbus.DIAG_RETURN_SLAVE_MESSAGE_COUNT:
            replyData = slaveMessageCount;
            break;

        case Modbus.DIAG_RETURN_SLAVE_NO_RESPONSE_COUNT:
            replyData = slaveNoResponseCount;
            break;

        case Modbus.DIAG_RETURN_BUS_COMM_ERROR_COUNT:
            replyData = slaveCommErrorCount;
            break;

        case Modbus.DIAG_RETURN_SLAVE_NAK_COUNT:
        case Modbus.DIAG_RETURN_SLAVE_BUSY_COUNT:
        case Modbus.DIAG_RETURN_BUS_CHARACTER_OVERRUN_COUNT:
        case Modbus.DIAG_RETURN_OVERRUN_ERROR_COUNT:
            replyData = 0;
            break;

        case Modbus.DIAG_CLEAR_OVERRUN_COUNTER:
            break;

        case Modbus.DIAG_RESTART_COMM_OPTION:
            resetCounters ();
            break;

        case Modbus.DIAG_CLEAR_COUNTERS:
            {
                resetCounters ();
                int addr = model.getDiagnosticRegister ();
                if (addr >= 0)
                    model.setValue (addr, 0);
            }
            break;

        case Modbus.DIAG_RETURN_DIAGNOSTIC_REGISTER:
            {
                int addr = model.getDiagnosticRegister ();
                if (addr < 0)
                    Modbus.addressError ("S026", "No diagnostic register");
                AddressMap.Map space =
                    model.getAddressMap ().getAddressSpace (addr);
                if (space == null)
                    Modbus.addressError ("S026", "No diagnostic register");
                int size;
                try
                {
                    size = model.getValueSize (addr);
                }
                catch (ModbusException e)
                {
                    Modbus.addressError ("S026", "No diagnostic register");
                    throw new AssertionError ("Unreachable");
                }
                byte [] data = model.pack (addr,
                    size == 0 ? 16 : size == 1 ? 2 : 1, space, true,
                    slaveId, false);
                if (size == 0 && model.getBitReverse ())
                    Bytes.reverseBits (data);
                if (size <= 1 && model.getByteSwap ())
                    Bytes.byteSwap (data);
                replyData = Bytes.toShort (data, data.length - 2);
            }
            break;

        case Modbus.DIAG_FORCE_LISTEN_ONLY_MODE:
            listenOnlyMode = true;
            throw new ModbusException (Modbus.ERROR_TARGET_FAILED_TO_RESPOND,
                "S002", "In listen-only mode");

        case Modbus.DIAG_CHANGE_ASCII_INPUT_DELIMITER:
            if (packetType != null)
                packetType.setDelimiter ((byte) requestData);
            break;

        default:
            throw new ModbusException (Modbus.ERROR_ILLEGAL_FUNCTION,
                "S004", "Unknown sub-function (" + subfunc + ")");
        }

        MessageBuilder body = new MessageBuilder ();
        body.addInt (subfunc);
        body.addInt (replyData);
        ModbusResponse reply = request.response (body.getData ());
        return reply;
    }

    private ModbusResponse handleEncapsulatedInterfaceTransport (
            ModbusRequest request)
        throws ModbusException
    {
        checkSlaveId (request);
        request.checkMinSize (1);

        switch (request.getByte (0))
        {
        case Modbus.MEI_READ_DEVICE_IDENTIFICATION:
            return handleReadDeviceIdentification (request);

        default:
            throw new ModbusException (Modbus.ERROR_ILLEGAL_FUNCTION,
                "S003",
                "Function " + Modbus.FUNC_ENCAPSULATED_INTERFACE_TRANSPORT +
                "/" + request.getByte (0) + " not supported");
        }
    }

    private ModbusResponse handleReadDeviceIdentification (
            ModbusRequest request)
        throws ModbusException
    {
        checkSlaveId (request);
        if (deviceId == null)
        {
            throw new ModbusException (Modbus.ERROR_ILLEGAL_FUNCTION,
                "S003",
                "Function " + Modbus.FUNC_ENCAPSULATED_INTERFACE_TRANSPORT +
                "/" + Modbus.MEI_READ_DEVICE_IDENTIFICATION + " not supported");
        }
        checkSize (request, 3, 0);
        int code = request.getByte (1);
        int objectId = request.getByte (2);
        if (objectId != 0)
        {
            if (deviceId.get (objectId) == null && code != 0x04)
                objectId = 0;
        }

        // Get list of object IDs to send.

        List<Integer> ids;

        switch (code)
        {
        case 0x01:
            ids = deviceId.getBasicIds (objectId);
            break;

        case 0x02:
            ids = deviceId.getRegularIds (objectId);
            break;

        case 0x03:
            ids = deviceId.getExtendedIds (objectId);
            break;

        case 0x04:
            if (deviceId.get (objectId) == null)
            {
                Modbus.addressError ("S007", "Invalid object ID: " +
                    String.format ("%02x", objectId));
            }
            ids = new ArrayList<Integer> ();
            ids.add (objectId);
            break;

        default:
            Modbus.dataError ("S024",
                "Invalid Read Device ID code: " +
                String.format ("%02x", code));
            throw new AssertionError ("Unreachable");
        }

        // See how many of the objects in the list will fit in the response
        // message.

        int size = 7;
        int nextId = 0;
        int count = 0;

        for (int id : ids)
        {
            byte [] data = deviceId.getBytes (id);
            int len = data.length > maxPdu - 9 ? maxPdu - 9 : data.length;
            if (size + 2 + len > maxPdu)
            {
                nextId = id;
                break;
            }
            count++;
            size += 2 + len;
        }

        // Construct the response message.

        MessageBuilder body = new MessageBuilder ();
        body.addByte (Modbus.MEI_READ_DEVICE_IDENTIFICATION);
        body.addByte (code);
        body.addByte (deviceId.computeConformityLevel ());
        body.addByte (nextId == 0 ? 0 : 0xff);    // More follows?
        body.addByte (nextId);      // Next object Id
        body.addByte (count);       // Number of objects

        int n = 0;

        for (int id : ids)
        {
            if (n >= count)
                break;
            body.addByte (id);
            byte [] data = deviceId.getBytes (id);
            int len = data.length > maxPdu - 9 ? maxPdu - 9 : data.length;
            body.addByte (len);
            body.addData (data, 0, len);
            n++;
        }

        ModbusResponse reply = request.response (body.getData ());
        return reply;
    }

    private ModbusResponse handleWriteInputRegisters (ModbusRequest request)
        throws ModbusException
    {
        request.checkMinSize (5);
        int address = request.getInt (0);
        int count = request.getInt (2);
        if (strictChecking && count == 0)
            Modbus.dataError ("S008", "Invalid count (0)");
        int size = checkSize (request, 5, request.getByte (4));
        byte [] data = request.getData (5, size);
        AddressMap.Map space = model.getAddressMap ().getInputRegisterMap ();
        int addr = model.fromInputRegister (address);
        int nvalues = model.getNumValues (addr, count, space);
        if (strictChecking &&
            model.getTotalBytes (addr, nvalues, space) != size)
        {
            Modbus.dataError ("S012", "Count (" + count +
                ") does not match number of data bytes (" + size + ")");
        }
        if (enforceCountLimits && size > 123 * 2)
        {
            Modbus.dataError ("S013", "Number of data bytes (" + size +
                ") exceeds count limit (123 * 2 bytes)");
        }
        model.unpack (data, addr, nvalues, space, strictChecking,
            slaveId, false);

        MessageBuilder body = new MessageBuilder ();
        body.addInt (address);
        body.addInt (count);
        ModbusResponse reply = request.response (body.getData ());
        return reply;
    }

    private ModbusResponse handleWriteDiscreteInputs (ModbusRequest request)
        throws ModbusException
    {
        request.checkMinSize (5);
        int address = request.getInt (0);
        int nbits = request.getInt (2);
        if (strictChecking && nbits == 0)
            Modbus.dataError ("S008", "Invalid count (0)");
        int nbytes = checkSize (request, 5, (nbits + 7) / 8);
        if (strictChecking && nbytes != request.getByte (4))
        {
            Modbus.dataError ("S015", "Number of coils (" + nbits +
                ") does not match byte count (" + nbytes + ")");
        }
        if (enforceCountLimits && nbits > 1968)
        {
            Modbus.dataError ("S014", "Count (" + nbits +
                ") exceeds count limit (1968)");
        }
        byte [] data = request.getData (5, nbytes);
        AddressMap.Map space = model.getAddressMap ().getDiscreteInputMap ();
        space.checkInRange (address, nbits);
        int valueSize = model.getDiscreteInputValueSize ();
        ModelAddress ma = model.fromDiscreteInput (address);
        int addr = ma.getAddress ();
        int shift = ma.getShift ();
        int multiplier = valueSize == 0 ? 1 : valueSize * 8;
        int nvalues = (nbits + shift + multiplier - 1) / multiplier;
        int size = valueSize == 0 ? (nvalues + 7) / 8 : nvalues * valueSize;
        byte [] newData = new byte [size];
        shiftLeft (newData, data, shift);
        byte [] mask = new byte [nbytes];
        setMask (mask, nbits);
        byte [] shmask = new byte [size];
        shiftLeft (shmask, mask, shift);
        if (model.bitReverseCoils ())
        {
            Bytes.reverseBits (newData);
            Bytes.reverseBits (shmask);
        }
        if (model.byteSwapCoils ())
        {
            Bytes.byteSwap (newData);
            Bytes.byteSwap (shmask);
        }
        byte [] oldData = model.pack (addr, nvalues, space, false,
            slaveId, false);
        assert oldData.length == size;
        Bytes.andNot (oldData, shmask);
        Bytes.or (newData, oldData);
        model.unpack (newData, addr, nvalues, space, strictChecking,
            slaveId, false);

        MessageBuilder body = new MessageBuilder ();
        body.addInt (address);
        body.addInt (nbits);
        ModbusResponse reply = request.response (body.getData ());
        return reply;
    }

    /**
    * Sets the first {@code len} bits of a byte array to 1, and the remaining
    * bits to 0.
    * <p>The "first" bit in each byte is the least-significant (rightmost) bit
    * (as required for Modbus coils and discrete-inputs) - this looks
    * confusing if you print the array using {@link Bytes#toHexString}.
    * @param mask the byte array.
    * @param len how many bits to set to 1.
    */
    private static void setMask (byte [] mask, int len)
    {
        int byteLen = len / 8;
        int bitLen = len % 8;

        for (int i = 0 ; i < mask.length ; i++)
        {
            if (i < byteLen)
                mask [i] = (byte) 0xff;
            else if (i == byteLen)
                mask [i] = (byte) (0xff >> (8 - bitLen));
            else
                mask [i] = 0;
        }
    }

    /**
    * Shifts the contents of a byte array left by a specified number of bits.
    * <p>The "first" bit in each byte is the least-significant (rightmost) bit
    * (as required for Modbus coils and discrete-inputs) - this looks
    * confusing if you print the result using {@link Bytes#toHexString}.
    * @param dest where to store the result.
    * @param src the byte array to be shifted.
    * @param shift how many bits to shift by.
    */
    private static void shiftLeft (byte [] dest, byte [] src, int shift)
    {
        if (shift < 0)
        {
            shiftRight (dest, src, -shift);
            return;
        }
        int byteShift = shift / 8;
        int bitShift = shift % 8;
        byte last = 0;

        for (int i = 0 ; i < dest.length ; i++)
        {
            int j = i - byteShift;
            byte b = (j >= 0 && j < src.length) ? src [j] : 0;
            dest [i] = (byte) (((last & 0xff) >> (8 - bitShift)) |
                (b << bitShift));
            last = b;
        }
    }

    /**
    * Shifts the contents of a byte array right by a specified number of bits.
    * <p>The "first" bit in each byte is the least-significant (rightmost) bit
    * (as required for Modbus coils and discrete-inputs) - this looks
    * confusing if you print the result using {@link Bytes#toHexString}.
    * @param dest where to store the result.
    * @param src the byte array to be shifted.
    * @param shift how many bits to shift by.
    */
    private static void shiftRight (byte [] dest, byte [] src, int shift)
    {
        if (shift < 0)
        {
            shiftLeft (dest, src, -shift);
            return;
        }
        int byteShift = shift / 8;
        int bitShift = shift % 8;
        byte b = src [byteShift];

        for (int i = 0 ; i < dest.length ; i++)
        {
            int j = i + byteShift + 1;
            byte next = (j >= 0 && j < src.length) ? src [j] : 0;
            dest [i] = (byte) (((b & 0xff) >> bitShift) |
                (next << (8 - bitShift)));
            b = next;
        }
    }

}