#include <opencv/highgui.h>
#include <opencv/cv.h>
#include <cstdlib>
#include <stdio.h>
#include <opencv/cxcore.h>
#include <X11/extensions/XTest.h>


/*---------------------------------------------------------------------*
 * Structure permettant de stocker des informations sur la trajectoire *
 *---------------------------------------------------------------------*/
typedef struct myPoint
{
  CvPoint myTarget;
  myPoint *prevPoint;
  myPoint *nextPoint;
  int nFingers;
  int rank;
} myPoint;

/*--------------------------------------------------------------------------*
 * Fonction permettant d'executer cvEqualizeHist() sur une image à 3 canaux *
 *--------------------------------------------------------------------------*/
void autoEqualize(IplImage *imgIN, IplImage *imgOUT)
{
  IplImage *imgB=cvCreateImage(cvGetSize(imgIN), imgIN->depth, 0);
  IplImage *imgG=cvCloneImage(imgB);
  IplImage *imgR=cvCloneImage(imgB);

  cvSplit(imgIN, imgB, imgG, imgR, NULL);
  cvEqualizeHist(imgB, imgB);
  cvEqualizeHist(imgG, imgG);
  cvEqualizeHist(imgR, imgR);
  cvMerge(imgB, imgG, imgR, NULL, imgOUT);

  cvReleaseImage(&imgB);
  cvReleaseImage(&imgG);
  cvReleaseImage(&imgR);
}

/*-------------------------------------------*
 * Fonction permettant de capturer une frame *
 *-------------------------------------------*/
IplImage *getFrame(CvCapture *capture)
{
  IplImage *img;
  int flip=0;

  cvGrabFrame(capture);
  img=cvRetrieveFrame(capture);
  if(img->origin!=IPL_ORIGIN_TL) flip=CV_CVTIMG_FLIP;
  cvConvertImage(img, img, flip);

  return img;
}

/*--------------------------------------------------------------------*
 * Fonction segmentant une image par sous-échantillonnage chromatique *
 *--------------------------------------------------------------------*/
void mySegmentation(IplImage *imgIN, IplImage *imgOUT, double factor)
{
  IplImage *imgTMP=cvCloneImage(imgIN);
  IplImage *imgBIN=cvCreateImage(cvGetSize(imgIN), imgIN->depth, 0);
  IplImage *imgG=cvCreateImage(cvGetSize(imgIN), imgIN->depth, 0);
  IplImage *imgR=cvCloneImage(imgG);
  IplImage *imgB=cvCloneImage(imgG);

  //Sous-échantillonnage
  cvConvertScale(imgTMP, imgTMP, 1/factor);
  cvConvertScale(imgTMP, imgTMP, factor);

  //Suppression des zones blanches par soustraction du canal vert au canal rouge (Utilisation d'un masque)
  cvConvertImage(imgTMP, imgBIN);
  cvThreshold(imgBIN, imgBIN, 250, 1, CV_THRESH_BINARY_INV);
  cvSplit(imgTMP, imgB, imgG, imgR, NULL);
  cvSub(imgR, imgG, imgBIN, imgBIN);
  cvThreshold(imgBIN, imgBIN, 1, 255, CV_THRESH_BINARY);

  //Traitement final
  cvSmooth(imgBIN, imgBIN, CV_MEDIAN, 3);
  cvDilate(imgBIN, imgBIN);
  cvCopy(imgBIN, imgOUT);

  cvReleaseImage(&imgTMP);
  cvReleaseImage(&imgBIN);
  cvReleaseImage(&imgG);
  cvReleaseImage(&imgR);
  cvReleaseImage(&imgB);
}

/*----------------------------------------------------------------*
 * Fonction determinant le plus petit CvRect encadrant un contour *
 *----------------------------------------------------------------*/
