/*
 * detect_errors_adc.c
 *
 *  Created on: 7 дек. 2020 г.
 *      Author: star
 */
#include <adc_tools.h>
#include <detect_error_3_phase.h>
#include <detect_errors_adc.h>
#include <detect_phase_break.h>
#include <edrk_main.h>
#include <params_protect_adc.h>
#include <protect_levels.h>
#include "digital_filters.h"

#include "IQmathLib.h"

//ANALOG_PROTECT_LEVELS analog_protect_levels = ANALOG_PROTECT_LEVELS_DEFAULTS;

//#pragma DATA_SECTION(analog_protect,".fast_vars");
#pragma DATA_SECTION(analog_protect,".slow_vars");
ANALOG_ADC_PROTECT analog_protect = ANALOG_ADC_PROTECT_DEFAULTS;

//#pragma DATA_SECTION(break_Iout_1_state,".fast_vars");
#pragma DATA_SECTION(break_Iout_1_state,".slow_vars");
BREAK_PHASE_I break_Iout_1_state = BREAK_PHASE_I_DEFAULTS;

//#pragma DATA_SECTION(break_Iout_2_state,".fast_vars");
#pragma DATA_SECTION(break_Iout_2_state,".slow_vars");
BREAK_PHASE_I break_Iout_2_state = BREAK_PHASE_I_DEFAULTS;

int detect_error_Izpt();
int detect_error_Ibreak(int res_num);
void init_protect_3_phase(void);

void init_analog_protect_levels(void) {
    init_protect_3_phase();
}

#define AMPL_TO_RMS 0.709

//#define LEVEL_I_1_2_DIBALANCE 1118481  // 200 A
#define LEVEL_I_1_2_DIBALANCE   1677721  // 300 A

