#ifndef FINDLEDSMODULE_H
#define FINDLEDSMODULE_H

#include "../opencvext.h"
#include "imagesource.h"
#include <iostream>
#include <list>
#include "../microeva.h"
#include "../microadam.h"
#include "../image.h"
#include "correlation.h"
#include "hsvrange.h"

using namespace std;

namespace seemicro
{

/**
  * Maximale Anzahl an zu vergebenden Markierungen
  * 5 sollten zwar für 5 LEDs reichen,
  * aber es gibt aj so viel Spam heutzutage, insbesondere
  * weil die LEDs teilweise nur grau statt grün leuchten.
  */
const uchar MAXMARKS = 100;

/**
  * Finden der Leuchtdioden von micro.Eva
  * mittels 1) FloodFill der grünen Bildbereiche und danach
  * 2) Schwerpunktberechnung.
  * Abgeleitet vom ImageSource, um das Markerbild einfach
  * speichern und debuggen zu können. Eine wirkliche
  * Ausgabe des Bildes ist jedoch nicht vorgesehen.
  */
struct FindLEDSModule : public ImageSource
{
  /**
    * RGB-Eingangsbild
    */
  ImageSource& src;

  /**
    * da kommt unsere Ausgabe hin
    */
  Micro& micro;
  MicroEva *eva;

  /**
    * um festzustellen, wann sich eva.radius ändert
    */
  CorrelationModule &corr;

  HSVRange& range;

  myrect ROI;

  int w, h;// Abkürzungen für img->width, img->height

  /**
    * zur Online-Schwerpunktberechnung (während Floodfill):
    *
    */
  mypoint markierungen[MAXMARKS];

  /**
    * Anzahl der Pixel, die diesen Schwerpunkt bilden,
    */
  int anzahl[MAXMARKS];

  /**
    * Abstand zum Radmittelpunkt, normiert zum Radius
    */
  float abstand[MAXMARKS];

  /**
    * Anzahl der Vergebenen marken
    */
  unsigned short int markiert;

  /**
    * Winkel zwischen positiver x-Achse und LED-Position, relativ zur Radmitte
    */
  float gamma[MAXMARKS];

  /**
    * Winkel bei am Rand anliegendem Arm, relativ zur Radmitte
    */
  float gamma0[MAXMARKS];

  /**
    * Winkeldifferenz zwischen gamma0 und gamma
    */
  float beta[MAXMARKS];

  /**
    * gemerkt: Zwischenergebnis bei Suche nach den konsistentesten
    * LED-Positionen.
    * gewählt: Welcher Schwerpunkt (LED) wurde welcher Armnummer zugeordnet
    * armnummer[]: welche armnummer hat welcher scherpunkt?
    */
  char gemerkt[MAXMARKS], gewaehlt[MAXMARKS], armnummer[MAXMARKS];

  FindLEDSModule(ImageSource& Asrc, Micro& Am, CorrelationModule& Ac,
    HSVRange& Ar) :
    src(Asrc), micro(Am), corr(Ac), range(Ar)
  {
    eva = dynamic_cast<MicroEva*>(&micro);
    if(src.img->nChannels != 3)
    {
      cerr << "FindLEDSModule(): Eingabebild muß RGB-Bild sein!\n";
      throw bad_alloc();
    }
    img = cvCreateImage(cvSize(src.img->width, src.img->height), IPL_DEPTH_8U, 1);
    w = img->width;
    h = img->height;
  }

  ~FindLEDSModule()
  {
    cvReleaseImage(&img);
  }

  /**
    * stellt fest, ob ein Pixel im Quellbild die Objektfarbe hat
    */
  inline bool remarkable(const int x, const int y, const HSVRange& range)
  {
    int pix = getPixel8u_3(src.img, x, y);
    unsigned short pH, pS, pV;
    rgb2hsv(pix, &pH, &pS, &pV);

    if(pH>range.hmax) return false;
    if(pH<range.hmin) return false;
    if(pS>range.smax) return false;
    if(pS<range.smin) return false;
    if(pV>range.vmax) return false;
    if(pV<range.vmin) return false;
    return true;
  }

