#ifndef OPENCVEXT_H
#define OPENCVEXT_H

#include <opencv/cv.hpp>
#include <iostream>
#include <cmath>
#include <ostream>

namespace seemicro
{
  inline double sqr(const double x)
  {
    return x*x;
  }

  /**
    * Holt ein Pixel aus einem IplImage.
    * Version für Bilder vom Typ unsigned int, 8 bit, single plane,
    */
  inline uchar getPixel8u_1(const IplImage *img, const int x, const int y)
  {
    int step = img->widthStep;
    uchar *p = (uchar*)img->imageData + step*y;
    return p[x];
  }

  /**
    * Setzt ein Pixel in einem IplImage.
    * Version für Bilder vom Typ unsigned int, 8 bit, single plane,
    */
  inline void putPixel8u_1(const IplImage *img, const int x, const int y, const uchar c)
  {
    if(x >= img->width) return;
    if(y >= img->height) return;
    int step = img->widthStep;
    uchar *p = (uchar*)img->imageData + step*y;
    p[x] = c;
  }

  /**
    * Holt ein Pixel aus einem IplImage.
    * Version für Bilder vom Typ unsigned int, 8 bit, 3 planes.
    */
  inline int getPixel8u_3(const IplImage *img, const int x, const int y)
  {
    int step = img->widthStep;
    uchar *p = (uchar*)img->imageData + step*y + x*3;
    // assume bgr-image
    return CV_RGB( p[2], p[1], p[0] );
  }

  /**
    * Setzt ein Pixel in einem IplImage.
    * Version für Bilder vom Typ unsigned int, 8 bit, 3 planes.
    */
  // unsigned int, 8 bit, 3 planes
  inline void putPixel8u_3(const IplImage *img, const int x, const int y, const int color)
  {
    if(x >= img->width) return;
    if(y >= img->height) return;
    int step = img->widthStep;
    uchar *p = (uchar*)img->imageData + step*y + x*3;
    // assume bgr-image
    p[2]=(uchar) (color>>16);
    p[1]=(uchar) (color>>8);
    p[0]=(uchar) color;
  }

  /**
    * Holt ein Pixel aus einem IplImage.
    * Version für Bilder vom Typ float, 32 bit, single plane.
    */
  inline double getPixel32f_1(const IplImage *img, const int x, const int y)
  {
    int step = img->widthStep;
    char *p = (img->imageData + step*y + x*4);
    return *(float *)p;
  }

  /**
    * Setzt ein Pixel in einem IplImage.
    * Version für Bilder vom Typ float, 32 bit, single plane
    */
  inline void putPixel32f_1(const IplImage *img, const int x,
    const int y, const double color)
  {
    if(x >= img->width) return;
    if(y >= img->height) return;
    int step = img->widthStep;
    float *p = (float*)((uchar*)(img->imageData + step*y + x*4));
    *p=color;
  }

  /**
    * Führt eine zirkuläre Verschiebeoperation eines Bildes um (dx, dy) Pixel
    * aus. Was an einem Rand hinausgeschoben wird, wird am gegenüberliegenden
    * Rand wieder ins Bild eingeblendet - daher die Bezeichnung "zirkulär".
    * src & dst dürfen dasselbe Bild beinhalten - dann wird temporär
    * ein weiteres IplImage angelegt.
    *
    * src2 is split into 4 regions:
    *
    *        dx
    * +----+----+
    * | 1  | 2  |
    * +----+----+
    * | 3  | 4  | dy
    * +----+----+
    *
    * those regions are copied into dst:
    *  dx
    * +----+----+
    * | 4  | 3  | dy
    * +----+----+
    * | 2  | 1  |
    * +----+----+
    *
    */
  void circular_shift(IplImage *src, IplImage *dst, int dx, int dy);

  /**
    * Kapselung von CvPoint aus OpenCV, um einfacher lesbaren
    * Quelltext bei Koordinatenoperationen zu haben, sowie
    * programmiertechnisch schnell einen Punkt ausgeben zu können (wichtig beim
    * Debugging :)
    */
  struct mypoint
  {
    CvPoint p;

    mypoint() { }
    mypoint(const CvPoint& c) { p=c; }
    mypoint(int x0, int y0) { p.x=x0;p.y=y0; }

    /**
      * Konstruiert einen Punkt aus Polarkoordinaten.
      */
    void fromPolar(double l, double alpha)
    {
      p.x=int(l*cos(alpha));
      p.y=int(l*sin(alpha));
    }

    friend mypoint operator+(const mypoint&, const mypoint&);
    friend mypoint operator-(const mypoint& p1, const mypoint& p2);
    operator CvPoint() { return p; }
  };

  /**
    * Gibt einen mypoint in einen Stream aus.
    */
  std::ostream& operator<<(std::ostream& s, const mypoint& p);

