/*!
  Get the coordinates of the mouse pointer.

  \warning Not implemented yet.
  
  \param ip [out] : The coordinates of the mouse pointer.
  
  \return true if a pointer motion event was received, false otherwise.
  
  \exception vpDisplayException::notInitializedError : If the display
  was not initialized.
*/
bool 
vpDisplayOpenCV::getPointerMotionEvent (vpImagePoint &ip )
{
  bool ret = false;

  if (displayHasBeenInitialized) {
    //flushDisplay() ;
    double u, v;
    if (move){
      ret = true ;
      u = (unsigned int)x_move;
      v = (unsigned int)y_move;
      ip.set_u( u );
      ip.set_v( v );
      move = false;
    }
  }

  else {
    vpERROR_TRACE("OpenCV not initialized " ) ;
    throw(vpDisplayException(vpDisplayException::notInitializedError,
                             "OpenCV not initialized")) ;
  }
  return ret;
}

/*!
  Display a selection of the gray level image \e I (8bits).

  \warning Display has to be initialized.

  \warning Suppress the overlay drawing in the region of interest.

  \param I : Image to display.
  
  \param iP : Top left corner of the region of interest
  
  \param w : Width of the region of interest
  
  \param h : Height of the region of interest

  \sa init(), closeDisplay()
*/
void vpDisplayGTK::displayImageROI ( const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int w, const unsigned int h )
{
  if (displayHasBeenInitialized)
  {
    vpImage<unsigned char> Itemp;
    vpImageTools::crop(I,(unsigned int)iP.get_i(),(unsigned int)iP.get_j(), h, w,Itemp);
    /* Copie de l'image dans le pixmap fond */
    gdk_draw_gray_image(background,
                        gc, (gint)iP.get_u(), (gint)iP.get_v(), (gint)w, (gint)h,
                        GDK_RGB_DITHER_NONE,
                        I.bitmap,
                        (gint)w);

    /* Le pixmap background devient le fond de la zone de dessin */
    gdk_window_set_back_pixmap(widget->window, background, FALSE);

    /* Affichage */
    //gdk_window_clear(GTK_WINDOW(widget));
    //gdk_flush();
  }
  else
  {
    vpERROR_TRACE("GTK not initialized " ) ;
    throw(vpDisplayException(vpDisplayException::notInitializedError,
                             "GTK not initialized")) ;
  }
}

/*!
  Wait for a click from one of the mouse button and get the position
  of the clicked image point.

  \param ip [out] : The coordinates of the clicked image point.

  \param blocking [in] : true for a blocking behaviour waiting a mouse
  button click, false for a non blocking behaviour.

  \return 
  - true if a button was clicked. This is always the case if blocking is set 
    to \e true.
  - false if no button was clicked. This can occur if blocking is set
    to \e false.

*/
bool vpDisplayWin32::getClick(vpImagePoint &ip, bool blocking)
{
  //wait if the window is not initialized
  waitForInit();

  bool ret = false ;
  double u, v;
  //tells the window there has been a getclick demand
//   PostMessage(window.getHWnd(), vpWM_GETCLICK, 0,0);
  //waits for a click
  if(blocking){
    WaitForSingleObject(window.semaClick, NULL);
    WaitForSingleObject(window.semaClickUp, NULL);//to erase previous events
    WaitForSingleObject(window.semaClick, INFINITE);
    ret = true;  
  }  
  else
    ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClick, NULL));
  
  u = window.clickX;
  v = window.clickY;
  ip.set_u( u );
  ip.set_v( v );

  return ret;
}

/*!
  Display a string at the image point \e ip location.

  To select the font used to display the string, use setFont().

  \param ip : Upper left image point location of the string in the display.
  \param text : String to display in overlay.
  \param color : String color.

  \sa setFont()
*/
void vpDisplayOpenCV::displayCharString( const vpImagePoint &ip,
                                     const char *text, 
				     const vpColor &color )
{
  if (displayHasBeenInitialized)
  {
    if (color.id < vpColor::id_unknown) {
      cvPutText( background, text, 
		 cvPoint( vpMath::round( ip.get_u() ),
			  vpMath::round( ip.get_v()+fontHeight ) ), 
		 font, col[color.id] );
    }
    else {
      cvcolor = CV_RGB(color.R, color.G, color.B) ;
      cvPutText( background, text, 
		 cvPoint( vpMath::round( ip.get_u() ),
			  vpMath::round( ip.get_v()+fontHeight ) ), 
		 font, cvcolor );
    }
  }
  else
  {
    vpERROR_TRACE("OpenCV not initialized " ) ;
    throw(vpDisplayException(vpDisplayException::notInitializedError,
                             "OpenCV not initialized")) ;
  }
}

