/*
 * vector_control.c
 *
 *  Created on: 16 íîÿá. 2020 ã.
 *      Author: star
 */
#include "IQmathLib.h"

#include "vector_control.h"

#include <adc_tools.h>
#include <edrk_main.h>
#include <master_slave.h>
#include <params_motor.h>
#include <params_norma.h>
#include <params_pwm24.h>
#include "math.h"
#include "mathlib.h"
#include "filter_v1.h"
#include "abc_to_dq.h"
#include "regul_power.h"
#include "regul_turns.h"
#include "teta_calc.h"

//#define CALC_TWO_WINDINGS

#define I_ZERO_LEVEL_IQ 27962   // 111848 ~ 20A   //279620 ~ 50A //55924 ~ 10A


#pragma DATA_SECTION(vect_control,".fast_vars");
VECTOR_CONTROL vect_control = VECTOR_CONTROL_DEFAULTS;


void Idq_to_Udq_2_windings(_iq Id_zad, _iq Iq_zad,
           _iq Id_measured1, _iq Iq_measured1, _iq* Ud_zad1, _iq* Uq_zad1,
           _iq Id_measured2, _iq Iq_measured2, _iq* Ud_zad2, _iq* Uq_zad2, int reset);
void analog_Udq_calc(_iq Ud1, _iq Uq1, _iq Ud2, _iq Uq2);
void analog_dq_calc(_iq winding_displacement);
_iq calcId(_iq Iq_limit, _iq Iq, int reset, int prepare_stop_PWM);
inline void calcUdUqCompensation(_iq Frot);

void initVectorControl() {
    vect_control.pidD1.Kp = _IQ(0.3);//_IQ(0.2);
    vect_control.pidD1.Ki = _IQ(0.01);//_IQ(0.9);
    vect_control.pidD1.OutMax = _IQ(0.9);
    vect_control.pidD1.OutMin = -_IQ(0.9);
    vect_control.pidQ1.Kp = _IQ(0.3);
    vect_control.pidQ1.Ki = _IQ(0.01);
    vect_control.pidQ1.OutMax = _IQ(0.9);
    vect_control.pidQ1.OutMin = -_IQ(0.9);
    vect_control.pidD2.Kp = _IQ(0.3);
    vect_control.pidD2.Ki = _IQ(0.3);
    vect_control.pidD2.OutMax = _IQ(0.9);
    vect_control.pidD2.OutMin = -_IQ(0.9);
    vect_control.pidQ2.Kp = _IQ(0.3);
    vect_control.pidQ2.Ki = _IQ(0.3);
    vect_control.pidQ2.OutMax = _IQ(0.9);
    vect_control.pidQ2.OutMin = -_IQ(0.9);

//    vect_control.iqId_nominal = _IQ(MOTOR_CURRENT_NOMINAL * sqrtf(1 - COS_FI * COS_FI) / NORMA_ACP);
    vect_control.iqId_nominal = _IQ(MOTOR_CURRENT_NOMINAL * 0.4 / NORMA_ACP);
    vect_control.iqId_min = _IQ(200 / NORMA_ACP);
    vect_control.iqId_start = _IQ(200.0 / NORMA_ACP);
    vect_control.koef_rmp_Id = (_iq)(vect_control.iqId_nominal / FREQ_PWM);
    vect_control.koef_filt_I = _IQ(0.5);


    vect_control.koef_Ud_comp = _IQ((L_SIGMA_S + L_M * L_SIGMA_R / (L_M + L_SIGMA_R)) * 2 * 3.14 * NORMA_FROTOR);  //Lsigm_s + Lm*Lsigm_r / (Lm + Lsigm_r)
    vect_control.koef_Uq_comp = _IQ((L_M + L_SIGMA_S) * 2 * 3.14 * NORMA_FROTOR);  //Lm + Lsigm_s
//    vect_control.koef_Ud_comp = _IQ(0.0002369 * 2 * 3.14 * NORMA_FROTOR);  //Lsigm_s + Lm*Lsigm_r / (Lm + Lsigm_r)
//    vect_control.koef_Uq_comp = _IQ(0.0043567 * 2 * 3.14 * NORMA_FROTOR);  //Lm + Lsigm_s
    vect_control.koef_zero_Uzad = _IQ(0.993);   //_IQ(0.993);    //_IQ(0.03);
    init_Pvect();
    init_Fvect();
    init_teta_calc_struct();
}

