#include "lip/app/opcua/lip_opcua_cfg_defs.h"
#if LIP_OPCUA_SERVER_SUBS_ENABLE
#include "lip_opcua_server_monitem_queue.h"
#include "lip/app/opcua/types/lip_opcua_statuscode.h"

#if  LIP_OPCUA_SERVER_MONITEMS_QUEUE_ENABLE

    static inline uint16_t f_queue_next_pos(t_lip_opcua_monitem_notification_ctx *ctx, uint16_t pos) {
        uint16_t new_pos = pos + 1;
        return new_pos == ctx->size ? 0 : new_pos;
    }

    static inline uint16_t f_queue_prev_pos(t_lip_opcua_monitem_notification_ctx *ctx, uint16_t pos) {
        return pos == 0 ? ctx->size - 1  : pos - 1;
    }

    LIP_OPCUA_SERVER_MONITEMS_QUEUE_MEM(static t_lip_opcua_notification_info f_queue_notifications[LIP_OPCUA_SERVER_MONITEMS_QUEUE_MAX_SIZE]);
    LIP_OPCUA_SERVER_MONITEMS_QUEUE_MEM(static uint8_t f_queue_notifications_busy[LIP_OPCUA_SERVER_MONITEMS_QUEUE_MAX_SIZE]);

    static t_lip_opcua_notification_info *f_queue_alloc(t_lip_opcua_counter *preq_size) {
        t_lip_opcua_notification_info *ret = NULL;

        /* сюда сохраняется информация о найденном свободном промежутке макс. размера */
        int res_qidx = -1;
        uint16_t res_qsize = 0;

        /* сюда сохраняется информация по анализируемому в текущий момент свободному промежутку */
        int cur_qidx = -1;
        uint16_t cur_qsize = 0;

        uint16_t req_size = *preq_size > LIP_OPCUA_SERVER_MONITEMS_QUEUE_MAX_SIZE ? LIP_OPCUA_SERVER_MONITEMS_QUEUE_MAX_SIZE : *preq_size;

        /* ищем req_size подряд свободных элементов для их резирвирования в очереди или максимальное число
         * подряд идущих элементов меньше запрошенного размера, если не найдется req_size */
        for (int i = 0; (i < LIP_OPCUA_SERVER_MONITEMS_QUEUE_MAX_SIZE) && (res_qsize < req_size); ++i) {
            if (!f_queue_notifications_busy[i]) {
                if (cur_qsize == 0) {
                    cur_qidx = i;
                }
                ++cur_qsize;

                if (cur_qsize > res_qsize) {
                    res_qidx = cur_qidx;
                    res_qsize = cur_qsize;
                }
            } else {
                cur_qidx = -1;
                cur_qsize = 0;
            }
        }

        if (res_qsize > 1) {
            ret = &f_queue_notifications[res_qidx];
            *preq_size = res_qsize;
            uint8_t *pbusy = &f_queue_notifications_busy[res_qidx];
            for (int i = 0; i < res_qsize; ++i) {
                *pbusy++ = 1;
            }
            lip_printf(LIP_LOG_MODULE_OPCUA, LIP_LOG_LVL_DEBUG_MEDIUM, 0, "lip opcua: monitem queue allocate: pos %d, size %d!\n", res_qidx, res_qsize);
        }
        return ret;
    }

    static void f_queue_free(t_lip_opcua_notification_info *qinfo, uint16_t size) {
        int pos = qinfo - &f_queue_notifications[0];
        if ((pos >= 0) && ((pos + size) <= LIP_OPCUA_SERVER_MONITEMS_QUEUE_MAX_SIZE)) {
            uint8_t *pbusy = &f_queue_notifications_busy[pos];
            for (int i = 0; i < size; ++i) {
                *pbusy++ = 0;
            }
            lip_printf(LIP_LOG_MODULE_OPCUA, LIP_LOG_LVL_DEBUG_MEDIUM, 0, "lip opcua: monitem queue free: pos %d, size %d!\n", pos, size);
        }
    }

    static uint32_t f_ov_status_flags(uint32_t status) {
        /* При отбрасывании значения необходимо сохранить его флаги Structure/Semantic Changed для переноса в следующее значение, а также
             * для этого следующего значения установить признак Overflow с указанием InfoType == DataValue. Т.к. значения Structure/Semantic Changed
             * будут заменены статусом новых занчений, то в finish они уже будут недоступны, поэтому сохраняем их тут, а используем уже в finish */
        return (status & (LIP_OPCUA_STATUSCODE_STRUCTURE_CHANGED | LIP_OPCUA_STATUSCODE_SEMANTICS_CHANGED))
                     | LBITFIELD_SET(LIP_OPCUA_STATUSCODE_INFO_TYPE, LIP_OPCUA_STATUSCODE_INFO_TYPE_DATAVAL) | LIP_OPCUA_STATUSCODE_DATAVAL_OVERFLOW;
    }

