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

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

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

    void
    CMMultiplyEachElement::reset()
    {
    }

    void
    CMMultiplyEachElement::initialize(InputDataBundle &input)
    {
        const auto &inputcma = input["CMA"].last().cast<Matrix<std::complex<float> > >();
        const auto &inputcmb = input["CMB"].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 = inputcma.rows();
        this->nb_channels = static_cast<int>(std::floor(std::sqrt(inputcma.cols())));
        if(inputcma.cols() != this->nb_channels * this->nb_channels){
            std::stringstream ss;
            ss << "CMMultiplyEachElement: Incorrect correlation matrix size.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }
        if(!(inputcma.rows() == inputcmb.rows() && inputcma.cols() == inputcmb.cols())){
            std::stringstream ss;
            ss << "CMMultiplyEachElement: Size mismatched between CMA and CMB.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }
        Matrix<std::complex<float> > x(inputcma.rows(), inputcma.cols());
        this->Rxx.noalias() = x.setZero();
    }

    bool
    CMMultiplyEachElement::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 &inputcma = input["CMA"].last().cast<Matrix<std::complex<float> > >();
        const auto &inputcmb = input["CMB"].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){
            Rxx.noalias() = inputcma.cwiseProduct(inputcmb);
            if(this->enable_debug){ std::cout << "Count[" << count << "] : CMMultiplyEachElement Executed." << std::endl; }
            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 */
