#include "../libseemicro/seemicro.h"
#include <iostream>
#include <sstream>
#include <list>
#include <iomanip>

extern "C"
{
  #include <opencv/highgui.h>
}

using namespace std;
using namespace seemicro;

MicroAdam adam;// wird die erkannten Parameter enthalten
MicroEva eva;

videoDevice *vdev;
int devnr=2;
string devicestring = "/dev/video2";
int reqWidth = 0;
int reqHeight = 0;

string logfilename = "seemicro.log";
bool doStartLogging = false;

bool startSerial = true;
string serialDevice = "/dev/ttyS0";

ImageFile *im = NULL;
PreviewOutput *pr = NULL;
CannyOutput *cn = NULL;
CorrelationOutput *cr = NULL;
SearchRadiusOutput *so = NULL;
EinzeichnenOutput *ei = NULL;
GraphOutput *gr = NULL;
LogOutput *lg = NULL;
FindLEDSOutput *io = NULL;
HSVColorChooser *hsv = NULL;
Homeo2Serial *homeo = NULL;

MicroController *mc = NULL;

int mode = 0;// 0 = recognize adam, 1 = recognize eva
int savedFrame = 0;// number of saved frames

CvFont font;

long timeForWaitKey, timeForWaitKeyKey;

void buildAll(ImageSource *src)
{
  Micro *m = &adam;
  if(mode==1) m = &eva;

  mc = new MicroController(src, m);

  if(DEBUGLEVEL>=2) cerr << "\nCreating Output Modules...";

  if(mode==0)
  {
    ei = new EinzeichnenOutput(*mc->ca, *mc->se, &font);
    if(DEBUGLEVEL>=2) cerr << "einzeichnen ";
    ei->registerMe(mc->outputList);
  }
  else if(mc->sleds)
  {
    io = new FindLEDSOutput(*mc->sleds, "SearchLEDS", &font);
    io->registerMe(mc->outputList);
  }

  if(src==vdev)
  {
    gr = new GraphOutput(*m);
    if(DEBUGLEVEL>=2) cerr << "graph ";
    gr->registerMe(mc->outputList);

    if(doStartLogging)
    {
      doStartLogging = false;
      lg = new LogOutput(m, logfilename);
      if(DEBUGLEVEL>=2) cerr << "logging to '" << logfilename << "'\n";
      lg->registerMe(mc->outputList);
    }

    if(startSerial)
    {
      homeo = new Homeo2Serial(*m, serialDevice);
      homeo->registerMe(mc->outputList);
    }
  }

  cvMoveWindow("Canny",		  0,  580);
  cvMoveWindow("Correlation",	 650,   0);
  cvMoveWindow("SearchRadius",	 350,   0);
  cvMoveWindow("SearchLEDS",	 320,   0);
  cvMoveWindow("Eingezeichnet",  650, 580);
  cvMoveWindow("HSVColorChooser",790,   0);
  cvMoveWindow("Graph",		 320, 350);
  cvMoveWindow("Homeo2Serial",	 650, 350);
}

/**
  * Destroy things allocated with new
  */
void deleteAll(void)
{
  if(mc) delete mc;mc = NULL;
  if(pr) delete pr;pr = NULL;
  if(cn) delete cn;cn = NULL;
  if(cr) delete cr;cr = NULL;
  if(so) delete so;so = NULL;
  if(ei) delete ei;ei = NULL;
  if(gr) delete gr;gr = NULL;
  if(lg) delete lg;lg = NULL;
  if(io) delete io;io = NULL;
  if(homeo) delete homeo;homeo = NULL;
}

void startVideoMode(void)
{
  // start video mode
  try
  {
    vdev = new videoDevice(devicestring,
	// (mode==0) ? VIDEO_PALETTE_GREY :
	VIDEO_PALETTE_RGB24,
	-1, reqWidth, reqHeight);
  }
  catch(bad_alloc)
  {
    cerr << "Failed to enter video mode: Could not open device or out of memory.\n";
    return;
  }

  // chain neu aufbauen
  deleteAll();
  buildAll(vdev);
  mc->handleFrame();

  cout << "Enter 'v' again to stop videomode.\n";
}

