/* (C) 1995 Digital Designs and Systems */
/* version 4 */
/* added cool LED display */
/* slowed download for 286 based machines */
/* added blue/red colors - added menu for ease of use */


#include <stdio.h>

#include <config.h>
#include <cvmvid.h>
#include <imgprs.h>
#include <dev/sc26c94.h>
#include <cvmvid/mux.h>


#ifdef STEPPER_MOTOR
#include "disktbl.c"
#endif

#define MAX(a,b) (((a)>(b))?(a):(b))
#define MIN(a,b) (((a)<(b))?(a):(b))

LED_Display(int option) {
  static int pos=0;
  int dat;
  if (pos > 0 ) dat = 1<<pos;
  else dat = 1<<(-pos);


  POKE(BASE_PORTJ,(dat)<<PORTJ_SHIFT);

/*
  switch(option) {
  case 1 :   WRITE_PORTJ(dat); break;
  case 2 :   WRITE_PORTJ(~dat); break;
    default :   WRITE_PORTJ(dat); break;
  }
*/
  ++pos;
  if (pos == 8) pos = -7;

  
}

FindBright(IMAGE *src, int *srch, int *res,int color) {
  int width = src->xres;
  int height = src->yres;
  int x,y,dat,i;
  int *img;
  int max,maxpos;

  for(y=0; y<height; y++) {
    max = 0; maxpos = 0;
    img = src->image + y*width;
    for(x=0;x<width;x++) {
      dat = (*(img+x))&0xff;
      if (dat > max) {
	max = dat; maxpos = x;
      } 
    } /* for x */
    res[y] = maxpos; /* 2*maxpos-30; */

    for(i=0;i<4;i++)
      if (i+maxpos <width) *(src->image + y*width + maxpos + i) = color;
  }
}

#ifdef STEPPER_MOTOR
MillisecondFunction() {
  static int cnt=0;
  ++cnt;
    table_advance(1);
  if (cnt > 16) {

    cnt = 0;
  }
}
#endif

ScaleDepth(float scale, int shift, IMAGE *depth, IMAGE *sdepth) {
 int i,j,dat;
 int *d,*s;
 int xres;
 xres = depth->xres;
 d = depth->image;
 s = sdepth->image;

  for(i=0;i<depth->xres;i++) {
    for(j=0;j<depth->yres;j++) {
      dat = d[i*xres + j];
      s[i*xres + j] = scale*dat + shift;
    }
  }
}



DumpDepth(IMAGE *depth) {
  int i,j,dat,k;
  int *d;
  int xres;
  xres = depth->xres;
  d = depth->image;

  for(i=0;i<depth->xres;i++) {
    for(j=0;j<depth->yres;j++) {
      POKE(BASE_PORTJ,(i)<<PORTJ_SHIFT);
      dat = d[i*xres + j];
      fprintf(iochanb,"%2.2x",dat);
      for(k=0;k<0x8000;k++) asm(" nop");
    }
    fprintf(iochanb,"HS\n\r");
    printf("*");
  }
  fprintf(iochanb,"VS\n\r");
}

