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

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

namespace hark
{
namespace node
{
    CMChannelSelector::CMChannelSelector():
        NodeDef("CMChannelSelector")
    {
        *this
            << Terminal::in("INPUTCM")
            << Terminal::in("SELECTOR").def(std::vector<int>())
            >> Terminal::out("OUTPUTCM");
    }

    void
    CMChannelSelector::reset()
    {
    }

    void
    CMChannelSelector::initialize(InputDataBundle &input)
    {
        this->selector = input["SELECTOR"].last().cast<std::vector<int> >();

        const auto &inputcm = input["INPUTCM"].last().cast<Matrix<std::complex<float> > >();
        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 << "CMChannelSelector: Incorrect correlation matrix size.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }
        for(size_t c = 0; c < selector.size(); c){
            if(!(this->selector[c] >= 0 && this->selector[c] < this->nb_channels)){
                std::stringstream ss;
                ss << "CMChannelSelector: Nonexistent channel is selected.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
                throw RuntimeError(ss.str());
            }
        }
    }

    bool
    CMChannelSelector::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> > >();

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

        auto out = data::create<Matrix<std::complex<float> > >(this->pslength, this->selector.size() * this->selector.size());
        auto &outputcm = out.cast<Matrix<std::complex<float> > >();
        for(size_t k = 0; k < this->pslength; k++) {
            for(size_t i = 0; i < this->selector.size(); i++) {
                for(size_t j = 0; j < this->selector.size(); j++) {
                    outputcm(k, i * this->selector.size() + j) = inputcm(k, this->selector[i] * this->nb_channels + j);
                }
            }
        }

        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 */
