/***************************************************************************/
/* Error concealment                                                       */
/* (c) 2000 IVB                                                            */
/***************************************************************************/

#include <math.h>
#include "global.h"

#define SWAP(a,b) temp=(a);(a)=(b);(b)=temp;
#define SQR(x) x*x;

/* Definitions for golden section search 
  The first parameter is the default ratio by which successive intervals
  are magnified; the second is the maximum magnification allowed for
  a parabolic-fit step.     
*/
#define  GOLD    1.618034
#define  GLIMIT  100
#define  TINY    1e-20
#define  SHFT(a,b,c,d) (a)=(b); (b)=(c); (c)=(d);
#define  SHFT2(a,b,c) (a)=(b); (b)=(c);
#define  SHFT3(a,b,c,d) (a)=(b); (b)=(c); (c)=(d);

float cost_fun_map_est0(float x, float *nbhd, int *available, double *b, float s, float g);
float cost_fun_map_est1(float x, float *nbhd, int *available, double *b, float s, float g);
void mnbrak(float *ax, float *bx, float *cx, float *fa, float *fb, float *fc, float *nbhd, int *available, double *b, float s, float g, float (*func)(float, float*, int*, double*, float, float));
float golden(float ax, float bx, float cx, float tol, float *xmin, float *nbhd, int *available, double *b, float s, float g, float (*f)(float, float*, int*, double*, float, float));


/***************************************************************************/
/*                             select()                                    */
/***************************************************************************/
/* from Numerical Recipes in C, pp.342                                     */

Real select(int k, int n, Real *arr)
{
  int i, ir, j, l, mid;
  Real a, temp;
  
  /* l = 1; ir = n; */     /* for arr[1..n], as in Num. Recipes */
  
  l = 0; ir = n-1;         /* for arr[0..n-1] */
  for (;;)
  {
    if (ir <= l+1)
    {
      if (ir == l+1 && arr[ir] < arr[l])
      {
        SWAP(arr[l],arr[ir])
      }
      return arr[k];
    }
    else
    {
      mid = (l+ir) >> 1;
      SWAP(arr[mid],arr[l+1])
      if (arr[l] > arr[ir])
      {
        SWAP(arr[l],arr[ir])
      }
      if (arr[l+1] > arr[ir])
      {
        SWAP(arr[l+1],arr[ir])
      }
      if (arr[l] > arr[l+1])
      {
        SWAP(arr[l],arr[l+1])
      } 
      i = l+1;
      j = ir;
      a = arr[l+1];
      for(;;)
      {
        do i++; while (arr[i] < a);
        do j--; while (arr[j] > a);
        if (j < i) break;
        SWAP(arr[i],arr[j])
      }
      arr[l+1] = arr[j];
      arr[j] = a;
      if (j >= k) ir = j-1;
      if (j <= k) l = i;
    }
  }
}
  
/***************************************************************************/
/*                             scalar_median()                             */
/***************************************************************************/
Real scalar_median(int N, Real *arr)
{
  Real a, b, med;
  
  /* if N is odd, median is the element (N+1)/2, if elements numbered 1..N */
  if (N%2)
    med = select((int)(N+1)/2-1, N, arr);
  else
  {
    a = select((int)N/2-1, N, arr);
    b = select((int)N/2, N, arr);
    med = (a+b)/2.0;
  } 
  return med;
}


/***************************************************************************/
/*                             weighted_mean()                             */
/***************************************************************************/
float weighted_mean(int N, float *arr, double *weight)
{
  int i;
  float sum, sumw;
  
  sum = 0.0; sumw = 0.0;
  
  for (i = 0; i < N; i++)
  {
    sum += arr[i]*weight[i];
    sumw += weight[i];
  }
  
  return (float) (sum/sumw);
}
  
