/* 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 40
#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 x,y; 
int values[1024];
int harray[1024];

int c;
int fp;
char *name;

short done = 0;
main()
{
  int cmd;
  int mode;
  char buffer[80];
  int base, i, result, 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 'C' : system("clear"); night_vision1(); break;
		case 'F' : system("clear"); night_vision2(); 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(9,14); printf("F : night vision (freeze frame).");
  	GotoXY(12,14); printf("C : night vision (Continuous.");
	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 : ");
}


void Smooth_Function(y, smooth_factor)
int *y, smooth_factor;
{
  int i,j,t;
  int base = 455; /* must be same as CS455-histogram */
  int original[256];
  int temp[256];
  int how_many = 5; /* default is 5 */
  int sum = 0;
  int largest;
  float scale;
  how_many = smooth_factor;
  
  for (i=0;i<=255;i++)
      original[i] = y[i]; 
  t = 0;
  
  for (i=0; i<=255;i++)  /* was 246 RWW */
     temp[i] = 0; 
  for (i=how_many;i < (255 - how_many);i++)  /* was 246 RWW */
  {
    sum = 0;
    for (j=i-how_many;j <= i+how_many; j++)
    {
       sum += y[j]; 
    } /* end for j */
    temp[i] = sum / (how_many * 2 + 1); 
  } /* end for i */
   
    largest = 0;
    for (i=0;i <= 255;i++)
      if (original[i] > largest)
        largest = original[i];
    scale = 1.0/((float)largest/((float)base-25.0));
  
  for (i=0;i <= 254;i++)
      temp[i] = temp[i] * scale;
  for (i=0;i <= 254;i++)
      original[i] = original[i] * scale; 
  for (i=0; i <= 255;i++)  /* was 246 RWW */
  {   y[i] = temp[i]; 
  } 
 } /* end function */


night_vision1()
{
	int done = 0,i;
	char ch;
	int harray[256];
	ungetc(0,stdin);
do {
	VIS$LINEARIZE_LUT(i,0);
	VIS$FREEZE(); 
	HISTOGRAM(0,0,640,480,YES,harray,2);
	VIS$VIEW();
	delay();    
/*	set_raw_mode();
	scanf("%s",&ch);
	set_normal_mode();
	if (ch==0)
	  ungetc(done,stdin);
	else
	  done = 1; */

   }while (done == 0);
}

night_vision2()
{
	int harray[256],i;
	int harray2[256];
	HISTOGRAM2(0,0,640,480);
	getkey();
	VIS$SET_MODE(1);
	VIS$FREEZE();
	HISTOGRAM(0,0,640,480,YES,harray2,2);
	HISTOGRAM2(0,0,640,480);
	getkey();
}



/************ 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(); 
}

/****************** functions from vks101.c ************************/

/***  histogram is the same as VIS$CS455HISTOGRAM but with out graphics */
HISTOGRAM(x1,y1,x2,y2,equalize,harray, color)
  int x1,y1,x2,y2, equalize, *harray, color;
  {
    int i,x;
    int offset,base,largest,grpfact,barwidth,area,X1,X2;
    int values[641];
    float scale,eos;

    /* CONSTANTS */
    grpfact=4;
    barwidth=8;
    base= 455 /* was 475 RWW */ ;
    area=(y2-y1)*(x2-x1);
    eos=256/grpfact-1;     /* End Of Scale */



    /* INITIALIZE ARRAYS */
    for (i=0;i<=255;i++)
    {
      harray[i] = 0;
    }

    /* FILL HARRAY WITH READ_HORIZONTAL_LINE this is the histogram */
    for (i=y1;i<=y2;i++)
    {
      rhline(x1,i,x2-x1,values);
      for (x=0;x<x2-x1;x++)
        harray[ (values[x] & 0x00ff) ]++;  /* for 8 bits only 0..255 */
     }

  if (equalize)
  {
      equalize_histogram_dave(harray,area);

   }
}


