/*
 * limit_power.c
 *
 *  Created on: 15 авг. 2024 г.
 *      Author: yura
 */

#include "IQmathLib.h"

#include <adc_tools.h>
#include <detect_overload.h>
#include <edrk_main.h>
#include <params_motor.h>
#include <params_pwm24.h>
#include "mathlib.h"
#include "math_pi.h"


#include "limit_power.h"
#include "limit_lib.h"

#include "pll_tools.h"

#include "uom_tools.h"




#define KOEF_50HZ       (FREQ_PWM*2.0*50.0/PI)
#define LEVEL_01HZ_IQ      _IQ(10.0/KOEF_50HZ)  // 0.1 HZ

_iq level_50hz       = _IQmpyI32(LEVEL_01HZ_IQ, 500);
_iq level_minimal_level_work_hz       = _IQmpyI32(LEVEL_01HZ_IQ, 350);

_iq delta_freq_test = 0;


//_iq level_01hz = _IQ(LEVEL_01HZ);


//#define LEVEL_50HZ      (5000.0/KOEF_50HZ)  // 50 HZ
//#define LEVEL_05HZ      (50.0/KOEF_50HZ)    // 50 HZ
//
//#define LEVEL_3HZ       (300.0/KOEF_50HZ) // 50 HZ
//#define LEVEL_2HZ       (200.0/KOEF_50HZ) // 50 HZ
//#define LEVEL_1HZ       (100.0/KOEF_50HZ) // 50 HZ



#define LEVEL1_FREQ_DECR    10 // 1.5 Hz  от 49.0
#define LEVEL2_FREQ_DECR    100 // 10 Hz  до 40
//#define LEVEL1_FREQ_DECR    15 // 1.5 Hz  от 48.5
//#define LEVEL2_FREQ_DECR    100 // 10 Hz  до 40

#define PLUS_LIMIT_KOEFFS   0.0001
#define MINUS_LIMIT_KOEFFS   0.05

#define MAX_COUNT_GO_UOM    (FREQ_PWM*5) // 5 sec
#define SET_LIMIT_UOM       0.5

