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

/*
 * 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/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 ()
{
  char *inFile = "../../data/v.nrrd";
  char *randFile = "../../data/R.nrrd";
  char *outFile = "lic2d.ppm";
  int imgSizeX = 1020;
  int imgSizeY = 561;
  double h = 0.005;
  int stepNum = 10;
 
  // Read in the data
  char* err;
  Nrrd *V;
  int status;
  V = nrrdNew();
  if (V == NULL) {
    err = biffGetDone(NRRD);
    fprintf(stderr, "Trouble allocating nrrd struct:\n%s", err);
    free(err);
    return -1;
  }

  status = nrrdLoad(V, inFile, NULL);
  if (status) {
    err = biffGetDone(NRRD);
    fprintf(stderr, "Trouble reading \"%s\":\n%s", inFile, err);
    free(err);
    nrrdNix(V);
    return -1;
  }

  // Read in the noise
  Nrrd *R;
  R = nrrdNew();
  if (R == NULL) {
    err = biffGetDone(NRRD);
    fprintf(stderr, "Trouble allocating nrrd struct:\n%s", err);
    free(err);
    nrrdNuke(V);
    return -1;
  }

  status = nrrdLoad(R, randFile, NULL);
  if (status) {
    err = biffGetDone(NRRD);
    fprintf(stderr, "Trouble reading \"%s\":\n%s", randFile, err);
    free(err);
    nrrdNix(R);
    nrrdNuke(V);
    return -1;
  }

  double *out_data = malloc(sizeof(double) * 3 * 1020 * 561);
  if (out_data == NULL) {
    fprintf(stderr, "Trouble with malloc\n");
    nrrdNuke(V);
    nrrdNuke(R);
    return -1;
  }

  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, -2.583, sum, 2.583);

          c = (1 - (((2 * l - 1)>0)?(2 * l - 1):(1 - 2 * l))) * s;
          m = l - c / 2;
          if (crl < 0) {
            *(out_data + xi * 3 + yi * 3060 + 0) = c + m;
            *(out_data + xi * 3 + yi * 3060 + 1) = m;
            *(out_data + xi * 3 + yi * 3060 + 2) = c + m;
          }
          else {
            *(out_data + xi * 3 + yi * 3060 + 0) = m;
            *(out_data + xi * 3 + yi * 3060 + 1) = c + m;
            *(out_data + xi * 3 + yi * 3060 + 2) = m;
          }
          break;
        }
      }
    }
  }

  nrrdNuke(V);
  nrrdNuke(R);

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

  unsigned char *uc_out_data = malloc(sizeof(unsigned char) * 3 * 1020 * 561);
  if (uc_out_data == NULL) {
    fprintf(stderr, "Trouble with malloc\n");
    free(out_data);
  }
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 1020; j++) {
      for (int k = 0; k < 561; k++) {
        *(uc_out_data + k * 3060 + j * 3 + i) = ulerp(0, *(out_data + k * 3060 + j * 3 + i), 1);
      }
    }
  }

  free(out_data);

  // Write out data
  Nrrd *nout;
  nout = nrrdNew();
  if (nout == NULL) {
    err = biffGetDone(NRRD);
    fprintf(stderr, "Trouble allocating nrrd struct:\n%s", err);
    free(err);
    free(uc_out_data);
    return -1;
  }

  size_t sizes[3] = {3, 1020, 561};
  status = nrrdWrap_nva(nout, uc_out_data, nrrdTypeUChar, 3, sizes);
  if (status) {
    err = biffGetDone(NRRD);
    fprintf(stderr, "Trouble wrapping nrrd struct:\n%s", err);
    free(err);
    free(uc_out_data);
    nrrdNix(nout);
    return -1;
  }

  status = nrrdSave(outFile, nout, NULL);
  if (status) {
    err = biffGetDone(NRRD);
    fprintf(stderr, "Trouble wrapping nrrd struct:\n%s", err);
    free(err);
    nrrdNuke(nout);
    return -1;
  }

  nrrdNuke(nout);
  return 0;
}