#include "gtl_math_apfc.h"

namespace gtl {
    namespace math {

        apfc::apfc(gtl::analog_data *ref, gtl::analog_data *data) :
            _is_acq(true),
            _av(1),
            _afc(afcs::magnitude),
            _pfc(pfcs::deg),
            _av_cnt(0),
            _av_filled(false),
            _threshold(50),
            _ref(nullptr),
            _data(nullptr)
        {
            _device = static_cast<gtl::device*>(data->root());

            if(_device)
                _device->lock_ai();

            _ref = new gtl::math::fft(ref);
            _data = new gtl::math::fft(data);

            connect(_device, &gtl::device::recieved_data, this, &gtl::math::apfc::device_recieved_data);
            connect(_data, &gtl::math::fft::initialized, this, &apfc::init);
            connect(_data, &gtl::math::fft::frequency_changed, this, &apfc::frequency_changed);
            connect(_data, &gtl::math::fft::resolution_changed, this, &apfc::resolution_changed);
            connect(_data, &gtl::math::fft::window_changed, this, &apfc::window_changed);
            connect(_data, &gtl::math::fft::lines_changed, this, &apfc::lines_changed);
            connect(_data, &gtl::math::fft::overlap_changed, this, &apfc::overlap_changed);

            init();

            if(_device)
                _device->unlock_ai();
        }

        apfc::~apfc()
        {
            emit deleting();

            cleanup();
            if(_ref)
                delete _ref;
            if(_data)
                delete _data;
        }

        void apfc::set_ref(analog_data *ref)
        {
            if(_device)
                _device->lock_ai();

            if(_ref)
                delete _ref;

            _ref = new gtl::math::fft(ref);

            _ref->set_frequency(_data->frequency());
            _ref->set_resolution(_data->resolution());
            _ref->set_overlap(_data->overlap());
            _ref->set_window(_data->window());

            init();

            if(_device)
                _device->unlock_ai();
        }

        void apfc::set_data(analog_data *data)
        {
            _device = static_cast<gtl::device*>(data->root());

            if(_device)
                _device->lock_ai();

            disconnect(_device, &gtl::device::recieved_data, this, &gtl::math::apfc::device_recieved_data);
            disconnect(_data, &gtl::math::fft::initialized, this, &apfc::init);
            disconnect(_data, &gtl::math::fft::frequency_changed, this, &apfc::frequency_changed);
            disconnect(_data, &gtl::math::fft::resolution_changed, this, &apfc::resolution_changed);
            disconnect(_data, &gtl::math::fft::window_changed, this, &apfc::window_changed);
            disconnect(_data, &gtl::math::fft::lines_changed, this, &apfc::lines_changed);
            disconnect(_data, &gtl::math::fft::overlap_changed, this, &apfc::overlap_changed);

            if(_data)
                delete _data;

            _data = new gtl::math::fft(data);

            _data->set_frequency(_ref->frequency());
            _data->set_resolution(_ref->resolution());
            _data->set_overlap(_ref->overlap());
            _data->set_window(_ref->window());

            connect(_device, &gtl::device::recieved_data, this, &gtl::math::apfc::device_recieved_data);
            connect(_data, &gtl::math::fft::initialized, this, &gtl::math::apfc::init);
            connect(_data, &gtl::math::fft::frequency_changed, this, &gtl::math::apfc::frequency_changed);
            connect(_data, &gtl::math::fft::resolution_changed, this, &gtl::math::apfc::resolution_changed);
            connect(_data, &gtl::math::fft::window_changed, this, &gtl::math::apfc::window_changed);
            connect(_data, &gtl::math::fft::lines_changed, this, &gtl::math::apfc::lines_changed);
            connect(_data, &gtl::math::fft::overlap_changed, this, &gtl::math::apfc::overlap_changed);

            init();

            if(_device)
                _device->unlock_ai();
        }

        qreal apfc::frequency() const
        {
            return _data->frequency();
        }

        void apfc::set_frequency(qreal value)
        {
            _ref->set_frequency(value);
            _data->set_frequency(value);

            init();
        }

        qreal apfc::resolution() const
        {
            return _data->resolution();
        }

        void apfc::set_resolution(qreal value)
        {
            _ref->set_resolution(value);
            _data->set_resolution(value);

            init();
        }

        apfc::windows apfc::window() const
        {
            return (apfc::windows) _data->window();
        }

        void apfc::set_window(windows value)
        {
            _ref->set_window((fft::windows) value);
            _data->set_window((fft::windows) value);
        }

        int apfc::lines() const
        {
            return _data->lines();
        }

        void apfc::set_lines(int value)
        {
            _ref->set_lines(value);
            _data->set_lines(value);

            init();
        }

        uint apfc::average() const
        {
            return _av;
        }

        void apfc::set_average(int value)
        {
            if( value != _av && value > 0)
            {
                _av = value;
                init();
                emit average_changed();
            }
        }

        qreal apfc::overlap() const
        {
            return _data->overlap();
        }

        void apfc::set_overlap(int value)
        {
            _ref->set_overlap(value);
            _data->set_overlap(value);
        }

        qreal apfc::acq_time() const
        {
            int nl = _data->lines();
            qreal f = _data->frequency();
            qreal ol = _data->overlap();

            return (nl + 1)/f * ((_av - 1)* (1.0 - ol/100.0) + 1);
        }

        apfc::afcs apfc::afc() const
        {
            return _afc;
        }

        void apfc::set_afc(afcs value)
        {
            if( value != _afc )
            {
                _afc = value;
                emit afc_changed();
            }
        }

