/*
   HARK Library
   Copyright (C) 2021 Honda Research Institute Japan Inc.
 */

#if !defined(_HARKLIB_NODE_LOCALIZE_MU_SI_C_H_INCLUDED_)
#define _HARKLIB_NODE_LOCALIZE_MU_SI_C_H_INCLUDED_ 1

#include "hark-lib/hark-lib.h"
#include "hark-lib/node.h"
#include "hark-lib/matrix.h"
#include "hark-lib/tensor.h"
#include "hark-lib/source.h"
#include "hark-lib/tf.h"

namespace hark
{
namespace node
{
    class LocalizeMUSIC: public NodeDef
    {
    public:
        LocalizeMUSIC();

        void initialize(InputDataBundle &input);
        virtual void reset();
        virtual bool operator () (size_t count, InputDataBundle &input, OutputDataBundle &output);

    private:
        template <typename T>
        T cubic_spline(const std::vector<T>& x, const std::vector<T>& y, const T x1) {

            int n = x.size()-1;
            if(x.size() != y.size()) {
               std::stringstream ss;
               ss << "LocalizeMUSIC: cubic_spline(): Vector size not matched.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
               throw RuntimeError(ss.str());
            }
            if((x[0] > x1) || (x[n] < x1)) {
               std::stringstream ss;
               ss << "LocalizeMUSIC: cubic_spline(): Interpolated point is out of range.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
               throw RuntimeError(ss.str());
            }
  
            std::vector<T> h(n);
            std::vector<T> b(n);
            std::vector<T> d(n);
            std::vector<T> g(n);
            std::vector<T> u(n);
            std::vector<T> r(n + 1);
 
            int i = -1, i1, k;
            T y1, qi, si, xx;

            for(i1 = 1; i1 < n && i < 0; i1++) {
                if(x1 < x[i1]){
                    i = i1 - 1;
                }
            }
            if(i < 0) { i = n - 1; }

            for(i1 = 0; i1 < n; i1++) {
                h[i1] = x[i1+1] - x[i1];
            }
            for(i1 = 1; i1 < n; i1++) {
                b[i1] = 2.0 * (h[i1] + h[i1-1]);
                d[i1] = 3.0 * ((y[i1+1] - y[i1]) / h[i1] - (y[i1] - y[i1-1]) / h[i1-1]);
            }

            g[1] = h[1] / b[1];
            for(i1 = 2; i1 < n-1; i1++) {
                g[i1] = h[i1] / (b[i1] - h[i1-1] * g[i1-1]);
            }
            u[1] = d[1] / b[1];
            for(i1 = 2; i1 < n; i1++) {
                u[i1] = (d[i1] - h[i1-1] * u[i1-1]) / (b[i1] - h[i1-1] * g[i1-1]);
            }
            k = (i > 1) ? i : 1;
            r[0]   = 0.0;
            r[n]   = 0.0;
            r[n-1] = u[n-1];
            for(i1 = n-2; i1 >= k; i1--) {
                r[i1] = u[i1] - g[i1] * r[i1+1];
            }
            xx = x1 - x[i];
            qi = (y[i+1] - y[i]) / h[i] - h[i] * (r[i+1] + 2.0 * r[i]) / 3.0;
            si = (r[i+1] - r[i]) / (3.0 * h[i]);
            y1 = y[i] + xx * (qi + xx * (r[i]  + si * xx));

            return y1;
        }

        template <typename T>
        std::vector<T> cubic_spline(const std::vector<T>& x, const std::vector<T>& y, const std::vector<T>& x1) {
            std::vector<T> y1(x1.size());
            for(size_t cnt = 0; cnt < x1.size(); cnt++)
                y1[cnt] = cubic_spline(x, y, x1[cnt]);
            return y1;
        }

        std::string music_algorithm;
        int nb_channels;
        std::vector<int> channel_selection;
        std::vector<int> ch_selection;
        int length;
        int samplingRate;
        std::string FilenameAMatrix;
        std::string select_tf_type;
        int num_source;
        int mindeg;
        int maxdeg;
        int lower_bound;
        int upper_bound;
        float t_s;
        std::string spectrum_weight_type;
        //Matrix<float> SSW;
        //std::vector<float> SSW_SQ;
        std::vector<float> MSSW;
        bool enable_eigenvalue_weight;
        int maxnum_out_peaks;
        bool is_debug;
        bool bInit_flag;

        TransferFunction tfData;

        int pslength;
        Matrix<std::complex<float> > x;
        std::deque<Matrix<std::complex<float> > > xbuf;
        std::vector<Matrix<std::complex<float> > > Rxx;
        std::vector<Matrix<std::complex<float> > > RxxN;
        int sum_count;
        int period;
        int window;
        std::string window_type;
        int look_ahead;
        int look_back;

        std::string ptfname;
        int nid;

        int RxxNinit_flag;

        //LocalizationMUSIC *localizer;
        std::unique_ptr<LocalizationMUSIC> localizer;

    };
}
}

#endif /* #if !defined(_HARKLIB_NODE_LOCALIZE_MU_SI_C_H_INCLUDED_) */

/* end of file */