  /**
    * Berechnet Abstand des Punktes vom Koordinatenursprung
    */
  double abs(const mypoint& p);

  /**
    * Kapselung vin CvRect aus OpenCV,
    * vor allem um eine einfache Ausgabe in Streams zu ermöglichen.
    */
  struct myrect
  {
    CvRect r;
    myrect() { }
    myrect(const CvRect c) { r=c; }
    myrect(const int x0, const int y0, const int w, const int h)
    {
      r.x=x0;r.y=y0;r.width=w;r.height=h;
    }
    myrect(const mypoint& p1, const mypoint& p2)
    {
      r.x=p1.p.x;
      r.y=p1.p.y;
      r.width=p2.p.x-p1.p.x;
      r.height=p2.p.y-p1.p.y;
    }
    void crop(const int w, const int h)
    {
      if(r.x<0) r.x = 0;
      if(r.y<0) r.y = 0;
      if(r.x>=w) r.x = w-10;
      if(r.y>=h) r.y = h-10;
      if(r.x+r.width>=w) r.width = w-r.x;
      if(r.y+r.height>=h) r.height = h-r.y;

      if(r.width <= 0) r.width = 1;
      if(r.height <= 0) r.height = 1;
    }
    mypoint p1()
    {
      return mypoint(r.x, r.y);
    }
    mypoint p2()
    {
      return mypoint(r.x+r.width, r.y+r.height);
    }
    operator CvRect() { return r; }
  };

  /**
    * Gibt ein myrect in einen Stream aus.
    */
  std::ostream& operator<<(std::ostream& s, const myrect& r);

/**
  * Normalisiert geg. Winkel so, daß
  * er zwischen 0 und 2*PI liegt
  */
inline float normalize2PI(const float angle)
{
  float ret = angle;
  int npi = int(angle/M_PI*0.5);
  if(npi<0 || angle<0) npi--;
  if(npi!=0) ret = angle - npi*2*M_PI;

  if(ret<0) ret=0;
  if(ret>2*M_PI) ret=2*M_PI-1e-8;
  assert(ret>=0);
  if(ret>2*M_PI) std::cerr << "ret=" << ret << " angle=" << angle
    << " npi=" << npi;
  assert(ret <= float(2*M_PI));
  return ret;
}

/**
  * Berechnet absolute Differenz zwischen zwei Winkeln,
  * vor allem wenn a1 ~ 2PI und a2 ~ 0.
  * 0 <= return value <= M_PI.
  * Unabhängig von OpenCV, aber hat auch in kein anderes Modul gepaßt.
  */
inline float anglediff(float a1, float a2)
{
  a1 = normalize2PI(a1);
  a2 = normalize2PI(a2);

/*  int ia1 = int(a1/2/M_PI);
  if(ia1>0) a1-=ia1*2*M_PI;
  int ia2 = int(a2/2/M_PI);
  if(ia2>0) a2-=ia2*2*M_PI;

  if(a1<0 || a1>2*M_PI)
  {
    std::cerr << "angle out of range:" << a1 << std::endl;
  }
  if(a2<0 || a2>2*M_PI)
  {
    std::cerr << "angle out of range:" << a2 << std::endl;
  }
*/
  float diff = fabs(a1-a2);
  if(diff>=M_PI) return 2*M_PI-diff;
  return diff;
}

inline void cvPutTextSenkrecht(IplImage *img, const char *text, CvPoint p,
           CvFont *font, int color)
{
  CvSize size;
  int ymin;
  cvGetTextSize(text, font, &size, &ymin);

  size.height -= ymin;
  IplImage *tmp = cvCreateImage(size, img->depth, img->nChannels);
  cvZero(tmp);
  cvPutText(tmp, text, cvPoint(0,size.height+ymin-1), font, color);

  // um 90° im Gegenuhrzeigersinn gedreht wieder einzeichnen
  for(int x=0; x<size.width; x++)
    for(int y=0; y<size.height; y++)
    {
      int destX = p.x+y;
      if(destX<0 || destX>=img->width) continue;
      int destY = p.y-x;
      if(destY<0 || destY>=img->height) continue;
      if(img->nChannels==1)
      {
	if(img->depth==IPL_DEPTH_8U)
	{
	  char c = getPixel8u_1(tmp, x, y);
	  putPixel8u_1(img, destX, destY, c);
	}
	else
	{
	  std::cerr << "cvPutTextSenkrecht(): image format not supported\n";
	  abort();
	}
      }
      else if(img->nChannels==3)
      {
	std::cerr << "cvPutTextSenkrecht(): image format not supported\n";
	abort();
      }
      else
      {
	std::cerr << "cvPutTextSenkrecht(): image format not supported\n";
	abort();
      }
    }

  cvReleaseImage(&tmp);
}

} //namespace seemicro

#endif

