/*
 * C1_can.h
 *
 *  Created on: Aug 24, 2022
 *      Author: 210055498
 */

#define TWO_PI    6.283185307

#include "Func.h"
//#include "F28x_Project.h"
#include "math.h"

#pragma CODE_SECTION(abc2xy, ".TI.ramfunc");
#pragma CODE_SECTION(abc2xyz, ".TI.ramfunc");
#pragma CODE_SECTION(xy2abc, ".TI.ramfunc");
#pragma CODE_SECTION(xy2dq, ".TI.ramfunc");
#pragma CODE_SECTION(dq2xy, ".TI.ramfunc");
#pragma CODE_SECTION(svm, ".TI.ramfunc");
#pragma CODE_SECTION(HPFilt1stCoef, ".TI.ramfunc");
#pragma CODE_SECTION(DigFilt1, ".TI.ramfunc");
#pragma CODE_SECTION(VLPI, ".TI.ramfunc");
#pragma CODE_SECTION(VLPI, ".TI.ramfunc");
#pragma CODE_SECTION(Lookup1D1, ".TI.ramfuncs");

void abc2xy(struct abc* in, struct xy* out)
{
   out->x = 0.66666666* in->a - 0.3333333 * in->b - 0.3333333 * in->c;
   out->y = 0.57735027 * in->b - 0.57735027 * in->c;
}

void abc2xyz(struct abc* in, struct xyz* out)
{
   out->x = 0.66666666* in->a - 0.3333333 * in->b - 0.3333333 * in->c;
   out->y = 0.57735027 * in->b - 0.57735027 * in->c;
   out->z = 0.471404521 * (in->a + in->b + in->c);
}

void xy2abc(struct xy* in, struct abc* out)
{
   out->a = in->x;
   out->b = -0.5 * in->x + 0.86602540 * in->y;
   out->c = -0.5 * in->x - 0.86602540 * in->y;
}

void xy2dq(struct xy* in, struct dq* out, struct phase* Phase)
{
   out->d = in->x * Phase->Cos + in->y * Phase->Sin;
   out->q = in->y * Phase->Cos - in->x * Phase->Sin;
}

void xyz2dqo(struct xyz* in, struct dqo* out, struct phase* Phase)
{
   out->d = in->x * Phase->Cos + in->y * Phase->Sin;
   out->q = in->y * Phase->Cos - in->x * Phase->Sin;
   out->o = in->z;
}

void dq2xy(struct dq* in, struct xy* out, struct phase* Phase)
{
   out->x = in->d * Phase->Cos - in->q * Phase->Sin;
   out->y = in->d * Phase->Sin + in->q * Phase->Cos;
}

void LPFilt1stCoef(struct Dfilt1* fpt, float fc, float Tsmp)
{
   register float tmp = fc * TWO_PI * Tsmp;

   fpt->N1 = fpt->N0 = tmp / (tmp + 2.0);
   fpt->D0 = (tmp - 2.0) / (tmp + 2.0);
} /* LPFilt1stCoef */

void svm(struct abc* in, struct abc* out)
{
   register float Avr = 0.5 * (__fmin(in->a, __fmin(in->b, in->c)) + __fmax(in->a, __fmax(in->b, in->c)));

   out->a = in->a - Avr;
   out->b = in->a - Avr;
   out->c = in->a - Avr;
}

//void HPFilt1stCoef(struct Dfilt1* fpt, float fc, float Tsmp)
//{
//   register float tmp, wT = fc * TWO_PI * Tsmp;
//
//   tmp = 1.0 / (wT + 2.0);
//   fpt->N1 = 2.0 *tmp;
//   fpt->N0 = -(fpt->N1);
//   fpt->D0 = (wT - 2.0) * tmp;
//} /* HPFilt1stCoef */