  /**
    * markiert angegebenes Pixel und sucht rechts/unten rekursiv nach weiteren
    * Objektpixeln
    */
  inline void recursive_mark(const int x, const int y, const uchar m, const HSVRange& range)
  {
    putPixel8u_1(img, x, y, m);
    markierungen[m-1].p.x += x;
    markierungen[m-1].p.y += y;
    anzahl[m-1]++;

    if(x+1<w && getPixel8u_1(img, x+1, y)==0 && remarkable(x+1, y, range)) recursive_mark(x+1, y, m, range);
    if(y+1<h)
    {
      if(          getPixel8u_1(img, x  , y+1)==0 && remarkable(x, y+1, range)) recursive_mark(x, y+1, m, range);
      if(x+1<w  && getPixel8u_1(img, x+1, y+1)==0 && remarkable(x+1, y+1, range)) recursive_mark(x+1, y+1, m, range);
      if(x-1>=0 && getPixel8u_1(img, x-1, y+1)==0 && remarkable(x-1, y+1, range)) recursive_mark(x-1, y+1, m, range);
    }
  }

  inline void FloodFill(IplImage *img, myrect& ROI, unsigned short int& markiert, const HSVRange& range)
  {
    cvZero(img);// alte Markierungen löschen
    markiert = 0;

    for(int x=ROI.r.x; x<ROI.r.x+ROI.r.width; x++)
      for(int y=ROI.r.y; y<ROI.r.y+ROI.r.height; y++)
      {
        if(!remarkable(x, y, range)) continue;
	unsigned short int m = getPixel8u_1(img, x, y);
	if(m!=0) continue;

        // Womit ist rechts oder drunter markiert?
	if(x+1<w) m = getPixel8u_1(img, x+1, y);
	if(m==0 && y+1<h)
	{
	                     m = getPixel8u_1(img, x  , y+1);
	  if(m==0 && x+1<w)  m = getPixel8u_1(img, x+1, y+1);
	  if(m==0 && x-1>=0) m = getPixel8u_1(img, x-1, y+1);
	}

	if(m==0)
	{
	  // Neue Markierung vergeben
	  if(markiert<MAXMARKS-1) markiert++;
	  m = markiert;
	  markierungen[m-1] = mypoint(0, 0);
	  anzahl[m-1] = 0;
	}
        recursive_mark(x, y, m, range);
      } // for()

    if(DEBUGLEVEL>=3) cerr << "Vergebene Markierungen: " << markiert << endl;

    // Schwerpunktbildung: sum(x|y)/anzahl der pixel
    for(int i=0; i<markiert; i++)
    {
      markierungen[i].p.x /= anzahl[i];
      markierungen[i].p.y /= anzahl[i];
      if(DEBUGLEVEL>=3)
        cerr << "#" << i << " (" << markierungen[i].p.x << " " <<
          markierungen[i].p.y << ") " << anzahl[i] << " pix\n";
    }
  }

  void FloodFillMarks2Colors(IplImage *out)
  {
    for(int x=0; x<img->width; x++)
      for(int y=0; y<img->height; y++)
      {
	uchar m = getPixel8u_1(img, x, y);
	int n = 0;
	switch(m)
	{
	  case 0: break;
	  case 1: n = CV_RGB(255, 0, 0);break;// rot
	  case 2: n = CV_RGB(0, 255, 0);break;// grün
	  case 3: n = CV_RGB(0, 0, 255);break;// blau
	  case 4: n = CV_RGB(255, 255, 0);break;// gelb
	  case 5: n = CV_RGB(255, 0, 255);break;// magenta
	  case 6: n = CV_RGB(0, 255, 255);break;// cyan
	  case 7: n = CV_RGB(255, 0, 128);break;// pink
	  case 8: n = CV_RGB(255, 128, 0);break;// orange
	  case 9: n = CV_RGB(128, 0, 255);break;// violett
	  case 10: n = CV_RGB(0, 128, 255);break;// hellblau
	  default:
		  {
		    static int been_here=0;
		    if(!been_here)
		    {
		      been_here++;
		      cerr << "No more colors defined for marks>=" << int(m) << endl;
		    }
		  }
	} // switch
	putPixel8u_3(out, x, y, n);
      } // for y
  }

