#include <math.h>
#include <stdio.h>
#ifdef HAVE_UNISTD_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;
	}
}
