/* (C) 1995 Digital Designs and Systems */

#include <stdio.h>

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

#include "disktbl.c"
#define MAX(a,b) (((a)>(b))?(a):(b))
#define MIN(a,b) (((a)<(b))?(a):(b))



FindBright(IMAGE *src, int *srch, int *res) {
  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) = RED;
  }
}


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

    cnt = 0;
  }
}

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;
  int *d;
  int xres;
  xres = depth->xres;
  d = depth->image;

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

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

  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("scanner3  - %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);

  Graph_ClearDisplay(COLOR_BLACK);

  rgbbars(64);
  printf("Prescan - press space to start scanning\n");
  while(1) { 
    int *d;
    printf("*");

    GrabImage(img1,p_main_img) ;
    d = depth->image;
    FindBright(img1, srch, d) ;
    DisplayImage(img1,p_main_dis) ;

    if (char_ready(iochana)) {
      ch = getc(iochana);
      if (ch == ' ') break;
    }
  }

  EnableFrameOneShot(&DEV_VIDEO_IN1);
  StartupSystemMSClock();
  TimerSetUserFunction(MillisecondFunction);
  SetSystemMSClockRate(5000);
  k = 0;
  table_set_pos(0);
  while(1) {
    int i, j,*d;
    float scale;
    
    table_start();
    scale = (0x4000-0x600)/(YRES * 1.0);
    printf("Scanning - Press space to stop scanning\n");
    for(i=0; i<YRES; i++) {
      printf("*");
      for(j=0;j<2;j++) {
	EnableGrabOdd(&DEV_VIDEO_IN1);
	WaitGrabComplete(&DEV_VIDEO_IN1);
	
	GrabImage(img1,p_main_img) ;
	d = depth->image + YRES * i;
	FindBright(img1, srch, d) ;
	DisplayImage(img1,p_main_dis) ;
      }
      if (char_ready(iochana)) {
	ch = getc(iochana);
	if (ch == ' ') break;
      }
    }
    table_stop();
    printf("Displaying depth map.\n");

    scale = 2.0;
    shift = -30;
    while(1) {
      ScaleDepth(scale,shift,depth,sdepth);
      DisplayImage(sdepth, p_depth_dis);
      while(!char_ready(iochana));
      ch=getc(iochana);
      if (ch == 'q' || ch == 'Q') break;
      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;
      }
      printf("scale %f  shift %d\n",scale,shift);
      fprintf(iochanb,"scale %f  shift %d\n",scale,shift);
    }
    table_start();
    printf("*");    
    if (ch == 'Q') break;
  }
  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;

}