/*
 * calc_rms_vals.c
 *
 *  Created on: 14 дек. 2020 г.
 *      Author: star
 */
#include <adc_tools.h>
#include <calc_rms_vals.h>
#include <edrk_main.h>
#include <params_norma.h>
#include <params_pwm24.h>

#include "IQmathLib.h"
#include "mathlib.h"

#define NUM_OF_CHANNELS 6

//#define USE_CALC_U_IN_RMS 1
//#define USE_CALC_I_OUT_RMS 1


#pragma DATA_SECTION(in_U_rms_calc_buffer,".slow_vars")
RMS_BUFFER in_U_rms_calc_buffer[NUM_OF_CHANNELS] = {RMS_BUFFER_DEFAULTS, RMS_BUFFER_DEFAULTS, \
                                      RMS_BUFFER_DEFAULTS, RMS_BUFFER_DEFAULTS, \
                                      RMS_BUFFER_DEFAULTS, RMS_BUFFER_DEFAULTS};

#pragma DATA_SECTION(out_I_rms_calc_buffer,".slow_vars")
RMS_BUFFER_WITH_THINNING out_I_rms_calc_buffer[6] = {RMS_BUFFER_WITH_THINNING_DEFAULTS, RMS_BUFFER_WITH_THINNING_DEFAULTS, \
                                                     RMS_BUFFER_WITH_THINNING_DEFAULTS, RMS_BUFFER_WITH_THINNING_DEFAULTS, \
                                                     RMS_BUFFER_WITH_THINNING_DEFAULTS, RMS_BUFFER_WITH_THINNING_DEFAULTS};

///////////////////////////////////////////////////
void init_Uin_rms(void)
{
    int k,l;
    // чистка буфера нулями и не только
    for (k=0;k<NUM_OF_CHANNELS;k++)
    {
        for (l=0;l<RMS_BUFFER_SIZE;l++)
            in_U_rms_calc_buffer[k].values[l] = 0;
        in_U_rms_calc_buffer[k].position = 0;
        in_U_rms_calc_buffer[k].array_size = RMS_BUFFER_SIZE;
    }
}
///////////////////////////////////////////////////

void  add_to_buff(RMS_BUFFER *b, _iq val) {
    b->values[b->position++] = val;
    if (b->position >= b->array_size) { b->position = 0;}
}

#define add_to_buff_def(b, val) { (b)->values[(b)->position++] = (val); if ((b)->position >= (b)->array_size) { (b)->position = 0;}}

static void calc_Uin_rms(void);
static void calc_Iout_rms(void);

#define CONST_PI    52707178

//#pragma CODE_SECTION(fill_rms_array_IQ15,".fast_run1");
void fill_rms_array_IQ15(RMS_BUFFER_WITH_THINNING *v) {
    static _iq norma_f_rot = NORMA_FROTOR;
    int period_by_teta = 0;
//    int freq = _IQtoF(v->signal_freq) * NORMA_FROTOR;
//    int freq = _IQtoF(v->signal_freq * norma_f_rot);    //Max IQ24 = 127. Частота в Гц обычно меньше
    int freq = (v->signal_freq * norma_f_rot) / (1LL << GLOBAL_Q);    //В 10 раз быстрее, меньше точность
    int count_miss_writing = 100;
    if (freq != 0) {
        freq *= v->elements_in_period;  //"оптимизация"
        count_miss_writing = (int)(v->freq_pwm / freq);
        if (count_miss_writing > 100) {count_miss_writing = 100;}
    }

    if (v->use_teta) {
        v->internal.teta_period_counter += 1;
            if ((v->teta < CONST_PI && v->internal.teta_prev > CONST_PI) ||
                    (v->teta > CONST_PI && v->internal.teta_prev < CONST_PI)) {
                v->internal.zero_teta_period = v->internal.teta_period_counter;
                v->internal.teta_period_counter = 0;
            }
            v->internal.teta_prev = v->teta;
            period_by_teta = v->internal.zero_teta_period / v->elements_in_period;
            if (period_by_teta != 0) {
                count_miss_writing = (count_miss_writing + period_by_teta) / 2;
            }
    }


    if (v->internal.miss_write_counter++ < count_miss_writing) {
        return;
    }

    v->internal.miss_write_counter = 0;
    v->values[v->position++] = _IQtoIQ15(v->val);
    if (v->position >= v->array_size) {
        v->position = 0;
    }
}

