#include "lip/app/ptp/lip_ptp_cfg_defs.h"
#if LIP_PTP_ENABLE
#include "lip_ptp_port_announce.h"
#include "lip_ptp_bmca_state_sel.h"
#include "lip/app/ptp/lip_ptp_proto_defs.h"
#include "lip/app/ptp/lip_ptp_internal.h"
#include <string.h>

/* recordOtherAnnounceInfo - 10.3.12.2.1 IEEE 802.1AS-2020 */
static void f_save_ann_time_info(t_lip_ptp_port_announce_ctx *ctx, t_lip_ptp_msg_announce *ann) {
    ctx->ann_time_info.flags1 = ann->hdr.flags1;
    ctx->ann_time_info.timeSource = ann->timeSource;
    ctx->ann_time_info.currentUtcOffset = NTOH16(ann->currentUtcOffset);
}

/* Контекст для разбора TLV от announce */
typedef struct {
    t_lip_ptp_inst_ctx *inst; /* Указатель на контекст состояния узла PTP */
    bool has_path_trace; /* признак, найдено ли TLV PathTrace */
    uint8_t *put_pos; /* указатель на текущую позицию буфера для сохранения TLV для передачи дальше */
    uint8_t *put_end; /* указатель на конец буфера для сохрениня TLV для передачи дальше */
} t_ann_tlv_save_ctx;


/* Обработка TLV с сохранением всех, которые должны передавать дальше.
 * Для PathTrace добавляем свой ClockID, если хватеает места */
static void f_ann_tlv_parse_cb(void *ctx, uint16_t tlv_type, const uint8_t *data, uint16_t len) {
    t_ann_tlv_save_ctx *ann_ctx = (t_ann_tlv_save_ctx *)ctx;
    if (tlv_type == LIP_PTP_TLV_CODE_PATH_TRACE) {
        uint16_t path_trace_size = len + LIP_PTP_CLOCK_IDENTITY_SIZE;
        ann_ctx->has_path_trace = true;


        if ((ann_ctx->put_pos + path_trace_size + sizeof(t_lip_ptp_tlv_hdr)) <= ann_ctx->put_end) {
            ann_ctx->put_pos = f_set_tlv_hdr(ann_ctx->put_pos, LIP_PTP_TLV_CODE_PATH_TRACE, path_trace_size);
            memcpy(ann_ctx->put_pos, data, len);
            ann_ctx->put_pos += len;
            memcpy(ann_ctx->put_pos, ann_ctx->inst->sys_id.clockIdentity, LIP_PTP_CLOCK_IDENTITY_SIZE);
            ann_ctx->put_pos += LIP_PTP_CLOCK_IDENTITY_SIZE;
        }
    } else if ((tlv_type == LIP_PTP_TLV_CODE_ALT_TIME_OFFS_IND)
               || (tlv_type == LIP_PTP_TLV_CODE_ORG_EXT_PROP)
               || (tlv_type == LIP_PTP_TLV_CODE_ENH_ACCUR_METRICS)
               || ((tlv_type >= 0x4002) && (tlv_type <= 0x7EFF))
               || ((tlv_type >= 0x7FF0) && (tlv_type <= 0x7FFF))) {
        /* Часть опций (table 10-6 IEEE 802.1AS-2020) должны передать дальше без изменений.
                   Сохраняем их вместе с PathTrace в общий буфер без изменений. */
        if ((ann_ctx->put_pos + len + sizeof(t_lip_ptp_tlv_hdr)) <= ann_ctx->put_end) {
            ann_ctx->put_pos = f_set_tlv_hdr(ann_ctx->put_pos, tlv_type, len);
            memcpy(ann_ctx->put_pos, data, len);
            ann_ctx->put_pos += len;
        }
    }
}

/* Контекст для проверки Announce для TLV */
typedef struct {
    t_lip_ptp_inst_ctx *inst;  /* Указатель на контекст состояния узла PTP */
    bool has_path_trace;  /* признак, найдено ли TLV PathTrace */
    bool has_self; /* признак, нашли ли свой ClockID в PathTrace */
} t_ann_tlv_check_ctx;

/* обработка TLV для проверки корректности Announce */
static void f_ann_tlv_check_cb(void *ctx, uint16_t tlv_type, const uint8_t *data, uint16_t len) {
    if (tlv_type == LIP_PTP_TLV_CODE_PATH_TRACE) {
        t_ann_tlv_check_ctx *check_ctx = (t_ann_tlv_check_ctx *)ctx;
        check_ctx->has_path_trace = true;
        const uint8_t *end_ptr = data + len;
        while ((data + LIP_PTP_CLOCK_IDENTITY_SIZE) <= end_ptr) {
            if (memcmp(data, &check_ctx->inst->sys_id.clockIdentity, LIP_PTP_CLOCK_IDENTITY_SIZE) == 0) {
                check_ctx->has_self = true;
            }
            data += LIP_PTP_CLOCK_IDENTITY_SIZE;
        }
    }
}


