/* --------------------------------------------------------------------- */
/* -                       ROBOT_VI.C                                  - */
/* --------------------------------------------------------------------- */
/*         Filename:      ROBOT_VI.C                                   - */
/*         Description:   Contains the robot calls for Alpha II        - */
/*                        Microbot Five Axis Robot                     - */
/*         Written by:						       - */
/*	       	       Shawn Lovelidge                                 - */
/*         Other authors of this code include:			       - */
/*	       	Roger W. Webster, Ph.D                                 - */
/*                     Robot Vision and Artificial Intelligence Lab    - */
/*                     Department of Computer Science                  - */
/*                     Millersville University                         - */
/*                     Millersville, PA 17551                          - */
/*                     (717) 872-3539 or (717) 872-3860                - */
/* --------------------------------------------------------------------- */

/* --------------------------------------------------------------------- */
/* -    necessary includes & defines for this file                     - */
/* --------------------------------------------------------------------- */
#include <math.h>
#include <stdio.h>
#include "mytest.h" 

/* --------------------------------------------------------------------- */
int xarr[2000];
int yarr[2000];
int value[640];
int i,j;
int xcent = 0;
int ycent = 0;
  
/****************************************************************************
   This procedure will find all object within in the minx,miny,maxx,maxy
   that the user specifies.
*****************************************************************************/

int VIS$GOLFBALL_FIND(minx,miny,maxx,maxy,increments,xcent,ycent,dx, dy)
  int   minx,*miny,maxx,maxy,increments,*xcent,*ycent;
  float *dx;
  float *dy;
{
 float angle,slope;
 float ballsize = 1.0; /* hi shawn changed this here -> (28/16); */
 int   Target_grey = 1;
 int   New_grey    = 2;
 int   length = 0;
 int   area;
 int   mx,my,mxx,mxy;
     
     VIS$DRAW_BOX (minx,*miny,maxx,maxy, 255, 5);
     VIS$BORDER_FOLLOW(&length,xarr,yarr,Target_grey,New_grey,
			   minx,*miny,maxx,maxy,increments);
     if (length > 0 )
     {
       VIS$GET_MAX_XY(xarr,yarr,length,&mx,&my,&mxx,&mxy);
       VIS$DRAW_BOX (mx,my,mxx,mxy, 255, 15);
       VIS$CENTROID(mx,my,mxx,mxy,xcent,ycent,&area);
       VIS$CROSS_HAIR(*xcent, *ycent, 255, 25);
       *dx = (mxx - mx )/ ballsize;
       *dy = (mxy - my )/ ballsize;
#ifdef ROGER 
       printf ("mxy: %d\n", mxy); 
       getchar(); 
#endif 
       CHANGE_OBJECT(mx,my,mxx,mxy);
       *miny = mxy+2; 
       return(1);
     }
     else
       return(0); /* nothing found */
#ifdef ROGER 
#endif 
}


/****************************************************************************
   This procedure will find all object within in the minx,miny,maxx,maxy
   that the user specifies and return the area of object.
*****************************************************************************/
 void VIS$FIND_AREA(minx,miny,maxx,maxy,area,threshold)
  int   minx,miny,maxx,maxy,*area,threshold;
{
 float angle,slope;
 float ballsize = 1.0; /* hi shawn changed this here -> (28/16); */
 int   Target_grey = 1;
 int   New_grey    = 2;
 int   length = 0;
 int   increments= 10;
 int   mx,my,mxx,mxy;

     VIS$INIT_FB();
     VIS$INIT_LUTS();
     SCREEN(minx,miny,maxx,maxy,threshold);
     VIS$DRAW_BOX (minx,miny,maxx,maxy, 255, 5);
     VIS$BORDER_FOLLOW(&length,xarr,yarr,Target_grey,New_grey,
			   minx,miny,maxx,maxy,increments);

     if (length > 0 )
     {
       VIS$GET_MAX_XY(xarr,yarr,length,&mx,&my,&mxx,&mxy);
       GET_AREA(mx,my,mxx,mxy,area);
     }
     
     if ((my < maxy ) && ( length != 0))
     { 
       miny = my; 
     }

}
/****************************************************************************
  This procedure is used for changing the object values which are understood
  as 1's to a specified value, in this case the value is 2. Then
  VIS$BORDER_FOLLOW(); is called again, to find the next line.             
*****************************************************************************/
GET_AREA(minx,miny,maxx,maxy,area)
int minx,miny,maxx,maxy,*area;
{
int dx;
minx -= 6;   /* buffer around the objects */
miny -= 6;   /* to accound for any noise  */
maxx += 6;   /* around the object.        */
maxy += 6;

dx = maxx - minx;  /* dx is the component in the x direction */

  for (j=miny;j<maxy;j++)
    {
     i=0;
     VIS$READ_HORIZONTAL_LINE(minx,j,dx,value);
     do {
         if (( value[i] == 1) || ( value[i] == 4095))
	 {
           value[i] = 2;
	   (*area)++;
         }
         i++;
       } 
       while ( i < dx );
     VIS$WRITE_HORIZONTAL_LINE(minx,j,dx,value);
    }
value[2] = 255;
}


