/*
 * GeneratorSymmetricalComponents.cpp
 *
 *      Author: Aleksey Gerasimenko
 *      gerasimenko.aleksey.n@gmail.com
 */

#include "SYSCTRL/GeneratorSymmetricalComponents.h"

namespace SYSCTRL
{
//CONSTRUCTOR
GeneratorSymmetricalComponents::GeneratorSymmetricalComponents():
        m_amplitude(),
        m_phase(),
        m_ort_direct_alpha(FP_ZERO),
        m_ort_direct_beta(FP_ZERO),
        m_ort_inverse_alpha(FP_ZERO),
        m_ort_inverse_beta(FP_ZERO),
        m_direct_alpha(FP_ZERO),
        m_direct_beta(FP_ZERO),
        m_inverse_alpha(FP_ZERO),
        m_inverse_beta(FP_ZERO),
        m_direct_a(FP_ZERO),
        m_direct_b(FP_ZERO),
        m_direct_c(FP_ZERO),
        m_inverse_a(FP_ZERO),
        m_inverse_b(FP_ZERO),
        m_inverse_c(FP_ZERO),
        m_out_a(FP_ZERO),
        m_out_b(FP_ZERO),
        m_out_c(FP_ZERO)
//
{}//CONSTRUCTOR


#pragma CODE_SECTION("ramfuncs");
void GeneratorSymmetricalComponents::configure(GeneratorSymmetricalComponentsConfiguration& config)
{
    m_amplitude = config.amplitude;
    m_phase = config.phase;
    //
}//


#pragma CODE_SECTION("ramfuncs");
void GeneratorSymmetricalComponents::execute(float ort_alpha, float ort_beta)
{
    m_phase.direct.cos_phase = cosf(m_phase.direct.phase);
    m_phase.direct.sin_phase = sinf(m_phase.direct.phase);
    m_phase.inverse.cos_phase = cosf(m_phase.inverse.phase);
    m_phase.inverse.sin_phase = sinf(m_phase.inverse.phase);
    //
    m_ort_direct_alpha = ort_alpha * m_phase.direct.cos_phase - ort_beta * m_phase.direct.sin_phase;
    m_ort_direct_beta = ort_alpha * m_phase.direct.sin_phase + ort_beta * m_phase.direct.cos_phase;
    //
    m_ort_inverse_alpha = ort_alpha * m_phase.inverse.cos_phase + ort_beta * m_phase.inverse.sin_phase;
    m_ort_inverse_beta = ort_alpha * m_phase.inverse.sin_phase - ort_beta * m_phase.inverse.cos_phase;
    //
    FLTSYSLIB::Transformation::park_inverse(m_ort_direct_alpha, m_ort_direct_beta, m_amplitude.direct.d, m_amplitude.direct.q, m_direct_alpha, m_direct_beta);
    FLTSYSLIB::Transformation::park_inverse(m_ort_inverse_alpha, m_ort_inverse_beta, m_amplitude.inverse.d, m_amplitude.inverse.q, m_inverse_alpha, m_inverse_beta);
    FLTSYSLIB::Transformation::clarke_inverse(m_direct_alpha, m_direct_beta, m_direct_a, m_direct_b, m_direct_c);
    FLTSYSLIB::Transformation::clarke_inverse(m_inverse_alpha, m_inverse_beta, m_inverse_a, m_inverse_b, m_inverse_c);
    //
    m_out_a = m_direct_a + m_inverse_a + m_amplitude.zero_a;
    m_out_b = m_direct_b + m_inverse_b + m_amplitude.zero_b;
    m_out_c = m_direct_c + m_inverse_c + m_amplitude.zero_c;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void GeneratorSymmetricalComponents::get_output(float& out_a, float& out_b, float& out_c)
{
    out_a = m_out_a;
    out_b = m_out_b;
    out_c = m_out_c;
    //
}//
//

} /* namespace SYSCTRL */