/***************************************************************************/
/*                             weighted_median()                           */
/***************************************************************************/
float weighted_median(int N, float *arr, float *weight)
{
  int i, j, minj;
  float wsum, minwsum;
  
  minj = 0; 
  minwsum = 1.0e9;
  
  for (j = 0; j < N; j++)
  {
    wsum = 0.0;
    for (i = 0; i < N; i++)
    {
      wsum += weight[i]*fabs(arr[j]-arr[i]);
    }
    
    if (wsum < minwsum)
    {
      minwsum = wsum;
      minj = j;
    }
  }
  
  return arr[minj];
}

/***************************************************************************/
/*                             huber()                                     */
/***************************************************************************/
float huber(float x, float g)
{
  /* Huber function. Parameter g must be nonnegative */
  
  return (float) (fabs(x) < g ? x*x : g*g + 2*g*(fabs(x) - g)); 
}

/***************************************************************************/
/*                             map_est0()                                  */
/***************************************************************************/
float map_est0(float *nbhd, int *available, double *b)
{
  float s, g;
  float xmin, x, fmin, ax, bx, cx, fa, fb, fc;
  float tol;
  
  s = 1.0;
  g = 100.0;
  x = xmin = 0.0;
  tol = 1e-6;
  
  /* bracket the minimum */
  mnbrak(&ax, &bx, &cx, &fa, &fb, &fc, nbhd, available, b, s, g, 
         cost_fun_map_est1);
  
  /* find the minimum */
  fmin = golden(ax, bx, cx, tol, &xmin, nbhd, available, b, s, g, 
         cost_fun_map_est1);
  
  return xmin;
}


/***************************************************************************/
/*                             map_est1()                                  */
/***************************************************************************/
float map_est1(float *nbhd, int *available, double *b, float *gam)
{
  float s, g;
  float xmin, fmin, ax, bx, cx, fa, fb, fc;
  float tol;
  int i;
  
  s = 1.0;
  g = (*gam);
  xmin = 0.0;
  tol = 1e-6;
  
  /* IVB 5/31/02: initial bracket for the minimum */
  ax = -1000.0; bx = 0; cx = 1000.0;
  
  /*
  printf("map_est1(): g = %6.2f\n", g);
  
  for (i = 0; i < 4; i++)
    printf("b[%d] = %6.2f  ",i,b[i]);
  printf("\n");
  
  for (i = 0; i < 12; i++)
    if (available[i])
      printf("nbhd[%d] = %6.2f  ",i,nbhd[i]);
  printf("\n");
  */
  
  /* bracket the minimum */
  /* printf("bracketing minimum... "); */
  mnbrak(&ax, &bx, &cx, &fa, &fb, &fc, nbhd, available, b, s, g, 
         cost_fun_map_est1);
  
  /*
  printf("ax = %6.2f, bx = %6.2f, cx = %6.2f\n", ax, bx, cx);
  */
  
  /* find the minimum */
  /* printf("finding minimum... "); */
  xmin = (ax+cx)/2.0;
  fmin = golden(ax, bx, cx, tol, &xmin, nbhd, available, b, s, g, 
         cost_fun_map_est1);
  
  /* printf("done\n");        */
  
  return (float) xmin;
}

/***************************************************************************/
/*                        cost_fun_map_est1()                              */
/***************************************************************************/
/* evaluation of the cost function for map_est1 at x */
/* 5/29/02 new version, b's match up with directional derivatives */

