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

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


void ScratchPad::configureQuadrilateral(const float gridsz)
{
  VECTOR      bx,by,bz;
  VECTOR      pu,pv,pw;
  VEC2D       vtp0,vtp1;
  float       u,v,d;
  float       xmin,xmax,ymin,ymax,xabs,yabs;
  int         i,j,idx;

  // Initialise the scratchpad
	  
  // Point u in 3D
  VECTORDIFF(*(patch->vertex[1]->point),*(patch->vertex[0]->point),pu);

  // Point v in 3D
  VECTORDIFF(*(patch->vertex[3]->point),*(patch->vertex[0]->point),pv);

  // Point w in 3D
  VECTORDIFF(*(patch->vertex[2]->point),*(patch->vertex[0]->point),pw);

  // Find z vector
  VECTORCROSSPRODUCT(pu,pv,bz);
  VECTORNORMALIZE(bz);

  // Find y vector
  VECTORCOPY(pv,by);
  VECTORNORMALIZE(by);
  
  // Find x vector
  VECTORCROSSPRODUCT(by,bz,bx);
  
  // Transform to a 2D problem

  // points
  TRANSFORMVECTOR(pu,bx,by,du);
  TRANSFORMVECTOR(pv,bx,by,dv);
  TRANSFORMVECTOR(pw,bx,by,dw);
  
  // Rescale the patch for better density estimation
  xmin = MIN(0.0f,MIN(du.u,MIN(dv.u,dw.u)));
  xmax = MAX(0.0f,MAX(du.u,MAX(dv.u,dw.u)));
  xabs = fabs(xmax-xmin);
  
  ymin = MIN(0.0f,MIN(du.v,MIN(dv.v,dw.v)));
  ymax = MAX(0.0f,MAX(du.v,MAX(dv.v,dw.v)));
  yabs = fabs(ymax-ymin);
  
  if (xabs>yabs)
    {
      scalef = xabs/yabs;
      du.v *= scalef;
      dv.v *= scalef;
      dw.v *= scalef;
    }
  else
    {
      scalef = yabs/xabs;
      du.u *= scalef;
      dv.u *= scalef;
      dw.u *= scalef;
    }

  // edges
  VEC2DCOPY(du,cu);
  VEC2DCOPY(dv,cv);
  VEC2DSUBTRACT(dw,du,ct);
  VEC2DSUBTRACT(dw,dv,cs);
  
  // magic edge
  VEC2DSUBTRACT(ct,cv,cw);
  
  // Norm 
  lu = VEC2DNORM(cu);
  lv = VEC2DNORM(cv);
  lt = VEC2DNORM(ct);
  ls = VEC2DNORM(cs);

  // Normalised edges
  VEC2DSCALEINVERSE(lu,cu,nu);
  VEC2DSCALEINVERSE(lv,cv,nv);
  VEC2DSCALEINVERSE(lt,ct,nt);
  VEC2DSCALEINVERSE(ls,cs,ns);

  // Opposite edges 
  VEC2DOPPOSITE(nu,nnu);
  VEC2DOPPOSITE(nv,nnv);
  VEC2DOPPOSITE(ns,nns);
  VEC2DOPPOSITE(nt,nnt);

  // Handle kernel size
  hmin = 0.5 * MIN(MIN(lu,lv),MIN(ls,lt));
  hmax = MAX(MAX(lu,lv),MAX(ls,lt));

  // Choose recursion level
  chooseRecursionLevel(gridsz);

  // Initialize coefficients with position
  d = 1.0f / curnumsubfp;
  v = 0.0f;
  idx = 0;
  for (i=0;i<curnumline;i++)
    {
      u = 0.0f;

      VEC2DSCALE(v,cv,vtp0);
      VEC2DSCALE(v,cw,vtp1);
      VEC2DADD(vtp1,cu,vtp1);

      for (j=0;j<curnumline;j++)
	{
	  VEC2DADDSCALED(vtp0,u,vtp1,elems[idx].data.dde);
	  u += d;
	  idx++;
	}
      v += d;
    }


  ao = 2.0*M_PI/(VEC2DAngle(&nu,&nv));
  au = 2.0*M_PI/(VEC2DAngle(&nt,&nnu));
  av = 2.0*M_PI/(VEC2DAngle(&nnv,&ns));
  aw = 2.0*M_PI/(VEC2DAngle(&nns,&nnt));

  nao = (int) rint(ao) - 1;
  nau = (int) rint(au) - 1;
  nav = (int) rint(av) - 1;
  naw = (int) rint(aw) - 1;
}



