#include "motor_sim_helper.h"

PmsmSimUtModelClass::P_PmsmSimUt_T MotorSimHelper::DefaultParameter;

void MotorSimHelper::StoreDefaultParam()
{
    DefaultParameter = PmsmSimUtModelClass::PmsmSimUt_P;
}

void MotorSimHelper::RestoreDefaultParam(PmsmSimUtModelClass *sys)
{
    sys->PmsmSimUt_P = DefaultParameter;
}

void MotorSimHelper::TransitModelOutput(PmsmSimUtModelClass *sys, McStatus *status)
{
    McPuBase      *b = status->Base;
    McStatusNamed *n = &status->Named;
    McStatusPu    *p = &status->Pu;

    p->swIa = (s16)((sys->PmsmSimUt_Y.Out.Iabc[0] * 100 * 16384) / b->uwIbAp);
    p->swIb = (s16)((sys->PmsmSimUt_Y.Out.Iabc[1] * 100 * 16384) / b->uwIbAp);
    p->swIc = (s16)((sys->PmsmSimUt_Y.Out.Iabc[2] * 100 * 16384) / b->uwIbAp);

    double ialpha = 0.6667 * (sys->PmsmSimUt_Y.Out.Iabc[0] - 0.5 * sys->PmsmSimUt_Y.Out.Iabc[1] - 0.5 * sys->PmsmSimUt_Y.Out.Iabc[2]);
    double ibeta  = 0.57735 * (sys->PmsmSimUt_Y.Out.Iabc[1] - sys->PmsmSimUt_Y.Out.Iabc[2]);

    p->swIalpha = (s16)((ialpha * 100 * 16384) / b->uwIbAp);
    p->swIalpha = (s16)((ibeta * 100 * 16384) / b->uwIbAp);
    p->swIzero  = 0;

    p->swId = (s16)((sys->PmsmSimUt_Y.Out.Idq[0] * 100 * 16384) / b->uwIbAp);
    p->swIq = (s16)((sys->PmsmSimUt_Y.Out.Idq[1] * 100 * 16384) / b->uwIbAp);
    p->swI0 = 0;

    p->swUa = (s16)((sys->PmsmSimUt_Y.Out.Uabc[0] * 10 * 16384) / b->uwUbVt);
    p->swUb = (s16)((sys->PmsmSimUt_Y.Out.Uabc[1] * 10 * 16384) / b->uwUbVt);
    p->swUc = (s16)((sys->PmsmSimUt_Y.Out.Uabc[2] * 10 * 16384) / b->uwUbVt);

    double ualpha = 0.6667 * (sys->PmsmSimUt_Y.Out.Uabc[0] - 0.5 * sys->PmsmSimUt_Y.Out.Uabc[1] - 0.5 * sys->PmsmSimUt_Y.Out.Uabc[2]);
    double ubeta  = 0.57735 * (sys->PmsmSimUt_Y.Out.Uabc[1] - sys->PmsmSimUt_Y.Out.Uabc[2]);

    p->swUalpha = (s16)((ualpha * 10 * 16384) / b->uwUbVt);
    p->swUbeta  = (s16)((ubeta * 10 * 16384) / b->uwUbVt);
    p->swUzero  = 0;

    p->swElecAngle = (s16)(sys->PmsmSimUt_Y.Out.Theta / 2 / 3.1415926 * 32768);
    p->swElecOmega = (s16)((sys->PmsmSimUt_Y.Out.We * 10 * 32768) / b->uwWeb);

    p->swUac = (s16)(((s32)5400 << 14) / b->uwUbVt);
    p->swUdc = (s16)(((s32)5400 << 14) / b->uwUbVt);
}

void MotorSimHelper::SetModelICmdPu(PmsmSimUtModelClass *sys, double id_pu, double iq_pu, McPuBase *base)
{
    sys->PmsmSimUt_U.CtrlIn.IdCmd = (double)id_pu / (double)(1 << 14) * (double)base->uwIbAp / 100.0;
    sys->PmsmSimUt_U.CtrlIn.IqCmd = (double)iq_pu / (double)(1 << 14) * (double)base->uwIbAp / 100.0;
}

void MotorSimHelper::SetModelUCmdPu(PmsmSimUtModelClass *sys, double ud_pu, double uq_pu, McPuBase *base)
{
    sys->PmsmSimUt_U.ICtrlIn.UdCtrl = (double)ud_pu / (double)(1 << 14) * ((double)base->uwUbVt / 10.0);
    sys->PmsmSimUt_U.ICtrlIn.UqCtrl = (double)uq_pu / (double)(1 << 14) * ((double)base->uwUbVt / 10.0);
}