float cost_fun_map_est1(float x, float *nbhd, int *available, double *b, float s, float g)
{
  double f, sum0, sum1, sum2, sum3, sum4;
  int i;
  
  f = sum0 = sum1 = sum2 = sum3 = sum4 = 0.0;
    
  if (available[0])
  {
    sum0 += b[3]*huber(((nbhd[0] - x)/sqrt(2.0))/s, g);
  }
  if (available[1])
  { 
    sum0 += b[2]*huber((nbhd[1] - x)/s, g); 
    
    if (available[6])
    {
      /*
      sum1 += b[3]*huber(((nbhd[1] - nbhd[6])/sqrt(2.0))/s, g);
      */
    }
  }
  if (available[2])
  { 
    sum0 += b[1]*huber(((nbhd[2] - x)/sqrt(2.0))/s, g);
    
    if (available[6])
    {
      /*
      sum1 += b[2]*huber((nbhd[2] - nbhd[6])/s, g);
      */
    }
  }
  if (available[3])
  {
    if (available[6])
    {
      /*
      sum1 += b[1]*huber(((nbhd[3] - nbhd[6])/sqrt(2.0))/s, g);
      */
    }
  }
  if (available[4])
  {
    if (available[9])
    {
      /*
      sum4 += b[3]*huber(((nbhd[4] - nbhd[9])/sqrt(2.0))/s, g);
      */
    }
  }
  if (available[5])
  {
    
    sum0 += b[0]*huber((nbhd[5] - x)/s, g);
        
    if (available[9])
    {
      /*
      sum4 += b[2]*huber((nbhd[5] - nbhd[9])/s, g); 
      */
    }
    if (available[10])
    {
      /*
      sum2 += b[3]*huber(((nbhd[5] - nbhd[10])/sqrt(2.0))/s, g);
      */
    }
  }
  if (available[6])
  {
    sum1 += b[0]*huber((x - nbhd[6])/s, g);
    
    if (available[10])
    {
      /*
      sum2 += b[1]*huber(((nbhd[6] - nbhd[10])/sqrt(2.0))/s, g);
      */
    }
    if (available[11])
    {
      /*
      sum3 += b[2]*huber((nbhd[6] - nbhd[11])/s, g);
      */
    }
  }
  if (available[7])
  {
    if (available[11])
    {
      /*
      sum3 += b[1]*huber(((nbhd[7] - nbhd[11])/sqrt(2.0))/s, g);
      */
    }
  }
  if (available[8])
  {
    if (available[9])
    {
      /*
      sum4 += b[0]*huber((nbhd[8] - nbhd[9])/s, g);
      */
    }
  }
  if (available[9])
  {
    sum4 += b[1]*huber(((x - nbhd[9])/sqrt(2.0))/s, g);
    
    if (available[10])
    {
      /*
      sum2 += b[0]*huber((nbhd[9] - nbhd[10])/s, g);
      */
    }
  }
  if (available[10])
  {
    sum2 += b[2]*huber((x - nbhd[10])/s, g);
    
    if (available[11])
    {
      /*
      sum3 += b[0]*huber((nbhd[10] - nbhd[11])/s, g);
      */
    }
  }
  if (available[11])
  {
    sum3 += b[3]*huber(((x - nbhd[11])/sqrt(2.0))/s, g);
  }
  
  f = sum0 + sum1 + sum2 + sum3 + sum4;
  
  return (float) f;
}

/***************************************************************************/
/*                        cost_fun_map_est0()                              */
/***************************************************************************/
/* evaluation of the cost function for map_est1 at x */
/* 5/29/02 - older, non-adaptive version, b's match up with sums */
/* with sum4 and sum3 <-> b[3] */

