//-------------------------------------------------------//
//        Project Code : CodeLibrary
//           File Name : SoapPll3ph.cpp
//          Created on : 2019. 06. 07.
//         Description :
//              Author : KimJeongWoo
//  Last modified Date :
//-------------------------------------------------------//

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


//Synchronous Observer-Aided Preprocessing PLL

void sSoapPll::Init(float mTsamp, float mVsRatePeak, float mFreqRate)
{

	Tprd = mTsamp;
	Zeta = 1;
	Wc = 2. * 5. * PI;

	Kp = 2. * Zeta * Wc;
	Ki = Wc * Wc;

	k = 1.5;
	rho = 1.;
	c1 = 0.5 * (1. + rho) * k;
	c2 = 0.5 * rho * k * k;

	Reset(mVsRatePeak, mFreqRate);

}

void sSoapPll::Reset(float mVsRatePeak, float mFreqRate)
{
	Integral = 0.;
	Wehat = 2. * PI * mFreqRate;
	Weff = 2. * PI * mFreqRate;
	Thetahat = 0.;

	Thetd = 0.;

	VdePos = 0.;
	VqePos = mVsRatePeak;	//140.*INV_SQRT3*SQRT2;
	Vdehat = 0.;
	Vqehat = mVsRatePeak;	//0.;
}

float sSoapPll::Run(float mVde, float mVqe, float mFreqRate, float mTsamp)
{
	float coefficient = 0.;
	float out = 0.;

	coefficient = Wehat * mTsamp;

	// Synchronous Observer-Aided Preprocessing (SOAP)
	ErrVde = mVde - Vdehat;
	VqePos -= c2 * coefficient * ErrVde;
	Vdehat += 2. * coefficient * (c1 * ErrVde + mVqe - VqePos);

	ErrVqe = mVqe - Vqehat;
	VdePos += c2 * coefficient * ErrVqe;
	Vqehat += 2. * coefficient * (c1 * ErrVqe - mVde + VdePos);

	//PLL
	Thetd = atan2(-VdePos, VqePos);
	Integral += Ki * mTsamp * Thetd;
	Wehat = Weff + Integral;
	FreqOut = Wehat * 0.5 * INV_PI;

	Thetahat += mTsamp * (Kp * Thetd + Wehat);
	Thetahat = BOUND_PI(Thetahat);

	out = Thetahat;

	return out;
}

void sSoapPll::Preparing(float mVde, float mVqe, float mTheta, float mWeRate)
{
	Wehat = mWeRate;
	Thetahat = mTheta;

	VqePos = Vqehat = mVqe;
	VdePos = Vdehat = mVde;

	Integral = 0;

}