        apfc::pfcs apfc::pfc() const
        {
            return _pfc;
        }

        void apfc::set_pfc(pfcs value)
        {
            if( value != _pfc )
            {
                _pfc = value;
                emit pfc_changed();
            }
        }

        qreal apfc::threshold() const
        {
            return _threshold;
        }

        void apfc::set_threshold(qreal newThreshold)
        {
            if (qFuzzyCompare(_threshold, newThreshold))
                return;
            _threshold = newThreshold;
            emit threshold_changed();
        }

        void apfc::calculate()
        {
            qreal ref_a = 0;
            qreal data_a = 0;
            qreal ref_p = 0;
            qreal data_p = 0;

            qreal ref_akf = 0;
            qreal data_akf = 0;
            qreal vkf = 0;

            vector<qreal> ref_a_arr(_size_fft_out);
            qreal ref_a_max = 0;
            for(int i = 0; i < _size_fft_out; i++)
            {
                ref_a_arr[i] = abs(_ref->at(i))/_size_fft_calc;
                if(ref_a_arr[i] > ref_a_max)
                    ref_a_max = ref_a_arr[i];
            }

            for(int i = 0; i < _size_fft_out; i++)
            {
                ref_a = ref_a_arr[i];//abs(_ref->at(i))/_size_fft_calc;
                data_a = abs(_data->at(i))/_size_fft_calc;

                if(ref_a >= ref_a_max*(_threshold/100.))
                {
                    if(ref_a > 1e-5 && data_a > 1e-5)
                    {
                        qreal calc_value = 0;
                        ////AMPL////
                        if(_afc == magnitude)
                            calc_value = data_a/ref_a;
                        else if(_afc == db)
                            calc_value= 20*log(data_a/ref_a);
                        else if(_afc == coherence)
                        {
                            vkf = pow(abs(_ref->at(i)*std::conj(_data->at(i)))/_size_fft_calc, 2); // Модуль ВКФ в квадрате
                            ref_akf = abs(_ref->at(i)*std::conj(_ref->at(i)))/_size_fft_calc; // АКФ ref
                            data_akf = abs(_data->at(i)*std::conj(_data->at(i)))/_size_fft_calc; // АКФ data

                            calc_value = vkf/(ref_akf*data_akf);
                        }

                        (_calculated_it + i)->first = roundf(calc_value*1000)/1000;

                        ////PHASE////
                        ref_p = arg(_ref->at(i));
                        data_p = arg(_data->at(i));
                        qreal delta_phase = data_p - ref_p;
                        if(delta_phase < 0)
                            delta_phase += 2*M_PI;

                        if(_pfc == deg)
                            calc_value = 180.*delta_phase/M_PI;
                        else if(_pfc == rad)
                            calc_value = delta_phase;

                        (_calculated_it + i)->second = roundf(calc_value*1000)/1000;
                    }

                }
                else
                {
                    (_calculated_it + i)->first = 0;
                    (_calculated_it + i)->second = 0;
                }
            }
        }

        void apfc::before_updating()
        {

        }

        void apfc::after_updating()
        {

        }

        void apfc::device_recieved_data()
        {
            if(_ref->samples() != _data->samples())
                return;

            if(_device)
                _device->lock_ai();

            before_updating();

            // Вычисление , усреднение
            int avdiv = _av_filled ? _av : _av_cnt + 1;
            if( _av > 1 )
                _calculated_it = _buffer_out[_av_cnt]->begin();
            else
                _calculated_it = begin();

            calculate();

            if( _av > 1 )
            {
                for(int i = 0; i < _size_fft_out; i++)
                {
                    qreal ampl = 0;
                    qreal phase = 0;
                    for(int j = 0; j < avdiv; j++)
                    {
                        ampl += _buffer_out[j]->at(i).first/avdiv;
                        phase += _buffer_out[j]->at(i).second/avdiv;
                    }
                    at(i).first = ampl;
                    at(i).second = phase;
                }
            }

            _av_cnt++;
            _ac = 100. * ( (qreal) _av_cnt )/_av;
            emit acquired(_ac);

            if(_av_cnt == _av)
            {
                _av_cnt = 0;
                _av_filled = true;
            }

            after_updating();

            if(_device)
                _device->unlock_ai();

            emit changed();
        }

        void apfc::cleanup()
        {
            if(_device)
                _device->lock_ai();

            for(std::vector<std::pair<qreal,qreal>>* ptr:_buffer_out)
            {
                if( ptr )
                {
                    ptr->clear();
                    ptr->shrink_to_fit();
                    delete ptr;
                }
            }
            _buffer_out.clear();
            _buffer_out.shrink_to_fit();

            clear();
            shrink_to_fit();

            if(_device)
                _device->unlock_ai();
        }

        void apfc::init()
        {
            if(_device)
                _device->lock_ai();

            cleanup();

            _size_fft_out = std::min(_data->size_fft_out(), _ref->size_fft_out());
            _size_fft_calc = std::min(_data->size_fft_calc(), _ref->size_fft_calc());

            _av_cnt = 0;
            _av_filled = false;

            for(int i = 0; i < _av; i++)
                _buffer_out.push_back(new std::vector<std::pair<qreal,qreal>>(_size_fft_out));
            _calculated_it = _buffer_out[0]->begin();

            resize(_size_fft_out);

            if(_device)
                _device->unlock_ai();

            emit initialized();
            emit acq_time_changed();
        }

        void apfc::stop_acq()
        {
            _is_acq = false;
        }

    } // namespace math
} // namespace gtl