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

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


void ScratchPad::configureTriangular(const float gridsz)
{
  double      temp;
  double      lu2,lv2,lw2;
  VECTOR      bx,by,bz;
  VECTOR      lcu,lcv;
  float       xmin,xmax,ymin,ymax,xabs,yabs;

  // Initialise the scratchpad
	  
  // Edge u in 3D
  VECTORDIFF(*(patch->vertex[1]->point),*(patch->vertex[0]->point),bx);
  VECTORCOPY(bx,lcu);

  // Edge v in 3D
  VECTORDIFF(*(patch->vertex[2]->point),*(patch->vertex[0]->point),by);
  VECTORCOPY(by,lcv);

  // Find z vector
  VECTORCROSSPRODUCT(bx,by,bz);
  VECTORNORMALIZE(bz);

  // Find y vector
  VECTORNORMALIZE(by);

  // Find x vector
  VECTORCROSSPRODUCT(by,bz,bx);
  
  // Transform to a 2D problem
  TRANSFORMVECTOR(lcu,bx,by,cu);
  TRANSFORMVECTOR(lcv,bx,by,cv);

  // Rescale the patch for better density estimation
  xmin = MIN(0.0f,MIN(cu.u,cv.u));
  xmax = MAX(0.0f,MAX(cu.u,cv.u));
  xabs = fabs(xmax-xmin);
  
  ymin = MIN(0.0f,MIN(cu.v,cv.v));
  ymax = MAX(0.0f,MAX(cu.v,cv.v));
  yabs = fabs(ymax-ymin);
  
  if (xabs>yabs)
    {
      scalef = xabs/yabs;
      cu.v *= scalef;
      cv.v *= scalef;
    }
  else
    {
      scalef = yabs/xabs;;
      cu.u *= scalef;
      cv.u *= scalef;
    }

  VEC2DDIFF(cu,cv,cw);
  
  // Squared norm
  lu2 = VEC2DNORM2(cu);
  lv2 = VEC2DNORM2(cv);
  lw2 = VEC2DNORM2(cw);
  
  // Norm
  lu = sqrt(lu2);
  lv = sqrt(lv2);
  lw = sqrt(lw2);

  // Calculate altitude length
  temp = (lu2+lv2-lw2)/(2.0*lu);
  ihu = 1.0 / sqrt(lv2-(temp*temp));
      
  temp = (lv2+lw2-lu2)/(2.0*lv);
  ihv = 1.0 / sqrt(lw2-(temp*temp));
  
  temp = (lw2+lu2-lv2)/(2.0*lw);
  ihw = 1.0 / sqrt(lu2-(temp*temp));  
  
  // Find length
  hmin = 0.5 * MIN(lu,MIN(lv,lw));
  hmax = MAX(lu,MAX(lv,lw));

  // Choose recursion level
  chooseRecursionLevel(gridsz);

  // Scale according to recursion level
  VEC2DSCALEINVERSE(curnumsubfp,cu,du);
  VEC2DSCALEINVERSE(curnumsubfp,cv,dv);
  VEC2DSCALEINVERSE(curnumsubfp,cw,dw);

  // Record normalised edges
  VEC2DSCALEINVERSE(lu,cu,nu);
  VEC2DSCALEINVERSE(lv,cv,nv);
  VEC2DSCALEINVERSE(lw,cw,nw);
  
  // Record opposite of normalised edges
  VEC2DOPPOSITE(nu,nnu);
  VEC2DOPPOSITE(nv,nnv);
  VEC2DOPPOSITE(nw,nnw);


  ao = 2.0*M_PI / (VEC2DAngle(&nu,&nv));
  au = 2.0*M_PI / (VEC2DAngle(&nnw,&nnu));
  av = 2.0*M_PI / (VEC2DAngle(&nnv,&nw));

  nao = (int)floor(ao) - 1;
  nau = (int)floor(au) - 1;
  nav = (int)floor(av) - 1;

}


