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

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

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

    void
    CMSave::reset()
    {
    }

    void
    CMSave::initialize(InputDataBundle &input)
    {
        this->current_filename = "";

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

        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 << "CMSave: Incorrect correlation matrix size.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
            throw RuntimeError(ss.str());
        }
        this->Rxx.noalias() = inputcm;
    }

    void
    CMSave::saveCM(std::string &filename)
    {
        harkio_TransferFunction *mat = harkio_TransferFunction_new();
        float coord[3];
        mat->cfg = nullptr;
        mat->nbrs = nullptr;
        for(int k = 0; k < nb_channels; k++){
            harkio_Positions_append(mat->mics, harkio_Position_new(k, Cartesian, coord, nullptr));
        }
        mat->poses->type = const_cast<char*>("soundsource");
        for(int k = 0; k < pslength; k++){
            harkio_Positions_append(mat->poses, harkio_Position_new(k, Cartesian, coord, nullptr));
        }
        mat->loctfs = (harkio_Matrix **) malloc(sizeof(harkio_Matrix *) * mat->poses->size);
        mat->septfs = nullptr;
        for(int k = 0; k < this->pslength; k++){
            harkio_Matrix *m = harkio_Matrix_new(const_cast<char*>("complex"), nb_channels, nb_channels);
            for(int i = 0; i < this->nb_channels; i++){
                for(int j = 0; j < this->nb_channels; j++){
                    COMPLEX_TYPE tmp = Rxx(k, i * this->nb_channels + j).__rep();
                    harkio_Matrix_setValueComplex(m, i, j, tmp);
                }
            }
            mat->loctfs[k] = m;
        }
        harkio_TransferFunction_write(mat, const_cast<char*>(filename.c_str()), const_cast<char*>("correlation matrix"));
    }

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

        this->Rxx.noalias() = inputcm;
        if(operation_flag){
            std::string filename = input["FILENAME"].last().cast<std::string>();
            if(filename.empty()){
                std::stringstream ss;
                ss << "CMSave: No file name is given.\n" << "FILE: " << __FILE__ << ", LINE: " << __LINE__;
                throw RuntimeError(ss.str());
            }
            if(this->current_filename != filename){
                this->current_filename.swap(filename);
            }
            this->saveCM(this->current_filename);
        }

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