#include <string.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <math.h>

#include <main.h>
#include <usart.h>
#include <PontCoopScheduler.h>
#include <mbusComm.h>
#include <loopCtrl.h>
#include <show.h>
#include <logger.h>
#include <frontend.h>
#include <wizHelper.h>
#include <mbusParserExt.h>
#include <mqttComm.h>
#include <oled.h>
#include <ringbuffer.h>

#include <mbus/mbus-protocol.h>

static const char MBUS_TOPIC[] = "IoT/MBGW3/Measurement";

static const uint8_t MBUS_QUERY_CMD = 0x5b;

typedef enum {
    MBCR_SUCCESS = 0,
    MBCR_ERROR_TIMEOUT,
    MBCR_ERROR_LOOP_FAILURE,
    MBCR_ERROR_TX_REG_UNACCESSIBLE,
    MBCR_ERROR_OUT_OF_MEMORY__FRAME,
    MBCR_ERROR_OUT_OF_MEMORY__USERDATA,
    MBCR_ERROR_STATE_ENGINE__START1,
    MBCR_ERROR_STATE_ENGINE__LENGTH1,
    MBCR_ERROR_STATE_ENGINE__LENGTH2,
    MBCR_ERROR_STATE_ENGINE__START2,
    MBCR_ERROR_STATE_ENGINE__INVALID_CHKSUM,
    MBCR_ERROR_STATE_ENGINE__STOP,
    MBCR_ERROR_STATE_ENGINE__ILLEGAL_STATE,
    MBCR_ERROR_STATE_ENGINE__UNKNOWN
} e_mbusCommResult;

typedef enum {
    MBCS_IDLE,
    MBCS_SEND,
    MBCS_SEND_CONTINUED,
    MBCS_SENDING,
    MBCS_SENDING_DONE,
    MBCS_ENABLE_FRONTEND,
    MBCS_START1,
    MBCS_LENGTH1,
    MBCS_LENGTH2,
    MBCS_START2,
    MBCS_C_FIELD,
    MBCS_A_FIELD,
    MBCS_CI_FIELD,
    MBCS_USERDATA,
    MBCS_CHKSUM,
    MBCS_STOP,
    MBCS_DONE,
    MBCS_TIMEOUT,
    MBCS_DISABLE_FRONTEND,
    MBCS_ERROR,
    MBCS_ERROR_CONTINUED
} e_mbusCommState;

typedef struct {
  uint8_t start1;
  uint8_t length1;
  uint8_t length2;
  uint8_t start2;
  uint8_t l;
  uint8_t c;
  uint8_t a;
  uint8_t ci;
  uint8_t *userdata;
  uint8_t chksum;
  uint8_t stop;
} t_longframe;

typedef struct {
    uint16_t size;
    uint16_t readIdx;
    uint16_t writeIdx;
    uint8_t *buffer;
} linearBuffer_t;

typedef struct {
    uint32_t requestId;
    e_mbusCommState state;
    uint8_t retryCnt;
    uint8_t cmd;
    uint8_t addr;
    linearBuffer_t sendBuffer;
    linearBuffer_t receiveBuffer;
    uint32_t startTime;
    uint8_t receiveCnt;
    bool waitForOctet;
    e_mbusCommResult result;
    t_longframe frame;
    t_mbusDevice *device;
} t_mbusCommHandle;


static t_mbusCommHandle mbusCommHandle = { .requestId = 0, .state = MBCS_IDLE, .retryCnt = 0, .cmd = 0, .addr = 0, .startTime = 0, .receiveCnt = 0, .waitForOctet = false };

static t_mbusCommStats mbusCommStats = { .mbusRequestCnt = 0, .mbusErrorCnt = 0, .uartOverrunCnt = 0, .uartFramingErrCnt = 0, .uartParityErrCnt = 0 };

static bool mbusCommEnabled = true;

void mbusCommSetStats(t_mbusCommStats stats) {
    mbusCommStats = stats;
}

t_mbusCommStats *mbusCommGetStats() {
    return &mbusCommStats;
}


