//-------------------------------------------------------//
//        Project Code : CodeLibrary
//           File Name : SogiFll1ph.cpp
//          Created on : 2023. 7. 13.
//         Description :
//              Author : "Kim JeongWoo"
//  Last modified Date :
//-------------------------------------------------------//

#include <PLL/SogiFll1ph.h>
#include "CodeLibMath.h"


void sSogiFll1ph::Init(float mFreqRate, float mTsamp, float mVacPeakMax, float *Vac, float *Iac)
{
	pVac = (float *) Vac;
	pIac = (float *) Iac;


	Vs_U[0] = 0.0;
	Vs_U[1] = 0.0;
	Vs_U[2] = 0.0;

	Is_U[0] = 0.0;
	Is_U[1] = 0.0;
	Is_U[2] = 0.0;

	Vs_Osg_U[0] = 0.0;
	Vs_Osg_U[1] = 0.0;
	Vs_Osg_U[2] = 0.0;

	Vs_Osg_Qu[0] = 0.0;
	Vs_Osg_Qu[1] = 0.0;
	Vs_Osg_Qu[2] = 0.0;

	Vs_u_Q[0] = 0.0;
	Vs_u_Q[1] = 0.0;

	Vs_u_D[0] = 0.0;
	Vs_u_D[1] = 0.0;

	Is_Osg_U[0] = 0.0;
	Is_Osg_U[1] = 0.0;
	Is_Osg_U[2] = 0.0;

	Is_Osg_Qu[0] = 0.0;
	Is_Osg_Qu[1] = 0.0;
	Is_Osg_Qu[2] = 0.0;

	Is_u_Q[0] = 0.0;
	Is_u_Q[1] = 0.0;

	Is_u_D[0] = 0.0;
	Is_u_D[1] = 0.0;

	Vs_ylf[0] = 0.0;
	Vs_ylf[1] = 0.0;

	Is_ylf[0] = 0.0;
	Is_ylf[1] = 0.0;

	FreqOut = 0.0;
	FreqRate = mFreqRate;

	Theta = 0.0;
	SinTheta = 0.0;
	CosTheta = 0.0;

	Tsamp = mTsamp;
	VacPeakMax = mVacPeakMax;

	x3[0] = 0;
	x3[1] = 0;

	ef2 = 0;

	WeRate = 2 * PI * FreqRate;
	WeHat = 2 * PI * FreqRate;

	SettlingTime = 0.03; //3ms
	DampingRatio = 0.7;
	ErrBand = 0.01;
	Wn = ((log((1 / sqrt(1 - DampingRatio * DampingRatio)) / ErrBand)) / SettlingTime) / DampingRatio;

	Ti = 2 * (DampingRatio / Wn);
	Kp = Wn * Wn * Ti;
	Ki = Kp / Ti;


	LpfCoefUpdate();
	CoefUpdate(WeRate);
}

void sSogiFll1ph::LpfCoefUpdate()
{
	// loop filter coefficients for 20kHz
	Lpf.b0 = ((2.0) * Kp + Ki * Tsamp) * 0.5 / VacPeakMax;
	Lpf.b1 = -((2.0) * Kp - Ki * Tsamp) * 0.5 / VacPeakMax;
	Lpf.a1 = 1.0;
}

void sSogiFll1ph::CoefUpdate(float mWe)
{
	float temp;

	Osg.k = (float)(1.2);

	Osg.x = (float)(2.0 * Osg.k * mWe * Tsamp);
	Osg.y = (float)(mWe * Tsamp * mWe * Tsamp);

	temp = (float)1.0 / (Osg.x + Osg.y + 4.0);

	Osg.b0 = ((float)Osg.x * temp);
	Osg.b2 = ((float)(-1.0) * Osg.b0);
	Osg.a1 = ((float)(2.0 * (4.0 - Osg.y)) * temp);
	Osg.a2 = ((float)(Osg.x - Osg.y - 4) * temp);

	Osg.qb0 = ((float)(Osg.k * Osg.y) * temp);
	Osg.qb1 = (Osg.qb0 * (float)(2.0));
	Osg.qb2 = Osg.qb0;
}

void sSogiFll1ph::Run(void)
{
	Vs_U[0] = *pVac;

	Vs_Osg_U[0] = (Osg.b0 * (Vs_U[0] - Vs_U[2])) + (Osg.a1 * Vs_Osg_U[1]) + (Osg.a2 * Vs_Osg_U[2]);
	Vs_Osg_U[2] = Vs_Osg_U[1];
	Vs_Osg_U[1] = Vs_Osg_U[0];

	Vs_Osg_Qu[0] = ((Osg.qb0 * Vs_U[0]) + (Osg.qb1 * Vs_U[1]) + (Osg.qb2 * Vs_U[2]) + (Osg.a1 * Vs_Osg_Qu[1]) + (Osg.a2 * Vs_Osg_Qu[2]));
	Vs_Osg_Qu[2] = Vs_Osg_Qu[1];
	Vs_Osg_Qu[1] = Vs_Osg_Qu[0];

	Vs_U[2] = Vs_U[1];
	Vs_U[1] = Vs_U[0];

	Vs_u_D[0] = (CosTheta * Vs_Osg_U[0]) + (SinTheta * Vs_Osg_Qu[0]);
	Vs_u_Q[0] = (CosTheta * Vs_Osg_Qu[0]) - (SinTheta * Vs_Osg_U[0]);

	//
	Is_U[0] = *pIac;

	Is_Osg_U[0] = (Osg.b0 * (Is_U[0] - Is_U[2])) + (Osg.a1 * Is_Osg_U[1]) + (Osg.a2 * Is_Osg_U[2]);
	Is_Osg_U[2] = Is_Osg_U[1];
	Is_Osg_U[1] = Is_Osg_U[0];

	Is_Osg_Qu[0] = ((Osg.qb0 * Is_U[0]) + (Osg.qb1 * Is_U[1]) + (Osg.qb2 * Is_U[2]) + (Osg.a1 * Is_Osg_Qu[1]) + (Osg.a2 * Is_Osg_Qu[2]));
	Is_Osg_Qu[2] = Is_Osg_Qu[1];
	Is_Osg_Qu[1] = Is_Osg_Qu[0];

	Is_U[2] = Is_U[1];
	Is_U[1] = Is_U[0];

	Is_u_D[0] = (CosTheta * Is_Osg_U[0]) + (SinTheta * Is_Osg_Qu[0]);
	Is_u_Q[0] = (CosTheta * Is_Osg_Qu[0]) - (SinTheta * Is_Osg_U[0]);

	Vs_ylf[0] = Vs_ylf[1] + (Lpf.b0 * Vs_u_D[0]) + (Lpf.b1 * Vs_u_D[1]);
	Vs_ylf[1] = Vs_ylf[0];
	Vs_u_D[1] = Vs_u_D[0];

	FreqOut = (FreqRate - Vs_ylf[0]);
	WeHat = ((FreqOut) * (float(2.0 * PI)));

	Theta = Theta + WeHat * Tsamp;
	Theta = BOUND_PI(Theta);

	SinTheta = (float)sin(Theta);
	CosTheta = (float)cos(Theta);

	ef2 = ((Vs_U[0] - Vs_Osg_U[0]) * Vs_Osg_Qu[0]) * 0.7 * Tsamp * -1.0;

	x3[0] = x3[1] + ef2;
	x3[1] = x3[0];

	WeHat = WeRate + x3[0];
	FreqRate = WeHat / (2.0 * PI);

	CoefUpdate(WeHat);

}