
#include <cmath>
#include <complex>
#include <algorithm>
#include <iostream>

#include "hark-lib/node/CMInverseMatrix.h"

namespace hark
{
namespace node
{
    CMInverseMatrix::CMInverseMatrix():
        NodeDef("CMInverseMatrix")
    {
        *this
            << Terminal::in("INPUTCM")
            << Terminal::in("OPERATION_FLAG").def(false)
            << Terminal::in("FIRST_FRAME_EXECUTION").def(false)
            << Terminal::in("ENABLE_DEBUG").def(false)
            >> Terminal::out("OUTPUTCM");
    }

    void
    CMInverseMatrix::reset()
    {
    }

    void
    CMInverseMatrix::initialize(InputDataBundle &input)
    {
        const auto &inputcm = input["INPUTCM"].last().cast<Matrix<std::complex<float> > >();

        this->first_frame_execution = input["FIRST_FRAME_EXECUTION"].last().as<bool>();
        this->enable_debug = input["ENABLE_DEBUG"].last().as<bool>();

        this->pslength = inputcm.rows();
        this->nb_channels = static_cast<int>(std::floor(std::sqrt(inputcm.cols())));
        if(inputcm.cols() != this->nb_channels * this->nb_channels){
            std::stringstream ss;
            ss << "CMInverseMatrix: Incorrect correlation matrix size.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }
        Matrix<std::complex<float> > x(this->pslength, this->nb_channels * this->nb_channels);
        this->Rxx.noalias() = x.setZero();
    }

    bool
    CMInverseMatrix::operator () (size_t count, InputDataBundle &input, OutputDataBundle &output)
    {
        ////const auto &operation_flag = input["OPERATION_FLAG"].last().as<bool>();

#ifdef DEBUG
        ptime start_time;
        ptime end_time;
        start_time = microsec_clock::local_time();
#endif

        const auto &inputcm = input["INPUTCM"].last().cast<Matrix<std::complex<float> > >();
        const auto &operation_flag = input["OPERATION_FLAG"].last().as<bool>();

        if(count == 0){ this->initialize(input); }

        auto out = data::create<Matrix<std::complex<float> > >(this->pslength, this->nb_channels * this->nb_channels);
        auto &outputcm = out.cast<Matrix<std::complex<float> > >();
        if(operation_flag || this->first_frame_execution){
            Matrix<std::complex<float> > tmp_mat(this->nb_channels, this->nb_channels);
            Matrix<std::complex<float> > inv_mat(this->nb_channels, this->nb_channels);
            for(size_t k = 0; k < this->pslength; k++){
                for(size_t i = 0; i < this->nb_channels; i++){
                    for(size_t j = 0; j < this->nb_channels; j++){
                        tmp_mat(i, j) = inputcm(k, i * this->nb_channels + j);
                    }
                }
                inv_mat.noalias() = tmp_mat.inverse();
                for(size_t i = 0; i < this->nb_channels; i++){
                    for(size_t j = 0; j < this->nb_channels; j++){
                        Rxx(k, i * this->nb_channels + j) = inv_mat(i, j);
                    }
                }
            }
            this->first_frame_execution = false;
        }
        outputcm.noalias() = Rxx;

        output["OUTPUTCM"].push(out);

#ifdef DEBUG
        end_time = microsec_clock::local_time();
        std::cout << end_time - start_time << std::endl;
#endif

        return true;
    }
}
}

/* end of file */
