/**
* @file AlgebraTools.cpp
* @brief AlgebraTools Class
* @author Michael Koch
* @date 08/19/2008

 */
#include "AlgebraTools.h"

using namespace OBJREC;
using namespace std;
using namespace NICE;



//calculate mean


// refactor-nice.pl: check this substitution
// old: void AlgebraTools::calculateMean(const Matrix &data, Vector &mean)
void AlgebraTools::calculateMean(const NICE::Matrix &data, NICE::Vector &mean)
{
    uint features = data.rows();
    uint dimension = data.cols();
    mean = Vector(dimension);
    for (uint k = 0;k < mean.size();k++)
    {
        mean[k] = 0.0;
    }
    for (uint r = 0;r < features;r++)
    {
        for (uint c = 0;c < dimension;c++)
        {
            // refactor-nice.pl: check this substitution
            // old: if (isnan(data[r][c]) || isinf(data[r][c]))
            if(isnan(data(r, c)) ||(isnan(data(r, c))))
            {
                //strange data
            }
            else
            {
                // refactor-nice.pl: check this substitution
                // old: mean[c] = mean[c] + data[r][c];
                mean[c] = mean[c] + data(r, c);
            }
        }
    }
    for (uint r = 0;r < mean.size();r++)
    {
        if (features > 0)
        {
            mean[r] = mean[r] / (double)features;
        }
    }
    //      cout<<"mean-calc"<<mean<<endl;
}

//calculate mean
void AlgebraTools::calculateMean(const NICE::Matrix &data, NICE::Vector &mean, NICE::Vector &sigma)
{
    uint features = data.rows();
    uint dimension = data.cols();
    mean = Vector(dimension);
    sigma = Vector(dimension);

    for (uint k = 0;k < mean.size();k++)
    {
        mean[k] = 0.0;
        sigma[k] = 0.0;
    }
    for (uint r = 0;r < features;r++)
    {
        for (uint c = 0;c < dimension;c++)
        {

            // refactor-nice.pl: check this substitution
            // old: if (isnan(data[r][c]) || isinf(data[r][c]))
            if(isnan(data(r, c)) ||(isnan(data(r, c))))
            {
                //strange data
            }
            else
            {
                // refactor-nice.pl: check this substitution
                // old: mean[c] = mean[c] + data[r][c];
                mean[c] = mean[c] + data(r, c);
            }

        }
    }
    for (uint k = 0;k < mean.size();k++)
    {
        if (features > 0)
        {
            mean[k] = mean[k] / (double)features;
        }
    }
    //      cout<<"mean-calc"<<mean<<endl;
    //calc sigma
    for (uint r = 0;r < features;r++)
    {
        for (uint c = 0;c < dimension;c++)
        {

            // refactor-nice.pl: check this substitution
            // old: sigma[c] = sigma[c] + (mean[c] - data[r][c]) * (mean[c] - data[r][c]);
            sigma[c] = sigma[c] + (mean[c] -data(r, c)) * (mean[c] -data(r, c));

        }
    }
    for (uint k = 0;k < sigma.size();k++)
    {
        if (features > 0)
        {
            sigma[k] = sigma[k] / features;
            sigma[k] = sqrt(sigma[k]);
        }
    }
}


// refactor-nice.pl: check this substitution
// old: double AlgebraTools::trace(const Matrix &M)
double AlgebraTools::trace(const NICE::Matrix &M)
{
    if (M.cols() != M.rows())
    {
        throw("non quadratic Matrix");
    }
    else
    {
        uint n = M.rows();
        double sum = 0.0;
        for (uint i = 0;i < n;i++)
        {
            // refactor-nice.pl: check this substitution
            // old: sum += M[i][i];
            sum +=M(i, i);
        }
        return sum;
    }

}





