#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>

#include <opencv2/highgui/highgui_c.h>
#include <opencv2/imgproc/imgproc_c.h>

#include <ardrone_tool/UI/ardrone_input.h>

#define RESULT_NAME "Result Template matching"
#define TEMPLATE_FILE "/home/wissam/ARDrone_SDK_2_0_1/Examples/Linux/video_demo/Sources/Landing/stemplate_4.2.png"

extern IplImage* inCamera;
typedef unsigned char uchar; 

CvPoint GetTemplatePosition(IplImage* image, IplImage* template) {
  int rows, cols;
  double minVal, maxVal;
  CvPoint minLoc, maxLoc, matchLoc, matchCenter;
  CvMat* result;

  rows = image->height - template->height + 1;
  cols = image->width - template->width + 1;

  result = cvCreateMat(rows, cols, CV_32FC1);
  /* printf("ROWSxCOLS --  %dx%d\n", rows, cols); */

  cvMatchTemplate(image, template, result, CV_TM_SQDIFF);
  cvNormalize(result, result, 0, 1, CV_MINMAX, NULL);

  /* Get the center. */
  cvMinMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc, NULL);
  matchLoc = minLoc;
  matchCenter.x = matchLoc.x + template->width / 2;
  matchCenter.y = matchLoc.y + template->height / 2;

  /* Rectangle. */
  CvPoint tmp;
  tmp.x = matchLoc.x + template->width;
  tmp.y = matchLoc.y + template->height;
  cvRectangle(image, matchLoc, tmp, cvScalar(0.0, 0.0, 0.0, 0.0), 2, 8, 0 );

  /* Circle */
  /* cvCircle(image, matchCenter, 5, cvScalar(0.0, 0.0, 0.0, 0.0), 1, 8, 0); */

  return matchCenter;
}

int GetColorDistance(uchar r1, uchar g1, uchar b1, uchar r2, uchar g2, uchar b2) {
  return (r2 - r1) * (r2 - r1) + (g2 - g1) * (g2 - g1) + (b2 - b1) * (b2 - b1);
}

void GetPixelColor(IplImage* img, int row, int col, uchar* r, uchar* g, uchar* b) {
  *b = img->imageData[(img->width * row + col) * img->nChannels];
  *g = img->imageData[(img->width * row + col) * img->nChannels + 1];
  *r = img->imageData[(img->width * row + col) * img->nChannels + 2];
}

int CheckTemplateColor(IplImage* image, CvPoint* templateCenter) {
  int templateColors[3][3] = {
    /* {0, 255, 0}, */
    /* {255, 0, 0}, */
    /* {0, 0, 255} */

    /*{106, 111, 83},
    {150, 60, 45},
    {76, 78, 95}*/
    
    /*{90, 150, 120},
    {80, 80, 180}
  */
    {65, 125, 110}, 
    {65, 75,118}

  };
  int tolerance = 2000;
  int colorCheck[2] = {0};
  int colorDistance;
  int col, colIncStep = 1;
  uchar r, g, b;

  /* Check center. */
  GetPixelColor(image, templateCenter->y, templateCenter->x, &r, &g, &b);
  colorDistance = GetColorDistance(r, g, b, templateColors[0][0], templateColors[0][1], templateColors[0][2]);
  if (colorDistance > tolerance) {
    return 0;
  } else {
     //printf("VALIDATE CENTER DISTANCE : %d\n", colorDistance); 
  }

  /* Check second color. */
  for (col = templateCenter->x; col < image->width; col += colIncStep) {
    GetPixelColor(image, templateCenter->y, col, &r, &g, &b);
    if ((colorDistance = GetColorDistance(r, g, b, templateColors[0][0], templateColors[0][1], templateColors[0][2])) > tolerance) {
      if ((colorDistance = GetColorDistance(r, g, b, templateColors[1][0], templateColors[1][1], templateColors[1][2])) < tolerance) {
        colorCheck[0] = 1;
        // printf("VALIDATE SECOND DISTANCE : %d\n", colorDistance); 
        break;
      }
    }
  }
  if (!colorCheck[0]) {
    return 0;
  }

  /* Check third color. */
  /*for (; col < image->width; col += colIncStep) {
    GetPixelColor(image, templateCenter->y, col, &r, &g, &b);
    if ((colorDistance = GetColorDistance(r, g, b, templateColors[1][0], templateColors[1][1], templateColors[1][2])) > tolerance) {
      if ((colorDistance = GetColorDistance(r, g, b, templateColors[2][0], templateColors[2][1], templateColors[2][2])) < tolerance) {
        colorCheck[1] = 1;
         printf("VALIDATE THIRD DISTANCE : %d\n", colorDistance); 
        break;
      }
    }
  }
  if (!colorCheck[1]) {
    return 0;
  }*/

  return 1;
}

int CheckAllTemplates(IplImage* image, IplImage** template, CvPoint* center, int size) {
  CvPoint templateCenter;
  int i;

  for (i = 0; i < size; ++i) {
 

    templateCenter = GetTemplatePosition(image, template[i]);
  

    if (CheckTemplateColor(image, &templateCenter)) {
      *center = templateCenter;
      return i + 1;
    }
  }
  return 0;
}

