#include "lip/app/ptp/lip_ptp_cfg_defs.h"
#if LIP_PTP_ENABLE
#include "lip_ptp_local_clk_sync.h"
#include "lip/app/ptp/lip_ptp_internal.h"
#include "lip/app/ptp/misc/lip_ptp_tstmp_ops.h"

void lip_ptp_local_clk_sync_init(t_lip_ptp_inst_ctx *inst) {
    t_lip_ptp_local_clk_ctx *loc_clk = &inst->local_clk;
    memset(loc_clk, 0, sizeof(*loc_clk));
}

void lip_ptp_local_clk_sync_reset(t_lip_ptp_inst_ctx *inst) {

}

void lip_ptp_local_clk_sync_update(t_lip_ptp_inst_ctx *inst, const t_lip_ptp_tstmp *loc, const t_lip_ptp_tstmp *rem) {
    t_lip_ptp_local_clk_ctx *clk_ctx = &inst->local_clk;
    /* тут был код проверки ошибки перерасчета времени по текущему sync и по прошлому.
     * возможно в будущем можно сделать какое-то усреднение этих ошибок и доп. коррекцию */
#if 0
    if (clk_ctx->clk_offs_valid) {
        t_lip_ptp_tstmp cor_loc;
        /* сперва корректируем локальный клок компенсирая начальную разницу */
        if (clk_ctx->clk_offs_sign < 0) {
            lip_ptp_tstmp_add(&cor_loc, loc, &clk_ctx->local_clk_offs);
        } else {
            lip_ptp_tstmp_sub(&cor_loc, loc, &clk_ctx->local_clk_offs);
        }
        t_lip_fns int_loc = lip_ptp_tstmp_delta_fns(loc, &clk_ctx->syn_rx_local);
        t_lip_fns int_log_mul = int_loc * (inst->gm_stats.rateRatio - 1.);
        lip_ptp_tstmp_add_fns(&cor_loc, int_log_mul);


        /* затем рассчитываем отклонения от этой изначальной разницы в fnd */
        t_lip_fns cor = lip_ptp_tstmp_delta_fns(rem, &cor_loc);

        lip_printf(LIP_LOG_MODULE_PTP, LIP_LOG_LVL_DEBUG_MEDIUM, 0,
                   "lip ptp: local clk offs delta = " LIP_PTP_PRINT_FNS_FMT ", interval " LIP_PTP_PRINT_FNS_FMT ", int mul " LIP_PTP_PRINT_FNS_FMT ", rate %.9f\n",
                   LIP_PTP_PRINT_FNS_VAL(cor), LIP_PTP_PRINT_FNS_VAL(int_loc), LIP_PTP_PRINT_FNS_VAL(int_log_mul), inst->gm_stats.rateRatio);
        clk_ctx->clk_offs_sign = lip_ptp_tstmp_cmp(loc, rem);
        if (clk_ctx->clk_offs_sign < 0) {
            lip_ptp_tstmp_sub(&clk_ctx->local_clk_offs, rem, loc);
        } else {
            lip_ptp_tstmp_sub(&clk_ctx->local_clk_offs, loc, rem);
        }
    }
#endif


    clk_ctx->clk_offs_sign = lip_ptp_tstmp_cmp(loc, rem);
    if (clk_ctx->clk_offs_sign < 0) {
        lip_ptp_tstmp_sub(&clk_ctx->local_clk_offs, rem, loc);
    } else {
        lip_ptp_tstmp_sub(&clk_ctx->local_clk_offs, loc, rem);
    }
    clk_ctx->clk_offs_valid = true;
    clk_ctx->syn_rx_gm = *rem;
    clk_ctx->syn_rx_local = *loc;
}

void lip_ptp_local_clk_sync_to_gm(t_lip_ptp_inst_ctx *inst, const t_lip_ptp_tstmp *loc, t_lip_ptp_tstmp *gm) {
    t_lip_ptp_local_clk_ctx *clk_ctx = &inst->local_clk;
    if (clk_ctx->clk_offs_valid) {
        /* сперва устраняем смещение между локальным клоком и GM на момент последнего sync */
        if (clk_ctx->clk_offs_sign < 0) {
            lip_ptp_tstmp_add(gm, loc, &clk_ctx->local_clk_offs);
        } else {
            lip_ptp_tstmp_sub(gm, loc, &clk_ctx->local_clk_offs);
        }
        /* дополнительная коррекция исходя из прошедшего времени с момента sync
         * для учета разницы частоты локального клока и GM и получившейся за счет
         * это разбежки */
        t_lip_fns cor = lip_ptp_tstmp_delta_fns(loc, &clk_ctx->syn_rx_local);
        lip_ptp_tstmp_add_fns(gm, cor * (inst->gm_stats.rateRatio - 1.));
    }
}

void lip_ptp_local_clk_sync_from_gm(t_lip_ptp_inst_ctx *inst, const t_lip_ptp_tstmp *gm, t_lip_ptp_tstmp *loc) {
    t_lip_ptp_local_clk_ctx *clk_ctx = &inst->local_clk;
    if (clk_ctx->clk_offs_valid) {
        /* сперва устраняем смещение между локальным клоком и GM на момент последнего sync */
        if (clk_ctx->clk_offs_sign > 0) {
            lip_ptp_tstmp_add(loc, gm, &clk_ctx->local_clk_offs);
        } else {
            lip_ptp_tstmp_sub(loc, gm, &clk_ctx->local_clk_offs);
        }
        /* дополнительная коррекция исходя из прошедшего времени с момента sync
         * для учета разницы частоты локального клока и GM и получившейся за счет
         * это разбежки */
        t_lip_fns cor = lip_ptp_tstmp_delta_fns(loc, &clk_ctx->syn_rx_local);
        lip_ptp_tstmp_add_fns(loc, cor * (1./inst->gm_stats.rateRatio - 1.));
    }
}


#endif
