#include <math.h>
#include <stdlib.h>
#include <string.h>

#include "ScratchPad.h"
#include "Common.h"
#include "PerceivedLuminance.h"




// ============================= PUBLIC METHODS ============================= 


ScratchPad::ScratchPad(const int recursion)
{
  // Verify precondition when debugging
  DebugAssert(recursion>=0,"The recursion level is a >=0 number");
  
  // Initialise maximum recursion level
  maxr = recursion;
  maxnumsub = 1<<recursion;
  maxnumline = maxnumsub+1;
  maxnumcoef = maxnumline*maxnumline;

  // Memory allocation
  idxs = new int[maxnumcoef];
  elems = new ScratchPadElement[maxnumcoef];

  // Alloc a Epanechnikov kernel by default
  kernel = new EpanechnikovKernel();
}


ScratchPad::~ScratchPad()
{
  // Delete element array
  if (elems!=NULL)
    delete elems;

  // Delete index array
  if (idxs!=NULL)
    delete idxs;
}



void ScratchPad::configure(PATCH* p,const float gridsz)
{
  // Verify precondition when debugging
  DebugAssert(p!=NULL,"The patch must be non NULL");
  
  // Clear contents.
  clear();

  // Save patch informations
  basicConfigurePatch(p);

  // Patch specific initialisation.
  if (issquare)
    configureQuadrilateral(gridsz);
  else
    configureTriangular(gridsz);
}


void ScratchPad::chooseRecursionLevel(const float gridsz)
{
  int tpr;

  tpr = (gridsz==0.0) ? maxr : (int) ceil( log(hmax/(gridsz*sqrt(scalef))) / log(2.0) );
  if (tpr>maxr)
    tpr = maxr;
  if (tpr<0)
    tpr = 0;

  printf("Recursion level choosen : %i\n",tpr);

  basicConfigureRecursionLevel(tpr);
}


void ScratchPad::setKernel(Kernel *k)
{
  // Verify precondition when debugging
  DebugAssert(k!=NULL,"The kernel must be non NULL");

  // Delete old kernel before
  if (kernel != NULL)
    delete kernel;

  // Assign the new kernel
  kernel = k;
}

 
void ScratchPad::setKernelSize(const float hsz)
{
  // Verify precondition when debugging
  DebugAssert(hsz>0.0f,"The kernel size is a stricly positive number");
  
  kernel->setSize(hsz*sqrt(scalef));
}


void ScratchPad::setBaseRadiance(const COLOR &base)
{
  //COLORCOPY(base,accum);
  accum = base;
}


void ScratchPad::addImpact(const VEC2D &pos,const COLOR &weight)
{
  // Special processing for large kernels.
  if (deState.largeKernel)
    {
      // Do not take kernels too big
      if (kernel->geth()>hmin)
	{
	  COLORADDSCALED(accum,invarea,weight,accum);
	  return;
	}
    }

  // Initialise reflections
  nrefl = 0;

  // Usual density estimation.
  if (issquare)
    addImpactQuadrilateral(pos,weight);
  else
    addImpactTriangular(pos,weight);
}


DiffuseCompressedSurface* ScratchPad::constructDiffuseCompressedSurface(const float t)
{
  CoefficientStore             *cs;
  DiffuseCompressedSurface     *dcs;

  cs = constructCoefficientStore(t);
  dcs = (cs==NULL) ? (DiffuseCompressedSurface*)NULL : new DiffuseCompressedSurface(patch,curr,cs);
  return dcs;
}