double AlgebraTools::matrixRowColNorm(const NICE::Matrix &M)
{
    //max row norm
    double maxr = 0.0, maxc = 0.0;
    for (uint r = 0;r < M.rows();r++)
    {
        double sum = 0.0;
        for (uint c = 0;c < M.cols();c++)
        {
            sum +=fabs(M(r, c));
        }
        if (r == 0)
        {
            maxr = sum;
        }
        else
        {
            if (sum > maxr)
            {
                maxr = sum;
            }
        }
    }

    //max col norm
    for (uint c = 0;c < M.cols();c++)
    {
        double sum = 0.0;
        for (uint r = 0;r < M.rows();r++)
        {
            sum +=fabs(M(r, c));
        }
        if (c == 0)
        {
            maxc = sum;
        }
        else
        {
            if (sum > maxc)
            {
                maxc = sum;
            }
        }
    }
    if (maxr > maxc)
    {
        return maxr;
    }
    else
    {
        return maxc;
    }
}
void AlgebraTools::orthogonalize(Matrix &M, bool iterative)
{
#if 1
    fprintf (stderr, "AlgebraTools::orthogonalize: FIX THIS CODE !\n");
    exit(-1);
#else

    //iterative
    if (iterative)
    {
        NICE::Vector onevec = Vector(M.rows());
        for (uint k = 0;k < onevec.size();k++)
        {
            onevec[k] = 1.0;
        }
        NICE::Matrix One = diag(onevec);
        NICE::Matrix Mt = M.transpose();
        NICE::Matrix error;
	error.multiply (M, Mt);
	error -= One;
        double errornorm = matrixRowColNorm(error);
        double deltaerror = 2.0;
        //reduceddim
        while (deltaerror > 1.0e-03)
        {
            double norm = matrixRowColNorm(M);
            if (norm > 0.0)
            {
                // refactor: M = (1.0 / norm) * M;
                M *= (1.0 / norm);
            }
            Mt = M.transpose();
            M = 1.5 * M - 0.5 * ((M * Mt) * M);
            Mt = M.transpose();
            //convergencetest
            error = (M * (Mt) - One);

            deltaerror = errornorm;
            errornorm = matrixRowColNorm(error);
            deltaerror = fabs(deltaerror - errornorm);

        }
    }
    else
    {
        //noniteratve
        // refactor-nice.pl: check this substitution
        // old: Matrix D, eigenvectors;
        NICE::Matrix D, eigenvectors;
        // refactor-nice.pl: check this substitution
        // old: Vector eigenvalue;
        NICE::Vector eigenvalue;
        // refactor-nice.pl: check this substitution
        // old: Matrix MMt = M*!M;
        NICE::Matrix MMt;
	MMt.multiply ( M, M.transpose() );
        Eigenvalue(MMt, eigenvalue, eigenvectors);
        D = diag(eigenvalue);
        for (int d = 0;d < D.rows();d++)
        {
            // refactor-nice.pl: check this substitution
            // old: if (D[d][d] > 0.0)
            if(D(d, d) > 0.0)
            {
                // refactor-nice.pl: check this substitution
                // old: D[d][d] = 1.0 / sqrt(D[d][d]);
                D(d, d) = 1.0 /D(d, d));
            }
        }
        // refactor-nice.pl: check this substitution
        // old: Matrix Et = !eigenvectors;
        NICE::Matrix Et = !eigenvectors;
        // refactor-nice.pl: check this substitution
        // old: Matrix MMtinvsqrt = eigenvectors * D * Et;
        NICE::Matrix MMtinvsqrt = eigenvectors * D * Et;
        M = MMtinvsqrt * M;
    }