void help(void)
{
  cout << "Help\n"
       << "----\n\n"
       << "Distance/Angle resolution is the number of accumulator cells.\n"
       << "More cells -> more precision, more resources.\n\n"
       << "Keys to enter into windows (not this xterm!):\n"
       << "\t'a' for adam mode\n"
       << "\t'e' for eva mode\n"
       << "\t'q' to quit\n"
       << "\t'm' for radius search\n"
       << "\t's' for angles search (adam)\n"
//       << "\t'r' for full recognition\n"
       << "\t'i' in video mode: save input frame as frameX.pbm (X starts at 0)\n"
       << "\t'v' for video mode\n"
       << "\t'h' to get this help again\n"
#ifdef DEBUG
       << "\t'd' decrease debug level\n"
       << "\t'D' increase debug level\n"
#endif
       << "\t'p' toggle preview window\n"
       << "\t'S' toggle searchradius window\n"
       << "\t'c' toggle canny window\n"
       << "\t'C' toggle correlation window\n"
       << "\t'H' toggle hsvcolorchooser window\n"
       << "\t'f' toggle SearchLEDS window\n"
       << "\t'F' select SearchLEDS background\n"
       << "\tnumber to select video device.\n\n";
}

/**
  * Liefert false, wenn 'q' gedrückt wurde,
  * sonst true
  */