void init_protect_3_phase(void) {
    analog_protect.in_voltage[0].setup.levels.iqVal_module_max = edrk.iqMAX_U_IN;
    analog_protect.in_voltage[0].setup.levels.iqVal_U_max = edrk.iqMAX_U_IN;
    analog_protect.in_voltage[0].setup.levels.iqVal_V_max = edrk.iqMAX_U_IN;
    analog_protect.in_voltage[0].setup.levels.iqVal_W_max = edrk.iqMAX_U_IN;
    analog_protect.in_voltage[0].setup.levels.iqNominal_plus10 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_10_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[0].setup.levels.iqNominal_plus20 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_20_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[0].setup.levels.iqNominal_minus10 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_10_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[0].setup.levels.iqNominal_minus20 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_20_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[0].setup.levels.iqAsymmetry_delta = _IQ(edrk.zadanie.ZadanieU_Charge*ASYMMETRY_DELTA_PERCENTS/NORMA_ACP);
    analog_protect.in_voltage[0].setup.use.all = 0;
    analog_protect.in_voltage[0].setup.use.bits.phase_U = 0;
    analog_protect.in_voltage[0].setup.use.bits.phase_V = 0;
    analog_protect.in_voltage[0].setup.use.bits.phase_W = 0;
    analog_protect.in_voltage[0].setup.use.bits.module = 0;
    analog_protect.in_voltage[0].setup.use.bits.detect_minus_10 = 1;
    analog_protect.in_voltage[0].setup.use.bits.detect_minus_20 = 1;
    analog_protect.in_voltage[0].setup.use.bits.detect_plus_10 = 0;
    analog_protect.in_voltage[0].setup.use.bits.detect_plus_20 = 1;
    analog_protect.in_voltage[0].setup.use.bits.system_asymmetry_by_delta = 1;

    analog_protect.in_voltage[1].setup.levels.iqVal_module_max = edrk.iqMAX_U_IN;
    analog_protect.in_voltage[1].setup.levels.iqVal_U_max = edrk.iqMAX_U_IN;
    analog_protect.in_voltage[1].setup.levels.iqVal_V_max = edrk.iqMAX_U_IN;
    analog_protect.in_voltage[1].setup.levels.iqNominal_plus10 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_10_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[1].setup.levels.iqNominal_plus20 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_20_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[1].setup.levels.iqNominal_minus10 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_10_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[1].setup.levels.iqNominal_minus20 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_20_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[1].setup.levels.iqAsymmetry_delta = _IQ(edrk.zadanie.ZadanieU_Charge*ASYMMETRY_DELTA_PERCENTS/NORMA_ACP);
    analog_protect.in_voltage[1].setup.use.all = 0;
    analog_protect.in_voltage[1].setup.use.bits.phase_U = 0;
    analog_protect.in_voltage[1].setup.use.bits.phase_V = 0;
    analog_protect.in_voltage[1].setup.use.bits.phase_W = 0;
    analog_protect.in_voltage[1].setup.use.bits.module = 0;
    analog_protect.in_voltage[1].setup.use.bits.detect_minus_10 = 1;
    analog_protect.in_voltage[1].setup.use.bits.detect_minus_20 = 1;
    analog_protect.in_voltage[1].setup.use.bits.detect_plus_10 = 0;
    analog_protect.in_voltage[1].setup.use.bits.detect_plus_20 = 1;
    analog_protect.in_voltage[1].setup.use.bits.system_asymmetry_by_delta = 1;

    /////////////////
    analog_protect.out_I[0].setup.levels.iqVal_module_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP);
    analog_protect.out_I[0].setup.levels.iqVal_U_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP);
    analog_protect.out_I[0].setup.levels.iqVal_V_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP);
    analog_protect.out_I[0].setup.levels.iqVal_W_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP);
    analog_protect.out_I[0].setup.use.all = 0;
    analog_protect.out_I[0].setup.use.bits.phase_U = 1;
    analog_protect.out_I[0].setup.use.bits.phase_V = 1;
    analog_protect.out_I[0].setup.use.bits.phase_W = 1;
    analog_protect.out_I[0].setup.use.bits.break_phase = 1;

    analog_protect.out_I[1].setup.levels.iqVal_module_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP);
    analog_protect.out_I[1].setup.levels.iqVal_U_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP);
    analog_protect.out_I[1].setup.levels.iqVal_V_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP);
    analog_protect.out_I[1].setup.levels.iqVal_W_max = _IQ(LEVEL_ADC_I_AF / NORMA_ACP);
    analog_protect.out_I[1].setup.use.all = 0;
    analog_protect.out_I[1].setup.use.bits.phase_U = 1;
    analog_protect.out_I[1].setup.use.bits.phase_V = 1;
    analog_protect.out_I[1].setup.use.bits.phase_W = 1;
    analog_protect.out_I[1].setup.use.bits.break_phase = 1;

    analog_protect.iqI_zpt_level = _IQ(LEVEL_ADC_I_ZPT / NORMA_ACP);
    analog_protect.iqI_break_level = _IQ(LEVEL_ADC_I_BREAK / NORMA_ACP);
}

#define min(x,y) (x) < (y) ? (x) : (y)
#define max(x,y) (x) > (y) ? (x) : (y)