void ScratchPad::doReflexQuadrilateral(const VEC2D &orig,const int forbid)
{
  if ( (forbid !=0) && (nrefl<maxrefl) )
    {
      if (kernel->intersectSegment(origin2d,nu,lu,orig))
	{
	  if ((((insideu)&&(reflu>0))||(!insideu)) && (((insideo)&&(reflo>0))||(!insideo)))
	    {
	      VEC2DReflectPointLine(orig,origin2d,nu,refl[nrefl]);
	      nrefl++;
	      if (insideo) reflo--;
	      if (insideu) reflu--;
	      doReflexQuadrilateral(refl[nrefl-1],0);
	    }
	}
    }
  
  if ( (forbid !=1) && (nrefl<maxrefl) )
    {
      if (kernel->intersectSegment(origin2d,nv,lv,orig))
	{
	  if ((((insidev)&&(reflv>0))||(!insidev)) && (((insideo)&&(reflo>0))||(!insideo)))
	    {
	      VEC2DReflectPointLine(orig,origin2d,nv,refl[nrefl]);
	      nrefl++;
	      if (insideo) reflo--;
	      if (insidev) reflv--;
	      doReflexQuadrilateral(refl[nrefl-1],1);
	    }
	}
    }

  if ( (forbid != 2) && (nrefl<maxrefl) )
    {
      if (kernel->intersectSegment(du,nt,lt,orig))
	{
	  if ((((insidew)&&(reflw>0))||(!insidew)) && (((insideu)&&(reflu>0))||(!insideu)))
	    {
	      VEC2DReflectPointLine(orig,du,nt,refl[nrefl]);
	      nrefl++;
	      if (insideu) reflu--;
	      if (insidew) reflw--;
	      doReflexQuadrilateral(refl[nrefl-1],2);
	    }
	}
    }

  if ( (forbid != 3) && (nrefl<maxrefl) )
    {
      if (kernel->intersectSegment(dv,ns,ls,orig))
	{
	  if ((((insidew)&&(reflw>0))||(!insidew)) && (((insidev)&&(reflv>0))||(!insidev)))
	    {
	      VEC2DReflectPointLine(orig,dv,ns,refl[nrefl]);
	      nrefl++;
	      if (insidev) reflv--;
	      if (insidew) reflw--;
	      doReflexQuadrilateral(refl[nrefl-1],3);
	    }
	}
    }
}



int ScratchPad::testAndSetImpact(int idx)
{
  int    i;
  float tp;
  
  // Check kernel
  tp = kernel->evaluate(elems[idx].data.dde,refl[0]);
  if (tp==0.0f)
    return FALSE;
  
  // Accumulate reflections
  for (i=1;i<nrefl;i++)
    tp += kernel->evaluate(elems[idx].data.dde,refl[i]);
      
  // Store everything
  COLORADDSCALED(elems[idx].rad,tp*scalef,curweight,elems[idx].rad);
  
  return TRUE;
}


void ScratchPad::addImpactQuadrilateralAux(int u,int v,int goUp)
{
  int           umin,umax,utp,prevset;
  int           uminold,umaxold;
  int           idxline;
  
  // Find a first vmin
  umin = u-1;
  
  // Find a first vmax
  umax = u+1;
  
  while (umin != umax)
    {
      // Stay within the bounds
      if (umin < 0) 
	umin = 0;
      if (umax > curnumsub)
	umax = curnumsub;
      
      // Find start index of this line
      idxline = v * curnumline;
      
      // Save values of umin and umax
      uminold = umin;
      umaxold = umax;
      
      // Try to minimize umin
      while (umin>=0)
	{
	  if (testAndSetImpact(idxline+umin))
	    umin--;
	  else
	    break;
	}

      // Try to maximize vmax
      while (umax<=curnumsub)
	{
	  if (testAndSetImpact(idxline+umax))
	    umax++;
	  else
	    break;	  
	}
      
      // Fill between umin and umax
      prevset = (umin != uminold);
      utp = uminold+1;
      while (utp<umaxold)
	{
	  if (!testAndSetImpact(idxline+utp))
	    {
	      if (prevset)
		umax = utp;
	      else
		umin = utp;
	    }
	  else
	    prevset = TRUE;
	  utp++;
	}
      
      // Process next line
      if (goUp) v++;
      else      v--;
      
      // Keep everything within the bounds
      if (v<0)          break;
      if (v>curnumsub)  break;      
    }      
}



