/* See float_image_interpolate_C1.h */
/* Last edited on 2009-06-03 23:23:24 by stolfi */ 

#define _GNU_SOURCE
#include <limits.h>
#include <float.h>
#include <assert.h>
#include <math.h>

#include <affirm.h>
#include <bool.h>
#include <indexing.h>
#include <float_image.h>

#include <float_image_interpolate_C1.h>

/* INTERNAL PROTOTYPES */

void float_image_interpolate_C1_get_indices_and_weights(double z, int N, ix_reduction_t red, int i[], double w[]);
  /* Computes the indices {i[0..3]} of the pixels needed to
    interpolate a row (or column) of samples at the fractional
    coordinate {z}, and their respective weights {w[0..3]}. Indices {i}
    outside the range {0..N-1} are reduced according to {ix_reduce(i, N,
    red)}. The reduced index may be -1 to denote `no such pixel'. */

void float_image_interpolate_C1_get_samples_and_weights(float_image_t *A, int c, double x, double y, ix_reduction_t red, , float *p[]);
  /* Computes the addresses {p[0..15]} of the 16 samples of channel
    {c} that are involved in the interpolation of image {A} at the
    point {(x,y)}. Indices {i} outside the range {0..N-1} are reduced
    according to {ix_reduce(i, N, red)}. If the reduced index is
    invalid, sets the pointer to NULL. */

/* IMPLEMENTATIONS */

void float_image_interpolate_C1_get_indices_and_weights(double z, int N, ix_reduction_t red, int i[], double w[])
  {
    bool_t debug = FALSE;

    /* Adjust {z} to be relatice to start of first pixel: */
    z = z - 1.5;
    
    /* Get the raw index of the first pixel: */
    int iz = (int)floor(z);
    
    /* Get the fraction {f} fromdata pixel 0: */
    double fz = z - iz;

    /* Make sure that the fraction is in {[0 _ 1]}: */
    double fz = z - iz;
    if (fz < 0.0) { iz--; fz += 1.0; }
    if (fz > 1.0) { iz++; fz -= 1.0; }

    /* Compute the reduced indices {i[0..3]}: */
    int k;
    for (k = 0; k < 4; k++)
      { i[k] = ix_reduce(iz, N, red);
        iz++;
      }

    /* Compute the interpolation weights {w[0..4]}: */
    /* Cubic C1 interpolant, unit sum: */
    double gz = 1.0 - fz;
    double a = 2.0/3.0;
    double b = 7.0/3.0;
    double c = b-1;
    w[0] = -a*fz*fz*gz; 
    w[1] = 1.0 - fz*fz*(b - c*fz);
    w[2] = 1.0 - gz*gz*(b - c*gz);
    w[3] = -a*gz*gz*fz;

    if (debug)
      { fprintf(stderr, "z = %7.4f", z);
        fprintf(stderr, " is = %d %d %d %d\n", is[0], is[1], is[2], is[3]);
        fprintf(stderr, "\n");
      }
  }
  
void float_image_interpolate_C1_get_samples(float_image_t *A, int c, double x, double y, ix_reduction_t red, float *p[])
  {
    !! revise
    int ix[4];
    float_image_interpolate_C1_get_indices(x, A->sz[1], red, ix);
    
    int iy[4];
    float_image_interpolate_C1_get_indices(y, A->sz[2], red, iy);
      
    int k = 0;
    int dx, dy;
    for (dy = 0; dy < 4; dy++)
      { for (dx = 0; dx < 4; dx++)
          { p[k] = float_image_get_sample_address(A, c, ix[dx], iy[dy]);
            k++;
          }
      }
  }

float float_image_interpolate_C1_sample(float_image_t *A, int c, double x, double y, ix_reduction_t red)
  { !! revise
    /* Get pixel values and weights: */
    float *p[16];
    float_image_interpolate_C1_get_samples(A, c, x, y, red, p);
    
    double wx[4];
    float_image_interpolate_C1_get_weights(x, wx);

    double wy[4];
    float_image_interpolate_C1_get_weights(y, wy);
    
    /* Apply interpolation formula: */
    double u[4];
    int ky;
    for (ky = 0; ky < 4;ky++)
      { float **q = &(p[4*ky]);
        u[ky] = wx[0]*(*(q[0])) + wx[1]*(*(q[1])) + wx[2]*(*(q[2])) + wx[3]*(*(q[3]));
      }
    return wy[0]*u[0] + wy[1]*u[1] + wy[2]*u[2] + wy[3]*u[3];
  }
  
void float_image_interpolate_C1_pixel(float_image_t *A, double x, double y, ix_reduction_t red, float v[])
  { !! revise
    /* Get pixel values and weights: */
    float *p[16];
    float_image_interpolate_C1_get_samples(A, 0, x, y, red, p);
    
    double wx[4];
    float_image_interpolate_C1_get_weights(x, wx);

    double wy[4];
    float_image_interpolate_C1_get_weights(y, wy);
    
    /* Apply interpolation formula to all channels: */
    int NC = A->sz[0];         /* Number of channels. */
    ix_step_t cst = A->st[0];  /* Position increment between channels. */
    int c;
    for (c = 0; c < NC; c++)
      { double u[4];
        int ky;
        for (ky = 0; ky < 4; ky++)
          { float **q = &(p[4*ky]);
            u[ky] = wx[0]*(*(q[0])) + wx[1]*(*(q[1])) + wx[2]*(*(q[2])) + wx[3]*(*(q[3]));
            /* Advance sample pointers to next channel: */
            q[0] += cst; q[1] += cst;  q[2] += cst;  q[3] += cst;
          }
         v[c] = wy[0]*u[0] + wy[1]*u[1] + wy[2]*u[2] + wy[3]*u[3];
      }
  }