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

/** @todo добавить обработку hsec ? */

void lip_ptp_tstmp_add_ns(t_lip_ptp_tstmp *tsmp, int32_t ns) {
    int32_t dns = tsmp->ns + ns;
    if (dns > LIP_PTP_SEC_NS_MUL) {
        ++tsmp->sec;
        if (tsmp->sec == 0)
            ++tsmp->hsec;
        tsmp->ns = dns - LIP_PTP_SEC_NS_MUL;
    } else if (dns < 0) {
        if (tsmp->sec == 0)
            --tsmp->hsec;
        --tsmp->sec;
        tsmp->ns = dns + LIP_PTP_SEC_NS_MUL;
    } else {
        tsmp->ns = dns;
    }
}

t_lip_fns lip_ptp_tstmp_delta_fns(const t_lip_ptp_tstmp *t1, const t_lip_ptp_tstmp *t2) {
    int32_t dsec = t1->sec - t2->sec;
    int32_t dns = t1->ns - t2->ns;
    int32_t dfract = t1->fract_ns - t2->fract_ns;
    if (dfract < 0) {
        dfract += LIP_PTP_FNS_FRACT_MAX;
        --dns;
    }
    if (dns < 0) {
        --dsec;
        dns += LIP_PTP_SEC_NS_MUL;
    }
    return (((t_lip_fns)dsec * LIP_PTP_SEC_NS_MUL + dns) << LIP_PTP_FNS_SHIFT) + dfract;
}


void lip_ptp_tstmp_add_fns(t_lip_ptp_tstmp *tstmp, t_lip_fns fns) {
    if (fns < 0) {
        fns = -fns;
        uint32_t fns_fract = fns & LIP_PTP_FNS_FRACT_MASK;
        int64_t fns_nsec   = (fns >> LIP_PTP_FNS_SHIFT);
        int32_t  dfns = tstmp->fract_ns - fns_fract;

        if (dfns < 0) {
            dfns += LIP_PTP_FNS_FRACT_MAX;
            fns_nsec += 1;
        }
        tstmp->fract_ns = dfns;

        fns_nsec = tstmp->ns - fns_nsec;
        while (fns_nsec < 0) {
            fns_nsec += LIP_PTP_SEC_NS_MUL;
            --tstmp->sec;
        }
        tstmp->ns = fns_nsec & 0xFFFFFFFF;
    } else {
        uint32_t fns_fract = fns & LIP_PTP_FNS_FRACT_MASK;
        uint64_t fns_nsec   = (fns >> LIP_PTP_FNS_SHIFT);
        uint32_t dfns = tstmp->fract_ns + fns_fract;
        if (dfns >= LIP_PTP_FNS_FRACT_MAX) {
            dfns -= LIP_PTP_FNS_FRACT_MAX;
            fns_nsec++;
        }
        tstmp->fract_ns = dfns;
        fns_nsec += tstmp->ns;
        while (fns_nsec >= LIP_PTP_SEC_NS_MUL) {
            fns_nsec -= LIP_PTP_SEC_NS_MUL;
            tstmp->sec++;
        }
        tstmp->ns = fns_nsec & 0xFFFFFFFF;
    }
}


int lip_ptp_tstmp_cmp(const t_lip_ptp_tstmp *t1, const t_lip_ptp_tstmp *t2) {
    return  (t1->hsec != t2->hsec)    ? (int)(t1->hsec - t2->hsec) :
               (t1->sec != t2->sec)   ? (int)(t1->sec - t2->sec) :
               (t1->ns != t2->ns)     ? (int)(t1->ns - t2->ns) :
                                        (int)(t1->fract_ns - t2->fract_ns);
}

void lip_ptp_tstmp_sub(t_lip_ptp_tstmp *t_res, const t_lip_ptp_tstmp *t1, const t_lip_ptp_tstmp *t2) {
    int32_t fract_ns = t1->fract_ns - t2->fract_ns;
    int32_t ns = t1->ns - t2->ns;
    uint32_t s = t1->sec - t2->sec;

    if (fract_ns < 0) {
        fract_ns += LIP_PTP_FNS_FRACT_MAX;
        --ns;
    }
    if (ns < 0) {
        ns += LIP_PTP_SEC_NS_MUL;
        s--;
    }

    t_res->fract_ns = fract_ns;
    t_res->ns = ns;
    t_res->sec = s;
    t_res->hsec = 0;
}



void lip_ptp_tstmp_add(t_lip_ptp_tstmp *t_res, const t_lip_ptp_tstmp *t1, const t_lip_ptp_tstmp *t2) {
    uint32_t fract_ns = t1->fract_ns + t2->fract_ns;
    uint32_t ns = t1->ns + t2->ns;
    uint32_t s = t1->sec + t2->sec;


    if (fract_ns >= LIP_PTP_FNS_FRACT_MAX) {
        fract_ns -= LIP_PTP_FNS_FRACT_MAX;
        ++ns;
    }
    if (ns >= LIP_PTP_SEC_NS_MUL) {
        ns -= LIP_PTP_SEC_NS_MUL;
        ++s;
    }

    t_res->fract_ns = fract_ns;
    t_res->ns = ns;
    t_res->sec = s;
    t_res->hsec = 0;
}

#endif
