/* --------------------------------------------------------------------- */
/* -                       PROJECT.C                                   - */
/* --------------------------------------------------------------------- */
/*         Filename:      PROJECT.C                                    - */
/*         Description:   Contains the robot calls for Alpha II        - */
/*                        Microbot Five Axis Robot                     - */
/*         Written by:						       - */
/*	       	       KUO-HUNG HUANG                                  - */
/*         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"
#define SIZE_OF_HOLE  20

main ()
{
  float distance; 
  float real_angle;
  int threshold, goalx, goaly, total_area,area;
  char again, buffer[40];
  int xarr[THREE], yarr[THREE];
  int xaxis;  /* x position of 1st scan line */

  again = 'y';
  Initialize(); 
  Control (MINX,MINY,MAXX,MAXY,&distance,&real_angle,&threshold,&goalx,&goaly);
  ROBOT$INIT();  /* rww was ROBOT$INIT_GOLF(245) */
  while (again == 'y')
  {
    scan (xarr,yarr,threshold,goaly,distance,real_angle);

                              /* return x position at 1 st scan line */
    correct_x (&xaxis,real_angle,yarr[ZERO]); 


    feedback (&real_angle,&distance,goalx,goaly,xarr,yarr);
    area = 0;
				/* find the area of object inside hole */
    VIS$FIND_AREA(goalx-SIZE_OF_HOLE,goaly-SIZE_OF_HOLE,
		goalx+SIZE_OF_HOLE,goaly+SIZE_OF_HOLE,&area,threshold);
    

                                                       /* display data */
    sprintf (buffer,"goal     is (%d,%d) \n",goalx,goaly);
    text ( 30,350, 0, 1, 255, buffer);
    sprintf (buffer,"distance is %3.2f inches\n",distance);
    text ( 30,375, 0, 1, 255, buffer);
    sprintf (buffer,"angle    is %3.2f degree\n",real_angle);
    text ( 30,405, 0, 1, 255, buffer);

    VIS$DRAWLINE (goalx,goaly,xaxis,yarr[ZERO],5); /* draw path */

    if (area < (SIZE_OF_HOLE * SIZE_OF_HOLE / 4) )
    {   
       printf (" \n ALMOST! TRY AGAIN ? ");
       if ((xarr[ZERO] != 0) && (xarr[ONE] != 0)) 
         VIS$DRAWLINE (xarr[ZERO],yarr[ZERO],xarr[ONE],yarr[ONE],1);
       if ((xarr[TWO] != 0) && (xarr[ONE] != 0))
         VIS$DRAWLINE (xarr[ONE],yarr[ONE],xarr[TWO],yarr[TWO],1);
       sprintf (buffer,"I need to practice\n");
       text ( 50, 440, 0, 3, 255, buffer);
       again = getchar();
       getchar();
    }
    else
    {   
       printf (" \n GOT IT! ");
       if ((xarr[ZERO] != 0) && (xarr[ONE] != 0)) 
         VIS$DRAWLINE (xarr[ZERO],yarr[ZERO],xarr[ONE],yarr[ONE],1);
       if ((xarr[TWO] != 0) && (xarr[ONE] != 0))
         VIS$DRAWLINE (xarr[ONE],yarr[ONE],xarr[TWO],yarr[TWO],1);
       else
         VIS$DRAWLINE (xarr[ONE],yarr[ONE],goalx,goaly,1);

       sprintf (buffer,"I got it in the hole !!!\n");
       text ( 30, 440, 0, 3, 255, buffer);
       again = 'n';
    }
  }  
}

#ifdef ROGER
    /* text ( x,y, direction, size, grey_shade, buffer); */
#endif