//Header File for Image Processing library.
//By Abhishek Tiwari,Himanshu Arora & Tirthankar Bandyopadhyay
//Last Modified on 18-06-99 12:00pm

#include <windows.h>
#include <mil.h>
#include <math.h>
#include <time.h>

#define PI 3.14159
#define vel 0.088
clock_t start,finish;
double duration;

//Set various parameters
#define Image_X 320
#define Image_Y 240
#define Zdanger .1
#define zref 215
#define Zfar .5
#define Ythresh1 5
#define Ythresh2 15
#define Xthresh 15


#define Threshold 100
#define Smooth 0
#define Sharpen 6
#define Edge_Horiz 1
#define Edge_Vert 2
#define Edge 3
#define Edge2 7
#define Laplacian_Edge 5

#define Low_Pass1 8
#define Low_Pass2 9
#define Low_Pass3 10
#define Low_Pass4 11
#define High_Pass1 12
#define High_Pass2 13
#define High_Pass3 14
#define Median 15
#define six 6
#define eight 8



//Declarations for the variables.
BYTE array[Image_Y][Image_X],array1[Image_Y][Image_X],array2[Image_Y][Image_X];
BYTE array3[Image_Y][Image_X];
int i,j,x,y,s,t,k,l,a,b,v,w,p;
int c=0;
int r=0;
int q=0;
int sort[81];
		int a;
        float t1,t2,tcon[Image_Y][Image_X];
		float XOL[40];XOR[40],ZOL[40],ZOR[40],X[40][40],Z[40][40],Y[40][40];
		float XL,XR,ZL,ZR;
        BYTE depth[Image_Y][Image_X];
		char ccc;
/*int flowx[40][40];
int flowy[40][40];*/
//Declarations for the functions.
void Binarize(MIL_ID,MIL_ID);
void Convolve(float,float,int);
void Canny_Edge(MIL_ID,MIL_ID,float,int);
float Gauss(double,double);
float DerGauss(double,double);
void Filter(MIL_ID,MIL_ID,int,int);
int Bubble_Sort(int,int,int);
void swap(int*,int*);
void Erode(MIL_ID,MIL_ID,int,int);
void Dilate(MIL_ID,MIL_ID,int,int);
void Open(MIL_ID,int,int,int);
void Close(MIL_ID,int,int,int);
void Opticflow(MIL_ID,MIL_ID,int);

//Original functions definitions.