static void printError() {
    float errorRatio = ((float) mbusCommHandle.device->failures) / ((float) mbusCommHandle.device->requests);
    coloredMsg(LOG_YELLOW, true, "mbc pe [%d] Error ratio is %.2f", 
               mbusCommHandle.requestId,
               errorRatio);
    mqttPublishf(MBUS_TOPIC, "{\"Status\":\"Error\", \"RequestId\":\"%d\", \"Device\":\"%s\", \"Errors\":\"%d\", \"Requests\":\"%d\", \"ErrorRatio\":\"%.2f\"}", 
                 mbusCommHandle.requestId, mbusCommHandle.device->deviceName,
                 mbusCommHandle.device->failures, mbusCommHandle.device->requests, errorRatio);
    oledPrintf(OLED_SCREEN0, "Err:%d/%d %.2f", mbusCommHandle.device->failures, mbusCommHandle.device->requests, errorRatio);
}



static void parseAndPrintFrame() {
    t_longframe *frame = &(mbusCommHandle.frame);

    mbus_frame reply;
    memset(&reply, 0, sizeof(reply));

    //mbus_parse(&reply, buf, len);
    reply.start1 = frame->start1;
    reply.length1 = frame->length1;
    reply.length2 = frame->length2;
    reply.start2 = frame->start2;
    reply.control = frame->c;
    reply.address = frame->a;
    reply.control_information = frame->ci;
    memcpy(reply.data, frame->userdata, frame->length1 - 3);
    reply.checksum = frame->chksum;
    reply.stop = frame->stop;
    reply.type = MBUS_FRAME_TYPE_LONG;
    reply.data_size = frame->length1 - 3;

    mbus_frame_data frame_data;
    memset(&frame_data, 0, sizeof(frame_data));

    int r = mbus_frame_data_parse(&reply, &frame_data);
    if (r == 0) {
        mbus_data_variable *data_var = &(frame_data.data_var);
        coloredMsg(LOG_YELLOW, false, "mbc papf [%d] sts: %02x", mbusCommHandle.requestId, data_var->header.status);
        if ((data_var->header.status & 0x01)) {
            coloredMsg(LOG_RED, true, "mbc papf [%d] sts: Application Busy", mbusCommHandle.requestId);
        }
        if ((data_var->header.status & 0x02)) {
            coloredMsg(LOG_RED, true, "mbc papf [%d] sts: Any Application Error", mbusCommHandle.requestId);
        }
        if ((data_var->header.status & 0x04)) {
            coloredMsg(LOG_RED, true, "mbc papf [%d] sts: Power Low", mbusCommHandle.requestId);
        }
        if ((data_var->header.status & 0x08)) {
            coloredMsg(LOG_RED, true, "mbc papf [%d] sts: Permanent Error", mbusCommHandle.requestId);
        }
        if ((data_var->header.status & 0x10)) {
            coloredMsg(LOG_RED, true, "mbc papf [%d] sts: Temporary Error", mbusCommHandle.requestId);
        }
        if ((data_var->header.status & 0x20)) {
            coloredMsg(LOG_RED, true, "mbc papf [%d] sts: Specific to manufacturer Error 1", mbusCommHandle.requestId);
        }
        if ((data_var->header.status & 0x40)) {
            coloredMsg(LOG_RED, true, "mbc papf [%d] sts: Specific to manufacturer Error 2", mbusCommHandle.requestId);
        }
        if ((data_var->header.status & 0x80)) {
            coloredMsg(LOG_RED, true, "mbc papf [%d] sts: Specific to manufacturer Error 3", mbusCommHandle.requestId);
        }
        mbus_data_record *record;
        int i;
        const char *keys[MBUSDEVICE_NUM_OF_CONSIDEREDFIELDS];
        float values[MBUSDEVICE_NUM_OF_CONSIDEREDFIELDS];
        uint8_t numOfConsideredFields = 0;
        for (record = data_var->record, i = 0;
             record;
             record = record->next, i++) {
            for (uint8_t j = 0; j < MBUSDEVICE_NUM_OF_CONSIDEREDFIELDS; j++) {
                if (mbusCommHandle.device->consideredField[j] == i) {
                    parsedVIB_t parsedVIB = parseVIB(record->drh.vib);
                    coloredMsg(LOG_YELLOW, false, "mbc papf [%d] parsed VIB N: %s, U: %s, E: %d",
                               mbusCommHandle.requestId,
                               parsedVIB.name, parsedVIB.unit, parsedVIB.exponent);
                    if (parsedVIB.found) {
                        uint32_t value = strtol(mbus_data_record_value(record), NULL, 10);
                        float weightedValue = ((float) value) * powf(10.0, ((float) parsedVIB.exponent));
                        coloredMsg(LOG_YELLOW, true, "mbc papf [%d] %s is %.1f %s (%d * 10^%d)",
                                   mbusCommHandle.requestId, parsedVIB.name, weightedValue, parsedVIB.unit,
                                   value, parsedVIB.exponent);
                        keys[numOfConsideredFields] = parsedVIB.name;
                        values[numOfConsideredFields] = weightedValue;
                        numOfConsideredFields++;
                    } else {
                        coloredMsg(LOG_YELLOW, true, "mbc papf [%d] L:%d, VIF: 0x%02x U:%s V:%s", 
                                   mbusCommHandle.requestId,
                                   mbusCommHandle.device->consideredField[j],
                                   record->drh.vib.vif,
                                   mbus_data_record_unit(record),
                                   mbus_data_record_value(record));
                    }
                    
                }
            }
        }
        float errorRatio = ((float) mbusCommHandle.device->failures) / ((float) mbusCommHandle.device->requests);
        coloredMsg(LOG_YELLOW, true, "mbc papf [%d] Error ratio is %.2f", 
                                   mbusCommHandle.requestId,
                                   errorRatio);
        if (numOfConsideredFields == 1) {
            mqttPublishf(MBUS_TOPIC, "{\"Status\":\"Ok\", \"RequestId\":\"%d\", \"Device\":\"%s\", \"Errors\":\"%d\", \"Requests\":\"%d\", \"ErrorRatio\":\"%.2f\", " \
                                     "\"Values\":{\"%s\":\"%.1f\"}}", 
                         mbusCommHandle.requestId, mbusCommHandle.device->deviceName,
                         mbusCommHandle.device->failures, mbusCommHandle.device->requests, errorRatio,
                         keys[0], values[0]);
        } else if (numOfConsideredFields == 2) {
            mqttPublishf(MBUS_TOPIC, "{\"Status\":\"Ok\", \"RequestId\":\"%d\", \"Device\":\"%s\", \"Errors\":\"%d\", \"Requests\":\"%d\", \"ErrorRatio\":\"%.2f\", " \
                                     "\"Values\":{\"%s\":\"%.1f\", \"%s\":\"%.1f\"}}", 
                         mbusCommHandle.requestId, mbusCommHandle.device->deviceName,
                         mbusCommHandle.device->failures, mbusCommHandle.device->requests, errorRatio,
                         keys[0], values[0], keys[1], values[1]);
        } else if (numOfConsideredFields == 3) {
            mqttPublishf(MBUS_TOPIC, "{\"Status\":\"Ok\", \"RequestId\":\"%d\", \"Device\":\"%s\", \"Errors\":\"%d\", \"Requests\":\"%d\", \"ErrorRatio\":\"%.2f\", " \
                                     "\"Values\":{\"%s\":\"%.1f\", \"%s\":\"%.1f\", \"%s\":\"%.1f\"}}", 
                         mbusCommHandle.requestId, mbusCommHandle.device->deviceName,
                         mbusCommHandle.device->failures, mbusCommHandle.device->requests, errorRatio,
                         keys[0], values[0], keys[1], values[1], keys[2], values[2]);
        } else if (numOfConsideredFields == 4) {
            mqttPublishf(MBUS_TOPIC, "{\"Status\":\"Ok\", \"RequestId\":\"%d\", \"Device\":\"%s\", \"Errors\":\"%d\", \"Requests\":\"%d\", \"ErrorRatio\":\"%.2f\", " \
                                     "\"Values\":{\"%s\":\"%.1f\", \"%s\":\"%.1f\", \"%s\":\"%.1f\", \"%s\":\"%.1f\"}}", 
                         mbusCommHandle.requestId, mbusCommHandle.device->deviceName,
                         mbusCommHandle.device->failures, mbusCommHandle.device->requests, errorRatio,
                         keys[0], values[0], keys[1], values[1], keys[2], values[2], keys[3], values[3]);
        }
        oledPrintf(OLED_SCREEN0, "Ok:%d/%d %.2f", mbusCommHandle.device->failures, mbusCommHandle.device->requests, errorRatio);
        mbus_data_record_free(data_var->record);
    } else {
        coloredMsg(LOG_RED, true, "mbc papf [%d] err: unable to parse frame", mbusCommHandle.requestId);
    }
}



