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

#include "SYSCTRL/DRCDecomposer.h"

namespace SYSCTRL
{
//CONSTRUCTOR
DRCDecomposer::DRCDecomposer():
        m_time_sample(-1.0),
        m_filter_direct(),
        m_filter_quadrature(),
        _aux_reg(FP_ZERO),
        direct(FP_ZERO),
        quadrature(FP_ZERO),
        _execute(&SYSCTRL::DRCDecomposer::_execute_undef)
{}//CONSTRUCTOR

void DRCDecomposer::setup(float time_sample)
{
    if(time_sample > FP_ZERO)
    {
        m_time_sample = time_sample;
        m_filter_direct.setup(m_time_sample);
        m_filter_quadrature.setup(m_time_sample);
    }
    //
}//
//
void DRCDecomposer::configure(const DRCDecomposerConfiguration& config)
{
    if(m_time_sample > FP_ZERO)
    {
        m_filter_direct.configure(config.filter);
        m_filter_quadrature.configure(config.filter);

        if(m_filter_direct.compare(FLTSYSLIB::Filter::OPERATIONAL) && m_filter_quadrature.compare(FLTSYSLIB::Filter::OPERATIONAL))
        //if(m_filter_direct.compare(FLTSYSLIB::FilterSecond::OPERATIONAL) && m_filter_quadrature.compare(FLTSYSLIB::FilterSecond::OPERATIONAL))
        {
            _execute = &SYSCTRL::DRCDecomposer::_execute_operational;
            //
        }//if
        //
    }//if
    //
}//
//
void DRCDecomposer::reset()
{
    m_filter_direct.reset();
    m_filter_quadrature.reset();
    this->direct = FP_ZERO;
    this->quadrature = FP_ZERO;
    //
}//
//
void DRCDecomposer::get_outputs(float& outd, float& outq)
{
    this->direct = m_filter_direct.get_output();
    this->quadrature = m_filter_quadrature.get_output();
    outd = this->direct;
    outq = this->quadrature;
    //
}//
//
void DRCDecomposer::execute(float in_a, float in_b, float in_c,
                            float ort_direct_a, float ort_direct_b, float ort_direct_c,
                            float ort_orthogonal_a, float ort_orthogonal_b, float ort_orthogonal_c)
{
    (this->*_execute)(in_a, in_b, in_c,
                      ort_direct_a, ort_direct_b, ort_direct_c,
                      ort_orthogonal_a, ort_orthogonal_b, ort_orthogonal_c);
    //
}//
//
void DRCDecomposer::_execute_undef(float in_a, float in_b, float in_c,
                    float ort_direct_a, float ort_direct_b, float ort_direct_c,
                    float ort_orthogonal_a, float ort_orthogonal_b, float ort_orthogonal_c)
{
    //
}//
//
#pragma CODE_SECTION("ramfuncs");
void DRCDecomposer::_execute_operational(float in_a, float in_b, float in_c,
                          float ort_direct_a, float ort_direct_b, float ort_direct_c,
                          float ort_orthogonal_a, float ort_orthogonal_b, float ort_orthogonal_c)
{
    _aux_reg = (0.4714045)*(in_a * ort_direct_a + in_b * ort_direct_b + in_c * ort_direct_c);
    direct = m_filter_direct.execute(_aux_reg);
    _aux_reg = (-0.4714045)*(in_a * ort_orthogonal_a + in_b * ort_orthogonal_b + in_c * ort_orthogonal_c);
    quadrature = m_filter_quadrature.execute(_aux_reg);
    //
}//
//


} /* namespace SYSCTRL */