#endif

void lip_opcua_server_monitem_queue_clear(t_lip_opcua_monitem_notification_ctx *ctx) {
    ctx->rdy_cnt = 0;
    ctx->pub_rdy = 0;
}

void lip_opcua_server_monitem_queues_init(void) {
#if  LIP_OPCUA_SERVER_MONITEMS_QUEUE_ENABLE
    memset(f_queue_notifications_busy, 0, sizeof(f_queue_notifications_busy));
#endif
}

void lip_opcua_server_monitem_queue_init(t_lip_opcua_monitem_notification_ctx *ctx, t_lip_opcua_counter req_size, bool discart_oldest) {
#if  LIP_OPCUA_SERVER_MONITEMS_QUEUE_ENABLE
    t_lip_opcua_notification_info *qalloc = NULL;
    if (req_size > 1) {
        qalloc = f_queue_alloc(&req_size);
        if (qalloc) {
            ctx->size = req_size;
            ctx->queue.get_pos = 0;
            ctx->queue.ov_status = 0;
            ctx->queue.set_next_ov = 0;
            ctx->queue.discart_oldest = discart_oldest;
            ctx->queue.notifications = qalloc;
        }
    }
    if (qalloc == NULL) {
#endif
        ctx->size = 1;
#if  LIP_OPCUA_SERVER_MONITEMS_QUEUE_ENABLE
    }
#endif
    ctx->pub_rdy = ctx->rdy_cnt = 0;
}

void lip_opcua_server_monitem_queue_update_size(t_lip_opcua_monitem_notification_ctx *ctx, t_lip_opcua_counter req_size, bool discart_oldest) {
#if  LIP_OPCUA_SERVER_MONITEMS_QUEUE_ENABLE
    if (req_size > LIP_OPCUA_SERVER_MONITEMS_QUEUE_MAX_SIZE)
        req_size = LIP_OPCUA_SERVER_MONITEMS_QUEUE_MAX_SIZE;
    if (req_size != ctx->size) {
        /* если размер изменился, то пересоздаем очередь. все данные будет очищены */
        lip_opcua_server_monitem_queue_delete(ctx);
        lip_opcua_server_monitem_queue_init(ctx, req_size, discart_oldest);
    } else {
        /* если изменился только параметр discart_oldest, то можем просто обновить
         * значение и оно будет применяться автоматом к новым добавляемым элементам */
        ctx->queue.discart_oldest = discart_oldest;
    }
#endif
}

void lip_opcua_server_monitem_queue_delete(t_lip_opcua_monitem_notification_ctx *ctx) {
#if  LIP_OPCUA_SERVER_MONITEMS_QUEUE_ENABLE
    if (ctx->size > 1) {
        f_queue_free(ctx->queue.notifications, ctx->size);
    }
#endif
    ctx->size = ctx->rdy_cnt = 0;
}



void lip_opcua_server_monitem_queue_drop_oldest(t_lip_opcua_monitem_notification_ctx *ctx) {
    if (ctx->rdy_cnt > 0) {
#if  LIP_OPCUA_SERVER_MONITEMS_QUEUE_ENABLE
        if (ctx->size > 1) {
            ctx->queue.ov_status = f_ov_status_flags(ctx->queue.notifications[ctx->queue.get_pos].data.status);
            --ctx->rdy_cnt;
            if (ctx->pub_rdy)
                --ctx->pub_rdy;

            if (ctx->rdy_cnt == 0) {
                /* если очитстили всю очередь, то некому поставить ov flag, сохраняем признак,
                 * чтобы поставить при следущем добавлении элемента */
                ctx->queue.set_next_ov = true;
            } else {
                ctx->queue.get_pos = f_queue_next_pos(ctx, ctx->queue.get_pos);
                ctx->queue.notifications[ctx->queue.get_pos].data.status |= ctx->queue.ov_status;
            }
        } else {
#endif
            ctx->rdy_cnt = 0;
            ctx->pub_rdy = 0;
#if  LIP_OPCUA_SERVER_MONITEMS_QUEUE_ENABLE
        }
#endif
    }
}