bool handleKeys(void)
{
  int k;

  mc->startTimer();
  k = cvWaitKey(5);// 5 msec waiting time & time to redraw windows
  mc->stopTimer();
  if(mc->keyframe) timeForWaitKeyKey = mc->getTimer();
  timeForWaitKey = mc->getTimer();

  if(k==-1) return true;
  //cerr << "handleKey: " << k << "\n";
  switch(k)
  {
    case 'q':
      return false;

    case 'a':
      if(mode==0) return true;
      if(vdev) return true;
      cout << "Switching to Adam mode...";
      mode = 0;
      deleteAll();
      buildAll(im);
      mc->handleFrame();
      return true;

    case 'e':
      if(mode==1) return true;
      if(vdev) return true;
      cout << "Switching to Eva mode...";
      mode = 1;
      deleteAll();
      buildAll(im);
      mc->handleFrame();
      return true;

    case 'm':
      cerr << "Starting Radius Search...";
      adam.radiusValid = false;
      eva.radiusValid = false;
      mc->co->paramChanged = true;
      return true;

    case 's':
      cerr << "Starting Angle Search...";
      adam.alphaValid = false;// ist implizites adam.betaValid = false;
      eva.alphaValid = false;// ist implizites adam.betaValid = false;
      return true;

    case 'r':
      adam.radiusValid = false;
      adam.alphaValid = false;
      eva.radiusValid = false;
      eva.alphaValid = false;
      return true;

    case '0':
    case '1':
    case '2':
    case '3':
    case '4':
    case '5':
    case '6':
    case '7':
    case '8':
    case '9':
      {
	devnr = k - '0';
	stringstream ss (stringstream::in | stringstream::out);
	ss << "/dev/video" << devnr;
	ss >> devicestring;
	cout << "Selected device " << devicestring << ".\n";
	return true;
      }

    case 'v':
      if(!vdev)
      {
	startVideoMode();
	return true;
      }

      cout << "Leaving video mode.\n";

      // chain neu aufbauen
      deleteAll();
      delete vdev;vdev=0;
      buildAll(im);
      return true;
    case 'i':
      if(!vdev) return true;
      {
	stringstream ss(stringstream::in | stringstream::out);
      ss << "frame" << savedFrame << ".pbm";
      string fname;
      ss >> fname;
      cvSaveImage(fname.c_str(), vdev->img);
      cerr << "Saved image '" << fname << "'.\n";
      savedFrame++;
      }
      return true;
    case 'h':
      help();
      return true;
//#ifdef DEBUG
    case 'd':
      if(DEBUGLEVEL>0) DEBUGLEVEL--;
      cerr << "Debug level set to " << DEBUGLEVEL
       	<< " (0 = fatal, 1 = warn, 2 = info, 3 = flood)\n";
      return true;
    case 'D':
      if(DEBUGLEVEL<3) DEBUGLEVEL++;
      cerr << "Debug level set to " << DEBUGLEVEL
       	<< " (0 = fatal, 1 = warn, 2 = info, 3 = flood)\n";
      return true;
//#endif
    case 'p':
      if(pr)
      {
        cout << "preview off\n";
	mc->outputList.remove(pr);
	delete pr; pr = NULL;
	return true;
      }

      {
	ImageSource* src = vdev ? dynamic_cast<ImageSource*>(vdev) :
	  dynamic_cast<ImageSource*>(im);
	pr = new PreviewOutput(src, &font);
	cout << "preview on\n";
	pr->registerMe(mc->outputList);
	return true;
      }
    case 'c':
      if(cn)
      {
        cout << "canny off\n";
	mc->outputList.remove(cn);
	delete cn; cn = NULL;
	return true;
      }

      cn = new CannyOutput(*mc->ca);
      cout << "canny on\n";
      cn->registerMe(mc->outputList);
      return true;
    case 'C':
      if(cr)
      {
        cout << "correlation off\n";
	mc->outputList.remove(cr);
	delete cr; cr = NULL;
	return true;
      }

      cr = new CorrelationOutput(*mc->co);
      cerr << "correlation on\n";
      cr->registerMe(mc->outputList);
      return true;
    case 'S':
      if(so)
      {
        cout << "searchradius off\n";
	mc->outputList.remove(so);
	delete so; so = NULL;
	return true;
      }
      so = new SearchRadiusOutput(*mc->sr, &font);
      cout << "searchradius on\n";
      so->registerMe(mc->outputList);
      return true;
    case 'H':
      if(hsv)
      {
        cout << "hsv off\n";
	mc->outputList.remove(hsv);
	delete hsv; hsv = NULL;
	return true;
      }
      hsv = new HSVColorChooser(*mc->range);
      cout << "hsv on\n";
      hsv->registerMe(mc->outputList);
      return true;

    case 'f':
      if(!mc->sleds) return true;
      if(io)
      {
        cout << "searchleds off\n";
	mc->outputList.remove(io);
	delete io; io = NULL;
	return true;
      }
      io = new FindLEDSOutput(*mc->sleds, "SearchLEDS", &font);
      io->registerMe(mc->outputList);
      cout << "searchleds on\n";
      return true;
    case 'F':
      if(io)
      {
	if(io->whichSrc==1)
	{
	  io->whichSrc = 0;
	  cout << "Using FloodFill's marker image as background for SearchLEDSOutput\n";
	}
	else
	{
	  io->whichSrc = 1;
	  cout << "Using FloodFill's input image as background for SearchLEDSOutput\n";
	}
      }
      return true;
   case 'l':
      if(lg)
      {
        cout << "logging off\n";
	mc->outputList.remove(lg);
	delete lg; lg = NULL;
	return true;
      }

      {
	Micro *m = &adam;
	if(mode==1) m = &eva;
	lg = new LogOutput(m, logfilename);
	cout << "logging is now on\n";
	lg->registerMe(mc->outputList);
	return true;
      }

  } // switch()

  cerr << "Unknown key.";
  return true;
}

void cmdlineHelp(char **argv)
{
  cout << argv[0] <<
    " [--help | --image=file.jpg | --dev=/dev/video0 [--width=x --height=y]]\n"
  << "\t\t[--mode=adam|eva] [--debug=level] [--log=seemicro.log]\n\n"
  << " --help\n"
  << " --image=file.jpg - load image from file.jpg and recognize\n"
  << "\tOpenCV supports:\n"
  << "\t\tWindows bitmaps - BMP, DIB\n"
  << "\t\tJPEG files - JPEG, JPG, JPE\n"
  << "\t\tPortable Network Graphics - PNG\n"
  << "\t\tPortable image format - PBM, PGM, PPM\n"
  << "\t\tSun rasters - SR, RAS\n"
  << "\t\tTIFF files - TIFF, TIF\n"
  << " --dev=/dev/video0 - open this video device without changing preset image size\n"
  << " --width=x - request video of x pixels width\n"
  << " --height=y - request video of y pixels height\n"
  << " --log=file.log\n"
  << " --mode=adam|eva - Set initial recognition mode\n"
  << " --debug=level - Set debug level to 0, 1, 2, 3\n"
  << " also: standard x toolkit options are accepted, eg. -fg green, see X(7) OPTIONS\n\n";
  exit(0);
}

