#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 "Filter"
#define STEP_1 1
#define STEP_2 2
#define STEP_3 3

extern IplImage* inCamera;
typedef unsigned char uchar; 

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 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;
}

int FilterLightPixels(IplImage* img, CvPoint* center) {
  const int tolerance = 1;
  int distance;
  int row, col;
  unsigned char r, g, b;
  int pixelCounter = 0;

  center->x = 0;
  center->y = 0;
  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 <= tolerance) {
        cvSet2D(img, row, col, CV_RGB(255, 255, 255));
        center->x += col;
        center->y += row;
        ++pixelCounter;
      } else {
        cvSet2D(img, row, col, CV_RGB(0, 0, 0));
      }
    }
  }
  if (pixelCounter) {
    center->x /= pixelCounter;
    center->y /= pixelCounter;
    cvCircle(img, *center, 5, CV_RGB(255, 0, 0), 1, 8, 0);
  }

  return pixelCounter;
}

int Approx(int numberOfWhitePixel) {
  if (numberOfWhitePixel >= 20 && numberOfWhitePixel <= 120) {
    return STEP_3;
  } else if (numberOfWhitePixel >= 121 && numberOfWhitePixel <= 300) {
    return STEP_2;
  } else if (numberOfWhitePixel >= 301) {
    return STEP_1;
  } else {
    return 0;
  }
}

void Landing(void) {
  cvNamedWindow(RESULT_NAME, CV_WINDOW_AUTOSIZE);
  int numberOfWhitePixel; 
  CvPoint center, cameraSnapCenter;
  int distance, signe;
  do {
    IplImage* cameraSnap = cvCloneImage(inCamera);
    cameraSnap->width = inCamera->width;
    cameraSnap->height = inCamera->height;


    numberOfWhitePixel = FilterLightPixels(cameraSnap, &center);


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

   printf("white %d \n", numberOfWhitePixel);
   numberOfWhitePixel = Approx(numberOfWhitePixel);
   switch(numberOfWhitePixel){
        case STEP_1:
        /*proche recule*/
        printf("Recule \n");

        ardrone_tool_set_progressive_cmd(1, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0);
        break;
        
        case STEP_2:
        /* millieu bouge pas et centre toi*/
        printf("Bouge pas \n");
        ardrone_tool_set_progressive_cmd(0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
	
        cameraSnapCenter.x = cameraSnap->width / 2;
        cameraSnapCenter.y = cameraSnap->height / 2;
        //si on est pas au centre sur Y//
        if(Distance(center.y, cameraSnapCenter.y) > cameraSnap->height *7){
            distance = center.y - cameraSnapCenter.y;          
            signe = (distance > 0) ? 1.0 : -1.0;  // +1 monter, -1 on descend
            ardrone_tool_set_progressive_cmd(1, 0.0, 0.0, signe, 0.0, 0.0, 0.0);
	    
           printf("translate y %d\n",signe);

        }
        else if(Distance(center.x, cameraSnapCenter.x) > cameraSnap->width * 7){
          distance = center.x - cameraSnapCenter.x;
          signe = (distance > 0) ? 1.0 : -1.0;  // -1 allez a gauche, +1 allez a droite
          ardrone_tool_set_progressive_cmd(0, 0.0, 0.0, 0, signe, 0.0, 0.0);
          
          printf("rotate %d\n",signe);
  
        }
	else
	    ardrone_tool_set_progressive_cmd(0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
 	


        break;

        case STEP_3:
        /* avance */
        printf("Avance \n");
        ardrone_tool_set_progressive_cmd(1, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0);
        break;
	
        default:
        /*nique ta mere*/
        /* bouge pas */
        ardrone_tool_set_progressive_cmd(0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
        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; 
 }