/*
 * rtc_adjust_cloc.c
 *
 *  Created on: 12/04/2021
 *      Author: alexrayne <alexraynepe196@gmail.com>
 * ------------------------------------------------------------------------------
 *  internal clock adjusting to RTC reference clock
 */

#include "rtc_adjust_clock.h"



////////////////////////////////////////////////////////////////////////////////////
#include "sys/log.h"
LOG_MODULE_LOCAL("rtc-adj");
#ifdef LOG_LEVEL_RTCADJ
#define LOG_LEVEL LOG_LEVEL_RTCADJ
#else
#define LOG_LEVEL LOG_LEVEL_NONE
#endif

#include <trace_probes.h>



////////////////////////////////////////////////////////////////////////////////////

/** ru:
 * КИХ фильтр имеет неплохую динамику, и сходимость. Но он принципиально релеен, и
 *      значит у него всегда есть рассогласование. после отключения подстройки, его
 *      коррекция гарантировано поплывет
 */
/** @brief FIR filter have better dinamics and robust. But it is relay-principle -
 *           always have error, so after adjusting off this correction have
 *           drift clock rapid
 */
#define SYNC_FIR_STYLE  0

/** ru:
 * ПИД имеет непроверенную сходимость, и гораздо инерционнее, непонятную АЧХ.
 *      Но он подбирает коррекцию близко к требуемой.
 */
/** @brief PIDnot chacked for stability, and inertial, not checked AFH
 *      BUT It evaluates correction close to actual value.
 */
#define SYNC_PID_STYLE  1

#ifndef RTC_SYNC_STYLE
#define SYNC_STYLE      SYNC_PID_STYLE
//#define SYNC_STYLE      SYNC_FIR_STYLE
#else
#define SYNC_STYLE      RTC_SYNC_STYLE
#endif


////////////////////////////////////////////////////////////////////////////////////
/* FIR/PID filter - helps adjust constant drift. Whet sync-corrections loose, it
 *      keep adjust clock-rate.
 *
 * ru: это накопитель FIR-фильтр, для оценки постоянной погрешности часов.
 *  в случае пропажи синхросигналов, он остается для коррекции скорости часов.
 * */

enum {
    RTC_SYNC_ACC_NONE   =  RTIMER_ARCH_SECOND * RTC_SYNC_AVG_WINDOW,
    RTC_SYNC_DIFF_NONE  =  RTIMER_ARCH_SECOND,

/// SCALE > 0 - use value G = 1/scale
///       < 0 - uses value G = (1-1/scale)
///       = 0 - G = 0

#if SYNC_STYLE == SYNC_FIR_STYLE
    // FIR коэффициенты
    RTC_SYNC_ACC_SCALE  = -8,
    RTC_SYNC_GAIN_SCALE = 2,
    RTC_SYNC_DIFF_SCALE = 2,
    // limit scale for accumulated drifts: accu += limited(err).
    //      Should help damping accident large error shocks with amplitude much more  stationary error.
    RTC_SYNC_SHOCK_LIMIT = 2,
#else
    // PID коэффициенты
    RTC_SYNC_ACC_SCALE  = RTC_SYNC_AVG_WINDOW*2,
    RTC_SYNC_GAIN_SCALE = -4,
    RTC_SYNC_DIFF_SCALE = 4,
    // limit scale for accumulated drifts
    RTC_SYNC_SHOCK_LIMIT = 2,
#endif

};

//  if >= RTIMER_ARCH_SECOND - no valid value
int rtc_sync_diff;
int rtc_last_diff;
rtimer_clock_t rtc_last_1sec;
clock_time_t clock_last_correction;
clock_time_t rtc_last_correction;
int clock_adjust_diff = 0;
int clock_adjust_last = 0;

int rtc_sync_diffs[RTC_SYNC_AVG_WINDOW];
//  if >= RTIMER_ARCH_SECOND - accumulator empty, need initiate
int rtc_sync_diffsum;
//  sync PI accumulator
int rtc_sync_acc;

//static
clock_time_t clock_last_correction_time;


// limit diff, to damp accident error shock
static
int rtc_sync_error_limit(int diff, int accu){

    // limit diff, to damp accident error shock
    int limit = RTC_SYNC_SHOCK_LIMIT * ABS(rtc_sync_diffsum);
    if ( (diff > 0) && (diff > limit) )
        diff = limit;
    else if ( (diff < 0) && (diff < -limit) )
        diff = -limit;

    return diff;
}

static
void rtc_sync_accumulate(int x, int delta){
    //FIR
    rtc_sync_diffsum -= rtc_sync_diffs[RTC_SYNC_AVG_WINDOW-1];
    rtc_sync_diffsum += x;

    // shift diffs, insert rtc_sync_diff at diffs head
    int diff = rtc_sync_error_limit(x, rtc_sync_diffsum/RTC_SYNC_AVG_WINDOW );

    for (unsigned i = 0; i < RTC_SYNC_AVG_WINDOW; ++i ){
        int save = rtc_sync_diffs[i];
        rtc_sync_diffs[i] = diff;
        diff = save;
    }

    // PID
    diff = rtc_sync_error_limit(delta, rtc_sync_acc/ABS(RTC_SYNC_ACC_SCALE) );
    rtc_sync_acc += diff;
    LOG_TRACE("+: %d\n", x);
}

static
void rtc_sync_initiate(int x){
    rtc_sync_acc     = x*ABS(RTC_SYNC_ACC_SCALE);
    rtc_sync_diffsum = x*RTC_SYNC_AVG_WINDOW;
    for (unsigned i = 0; i < RTC_SYNC_AVG_WINDOW; ++i )
        rtc_sync_diffs[i] = x;
    LOG_TRACE("$: %d\n", x);
}