/* Сохраняем опции (TLV) принятого Announce, которые должны передать дальше.
   Для PathTrace добавляем в конце свой clockId, если есть место (если нет,
   отбрасываем полностью).
   Используется один буфер на все порты для экономии места, поэтому должна
   вызываться только потенциальным slave-портом, т.е. при принятом векторе
   не хуже текущего.
 */
static void f_save_ann_opts(t_lip_ptp_port_ctx *p, t_lip_payload *pl) {
    t_lip_ptp_msg_announce *ann = (t_lip_ptp_msg_announce*)pl->data;
    t_ann_tlv_save_ctx ann_ctx;
    ann_ctx.inst = f_ptp_port_inst_ctx(p);
    ann_ctx.put_pos = ann_ctx.inst->bmca.ann_rx_opts;
    ann_ctx.put_end = ann_ctx.put_pos + LIP_PTP_ANNOUNCE_OPT_SIZE_MAX;
    ann_ctx.has_path_trace = false;
    lip_ptp_parse_tlv(&pl->data[sizeof(t_lip_ptp_msg_announce)], &pl->data[pl->size], &ann_ctx, f_ann_tlv_parse_cb, NULL);

    /* Если напрямую подключены к GM, но GM не поддерживает PathTrace, то можем сконструировать
     * его сами для передачи дальше, он будет состоять из ClockID от GM и нашего.
     * Прямое подключение определяем по совпадению clockID у передающего порта и GM */
    if (!ann_ctx.has_path_trace && lip_ptp_clock_id_is_equal(&ann->grandmasterIdentity, &ann->hdr.srcPortIdentity.clockIdentity)) {
        ann_ctx.put_pos = f_set_tlv_hdr(ann_ctx.put_pos, LIP_PTP_TLV_CODE_PATH_TRACE, 2*LIP_PTP_CLOCK_IDENTITY_SIZE);
        memcpy(ann_ctx.put_pos, ann->grandmasterIdentity, LIP_PTP_CLOCK_IDENTITY_SIZE);
        ann_ctx.put_pos += LIP_PTP_CLOCK_IDENTITY_SIZE;
        memcpy(ann_ctx.put_pos, ann_ctx.inst->sys_id.clockIdentity, LIP_PTP_CLOCK_IDENTITY_SIZE);
        ann_ctx.put_pos += LIP_PTP_CLOCK_IDENTITY_SIZE;
    }

    ann_ctx.inst->bmca.ann_rx_opts_size = ann_ctx.put_pos - ann_ctx.inst->bmca.ann_rx_opts;
    ann_ctx.inst->bmca.ann_rx_opts_port = p;

}

/* Передача Announce по порту (порт должен быть в состоянии master) */
static void f_tx_announce(t_lip_ptp_port_ctx *p) {
    t_lip_ptp_inst_ctx *inst = f_ptp_port_inst_ctx(p);
    t_lip_ptp_port_announce_ctx *ctx = &p->announce;
    const uint16_t ann_opt_size = inst->bmca.ann_tx_opts_size_ptr ? *inst->bmca.ann_tx_opts_size_ptr : 0;
    const uint16_t ann_size = sizeof(t_lip_ptp_msg_announce) + ann_opt_size;

    t_lip_tx_frame *frame = lip_ptp_prepare_frame(
        p, LIP_PTP_MSGTYPE_ANNOUNCE, ann_size, 0);
    if (frame) {
        t_lip_ptp_msg_announce *msg = (t_lip_ptp_msg_announce*)frame->buf.cur_ptr;
        memcpy(&msg->grandmasterPriority1, &ctx->port_prio.rootSystemIdentity, LIP_PTP_SYSTEM_IDENTITY_SIZE);
        msg->stepsRemoved = inst->bmca.maseter_steps_removed_be;
        if (inst->gm_stats.time_info) {
            msg->hdr.flags1 = inst->gm_stats.time_info->flags1;
            msg->currentUtcOffset = HTON16(inst->gm_stats.time_info->currentUtcOffset);
            msg->timeSource = inst->gm_stats.time_info->timeSource;
        }
        ++ctx->seq_id;
        msg->hdr.seqId = HTON16(ctx->seq_id);
        msg->hdr.logMsgInterval = ctx->tx_log_interval;
        if (ann_opt_size != 0) {
            uint8_t *opt = frame->buf.cur_ptr + sizeof(t_lip_ptp_msg_announce);
            memcpy(opt, inst->bmca.ann_tx_opts_ptr, ann_opt_size);
        }
        lip_ptp_frame_flush(frame, ann_size);
    }
}