float cost_fun_map_est0(float x, float *nbhd, int *available, double *b, float s, float g)
{
  float f, sum0, sum1, sum2, sum3, sum4;
  int i;
  
  f = sum0 = sum1 = sum2 = sum3 = sum4 = 0.0;
    
  if (available[0])
  {
    sum0 += b[0]*huber(((nbhd[0] - x)/sqrt(2.0))/s, g);
  }
  if (available[1])
  { 
    sum0 += b[0]*huber((nbhd[1] - x)/s, g);
    if (available[6])
    {
      sum1 += b[1]*huber(((nbhd[1] - nbhd[6])/sqrt(2.0))/s, g);
    }
  }
  if (available[2])
  { 
    sum0 += b[0]*huber(((nbhd[2] - x)/sqrt(2.0))/s, g);
    if (available[6])
    {
      sum1 += b[1]*huber((nbhd[2] - nbhd[6])/s, g);
    }
  }
  if (available[3])
  {
    if (available[6])
    {
      sum1 += b[1]*huber(((nbhd[3] - nbhd[6])/sqrt(2.0))/s, g);
    }
  }
  if (available[4])
  {
    if (available[9])
    {
      sum4 += b[3]*huber(((nbhd[4] - nbhd[9])/sqrt(2.0))/s, g);
    }
  }
  if (available[5])
  {
    
    sum0 += b[0]*huber((nbhd[5] - x)/s, g);
    
    if (available[9])
    {
      sum4 += b[3]*huber((nbhd[5] - nbhd[9])/s, g); 
    }
    if (available[10])
    {
      sum2 += b[2]*huber(((nbhd[5] - nbhd[10])/sqrt(2.0))/s, g);
    }
  }
  if (available[6])
  {
    sum1 += b[1]*huber((x - nbhd[6])/s, g);
    if (available[10])
    {
      sum2 += b[2]*huber(((nbhd[6] - nbhd[10])/sqrt(2.0))/s, g);
    }
    if (available[11])
    {
      sum3 += b[3]*huber((nbhd[6] - nbhd[11])/s, g);
    }
  }
  if (available[7])
  {
    if (available[11])
    {
      sum3 += b[3]*huber(((nbhd[7] - nbhd[11])/sqrt(2.0))/s, g);
    }
  }
  if (available[8])
  {
    if (available[9])
    {
      sum4 += b[3]*huber((nbhd[8] - nbhd[9])/s, g);
    }
  }
  if (available[9])
  {
    sum4 += b[3]*huber(((x - nbhd[9])/sqrt(2.0))/s, g);
    
    if (available[10])
    {
      sum2 += b[2]*huber((nbhd[9] - nbhd[10])/s, g);
    }
  }
  if (available[10])
  {
    sum2 += b[2]*huber((x - nbhd[10])/s, g);
    
    if (available[11])
    {
      sum3 += b[3]*huber((nbhd[10] - nbhd[11])/s, g);
    }
  }
  if (available[11])
  {
    sum3 += b[3]*huber(((x - nbhd[11])/sqrt(2.0))/s, g);
  }
  
  f = sum0 + sum1 + sum2 + sum3 + sum4;
  
  return (float) f;
}



/***************************************************************************/
/*                           Max() and Sign()                              */
/***************************************************************************/
/* Utility functions for mnbrak() */

float Max(float a, float b) 
{
  if (a>=b) return a; 
  else return b;
}

float Sign(float a, float b) 
{
  if (b>=0) return fabs(a);
  else return -fabs(a);
}

/***************************************************************************/
/*                             mnbrak()                                    */
/***************************************************************************/
/* Initial bracketing of a minimum, adapted from "Numerical Recipes in C" */

