/* David Marten		  */
/* CS455		  */
/* program 2 Blocks World */


#include <stdio.h>
#include <math.h>
#include </usr/include/sys/types.h>
#include </usr/include/sys/ioctl.h>
#include <stdlib.h>
#include <strings.h>
#include <time.h>

#define NO 0
#define YES 1
#define esc 27
#define MAXX 640
#define MAXY 480
#define Offset 50
#define PI 3.141592654
#define RADS 57.29577951

struct sgttyb tab6;
struct sgttyb *addr = &tab6;

int Startx = Offset;
int Starty = Offset;
int Endx = MAXX - Offset;
int Endy = MAXY - Offset;

int result;
int fp;
char *name;
short done = 0;

main()
{
  int cmd;
  int mode;
  char buffer[80];
  int base, i, r;
  char req;
  int x1, y1, x2, y2;
  int dx, threshold;
  int length, xvals[1024], yvals[1024];
  mode = 2;
  name = "/dev/tty";
  system (buffer);  fp = open(name, mode);
  initialize_vision_system();
  if (fp != -1)
    do {
	fflush(stdin);
	menu();
	set_raw_mode();
	cmd = toupper(getchar());
	fflush(stdin);
	set_normal_mode();
	fflush(stdin);
	switch (cmd)
	  {
		case 'V' : system("clear"); view_object_window(); break;
		case 'I' : system("clear"); initialize_vision_system(); break;
		case 'G' : system("clear"); find_objects(); break;
		case 'E' : done = 1; break;
	  }
	}while (done == 0);
}

menu()
{
	int test;
	system("clear");
	GotoXY(6,14); printf("I : initialize vision system");
	GotoXY(15,14); printf("G : get object.");
        GotoXY(18,14); printf("V : View object window.");
	GotoXY(22,14); printf("E : Exit.");
	GotoXY(26,14); printf("Please pick a function : ");
}


/************ Function sets keyboard input to unbuffered ****************/
set_raw_mode()
{
    /* addr->sg_flags = 0300; /* MVAXII was 0310 in octal RWW fixed 8/9/90 */
    result = ioctl(fp, TIOCGETP, addr);
    addr->sg_flags = O_RAW; /* RWW fixed 7/21/91for SUN  */
    addr->sg_ispeed = 13;   /* 7 is 300 baud 13  is 9600 baud*/ 
    addr->sg_ospeed = 13;   /* 7 is 300 baud 13  is 9600 baud*/ 
    result = ioctl(fp, TIOCSETP, addr);
}

/************ Function sets keyboard input back to buffered ************/
set_normal_mode()
{
    /* addr->sg_flags = 0300; /* MVAXII was 0310 in octal RWW fixed 8/9/90 */
    
    addr->sg_flags = O_ALLDELAY | ECHO | CRMOD; /* RWW fixed 7/21/91for SUN  */
    addr->sg_ispeed = 13;   /* 7 is 300 baud 13  is 9600 baud*/ 
    addr->sg_ospeed = 13;   /* 7 is 300 baud 13  is 9600 baud*/ 
    result = ioctl(fp, TIOCSETP, addr);
}


/************** Function to wait for key to be pressed ********/
getkey()
{
	int cmd;
	printf("\n\nHit any key to continue");
	set_raw_mode();
	cmd = toupper(getchar());
	fflush(stdin);
	set_normal_mode();
	fflush(stdin);
}


/*********** Function moves the cursor to the screen coordinates
	   X  ( lines down from top), Y   ( spaces in from right )***********/
GotoXY( x, y )
int x, y;
{
   printf("\n");
   printf( "%c%c%d%c%d%c", (char)esc, '[', x, ';', y, 'H' );
}

initialize_vision_system()
{
  	VIS$OPEN();
  	VIS$INIT_FB();
  	VIS$INIT_LUTS();
  	VIS$VIEW();
	setcamera(0);
        setup_luts();
	ROBOT$SPEECH_INIT(); 
}

delay()
{
   int i,j;
   for (i=0;i<10000000;i++)
	j = i + i + i;
}

