#include "opencv2/opencv.hpp"
#include "opencv2/contrib/contrib.hpp"
#include "opencv2/objdetect/objdetect.hpp"
#include <X11/Xlib.h>
#include <iostream>
#include <unistd.h>
#include <stdio.h>
#include <string.h>

using namespace cv;



int main(int, char**)
{
  //choisir l'ecran
  Display* dpy = XOpenDisplay(0);
  int scr = XDefaultScreen(dpy);
  Window root_window = XRootWindow(dpy, scr);

  //calculer la taille et de l'ecran
  int height = DisplayHeight(dpy, scr);
  int width  = DisplayWidth(dpy, scr);
  std::cout << "Screen size : " << width << "x" << height << std::endl;

  //initialiser la position du curseur au milieu de l'écran
  int sx,sy;
  sx=width/2; sy=height/2;
    
  vector<Mat> images;
  vector<int> labels;

  CascadeClassifier face_cascade;
  CascadeClassifier eye_cascade;
  CascadeClassifier nose_cascade;

  if( !face_cascade.load("./haarcascade_frontalface_default.xml") ){ printf("--(!)Error loading\n"); return -1; };
  if( !eye_cascade.load("./haarcascade_eye_tree_eyeglasses.xml")){ printf("--(!)Error loading\n"); return -1; };
  if( !nose_cascade.load("./haarcascade_mcs_nose.xml")){ printf("--(!)Error loading\n"); return -1; };
    

  CvCapture* capture;
  capture = cvCaptureFromCAM( -1 );

  Mat grImage;
  namedWindow("face",1);

  for(;;)
    {
      Mat frame;
      frame =  cvQueryFrame( capture );
      //cap >> frame; // get a new frame from camera
      cvtColor(frame, grImage, CV_BGR2GRAY);
      equalizeHist( grImage, grImage );
	    
      vector< Rect_<int> > faces;
      //face_cascade.detectMultiScale(grImage, faces);

      face_cascade.detectMultiScale( grImage, faces, 1.1, 2, 0|CV_HAAR_SCALE_IMAGE, Size(30, 30) );

      for(int i = 0; i < faces.size(); i++) {

	Point center_face ( faces[i].x + faces[i].width*0.5, faces[i].y + faces[i].height*0.5 );
		
	Rect face_i = faces[i];
	rectangle(frame, face_i, CV_RGB(0, 255,0), 1);

	Mat faceROI = grImage( faces[i] );
	std::vector<Rect> eyes;
	std::vector<Rect> nose;

	eye_cascade.detectMultiScale( faceROI, eyes, 1.1, 2, 0 |CV_HAAR_SCALE_IMAGE, Size(30, 30) );
	nose_cascade.detectMultiScale( faceROI, nose, 1.1, 2, 0 |CV_HAAR_SCALE_IMAGE, Size(30, 30) );

	Point tab_eye[2];
	if(eyes.size() == 2){
	  int radius;
	  for( size_t j = 0; j < eyes.size(); j++ ) {
	    Point center( faces[i].x + eyes[j].x + eyes[j].width*0.5, faces[i].y + eyes[j].y + eyes[j].height*0.5 );
	    tab_eye[j] = center;
	    radius = cvRound( (eyes[j].width + eyes[j].height)*0.25 );
	    //circle( frame, center, radius, Scalar( 255, 0, 0 ), 4, 8, 0 );
	  }

	  /*Point center_nose( faces[i].x + nose[0].x + nose[0].width*0.5, faces[i].y + nose[0].y + nose[0].height*0.5 );
	    int radius_nose = cvRound( (nose[0].width + nose[0].height)*0.25 );

	    circle( frame, center_nose, radius_nose, Scalar( 255, 0, 0 ), 4, 8, 0 );*/

	  Point right;
	  Point left;
	  if(tab_eye[0].x > tab_eye[1].x){
	    right = tab_eye[1];
	    left = tab_eye[0];
	  }
	  else{
	    right = tab_eye[0];
	    left = tab_eye[1];
	  }

	  circle( frame, right, radius, Scalar( 255, 0, 0 ), 4, 8, 0 );
	  circle( frame, left, radius, Scalar( 0, 0, 255 ), 4, 8, 0 );

	  int incli = 15;
	  int Spas = 10;
		  
	  if(right.y + incli < left.y){
	    //printf("penche à gauche\n");
	    sx = sx - Spas;
	    if(sx < 0){
	      sx = 0;
	    }
	  }
	  else if(left.y + incli < right.y){
	    //printf("penche à droite\n");
	    sx = sx + Spas;
	    if(sx > width){
	      sx = width;
	    }
	  }
	  else if((center_face.y - (left.y + right.y)/2) > 29){
	    //printf("bas\n");
	    sy = sy + Spas;
	    if( sy > height){
	      sy = height;
	    }
	  }
	  else if((center_face.y - (left.y + right.y)/2) < 23){
	    //printf("haut\n");
	    sy = sy - Spas;
	    if(sy < 0){
	      sy = 0;
	    }
	  }
	  XWarpPointer(dpy, None, root_window, 0, 0, 0, 0, sx, sy);
	  XFlush(dpy);
	  usleep(50);

	  //printf("diff nose eye =%d\n",center_face.y - (left.y + right.y)/2);
		  
	}

      }
        

      imshow("face", frame);
      if(waitKey(30) >= 0) break;
    }

  return 0;
}