/*
 * This file is part of the ESO SINFONI Pipeline
 * Copyright (C) 2004,2005 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, 51 Franklin St, Fifth Floor, Boston, MA  02111-1307  USA
 */
/**************************************************************************
* E.S.O. - VLT project
*
*
*
* who       when      what
* --------  --------  ----------------------------------------------
* schreib  16/04/03  created
*/
#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif

#include "sinfo_svd.h"
#include "sinfo_msg.h"
/**@{*/
/**
 * @addtogroup sinfo_utilities Single Value Decomposition utilities
 *
 * TBD
 */

void sinfo_fpol(float x, float *p, int np)
{
    int j ;
    
    p[1] = 1.0 ;
    for ( j = 2 ; j <= np ; j++ )
    {
        p[j] = p[j-1]*x ;
    }
}

void 
sinfo_svb_kas(float **u, float w[], float **v, int m, 
              int n, float b[],float x[])


{
        int jj,j,i;
        float s,*tmp;

        tmp=sinfo_vector(1,n);
        for (j=1;j<=n;j++) {
                s=0.0;
                if (w[j]) {
                        for (i=1;i<=m;i++) s += u[i][j]*b[i];
                        s /= w[j];
                }
                tmp[j]=s;
        }
        for (j=1;j<=n;j++) {
                s=0.0;
                for (jj=1;jj<=n;jj++) s += v[j][jj]*tmp[jj];
                x[j]=s;
        }
        sinfo_free_vector(tmp,1/*,n*/);
}

void sinfo_svd_variance(float **v , int ma , float w[] , float **cvm)
{
        int k,j,i;
        float sum,*wti;

        wti=sinfo_vector(1,ma);
        for (i=1;i<=ma;i++) {
                wti[i]=0.0;
                if (w[i]) wti[i]=1.0/(w[i]*w[i]);
        }
        for (i=1;i<=ma;i++) {
            for (j=1;j<=i;j++) {
               for (sum=0.0,k=1;k<=ma;k++) sum += (v[i][k]*v[j][k]*wti[k]);
               cvm[j][i]=cvm[i][j]=sum;
            }
        }
        sinfo_free_vector(wti,1/*,ma*/);
}

#define TOL 1.0e-5

void sinfo_svd_fitting ( float *x,
                   float *y,
                   float *sig,
                   int   ndata,
                   float *a,
                   int   ma,
                   float **u,
                   float **v,
                   float *w,
                   float **cvm,
                   float *chisq,
                   void (*funcs)(float,float *,int) )
{
        int j,i;
        float /*sini,*/wmax,tmp,thresh,sum,*b,*afunc;


        b=sinfo_vector(1,ndata);
        afunc=sinfo_vector(1,ma);
        for (i=1;i<=ndata;i++) {

                (*funcs)(x[i],afunc,ma);
                tmp=1.0/sig[i];
                for (j=1;j<=ma;j++) {
                        u[i][j]=afunc[j]*tmp;
                }
                b[i]=y[i]*tmp;
        }
        sinfo_svd_compare(u,ndata,ma,w,v);

        wmax=0.0;
        for (j=1;j<=ma;j++)
                if (w[j] > wmax) wmax=w[j];
        thresh=TOL*wmax;
        for (j=1;j<=ma;j++) {
          if (w[j] < thresh) {
            w[j]=0.0;
            sinfo_msg_warning("SVD_FITTING detected singular value in fit !");
          }
        }
        sinfo_svb_kas(u,w,v,ndata,ma,b,a);
        *chisq=0.0;
        for (i=1;i<=ndata;i++) {
                (*funcs)(x[i],afunc,ma);
                for (sum=0.0,j=1;j<=ma;j++) sum += a[j]*afunc[j];
                *chisq += (tmp=(y[i]-sum)/sig[i],tmp*tmp);
        }
        sinfo_free_vector(afunc,1/*,ma*/);
        sinfo_free_vector(b,1/*,ndata*/);
        sinfo_svd_variance(v,ma,w,cvm);

}

#undef TOL



static float at,bt,ct;
#define SINFO_PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ? \
(ct=bt/at,at*sqrt(1.0+ct*ct)) : (bt ? (ct=at/bt,bt*sqrt(1.0+ct*ct)): 0.0))


