/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include <stdlib.h>
#include <stdio.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>

#include <FL/Fl.H>
#include <FL/Fl_Double_Window.H>
#include <FL/Fl_Input.H>
#include <FL/Fl_Pack.H>
#include <FL/Fl_Round_Button.H>
#include <Fl/Fl_Scroll.H>
#include <FL/Fl_Window.H>
#include <FL/fl_draw.H>

#include "../shared/image.h"
#include "../shared/ImageViewer.h"

#include "../../agent/Motion/Kinematics.h"
#include "../../agent/Vision/VisionInterface.h"
#include "../../agent/Vision/Vision.h"

bool LabellingMode = false;
static const int NumLabelButtons = 10;
static const int NumLabelButtonRows =  2;
static const int NumLabelButtonsPerRow =  NumLabelButtons / NumLabelButtonRows;

bool TrainingMode = false;

bool UsePlanarYUV = true;
bool ProfileMode = false;
bool SortMode    = false;
bool DisplayRaw  = false;

bool TestBWBallVision = false;

bool Interactive = true;

int RobotModelNum = 7;

static const int NumSortClasses = 6;

static int width =206;
static int height=160;
Kinematics kin(Kinematics::ERS7);
char* model="ERS7";

SImage *DebugImage=NULL;

FILE *sort_files[NumSortClasses];
FILE *train_file;

int RepeatCount=50;

extern bool ShouldThresholdImage;

//============================== gui support stuff declaration ==============================//

class StatusBox : public Fl_Box {
private:
  char labelStr[256];
public:
  StatusBox(int x,int y,int w,int h);
  ~StatusBox();

  virtual void resize(int x,int y,int w,int h);
  virtual void draw();
  virtual int handle(int event);

  void setLabel(const char *label);
};

class LabellingBox : public Fl_Round_Button {
private:
  char labelStr[256];
  Fl_Input *input;
  bool active;
public:
  LabellingBox(int x,int y,int w,int h);
  ~LabellingBox();

  virtual void resize(int x,int y,int w,int h);
  virtual void draw();
  virtual int handle(int event);

  void setLabel(const char *label);
  void setInput(Fl_Input *_input);
  const char *getLabel()
  {
    return labelStr;
  }
};

class LabelEntry : public Fl_Input {
public:
  LabelEntry(int x, int y, int w, int h) :
    Fl_Input(x,y,w,h)
  {}
};

//============================== VisTest declaration ==============================//

class LessThanStr{
public:
  bool operator()(const char *s1,const char *s2) const
  {
    return strcmp(s1,s2) < 0;
  }
};

class VisTest {
private:
  Vision vision;
  CMVision::image_idx<rgb> vis_img;
  CMVision::image<rgb> vis_img_raw;
  const char *imgFilename;

  SImage img;
  SImage imgProcessed;

  CMVision::image_yuv<uchar> vis_img_raw_robot;
  uchar *yuv_planar_data;

  VisionInterface::ObjectInfo objInfo;

  double head_angles[3];
  double body_angle;
  double body_height;

  // file name to region label map
  typedef std::map<const char *,RegionLabelMapT,LessThanStr> ImageLabelMapT;

  ImageLabelMapT imageLabelMap;

public:
  VisTest();
  ~VisTest();

  void init();
  void processFrame(int img_idx,const char *filename);

  SImage *getImg()
  { return &img; }
  SImage *getImgProcessed()
  { return &imgProcessed; }

  void labelPt(int x,int y,const char *label);
  void loadLabels(int num_imgs,char **img_filenames,const char *filename);
  void saveLabels(const char *filename) const;
};

//============================== VisTestWin declaration ==============================//

class VisTestWin : public Fl_Double_Window {
private:
  Fl_Scroll *scroll;
  ImageViewer *imgViewer;
  StatusBox *status;
  LabellingBox *labellers[NumLabelButtons];
  LabelEntry *labelEntry;

  bool keep_running;

  VisTest visTest;