void vectorControlConstId (_iq Pzad, _iq Fzad, int direction, _iq Frot,
						   int n_alg, unsigned int master, _iq mzz_zad,
						   _iq winding_displacement,
						   _iq theta_from_master, _iq Iq_from_master, _iq P_from_slave,
						   _iq *theta_to_slave, _iq *Iq_to_slave, _iq *P_to_master,
						   int reset, int prepare_stop_PWM) {
    _iq Iq_zad = 0, Iq_zad_power = 0, Id_zad = 0;
    _iq P_measured = 0;
    static _iq Ud_zad1 = 0, Uq_zad1 = 0, Ud_zad2 = 0, Uq_zad2 = 0;

//    if(direction < 0) { Frot = -Frot; }

    if (reset) {
        Ud_zad1 = 0;
        Uq_zad1 = 0;
        Ud_zad2 = 0;
        Uq_zad2 = 0;
    }
    analog_dq_calc(winding_displacement);

    P_measured = vect_control.iqPvsi1 + vect_control.iqPvsi2;
    *P_to_master = P_measured;
    P_measured += P_from_slave;



    vector_power(Pzad, P_measured, n_alg, master, (vect_control.iqIq1 + vect_control.iqIq2),
                 edrk.zadanie.iq_Izad_rmp, &Iq_zad_power, reset);
    vector_turns(Fzad, Frot, (vect_control.iqIq1 + vect_control.iqIq2),
                 Iq_zad_power, n_alg, master, &Iq_zad, reset);

    Id_zad = calcId(edrk.zadanie.iq_Izad,  Iq_zad, reset, prepare_stop_PWM);

    if (master == MODE_SLAVE) {
        vect_control.iqTheta = theta_from_master;
        *theta_to_slave = theta_from_master;
        Iq_zad = Iq_from_master;
        Iq_zad_power = Iq_from_master;
    } else {
//        calc_teta_Id(Frot, vect_control.iqId1, vect_control.iqIq1, &vect_control.iqTheta, theta_to_slave,
//                        &vect_control.iqFsl, &vect_control.iqFstator, master, reset);
        calc_teta_Id(Frot, Id_zad, Iq_zad, &vect_control.iqTheta, theta_to_slave,
                                &vect_control.iqFsl, &vect_control.iqFstator, master, reset);
    }

    calcUdUqCompensation(Frot);

    if (prepare_stop_PWM && Id_zad == 0) {
        vect_control.iqUdKm = _IQmpy(vect_control.iqUdKm, vect_control.koef_zero_Uzad);
        vect_control.iqUqKm = _IQmpy(vect_control.iqUqKm, vect_control.koef_zero_Uzad);
    } else {
        Idq_to_Udq_2_windings((Id_zad >> 1), (Iq_zad >> 1),
                              vect_control.iqId1, vect_control.iqIq1, &Ud_zad1, &Uq_zad1,
                              vect_control.iqId2, vect_control.iqIq2, &Ud_zad2, &Uq_zad2, reset);

        vect_control.iqUdKm = Ud_zad1 + vect_control.iqUdCompensation;
        vect_control.iqUqKm = Uq_zad1 + vect_control.iqUqCompensation;
    }

    vect_control.sqrtIdq1 = _IQsqrt(_IQmpy(vect_control.iqId1, vect_control.iqId1) + _IQmpy(vect_control.iqIq1, vect_control.iqIq1));
    analog_Udq_calc(Ud_zad1, Uq_zad1, Ud_zad2, Uq_zad2);
    *Iq_to_slave = Iq_zad;

    vect_control.Iq_zad1 = Iq_zad;
    vect_control.Id_zad1 = Id_zad;

}