static float maxarg1,maxarg2;
#define SINFO_MAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ?\
        (maxarg1) : (maxarg2))
#define SINFO_SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))

void sinfo_svd_compare(float **a,int m,int n,float w[],float **v)
{
        int flag,i,its,j,jj,k,l=0,nm=0;
        float c,f,h,s,x,y,z;
        float anorm=0.0,g=0.0,scale=0.0;
        float *rv1;

        if (m < n) {
           sinfo_msg_error("SVDCMP: You must augment A with extra zero rows");
        }
        rv1=sinfo_vector(1,n);
        for (i=1;i<=n;i++) {
                l=i+1;
                rv1[i]=scale*g;
                g=s=scale=0.0;
                if (i <= m) {
                        for (k=i;k<=m;k++) scale += fabs(a[k][i]);
                        if (scale) {
                                for (k=i;k<=m;k++) {
                                        a[k][i] /= scale;
                                        s += a[k][i]*a[k][i];
                                }
                                f=a[i][i];

                                g = -SINFO_SIGN(sqrt(s),f);
                                h=f*g-s;
                                a[i][i]=f-g;
                                if (i != n) {
                                        for (j=l;j<=n;j++) {
                                                for (s=0.0,k=i;k<=m;k++) {
                                                  s += a[k][i]*a[k][j];
                                                }
                                                f=s/h;
                                                for (k=i;k<=m;k++) {
                                                  a[k][j] += f*a[k][i];
                                                }
                                        }
                                }
                                for (k=i;k<=m;k++) a[k][i] *= scale;
                        }
                }
                w[i]=scale*g;
                g=s=scale=0.0;
                if (i <= m && i != n) {
                        for (k=l;k<=n;k++) scale += fabs(a[i][k]);
                        if (scale) {
                          for (k=l;k<=n;k++) {
                                 a[i][k] /= scale;
                                 s += a[i][k]*a[i][k];
                          }
                          f=a[i][l];

                          g = -SINFO_SIGN(sqrt(s),f);
                          h=f*g-s;
                          a[i][l]=f-g;
                          for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
                          if (i != m) {
                            for (j=l;j<=m;j++) {
                              for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
                              for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
                            }
                          }
                          for (k=l;k<=n;k++) a[i][k] *= scale;
                        }
                }
                anorm=SINFO_MAX(anorm,(fabs(w[i])+fabs(rv1[i])));
        }

        for (i=n;i>=1;i--) {
                if (i < n) {
                        if (g) {
                          for (j=l;j<=n;j++)
                              v[j][i]=(a[i][j]/a[i][l])/g;
                          for (j=l;j<=n;j++) {
                              for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
                              for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
                          }
                        }
                        for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
                }
                v[i][i]=1.0;
                g=rv1[i];
                l=i;
        }
        for (i=n;i>=1;i--) {
                l=i+1;
                g=w[i];
                if (i < n)
                        for (j=l;j<=n;j++) a[i][j]=0.0;
                if (g) {
                        g=1.0/g;
                        if (i != n) {
                            for (j=l;j<=n;j++) {
                              for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
                              f=(s/a[i][i])*g;
                              for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
                            }
                        }
                        for (j=i;j<=m;j++) a[j][i] *= g;
                } else {
                        for (j=i;j<=m;j++) a[j][i]=0.0;
                }
                ++a[i][i];
        }
        for (k=n;k>=1;k--) {
                for (its=1;its<=30;its++) {
                        flag=1;
                        for (l=k;l>=1;l--) {
                                nm=l-1;
                                if (fabs(rv1[l])+anorm == anorm) {
                                        flag=0;
                                        break;
                                }
                                if (fabs(w[nm])+anorm == anorm) break;
                        }
                        if (flag) {
                                c=0.0;
                                s=1.0;
                                for (i=l;i<=k;i++) {
                                        f=s*rv1[i];
                                        if (fabs(f)+anorm != anorm) {
                                                g=w[i];

                                                h=SINFO_PYTHAG(f,g);
                                                w[i]=h;
                                                h=1.0/h;
                                                c=g*h;
                                                s=(-f*h);
                                                for (j=1;j<=m;j++) {
                                                        y=a[j][nm];
                                                        z=a[j][i];
                                                        a[j][nm]=y*c+z*s;
                                                        a[j][i]=z*c-y*s;
                                                }
                                        }
                                }
                        }
                        z=w[k];
                        if (l == k) {
                                if (z < 0.0) {
                                        w[k] = -z;
                                        for (j=1;j<=n;j++) v[j][k]=(-v[j][k]);
                                }
                                break;
                        }
                        if (its == 30) {
                         sinfo_msg_error("No convergence in 30 "
                                         "SVDCMP iterations");
                        }
                        x=w[l];
                        nm=k-1;
                        y=w[nm];
                        g=rv1[nm];
                        h=rv1[k];
                        f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);

                        g=SINFO_PYTHAG(f,1.0);
                        f=((x-z)*(x+z)+h*((y/(f+SINFO_SIGN(g,f)))-h))/x;
                        c=s=1.0;
                        for (j=l;j<=nm;j++) {
                                i=j+1;
                                g=rv1[i];
                                y=w[i];
                                h=s*g;
                                g=c*g;

                                z=SINFO_PYTHAG(f,h);
                                rv1[j]=z;
                                c=f/z;
                                s=h/z;
                                f=x*c+g*s;
                                g=g*c-x*s;
                                h=y*s;
                                y=y*c;
                                for (jj=1;jj<=n;jj++) {
                                        x=v[jj][j];
                                        z=v[jj][i];
                                        v[jj][j]=x*c+z*s;
                                        v[jj][i]=z*c-x*s;
                                }

                                z=SINFO_PYTHAG(f,h);
                                w[j]=z;
                                if (z) {
                                        z=1.0/z;
                                        c=f*z;
                                        s=h*z;
                                }
                                f=(c*g)+(s*y);
                                x=(c*y)-(s*g);
                                for (jj=1;jj<=m;jj++) {
                                        y=a[jj][j];
                                        z=a[jj][i];
                                        a[jj][j]=y*c+z*s;
                                        a[jj][i]=z*c-y*s;
                                }
                        }
                        rv1[l]=0.0;
                        rv1[k]=f;
                        w[k]=x;
                }
        }
        sinfo_free_vector(rv1,1/*,n*/);
}

