#include "flags.h"
#include "field.h"
#include "stack.h"
#include "genrl.h"
#include "fmm.h"
#include <math.h>


void compute_skel_dt(const FIELD<float>* skel,const FIELD<float>* grad,FIELD<float>* skel_dt,float max_dist)
{
   float sk_max=0; int max_i=0,max_j=0,dX=skel->dimX(),dY=skel->dimY(); 

   skel->writePPM("skel.ppm");

   for(int j=0;j<dY;j++)
     for(int i=0;i<dX;i++)
	   if (skel->value(i,j)==1)
	     if (grad->value(i,j)>sk_max) 
	     { max_i=i; max_j=j; sk_max=grad->value(i,j); }

   STACK<Coord> s(500);
   s.Push(Coord(max_i,max_j));
   *skel_dt = 0;

   while (s.Count())				//For all pixels to fill from:
   {
      Coord c = s.Pop();			//Get pixel to fill
      skel_dt->value(c.i,c.j) = 1;
      for(int ii=c.i-1;ii<=c.i+1;++ii)
        for(int jj=c.j-1;jj<=c.j+1;++jj)
          if ((ii!=c.i || jj!=c.j) && skel->value(ii,jj)==1 && skel_dt->value(ii,jj)!=1)
 	    s.Push(Coord(ii,jj));
   }
   
   skel_dt->writePPM("skel.ppm");

   FLAGS flags(*skel_dt,-0.9,true);
   FastMarchingMethod fmm(skel_dt,&flags,10000000);
   int nfail,nextr;			
   fmm.execute(nfail,nextr,max_dist);
}


void compute_skel_dt_interpolation(const FIELD<float>* dt,const FIELD<float>* skel_dt,FIELD<float>* interp)
{
   int dX = dt->dimX(),dY=dt->dimY();

   for(int j=0;j<dY;j++)
     for(int i=0;i<dX;i++)
     {
	 float T = dt->value(i,j);			//compute smooth interpolation between dt and skel_dt
	 float D = skel_dt->value(i,j);			//(take care that both dt,skel_dt may be 0)
	 float B = ((D)? min(T/2/D,0.5):0.5) + 0.5*((T)? max(1-D/T,0):0);
	 interp->value(i,j)= 1 - pow(B,0.5);
     }
}