  /**
    * Verarbeite ein Bild, das keinen Keyframe darstellt
    */
  virtual void processFrame(myrect *AROI)
  {
    if(!paramChanged && !src.outputChanged &&
       !corr.outputChanged && !range.outputChanged) return;
    if(AROI) ROI = *AROI;
    else ROI = myrect(0, 0, w, h);

    FloodFill(img, ROI, markiert, range);

    // -------------------------------------------------------------------------------
    // Erkennung auf Pixelebene abgeschlossen
    // -------------------------------------------------------------------------------

    // sehr nahe Schwerpunkte (<10 pixel, mindestabstand) zusammenwerfen
    int i,j;
    for(i=0; i<markiert; i++)
    {
      mypoint& s1 = markierungen[i];
      bool zusammengelegt;
      // Folgende while-Konstruktion existiert aufgrund fehlerhafter
      // Optimierung im gcc 3.2 (SuSE Prerelease) (Endlosschleife)
      // Bei "restart iteration of while" stand einst
      //	// "restart iteration of outer loop
      //	i--;break;
      do {
        zusammengelegt=false;
        for(j=i+1; j<markiert; j++)
        {
          if(anzahl[j] == 0) continue;// als gelöscht gekennzeichneter Schwerpunkt
          mypoint& s2 = markierungen[j];
	  if(abs(s1-s2)<10)
  	  {
 	    if(DEBUGLEVEL>=3)
	      cerr << "Lege #" << i << " und #" << j << " zusammen.\n";
	    // neuen Schwerpunkt, gewichtet nach Pixelzahlen
	    int alle = anzahl[i] + anzahl[j];
 	    float gi = anzahl[i]/(float)alle;
	    float gj = anzahl[j]/(float)alle;

	    markierungen[i].p.x = int(markierungen[i].p.x*gi + markierungen[j].p.x*gj);
	    markierungen[i].p.y = int(markierungen[i].p.y*gi + markierungen[j].p.y*gj);
	    anzahl[i] = alle;

	    anzahl[j] = 0;
	    //restart iteration of while
	    zusammengelegt=true;
	    break;
	  } // if
        } // for
      } while(zusammengelegt==true);
    }

    // ungefähre Anzahl der Pixel berechnen, die eine LED im Bild hat
    // Gemessen wurde:
    //
    //  Modell | Radradius in Pixeln | Anzahl der Pixel pro LED
    //  -------+---------------------+-------------------------
    //  Adam   | 104		     | 55
    //         |  71                 | 2,5,6,11,14
    //  Eva    |  68		     | 1-10
    //         |		     | 2-20
    //	       |		     | 3-15
    // Ansatz: Radradius ~ LED-Fläche = PI*radius_LED^2
    //	       radius_LED = radradius*c (c wurde aus obiger Tabelle abgeleitet: 0.045 < c < 0.068)
    int pixPerLED = int(sqr(micro.radius*0.07));

    int MaxPixPerLED = int(pixPerLED*1.4);
    int MinPixPerLED = int(pixPerLED*0.1);
    if(DEBUGLEVEL>=3) cerr << "Pixel pro LED: ideal=" << pixPerLED
      << " min=" << MinPixPerLED
      << " max=" << MaxPixPerLED << endl;

    // Schwerpunkte mit >MaxPixPerLED (früher war es fest >50) oder <MinPixPerLED Pixeln rauswerfen
    for(int i=0; i<markiert; i++)
    {
      if(anzahl[i] > MaxPixPerLED) anzahl[i] = 0;
      if(anzahl[i] < MinPixPerLED) anzahl[i] = 0;
    }

    // Abstand zum Radmittelpunkt berechnen (r)
    if(!micro.mitteValid && DEBUGLEVEL>=1) cerr << "Radmitte unbekannt :(\n";
    if(!micro.radiusValid && DEBUGLEVEL>=1) cerr << "Radius unbekannt :(\n";

    const float LED_MINABSTAND = 0.1;
    const float LED_MAXABSTAND = 0.8;
    for(int i=0; i<markiert; i++)
    {
      if(anzahl[i] == 0) continue;// als gelöscht gekennzeichneter Schwerpunkt
      mypoint& s1 = markierungen[i];
      s1 = s1 - micro.mitte;
      abstand[i] = abs(s1)/micro.radius;

      // Schwerpunkte mit zu großen/zu kleinem Abstand vom Mittelpunkt
      // rauswerfen, da sie sich nur auf einem Ring darum befinden können
      if(abstand[i] > LED_MAXABSTAND) anzahl[i] = 0;
      if(abstand[i] < LED_MINABSTAND) anzahl[i] = 0;
      //cerr << "Abstand zur Radmitte #" << i << ": " << abstand[i] << endl;
    }

    // Schwerpunkte mit der Anzahl der Pixel gewichten
    // zuviele = schlecht, zuwenige auch
    // auch müssen die Schwerpkt. einen Mindestabstand voneinander
    // aufweisen
    // TODO

    // Gamma, Winkel zwischen positiver x-Achse und LED-Position,
    // relativ zur Radmitte berechnen
    // Inverse Funktion von:
    //  x = cos(gamma)*r
    //  y = sin(gamma)*r,
    // sowie Berechnung von beta und gamma0, welches der LED-Position bei am Rand anliegendem
    // Arm entspricht: gamma = gamma0 + beta
    // beta ~ 1/r
    for(int i=0; i<markiert; i++)
    {
      if(anzahl[i] == 0) continue;// als gelöscht gekennzeichneter Schwerpunkt
      gamma[i] = acos((markierungen[i].p.x) / (abstand[i]*micro.radius));
      // da 0 <= acos() <= M_PI:
      if(markierungen[i].p.y > 0) // < was for eva
      gamma[i] = 2*M_PI - gamma[i];

      // 0 <= beta <= 1
      // wobei 0 bedeutet: Arm liegt am Rad außen an
      // und 1 bedeutet: Arm ist maximal nach innen zur Radmitte bewegt
      beta[i] = 1-(abstand[i]-LED_MINABSTAND)/(LED_MAXABSTAND-LED_MINABSTAND);

      // Als Eva-Armansatzpunkt wird die LED-Position bei am Rad außen
      // anliegendem Arm betrachtet
      // Ist der Arm maximal ausgefahren, also beta==1, so
      // erscheint die LED schätzungsweise um 10 Grad im Gegenurzeigersinn
      // vom Armansatzpunkt bei beta==0
      gamma0[i] = gamma[i] + beta[i]*20/180*M_PI;
      //cerr << "#" << i << " gamma=" << gamma[i]*180/M_PI << "° gamma0="
      //  << gamma0[i]*180/M_PI << "° beta=" << int(beta[i]*100) << "%\n";
    }

    if(eva)
    {
      // -------------------------------------------------------------------------------
      // Erkennung der LED von micro.Eva
      // -------------------------------------------------------------------------------

      // Die Markierungen die auswählen, die am besten ins 72°-Raster passen
      // per Fehlerminimierung
      for(int z=0; z<markiert; z++) gewaehlt[z] = 0;
      float fehler1min = 1e6;

      for(int y=0; y<markiert; y++)
      {
	if(anzahl[y] == 0) continue;

	// jeden Schwerpunkt einmal als Ausgangspunkt merken
	for(int z=0; z<markiert; z++)
	{
	  gemerkt[z] = 0;
	}
	gemerkt[y] = 1;// das ist Arm #1

	float fehler1temp = 0;
	for(int i=1; i<5; i++) // je ein gamma0 Arm #2-5 zuordnen
	{
	  // wo wäre der nächste Arm zu finden?
	  float igamma0 = gamma0[y] + i*72./360.*2.*M_PI;
	  if(igamma0 > 2*M_PI) igamma0 -= 2.*M_PI;
	  float fehler2min = 50./360.*2.*M_PI;
	  //	cerr << "fehler2min=" << fehler2min << endl;
	  int minx = -1;
	  for(int x=0; x<markiert; x++) // am besten passenden Schwerpunkt finden
	  {
	    if(anzahl[x] == 0) continue;
	    if(gemerkt[x]) continue;
	    float adiff =  anglediff(igamma0, gamma0[x]);
	    if(adiff<fehler2min)
	    {
	      //cerr << "igamma0=" << igamma0 << " gamma0[x]=" << gamma0[x]
	      //  << " fehler2min=" << adiff << endl;
	      fehler2min = adiff;
	      minx = x;
	    }
	  }
	  if(minx==-1)
	    // kein passender Schwerpunkt gefunden
	    fehler1temp += 100;
	  else
	  {
	    fehler1temp += fehler2min;
	    gemerkt[minx] = i+1;// das ist Arm #i+1
	  }
	}
	if(fehler1temp<fehler1min)
	{
	  fehler1min = fehler1temp;
	  //cerr << " fehler1min=" << fehler1min << endl;
	  memcpy(gewaehlt, gemerkt, sizeof(gewaehlt));
	}
      }
      // in gewaehlt[] stehen nun die Indizes der max. 5 Schwerpunkte,
      // die am besten auf Armpositionen passen

      // alpha berechnen
      // finde das i, für das min(gamma0[i], alpha)
      // auch falls !alphaValid, kann Zuordnung so erfolgen
      // da dann egal ist, welches i gewinnt
      float fehler3=10;
      int alphai = -1;
      for(int i=0; i<markiert; i++)
      {
	if(!gewaehlt[i]) continue;
	float adiff = anglediff(micro.alpha, gamma0[i]);
	if(fehler3 > adiff)
	{
	  alphai = i;
	  fehler3 = adiff;
	}
      }
      if(fehler3<10)
      {
	micro.alpha = gamma[alphai];
	micro.alphaValid = true;
      }

      if(alphai==-1)
      {
	if(DEBUGLEVEL>=1)
	  cerr << "Kein Schwerpunkt gefunden -> kein alpha.\n";
      }
      else
      {
	// Zuordnen der Arm# in gewaehlt[]
	// zu den richtigen beta[]s
	// Es ist eine Rotation der in gewaehlt[] gespeicherten Indizes
	// um ga Positionen nach links (bzw. 5-ga nach rechts), z.B. sei ga=3
	// gewaehlt[]:  1 2 3 4 5
	// armnummer[]: 4 5 1 2 3
	// alphai ist der "neue" Arm#1
	char ga = gewaehlt[alphai];// alte Arnmummer, die nun Arm #1 wird
	eva->beta[0] = beta[alphai];
	armnummer[alphai] = 1;
	//cerr << "alphai=" << alphai << " alt=" << int(ga) << " neu: 1!" << endl;
	for(int i=0; i<markiert; i++)
	{
	  char gi = gewaehlt[i];// alte Armnummer
	  if(gi<1)
	  {
	    armnummer[i] = 0;
	    continue;
	  }
	  if(gi==ga) continue;// schon vor der Schleife behandelt
	  int an = 1+((gi+(5-ga)) % 5);
	  eva->beta[an-1] = beta[i];
	  armnummer[i] = an;
	  //cerr << " i=" << i << " alt=" << int(gi) << " neu=" << an << endl;
	}
	//cerr << endl;
      }
    }
    else
    // -------------------------------------------------------------------------------
    // Erkennung der LED von micro.Adam
    // -------------------------------------------------------------------------------
    {
      MicroAdam *adam = dynamic_cast<MicroAdam*>(&micro);
      if(adam)
      {
	// alpha += 180°, wenn gamma[LED] nahe alpha+beta liegt

	int ledIndex=-1;
	for(int i=0; i<markiert; i++)
	{
	  // gelöschter Schwerpunkt? weiter!
	  if(anzahl[i] == 0) continue;
	  // Schwerpunkt zu nah am Mittelpunkt? Kann nicht sein!
	  if(abstand[i]<0.55)
	  {
	    anzahl[i]=0; continue;
	  }
	  float d = anglediff(gamma[i], micro.alpha+adam->beta);
	  if(d > 10./180.*M_PI) continue;

	  // if more than 1 LED is found, situation
	  // is ambiguous and we don't try to correct alpha
	  if(ledIndex!=-1)
	  {
	    ledIndex=-1;
	    break;// leave for()
	  }
	  ledIndex=i;
	  //cerr << "Remembering Index: " << ledIndex;
	}
	if(ledIndex!=-1)
	{
	  if(anglediff(gamma[ledIndex], micro.alpha+adam->beta)
	      < 10./180.*M_PI)
	  {
	    if(DEBUGLEVEL>=2)
	      cerr << "Switching alpha by 180°: gamma[]=" << gamma[ledIndex]
	        << " alpha=" << micro.alpha << "\n";
	    micro.alpha += M_PI;
	  }
	}
      }
    }

    if(DEBUGLEVEL>=3) cerr << "<-FindLEDS\n";
    paramChanged = false;
    range.outputChanged = false;
    outputChanged = true;
  }

  virtual void processKeyFrame(void)
  {
    if(DEBUGLEVEL>=3) cerr << "FindLEDSModule::processKeyFrame()\n";
    processFrame(NULL);
  }

};

}

#endif // FINDLEDSMODULE_H