#undef SINFO_SIGN
#undef SINFO_MAX
#undef SINFO_PYTHAG

#define NR_END 1
#define FREE_ARG char*

void sinfo_nerror(const char error_text[])
{
        fprintf(stderr,"Runtime ERROR ...\n");
        fprintf(stderr,"%s\n",error_text);
        fprintf(stderr,"exiting system \n");
    return;
}

float *sinfo_vector(long nl, long nh)
{
        float *v;

        v=(float *)cpl_malloc((size_t) ((nh-nl+1+NR_END)*sizeof(float)));
        if (!v) {
           sinfo_msg_error("allocation failure in sinfo_vector()");
        }
        return v-nl+NR_END;

}

void sinfo_free_vector(float *v, long nl/* , long nh*/)

{
        cpl_free((FREE_ARG) (v+nl-NR_END));
}

float **sinfo_matrix(long nrl, long nrh, long ncl, long nch)
{
        long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
        float **m;

        m=(float **) cpl_malloc((size_t)((nrow+NR_END)*sizeof(float*)));
        if (!m) {
          sinfo_msg_error("aloccation failure 1 in sinfo_matrix()");
        }
        m += NR_END;
        m -= nrl;

        m[nrl]=(float *)cpl_malloc((size_t)((nrow*ncol+NR_END)*sizeof(float)));
        if (!m[nrl]) {
           sinfo_msg_error("allocation failure 2 in sinfo_matrix()");
        }
        m[nrl] += NR_END;
        m[nrl] -= ncl;

        for(i=nrl+1;i<=nrh;i++) m[i] = m[i-1]+ncol;
        return m;
}

void sinfo_free_matrix(float **m,long nrl/*, long nrh*/, long ncl/*, long nch*/)
{
        cpl_free((FREE_ARG)(m[nrl]+ncl-NR_END));
        cpl_free((FREE_ARG)(m+nrl-NR_END));
}

/**@}*/