  int imgIdx;
  int numImgs;
  char **imgFilenames;
  int lastFrameIdx;

public:
  VisTestWin(int num_imgs,char **img_filenames);
  ~VisTestWin();

  virtual void resize(int x,int y,int w,int h);
  virtual void draw();
  virtual int handle(int event);

  void run();
};

//============================== VisTest definition ==============================//

VisTest::VisTest() :
  vision(RobotModelNum==7)
{
  yuv_planar_data = NULL;
}

VisTest::~VisTest()
{
  delete[] yuv_planar_data;

  vision.close();

  for(ImageLabelMapT::iterator image_iter=imageLabelMap.begin();
      image_iter!=imageLabelMap.end();
      image_iter++){
    for(RegionLabelMapT::iterator reg_iter=(*image_iter).second.begin();
        reg_iter!=(*image_iter).second.end();
        reg_iter++){
      delete (*reg_iter).second;
    }
  }
  imageLabelMap.clear();
}

void VisTest::init()
{
  // Pass the image to vision in the exactly same format as on the
  // robot.

  yuv_planar_data = new uchar[3*width*height];

  char vision_cfg_fname[1024];
  char *dogroot;
  dogroot = getenv("DOGROOT");
  if(dogroot){
    strcpy(vision_cfg_fname,dogroot);
    strcat(vision_cfg_fname,"/util/vision_test/vision.cfg");
  }else{
    strcpy(vision_cfg_fname,"vision.cfg");
  }

  vision.initialize(vision_cfg_fname,width,height);
  vis_img.width =width;
  vis_img.height=height;
  vis_img.row_stride=width;

  vis_img_raw.width =width;
  vis_img_raw.height=height;
  vis_img_raw.row_stride=width;

  vis_img_raw_robot.width = width;
  vis_img_raw_robot.height = height;
  vis_img_raw_robot.row_stride = 3*width;
  vis_img_raw_robot.buf_y = yuv_planar_data;
  vis_img_raw_robot.buf_u = yuv_planar_data + width;
  vis_img_raw_robot.buf_v = yuv_planar_data + 2*width;

  for(int i=0; i<3; i++)
    head_angles[i]=0.0;
  body_angle=16.0 * M_PI / 180.0;
  body_height=104.0;

  kin.getHeadAngles(head_angles,vector3d(1000.0,0.0,199.216774326),body_angle,body_height);  

  imgFilename = NULL;
}