void mbusCommISR() {
    show(DEBUG_1, TOGGLE);

    uint32_t isrflags   = READ_REG(mbusUart.Instance->SR);

    if (((isrflags & USART_SR_RXNE) != RESET) || ((isrflags & USART_SR_ORE) != RESET)) {
        if ((isrflags & USART_SR_ORE) != RESET) {
            mbusCommStats.uartOverrunCnt += 1;
        }
        // it is required to read the DR in any case here, not only when the buffer has space
        // otherwise the interrupt flag won't be disabled, particularly important in case of
        // ORE
        uint8_t data = (uint8_t)(mbusUart.Instance->DR & (uint8_t)0x00FF);
        if (mbusCommHandle.receiveBuffer.writeIdx < mbusCommHandle.receiveBuffer.size) {
            mbusCommHandle.receiveBuffer.buffer[mbusCommHandle.receiveBuffer.writeIdx] = data;
            mbusCommHandle.receiveBuffer.writeIdx += 1;
        }
    } 
    
    if ((isrflags & USART_SR_TXE) != RESET) {
        coloredMsg(LOG_RED, false, "TXE");
        if (mbusCommHandle.sendBuffer.readIdx < mbusCommHandle.sendBuffer.writeIdx) {
            mbusUart.Instance->DR = mbusCommHandle.sendBuffer.buffer[mbusCommHandle.sendBuffer.readIdx];
            mbusCommHandle.sendBuffer.readIdx += 1;
            if (mbusCommHandle.sendBuffer.readIdx == mbusCommHandle.sendBuffer.writeIdx) {
                __HAL_UART_DISABLE_IT(&mbusUart, UART_IT_TXE);
                __HAL_UART_ENABLE_IT(&mbusUart, UART_IT_TC);
            }
        }
    } 
    
    if ((isrflags & USART_SR_TC) != RESET) {
        coloredMsg(LOG_RED, false, "TC");
        __HAL_UART_DISABLE_IT(&mbusUart, UART_IT_TC);
        mbusCommHandle.state = MBCS_SENDING_DONE;
    } 
    
    if ((isrflags & USART_SR_FE) != RESET) {
            mbusCommStats.uartFramingErrCnt += 1;
    }
    
    if ((isrflags & USART_SR_PE) != RESET) {
            mbusCommStats.uartParityErrCnt += 1;
    }
}


