/*! \file /lic2d/bmark-teem.c
 *
 * \author Nick Seltzer and Gordon Kindlmann
 */

/*
 * COPYRIGHT (c) 2011 The Diderot Project (http://diderot-language.cs.uchicago.edu)
 * All rights reserved.
 */

#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include "teem/nrrd.h"
#include "teem/ell.h"
#include "teem/gage.h"

#include "teem-defs.h"

STATIC_INLINE void scale2(double scl, double vec[2], double res[2])
{
  res[0] = scl * vec[0];
  res[1] = scl * vec[1];
}

STATIC_INLINE void add2(double vec1[2], double vec2[2], double res[2])
{
  res[0] = vec1[0] + vec2[0];
  res[1] = vec1[1] + vec2[1];
}

STATIC_INLINE double dot2(double vec1[2], double vec2[2])
{
  return (vec1[0] * vec2[0] + vec1[1] * vec2[1]);
}

STATIC_INLINE double mag2(double vec[2])
{
  return sqrt(dot2(vec, vec));
}

STATIC_INLINE void norm2(double vec[2], double res[2])
{
  double mag = mag2(vec);
  res[0] = vec[0] / mag;
  res[1] = vec[1] / mag;
}

STATIC_INLINE bool inside(double pos[2])
{
  // XXX - Hack
  if (pos[0] < 0.00 || pos[0] >= 6.78) return false;
  if (pos[1] < 0.00 || pos[1] >= 3.72) return false;
  return true;
}

void atR(void *_data, double pos[2], double *out)
{
  float *data = (float*)_data;
  double temp[2][2];
  int a = floor(pos[0] * 150) + 1;
  int b = floor(pos[1] * 150) + 1;
  for (int i = 0; i < 2; i++) {
    for (int j = 0; j < 2; j++) {
      temp[i][j] = *(data + (i + a) + (j + b) * 1020);
    }
  }
  double temp2[2];
  double t = pos[0]*150 - floor(pos[0]*150);
  for (int i = 0; i < 2; i++) {
    temp2[i] = (temp[0][i] * (1 - t) + temp[1][i] * t);
  }
  t = pos[1]*150 - floor(pos[1]*150);
  *out = (temp2[0] * (1 - t) + temp2[1] * t);
}

void at(void* _data, double pos[2], double out[2])
{
  // XXX - Hack
  float *data = (float*)_data;
  double tempx[4][4];
  double tempy[4][4];
  int a = floor(pos[0] + 3);
  int b = floor(pos[1] + 3);
  for (int i = 0; i < 4; i++) {
    for (int j = 0; j < 4; j++) {
      tempx[i][j] = data[0 + 2*(i + a - 1 + 15*(j + b - 1))];
      tempy[i][j] = data[1 + 2*(i + a - 1 + 15*(j + b - 1))];
    }
  }
  double t = pos[0]+3 - a;
  double tempxx[4];
  double tempyx[4];
  for (int i = 0; i < 4; i++) {
    tempxx[i] = (tempx[0][i] + 4 * tempx[1][i] + tempx[2][i] +
                 (-3 * tempx[0][i] + 3 * tempx[2][i]) * t +
                 (3 * tempx[0][i] - 6 * tempx[1][i] + 3 * tempx[2][i]) * t * t +
                 (-1 * tempx[0][i] + 3 * tempx[1][i] - 3 * tempx[2][i] + tempx[3][i]) * t * t * t) / 6.0;
    tempyx[i] = (tempy[0][i] + 4 * tempy[1][i] + tempy[2][i] +
                 (-3 * tempy[0][i] + 3 * tempy[2][i]) * t +
                 (3 * tempy[0][i] - 6 * tempy[1][i] + 3 * tempy[2][i]) * t * t +
                 (-1 * tempy[0][i] + 3 * tempy[1][i] - 3 * tempy[2][i] + tempy[3][i]) * t * t * t) / 6.0;
  }
  t = pos[1]+3 - b;
  out[0] = (tempxx[0] + 4 * tempxx[1] + tempxx[2] +
               (-3 * tempxx[0] + 3 * tempxx[2]) * t +
               (3 * tempxx[0] - 6 * tempxx[1] + 3 * tempxx[2]) * t * t +
               (-1 * tempxx[0] + 3 * tempxx[1] - 3 * tempxx[2] + tempxx[3]) * t * t * t) / 6.0;
  out[1] = (tempyx[0] + 4 * tempyx[1] + tempyx[2] +
               (-3 * tempyx[0] + 3 * tempyx[2]) * t +
               (3 * tempyx[0] - 6 * tempyx[1] + 3 * tempyx[2]) * t * t +
               (-1 * tempyx[0] + 3 * tempyx[1] - 3 * tempyx[2] + tempyx[3]) * t * t * t) / 6.0;
  return;
}