void calc_all_limit_koeffs(void)
{
    _iq sum_limit, delta_freq;

    static unsigned int prev_uom = 0;
    static int flag_enable_go_uom = 0;


    static _iq level1_freq_decr = _IQmpyI32(LEVEL_01HZ_IQ, LEVEL1_FREQ_DECR);
    static _iq level2_freq_decr = _IQmpyI32(LEVEL_01HZ_IQ, LEVEL2_FREQ_DECR);

    static _iq iq_set_limit_uom = _IQ(SET_LIMIT_UOM);
    static unsigned int count_go_uom = 0;

    static _iq iq_plus_limit_koeffs = _IQ(PLUS_LIMIT_KOEFFS);
    static _iq iq_minus_limit_koeffs = _IQ(MINUS_LIMIT_KOEFFS);

    static long freq_test = 30;
    //*LEVEL_01HZ_IQ;
    static _iq minus_delta_freq_test = _IQdiv32(LEVEL_01HZ_IQ); // 0.1/32


    static int uom_test = 50;
    static int prev_imit_limit_freq = 0, prev_imit_limit_uom = 0;

    static _iq iq_new_uom_level_kwt = 0;


    update_uom();

    // temper
    edrk.all_limit_koeffs.local_temper_limit = edrk.temper_limit_koeffs.sum_limit;


    // uin_freq
    if (edrk.Status_Ready.bits.ready_final)  //|| edrk.imit_limit_freq
    {

        get_freq_50hz_iq();

    //    freq = LEVEL_50HZ - edrk.iq_freq_50hz;

        if (edrk.imit_limit_freq && prev_imit_limit_freq == 0)
            delta_freq_test = _IQmpyI32(LEVEL_01HZ_IQ, freq_test);

        if (delta_freq_test>0)
        {
            if (delta_freq_test>0)
                delta_freq_test -= minus_delta_freq_test;
            if (delta_freq_test<0)
                delta_freq_test = 0;
        }

        if (edrk.iq_freq_50hz>level_minimal_level_work_hz)
        {
            edrk.all_limit_koeffs.local_uin_freq_limit = linear_decrease_iq( (level_50hz - edrk.iq_freq_50hz),
                                                                             level2_freq_decr, level1_freq_decr);
        }
        else
            edrk.all_limit_koeffs.local_uin_freq_limit = CONST_IQ_1;
    }
    else
    {
        edrk.all_limit_koeffs.local_uin_freq_limit = CONST_IQ_1;
    }
    prev_imit_limit_freq = edrk.imit_limit_freq;

    //
    /// UOM
    //
    if (edrk.from_uom.ready || edrk.set_limit_uom_50)
    {
        if (edrk.set_limit_uom_50)
        {
            edrk.from_uom.level_value = uom_test;
        }

        if (edrk.imit_limit_uom && prev_imit_limit_uom == 0)
            edrk.from_uom.level_value++;


        if (prev_uom!=edrk.from_uom.level_value && edrk.from_uom.level_value > prev_uom)
        {
            if (edrk.iq_power_kw_full_filter_abs > edrk.from_uom.iq_level_value_kwt)
                flag_enable_go_uom = 1;
        }
        else
            flag_enable_go_uom = 0;

        if (flag_enable_go_uom)
        {
            count_go_uom = MAX_COUNT_GO_UOM;
            edrk.all_limit_koeffs.local_uom_limit = iq_set_limit_uom; // даем сброс
        }

        if (count_go_uom)
        {
            // держим сброс
            count_go_uom--;
        }
        else
            edrk.all_limit_koeffs.local_uom_limit = CONST_IQ_1; // возвращаемся

        prev_uom = edrk.from_uom.level_value;

    }
    else
    {

        edrk.power_limit.bits.limit_from_uom_fast = 0;
        edrk.all_limit_koeffs.uom_limit = CONST_IQ_1;
        prev_uom = 0;
    }
    prev_imit_limit_uom = edrk.imit_limit_uom;
 //   if ()


    //// temper
    edrk.all_limit_koeffs.temper_limit = zad_intensiv_q(iq_plus_limit_koeffs, iq_minus_limit_koeffs,
                                                        edrk.all_limit_koeffs.temper_limit,
                                                        edrk.all_limit_koeffs.local_temper_limit);

    edrk.power_limit.bits.limit_by_temper = (edrk.all_limit_koeffs.temper_limit<CONST_IQ_1) ? 1 : 0;

    ////// freq
    edrk.all_limit_koeffs.uin_freq_limit = zad_intensiv_q(iq_plus_limit_koeffs, iq_minus_limit_koeffs,
                                                        edrk.all_limit_koeffs.uin_freq_limit,
                                                        edrk.all_limit_koeffs.local_uin_freq_limit);

    edrk.power_limit.bits.limit_from_freq = (edrk.all_limit_koeffs.uin_freq_limit<CONST_IQ_1) ? 1 : 0;


    ///// uom
    edrk.all_limit_koeffs.uom_limit = zad_intensiv_q(iq_plus_limit_koeffs, iq_minus_limit_koeffs,
                                                        edrk.all_limit_koeffs.uom_limit,
                                                        edrk.all_limit_koeffs.local_uom_limit);

    edrk.power_limit.bits.limit_from_uom_fast = (edrk.all_limit_koeffs.uom_limit<CONST_IQ_1) ? 1 : 0;






    // sum_limit
    sum_limit = edrk.all_limit_koeffs.temper_limit;
    sum_limit = _IQmpy(sum_limit, edrk.all_limit_koeffs.moment_limit );
    sum_limit = _IQmpy(sum_limit, edrk.all_limit_koeffs.power_limit );
    sum_limit = _IQmpy(sum_limit, edrk.all_limit_koeffs.uin_freq_limit );
    sum_limit = _IQmpy(sum_limit, edrk.all_limit_koeffs.uom_limit );

    edrk.all_limit_koeffs.sum_limit = sum_limit;

}




void init_all_limit_koeffs(void)
{
    edrk.all_limit_koeffs.moment_limit = CONST_IQ_1;
    edrk.all_limit_koeffs.power_limit = CONST_IQ_1;
    edrk.all_limit_koeffs.temper_limit = CONST_IQ_1;
    edrk.all_limit_koeffs.uin_freq_limit = CONST_IQ_1;
    edrk.all_limit_koeffs.uom_limit = CONST_IQ_1;
    edrk.all_limit_koeffs.sum_limit = CONST_IQ_1;

    edrk.all_limit_koeffs.local_moment_limit = CONST_IQ_1;
    edrk.all_limit_koeffs.local_power_limit = CONST_IQ_1;
    edrk.all_limit_koeffs.local_temper_limit = CONST_IQ_1;
    edrk.all_limit_koeffs.local_uin_freq_limit = CONST_IQ_1;
    edrk.all_limit_koeffs.local_uom_limit = CONST_IQ_1;
    edrk.all_limit_koeffs.local_sum_limit = CONST_IQ_1;

}