#include <stdio.h>
#include <stdlib.h>
#include <cv.h> 
#include <highgui.h>
//#include <opencv2/highgui.hpp> //Pour le cvRound
//#include "opencv2/videoio/videoio_c.h" //Pour le CvCapture*

//#include <cxcore.h>
//#include <SFML/Window.h>
#include <SFML/Audio.hpp>
#include <SFML/Graphics.hpp>

#define CONFIG
//#define SFML

//ATTENTION AFFICHAGE OPENCV INCOMPATIBLE AVEC AFFICHAGE SFML

/*Headers*/
void controle_moteur(int vecX, int vecY, int rayon);
int limite_moteur(int val_pwm);
int image_CV2SFML(IplImage* imcv, sf::Image imsf); //Construction de imsf (RGBA) à partir de imcv (BGR), avec alpha constant (=1)

int main(int argc, char* argv[])
{
	int height,width,step,channels;  //parameters of the image we are working on
	int posX, posY; //Position objet
	CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments)); //Variable moyenne position
	int boucle;
#ifdef SFML

	//Initialisation SFML
	
	sf::Texture txFlux;
	sf::Sprite spFlux;
	sf::Image imFlux;
   	sf::Event event;
	
	//Création de la fenetre principale
	sf::RenderWindow window(sf::VideoMode(800, 600), "KirbyTrack");
#endif

    // Open capture device. 0 is /dev/video0, 1 is /dev/video1, etc.
    CvCapture* capture = cvCaptureFromCAM( 0 );
    
    if( !capture ){
            printf("ERROR: capture is NULL \n" );
            return EXIT_FAILURE;
    }
    

    // grab an image from the capture
    IplImage* frame = cvQueryFrame( capture );
    
#ifdef CONFIG
    // Create a window in which the captured images will be presented
    cvNamedWindow( "Camera", CV_WINDOW_AUTOSIZE );
    cvNamedWindow( "HSV", CV_WINDOW_AUTOSIZE );
    cvNamedWindow( "Binaire", CV_WINDOW_AUTOSIZE );
#endif
    // get the image data
      height    = frame->height;
      width     = frame->width;
      step      = frame->widthStep;
      
     // capture size - 
    CvSize size = cvSize(width,height);
    
#ifdef SFML
	//Intialisation de la texture
	if (!txFlux.create(width, height)){
		printf("Erreur création texture\n");
		return EXIT_FAILURE;
	}
#endif

    // Initialize different images that are going to be used in the program
    IplImage*  hsv_frame    = cvCreateImage(size, IPL_DEPTH_8U, 3); // image converted to HSV plane
    IplImage*  threshold   = cvCreateImage(size, IPL_DEPTH_8U, 1);
    
    //Controle couleur

	//Setup Kirby
	 int iLowH = 139;
	 int iHighH = 179;

	 int iLowS = 48; 
	 int iHighS = 255;

	 int iLowV = 101;
	 int iHighV = 255;
	CvScalar valinf={iLowH,iLowS,iLowV};
	CvScalar valsup={iHighH,iHighS,iHighV};

#ifdef CONFIG
	cvNamedWindow("Control", CV_WINDOW_AUTOSIZE); //create a window called "Control"

	 //Create trackbars in "Control" window
	 cvCreateTrackbar("LowH", "Control", &iLowH, 179,NULL); //Hue (0 - 179)
	 cvCreateTrackbar("HighH", "Control", &iHighH, 179,NULL);

	 cvCreateTrackbar("LowS", "Control", &iLowS, 255,NULL); //Saturation (0 - 255)
	 cvCreateTrackbar("HighS", "Control", &iHighS, 255,NULL);

	 cvCreateTrackbar("LowV", "Control", &iLowV, 255,NULL); //Value (0 - 255)
	 cvCreateTrackbar("HighV", "Control", &iHighV, 255,NULL);

	boucle = 1;
