/*===========================================================================
  Copyright (C) 1991-2009 European Southern Observatory (ESO)
 
  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., 675 Massachusetts Ave, Cambridge, 
  MA 02139, USA.
 
  Correspondence concerning ESO-MIDAS should be addressed as follows:
	Internet e-mail: midas@eso.org
	Postal address: European Southern Observatory
			Data Management Division 
			Karl-Schwarzschild-Strasse 2
			D 85748 Garching bei Muenchen 
			GERMANY
===========================================================================*/


/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
.IDENT        mutil.c
.MODULE       subroutines 
.ENVIRONMENT  UNIX
.LANGUAGE     C
.AUTHOR       Cristian Levin - ESO La Silla
.PURPOSE      Miscelanous mathematical algorithms:
              - linear square fitting.
              - fast polynomial evaluation.
              - search of the median.
              - polynomial interpolation.
.KEYWORDS     interpolation, fitting, polynomial evaluation.
.VERSION 1.0  1-Apr-1991   Implementation

 090723		last modif
------------------------------------------------------------*/

#include "cpl_matrix.h"
#include "mutil.h"
#include "nrutil.h"
#include <stdio.h>
#include <math.h>

/******************************************************
 *
 * lfit(): general linear least squares solution.
 *
 ******************************************************/
void lfit( x, y, ndata, a, mfit, funcs )
int ndata, mfit;	/* IN: no. of values, no. of coefficients */
double x[], y[], a[];	/* IN: (Xi,Yi)   OUT: (Ai) */
void (*funcs)();
{
    lsqfit_nr(x, y, NULL, ndata, a, mfit, funcs);
}

void fit_poly (float inimage[], float outimage[], double start,
	  double step, int npix, int fit_deg)
{
  float xout;
  double *af;
  double *ad, *xin, *yin;
  int i;

  xin = dvector (0, npix - 1);
  yin = dvector (0, npix - 1);

  for (i = 0; i < npix; i++)
    {
      xin[i] = start + i * step;
      yin[i] = (double) inimage[i];
    }

  ad = dvector (1, fit_deg);
  af = dvector (1, fit_deg);

  lfit (xin, yin, npix, ad, fit_deg, dpoly);
  for (i = 1; i <= fit_deg; i++)
    {
      af[i] = ad[i];
    }

  for (i = 0; i < npix; i++)
    {
      xout = start + step * i;
      outimage[i] = (float) eval_dpoly (xout, ad, fit_deg);
    }

  free_dvector(xin, 0, npix - 1), free_dvector(yin, 0, npix - 1);
  free_dvector(ad, 1, fit_deg),  free_dvector(af, 1, fit_deg);

}



void dpoly(double x, double p[], int np)
{
  int j;

  p[1] = 1.0;
  for (j = 2; j <= np; j++)
    p[j] = p[j - 1] * x;
}

void dleg(double x, double pl[], int nl)
{
  int j;
  double twox, f2, f1, d;

  pl[1] = 1.0;
  pl[2] = x;
  if (nl > 2)
    {
      twox = 2.0 * x;
      f2 = x;
      d = 1.0;
      for (j = 3; j <= nl; j++)
	{
	  f1 = d;
	  d += 1.0;
	  f2 += twox;
	  pl[j] = (f2 * pl[j - 1] - f1 * pl[j - 2]) / d;
	}
    }
}

void dcheb(double x, double pl[], int nl)
{
  int j;
  double twox;

  pl[1] = 1.0;
  pl[2] = x;
  if (nl > 2)
    {
      twox = 2.0 * x;
      for (j = 3; j <= nl; j++)
	  pl[j] = twox * pl[j - 1] -  pl[j - 2];
    }
}

/************************************************************
 *
 * eval_fpoly(), eval_dpoly(): fast polynomial evaluation.
 *
 ************************************************************/

float eval_fpoly(x, a, n)
float x;
float *a;	/* coefficients (range: 1..n) */
int n;
{
    int i;
    float y = 0.0;

    for ( i = n; i >= 1; i-- )
	y = x*y + a[i];

    return(y);
}

double eval_dpoly(x, a, n)
double x;
double *a;	/* coefficients (range: 1..n) */
int n;
{
    int i;
    double y = 0.0;

    for ( i = n; i >= 1; i-- )
	y = x*y + a[i];

    return(y);
}

#ifdef __STDC__
double select_pos (unsigned long k, unsigned long n, double arr[])
#else
double select_pos (k, n, arr)
     unsigned long k, n;
     double arr[];
#endif

/*      select the kth smallest value in arr[1...n], NR, pg. 342 */

#define SWAPD(a, b) 	{double temp = (a); (a) = (b); (b) = temp; }