//void ZPFilt1stCoef(struct Dfilt1* fpt, struct ZP1Struct* dpt, float Tsmp)
//{
//   register float tmp, TsmpTwoPi = TWO_PI * Tsmp;
//
//   tmp = dpt->P / (dpt->Z * (-2.0 + TsmpTwoPi * dpt->P));
//   fpt->N1 = tmp * (-2.0 + TsmpTwoPi * dpt->Z);
//   fpt->N0 = tmp * (2.0 + TsmpTwoPi * dpt->Z);
//   fpt->D0 = (2.0 + TsmpTwoPi * dpt->P) / (-2.0 + TsmpTwoPi * dpt->P);
//}

//void HPFilt2ndCoef(struct Dfilt2* fpt, struct Filt2Struct* dpt, float Tsmp)
//{
//   float inv_b2;
//   float wT = dpt->fc * TWO_PI * Tsmp;
//
//   inv_b2 = 1.0 / (2.0 * wT + 4.0 * dpt->Q + wT * wT * dpt->Q);
//   fpt->D1 = (2.0 * wT * wT * dpt->Q - 8.0 * dpt->Q) * inv_b2;
//   fpt->D0 = (4.0 * dpt->Q + wT * wT * dpt->Q - 2* wT) * inv_b2;
//
//   fpt->N2 = fpt->N0 = 4.0 * dpt->Q * inv_b2;
//   fpt->N1 = -2.0 * fpt->N0;
//} /* HPFilt2ndCoef */

//void NFilt2ndCoef(struct Dfilt2* fpt, struct Filt2Struct* dpt, float Tsmp)
//{
//   register float wT = dpt->fc * TWO_PI * Tsmp;
//   register float Q4 = 4.0 * dpt->Q;
//   register float w2T2Q = wT * wT * dpt->Q;
//   register float inv_b2 = 1.0 / (2.0 * wT + Q4 + w2T2Q);
//
//   fpt->D1 = 2.0 * (w2T2Q - Q4) * inv_b2;
//   fpt->D0 = (Q4 + w2T2Q - 2.0 * wT) * inv_b2;
//   fpt->N2 = (Q4  + w2T2Q) * inv_b2;
//   fpt->N0 = fpt->N2;
//   fpt->N1 = fpt->D1;
//} /* NFilt2ndCoef */

//void LPFilt2ndCoef(struct Dfilt2* fpt, struct Filt2Struct* dpt, float Tsmp)
//{
//   float inv_b2;
//   float wT = dpt->fc * TWO_PI * Tsmp;
//
//   inv_b2 = 1.0 / (2.0 * wT + 4.0 * dpt->Q + wT * wT * dpt->Q);
//   fpt->D1 = (2.0 * wT * wT * dpt->Q - 8.0 * dpt->Q) * inv_b2;
//   fpt->D0 = (4.0 * dpt->Q + wT * wT * dpt->Q - 2* wT) * inv_b2;
//
//   fpt->N2 = fpt->N0 = (dpt->Q * wT * wT) * inv_b2;
//   fpt->N1 = 2.0 * fpt->N0;
//} /* LPFilt2ndCoef */

//void BGFilt2ndCoef(struct Dfilt2* fpt, struct BGF2Struct* dpt, float Tsmp)
//{
//   float G, inv_b2, wT = dpt->fc * TWO_PI * Tsmp;
//
//   inv_b2 = 1.0 / (wT * wT * dpt->Qd + 2.0 * wT + 4.0 * dpt->Qd);
//   fpt->D1 = (2.0 * wT * wT - 8.0 ) * dpt->Qd * inv_b2;
//   fpt->D0 = (wT * wT * dpt->Qd - 2.0 * wT + 4.0 * dpt->Qd) * inv_b2;
//
//   G = dpt->Qd / dpt->Qn;
//   fpt->N2 = G * (wT * wT * dpt->Qn + 2.0 * wT + 4.0 * dpt->Qn) * inv_b2;
//   fpt->N1 = fpt->D1;
//   fpt->N0 = G * (wT * wT * dpt->Qn - 2.0 * wT + 4.0 * dpt->Qn) * inv_b2;
//} /* BGFilt2ndCoef */