void reinit_protect_I_and_U_settings(void) {
    int max_I = 0;
    analog_protect.in_voltage[0].setup.levels.iqVal_module_max = edrk.iqMAX_U_IN;
    analog_protect.in_voltage[0].setup.levels.iqVal_U_max = edrk.iqMAX_U_IN;
    analog_protect.in_voltage[0].setup.levels.iqVal_V_max = edrk.iqMAX_U_IN;
    analog_protect.in_voltage[0].setup.levels.iqVal_W_max = edrk.iqMAX_U_IN;
    analog_protect.in_voltage[0].setup.levels.iqNominal_plus10 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_10_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[0].setup.levels.iqNominal_plus20 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_20_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[0].setup.levels.iqNominal_minus10 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_10_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[0].setup.levels.iqNominal_minus20 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_20_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[0].setup.levels.iqAsymmetry_delta = _IQ(edrk.zadanie.ZadanieU_Charge*ASYMMETRY_DELTA_PERCENTS/NORMA_ACP);

    analog_protect.in_voltage[1].setup.levels.iqVal_module_max = edrk.iqMAX_U_IN;
    analog_protect.in_voltage[1].setup.levels.iqVal_U_max = edrk.iqMAX_U_IN;
    analog_protect.in_voltage[1].setup.levels.iqVal_V_max = edrk.iqMAX_U_IN;
    analog_protect.in_voltage[1].setup.levels.iqVal_W_max = edrk.iqMAX_U_IN;
    analog_protect.in_voltage[1].setup.levels.iqNominal_plus10 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_10_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[1].setup.levels.iqNominal_plus20 = _IQ(edrk.zadanie.ZadanieU_Charge*PLUS_20_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[1].setup.levels.iqNominal_minus10 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_10_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[1].setup.levels.iqNominal_minus20 = _IQ(edrk.zadanie.ZadanieU_Charge*MINUS_20_PERCENT/NORMA_ACP);
    analog_protect.in_voltage[1].setup.levels.iqAsymmetry_delta = _IQ(edrk.zadanie.ZadanieU_Charge*ASYMMETRY_DELTA_PERCENTS/NORMA_ACP);

    max_I = max(protect_levels.alarm_Imax_U02, protect_levels.alarm_Imax_U03);
    max_I = max(max_I, protect_levels.alarm_Imax_U04);
    analog_protect.out_I[0].setup.levels.iqVal_module_max = _IQ(((float)max_I) / NORMA_ACP);
    analog_protect.out_I[0].setup.levels.iqVal_U_max = _IQ(((float)protect_levels.alarm_Imax_U02) / NORMA_ACP);
    analog_protect.out_I[0].setup.levels.iqVal_V_max = _IQ(((float)protect_levels.alarm_Imax_U03) / NORMA_ACP);
    analog_protect.out_I[0].setup.levels.iqVal_W_max = _IQ(((float)protect_levels.alarm_Imax_U04) / NORMA_ACP);

    max_I = max(protect_levels.alarm_Imax_U05, protect_levels.alarm_Imax_U06);
    max_I = max(max_I, protect_levels.alarm_Imax_U07);
    analog_protect.out_I[1].setup.levels.iqVal_module_max = _IQ(((float)max_I) / NORMA_ACP);
    analog_protect.out_I[1].setup.levels.iqVal_U_max = _IQ(((float)protect_levels.alarm_Imax_U05) / NORMA_ACP);
    analog_protect.out_I[1].setup.levels.iqVal_V_max = _IQ(((float)protect_levels.alarm_Imax_U06) / NORMA_ACP);
    analog_protect.out_I[1].setup.levels.iqVal_W_max = _IQ(((float)protect_levels.alarm_Imax_U07) / NORMA_ACP);

    analog_protect.iqI_zpt_level = _IQ(protect_levels.alarm_Izpt_max / NORMA_ACP);
    analog_protect.iqI_break_level = _IQ(protect_levels.alarm_Imax_U01 / NORMA_ACP);

    if (edrk.SumSbor == 0) {
        analog_protect.in_voltage[0].setup.timers_inited = 0;
        analog_protect.in_voltage[1].setup.timers_inited = 0;
        analog_protect.out_I[0].setup.timers_inited = 0;
        analog_protect.out_I[1].setup.timers_inited = 0;
    }
}

#define TIME_DETECT_WARNING_U_PREDELS   100

void detect_protect_adc(_iq teta_ch1, _iq teta_ch2) {
    static unsigned int timer_U_in1_minus10 = 0;
    static unsigned int timer_U_in1_minus20 = 0;
    static unsigned int timer_U_in1_plus20 = 0;

    static unsigned int timer_U_in2_minus10 = 0;
    static unsigned int timer_U_in2_minus20 = 0;
    static unsigned int timer_U_in2_plus20 = 0;

    static unsigned int counter_in1_minus10 = 0;
    static unsigned int counter_in2_minus10 = 0;
    static unsigned int counter_in1_minus20 = 0;
    static unsigned int counter_in2_minus20 = 0;


    //Входные напряжения
    //0
    analog_protect.in_voltage[0].errors.all = 0;
    analog_protect.in_voltage[0].iqVal_U = analog.iqUin_A1B1_rms;
    analog_protect.in_voltage[0].iqVal_V = analog.iqUin_B1C1_rms;
    analog_protect.in_voltage[0].iqVal_W = analog.iqUin_C1A1_rms;
    analog_protect.in_voltage[0].iqVal_mod = filter.iqUin_m1;

//    analog_protect.in_voltage[0].calc(&analog_protect.in_voltage[0]);

    //1
    analog_protect.in_voltage[1].errors.all = 0;
    analog_protect.in_voltage[1].iqVal_U = analog.iqUin_A2B2_rms;
    analog_protect.in_voltage[1].iqVal_V = analog.iqUin_B2C2_rms;
    analog_protect.in_voltage[1].iqVal_W = analog.iqUin_C2A2_rms;
    analog_protect.in_voltage[1].iqVal_mod = filter.iqUin_m2;

//    analog_protect.in_voltage[1].calc(&analog_protect.in_voltage[1]);

    //
    edrk.errors.e0.bits.U_A1B1_MAX |= analog_protect.in_voltage[0].errors.bits.phase_U_max;
    edrk.errors.e0.bits.U_B1C1_MAX |= analog_protect.in_voltage[0].errors.bits.phase_V_max;
    edrk.errors.e0.bits.U_IN_MAX |= analog_protect.in_voltage[0].errors.bits.module_max;
    //
    edrk.errors.e0.bits.U_A2B2_MAX |= analog_protect.in_voltage[1].errors.bits.phase_U_max;
    edrk.errors.e0.bits.U_B2C2_MAX |= analog_protect.in_voltage[1].errors.bits.phase_V_max;
    edrk.errors.e0.bits.U_IN_MAX |= analog_protect.in_voltage[1].errors.bits.module_max;

    edrk.warnings.e8.bits.U_IN_20_PROCENTS_HIGH = analog_protect.in_voltage[0].over_limit.bits.module_20_percent_hi || analog_protect.in_voltage[1].over_limit.bits.module_20_percent_hi;
    edrk.errors.e8.bits.U_IN_20_PROCENTS_HIGH |= analog_protect.in_voltage[0].errors.bits.module_20_percent_hi;
    edrk.errors.e8.bits.U_IN_20_PROCENTS_HIGH |= analog_protect.in_voltage[1].errors.bits.module_20_percent_hi;

    if (edrk.from_shema_filter.bits.QTV_ON_OFF == 1
   //         && edrk.to_shema.bits.QTV_ON
            )
    {

        // контроль напряжений только при включенном силовом автомате
        edrk.errors.e8.bits.U_IN_10_PROCENTS_LOW |= analog_protect.in_voltage[0].errors.bits.module_10_percent_low;
        edrk.errors.e8.bits.U_IN_20_PROCENTS_LOW |= analog_protect.in_voltage[0].errors.bits.module_20_percent_low;

        edrk.errors.e8.bits.U_IN_10_PROCENTS_LOW |= analog_protect.in_voltage[1].errors.bits.module_10_percent_low;
        edrk.errors.e8.bits.U_IN_20_PROCENTS_LOW |= analog_protect.in_voltage[1].errors.bits.module_20_percent_low;

        edrk.warnings.e8.bits.U_IN_10_PROCENTS_LOW = pause_detect_error(&counter_in1_minus10,
                                                                        TIME_DETECT_WARNING_U_PREDELS,
                                                                        analog_protect.in_voltage[0].over_limit.bits.module_10_percent_low);

        edrk.warnings.e8.bits.U_IN_10_PROCENTS_LOW = pause_detect_error(&counter_in2_minus10,
                                                                         TIME_DETECT_WARNING_U_PREDELS,
                                                                         analog_protect.in_voltage[1].over_limit.bits.module_10_percent_low);

        edrk.warnings.e8.bits.U_IN_20_PROCENTS_LOW = pause_detect_error(&counter_in1_minus20,
                                                                        TIME_DETECT_WARNING_U_PREDELS,
                                                                        analog_protect.in_voltage[0].over_limit.bits.module_20_percent_low);

        edrk.warnings.e8.bits.U_IN_20_PROCENTS_LOW = pause_detect_error(&counter_in2_minus20,
                                                                         TIME_DETECT_WARNING_U_PREDELS,
                                                                         analog_protect.in_voltage[1].over_limit.bits.module_20_percent_low);

        edrk.errors.e9.bits.DISBALANCE_Uin_1 |= analog_protect.in_voltage[0].errors.bits.system_asymmetry;
        edrk.errors.e9.bits.DISBALANCE_Uin_2 |= analog_protect.in_voltage[1].errors.bits.system_asymmetry;

    }
    else
    {
        edrk.warnings.e8.bits.U_IN_10_PROCENTS_LOW = 0;
        edrk.warnings.e8.bits.U_IN_20_PROCENTS_LOW = 0;
    }

    //Выходные токи
    analog_protect.out_I[0].errors.all = 0;
    analog_protect.out_I[0].iqVal_U = analog.iqIu_1;
    analog_protect.out_I[0].iqVal_V = analog.iqIv_1;
    analog_protect.out_I[0].iqVal_W = analog.iqIw_1;
    analog_protect.out_I[0].iqVal_mod = analog.iqIm_1;
    analog_protect.out_I[0].break_phase = &break_Iout_1_state;
    analog_protect.out_I[0].iqTeta = teta_ch1;

//    analog_protect.out_I[0].calc(&analog_protect.out_I[0]);

    edrk.errors.e1.bits.I_UO2_MAX = analog_protect.out_I[0].errors.bits.phase_U_max;
    edrk.errors.e1.bits.I_UO3_MAX = analog_protect.out_I[0].errors.bits.phase_V_max;
    edrk.errors.e1.bits.I_UO4_MAX = analog_protect.out_I[0].errors.bits.phase_W_max;

    if (analog_protect.out_I[0].errors.bits.break_phase) {
        edrk.errors.e8.bits.LOSS_OUTPUT_U1 = analog_protect.out_I[0].errors.bits.break_phase_U;
        edrk.errors.e8.bits.LOSS_OUTPUT_V1 = analog_protect.out_I[0].errors.bits.break_phase_V;
        edrk.errors.e8.bits.LOSS_OUTPUT_W1 = analog_protect.out_I[0].errors.bits.break_phase_W;
    }

    analog_protect.out_I[1].errors.all = 0;
    analog_protect.out_I[1].iqVal_U = analog.iqIu_2;
    analog_protect.out_I[1].iqVal_V = analog.iqIv_2;
    analog_protect.out_I[1].iqVal_W = analog.iqIw_2;
    analog_protect.out_I[1].iqVal_mod = analog.iqIm_2;
    analog_protect.out_I[1].break_phase = &break_Iout_2_state;
    analog_protect.out_I[1].iqTeta = teta_ch2;

//    analog_protect.out_I[1].calc(&analog_protect.out_I[1]);

    edrk.errors.e1.bits.I_UO5_MAX = analog_protect.out_I[1].errors.bits.phase_U_max;
    edrk.errors.e1.bits.I_UO6_MAX = analog_protect.out_I[1].errors.bits.phase_V_max;
    edrk.errors.e1.bits.I_UO7_MAX = analog_protect.out_I[1].errors.bits.phase_W_max;
    if (analog_protect.out_I[1].errors.bits.break_phase) {
        edrk.errors.e8.bits.LOSS_OUTPUT_U2 = analog_protect.out_I[1].errors.bits.break_phase_U;
        edrk.errors.e8.bits.LOSS_OUTPUT_V2 = analog_protect.out_I[1].errors.bits.break_phase_V;
        edrk.errors.e8.bits.LOSS_OUTPUT_W2 = analog_protect.out_I[1].errors.bits.break_phase_W;
    }

    edrk.errors.e8.bits.DISBALANCE_IM1_IM2 |= _IQabs(analog.iqIm_1 - analog.iqIm_2) > LEVEL_I_1_2_DIBALANCE ? 1 : 0;

    //I zpt
    edrk.errors.e0.bits.I_1_MAX |= detect_error_Izpt();
    //Тормозные резисторы
    edrk.errors.e1.bits.I_BREAK_1_MAX |= detect_error_Ibreak(1);
    edrk.errors.e1.bits.I_BREAK_2_MAX |= detect_error_Ibreak(2);
}

int detect_error_Izpt() {
    return analog.iqIin_1 > analog_protect.iqI_zpt_level ? 1 : 0;
}

int detect_error_Ibreak(int res_num) {
    if (res_num == 1) {
        return analog.iqIbreak_1 > analog_protect.iqI_break_level ||
                analog.iqIbreak_1 < -analog_protect.iqI_break_level ? 1 : 0;
    } else if (res_num == 2) {
        return analog.iqIbreak_2 > analog_protect.iqI_break_level ||
                analog.iqIbreak_2 < -analog_protect.iqI_break_level ? 1 : 0;
    } else {
        return 0;
    }
}