//----------------------------------------------------------------------------
// ThreeD Quadric Error Function
//----------------------------------------------------------------------------

#include <cmath>
#include <string>

namespace PolyVox {

//----------------------------------------------------------------------------

#define MAXROWS 12
#define EPSILON 1e-5

//----------------------------------------------------------------------------

Vector3DFloat evaluateQEF(
    double mat[][3], double *vec, int rows)
{
    // perform singular value decomposition on matrix mat
    // into u, v and d.
    // u is a matrix of rows x 3 (same as mat);
    // v is a square matrix 3 x 3 (for 3 columns in mat);
    // d is vector of 3 values representing the diagonal
    // matrix 3 x 3 (for 3 colums in mat).
    double u[MAXROWS][3], v[3][3], d[3];
    computeSVD(mat, u, v, d, rows);

    // solve linear system given by mat and vec using the
    // singular value decomposition of mat into u, v and d.
    if (d[2] < 0.1)
        d[2] = 0.0;
    if (d[1] < 0.1)
        d[1] = 0.0;
    if (d[0] < 0.1)
        d[0] = 0.0;

    double x[3];
    solveSVD(u, v, d, vec, x, rows);

    return {static_cast<float>(x[0]), static_cast<float>(x[1]), static_cast<float>(x[2])};
}

//----------------------------------------------------------------------------

void computeSVD(
    double mat[][3],                // matrix (rows x 3)
    double u[][3],                  // matrix (rows x 3)
    double v[3][3],                 // matrix (3x3)
    double d[3],                    // vector (1x3)
    int rows)
{
    std::memcpy(u, mat, rows * 3 * sizeof(double));

    double *tau_u = d;
    double tau_v[2];

    factorize(u, tau_u, tau_v, rows);

    unpack(u, v, tau_u, tau_v, rows);

    diagonalize(u, v, tau_u, tau_v, rows);

    singularize(u, v, tau_u, rows);
}

//----------------------------------------------------------------------------

void factorize(
    double mat[][3],                // matrix (rows x 3)
    double tau_u[3],                // vector, (1x3)
    double tau_v[2],                // vector, (1x2)
    int rows)
{
    int y;

    // bidiagonal factorization of (rows x 3) matrix into :-
    // tau_u, a vector of 1x3 (for 3 columns in the matrix)
    // tau_v, a vector of 1x2 (one less column than the matrix)

    for (int i = 0; i < 3; ++i) {

        // set up a vector to reference into the matrix
        // from mat(i,i) to mat(m,i), that is, from the
        // i'th column of the i'th row and down all the way
        // through that column
        double *ptrs[MAXROWS];
        int num_ptrs = rows - i;
        for (int q = 0; q < num_ptrs; ++q)
            ptrs[q] = &mat[q + i][i];

        // perform householder transformation on this vector
        double tau = factorize_hh(ptrs, num_ptrs);
        tau_u[i] = tau;

        // all computations below this point are performed
        // only for the first two columns:  i=0 or i=1
        if (i + 1 < 3) {

            // perform householder transformation on the matrix
            // mat(i,i+1) to mat(m,n), that is, on the sub-matrix
            // that begins in the (i+1)'th column of the i'th
            // row and extends to the end of the matrix at (m,n)
            if (tau != 0.0) {
                for (int x = i + 1; x < 3; ++x) {
                    double wx = mat[i][x];
                    for (y = i + 1; y < rows; ++y)
                        wx += mat[y][x] * (*ptrs[y - i]);
                    double tau_wx = tau * wx;
                    mat[i][x] -= tau_wx;
                    for (y = i + 1; y < rows; ++y)
                        mat[y][x] -= tau_wx * (*ptrs[y - i]);
                }
            }

            // perform householder transformation on i'th row
            // (remember at this point, i is either 0 or 1)

            // set up a vector to reference into the matrix
            // from mat(i,i+1) to mat(i,n), that is, from the
            // (i+1)'th column of the i'th row and all the way
            // through to the end of that row
            ptrs[0] = &mat[i][i + 1];
            if (i == 0) {
                ptrs[1] = &mat[i][i + 2];
                num_ptrs = 2;
            } else // i == 1
                num_ptrs = 1;

            // perform householder transformation on this vector
            tau = factorize_hh(ptrs, num_ptrs);
            tau_v[i] = tau;

            // perform householder transformation on the sub-matrix
            // mat(i+1,i+1) to mat(m,n), that is, on the sub-matrix
            // that begins in the (i+1)'th column of the (i+1)'th
            // row and extends to the end of the matrix at (m,n)
            if (tau != 0.0) {
                for (y = i + 1; y < rows; ++y) {
                    double wy = mat[y][i + 1];
                    if (i == 0)
                        wy += mat[y][i + 2] * (*ptrs[1]);
                    double tau_wy = tau * wy;
                    mat[y][i + 1] -= tau_wy;
                    if (i == 0)
                        mat[y][i + 2] -= tau_wy * (*ptrs[1]);
                }
            }

        }  // if (i + 1 < 3)
    }
}

//----------------------------------------------------------------------------

double factorize_hh(double *ptrs[], int n)
{
    double tau = 0.0;

    if (n > 1) {
        double xnorm;
        if (n == 2)
            xnorm = std::abs(*ptrs[1]);
        else {
            double scl = 0.0;
            double ssq = 1.0;
            for (int i = 1; i < n; ++i) {
                double x = std::abs(*ptrs[i]);
                if (x != 0.0) {
                    if (scl < x) {
                        ssq = 1.0 + ssq * (scl / x) * (scl / x);
                        scl = x;
                    } else
                        ssq += (x / scl) * (x / scl);
                }
            }
            xnorm = scl * sqrt(ssq);
        }

        if (xnorm != 0.0) {
            double alpha = *ptrs[0];
            double beta = sqrt(alpha * alpha + xnorm * xnorm);
            if (alpha >= 0.0)
                beta = -beta;
            tau = (beta - alpha) / beta;

            double scl = 1.0 / (alpha - beta);
            *ptrs[0] = beta;
            for (int i = 1; i < n; ++i)
                *ptrs[i] *= scl;
        }
    }

    return tau;
}

//----------------------------------------------------------------------------

void unpack(
    double u[][3],                  // matrix (rows x 3)
    double v[3][3],                 // matrix (3x3)
    double tau_u[3],                // vector, (1x3)
    double tau_v[2],                // vector, (1x2)
    int rows)
{
    int i, y;

    // reset v to the identity matrix
    v[0][0] = v[1][1] = v[2][2] = 1.0;
    v[0][1] = v[0][2] = v[1][0] = v[1][2] = v[2][0] = v[2][1] = 0.0;

    for (i = 1; i >= 0; --i) {
        double tau = tau_v[i];

        // perform householder transformation on the sub-matrix
        // v(i+1,i+1) to v(m,n), that is, on the sub-matrix of v
        // that begins in the (i+1)'th column of the (i+1)'th row
        // and extends to the end of the matrix at (m,n).  the
        // householder vector used to perform this is the vector
        // from u(i,i+1) to u(i,n)
        if (tau != 0.0) {
            for (int x = i + 1; x < 3; ++x) {
                double wx = v[i + 1][x];
                for (y = i + 1 + 1; y < 3; ++y)
                    wx += v[y][x] * u[i][y];
                double tau_wx = tau * wx;
                v[i + 1][x] -= tau_wx;
                for (y = i + 1 + 1; y < 3; ++y)
                    v[y][x] -= tau_wx * u[i][y];
            }
        }
    }

    // copy superdiagonal of u into tau_v
    for (i = 0; i < 2; ++i)
        tau_v[i] = u[i][i + 1];

    // below, same idea for u:  householder transformations
    // and the superdiagonal copy

    for (i = 2; i >= 0; --i) {
        // copy superdiagonal of u into tau_u
        double tau = tau_u[i];
        tau_u[i] = u[i][i];

        // perform householder transformation on the sub-matrix
        // u(i,i) to u(m,n), that is, on the sub-matrix of u that
        // begins in the i'th column of the i'th row and extends
        // to the end of the matrix at (m,n).  the householder
        // vector used to perform this is the i'th column of u,
        // that is, u(0,i) to u(m,i)
        if (tau == 0.0) {
            u[i][i] = 1.0;
            if (i < 2) {
                u[i][2] = 0.0;
                if (i < 1)
                    u[i][1] = 0.0;
            }
            for (y = i + 1; y < rows; ++y)
                u[y][i] = 0.0;
        } else {
            for (int x = i + 1; x < 3; ++x) {
                double wx = 0.0;
                for (y = i + 1; y < rows; ++y)
                    wx += u[y][x] * u[y][i];
                double tau_wx = tau * wx;
                u[i][x] = -tau_wx;
                for (y = i + 1; y < rows; ++y)
                    u[y][x] -= tau_wx * u[y][i];
            }
            for (y = i + 1; y < rows; ++y)
                u[y][i] = u[y][i] * -tau;
            u[i][i] = 1.0 - tau;
        }
    }
}

//----------------------------------------------------------------------------

void diagonalize(
    double u[][3],                  // matrix (rows x 3)
    double v[3][3],                 // matrix (3x3)
    double tau_u[3],                // vector, (1x3)
    double tau_v[2],                // vector, (1x2)
    int rows)
{
    int i, j;

    chop(tau_u, tau_v, 3);

    // progressively reduce the matrices into diagonal form

    int b = 3 - 1;
    while (b > 0) {
        if (tau_v[b - 1] == 0.0)
            --b;
        else {
            int a = b - 1;
            while (a > 0 && tau_v[a - 1] != 0.0)
                --a;
            int n = b - a + 1;

            double u1[MAXROWS][3];
            double v1[3][3];
            for (j = a; j <= b; ++j) {
                for (i = 0; i < rows; ++i)
                    u1[i][j - a] = u[i][j];
                for (i = 0; i < 3; ++i)
                    v1[i][j - a] = v[i][j];
            }

            qrstep(u1, v1, &tau_u[a], &tau_v[a], rows, n);

            for (j = a; j <= b; ++j) {
                for (i = 0; i < rows; ++i)
                    u[i][j] = u1[i][j - a];
                for (i = 0; i < 3; ++i)
                    v[i][j] = v1[i][j - a];
            }

            chop(&tau_u[a], &tau_v[a], n);
        }
    }
}

//----------------------------------------------------------------------------

void chop(double *a, double *b, int n)
{
    double ai = a[0];
    for (int i = 0; i < n - 1; ++i) {
        double bi = b[i];
        double ai1 = a[i + 1];
        if (std::abs(bi) < EPSILON * (std::abs(ai) + std::abs(ai1)))
            b[i] = 0.0;
        ai = ai1;
    }
}

//----------------------------------------------------------------------------

void qrstep(
    double u[][3],                  // matrix (rows x cols)
    double v[][3],                  // matrix (3 x cols)
    double tau_u[],                 // vector (1 x cols)
    double tau_v[],                 // vector (1 x cols - 1)
    int rows, int cols)
{
    int i;

    if (cols == 2) {
        qrstep_cols2(u, v, tau_u, tau_v, rows);
        return;
    }

    // handle zeros on the diagonal or at its end
    for (i = 0; i < cols - 1; ++i)
        if (tau_u[i] == 0.0) {
            qrstep_middle(u, tau_u, tau_v, rows, cols, i);
            return;
        }
    if (tau_u[cols - 1] == 0.0) {
        qrstep_end(v, tau_u, tau_v, cols);
        return;
    }

    // perform qr reduction on the diagonal and off-diagonal

    double mu = qrstep_eigenvalue(tau_u, tau_v, cols);
    double y = tau_u[0] * tau_u[0] - mu;
    double z = tau_u[0] * tau_v[0];

    double ak = 0.0;
    double bk = 0.0;
    double zk;
    double ap = tau_u[0];
    double bp = tau_v[0];
    double aq = tau_u[1];
    double bq = tau_v[1];

    for (int k = 0; k < cols - 1; ++k) {
        double c, s;

        // perform Givens rotation on V

        computeGivens(y, z, &c, &s);

        for (i = 0; i < 3; ++i) {
            double vip = v[i][k];
            double viq = v[i][k + 1];
            v[i][k] = vip * c - viq * s;
            v[i][k + 1] = vip * s + viq * c;
        }

        // perform Givens rotation on B

        double bk1 = bk * c - z * s;
        double ap1 = ap * c - bp * s;
        double bp1 = ap * s + bp * c;
        double zp1 = aq * -s;
        double aq1 = aq * c;

        if (k > 0)
            tau_v[k - 1] = bk1;

        ak = ap1;
        bk = bp1;
        zk = zp1;
        ap = aq1;

        if (k < cols - 2)
            bp = tau_v[k + 1];
        else
            bp = 0.0;

        y = ak;
        z = zk;

        // perform Givens rotation on U

        computeGivens(y, z, &c, &s);

        for (i = 0; i < rows; ++i) {
            double uip = u[i][k];
            double uiq = u[i][k + 1];
            u[i][k] = uip * c - uiq * s;
            u[i][k + 1] = uip * s + uiq * c;
        }

        // perform Givens rotation on B

        double ak1 = ak * c - zk * s;
        bk1 = bk * c - ap * s;
        double zk1 = bp * -s;

        ap1 = bk * s + ap * c;
        bp1 = bp * c;

        tau_u[k] = ak1;

        ak = ak1;
        bk = bk1;
        zk = zk1;
        ap = ap1;
        bp = bp1;

        if (k < cols - 2)
            aq = tau_u[k + 2];
        else
            aq = 0.0;

        y = bk;
        z = zk;
    }

    tau_v[cols - 2] = bk;
    tau_u[cols - 1] = ap;
}

//----------------------------------------------------------------------------

void qrstep_middle(
    double u[][3],                  // matrix (rows x cols)
    double tau_u[],                 // vector (1 x cols)
    double tau_v[],                 // vector (1 x cols - 1)
    int rows, int cols, int col)
{
    double x = tau_v[col];
    double y = tau_u[col + 1];
    for (int j = col; j < cols - 1; ++j) {
        double c, s;

        // perform Givens rotation on U

        computeGivens(y, -x, &c, &s);
        for (int i = 0; i < rows; ++i) {
            double uip = u[i][col];
            double uiq = u[i][j + 1];
            u[i][col] = uip * c - uiq * s;
            u[i][j + 1] = uip * s + uiq * c;
        }

        // perform transposed Givens rotation on B

        tau_u[j + 1] = x * s + y * c;
        if (j == col)
            tau_v[j] = x * c - y * s;

        if (j < cols - 2) {
            double z = tau_v[j + 1];
            tau_v[j + 1] *= c;
            x = z * -s;
            y = tau_u[j + 2];
        }
    }
}

//----------------------------------------------------------------------------

void qrstep_end(
    double v[][3],                  // matrix (3 x 3)
    double tau_u[],                 // vector (1 x 3)
    double tau_v[],                 // vector (1 x 2)
    int cols)
{
    double x = tau_u[1];
    double y = tau_v[1];

    for (int k = 1; k >= 0; --k) {
        double c, s;

        // perform Givens rotation on V

        computeGivens(x, y, &c, &s);

        for (int i = 0; i < 3; ++i) {
            double vip = v[i][k];
            double viq = v[i][2];
            v[i][k] = vip * c - viq * s;
            v[i][2] = vip * s + viq * c;
        }

        // perform Givens rotation on B

        tau_u[k] = x * c - y * s;
        if (k == 1)
            tau_v[k] = x * s + y * c;
        if (k > 0) {
            double z = tau_v[k - 1];
            tau_v[k - 1] *= c;

            x = tau_u[k - 1];
            y = z * s;
        }
    }
}

//----------------------------------------------------------------------------

double qrstep_eigenvalue(
    double tau_u[],                 // vector (1 x 3)
    double tau_v[],                 // vector (1 x 2)
    int cols)
{
    double ta = tau_u[1] * tau_u[1] + tau_v[0] * tau_v[0];
    double tb = tau_u[2] * tau_u[2] + tau_v[1] * tau_v[1];
    double tab = tau_u[1] * tau_v[1];
    double dt = (ta - tb) / 2.0;
    double mu;
    if (dt >= 0.0)
        mu = tb - (tab * tab) / (dt + sqrt(dt * dt + tab * tab));
    else
        mu = tb + (tab * tab) / (sqrt(dt * dt + tab * tab) - dt);
    return mu;
}

//----------------------------------------------------------------------------

void qrstep_cols2(
    double u[][3],                  // matrix (rows x 2)
    double v[][3],                  // matrix (3 x 2)
    double tau_u[],                 // vector (1 x 2)
    double tau_v[],                 // vector (1 x 1)
    int rows)
{
    int i;
    double tmp;

    // eliminate off-diagonal element in [ 0  tau_v0 ]
    //                                   [ 0  tau_u1 ]
    // to make [ tau_u[0]  0 ]
    //         [ 0         0 ]

    if (tau_u[0] == 0.0) {
        double c, s;

        // perform transposed Givens rotation on B
        // multiplied by X = [ 0 1 ]
        //                   [ 1 0 ]

        computeGivens(tau_v[0], tau_u[1], &c, &s);

        tau_u[0] = tau_v[0] * c - tau_u[1] * s;
        tau_v[0] = tau_v[0] * s + tau_u[1] * c;
        tau_u[1] = 0.0;

        // perform Givens rotation on U

        for (i = 0; i < rows; ++i) {
            double uip = u[i][0];
            double uiq = u[i][1];
            u[i][0] = uip * c - uiq * s;
            u[i][1] = uip * s + uiq * c;
        }

        // multiply V by X, effectively swapping first two columns

        for (i = 0; i < 3; ++i) {
            tmp = v[i][0];
            v[i][0] = v[i][1];
            v[i][1] = tmp;
        }
    }

    // eliminate off-diagonal element in [ tau_u0  tau_v0 ]
    //                                   [ 0       0      ]

    else if (tau_u[1] == 0.0) {
        double c, s;

        // perform Givens rotation on B

        computeGivens(tau_u[0], tau_v[0], &c, &s);

        tau_u[0] = tau_u[0] * c - tau_v[0] * s;
        tau_v[0] = 0.0;

        // perform Givens rotation on V

        for (i = 0; i < 3; ++i) {
            double vip = v[i][0];
            double viq = v[i][1];
            v[i][0] = vip * c - viq * s;
            v[i][1] = vip * s + viq * c;
        }
    }

    // make colums orthogonal,

    else {
        double c, s;

        // perform Schur rotation on B

        computeSchur(tau_u[0], tau_v[0], tau_u[1], &c, &s);

        double a11 = tau_u[0] * c - tau_v[0] * s;
        double a21 = - tau_u[1] * s;
        double a12 = tau_u[0] * s + tau_v[0] * c;
        double a22 = tau_u[1] * c;

        // perform Schur rotation on V

        for (i = 0; i < 3; ++i) {
            double vip = v[i][0];
            double viq = v[i][1];
            v[i][0] = vip * c - viq * s;
            v[i][1] = vip * s + viq * c;
        }

        // eliminate off diagonal elements

        if ((a11 * a11 + a21 * a21) < (a12 * a12 + a22 * a22)) {

            // multiply B by X

            tmp = a11;
            a11 = a12;
            a12 = tmp;
            tmp = a21;
            a21 = a22;
            a22 = tmp;

            // multiply V by X, effectively swapping first
            // two columns

            for (i = 0; i < 3; ++i) {
                tmp = v[i][0];
                v[i][0] = v[i][1];
                v[i][1] = tmp;
            }
        }

        // perform transposed Givens rotation on B

        computeGivens(a11, a21, &c, &s);

        tau_u[0] = a11 * c - a21 * s;
        tau_v[0] = a12 * c - a22 * s;
        tau_u[1] = a12 * s + a22 * c;

        // perform Givens rotation on U

        for (i = 0; i < rows; ++i) {
            double uip = u[i][0];
            double uiq = u[i][1];
            u[i][0] = uip * c - uiq * s;
            u[i][1] = uip * s + uiq * c;
        }
    }
}

//----------------------------------------------------------------------------

void computeGivens(
    double a, double b, double *c, double *s)
{
    if (b == 0.0) {
        *c = 1.0;
        *s = 0.0;
    } else if (std::abs(b) > std::abs(a)) {
        double t = -a / b;
        double s1 = 1.0 / sqrt(1 + t * t);
        *s = s1;
        *c = s1 * t;
    } else {
        double t = -b / a;
        double c1 = 1.0 / sqrt(1 + t * t);
        *c = c1;
        *s = c1 * t;
    }
}

//----------------------------------------------------------------------------

void computeSchur(
    double a1, double a2, double a3,
    double *c, double *s)
{
    double apq = a1 * a2 * 2.0;

    if (apq == 0.0) {
        *c = 1.0;
        *s = 0.0;
    } else {
        double t;
        double tau = (a2 * a2 + (a3 + a1) * (a3 - a1)) / apq;
        if (tau >= 0.0)
            t = 1.0 / (tau + sqrt(1.0 + tau * tau));
        else
            t = - 1.0 / (sqrt(1.0 + tau * tau) - tau);
        *c = 1.0 / sqrt(1.0 + t * t);
        *s = t * (*c);
    }
}

//----------------------------------------------------------------------------

void singularize(
    double u[][3],                  // matrix (rows x 3)
    double v[3][3],                 // matrix (3x3)
    double d[3],                    // vector, (1x3)
    int rows)
{
    int i, j, y;

    // make singularize values positive

    for (j = 0; j < 3; ++j)
        if (d[j] < 0.0) {
            for (i = 0; i < 3; ++i)
                v[i][j] = -v[i][j];
            d[j] = -d[j];
        }

    // sort singular values in decreasing order

    for (i = 0; i < 3; ++i) {
        double d_max = d[i];
        int i_max = i;
        for (j = i + 1; j < 3; ++j)
            if (d[j] > d_max) {
                d_max = d[j];
                i_max = j;
            }

        if (i_max != i) {
            // swap eigenvalues
            double tmp = d[i];
            d[i] = d[i_max];
            d[i_max] = tmp;

            // swap eigenvectors
            for (y = 0; y < rows; ++y) {
                tmp = u[y][i];
                u[y][i] = u[y][i_max];
                u[y][i_max] = tmp;
            }
            for (y = 0; y < 3; ++y) {
                tmp = v[y][i];
                v[y][i] = v[y][i_max];
                v[y][i_max] = tmp;
            }
        }
    }
}

//----------------------------------------------------------------------------

void solveSVD(
    double u[][3],                  // matrix (rows x 3)
    double v[3][3],                 // matrix (3x3)
    double d[3],                    // vector (1x3)
    double b[],                     // vector (1 x rows)
    double x[3],                    // vector (1x3)
    int rows)
{
    static double zeroes[3] = { 0.0, 0.0, 0.0 };

    int i, j;

    // compute vector w = U^T * b

    double w[3];
    std::memcpy(w, zeroes, sizeof(w));
    for (i = 0; i < rows; ++i) {
        if (b[i] != 0.0)
            for (j = 0; j < 3; ++j)
                w[j] += b[i] * u[i][j];
    }

    // introduce non-zero singular values in d into w

    for (i = 0; i < 3; ++i) {
        if (d[i] != 0.0)
            w[i] /= d[i];
    }

    // compute result vector x = V * w

    for (i = 0; i < 3; ++i) {
        double tmp = 0.0;
        for (j = 0; j < 3; ++j)
            tmp += w[j] * v[i][j];
        x[i] = tmp;
    }
}

}