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

#include "SYSCTRL/AlgorithmStop.h"

namespace SYSCTRL
{
//CONSTRUCTOR
AlgorithmStop::AlgorithmStop(SYSCTRL::SystemEnvironment& env):
        SYSCTRL::AlgorithmBase(),
#if ((TYPECONTROL == SCALARCONTROL)&&(SCALARCONTROL_VOLTAGEREGULATOR == SCALARCONTROL_VOLTAGEREGULATOR_ABC))
        m_orts(),
#endif
        m_env(env),
        _execute(&SYSCTRL::AlgorithmStop::_execute_undef)
//
{}//CONSTRUCTOR
//
void AlgorithmStop::setup()
{
    _execute = &SYSCTRL::AlgorithmStop::_execute_run;
    //
}//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmStop::reset()
{

}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmStop::execute()
{
    (this->*_execute)();
    //
}//
//

#if TYPECONTROL == VECTORCONTROL
#pragma CODE_SECTION("ramfuncs");
void AlgorithmStop::_execute_run()
{
    m_env.hardware.ref_control_order = ORDER_START;
    //
    m_env.timer_stop.execute();
    //
    if(m_env.timer_stop.is_finished()){
        //
        m_voltage_a = FP_ZERO;
        m_voltage_b = FP_ZERO;
        m_voltage_c = FP_ZERO;
        //
    }else{
        //
#if TYPE_VOLTAGE_CONTROLLER == VOLTAGE_CONTROLLER_PII

        m_env.voltage_reference_load_direct = m_env.reference_voltage_direct_intensity.execute(m_env.voltage_grid_direct);
        //m_env.voltage_reference_load_direct = 0.33333*(m_env.rms_voltage_input_a + m_env.rms_voltage_input_b + m_env.rms_voltage_input_c);
        //m_env.voltage_reference_load_quadrature = m_env.voltage_grid_quadrature;
        m_env.voltage_reference_load_quadrature = FP_ZERO;
        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

#if TYPE_VOLTAGE_CONTROLLER == VOLTAGE_CONTROLLER_I
        m_env.voltage_reference_load_direct = m_env.reference_voltage_direct_intensity.execute(m_env.voltage_grid_direct);
        m_env.voltage_reference_load_quadrature = FP_ZERO;
        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;
        //
    }//if else
    //
    //
}//
#endif
//
#if ((TYPECONTROL == SCALARCONTROL)&&(SCALARCONTROL_VOLTAGEREGULATOR == SCALARCONTROL_VOLTAGEREGULATOR_ABC))
#pragma CODE_SECTION("ramfuncs");
void AlgorithmStop::_execute_run()
{
    m_env.hardware.ref_control_order = ORDER_START;
    //
    m_env.timer_stop.execute();
    //
    //
    //reference constructor
//    m_env.phase_control.common_ref.voltage_rms_real = m_env.phase_control.common_ref.voltage_module_rms;
    //m_env.phase_control.common_ref.voltage_rms_jm = FP_ZERO;
//    m_env.phase_control.phase_a.reference.voltage_ampl_real_const = m_env.phase_control.common_ref.voltage_rms_real;
    //m_env.phase_control.phase_a.reference.voltage_ampl_jm_const = m_env.phase_control.common_ref.voltage_rms_jm;
//    m_env.phase_control.phase_a.reference.voltage_ampl_jm_const = m_env.projection_voltage_input_a.reactive;
//    m_env.phase_control.phase_b.reference.voltage_ampl_real_const = m_env.phase_control.common_ref.voltage_rms_real;
    //m_env.phase_control.phase_b.reference.voltage_ampl_jm_const = m_env.phase_control.common_ref.voltage_rms_jm;
//    m_env.phase_control.phase_b.reference.voltage_ampl_jm_const = m_env.projection_voltage_input_b.reactive;
//    m_env.phase_control.phase_c.reference.voltage_ampl_real_const = m_env.phase_control.common_ref.voltage_rms_real;
    //m_env.phase_control.phase_c.reference.voltage_ampl_jm_const = m_env.phase_control.common_ref.voltage_rms_jm;
//    m_env.phase_control.phase_c.reference.voltage_ampl_jm_const = m_env.projection_voltage_input_c.reactive;
    //
    //m_env.phase_control.phase_a.reference.voltage_ampl_real = m_env.phase_control.phase_a.reference.voltage_ampl_real_const;
    //m_env.phase_control.phase_a.reference.voltage_ampl_jm = m_env.phase_control.phase_a.reference.voltage_ampl_jm_const;
    //m_env.phase_control.phase_b.reference.voltage_ampl_real = m_env.phase_control.phase_b.reference.voltage_ampl_real_const;
    //m_env.phase_control.phase_b.reference.voltage_ampl_jm = m_env.phase_control.phase_b.reference.voltage_ampl_jm_const;
    //m_env.phase_control.phase_c.reference.voltage_ampl_real = m_env.phase_control.phase_c.reference.voltage_ampl_real_const;
    //m_env.phase_control.phase_c.reference.voltage_ampl_jm = m_env.phase_control.phase_c.reference.voltage_ampl_jm_const;

    m_env.phase_control.phase_a.reference.voltage_ampl_real = m_env.projection_voltage_input_a.active;
    m_env.phase_control.phase_a.reference.voltage_ampl_jm = m_env.projection_voltage_input_a.reactive;
    m_env.phase_control.phase_b.reference.voltage_ampl_real = m_env.projection_voltage_input_b.active;
    m_env.phase_control.phase_b.reference.voltage_ampl_jm = m_env.projection_voltage_input_b.reactive;
    m_env.phase_control.phase_c.reference.voltage_ampl_real = m_env.projection_voltage_input_c.active;
    m_env.phase_control.phase_c.reference.voltage_ampl_jm = m_env.projection_voltage_input_c.reactive;

    //
    //control execute
#if TYPE_ORTS == TYPE_ORTS_NORMAL
    m_orts = m_env.main_abc_orts.phase_a;
    _execute_single_phase(m_env.phase_control.phase_a,
                          m_env.regulator_voltage_load_a_active,
                          m_env.regulator_voltage_load_a_reactive);
    //
    m_orts = m_env.main_abc_orts.phase_b;
    _execute_single_phase(m_env.phase_control.phase_b,
                          m_env.regulator_voltage_load_b_active,
                          m_env.regulator_voltage_load_b_reactive);
    //
    m_orts = m_env.main_abc_orts.phase_c;
    _execute_single_phase(m_env.phase_control.phase_c,
                          m_env.regulator_voltage_load_c_active,
                          m_env.regulator_voltage_load_c_reactive);
    //
#endif
#if TYPE_ORTS == TYPE_ORTS_BACK
    m_orts.active = m_env.main_abc_orts.phase_c.reactive;
    m_orts.reactive = -m_env.main_abc_orts.phase_c.active;
    _execute_single_phase(m_env.phase_control.phase_a,
                          m_env.regulator_voltage_load_a_active,
                          m_env.regulator_voltage_load_a_reactive);
    //
    m_orts.active = m_env.main_abc_orts.phase_a.reactive;
    m_orts.reactive = -m_env.main_abc_orts.phase_a.active;
    _execute_single_phase(m_env.phase_control.phase_b,
                          m_env.regulator_voltage_load_b_active,
                          m_env.regulator_voltage_load_b_reactive);
    //
    m_orts.active = m_env.main_abc_orts.phase_b.reactive;
    m_orts.reactive = -m_env.main_abc_orts.phase_b.active;
    _execute_single_phase(m_env.phase_control.phase_c,
                          m_env.regulator_voltage_load_c_active,
                          m_env.regulator_voltage_load_c_reactive);
    //
#endif
#if TYPE_ORTS == TYPE_ORTS_FORWARD
    m_orts.active = -m_env.main_abc_orts.phase_b.reactive;
    m_orts.reactive = m_env.main_abc_orts.phase_b.active;
    _execute_single_phase(m_env.phase_control.phase_a,
                          m_env.regulator_voltage_load_a_active,
                          m_env.regulator_voltage_load_a_reactive);
    //
    m_orts.active = -m_env.main_abc_orts.phase_c.reactive;
    m_orts.reactive = m_env.main_abc_orts.phase_c.active;
    _execute_single_phase(m_env.phase_control.phase_b,
                          m_env.regulator_voltage_load_b_active,
                          m_env.regulator_voltage_load_b_reactive);
    //
    m_orts.active = -m_env.main_abc_orts.phase_a.reactive;
    m_orts.reactive = m_env.main_abc_orts.phase_a.active;
    _execute_single_phase(m_env.phase_control.phase_c,
                          m_env.regulator_voltage_load_c_active,
                          m_env.regulator_voltage_load_c_reactive);
    //
#endif


    if(m_env.timer_stop.is_finished()){
        m_voltage_a = FP_ZERO;
        m_voltage_b = FP_ZERO;
        m_voltage_c = FP_ZERO;
    }else{
        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)&&(SCALARCONTROL_VOLTAGEREGULATOR == SCALARCONTROL_VOLTAGEREGULATOR_RMS))
#pragma CODE_SECTION("ramfuncs");
void AlgorithmStop::_execute_run()
{
    m_env.hardware.ref_control_order = ORDER_START;
    //
    m_env.timer_stop.execute();
    //
    m_env.phase_control.phase_a.reference.voltage_ampl_real = m_env.reference_voltage_load_rms_intensity.execute(m_env.rms_voltage_load_module);
    m_env.phase_control.phase_a.reference.voltage_cell_real = m_env.regulator_voltage_load_rms.execute(m_env.phase_control.phase_a.reference.voltage_ampl_real, m_env.rms_voltage_load_module);
    //
#if TYPE_ORTS == TYPE_ORTS_NORMAL
    m_env.phase_control.phase_a.reference.voltage_cell = m_env.phase_control.phase_a.reference.voltage_cell_real * m_env.main_abc_orts.phase_a.active;
    m_env.phase_control.phase_b.reference.voltage_cell = m_env.phase_control.phase_a.reference.voltage_cell_real * m_env.main_abc_orts.phase_b.active;
    m_env.phase_control.phase_c.reference.voltage_cell = m_env.phase_control.phase_a.reference.voltage_cell_real * m_env.main_abc_orts.phase_c.active;
#endif
#if TYPE_ORTS == TYPE_ORTS_BACK
    m_env.phase_control.phase_a.reference.voltage_cell = m_env.phase_control.phase_a.reference.voltage_cell_real * m_env.main_abc_orts.phase_c.reactive;
    m_env.phase_control.phase_b.reference.voltage_cell = m_env.phase_control.phase_a.reference.voltage_cell_real * m_env.main_abc_orts.phase_a.reactive;
    m_env.phase_control.phase_c.reference.voltage_cell = m_env.phase_control.phase_a.reference.voltage_cell_real * m_env.main_abc_orts.phase_b.reactive;
#endif
#if TYPE_ORTS == TYPE_ORTS_FORWARD
    m_env.phase_control.phase_a.reference.voltage_cell = m_env.phase_control.phase_a.reference.voltage_cell_real * (-m_env.main_abc_orts.phase_b.reactive);
    m_env.phase_control.phase_b.reference.voltage_cell = m_env.phase_control.phase_a.reference.voltage_cell_real * (-m_env.main_abc_orts.phase_c.reactive);
    m_env.phase_control.phase_c.reference.voltage_cell = m_env.phase_control.phase_a.reference.voltage_cell_real * (-m_env.main_abc_orts.phase_a.reactive);
#endif
    //
    m_env.phase_control.phase_a.reference.voltage_cell_relative = m_env.phase_control.phase_a.reference.voltage_cell * m_env.cell_dc_voltage_a_reciprocal;
    m_env.phase_control.phase_b.reference.voltage_cell_relative = m_env.phase_control.phase_b.reference.voltage_cell * m_env.cell_dc_voltage_b_reciprocal;
    m_env.phase_control.phase_c.reference.voltage_cell_relative = m_env.phase_control.phase_c.reference.voltage_cell * m_env.cell_dc_voltage_c_reciprocal;
    //
    if(m_env.timer_stop.is_finished()){
        m_voltage_a = FP_ZERO;
        m_voltage_b = FP_ZERO;
        m_voltage_c = FP_ZERO;
    }else{
        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)&&(SCALARCONTROL_VOLTAGEREGULATOR == SCALARCONTROL_VOLTAGEREGULATOR_ABC))
#pragma CODE_SECTION("ramfuncs");
inline void AlgorithmStop::_execute_single_phase(SYSCTRL::AlgorithmSinglePhaseControl& phase,
                                                 FLTSYSLIB::PIController& regulator_active,
                                                 FLTSYSLIB::PIController& regulator_reactive)
{
    phase.reference.voltage_cell_ampl_real = regulator_active.execute(phase.reference.voltage_ampl_real, phase.feedback.voltage_ampl_real);
    phase.reference.voltage_cell_ampl_jm = regulator_reactive.execute(phase.reference.voltage_ampl_jm, phase.feedback.voltage_ampl_jm);
    //
    phase.reference.voltage_cell_real = phase.reference.voltage_cell_ampl_real * m_orts.active;
    phase.reference.voltage_cell_jm = phase.reference.voltage_cell_ampl_jm * m_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 AlgorithmStop::_execute_run()
{

    m_env.hardware.ref_control_order = ORDER_START;
    //
    m_env.timer_stop.execute();
    //
    if(m_env.timer_stop.is_finished()){
        //
        m_voltage_a = FP_ZERO;
        m_voltage_b = FP_ZERO;
        m_voltage_c = FP_ZERO;
        //
    }else{
        //

        m_env.drc_voltage_reference_load_direct = m_env.drc_reference_voltage_direct_intensity.execute(m_env.drc_voltage_grid_direct);
        m_env.drc_voltage_reference_load_quadrature = FP_ZERO;
        //
        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 = FP_ZERO;
        m_env.drc_negative_voltage_cell_direct = FP_ZERO;
        m_env.drc_negative_voltage_cell_quadrature = FP_ZERO;
        m_env.drc_negative_voltage_cell_alpha = FP_ZERO;
        m_env.drc_negative_voltage_cell_beta = FP_ZERO;
        m_env.drc_negative_voltage_cell_a = FP_ZERO;
        m_env.drc_negative_voltage_cell_b = FP_ZERO;
        m_env.drc_negative_voltage_cell_c = 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::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;
        //
    }//if else
    //
}//
#endif
} /* namespace SYSCTRL */