#include "utils.h"

void util::saveFileToMatlab(std::string fileName, cv::Mat a, std::string varName){
    int rows = (int) a.rows;
    int cols = (int) a.cols;

    std::ofstream myfile;
    myfile.open (fileName);
    myfile << "%File generated with Pollen3D \n";

    myfile << varName << " = [";

    for (int row = 0; row < rows; row++){
        for (int col = 0; col < cols; col++){
            myfile << a.at<double>(row,col) << " ";
        }
        if ( row != rows - 1) myfile << ";\n";
        else                  myfile << " ];\n";
    }

    myfile.close();
}


double util::rad2deg(double angRad){
    return angRad / PI * 180.0;
}

double util::deg2rad(double angDeg){
    return angDeg / 180.0 * PI;
}

/*! Make matrix non-homogenious. It is mainly used for measurement matrix \f$W\f$.
 * If ((W mod 3) == 0) it will supress every third row
 */
void util::makeNonHomogenious(cv::Mat &m)
{
    // Make W non-homogenious
    bool isNonHomogenious = !( (m.rows % 3) == 0 && (m.at<double>(2,0) == 1));
    if (isNonHomogenious)
        return;

    cv::Mat newM;

    for (int i = 0 ; i < m.rows / 3 ; i++){
        newM.push_back(m.row(3*i));
        newM.push_back(m.row(3*i + 1));
    }

    m.release();
    m = newM.clone();
}

/*! Copy elements of matrix with indices idx to vector
 * Equivalent to MATLAB: v = M(idx);
 */
void util::copyMatElementsToVector(const cv::Mat &mat, const cv::Mat &idx, std::vector<double> &vec)
{
    int nbIdx = (int) idx.total();

    for (int i = 0; i < nbIdx; i++){
        int col = (int) idx.at<cv::Point>(i).x;
        int row = (int) idx.at<cv::Point>(i).y;

        vec[i] = mat.at<double>(row,col);
    }
}

/*! Copy vector to matrix elements with indices idx
 * Equivalent to MATLAB: M(idx) = v;
 */
void util::copyVectorToMatElements(const std::vector<double> &vec, const cv::Mat &idx, cv::Mat &mat)
{
    int nbIdx = (int) idx.total();

    for (int i = 0; i < nbIdx; i++){
        int col = (int) idx.at<cv::Point>(i).x;
        int row = (int) idx.at<cv::Point>(i).y;

        mat.row(row).col(col) = vec[i];
    }
}

/*! Concatenate vector of matrices
 * \param matArray vector of matrices
 * \param method concatenation method
 * - CONCAT_HORIZONTAL (Horizontally) : M = [M1 M2 M3 M4]
 * - CONCAT_VERTICAL (Vertically)  : M = [M1;M2;M3;M4]
 */
cv::Mat util::concatenateMat(const std::vector<cv::Mat> &matArray, int method)
{
    cv::Mat outMat;
    if (method == CONCAT_VERTICAL){
        for(auto& m:matArray){
            outMat.push_back(m.clone());
        }
    }else{
        for(auto& m:matArray){
            cv::Mat mt = m.t();
            outMat.push_back(mt.clone());
        }
        outMat = outMat.t();
    }
    return outMat.clone();
}