#include <math.h>
#include <stdio.h>
#ifndef NOUNISTD_H
#include <unistd.h>
#endif
#include "dalloca.h"
#include "head.h"

#define ERRR (-1)
#ifndef NULL
#define NULL 0
#endif

static int ndatat=0;    /* defining declaration */
static double *xt=0, *yt=0, aa=0.0, abdevt=0.0;    /* defining declaration */
static double rofunc(double b);
static void sort(int n, double *ra);

void Ft_medfit(double *x, double *y, int ndata, double *a, double *b, double *abdev)
{
    int j;
    double bb,b1,b2,del,f,f1,f2,sigb,temp;
    double sx=0.0,sy=0.0,sxy=0.0,sxx=0.0,chisq=0.0;

    ndatat=ndata;
    xt=x;
    yt=y;
    for (j=1;j<=ndata;j++) {
        sx += x[j];
        sy += y[j];
        sxy += x[j]*y[j];
        sxx += x[j]*x[j];
    }
    del=ndata*sxx-sx*sx;
    aa=(sxx*sy-sx*sxy)/del;
    bb=(ndata*sxy-sx*sy)/del;
    for (j=1;j<=ndata;j++)
        chisq += (temp=y[j]-(aa+bb*x[j]),temp*temp);
    sigb=sqrt(chisq/del);
    b1=bb;
    f1=rofunc(b1);
    b2=bb+((f1 > 0.0) ? fabs(3.0*sigb) : -fabs(3.0*sigb));
    f2=rofunc(b2);
    while (f1*f2 > 0.0) {
        bb=2.0*b2-b1;
        b1=b2;
        f1=f2;
        b2=bb;
        f2=rofunc(b2);
    }
    sigb=0.01*sigb;
    while (fabs(b2-b1) > sigb) {
        bb=0.5*(b1+b2);
        if (bb == b1 || bb == b2) break;
        f=rofunc(bb);
        if (f*f1 >= 0.0) {
            f1=f;
            b1=bb;
        } else {
            f2=f;
            b2=bb;
        }
    }
    *a=aa;
    *b=bb;
    *abdev=abdevt/ndata;
}

static double rofunc(double b)
{
    int j,n1,nmh,nml;
    double *arr,d,sum=0.0;

    ADVECTOR(arr, 1, ndatat, "rofunc", Ft_catcher);

    n1=ndatat+1;
    nml=n1/2;
    nmh=n1-nml;
    for (j=1;j<=ndatat;j++) arr[j]=yt[j]-b*xt[j];
    sort(ndatat,arr);
    aa=0.5*(arr[nml]+arr[nmh]);
    abdevt=0.0;
    for (j=1;j<=ndatat;j++) {
        d=yt[j]-(b*xt[j]+aa);
        abdevt += fabs(d);
        sum += d > 0.0 ? xt[j] : -xt[j];
    }
    return(sum);
}

static void sort(int n, double *ra)
{
    int l,j,ir,i;
    double rra;

    l=(n >> 1)+1;
    ir=n;
    for (;;) {
        if (l > 1)
            rra=ra[--l];
        else {
            rra=ra[ir];
            ra[ir]=ra[1];
            if (--ir == 1) {
                ra[1]=rra;
                return;
            }
        }
        i=l;
        j=l << 1;
        while (j <= ir) {
            if (j < ir && ra[j] < ra[j+1]) ++j;
            if (rra < ra[j]) {
                ra[i]=ra[j];
                j += (i=j);
            }
            else j=ir+1;
        }
        ra[i]=rra;
    }
}