#include "lip/app/ptp/lip_ptp_cfg_defs.h"
#if LIP_PTP_ENABLE
#include "lip_ptp_pdelay_req.h"
#include "lip/app/ptp/misc/lip_ptp_tstmp_ops.h"
#include "lip/app/ptp/nbrate/lip_ptp_nb_rate_ratio.h"
#include <string.h>


static void f_send_pdelay_req(t_lip_ptp_port_ctx *p) {
    t_lip_ptp_pdelay_req_ctx *ctx = &p->md.eth.pdelay_req;
    t_lip_tx_frame *frame = lip_ptp_prepare_frame(
        p, LIP_PTP_MSGTYPE_PDELAY_REQ, sizeof(t_lip_ptp_msg_pdelay_req),
        lip_ptp_port_cmlds_mode_is_en(p) ? LIP_PTP_MSG_PREP_FLAG_CMLDS : 0);
    if (frame) {
        t_lip_ptp_msg_pdelay_req *ptp_msg = (t_lip_ptp_msg_pdelay_req *)frame->buf.cur_ptr;
        ++ctx->seq_id;
        ptp_msg->hdr.seqId = HTON16(ctx->seq_id);
        ptp_msg->hdr.logMsgInterval = ctx->log_interval;
        ptp_msg->hdr.corField = lip_ptp_port_cmlds_mode_is_en(p) ? -p->delay_assymetry : 0;
        lip_ptp_frame_flush(frame, sizeof(t_lip_ptp_msg_pdelay_req));
    }
}

static void f_proc_invalid_resp(t_lip_ptp_port_ctx *p) {
    t_lip_ptp_pdelay_req_ctx *ctx = &p->md.eth.pdelay_req;
    if (ctx->lost_resp < LIP_PTP_PDELAY_ALLOW_LOST_RESP) {
        ++ctx->lost_resp;
    } else {
        p->stats.flags &= ~(LIP_PTP_PORT_STATS_FLAG_NB_PDELAY_CAP | LIP_PTP_PORT_STATS_FLAG_LINK_DELAY_VALID);

    }
    ctx->state = LIP_PTP_PDELAY_REQ_STATE_WT_REQ_SEND;
}

/* выполнение текущего значения meanLinkDelay по полям одного обена pdelayReq/Resp/FollowUp в соответствии с IEEE 802.1AS-2020 eq 11-5 */
static t_lip_fns f_calc_cur_prop_time(const t_lip_ptp_tstmp *t1, const t_lip_ptp_tstmp *t4, uint64_t t32_delta, double rate) {
    t_lip_fns t41 = lip_ptp_tstmp_delta_fns(t4, t1);
    t_lip_fns t41_cor = (t_lip_fns)t41 * rate;
    t_lip_fns res = (t41_cor - t32_delta) >> 1;
    return res;
}

/* выполнение усреднения meanLinkDelay в соответствии с IEEE 802.1AS */
static void f_update_avg_prop_time(t_lip_ptp_port_ctx *p, t_lip_fns prop_time) {
    t_lip_ptp_pdelay_req_ctx *ctx = &p->md.eth.pdelay_req;
    if (ctx->mean_link_delay_avg_cnt <= LIP_PTP_PDELAY_TIME_AVG_CNT) {
        ++ctx->mean_link_delay_avg_cnt;
        p->stats.meanLinkDelay = ((ctx->mean_link_delay_avg_cnt - 1) * p->stats.meanLinkDelay + prop_time) / ctx->mean_link_delay_avg_cnt; /* IEEE 802.1AS-2020 eq 11-4 */
    } else {
        p->stats.meanLinkDelay = (t_lip_fns)(LIP_PTP_PDELAY_TIME_AVG_APLPHA * p->stats.meanLinkDelay + (1 - LIP_PTP_PDELAY_TIME_AVG_APLPHA) * prop_time); /* IEEE 802.1AS-2020 eq 11-2 */
    }
}

static bool f_pkt_tstmp_is_zero(t_lip_ptp_pkt_tstmp *t) {
    return (t->sec_l == 0) && (t->sec_h == 0) && (t->ns == 0);
}