find_objects()
{
  int done=1;
  do
  {
    done=find_object();
    fflush(stdin);
    if (done !=0)
      getkey();
  }
  while (done !=0);
  text(Startx+100,Starty+100,0,2,128,"** DONE **");
/* ROBOT$SPEAK("No more dominos"); */

}

find_object()
{
   int values[640];
   int length;
   int minx,miny,maxx,maxy;
   int ycords[1000];
   int xcords[1000];
   int vsums[640];
   int hsums[480];
   int i,j,done=1,z,x=0,y=0,notx=0,noty=0,area=0,dx,dy,x2,y2;
   int centerx,centery;
   float a=0,b=0,c=0, numer=0,denom=0, ans=0, x1,y1;
   for (i=0; i<1000; i++)
   {
     ycords[i] =0;
     xcords[i] =0;
   }
   for (i=0; i<640; i++)
     vsums[i] =0;
   for (i=0; i<480; i++)
     hsums[i] =0;
   for (i=1;i<3;i++)
      VIS$SELECT_LUT(i,10);
   VIS$VIEW();
   VIS$FREEZE();
   VIS$DRAW_BOX(Startx,Starty,Endx,Endy,128,0);     
     done=VIS$BORDER_FOLLOW(&length,xcords,ycords,0,128,Startx,Starty,Endx,Endy,5);
   if (done == 0)
      return 0;
   VIS$GET_MAX_XY(xcords,ycords,length, &minx,&miny,&maxx,&maxy);
   dx = maxx-minx+1;
   dy = maxy-miny+1;   
   for ( i=0; i<=dy;i++)
   {
      VIS$READ_HORIZONTAL_LINE(minx,miny+i,dx,values);
      for (z=0;z<dx;z++)
         if (values[z] == 0)
             hsums[i] += 1;
         x = 0;
   }
   for ( i=0; i<=dx;i++)
   {
      VIS$READ_VERTICAL_LINE(minx+i,miny,dy,values);
      for (z=0;z<dy;z++)
         if (values[z] == 0)
             vsums[i] += 1;
         x = 0;
   }
   x1 = (float)((maxx-minx+1.0)/2.0 + minx);
   x = round(x1);
   y1 = (float)((maxy-miny+1.0)/2.0 + miny);
   y = round(y1);
   VIS$DRAW_BOX(minx,miny,maxx,maxy,128,0);
   for (i=1;i<dx;i++)  /* find centroid */
      area += vsums[i];
   for (i=1;i<dx;i++)
      notx += i * vsums[i];
   notx = notx/area;
   for (i=1;i<dy;i++)
      noty += i*hsums[i];
   noty = noty/area;
   centerx= notx+minx;
   centery= noty+miny;
   VIS$CROSS_HAIR(centerx,centery,128,5);   
   z=maxx;
   for (i=0;i< dx;i++)
   {
      a += (x-z)*(x-z)*vsums[i];
      z--;
   }
   z=maxy;
   for (i=0;i<dy;i++)
   {
      c += (y-z)*(y-z)*hsums[i];
      z--;
   }
   for (j=miny;j<miny+dy;j++)
     for (i=minx;i<minx+dx;i++)
       if (VIS$GET_PIXEL(i,j) == 0)
       {
         x2 = x-i;
         y2 = (y-j)*-1;
         b += x2*y2;
       }
   b *= 2.0;
   numer = (a-c);
   denom = b*b+((a-c)*(a-c));
   denom = sqrt(denom);
   ans = numer/denom;
   ans = acos(ans)*57.29577951;
   ans = ans/2.0;
   if (b<0)
      ans = ans*-1;
   printf("fullans = %f\n",ans);
   draw_lines(&centerx,&centery,&ans,&area);
   return (done);      
}
   

setup_luts()
{
   int i,x;
   int values[256];
   VIS$FREEZE();
   x=VIS$OPTIMAL_THRESHOLD(Startx,Starty,Endx,Endy,NO,2,8);
   VIS$THRESHOLD_LUT(0,10,x); /* sets input lut 0 to a binary threshold lut */
   for (i=0;i<256;i++)
      values[i]=i;
   values[128]=255;
   VIS$LOAD_LUT(1,10,values);
   values[255]=0;
   VIS$LOAD_LUT(2,10,values);
   VIS$LOAD_LUT(3,10,values);
   VIS$VIEW();
}