{

  unsigned long i, ir, j, l, mid;
/*  double a, temp; */
  double a; 

  l = 1;
  ir = n;
  for (;;)
    {
      if (ir <= l + 1)
	{
	  if (ir == l + 1 && arr[ir] < arr[l])
	    {
	      SWAPD (arr[l], arr[ir])
	    }
	  return arr[k];
	}
      else
	{
	  mid = (l + ir) >> 1;
	  SWAPD (arr[mid], arr[l + 1])
	    if (arr[l + 1] > arr[ir])
	    {
	      SWAPD (arr[l + 1], arr[ir])
	    }
	  if (arr[l] > arr[ir])
	    {
	      SWAPD (arr[l], arr[ir])
	    }
	  if (arr[l + 1] > arr[l])
	    {
	      SWAPD (arr[l + 1], arr[l])
	    }
	  i = l + 1;
	  j = ir;
	  a = arr[l];
	  for (;;)
	    {
	      do
		i++;
	      while (arr[i] < a);
	      do
		j--;
	      while (arr[j] > a);
	      if (j < i)
		break;
	      SWAPD (arr[i], arr[j])
	    }
	  arr[l] = arr[j];
	  arr[j] = a;
	  if (j >= k)
	    ir = j - 1;
	  if (j <= k)
	    l = i;
	}
    }
}
#undef SWAPD
#undef SWAP

#ifdef __STDC__
void heap_copy(int n, float arr[], float out[])
#else
void heap_copy (n, arr, out)
  float arr[];
  float out[];
  int   n;
#endif
/*              copy arrays         */
{
  int    j; 
  float  diff; 

  for (j=0; j<n; j++)  out[j] = arr[j];

}


#ifdef __STDC__
int heap_compare(int n, float arr1[], float arr2[])
#else
int heap_compare (n, arr1, arr2)
  float arr1[];
  float arr2[]; 
  int   n;
#endif
/*              sort distribution (heapsort)           */
{
  int j;
  int check; 

  printf("Comparing arrays of size %d\n",n); 

  check = 0;

 
  for (j = 0; j < 4; j++) 
       printf("HEAPSORT: Array elements [%d] = %f %f\n",j,arr1[j],arr2[j]); 
  for (j = n-4; j < n; j++) 
       printf("HEAPSORT: Array elements [%d] = %f %f\n",j,arr1[j],arr2[j]); 
 

  for (j = 0; j < n; j++)
    {
      // printf("Index : %d\n",j);      
      if (arr1[j] != arr2[j]) { 
          check=1;
          printf("HEAPSORT: Array difference at index %d (%f, %f)\n",j,arr1[j],arr2[j]); 
      }
    }
  printf ("Comparison flag = %d\n",check); 

  return(check); 
}

#ifdef __STDC__
void heap_sort (int n, float arr[])
#else
void heap_sort (n, arr)
  float arr[];
  int   n;
#endif
/*              sort distribution (heapsort)           */
{
  int i, j, index;
  float a, *b;

  for (j = 1; j < n; j++)
    {
      a = arr[j];
      i = j - 1;
      while (i >= 0 && arr[i] > a)
	{
	  arr[i + 1] = arr[i];
	  i--;
	}
      arr[i + 1] = a;
    }

  return;
}

#ifdef __STDC__
float heap_median (int n, float arr[])
#else
float heap_median (n, arr)
  float arr[];
  int   n;
#endif
/*              find median of distribution (heapsort)           */
{
  int i, j, index;
  float a, *b, hmed;
  int   n2, n2p; 

  b = (float*) malloc ((size_t)((n)*sizeof (float)));

  heap_copy(n, arr, b); 
  // printf("heap_sort: Comparing copies\n"); 
  // heap_compare (n, arr, b); 
  heap_sort(n,b); 

  index = (n - 1) / 2;
  hmed = b[index]; 

  // n2p=(n2=n/2)+1;
  // median = (n % 2 ? b[n2p] : 0.5*(b[n2]+b[n2p])); // b[(n-1)/2];

  free(b); 
  // printf("heap_sort: Median value %f\n",hmed); 
  return(hmed);
}


/************************************************************
 *
 * median(): efficient search of the median.
 *
 ************************************************************/

float median(x, n)
float x[];
int n;
{
    int n2 ,n2p;
    void piksrt();
    
    piksrt(n, x);
    n2p = (n2 = n/2) + 1;
    return((n % 2) ? x[n2p] : 0.5 * (x[n2] + x[n2p]));
}

/************************************************************
 *
 * piksrt(): sorting by straight insertion.
 *
 ************************************************************/

void piksrt(n,arr)
int n;
float arr[];
{
	int i,j;
	float a;

	for (j=2;j<=n;j++) {
		a=arr[j];
		i=j-1;
		while (i > 0 && arr[i] > a) {
			arr[i+1]=arr[i];
			i--;
		}
		arr[i+1]=a;
	}
}

#ifdef __STDC__
  float pik_median (int n, float arr[])
#else
  float pik_median ( n, arr )
  float arr[];
  int   n;
#endif
/*              find median of distribution (use piksort)  
		NR page 330  modified for mos by Sabine ??  
                input array arr[] is conserved by buffer b[] -- here we start
                with arr[0,..,n-1] both in contrary to select_pos     TS-0896  
*/



{
  int i, j, index;
  float a, b[101];


  for (j = 0; j < n; j++)
    b[j] = arr[j];

  for (j = 1; j < n; j++)
    {
      a = b[j];
      i = j - 1;
      while (i >= 0 && b[i] > a)
	{
	  b[i + 1] = b[i];
	  i--;
	}
      b[i + 1] = a;
    }
  index = (n - 1) / 2;
  return(b[index]);
}

