/***************************************************************/
/*   sml5.c : Regression Analysis for 5 variables,             */
/*             with linear regression for 2 variables,         */
/*              and quadratic regression for 3 variables.      */
/*-------------------------------------------------------------*/
/*   For the compensation of aircraft's magnetic noise,        */
/*    2 linear parameters are assigned to Lat. and Long.,      */
/*     while 3 quadratic parameters are the observed magnetic  */
/*      field components by the 3-axis fluxgate magnetometer.  */
/*   The 2 linear parameters represents the normal magnetic    */
/*    field change independent of aircraft's attitude.         */
/*     Here, the obs. altitude is not taken into regression,   */
/*      because of the small range of altitude variation.      */
/*       (So, the compensation test should be flown nearly     */
/*        in a constant level, far from anomaly sources.)      */
/***************************************************************/
#include    <stdio.h>
#include    <stdlib.h>

static double c[12][13];
static double uu[3], vv[3], xx[5], yy[5], zz[5];
static int kf=-1;

int sm5opn()
{   int i1, i2;
    if (kf > 0)
        { fprintf(stderr, "sm5opn: illegal sequence\n"); return(-1); }
    for (i1=0; i1<12; i1++) for (i2=0; i2<13; i2++) c[i1][i2] = 0.;
    uu[0] = vv[0] =  xx[0] = yy[0] = zz[0] = 1.;
    kf = 1; return(0);
}

int sm5ex(double u, double v, double x, double y, double z, double src)
{   int i1, i2, i3, i4, i5, i6, j, k;
    if (kf < 1)
        { fprintf(stderr, "sm5ex: illegal call sequence\n"); return(-1); }
    for (j=0; j<2; j++) { uu[j+1] = uu[j]*u; vv[j+1] = vv[j]*v; }
    for (j=0; j<4; j++) {
        xx[j+1] = xx[j]*x; yy[j+1] = yy[j]*y; zz[j+1] = zz[j]*z;
    }
    for (j=0,i1=0; i1<=1; i1++) {
        for (i2=0; i2<=i1; i2++,j++) {
            c[j][12] += src*uu[i1-i2]*vv[i2];
            for (k=0,i4=0; i4<=1; i4++) {
                for (i5=0; i5<=i4; i5++,k++) {
                    c[j][k] += uu[i1-i2+i4-i5]*vv[i2+i5];
                }
            }
            for (i4=1; i4<=2; i4++) {
                for (i5=0; i5<=i4; i5++) for (i6=0; i6<=i5; i6++,k++) {
                    c[j][k] += uu[i1-i2]*vv[i2] * xx[i4-i5]*yy[i5-i6]*zz[i6];
                }
            }
        }
    }
    for (i1=1; i1<=2; i1++) {
        for (i2=0; i2<=i1; i2++) for (i3=0; i3<=i2; i3++,j++) {
            c[j][12] += src*xx[i1-i2]*yy[i2-i3]*zz[i3];
            for (k=0,i4=0; i4<=1; i4++) {
                for (i5=0; i5<=i4; i5++,k++) {
                    c[j][k] += xx[i1-i2]*yy[i2-i3]*zz[i3] * uu[i4-i5]*vv[i5];
                }
            }
            for (i4=1; i4<=2; i4++) {
                for (i5=0; i5<=i4; i5++) for (i6=0; i6<=i5; i6++,k++) {
                    c[j][k] += xx[i1-i2+i4-i5]*yy[i2-i3+i5-i6]*zz[i3+i6];
                }
            }
        }
    }
    kf = 2; return(0);
}

int sm5cls(double *coef)
{   int i1, i2, i3;
    if (kf < 0)
        { fprintf(stderr, "sm5cls: illegal call sequence\n"); return(-1); }
    if (c[0][0] < 12.)
        { fprintf(stderr, "sm5cls: not enough data\n"); return(-2); }
    for (i1=0; i1<12; i1++) {
        for (i2=i1+1; i2<13; i2++) c[i1][i2] /= c[i1][i1];
        for (i3=0; i3<12; i3++) {
            if (i3 == i1) continue;
            for (i2=i1+1; i2<13; i2++) c[i3][i2] -= c[i3][i1]*c[i1][i2];
        }
    }
    for (i1=0; i1<12; i1++) *coef++ = c[i1][12];
    kf = 0; return(0);
}

int sm5rv(double ur, double vr, double xr, double yr, double zr, double *sr)
{   int i1, i2, i3, j;
    double tmp;
    if (kf != 0)
        { fprintf(stderr, "sm5rv: illegal call sequence\n"); return(-1); }
    uu[1] = ur; vv[1] = vr;
    for (j=0; j<2; j++) {
        xx[j+1] = xx[j]*xr; yy[j+1] = yy[j]*yr; zz[j+1] = zz[j]*zr;
    }
    for (tmp=0.,j=0,i1=0; i1<=1; i1++) {
        for (i2=0; i2<=i1; i2++,j++) {
            tmp += c[j][12]*uu[i1-i2]*vv[i2];
        }
    }
    for (i1=1; i1<=2; i1++) {
        for (i2=0; i2<=i1; i2++) for (i3=0; i3<=i2; i3++,j++) {
            tmp += c[j][12]*xx[i1-i2]*yy[i2-i3]*zz[i3];
        }
    }
    *sr = tmp;
    return(0);
}

void sm5opn_()
{   if (sm5opn() < 0) exit(1);
}
void sm5ex_(float *fu, float *fv, float *fx, float *fy, float *fz, float *fs)
{   if (sm5ex((double)*fu,(double)*fv,
              (double)*fx,(double)*fy,(double)*fz,(double)*fs) < 0) exit(1);
}
void sm5cls_(float *fc, int *mc, float *fe, int*me)
{   int i; double dc[12];
    if (sm5cls(dc) < 0) exit(1);
    for (i=0; i<3; i++) if(i < *mc) *fc++ = dc[i];
    for (i=0; i<9; i++) if(i < *me) *fe++ = dc[i+3];
}
void sm5rv_(float *fur, float *fvr,
            float *fxr, float *fyr, float *fzr, float *fsr)
{   double dsr;
    if (sm5rv((double)*fur,(double)*fvr,
              (double)*fxr,(double)*fyr,(double)*fzr,&dsr) < 0) exit(1);
    *fsr = dsr;
}