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

/* this is a even faster scanner program! */
#include <stdio.h>

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

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 = -1;
    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*3 - 30;

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

main() {
  IMAGE *img1 , *depth;
  int *srch, *res;
  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("scanner1  - %s %s\n",__DATE__,__TIME__);

  InitializeCVMVIDEO();
  InitializeVideo(&DEV_VIDEO_IN1,FRAME_NTSC_NI);
  InitializeVideo(&DEV_VIDEO_OUT1,DISPLAY_MED_RES_NTSC);
  InitializeServo(SERIAL_PORT3);  /* APAI use this version for now */

  img1 = CreateImage(XRES, YRES, PACKED_RGB_24_2);
  depth = 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);
  EnableFrameOneShot(&DEV_VIDEO_IN1);
  k = 0;

  while(1) {
    int i,pos, *d;
    float scale;

    scale = (0x4000-0x600)/(YRES * 1.0);
    for(i=0; i<YRES; i++) {
      pos = 0x600 + i * scale;
      ServoSetPosition(1, pos);
      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) ;
    }
    ServoSetPositionVelocity(1, 0x1000, 0x20);
    DisplayImage(depth, p_depth_dis);
    while(!char_ready(iochana)) ;
    getc(iochana);
    printf("*");    
  }



}


#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 '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;

}