/*! \file vr-lite-cam.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 <sys/time.h>

static double GetTime ()
{
    struct timeval tv;

    gettimeofday (&tv, 0);

    return (double)tv.tv_sec + 0.000001 * (double)tv.tv_usec;
}

#define STATIC_INLINE static inline

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
  if(pos[0] < -0.5 || pos[0] > 175.5) return false;
  if(pos[1] < -0.5 || pos[1] > 186.5) return false;
  if(pos[2] < -0.5 || pos[2] > 189.5) 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 ()
{
  char *infile = "../../data/vfrhand-nohip.nhdr";
  char *outfile = "vr-lite-cam.pgm";
  double Zero[3] = {0, 0, 0};
  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.5;
  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*3.1415926536/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 = 30.0;

  // Read in the data
  char* err;
  Nrrd *nin;
  int status;
  nin = nrrdNew();
  if(nin == NULL)
  {
    err = biffGetDone(NRRD);
    fprintf(stderr, "Trouble allocating nrrd struct:\n%s", err);
    free(err);
    return -1;
  }


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

  // Get data for mip
  gageContext *ctx;
  gagePerVolume *pvl;
  const double *val;
  const double *grad;
  double kparm[NRRD_KERNEL_PARMS_NUM];

  ctx = gageContextNew();
  if(ctx == NULL)
  {
    err = biffGetDone(GAGE);
    fprintf(stderr, "Trouble allocating gage context:\n%s", err);
    free(err);
    nrrdNuke(nin);
    return -1;
  }

  pvl = gagePerVolumeNew(ctx, nin, gageKindScl);
  if(pvl == NULL)
  {
    err = biffGetDone(GAGE);
    fprintf(stderr, "Trouble allocating gage PerVolume:\n%s", err);
    free(err);
    gageContextNix(ctx);
    nrrdNuke(nin);
    return -1;
  }

  status = gagePerVolumeAttach(ctx, pvl);
  if(status)
  {
    err = biffGetDone(GAGE);
    fprintf(stderr, "Trouble attaching gage PerVolume:\n%s", err);
    free(err);
    gageContextNix(ctx);
    nrrdNuke(nin);
    return -1;
  }

  // It's okay to use kparm unitialized here: gageKernelSet ignores it.
  status = gageKernelSet(ctx, gageKernel00, nrrdKernelBSpline3, kparm);
  if(status)
  {
    err = biffGetDone(GAGE);
    fprintf(stderr, "Trouble setting kernel:\n%s", err);
    free(err);
    gageContextNix(ctx);
    nrrdNuke(nin);
    return -1;
  }

  // It's okay to use kparm unitialized here: gageKernelSet ignores it.
  status = gageKernelSet(ctx, gageKernel11, nrrdKernelBSpline3D, kparm);
  if(status)
  {
    err = biffGetDone(GAGE);
    fprintf(stderr, "Trouble setting kernel:\n%s", err);
    free(err);
    gageContextNix(ctx);
    nrrdNuke(nin);
    return -1;
  }

  status = gageQueryItemOn(ctx, pvl, gageSclValue);
  if(status)
  {
    err = biffGetDone(GAGE);
    fprintf(stderr, "Trouble with gageQueryItemOn:\n%s", err);
    free(err);
    gageContextNix(ctx);
    nrrdNuke(nin);
    return -1;
  }

  status = gageQueryItemOn(ctx, pvl, gageSclGradVec);
  if(status)
  {
    err = biffGetDone(GAGE);
    fprintf(stderr, "Trouble with gageQueryItemOn:\n%s", err);
    free(err);
    gageContextNix(ctx);
    nrrdNuke(nin);
    return -1;
  }

  status = gageUpdate(ctx);
  if(status)
  {
    err = biffGetDone(GAGE);
    fprintf(stderr, "Trouble with gageUpdate:\n%s", err);
    free(err);
    gageContextNix(ctx);
    nrrdNuke(nin);
    return -1;
  }

  val = gageAnswerPointer(ctx, pvl, gageSclValue);
  if(val == NULL)
  {
    err = biffGetDone(GAGE);
    fprintf(stderr, "Trouble getting answer pointer:\n%s", err);
    free(err);
    gageContextNix(ctx);
    nrrdNuke(nin);
    return -1;
  }

  grad = gageAnswerPointer(ctx, pvl, gageSclGradVec);
  if(grad == NULL)
  {
    err = biffGetDone(GAGE);
    fprintf(stderr, "Trouble getting answer pointer:\n%s", err);
    free(err);
    gageContextNix(ctx);
    nrrdNuke(nin);
    return -1;
  }

  
  int ui, vi;
  double rayU, rayV, rayN;
  double rayVec[3];
  double rayDir[3];
  double rayPos[3];
  double transp, gray;
  double output[4];
  double norm[3];
  double alpha;
  double ld;
  double hd;
  double mat;
  bool set = false;
  double max_gray = 0;
  double min_gray = 0;
  double *out_data = malloc(sizeof(double) * 4 * imgResU * imgResV);
  if(out_data == NULL)
  {
    fprintf(stderr, "Trouble with malloc.\n");
    gageContextNix(ctx);
    nrrdNuke(nin);
    return -1;
  }

  double t0 = GetTime();
  for(vi = 0; vi < imgResV; vi++)
  {
    rayV = lerp(-camVmax, camVmax, -0.5, (double)(vi), (double)(imgResV) - 0.5);
    scale_vec3(rayV, camV, temp);
    for(ui = 0; ui < imgResU; ui++)
    {
      rayU = lerp(-camUmax, camUmax, -0.5, (double)(ui), (double)(imgResU) - 0.5);
      scale_vec3(rayU, camU, temp2);
      add3(temp, temp2, temp2);
      scale_vec3(camDist, camN, temp3);
      add3(temp2, temp3, temp3);
      scale_vec3(1 / camDist, temp3, rayVec);
      norm3(rayVec, rayDir);
      transp = 1;
      gray = 0;
      output[0] = 0;
      output[1] = 0;
      output[2] = 0;
      output[3] = 0;
      for(rayN = camVspNear; rayN <= camVspFar; rayN += rayStep)
      {
        scale_vec3(rayN, rayDir, temp2);
        add3(temp2, camEye, rayPos);
        if(inside(rayPos))
        {
          status = gageProbe(ctx, rayPos[0], rayPos[1], rayPos[2]);
          if(status)
          {
            err = ctx->errStr;
            fprintf(stderr, "Trouble with gageProbe:\n%s", err);
            free(err);
            free(out_data);
            gageContextNix(ctx);
            nrrdNuke(nin);
            return -1;
          }
          if(*val > valOpacMin)
          {
            temp2[0] = *(grad + 0);
            temp2[1] = *(grad + 1);
            temp2[2] = *(grad + 2);
            sub3(Zero, temp2, temp2);
            norm3(temp2, norm);
            alpha = lerp(0.0, 1.0,  valOpacMin, *val, valOpacMax);
            alpha = (alpha < 1.0) ? alpha : 1;
            ld = dot3(norm, lightDir);
            ld = (ld < 0) ? 0 : ld;
            sub3(camEye, rayPos, temp2);
            norm3(temp2, temp2);
            add3(temp2, lightDir, temp2);
            norm3(temp2, temp2);
            hd = dot3(norm, temp2);
            hd = (hd < 0) ? 0 : hd;
            mat = phongKa +
                  phongKd * ((ld > 0) ? ld : 0) +
                  phongKs * ((hd > 0) ? pow(hd, phongSp) : 0);
            gray = gray + transp * alpha * mat;
            transp *= 1 - alpha;
          }
        }
        if(transp < 0.01)
        {
          transp = 0;
          break;
        }
      }
      *(out_data + ui * imgResV * 4 + vi * 4 + 0) = gray;
      *(out_data + ui * imgResV * 4 + vi * 4 + 1) = gray;
      *(out_data + ui * imgResV * 4 + vi * 4 + 2) = gray;
      *(out_data + ui * imgResV * 4 + vi * 4 + 3) = 1.0f - transp;
      if(!set)
      {
        max_gray = gray;
        min_gray = gray;
        set = true;
      }
      max_gray = (gray > max_gray) ? gray : max_gray;
      min_gray = (gray < min_gray) ? gray : min_gray;
    }
  }

  unsigned char *uc_out_data = malloc(sizeof(unsigned char) * imgResU * imgResV);
  for(vi = 0; vi < imgResV; vi++)
  {
    for(ui = 0; ui < imgResU; ui++)
    {
      *(uc_out_data + vi * imgResU + ui) = ulerp(min_gray, *(out_data + ui * imgResV * 4 + vi * 4 + 0), max_gray);
    }
  }
  double totalTime = GetTime() - t0;

  printf("usr=%f\n", totalTime);

  // 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(out_data);
    free(uc_out_data);
    gageContextNix(ctx);
    nrrdNuke(nin);
    return -1;
  }

  status = nrrdWrap_va(nout, uc_out_data, nrrdTypeUChar, 2, imgResU, imgResV);
  if(status)
  {
    err = biffGetDone(NRRD);
    fprintf(stderr, "Trouble wrapping nrrd struct:\n%s", err);
    free(err);
    nrrdNix(nout);
    free(out_data);
    gageContextNix(ctx);
    nrrdNuke(nin);
    return -1;
  }

  status = nrrdSave(outfile, nout, NULL);
  if(status)
  {
    err = biffGetDone(NRRD);
    fprintf(stderr, "Trouble saving nrrd struct:\n%s", err);
    free(err);
    nrrdNuke(nout);
    free(out_data);
    gageContextNix(ctx);
    nrrdNuke(nin);
    return -1;
  }
  

  free(out_data);
  nrrdNuke(nout);
  gageContextNix(ctx);
  nrrdNuke(nin);
  return 0;

}