void mbusCommExec() {
    static uint8_t userdataIdx = 0;
    static uint8_t calculatedChksum = 0;
    uint8_t receivedOctet = 0;

    if ((mbusCommHandle.startTime != 0) && ((mbusCommHandle.startTime + 2500) < HAL_GetTick())) {
        coloredMsg(LOG_RED, false, "TIMEOUT!! %d %ld %ld", mbusCommHandle.state, mbusCommHandle.startTime, HAL_GetTick());
        mbusCommHandle.startTime = 0;
        mbusCommHandle.state = MBCS_TIMEOUT;
    } else if (mbusCommHandle.waitForOctet) {
        if (mbusCommHandle.receiveBuffer.readIdx >= mbusCommHandle.receiveBuffer.writeIdx) {
            return;
        }
        receivedOctet = mbusCommHandle.receiveBuffer.buffer[mbusCommHandle.receiveBuffer.readIdx];
        mbusCommHandle.receiveBuffer.readIdx += 1;
        mbusCommHandle.waitForOctet = false;
    }

    switch (mbusCommHandle.state) {
        case MBCS_IDLE:
        // coloredMsg(LOG_YELLOW, false, "mbc hre [%d] state IDLE", mbusCommHandle.requestId);
        break;

        case MBCS_SEND:
        coloredMsg(LOG_YELLOW, false, "mbc hre [%d] state SEND", mbusCommHandle.requestId);
        mbusCommHandle.sendBuffer.buffer[0] = 0x10;
        mbusCommHandle.sendBuffer.buffer[1] = mbusCommHandle.cmd;
        mbusCommHandle.sendBuffer.buffer[2] = mbusCommHandle.addr;
        mbusCommHandle.sendBuffer.buffer[3] = mbusCommHandle.cmd + mbusCommHandle.addr; // checksum
        mbusCommHandle.sendBuffer.buffer[4] = 0x16;
        mbusCommHandle.sendBuffer.readIdx = 0;
        mbusCommHandle.sendBuffer.writeIdx = 5;
        mbusCommHandle.state = MBCS_SEND_CONTINUED;
        // no break !!

        case MBCS_SEND_CONTINUED:
        coloredMsg(LOG_YELLOW, false, "mbc hre [%d] state SEND_CONTINUED", mbusCommHandle.requestId);
        show(LED_RED, OFF);
        if (! loopActive) {
            coloredMsg(LOG_YELLOW, true, "mbc hre [%d] enabling loop, try %d", mbusCommHandle.requestId, mbusCommHandle.retryCnt);
            mbusCommHandle.retryCnt++;
            loopEnable();
            // FIXME somehow manage to delay for about 100ms
        } else {
            mbusCommHandle.retryCnt = 0;
            // FIXME sending possibly needs to be done in a different way
            // HAL_UART_Transmit(&mbusUart, mbusCommHandle.sendBuffer.buffer, 5, HAL_MAX_DELAY);
            
            // enable transmitter interrupt
            coloredMsg(LOG_YELLOW, false, "mbc hre [%d] enable transmitter interrupt", mbusCommHandle.requestId);
            __HAL_UART_ENABLE_IT(&mbusUart, UART_IT_TXE);

            // transition from here to SENDING_DONE is initiate by mbusCommTxCpltCallback
            // interrupt callback
            mbusCommHandle.state = MBCS_SENDING;
        }
        break;

        case MBCS_SENDING:
        break;

        case MBCS_SENDING_DONE:
        coloredMsg(LOG_YELLOW, false, "mbc hre [%d] state SENDING_DONE", mbusCommHandle.requestId);
        mbusCommHandle.state = MBCS_ENABLE_FRONTEND;
        // FIXME somehow manage to delay for about 3ms
        break;

        case MBCS_ENABLE_FRONTEND:
        coloredMsg(LOG_YELLOW, false, "mbc hre [%d] state ENABLE_FRONTEND", mbusCommHandle.requestId);
        frontendEnable();
        calculatedChksum = 0;
        userdataIdx = 0;
        mbusCommHandle.receiveBuffer.readIdx = 0;
        mbusCommHandle.receiveBuffer.writeIdx = 0;
        mbusCommHandle.waitForOctet = true; // start receiver
        mbusCommHandle.startTime = HAL_GetTick(); // start receiver timeout
        mbusCommHandle.state = MBCS_START1;
        break;

        case MBCS_START1:
        if (receivedOctet == 0x68) {
            mbusCommHandle.frame.start1 = receivedOctet;
            mbusCommHandle.waitForOctet = true;
            mbusCommHandle.state = MBCS_LENGTH1;
        } else {
            coloredMsg(LOG_RED, true, "mbc hre [%d] err: invalid start1 symbol %02x", mbusCommHandle.requestId, receivedOctet);
            mbusCommHandle.result = MBCR_ERROR_STATE_ENGINE__START1;
            mbusCommHandle.state = MBCS_ERROR;
        }
        break;

        case MBCS_LENGTH1:
        if (receivedOctet <= 3) {
            coloredMsg(LOG_RED, true, "mbc hre [%d] err: length to small %02x", mbusCommHandle.requestId, receivedOctet);
            mbusCommHandle.result = MBCR_ERROR_STATE_ENGINE__LENGTH1;
            mbusCommHandle.state = MBCS_ERROR;
        } else {
            mbusCommHandle.frame.length1 = receivedOctet;
            mbusCommHandle.frame.userdata = (uint8_t*) malloc(mbusCommHandle.frame.length1 - 3);
            if (! mbusCommHandle.frame.userdata) {
                coloredMsg(LOG_RED, true, "mbc hre [%d] err: unable to allocate memory for userdata", mbusCommHandle.requestId);
                mbusCommHandle.result = MBCR_ERROR_OUT_OF_MEMORY__USERDATA;
                mbusCommHandle.state = MBCS_ERROR;
            } else {
                mbusCommHandle.waitForOctet = true;
                mbusCommHandle.state = MBCS_LENGTH2;
            }
        }
        break;

        case MBCS_LENGTH2:
        if (mbusCommHandle.frame.length1 != receivedOctet) {
            coloredMsg(LOG_RED, true, "mbc hre [%d] err: invalid length2 %02x vs. %02x", 
                       mbusCommHandle.requestId, mbusCommHandle.frame.length1, receivedOctet);
            mbusCommHandle.result = MBCR_ERROR_STATE_ENGINE__LENGTH2;
            mbusCommHandle.state = MBCS_ERROR;
        } else {
            mbusCommHandle.frame.length2 = receivedOctet;
            mbusCommHandle.waitForOctet = true;
            mbusCommHandle.state = MBCS_START2;
        }
        break;

        case MBCS_START2:
        if (receivedOctet == 0x68) {
            mbusCommHandle.frame.start2 = receivedOctet;
            mbusCommHandle.waitForOctet = true;
            mbusCommHandle.state = MBCS_C_FIELD;
        } else {
            coloredMsg(LOG_RED, true, "mbc hre [%d] err: invalid start2 symbol %02x", 
                       mbusCommHandle.requestId, receivedOctet);
            mbusCommHandle.result = MBCR_ERROR_STATE_ENGINE__START2;
            mbusCommHandle.state = MBCS_ERROR;
        }
        break;

        case MBCS_C_FIELD:
        mbusCommHandle.frame.c = receivedOctet;
        calculatedChksum += receivedOctet;
        mbusCommHandle.waitForOctet = true;
        mbusCommHandle.state = MBCS_A_FIELD;
        break;

        case MBCS_A_FIELD:
        mbusCommHandle.frame.a = receivedOctet;
        calculatedChksum += receivedOctet;
        mbusCommHandle.waitForOctet = true;
        mbusCommHandle.state = MBCS_CI_FIELD;
        break;

        case MBCS_CI_FIELD:
        mbusCommHandle.frame.ci = receivedOctet;
        calculatedChksum += receivedOctet;
        mbusCommHandle.waitForOctet = true;
        mbusCommHandle.state = MBCS_USERDATA;
        break;

        case MBCS_USERDATA:
        mbusCommHandle.frame.userdata[userdataIdx] = receivedOctet;
        calculatedChksum += receivedOctet;
        userdataIdx++;
        mbusCommHandle.waitForOctet = true;
        if (userdataIdx == (mbusCommHandle.frame.length1 - 3)) {
            mbusCommHandle.state = MBCS_CHKSUM;
        }
        break;

        case MBCS_CHKSUM:
        if (receivedOctet != calculatedChksum) {
            coloredMsg(LOG_RED, true, "mbc hre [%d] err: invalid checksum %02x vs %02x", 
                       mbusCommHandle.requestId, calculatedChksum, receivedOctet);
            mbusCommHandle.result = MBCR_ERROR_STATE_ENGINE__INVALID_CHKSUM;
            mbusCommHandle.state = MBCS_ERROR;
        } else {
            mbusCommHandle.frame.chksum = receivedOctet;
            mbusCommHandle.waitForOctet = true;
            mbusCommHandle.state = MBCS_STOP;
        }
        break;

        case MBCS_STOP:
        if (receivedOctet == 0x16) {
            mbusCommHandle.frame.stop = receivedOctet;
            mbusCommHandle.state = MBCS_DONE;
        } else {
            coloredMsg(LOG_RED, true, "mbc hre [%d] err: invalid stop symbol %02x", 
                       mbusCommHandle.requestId, receivedOctet);
            mbusCommHandle.result = MBCR_ERROR_STATE_ENGINE__STOP;
            mbusCommHandle.state = MBCS_ERROR;
        }
        break;

        case MBCS_DONE:
        // coloredMsg(LOG_YELLOW, false, "mbc hre [%d] state DONE", mbusCommHandle.requestId);
        parseAndPrintFrame();
        if (mbusCommHandle.frame.userdata != NULL) {
            free(mbusCommHandle.frame.userdata);
            mbusCommHandle.frame.userdata = NULL;
        }
        mbusCommHandle.result = MBCR_SUCCESS;
        mbusCommHandle.state = MBCS_DISABLE_FRONTEND;
        break;

        case MBCS_ERROR:
        coloredMsg(LOG_RED, false, "mbc hre [%d] state ERROR", mbusCommHandle.requestId);
        show(LED_RED, ON);
        mbusCommHandle.state = MBCS_ERROR_CONTINUED;
        // no break

        case MBCS_ERROR_CONTINUED:
        // stay here until timeout occurs
        break;

        case MBCS_TIMEOUT:
        coloredMsg(LOG_RED, false, "mbc hre [%d] state TIMEOUT", mbusCommHandle.requestId);
        mbusCommStats.mbusErrorCnt += 1;
        mbusCommHandle.device->failures += 1;
        mbusCommHandle.startTime = 0; // disable timeout
        mbusCommHandle.waitForOctet = false; // disable receiver
        printError();
        if (mbusCommHandle.frame.userdata != NULL) {
            free(mbusCommHandle.frame.userdata);
            mbusCommHandle.frame.userdata = NULL;
        }
        // no break

        case MBCS_DISABLE_FRONTEND:
        // coloredMsg(LOG_YELLOW, false, "mbc hre [%d] state DISABLE_FRONTEND", mbusCommHandle.requestId);
        frontendDisable();
        mbusCommHandle.startTime = 0; // disable timeout
        mbusCommHandle.state = MBCS_IDLE;
        break;
        

        default:
        mbusCommHandle.state = MBCS_IDLE;
        break;
    }
}

