/***********************************************************************
 *                   GNU Lesser General Public License
 *
 * This file is part of the GFDL Flexible Modeling System (FMS).
 *
 * FMS is free software: you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or (at
 * your option) any later version.
 *
 * FMS 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 Lesser General Public
 * License along with FMS.  If not, see <http://www.gnu.org/licenses/>.
 **********************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "mosaic_util.h"
#include "interp.h"
#include "create_xgrid.h"

/*********************************************************************
   void cublic_spline_sp(size1, size2, grid1, grid2, data1, data2)

   Calculate a shape preserving cubic spline. Monotonicity is ensured over each subinterval
   unlike classic cubic spline interpolation.
   It will be used to interpolation data in 1-D space.

   INPUT Arguments:
    grid1:    grid for input data grid.
    grid2:    grid for output data grid.
    size1:    size of input grid.
    size2:    size of output grid.
    data1:    input data associated with grid1.

   OUTPUT ARGUMENTS:
    data2:    output data associated with grid2. (OUTPUT)

*********************************************************************/

void cubic_spline_sp(int size1, int size2, const double *grid1, const double *grid2, const double *data1,
                     double *data2 )
{
  double *delta=NULL, *d=NULL, *dh=NULL, *b=NULL, *c = NULL;
  double s, w1, w2, p;
  int i, k, n, klo, khi, kmax;

  for(i=1; i<size1; i++) {
    if( grid1[i] <= grid1[i-1] ) error_handler("cubic_spline_sp: grid1 is not monotonic increasing");
  }

  for(i=0; i<size2; i++) {
    if( grid2[i] < grid1[0] || grid2[i] > grid1[size1-1]) error_handler("cubic_spline_sp: grid2 lies outside grid1");
  }

  if(size1 < 2) error_handler("cubic_spline_sp: the size of input grid should be at least 2");
  if(size1 == 2) {  /* when size1 is 2, it just reduced to a linear interpolation */
    p = (data1[1]-data1[0])/(grid1[1]-grid1[0]);
    for(i=0; i< size2; i++) data2[i] = p*(grid2[i] - grid1[0]) + data1[0];
    return;
  }
  delta = (double *)malloc((size1-1)*sizeof(double));
  dh = (double *)malloc((size1-1)*sizeof(double));
  d = (double *)malloc(size1*sizeof(double));
  for(k=0;k<size1-1;k++) {
    dh[k] = grid1[k+1]-grid1[k];
    delta[k] = (data1[k+1]-data1[k])/(dh[k]);
  }
  /*
    Interior slopes
  */
  for(k=1;k<size1-1;k++) {
    if( delta[k]*delta[k-1] > 0.0 ) {
      w1 = 2.0*dh[k] + dh[k-1];
      w2 = dh[k] + 2.0*dh[k-1];
      d[k] = (w1+w2)/(w1/delta[k-1]+w2/delta[k]);
    }
    else {
      d[k] = 0.0;
    }
  }
  /*
    End slopes
  */
  kmax = size1-1;
  d[0] = ((2.0*dh[0] + dh[1])*delta[0] - dh[0]*delta[1])/(dh[0]+dh[1]);

  if ( d[0]*delta[0] < 0.0 ) {
    d[0] = 0.0;
  }
  else {
    if ( delta[0]*delta[1] < 0.0 && fabs(d[0]) > fabs(3.0*delta[0])) {
      d[0]=3.0*delta[0];
    }
  }

  d[kmax] = ((2.0*dh[kmax-1] + dh[kmax-2])*delta[kmax-1] - dh[kmax-1]*delta[kmax-2])/(dh[kmax-1]+dh[kmax-2]);
  if ( d[kmax]*delta[kmax-1] < 0.0 ) {
    d[kmax] = 0.0;
  }
  else {
    if ( delta[kmax-1]*delta[kmax-2] < 0.0 && fabs(d[kmax]) > fabs(3.0*delta[kmax-1])) {
      d[kmax]=3.0*delta[kmax-1];
    }
  }

  /* Precalculate coefficients */
  b = (double *)malloc((size1-1)*sizeof(double));
  c = (double *)malloc((size1-1)*sizeof(double));
  for (k=0; k<size1-1; k++) {
    c[k]   = (3.0*delta[k]-2.0*d[k]-d[k+1])/dh[k];
    b[k]   = (d[k]-2.0*delta[k]+d[k+1])/(dh[k]*dh[k]);
  }
  /* interpolate data onto grid2 */
  for(k=0; k<size2; k++) {
    n = nearest_index(grid2[k],grid1, size1);
    if (grid1[n] < grid2[k]) {
      klo = n;
    }
    else {
      if(n==0) {
        klo = n;
      }
      else {
        klo = n -1;
      }
    }
    khi = klo+1;
    s   = grid2[k] - grid1[klo];
    data2[k] = data1[klo]+s*(d[klo]+s*(c[klo]+s*b[klo]));
  }

  free(c);
  free(b);
  free(d);
  free(dh);
  free(delta);

}/* cubic spline sp */