#pragma CODE_SECTION(analog_dq_calc,".fast_run");
void analog_dq_calc(_iq winding_displacement)
{
    ABC_TO_DQ abc_dq_converter = ABC_TO_DQ_DEFAULTS;

    abc_dq_converter.Ia = analog.iqIu_1;
    abc_dq_converter.Ib = analog.iqIv_1;
    abc_dq_converter.Ic = analog.iqIw_1;
    abc_dq_converter.Tetta = vect_control.iqTheta + winding_displacement;
    abc_dq_converter.calc(&abc_dq_converter);
    vect_control.iqId1 = abc_dq_converter.Id;
    vect_control.iqIq1 = abc_dq_converter.Iq;
    vect_control.iqId1_filt = exp_regul_iq(vect_control.koef_filt_I, vect_control.iqId1_filt, vect_control.iqId1);
    vect_control.iqIq1_filt = exp_regul_iq(vect_control.koef_filt_I, vect_control.iqIq1_filt, vect_control.iqIq1);

    abc_dq_converter.Ia = analog.iqIu_2;
    abc_dq_converter.Ib = analog.iqIv_2;
    abc_dq_converter.Ic = analog.iqIw_2;
    abc_dq_converter.Tetta = vect_control.iqTheta + winding_displacement;
    abc_dq_converter.calc(&abc_dq_converter);
    vect_control.iqId2 = abc_dq_converter.Id;
    vect_control.iqIq2 = abc_dq_converter.Iq;
    vect_control.iqId2_filt = exp_regul_iq(vect_control.koef_filt_I, vect_control.iqId2_filt, vect_control.iqId2);
    vect_control.iqIq2_filt = exp_regul_iq(vect_control.koef_filt_I, vect_control.iqIq2_filt, vect_control.iqIq2);


    if (_IQabs(vect_control.iqId1) < I_ZERO_LEVEL_IQ) { vect_control.iqId1 = 0; }
    if (_IQabs(vect_control.iqIq1) < I_ZERO_LEVEL_IQ) { vect_control.iqIq1 = 0; }
    if (_IQabs(vect_control.iqId2) < I_ZERO_LEVEL_IQ) { vect_control.iqId2 = 0; }
    if (_IQabs(vect_control.iqIq2) < I_ZERO_LEVEL_IQ) { vect_control.iqIq2 = 0; }

    vect_control.iqPvsi1 = _IQmpy(_IQmpy(vect_control.iqIq1, _IQabs(vect_control.iqUq1)), 25165824L);
    vect_control.iqPvsi2 = _IQmpy(_IQmpy(vect_control.iqIq2, _IQabs(vect_control.iqUq2)), 25165824L);

}

#pragma CODE_SECTION(analog_dq_calc_external,".fast_run");
void analog_dq_calc_external(_iq winding_displacement, _iq theta)
{
    ABC_TO_DQ abc_dq_converter = ABC_TO_DQ_DEFAULTS;

    abc_dq_converter.Ia = analog.iqIu_1;
    abc_dq_converter.Ib = analog.iqIv_1;
    abc_dq_converter.Ic = analog.iqIw_1;
    abc_dq_converter.Tetta = theta + winding_displacement;
    abc_dq_converter.calc(&abc_dq_converter);
    vect_control.iqId1 = abc_dq_converter.Id;
    vect_control.iqIq1 = abc_dq_converter.Iq;


    abc_dq_converter.Ia = analog.iqIu_2;
    abc_dq_converter.Ib = analog.iqIv_2;
    abc_dq_converter.Ic = analog.iqIw_2;
    abc_dq_converter.Tetta = theta + winding_displacement;
    abc_dq_converter.calc(&abc_dq_converter);
    vect_control.iqId2 = abc_dq_converter.Id;
    vect_control.iqIq2 = abc_dq_converter.Iq;

    if (_IQabs(vect_control.iqId1) < I_ZERO_LEVEL_IQ) { vect_control.iqId1 = 0; }
    if (_IQabs(vect_control.iqIq1) < I_ZERO_LEVEL_IQ) { vect_control.iqIq1 = 0; }
    if (_IQabs(vect_control.iqId2) < I_ZERO_LEVEL_IQ) { vect_control.iqId2 = 0; }
    if (_IQabs(vect_control.iqIq2) < I_ZERO_LEVEL_IQ) { vect_control.iqIq2 = 0; }

    vect_control.iqPvsi1 = _IQmpy(_IQmpy(vect_control.iqIq1, _IQabs(vect_control.iqUq1)), 25165824L);
    vect_control.iqPvsi2 = _IQmpy(_IQmpy(vect_control.iqIq2, _IQabs(vect_control.iqUq2)), 25165824L);

}

