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

#include "SYSCTRL/AlgorithmWork.h"

namespace SYSCTRL
{
//CONSTRUCTOR
AlgorithmWork::AlgorithmWork(SYSCTRL::SystemEnvironment& env):
        AlgorithmBase(),
        m_env(env),
        m_reference_switcher(false),
        _execute(&SYSCTRL::AlgorithmWork::_execute_undef)
//
{}//CONSTRUCTOR
//

void AlgorithmWork::setup()
{
    _execute = &SYSCTRL::AlgorithmWork::_execute_run;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmWork::reset()
{
#if TYPECONTROL == VECTORCONTROL
    //
    m_env.regulator_voltage_load_direct.reset();
    m_env.regulator_voltage_load_quadrature.reset();
    //
    m_env.integrator_direct.reset();
    m_env.integrator_quadrature.reset();
    //
    m_env.regulator_current_limit.set_to_high_saturation();
    m_env.regulator_current_pfc.reset();
    //
#endif

#if TYPECONTROL == SCALARCONTROL
    //
    m_env.regulator_current_limit_a.set_to_high_saturation();
    m_env.regulator_current_pfc_a.reset();

    m_env.regulator_current_limit_b.set_to_high_saturation();
    m_env.regulator_current_pfc_b.reset();

    m_env.regulator_current_limit_c.set_to_high_saturation();
    m_env.regulator_current_pfc_c.reset();

    m_env.regulator_voltage_load_a_active.reset();
    m_env.regulator_voltage_load_a_reactive.reset();

    m_env.regulator_voltage_load_b_active.reset();
    m_env.regulator_voltage_load_b_reactive.reset();

    m_env.regulator_voltage_load_c_active.reset();
    m_env.regulator_voltage_load_c_reactive.reset();
    //
#endif

#if TYPECONTROL == DIRECTREVERSECONTROL

    m_env.drc_positive_voltage_controller_direct.reset();
    m_env.drc_positive_voltage_controller_quadrature.reset();
    m_env.drc_negative_voltage_controller_direct.reset();
    m_env.drc_negative_voltage_controller_quadrature.reset();
    //
    m_env.drc_reference_voltage_direct_intensity.reset();
    //
    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();
    //
    m_env.drc_regulator_current_limit.set_to_low_saturation();
    m_env.drc_regulator_current_pfc.reset();
    //
    m_reference_switcher = false;
    //
#endif


    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmWork::reset_switcher()
{
    m_reference_switcher = false;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmWork::execute()
{
    (this->*_execute)();
    //
}//
//


#if TYPECONTROL == VECTORCONTROL
#pragma CODE_SECTION("ramfuncs");
void AlgorithmWork::_execute_run()
{
    //
    m_env.hardware.ref_control_order = ORDER_START;
    //
    //if(m_env.algorithm_control.signal.enable_current_limit)
    if(false)
    {
        m_env.voltage_reference_load_direct = m_env.regulator_current_limit.execute(m_env.rms_current_load_module, m_env.current_reference_limit);
    }
    else
    {
        m_env.regulator_current_limit.set_to_low_saturation();
        m_env.voltage_reference_load_direct = m_env.regulator_current_limit.get_output();
    }
    //
    //if(m_env.algorithm_control.signal.enable_pfc)
    if(false)
    {
        m_env.regulator_current_pfc.reset();
        m_env.voltage_reference_load_quadrature = m_env.regulator_current_pfc.get_output();
    }
    else
    {
        m_env.regulator_current_pfc.reset();
        m_env.voltage_reference_load_quadrature = m_env.regulator_current_pfc.get_output();
    }
    //

    // for PII-Controller
#if TYPE_VOLTAGE_CONTROLLER == VOLTAGE_CONTROLLER_PII
    m_env.voltage_pi_reg_out_direct = m_env.regulator_voltage_load_direct.execute(m_env.voltage_reference_load_direct, m_env.voltage_load_direct);
    //m_env.voltage_pi_reg_out_quadrature = m_env.regulator_voltage_load_quadrature.execute(m_env.voltage_reference_load_quadrature, m_env.voltage_load_quadrature);
    m_env.voltage_pi_reg_out_quadrature = FP_ZERO;
    m_env.voltage_cell_direct = m_env.integrator_direct.execute(m_env.voltage_pi_reg_out_direct);
    //m_env.voltage_cell_quadrature = m_env.integrator_quadrature.execute(m_env.voltage_pi_reg_out_quadrature);
    m_env.voltage_cell_quadrature = FP_ZERO;
#endif

    // for I-Controller
#if TYPE_VOLTAGE_CONTROLLER == VOLTAGE_CONTROLLER_I
    m_env.voltage_cell_direct = m_env.integrator_direct.execute(m_env.voltage_reference_load_direct - m_env.voltage_load_direct);
    m_env.voltage_cell_quadrature = FP_ZERO;
#endif
    //
    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 AlgorithmWork::_execute_run()
{

    m_env.hardware.ref_control_order = ORDER_START;

    m_env.phase_control.phase_a.reference.voltage_dc = m_env.phase_control.common_ref.voltage_dc;
    m_env.phase_control.phase_b.reference.voltage_dc = m_env.phase_control.common_ref.voltage_dc;
    m_env.phase_control.phase_c.reference.voltage_dc = m_env.phase_control.common_ref.voltage_dc;

    m_env.phase_control.phase_a.reference.current_ampl_limit_const = m_env.phase_control.common_ref.current_limit_rms;
    m_env.phase_control.phase_b.reference.current_ampl_limit_const = m_env.phase_control.common_ref.current_limit_rms;
    m_env.phase_control.phase_c.reference.current_ampl_limit_const = m_env.phase_control.common_ref.current_limit_rms;

    m_env.phase_control.phase_a.reference.current_ampl_pfc_const = m_env.phase_control.common_ref.current_pfc_rms;
    m_env.phase_control.phase_b.reference.current_ampl_pfc_const = m_env.phase_control.common_ref.current_pfc_rms;
    m_env.phase_control.phase_c.reference.current_ampl_pfc_const = m_env.phase_control.common_ref.current_pfc_rms;



    _execute_single_phase(m_env.phase_control.phase_a,
                          m_env.projection_voltage_input_a,
                          m_env.main_abc_orts.phase_a,
                          m_env.regulator_current_limit_a,
                          m_env.regulator_current_pfc_a,
                          m_env.regulator_dc_a,
                          m_env.cell_dc_voltage_a,
                          m_env.regulator_voltage_load_a_active,
                          m_env.regulator_voltage_load_a_reactive);

    _execute_single_phase(m_env.phase_control.phase_b,
                          m_env.projection_voltage_input_b,
                          m_env.main_abc_orts.phase_b,
                          m_env.regulator_current_limit_b,
                          m_env.regulator_current_pfc_b,
                          m_env.regulator_dc_b,
                          m_env.cell_dc_voltage_b,
                          m_env.regulator_voltage_load_b_active,
                          m_env.regulator_voltage_load_b_reactive);

    _execute_single_phase(m_env.phase_control.phase_c,
                          m_env.projection_voltage_input_c,
                          m_env.main_abc_orts.phase_c,
                          m_env.regulator_current_limit_c,
                          m_env.regulator_current_pfc_c,
                          m_env.regulator_dc_c,
                          m_env.cell_dc_voltage_c,
                          m_env.regulator_voltage_load_c_active,
                          m_env.regulator_voltage_load_c_reactive);

    m_voltage_a = m_env.phase_control.phase_a.reference.voltage_cell_relative;
    m_voltage_b = m_env.phase_control.phase_b.reference.voltage_cell_relative;
    m_voltage_c = m_env.phase_control.phase_c.reference.voltage_cell_relative;
    //
}//
#endif
//
#if TYPECONTROL == SCALARCONTROL
#pragma CODE_SECTION("ramfuncs");
void AlgorithmWork::_execute_single_phase(SYSCTRL::AlgorithmSinglePhaseControl& phase,
                                          SYSCTRL::ProjectionAnalogSignalStructure& projection,
                                          SYSCTRL::VectorOrthogonalProjection& orts,
                                          FLTSYSLIB::PIController& regulator_limit,
                                          FLTSYSLIB::PIController& regulator_pfc,
                                          FLTSYSLIB::PIController& regulator_dc,
                                          float& dc_volatage,
                                          FLTSYSLIB::PIController& regulator_active,
                                          FLTSYSLIB::PIController& regulator_reactive)
{


    if(phase.control_bit.signal.enable_current_limit)
    {

        phase.reference.voltage_limit = regulator_limit.execute(phase.reference.current_ampl_limit_const, phase.feedback.current_ampl_real);

        if(phase.control_bit.signal.enable_pfc)
        {
            phase.reference.voltage_pfc = regulator_pfc.execute(phase.reference.current_ampl_pfc_const, phase.feedback.current_ampl_jm) +
                                          regulator_dc.execute(phase.reference.voltage_dc, dc_volatage);
        }
        else
        {
            regulator_pfc.reset();
            phase.reference.voltage_pfc = projection.reactive;
            //
        }//if else
    }
    else
    {
        regulator_limit.set_to_high_saturation();
        phase.reference.voltage_limit = regulator_limit.get_output();
        //
        regulator_pfc.reset();
        phase.reference.voltage_pfc = projection.reactive;
        //
    }//if else


    if(phase.control_bit.signal.enable_pfc)
    {
        phase.reference.voltage_ampl_real = sqrtf(phase.reference.voltage_limit * phase.reference.voltage_limit - phase.reference.voltage_pfc * phase.reference.voltage_pfc);
        phase.reference.voltage_ampl_jm = phase.reference.voltage_pfc;
        //
    }
    else
    {
        phase.reference.voltage_ampl_real = phase.reference.voltage_limit;
        phase.reference.voltage_ampl_jm = phase.reference.voltage_pfc;
        //
    }//if else
    //
    phase.reference.voltage_cell_ampl_real = regulator_active.execute(phase.reference.voltage_ampl_real, phase.feedback.voltage_ampl_real);
    //
    if(phase.control_bit.signal.enable_pfc)
    {
        phase.reference.voltage_cell_ampl_jm = regulator_reactive.execute(phase.reference.voltage_ampl_jm, phase.feedback.voltage_ampl_jm);
        //
    }
    else
    {
        phase.reference.voltage_cell_ampl_jm = FP_ZERO;
        //
    }//if
    //
    phase.reference.voltage_cell_real = phase.reference.voltage_cell_ampl_real * orts.active;
    phase.reference.voltage_cell_jm = phase.reference.voltage_cell_ampl_jm * orts.reactive;
    phase.reference.voltage_cell = phase.reference.voltage_cell_real - phase.reference.voltage_cell_jm;
    //
    phase.reference.voltage_cell_relative = phase.reference.voltage_cell * phase.feedback.voltage_cell_dc_reciprocal;
    //
}//
#endif
//


#if TYPECONTROL == DIRECTREVERSECONTROL
#pragma CODE_SECTION("ramfuncs");
void AlgorithmWork::_execute_run()
{

    //
    m_env.hardware.ref_control_order = ORDER_START;
    //

    if(m_reference_switcher)
    {
        //if(m_env.algorithm_control.signal.enable_current_limit)
        if(false)
        {
            m_env.drc_voltage_reference_load_direct = m_env.drc_regulator_current_limit.execute(m_env.rms_current_load_module, m_env.drc_current_reference_limit);
        }
        else
        {
            m_env.drc_regulator_current_limit.set_to_low_saturation();
            m_env.drc_voltage_reference_load_direct = m_env.drc_regulator_current_limit.get_output();
        }
    }
    else
    {
        m_env.drc_voltage_reference_load_direct = m_env.drc_reference_voltage_direct_intensity.execute(m_env.drc_regulator_current_limit.get_output());
        //
        if(m_env.drc_voltage_reference_load_direct >= m_env.drc_regulator_current_limit.get_output())
        {
            m_reference_switcher = true;
        }
    }

    //
    //if(m_env.algorithm_control.signal.enable_pfc)
    if(false)
    {
        m_env.drc_regulator_current_pfc.reset();
        m_env.drc_voltage_reference_load_quadrature = m_env.drc_regulator_current_pfc.get_output();
    }
    else
    {
        m_env.drc_regulator_current_pfc.reset();
        m_env.drc_voltage_reference_load_quadrature = m_env.drc_regulator_current_pfc.get_output();
    }
    //
    m_env.drc_positive_voltage_cell_direct = m_env.drc_positive_voltage_controller_direct.execute(m_env.drc_voltage_reference_load_direct, m_env.drc_positive_voltage_load_direct);
    //m_env.drc_positive_voltage_cell_quadrature = m_env.drc_positive_voltage_controller_quadrature.execute(m_env.drc_voltage_reference_load_quadrature, m_env.drc_positive_voltage_load_quadrature);
    m_env.drc_positive_voltage_cell_quadrature = FP_ZERO;
    m_env.drc_negative_voltage_cell_direct = m_env.drc_negative_voltage_controller_direct.execute(m_env.drc_voltage_reference_zero, m_env.drc_negaative_voltage_load_direct);
    m_env.drc_negative_voltage_cell_quadrature = m_env.drc_negative_voltage_controller_quadrature.execute(m_env.drc_voltage_reference_zero, m_env.drc_negative_voltage_load_quadrature);
    //m_env.drc_negative_voltage_cell_direct = FP_ZERO;
    //m_env.drc_negative_voltage_cell_quadrature = FP_ZERO;
    //
    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::park_inverse(m_env.main_ab_orts.active,
                                            m_env.main_ab_orts.reactive,
                                            m_env.drc_negative_voltage_cell_direct,
                                            m_env.drc_negative_voltage_cell_quadrature,
                                            m_env.drc_negative_voltage_cell_alpha,
                                            m_env.drc_negative_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);
    //
    FLTSYSLIB::Transformation::clarke_inverse_back_order(m_env.drc_negative_voltage_cell_alpha,
                                                         m_env.drc_negative_voltage_cell_beta,
                                                         m_env.drc_negative_voltage_cell_a,
                                                         m_env.drc_negative_voltage_cell_b,
                                                         m_env.drc_negative_voltage_cell_c);
    //
    m_env.drc_voltage_cell_a = m_env.drc_positive_voltage_cell_a + m_env.drc_negative_voltage_cell_a;
    m_env.drc_voltage_cell_b = m_env.drc_positive_voltage_cell_b + m_env.drc_negative_voltage_cell_b;
    m_env.drc_voltage_cell_c = m_env.drc_positive_voltage_cell_c + m_env.drc_negative_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 */