void lip_ptp_port_announce_init(t_lip_ptp_port_ctx *p) {
    t_lip_ptp_port_announce_ctx *ctx = &p->announce;
    memset(ctx, 0, sizeof(t_lip_ptp_port_announce_ctx));
    ctx->info_state = LIP_PTP_PORT_ANNOUNCE_INFO_STATE_AGED;
    ctx->tx_log_interval = LIP_PTP_ANNOUNCE_LOG_INTERVAL;
    lip_ptp_set_log_interval_tmr(&ctx->tx_interval_tmr, ctx->tx_log_interval, 1);
    lip_ptp_bmca_state_sel_update_req(f_ptp_port_inst_ctx(p));
}

void lip_ptp_port_announce_down(t_lip_ptp_port_ctx *p) {
    t_lip_ptp_port_announce_ctx *ctx = &p->announce;
    ctx->info_state = LIP_PTP_PORT_ANNOUNCE_INFO_STATE_AGED;
}



void lip_ptp_port_proc_announce(t_lip_ptp_port_ctx *p, t_lip_payload *pl, const t_lip_ptp_rx_frame *frame) {
    if (pl->size >= sizeof(t_lip_ptp_msg_announce)) {
        t_lip_ptp_port_announce_ctx *ctx = &p->announce;
        t_lip_ptp_msg_announce *msg = (t_lip_ptp_msg_announce *)pl->data;
        /* Qualifies Announce - 10.3.11.2.1 IEEE 802.1AS-2020 */
        uint16_t stepsRemoved = NTOH16(msg->stepsRemoved);
        if (!lip_ptp_check_clock_identity(&msg->hdr.srcPortIdentity.clockIdentity) /* a) - проверка что не собственное сообщение */
            && (stepsRemoved <= LIP_PTP_ANNOUNCE_STEPS_MAX) /* b) - проверка stepsRemoved */
            ) {
            /** c) проход по TLV, поиск PathTrace, проверка что нет своего clockID */
            t_ann_tlv_check_ctx check_ctx;
            check_ctx.inst = f_ptp_port_inst_ctx(p);
            check_ctx.has_path_trace = false;
            check_ctx.has_self = false;
            lip_ptp_parse_tlv(&pl->data[sizeof(t_lip_ptp_msg_announce)], &pl->data[pl->size], &check_ctx, f_ann_tlv_check_cb, NULL);

            if (!check_ctx.has_self) {
                /* rcvInfo - 10.3.12.2.1 IEEE 802.1AS-2020 */
                /* convey master info? */
                t_lip_ptp_bmca_prio_vector msg_prio;
                memcpy(&msg_prio.rootSystemIdentity, &msg->grandmasterPriority1, LIP_PTP_SYSTEM_IDENTITY_SIZE);
                msg_prio.stepsRemoved = msg->stepsRemoved;
                memcpy(&msg_prio.sourcePortIdentity, &msg->hdr.srcPortIdentity, sizeof(t_lip_ptp_port_identity));
                msg_prio.portNum = p->pkt_port_num_be;
                int cmp_res = f_cmp_prio(&msg_prio, &ctx->port_prio);
                if (cmp_res == 0) {
                    /* Repeated - Сравнивается перед Superior, т.к. условие проверки
                     * Repeated выполняется и для Superior, но действия другие */
                    ltimer_restart(&ctx->rcv_ann_tout_tmr);
                    f_save_ann_time_info(ctx, msg);
                    f_save_ann_opts(p, pl);
                } else if ((cmp_res < 0) ||
                           (memcmp(&msg_prio.sourcePortIdentity, &ctx->port_prio.sourcePortIdentity, sizeof(t_lip_ptp_port_identity)) == 0)) {
                    /* Superior */
                    lip_printf(LIP_LOG_MODULE_PTP, LIP_LOG_LVL_PROGRESS, 0, "lip ptp: port %d recv superior announce (steps %d), time %d!\n",
                               LIP_PTP_PRINT_PORT_NUM(p), NTOH16(msg_prio.stepsRemoved), lclock_get_ticks());

                    lip_printf(LIP_LOG_MODULE_PTP, LIP_LOG_LVL_DEBUG_LOW, 0,
                               "lip ptp: port prio %d,%d,%d,%d,%d - %d,%d,%d,%d,%d,%d - %d\n",
                               ctx->port_prio.rootSystemIdentity.priority1,
                               ctx->port_prio.rootSystemIdentity.clockClass,
                               ctx->port_prio.rootSystemIdentity.clockAccuracy,
                               ctx->port_prio.rootSystemIdentity.offsetScaledLogVariance,
                               ctx->port_prio.rootSystemIdentity.priority2,
                               ctx->port_prio.rootSystemIdentity.clockIdentity[0],
                               ctx->port_prio.rootSystemIdentity.clockIdentity[1],
                               ctx->port_prio.rootSystemIdentity.clockIdentity[2],
                               ctx->port_prio.rootSystemIdentity.clockIdentity[3],
                               ctx->port_prio.rootSystemIdentity.clockIdentity[4],
                               ctx->port_prio.rootSystemIdentity.clockIdentity[5],
                               NTOH16(ctx->port_prio.stepsRemoved));
                    lip_printf(LIP_LOG_MODULE_PTP, LIP_LOG_LVL_DEBUG_LOW, 0,
                               "lip ptp: msg prio  %d,%d,%d,%d,%d - %d,%d,%d,%d,%d,%d - %d\n",
                               msg_prio.rootSystemIdentity.priority1,
                               msg_prio.rootSystemIdentity.clockClass,
                               msg_prio.rootSystemIdentity.clockAccuracy,
                               msg_prio.rootSystemIdentity.offsetScaledLogVariance,
                               msg_prio.rootSystemIdentity.priority2,
                               msg_prio.rootSystemIdentity.clockIdentity[0],
                               msg_prio.rootSystemIdentity.clockIdentity[1],
                               msg_prio.rootSystemIdentity.clockIdentity[2],
                               msg_prio.rootSystemIdentity.clockIdentity[3],
                               msg_prio.rootSystemIdentity.clockIdentity[4],
                               msg_prio.rootSystemIdentity.clockIdentity[5],
                               NTOH16(msg_prio.stepsRemoved));
                    f_cpy_prio(&ctx->port_prio, &msg_prio);
                    f_save_ann_time_info(ctx, msg);
                    f_save_ann_opts(p, pl);
                    lip_ptp_set_log_interval_tmr(&ctx->rcv_ann_tout_tmr, msg->hdr.logMsgInterval, LIP_PTP_ANNOUNCE_RECV_INTERVALS_TOUT);
                    ctx->info_state = LIP_PTP_PORT_ANNOUNCE_INFO_STATE_RECEIVED;
                    lip_ptp_bmca_state_sel_update_req(check_ctx.inst);
                }
            }
        }

    }
}