void ScratchPad::display(CoefficientStore *cs)
{
  COLOR                *next;
  VECTOR               v[3];
  RGB                  c[3];
  int                  deltaline,deltacol,deltashear,deltaaux,deltax,deltay;
  int                  idx,idxcp;
  int                  i,j,aux;


  aux = curnumline*curnumsub;
  coefstream.setCoefficientStore(cs);
  coefstream.startRead();

  if (issquare)
    {
      next = coefstream.nextRead();
      elems[0].rad = *next; //COLORCOPY(*next,elems[0].rad);
      RadianceToRGB(elems[0].rad,&(elems[0].rgb));
      VECTORCOPY(*(patch->vertex[0]->point),elems[0].data.dd); 

      next = coefstream.nextRead();
      elems[curnumsub].rad = *next; //COLORCOPY(*next,elems[curnumsub].rad);
      RadianceToRGB(elems[curnumsub].rad,&(elems[curnumsub].rgb));
      VECTORCOPY(*(patch->vertex[1]->point),elems[curnumsub].data.dd); 

      next = coefstream.nextRead();
      elems[aux].rad = *next; //COLORCOPY(*next,elems[aux].rad);
      RadianceToRGB(elems[aux].rad,&(elems[aux].rgb));
      VECTORCOPY(*(patch->vertex[3]->point),elems[aux].data.dd); 

      next = coefstream.nextRead();
      elems[aux+curnumsub].rad = *next; 
                              //COLORCOPY(*next,elems[aux+curnumsub].rad);
      RadianceToRGB(elems[aux+curnumsub].rad,&(elems[aux+curnumsub].rgb));
      VECTORCOPY(*(patch->vertex[2]->point),elems[aux+curnumsub].data.dd); 
    }
    else
    {
      next = coefstream.nextRead();
      elems[0].rad = *next; //COLORCOPY(*next,elems[0].rad);
      RadianceToRGB(elems[0].rad,&(elems[0].rgb));
      VECTORCOPY(*(patch->vertex[1]->point),elems[0].data.dd); 

      next = coefstream.nextRead();
      elems[aux].rad = *next; //COLORCOPY(*next,elems[aux].rad);
      RadianceToRGB(elems[aux].rad,&(elems[aux].rgb));
      VECTORCOPY(*(patch->vertex[0]->point),elems[aux].data.dd); 

      next = coefstream.nextRead();
      elems[aux+curnumsub].rad = *next; 
                              //COLORCOPY(*next,elems[aux+curnumsub].rad);
      RadianceToRGB(elems[aux+curnumsub].rad,&(elems[aux+curnumsub].rgb));
      VECTORCOPY(*(patch->vertex[2]->point),elems[aux+curnumsub].data.dd); 
    }

  iterateOutIn(&calculateCoefficient2,curr);

  if (issquare)
    {
      deltay = curnumline;
      deltaaux = deltay+1;
      deltax  = (1<<curr);
      idx = 0;

      for (i=0;i<deltax;i++)
	{
	  idxcp = idx;
	  for (j=0;j<deltax;j++)
	    {
	      VECTORCOPY(elems[idxcp].data.dd,v[0]);
	      VECTORCOPY(elems[idxcp+deltaaux].data.dd,v[1]);
	      VECTORCOPY(elems[idxcp+deltay].data.dd,v[2]);
	      RGBCOPY(elems[idxcp].rgb,c[0]);
	      RGBCOPY(elems[idxcp+deltaaux].rgb,c[1]);
	      RGBCOPY(elems[idxcp+deltay].rgb,c[2]);
	      RenderPolygonGouraud(3,v,c);
	      
	      VECTORCOPY(elems[idxcp+1].data.dd,v[0]);
	      VECTORCOPY(elems[idxcp+deltaaux].data.dd,v[1]);
	      VECTORCOPY(elems[idxcp].data.dd,v[2]);
	      RGBCOPY(elems[idxcp+1].rgb,c[0]);
	      RGBCOPY(elems[idxcp+deltaaux].rgb,c[1]);
	      RGBCOPY(elems[idxcp].rgb,c[2]);
	      RenderPolygonGouraud(3,v,c);
	      
	      idxcp++;
	    }
	  idx += deltay;
	}
    }
  else
    {

      deltaline = curnumline;
      deltashear = deltaline+1;
      deltacol  = curnumsub;
      idx = 0;
      
      for (i=0;i<deltacol;i++)
	{
	  idxcp = idx;
	  for (j=0;j<i;j++)
	    {
	      VECTORCOPY(elems[idxcp].data.dd,v[0]);
	      VECTORCOPY(elems[idxcp+deltashear].data.dd,v[1]);
	      VECTORCOPY(elems[idxcp+deltaline].data.dd,v[2]);
	      RGBCOPY(elems[idxcp].rgb,c[0]);
	      RGBCOPY(elems[idxcp+deltashear].rgb,c[1]);
	      RGBCOPY(elems[idxcp+deltaline].rgb,c[2]);
	      RenderPolygonGouraud(3,v,c);

	      VECTORCOPY(elems[idxcp+1].data.dd,v[0]);
	      VECTORCOPY(elems[idxcp+deltashear].data.dd,v[1]);
	      VECTORCOPY(elems[idxcp].data.dd,v[2]);
	      RGBCOPY(elems[idxcp+1].rgb,c[0]);
	      RGBCOPY(elems[idxcp+deltashear].rgb,c[1]);
	      RGBCOPY(elems[idxcp].rgb,c[2]);
	      RenderPolygonGouraud(3,v,c);

	      idxcp++;
	    }
      
	  VECTORCOPY(elems[idxcp].data.dd,v[0]);
	  VECTORCOPY(elems[idxcp+deltashear].data.dd,v[1]);
	  VECTORCOPY(elems[idxcp+deltaline].data.dd,v[2]);
	  RGBCOPY(elems[idxcp].rgb,c[0]);
	  RGBCOPY(elems[idxcp+deltashear].rgb,c[1]);
	  RGBCOPY(elems[idxcp+deltaline].rgb,c[2]);
	  RenderPolygonGouraud(3,v,c);
	  
	  idx += deltaline;
	}
    }
}