#endif
}
// refactor-nice.pl: check this substitution
// old: Vector AlgebraTools::meanOfMatrix(const Matrix &M)
NICE::Vector AlgebraTools::meanOfMatrix(const NICE::Matrix &M)
{
#if 1
    fprintf (stderr, "AlgebraTools:: FIX THIS CODE !\n");
    exit(-1);
#else
    // refactor-nice.pl: check this substitution
    // old: Vector meanvec = Vector(M.cols());
    NICE::Vector meanvec = Vector(M.cols());
    //initialize meanvec
    for (uint k = 0;k < meanvec.size();k++)
    {
        meanvec[k] = 0.0;
    }
    //calculate mean
    for (int f = 0;f < M.rows();f++)
    {
        meanvec = M[f] + meanvec;
    }
    if (M.rows() > 0)
    {
        meanvec = 1.0 / M.rows() * meanvec;
    }
    return meanvec;
#endif
}
// refactor-nice.pl: check this substitution
// old: double AlgebraTools::meanOfVector(const Vector &vec)
double AlgebraTools::meanOfVector(const NICE::Vector &vec)
{
#if 1
    fprintf (stderr, "AlgebraTools:: FIX THIS CODE !\n");
    exit(-1);
#else
    double mean = 0.0;
    for (uint k = 0;k < vec.size();k++)
    {
        mean += vec[k];
    }
    if (vec.size() > 0)
    {
        mean /= vec.size();
    }
    return mean;
#endif
}

// refactor-nice.pl: check this substitution
// old: Matrix AlgebraTools::vectorToMatrix(const Vector &vec)
NICE::Matrix AlgebraTools::vectorToMatrix(const NICE::Vector &vec)
{
    if (vec.size() > 0)
    {
        // refactor-nice.pl: check this substitution
        // old: Matrix D = Matrix(vec.size(), 1, 0);
        NICE::Matrix D = Matrix(vec.size(), 1, 0);
        for (uint k = 0;k < vec.size();k++)
        {
            // refactor-nice.pl: check this substitution
            // old: D[k][0] = vec[k];
            D(k, 0) = vec[k];
        }
        return D;
    }
    else
    {
        return Matrix();
    }
}
// refactor-nice.pl: check this substitution
// old: Vector AlgebraTools::matrixToVector(const Matrix &mat)
NICE::Vector AlgebraTools::matrixToVector(const NICE::Matrix &mat)
{
    if (mat.rows() < 1 && mat.cols() < 1)
    {
        fprintf(stderr, "Matrix has no entries!");
    }
    if (mat.rows() > mat.cols())
    {
        // refactor-nice.pl: check this substitution
        // old: Vector tmp = Vector(mat.rows());
        NICE::Vector tmp (mat.rows());
	tmp.set(0.0);
        for (uint i = 0;i < mat.rows();i++)
        {
            // refactor-nice.pl: check this substitution
            // old: tmp[i] = mat[i][0];
            tmp[i] =mat(i, 0);
        }
        return tmp;
    }
    else
    {
        // refactor-nice.pl: check this substitution
        // old: Vector tmp = Vector(mat.cols());
        NICE::Vector tmp = Vector(mat.cols());
        for (uint i = 0;i < mat.cols();i++)
        {
            // refactor-nice.pl: check this substitution
            // old: tmp[i] = mat[0][i];
            tmp[i] =mat(0, i);
        }
        return tmp;
    }
}
// refactor-nice.pl: check this substitution
// old: Matrix AlgebraTools::diag(const Vector &diagonalelements)
NICE::Matrix AlgebraTools::diag(const NICE::Vector &diagonalelements)
{
    if (diagonalelements.size() > 0)
    {
        // refactor-nice.pl: check this substitution
        // old: Matrix D = Matrix(diagonalelements.size(), diagonalelements.size(), 0);
        NICE::Matrix D (diagonalelements.size(), diagonalelements.size());
	D.set(0.0);
        for (uint k = 0;k < diagonalelements.size();k++)
        {
            // refactor-nice.pl: check this substitution
            // old: D[k][k] = diagonalelements[k];
            D(k, k) = diagonalelements[k];
        }
        return D;
    }
    else
    {
        return Matrix();
    }

}