CvRect myMinAreaRect(CvSeq *seqContour)
{
  CvPoint arrContour[seqContour->total];
  cvCvtSeqToArray(seqContour, arrContour, CV_WHOLE_SEQ);
  int Xmax, Xmin, Ymax, Ymin;
  CvRect myRect;

  Xmax=arrContour[0].x;
  Ymax=arrContour[0].y;
  Xmin=Xmax;
  Ymin=Ymax;

  for(int i=1; i<seqContour->total; i++)
  {
    if(arrContour[i].x>Xmax) Xmax=arrContour[i].x;
    if(arrContour[i].x<Xmin) Xmin=arrContour[i].x;
    if(arrContour[i].y>Ymax) Ymax=arrContour[i].y;
    if(arrContour[i].y<Ymin) Ymin=arrContour[i].y;
  }

  myRect.x=Xmin-10;
  myRect.y=Ymin-10;
  myRect.width=Xmax-Xmin+20;
  myRect.height=Ymax-Ymin+20;

  return myRect;
}

/*----------------------------------------------------------*
 * Fonction retournant le point "le plus haut" d'un contour *
 *----------------------------------------------------------*/
CvPoint myTarget(CvPoint *poly, int total)
{
  CvPoint pt=poly[0];
  for(int i=1; i<total; i++) if(poly[i].y<pt.y) pt=poly[i];

  return pt;
}

/*----------------------------------------------------*
 * Fonction calculant l'angle forme par deux vecteurs *
 *----------------------------------------------------*/
double getAngle(CvPoint U, CvPoint V)
{
  double angle=acos((U.x*V.x+U.y*V.y)/(sqrt(U.x*U.x+U.y*U.y)*sqrt(V.x*V.x+V.y*V.y)));
  return angle;
}

void myTrajectoryReset(myPoint *traj, int nPt, int N)
{
  for(int i=0; i<nPt; i++) traj[i].myTarget=traj[N].myTarget;
}

void myFingerReset(myPoint *traj, int nPt)
{
  for(int i=0; i<nPt; i++) traj[i].nFingers=3;
}

/*---------------------------------------------------*
 * Fonction caculant le determinant de deux vecteurs *
 *---------------------------------------------------*/
double getDet(CvPoint U, CvPoint V)
{
  double det=U.x*V.y-U.y*V.x;
  return det;
}

void myNext()
{
  Display *dpy = XOpenDisplay(NULL);

  XTestFakeKeyEvent(dpy, 114, true, CurrentTime);
  XTestFakeKeyEvent(dpy, 114, false, CurrentTime);
  XCloseDisplay(dpy);
}

void myPrev()
{
  Display *dpy = XOpenDisplay(NULL);

  XTestFakeKeyEvent(dpy, 113, true, CurrentTime);
  XTestFakeKeyEvent(dpy, 113, false, CurrentTime);
  XCloseDisplay(dpy);
}

void myUp()
{
  Display *dpy = XOpenDisplay(NULL);

  XTestFakeKeyEvent(dpy, 111, true, CurrentTime);
  XTestFakeKeyEvent(dpy, 111, false, CurrentTime);
  XCloseDisplay(dpy);
}

void myDown()
{
  Display *dpy = XOpenDisplay(NULL);

  XTestFakeKeyEvent(dpy, 116, true, CurrentTime);
  XTestFakeKeyEvent(dpy, 116, false, CurrentTime);
  XCloseDisplay(dpy);
}

void myValid()
{
  Display *dpy = XOpenDisplay(NULL);

  XTestFakeKeyEvent(dpy, 36, true, CurrentTime);
  XTestFakeKeyEvent(dpy, 36, false, CurrentTime);
  XCloseDisplay(dpy);
}

void myBack()
{
  Display *dpy = XOpenDisplay(NULL);

  XTestFakeKeyEvent(dpy, 36, true, CurrentTime);
  XTestFakeKeyEvent(dpy, 36, false, CurrentTime);
  XTestFakeKeyEvent(dpy, 27, true, CurrentTime);
  XTestFakeKeyEvent(dpy, 27, false, CurrentTime);
  XCloseDisplay(dpy);
}

void myLock()
{
  Display *dpy = XOpenDisplay(NULL);

  XTestFakeKeyEvent(dpy, 55, true, CurrentTime);
  XTestFakeKeyEvent(dpy, 55, false, CurrentTime);
  XCloseDisplay(dpy);
}

void myUnlock()
{
  Display *dpy = XOpenDisplay(NULL);

  XTestFakeKeyEvent(dpy, 40, true, CurrentTime);
  XTestFakeKeyEvent(dpy, 40, false, CurrentTime);
  XCloseDisplay(dpy);
}