equalize_histogram_dave(harray,area)
int harray[255];
int area;
{
  int values[256];
  float t[255],sum[255],increment,tolerance;
  int i,x,cmdd  ;
  int earray[255]; /* for histogram */

  /* INITIALIZE ARRAYS */
  for (i=0;i<255;i++)
    {
    t[i]=0.0;
    sum[i]=0.0;
    earray[i]=0;
    }

  /* TRANSFORM HARRAY INTO T */
  for (i=0;i<255;i++)
    t[i] = (float)harray[i]/(float)area;

  /* SUMMATION OF T */
  for (i=0;i<255;i++)
    {
    if (i==0)
      sum[i] = t[0];
    else
      sum[i] = sum[i-1] + t[i];
    }
      mapsums(sum,values);
 }


mapsums(sum,values)
int values[256];
float sum[255];
{
 float temp[255];
 int i,x;
 for (i=0;i<255;i++)
 {
    if (i==0)
      temp[i] = 0;
    else
      temp[i] = i*1.0/255.0 + 1/510;        
 }
 for (i=0;i<255;i++)
 {
   x=0;
   while ( temp[x] < sum[i])
	x++;
   values[i] = x;
 
 }
 VIS$LOAD_LUT (0, 14, values);
 VIS$SET_MODE(1);
 VIS$FREEZE();
 VIS$SET_MODE(0);
}
HISTOGRAM2(x1,y1,x2,y2)
int x1,y1,x2,y2;
{
    int i,x, eos;
    int offset,base,largest,grpfact,barwidth,area,X1,X2;
    int values[641];
    int harray[255];     /* Histogram array, read from fb */
    int darray[255];     /* Display array, bars displayed on screen */
    float scale;

	VIS$FREEZE();
    	/* CONSTANTS */
    	grpfact=4;
    	barwidth=8;
    	base=470;
    	area=(y2-y1)*(x2-x1); 
    	eos=256/grpfact-1;     /* End Of Scale */

    	/* BOX AOI */
    	VIS$DRAW_BOX(x1,y1,x2,y2,0,10);

    	/* INITIALIZE ARRAYS */
    	for (i=0;i<=255;i++)
      	{
      	harray[i] = 0;
      	darray[i] = 0;
      	}

    	/* FILL HARRAY WITH READ_HORIZONTAL_LINE */
    	for (i=y1;i<=y2;i++)
      	{
         	rhline(x1,i,x2-x1,values);
		for (x=0;x<x2-x1;x++)
           	harray[ (values[x] & 0x00ff) ]++;  /* for 8 bits only 0..255 */
      	}

    	/* GROUP HARRAY BY GRPFACT AND STORE IN DARRAY */
    	for (i=0;i<=eos;i++)
      	{
      		for (x=0;x<=grpfact-1;x++)
        		darray[i] += harray[x+(i*grpfact)];
      		darray[i] /= grpfact;   /* Take this out? */
      	}

    		/* FIND LARGEST VALUE IN DARRAY FOR SCALING */
    		largest = 0;
    		for (i=0;i<=eos;i++)
      		   if (darray[i] > largest)
        		largest = darray[i];
    		scale = 1.0/((float)largest/((float)base-25.0));
    	    
    		/* DISPLAY HISTOGRAM FUNCTION OVER IMAGE */
    		X1=0;
    		X2=barwidth;
    		VIS$DRAW_BOX(0,25,barwidth*(256/grpfact+1),base,0,0);
    		for (i=0;i<=eos;i++)
      		{
      		   X1+=barwidth;
      		   X2+=barwidth;
      		   VIS$BLOCK(X1,(int)(base-(darray[i]*scale)), X2, base, 0);
      		}
 
} /* end function */

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


find_objects()
{
  int done=1;
/*  setup_luts();  */
  do
  {
    done=find_object();
    fflush(stdin);
    getkey();
  }
  while (done !=0);
}

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<256e;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.");  
   text(Startx+5,Starty+35,0,2,512,"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 !!");
     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);
 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);
 x1=round(*angle);
 sprintf(ch,"Centroid : X = %d, Y = %d",*x,*y); 
 text(Startx+5,Starty+5,0,2,128,ch);
 sprintf(ch,"Angle    : %d, Area : %d",x1,*area);
 text(Startx+5,Starty+30,0,2,128,ch);
}