#ifdef RTC_SYNC_CLOCK_TOL
static
bool rtc_sync_diff_vaild(int x){
    enum {
        RTC_SYNC_TOL = (RTC_SYNC_CLOCK_TOL * RTIMER_ARCH_SECOND / CLOCK_SECOND),
    };
    return (x < RTC_SYNC_TOL) && (x > -RTC_SYNC_TOL);
}
#else
#define rtc_sync_diff_vaild(x) true
#endif


static inline
int  rtc_sync_gained( int x, int gain){
    if (gain > 0){
        return x / gain;
    }
    else if (gain == 0)
        return 0;
    else
        return x - ( x/(-gain) );
}

/// @brief event handle for time-stamp event from RTC
///     it must be invoked by App BSP on time-regular event/IRQ from RTC
void rtc_adjust_sync(rtimer_clock_t stamp_1sec ){
    if (rtc_last_1sec > 0) {
        // remember last clock-delta
        rtc_sync_diff = stamp_1sec - rtc_last_1sec - RTIMER_ARCH_SECOND;

        LOG_TRACE("diff at %lu <- %lu + %d \n", stamp_1sec, rtc_last_1sec, rtc_sync_diff);
        // unlikely
        if (    ( rtc_sync_diff > RTC_SYNC_DIFF_NONE )
             || (!rtc_sync_diff_vaild(rtc_sync_diff)) )
        {
            rtc_last_1sec   = stamp_1sec;
            rtc_last_diff   = 0;
            LOG_ERR("resync(%lu): %d # %d \n", stamp_1sec, rtc_sync_diff, clock_adjust_diff);
            return;
        }

        if (rtc_sync_diffsum < RTC_SYNC_ACC_NONE)
            rtc_sync_accumulate(rtc_sync_diff + clock_adjust_diff, rtc_sync_diff);
        else
            rtc_sync_initiate(rtc_sync_diff);

        if (rtc_sync_diffsum < RTC_SYNC_ACC_NONE){

#if SYNC_STYLE == SYNC_PID_STYLE
            clock_adjust_diff = (rtc_sync_diff - rtc_last_diff)/RTC_SYNC_DIFF_SCALE
                              + rtc_sync_gained(rtc_sync_diff , RTC_SYNC_GAIN_SCALE)
                                    //rtc_sync_diffsum / RTC_SYNC_AVG_WINDOW
                              + rtc_sync_acc / ABS(RTC_SYNC_ACC_SCALE)
                              ;
            LOG_DBG("sync(%lu): %d + [%d] -> %d\n", stamp_1sec, rtc_sync_diff, rtc_sync_acc, clock_adjust_diff);
#else //if SYNC_STYLE == SYNC_FIR_STYLE
            clock_adjust_diff = rtc_sync_gained(rtc_sync_diffsum, RTC_SYNC_ACC_SCALE) / (RTC_SYNC_AVG_WINDOW)
                              + rtc_sync_gained(rtc_sync_diff , RTC_SYNC_GAIN_SCALE)
                              + (rtc_sync_diff - rtc_last_diff)/RTC_SYNC_DIFF_SCALE
                              ;
            LOG_TRACE("sync(%lu): (%d+%d) + [%d] -> %d\n", stamp_1sec, clock_adjust_diff, rtc_sync_diff, rtc_sync_diffsum
                                                        , clock_adjust_diff );
#endif

        }
        else if ( rtc_sync_diff < RTC_SYNC_DIFF_NONE ){
            clock_adjust_diff = rtc_sync_diff;
            LOG_DBG("sync(%lu): %d <-%d\n", stamp_1sec, clock_adjust_diff, rtc_last_diff);
        }
        else
            clock_adjust_diff = 0;

        rtc_last_diff = rtc_sync_diff;
        rtc_last_correction = clock_time();
    }
    else {
        // first time sync.
        LOG_INFO("1sync(%lu): %d \n", stamp_1sec, clock_adjust_diff);
        rtc_last_diff = 0;
    }

    // (rtc_last_1sec + RTIMER_ARCH_SECOND) = stamp_1sec - rtc_last_diff
    // plan next sync-time point acounts with correction
    rtc_last_1sec    = stamp_1sec - rtc_last_diff;
}


void rtc_adjust_reset(void){
    rtc_sync_acc        = 0;
    rtc_sync_diffsum    = RTC_SYNC_ACC_NONE;
    rtc_sync_diff       = RTC_SYNC_DIFF_NONE;
    rtc_last_diff       = 0;
    clock_adjust_diff   = 0;
    clock_adjust_last   = 0;
    rtc_last_correction = 0;
    clock_last_correction=0;
    //rtc_last_1sec       = 0;
    for (unsigned i = 0; i < RTC_SYNC_AVG_WINDOW; ++i )
        rtc_sync_diffs[i] = 0;

    LOG_TRACE("reset\n");
}

/// @brief platform clock invoke this function to get 1sec-to-RTC
///         correction, measured by internal [rtimer]
int rtc_adjust_1sec_correction( void ){
    clock_time_t now = clock_time();

    if ( (rtc_last_correction - clock_last_correction) > 0 ){   //unlikely
        clock_last_correction = now;
        return clock_adjust_diff;
    }

    if ( (now - clock_last_correction) > (CLOCK_SECOND + RTC_SYNC_CLOCK_TOL*2) ){ //unlikely
        clock_last_correction += CLOCK_SECOND;
        return clock_adjust_diff;
    }

    return 0;
}