//Optical Flow Field Estimation on a grayscale image
void Opticflow(MIL_ID SrcBuffer1,MIL_ID SrcBuffer2,MIL_ID SrcBuffer3,int n,float tim)
{
    int mask0[3][3]; 
	float t1,t2,tcon[Image_Y][Image_X],dep[40][40],dep1[40][40];
        BYTE depth[Image_Y][Image_X];
		char ccc;
	printf("TIME = %f\n", tim);
	MbufGet(SrcBuffer1,array2);
	MbufGet(SrcBuffer2,array3);
	mask0[0][0]=1;                      /*defining the mask for convolution */
	mask0[0][2]=1;                      /*    to soomthen the depth plot    */
	mask0[2][0]=1;
	mask0[2][2]=1;
	mask0[0][1]=2;
	mask0[1][0]=2;
	mask0[1][2]=2;
	mask0[2][1]=2;
	mask0[1][1]=4;
       
        /* loop to calculate the optical flow and the depht as depicted by the flow fields */
	
	for(i=six;i+six<Image_Y-50;i=i+six)
	{
	  for(j=eight;j+eight<Image_X;j=j+eight)
	  {
            q=1000000;

	    /* center first search for the best match */
	    
            for(w=0;w<n+1;w++)
            {
  	      for(v=-w;v<w+1;v++)
	      {
                p=0;
                for(k=i;k<i+six;k++)
		{
		  for(l=j;l<j+eight;l++)
		  {
                    p=p+(array2[k][l]-array3[k+w][l+v])*(array2[k][l]-array3[k+w][l+v]);
		  }
		}
 		if(q>p)
		{
		  q=p;
		  s=w;
		  t=v;
		}
		p=0;
                for(k=i;k<i+six;k++)
		{
		  for(l=j;l<j+eight;l++)
		  {
                    p=p+(array2[k][l]-array3[k-w][l+v])*(array2[k][l]-array3[k-w][l+v]);
		  }
		}
 		if(q>p)
		{
		  q=p;
		  s=-w;
		  t=v;
		}
		p=0;
                for(k=i;k<i+six;k++)
		{
		  for(l=j;l<j+eight;l++)
		  {
                    p=p+(array2[k][l]-array3[k+v][l+w])*(array2[k][l]-array3[k+v][l+w]);
		  }
		}
 		if(q>p)
		{
		  q=p;
	 	  s=v;
		  t=w;
		}
		p=0;
                for(k=i;k<i+six;k++)
		{
		  for(l=j;l<j+eight;l++)
		  {
                    p=p+(array2[k][l]-array3[k+v][l-w])*(array2[k][l]-array3[k+v][l-w]);
		  }
		}
 		if(q>p)
		{
		  q=p;
		  s=v;
		  t=-w;
		}
	      }
	    }

	    /* optical flow fields are stored in s and t */
	    
	    if(s==0)
	    {
	      if(t==0)                                  /* finding the time to contact */ 
	        tcon[i][j]=20;                          /* and the depth               */
	      else
		tcon[i][j]=abs((j-160)*tim/t);
	    }
	    else
	    {
	      if(t==0)
		tcon[i][j]=abs((i-120)*tim/s);
	      else
		tcon[i][j]=(abs((i-120)*tim/s)+abs((j-160)*tim/t))/2;
	    }
            dep[i/6][j/8]= tcon[i][j]*vel ;
	    if (dep[i/6][j/8]>1.76)
	      dep[i/6][j/8]=1.76;
            MgraLine(M_DEFAULT,SrcBuffer1,j+3,i+2,j+3+t,i+2+s);      /* displaying flow fields */
            //MgraArc(M_DEFAULT,SrcBuffer1, j+3+t,i+2+s,1,1,0,360);  /*    on the image        */
	  }
	}
	
	/* loop to smoothen the depth plot and find the real world */
	/* coordinates for points in the image */
	
	for (i=1; i<39; i++)
	{
	  for (j=1; j<39; j++)
	  {
	    dep1[i][j]=0;
	    for (y=-1; y<=1; y++)
	    {
	      for (x=-1; x<=1; x++)
	        dep1[i][j] = dep1[i][j] + mask0[y+1][x+1]*dep[i+y][j+x];
	    }
	    dep1[i][j]=dep1[i][j]/16;
	    Y[i][j] = dep1[i][j]*(120-i*6)*100/zref;

	    /* deciding if the current point is an obstacle       */
	    /* and storing its real world coordinates in an array */ 
	    
	    if((Y[i][j]>Ythresh1) && (Y[i][j]<Ythresh2))
	    {
	      X[i][j] = dep1[i][j]*(j*8-160)*100/zref;
	      printf("%d %d %d  ",i,j,X[i][j]);
	      if ((dep1[i][j]<Zfar) && (dep1[i][j-1]>Zfar))
	      {
	        XOL[a] = X[i][j];
	        ZOL[a] = dep1[i][j];
	        a++;
	      }
	      if ((dep1[i][j]>Zfar) && (dep1[i][j-1]<Zfar) /*&& (abs(X[i][j])<Xthresh)*/)
	      {
	        XOR[b]=X[i][j];
	        ZOR[b]=dep1[i][j];
	        b++;
	      }
	      if ((b-a)>4)
	        XOL[a]=-30;
	      if ((b-a)<-4)
	        XOR[b]=30;
	    }
	  }
	}

	/* loop to output the depth plot as an image*/
	
	for(i=six;i+six<Image_Y-50;i=i+six)
	{
	  for(j=eight;j+eight<Image_X;j=j+eight)
	  {
            if(dep1[i/6][j/8]>1.76)
              depth[i][j]=255;
	    else
	      depth[i][j]=dep1[i/6][j/8]*255/1.76;
            for(k=i;k<i+six;k++)
	    {
              for(l=j;l<j+eight;l++)
              {
                  tcon[k][l]=tcon[i][j];
                  depth[k][l]=depth[i][j];
	      }
            }
	  }
	}

	/*------------------------------STRATEGIES--------------------------- */

	/* loop to locate the edges of the obstacles */
	
	XL=20000;
	XR=-20000;
	for (i=0; i<=a; i++)
	{
	  if (XL>XOL[i])
	  {
 	    XL=XOL[i];
	    ZL=ZOL[i];
	  }
	  if (XR<XOR[i])
	  {
	    XR=XOR[i];
	    ZR=ZOR[i];
	  }
	}
	printf("%f  %f\n",XL,XR);
	printf("%f  %f\n",ZL,ZR);

	/* depending on the obstacle edges, deciding upon the next move */
	
	if (XL>0 && XL<Xthresh)
	  printf("\nMOVE LEFT\n");
	else
	{
	  if (XR<0 && abs(XR)<Xthresh)
	  printf("\nMOVE RIGHT\n");
	  else
	  {
	    if (XR>abs(XL))
	      printf("\nMOVE LEFT\n");
	    else
	      if (XR<abs(XL))
	        printf("\nMOVE RIGHT\n");
	  }
	}
	if ((XL>Xthresh)||(XR<-Xthresh))
	  printf("\nMOVE STRAIGHT\n");
	if ((XL==0)&&(XR==0))
	  printf("\nMOVE STRAIGHT\n");
	if ((XL<Xthresh) && (XL>0) && (ZL<Zdanger))
	  printf("\nSTOP\n");
	if ((XR>-Xthresh) && (XR<0) && (ZR<Zdanger))
	  printf("\nSTOP\n");

	MbufPut(SrcBuffer3,depth);
	
}

		//Routine for Binarizing the Image.