int main(int argc, char **argv)
{
  // it changes argv (but not argc) to remove x toolkit options
  cvInitSystem(argc, argv);

  cvInitFont(&font, CV_FONT_VECTOR0, 0.5, 1.0, 0.0, 1);

  string imagefilename = "../adam.jpg";
  bool doStartVideoMode = false;
  for (int i=1; i<argc; i++)
  {
    if(!argv[i]) break;// oder continue?
    if(!strcmp("--help", argv[i]))
      cmdlineHelp(argv);
    if(!strncmp("--image=", argv[i], 8))
    {
      doStartVideoMode = false;
      imagefilename = argv[i]+8;
      continue;
    }
    if(!strncmp("--dev=", argv[i], 6))
    {
      doStartVideoMode = true;
      devicestring = argv[i]+6;
      continue;
    }
    if(!strncmp("--log=", argv[i], 6))
    {
      doStartLogging = true;
      logfilename = argv[i]+6;
      continue;
    }
    if(!strncmp("--width=", argv[i], 8))
    {
      reqWidth = atoi(argv[i]+8);
      continue;
    }
    if(!strncmp("--height=", argv[i], 9))
    {
      reqHeight = atoi(argv[i]+9);
      continue;
    }

    if(!strncmp("--mode=", argv[i], 7))
    {
      if(!strcmp("adam", argv[i]+7))
      {
	mode=0;
	continue;
      }
      if(!strcmp("eva", argv[i]+7))
      {
	mode=1;
	continue;
      }
      cerr << "Invalid option: " << argv[i] << endl;
      exit(1);
    }
    if(!strncmp("--debug=", argv[i], 8))
    {
      int l = argv[i][8] - '0';
      if(l<=3 && l>=0)
      {
	DEBUGLEVEL=l;
	continue;
      }
      cerr << "Invalid option: " << argv[i] << endl;
      exit(1);
    }
    cerr << "Unknown option: " << argv[i] << endl;
    exit(1);
  }

  // Build ImageFile-Chain for Adam
  im = new ImageFile(imagefilename);
  {
//    reqWidth = im->img->width;
//    reqHeight = im->img->height;
  }
  if(DEBUGLEVEL>=2) cout << "Loaded image. Size: " <<
    im->img->width << "x" << im->img->height <<"\n";

  if(doStartVideoMode) startVideoMode();
  else buildAll(im);

  if(DEBUGLEVEL>=2) cerr << "Handling First Frame... ";
  mc->handleFrame();
  if(DEBUGLEVEL>=2) cerr << "Done.\n";

  help();

  if(DEBUGLEVEL>=2) cerr << "Entering loop...\n";
  while(1)
  {
    if(vdev)
    {
      mc->handleFrame();
    }
    else
    {
      mc->reEvalIfParamChanged();
    }
    if(!handleKeys()) break;

    if(vdev)
    {
#if 1
      cerr << "\rmsecs:: recognize/output/waitkey Normal: "
	<< setw(4) << mc->timeForFrame/1000 << "/"
	<< setw(4) << mc->timeForFrameOut/1000 << "/"
	<< setw(3) << timeForWaitKey/1000 << " Key: "
	<< setw(4) << mc->timeForKeyFrame/1000 << "/"
	<< setw(4) << mc->timeForKeyFrameOut/1000 << "/"
	<< setw(3) << timeForWaitKeyKey/1000 << " ";
#endif

      static bool alreadyWarned = false;
      if(!alreadyWarned)
      {
	long usecsperframe = mc->timeForFrame + mc->timeForFrameOut + timeForWaitKey;
	if(usecsperframe > 1e6/vdev->keyFrameRate && DEBUGLEVEL>0)
	{
	  cerr << "Your system seems to be too slow for this. Time taken: " << usecsperframe << " usecs. Time allowed: "
	    << (1e6/vdev->keyFrameRate) << "\n";
	  alreadyWarned = true;
	}
      }
    }
  } // while

  if(DEBUGLEVEL>=2) cerr << "DeleteAll()... ";
  deleteAll();
  if(DEBUGLEVEL>=2) cerr << "Done.\n";
}