/*!
  Display a point at the image point \e ip location.
  \param ip : Point location.
  \param color : Point color.
*/
void vpDisplayGTK::displayPoint ( const vpImagePoint &ip,
				  const vpColor &color )
{
  if (displayHasBeenInitialized)
  {
    if (color.id < vpColor::id_unknown)
      gdk_gc_set_foreground(gc, col[color.id]);
    else {
      gdkcolor.red   = 256 * color.R;
      gdkcolor.green = 256 * color.G;
      gdkcolor.blue  = 256 * color.B;
      gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
      gdk_gc_set_foreground(gc, &gdkcolor);     
    }

    gdk_draw_point(background,gc, 
		   vpMath::round( ip.get_u() ), 
		   vpMath::round( ip.get_v() ) );
  }
  else
  {
    vpERROR_TRACE("GTK not initialized " ) ;
    throw(vpDisplayException(vpDisplayException::notInitializedError,
                             "GTK not initialized")) ;
  }
}

/*!
  Wait for a click from one of the mouse button and get the position
  of the clicked image point.

  \param ip [out] : The coordinates of the clicked image point.

  \param blocking [in] : true for a blocking behaviour waiting a mouse
  button click, false for a non blocking behaviour.

  \return 
  - true if a button was clicked. This is always the case if blocking is set 
    to \e true.
  - false if no button was clicked. This can occur if blocking is set
    to \e false.

*/
bool
vpDisplayGTK::getClick(vpImagePoint &ip, bool blocking)
{
  bool ret = false;

  if (displayHasBeenInitialized) {

    double u, v ;
    do {
      GdkEvent *ev = NULL;
      while ((ev = gdk_event_get())!=NULL){
        if (ev->any.window == widget->window && ev->type == GDK_BUTTON_PRESS) {
          u = ((GdkEventButton *)ev)->x ;
          v = ((GdkEventButton *)ev)->y ;
	  ip.set_u( u );
	  ip.set_v( v );
          ret = true ;
        }
        gdk_event_free(ev) ;
      }
      if (blocking){
        flushDisplay();
        vpTime::wait(100);
      }
    } while ( ret == false && blocking == true);
  }
  else {
    vpERROR_TRACE("GTK not initialized " ) ;
    throw(vpDisplayException(vpDisplayException::notInitializedError,
                             "GTK not initialized")) ;
  }
  return ret ;
}

/*!
  Wait for a mouse button click release and get the position of the
  image point were the click release occurs.  The button used to click is
  also set. Same method as getClick(unsigned int&, unsigned int&,
  vpMouseButton::vpMouseButtonType &, bool).

  \param ip [out] : Position of the clicked image point.

  \param button [in] : Button used to click.

  \param blocking [in] : true for a blocking behaviour waiting a mouse
  button click, false for a non blocking behaviour.

  \return 
  - true if a button was clicked. This is always the case if blocking is set 
    to \e true.
  - false if no button was clicked. This can occur if blocking is set
    to \e false.

  \sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool)

*/
bool vpDisplayWin32::getClickUp(vpImagePoint &ip,
                                vpMouseButton::vpMouseButtonType& button,
                                bool blocking)
{
  //wait if the window is not initialized
  waitForInit();
  bool ret = false;
  double u, v;
  //tells the window there has been a getclickup demand
//   PostMessage(window.getHWnd(), vpWM_GETCLICKUP, 0,0);

  //waits for a click release
  if(blocking){
    WaitForSingleObject(window.semaClickUp, 0);
    WaitForSingleObject(window.semaClick, 0);//to erase previous events
    WaitForSingleObject(window.semaClickUp, INFINITE);
    ret = true;
  }
  else
    ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClickUp, 0));
  
  u = window.clickXUp;
  v = window.clickYUp;
  ip.set_u( u );
  ip.set_v( v );
  button = window.clickButtonUp;

  return ret;
}

/*!
  Display a string at the image point \e ip location.

  To select the font used to display the string, use setFont().

  \param ip : Upper left image point location of the string in the display.
  \param text : String to display in overlay.
  \param color : String color.

  \sa setFont()
*/
void vpDisplayGTK::displayCharString ( const vpImagePoint &ip,
				       const char *text, 
				       const vpColor &color )
{
  if (displayHasBeenInitialized)
  {
    if (color.id < vpColor::id_unknown)
      gdk_gc_set_foreground(gc, col[color.id]);
    else {
      gdkcolor.red   = 256 * color.R;
      gdkcolor.green = 256 * color.G;
      gdkcolor.blue  = 256 * color.B;
      gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
      gdk_gc_set_foreground(gc, &gdkcolor);     
    }
    if (font != NULL)
      gdk_draw_string(background, font, gc,
                      vpMath::round( ip.get_u() ),
                      vpMath::round( ip.get_v() ),
                      (const gchar *)text);
    else
      std::cout << "Cannot draw string: no font is selected" << std::endl;
  }
  else
  {
    vpERROR_TRACE("GTK not initialized " ) ;
    throw(vpDisplayException(vpDisplayException::notInitializedError,
                             "GTK not initialized")) ;
  }
}