void VisTest::processFrame(int img_idx,const char *img_filename)
{
  imgFilename = img_filename;

  memset(&objInfo,0,sizeof(objInfo));

  char ang_filename[256];
  struct stat stats;

  img.allocate(width,height);
  imgProcessed.allocate(width,height);

  const char *img_basestart;
  bool raw_image=false;
  img_basestart = strrchr(img_filename,'/');
  if(img_basestart==NULL)
    img_basestart = img_filename;
  else
    img_basestart++;
  raw_image = (*img_basestart == 'i');

  strncpy(ang_filename,img_filename,256);
  ang_filename[255]=0;
  ang_filename[img_basestart-img_filename]='i';
  strcpy(&ang_filename[strlen(ang_filename)-3],"inf");
      
  if(stat(img_filename,&stats)==-1) {
    printf("could not stat %s\n",img_filename);
    return;
  }
      
  if(stat(ang_filename,&stats)==-1) {
    printf("could not stat %s\n",ang_filename);
    exit(123);
  } else {
    FILE *ang_file;
    double a0,a1,a2,a3,a4;
        
    if((ang_file=fopen(ang_filename,"rt"))==NULL)
      goto ang_error;
        
    if(fscanf(ang_file,"%lg %lg %lg %lg %lg",&a0,&a1,&a2,&a3,&a4)!=5)
      goto ang_error;
        
    body_angle     = a0;
    body_height    = a1;
    head_angles[0] = a2;
    head_angles[1] = a3;
    head_angles[2] = a4;
    goto ang_ok;
        
  ang_error:
    fprintf(stderr,"error reading angles from file\n");

  ang_ok:
    if(ang_file)
      fclose(ang_file);
  }
      
  if(!img.load(img_filename)) {
    printf("error loading file %s\n",img_filename);
    return;
  }
      
  if(img.h != height || img.w!=width){
    printf("image '%s' has wrong size w=%d h=%d, expected w=%d h=%d\n",
           img_filename,img.w,img.h,width,height);
    exit(5);
  }

  vis_img.buf    =img.img;
  vis_img_raw.buf=img.img;
      
  // Build YUV planar image 
  {
    uchar *y_buf = (uchar *)vis_img_raw_robot.buf_y;
    uchar *u_buf = (uchar *)vis_img_raw_robot.buf_u;
    uchar *v_buf = (uchar *)vis_img_raw_robot.buf_v;

    for(int h=0; h<height; h++) {
      for(int w=0; w<width; w++) {
        y_buf[w] = img.getpixel_fast(w, h).red;
        u_buf[w] = img.getpixel_fast(w, h).green;
        v_buf[w] = img.getpixel_fast(w, h).blue;
      }
      y_buf += vis_img_raw_robot.row_stride;
      u_buf += vis_img_raw_robot.row_stride;
      v_buf += vis_img_raw_robot.row_stride;
    }
  }

  // convert image to UYV for display
  if(raw_image){
    for(int y=0; y<img.h; y++){
      for(int x=0; x<img.w; x++){
        rgb *pix;
        pix = &img.img[y*img.w+x];
        swap(pix->red,pix->green);
      }
    }
  }
      
  vision.setHeadAngles(head_angles);
  vision.setBodyParams(body_angle,body_height);

  if(TrainingMode){
    RegionLabelsOld = &imageLabelMap[img_filename];
    printf("img file '%s' has %u label(s)\n",img_filename,RegionLabelsOld->size());
    RegionLabelsNew = new RegionLabelMapT;
  }

  ulong start_time,end_time;
  start_time = GetTime();
      
  for(int cnt=0; cnt<(ProfileMode ? RepeatCount : 1); cnt++){
    printf("processing frame %d ('%s')\n",img_idx,img_filename);
    if(!Interactive && (!ProfileMode || cnt==0))
      fprintf(stderr,"processing frame %d ('%s')\n",img_idx,img_filename);
    if(raw_image){
      if(UsePlanarYUV){
        vision.runLowLevelVision(vis_img_raw_robot);
      }else{
        vision.runLowLevelVision(vis_img_raw);
      }
    }else{
      ShouldThresholdImage = (cnt==0);
      vision.runLowLevelVision(vis_img);
    }
    if(Interactive){
      CMVision::RmapToRgb(imgProcessed.img,vision.rmap,vision.num_runs,width,height,vision.color,vision.num_colors);
    }
    if(raw_image){
      if(UsePlanarYUV)
        vision.runHighLevelVision(&objInfo, vis_img_raw_robot);
      else
        vision.runHighLevelVision(&objInfo,vis_img_raw);
    }else{
      vision.runHighLevelVision(&objInfo,vis_img);
    }
    
    if(!ProfileMode || cnt==0)
      printf("done processing frame %d ('%s')\n",img_idx,img_filename);
    
    if(SortMode) {
      if(0 <= DebugClass && DebugClass < NumSortClasses){
        fprintf(sort_files[DebugClass],"%02d %s %s\n",DebugClass,img_filename,DebugInfo);
      }else{
        fprintf(stderr,"invalid sort class %d (out of range)\n",DebugClass);
      }
    }
  }

  end_time = GetTime();

  double elapsed_time;
  elapsed_time = (end_time-start_time)/1.0e6;
  if(ProfileMode)
    elapsed_time /= RepeatCount;

  fprintf(stderr,"frame %d ('%s') took %g seconds\n",img_idx,img_filename,elapsed_time);

  if(TrainingMode){
    for(RegionLabelMapT::iterator reg_iter=RegionLabelsOld->begin();
        reg_iter!=RegionLabelsOld->end();
        reg_iter++){
      free((*reg_iter).second);
    }
    imageLabelMap[img_filename] = *RegionLabelsNew;
    //ImageLabelMapT::iterator image_iter;
    //image_iter = imageLabelMap.find(img_filename);
    //if(image_iter!=imageLabelMap.end()){
    printf("after vision, img file '%s' has %u label(s)\n",img_filename,RegionLabelsNew->size());
    for(RegionLabelMapT::iterator reg_iter=RegionLabelsNew->begin();
        reg_iter!=RegionLabelsNew->end();
        reg_iter++){
      fprintf(train_file,"%s\n",(*reg_iter).second);
    }
    //}
    delete RegionLabelsNew;
  }

  //for(int robot_num=0; robot_num<3; robot_num++) {
  //  VisionInterface::VObject *obj;
  //  obj = &objInfo.red_robots[robot_num];
  //  printf("red robot %d\n",robot_num);
  //  printf("  conf     %10g\n",obj->confidence);
  //  printf("  distance %10g left %10g right %10g\n",obj->distance,obj->left,obj->right);
  //  printf("  loc (%10g,%10g,%10g)\n",obj->loc.x,obj->loc.y,obj->loc.z);
  //}
  //
  //for(int robot_num=0; robot_num<3; robot_num++) {
  //  VisionInterface::VObject *obj;
  //  obj = &objInfo.blue_robots[robot_num];
  //  printf("blue robot %d\n",robot_num);
  //  printf("  conf     %10g\n",obj->confidence);
  //  printf("  distance %10g left %10g right %10g\n",obj->distance,obj->left,obj->right);
  //  printf("  loc (%10g,%10g,%10g)\n",obj->loc.x,obj->loc.y,obj->loc.z);
  //}
}