void fill_RMS_buff_interrupt(_iq teta_ch1, _iq teta_ch2) {
#ifdef USE_CALC_U_IN_RMS
//    add_to_buff(&in_U_rms_calc_buffer[0], analog.iqUin_A1B1);
//    add_to_buff(&in_U_rms_calc_buffer[1], analog.iqUin_B1C1);
//    add_to_buff(&in_U_rms_calc_buffer[2], analog.iqUin_C1A1);
//    add_to_buff(&in_U_rms_calc_buffer[3], analog.iqUin_A2B2);
//    add_to_buff(&in_U_rms_calc_buffer[4], analog.iqUin_B2C2);
//    add_to_buff(&in_U_rms_calc_buffer[5], analog.iqUin_C2A2);
//
    add_to_buff_def(&in_U_rms_calc_buffer[0], analog.iqUin_A1B1);   //define в 10 раз быстрее
    add_to_buff_def(&in_U_rms_calc_buffer[1], analog.iqUin_B1C1);
    add_to_buff_def(&in_U_rms_calc_buffer[2], analog.iqUin_C1A1);
    add_to_buff_def(&in_U_rms_calc_buffer[3], analog.iqUin_A2B2);
    add_to_buff_def(&in_U_rms_calc_buffer[4], analog.iqUin_B2C2);
    add_to_buff_def(&in_U_rms_calc_buffer[5], analog.iqUin_C2A2);
#endif //USE_CALC_U_IN_RMS
#ifdef USE_CALC_I_OUT_RMS
    out_I_rms_calc_buffer[0].val = analog.iqIu_1;
    out_I_rms_calc_buffer[0].teta = teta_ch1;
    out_I_rms_calc_buffer[0].freq_pwm = FREQ_PWM * 2;
    out_I_rms_calc_buffer[0].signal_freq = edrk.f_stator;
    out_I_rms_calc_buffer[0].add_value(&out_I_rms_calc_buffer[0]);
    out_I_rms_calc_buffer[1].val = analog.iqIv_1;
    out_I_rms_calc_buffer[1].teta = teta_ch1;
    out_I_rms_calc_buffer[1].freq_pwm = FREQ_PWM * 2;
    out_I_rms_calc_buffer[1].signal_freq = edrk.f_stator;
    out_I_rms_calc_buffer[1].add_value(&out_I_rms_calc_buffer[1]);
    out_I_rms_calc_buffer[2].val = analog.iqIw_1;
    out_I_rms_calc_buffer[2].teta = teta_ch1;
    out_I_rms_calc_buffer[2].freq_pwm = FREQ_PWM * 2;
    out_I_rms_calc_buffer[2].signal_freq = edrk.f_stator;
    out_I_rms_calc_buffer[2].add_value(&out_I_rms_calc_buffer[2]);

    out_I_rms_calc_buffer[3].val = analog.iqIu_1;
    out_I_rms_calc_buffer[3].teta = teta_ch2;
    out_I_rms_calc_buffer[3].freq_pwm = FREQ_PWM * 2;
    out_I_rms_calc_buffer[3].signal_freq = edrk.f_stator;
    out_I_rms_calc_buffer[3].add_value(&out_I_rms_calc_buffer[3]);
    out_I_rms_calc_buffer[4].val = analog.iqIu_1;
    out_I_rms_calc_buffer[4].teta = teta_ch2;
    out_I_rms_calc_buffer[4].freq_pwm = FREQ_PWM * 2;
    out_I_rms_calc_buffer[4].signal_freq = edrk.f_stator;
    out_I_rms_calc_buffer[4].add_value(&out_I_rms_calc_buffer[4]);
    out_I_rms_calc_buffer[5].val = analog.iqIu_1;
    out_I_rms_calc_buffer[5].teta = teta_ch2;
    out_I_rms_calc_buffer[5].freq_pwm = FREQ_PWM * 2;
    out_I_rms_calc_buffer[5].signal_freq = edrk.f_stator;
    out_I_rms_calc_buffer[5].add_value(&out_I_rms_calc_buffer[5]);
#endif //USE_CALC_I_OUT_RMS
}


void calc_Uin_rms(void) {

    RMS_CALC_ARRAY rc = RMS_CALC_DEFAULTS;

    rc.size_array = in_U_rms_calc_buffer[0].array_size;
    rc.data_array = in_U_rms_calc_buffer[0].values;
    analog.iqUin_A1B1_rms = rc.calc(&rc);
    rc.size_array = in_U_rms_calc_buffer[1].array_size;
    rc.data_array = in_U_rms_calc_buffer[1].values;
    analog.iqUin_B1C1_rms = rc.calc(&rc);
    rc.size_array = in_U_rms_calc_buffer[2].array_size;
    rc.data_array = in_U_rms_calc_buffer[2].values;
    analog.iqUin_C1A1_rms = rc.calc(&rc);

    rc.size_array = in_U_rms_calc_buffer[3].array_size;
    rc.data_array = in_U_rms_calc_buffer[3].values;
    analog.iqUin_A2B2_rms = rc.calc(&rc);
    rc.size_array = in_U_rms_calc_buffer[4].array_size;
    rc.data_array = in_U_rms_calc_buffer[4].values;
    analog.iqUin_B2C2_rms = rc.calc(&rc);
    rc.size_array = in_U_rms_calc_buffer[5].array_size;
    rc.data_array = in_U_rms_calc_buffer[5].values;
    analog.iqUin_C2A2_rms = rc.calc(&rc);

}