//void BPFilt2ndCoef(struct Dfilt2* fpt, struct Filt2Struct* dpt, float Tsmp)
//{
//   float inv_b2, wT = dpt->fc * TWO_PI * Tsmp;
//
//   inv_b2 = 1.0 / (2.0 * wT + 4.0 * dpt->Q + wT * wT * dpt->Q);
//   fpt->D1 = (2.0 * wT * wT * dpt->Q - 8.0 * dpt->Q) * inv_b2;
//   fpt->D0 = (4.0 * dpt->Q + wT * wT * dpt->Q - 2* wT) * inv_b2;
//
//   fpt->N2 = 2.0 * wT * inv_b2;
//   fpt->N0 = -(fpt->N2);
//   fpt->N1 = 0.0;
//} /* BPFilt2ndCoef */

//void ZPZPFilt2ndCoef(struct Dfilt2* fpt, struct ZP2Struct* dpt, float Tsmp)
//{
//   float SpT = -2.0 * TWO_PI * (dpt->P1 + dpt->P2) * Tsmp;
//   float p2T2 = TWO_PI * TWO_PI * dpt->P1 * dpt->P2 * Tsmp * Tsmp;
//   float SzT = -2.0 * TWO_PI * (dpt->Z1 + dpt->Z2) * Tsmp;
//   float z2T2 = TWO_PI * TWO_PI * dpt->Z1 * dpt->Z2 * Tsmp * Tsmp;
//   float inv_b2 = 1.0 / (p2T2 + SpT + 4.0);
//   float G = dpt->P1 * dpt->P2 / (dpt->Z1 * dpt->Z2);
//
//   fpt->D1 = inv_b2 * (2.0 * p2T2 - 8.0);
//   fpt->D0 = inv_b2 * (p2T2 - SpT + 4.0);
//   fpt->N2 = inv_b2 * (z2T2 + SzT + 4.0) * G;
//   fpt->N1 = inv_b2 * (2.0 * z2T2 - 8.0) * G;
//   fpt->N0 = inv_b2 * (z2T2 - SzT + 4.0) * G;
//}

float DigFilt1(float in, struct Dfilt1 *d)
{
   register float tmp;

   tmp = in - d->Zminus1 * d->D0;
   d->out = tmp * d->N1 + d->Zminus1 * d->N0;
   d->Zminus1 = tmp;
   return(d->out);
}

//float DigFilt2(float in, struct Dfilt2 *d)
//{
//   register float tmp;
//
//   tmp = in - (d->Zminus1*d->D1 + d->Zminus2*d->D0);
//   d->out = tmp*d->N2 + d->Zminus1*d->N1 + d->Zminus2*d->N0;
//   d->Zminus2 = d->Zminus1;
//   d->Zminus1 = tmp;
//   return(d->out);
//}

float VLPI(struct VLPI_Data *d)
{
   register float prop, integ;

   prop = (d->Error) * (d->Kp) + (d->FF);
   prop = __fsat(prop, d->UL, d->LL);
   integ = 0.5 * (d->Ki) * ((d->Error) + (d->ErrorPrev)) * (*(d->Tsmp)) + (d->Integral);
   d->ErrorPrev = d->Error;
   d->Integral = __fsat(integ, (d->UL) - prop, (d->LL) - prop);
   return(prop + d->Integral);
}

/***************************************************

struct Lookup1D1Struct {
   unsigned long N;
   float X0;
   float Step;
   float InvStep;
   float* Data;
};

****************************************************/
float Lookup1D1(struct Lookup1D1Struct* p, float input)
{
   register float  fraction;
   register int    n;
   register int    LastArrayIndex = p->N - 1;

   input = __fsat(input, p->X0 + p->Step * (float)LastArrayIndex, p->X0);
   input = (input - p->X0) * p->InvStep;
   fraction = __fracf32(input);
   n = (int)input;
   if (n >= LastArrayIndex) {
      return(*(p->Data + LastArrayIndex));
   }
   else  {
      return(*(p->Data + n) + (*(p->Data + n + 1) - *(p->Data + n)) * fraction);
   }
}