/****************************************************************************
  This procedure is used for changing the object values which are understood
  as 1's to a specified value, in this case the value is 2. Then
  VIS$BORDER_FOLLOW(); is called again, to find the next line.             
*****************************************************************************/

CHANGE_OBJECT(minx,miny,maxx,maxy)
int minx,miny,maxx,maxy;
{
int dx;
minx -= 6;   /* buffer around the objects */
miny -= 6;   /* to accound for any noise  */
maxx += 6;   /* around the object.        */
maxy += 6;

dx = maxx - minx;  /* dx is the component in the x direction */

  for (j=miny;j<maxy;j++)
    {
     i=0;
     VIS$READ_HORIZONTAL_LINE(minx,j,dx,value);
     do {
         if (( value[i] == 1) || ( value[i] == 4095)) 
           value[i] = 2;
         i++;
       } 
       while ( i < dx );
     VIS$WRITE_HORIZONTAL_LINE(minx,j,dx,value);
    }
value[2] = 255;
}

/*****************************************************************************/
VIS$ORIENTATION(mx,my,mxx,mxy,xcent,ycent,angle,slope)
int mx,my,mxx,mxy,xcent,ycent;
float angle,slope;
{
int   dy;
int   dx;
int   y2,y1;
int   x2,x1;
float hyp = 50.0;
float radians = 1.57079632680;

if (slope > 0.0 ) {                   /* object is oriented verticaly   */ 
	dx = (int)((round(hyp * cos(angle))));
	dy = (int)((round(hyp * sin(angle))));

	x1 = xcent + dx;
	y1 = ycent - dy;
	x2 = xcent - dx;
	y2 = ycent + dy;

	VIS$DRAWLINE(x1,y1,x2,y2,25);

	dx = (int)((round(hyp * cos(angle+radians))));
	dy = (int)((round(hyp * sin(angle+radians))));

	x1 = xcent + dx;
	y1 = ycent - dy;
	x2 = xcent - dx;
	y2 = ycent + dy;

	VIS$DRAWLINE(x1,y1,x2,y2,25);
    }

if (slope < 0.0)  { /* in Q II      */ 
	dx = (int)((round(hyp * cos(angle))));
	dy = (int)((round(hyp * sin(angle))));

	x1 = xcent - dx;
	y1 = ycent - dy;
	x2 = xcent + dx;
	y2 = ycent + dy;

	VIS$DRAWLINE(x1,y1,x2,y2,25);

	dx = (int)((round(hyp * cos(angle+radians))));
	dy = (int)((round(hyp * sin(angle+radians))));

	x1 = xcent - dx;
	y1 = ycent - dy;
	x2 = xcent + dx;
	y2 = ycent + dy;

	VIS$DRAWLINE(x1,y1,x2,y2,25);
    }

}
/*****************************************************************************/
VIS$AXIS_OFLEAST_INERTIA(minx,miny,maxx,maxy,angle,xcent,ycent,slope)
int minx,miny,maxx,maxy;
float *angle;
int   xcent,ycent;
float *slope;
{
 int startx,starty;
 int dx,dy;
 int x,y;
 int hsums[475];
 int vsums[640];
 float a = 0.0;
 float b = 0.0;
 float c = 0.0;
 

 dx = maxx - minx +1;
 dy = maxy - miny +1;
 x = y = 0;

  VIS$GET_V_SUMS(minx,miny,dx,dy,vsums,&startx);
  VIS$GET_H_SUMS(minx,miny,dx,dy,hsums,&starty);

 for (i=0;i<dx;i++) {
   x = (minx+i)-xcent;
   a += ((x*x) * vsums[i]);
		     }

 for (i=0;i<dy;i++) {
  y = ycent-miny-i;
  b += ((y*y) * hsums[i]);
                    }
 
 for (i=0;i<dy;i++)
   {
     VIS$READ_HORIZONTAL_LINE(minx,i+miny,dx,value);
     y = ycent-miny-i;
     for (j=0;j<dx;j++)
        if ((value[j] == 1 ) || (value[j] == 4095))
	 {
	  x = ((minx+j)-xcent);
          c += x * y;
	  x = 0;
         }
     y = 0;
   }

 c *= 2.0;
 *slope = c;
 *angle = (float)( (a-b) / sqrt( (c*c) + ( (a-b)*(a-b) ) ) );
 *angle = acos(*angle);
 *angle = ((float) *angle /(float) 2);

 if ( *slope < 0.0 )
  *slope = -1.0;
 else
  *slope = 1.0;
}
/*****************************************************************************/