/*********************************************************************
   void cublic_spline(size1, size2, grid1, grid2, data1, data2, yp1, ypn  )

   This alborithm is to get an interpolation formula that is smooth in the first
   derivative, and continuous in the second derivative, both within an interval
   and its boundaries. It will be used to interpolation data in 1-D space.

   INPUT Arguments:
    grid1:    grid for input data grid.
    grid2:    grid for output data grid.
    size1:    size of input grid.
    size2:    size of output grid.
    data1:    input data associated with grid1.
    yp1:      first derivative of starting point.
              Set to 0 to be "natural"           (INPUT)
    ypn:      first derivative of ending point.
              Set to 0 to be "natural"           (INPUT)

   OUTPUT ARGUMENTS:
    data2:    output data associated with grid2. (OUTPUT)

*********************************************************************/


void cubic_spline(int size1, int size2, const double *grid1, const double *grid2, const double *data1,
                  double *data2, double yp1, double ypn  )
{
  double *y2=NULL, *u=NULL;
  double p, sig, qn, un, h, a, b;
  int i, k, n, klo, khi;

  for(i=1; i<size1; i++) {
    if( grid1[i] <= grid1[i-1] ) error_handler("cubic_spline: grid1 is not monotonic increasing");
  }

  for(i=0; i<size2; i++) {
    if( grid2[i] < grid1[0] || grid2[i] > grid1[size1-1]) error_handler("cubic_spline: grid2 lies outside grid1");
  }

  if(size1 < 2) error_handler("cubic_spline: the size of input grid should be at least 2");
  if(size1 == 2) {  /* when size1 is 2, it just reduced to a linear interpolation */
    p = (data1[1]-data1[0])/(grid1[1]-grid1[0]);
    for(i=0; i< size2; i++) data2[i] = p*(grid2[i] - grid1[0]) + data1[0];
    return;
  }
  y2 = (double *)malloc(size1*sizeof(double));
  u = (double *)malloc(size1*sizeof(double));
  if (yp1 >.99e30) {
    y2[0]=0.;
    u[0]=0.;
  }
  else {
    y2[0]=-0.5;
    u[0]=(3./(grid1[1]-grid1[0]))*((data1[1]-data1[0])/(grid1[1]-grid1[0])-yp1);
  }

  for(i=1; i<size1-1; i++) {
    sig=(grid1[i]-grid1[i-1])/(grid1[i+1]-grid1[i-1]);
    p=sig*y2[i-1]+2.;
    y2[i]=(sig-1.)/p;
    u[i]=(6.*((data1[i+1]-data1[i])/(grid1[i+1]-grid1[i])-(data1[i]-data1[i-1])
              /(grid1[i]-grid1[i-1]))/(grid1[i+1]-grid1[i-1])-sig*u[i-1])/p;

  }

  if (ypn > .99e30) {
    qn=0.;
    un=0.;
  }
  else {
    qn=0.5;
    un=(3./(grid1[size1-1]-grid1[size1-2]))*(ypn-(data1[size1-1]-data1[size1-2])/(grid1[size1-1]-grid1[size1-2]));
  }

  y2[size1-1]=(un-qn*u[size1-2])/(qn*y2[size1-2]+1.);

  for(k=size1-2; k>=0; k--) y2[k] = y2[k]*y2[k+1]+u[k];

  /* interpolate data onto grid2 */
  for(k=0; k<size2; k++) {
    n = nearest_index(grid2[k],grid1, size1);
    if (grid1[n] < grid2[k]) {
      klo = n;
    }
    else {
      if(n==0) {
        klo = n;
      }
      else {
        klo = n -1;
      }
    }
    khi = klo+1;
    h   = grid1[khi]-grid1[klo];
    a   = (grid1[khi] - grid2[k])/h;
    b   = (grid2[k] - grid1[klo])/h;
    data2[k] = a*data1[klo] + b*data1[khi]+ ((pow(a,3.0)-a)*y2[klo] + (pow(b,3.0)-b)*y2[khi])*(pow(h,2.0))/6;
  }

  free(y2);
  free(u);

}/* cubic spline */