void VisTest::labelPt(int x,int y,const char *label)
{
  if(x<0 || x>=vis_img.width ||
     y<0 || y>=vis_img.height)
    return;

  printf("labelling (%d,%d) with '%s'\n",x,y,label);
  region *reg;
  reg = CMVision::FindRegion(vision.rmap,vision.num_runs,vision.reg,x,y);
  if(!reg){
    printf("reg: NULL\n");
    return;
  }

  printf("reg: color=%d bbox=([%d,%d],[%d,%d]) cen=(%g,%g) area=%d\n",
         reg->color,reg->x1,reg->x2,reg->y1,reg->y2,reg->cen_x,reg->cen_y,reg->area);

  ulong reg_id;
  reg_id = Vision::calcRegionId(reg);
  imageLabelMap[imgFilename][reg_id] = strdup(label);
}

void VisTest::saveLabels(const char *filename) const
{
  FILE *label_file;

  label_file = fopen(filename,"wt");
  if(label_file == NULL){
    fprintf(stderr,"unable to open label file ('%s') for writing\n",filename);
    return;
  }

  for(ImageLabelMapT::const_iterator image_iter=imageLabelMap.begin();
      image_iter!=imageLabelMap.end();
      image_iter++){
    for(RegionLabelMapT::const_iterator reg_iter=(*image_iter).second.begin();
        reg_iter!=(*image_iter).second.end();
        reg_iter++){
      printf("label: image='%s' reg_id=%lu label='%s'\n",
             (*image_iter).first,(*reg_iter).first,(*reg_iter).second);
      fprintf(label_file,
              "%s %lu %s\n",
              (*image_iter).first,(*reg_iter).first,(*reg_iter).second);
    }
  }

  fclose(label_file);
}