void ScratchPad::print()
{
  int i;

  // Print some general information
  fprintf(stderr,"SCRATCHPAD :\n");
  fprintf(stderr,"Maximum subdivision level : %i\n",maxr);
  fprintf(stderr,"Current subdivision level : %i\n",curr);
  fprintf(stderr,"Current patch id : %i\n",patch->id);
  fprintf(stderr,"Current patch type : %s\n", issquare ? "square" : "triangle");

  // Print coefficients
  for (i=0;i<curnumcoef;i++)
    {
      fprintf(stderr,"\nCoefficient %i :",i);
      ColorPrint(stderr,elems[i].rad);
    }

  // Finish cleanly
  fprintf(stderr,"\n");
}


void ScratchPad::add(const ScratchPad *toadd,const COLOR& c)
{
  COLOR tp;
  int   i,j,idx;
  
  i=0;

  if (issquare)
    {
      for(i=0;i<curnumcoef;i++)
	{
	  COLORPROD(toadd->elems[i].rad,c,tp);
	  COLORADD(elems[i].rad,tp,elems[i].rad);
	}
    }
  else
    {
      idx = 0;
      for (i=0;i<curnumline;i++)
	{
	  for (j=0;j<=i;j++)
	    {
	      COLORPROD(toadd->elems[idx+j].rad,c,tp);
	      COLORADD(elems[idx+j].rad,tp,elems[idx+j].rad);
	    }
	  idx += curnumline;
	}
    }
}



// ============================= PRIVATE METHODS ============================= 


void ScratchPad::basicConfigure(PATCH* p,const int recursion)
{
  basicConfigurePatch(p);
  basicConfigureRecursionLevel(recursion);
}


void ScratchPad::basicConfigurePatch(PATCH *p)
{
  // Save patch.
  patch = p;
  issquare = (p->nrvertices==4);
  invarea  = 1.0f / patch->area;
}


void ScratchPad::basicConfigureRecursionLevel(const int recursion)
{
  // Recursion level info.
  curr = recursion;
  curnumsub = 1<<recursion;
  curnumline = curnumsub+1;
  curnumcoef = issquare ? curnumline*curnumline : (((curnumline+1)*curnumline)>>1);
  curnumsubfp = (float)curnumsub;
}


void ScratchPad::clear()
{
  // Clear ScratchPadElement array contents
  memset(elems,0,maxnumcoef*sizeof(ScratchPadElement));

  // Clear accumulation buffer
  COLORSETMONOCHROME(accum,0.0f);
}



void ScratchPad::addAccum()
{
  int              i,j,idx;

  if (issquare)
    {
      for(i=0;i<curnumcoef;i++)
	{
	  COLORADD(elems[i].rad,accum,elems[i].rad);
	}
    }
  else
    {
      idx = 0;
      for(i=0;i<curnumline;i++)
	{
	  for (j=0;j<=i;j++)
	    {
	      COLORADD(elems[idx+j].rad,accum,elems[idx+j].rad);
	    }
	  idx += curnumline;
	}
    }     
}



