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

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


//Second Order Generalized Integrators PLL
void sSogiPll::Reset(float mFreqRate, float mTsamp)
{

	k = 0.5;
	Wehat = 2. * PI * mFreqRate;

	d0 = Wehat * Wehat * mTsamp * mTsamp;
	d1 = 2 * k * Wehat * mTsamp;

	//동상 계수
	D_a0 = (d0 + d1 + 4.);
	D_a1 = (2. * d0 - 8.) / D_a0;
	D_a2 = (d0 - d1 + 4.) / D_a0;
	D_b0 = (d1) / D_a0;
	D_b1 = 0;
	D_b2 = -D_b0;

	//90도 지연 계수
	Q_a0 = D_a0;
	Q_a1 = D_a1;
	Q_a2 = D_a2;
	Q_b0 = (k * d0) / Q_a0;
	Q_b1 = (2. * Q_b0);
	Q_b2 = Q_b0;



	//d축 전압
	Eds[0] = 0.;  //x[n]
	Eds[1] = 0.;  //x[n-1]	
	Eds[2] = 0.;  //x[n-2]

	EdsIn[0] = 0.; //y[n]
	EdsIn[1] = 0.; //y[n-1]
	EdsIn[2] = 0.; //y[n-2]

	EdsLag[0] = 0.; //y[n]
	EdsLag[1] = 0.; //y[n-1]
	EdsLag[2] = 0.; //y[n-2]


	//q축 전압
	Eqs[0] = 0.;
	Eqs[1] = 0.;
	Eqs[2] = 0.;

	EqsIn[0] = 0.;
	EqsIn[1] = 0.;
	EqsIn[2] = 0.;

	EqsLag[0] = 0.; //y[n]
	EqsLag[1] = 0.; //y[n-1]
	EqsLag[2] = 0.; //y[n-2]

	EdsPos = 0.;
	EqsPos = 0.;
	EdsNeg = 0.;
	EqsNeg = 0.;

	EdePos = 0.;
	EqePos = 0.;

	//SRF-PLL. 위상 동기화 제어기
	Err = 0.;
	Integ = 0.;

	Fb = 0.;

	Thetahat = 0.;
}

void sSogiPll::Init(float mVsRatePeak, float mFreqRate, float mTsamp)
{
	//Ksogi가 작으면 필터링 성능이 증가하지만, 과도 상태가 증가함. Ksogi 값이 너무 작을 경우 과도상태가 매우 길어져서 제어기가 제대로 동작하지 못함.
	//강한 필터링에 과도 상태를 빠르게 하기 위해서는 SOGI-FLL로 구성하면 되지만, 주파수 추정할 때 Gain 튜닝에도 불구하고 리플이 존재함.
	k = 0.5;
	Wehat = 2. * PI * mFreqRate;

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

	//PI제어기
	Kp = 2. * Zeta * Wc / mVsRatePeak;
	Ki = Wc * Wc / mVsRatePeak;
}

float sSogiPll::Run(float mEds, float mEqs, float mFreqRate, float mTsamp)
{
	d0 = Wehat * Wehat * mTsamp * mTsamp;
	d1 = 2 * k * Wehat * mTsamp;

	//동상 계수
	D_a0 = (d0 + d1 + 4.);
	D_a1 = (2. * d0 - 8.) / D_a0;
	D_a2 = (d0 - d1 + 4.) / D_a0;
	D_b0 = (d1) / D_a0;
	D_b1 = 0;
	D_b2 = -D_b0;

	//90도 지연 계수
	Q_a0 = D_a0;
	Q_a1 = D_a1;
	Q_a2 = D_a2;
	Q_b0 = (k * d0) / Q_a0;
	Q_b1 = (2. * Q_b0);
	Q_b2 = Q_b0;


	Eds[0] = mEds;
	Eqs[0] = mEqs;


	//동상 전압
	EdsIn[0] =  (D_b0 * Eds[0] + D_b1 * Eds[1] + D_b2 * Eds[2])
					- (D_a1 * EdsIn[1] + D_a2 * EdsIn[2]);

	EqsIn[0] =  (D_b0 * Eqs[0] + D_b1 * Eqs[1] + D_b2 * Eqs[2])
					- (D_a1 * EqsIn[1] + D_a2 * EqsIn[2]);
	
	//90도 지연 전압
	EdsLag[0] = (Q_b0 * Eds[0] + Q_b1 * Eds[1] + Q_b2 * Eds[2])
					- (Q_a1 * EdsLag[1] + Q_a2 * EdsLag[2]);

	EqsLag[0] = (Q_b0 * Eqs[0] + Q_b1 * Eqs[1] + Q_b2 * Eqs[2])
					- (Q_a1 * EqsLag[1] + Q_a2 * EqsLag[2]);


	//d축 전압
	Eds[2] = Eds[1];
	Eds[1] = Eds[0];
	EdsIn[2] = EdsIn[1];
	EdsIn[1] = EdsIn[0];
	EdsLag[2] = EdsLag[1];
	EdsLag[1] = EdsLag[0];

	//q축 전압
	Eqs[2] = Eqs[1];
	Eqs[1] = Eqs[0];
	EqsIn[2] = EqsIn[1];
	EqsIn[1] = EqsIn[0];
	EqsLag[2] = EqsLag[1];
	EqsLag[1] = EqsLag[0];

	//정상분 dq 전압
	EdsPos = 0.5 * EdsIn[0] - 0.5 * EqsLag[0];
	EqsPos = 0.5 * EdsLag[0] + 0.5 * EqsIn[0];

	//역상분 dq 전압
	EdsNeg = 0.5 * EdsIn[0] + 0.5 * EqsLag[0];
	EqsNeg = -0.5 * EdsLag[0] + 0.5 * EqsIn[0];

	//정상분 동기 좌표계 전압
	//a =  Theta_hat* Thetahat;
	EdePos = cos(Thetahat) * EdsPos + sin(Thetahat) * EqsPos;
	EqePos = -sin(Thetahat) * EdsPos + cos(Thetahat) * EqsPos;

	//SRF-PLL. 위상 동기화 제어기
	Err = 0 - EdePos;
	Integ = Integ + Ki * Err * mTsamp;

	Wehat = Integ + 2. * PI * mFreqRate;
	Fb = Err * Kp + Wehat;

	FreqOut = Wehat * 0.5 * INV_PI;

	Thetahat = Thetahat + Fb * mTsamp;
	Thetahat = Thetahat + ((Thetahat > PI) ? -2. * PI : (Thetahat < -PI) ? 2. * PI : 0.);

	return Thetahat;
}