/************************************************************************
  			Copyright Michael Bunk 2004
*************************************************************************/

#include "searchanglesadam.h"
#include "../opencvext.h"

namespace seemicro
{

/**
  * Normalisiert gegebenen Winkel so, daß
  * er zwischen 0 und PI liegt
  */
float normalizePI(const float angle)
{
  float ret = angle;
  int npi = int(angle/M_PI);
  if(npi<0 || angle<0) npi--;// int(-0.1/PI) ist ja auch 0
  if(npi!=0) ret = angle - npi*M_PI;
  if(ret<0 && DEBUGLEVEL>=1)
  {
    cerr << "npi=" << npi << " ret=" << ret << " angle=" << angle;
  }
  assert(ret>=0);
  assert(ret<M_PI);
  return ret;
}

void alphaBetaFromPoint(const mypoint p, const float radius,
    const float theta, float &alpha, float& beta)
{
  float r1 = p.p.x/radius;
  if(r1>1)
  {
    if(DEBUGLEVEL>=1) cerr << "alphaBetaFromPoint(): r1>1: r1="
      << r1 << "\n\n\n\n";
    r1=1;
  }
  if(r1<-1)
  {
    if(DEBUGLEVEL>=1) cerr << "alphaBetaFromPoint(): r1<-1: r1="
      << r1 << "\n\n\n\n";
    r1=-1;
  }

  assert(r1 >= -1);
  assert(r1 <=  1);

  alpha = acos(r1);
  // 0 <= alpha <= M_PI
  assert(alpha >= 0);
  assert(alpha <= float(M_PI));// das Makro ist auf double abgestimmt...

  // y>0 bedeutet: Schnittpunkt in unterer Bildhälfte
  if(p.p.y>0) alpha=2*M_PI-alpha;

  assert(alpha >= 0);
  assert(alpha <= float(2*M_PI));

  // make sure 0 <= theta < PI
  float anstieg = normalizePI(theta);

  // es soll gelten: -20° < beta < 20°
  if(alpha<M_PI/2)
  {
    if(anstieg<M_PI/2) beta = alpha-anstieg;
    else beta = alpha-(anstieg-M_PI);
  }
  else if(alpha>=M_PI/2 && alpha < M_PI)
  {
    if(anstieg<M_PI/2) beta = alpha-(anstieg+M_PI);
    else beta = alpha-anstieg;
  }
  else if(alpha>=M_PI && alpha < 3.*M_PI/2.)
  {
    if(anstieg<M_PI/2) beta = alpha-(anstieg+M_PI);
    else beta = alpha-anstieg;
  }
  else
  {
    if(anstieg<M_PI/2) beta = alpha-(anstieg+M_PI);
    else beta = alpha-(anstieg+M_PI);
  }

  if(beta<-M_PI/2) beta += M_PI;
  if(beta>M_PI/2) beta -= M_PI;
//  cerr << "alpha: " << alpha << " anstieg mline: " << anstieg
//    << " beta: " << beta << endl;
  if(fabs(anglediff(beta, 0)) > 30./180.*M_PI)
  {
    if(DEBUGLEVEL>=1) cerr << "alphaBetaFromPoint(): beta invalid! " << endl;
  }
  assert(beta>-M_PI);
  assert(beta<M_PI);
}

int SearchAnglesAdam::searchAngles()
{
  if(!adam.mitteValid && DEBUGLEVEL>=1)
    cerr << "searchAngles(): mitte must be valid! Ignoring.\n";
  if(!adam.radiusValid && DEBUGLEVEL>=1)
    cerr << "searchAngles(): radius must be valid! Ignoring.\n";

  paramChanged = false;
  int half_randbreite = int(adam.radius*0.25);
  rect = myrect(
      int(adam.mitte.p.x-adam.radius+1+half_randbreite),
      int(adam.mitte.p.y-adam.radius+1+half_randbreite),
      int(2*adam.radius-1-2*half_randbreite),
      int(2*adam.radius-1-2*half_randbreite));
  rect.crop(canny.img->width, canny.img->height);
#ifdef DEBUG
  if(DEBUGLEVEL>=3) cerr << "Clipped inside: " << rect << endl;
#endif

  CvMat ausschnitt;
  cvGetSubRect(canny.img, &ausschnitt, rect.r);
  IplImage *klon, image_header;
  klon = cvCloneImage(cvGetImage(&ausschnitt, &image_header));

  if(angres<7 || angres>1e3) angres=7;
  if(distres<7 || distres>1e3) distres=7;
  if(hough_threshold<1) hough_threshold=1;
  if(lines) cvClearSeq(lines);

#ifdef DEBUG
  if(DEBUGLEVEL>=3) cerr << "->cvHoughLines()\n";
#endif
  // distance resolution in "pixel related units"
  double dr = sqrt(sqr(rect.r.width)+sqr(rect.r.height))/distres;
  if(dr<=0) dr=1;
  // angle resolution in some other strange unit
  double ar = 2*M_PI/double(angres);
  if(ar<=0) ar=2*M_PI/90;
  lines = cvHoughLines2(klon, storage, CV_HOUGH_PROBABILISTIC,
      dr, ar,
      hough_threshold, // threshold
      adam.radius/4, // minimum line length
      houghMaxGap); // maximum gap between line segments

  // linien in polardarstellung umrechnen
  for(int i = 0; i < lines->total; i++)
  {
    CvPoint* line = (CvPoint*)cvGetSeqElem(lines, i);

#ifdef DEBUG
    if(DEBUGLEVEL>=3)
    {
      int len = int(sqrt(sqr(line[0].x-line[1].x)+
	  sqr(line[0].y-line[1].y)));
      cerr << "\tline " << i << ": " << line[0].x << "-" << line[0].y << " "
        << line[1].x << "-" << line[1].y << " len:" << len << endl;
    }
#endif

    // linien waren relativ zu rect,
    // wieder relativ zu Gesamtbild (in/eingezeichnet) machen
    // diese umrechnung hernach im frontend beachten!
    line[0].x += rect.r.x;
    line[0].y += rect.r.y;
    line[1].x += rect.r.x;
    line[1].y += rect.r.y;

    polarLine l;
    l.fromCart(canny.img->width, canny.img->height, line[0], line[1]);

#ifdef DEBUG
    if(DEBUGLEVEL>=3) cerr << " / l: r=" << l.r << " theta="
      << l.theta/M_PI*180 << "° (" << setprecision(1) << fixed
      << l.theta << ")" << endl;
#endif

    if(i>=MAXPL) continue;
    if(l.r < 0)
    {
      l.theta -= M_PI;
      l.r = -l.r;
    }
    polarLines[i] = l;
  }

  // gibt es 2 parallele linien, deren abstand ~= armbreite ist?
  int n = min(MAXPL, lines->total);
 if(n<=1)
  {
#ifdef DEBUG
    if(DEBUGLEVEL>=2) cerr << "Weniger als 2 Geraden gefunden.\n";
#endif
    cvReleaseImage(&klon);
    adam.alphaValid = false;
    return -1;
  }

  CvMat* mat = cvCreateMat(n, n, CV_32FC1);
  cvSet(mat, cvScalar(1e10));
  for(int i=1; i<n; i++)
    for(int j=0; j<i; j++)
    {
      // t1 = t2 + PI*n, n=N (natürl. Zahl) - die sind alle parallel
      float t1 = polarLines[i].theta;
      float t2 = polarLines[j].theta;
      float r1 = polarLines[i].r;
      float r2 = polarLines[j].r;
      assert(t1>=0);
      assert(t2>=0);
      if(t1>=M_PI) { t1-=M_PI;r1=-r1; }
      if(t2>=M_PI) { t2-=M_PI;r2=-r2; }
      double dtheta = sqr(t1-t2);
      if(dtheta > 0.20*0.20) continue;
      double dArm = sqr( fabs(r1-r2) - adam.armbreite(adam.radius) );
      if(dArm > 30) continue;
      cvSetReal2D(mat, j, i, dArm*0.15*0.15+dtheta*30);
    }

  double minVal, maxVal;
  CvPoint minLoc, maxLoc;
  cvMinMaxLoc(mat, &minVal, &maxVal, &minLoc, &maxLoc);
  cvReleaseMat(&mat);

  if(minLoc.x==minLoc.y)
  {
#ifdef DEBUG
    if(DEBUGLEVEL>=2) cerr << "No matchin lines.\n";
#endif
    cvReleaseImage(&klon);
    adam.alphaValid = false;
    return -2;
  }

#ifdef DEBUG
  if(DEBUGLEVEL>=3)  cerr << "Best match: " << minLoc.x << " " << minLoc.y
    << " minVal: " << minVal << endl;
#endif
  l1 = polarLines[minLoc.x];
  l2 = polarLines[minLoc.y];

  // Linie berechnen, die zwischen den beiden gefundenen verläuft.
  // theta bleibt in etwa, arithm. Mittel wird auch berechnet,
  // damit stabiler
  float t1 = l1.theta;
  float t2 = l2.theta;
  float r1 = l1.r;
  float r2 = l2.r;
  assert(r1>=0);
  assert(r2>=0);
  assert(t1>=0);
  assert(t2>=0);
  assert(t1<2*M_PI);
  assert(t2<2*M_PI);
  if(t1>=M_PI) { t1-=M_PI;r1=-r1; }
  if(t2>=M_PI) { t2-=M_PI;r2=-r2; }
  mline = polarLine((r1+r2)/2, (t1+t2)/2);
  mline.normalize();// make sure r>0
#ifdef DEBUG
  if(DEBUGLEVEL>=3)
    cerr << "mline (mittelpunkt: gesamtbild): " << mline << endl;
#endif

  // gefundene linie hat polaren Koordinatenursprung
  // in Mitte von "eingezeichnet",
  // Umrechnung auf Radmittelpunkt notwendig
  mypoint bildmitte = mypoint(canny.img->width/2, canny.img->height/2);
  mypoint shift = adam.mitte-bildmitte;
//  dr2 = cos(mline.theta)*abs(shift);
  double thetaShift=atan2(double(-shift.p.y), double(shift.p.x));
//  cerr << " thetaShift " << thetaShift*180/M_PI;
  double thetaX = mline.theta-thetaShift;
//  cerr << " thetaX " << thetaX*180/M_PI;
//  cerr << " abs(shift) " << abs(shift);
//  cerr << " shift " << shift;
  dr2 = -cos(thetaX)*abs(shift);
//  cerr << " dr2 " << dr2<<endl;
  mline.r += dr2;

  polarLine mline2 = mline;
  mline2.normalize();
#ifdef DEBUG
  if(DEBUGLEVEL>=3)
    cerr << "Berechne Schnittpunkte:\n\tKreis: radius=" << adam.radius
      << "\n\tmline (mittelpunkt: radmitte): " << mline2 << endl;
#endif
  if(-1 == mline2.find_p1_p2(p1, p2, adam.radius))
  {
#ifdef DEBUG
    if(DEBUGLEVEL>=2)
      cerr << "Keine Schnittpunkte zwischen Kreis und Gerade gefunden :(\n";
#endif
    cvReleaseImage(&klon);
    return -2; // "keine passenden Geraden gefunden"
  }

  // alpha,beta (1,2) berechnen
  // theta ist nicht der Anstieg der Geraden!
  float alpha1, beta1, alpha2, beta2;
  alphaBetaFromPoint(p1, adam.radius, mline.theta + M_PI/2, alpha1, beta1);
  alphaBetaFromPoint(p2, adam.radius, mline.theta + M_PI/2, alpha2, beta2);

  // nimm die, bei denen abstand zu alpha_old / beta_old am kleinsten
  // findleds will switch by 180° if required

  // wo es besser paßt, muß maxX größer sein
  // in diesem falle, näher an 0
  float max1 = - anglediff(alpha1, adam.alpha);
  float max2 = - anglediff(alpha2, adam.alpha);

#ifdef DEBUG
  if(DEBUGLEVEL>=3)
    cerr << "max1: " << setprecision(2) << max1 << " max2: " << max2;
#endif
  if(max1>max2)
  {
    adam.alpha = alpha1;
    adam.beta = beta1;
//    cerr << " 1  wins\n";
  }
  else
  {
    adam.alpha = alpha2;
    adam.beta = beta2;
//    cerr << "  2 wins\n";
  }

  cvReleaseImage(&klon);
  adam.alphaValid = true;
  return 0;// Erfolg
}

} // namespace seemicro
