/*

	ÖÍÈÈ ÑÝÒ	  (ñ) 2002 ã.
	
	Processor:			TMS320C32

	Filename:			vector_troll.h

	ÑÈÑÒÅÌÀ ÂÅÊÒÎÐÍÎÃÎ ÓÏÐÀÂËÅÍÈy

	Edit date:		04-12-02

	Function:		

	Revisions:
*/
#include "IQmathLib.h"

#include "DSP281x_Device.h"     // DSP281x Headerfile Include File
#include "DSP281x_Examples.h"   // DSP281x Examples Include File
#include <math.h>
#include "mathlib.h"

#include "params_norma.h"
#include "math_pi.h"
#include "params_pwm24.h"
#include <math.h>
#include "params_norma.h"

_iq SQRT_32 = _IQ(0.8660254037844);
_iq CONST_23 = _IQ(2.0/3.0);
_iq CONST_15 = _IQ(1.5);




#define real float

float my_satur_float(float Input, float Positive, float Negative, float DeadZone)
{
    if (fabs(DeadZone)>0.000001 && Input>-DeadZone && Input<DeadZone)
        Input = 0;
    else
        if (Input>=Positive) Input=Positive;
    else
        if (Input<=Negative) Input=Negative;

    return Input;
}

int my_satur_int(int Input, int Positive, int Negative, int DeadZone)
{
    if (DeadZone!=0 && Input>-DeadZone && Input<DeadZone)
        Input = 0;
    else
    if (Input>=Positive) Input=Positive;
    else
    if (Input<=Negative) Input=Negative;

    return Input;
}

long my_satur_long(long Input, long Positive, long Negative, long DeadZone)
{

    if (DeadZone!=0 && Input>-DeadZone && Input<DeadZone)
        Input = 0;
    else
    if (Input>=Positive) Input=Positive;
    else
    if (Input<=Negative) Input=Negative;

    return Input;
}
/*


real pid_regul(real Kp_regul, real Tperiod_regul, real Minimum, real Maximum,
               real yk, real *uk1, real *yk1, real *yzad, real *ek, real *ek1, real *ek2,
               real d0, real d1, real d2)
{
    real uk;
    
    
    *ek = - yk + *yzad;
    uk = *uk1 +  Kp_regul * ( d0 * *ek +  d1 * *ek1 + d2 * *ek2 );
    
    if (uk>Maximum) uk=Maximum;
    if (uk<Minimum) uk=Minimum;
    
    
    *yk1 = yk;
    *ek2 = *ek1;
    *ek1 = *ek;
    *uk1 = uk;
           
    return uk;    
}


real pid_regul2(real Kp_regul, real Tperiod_regul, real Minimum, real Maximum,
               real yk, real *uk1, real *yk1, real *yk2, real *yzad, 
               real d0, real d1, real d2)
{
    real uk;
    
    
    uk = *uk1 +  Kp_regul * ( *yk1 - yk + d1 * ( *yzad - yk ) + d2 * ( 2 * *yk1  - *yk2  - yk )  );
    
    
    if (uk>Maximum) uk=Maximum;
    if (uk<Minimum) uk=Minimum;
    
    *yk2 = *yk1;    
    *yk1 = yk;
    
    *uk1 = uk;
           
    return uk;    
}






real pi_regul(real Kp_regul, real Tintegral_regul, real Tperiod_regul, real Minimum, real Maximum, real InpVar, real *VarIntegral)
{
    real VarProp, VarOut;         
    *VarIntegral += InpVar*Tperiod_regul/Tintegral_regul;
    VarProp = InpVar*Kp_regul;
    
    if (*VarIntegral>Maximum) *VarIntegral=Maximum;
    if (*VarIntegral<Minimum) *VarIntegral=Minimum;
    VarOut = *VarIntegral+VarProp;
    if (VarOut>Maximum) VarOut=Maximum;
    if (VarOut<Minimum) VarOut=Minimum;
    return VarOut;
    
}


real exp_regul(real Tperiod_regul, real Texp_regul, real InpVarCurr, real InpVarInstant)
{
    real VarOut;
    
    VarOut = InpVarCurr + Tperiod_regul*(InpVarInstant-InpVarCurr)/Texp_regul;   
    
    return VarOut;    
}


                                   
                                   

*/