static void f_calc_prop_time(t_lip_ptp_port_ctx *p, t_lip_fns t32) {
    t_lip_ptp_pdelay_req_ctx *ctx = &p->md.eth.pdelay_req;
    t_lip_fns d = f_calc_cur_prop_time(&ctx->t1, &ctx->t4, t32, p->stats.nbRateRatio);
    f_update_avg_prop_time(p, d);

    lip_printf(LIP_LOG_MODULE_PTP, LIP_LOG_LVL_DEBUG_MEDIUM, 0, "lip ptp: port %d udate pdelay: "\
               "avg="LIP_PTP_PRINT_FNS_FMT ", cur=" LIP_PTP_PRINT_FNS_FMT", t32=" LIP_PTP_PRINT_FNS_FMT ", rate=%.9f\n",
               LIP_PTP_PRINT_PORT_NUM(p), LIP_PTP_PRINT_FNS_VAL(p->stats.meanLinkDelay), LIP_PTP_PRINT_FNS_VAL(d),
               LIP_PTP_PRINT_FNS_VAL(t32),  p->stats.nbRateRatio);

    if (p->stats.meanLinkDelay > (LIP_PTP_PDELAY_TIME_THRESH_NS << LIP_PTP_FNS_SHIFT)) {
        lip_printf(LIP_LOG_MODULE_PTP, LIP_LOG_LVL_WARNING_HIGH, 0, "lip ptp: port %d pdelay thresh exceeded ("LIP_PTP_PRINT_FNS_FMT")!\n",
                   LIP_PTP_PRINT_PORT_NUM(p), LIP_PTP_PRINT_FNS_VAL(p->stats.meanLinkDelay));
        if (ctx->faults_cnt < LIP_PTP_PDELAY_ALLOW_FAULTS) {
            ++ctx->faults_cnt;
        } else {
            p->stats.flags &= ~LIP_PTP_PORT_STATS_FLAG_LINK_DELAY_VALID;
        }
    } else {
        p->stats.flags |= LIP_PTP_PORT_STATS_FLAG_LINK_DELAY_VALID;
    }

}



void lip_ptp_pdelay_req_init(t_lip_ptp_port_ctx *p) {
    t_lip_ptp_pdelay_req_ctx *ctx = &p->md.eth.pdelay_req;
    memset(ctx, 0, sizeof(t_lip_ptp_pdelay_req_ctx));
    ctx->log_interval = LIP_PTP_PDELAY_REQ_LOG_INTERVAL;
    lip_ptp_set_log_interval_tmr(&ctx->interval_tmr, ctx->log_interval, 1);
    ctx->state = LIP_PTP_PDELAY_REQ_STATE_WT_REQ_SEND;
    lip_ptp_port_cmlds_mode_set(p);
    p->stats.flags &= ~LIP_PTP_PORT_STATS_FLAG_LINK_DELAY_VALID;
}

void lip_ptp_pdelay_req_down(t_lip_ptp_port_ctx *p) {
    t_lip_ptp_pdelay_req_ctx *ctx = &p->md.eth.pdelay_req;
    ctx->state = LIP_PTP_PDELAY_REQ_STATE_INIT;
    p->stats.flags &= ~LIP_PTP_PORT_STATS_FLAG_LINK_DELAY_VALID;
}



void lip_ptp_pdelay_req_pull(t_lip_ptp_port_ctx *p) {
    t_lip_ptp_pdelay_req_ctx *ctx = &p->md.eth.pdelay_req;
    if (ltimer_expired(&ctx->interval_tmr)) {
        if (ctx->state != LIP_PTP_PDELAY_REQ_STATE_WT_REQ_SEND) {
            f_proc_invalid_resp(p);
        }
        f_send_pdelay_req(p);
        ctx->state = LIP_PTP_PDELAY_REQ_STATE_WT_REQ_TSTMP;
        ltimer_restart(&ctx->interval_tmr);
    }

}

void lip_ptp_pdelay_req_etstmp(t_lip_ptp_port_ctx *p, const t_lip_ptp_tstmp *tsmp) {
    t_lip_ptp_pdelay_req_ctx *ctx = &p->md.eth.pdelay_req;
    if (ctx->state == LIP_PTP_PDELAY_REQ_STATE_WT_REQ_TSTMP) {
        ctx->t1 = *tsmp;
        ctx->state = LIP_PTP_PDELAY_REQ_STATE_WT_RESP;
    }
}

