#include <stdint.h>
#include <cstdlib>
#include <cassert>
#include <cstring>

class DiagJob {
private:
    // Offsets into request message.
    static const uint16_t OFS_SRC_ADR = 0;
    static const uint16_t OFS_DST_ADR = 1;
    static const uint16_t OFS_SID_BYTE1 = 2;

    // Offsets into error response message.
    static const uint16_t OFS_ERROR_BYTE1 = 2;
    static const uint16_t OFS_ERROR_BYTE2 = 3;
    static const uint16_t OFS_ERROR_BYTE3 = 4;

    // Other constants.
    static const uint16_t RESPONSE_OK_FLAG = 0x40;
    static const uint16_t ERROR_INDICATOR = 0x7F;
    static const uint16_t ERROR_CODE_BUSY_REPEAT = 0x21;
    static const uint16_t ERROR_CODE_RESPONSE_PENDING = 0x78;
    static const uint16_t RESPONSE_STATUS_OK = 0xFF;
    static const uint16_t MAX_REQUEST_LENGTH = 4096;
    static const uint16_t MAX_PAYLOAD_LENGTH = 4096;


protected:
    virtual void responseReceived(const uint8_t* response, uint16_t responseLength) {
        assert(response != NULL);
        if (responseLength > OFS_DST_ADR) {
            // If this is a reply to a previous request.
            if (response[OFS_SRC_ADR] == m_request[OFS_DST_ADR] 
                && response[OFS_DST_ADR] == m_request[OFS_SRC_ADR]) {
                if (responseLength > OFS_ERROR_BYTE3) {
                    // If error received.
                    if (response[OFS_ERROR_BYTE1] == ERROR_INDICATOR) {
                        // If error corresponds to previous request (SID).
                        if (response[OFS_ERROR_BYTE2] == m_request[OFS_SID_BYTE1]) {
                            uint8_t errorCode = response[OFS_ERROR_BYTE3];
                            switch (errorCode) {
                            case ERROR_CODE_BUSY_REPEAT:
                                // Try sending same command again.
                                transmit(m_request, m_requestLength);
                                break;
                            case ERROR_CODE_RESPONSE_PENDING:
                                // Be patient, restart time-out timer.
                                restartTimer();
                                break;
                            default:
                                // Must be a real error.
                                m_responsePayloadLength = 0;
                                m_responseStatus = errorCode;
                                break;
                            }
                        }
                    }

                    // If positive response.
                    else if (responseLength >= m_requestLength) {
                        if (response[OFS_SID_BYTE1] == (m_request[OFS_SID_BYTE1] | RESPONSE_OK_FLAG)) {
                            // Compare remaining SID bytes (excluding SA, DA, SD1)
                            if (memcmp(&response[OFS_SID_BYTE1 + 1], &m_request[OFS_SID_BYTE1 + 1], 
                                    m_requestLength - (OFS_SID_BYTE1 + 1)) == 0) {
                                // Store response payload for later use.
                                m_responsePayloadLength = responseLength - m_requestLength;
                                memcpy(&m_responsePayload[0], &m_responsePayload[m_requestLength], 
                                    m_responsePayloadLength);
                                // Response successfully received.
                                m_responseStatus = RESPONSE_STATUS_OK;
                            }
                        }
                    }
                }
            }
        }
    }

    void transmit(const uint8_t* request, uint16_t requestLength) { 
        // ...
    }

    void restartTimer() {
        // ... 
    }

private:
    uint8_t m_request[MAX_REQUEST_LENGTH];
    uint16_t m_requestLength;
    uint8_t m_responsePayload[MAX_PAYLOAD_LENGTH];
    uint16_t m_responsePayloadLength;
    uint8_t m_responseStatus;
};