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

#include "SYSCTRL/AlgorithmContext.h"

namespace SYSCTRL
{
//CONSTRUCTOR
AlgorithmContext::AlgorithmContext(SYSCTRL::SystemEnvironment&  env):
        m_mode(SYSCTRL::AlgorithmContext::UNDEFINED),
        m_algorithm(SYSCTRL::AlgorithmContext::UNKNOWN),
        m_algorithm_previous(SYSCTRL::AlgorithmContext::UNKNOWN),
        m_env(env),
        m_algorithm_pointer(&m_off),
        m_fault(env),
        m_work(env),
        m_stop(env),
        m_start(env),
        m_off(env),
        m_source(env),
        _strategy(&SYSCTRL::AlgorithmContext::_strategy_undef),
        _set_fault(&SYSCTRL::AlgorithmContext::_set_empty_exe),
        _set_work(&SYSCTRL::AlgorithmContext::_set_empty_exe),
        _set_stop(&SYSCTRL::AlgorithmContext::_set_empty_exe),
        _set_start(&SYSCTRL::AlgorithmContext::_set_empty_exe),
        _set_off(&SYSCTRL::AlgorithmContext::_set_empty_exe),
        _set_source(&SYSCTRL::AlgorithmContext::_set_empty_exe)
//
{}//CONSTRUCTOR


void AlgorithmContext::setup()
{
    static bool status = true;
    if(m_mode == SYSCTRL::AlgorithmContext::UNDEFINED)
    {

        m_algorithm = SYSCTRL::AlgorithmContext::UNKNOWN;
        m_algorithm_previous = SYSCTRL::AlgorithmContext::UNKNOWN;

        m_fault.setup();
        m_work.setup();
        m_stop.setup();
        m_start.setup();
        m_off.setup();
        m_source.setup();

        _strategy = &SYSCTRL::AlgorithmContext::_strategy_undef;
        _set_fault = &SYSCTRL::AlgorithmContext::_set_empty_exe;
        _set_work = &SYSCTRL::AlgorithmContext::_set_empty_exe;
        _set_stop = &SYSCTRL::AlgorithmContext::_set_empty_exe;
        _set_start = &SYSCTRL::AlgorithmContext::_set_empty_exe;
        _set_off = &SYSCTRL::AlgorithmContext::_set_empty_exe;
        _set_source = &SYSCTRL::AlgorithmContext::_set_empty_exe;

        if(status)
        {
            m_mode = SYSCTRL::AlgorithmContext::CONFIGURATE;
            //
        }//if
        //
    }//if
    //
}//
//
void AlgorithmContext::configure()
{
    static bool status = true;
    if(m_mode == SYSCTRL::AlgorithmContext::CONFIGURATE)
    {

        _strategy = &SYSCTRL::AlgorithmContext::_strategy_off;
        _set_fault = &SYSCTRL::AlgorithmContext::_set_fault_exe;
        _set_work = &SYSCTRL::AlgorithmContext::_set_work_exe;
        _set_stop = &SYSCTRL::AlgorithmContext::_set_stop_exe;
        _set_start = &SYSCTRL::AlgorithmContext::_set_start_exe;
        _set_off = &SYSCTRL::AlgorithmContext::_set_off_exe;
        _set_source = &SYSCTRL::AlgorithmContext::_set_source_exe;

        if(status)
        {
            m_algorithm = SYSCTRL::AlgorithmContext::OFF;
            m_algorithm_previous = SYSCTRL::AlgorithmContext::OFF;

            m_mode = SYSCTRL::AlgorithmContext::OPERATIONAL;
            set_strategy_off();
            set_off();
            //
        }//if
        //
    }//if
    //
}//
//
SYSCTRL::AlgorithmContext::mode_t AlgorithmContext::get_mode()
{
    return m_mode;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
SYSCTRL::AlgorithmContext::algorithm_t AlgorithmContext::get_algorithm()
{
    return m_algorithm;
}//
//
bool AlgorithmContext::compare(const SYSCTRL::AlgorithmContext::mode_t mode)
{
    return mode == m_mode;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
bool AlgorithmContext::compare_algorithm(const SYSCTRL::AlgorithmContext::algorithm_t algorithm)
{
    return algorithm == m_algorithm;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::get_ref_invertor_voltage(float& volt_a, float& volt_b, float& volt_c)
{
    m_algorithm_pointer->get_ref_invertor_voltage(volt_a, volt_b, volt_c);
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::strategy()
{
    (this->*_strategy)();
    //
}//
//
void AlgorithmContext::_strategy_undef()
{}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::_strategy_fault()
{
    if((!m_env.system_fault.boolbit.b0)&&(m_env.system_reset.boolbit.b0))
    {
        set_strategy_off();
        set_off();
        //
    }//if
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::_strategy_work()
{
    if(m_env.system_fault.boolbit.b0)
    {
        m_env.enable_work_reset.boolbit.b0 = true;
        set_strategy_fault();
        set_fault();
    }
    else
    {
        //
        if(!m_env.enable_work.boolbit.b0)
        {
            m_env.enable_work_reset.boolbit.b0 = true;
            set_strategy_off();
            set_off();
            //
        }else{

            if((m_env.local_remote.state.signal.is_off & m_env.remote_stop.state.signal.is_switched_on) |
               ((m_env.local_remote.state.signal.is_on & m_env.external_stop.signal.is_on)))
                {
                    //m_env.reference_intensity_voltage_load_direct.set_output(m_env.voltage_load_active);
                    //m_env.reference_intensity_voltage_load_quadrature.set_output(m_env.voltage_load_reactive);

                    set_strategy_stop();
                    set_stop();
                    //
                  }//if
        }
        //
    }//if else
    //
}//
//

#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::_strategy_stop()
{
    if(m_env.system_fault.boolbit.b0)
    {
        m_env.enable_work_reset.boolbit.b0 = true;
        set_strategy_fault();
        set_fault();
    }
    else
    {
        if(!m_env.enable_work.boolbit.b0 |
           //(m_env.local_remote.state.signal.is_off & m_env.remote_stop.state.signal.is_switched_on) |
           //(m_env.local_remote.state.signal.is_on & m_env.external_stop.signal.is_on)
           m_env.auxiliary_km2.state.signal.is_on
           )
        {
            m_env.enable_work_reset.boolbit.b0 = true;
            set_strategy_off();
            set_off();
            //
        }
        //
    }//if else
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::_strategy_start()
{
    if(m_env.system_fault.boolbit.b0)
    {
        m_env.enable_work_reset.boolbit.b0 = true;
        set_strategy_fault();
        set_fault();
    }
    else
    {
        if(!m_env.enable_work.boolbit.b0)
        {
            m_env.enable_work_reset.boolbit.b0 = true;
            set_strategy_off();
            set_off();
            //
        }
        else
        {

            if((m_env.local_remote.state.signal.is_off & m_env.remote_stop.state.signal.is_switched_on) |
               ((m_env.local_remote.state.signal.is_on & m_env.external_stop.signal.is_on))
               )
            {
                m_env.enable_work_reset.boolbit.b0 = true;
                set_strategy_off();
                set_off();
                //
            }//if



            if(m_env.auxiliary_km2.state.signal.is_off)
            {
                //m_env.regulator_voltage_load_a.set_output(m_env.reference.start.voltage_a);
                //m_env.regulator_voltage_load_b.set_output(m_env.reference.start.voltage_b);
                //m_env.regulator_voltage_load_c.set_output(m_env.reference.start.voltage_c);
                //
                //m_env.current_regulator_active.reset();
                //m_env.current_regulator_reactive.reset();
                //
                set_strategy_work();
                set_work();
                //
                //set_strategy_stop();
                //set_stop();
                //
            }//if

            //
        }//if else
        //
    }//if else
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::_strategy_off()
{
    if(m_env.system_fault.boolbit.b0)
    {
        set_strategy_fault();
        set_fault();
    }
    else
    {
        if(m_env.enable_work_is_on.boolbit.b0)
        {
            set_strategy_start();
            set_start();
            //
            //set_strategy_source();
            //set_source();
            //
            //set_strategy_stop();
            //set_stop();
            //
        }//if
        //
    }//if else
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::_strategy_source()
{
    if(m_env.system_fault.boolbit.b0)
    {
        m_env.enable_work_reset.boolbit.b0 = true;
        set_strategy_fault();
        set_fault();
    }
    else
    {
        //
        if((!m_env.enable_work.boolbit.b0) |
           (m_env.local_remote.state.signal.is_off & m_env.remote_stop.state.signal.is_switched_on) |
           ((m_env.local_remote.state.signal.is_on & m_env.external_stop.signal.is_on))
          )
        {
            m_env.enable_work_reset.boolbit.b0 = true;
            set_strategy_off();
            set_off();
            //
        }//if
        //
    }//if else
    //
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::set_strategy_fault()
{
    _strategy = &SYSCTRL::AlgorithmContext::_strategy_fault;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::set_strategy_work()
{
    _strategy = &SYSCTRL::AlgorithmContext::_strategy_work;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::set_strategy_stop()
{
    _strategy = &SYSCTRL::AlgorithmContext::_strategy_stop;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::set_strategy_start()
{
    _strategy = &SYSCTRL::AlgorithmContext::_strategy_start;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::set_strategy_off()
{
    _strategy = &SYSCTRL::AlgorithmContext::_strategy_off;
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::set_strategy_source()
{
    _strategy = &SYSCTRL::AlgorithmContext::_strategy_source;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::set_fault()
{
    (this->*_set_fault)();
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::set_work()
{
    (this->*_set_work)();
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::set_stop()
{
    (this->*_set_stop)();
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::set_start()
{
    (this->*_set_start)();
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::set_off()
{
    (this->*_set_off)();
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::set_source()
{
    (this->*_set_source)();
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::execute()
{
    m_algorithm_pointer->execute();
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::_set_fault_exe()
{
    //
    m_algorithm_previous = m_algorithm;
    m_algorithm = SYSCTRL::AlgorithmContext::FAULT;
    //
    _set_fault = &SYSCTRL::AlgorithmContext::_set_empty_exe;
    _set_work = &SYSCTRL::AlgorithmContext::_set_work_exe;
    _set_stop = &SYSCTRL::AlgorithmContext::_set_stop_exe;
    _set_start = &SYSCTRL::AlgorithmContext::_set_start_exe;
    _set_off = &SYSCTRL::AlgorithmContext::_set_off_exe;
    _set_source = &SYSCTRL::AlgorithmContext::_set_source_exe;
    //
    m_env.timer_start.reset();
    m_env.timer_stop.reset();
    //
    m_algorithm_pointer = &m_fault;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::_set_work_exe()
{
    //
    m_algorithm_previous = m_algorithm;
    m_algorithm = SYSCTRL::AlgorithmContext::WORK;
    //
    _set_fault = &SYSCTRL::AlgorithmContext::_set_fault_exe;
    _set_work = &SYSCTRL::AlgorithmContext::_set_empty_exe;
    _set_stop = &SYSCTRL::AlgorithmContext::_set_stop_exe;
    _set_start = &SYSCTRL::AlgorithmContext::_set_start_exe;
    _set_off = &SYSCTRL::AlgorithmContext::_set_off_exe;
    _set_source = &SYSCTRL::AlgorithmContext::_set_source_exe;
    //
    //
#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_low_saturation();
    m_env.regulator_current_pfc.reset();
    //
#endif

#if TYPECONTROL == SCALARCONTROL
    //
    //m_env.regulator_dc_a.reset();
    //m_env.regulator_dc_b.reset();
    //m_env.regulator_dc_c.reset();
    //
    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();


#if SCALARCONTROL_VOLTAGEREGULATOR  == SCALARCONTROL_VOLTAGEREGULATOR_ABC
    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 SCALARCONTROL_VOLTAGEREGULATOR  == SCALARCONTROL_VOLTAGEREGULATOR_RMS
    m_env.reference_voltage_load_rms_intensity.set_output(m_env.rms_voltage_load_module);
    m_env.regulator_voltage_load_rms.reset();
#endif
    //
#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_regulator_current_limit.set_to_low_saturation();
    m_env.drc_regulator_current_pfc.reset();

    m_env.drc_reference_voltage_direct_intensity.set_output(m_env.drc_voltage_grid_direct);
    m_work.reset_switcher();
    //
#endif

    //
    m_env.timer_start.reset();
    m_env.timer_stop.reset();
    //
    m_algorithm_pointer = &m_work;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::_set_stop_exe()
{
    //
    m_algorithm_previous = m_algorithm;
    m_algorithm = SYSCTRL::AlgorithmContext::STOP;
    //
    _set_fault = &SYSCTRL::AlgorithmContext::_set_fault_exe;
    _set_work = &SYSCTRL::AlgorithmContext::_set_work_exe;
    _set_stop = &SYSCTRL::AlgorithmContext::_set_empty_exe;
    _set_start = &SYSCTRL::AlgorithmContext::_set_start_exe;
    _set_off = &SYSCTRL::AlgorithmContext::_set_off_exe;
    _set_source = &SYSCTRL::AlgorithmContext::_set_source_exe;
    //
#if TYPECONTROL == VECTORCONTROL
    m_env.reference_voltage_direct_intensity.set_output(m_env.regulator_current_limit.get_output());
#endif

#if TYPECONTROL == DIRECTREVERSECONTROL
    //m_env.drc_reference_voltage_direct_intensity.set_output(m_env.drc_voltage_grid_direct);
    m_env.drc_reference_voltage_direct_intensity.set_output(m_env.drc_regulator_current_limit.get_output());
#endif

    //
    //
    /* FOR DEBUG ONLY!!!! */
    /*
    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();
    */

    /* FOR DEBUG ONLY!!!! */
    /* VECTOR CONTROL */
    /*
    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.timer_start.reset();
    m_env.timer_stop.start();
    //
    m_algorithm_pointer = &m_stop;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::_set_start_exe()
{
    //
    m_algorithm_previous = m_algorithm;
    m_algorithm = SYSCTRL::AlgorithmContext::START;
    //
    _set_fault = &SYSCTRL::AlgorithmContext::_set_fault_exe;
    _set_work = &SYSCTRL::AlgorithmContext::_set_work_exe;
    _set_stop = &SYSCTRL::AlgorithmContext::_set_stop_exe;
    _set_start = &SYSCTRL::AlgorithmContext::_set_empty_exe;
    _set_off = &SYSCTRL::AlgorithmContext::_set_off_exe;
    _set_source = &SYSCTRL::AlgorithmContext::_set_source_exe;
    //
#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();
    //
    m_env.drc_positive_voltage_cell_direct = FP_ZERO;
    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_positive_voltage_cell_alpha = FP_ZERO;
    m_env.drc_positive_voltage_cell_beta = FP_ZERO;
    m_env.drc_negative_voltage_cell_alpha = FP_ZERO;
    m_env.drc_negative_voltage_cell_beta = FP_ZERO;
    //
    m_env.drc_positive_voltage_cell_a = FP_ZERO;
    m_env.drc_positive_voltage_cell_b = FP_ZERO;
    m_env.drc_positive_voltage_cell_c = 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;
    //
#endif


    //
    m_env.timer_start.start();
    m_env.timer_stop.reset();
    //
    m_algorithm_pointer = &m_start;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::_set_off_exe()
{
    //
    m_algorithm_previous = m_algorithm;
    m_algorithm = SYSCTRL::AlgorithmContext::OFF;
    //
    _set_fault = &SYSCTRL::AlgorithmContext::_set_fault_exe;
    _set_work = &SYSCTRL::AlgorithmContext::_set_work_exe;
    _set_stop = &SYSCTRL::AlgorithmContext::_set_stop_exe;
    _set_start = &SYSCTRL::AlgorithmContext::_set_start_exe;
    _set_off = &SYSCTRL::AlgorithmContext::_set_empty_exe;
    _set_source = &SYSCTRL::AlgorithmContext::_set_source_exe;
    //
    m_env.timer_start.reset();
    m_env.timer_stop.reset();
    //
    m_algorithm_pointer = &m_off;
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void AlgorithmContext::_set_source_exe()
{
    //
    m_algorithm_previous = m_algorithm;
    m_algorithm = SYSCTRL::AlgorithmContext::SOURCE;
    //
    _set_fault = &SYSCTRL::AlgorithmContext::_set_fault_exe;
    _set_work = &SYSCTRL::AlgorithmContext::_set_work_exe;
    _set_stop = &SYSCTRL::AlgorithmContext::_set_stop_exe;
    _set_start = &SYSCTRL::AlgorithmContext::_set_start_exe;
    _set_off = &SYSCTRL::AlgorithmContext::_set_off_exe;
    _set_source = &SYSCTRL::AlgorithmContext::_set_empty_exe;
    //
    m_env.timer_start.reset();
    m_env.timer_stop.reset();
    //
    m_algorithm_pointer = &m_source;
    //
}//
//
void AlgorithmContext::_set_empty_exe()
{
    //
}//
//
//
} /* namespace SYSCTRL */