#endif
	 
    while(boucle)//while(window.isOpen())
    {  

#ifdef SFML
	boucle = window.isOpen();

	// on inspecte tous les évènements de la fenêtre qui ont été émis depuis la précédente itération
        while (window.pollEvent(event))
        {
            // évènement "fermeture demandée" : on ferme la fenêtre
            if (event.type == sf::Event::Closed)
                window.close();
        }
#endif

        // Get one frame
        frame = cvQueryFrame( capture );
        
        if( !frame ){
                printf("ERROR: frame is null...\n" );
                break;
        }
 
	
        // Covert color space to HSV as it is much easier to filter colors in the HSV color-space.
        cvCvtColor(frame, hsv_frame, CV_BGR2HSV);
	
	//Blur
	cvSmooth( hsv_frame, hsv_frame, CV_GAUSSIAN, 15, 0,0,0); //suppression des parasites par flou gaussien

	//Binarisation
        cvInRangeS(hsv_frame, valinf,valsup, threshold);
      
        //cvSmooth( threshold, threshold, CV_GAUSSIAN, 9, 9 ); //Legère suppression des parasites
        
        // Calculate the moments to estimate the position of the ball 
        
        cvMoments(threshold, moments, 1); 
        // The actual moment values 
        double moment10 = cvGetSpatialMoment(moments, 1, 0); 
        double moment01 = cvGetSpatialMoment(moments, 0, 1); 
        double area = cvGetCentralMoment(moments, 0, 0);
        
        posX= moment10/area;
        posY= moment01/area;
        
	//Affichage zone suivie objet
        cvCircle(frame, cvPoint(width/2,height/2), height/6, CV_RGB(0, 255, 0), 4, 8, 0 );

	if(posX<5&&posY<5){ //Si aucun objet spotted, pointeur rouge au centre
		posX=width/2;
		posY=height/2;
		cvLine(frame, cvPoint(posX-20,posY), cvPoint(posX+20,posY), CV_RGB(255, 0, 0), 4, 8, 0 );
	cvLine(frame, cvPoint(posX,posY-20), cvPoint(posX,posY+20), CV_RGB(255, 0, 0), 4, 8, 0 );
	}
	else{ //Objet spotted
	//Affichage position de l'objet
	cvLine(frame, cvPoint(posX-20,posY), cvPoint(posX+20,posY), CV_RGB(0, 0, 255), 4, 8, 0 );
	cvLine(frame, cvPoint(posX,posY-20), cvPoint(posX,posY+20), CV_RGB(0, 0, 255), 4, 8, 0 );
        }

#ifdef CONFIG
         cvShowImage( "Camera", frame ); // Original stream with detected ball overlay
         cvShowImage( "HSV", hsv_frame); // Original stream in the HSV color space
         cvShowImage( "Binaire", threshold); // The stream after color filtering
#endif

#ifdef SFML
	//Affichage SFML
	/* Clear the screen */
        window.clear(sf::Color::Black);	

	//Conversion de la frame en image smfl  
	if(image_CV2SFML(frame, imFlux)){
		printf("Erreur conversion OpenCV-SFML\n");
		break;
	}
     
	spFlux.setTexture(txFlux);

   	window.draw(spFlux);
	
	/* Update the window */
        //window.display();

	//sfSprite_destroy(sprite);
    	//sfTexture_destroy(texture);
#endif

	//controle_moteur(posX-width/2, posY-height/2, height/6); //Envoie commande moteur
#ifdef CONFIG
        if( (cvWaitKey(10) ) >= 0 ) break; //Arret capture
#endif
    }
    
	//cvWaitKey(0); //Fin programme
	
     // Release the capture device housekeeping
     cvReleaseCapture( &capture );
     
     cvReleaseImage(&threshold);
     
     return EXIT_SUCCESS;
   }

/*On se rapproche de (vecX, vecY) si la position se situe en dehors d'un cercle centre sur la camera*/
void controle_moteur(int vecX, int vecY, int rayon){

	int val_pwm[2];

	/*Lecture valeur*/
	FILE* fichier = NULL;
	fichier = fopen("/dev/ttyACM0","r");
	if(fichier==NULL){
		printf("Erreur ouverture fichier\n");
		return ;
	}
	
	fscanf(fichier,"%d,%d",&val_pwm[0],&val_pwm[1]);
	
	fclose(fichier);

	/*Ecriture nouvelle valeur*/
	fichier = fopen("/dev/ttyACM0","w");
	if(fichier==NULL){
		printf("Erreur ouverture fichier\n");
		return ;
	}
	double norme = 1.0*vecX*vecX + 1.0*vecY*vecY;
	
	if (norme > rayon*rayon){
		if(vecX >= vecY && limite_moteur(val_pwm[0])){ /*Ecart sur x plus important*/
			fprintf(fichier,"%d,%d",val_pwm[0]++,val_pwm[1]);
		}
		else if(vecX <= vecY && limite_moteur(val_pwm[1])){	/*Ecart sur y plus important*/
			fprintf(fichier,"%d,%d",val_pwm[0],val_pwm[1]++);
		}
	}

	fclose(fichier);
	return;
}

/*Verifie que les valeurs envoyees aux moteurs sont correctes*/
int limite_moteur(int val_pwm){
	int MAX_PWM = 255;
	if (val_pwm > MAX_PWM || val_pwm < 0){
		return 0;
	}
	else{
		return 1;
	}
}
/*
int image_CV2SFML(IplImage* imcv, sf::Image imFlux){
	
}
*/