void mbusCommEnable(bool enable) {
    mbusCommEnabled = enable;
}

static e_mbusCommRequestResult mbusCommRequest(t_mbusDevice *mbusDevice) {
    e_mbusCommRequestResult res = MBCRR_BUSY;

    if (mbusCommEnabled) {
        if (mbusCommHandle.state == MBCS_IDLE) {
            mbusCommHandle.requestId += 1;
            mbusCommHandle.state = MBCS_SEND;
            mbusCommHandle.retryCnt = 0;
            mbusCommHandle.cmd = MBUS_QUERY_CMD;
            mbusCommHandle.addr = mbusDevice->address;
            mbusCommHandle.device = mbusDevice;
            mbusDevice->requests += 1;

            coloredMsg(LOG_YELLOW, true, "mbc mcr [%d] new request %s", 
                    mbusCommHandle.requestId,
                    mbusDevice->deviceName);

            oledPrint(OLED_SCREEN0, mbusDevice->deviceName);
            res = MBCRR_TRIGGERED;   

            mbusCommStats.mbusRequestCnt += 1;
        }
    } else {
        res = MBCRR_DISABLED;
    }

    return res;
}


#define PERIOD 10

static uint8_t numOfDevices = 8;
static t_mbusDevice devices[] = {
    {
        .deviceName = "TotalPower",
        .address = 80,
        .consideredField = { 0, 17, -1, -1 },
        .requests = 0,
        .failures = 0,
        .period = PERIOD,
        .delay = 0,
        .waiting = false
    },
    {
        .deviceName = "ComputerPower",
        .address = 85,
        .consideredField = { 0, 4, 2, 3 },
        .requests = 0,
        .failures = 0,
        .period = PERIOD,
        .delay = 0,
        .waiting = false
    },
    {
        .deviceName = "DryerPower",
        .address = 81,
        .consideredField = { 0, 4, 2, 3 },
        .requests = 0,
        .failures = 0,
        .period = PERIOD,
        .delay = 0,
        .waiting = false
    },
    {
        .deviceName = "LaundryPower",
        .address = 82,
        .consideredField = { 0, 4, 2, 3 },
        .requests = 0,
        .failures = 0,
        .period = PERIOD,
        .delay = 0,
        .waiting = false
    },
    {
        .deviceName = "DishwasherPower",
        .address = 83,
        .consideredField = { 0, 4, 2, 3 },
        .requests = 0,
        .failures = 0,
        .period = PERIOD,
        .delay = 0,
        .waiting = false
    },
    {
        .deviceName = "LightPower",
        .address = 84,
        .consideredField = { 0, 4, 2, 3 },
        .requests = 0,
        .failures = 0,
        .period = PERIOD,
        .delay = 0,
        .waiting = false
    },
    {
        .deviceName = "FreezerPower",
        .address = 86,
        .consideredField = { 0, 4, 2, 3 },
        .requests = 0,
        .failures = 0,
        .period = PERIOD,
        .delay = 0,
        .waiting = false
    },
    {
        .deviceName = "FridgePower",
        .address = 87,
        .consideredField = { 0, 4, 2, 3 },
        .requests = 0,
        .failures = 0,
        .period = PERIOD,
        .delay = 0,
        .waiting = false
    }
};