void ScratchPad::doReflexTriangular(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--;
	      doReflexTriangular(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--;
	      doReflexTriangular(refl[nrefl-1],1);
	    }
	}
    }

  if ((forbid != 2) && (nrefl<maxrefl))
    {
      if (kernel->intersectSegment(cu,nnw,lw,orig))
	{
	  if ((((insidev)&&(reflv>0))||(!insidev)) && (((insideu)&&(reflu>0))||(!insideu)))
	    {
	      VEC2DReflectPointLine(orig,cu,nw,refl[nrefl]);
	      nrefl++;
	      if (insideu) reflu--;
	      if (insidev) reflv--;
	      doReflexTriangular(refl[nrefl-1],2);
	    }
	}
    }
}



void ScratchPad::addImpactTriangular(const VEC2D &pos,const COLOR &weight)
{
  int                  ipos;
  int                  u,v,idx,l;
  int                  umin,umax,vmin,vmax,wmin,wmax,vmintp,vmaxtp;
  double               wi,tp;
  double               utmin,utmax,vtmin,vtmax,wtmin,wtmax;
  float                ksum;
  VEC2D                delta;
  COLOR                *curcol;

  // Calculate impact position
  VEC2DCOMB2(pos.u,cu,pos.v,cv,refl[0]);
  nrefl++;

  if (deState.edgeCompensate)
    {
      reflo = nao;
      reflu = nau;
      reflv = nav;
  
      insideo = kernel->isInside(origin2d,refl[0]);
      insideu = kernel->isInside(cu,refl[0]);
      insidev = kernel->isInside(cv,refl[0]);

      doReflexTriangular(refl[0],-1);
    }

  // Calculate bounding area
  tp = kernel->geth()*ihv;
  utmin = pos.u-tp;
  utmax = pos.u+tp;

  tp = kernel->geth()*ihu;
  vtmin = pos.v-tp;
  vtmax = pos.v+tp;

  tp = kernel->geth()*ihw;
  wi = 1.0-pos.u-pos.v;
  wtmin = wi-tp;
  wtmax = wi+tp;
  
  // Adjust boundaries (also do the clipping)
  if (utmin<0.0)       umin = 0;
  else                 umin = (int)floor(utmin*curnumsubfp)+1;

  if (utmax>1.0)       umax = curnumsub;
  else                 umax = (int)ceil(utmax*curnumsubfp)-1;

  if (vtmin<0.0)       vmin = 0;
  else                 vmin = (int)floor(vtmin*curnumsubfp)+1;

  if (vtmax>1.0)       vmax = curnumsub;
  else                 vmax = (int)ceil(vtmax*curnumsubfp)-1;

  if (wtmin<0.0)       wmin = 0;
  else                 wmin = (int)floor(wtmin*curnumsubfp)+1;

  if (wtmax>1.0)       wmax = curnumsub;
  else                 wmax = (int)ceil(wtmax*curnumsubfp)-1;


  // Line per line search
  l = curnumsub-umin;
  idx = l*curnumline;

  for (u=umin;u<=umax;u++)
    {
      // Minimum v for this line
      vmintp = l - wmax;
      if (vmintp<vmin)
	vmintp = vmin;
      
      // Maximum v for this line
      vmaxtp = l - wmin;
      if (vmaxtp>vmax)
	vmaxtp = vmax;
      if (vmaxtp>l)
	vmaxtp = l;
      
      // Column per column
      for (v=vmintp;v<=vmaxtp;v++)
	{
	  curcol = &(elems[idx+v].rad);
	  VEC2DCOMB2(u,du,v,dv,delta);
	  
	  // Sum all contributions
	  ksum = 0.0f;
	  for (ipos=0;ipos<nrefl;ipos++)
	    ksum += kernel->evaluate(refl[ipos],delta);

	  // Update the current color
	  if (ksum>EPSILON)
	    {
	      COLORADDSCALED(*curcol,ksum*scalef,weight,*curcol);
	    }
	}
      
      // One line up
      idx -= curnumline;
      l--;
    }
}