void ScratchPad::addImpactQuadrilateral(const VEC2D &pos,const COLOR &weight)
{
  double        tp;
  int           u,v;

  // Save weight
  //COLORCOPY(weight,curweight);
  curweight = weight;

  // Find the position of the impact
  tp = pos.u*pos.v;
  VEC2DCOORD(pos.u,cu,pos.v,cv,tp,cw,refl[0]);
  nrefl = 1;

  if (deState.edgeCompensate)
    {
      reflo = nao;
      reflu = nau;
      reflv = nav;
      reflw = naw;
  
      insideo = kernel->isInside(origin2d,refl[0]);
      insideu = kernel->isInside(du,refl[0]);
      insidev = kernel->isInside(dv,refl[0]);
      insidew = kernel->isInside(dw,refl[0]);
  
      doReflexQuadrilateral(refl[0],-1);
    }

  // Find the nearest grid point
  u = (int) rint(pos.u*curnumsubfp);
  v = (int) rint(pos.v*curnumsubfp);

  // Contribute 
  addImpactQuadrilateralAux(u,v,TRUE);
  v--;
  if (v>=0)
    addImpactQuadrilateralAux(u,v,FALSE);
}



void ScratchPad::iterateOutInQuadrilateral(int (ScratchPad::*functocall)(ScratchPadElement&,ScratchPadElement&,ScratchPadElement&),int recursion)
{
  int                  deltax,deltay;
  int                  idx,idxcp;
  int                  i,j,nx,ny;

  // Initialise recursion
  deltay  = (curnumline<<curr);
  deltax  = (1<<curr);
  nx = ny = 1;

  // Recursion
  while (recursion != 0)
    {
      // Subdivide  X
      deltax >>= 1;

      // Go to the first one
      idxcp = deltax;
      
      for (j=0;j<=ny;j++)
	{
	  idx = idxcp;
	  for (i=0;i<nx;i++)
	    {
	      // o-x-o
	      //   ^
	      //   |
	      //   |
	      //   calculate this kind of point.
	      (this->*functocall)(elems[idx-deltax],elems[idx+deltax],elems[idx]);
	      idx += 2*deltax;
	    }
	  idxcp += deltay;
	}
      nx <<= 1;

      // Subdivide Y
      deltay >>= 1;

      // Go to the first one
      idxcp = deltay;

      for (j=0;j<ny;j++)
	{
	  idx = idxcp;
	  for (i=0;i<=nx;i++)
	    {
	      // o
	      // |
	      // x <--- calculate this kind of point.
	      // |
	      // o
	      (this->*functocall)(elems[idx-deltay],elems[idx+deltay],elems[idx]);
	      idx += deltax;
	    }
	  idxcp += 2*deltay;
	}
      ny <<= 1;
      recursion--;
    }
}


void ScratchPad::iterateInOutQuadrilateral(int (ScratchPad::*functocall)(ScratchPadElement&,ScratchPadElement&,ScratchPadElement&))
{
  int                  deltax,deltay;
  int                  numsub;
  int                  idx,idxcp;
  int                  i,j,nx,ny;

  // Initialise recursion
  deltay = curnumline;
  deltax  = 1;
  numsub = curnumsub;
  nx = ny = numsub;

  // Recursion
  while (deltax != numsub)
    {
      // Compress Y
      idxcp = deltay;
      ny >>= 1;

      for (j=0;j<ny;j++)
	{
	  idx = idxcp;
	  for (i=0;i<=nx;i++)
	    {
	      // o
	      // |
	      // x <--- calculate this kind of point.
	      // |
	      // o
	      (this->*functocall)(elems[idx-deltay],elems[idx+deltay],elems[idx]);
	      idx += deltax;
	    }
	  idxcp += 2 * deltay;
	}
      deltay <<= 1;

      // Compress X
      idxcp = deltax;
      nx >>= 1;

      for (j=0;j<=ny;j++)
	{
	  idx = idxcp;
	  for (i=0;i<nx;i++)
	    {
	      // o-x-o
	      //   ^
	      //   |
	      //   |
	      //   calculate this kind of point.
	      (this->*functocall)(elems[idx-deltax],elems[idx+deltax],elems[idx]);
	      idx += 2*deltax;
	    }
	  idxcp += deltay;
	}
      deltax <<= 1; 
    }      
}