void lip_ptp_pdelay_req_proc_resp(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_pdelay_resp)) {
        t_lip_ptp_pdelay_req_ctx *ctx = &p->md.eth.pdelay_req;
        t_lip_ptp_msg_pdelay_resp *presp = (t_lip_ptp_msg_pdelay_resp *)pl->data;
        uint16_t seq_id = NTOH16(presp->hdr.seqId);
        if ((ctx->state == LIP_PTP_PDELAY_REQ_STATE_WT_RESP)
            && (seq_id == ctx->seq_id)
            && lip_ptp_check_port_identity(p, &presp->reqPortIdentity)
            && frame->tstmp_valid
            ) {

            ctx->t4 = frame->tstmp;

            if (presp->hdr.flags0 & LIP_PTP_MSGFLAG0_TWO_STEP) {
                ctx->state = LIP_PTP_PDELAY_REQ_STATE_WT_FOLLOW_UP;
                memcpy(&ctx->cur_presp_srcid, &presp->hdr.srcPortIdentity, sizeof(t_lip_ptp_port_identity));
#if !LIP_PTP_PDELAY_REQ_2STEP_COR_DISABLE
                sm->t2_cor = NTOH64(presp->hdr.corField);
#endif
                f_pkt_tstmp_to_host(&presp->reqReceiptTstmp, &ctx->t2);
            } else {
                p->stats.flags |= LIP_PTP_PORT_STATS_FLAG_NB_PDELAY_CAP;

                t_lip_fns t32 = HTON64(presp->hdr.corField);
                p->nb_rate_ratio.src = LIP_PTP_RATE_RATIO_SRC_SYNC;
                p->stats.flags &= ~LIP_PTP_PORT_STATS_FLAG_CMLDS; /* cmlds всегда использует 2-step */
                f_calc_prop_time(p, t32);
            }
        } else {
            lip_printf(LIP_LOG_MODULE_PTP, LIP_LOG_LVL_DEBUG_HIGH, 0, "lip ptp: port %d invalid pdelay response %d, %d, %d, %d (total %d)!\n",
                       LIP_PTP_PRINT_PORT_NUM(p), ctx->state, seq_id == ctx->seq_id, lip_ptp_check_port_identity(p, &presp->reqPortIdentity), frame->tstmp_valid, ctx->lost_resp);
            f_proc_invalid_resp(p);
        }
    }
}

void lip_ptp_pdelay_req_proc_follow_up(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_pdelay_resp_follow_up)) {
        t_lip_ptp_pdelay_req_ctx *ctx = &p->md.eth.pdelay_req;
        t_lip_ptp_msg_pdelay_resp_follow_up *pfup = (t_lip_ptp_msg_pdelay_resp_follow_up *)pl->data;
        uint16_t seq_id = NTOH16(pfup->hdr.seqId);
        if ((ctx->state == LIP_PTP_PDELAY_REQ_STATE_WT_FOLLOW_UP)
            && (seq_id == ctx->seq_id)
            && lip_ptp_check_port_identity(p, &pfup->reqPortIdentity)
            && (memcmp(&ctx->cur_presp_srcid, &pfup->hdr.srcPortIdentity, sizeof(t_lip_ptp_port_identity))==0)) {

            ctx->lost_resp = 0;
            p->stats.flags |= LIP_PTP_PORT_STATS_FLAG_NB_PDELAY_CAP;

            t_lip_fns t32 = 0;

            if (f_pkt_tstmp_is_zero(&pfup->respOriginTstmp)) {
                t32 = HTON64(pfup->hdr.corField);
                /* если передается только разность меток, то rateRatio можем определить только по Sync */
                p->nb_rate_ratio.src = LIP_PTP_RATE_RATIO_SRC_SYNC;
                p->stats.flags &= ~LIP_PTP_PORT_STATS_FLAG_CMLDS; /* cmlds всегда использует обе отметки времени */
            } else {
                /* update rate-ratio */                
                t_ptp_nb_rate_ratio_tsmp rr_tstmp;
                f_pkt_tstmp_to_host(&pfup->respOriginTstmp, &rr_tstmp.t_rem);
                rr_tstmp.t_rem_cor = HTON64(pfup->hdr.corField);
                rr_tstmp.t_loc = ctx->t4;
                p->nb_rate_ratio.src = LIP_PTP_RATE_RATIO_SRC_PDELAY;
                lip_ptp_nb_rate_ratio_calc(p, &rr_tstmp);

                t32 = lip_ptp_tstmp_delta_fns(&rr_tstmp.t_rem, &ctx->t2);
                t32 += rr_tstmp.t_rem_cor;
#if !LIP_PTP_PDELAY_REQ_2STEP_COR_DISABLE
                if (f_cmlds_mode_is_en(sm)) {
                    t32 -= sm->t2_cor;
                } else {
                    t32 += sm->t2_cor;
                }
#endif
            }
            f_calc_prop_time(p, t32);
        }
    }
}
#endif