view_object_window()
{
   int values[640],i,x=0;
do{
   x = 0;
   VIS$VIEW();
   VIS$DYNAMIC_OUTLUT(YES);
   VIS$VMASK();
   VIS$DRAW_BOX(Startx,Starty,Endx,Endy,512,0);
   VIS$DRAW_BOX(Startx,Starty,Endx,Endy,512,1);
   VIS$DRAW_BOX(Startx,Starty,Endx,Endy,512,2);
   VIS$DRAW_BOX(Startx,Starty,Endx,Endy,512,3);
   text(Startx+5,Starty+5,0,2,512,"Place objects fully inside box.");
  /* ROBOT$SPEAK("Place objects fully inside box."); */
   text(Startx+5,Starty+35,0,2,512,"Hit any key to continue.");
  /* ROBOT$SPEAK("Hit any key to continue."); */
   getkey();
   VIS$FREEZE();
   VIS$READ_HORIZONTAL_LINE(Startx+1,Starty+1,MAXX-2*Offset-1,values);
   for (i=0;i<MAXX-2*Offset-1;i++)
      if (values[i]==0)
          x += 1;
   VIS$READ_HORIZONTAL_LINE(Startx+1,MAXY-Offset-1,MAXX-2*Offset-1,values);
   for (i=0;i<MAXX-2*Offset-1;i++)
      if (values[i]==0)
          x += 1;
   VIS$READ_VERTICAL_LINE(Startx+1,Starty+1,MAXY-2*Offset-1,values);
   for (i=0;i<MAXY-2*Offset-1;i++)
      if (values[i]==0)
          x += 1;
   VIS$READ_VERTICAL_LINE(MAXX-Offset-1,Starty+1,MAXY-2*Offset-1,values);
   for (i=0;i<MAXY-2*Offset-1;i++)
      if (values[i]==0)
          x += 1;
   if ( x>0)
   {
     text(Startx+5,Starty+65,0,2,512,"ALL OBJECTS ARE NOT IN BOX !!");
  /* ROBOT$SPEAK("ALL OBJECTS ARE NOT IN BOX"); */
     delay();
     clear_text();
   }
  } while ( x !=0 );
  VIS$VIEW();
  clear_text();
 } 

clear_text()
{
   int i;
   for (i=0;i<=100;i++)
      VIS$DRAWLINE(Startx+1,Starty+i,MAXX-Offset-1,Offset+i,255);
}

draw_lines(x,y,angle,area)
int *x,*y,*area;
float *angle;
{
 float a;
 char ch[80];
 int y1=0,y2=0,x1=0,x2=0,c=60;
 a= *angle/RADS;
 a *=-1;
 y2 = round(c*sin(a));
 x2 = round(c*cos(a));
 c *= -1;
 y1 = round(c*sin(a));
 x1 = round(c*cos(a));
 VIS$DRAWLINE(*x+x1,*y+y1,*x+x2,*y+y2,128);
 text(*x+x2+10,*y+y2-5,0,1,128,"Axis of least inertia");
 if (*angle < 0)
    a = *angle+90;
 else
    a = *angle-90;
 a = a/RADS;
 a *= -1;
 c=30;
 y2=round(c*sin(a));
 x2=round(c*cos(a));
 c *= -1;
 y1 = round(c*sin(a));
 x1 = round(c*cos(a));
 VIS$DRAWLINE(*x+x1,*y+y1,*x+x2,*y+y2,128);
 text(*x+x2+10,*y+y2+-5,0,1,128,"Robot Gripper");
 x1=round(*angle);
 sprintf(ch,"Centroid : X = %d, Y = %d",*x,*y); 
/* ROBOT$SPEAK(ch); */
 text(Startx+5,Starty+5,0,2,128,ch);
 sprintf(ch,"Angle    : %d, Area : %d Pix.",x1,*area);
/* ROBOT$SPEAK(ch); */
 text(Startx+5,Starty+30,0,2,128,ch);
}