void lip_ptp_port_announce_prio_update(t_lip_ptp_port_ctx *p, const t_lip_ptp_bmca_prio_vector *master_prio) {
    t_lip_ptp_port_announce_ctx *ctx = &p->announce;
    f_cpy_prio(&ctx->port_prio, master_prio);
    ctx->info_state = LIP_PTP_PORT_ANNOUNCE_INFO_STATE_MINE;
    ctx->tx_new_info = true;
}

void lip_ptp_port_announce_pull(t_lip_ptp_port_ctx *p) {
    t_lip_ptp_port_announce_ctx *ctx = &p->announce;
    if (ctx->info_state == LIP_PTP_PORT_ANNOUNCE_INFO_STATE_RECEIVED) {
        /* announce timeout */
        if (ltimer_expired(&ctx->rcv_ann_tout_tmr)) {
            lip_printf(LIP_LOG_MODULE_PTP, LIP_LOG_LVL_PROGRESS, 0, "lip ptp: port %d announce tout (time %d)!\n", LIP_PTP_PRINT_PORT_NUM(p), lclock_get_ticks());
            ctx->info_state = LIP_PTP_PORT_ANNOUNCE_INFO_STATE_AGED;
            lip_ptp_bmca_state_sel_update_req(f_ptp_port_inst_ctx(p));
        }
    }

    if (p->stats.state == LIP_PTP_PORT_STATE_MASTER) {
        t_lip_ptp_inst_ctx *inst = f_ptp_port_inst_ctx(p);
        /* Если bmca на стадии обновления, то не посылаем информацию до обновления */
        if (!inst->bmca.update_req) {
            /** @note slowdown на текущий момент не реализован, т.к. используется
             *  фиксированный интервал */
            if (ltimer_expired(&ctx->tx_interval_tmr) || ctx->tx_new_info) {
                ctx->tx_new_info = false;
                f_tx_announce(p);
                ltimer_restart(&ctx->tx_interval_tmr);
            }
        }

    }
}


#endif