void VisTest::loadLabels(int num_imgs,char **img_filenames,const char *filename)
{
  printf("loading labels from file '%s'\n",filename);

  FILE *label_file;

  label_file = fopen(filename,"rt");
  if(label_file == NULL){
    fprintf(stderr,"unable to open label file ('%s') for reading\n",filename);
    return;
  }

  for(int i=0; i<num_imgs; i++){
    // create entry in map using this string pointer
    imageLabelMap[img_filenames[i]];
  }

  char buf[1024];
  while(fgets(buf,sizeof(buf),label_file)){
    char filename[1024];
    ulong reg_id=0;
    int chars_used=0;
    filename[0] = 0;
    if(sscanf(buf,"%s %lu %n\n",filename,&reg_id,&chars_used)>=2){
      char *label;
      int label_len;
      label = &buf[min(chars_used,(int)strlen(buf))];
      label_len = strlen(label);
      if(label[label_len-1]=='\n')
        label[label_len-1] = 0;
      printf("read label: image='%s' reg_id=%lu label='%s'\n",
             filename,reg_id,label);

      imageLabelMap[filename][reg_id] = strdup(label);
      //ImageLabelMapT::iterator img_iter;
      //img_iter = imageLabelMap.find(filename);
      //if(img_iter == imageLabelMap.end()){
      //  printf("ignoring label for image '%s' because image not being processed\n",filename);
      //}else{
      //  (*img_iter).second[reg_id] = strdup(label);
      //}
    }else{
      printf("no label found for line '%s'\n",buf);
    }
  }

  fclose(label_file);
}

//============================== gui support stuff definition ==============================//

StatusBox::StatusBox(int x,int y,int w,int h) : 
  Fl_Box(x,y,w,h)
{
  align(FL_ALIGN_INSIDE | FL_ALIGN_LEFT);
  box(FL_EMBOSSED_BOX);

  labelStr[0]=0;
  label(labelStr);
}

StatusBox::~StatusBox()
{
}

void StatusBox::resize(int x,int y,int w,int h)
{
  Fl_Box::resize(x,y,w,h);
}

void StatusBox::draw()
{
  Fl_Box::draw();
}

int StatusBox::handle(int event)
{
  return Fl_Box::handle(event);
}

void StatusBox::setLabel(const char *label)
{
  strncpy(labelStr,label,sizeof(labelStr)-1);
  labelStr[sizeof(labelStr)-1] = 0;

  redraw();
}

LabellingBox::LabellingBox(int x,int y,int w,int h) : 
  Fl_Round_Button(x,y,w,h)
{
  box(FL_EMBOSSED_BOX);
  type(FL_RADIO_BUTTON);

  labelStr[0]=0;
  strcpy(labelStr,"side");
  label(labelStr);

  input = NULL;
}

LabellingBox::~LabellingBox()
{
}

void LabellingBox::resize(int x,int y,int w,int h)
{
  Fl_Round_Button::resize(x,y,w,h);
}

void LabellingBox::draw()
{
  Fl_Round_Button::draw();
}

int LabellingBox::handle(int event)
{
  bool used=false;

  switch(event){
    case FL_PUSH:
      {
        switch(Fl::event_button()){
          case 2:
          case 3:
            setLabel(input->value());
            redraw();
            used = true;
            break;
        }
      }
      break;
  }

  if(used)
    return 1;

  return Fl_Round_Button::handle(event);
}

void LabellingBox::setLabel(const char *label)
{
  strncpy(labelStr,label,sizeof(labelStr));
  labelStr[sizeof(labelStr)] = 0;

  redraw();
}

void LabellingBox::setInput(Fl_Input *_input)
{
  input = _input;
}

//============================== VisTestWin definition ==============================//

