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

/*
 * This code is part of the Diderot Project (http://diderot-language.cs.uchicago.edu)
 *
 * COPYRIGHT (c) 2015 The University of Chicago
 * 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 double dot3(double vec1[3], double vec2[3])
{
  return (vec1[0] * vec2[0] + vec1[1] * vec2[1] + vec1[2] * vec2[2]);
}

STATIC_INLINE double mag3(double vec[3])
{
  return sqrt(dot3(vec, vec));
}

STATIC_INLINE void norm3(double vec[3], double res[3])
{
  double mag = mag3(vec);
  res[0] = vec[0] / mag;
  res[1] = vec[1] / mag;
  res[2] = vec[2] / mag;
}

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

STATIC_INLINE void sub3(double vec1[3], double vec2[3], double res[3])
{
  res[0] = vec1[0] - vec2[0];
  res[1] = vec1[1] - vec2[1];
  res[2] = vec1[2] - vec2[2];
}

// Note res cannot be vec1 or vec2
STATIC_INLINE void cross(double vec1[3], double vec2[3], double res[3])
{
  res[0] = vec1[1] * vec2[2] - vec1[2] * vec2[1];
  res[1] = vec1[2] * vec2[0] - vec1[0] * vec2[2];
  res[2] = vec1[0] * vec2[1] - vec1[1] * vec2[0];
}

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

STATIC_INLINE bool inside(double pos[3])
{
  // XXX - Hack specific to ../../data/vfrhand-nohip.nhdr"
  // but for some reason still not matching what Diderot says
  if(pos[0] < 2*0.9375 || pos[0] > (176-3)*0.9375) return false;
  if(pos[1] < 2*0.9375 || pos[1] > (187-3)*0.9375) return false;
  if(pos[2] < 2.0      || pos[2] > (190-3)) return false;
  return true;
}

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 = argv[0];
  char *infile = "../../data/vfrhand-nohip.nhdr";
  char *outfile = "bmark-teem.nrrd";
  double camEye[3] = {127.331, -1322.05, 272.53};
  double camAt[3] = {63.0, 82.6536, 98.0};
  double camUp[3] = {0.9987, 0.0459166, -0.0221267};
  double camNear = -78.0;
  double camFar = 78.0;
  double camFOV = 5.0;
  int imgResU = 480;
  int imgResV = 345;
  double rayStep = 0.1;
  double valOpacMin = 400.0;        // 400.0 for skin, 1150.0 for bone
  double valOpacMax = 700.0;        // 700.0 for skin, 1450.0 for bone


  double temp[3];
  double temp2[3];
  double temp3[3];
  sub3(camAt, camEye, temp);
  double camDist = mag3( temp );
  double camVspNear = camDist + camNear;
  double camVspFar = camDist + camFar;
  double camN[3];
  norm3(temp, camN);
  double camU[3];
  cross(camN, camUp, temp);
  norm3(temp, camU);
  double camV[3];
  cross(camN, camU, camV);
  double camVmax = tan(camFOV*AIR_PI/360.0)*camDist;
  double camUmax = camVmax*(double)(imgResU)/(double)(imgResV);
  double lightVspDir[3] = {0.9, -1.0, -2.5};
  scale_vec3(lightVspDir[0], camU, temp);
  scale_vec3(lightVspDir[1], camV, temp2);
  add3(temp, temp2, temp);
  scale_vec3(lightVspDir[2], camN, temp2);
  add3(temp, temp2, temp);
  double lightDir[3];
  norm3(temp, lightDir);
  double phongKa = 0.05;
  double phongKd = 0.80;
  double phongKs = 0.20;
  double phongSp = 50.0;

  // Read in the data
  char* err;
  Nrrd *nin;
  int status;
  airArray *mop;
  mop = airMopNew();

  nin = nrrdNew();
  airMopAdd(mop, nin, (airMopper)nrrdNuke, airMopAlways);

  // Get data for mip
  if (nrrdLoad(nin, infile, NULL)) {
    airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
    fprintf(stderr, "%s: Trouble reading \"%s\":\n%s", me, infile, err);
    airMopError(mop); 
    return -1;
  }

  gageContext *ctx;
  gagePerVolume *pvl;
  const double *val;
  const double *grad;
  double kparm[NRRD_KERNEL_PARMS_NUM] = {0.0};

  ctx = gageContextNew();
  airMopAdd(mop, ctx, (airMopper)gageContextNix, airMopAlways);
  if (!( pvl = gagePerVolumeNew(ctx, nin, gageKindScl) )) {
    airMopAdd(mop, err = biffGetDone(GAGE), airFree, airMopAlways);
    fprintf(stderr, "%s: couldn't create gage context:\n%s", me, err);
    airMopError(mop); 
    return -1;
  }
  if (gagePerVolumeAttach(ctx, pvl)
      || gageKernelSet(ctx, gageKernel00, nrrdKernelBSpline3, kparm)
      || gageKernelSet(ctx, gageKernel11, nrrdKernelBSpline3D, kparm)
      || gageQueryItemOn(ctx, pvl, gageSclValue)
      || gageQueryItemOn(ctx, pvl, gageSclGradVec)
      || gageUpdate(ctx)) {
    airMopAdd(mop, err = biffGetDone(GAGE), airFree, airMopAlways);
    fprintf(stderr, "%s: Trouble setting up gage context:\n%s", me, err);
    airMopError(mop); 
    return -1;
  }

  if (!( (val = gageAnswerPointer(ctx, pvl, gageSclValue))
         && (grad = gageAnswerPointer(ctx, pvl, gageSclGradVec)) )) {
    fprintf(stderr, "%s: Trouble getting answer pointer\n", me);
    airMopError(mop); 
    return -1;
  }

  int ui, vi;
  double rayU, rayV, rayN;
  double rayVec[3];
  double toEye[3];
  double rayPos[3];
  double transp, gray;
  double norm[3];
  double alpha;
  double ld;
  double hd;
  double mat;

  // allocate output image
  Nrrd *nimg = nrrdNew();
  airMopAdd(mop, nimg, (airMopper)nrrdNuke, airMopAlways);
  if (nrrdAlloc_va(nimg, nrrdTypeDouble, 3,
                   AIR_CAST(size_t, 4),
                   AIR_CAST(size_t, imgResU),
                   AIR_CAST(size_t, imgResV))) {
    airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
    fprintf(stderr, "%s: Trouble allocating image output:\n%s", me, err);
    airMopError(mop); 
    return -1;
  }
  double *out_data = AIR_CAST(double *, nimg->data);
  /*
  fprintf(stderr, "hit return\n");
  fgetc(stdin);
  fprintf(stderr, "here we go\n");
  */
  double t0 = airTime(); // start timing
  for(vi = 0; vi < imgResV; vi++) {
    rayV = lerp(-camVmax, camVmax, -0.5, (double)(vi), (double)(imgResV) - 0.5);
    for(ui = 0; ui < imgResU; ui++) {
      double len;
      rayU = lerp(-camUmax, camUmax, -0.5, (double)(ui), (double)(imgResU) - 0.5);
      ELL_3V_SCALE_ADD3(rayVec,   1.0, camN,   rayU/camDist, camU,   rayV/camDist, camV);
      ELL_3V_SCALE(toEye, -1, rayVec);
      ELL_3V_NORM(toEye, toEye, len);
      transp = 1;
      gray = 0;

      for (rayN = camVspNear; rayN <= camVspFar; rayN += rayStep) {
        ELL_3V_SCALE_ADD2(rayPos,   1.0, camEye,  rayN, rayVec);

        if (inside(rayPos)) {
          if (gageProbeSpace(ctx, rayPos[0], rayPos[1], rayPos[2],
                             AIR_FALSE /* index space */, AIR_TRUE /* clamp */)) {
            fprintf(stderr, "%s: Trouble with gageProbe:\n%s", me, ctx->errStr);
            airMopError(mop); return -1;
          }
          if (*val > valOpacMin) {
            ELL_3V_SCALE(temp2, -1, grad);
            ELL_3V_NORM(norm, temp2, len);
            alpha = lerp(0.0, 1.0,  valOpacMin, *val, valOpacMax);
            alpha = (alpha < 1.0) ? alpha : 1;
            ld = dot3(norm, lightDir);
            ld = (ld < 0) ? 0 : ld;
            ELL_3V_ADD2(temp2, toEye, lightDir);
            ELL_3V_NORM(temp2, temp2, len);
            hd = dot3(norm, temp2);
            hd = (hd < 0) ? 0 : hd;
            mat = (phongKa +
                   phongKd * ld +
                   phongKs * pow(hd, phongSp));
            gray = gray + transp * alpha * mat;
            transp *= 1 - alpha;
          }
        }
        if (transp < 0.01) {
          transp = 0;
          break;
        }
      }

      ELL_4V_SET(out_data + 4*(ui + imgResU*vi), gray, gray, gray, 1.0-transp);

    }
  }

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

  if (nrrdSave(outfile, nimg, NULL)) {
    airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
    fprintf(stderr, "%s: Trouble writing output:\n%s", me, err);
    airMopError(mop); 
    return -1;
  }
  
  airMopOkay(mop); 
  return 0;

}