CoefficientStore* ScratchPad::constructCoefficientStore(const float t)
{
  CoefficientStore *cs = NULL;
  int              nc;
  int              aux;

  // Take accumulated power into account
  addAccum();

  // Copy coefficients
  aux = curnumline*curnumsub;

  //COLORCOPY(elems[0].rad,elems[0].data.drc);
  elems[0].data.drc = elems[0].rad;
  elems[0].amc = perceivedLuminance(elems[0].rad);

  if (issquare)
    {
      //COLORCOPY(elems[curnumsub].rad,elems[curnumsub].data.drc);
      elems[curnumsub].data.drc = elems[curnumsub].rad;
      elems[curnumsub].amc = perceivedLuminance(elems[curnumsub].rad);
    }

  //COLORCOPY(elems[aux].rad,elems[aux].data.drc);
  elems[aux].data.drc = elems[aux].rad;
  elems[aux].amc = perceivedLuminance(elems[aux].rad);

  //COLORCOPY(elems[aux+curnumsub].rad,elems[aux+curnumsub].data.drc);
  elems[aux+curnumsub].data.drc = elems[aux+curnumsub].rad;
  elems[aux+curnumsub].amc = perceivedLuminance(elems[aux+curnumsub].rad);

  // Find wavelet scale coefficients
  findWaveletCoefficients();

  // Simplify wavelet coefficients
  nc = simplifyWaveletCoefficients(t);

  // Construct the coefficient strore
  cs = new CoefficientStore(curnumcoef,nc);
  
  coefstream.setCoefficientStore(cs);

  coefstream.startWrite();

  writeCoefficient(elems[0]);
  if (issquare)
    writeCoefficient(elems[curnumsub]);
  writeCoefficient(elems[aux]);
  writeCoefficient(elems[aux+curnumsub]);
  
  iterateOutIn(&storeCoefficient,curr);
  
  coefstream.endWrite();

  return cs;
}


void ScratchPad::iterateOutIn(int (ScratchPad::*functocall)(ScratchPadElement&,ScratchPadElement&,ScratchPadElement&),int recursion)
{
  // Recursion level is -1 --> maximum recursion level
  if (recursion==-1)
    recursion = curr;
  
  if (issquare)
    iterateOutInQuadrilateral(functocall,recursion);
  else
    iterateOutInTriangular(functocall,recursion);
}


void ScratchPad::iterateInOut(int (ScratchPad::*functocall)(ScratchPadElement&,ScratchPadElement&,ScratchPadElement&))
{
  if (issquare)
    iterateInOutQuadrilateral(functocall);
  else
    iterateInOutTriangular(functocall);
}


int ScratchPad::calculateCoefficient(ScratchPadElement &left,ScratchPadElement &right,ScratchPadElement &middle)
{
  float lpl,rpl,mpl;
  COLOR tp;
  
  COLORADD(left.rad,right.rad,tp);
  COLORADDSCALED(middle.rad,-0.5f,tp,middle.data.drc);

  lpl = perceivedLuminance(left.rad);
  rpl = perceivedLuminance(right.rad);
  mpl = perceivedLuminance(middle.rad);

  middle.amc = fabs(mpl-(0.5f*(lpl+rpl)));
  return TRUE;
}

int ScratchPad::calculateCoefficient2(ScratchPadElement &left,ScratchPadElement &right,ScratchPadElement &middle)
{
  COLOR *next;
  COLOR tp;

  next = coefstream.nextRead();
  COLORADD(left.rad,right.rad,tp);
  COLORADDSCALED(*next,0.5f,tp,middle.rad);
  RadianceToRGB(middle.rad,&(middle.rgb));
  MIDPOINT(left.data.dd,right.data.dd,middle.data.dd);
  return TRUE;
}



void ScratchPad::findWaveletCoefficients()
{
  iterateInOut(&calculateCoefficient);
}



int ScratchPad::simplifyWaveletCoefficients(const float threshold)
{
  int   i,j,idx,nnc;
  
  return curnumcoef;

  nnc = 0;
  
  if (issquare)
    {
      for(idx=0;idx<curnumcoef;idx++)
	{
	  if (elems[idx].amc < threshold)
	    elems[idx].amc = 0.0f;
	  else
	    nnc++;
	}
    }
  else
    {
      idx = 0;
      for (i=0;i<curnumline;i++)
	{
	  for (j=0;j<=i;j++)
	    {
	      if (elems[idx+j].amc < threshold)
		elems[idx+j].amc = 0.0f;
	      else
		nnc++;
	    }
	  idx += curnumline;
	}
    }
    
  return nnc;
}


int ScratchPad::storeCoefficient(ScratchPadElement&,ScratchPadElement&,ScratchPadElement &middle)
{
  writeCoefficient(middle);
  return TRUE;
}

void ScratchPad::writeCoefficient(ScratchPadElement &spe)
{
  if (spe.amc!=0.0)
    coefstream.nextWrite(&spe.data.drc); 
  else
    coefstream.skipWrite();
}