void Binarize(MIL_ID SrcBuffer,MIL_ID DstBuffer)
{
	MbufGet(SrcBuffer,array);

	for(i=0;i<Image_Y;i++)
	{
		for(j=0;j<Image_X;j++)
		{
			if(array[i][j]<Threshold)
			array1[i][j] = 0;
		else
			array1[i][j] = 255;
		}
	}
 
	MbufPut(DstBuffer,array1);
}

/* routine to convolve an array with a desired mask */

void Convolve(float **array5, float **array4, int type)
{
	char ccc;
        float value,value1,value2;
	int X,Y;
        int mask0[3][3] = {1,2,1,2,4,2,1,2,1}; 
	int mask1[3][3] = {-2,-2,-2,0,0,0,2,2,2}; 
	int mask2[3][3] = {-2,0,2,-2,0,2,-2,0,2}; 
	int mask31[3][3] = {1,2,1,0,0,0,-1,-2,-1};
	int mask32[3][3] = {-1,0,1,-2,0,2,-1,0,1};
	int mask5[3][3] = {0,-1,0,-1,4,-1,0,-1,0};
	int mask6[3][3] = {0,-1,0,-1,5,-1,0,-1,0};
	int mask71[3][3] = {1,1,1,0,0,0,-1,-1,-1};
	int mask72[3][3] = {-1,0,1,-1,0,1,-1,0,1};

	
	X=39;
	Y=39;
	for(x=1;x<Y-1;x++)
	{
	  for(y=1;y<X-1;y++)
	  {
	  /*Now Apply Mask*/
	    value = 0;value1 = 0;value2 = 0;
	    for(i=-1;i<=1;i++)
	    {
	      for(j=-1;j<=1;j++)
	      {
	        switch(type)
	        {
	          case 0 : value = value + array5[x+i][y+j]*mask0[i+1][j+1]/16;break;
	          case 1 : value = value + array5[x+i][y+j]*mask1[i+1][j+1];break;
	          case 2 : value = value + array5[x+i][y+j]*mask2[i+1][j+1];break;
	          case 3 : value1 = value1 + array5[x+i][y+j]*mask31[i+1][j+1];
                           value2 = value2 + array5[x+i][y+j]*mask32[i+1][j+1];break;	
	          case 5 : value = value + array5[x+i][y+j]*mask5[i+1][j+1];break;
	          case 6 : value = value + array5[x+i][y+j]*mask6[i+1][j+1];break;
	          case 7 : value1 = value1 + array5[x+i][y+j]*mask71[i+1][j+1];
                           value2 = value2 + array5[x+i][y+j]*mask72[i+1][j+1];break;
         	}
	      }
	    }
            switch(type)
	    {
	      case 0 : array4[x][y] = value;break;
	      case 1 : array4[x][y] = fabs(value);break;
	      case 2 : array4[x][y] = fabs(value);break;
	      case 3 : array4[x][y] = fabs(value1)/2 + fabs(value2)/2;break;
	      case 5 : array4[x][y] = value;break;
	      case 6 : array4[x][y] = value;break;
	      case 7 : array4[x][y] = fabs(value1)/2 + fabs(value2)/2;break;
	    }
          }
        }
	

}