IplImage** LoadTemplates(char** templatesFiles, int* templatesNumber) {
  IplImage** template;
  int size;
  int i;

  for (i = 0;;++i) {
    if (templatesFiles[i] == NULL) {
      size = i;
      break;
    }
  }

  template = vp_os_malloc(sizeof(IplImage*) * size);

  for (i = 0; i < size; ++i) {
    template[i] = cvLoadImage(templatesFiles[i], CV_LOAD_IMAGE_COLOR);
  }

  printf("SIZE : %d\n", size);
  *templatesNumber = size;
  return template;
}

int DistanceOld(int x1, int y1, int x2, int y2) {
  return (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2);
}

int Distance(int x1, int x2) {
  return (x1 - x2) * (x1 - x2);
}

void move(CvPoint templateCenter, CvPoint cameraSnapCenter){
  int time;
  int distance;
  int signe;
  //calculer le temps pour allez jusqu'au pts x 640*360
 
  /*distance = templateCenter.x - cameraSnapCenter.x;
  time = abs(distance) * 10000; //~ 20 -> 200 ms 
  
  signe = (distance > 0) ? 1.0 : -1.0;
  
  printf("left or right %d\n", signe);
  ardrone_tool_set_progressive_cmd(1, signe, 0, 0.0, 0.0, 0.0, 0.0);
  
  usleep(time);
  ardrone_tool_set_progressive_cmd(0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
  */
  //calculer le temps pour allez jusqu'au pts y
  
  distance = templateCenter.y - cameraSnapCenter.y;
  time = /* en fonction du template */ 100 ;
  
  signe = (distance > 0) ? -1.0 : 1.0;  // neg pour aller en haut en terme de distance
  
  printf("up or down\n");
  ardrone_tool_set_progressive_cmd(1, 0, signe, 0.0, 0.0, 0.0, 0.0);
  
  usleep(time);
  ardrone_tool_set_progressive_cmd(0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
  
  return;
}

void trueCopyIplImage( IplImage* dest,  IplImage* src){
    dest->nSize = src->nSize;
    dest->ID = src->ID;
    dest->nChannels = src->nChannels;
    dest->depth = src->depth;
    dest->dataOrder = src->dataOrder;
    dest->origin = src->origin;
    dest->align = src->align;
    dest->width = src->width;
    dest->roi = NULL; // Arthur -> https://fr.wikipedia.org/wiki/Roi_Arthur apparemment il est NULL.
    dest->imageSize = src->imageSize;
    memcpy(dest->imageData, src->imageData, src->imageSize);
    dest->widthStep = src->widthStep;
    dest->imageDataOrigin = dest->imageData; 

}

void FilterLightPixels(IplImage* img) {
  int distance;
  int row, col;
  uchar r, g, b;

  for (row = 0; row < img->height; ++row) {
    for (col = 0; col < img->width; ++col) {
      GetPixelColor(img, row, col, &r, &g, &b);
      distance = GetColorDistance(255, 255, 255, r, g, b);
      if (distance <= 10) {
        //fprintf(stderr, "Distance : %d\n", distance);
        cvSet2D(img, row, col, CV_RGB(255, 255, 255));
      } else {
        cvSet2D(img, row, col, CV_RGB(0, 0, 0));
      }
    }
  }
}

void Landing(void) {
 // char* templatesFiles[] = {//"/home/wissam/ARDrone_SDK_2_0_1/Examples/Linux/video_demo/Sources/Landing/stemplate_4.0.png",
   //                         "/home/wissam/ARDrone_SDK_2_0_1/Examples/Linux/video_demo/Sources/Landing/template_60x60.png",
     //                       "/home/wissam/ARDrone_SDK_2_0_1/Examples/Linux/video_demo/Sources/Landing/template_43x43.png",
       //                     "/home/wissam/ARDrone_SDK_2_0_1/Examples/Linux/video_demo/Sources/Landing/template_30x30.png", NULL};
  //IplImage** template;
  //template = LoadTemplates(templatesFiles, &templatesNumber);
   //CvSize cvsize;
  //cvsize.width = inCamera->width; cvsize.height = inCamera->height ;
  //IplImage* copy = cvCreateImage(cvsize, inCamera->depth, inCamera->nChannels);
  
  cvNamedWindow(RESULT_NAME, CV_WINDOW_AUTOSIZE);
  int numberOfPixel; 
  CvPoint cameraSnapCenter;
  do {
    IplImage* cameraSnap = cvCloneImage(inCamera);
    cameraSnap->width = inCamera->width;
    cameraSnap->height = inCamera->height;

   
    //int check = CheckAllTemplates(cameraSnap,  &center, templatesNumber);
    FilterLightPixels(cameraSnap);
  //  printf("\nTemplate Matching:\n");
  //  printf("MATCH : %d\n", check);

    cvShowImage(RESULT_NAME, cameraSnap);
    if(cvWaitKey(1) == 'e'){ // emergency //
      ardrone_tool_input_reset(); 
      ardrone_tool_set_ui_pad_select(1);
      break;
    }

   
/*if(check != 0){// si la un template a match//   
   	cameraSnapCenter.x = cameraSnap->width / 2;
   	cameraSnapCenter.y=cameraSnap->height / 2;
      
      //si on est pas au centre sur Y//
   	if(Distance(templateCenter.y, cameraSnapCenter.y) > cameraSnap->width*cameraSnap->height * 0.1){
    	    move(templateCenter, cameraSnapCenter);
   	}
   
   	else{ //si on a rien match on tourne
   	    printf("search \n");
   	}
   }*/
   cvReleaseImage(&cameraSnap);
   
  }while(1);

    return; 
  }