t_lip_opcua_notification_info *lip_opcua_server_monitem_put_start(t_lip_opcua_monitem_notification_ctx *ctx) {
    t_lip_opcua_notification_info *ret = NULL;

#if  LIP_OPCUA_SERVER_MONITEMS_QUEUE_ENABLE
    if (ctx->size > 1) {
        uint16_t put_pos;
        /* случай заполненной очереди рассматриваем отдельно, т.к. необходимо
         * в нем заменять самый старый или последний элемент и менять статус */
        if (ctx->rdy_cnt == ctx->size) {
            if (ctx->queue.discart_oldest) {
                /* для discart_odest = true отбрасываем самое старое значение, т.е. то, которые было бы
                 * прочитано следующим и на которое указывавет get_pos */
                put_pos = ctx->queue.get_pos;
            } else {
                /* для discart_odest = false отпрасываем последнее записанное значение,т.е.
                 * на позиции до get_pos */
                put_pos = f_queue_prev_pos(ctx, ctx->queue.get_pos);
            }

            /* При отбрасывании значения необходимо сохранить его флаги Structure/Semantic Changed для переноса в следующее значение, а также
             * для этого следующего значения установить признак Overflow с указанием InfoType == DataValue. Т.к. значения Structure/Semantic Changed
             * будут заменены статусом новых занчений, то в finish они уже будут недоступны, поэтому сохраняем их тут, а используем уже в finish */
            ctx->queue.ov_status = f_ov_status_flags(ctx->queue.notifications[put_pos].data.status);
        } else {
            put_pos = ctx->queue.get_pos + ctx->rdy_cnt;
            if (put_pos >= ctx->size)
                put_pos -= ctx->size;
        }
        ret = &ctx->queue.notifications[put_pos];
    } else {
#endif
        ret = &ctx->local.notification;
#if  LIP_OPCUA_SERVER_MONITEMS_QUEUE_ENABLE
    }
#endif
    return ret;
}

void lip_opcua_server_monitem_put_finish(t_lip_opcua_monitem_notification_ctx *ctx) {
#if  LIP_OPCUA_SERVER_MONITEMS_QUEUE_ENABLE
    if (ctx->size > 1) {
        if (ctx->rdy_cnt == ctx->size) {
            if (ctx->queue.discart_oldest) {
                /* если удалили самый старый элемент, то нужно также сдвинуть указетель
                 * получения данных на следующий */
                ctx->queue.get_pos = f_queue_next_pos(ctx, ctx->queue.get_pos);
                /* устанавливаем overflow для следующего элемента после отброшенного. В данном случае
                 * этот как раз тот, что в начале очереди и на которого теперь указывает get_pos */
                ctx->queue.notifications[ctx->queue.get_pos].data.status |= ctx->queue.ov_status;
            } else {
                /* для discart_odest = false обновляем состояние только что записанного занчения, которое
                 * в конце очереди - на позицию раньше get_pos */
                uint16_t put_pos = f_queue_prev_pos(ctx, ctx->queue.get_pos);
                ctx->queue.notifications[put_pos].data.status |= ctx->queue.ov_status;
            }

            if (ctx->pub_rdy > 0) {
                if ((ctx->queue.discart_oldest) || (ctx->pub_rdy == ctx->size)) {
                    --ctx->pub_rdy;
                }
            }
        } else {
            /* если был overflow и очищена вся очередь, устанавливаем статус для первого элемента */
            if (ctx->queue.set_next_ov) {
                uint16_t put_pos = ctx->queue.get_pos + ctx->rdy_cnt;
                if (put_pos >= ctx->size)
                    put_pos -= ctx->size;
                ctx->queue.notifications[put_pos].data.status |= ctx->queue.ov_status;
                ctx->queue.set_next_ov = false;
            }
            ++ctx->rdy_cnt;
        }
    } else {
#endif
        if (ctx->rdy_cnt == 0) {
            ++ctx->rdy_cnt;
        }
        ctx->pub_rdy = 0;
#if  LIP_OPCUA_SERVER_MONITEMS_QUEUE_ENABLE
    }
#endif
}

void lip_opcua_server_monitem_queue_get_finish(t_lip_opcua_monitem_notification_ctx *ctx) {
#if  LIP_OPCUA_SERVER_MONITEMS_QUEUE_ENABLE
    if (ctx->size > 1) {
        ctx->queue.get_pos = f_queue_next_pos(ctx, ctx->queue.get_pos);
    }
#endif
    if (ctx->rdy_cnt > 0) {
        --ctx->rdy_cnt;
        if (ctx->pub_rdy > 0) {
            --ctx->pub_rdy;
        }
    }
}

t_lip_opcua_notification_info *lip_opcua_server_monitem_queue_get_start(t_lip_opcua_monitem_notification_ctx *ctx) {
    t_lip_opcua_notification_info *ret = NULL;
    if (ctx->rdy_cnt > 0) {
#if  LIP_OPCUA_SERVER_MONITEMS_QUEUE_ENABLE
        if (ctx->size > 1) {
            ret = &ctx->queue.notifications[ctx->queue.get_pos];
        } else {
#endif
            ret = &ctx->local.notification;
#if  LIP_OPCUA_SERVER_MONITEMS_QUEUE_ENABLE
        }
#endif
    }
    return ret;
}

#endif
