/* --------------------------------------------------------------------- */
/* -                       NEW_ROBOT.C                                 - */
/* --------------------------------------------------------------------- */
/*         Filename:      NEW_ROBOT.C                                  - */
/*         Description:   Contains the robot calls for Alpha II        - */
/*                        Microbot Five Axis Robot                     - */
/*         Written by:						       - */
/*	       	       Gary E. Shay                                    - */
/*         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" 

/* --------------------------------------------------------------------- */
/* -    Global variables and structures...                             - */
/* --------------------------------------------------------------------- */
#define S1 -1291.02
#define fabs(x) ((x < 0) ? (-x) : (x))
#define VIEW_MIN_Y 132.25 /* Inches to bottom of View Window */
#define VIEW_HEIGHT 66.25 /* View is VIEW_HEIGHT inches top-to-bottom */
#define STR_CONST 3.0     /* Difference in 'speeds' needed to hit the */
			  /*   ball to the bottom of the view and the */
			  /*   top of the view.                       */
#define BASE_STREN 244.0  /* The base speed to hit the bottom of view */
#define BASE_BACK   15.0

input_angle (dist,angle)
float *dist, *angle;
{
   printf("Please input an angle < -45 to 45 >\n");
   scanf("%f",angle);
   printf("Please input a distance.........:\n");
   scanf("%f",dist);
}


ROBOT$HIT_BALL ( dist, angle )
   float dist, angle;
{

   float  speed,
	  back,
	  ang2,
	  swing_back,
	  inches_in_view,
	  fract_part;
   int    swing_forth,
	  s_back,
          dec_speed,
	  base_ang,
	  slow_speed=220;
   double whole_part;
   char   buffer1[80];
   char   buffer2[80];

   ang2 = angle/DEG_PER_RAD;
   dist = (dist/cos(ang2)) + PAST_THE_CUP; /* was 18 now 0 ?? */
   dec_speed = floor(dist/40.0) + BASE_STREN;
   back = floor(dist/2) + floor(dist/20) + BASE_BACK;

   ROBOT$MOVE(240.0, 8.0, 0.0, 1.55, -90.0, -90.0);
   swing_forth = floor((-2) * back);
   s_back = floor(back);
   base_ang = floor((angle/DEG_PER_RAD) * S1 );
   sprintf(buffer1, "@STEP %d,%d,0,0,0,0",slow_speed,base_ang);
   ROBOT$SEND(buffer1);
   
   printf("Put the ball on the spot and hit enter.\n");
   getchar();  

   sprintf(buffer1, "@STEP %d,0,0,0,%d,%d",slow_speed,s_back,s_back);
   ROBOT$SEND(buffer1);
   sleep(1);
   sprintf(buffer2, "@STEP %d,0,0,0,%d,%d",dec_speed,swing_forth,swing_forth);
   ROBOT$SEND(buffer2);
   printf("Speed of ball: %d\nClub back %d motor steps.\n",dec_speed,
   s_back);
 }