/*! \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.04 || pos[0] > 6.76) return false;
  if (pos[1] < 0.04 || pos[1] > 3.7) 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);
  int b = floor(pos[1] * 150);
  for (int i = 0; i < 4; 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] * 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]*150 - floor(pos[0]*150);
  double tempxx[4];
  double tempyx[4];
  for (int i = 0; i < 4; i++) {
    tempxx[i] = 0.5 * (2 * tempx[0][i] +
                       (-tempx[0][i] + tempx[2][i]) * t +
                       (2 * tempx[0][i] - 5 * tempx[1][i] + 4 * tempx[2][i] - tempx[3][i]) * t * t +
                       (-tempx[0][i] + 3 * tempx[1][i] - 3 * tempx[2][i] + tempx[3][i]) * t * t * t);
    tempyx[i] = 0.5 * (2 * tempy[0][i] +
                       (-tempy[0][i] + tempy[2][i]) * t +
                       (2 * tempy[0][i] - 5 * tempy[1][i] + 4 * tempy[2][i] - tempy[3][i]) * t * t +
                       (-tempy[0][i] + 3 * tempy[1][i] - 3 * tempy[2][i] + tempy[3][i]) * t * t * t);
  }
  t = pos[1]*150 - floor(pos[1]*150);
  out[0] = 0.5 * (2 * tempxx[0] +
                     (-tempxx[0] + tempxx[2]) * t +
                     (2 * tempxx[0] - 5 * tempxx[1] + 4 * tempxx[2] - tempxx[3]) * t * t +
                     (-tempxx[0] + 3 * tempxx[1] - 3 * tempxx[2] + tempxx[3]) * t * t * t);
  out[1] = 0.5 * (2 * tempyx[0] +
                     (-tempyx[0] + tempyx[2]) * t +
                     (2 * tempyx[0] - 5 * tempyx[1] + 4 * tempyx[2] - tempyx[3]) * t * t +
                     (-tempyx[0] + 3 * tempyx[1] - 3 * tempyx[2] + tempyx[3]) * t * t * t);
  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] - floor(pos[0]);
  double tempxx[4];
  double tempyx[4];
  for (int i = 0; i < 4; i++) {
    tempxx[i] = 0.5 * (2 * tempx[0][i] +
                       (-tempx[0][i] + tempx[2][i]) * t +
                       (2 * tempx[0][i] - 5 * tempx[1][i] + 4 * tempx[2][i] - tempx[3][i]) * t * t +
                       (-tempx[0][i] + 3 * tempx[1][i] - 3 * tempx[2][i] + tempx[3][i]) * t * t * t);
    tempyx[i] = 0.5 * (2 * tempy[0][i] * 0 +
                       (-tempy[0][i] + tempy[2][i]) * 1 +
                       (2 * tempy[0][i] - 5 * tempy[1][i] + 4 * tempy[2][i] - tempy[3][i]) * t * 2 +
                       (-tempy[0][i] + 3 * tempy[1][i] - 3 * tempy[2][i] + tempy[3][i]) * t * t * 3);
  }
  t = pos[1] - floor(pos[1]);
  temp[0] = 0.5 * (2 * tempxx[0] * 0 +
                     (-tempxx[0] + tempxx[2]) * 1 +
                     (2 * tempxx[0] - 5 * tempxx[1] + 4 * tempxx[2] - tempxx[3]) * t * 2 +
                     (-tempxx[0] + 3 * tempxx[1] - 3 * tempxx[2] + tempxx[3]) * t * t * 3);
  temp[1] = 0.5 * (2 * tempyx[0] +
                     (-tempyx[0] + tempyx[2]) * t +
                     (2 * tempyx[0] - 5 * tempyx[1] + 4 * tempyx[2] - tempyx[3]) * t * t +
                     (-tempyx[0] + 3 * tempyx[1] - 3 * tempyx[2] + tempyx[3]) * t * t * t);
  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 = "../../data/v.nrrd";
  char *randFile = "../../data/R.nrrd";
  char *outFile = "bmark-teem.nrrd";
  int imgSizeX = 1020;
  int imgSizeY = 561;
  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, 1020),
                   AIR_CAST(size_t, 561))) {
    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 < 561; yi++) {
    pos0[1] = lerp(0.04, 3.7, 0, yi, 561);
    for (int xi = 0; xi < 1020; xi++) {
      pos0[0] = lerp(0.04, 6.76, 0, xi, 1020);
      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) {
        if (inside(forw)) {
          atR(R->data, forw, temp);
          sum += temp[0];
          num++;

          at(V->data, forw, temp);
          norm2(temp, temp);
          scale2(0.5 * h, temp, temp);
          add2(forw, temp, temp);
          if (inside(temp)) {
            at(V->data, temp, temp);
            norm2(temp, temp);
            scale2(h, temp, temp);
            add2(forw, temp, forw);
          }
        }
        if (inside(back)) {
          atR(R->data, back, temp);
          sum += temp[0];
          num++;

          at(V->data, back, temp);
          norm2(temp, temp);
          scale2(-0.5 * h, temp, temp);
          add2(back, temp, temp);
          if (inside(temp)) {
            at(V->data, temp, temp);
            norm2(temp, temp);
            scale2(-1 * h, temp, temp);
            add2(back, temp, back);
          }
        }
        step += 1;
        if ((!inside(forw) && !inside(back)) || step == stepNum) {
          at(V->data, pos0, temp);
          sum = pow(mag2(temp), 0.3) * sum / num;
          crl = crl_at(V->data, pos0);
          s = lerp(0, 1, 0, (crl>0)?crl:-1*crl, 2.794);
          l = lerp(0.0, 1.0, -1.6, sum, 2.6);

          c = (1 - (((2 * l - 1)>0)?(2 * l - 1):(1 - 2 * l))) * s;
          m = l - c / 2;
          if (crl > 0) {
            ELL_3V_SET(out_data + 3*(xi + 1020*yi), c+m, m, c+m);
          }
          else {
            ELL_3V_SET(out_data + 3*(xi + 1020*yi), m, c+m, m);
          }
          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;
}