void Idq_to_Udq_2_windings(_iq Id_zad, _iq Iq_zad,
                           _iq Id_measured1, _iq Iq_measured1, _iq* Ud_zad1, _iq* Uq_zad1,
                           _iq Id_measured2, _iq Iq_measured2, _iq* Ud_zad2, _iq* Uq_zad2, int reset)
{
    if (reset) {
        vect_control.pidD1.Ui = 0;
        vect_control.pidD1.Out = 0;
        vect_control.pidQ1.Ui = 0;
        vect_control.pidQ1.Out = 0;
#ifdef CALC_TWO_WINDINGS
        vect_control.pidD2.Ui = 0;
        vect_control.pidD2.Out = 0;
        vect_control.pidQ2.Ui = 0;
        vect_control.pidQ2.Out = 0;
#endif
    }
    vect_control.pidD1.Ref = Id_zad;
    vect_control.pidD1.Fdb = Id_measured1;
    vect_control.pidD1.calc(&vect_control.pidD1);
    *Ud_zad1 = vect_control.pidD1.Out;

    vect_control.pidQ1.Ref = Iq_zad;
    vect_control.pidQ1.Fdb = Iq_measured1;
    vect_control.pidQ1.calc(&vect_control.pidQ1);
    *Uq_zad1 = vect_control.pidQ1.Out;
#ifdef CALC_TWO_WINDINGS
    vect_control.pidD2.Ref = Id_zad;
    vect_control.pidD2.Fdb = Id_measured2;
    vect_control.pidD2.calc(&vect_control.pidD2);
    *Ud_zad2 = vect_control.pidD2.Out;

    vect_control.pidQ2.Ref = Iq_zad;
    vect_control.pidQ2.Fdb = Iq_measured2;
    vect_control.pidQ2.calc(&vect_control.pidQ2);
    *Uq_zad2 = vect_control.pidQ2.Out;
#else
    *Ud_zad2 = *Ud_zad1;
    *Uq_zad2 = *Ud_zad1;
//    *Uq_zad2 = *Uq_zad1;
#endif
}

#pragma CODE_SECTION(analog_Udq_calc,".fast_run");
void analog_Udq_calc(_iq Ud1, _iq Uq1, _iq Ud2, _iq Uq2)
{
    _iq Uzpt = filter.iqU_1_long + filter.iqU_2_long;
    vect_control.iqUd1 = _IQmpy(Ud1, _IQmpy(Uzpt, 8388608L));  // 8388608 = _IQ(0.5)
    vect_control.iqUq1 = _IQmpy(Uq1, _IQmpy(Uzpt, 8388608L));
    vect_control.iqUd2 = _IQmpy(Ud2, _IQmpy(Uzpt, 8388608L));
    vect_control.iqUq2 = _IQmpy(Uq2, _IQmpy(Uzpt, 8388608L));

}

_iq calcId(_iq Iq_limit, _iq Iq, int reset, int prepare_stop_PWM) {
    static _iq Id_rmp = 0;
    _iq Id_zad = 0;
    if (reset) {
        Id_rmp = 0;
    }


    if (prepare_stop_PWM) {
        Id_zad = 0;
    } else if (Iq <  vect_control.iqId_min) {
        Id_zad = vect_control.iqId_min;
    } else if (Iq > vect_control.iqId_nominal) {
        Id_zad = vect_control.iqId_nominal;
    } else  {
        Id_zad = Iq;
    }
//    Id_zad = Iq_limit < vect_control.iqId_nominal ? Iq_limit : vect_control.iqId_nominal;
    Id_rmp = zad_intensiv_q(vect_control.koef_rmp_Id, vect_control.koef_rmp_Id << 1, Id_rmp, Id_zad);
    return Id_rmp;
}

void calcUdUqCompensation(_iq Frot) {
    _iq Uzpt = (filter.iqU_1_long + filter.iqU_2_long) >> 1;
    _iq UdVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Ud_comp), vect_control.iqIq1 + vect_control.iqIq2);
    _iq UqVolt = _IQmpy(_IQmpy(Frot, vect_control.koef_Uq_comp), vect_control.iqId1 + vect_control.iqId2);
    vect_control.iqUdCompensation = -_IQdiv(UdVolt, Uzpt);
    vect_control.iqUqCompensation = _IQdiv(UqVolt, Uzpt);
}