//-------------------------------------------------------//
//        Project Code : CodeLibrary
//           File Name : HarRes.cpp
//          Created on : 2022. 11. 5.
//         Description : 고조파 제어기
//              Author : KimJeongWoo
//  Last modified Date :
//-------------------------------------------------------//

#include <CodeLibMath.h>
#include <DigitalCtrl/HarRes.h>



float sLeadCompensator::Run(float in)
{
	out = k1 * in;
	out += k2 * inPrev;
	out += k3 * outPrev;

	// update compensator-variables
	inPrev = in;
	outPrev = out;

	return out;
}

void sIinvHarRes::LeadCompUpdateGain()
{
	// gains for lead compensator in R controller
	// d-axis gains

	LeadCompd.WTcomp = Wr * Tsamp;
	ThetaRes = 1.5 * LeadCompd.WTcomp;		// 1.5Tsamp digital delay is considered
	ThetaRes = BOUND_PI(ThetaRes);
	LeadCompd.SinComp = sin(ThetaRes);
	LeadCompd.CosComp = 1. / cos(ThetaRes);

	LeadCompd.ZT = LeadCompd.WTcomp * LeadCompd.CosComp * (1. - LeadCompd.SinComp) * 0.5;
	LeadCompd.PT = LeadCompd.WTcomp * LeadCompd.CosComp * (1. + LeadCompd.SinComp) * 0.5;
	LeadCompd.Kc = sqrt(LeadCompd.PT / LeadCompd.ZT);

	LeadCompd.k1 = (1. + LeadCompd.ZT) * LeadCompd.Kc / (1. + LeadCompd.PT);
	LeadCompd.k2 = (LeadCompd.ZT - 1.) * LeadCompd.Kc / (1. + LeadCompd.PT);
	LeadCompd.k3 = (1. - LeadCompd.PT) / (1. + LeadCompd.PT);

	// q-axis gains
	LeadCompq.k1 = LeadCompd.k1;
	LeadCompq.k2 = LeadCompd.k2;
	LeadCompq.k3 = LeadCompd.k3;
}

void sIinvHarRes::Reset()
{
	Idr1 = 0.;
	Idr2 = 0.;
	Vdr1 = 0.;
	Vdr2 = 0.;

	Iqr1 = 0.;
	Iqr2 = 0.;
	Vqr1 = 0.;
	Vqr2 = 0.;

	VdssRef = 0.;
	VqssRef = 0.;
	VdssLead = 0.;
	VqssLead = 0.;

	LeadCompd.inPrev = 0.;
	LeadCompd.outPrev = 0.;
	LeadCompd.out = 0.;
	LeadCompq.inPrev = 0.;
	LeadCompq.outPrev = 0.;
	LeadCompq.out = 0.;
}

void sIinvHarRes::Init(float mRc, float mOrder, float mWcc, float mWeRate, float mTsamp)
{
	Idr1 = 0.;
	Idr2 = 0.;
	Vdr1 = 0.;
	Vdr2 = 0.;

	Iqr1 = 0.;
	Iqr2 = 0.;
	Vqr1 = 0.;
	Vqr2 = 0.;

	VdssRef = 0.;
	VqssRef = 0.;
	VdssLead = 0.;
	VqssLead = 0.;

	LeadCompd.inPrev = 0.;
	LeadCompd.outPrev = 0.;
	LeadCompd.out = 0.;
	LeadCompq.inPrev = 0.;
	LeadCompq.outPrev = 0.;
	LeadCompq.out = 0.;

	Tsamp = mTsamp;
	Wcc = mWcc;
	Rc = mRc;
	Kr = Rc * Wcc;
	order = mOrder;
	WeRate = mWeRate;
	Wr = order * WeRate;
	delay = 1.5;

	// gains of R controller
	Cr1 = tan( Wr * Tsamp * 0.5);
	Cr2 = 1. -  Cr1 *  Cr1;
	Cr3 = 1. +  Cr1 *  Cr1;
	Kr1 =  Cr1 /  Wr /  Cr3;
	Kr2 = 2. *  Cr2 /  Cr3;

	LeadCompUpdateGain();

} // initilize gains of resonant controller

void sIinvHarRes::UpdateGain(float mWe_hat)
{

    Kr = Rc * Wcc;
	// frequency updates
	Wr = order * mWe_hat;

	// gains of R controller
	Cr1 = tan(Wr * Tsamp * 0.5);
	Cr2 = 1. - Cr1 * Cr1;
	Cr3 = 1. + Cr1 * Cr1;
	// More simplified gain settings for R controller itself
	Kr1 = Cr1 / Wr / Cr3;
	Kr2 = 2. * Cr2 / Cr3;

	LeadCompUpdateGain();
}

void sIinvHarRes::Run(float mIds, float mIqs)
{

	// error currents in the synchronous d-q reference frame
	Idr = -Kr * mIds;
	Iqr = -Kr * mIqs;

	// d-axis harmonic voltage references
	VdssRef = Kr1 * (Idr - Idr2);
	VdssRef += Kr2 * Vdr1;
	VdssRef -= Vdr2;

	// q-axis harmonic voltage references
	VqssRef = Kr1 * (Iqr - Iqr2);
	VqssRef += Kr2 * Vqr1;
	VqssRef -= Vqr2;

	// update state variables for next sampling period
	// d-axis
	Idr2 = Idr1;
	Idr1 = Idr;
	Vdr2 = Vdr1;
	Vdr1 = VdssRef;

	// q-axis
	Iqr2 = Iqr1;
	Iqr1 = Iqr;
	Vqr2 = Vqr1;
	Vqr1 = VqssRef;

	// lead compensator for digital delay in R controller
	VdssLead = LeadCompd.Run(VdssRef);
	VqssLead = LeadCompq.Run(VqssRef);

	// limiter for voltage references of resonant controller
	VdssLead = LIMITER(VdssLead, 20, -20);
	VqssLead = LIMITER(VqssLead, 20, -20);

}