void mnbrak(float *ax, float *bx, float *cx, float *fa, float *fb, float *fc, float *nbhd, int *available, double *b, float s, float g, float (*func) (float, float*, int*, double*, float, float))
{
/* 
  Given a function FUNC(X), and given distinct initial points ax and
  bx, this routine searches in the downhill direction (defined by the
  function as evaluated at the initial points) and returns new points
  ax, bx, cx which bracket a minimum of the function. Also returned
  are the function values at the three points, fa, fb and fc. 
*/
  float dum,fu,q,r,u,ulim;
  /*
  *fa=FUNC(*ax);
  *fb=FUNC(*bx);
  */
  *fa = (*func)(*ax, nbhd, available, b, s, g);
  *fb = (*func)(*bx, nbhd, available, b, s, g);
  if (*fb > *fa) 
  {
    SHFT(dum, *ax, *bx, dum)
    SHFT(dum, *fb, *fa, dum)
  }
  *cx=*bx+GOLD*(*bx-*ax);
  /*
  *fc=FUNC(*cx);
  */
  *fc = (*func)(*cx, nbhd, available, b, s, g);
  while (*fb > *fc) 
  {
    r=(*bx-*ax)*(*fb-*fc);
    q=(*bx-*cx)*(*fb-*fa);
    u=*bx-((*bx-*cx)*q-(*bx-*ax)*r)/(2.*Sign(Max(fabs(q-r),TINY),q-r));
    ulim=*bx+GLIMIT*(*cx-*bx);
    if ((*bx-u)*(u-*cx) > 0.0) 
    {
      /*
      fu=FUNC(u);
      */
      fu = (*func)(u, nbhd, available, b, s, g);
      if (fu < *fc) 
      {
        *ax = (*bx);
        *bx=u;
        *fa= (*fb);
        *fb=fu;
        return;
      }
      else if (fu > *fb) 
      {
        *cx=u;
        *fc=fu;
        return;
      }
      u=(*cx)+GOLD*(*cx-*bx);
      /*
      fu=FUNC(u);
      */
      fu = (*func)(u, nbhd, available, b, s, g);
    }
    else if ((*cx-u)*(u-ulim) > 0.0) 
    {
      /*
      fu=FUNC(u);
      */
      fu = (*func)(u, nbhd, available, b, s, g);
      if (fu < *fc) 
      {
        SHFT(*bx,*cx,u,*cx+GOLD*(*cx-*bx))
        SHFT(*fb,*fc,fu,(*func)(u, nbhd, available, b, s, g))
      }
    }
    else if ((u-ulim)*(ulim-*cx)>=0) 
    {
      u=ulim;
      /*
      fu=FUNC(u);
      */
      fu = (*func)(u, nbhd, available, b, s, g);
    }
    else 
    {
      u=(*cx)+GOLD*(*cx-*bx);
      /*
      fu=FUNC(u);
      */
      fu = (*func)(u, nbhd, available, b, s, g);
    }
    SHFT(*ax,*bx,*cx,u)
    SHFT(*fa,*fb,*fc,fu)
  }
}

/***************************************************************************/
/*                             golden()                                    */
/***************************************************************************/
/* Golden section search, adapted from "Numerical Recipes in C" */

float golden(float ax, float bx, float cx, float tol, float *xmin, float *nbhd, int *available, double *b, float s, float g, float (*f) (float, float*, int*, double*, float, float))
{
/*
  Given a function F, and given a bracketing triplet of fabscissas 
  ax, bx, cx (such that bx is between ax and cx, and F(bx) is less 
  than both F(ax) and F(cx)), this routine performs a golden section 
  search for the minimum, isolating it to a fractional precision of 
  about tol. The fabscissa of the minimum is returned as XMIN, and the minimum
  function value is returned as GOLDEN, the eturned function value.
*/
  float f0,f1,f2,f3,x0,x1,x2,x3;
  float R,C;
  
  R=0.61803399; C=1.0-R;    
  
  x0=ax;  
  x3=cx;
  if (fabs(cx-bx) > fabs(bx-ax)) 
  {
    x1=bx; x2=bx+C*(cx-bx);
  }
  else 
  {
    x2=bx; x1=bx-C*(bx-ax);
  }
  
  /*
  f1=F(x1); 
  f2=F(x2);   
  */
  f1 = (*f)(x1, nbhd, available, b, s, g);
  f2 = (*f)(x2, nbhd, available, b, s, g);
  
  while (fabs(x3-x0) > tol*(fabs(x1)+fabs(x2))) 
  {
    if (f2 < f1) 
    {
      SHFT3(x0, x1, x2, R*x1+C*x3)
      SHFT2(f1, f2, (*f)(x2, nbhd, available, b, s, g))
    }
    else 
    {
      SHFT3(x3, x2, x1, R*x2+C*x0)
      SHFT2(f2, f1, (*f)(x1, nbhd, available, b, s, g))
    }
  }

  if (f1 < f2) 
  {
    *xmin=x1;
    return f1;
  }
  else 
  {
    *xmin=x2;
    return f2;
  }
}