main() {
  IMAGE *img1 , *depth, *sdepth;
  int *srch, *res,cnt;
  float scale;
  int shift,mode,act_chan;

  video_parameters *p_main_img, *p_main_dis, *p_depth_dis;
  int err;
  int ch,i,k,dly;
  dly = 0;


#define XRES 256
#define YRES 128 

  printf("scanner4  - %s %s\n",__DATE__,__TIME__);

  InitializeCVMVIDEO();
  InitializeVideo(&DEV_VIDEO_IN1,FRAME_NTSC_NI);
  InitializeVideo(&DEV_VIDEO_OUT1,DISPLAY_MED_RES_NTSC);

  img1 = CreateImage(XRES, YRES, PACKED_RGB_24_2);
  depth = CreateImage(YRES, YRES, PACKED_RGB_24_2);
  sdepth = CreateImage(YRES, YRES, PACKED_RGB_24_2);

  srch = (int *)malloc(512);
  res = (int  *)malloc(512);
  if (!srch || !res) {
    printf("Alloc error\n");
    return;
  }

  p_main_img = CreateParameters(PARAMS_GRAB_HIGHRES);
  AdjustParameters(p_main_img, img1);

  p_main_dis = CreateParameters(PARAMS_DISPLAY);
  SetParamsImageOffset(p_main_dis,0,0);

  p_depth_dis = CreateParameters(PARAMS_DISPLAY);
  SetParamsImageOffset(p_depth_dis,0,0);
  SetParamsDisplay(p_depth_dis, MULT_RED_TO_WHITE, NO_SHIFT, DISPLAY_RGB_MASK);


  EnableFrameOneShot(&DEV_VIDEO_IN1);
#ifdef STEPPER_MOTOR
  StartupSystemMSClock();
  TimerSetUserFunction(MillisecondFunction);
  SetSystemMSClockRate(5000);
#endif

  mode = 1;
  act_chan=1;
  while(mode != -1) {
    int i, j,*d;
    float scale;


    switch(mode) {
    case 1 :
      Graph_ClearDisplay(COLOR_BLACK);
      rgbbars(64);
      printf("Prescan \n");
      printf("s to start scanning\n");
      printf("d to display depth map\n");
      fprintf(iochanb,"Prescan \n\r");
      fprintf(iochanb,"s to start scanning\n\r");
      fprintf(iochanb,"d to display depth map\n\r");
      cnt = 0;
      
      CVMVIDEO_VideoMux(CVMVIDEO_MUX_OUTPUT0 | CVMVIDEO_MUX_CHAN0); 
      
      while(mode == 1) { 
	int *d;
	fprintf(iochana,"A %d\r",cnt++);
	fprintf(iochanb,"B %d\r",cnt++);
	EnableGrabOdd(&DEV_VIDEO_IN1);
	LED_Display(1);
	if ((cnt % 30)==15)  {
	  CVMVIDEO_VideoMux(CVMVIDEO_MUX_OUTPUT1 | CVMVIDEO_MUX_CHAN0);
	}
	if ((cnt & 30)==29) {
	  CVMVIDEO_VideoMux(CVMVIDEO_MUX_OUTPUT1 | CVMVIDEO_MUX_CHAN1);
	}
	WaitGrabComplete(&DEV_VIDEO_IN1);
	GrabImage(img1,p_main_img) ;
	d = depth->image;
	FindBright(img1, srch, d,BLUE) ;
	DisplayImage(img1,p_main_dis) ;
	
	if (char_ready(iochana)) {
	  ch = getc(iochana);
	  act_chan = 1;
	  if (ch == 's') mode = 2;
	  if (ch == 'd') mode = 3;
	}
	if (char_ready(iochanb)) {
	  ch = getc(iochanb);
	  act_chan = 2;
	  if (ch == 's') mode = 2;
	  if (ch == 'd') mode = 3;
	}
      }
    case 2 :
      
      k = 0;
#ifdef STEPPER_MOTOR
	table_set_pos(0);
	
	table_start();
#endif
	scale = (0x4000-0x600)/(YRES * 1.0);
	printf("Scanning - \n");
	printf("a to abort scan\n");
	printf("p to goto prescan\n");
	fprintf(iochanb,"Scanning - \n\r");
	fprintf(iochanb,"a to abort scan\n\r");
	fprintf(iochanb,"p to goto prescan\n\r");
	
	for(i=0; i<YRES; i++) {
	  fprintf(iochana,"%d  \r",i);
	  for(j=0;j<2;j++) {
	    EnableGrabOdd(&DEV_VIDEO_IN1);
	    LED_Display(2);
	    WaitGrabComplete(&DEV_VIDEO_IN1);
	    
	    GrabImage(img1,p_main_img) ;
	    d = depth->image + YRES * i;
	    FindBright(img1, srch, d,RED) ;
	    DisplayImage(img1,p_main_dis) ;
	  }
	  if (char_ready(iochana)) {
	    ch = getc(iochana);
	    if (ch == 'p') mode = 1;
	    if (ch == 'a') mode = 3;
	  } 
	  if (char_ready(iochanb)) {
	    ch = getc(iochanb);
	    if (ch == 'p') mode = 1;
	    if (ch == 'a') mode = 3;
	  } 
	  if (mode != 2) break;
	} /* for */
	if (mode == 3) {
	  printf("Scan aborted\n");
	  fprintf(iochanb,"Scan aborted\n\r");
	}
	mode = 3;
#ifdef STEPPER_MOTOR
	table_stop();
#endif
	break;
      case 3 :
	printf("Displaying depth map.\n");
	printf(" a,z to scale intensity \n");
	printf(" s,x to shift bias\n");
	printf(" D to dump depth map \n");
	printf(" p goto prescan\n");
	printf(" Q to quit program\n");

	fprintf(iochanb,"Displaying depth map.\n\r");
	fprintf(iochanb," a,z to scale intensity \n\r");
	fprintf(iochanb," s,x to shift bias\n\r");
	fprintf(iochanb," D to dump depth map \n\r");
	fprintf(iochanb," p goto prescan\n\r");
	fprintf(iochanb," Q to quit program\n\r");
	scale = 2.0;
	shift = -30;
	while(mode == 3) {
	  ScaleDepth(scale,shift,depth,sdepth);
	  DisplayImage(sdepth, p_depth_dis);
	  ch=-1;
	  while(ch == -1) {
	    if (char_ready(iochana)) ch=getc(iochana);
	    if (char_ready(iochanb)) ch=getc(iochanb);
	  }
	  switch(ch) {
	  case 'a' : scale *= 1.05; break;
	  case 'z' : scale /= 1.05; break;
	  case 's' : shift -= 3; break;
	  case 'x' : shift += 3; break;
	  case 'D' : DumpDepth(depth); break;
	  case 'p' : mode = 1; break;
	  case 'Q' : mode = -1; break;
	  }
	  printf("scale %f  shift %d\n",scale,shift);
	  fprintf(iochanb,"scale %f  shift %d\r\n",scale,shift);
	}

#ifdef STEPPER_MOTOR
	table_start();
#endif
	printf("*");    
	break;
      default : mode = -1; break;
      } /* switch */
  }
  printf("bye");
  TerminateVideo(&DEV_VIDEO_IN1);
  TerminateVideo(&DEV_VIDEO_OUT1);

}


#define FIX(a)  if (a<0) a=0; else if (a>63) a = 63;



key_board() {
  int i,ch;
  static int hue=32,sat=32,apl=32,sharp=32;
  if (char_ready(iochana)) {
    ch = getc(iochana);
    switch(ch) {
    case 'h' :  
      
    case 'n' :
      
    case 'a' : ++apl; break;
    case 'z' : --apl; break;
    case 's' : ++sharp; break;
    case 'x' : --sharp; break;
    case 'd' : ++sat; break;
    case 'c' : --sat; break;
    case 'f': ++hue; break;
    case 'v' : --hue; break;
    }
    FIX(sharp);
    FIX(apl);
    FIX(hue);
    FIX(sat);
      
    printf("%c apl=%d sharp=%d sat=%d hue=%d\n",ch,apl,sharp,sat,hue);
    send_sbx1762_iic_command(3,apl,sharp,sat,hue);
  }
  return ch;

}