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

#include "SYSCTRL/AlgorithmStart.h"

namespace SYSCTRL
{
//CONSTRUCTOR
AlgorithmStart::AlgorithmStart(SYSCTRL::SystemEnvironment& env):
        SYSCTRL::AlgorithmBase(),
        m_env(env),
#if TYPECONTROL == VECTORCONTROL
        m_reference_zero(FP_ZERO),
        m_reference_current_cell_direct(FP_ZERO),
        m_reference_current_cell_quadrature(FP_ZERO),
#endif

#if TYPECONTROL == DIRECTREVERSECONTROL
        m_reference_zero(FP_ZERO),
        m_reference_current_cell_direct(FP_ZERO),
        m_reference_current_cell_quadrature(FP_ZERO),
#endif
        _execute(&SYSCTRL::AlgorithmStart::_execute_undef)
{}//CONSTRUCTOR
//
void AlgorithmStart::setup()
{
    _execute = &SYSCTRL::AlgorithmStart::_execute_run;
    //
}//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmStart::reset()
{

#if TYPECONTROL == VECTORCONTROL
    //
    m_env.regulator_current_load_direct.reset();
    m_env.regulator_current_load_quadrature.reset();
    //
    m_env.referencer_current_bypass_direct.reset();
    m_env.referencer_current_bypass_quadrature.reset();
    //
#endif
#if TYPECONTROL == SCALARCONTROL
    m_env.current_referencer_a_active.reset();
    m_env.current_referencer_a_reactive.reset();
    m_env.current_referencer_b_active.reset();
    m_env.current_referencer_b_reactive.reset();
    m_env.current_referencer_c_active.reset();
    m_env.current_referencer_c_reactive.reset();

    m_env.current_regulator_a_active.reset();
    m_env.current_regulator_a_reactive.reset();
    m_env.current_regulator_b_active.reset();
    m_env.current_regulator_b_reactive.reset();
    m_env.current_regulator_c_active.reset();
    m_env.current_regulator_c_reactive.reset();
#endif

#if TYPECONTROL == DIRECTREVERSECONTROL
    //
    m_env.drc_regulator_current_load_direct.reset();
    m_env.drc_regulator_current_load_quadrature.reset();
    //
    m_env.drc_referencer_current_bypass_direct.reset();
    m_env.drc_referencer_current_bypass_quadrature.reset();
    //
#endif

    m_voltage_a = FP_ZERO;
    m_voltage_b = FP_ZERO;
    m_voltage_c = FP_ZERO;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmStart::execute()
{
    (this->*_execute)();
    //
}//
//
#if TYPECONTROL == VECTORCONTROL
#pragma CODE_SECTION("ramfuncs");
void AlgorithmStart::_execute_run()
{
    //
    m_env.hardware.ref_control_order = ORDER_START;
    //
    m_env.timer_start.execute();
    //
    m_reference_current_cell_direct = m_env.referencer_current_bypass_direct.execute(m_reference_zero, m_env.current_bypass_direct);
    m_reference_current_cell_quadrature = m_env.referencer_current_bypass_quadrature.execute(m_reference_zero, m_env.current_bypass_quadrature);
    //
    m_env.voltage_cell_direct = m_env.regulator_current_load_direct.execute(m_env.current_cell_direct, m_reference_current_cell_direct);
    m_env.voltage_cell_quadrature = m_env.regulator_current_load_quadrature.execute(m_env.current_cell_quadrature, m_reference_current_cell_quadrature);
    //
    FLTSYSLIB::Transformation::park_inverse(m_env.main_ab_orts.active,
                                            m_env.main_ab_orts.reactive,
                                            m_env.voltage_cell_direct,
                                            m_env.voltage_cell_quadrature,
                                            m_env.voltage_cell_alpha,
                                            m_env.voltage_cell_beta);
    //
    FLTSYSLIB::Transformation::clarke_inverse(m_env.voltage_cell_alpha,
                                              m_env.voltage_cell_beta,
                                              m_env.voltage_cell_a,
                                              m_env.voltage_cell_b,
                                              m_env.voltage_cell_c);
    //
    m_voltage_a = m_env.voltage_cell_a * m_env.cell_dc_voltage_a_reciprocal;
    m_voltage_b = m_env.voltage_cell_b * m_env.cell_dc_voltage_b_reciprocal;
    m_voltage_c = m_env.voltage_cell_c * m_env.cell_dc_voltage_c_reciprocal;
    //
}//
#endif


#if TYPECONTROL == SCALARCONTROL
#pragma CODE_SECTION("ramfuncs");
void AlgorithmStart::_execute_run()
{
    //
    m_env.hardware.ref_control_order = ORDER_START;
    //
    m_env.timer_start.execute();

    //**** Phase A ***
    //-reference constructor
    //m_env.start_control.phase_a.reference.current_bypass_active = FP_ZERO;
    //m_env.start_control.phase_a.reference.current_bypass_reactive = FP_ZERO;
    //
    m_env.start_control.phase_a.reference.current_bypass_active = m_env.start_control.common_ref.active;
    m_env.start_control.phase_a.reference.current_bypass_reactive = m_env.start_control.common_ref.reactive;
    //
    //m_env.start_control.phase_a.test_ref = m_env.start_control.common_ref;

    //-feedback constructor
    m_env.start_control.phase_a.feedback.current_bypass_active = m_env.projection_current_bypass_a.active;
    m_env.start_control.phase_a.feedback.current_bypass_reactive = m_env.projection_current_bypass_a.reactive;
    //
    m_env.start_control.phase_a.feedback.current_cell_active = m_env.projection_current_cell_a.active;
    m_env.start_control.phase_a.feedback.current_cell_reactive = m_env.projection_current_cell_a.reactive;
    //
    //-control
    _execute_single_phase(m_env.start_control.phase_a,
                          m_env.main_abc_orts.phase_a,
                          m_env.cell_dc_voltage_a_reciprocal,
                          m_env.current_referencer_a_active,
                          m_env.current_referencer_a_reactive,
                          m_env.current_regulator_a_active,
                          m_env.current_regulator_a_reactive);
    //<>

    //**** Phase B ***
    //-reference constructor
    //m_env.start_control.phase_b.reference.current_bypass_active = FP_ZERO;
    //m_env.start_control.phase_b.reference.current_bypass_reactive = FP_ZERO;
    //
    m_env.start_control.phase_b.reference.current_bypass_active = m_env.start_control.common_ref.active;
    m_env.start_control.phase_b.reference.current_bypass_reactive = m_env.start_control.common_ref.reactive;
    //
    //m_env.start_control.phase_b.test_ref = m_env.start_control.common_ref;

    //-feedback constructor
    m_env.start_control.phase_b.feedback.current_bypass_active = m_env.projection_current_bypass_b.active;
    m_env.start_control.phase_b.feedback.current_bypass_reactive = m_env.projection_current_bypass_b.reactive;
    //
    m_env.start_control.phase_b.feedback.current_cell_active = m_env.projection_current_cell_b.active;
    m_env.start_control.phase_b.feedback.current_cell_reactive = m_env.projection_current_cell_b.reactive;
    //
    //-control
    _execute_single_phase(m_env.start_control.phase_b,
                          m_env.main_abc_orts.phase_b,
                          m_env.cell_dc_voltage_b_reciprocal,
                          m_env.current_referencer_b_active,
                          m_env.current_referencer_b_reactive,
                          m_env.current_regulator_b_active,
                          m_env.current_regulator_b_reactive);
    //<>


    //**** Phase C ***
    //-reference constructor
    //m_env.start_control.phase_c.reference.current_bypass_active = FP_ZERO;
    //m_env.start_control.phase_c.reference.current_bypass_reactive = FP_ZERO;
    //
    m_env.start_control.phase_c.reference.current_bypass_active = m_env.start_control.common_ref.active;
    m_env.start_control.phase_c.reference.current_bypass_reactive = m_env.start_control.common_ref.reactive;
    //
    //m_env.start_control.phase_c.test_ref = m_env.start_control.common_ref;

    //-feedback constructor
    m_env.start_control.phase_c.feedback.current_bypass_active = m_env.projection_current_bypass_c.active;
    m_env.start_control.phase_c.feedback.current_bypass_reactive = m_env.projection_current_bypass_c.reactive;
    //
    m_env.start_control.phase_c.feedback.current_cell_active = m_env.projection_current_cell_c.active;
    m_env.start_control.phase_c.feedback.current_cell_reactive = m_env.projection_current_cell_c.reactive;
    //
    //-control
    _execute_single_phase(m_env.start_control.phase_c,
                          m_env.main_abc_orts.phase_c,
                          m_env.cell_dc_voltage_c_reciprocal,
                          m_env.current_referencer_c_active,
                          m_env.current_referencer_c_reactive,
                          m_env.current_regulator_c_active,
                          m_env.current_regulator_c_reactive);
    //<>

    m_voltage_a = m_env.start_control.phase_a.reference.voltage_cell_relative;
    m_voltage_b = m_env.start_control.phase_b.reference.voltage_cell_relative;
    m_voltage_c = m_env.start_control.phase_c.reference.voltage_cell_relative;
    //
}//
#endif
//
#if TYPECONTROL == SCALARCONTROL
#pragma CODE_SECTION("ramfuncs");
inline void AlgorithmStart::_execute_single_phase(SYSCTRL::AlgorithmStartSinglePhaseControl& phase,
                                                  SYSCTRL::VectorOrthogonalProjection& orts,
                                                  float dc_voltage_reciprocal,
                                                  FLTSYSLIB::PIController& referencer_active,
                                                  FLTSYSLIB::PIController& referencer_reactive,
                                                  FLTSYSLIB::PIController& regulator_active,
                                                  FLTSYSLIB::PIController& regulator_reactive)
{
    //
    phase.reference.current_cell_active = referencer_active.execute(phase.reference.current_bypass_active, phase.feedback.current_bypass_active);
    phase.reference.current_cell_reactive = referencer_reactive.execute(phase.reference.current_bypass_reactive, phase.feedback.current_bypass_reactive);
    //
    phase.reference.voltage_cell_ampl_active = regulator_active.execute(phase.feedback.current_cell_active, phase.reference.current_cell_active);
    phase.reference.voltage_cell_ampl_reactive = regulator_reactive.execute(phase.feedback.current_cell_reactive, phase.reference.current_cell_reactive);
    //
    //phase.reference.voltage_cell_ampl_active = regulator_active.execute(phase.feedback.current_cell_active, phase.test_ref.active);
    //phase.reference.voltage_cell_ampl_reactive = regulator_reactive.execute(phase.feedback.current_cell_reactive, phase.test_ref.reactive);
    //
    phase.reference.voltage_cell_active = phase.reference.voltage_cell_ampl_active * orts.active;
    phase.reference.voltage_cell_reactive = phase.reference.voltage_cell_ampl_reactive * orts.reactive;
    //
    phase.reference.voltage_cell = phase.reference.voltage_cell_active - phase.reference.voltage_cell_reactive;
    phase.reference.voltage_cell_relative = phase.reference.voltage_cell * dc_voltage_reciprocal;
    //
}//
#endif
//
#if TYPECONTROL == DIRECTREVERSECONTROL
#pragma CODE_SECTION("ramfuncs");
void AlgorithmStart::_execute_run()
{
    //
    m_env.hardware.ref_control_order = ORDER_START;
    //
    m_env.timer_start.execute();
    //
    m_reference_current_cell_direct = m_env.drc_referencer_current_bypass_direct.execute(m_reference_zero, m_env.drc_current_bypass_direct);
    m_reference_current_cell_quadrature = m_env.drc_referencer_current_bypass_quadrature.execute(m_reference_zero, m_env.drc_current_bypass_quadrature);
    //
    m_env.drc_positive_voltage_cell_direct = m_env.drc_regulator_current_load_direct.execute(m_env.drc_current_cell_direct, m_reference_current_cell_direct);
    m_env.drc_positive_voltage_cell_quadrature = m_env.drc_regulator_current_load_quadrature.execute(m_env.drc_current_cell_quadrature, m_reference_current_cell_quadrature);
    //
    FLTSYSLIB::Transformation::park_inverse(m_env.main_ab_orts.active,
                                            m_env.main_ab_orts.reactive,
                                            m_env.drc_positive_voltage_cell_direct,
                                            m_env.drc_positive_voltage_cell_quadrature,
                                            m_env.drc_positive_voltage_cell_alpha,
                                            m_env.drc_positive_voltage_cell_beta);
    //
    FLTSYSLIB::Transformation::clarke_inverse(m_env.drc_positive_voltage_cell_alpha,
                                              m_env.drc_positive_voltage_cell_beta,
                                              m_env.drc_positive_voltage_cell_a,
                                              m_env.drc_positive_voltage_cell_b,
                                              m_env.drc_positive_voltage_cell_c);
    //
    m_env.drc_voltage_cell_a = m_env.drc_positive_voltage_cell_a;
    m_env.drc_voltage_cell_b = m_env.drc_positive_voltage_cell_b;
    m_env.drc_voltage_cell_c = m_env.drc_positive_voltage_cell_c;
    //
    m_voltage_a = m_env.drc_voltage_cell_a * m_env.cell_dc_voltage_a_reciprocal;
    m_voltage_b = m_env.drc_voltage_cell_b * m_env.cell_dc_voltage_b_reciprocal;
    m_voltage_c = m_env.drc_voltage_cell_c * m_env.cell_dc_voltage_c_reciprocal;

}//
//
#endif


} /* namespace SYSCTRL */