/* --------------------------------------------------------------------- */
/* -                       NEW_SCREEN.C                                - */
/* --------------------------------------------------------------------- */
/*         Filename:      NEW_SCREEN.C                                 - */
/*         Description:   Contains the robot calls for Alpha II        - */
/*                        Microbot Five Axis Robot                     - */
/*         Written by:						       - */
/*	       	       Shawn D. Lovelidge                              - */
/*         Other authors of this code include:			       - */
/*	       	Roger W. Webster, Ph.D edited by kuo-- sep. 29, 1990   - */
/*                     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" 
#include <stdio.h>
#include <math.h>
#include "mytest.h"

/**** Prototypes ****/
void Initialize();
void SCREEN();
void Control();
float BALL_DISTANCE();

/********  globals *******/
int value[640];
float ppi = 0.0;

void Initialize()
/*************************************************************************
  initilize the buffer and luts
***************************************************************************/
{
   VIS$OPEN();
   VIS$INIT_FB();
   VIS$INIT_LUTS();  /* setup init */
   VIS$VIEW();
}

void Control(minx,miny,maxx,maxy,distance,angle,threshold,goalx,goaly)
/*************************************************************************
  initilize the luts, find the hole, call procedure to calcualte angle and
  distance , the return angle, distance, hole position, and threshold
***************************************************************************/
  int minx,miny,maxx,maxy;
  float *distance, *angle;
  int *threshold,*goalx,*goaly;
{
 float dx       = 0.0;
 float remainder= 0.0;
 float feet     = 0.0;
 double dbl_feet = 0.0;

 VIS$FREEZE();
 
 *threshold = VIS$OPTIMAL_THRESHOLD (200,200, 500,400, 0, 255, 5);
 printf("OPTIMAL threshold is: %d\n", *threshold);
 *threshold = 251; 
 printf("my threshold is: %d\n", *threshold);
 read_ppi (&ppi);
 printf("ppi is: %6.4f\n", ppi);
 VIS$INIT_FB();
 VIS$INIT_LUTS();
 sleep(1);
 SCREEN(minx,miny,maxx,maxy,*threshold);

 *distance = BALL_DISTANCE(minx,&miny,maxx,maxy,ppi,angle,goalx,goaly);
 printf("Distance in inches = %7.4f\n",*distance);
 *distance = *distance + PAST_THE_CUP; 
 printf("Dist w/added fixed = %7.4f\n",*distance);
 remainder = (float)modf((double)((*distance)/12),&dbl_feet);
 feet = (float)dbl_feet; 
 printf("Distance in feet = %7.4f inches = %7.4f \n",feet,remainder*12);
 printf("The dx is = %d , dy is = %d\n",*goalx,*goaly);
 *angle = RAD_TO_DEG * (*angle);
 printf("The angle is %3.3f\n",*angle);
 printf (" Wait for robot \n");
 
/* RWW temporary for test w/o camera */ 
/*
*distance = 240; 
 printf("The RWW Distance in inches = %f\n",*distance);
  *angle = 0.0;
 printf("The RWW angle is %3.3f\n",*angle);
*/
/* RWW temporary for test w/o camera */ 
}

void SCREEN(minx,miny,maxx,maxy,threshold)
/*************************************************************************
  set the color of the screen
***************************************************************************/
  int minx,miny,maxx,maxy;
  int threshold;
 
 {
   int  i;
   int value[640];

 for (i=0;i<=255;i++)
   value[i] = (i > threshold) ? 1:0;  /* ball is a 1 background = 0 */
 
 VIS$LOAD_LUT(0,OUT_LUT,value);
 /* RED GUN */
   value[0]  = 0;
   value[1]  = 255;
   value[2]  = 0;
   value[100] = 255;
   value[255]  = 100;
   VIS$LOAD_LUT(RED,OUT_LUT,value);

 /* GREEN GUN */
   value[0]  = 255;
   value[1]  = 0;
   value[2]  = 0;
   value[100] = 100;
   value[255]  = 0;
   VIS$LOAD_LUT(GREEN,OUT_LUT,value);

 /* BLUE GUN */
   value[0]  = 0;
   value[1]  = 0;
   value[2]  = 255;
   value[100] = 255;
   value[255]  = 100;
   VIS$LOAD_LUT(BLUE,OUT_LUT,value);

   VIS$FREEZE();
   VIS$DRAW_BOX(minx,miny,maxx,maxy,255,1);
   /* VIS$DRAWLINE(320,100,320,200,1);  */ 

}


void correct_x (xaxis,angle,scan)
/*************************************************************************
  find the x position at the first scan line and return it
***************************************************************************/
int *xaxis;
float angle;
int scan;

{
 float length, x_inch;

 length = FIXEDLENGTH + ((640 -scan) / ppi);
 x_inch = tan( (double) (angle / RAD_TO_DEG)) * length;
 *xaxis = (int) (x_inch * ppi);
 *xaxis += 320;

}



read_thre (threshold)
/*************************************************************************
  read the threshold value from the file --threshold
***************************************************************************/
int *threshold;

{
  FILE *fp, *fopen();

  if ((fp=fopen("threshold","r")) == NULL) exit(1);

  fscanf (fp, "%d",threshold);
  fclose (fp);
}

read_ppi (ppi)
/*************************************************************************
  read the ppi value from the file --ppi
***************************************************************************/
int *ppi;

{
  FILE *fp, *fopen();

  if ((fp=fopen("ppi","r")) == NULL) exit(1);

  fscanf (fp, "%f",ppi);
  fclose (fp);
}