#include "gtl_math_fft.h"

namespace gtl {
    namespace math {

        fft::fft(gtl::analog_data *ad)
            : QObject(ad)
            , _ad(new gtl::math::filter_iir(ad, true))
            , _rbw(10)//6.25
            , _w(windows::rectangular)
            , _nl(20)
            , _ol(0)
            , _done(false)
            , _cn(4)
            , _dr(1)
            , _buffer_ptr(0)
            , _buffer_ptr_offset(0)
        {
            _device = static_cast<gtl::device*>(_ad->root());

            if(_device)
                _device->lock_ai();

            fftw_make_planner_thread_safe();

            set_frequency(200);
            init();

            if(_device)
                _device->unlock_ai();

            connect(_ad, &gtl::analog_data::data_changed, this, &fft::data_changed);
        }


        fft::~fft()
        {
            emit deleting();
            cleanup();
            if(_ad)
                delete _ad;
        }

        qreal fft::frequency() const
        {
            return _f;
        }

        void fft::set_frequency(qreal value)
        {
            if(_device)
                _device->lock_ai();

            if(value > _ad->get_rate()/2)
              value = _ad->get_rate()/2;

            if(_f != value && value <= _ad->get_rate()/2 && value > 0)
            {
                _f = value;
                _dr = (uint) ( (_ad->get_rate()/2)/_f );
                _fs = _ad->get_rate() / _dr;
                _fk = _fs/2;

                int size = _fs/_rbw;//_size_r = (size_t) _fs/_rbw;
                _rbw = _fs / (qreal)size;//_rbw = _fs / (qreal)_size_r;

                _nl = (uint) round(_f/_rbw);

                init();
                emit frequency_changed();
            }

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

        qreal fft::resolution() const
        {
            return _rbw;
        }

        void fft::set_resolution(qreal value)
        {
            if(_device)
                _device->lock_ai();

            if(_rbw != value && _fk > value && value > 0)
            {
                _rbw = value;

                int size = _fs/_rbw;
                _rbw = _fs / (qreal)size;

                _nl = (uint) round(_f/_rbw);

                init();
                emit resolution_changed();
            }

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

        fft::windows fft::window() const
        {
            return _w;
        }

        void fft::set_window(windows value)
        {
            if(_w != value)
            {
                _w = value;
                init();
                emit window_changed();
            }
        }

        int fft::lines() const
        {
            return _nl;
        }

        void fft::set_lines(int value)
        {
            if(_device)
                _device->lock_ai();

            if(_nl != value && value > 0)
            {
//                _nl = value;
//                _rbw = _f/_nl;
                set_resolution(_f/value);
//                init();
                emit lines_changed();
            }

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

        qreal fft::overlap() const
        {
            return _ol;
        }

        void fft::set_overlap(int value)
        {
            if( value != _ol && value < 100 && value >= 0)
            {
                _ol = value;
                init();
                emit overlap_changed();
            }
        }

        analog_data *fft::ad() const
        {
            return static_cast<analog_data*>(_ad->parent());
        }


        int fft::samples() const
        {
            return _samples;
        }

        int fft::size_fft_out() const
        {
            return _size_out;
        }

        int fft::size_fft_calc() const
        {
            return _size_r_k;
        }

        void fft::init()
        {
            _ad->lock_device();
            cleanup();

            _fk = _fs/2;
            if(_fk < _rbw)
                _rbw = _fk;
            _size = _ad->get_rate()/_rbw;
            _fs = _ad->get_rate()/_dr;
            _size_r = ceil(_fs/_rbw);
            _size_r_k = _dr > 1 ? _size_r/2 + 1 : _size_r/2;
            _size_out = _nl < _size_r_k ? _nl + 1 : _nl;
            _buffer_ptr = 0;
            _buffer_ptr_offset = (int) (_size *  _ol) / 100 ;

            _buffer_in = new std::vector<qreal>(_size);
            _buffer_w = new std::vector<qreal>(_size_r);

            int N = _size_r;
            switch (_w) {
                case windows::rectangular:
                    std::generate(_buffer_w->begin(), _buffer_w->end(), [] () mutable { return 1.*1.051; });
                    break;
                case windows::cosine:
                    std::generate(_buffer_w->begin(), _buffer_w->end(), [n = 0, &N] () mutable { auto v = sin(M_PI*n/N); n++; return v*1.626; });
                    break;
                case windows::hann:
                    std::generate(_buffer_w->begin(), _buffer_w->end(), [n = 0, &N] () mutable { auto v = 0.5 - 0.5*cos(2*M_PI*n/N); n++; return v*2.07; });
                    break;
                case windows::bartlett_hann:
                    std::generate(_buffer_w->begin(), _buffer_w->end(), [n = 0, &N] () mutable { auto v = 0.62 - 0.48*fabs(n/N - 1/2) - 0.38*cos(2*M_PI*n/N); n++; return v*1.669; });
                    break;
                case windows::hamming:
                    std::generate(_buffer_w->begin(), _buffer_w->end(), [n = 0, &N] () mutable { auto v = 0.54 - 0.46*cos(2*M_PI*n/N); n++; return v*1.918; });
                    break;
                case windows::blackman:
                    std::generate(_buffer_w->begin(), _buffer_w->end(), [n = 0, &N] () mutable { auto v = 0.42 - 0.5*cos(2*M_PI*n/N) + 0.08*cos(4*M_PI*n/N); n++; return v*2.464; });
                    break;
                case windows::blackman_harris:
                    std::generate(_buffer_w->begin(), _buffer_w->end(), [n = 0, &N] () mutable { auto v = 0.359 - 0.488*cos(2*M_PI*n/N) + 0.141*cos(4*M_PI*n/N) - 0.012*cos(6*M_PI*n/N); n++; return v*2.883; });
                    break;
                case windows::flattop:
                    std::generate(_buffer_w->begin(), _buffer_w->end(), [n = 0, &N] () mutable { auto v = 1 - 1.93*cos(2*M_PI*n/N) + 1.29*cos(4*M_PI*n/N) - 0.388*cos(6*M_PI*n/N) + 0.032*cos(8*M_PI*n/N); n++; return v*1.035; });
                    break;
            }

            _fft_in = new std::vector<qreal>(_size_r);

            resize(_size_r);

            _plan = fftw_plan_dft_r2c_1d(_size_r, _fft_in->data(), reinterpret_cast<fftw_complex*>(data()), FFTW_ESTIMATE);

            ////ANTIALIAS FILTER
            gtl::math::filter_iir *filter = static_cast<gtl::math::filter_iir*>(_ad);
            filter->reset();
            filter->set_kind(gtl::math::filter_iir::kinds::elliptic);
            filter->set_type(gtl::math::filter_iir::types::lowpass);
            filter->set_order(10);
            filter->set_frequency(_rbw*_size_out);
            filter->set_ripple(1);
            filter->set_slope(1);

            _ad->unlock_device();

            _samples = 0;

            emit initialized();
        }

        void fft::cleanup()
        {
            _ad->lock_device();

            if(_plan)
            {
                fftw_destroy_plan(_plan);
                _plan = nullptr;
            }

            if(_buffer_in)
            {
                _buffer_in->clear();
                _buffer_in->shrink_to_fit();
                delete _buffer_in;
                _buffer_in = nullptr;
            }

            if(_buffer_w)
            {
                _buffer_w->clear();
                _buffer_w->shrink_to_fit();
                delete _buffer_w;
                _buffer_w = nullptr;
            }

            if(_fft_in)
            {
                _fft_in->clear();
                _fft_in->shrink_to_fit();
                delete _fft_in;
                _fft_in = nullptr;
            }

            clear();
            shrink_to_fit();

            _ad->unlock_device();
        }

        void fft::decimate(std::vector<qreal>::iterator in_it, std::vector<qreal>::iterator out_it, int size, int N, int R)
        {
            for( int  i = 0; i < size/R; i++)
                *(out_it + i) = *(in_it + i*R);
            return;
        }

        void fft::set_rate(qreal value)
        {
            _ad->set_rate(value);

            init();
        }

        void fft::data_changed()
        {
            _ad->lock_device();

            _samples += _ad->size();

            adcopy(_ad->size());

            _ad->unlock_device();
        }

        void fft::adcopy(int adoffset)
        {
            if(_buffer_ptr < (int) _size)
            {
                if(adoffset > _size)
                    adcopy(adoffset - _size);

                int adsize = _size;

                if(adoffset >= _ad->size())
                    adsize = _ad->size() % _size;

                std::vector<qreal>::iterator it_b = _ad->begin() + adoffset - (adoffset % _size);
                std::vector<qreal>::iterator it_e = it_b + adsize;

                /// Заполнение буфера данными
                if( adsize >= _size)
                {
                    if(_buffer_ptr)
                    {
                        int tmp = _buffer_ptr;
                        std::copy(it_b, it_e - _buffer_ptr, _buffer_in->begin() + _buffer_ptr);
                        _buffer_ptr = _size;
                        calc();

                        _buffer_ptr = tmp;
                        std::copy(it_e - _buffer_ptr, it_e, _buffer_in->begin());
                    }
                    else
                    {
                        std::copy(it_b, it_e, _buffer_in->begin());
                        _buffer_ptr = _size;
                    }
                }
                else
                {
                    if( (_size - _buffer_ptr) >= adsize )
                    {
                        std::copy(it_b, it_e, _buffer_in->begin() + _buffer_ptr);
                        _buffer_ptr += adsize;
                    }
                    else
                    {
                        int tmp = _buffer_ptr;
                        std::copy(it_b, it_b + (_size - _buffer_ptr), _buffer_in->begin() + _buffer_ptr);
                        _buffer_ptr = _size;
                        calc();

                        _buffer_ptr = std::distance(it_b, it_e) - (_size - tmp);
                        std::copy(it_e - _buffer_ptr, it_e, _buffer_in->begin());
                    }
                }
                /////////////////////
            }

            calc();
        }

        void fft::calc()
        {
            _done = false;

            if( _buffer_ptr >= (int) _size  && _plan )
            {
                ////////////////////////////////////////////////////////////////////////////////////////

                decimate(_buffer_in->begin(), _fft_in->begin(), _size, _cn, _dr);

                // Весовая функция
                for(int i = 0; i < _size_r; i++)
                    _fft_in->at(i) *= _buffer_w->at(i);

                // БПФ
                if(_plan) fftw_execute(_plan);

                if( _buffer_ptr_offset )
                    std::copy(_buffer_in->end() - _buffer_ptr_offset, _buffer_in->end(), _buffer_in->begin());
                _buffer_ptr = _buffer_ptr_offset;

                _done = true;
            }

            if(_done)
                emit changed();
        }

    } // namespace math
} // namespace gtl