/* c3d-vec.c
 *
 * COPYRIGHT (c) 2010 The Diderot Project (http://diderot-language.cs.uchicago.edu)
 * All rights reserved.
 *
 * Single-precision vector version.
 */

#include <stdio.h>
#include <math.h>
#include <assert.h>

typedef float float4 __attribute__ ((__vector_size__ (16)));

typedef union {
    float	f[4];
    float4	v;
} f4u_t;

#define IMG_SZ 11

float V[IMG_SZ][IMG_SZ][IMG_SZ];		// image data
float h[4][4] = {		// bspln3
	{ 1.33333,   2.0,  1.0,  0.166667 },   // -2 .. -1
	{ 0.666667,  0.0, -1.0, -0.5 },        // -1 .. 0
	{ 0.666667,  0.0, -1.0,  0.5 },        //  0 .. 1
	{ 1.33333,  -2.0,  1.0, -0.166667 },   //  1 .. 2
};
const int s = 2;       // kernel support

float4 a, b, c, d;

// fake vector dot product.  There are SSE instructions for computing this.
//
static inline float dot (float4 a, float4 b)
{
    f4u_t av, bv;
    av.v = a;
    bv.v = b;
    float s = 0.0;
    for (int i = 0;  i < 4;  i++)
	s += av.f[i] * bv.f[i];

    return s;
}

static inline void printv (float4 v)
{
    f4u_t f;
    f.v = v;
    fprintf(stderr, "<%f, %f, %f, %f>", f.f[0], f.f[1], f.f[2], f.f[3]);
}

float probe (float x[3]) { 
  float nxd, nyd, nzd, fx, fy, fz;
  int nx, ny, nz;

  fx = modff (x[0], &nxd); nx = (int)nxd;
  fy = modff (x[1], &nyd); ny = (int)nyd;
  fz = modff (x[2], &nzd); nz = (int)nzd;

  // compute kernel values for each axis
  float4 t, hx, hy, hz;
  t = (float4){fx + 1, fx, fx - 1, fx - 2};
  hx = d + t*(c + t*(b + t*a));
  t = (float4){fy + 1, fy, fy - 1, fy - 2};
  hy = d + t*(c + t*(b + t*a));
  t = (float4){fz + 1, fz, fz - 1, fz - 2};
  hz = d + t*(c + t*(b + t*a));

  f4u_t vy, vz;
  for (int i = 1-s;  i <= s;  i++) {
    for (int j = 1-s;  j <= s;  j++) {
      float4 v = (float4){
	  V[nx+i][ny+j][nz-1],
	  V[nx+i][ny+j][nz],
	  V[nx+i][ny+j][nz+1],
	  V[nx+i][ny+j][nz+2]};
      vz.f[j+s-1] = dot(v, hz);
    }
    vy.f[i+s-1] = dot(vz.v, hy);
  }

  return dot(vy.v, hx);
}

#define AIR_AFFINE(i,x,I,o,O) ( \
((float)(O)-(o))*((float)(x)-(i)) / ((float)(I)-(i)) + (o))

int
main() {
  int I, J, K, N;
  float P[3];

// initialize coefficients
  d = (float4){h[3][0], h[2][0], h[1][0], h[0][0]}; // x^0 coeffs
  c = (float4){h[3][1], h[2][1], h[1][1], h[0][1]}; // x^1 coeffs
  b = (float4){h[3][2], h[2][2], h[1][2], h[0][2]}; // x^2 coeffs
  a = (float4){h[3][3], h[2][3], h[1][3], h[0][3]}; // x^3 coeffs

  V[5][5][5] = 1.0;
  N=60;
  for (I=0; I<N; I++) {
    P[0] = AIR_AFFINE(0, I, N, 2.0001, 7.9999);
    for (J=0; J<N; J++) {
      P[1] = AIR_AFFINE(0, J, N, 2.0001, 7.9999);
      for (K=0; K<N; K++) {
        P[2] = AIR_AFFINE(0, K, N, 2.0001, 7.9999);
        printf("%g\n", probe(P));
      }
    }
  }

  return 0;
}