/*------------------------------*
 * Debut du programme principal *
 *------------------------------*/
int main()
{
//----------[Initialisation des variables]----------
  CvCapture *capture=cvCreateCameraCapture(-1);
  IplImage *imgIN=getFrame(capture);
  IplImage *imgBIN=cvCreateImage(cvGetSize(imgIN), imgIN->depth, 0);
  IplImage *imgTMP=cvCloneImage(imgBIN);

  CvSeq *seqContour, *handContour, *seqApprox;
  int nContours, area, tmpVar, nFingers, approxSize, nPt, moveLength, threshold;
  nPt=16;
  bool lock;
  lock=1;
  threshold=295;
  myPoint myTrajectory[nPt], *currentPt;
  double angle, det;
  CvPoint U, V, myFinger;
  CvRect myRect;
  CvMemStorage *stor0=cvCreateMemStorage(0);
  CvMemStorage *stor1=cvCreateMemStorage(0);
  CvFont font;
  char text[5];
  cvInitFont(&font , CV_FONT_HERSHEY_SIMPLEX, .5, .5, 0.0, 1);
  char key='\0';

  cvNamedWindow("BiNARY", CV_WINDOW_AUTOSIZE);




//----------[Initialisation de la liste chainée de trajectoire]----------
  for(int i=0; i<nPt; i++)
  {
    myTrajectory[i].nextPoint=&myTrajectory[(i+1)%nPt];
    myTrajectory[i].rank=i;
    myTrajectory[i].prevPoint=&myTrajectory[(i-1)%nPt];
    myTrajectory[i].nFingers=0;
  }
  currentPt=&myTrajectory[nPt-1];               //Pointeur qui permettra d'enregistrer les infos sur la trajectoire



  while(key!='q')
  {
    if(key==81) threshold-=5;               //Fleche gauche : diminiue le seuil de segmentation
    else if(key==83) threshold+=5;              //Fleche droite : augmente le seuil de segmentation


//----------[Chargement d'une image]----------
    imgIN=getFrame(capture);



//----------[Segmentation]----------
    mySegmentation(imgIN, imgBIN, threshold);



//----------[Détection des contours]----------
    cvCopy(imgBIN, imgTMP);
    nContours=cvFindContours(imgTMP, stor0, &seqContour, sizeof(CvContour), CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
    if(nContours>0)
    {
      area=tmpVar=0;
      for(int i=0; i<nContours; i++)        //Determination du contour ayant l'aire la plus grande
      {
    tmpVar=abs(cvContourArea(seqContour));
    if(tmpVar>area)
    {
      area=tmpVar;
      handContour=seqContour;
    }
    seqContour=seqContour->h_next;
      }



//----------[Sélection de la zone d'intérêt]----------
      myRect=myMinAreaRect(handContour);
      cvSetImageROI(imgBIN, myRect);
      IplImage *imgROI_SEG=cvCreateImage(cvGetSize(imgBIN), IPL_DEPTH_8U, imgBIN->nChannels);
      cvCopy(imgBIN, imgROI_SEG);
      cvResetImageROI(imgBIN);



//----------[Détection des contours de la région d'intérêt]----------
      IplImage *imgROI_TMP=cvCloneImage(imgROI_SEG);
      cvClearMemStorage(stor0);
      //On peut insérer ici un nouvel algorithme de segmentation plus fin
      nContours=cvFindContours(imgROI_TMP, stor0, &seqContour, sizeof(CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
      if(nContours>0)
      {
    area=tmpVar=0;
    for(int i=0; i<nContours; i++)          //Selection du contour ayant la plus grande aire
    {
      tmpVar=abs(cvContourArea(seqContour));
      if(tmpVar>area)
      {
        area=tmpVar;
        handContour=seqContour;
      }
      seqContour=seqContour->h_next;
    }

//  printf("%d\n", area);

    if(area>1000)
    {
//----------[Approche polygonale du contour précédent]----------
      seqApprox=cvApproxPoly(handContour, sizeof(CvContour), stor1, CV_POLY_APPROX_DP, 9, 0);
      if(seqApprox->total>0)
      {
        CvPoint arrTmp[seqApprox->total];
        CvPoint arrApprox[seqApprox->total];
        cvCvtSeqToArray(seqApprox, arrTmp, CV_WHOLE_SEQ);

        approxSize=seqApprox->total;
        int j=0;
        int k=0;
        for(int i=0;i<approxSize;i++)
        {
          U.x=arrTmp[(i+1)%approxSize].x-arrTmp[i].x;
          U.y=arrTmp[(i+1)%approxSize].y-arrTmp[i].y;

          if(sqrt(U.x*U.x+U.y*U.y)>15)          //Suppression des sommets "trop proches"
          {                         //afin d'obtenir des angles aigues au niveau des doigts
        arrApprox[j]=arrTmp[i];
        j++;
          }
          else { cvSeqRemove(seqApprox,i-k); k++; }
        }

        if(seqApprox->total>0)
        {
          cvSetImageROI(imgIN, myRect);



//----------[Détection des doigts]----------
          nFingers=0;
          for(int i=0; i<seqApprox->total; i++)
          {
        U.x=arrApprox[(i+1)%seqApprox->total].x-arrApprox[i].x;
        U.y=arrApprox[(i+1)%seqApprox->total].y-arrApprox[i].y;
        V.x=arrApprox[(i+2)%seqApprox->total].x-arrApprox[(i+1)%seqApprox->total].x;
        V.y=arrApprox[(i+2)%seqApprox->total].y-arrApprox[(i+1)%seqApprox->total].y;

        angle=getAngle(U, V);                   //Calcul de l'angle et du déterminant de chaque sommet
        det=getDet(U, V);
        myFinger=arrApprox[(i+1)%seqApprox->total];

        if(angle>2.0 && det<0 && myFinger.y<myRect.width*2/3)   //Sélection des doigts
        {
          cvCircle(imgIN, myFinger, 8, cvScalar(255,0,0), 2);
          nFingers++;
        }
          }
          (*currentPt).nFingers=nFingers;
          (*currentPt).myTarget=myTarget(arrApprox, seqApprox->total);

          cvResetImageROI(imgIN);

          ((*currentPt).myTarget).x+=myRect.x;
          ((*currentPt).myTarget).y+=myRect.y;


//----------[Traitement de la trajectoire]----------
          for(int i=0; i<nPt; i++)          //Barre vert représentant la projection de la trajectoire sur X
          {
        U=myTrajectory[i].myTarget;
        V=myTrajectory[(i+1)%nPt].myTarget;
          }

          tmpVar=(*currentPt).rank;         //Calcul de la longueur de la trajectoire projettee
          U=myTrajectory[tmpVar].myTarget;
          V=myTrajectory[(tmpVar+1)%nPt].myTarget;
          U.x=U.x-V.x;
          U.y=U.y-V.y;
          moveLength=sqrt(U.x*U.x+U.y*U.y);



//----------[LOCK/UNLOCK]----------
          if(moveLength<5)                  //Si la mouvement n'excede pas 5 pixel...
          {
        tmpVar=0;
        for(int i=0; i<nPt; i++) tmpVar+=myTrajectory[i].nFingers;
        if(tmpVar==nPt*5 && lock) { lock=0; myUnlock(); myFingerReset(myTrajectory, nPt); }     //... et qu'on detecte bien 5 doigts pendant nPt frames : "on déverouille"
        else if(tmpVar==0 && !lock) { lock=1; myLock(); myFingerReset(myTrajectory, nPt); }     //... et qu'on ne detecte aucun doigts pendant nPt frames : "on vérouille"
          }



          if(!lock)                     //Si on est déverouille...
          {
//----------[VALID/RETOUR]----------
        if(moveLength<5)                    //Si la mouvement n'excede pas 5 pixel...
        {
          tmpVar=0;
          for(int i=0; i<nPt; i++) tmpVar+=myTrajectory[i].nFingers;
          if(tmpVar==nPt*2) { myBack(); myFingerReset(myTrajectory, nPt); }
          else if(tmpVar==nPt*5) { myValid(); myFingerReset(myTrajectory, nPt); }
        }


//----------[MORE/LESS]----------
        tmpVar=1;
        for(int i=0; i<nPt; i++) tmpVar*=(myTrajectory[i].nFingers==1);     //... et qu'on détecte qu'un seul doigts sur nPt frames
        if(tmpVar==1)
        {
          if(((*currentPt).myTarget).x-myRect.x>myRect.width/4) { myUp(); } //... et que le doigt est à droite alors : "on augmente"
          else if(((*currentPt).myTarget).x-myRect.x<myRect.width/4) { myDown(); } // et que le doigt est à gauche alors : "on diminiue"
        }


//----------[NEXT/PREV]----------
            tmpVar=(*currentPt).rank;
        U=myTrajectory[tmpVar].myTarget;
        V=myTrajectory[(tmpVar+nPt-5)%nPt].myTarget;                //Sélection de la longueur de trajectoire sur 5 frames
        moveLength=U.x-V.x;

        if(abs(moveLength)>imgIN->width/3)                  //Si le mouvement est supérieur à 1/4 de l'ecran...
        {
          if(moveLength<0) { myPrev(); }            //... et que le mouvement va de droite à gauche alors : "precedent"
          else { myNext(); }                    //... et que le mouvement va de gauche à droite alors : "suivant"
          myTrajectoryReset(myTrajectory, nPt, tmpVar);
        }


//----------[UP/DOWN]----------
            tmpVar=(*currentPt).rank;
        U=myTrajectory[tmpVar].myTarget;
        V=myTrajectory[(tmpVar+nPt-7)%nPt].myTarget;                //Sélection de la longueur de trajectoire sur 5 frames
        moveLength=U.y-V.y;

        if(abs(moveLength)>imgIN->width/5)                  //Si le mouvement est supérieur à 1/4 de l'ecran...
        {
          if(moveLength<0) { myUp(); }          //... et que le mouvement va de droite à gauche alors : "precedent"
          else { myDown(); }                    //... et que le mouvement va de gauche à droite alors : "suivant"
          myTrajectoryReset(myTrajectory, nPt, tmpVar);
        }
          }
        }
      }
    }

    else
    {
      tmpVar=(*currentPt).rank;
      myTrajectoryReset(myTrajectory, nPt, tmpVar);
    }
    currentPt=(*currentPt).nextPoint;                       //On pointe sur l'élement suivant de la liste chainée

      }

//----------[Libération de la mémoire]----------
      cvReleaseImage(&imgROI_SEG);
      cvReleaseImage(&imgROI_TMP);
    }

//----------[Mise en forme de l'image à afficher]----------
    myRect.x=0;
    myRect.y=0;
    myRect.width=200;
    myRect.height=75;
    cvSetImageROI(imgBIN, myRect);
//     cvFillImage(imgBIN, 50);
    cvRectangle(imgBIN, cvPoint(0,0), cvPoint(myRect.width-1, myRect.height-1), cvScalar(255), 2);

    cvPutText(imgBIN, "THRESHOLD :", cvPoint(10, 25), &font, cvScalar(255, 255, 255));
    sprintf(text, "%d", threshold);
    cvPutText(imgBIN, text, cvPoint(120, 25), &font, cvScalar(255, 255, 255));

    cvPutText(imgBIN, "LOCK :", cvPoint(10, 50), &font, cvScalar(255, 255, 255));
    sprintf(text, "%d", lock);
    cvPutText(imgBIN, text, cvPoint(120, 50), &font, cvScalar(255, 255, 255));

    cvResetImageROI(imgBIN);


    cvShowImage("BiNARY", imgBIN);


    cvClearMemStorage(stor0);
    cvClearMemStorage(stor1);

    key=cvWaitKey(10);
  }

  cvDestroyAllWindows();

  cvReleaseCapture(&capture);
  cvReleaseImage(&imgIN);
  cvReleaseImage(&imgBIN);
  cvReleaseImage(&imgTMP);
  cvClearMemStorage(stor0);
  cvReleaseMemStorage(&stor0);
  cvClearMemStorage(stor1);
  cvReleaseMemStorage(&stor1);

  return 0;
}

// clear && g++ cam29bis.cpp -o cam29bis -lcv -lhighgui -lXtst -Wall && ./cam29bis
// xmodmap -pke