/* $Id: sinfo_qr.c,v 1.5 2012-03-03 10:18:26 amodigli Exp $
 *
 * This file is part of the SINFONI Pipeline
 * Copyright (C) 2002,2003 European Southern Observatory
 *
 * 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: amodigli $
 * $Date: 2012-03-03 10:18:26 $
 * $Revision: 1.5 $
 * $Name: not supported by cvs2svn $
 */
/**@{*/
/**
 * @defgroup sinfo_qr_companion functions for polynomial solution
 *
 * TBD
 */


#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif

#include "sinfo_solve_poly_root.h"
//#include "sinfoni_recipes_defaults.h"

#define GSL_SET_COMPLEX_PACKED(zp,n,x,y) \
                do {*((zp)+2*(n))=(x); *((zp)+(2*(n)+1))=(y);} while(0)
#define GSL_DBL_EPSILON        2.2204460492503131e-16


int
sinfo_qr_companion (double *h, size_t nc, gsl_complex_packed_ptr zroot)
{
    double t = 0.0;

    size_t iterations, e, i, j, k, m;

    double w, x, y, s, z;

    double p = 0, q = 0, r = 0;

    /* FIXME: if p,q,r, are not set to zero then the compiler complains
     that they ``might be used uninitialized in this
     function''. Looking at the code this does seem possible, so this
     should be checked. */

    int notlast;

    size_t n = nc;

    next_root:

    if (n == 0)
        return 1 ;

    iterations = 0;

    next_iteration:

    for (e = n; e >= 2; e--)
    {
        double a1 = fabs (FMAT (h, e, e - 1, nc));
        double a2 = fabs (FMAT (h, e - 1, e - 1, nc));
        double a3 = fabs (FMAT (h, e, e, nc));

        if (a1 <= GSL_DBL_EPSILON * (a2 + a3))
            break;
    }

    x = FMAT (h, n, n, nc);

    if (e == n)
    {
        GSL_SET_COMPLEX_PACKED (zroot, n-1, x + t, 0); /* one real root */
        n--;
        goto next_root;
        /*continue;*/
    }

    y = FMAT (h, n - 1, n - 1, nc);
    w = FMAT (h, n - 1, n, nc) * FMAT (h, n, n - 1, nc);

    if (e == n - 1)
    {
        p = (y - x) / 2;
        q = p * p + w;
        y = sqrt (fabs (q));

        x += t;

        if (q > 0)        /* two real roots */
        {
            if (p < 0)
                y = -y;
            y += p;

            GSL_SET_COMPLEX_PACKED (zroot, n-1, x - w / y, 0);
            GSL_SET_COMPLEX_PACKED (zroot, n-2, x + y, 0);
        }
        else
        {
            GSL_SET_COMPLEX_PACKED (zroot, n-1, x + p, -y);
            GSL_SET_COMPLEX_PACKED (zroot, n-2, x + p, y);
        }
        n -= 2;

        goto next_root;
        /*continue;*/
    }

    /* No more roots found yet, do another iteration */

    if (iterations == 60)  /* increased from 30 to 60 */
    {
        /* too many iterations - give up! */
        cpl_msg_error("qr:","too many iterations-give up") ;
        return -1 ;
    }

    if (iterations % 10 == 0 && iterations > 0)
    {
        /* use an exceptional shift */

        t += x;

        for (i = 1; i <= n; i++)
        {
            FMAT (h, i, i, nc) -= x;
        }

        s = fabs (FMAT (h, n, n - 1, nc)) + fabs (FMAT (h, n - 1, n - 2, nc));
        y = 0.75 * s;
        x = y;
        w = -0.4375 * s * s;
    }

    iterations++;

    for (m = n - 2; m >= e; m--)
    {
        double a1, a2, a3;

        z = FMAT (h, m, m, nc);
        r = x - z;
        s = y - z;
        p = FMAT (h, m, m + 1, nc) + (r * s - w) / FMAT (h, m + 1, m, nc);
        q = FMAT (h, m + 1, m + 1, nc) - z - r - s;
        r = FMAT (h, m + 2, m + 1, nc);
        s = fabs (p) + fabs (q) + fabs (r);
        p /= s;
        q /= s;
        r /= s;

        if (m == e)
            break;

        a1 = fabs (FMAT (h, m, m - 1, nc));
        a2 = fabs (FMAT (h, m - 1, m - 1, nc));
        a3 = fabs (FMAT (h, m + 1, m + 1, nc));

        if (a1 * (fabs (q) + fabs (r)) <= GSL_DBL_EPSILON * fabs (p) * (a2 + a3))
            break;
    }

    for (i = m + 2; i <= n; i++)
    {
        FMAT (h, i, i - 2, nc) = 0;
    }

    for (i = m + 3; i <= n; i++)
    {
        FMAT (h, i, i - 3, nc) = 0;
    }

    /* double QR step */

    for (k = m; k <= n - 1; k++)
    {
        notlast = (k != n - 1);

        if (k != m)
        {
            p = FMAT (h, k, k - 1, nc);
            q = FMAT (h, k + 1, k - 1, nc);
            r = notlast ? FMAT (h, k + 2, k - 1, nc) : 0.0;

            x = fabs (p) + fabs (q) + fabs (r);

            if (x == 0)
                continue;        /* FIXME????? */

            p /= x;
            q /= x;
            r /= x;
        }

        s = sqrt (p * p + q * q + r * r);

        if (p < 0)
            s = -s;

        if (k != m)
        {
            FMAT (h, k, k - 1, nc) = -s * x;
        }
        else if (e != m)
        {
            FMAT (h, k, k - 1, nc) *= -1;
        }

        p += s;
        x = p / s;
        y = q / s;
        z = r / s;
        q /= p;
        r /= p;

        /* do row modifications */

        for (j = k; j <= n; j++)
        {
            p = FMAT (h, k, j, nc) + q * FMAT (h, k + 1, j, nc);

            if (notlast)
            {
                p += r * FMAT (h, k + 2, j, nc);
                FMAT (h, k + 2, j, nc) -= p * z;
            }

            FMAT (h, k + 1, j, nc) -= p * y;
            FMAT (h, k, j, nc) -= p * x;
        }

        j = (k + 3 < n) ? (k + 3) : n;

        /* do column modifications */

        for (i = e; i <= j; i++)
        {
            p = x * FMAT (h, i, k, nc) + y * FMAT (h, i, k + 1, nc);

            if (notlast)
            {
                p += z * FMAT (h, i, k + 2, nc);
                FMAT (h, i, k + 2, nc) -= p * r;
            }
            FMAT (h, i, k + 1, nc) -= p * q;
            FMAT (h, i, k, nc) -= p;
        }
    }

    goto next_iteration;
}
/**@}*/