double crl_at(void* _data, double pos[2])
{
  // XXX - Hack
  float *data = (float*)_data;
  double tempx[4][4];
  double tempy[4][4];
  double temp[2];
  int a = floor(pos[0] * 50) + 5;
  int b = floor(pos[1] * 50) + 5;
  for (int i = 0; i < 4; i++) {
    for (int j = 0; j < 4; j++) {
      tempx[i][j] = *(data + (j + b - 1) * 700 + (i + a - 1) * 2 + 0);
      tempy[i][j] = *(data + (j + b - 1) * 700 + (i + a - 1) * 2 + 1);
    }
  }
  double t = pos[0]*50 - floor(pos[0]*50);
  double tempxx[4];
  double tempyx[4];
  for (int i = 0; i < 4; i++) {
    tempxx[i] = (tempx[0][i] + 4 * tempx[1][i] + tempx[2][i] +
                 (-3 * tempx[0][i] + 3 * tempx[2][i]) * t +
                 (3 * tempx[0][i] - 6 * tempx[1][i] + 3 * tempx[2][i]) * t * t +
                 (-1 * tempx[0][i] + 3 * tempx[1][i] - 3 * tempx[2][i] + tempx[3][i]) * t * t * t) / 6.0;
    tempyx[i] = ((-3 * tempy[0][i] + 3 * tempy[2][i]) * 1 +
                 (3 * tempy[0][i] - 6 * tempy[1][i] + 3 * tempy[2][i]) * t * 2 +
                 (-1 * tempy[0][i] + 3 * tempy[1][i] - 3 * tempy[2][i] + tempy[3][i]) * t * t * 3) / 6.0;
  }
  t = pos[1]*50 - floor(pos[1]*50);
  temp[0] = ((-3 * tempxx[0] + 3 * tempxx[2]) * 1 +
               (3 * tempxx[0] - 6 * tempxx[1] + 3 * tempxx[2]) * t * 2 +
               (-1 * tempxx[0] + 3 * tempxx[1] - 3 * tempxx[2] + tempxx[3]) * t * t * 3) / 6.0;
  temp[1] = (tempyx[0] + 4 * tempyx[1] + tempyx[2] +
               (-3 * tempyx[0] + 3 * tempyx[2]) * t +
               (3 * tempyx[0] - 6 * tempyx[1] + 3 * tempyx[2]) * t * t +
               (-1 * tempyx[0] + 3 * tempyx[1] - 3 * tempyx[2] + tempyx[3]) * t * t * t) / 6.0;
  return temp[1] - temp[0];
}

STATIC_INLINE double lerp(double out_min, double out_max, double in_min, double in, double in_max)
{
  return (out_min * (in_max - in) + out_max * (in - in_min)) / (in_max - in_min);
}

STATIC_INLINE unsigned char ulerp(double in_min, double in, double in_max)
{
  return (unsigned char)((0.0f * (in_max - in) + 255.0f * (in - in_min)) / (in_max - in_min));
}

int main (int argc, const char *argv[])
{
  const char *me;
  char *inFile = "imp-u.nrrd";
  char *randFile = "../../data/R.nrrd";
  char *outFile = "bmark-teem.nrrd";
  int imgSizeX = 500;
  int imgSizeY = 500;
  double h = 0.005;
  int stepNum = 10;
 
  me = argv[0];
  airArray *mop;
  mop = airMopNew();

  // Read in the data
  char* err;
  Nrrd *V = nrrdNew();
  airMopAdd(mop, V, (airMopper)nrrdNuke, airMopAlways);
  Nrrd *R =  nrrdNew();
  airMopAdd(mop, R, (airMopper)nrrdNuke, airMopAlways);
  if (nrrdLoad(V, inFile, NULL)
      || nrrdLoad(R, randFile, NULL)) {
    airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
    fprintf(stderr, "%s: Trouble reading:\n%s", me, err);
    airMopError(mop);
    return -1;
  }

  Nrrd *nout = nrrdNew();
  airMopAdd(mop, nout, (airMopper)nrrdNuke, airMopAlways);  
  if (nrrdAlloc_va(nout, nrrdTypeDouble, 3, 
                   AIR_CAST(size_t, 3),
                   AIR_CAST(size_t, 500),
                   AIR_CAST(size_t, 500))) {
    airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
    fprintf(stderr, "%s: Trouble allocating:\n%s", me, err);
    airMopError(mop);
    return -1;
  }
  double *out_data = AIR_CAST(double *, nout->data);

  double t0 = airTime(); // start timing
  double pos0[2];
  double forw[2];
  double back[2];
  double temp[2];
  double sum;
  double crl;
  double s, l, c, m;
  int step;
  int num;
  for (int yi = 0; yi < 500; yi++) {
    pos0[1] = lerp(0.0, 8.0, 0, yi, 499);
    for (int xi = 0; xi < 500; xi++) {
      pos0[0] = lerp(0.0, 8.0, 0, xi, 499);
      forw[0] = pos0[0];
      forw[1] = pos0[1];
      back[0] = pos0[0];
      back[1] = pos0[1];
      atR(R->data, pos0, &sum);
      sum = -sum;
      step = -1;
      num = -1;
      while (true) {
        at(V->data, pos0, temp);
        ELL_3V_SET(out_data + 3*(xi + 500*yi), temp[0], temp[1], temp[0]);
        break;
      }
    }
  }

  double totalTime = airTime() - t0;
  printf("usr=%f\n", totalTime);

  if (nrrdSave(outFile, nout, NULL)) {
    airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
    fprintf(stderr, "%s: Trouble saving:\n%s", me, err);
    airMopError(mop);
    return -1;
  }

  airMopOkay(mop);
  return 0;
}