VisTestWin::VisTestWin(int num_imgs,char **img_filenames) : 
  Fl_Double_Window(0,0,3*width,3*height+20+(LabellingMode?20+20*NumLabelButtonRows:0))
{
  scroll = new Fl_Scroll(0,0,3*width,3*height);
  //scroll->type(Fl_Scroll::BOTH_ALWAYS);
  imgViewer = new ImageViewer(0,0,width,height);
  imgViewer->setZoom(3,3);
  imgViewer->show();
  scroll->end();
  scroll->show();

  status = new StatusBox(0,3*height,3*width,20);
  status->show();

  if(LabellingMode){
    for(int row=0; row<NumLabelButtonRows; row++){
      int width_left=3*width;
      int x_pos=0;
      for(int col=0; col<NumLabelButtonsPerRow; col++){
        int width_here;
        int idx;
        width_here = width_left/(NumLabelButtonsPerRow-col);
        idx = row*NumLabelButtonsPerRow+col;
        labellers[idx] = new LabellingBox(x_pos,3*height+20+20*row,width_here,20);
        if(idx == 0)
          labellers[idx]->set();
        labellers[idx]->show();
        x_pos += width_here;
        width_left -= width_here;
      }
    }
    labelEntry = new LabelEntry(0,3*height+20+20*NumLabelButtonRows,3*width,20);
    for(int i=0; i<NumLabelButtons; i++){
      labellers[i]->setInput(labelEntry);
    }
  }else{
    for(int i=0; i<NumLabelButtons; i++){
      labellers[i] = NULL;
    }    
    labelEntry = NULL;
  }

  resizable(scroll);

  keep_running = true;

  imgIdx=0;
  lastFrameIdx=-1;

  numImgs = num_imgs;
  imgFilenames = img_filenames;

  visTest.init();
}

VisTestWin::~VisTestWin()
{
}

void VisTestWin::resize(int x,int y,int w,int h)
{
  Fl_Double_Window::resize(x,y,w,h);
}

void VisTestWin::draw()
{
  if(DisplayRaw)
    imgViewer->setImage(visTest.getImg());
  else
    imgViewer->setImage(visTest.getImgProcessed());

  Fl_Double_Window::draw();
}

int VisTestWin::handle(int event)
{
  int ret=0;

  switch(event){
    case FL_KEYDOWN:
      {
        int key = Fl::event_key();
        switch(key){
          case FL_Escape:
          case 'q':
            ret = 1;
            keep_running = false;
            break;
          case ' ':
          case '[':
          case FL_Right:
          case FL_Down:
          case FL_Page_Down:
            ret = 1;
            imgIdx++;
            break;
          case 'b':
          case ']':
          case FL_BackSpace:
          case FL_Left:
          case FL_Up:
          case FL_Page_Up:
            ret = 1;
            imgIdx--;
            break;
          case 'r':
            ret = 1;
            lastFrameIdx = -1; // forget you already did this frame
            break;
          case 's':
            ret = 1;
            visTest.saveLabels("labels.txt");
            break;
        }
      }
      break;
    case FL_PUSH:
      {
        int x,y;
        x = Fl::event_x();
        y = Fl::event_y();

        ret = 1;

        imgViewer->translatePoint(x,y,x,y);

        bool update_label_w_position=false;

        if(LabellingMode){
          switch(Fl::event_button()){
            case 1:
              {
                int labeller_idx=0;
                for(int i=1; i<NumLabelButtons; i++){
                  if(labellers[i]->value())
                    labeller_idx = i;
                }
                visTest.labelPt(x,y,labellers[labeller_idx]->getLabel());
              }
              break;
            case 2:
              break;
            case 3:
              update_label_w_position = true;
              break;
          }
        }else{
          update_label_w_position = true;
        }

        if(update_label_w_position){
          char buf[256];
          sprintf(buf,"x=%03d y=%03d\n",x,y);
          status->setLabel(buf);
        }
      }
      break;
  }

  if(imgIdx < 0) {
    printf("already at first image\n");
    imgIdx = 0;
  }
  if(imgIdx >= numImgs) {
    printf("no more images\n");
    imgIdx = numImgs-1;
  }

  if(imgIdx!=lastFrameIdx) {
    visTest.processFrame(imgIdx,imgFilenames[imgIdx]);
    lastFrameIdx = imgIdx;
    redraw();
  }

  int dw_ret;
  dw_ret = Fl_Double_Window::handle(event);
  return (ret || dw_ret);
}

