/* --------------------------------------------------------------------- */
/* -                       CALIBRATE.C                                 - */
/* --------------------------------------------------------------------- */
/*         Filename:      CALIBRATE.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 xvals[1024], yvals[1024];

/***********************************************************************/
 float VIS$GOLF_CALIBRATE(minx,miny,maxx,maxy, objectsize, ppi)
/***********************************************************************/
 int minx,miny,maxx,maxy;
 float objectsize, *ppi;
{
 int xcent = 0;
 int ycent = 0;
 int increment      = 10;
 float cum_diameter = 0.0;
 float ballcnt      = 0.0;
 float dx           = 0.0;
 float dy = 0.0;

while( VIS$GOLFBALL_FIND(minx, &miny,maxx,maxy,increment,&xcent,&ycent,&dx,
&dy))
{
   printf ("dx: %6.4f dy: %6.4f\n", dx, dy); 
   cum_diameter += ((dx+dy) / 2);
   ballcnt++;
   printf ("ball#: %6.4f ->cumultive diameter %6.4f \n", ballcnt,cum_diameter); 
   dx = dy = 0.0; 
    
 }
 printf ("ave diameter %6.4f \n", cum_diameter/ballcnt); 
 *ppi = (cum_diameter/ballcnt) / objectsize;  
}

/***********************************************************************/
 float BALL_DISTANCE(minx,miny,maxx,maxy,ppi,angle,xcent,ycent)
/***********************************************************************/
 int  minx,*miny,maxx,maxy;
 float ppi,*angle;
 int  *xcent, *ycent;
 {
 int increment    = 6;
 float dx = 0.0;
 float dy = 0.0;
 float a  = 0.0;
 float b  = 0.0;
 float c  = 0.0;

 if (VIS$GOLFBALL_FIND(minx,miny,maxx,maxy,increment,xcent,ycent,&dx, &dy))
  {
   b = (((480-*ycent)/ppi) + FIXEDLENGTH ); /* dy in robot space */
   a = (320 - (640-*xcent))/ppi;             /* dx in robot space */
   c = sqrt( (a*a) + (b*b) );               /* hypot. */
   *angle = asin((double)(a/c)); /* angle wrt the x axis in robot space */
   printf ("dist from y=480 to cup: %6.4f\n",  (480 - *ycent)/ppi); /*  */
   return(c);
  }
 else
  return(0.0);
 }