/*
  Compute the distance d = |Pw1-Pw2|
*/
inline double distance(const vpImagePoint &iP1, const double w1, const vpImagePoint &iP2, const double w2)
{
  double distancei = iP1.get_i() - iP2.get_i();
  double distancej = iP1.get_j() - iP2.get_j();
  double distancew = w1 -w2;
  return sqrt(vpMath::sqr(distancei)+vpMath::sqr(distancej)+vpMath::sqr(distancew));
}

/*!
  Display a line from image point \e ip1 to image point \e ip2.
  \param ip1,ip2 : Initial and final image points.
  \param color : Line color.
  \param thickness : Line thickness.
*/
void vpDisplayGTK::displayLine ( const vpImagePoint &ip1, 
				 const vpImagePoint &ip2,
				 const vpColor &color, 
				 unsigned int thickness )
{
  if (displayHasBeenInitialized)
  {
    if ( thickness == 1 ) thickness = 0;

    if (color.id < vpColor::id_unknown)
      gdk_gc_set_foreground(gc, col[color.id]);
    else {
      gdkcolor.red   = 256 * color.R;
      gdkcolor.green = 256 * color.G;
      gdkcolor.blue  = 256 * color.B;
      gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
      gdk_gc_set_foreground(gc, &gdkcolor);     
    }

    gdk_gc_set_line_attributes(gc, (gint)thickness,
			       GDK_LINE_SOLID, GDK_CAP_BUTT,
			       GDK_JOIN_BEVEL) ;
    gdk_draw_line(background, gc,
		  vpMath::round( ip1.get_u() ),
		  vpMath::round( ip1.get_v() ),
		  vpMath::round( ip2.get_u() ),
		  vpMath::round( ip2.get_v() ) );
  }
  else
  {
    vpERROR_TRACE("GTK not initialized " ) ;
    throw(vpDisplayException(vpDisplayException::notInitializedError,
                             "GTK not initialized")) ;
  }
}

/*!
 
  Constructs a rectangle with \e topLeft the top-left corner location
  and \e width and \e height the rectangle size.

*/
vpRect::vpRect(const vpImagePoint &topLeft, double width, double height)
{
  this->left = topLeft.get_u();
  this->top = topLeft.get_v();
  this->width = width;
  this->height = height;
};

/*!
  Display a selection of the color image \e I in RGBa format (32bits).

  \warning Display has to be initialized.

  \warning Suppress the overlay drawing in the region of interest.

  \param I : Image to display.
  
  \param iP : Top left corner of the region of interest
  
  \param w : Width of the region of interest
  
  \param h : Height of the region of interest

  \sa init(), closeDisplay()
*/
void vpDisplayGTK::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int w, const unsigned int h )
{
  if (displayHasBeenInitialized)
  {
    vpImage<vpRGBa> Itemp;
    vpImageTools::crop(I,(unsigned int)iP.get_i(), (unsigned int)iP.get_j(), h, w, Itemp);
    /* Copie de l'image dans le pixmap fond */
    gdk_draw_rgb_32_image(background,
                          gc, (gint)iP.get_u(), (gint)iP.get_v(), (gint)w, (gint)h,
                          GDK_RGB_DITHER_NONE,
                          (unsigned char *)Itemp.bitmap,
                          (gint)(4*w));

    /* Permet de fermer la fenetre si besoin (cas des sequences d'images) */
    //while (g_main_iteration(FALSE));

    /* Le pixmap background devient le fond de la zone de dessin */
    gdk_window_set_back_pixmap(widget->window, background, FALSE);

    /* Affichage */
    //gdk_window_clear(GTK_WINDOW(widget));
    //flushDisplay() ;
  }
  else
  {
    vpERROR_TRACE("GTK not initialized " ) ;
    throw(vpDisplayException(vpDisplayException::notInitializedError,
                             "GTK not initialized")) ;
  }
}

