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

#include "SYSCTRL/AlgorithmSource.h"

namespace SYSCTRL
{
//CONSTRUCTOR
AlgorithmSource::AlgorithmSource(SYSCTRL::SystemEnvironment& env):
        AlgorithmBase(),
        m_env(env),
        m_voltage_direct(FP_ZERO),
        m_voltage_quadrature(FP_ZERO),
        m_voltage_alpha(FP_ZERO),
        m_voltage_beta(FP_ZERO),
        m_voltage_a_gen(FP_ZERO),
        m_voltage_b_gen(FP_ZERO),
        m_voltage_c_gen(FP_ZERO),
        m_voltage_a_relative(FP_ZERO),
        m_voltage_b_relative(FP_ZERO),
        m_voltage_c_relative(FP_ZERO),
        m_ort_alpha(FP_ZERO),
        m_ort_beta(FP_ZERO),
        _execute(&SYSCTRL::AlgorithmSource::_execute_undef)
//
{}//CONSTRUCTOR
//
void AlgorithmSource::setup()
{
    _execute = &SYSCTRL::AlgorithmSource::_execute_run;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmSource::reset()
{}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmSource::execute()
{
    (this->*_execute)();
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmSource::_execute_run()
{

    m_env.hardware.ref_control_order = ORDER_START;

    m_voltage_direct = m_env.algorithm_source_references.voltage * cosf(m_env.algorithm_source_references.phase_shift);
    m_voltage_quadrature = m_env.algorithm_source_references.voltage * sinf(m_env.algorithm_source_references.phase_shift);
    //
    m_ort_alpha = m_env.main_ab_orts.active;
    m_ort_beta = m_env.main_ab_orts.reactive;
    //
    FLTSYSLIB::Transformation::park_inverse(m_ort_alpha, m_ort_beta, m_voltage_direct, m_voltage_quadrature, m_voltage_alpha, m_voltage_beta);
    FLTSYSLIB::Transformation::clarke_inverse(m_voltage_alpha, m_voltage_beta, m_voltage_a_gen, m_voltage_b_gen, m_voltage_c_gen);
    //
    m_voltage_a_relative = m_voltage_a_gen * m_env.cell_dc_voltage_a_reciprocal;
    m_voltage_b_relative = m_voltage_b_gen * m_env.cell_dc_voltage_b_reciprocal;
    m_voltage_c_relative = m_voltage_c_gen * m_env.cell_dc_voltage_c_reciprocal;
    //
    m_voltage_a = m_voltage_a_relative;
    m_voltage_b = m_voltage_b_relative;
    m_voltage_c = m_voltage_c_relative;
    //
}//
//
} /* namespace SYSCTRL */