float zad_intensiv(float StepP, float StepN, float InpVarCurr, float InpVarInstant)
{
    float deltaVar, VarOut;
    
    deltaVar = InpVarInstant-InpVarCurr;
    
    if ((deltaVar>StepP) || (deltaVar<-StepN))
    {
      if (deltaVar>0) InpVarCurr += StepP;
      else InpVarCurr -= StepN;
    }
    else 
      InpVarCurr=InpVarInstant;

    
    VarOut = InpVarCurr;
    return VarOut;
}

#pragma CODE_SECTION(zad_intensiv_q,".fast_run");
_iq zad_intensiv_q(_iq StepP, _iq StepN, _iq InpVarCurr, _iq InpVarInstant)
{	
    _iq deltaVar, VarOut;
    
    deltaVar = InpVarInstant-InpVarCurr;
    
    if ((deltaVar>StepP) || (deltaVar<-StepN))
    {
      if (deltaVar>0) InpVarCurr += StepP;
      else InpVarCurr -= StepN;
    }
    else 
      InpVarCurr=InpVarInstant;

    
    VarOut = InpVarCurr;
    return VarOut;       


}




                                   

real pi_regul3(real Kp_regul, real ki_regul, real Tperiod_regul, real Minimum, real Maximum,
               real InpVar, real *InpVarPrev,  real *OutVarPrev)
{
    real  VarOut;
    
                         
    VarOut = Kp_regul*(ki_regul/Kp_regul*Tperiod_regul/2.0+1)*InpVar + Kp_regul*(ki_regul/Kp_regul*Tperiod_regul/2.0-1) * (*InpVarPrev) + *OutVarPrev;
                             
    if (VarOut>Maximum) VarOut=Maximum;
    if (VarOut<Minimum) VarOut=Minimum;
    
    
    *InpVarPrev = InpVar;
    *OutVarPrev = VarOut;
    
    
    return VarOut;
    
}

 

real pi_regul4(real Kp_regul, real ki_regul, real Tperiod_regul, real Minimum, real Maximum,
               real InpVar, real *InpVarPrev,  real *OutVarPrev)
{
    real  VarOut;
    
                         
    VarOut = Kp_regul*InpVar + Kp_regul*(ki_regul/Kp_regul*Tperiod_regul-1) * (*InpVarPrev) + *OutVarPrev;
                             
    if (VarOut>Maximum) VarOut=Maximum;
    if (VarOut<Minimum) VarOut=Minimum;
        
    *InpVarPrev = InpVar;
    *OutVarPrev = VarOut;
    
    
    return VarOut;
    
}


/********************************************************************/    
/* Ðàñ÷åò ìîäóëy òîêà èç ïîêàçàíèé òðåõ ôàç */                                
/********************************************************************/
#pragma CODE_SECTION(im_calc,".fast_run");
_iq im_calc( _iq ia, _iq ib, _iq ic)
{
  _iq isa,isb, t;
  

  isa  = _IQmpy(CONST_15,ia);
  isb  =  _IQmpy(SQRT_32,ib-ic );
  
//  ( _IQ19mpy(SQRT_32, _IQ15toIQ19(ib)) -  _IQ15mpy(SQRT_32, _IQ15toIQ19(ic)) );

  t = _IQmag(isa,isb);
  t = _IQmpy(CONST_23,t);

  return  (t);

}

float exp_regul(float Tperiod_regul, float Texp_regul, float InpVarCurr, float InpVarInstant)
{
    float VarOut;
    
    VarOut = InpVarCurr + Tperiod_regul*(InpVarInstant-InpVarCurr)/Texp_regul;   
    
    return VarOut;    
}