/*!
 
  Constructs a rectangle with \e topLeft the top-left corner location
  and \e bottomRight the bottom-right corner.

*/
vpRect::vpRect(const vpImagePoint &topLeft, const vpImagePoint &bottomRight)
{
  this->left = topLeft.get_u();
  this->top = topLeft.get_v();
  
  setBottom( bottomRight.get_v() );
  setRight( bottomRight.get_u() );
};

/*!
  Computes the \f$ alpha \f$ angle of the two points and store them into alpha1 for the smallest and alpha2 for the biggest.

  \note this function is useful only during the initialization.

  \param pt1 : First point whose \f$ alpha \f$ angle is computed.
  \param pt2 : Second point whose \f$ alpha \f$ angle is computed.
*/ 
void
vpMeEllipse::computeAngle(vpImagePoint pt1, vpImagePoint pt2)
{

  getParameters() ;
  double j1, i1, j11, i11;
  j1 =  i1 =  0.0 ;

  int number_of_points = 2000 ;
  double incr = 2 * M_PI / number_of_points ; // angle increment

  double dmin1 = 1e6  ;
  double dmin2 = 1e6  ;

  double k =  0 ;
  while(k < 2*M_PI) {

//     j1 = a *cos(k) ; // equation of an ellipse
//     i1 = b *sin(k) ; // equation of an ellipse

    j1 = a *sin(k) ; // equation of an ellipse
    i1 = b *cos(k) ; // equation of an ellipse

    // (i1,j1) are the coordinates on the origin centered ellipse ;
    // a rotation by "e" and a translation by (xci,jc) are done
    // to get the coordinates of the point on the shifted ellipse
//     j11 = iPc.get_j() + ce *j1 - se *i1 ;
//     i11 = iPc.get_i() -( se *j1 + ce *i1) ;

    j11 = iPc.get_j() + ce *j1 + se *i1 ;
    i11 = iPc.get_i() - se *j1 + ce *i1 ;

    double  d = vpMath::sqr(pt1.get_i()-i11) + vpMath::sqr(pt1.get_j()-j11) ;
    if (d < dmin1)
    {
      dmin1 = d ;
      alpha1 = k ;
    }
    d = vpMath::sqr(pt2.get_i()-i11) + vpMath::sqr(pt2.get_j()-j11) ;
    if (d < dmin2)
    {
      dmin2 = d ;
      alpha2 = k ;
    }
    k += incr ;
  }
  //std::cout << "end vpMeEllipse::computeAngle(..)" << alpha1 << "  " << alpha2 << std::endl ;

  if (alpha2 < alpha1)
    alpha2 += 2 * M_PI;
  //else if (alpha2 == alpha1)
  else if (std::fabs(alpha2 - alpha1) < std::fabs(alpha1) * std::numeric_limits<double>::epsilon())
    alpha2 += 2 * M_PI;

  //std::cout << "end vpMeEllipse::computeAngle(..)" << alpha1 << "  " << alpha2 << std::endl ;
  
  
}

/*!
  Display a selection of the gray level image \e I (8bits).

  \warning Display has to be initialized.

  \warning Suppress the overlay drawing in the region of interest.

  \param I : Image to display.
  
  \param iP : Top left corner of the region of interest
  
  \param width : Width of the region of interest
  
  \param height : Height of the region of interest

  \sa init(), closeDisplay()
*/
void vpDisplayOpenCV::displayImageROI ( const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height )
{
  if (displayHasBeenInitialized)
  { 
    vpImage<unsigned char> Itemp;
    vpImageTools::createSubImage(I,(unsigned int)iP.get_i(),(unsigned int)iP.get_j(),height,width,Itemp);
    vpImage<vpRGBa> Ic;
    vpImageConvert::convert(Itemp,Ic);
    
    CvSize size = cvSize((int)this->width, (int)this->height);
    int depth = 8;
    int channels = 3;
    if (background != NULL){
      if(background->nChannels != channels || background->depth != depth
         || background->height != (int) I.getHeight() || background->width != (int) I.getWidth()){
        if(background->nChannels != 0) cvReleaseImage(&background);
        background = cvCreateImage( size, depth, channels );
      }
    }
    else background = cvCreateImage( size, depth, channels );
    
    IplImage* Ip = NULL;
    vpImageConvert::convert(Ic, Ip);
    
    unsigned char * input = (unsigned char*)Ip->imageData;
    unsigned char * output = (unsigned char*)background->imageData;
    
    unsigned int iwidth = Ic.getWidth();

    input = input;
    output = output + (int)(iP.get_i()*3*this->width+ iP.get_j()*3);
    
    unsigned int i = 0;
    while (i < height)
    {
      unsigned int j = 0;
      while (j < width)
      {
	*(output+3*j) = *(input+j*3);
	*(output+3*j+1) = *(input+j*3+1);
	*(output+3*j+2) = *(input+j*3+2);
	j++;
      }
      input = input + 3*iwidth;
      output = output + 3*this->width;
      i++;
    }

    cvReleaseImage(&Ip);
  }
  else
  {
    vpERROR_TRACE("openCV not initialized " ) ;
    throw(vpDisplayException(vpDisplayException::notInitializedError,
                             "OpenCV not initialized")) ;
  }
}