void ScratchPad::iterateOutInTriangular(int (ScratchPad::*functocall)(ScratchPadElement&,ScratchPadElement&,ScratchPadElement&),int recursion)
{
  int                  deltaline,deltacol,deltashear;
  int                  idx,idxcp;
  int                  ln,l,j,i;
  
  // Prepare for recursion
  deltaline = (curnumline*curnumsub)>>1;
  deltacol  = (curnumsub)>>1;
  ln = 1;
  
  // Recursion
  while (recursion != 0)
    {
      // Skip the first line
      idxcp = deltaline;
      l = 1;
      deltashear = deltaline+deltacol;
      
      // Visit lines 2 by 2
      for (j=0;j<ln;j++)
	{
	  idx = idxcp;
	  
	  // Visit an odd line
	  for (i=0;i<l;i++)
	    {
	      // o
	      // |
	      // x <--- calculate this kind of point.
	      // |
	      // o
	      (this->*functocall)(elems[idx-deltaline],elems[idx+deltaline],elems[idx]);
	      idx += deltacol;
	      
     /*	      // o
	      //  \
	      //   x <-- calculate this kind of point.
	      //    \
     	      //     o
     */	      (this->*functocall)(elems[idx-deltashear],elems[idx+deltashear],elems[idx]);
	      idx += deltacol;
	    }
	      
	  idxcp += deltaline;
	  idx = idxcp+deltacol;

	  // Visit an even line
	  for (i=0;i<l;i++)
	    {
	      // o-x-o
	      //   ^
	      //   |
	      //   |
	      //   calculate this kind of point.
	      (this->*functocall)(elems[idx-deltacol],elems[idx+deltacol],elems[idx]);
	      idx += 2*deltacol;
	    }

	  idxcp += deltaline;
	  l++;
	}

      deltacol >>= 1;
      deltaline >>= 1;
      ln <<= 1;
      recursion--;
    }
}


void ScratchPad::iterateInOutTriangular(int (ScratchPad::*functocall)(ScratchPadElement&,ScratchPadElement&,ScratchPadElement&))
{
  int                  deltaline,deltacol,deltashear;
  int                  numsub;
  int                  idx,idxcp;
  int                  ln,l,j,i;

  // Initialisation
  deltaline = curnumline;
  deltacol  = 1;
  
  numsub = (1<<curr);
  ln = (numsub>>1);

  // Recursion
  while (deltacol != numsub)
    {
      // Skip the first line
      idxcp = deltaline;
      l = 1;
      deltashear = deltaline+deltacol;

      // Visit lines 2 by 2
      for (j=0;j<ln;j++)
	{
	  idx = idxcp;
	  
	  // Visit an odd line
	  for (i=0;i<l;i++)
	    {
	      // o
	      // |
	      // x <--- calculate this kind of point.
	      // |
	      // o
	      (this->*functocall)(elems[idx-deltaline],elems[idx+deltaline],elems[idx]);
	      idx += deltacol;

     /*	      // o
	      //  \
	      //   x <-- calculate this kind of point.
	      //    \
	      //     o
     */      (this->*functocall)(elems[idx-deltashear],elems[idx+deltashear],elems[idx]);
	      idx += deltacol;
	    }

	  idxcp += deltaline;
	  idx = idxcp+deltacol;

	  // Visit an even line
	  for (i=0;i<l;i++)
	    {
	      // o-x-o
	      //   ^
	      //   |
	      //   |
	      //   calculate this kind of point.
	      (this->*functocall)(elems[idx-deltacol],elems[idx+deltacol],elems[idx]);
	      idx += 2*deltacol;
	    }

	  idxcp += deltaline;
	  l++;
	}
      
      deltacol <<= 1;
      deltaline <<= 1;
      ln >>= 1;
    }      
}