/*------------------------------------------------------------------------------
  void conserve_interp()
  conservative interpolation through exchange grid.
  Currently only first order interpolation are implemented here.
  ----------------------------------------------------------------------------*/
void conserve_interp(int nx_src, int ny_src, int nx_dst, int ny_dst, const double *x_src,
                     const double *y_src, const double *x_dst, const double *y_dst,
                     const double *mask_src, const double *data_src, double *data_dst )
{
  int n, nxgrid;
  int *xgrid_i1, *xgrid_j1, *xgrid_i2, *xgrid_j2;
  double *xgrid_area, *dst_area, *area_frac;

  /* get the exchange grid between source and destination grid. */
  xgrid_i1   = (int    *)malloc(MAXXGRID*sizeof(int));
  xgrid_j1   = (int    *)malloc(MAXXGRID*sizeof(int));
  xgrid_i2   = (int    *)malloc(MAXXGRID*sizeof(int));
  xgrid_j2   = (int    *)malloc(MAXXGRID*sizeof(int));
  xgrid_area = (double *)malloc(MAXXGRID*sizeof(double));
  dst_area   = (double *)malloc(nx_dst*ny_dst*sizeof(double));
  nxgrid = create_xgrid_2dx2d_order1(&nx_src, &ny_src, &nx_dst, &ny_dst, x_src, y_src, x_dst, y_dst, mask_src,
                                     xgrid_i1, xgrid_j1, xgrid_i2, xgrid_j2, xgrid_area );
  /* The source grid may not cover the destination grid
     so need to sum of exchange grid area to get dst_area
     get_grid_area(&nx_dst, &ny_dst, x_dst, y_dst, dst_area);
  */
  for(n=0; n<nx_dst*ny_dst; n++) dst_area[n] = 0;
  for(n=0; n<nxgrid; n++) dst_area[xgrid_j2[n]*nx_dst+xgrid_i2[n]] += xgrid_area[n];

  area_frac = (double *)malloc(nxgrid*sizeof(double));
  for(n=0; n<nxgrid; n++) area_frac[n] = xgrid_area[n]/dst_area[xgrid_j2[n]*nx_dst+xgrid_i2[n]];

  for(n=0; n<nx_dst*ny_dst; n++) {
    data_dst[n] = 0;
  }
  for(n=0; n<nxgrid; n++) {
    data_dst[xgrid_j2[n]*nx_dst+xgrid_i2[n]] += data_src[xgrid_j1[n]*nx_src+xgrid_i1[n]]*area_frac[n];
  }

  free(xgrid_i1);
  free(xgrid_j1);
  free(xgrid_i2);
  free(xgrid_j2);
  free(xgrid_area);
  free(dst_area);
  free(area_frac);

} /* conserve_interp */

/*------------------------------------------------------------------------------
  void conserve_interp_great_circle()
  conservative interpolation through exchange grid.
  Currently only first order interpolation are implemented here.
  great_circle algorithm is used for clipping and interpolation.
  ----------------------------------------------------------------------------*/