static void triggerMBusRequest(void *handle) {
    static uint8_t deviceIndex = 0;

    if (devices[deviceIndex].waiting) {
        e_mbusCommRequestResult r = mbusCommRequest(&(devices[deviceIndex]));
        if (r == MBCRR_TRIGGERED) {
            devices[deviceIndex].waiting = false;
            deviceIndex++;
        }
    } else {
        deviceIndex++;
    }
    if (deviceIndex >= numOfDevices) {
        deviceIndex = 0;
    }
}


static void mbusCommScheduler(void *handle) {
    static uint8_t state = 0;

    switch (state) {
        case 0:
            if (isNetworkAvailable()) {
                coloredMsg(LOG_YELLOW, true, "mbc mcs activate scheduler by network");
                schAdd(triggerMBusRequest, NULL, 0, 100);
                state = 2;
            }
            break;

        case 1:
            if (mbusCommEnabled) {
                coloredMsg(LOG_YELLOW, true, "mbc mcs activate scheduler by request");
                schAdd(triggerMBusRequest, NULL, 0, 100);
                state = 2;
            }
            break;

        case 2:
            if (! isNetworkAvailable()) {
                coloredMsg(LOG_YELLOW, true, "mbc mcs deactivate scheduler by network");
                schDel(triggerMBusRequest, NULL);
                loopDisable();
                state = 0;
            }
            if (! mbusCommEnabled) {
                coloredMsg(LOG_YELLOW, true, "mbc mcs deactivate scheduler by request");
                schDel(triggerMBusRequest, NULL);
                loopDisable();
                state = 1;
            }
            for (uint8_t i = 0; i < numOfDevices; i++) {
                devices[i].delay -= 1;
                if (devices[i].delay <= 0) {
                    devices[i].delay = devices[i].period;
                    devices[i].waiting = true;
                    coloredMsg(LOG_YELLOW, false, "mbc mcs scheduled: %s", devices[i].deviceName);
                }
            }

            // FIXME 
            // state = 3;
            break;
        
        case 3:
            coloredMsg(LOG_YELLOW, false, "mbc mcs waiting for godot");
            state = 4;
            // no break

        case 4:
            break;
    }
}

void mbusCommInit() {
    coloredMsg(LOG_GREEN, true, "mbc mci initializing Meterbus communication");

    // enable receive interrupts
    __HAL_UART_ENABLE_IT(&mbusUart, UART_IT_PE);
    __HAL_UART_ENABLE_IT(&mbusUart, UART_IT_ERR);
    __HAL_UART_ENABLE_IT(&mbusUart, UART_IT_RXNE);

    // init buffers
    mbusCommHandle.receiveBuffer.size = 256;
    mbusCommHandle.receiveBuffer.buffer = (uint8_t*) malloc(mbusCommHandle.receiveBuffer.size);
    mbusCommHandle.receiveBuffer.readIdx = 0;
    mbusCommHandle.receiveBuffer.writeIdx = 0;

    mbusCommHandle.sendBuffer.size = 8;
    mbusCommHandle.sendBuffer.buffer = (uint8_t*) malloc(mbusCommHandle.sendBuffer.size);
    mbusCommHandle.sendBuffer.readIdx = 0;
    mbusCommHandle.sendBuffer.writeIdx = 0;


    // FIXME
    schAdd(mbusCommScheduler, NULL, 0, 1000);
}