#include "IQmathLib.h"

#include "teta_calc.h"

#include <master_slave.h>
#include <params_alg.h>
#include <params_motor.h>
#include <params_norma.h>
#include <params_pwm24.h>

#include "mathlib.h"
#include "pid_reg3.h"


#define CONST_IQ_2PI  105414357 
#define PI 3.1415926535897932384626433832795

#define MAX_Ud_Pid_Out_Id  176160   //0.2 ~ 167772 //0.21 ~ 176160
#define BPSI_START 0.17	//0.15

TETTA_CALC tetta_calc = TETTA_CALC_DEF;

void init_teta_calc_struct()
{
    float Tr_cm = (L_M + L_SIGMA_R) / (R_ROTOR_SHTRIH / SLIP_NOM);
//    tetta_calc.k_r = _IQ(1 / FREQ_PWM / Tr_cm);
//    tetta_calc.k_t = _IQ(R_ROTOR / (L_M + L_SIGMA_R) / 2.0 / 3.14 / 50 / NORMA_FROTOR);
    tetta_calc.k_r = _IQ(0.005168);    //_IQ(0.015);
    tetta_calc.k_t = _IQ(0.0074);   //_IQ(0.0045);
    tetta_calc.Imds = 0;
    tetta_calc.theta = 0;
    tetta_calc.hz_to_angle = _IQ(2.0 * PI * NORMA_FROTOR / FREQ_PWM);
}

//#pragma CODE_SECTION(calc_teta_Id,".fast_run");
void calc_teta_Id(_iq Frot, _iq Id, _iq Iq, _iq *theta_out, _iq *theta_to_slave, _iq *Fsl_out, _iq *Fstator_out,
					unsigned int master, int reset) {

    _iq Fsl, Fst;
    _iq add_to_tic = 0;
    _iq to_slave = 0;

    if (reset) {
        tetta_calc.Imds = 0;
    }

    tetta_calc.Imds = tetta_calc.Imds + _IQmpy(tetta_calc.k_r, (Id - tetta_calc.Imds));

    if (master == MODE_SLAVE){
    	return;
    }

    Fsl = _IQmpy(tetta_calc.k_t, Iq);
    if (tetta_calc.Imds != 0) {
        Fsl = _IQdiv(Fsl, tetta_calc.Imds);
    } else {
        Fsl = 0;
    }

    if (Fsl > MAX_Ud_Pid_Out_Id) { Fsl = MAX_Ud_Pid_Out_Id;}
    if (Fsl < -MAX_Ud_Pid_Out_Id) { Fsl = -MAX_Ud_Pid_Out_Id;}
//    if (Fsl < 0) { Fsl = 0;}

    Fst = Frot * POLUS + Fsl;
    add_to_tic = _IQmpy(Fst, tetta_calc.hz_to_angle);
    tetta_calc.theta += add_to_tic;
    to_slave = tetta_calc.theta + add_to_tic;

    if (tetta_calc.theta > CONST_IQ_2PI) {
        tetta_calc.theta -= CONST_IQ_2PI;
    } else if (tetta_calc.theta < 0) {
        tetta_calc.theta += CONST_IQ_2PI;
    }

    if (to_slave > CONST_IQ_2PI) {
    	to_slave -= CONST_IQ_2PI;
    } else if (to_slave < 0) {
    	to_slave += CONST_IQ_2PI;
    }

    *Fsl_out = Fsl;
    *theta_out = tetta_calc.theta;
    *theta_to_slave = to_slave;
    *Fstator_out = Fst;

//    logpar.log26 = (int16)(_IQtoIQ15(Fsl));
//	logpar.log27 = (int16)(_IQtoIQ15(tetta_calc.Imds));
//	logpar.log28 = (int16)(_IQtoIQ15(Iq));
//	logpar.log3 = (int16)(_IQtoIQ15(Id));
}