#include "DSP281x_Device.h"
#include "IQmathLib.h"

#include "regul_turns.h"

#include <adc_tools.h>
#include <edrk_main.h>
#include <master_slave.h>
#include <params.h>
#include <params_motor.h>
#include <params_norma.h>
#include <params_pwm24.h>
#include "math.h"
#include "mathlib.h"
#include "pid_reg3.h"
#include "vector_control.h"

#pragma DATA_SECTION(turns,".fast_vars1");
TURNS turns = TURNS_DEFAULTS;

   //IQ_OUT_NOM    TODO:set Iq nominal

#define IQ_165_RPM 2306867		//165об/мин
#define IQ_170_RPM 2376772		//170об/мин
#define IQ_5_RPM 69905			//5об/мин

#define TIME_RMP_FAST 10.0	//sec
#define TIME_RMP_SLOW 30.0	//sec
#define F_DEST 3.0	//Hz

void init_Fvect()
{
    turns.pidFvect.Ref = 0;
    turns.pidFvect.Kp = _IQ(5); //	//_IQ(30);
    turns.pidFvect.Ki = _IQ(0.005);	//_IQ(0.008);//_IQ(0.002);	//
    turns.pidFvect.Kc = _IQ(0.5);
    turns.pidFvect.OutMax = _IQ(MOTOR_CURRENT_MAX / 2.0 / NORMA_ACP);
    turns.pidFvect.OutMin = -_IQ(MOTOR_CURRENT_MAX / 2.0 / NORMA_ACP);

    turns.Fzad_rmp = 0;
    turns.Fnominal = _IQ(FROT_MAX / NORMA_FROTOR);
    turns.koef_fast =
            _IQ(F_DEST / (float)NORMA_FROTOR / TIME_RMP_SLOW / (float)FREQ_PWM / 1.0);
    turns.koef_slow =
            _IQ(F_DEST / (float)NORMA_FROTOR / TIME_RMP_SLOW / (float)FREQ_PWM / 1.0);
    turns.Iq_out_max = _IQ(MOTOR_CURRENT_MAX / NORMA_ACP);
    turns.Id_out_max = 0;//_IQ(ID_OUT_NOM / NORMA_ACP);
    turns.mzz_zad_int = 0;
}

void reset_F_pid()
{
    turns.pidFvect.Up = 0;
    turns.pidFvect.Up1 = 0;
    turns.pidFvect.Ui = 0;
    turns.pidFvect.Ud = 0;
    turns.pidFvect.Out = 0;
}

//#pragma CODE_SECTION(vector_turns,".fast_run2");
void vector_turns(_iq Fzad, _iq Frot,
                  _iq Iq_measured, _iq Iq_limit, int n_alg,
                  unsigned int master, _iq *Iq_zad, int reset)
{
    static int prev_n_alg = 0;
	_iq koef_rmp;	//, koef_spad;
    _iq Iq_out_unsat, Iq_out_sat, Id_out_sat, Id_out_unsat;
    _iq deltaVar;

//    turns.mzz_zad_int = zad_intensiv_q(35480, 35480, turns.mzz_zad_int, Iq_limit);
    turns.mzz_zad_int = Iq_limit;

    turns.pidFvect.OutMax = turns.mzz_zad_int;
    turns.pidFvect.OutMin = -turns.mzz_zad_int;

    //Убираем режим торможения
    if (Fzad >= 0 && Frot > 0)
    {
        turns.pidFvect.OutMin = 0;
    }
    if (Fzad <= 0 && Frot < 0)
    {
        turns.pidFvect.OutMax = 0;
    }
    if (reset) { turns.Fzad_rmp = Frot;}

    if ((n_alg < ALG_MODE_FOC_OBOROTS) || (!edrk.Go))
    //Заносим текущее состояние в регул-тор, что бы при смене режима не было скачков
    {                       //
        turns.Fzad_rmp = Frot;
//		prev_Fzad = Frot;
        reset_F_pid(); //Было ниже, может быть что-то было не так
        turns.pidFvect.Ui = Iq_measured;
        turns.pidFvect.Out = Iq_measured;
        *Iq_zad = Iq_measured;

        if (!edrk.Go)
        {
            *Iq_zad = 0;
        }

        return;
    }
    if (master == MODE_SLAVE) {
    	turns.Fzad_rmp = Frot;
    	turns.pidFvect.Ui = Iq_measured;
    	turns.pidFvect.Out = Iq_measured;
    	return;
    }
    //В режиме поддержания мощности задаются максимальные допустимые обороты
    if (n_alg == ALG_MODE_FOC_POWER) {
        Fzad = turns.Fnominal;
    }
    //Для перехода их режима доддержания мощноси в режим поддержания оборотов
    //рампа пойдёт от текущего значения оборотов
    if (n_alg == ALG_MODE_FOC_OBOROTS && prev_n_alg != ALG_MODE_FOC_OBOROTS) {
    	turns.Fzad_rmp = Frot;
    }

    if (_IQabs(turns.Fzad_rmp) <= _IQabs(Fzad)
            && (((Fzad >= 0) && (turns.Fzad_rmp >= 0))
                    || ((Fzad < 0) && (turns.Fzad_rmp < 0))))
    {
        koef_rmp = turns.koef_slow;
    }
    else
    {
        koef_rmp = turns.koef_fast;
    }

    turns.Fzad_rmp = zad_intensiv_q(koef_rmp, koef_rmp, turns.Fzad_rmp, Fzad);

    turns.pidFvect.Ref = turns.Fzad_rmp;
    turns.pidFvect.Fdb = Frot;

    turns.pidFvect.calc(&turns.pidFvect);
    Iq_out_unsat = turns.pidFvect.Out;

    Iq_out_sat = _IQsat(Iq_out_unsat, turns.mzz_zad_int, -turns.mzz_zad_int);	//Test
    *Iq_zad = Iq_out_sat;

    prev_n_alg = n_alg;
}