void VisTestWin::run()
{
  if(DisplayRaw)
    DebugImage = visTest.getImg();
  else
    DebugImage = visTest.getImgProcessed();

  if(imgIdx!=lastFrameIdx){
    visTest.processFrame(imgIdx,imgFilenames[imgIdx]);
    lastFrameIdx = imgIdx;
    redraw();
  }

  while(keep_running) {
    Fl::check();

    usleep(10000);
  }
}

int main(int argc, char *argv[])
{
  const char *label_filename=NULL;

  int next_opt;
  for(next_opt=1; next_opt < argc && argv[next_opt][0]=='-'; next_opt++) {
    char *opt;
    opt = argv[next_opt];
    if(strcmp(opt,"--sort")==0) {
      SortMode = true;
    } else if(strcmp(opt,"--train")==0) {
      TrainingMode = true;
      next_opt++;
      if(!(next_opt < argc)) {
        fprintf(stderr,"must specify label filename after --train\n");
        exit(6);
      }
      label_filename = argv[next_opt];
    } else if(strcmp(opt,"--profile")==0) {
      ProfileMode = true;
    } else if(strcmp(opt,"--raw")==0) {
      DisplayRaw = true;
    } else if(strcmp(opt,"--label")==0) {
      LabellingMode = true;
    } else if(strcmp(opt,"--model")==0) {
      next_opt++;
      if(!(next_opt < argc)) {
        fprintf(stderr,"must specify numeric argument after --model\n");
        exit(6);
      }
      RobotModelNum = atoi(argv[next_opt]);      
    } else if(strcmp(opt,"--repeat")==0) {
      next_opt++;
      if(!(next_opt < argc)) {
        fprintf(stderr,"must specify numeric argument after --repeat\n");
        exit(5);
      }
      RepeatCount = atoi(argv[next_opt]);
    }
  }
  argc -= next_opt;
  argv = &argv[next_opt];

  Interactive = !(ProfileMode || SortMode || TrainingMode);

  switch(RobotModelNum){
    case 7:
      width  = 208;
      height = 160;
      kin = Kinematics(Kinematics::ERS7);
      model = "ERS7";
      break;
    case 210:
      width  = 176;
      height = 144;
      kin = Kinematics(Kinematics::ERS210);
      model = "ERS210";
      break;
    default:
      printf("invalid robot model number %d\n",RobotModelNum);
      exit(10);
  }

  if(SortMode){
    for(int sort_idx=0; sort_idx<NumSortClasses; sort_idx++){
      char filename[256];
      sprintf(filename,"%01d.vfl",sort_idx);
      if((sort_files[sort_idx]=fopen(filename,"w"))==NULL){
        fprintf(stderr,"couldn't open file '%s' for writing",filename);
        exit(7);
      }
    }
  }

  if(TrainingMode){
    char filename[256];
    sprintf(filename,"train.txt");
    if((train_file=fopen(filename,"w"))==NULL){
      fprintf(stderr,"couldn't open file '%s' for writing",filename);
      exit(7);
    }
  }

  //xw.initialize();
  //xw.createWindow(win,width,height,"Vision Test");
  //win_pix.initialize(&win,width,height);

  if(argc < 1) {
    fprintf(stderr,"must specify at least one image file\n");
    exit(1);
  }

  if(Interactive){
    Fl::visual(FL_RGB);

    VisTestWin *vis_test_win=NULL;
    vis_test_win = new VisTestWin(argc,&argv[0]);
    vis_test_win->show();

    vis_test_win->run();

    vis_test_win->hide();
    delete vis_test_win;
  }else{
    VisTest vis_test;
    vis_test.init();
    vis_test.loadLabels(argc,argv,label_filename);
    for(int img_idx=0; img_idx<argc; img_idx++){
      vis_test.processFrame(img_idx,argv[img_idx]);
    }
  }

  if(SortMode) {
    for(int sort_idx=0; sort_idx<NumSortClasses; sort_idx++) {
      fclose(sort_files[sort_idx]);
    }
  }
  
  return(0);
}