/*!
	
  Get the extremities of the line.

  \param ip1 : Coordinates of the first extremity.
  \param ip2 : Coordinates of the second extremity.
*/
void
vpMeLine::getExtremities(vpImagePoint &ip1, vpImagePoint &ip2)
{
  /*Return the coordinates of the extremities of the line*/
  ip1.set_i( PExt[0].ifloat );
  ip1.set_j( PExt[0].jfloat );
  ip2.set_i( PExt[1].ifloat );
  ip2.set_j( PExt[1].jfloat );
}

bool
vpImageSimulator::getPixel(const vpImagePoint &iP, unsigned char &Ipixelplan)
{
//  std::cout << "In get Pixel" << std::endl;
  //test si pixel dans zone projetee
  bool inside = false;
  for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
      if(listTriangle[i].inTriangle(iP)){
          inside = true;
          break;
      }
  if(!inside) return false;

//  if(!T1.inTriangle(iP) && !T2.inTriangle(iP)){
////      std::cout << "The pixel is inside the projected area" << std::endl;
//    return false;}

  //methoed algebrique
  double z;

  //calcul de la profondeur de l'intersection
  z = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
  //calcul coordonnees 3D intersection
  Xinter_optim[0]=iP.get_u()*z;
  Xinter_optim[1]=iP.get_v()*z;
  Xinter_optim[2]=z;

  //recuperation des coordonnes de l'intersection dans le plan objet
  //repere plan object : 
  //	centre = X0_2_optim[i] (premier point definissant le plan)
  //	base =  u:(X[1]-X[0]) et v:(X[3]-X[0])
  //ici j'ai considere que le plan est un rectangle => coordonnees sont simplement obtenu par un produit scalaire
  double u = 0, v = 0;
  for(unsigned int i = 0; i < 3; i++)
  {
    double diff = (Xinter_optim[i]-X0_2_optim[i]);
    u += diff*vbase_u_optim[i];
    v += diff*vbase_v_optim[i];
  }
  u = u/(euclideanNorm_u*euclideanNorm_u);
  v = v/(euclideanNorm_v*euclideanNorm_v);

  if( u > 0 && v > 0 && u < 1. && v < 1.)
  {
    double i2,j2;
    i2=v*(Ig.getHeight()-1);
    j2=u*(Ig.getWidth()-1);
    if (interp == BILINEAR_INTERPOLATION)
      Ipixelplan = Ig.getValue(i2,j2);
    else if (interp == SIMPLE)
      Ipixelplan = Ig[(unsigned int)i2][(unsigned int)j2];
    return true;
  }
  else
    return false;
}

bool 
vpImageSimulator::getPixelDepth(const vpImagePoint iP,double &Zpixelplan)
{
  //test si pixel dans zone projetee
  if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
    return false;

  Zpixelplan = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
  return true;
}

int
vpMeTracker::outOfImage(vpImagePoint iP, int half, int rows, int cols)
{
  int i = vpMath::round(iP.get_i());
  int j = vpMath::round(iP.get_j());
  return (! ((i> half+2) &&
    (i< rows -(half+2)) &&
    (j>half+2) &&
    (j<cols-(half+2)))
    ) ;
}

/*!  
  Display a rectangle.

  \param topLeft : Top-left corner of the rectangle.
  \param bottomRight : Bottom-right corner of the rectangle.
  \param color : Rectangle color.
  \param fill : When set to true fill the rectangle.
  \param thickness : Thickness of the four lines used to display the
  rectangle.

  \warning The thickness can not be set if the display uses the d3d library.
*/
void vpDisplayWin32::displayRectangle( const vpImagePoint &topLeft,
                                       const vpImagePoint &bottomRight,
                                       const vpColor &color, bool fill,
			                                 unsigned int thickness )
{
  //wait if the window is not initialized
  waitForInit();
  unsigned int width = static_cast<unsigned int>( bottomRight.get_j() - topLeft.get_j() );
  unsigned int height = static_cast<unsigned int>(bottomRight.get_i() - topLeft.get_i() );
  window.renderer->drawRect(topLeft,width,height,color, fill, thickness);
}