/*
 * Copyright (c) 2008 - 2020 Kyoto University and Honda Motor Co.,Ltd.
 * All rights reserved.
 * HARK was developed by researchers in Okuno Laboratory
 * at the Kyoto University and Honda Research Institute Japan Co.,Ltd.
 * 
 * Licensed under the HARK license, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 * https://www.hark.jp/notice/HARK_License_Agreement.pdf
 */

// HarkFileIO_NH.cpp: CHarkFileIO クラスのインプリメンテーション
//
//////////////////////////////////////////////////////////////////////

#include <math.h>
#include <vector>
#include <cstring>
#include <algorithm>

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

namespace hark
{

HarkFileIO::HarkFileIO()
{
    this->filename = "";
    this->ptf = NULL;
    this->file_type = FORMAT_TYPE::INVALID;
    this->mode = OPEN_MODE::READ;

    this->pPrinbrs = NULL;
    this->thresh = 0.0;

    this->mics.resize(0);
    this->srcs.resize(0);
    this->freqs.resize(0);
    this->locdata.resize(0);
    this->sepdata.resize(0);
}

HarkFileIO::~HarkFileIO()
{
    this->CloseFile();
}

// --------------------------------------------------------------------
// Open the Transfer Function or Separation Matrix file.
// Do nothing if the same filename is already open.
void HarkFileIO::OpenFile(const std::string &filename, OPEN_MODE mode)
{
    if(!this->filename.empty() && this->filename == filename && this->mode == mode){ return; }
    this->ReopenFile(filename, mode);
}

// --------------------------------------------------------------------
// Re-open the Transfer Function or Separation Matrix file.
// File that is already open will be closed.
void HarkFileIO::ReopenFile(const std::string &filename, OPEN_MODE mode)
{
    if(filename.empty()){
        std::stringstream ss;
        ss << "An attempt was made to open a file without specifying a filename.\n";
        throw RuntimeError(ss.str());
    }
    if(!this->filename.empty()){ this->CloseFile(); }
    this->filename = filename;
    this->mode = mode;

    if(mode == OPEN_MODE::READ){
        this->ptf = harkio_TransferFunction_fromFile(const_cast<char*>(this->filename.c_str()), const_cast<char*>(HARKFILEIO_SIGNATURE_TFMAT));
        this->file_type = FORMAT_TYPE::TF;
        if(!this->ptf){
            this->ptf = harkio_TransferFunction_fromFile(const_cast<char*>(this->filename.c_str()), const_cast<char*>(HARKFILEIO_SIGNATURE_SPMAT));
            this->file_type = FORMAT_TYPE::W;
            if(!this->ptf){
                std::stringstream ss;
                ss << "Failed to open \"" << this->filename << "\".\n"
                   << "@HarkFileIO::ReopenFile()\n";
                throw RuntimeError(ss.str());
            }
        }

        {
            float coord[3];
            float *pc;
            float *pp;

            COMPLEX_TYPE value;

            this->sampling_rate = this->ptf->cfg->samplingRate;
            this->num_fft = this->ptf->cfg->nfft;

            size_t num_mic, num_src, num_freq, num_data;
            num_mic = static_cast<size_t>(this->ptf->mics->size);
            num_src = static_cast<size_t>(this->ptf->poses->size);
            num_freq = static_cast<size_t>(this->ptf->cfg->nfft / 2 + 1);
            num_data = num_freq * num_src * num_mic;

            if(this->mics.size() != num_mic){ this->mics.resize(num_mic); }
            if(this->mics_polar.size() != num_mic){ this->mics_polar.resize(num_mic); }
            if(this->srcs.size() != num_src){ this->srcs.resize(num_src); }
            if(this->srcs_polar.size() != num_src){ this->srcs_polar.resize(num_src); }
            if(this->freqs.size() != num_freq){ this->freqs.resize(num_freq); }
            if(this->ptf->loctfs && this->locdata.size() != num_data){
                this->locdata.resize(num_data);
            }
            else{
                this->locdata.resize(0);
            }
            if(this->ptf->septfs && this->sepdata.size() != num_data){
                this->sepdata.resize(num_data);
            }
            else{
                this->sepdata.resize(0);
            }

            for(size_t i = 0; i < this->mics.size(); i++){
                if(this->ptf->mics->pos[i]->sys != ::Cartesian){
                    harkio_Position_Convert(this->ptf->mics->pos[i], coord, ::Cartesian);
                    pc = coord;
                    pp = this->ptf->mics->pos[i]->coord;
                }
                else{
                    harkio_Position_Convert(this->ptf->mics->pos[i], coord, ::Polar);
                    pc = this->ptf->mics->pos[i]->coord;
                    pp = coord;
                }

                this->mics[i].coord[0] = pc[0];
                this->mics[i].coord[1] = pc[1];
                this->mics[i].coord[2] = pc[2];
                this->mics[i].id = this->ptf->mics->pos[i]->id;
                this->mics[i].sys = hark::TF_Coordinate::Cartesian;

                this->mics_polar[i].coord[0] = pp[0];
                this->mics_polar[i].coord[1] = pp[1];
                this->mics_polar[i].coord[2] = pp[2];
                this->mics_polar[i].id = this->ptf->mics->pos[i]->id;
                this->mics_polar[i].sys = hark::TF_Coordinate::Polar;
            }

            for(size_t i = 0; i < this->srcs.size(); i++){
                if(this->ptf->poses->pos[i]->sys != ::Cartesian){
                    harkio_Position_Convert(this->ptf->poses->pos[i], coord, ::Cartesian);
                    pc = coord;
                    pp = this->ptf->poses->pos[i]->coord;
                }
                else{
                    harkio_Position_Convert(this->ptf->poses->pos[i], coord, ::Polar);
                    pc = this->ptf->poses->pos[i]->coord;
                    pp = coord;
                }
                this->srcs[i].coord[0] = pc[0];
                this->srcs[i].coord[1] = pc[1];
                this->srcs[i].coord[2] = pc[2];
                this->srcs[i].id = this->ptf->poses->pos[i]->id;
                this->srcs[i].sys = hark::TF_Coordinate::Cartesian;

                this->srcs_polar[i].coord[0] = pp[0];
                this->srcs_polar[i].coord[1] = pp[1];
                this->srcs_polar[i].coord[2] = pp[2];
                this->srcs_polar[i].id = this->ptf->poses->pos[i]->id;
                this->srcs_polar[i].sys = hark::TF_Coordinate::Polar;
            }

            for(size_t i = 0; i < this->freqs.size(); i++){
                this->freqs[i] = i;
            }

            if(this->file_type == FORMAT_TYPE::TF){ // Transfer Function
                if(!this->locdata.empty()){
                    // data order is (freqs, srcs, mics).
                    for(size_t k = 0; k < num_freq; k++){
                        for(size_t j = 0; j < num_src; j ++){
                            for(size_t i = 0; i < num_mic; i++){
                                harkio_Matrix_getValueComplex(this->ptf->loctfs[j], i, k, &value);
                                this->locdata[k * num_src * num_mic + j * num_mic + i] = std::complex<float>(value);
                            }
                        }
                    }
                }
                if(!this->sepdata.empty()){
                    // data order is (freqs, srcs, mics).
                    for(size_t k = 0; k < num_freq; k++){
                        for(size_t j = 0; j < num_src; j ++){
                            for(size_t i = 0; i < num_mic; i++){
                                harkio_Matrix_getValueComplex(this->ptf->septfs[j], i, k, &value);
                                this->sepdata[k * num_src * num_mic + j * num_mic + i] = std::complex<float>(value);
                            }
                        }
                    }
                }
            }
            else if(this->file_type == FORMAT_TYPE::W){ // Separation Matrix
                if(!this->sepdata.empty()){
                    // data order is (freqs, mics, srcs).
                    for(size_t k = 0; k < num_freq; k++){
                        for(size_t i = 0; i < num_mic; i++){
                            for(size_t j = 0; j < num_src; j ++){
                                harkio_Matrix_getValueComplex(this->ptf->septfs[j], i, k, &value);
                                this->sepdata[k * num_mic * num_src + i * num_src + j] = std::complex<float>(value);
                            }
                        }
                    }
                }
            }
            else{
                std::stringstream ss;
                ss << "File type is unknown\n";
                throw RuntimeError(ss.str());
            }
        }

    }
    else if(mode == OPEN_MODE::WRITE){
        if(this->ptf){ this->CloseFile(); }
        this->ptf = NULL;
        this->file_type = FORMAT_TYPE::UNDEFINED;
    }

}

// --------------------------------------------------------------------
// Close the Transfer Function or Separation Matrix file.
void HarkFileIO::CloseFile()
{
    if(this->pPrinbrs){ harkio_Neighbors_delete(&this->pPrinbrs); }
    if(this->ptf){ harkio_TransferFunction_delete(&this->ptf); }

    this->filename = "";
    this->ptf = NULL;
    this->file_type = FORMAT_TYPE::INVALID;

    this->pPrinbrs = NULL;
    this->thresh = 0.0;
}

// --------------------------------------------------------------------
//ファイル読み込み
/*
bool HarkFileIO::ReadHeader(const std::string &filename) 
{
    try{
        if(!OpenFile(filename)){ return false; }

        harkio_TransferFunction* tf = this->ptf;

        this->lFs = tf->cfg->samplingRate;
        this->lNFFT = tf->cfg->nfft;
        this->lNSrc = tf->poses->size;
        this->lNFreq = tf->cfg->nfft/2+1;
        this->lNMic = tf->mics->size;
        //this->iFileType = this->file_type;
    }
    catch(std::exception &ex){
        return false;
    }

    // std::cout << "Success Write Hark file" << std::endl ;
    return true ;
}
*/

// --------------------------------------------------------------------
//データの読込み
/*
void HarkFileIO::ReadData(const std::string &filename)
{
    this->OpenFile(filename, OPEN_MODE::READ);
}
*/

// --------------------------------------------------------------------
// テキストファイル(from PositionsXML)
/*
bool HarkFileIO::LoadPositionData(const std::string &filename, std::vector<Location> &poses)
{
    try {
        //harkio_XML* xml = harkio_XML_newFromXML(filename.c_str(), NULL);
        harkio_XML* xml = harkio_XML_newFromXML(const_cast<char*>(filename.c_str()), NULL);
        if(!xml){
            std::cerr << "Failed to read " << filename << "@CHarkFileIO::LoadPositionData" << endl;
            return false;
        }

        poses.resize(xml->poses->size);
        float pfc[3];
        for(size_t i = 0; i < poses.size(); i++){
            harkio_Position_Convert(xml->poses->pos[i], pfc, Cartesian);
            poses[i].x[0] = pfc[0];
            poses[i].x[1] = pfc[1];
            poses[i].x[2] = pfc[2];
            poses[i].idpos = xml->poses->pos[i]->id;
            poses[i].tfpos = xml->poses->pos[i]->id;
          //poses[i] = i; <==========
            //std::cout << "poses[" << i << "]," << poses[i].x[0] << "," << poses[i].x[1] << "," << poses[i].x[2] << std::endl;
        }

        harkio_XML_delete(&xml);
    }
    catch(std::exception &ex){
        return false;
    }

    // std::cout << "Success Write Hark file" << std::endl ;
    return true;
}
*/

// --------------------------------------------------------------------
// テキストファイル(from tfzipファイル)
/*
bool HarkFileIO::LoadPositionData(const std::string &filename, std::vector<Location> &poses)
{
    try{
        if(!OpenFile(filename)){ return false; }

        harkio_TransferFunction* tf = this->ptf;

        poses.resize(tf->mics->size);
        float pfc[3];
        for(size_t i = 0; i < poses.size(); i++){
            harkio_Position_Convert(tf->mics->pos[i], pfc, Cartesian);
            poses[i].x[0] = pfc[0];
            poses[i].x[1] = pfc[1];
            poses[i].x[2] = pfc[2];
            poses[i].idpos = tf->mics->pos[i]->id;
            poses[i].tfpos = tf->mics->pos[i]->id;
            // std::cout << "mic" << i << "," << poses[i].x[0]
            //                         << "," << poses[i].x[1]
            //                         << "," << poses[i].x[2]
            //                         << std::endl;
        }
    }
    catch(std::exception &ex){
        return false;
    }

    // std::cout << "Success Read Hark Data." << std::endl ;
    return true;
}
*/

// --------------------------------------------------------------------
// Neighborsリスト計算
bool HarkFileIO::CalcNeighbors(const std::string &filename, float thresh)
{
    try{
        std::vector<std::pair<float, int> > vifclosestIDs; // 距離, id

        if(!this->ptf){ this->OpenFile(filename); }

        harkio_TransferFunction* tf = this->ptf;

        if(this->pPrinbrs){ harkio_Neighbors_delete(&this->pPrinbrs); }

        // 注) this->pPrinbrsを使用する間はTFファイルを閉じないこと
        this->pPrinbrs = harkio_Neighbors_new(this->ptf->nbrs->poses);

        // 内部でNeighbor計算する
        //if(harkio_Neighbors_calcNeighbors(this->pPrinbrs, NearestNeighbor, nth, thresh) != EXIT_SUCCESS){
        //    std::cerr << "Failed harkio_Neighbors_calcNeighbors@CHarkFileIO::CalcNeighbors" << std::endl;
        //    if(this->pPrinbrs){
        //        harkio_Neighbors_delete(&this->pPrinbrs);
        //        this->pPrinbrs = NULL;
        //    }
        //    return false;
        //}
        for(size_t i = 0; i < this->pPrinbrs->size; i++){
            // Initialize
            vifclosestIDs.clear();

            // threshould以内を検索
            for(size_t j = 0; j < this->pPrinbrs->size; j++){
                float tmpdist = euclidDistance(this->pPrinbrs->poses->pos[i], this->pPrinbrs->poses->pos[j], 3);
                if(tmpdist < thresh){
                    vifclosestIDs.push_back(std::pair<float, int>(tmpdist, this->pPrinbrs->poses->pos[j]->id));
                }
            }

            // sort
            std::sort(vifclosestIDs.begin(), vifclosestIDs.end());

            // Neighbor構造体内に保存
            this->pPrinbrs->numNeighbors[i] = vifclosestIDs.size();
            this->pPrinbrs->neighbors[i] = (int *) malloc(sizeof(int) * this->pPrinbrs->numNeighbors[i]);
            for(size_t j = 0; j < this->pPrinbrs->numNeighbors[i]; j++){
                this->pPrinbrs->neighbors[i][j] = vifclosestIDs[j].second;
            }
            // TEST
            //for(size_t j = 0; j < this->pPrinbrs->numNeighbors[i]; j++){
            //    std::cout << this->pPrinbrs->neighbors[i][j] << "(" << vifclosestIDs[j].second << "," << vifclosestIDs[j].first << ")," ;
            //}
            //std::cout << std::endl;
        }

        // Must be specified to be freed with harkio_Neighbors_delete().
        this->pPrinbrs->alg = ::NearestNeighbor;

        this->thresh = thresh;
    }
    catch(std::exception &ex){
        return false ;
    }

    return true ;
}
// --------------------------------------------------------------------
// 現在のTFからidを指定してposindexを取得
int HarkFileIO::GetPosIndex(int id)
{
    try{
        if(!this->ptf){
            std::cerr << "Not found TF file! @CHarkFileIO::GetPosIndex" << std::endl;
            return -1;
        }
        for(size_t i = 0; i < this->ptf->poses->size; i++){
            if(this->ptf->poses->pos[i]->id == id){ return i; }
        }
    }
    catch(std::exception &ex){
        return -1;
    }

    // std::cout << "Success Read Hark Data." << std::endl;
    return -1;
}
// --------------------------------------------------------------------
// ユークリッド距離の算出
float HarkFileIO::_EuclidDistance(hark::Position &a, hark::Position &b)
{
    float coord[3];

    coord[0] = a.coord[0] - b.coord[0];
    coord[1] = a.coord[1] - b.coord[1];
    coord[2] = a.coord[2] - b.coord[2];

    float fdist = coord[0] * coord[0] + coord[1] * coord[1] + coord[2] * coord[2];
    fdist = std::sqrt(fdist);
    return fdist;
}

// --------------------------------------------------------------------
// 現在のTFから座標を指定してidを取得
int HarkFileIO::GetNearestPosID(hark::Position &cur)
{
    int id;
    float fmin, ftmp;

    {
        std::stringstream ss;
        if(this->file_type == FORMAT_TYPE::INVALID || this->filename == ""){
            ss << "[Error] File not open.\n";
        }
        if(this->file_type == FORMAT_TYPE::UNDEFINED){
            ss << "[Error] This file has been opened in write mode, but no data has been set yet.\n";
        }
        if(this->srcs.empty()){
            ss << "[Error] There are no sound source position coordinates.\n";
        }
        if(cur.sys != hark::TF_Coordinate::Cartesian){
            ss << "[Error] If it occurs in the user environment, it may be a bug in the program. This method only accepts Cartesian coordinates. If you're a developer, check out the method specs.\n";
        }
        throw RuntimeError(ss.str());
    }

    fmin = _EuclidDistance(this->srcs[0], cur);
    id = this->srcs[0].id;

    for(size_t i = 0; i < this->srcs.size(); i++){
        ftmp = _EuclidDistance(this->srcs[i], cur);
        if(ftmp < fmin){
            fmin = ftmp;
            id = this->srcs[i].id;
        }
    }
    return id;
}


void HarkFileIO::SetW(const std::string &filename,
                      std::vector<int> &freqs,
                      std::vector<hark::Position> &mics,
                      std::vector<hark::Position> &srcs,
                      std::vector<std::complex<float> > &w)
{
    size_t num_freq, num_mic, num_src, num_data;
    num_freq = freqs.size();
    num_mic = mics.size();
    num_src = srcs.size();
    num_data = num_freq * num_mic * num_src;
    if(w.size() != num_data){
        std::stringstream ss;
        ss << "[Error] Given number of sound sources, microphones, frequency bins and Separation Matrix size do not match.\n"
           << w.size() << " (Separation Matrix) != "
           << num_freq << " (Frequency Bins) * "
           << num_mic << " (Microphones) * "
           << num_src << " (Sound Sources)\n"
           << "@HarkFileIO::SetW\n";
        throw RuntimeError(ss.str());
    }
    if(num_freq != static_cast<size_t>(this->num_fft / 2 + 1)){
        this->num_fft = static_cast<int>((num_freq - 1) * 2);
        std::cout << "[Warning] Since num_fft did not match or was not set, it was set by calculating hackward from the number of frequency bins.\n@HarkFileIO::SetW" << std::endl;
    }
    if(this->sampling_rate < 1){
        this->sampling_rate = 16000;
        std::cout << "[Warning] Since sampling_rate is less than 1 or not set, it is assumed to be the default value of 16kHz.\n@HarkFileIO::SetW" << std::endl;
    }

    // If it was not opened in write mode, it will be reopened.
    this->OpenFile(filename, OPEN_MODE::WRITE);

    if(this->file_type == FORMAT_TYPE::UNDEFINED){
        // A new Separation Matrix file is created.
        this->freqs.resize(freqs.size()); this->freqs.assign(freqs.begin(), freqs.end());
        this->mics.resize(mics.size()); this->mics.assign(mics.begin(), mics.end());
        this->srcs.resize(srcs.size()); this->srcs.assign(srcs.begin(), srcs.end());
        this->sepdata.resize(w.size()); this->sepdata.assign(w.begin(), w.end());
        this->file_type = FORMAT_TYPE::W;
    }
    else if(this->file_type == FORMAT_TYPE::W){
        // The previously written Separation Matrix file is updated.
        this->freqs.resize(freqs.size()); this->freqs.assign(freqs.begin(), freqs.end());
        this->mics.resize(mics.size()); this->mics.assign(mics.begin(), mics.end());
        this->srcs.resize(srcs.size()); this->srcs.assign(srcs.begin(), srcs.end());
        this->sepdata.resize(w.size()); this->sepdata.assign(w.begin(), w.end());
    }
    else{
        std::stringstream ss;
        if(this->file_type == FORMAT_TYPE::INVALID){
            ss << "[Error] " << filename << " could not be opened.\n";
        }
        else if(this->file_type == FORMAT_TYPE::TF){
            // A file with the same filename has already been opened, but it is a TF.
            ss << "[Error] It looks like you are trying to write the Separation Matrix to the Transfer Function file. Check the filename specification.\n";
        }
        else if(this->file_type == FORMAT_TYPE::USER){
            ss << "[Error] It's already opened as a user format.\n";
        }
        else{
            ss << "[Error] It's already opened as a unknown format.\n";
        }
        ss << "@HarkFileIO::SetW\n";
        throw RuntimeError(ss.str());
    }
}

// --------------------------------------------------------------------
// 分離行列データの書き出し
bool HarkFileIO::WriteData(const std::string &filename)
{
    try{
        float  coord[3];
        COMPLEX_TYPE cvalue;

        size_t num_freq, num_src, num_mic;

        //{
        //    std::stringstream ss;
        //    ss << "HarkFileIO::WriteData\n"
        //       << "  num_mic = this->mics.size() = " << this->mics.size() << "\n"
        //       << "  num_src = this->srcs.size() = " << this->srcs.size() << "\n"
        //       << "  num_freq = this->freqs.size() = " << this->freqs.size() << "\n"
        //       << "  sampling_rate = " << this->sampling_rate << "\n"
        //       << "  num_fft = " << this->num_fft << "\n";
        //    std::cout << ss.str() << std::endl;
        //}

        if(filename.empty()){
            std::stringstream ss;
            ss << "Filename not specified.\n@HarkFileIO::WriteData";
            throw RuntimeError(ss.str());
        }

        if(this->file_type == FORMAT_TYPE::INVALID){
            std::stringstream ss;
            ss << "The file format has not been set because the data to be written has not been set. And file not open.\n@HarkFileIO::WriteData";
            throw RuntimeError(ss.str());
        }

        if(this->file_type == FORMAT_TYPE::UNDEFINED){
            // If you use the method in this class to set data, it will change to somthing
            // other than FORMAT_TYPE::UNDEFINED. Besides forgetting to set the data,
            // you will also get this error message if you forget to set the file type
            // (this->file_type) if you have done some rather dangerous coding that
            // directly modified member variables.
            std::stringstream ss;
            ss << "The file format has not been set because the data to be written has not been set.\n@HarkFileIO::WriteData";
            throw RuntimeError(ss.str());
        }

        if(this->file_type == FORMAT_TYPE::TF || this->file_type == FORMAT_TYPE::W){
            if(this->ptf){
                if(this->pPrinbrs){ harkio_Neighbors_delete(&this->pPrinbrs); }
                if(this->ptf){ harkio_TransferFunction_delete(&this->ptf); }
                this->pPrinbrs = NULL;
                this->ptf = NULL;
            }
            this->ptf = harkio_TransferFunction_new();
        }

        if(!this->ptf){
            std::stringstream ss;
            ss << "Failed to create harkio_Transferfunction. @HarkFileIO::WriteData\n";
            throw RuntimeError(ss.str());
        }

        // config
        this->ptf->cfg->samplingRate = this->sampling_rate;
        this->ptf->cfg->nfft = static_cast<int>(this->num_fft);
        // 以下ダミーで入力
        this->ptf->cfg->synchronousAverage = 1;
        
        num_freq = this->freqs.size();

        harkio_Position *pos;

        // poses
        this->ptf->poses->type = (char*) malloc(sizeof(char) * (strlen("soundsource") + 1));
        strcpy(this->ptf->poses->type, "soundsource");
        for(size_t k = 0; k < this->srcs.size(); k++){
            coord[0] = this->srcs[k].coord[0];
            coord[1] = this->srcs[k].coord[1];
            coord[2] = this->srcs[k].coord[2];
            pos = harkio_Position_new(this->srcs[k].id, ::Cartesian, coord, NULL);
            harkio_Positions_append(this->ptf->poses, pos); // Append this->ptf->poses->pos[k]
        }
        num_src = static_cast<size_t>(this->ptf->poses->size);
        if(num_src != this->srcs.size()){
            this->CloseFile();
            std::stringstream ss;
            ss << "Mismatched number of sound source positions for Transfer Function that were armed to write. @HarkFileIO::WriteData\n";
            throw RuntimeError(ss.str());
        }

        // nbrs
        if(this->ptf->nbrs){
            harkio_Neighbors_delete(&this->ptf->nbrs);
            this->ptf->nbrs = NULL;
        }
        this->ptf->nbrs = harkio_Neighbors_new(this->ptf->poses);
        harkio_Neighbors_calcNeighbors(this->ptf->nbrs, ::NearestNeighbor, 1, 300.0);

        // mics
        this->ptf->mics->type = (char*) malloc(sizeof(char) * (strlen("microphone") + 1));
        strcpy(this->ptf->mics->type, "microphone");
        for(size_t k = 0; k < this->mics.size(); k++){
            coord[0] = this->mics[k].coord[0];
            coord[1] = this->mics[k].coord[1];
            coord[2] = this->mics[k].coord[2];
            pos = harkio_Position_new(this->mics[k].id, ::Cartesian, coord, NULL);
            harkio_Positions_append(this->ptf->mics, pos); // Append this->ptf->mics->pos[k]
        }
        num_mic = static_cast<size_t>(this->ptf->mics->size);
        if(num_mic != this->mics.size()){
            this->CloseFile();
            std::stringstream ss;
            ss << "Mismatched number of microphone positions for Transfer Function that were armed to write. @HarkFileIO::WriteData\n";
            throw RuntimeError(ss.str());
        }

        // loctf
        if(!this->locdata.empty()){
            if(this->file_type == FORMAT_TYPE::TF){
                this->ptf->loctfs = (harkio_Matrix **) malloc(sizeof(harkio_Matrix *) * this->ptf->poses->size);
                for(size_t j = 0; j < num_src; j++){
                    this->ptf->loctfs[j] = harkio_Matrix_new(const_cast<char*>(HARKFILEIO_COMPLEX_TYPESTR), this->ptf->mics->size, this->num_fft / 2 + 1);
                }

                // data order is (freq, src, mic)
                for(size_t k = 0; k < num_freq; k++){
                    for(size_t j = 0; j < num_src; j++){
                        for(size_t i = 0; i < num_mic; i++){
                            size_t index = k * num_src * num_mic + j * num_mic + i;
                            __real__ cvalue = this->locdata[index].real();
                            __imag__ cvalue = this->locdata[index].imag();
                            harkio_Matrix_setValueComplex(this->ptf->loctfs[j], i, freqs[k], cvalue);
                        }
                    }
                }
            }
            else{
                this->ptf->loctfs = NULL;
            }
        }
        else{
            this->ptf->loctfs = NULL;
        }

        // septf
        if(!this->sepdata.empty()){
            if(this->file_type == FORMAT_TYPE::TF){
                this->ptf->septfs = (harkio_Matrix **) malloc(sizeof(harkio_Matrix *) * this->ptf->poses->size);
                for(size_t j = 0; j < num_src; j++){
                    this->ptf->septfs[j] = harkio_Matrix_new(const_cast<char*>(HARKFILEIO_COMPLEX_TYPESTR), this->ptf->mics->size, this->num_fft / 2 + 1);
                }

                // data order is (freq, src, mic)
                for(size_t k = 0; k < num_freq; k++){
                    for(size_t j = 0; j < num_src; j++){
                        for(size_t i = 0; i < num_mic; i++){
                            size_t index = k * num_src * num_mic + j * num_mic + i;
                            __real__ cvalue = this->sepdata[index].real();
                            __imag__ cvalue = this->sepdata[index].imag();
                            harkio_Matrix_setValueComplex(this->ptf->septfs[j], i, freqs[k], cvalue);
                        }
                    }
                }
            }
            else if(this->file_type == FORMAT_TYPE::W){
                this->ptf->septfs = (harkio_Matrix **) malloc(sizeof(harkio_Matrix *) * this->ptf->poses->size);
                for(size_t j = 0; j < num_src; j++){
                    this->ptf->septfs[j] = harkio_Matrix_new(const_cast<char*>(HARKFILEIO_COMPLEX_TYPESTR), this->ptf->mics->size, this->num_fft / 2 + 1);
                }

                // data order is (freq, mic, src)
                for(size_t k = 0; k < num_freq; k++){
                    for(size_t i = 0; i < num_mic; i++){
                        for(size_t j = 0; j < num_src; j++){
                            size_t index = k * num_mic * num_src + i * num_src + j;
                            __real__ cvalue = this->sepdata[index].real();
                            __imag__ cvalue = this->sepdata[index].imag();
                            harkio_Matrix_setValueComplex(this->ptf->septfs[j], i, freqs[k], cvalue);
                        }
                    }
                }
            }
            else{
                this->ptf->septfs = NULL;
            }
        }
        else{
            this->ptf->septfs = NULL;
        }

        if(this->file_type == FORMAT_TYPE::TF){
            harkio_TransferFunction_write(this->ptf, const_cast<char*>(filename.c_str()), const_cast<char*>(HARKFILEIO_SIGNATURE_TFMAT));
        }
        else if(this->file_type == FORMAT_TYPE::W){
            harkio_TransferFunction_write(this->ptf, const_cast<char*>(filename.c_str()), const_cast<char*>(HARKFILEIO_SIGNATURE_SPMAT));
        }
        else{
        }

        //harkio_TransferFunction_delete(&this->ptf); // Leave the file open.
    }
    catch(std::exception &ex){
        return false;
    }

    return true;
}

}

