#include "cve.h"

int getWorkLen(const int n, const int p, const int q) {
    int mpq; /**< Max of p and q */
    int nn = ((n - 1) * n) / 2;

    if (p > q) {
        mpq = p;
    } else {
        mpq = q;
    }
    if (nn * p < (mpq + 1) * mpq) {
        return 2 * (mpq + 1) * mpq;
    } else {
        return (nn + mpq) * mpq;
    }
}

double cost(const unsigned int method,
            const int n,
            const double *Y,
            const double *vecK,
            const double *colSums,
            double *y1, double *L) {
    int i, j, k;
    double tmp, sum;

    for (i = 0; i < n; ++i) {
        y1[i] = Y[i];
        L[i] = Y[i] * Y[i];
    }

    for (k = j = 0; j < n; ++j) {
        for (i = j + 1; i < n; ++i, ++k) {
            y1[i] += Y[j] * vecK[k];
            y1[j] += Y[i] * vecK[k];
            L[i] += Y[j] * Y[j] * vecK[k];
            L[j] += Y[i] * Y[i] * vecK[k];
        }
    }

    for (i = 0; i < n; ++i) {
        y1[i] /= colSums[i];
        L[i] /= colSums[i];
    }

    tmp = 0.0;
    if (method == CVE_METHOD_WEIGHTED) {
        sum = 0.0;
        for (i = 0; i < n; ++i) {
            tmp += (colSums[i] - 1.0) * (L[i] -= y1[i] * y1[i]);
            sum += colSums[i];
        }
        return tmp / (sum - (double)n); // TODO: check for division by zero!
    } else {
        for (i = 0; i < n; ++i) {
            tmp += (L[i] -= y1[i] * y1[i]);
        }
        return tmp / (double)n;
    }
}

void scaling(const unsigned int method,
             const int n,
             const double *Y, const double *y1, const double *L,
             const double *vecD, const double *vecK,
             const double *colSums,
             double *vecS) {
    int i, j, k, nn = (n * (n - 1)) / 2;
    double tmp;

    if (method == CVE_METHOD_WEIGHTED) {
        for (k = j = 0; j < n; ++j) {
            for (i = j + 1; i < n; ++i, ++k) {
                tmp = Y[j] - y1[i];
                vecS[k]  = (L[i] - (tmp * tmp));
                tmp = Y[i] - y1[j];
                vecS[k] += (L[j] - (tmp * tmp));
            }
        }
    } else {
        for (k = j = 0; j < n; ++j) {
            for (i = j + 1; i < n; ++i, ++k) {
                tmp = Y[j] - y1[i];
                vecS[k]  = (L[i] - (tmp * tmp)) / colSums[i];
                tmp = Y[i] - y1[j];
                vecS[k] += (L[j] - (tmp * tmp)) / colSums[j];
            }
        }
    }

    for (k = 0; k < nn; ++k) {
        vecS[k] *= vecK[k] * vecD[k];
    }
}