/**
 * @brief Convex Quadratic Programming library
 *
 * From:
 * http://ra.uni-trier.de/~huebner/software.html
 *
 * ----------------------------------------------------------------------
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 * ----------------------------------------------------------------------
 *
 * @author Ewgenij Hübner
 *
 * $$
 */

#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>

#include "initial_point.h"

int pdip_initial_point_feasible_x(const gsl_matrix *A, const gsl_vector *b,
                                  gsl_vector *x) {
    int signum;

    /* x = A^t*(AA^t)^{-1}*b */

    gsl_matrix *AA_t;
    gsl_vector *tmp;
    gsl_permutation *p;

    AA_t = gsl_matrix_calloc(A->size1, A->size1);
    if (AA_t == 0) {
        GSL_ERROR_VAL("failed to initialize space for finding initial point",
                      GSL_ENOMEM, 0);
    }

    tmp = gsl_vector_alloc(A->size1);
    if (tmp == 0) {
        gsl_matrix_free(AA_t);
        GSL_ERROR_VAL("failed to initialize space for finding initial point",
                      GSL_ENOMEM, 0);
    }

    p = gsl_permutation_alloc(A->size1);
    if (p == 0) {
        gsl_vector_free(tmp);
        gsl_matrix_free(AA_t);
        GSL_ERROR_VAL("failed to initialize space for finding initial point",
                      GSL_ENOMEM, 0);
    }

    /* status = gsl_blas_dsyrk(CblasUpper, CblasNoTrans, 1.0, cqp->A, 0.0,
     * AA_t); */
    gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, A, A, 0.0, AA_t);

    /* (AA^t)tmp=b */
    gsl_linalg_LU_decomp(AA_t, p, &signum);
    gsl_linalg_LU_solve(AA_t, p, b, tmp);

    /*
    gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, AA_t, tmp);
    tmp=(AA^t)^{-1}tmp */

    gsl_blas_dgemv(CblasTrans, 1.0, A, tmp, 0.0, x);

    gsl_matrix_free(AA_t);
    gsl_vector_free(tmp);
    gsl_permutation_free(p);

    return GSL_SUCCESS;
}

int pdip_initial_point_feasible_s(const gsl_matrix *C, const gsl_vector *d,
                                  const gsl_vector *x, gsl_vector *s) {
    /* s=Cx-d */

    gsl_blas_dcopy(d, s);

    gsl_blas_dgemv(CblasNoTrans, 1.0, C, x, -1.0, s);

    return GSL_SUCCESS;
}

int pdip_initial_point_y(const gsl_matrix *Q, const gsl_vector *q,
                         const gsl_matrix *A, const gsl_vector *x,
                         gsl_vector *y) {
    int signum;

    /* y=(AA^t)^{-1}(A(Qx+q)) */

    gsl_matrix *AA_t;
    gsl_vector *tmp;
    gsl_permutation *p;

    AA_t = gsl_matrix_calloc(A->size1, A->size1);
    if (AA_t == 0) {
        GSL_ERROR_VAL("failed to initialize space for finding initial point",
                      GSL_ENOMEM, 0);
    }

    tmp = gsl_vector_alloc(q->size);
    if (tmp == 0) {
        gsl_matrix_free(AA_t);
        GSL_ERROR_VAL("failed to initialize space for finding initial point",
                      GSL_ENOMEM, 0);
    }

    p = gsl_permutation_alloc(AA_t->size1);
    if (p == 0) {
        gsl_vector_free(tmp);
        gsl_matrix_free(AA_t);
        GSL_ERROR_VAL("failed to initialize space for finding initial point",
                      GSL_ENOMEM, 0);
    }

    gsl_blas_dcopy(q, tmp);
    gsl_blas_dsymv(CblasUpper, 1.0, Q, x, 1.0, tmp);
    gsl_blas_dgemv(CblasNoTrans, 1.0, A, tmp, 0.0, y);

    /* gsl_blas_dsyrk(CblasUpper, CblasNoTrans, 1.0, cqp->A, 0.0, AA_t); */

    gsl_vector_free(tmp);
    tmp = gsl_vector_alloc(y->size);
    if (tmp == 0) {
        gsl_permutation_free(p);
        gsl_matrix_free(AA_t);
        GSL_ERROR_VAL("failed to initialize space for finding initial point",
                      GSL_ENOMEM, 0);
    }

    gsl_blas_dcopy(y, tmp);

    gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, A, A, 0.0, AA_t);

    gsl_linalg_LU_decomp(AA_t, p, &signum);
    gsl_linalg_LU_solve(AA_t, p, tmp, y);

    /* gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, AA_t, y); */

    gsl_matrix_free(AA_t);
    gsl_vector_free(tmp);
    gsl_permutation_free(p);

    return GSL_SUCCESS;
}

int pdip_initial_point_z(gsl_vector *z) {
    double delta_z = 0.1;

    gsl_vector_set_all(z, delta_z);

    return GSL_SUCCESS;
}

int pdip_initial_point_strict_feasible(gsl_vector *x, gsl_vector *s) {
    double delta_x, delta_s, xs = 0.0, sum_x = 0.0, sum_s = 0.0, tmp,
                             rg = 1e-10;

    size_t i;

    // int status;

    delta_x = GSL_MAX_DBL(-1.5 * gsl_vector_min(x), 0.0);
    delta_s = GSL_MAX_DBL(-1.5 * gsl_vector_min(s), 0.0);

    if (delta_x < rg && delta_s < rg) {
        // status = gsl_blas_ddot(x, s, &tmp);
        gsl_blas_ddot(x, s, &tmp);

        if (tmp < rg) /* the initial point is optimal */
            return GSL_SUCCESS;
    }

    for (i = 0; i < x->size; i++) {
        xs +=
            (gsl_vector_get(x, i) + delta_x) * (gsl_vector_get(s, i) + delta_s);

        sum_x += gsl_vector_get(x, i);
        sum_s += gsl_vector_get(s, i);
    }

    sum_x += delta_x * (x->size);
    sum_s += delta_s * (s->size);

    delta_x += 0.5 * xs / sum_s;
    delta_s += 0.5 * xs / sum_x;

    // status = gsl_vector_add_constant(x, delta_x);
    // status = gsl_vector_add_constant(s, delta_s);
    gsl_vector_add_constant(x, delta_x);
    gsl_vector_add_constant(s, delta_s);

    return GSL_SUCCESS;
}