_iq calc_rms_array_simple(RMS_CALC_ARRAY *v) {
    _iq summ_squares = 0;
    int i = 0;
    if (v->data_array == 0) return 0;
    for (i = 0; i < v->size_array; ++i) {
        summ_squares += _IQmpy(v->data_array[i], v->data_array[i]);
    }
    return _IQsqrt(summ_squares / v->size_array);
}

//_iq calc_rms_array_var_period(RMS_CALC_ARRAY_THINNING *v) {
//    _iq summ_squares = 0;
//    int i = 0;
//    int count_elem = 0;
//    if (v->signal_period > v->size_array) {
//        v->signal_period = v->size_array;
//    }
//    count_elem = v->signal_period;
//    i = v->last_elem_position;
//    while (count_elem > 0) {
//        summ_squares += _IQmpy(v->data_array[i], v->data_array[i]);
//        i -= 1;
//        if (i < 0) { i = v->size_array - 1; }
//        count_elem -= 1;
//
//    }
//    v->Out_rms = _IQsqrt(summ_squares / v->signal_period);
//    return v->Out_rms;
//}

_iq calc_rms_array_var_period_IQ15(RMS_CALC_ARRAY_THINNING *v) {
    _iq16 summ_squares = 0;
    int i = 0;
    int count_elem = 0;
    if (v->data_array == 0) {
        return 0;
    }
    if (v->signal_period > v->size_array) {
        v->signal_period = v->size_array;
    }
    count_elem = v->signal_period;
    i = v->last_elem_position;
    while (count_elem > 0) {
        summ_squares += 0;//_IQ15mpy(v->data_array[i], v->data_array[i]);
        i -= 1;
        if (i < 0) { i = v->size_array - 1; }
        count_elem -= 1;

    }
    v->Out_rms =_IQ15toIQ(_IQ15sqrt(summ_squares / v->signal_period));
    return v->Out_rms;
}

float fast_round(float x)
{
    float d;
    long i;


    i = (long)x;

    if (x<0)
    {
      d = i - x;
      if (d>=0.5)
          i = i - 1;
    }
    else
    {
       d = x - i;
       if (d>=0.5)
           i = i + 1;
    }
    return (float)i;

}

float fast_round_with_delta(float prev, float x, float delta)
{
    float d;
    long i;


    i = (long)x;

    if (x<0)
    {
      d = i - x;
      if (d>=0.5)
          i = i - 1;
    }
    else
    {
       d = x - i;
       if (d>=0.5)
           i = i + 1;
    }

    if (fabs(prev-x)>=delta)
        return (float)i;
    else
        return (float)prev;

}


float fast_round_with_limiter(float x, float lim)
{
    float d;
    long i;


    if (fabs(x)<=lim)
        return 0;

    i = (long)x;

    if (x<0)
    {
      d = i - x;
      if (d>=0.5)
          i = i - 1;
    }
    else
    {
       d = x - i;
       if (d>=0.5)
           i = i + 1;
    }
    return (float)i;

}


#pragma CODE_SECTION(calc_rms,".fast_run");
_iq calc_rms(_iq input, _iq input_prev, _iq freq_signal)
{
    static _iq pi_pwm = _IQ(PI*NORMA_FROTOR/(FREQ_PWM/5.0));

    _iq cosw, sinw, wdt, y2, z1, z2, z3, y;

    wdt = _IQmpy(pi_pwm,freq_signal);
    sinw = _IQsinPU(wdt);
    cosw = _IQcosPU(wdt);

    if (cosw==0)
        return 0;

    z1 = input_prev - _IQmpy(input,cosw);
//    z2 = sinw;
    z3 = _IQdiv(z1,sinw);

    y2 = _IQmpy(input,input)+_IQmpy(z3,z3);

//    cosw = _IQsin();

    y  = _IQsqrt(y2);
    return y;
}