/*
 * detect_overload.c
 *
 *  Created on: 15 дек. 2020 г.
 *      Author: star
 */
#include <adc_tools.h>
#include <detect_overload.h>
#include <edrk_main.h>
#include <params_motor.h>
#include <params_pwm24.h>
#include "alg_simple_scalar.h"

#include "IQmathLib.h"

DETECT_OVERLOAD out_I_over_1_6 = DETECT_OVERLOAD_DEFAULTS;

#define CALLS_IN_PWM_INT 2  //Количество прерываний за период ШИМ (1 или 2)

void init_detect_overloads(void) {
    out_I_over_1_6.level_overload = _IQmpy(I_OUT_NOMINAL_IQ, _IQ(1.6));
    out_I_over_1_6.time_over_tics = (long) 15 * FREQ_PWM * CALLS_IN_PWM_INT;
    out_I_over_1_6.time_latch_tics = (long) 45 * FREQ_PWM * CALLS_IN_PWM_INT;
    out_I_over_1_6.tics_counter = 0;
    out_I_over_1_6.overload_detected = 0;

}

int calc_detect_overload(DETECT_OVERLOAD *v) {
    if (v->val > v->level_overload) {
        v->tics_counter += 1;
        if (v->tics_counter > v->time_over_tics) { v->tics_counter = v->time_over_tics;}
    } else {
        if (v->tics_counter > 0) { v->tics_counter -= 1; }
        else {v->tics_counter = 0;}
        if (v->overload_detected && v->tics_counter == 0) {
            v->overload_detected = 0;
        }
    }
    if (v->tics_counter >= v->time_over_tics) {
        v->overload_detected = 1;
        v->tics_counter = v->time_latch_tics;
    }
    return v->overload_detected;
}

#define LIMIT_DETECT_LEVEL 16273899 // 0.97  //15938355 //95%

void check_all_power_limits() {
    _iq level_I_nominal = 0;

    //edrk.power_limit.bits.limit_by_temper = edrk.temper_limit_koeffs.code_status;

    if (edrk.Go)
    {

        level_I_nominal = _IQmpy(LIMIT_DETECT_LEVEL, edrk.zadanie.iq_Izad_rmp);

        if ((filter.iqIm > level_I_nominal)  ||
                out_I_over_1_6.overload_detected)
        {
            edrk.power_limit.bits.limit_Iout = 1;
        } else
        {
            edrk.power_limit.bits.limit_Iout = 0;
        }
    }
    else
        edrk.power_limit.bits.limit_Iout = 0;

//    if (edrk.from_uom.code>1)
//        edrk.power_limit.bits.limit_UOM = 1;
//    else
//        edrk.power_limit.bits.limit_UOM = 0;

//filter.PowerScalar + edrk.iq_power_kw_another_bs
    if ( (edrk.iq_power_kw_full_filter_abs > _IQmpy(LIMIT_DETECT_LEVEL, edrk.zadanie.iq_limit_power_zad_rmp))
            || simple_scalar1.flag_decr_mzz_power
        // Данный способ для скалярного управления, для FOC, возможно, нужна векторная мощность.
        )
    {
        edrk.power_limit.bits.limit_from_SVU = 1;
    }
    else
    {
        edrk.power_limit.bits.limit_from_SVU = 0;
    }


}