void conserve_interp_great_circle(int nx_src, int ny_src, int nx_dst, int ny_dst, const double *x_src,
                                  const double *y_src, const double *x_dst, const double *y_dst,
                                  const double *mask_src, const double *data_src, double *data_dst )
{
  int n, nxgrid;
  int *xgrid_i1, *xgrid_j1, *xgrid_i2, *xgrid_j2;
  double *xgrid_area, *dst_area, *area_frac, *xgrid_di, *xgrid_dj;

  /* get the exchange grid between source and destination grid. */
  xgrid_i1   = (int    *)malloc(MAXXGRID*sizeof(int));
  xgrid_j1   = (int    *)malloc(MAXXGRID*sizeof(int));
  xgrid_i2   = (int    *)malloc(MAXXGRID*sizeof(int));
  xgrid_j2   = (int    *)malloc(MAXXGRID*sizeof(int));
  xgrid_area = (double *)malloc(MAXXGRID*sizeof(double));
  xgrid_di   = (double *)malloc(MAXXGRID*sizeof(double));
  xgrid_dj   = (double *)malloc(MAXXGRID*sizeof(double));
  dst_area   = (double *)malloc(nx_dst*ny_dst*sizeof(double));
  nxgrid = create_xgrid_great_circle(&nx_src, &ny_src, &nx_dst, &ny_dst, x_src, y_src, x_dst, y_dst, mask_src,
                                     xgrid_i1, xgrid_j1, xgrid_i2, xgrid_j2, xgrid_area, xgrid_di, xgrid_dj );
  /* The source grid may not cover the destination grid
     so need to sum of exchange grid area to get dst_area
     get_grid_area(&nx_dst, &ny_dst, x_dst, y_dst, dst_area);
  */
  for(n=0; n<nx_dst*ny_dst; n++) dst_area[n] = 0;
  for(n=0; n<nxgrid; n++) dst_area[xgrid_j2[n]*nx_dst+xgrid_i2[n]] += xgrid_area[n];

  area_frac = (double *)malloc(nxgrid*sizeof(double));
  for(n=0; n<nxgrid; n++) area_frac[n] = xgrid_area[n]/dst_area[xgrid_j2[n]*nx_dst+xgrid_i2[n]];

  for(n=0; n<nx_dst*ny_dst; n++) {
    data_dst[n] = 0;
  }
  for(n=0; n<nxgrid; n++) {
    data_dst[xgrid_j2[n]*nx_dst+xgrid_i2[n]] += data_src[xgrid_j1[n]*nx_src+xgrid_i1[n]]*area_frac[n];
  }

  free(xgrid_i1);
  free(xgrid_j1);
  free(xgrid_i2);
  free(xgrid_j2);
  free(xgrid_area);
  free(dst_area);
  free(area_frac);

} /* conserve_interp_great_circle */



void linear_vertical_interp(int nx, int ny, int nk1, int nk2, const double *grid1, const double *grid2,
                            double *data1, double *data2)
{
  int n1, n2, i, n, k, l;
  double w;

  for(k=1; k<nk1; k++) {
    if(grid1[k] <= grid1[k-1]) error_handler("interp.c: grid1 not monotonic");
  }
  for(k=1; k<nk2; k++) {
    if(grid2[k] <= grid2[k-1]) error_handler("interp.c: grid2 not monotonic");
  }

  if (grid1[0] > grid2[0] ) error_handler("interp.c: grid2 lies outside grid1");
  if (grid1[nk1-1] < grid2[nk2-1] ) error_handler("interp.c: grid2 lies outside grid1");

  for(k=0; k<nk2; k++) {
    n = nearest_index(grid2[k],grid1,nk1);
    if (grid1[n] < grid2[k]) {
      w = (grid2[k]-grid1[n])/(grid1[n+1]-grid1[n]);
      for(l=0; l<nx*ny; l++) {
        data2[k*nx*ny+l] = (1.-w)*data1[n*nx*ny+l] + w*data1[(n+1)*nx*ny+l];
      }
    }
    else {
      if(n==0)
        for(l=0;l<nx*ny;l++) data2[k*nx*ny+l] = data1[n*nx*ny+l];
      else {
        w = (grid2[k]-grid1[n-1])/(grid1[n]-grid1[n-1]);
        for(l=0; l<nx*ny; l++) {
          data2[k*nx*ny+l] = (1.-w)*data1[(n-1)*nx*ny+l] + w*data1[n*nx*ny+l];
        }
      }
    }
  }
}