void calc_Iout_rms() {
    int i = 0;
    _iq results[NUM_OF_CHANNELS];
    RMS_CALC_ARRAY_THINNING rc_Iout = RMS_CALC_THINNING_DEFAULTS;

    //Calc rms
    for (i = 0; i < NUM_OF_CHANNELS; i++) {
        rc_Iout.data_array = out_I_rms_calc_buffer[i].values;
        rc_Iout.last_elem_position = out_I_rms_calc_buffer[i].position;
        rc_Iout.size_array = out_I_rms_calc_buffer[i].array_size;
        rc_Iout.signal_period = out_I_rms_calc_buffer[i].elements_in_period;
        results[i] = rc_Iout.calc(&rc_Iout);
    }
    analog.iqIu_1_rms = results[0];
    analog.iqIv_1_rms = results[1];
    analog.iqIw_1_rms = results[2];
    analog.iqIu_2_rms = results[3];
    analog.iqIv_2_rms = results[4];
    analog.iqIw_2_rms = results[5];
}

void calc_RMS_values_main() {
#ifdef USE_CALC_U_IN_RMS
    calc_Uin_rms();
#endif
//    i_led2_off();
//    i_led2_on();
#ifdef USE_CALC_I_OUT_RMS
    calc_Iout_rms();
#endif
}

//float signal_amplitude = 141;
//_iq data_arr[RMS_BUFFER_SIZE];
//_iq16 data_arr_IQ16[RMS_BUFFER_SIZE];
//_iq position = 0;
//_iq result_simple = 0;
//_iq result_simple_t = 0;
//
//RMS_BUFFER_WITH_THINNING buffer_var_freq = RMS_BUFFER_WITH_THINNING_DEFAULTS;
//int freq_mian_signal = 1;
//int counter_wait_change_freq = 0;
//#define WAIT_CHANGE_FREQ_TICS 2000
/*
void test_calc_rms (_iq teta) {
    _iq amplitude = _IQ (signal_amplitude / NORMA_ACP);
    _iq current_val = 0;
    _iq add_var_freq = 0;
    static _iq add_50hz = _IQ(6.28 * 50.0 / FREQ_PWM / 2.0);
    static _iq teta_50hz = 0, teta_Low = 0;
    RMS_CALC_ARRAY rc = RMS_CALC_DEFAULTS;

    RMS_CALC_ARRAY_THINNING rct = RMS_CALC_THINNING_DEFAULTS;

    current_val = _IQmpy(amplitude, _IQcos(teta_50hz));
    data_arr[position] = current_val;
    data_arr_IQ16[position] = _IQtoIQ16(current_val);
    position += 1;
    if (position >= RMS_BUFFER_SIZE) position = 0;
    teta_50hz += add_50hz;
    rc.data_array = data_arr;
    rc.size_array = RMS_BUFFER_SIZE;
    result_simple = rc.calc(&rc);

    if (counter_wait_change_freq >= WAIT_CHANGE_FREQ_TICS) {
        if (freq_mian_signal < 20) { freq_mian_signal +=1;}
        counter_wait_change_freq = 0;
    } else {
        counter_wait_change_freq += 1;
    }

    add_var_freq = _IQ(6.28 * freq_mian_signal / FREQ_PWM / 2.0);
    teta_Low += add_var_freq;
    current_val = _IQmpy(amplitude, _IQcos(teta_Low));
    buffer_var_freq.val = current_val;
    buffer_var_freq.signal_freq = _IQ((float)freq_mian_signal / NORMA_FROTOR);
    buffer_var_freq.teta = teta_Low;
    buffer_var_freq.add_value(&buffer_var_freq);



    rct.data_array = buffer_var_freq.values;
    rct.size_array = buffer_var_freq.array_size;
    rct.last_elem_position = buffer_var_freq.position;
    rct.signal_period = buffer_var_freq.array_size;
    result_simple_t = rct.calc(&rct);
}
*/