#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <iostream>
#include <cxcore.h>
#include <cvaux.h>
#include <cv.h>
#include <highgui.h>

CvPoint coordCalibrage[17];
CvPoint balleTerrain;
CvPoint balleEcran;
CvPoint centreButEcran;
int iSaisie = 0;
bool afficherText = true;
CvMat* homographyMat = cvCreateMat(3, 3, CV_32FC1);

void findHomographyPerso(CvPoint * pts_ecran, CvMat* matHomography);
void useHomography(CvMat* matHomography);
void mouseCallback(int event, int x, int y, int flags, void* param);
double distance(CvPoint p1, CvPoint p2);
void dessinerDistance(IplImage *& img, int rayon, double d);

int main(int argc, char *argv[]) {
	int rayon = 9;
	int height, width, step, channels;
	uchar *data;
	IplImage* img = 0;

	if (argc < 2) {
		printf("Usage: main <image-file-name>\n\7");
		exit(0);
	}

	// load an image  
	img = cvLoadImage(argv[1], 1);
	if (!img) {
		printf("Could not load image file: %s\n", argv[1]);
		exit(0);
	}

	CvMat* matPointEcran = cvCreateMat(3, 1, CV_32FC1);
	CvMat* matPointTerrainReel = cvCreateMat(3, 1, CV_32FC1);

	// get the image data
	height = img->height;
	width = img->width;
	step = img->widthStep;
	channels = img->nChannels;
	data = (uchar *) img->imageData;
	printf("Processing a %dx%d image with %d channels\n", height, width,
			channels);

	// create a window
	cvNamedWindow("Terrain de Foot", CV_WINDOW_AUTOSIZE);
	cvMoveWindow("Terrain de Foot", 100, 100);

	// show the image
	cvShowImage("Terrain de Foot", img);

	// Set up the callback
	cvSetMouseCallback("Terrain de Foot", mouseCallback, (void*) img);

	while (true) {
		if (afficherText == true) {
			switch (iSaisie) {
			case 0:
				std::cout
						<< "* Cliquez sur le coin superieur droit du terrain\n";
				afficherText = false;
				break;
			case 1:
				std::cout
						<< "* Cliquez sur le coin superieur gauche du terrain\n";
				afficherText = false;
				break;
			case 2:
				std::cout
						<< "* Cliquez sur le coin inferieur droit du terrain\n";
				afficherText = false;
				break;
			case 3:
				std::cout
						<< "* Cliquez sur le coin inferieur gauche du terrain\n";
				afficherText = false;
				break;
			case 4:
				std::cout << "* Cliquez sur le milieu droit\n";
				afficherText = false;
				break;
			case 5:
				std::cout << "* Cliquez sur le milieu gauche\n";
				afficherText = false;
				break;
			case 6:
				std::cout << "* Cliquez sur le milieu centre\n";
				afficherText = false;
				break;
			case 7:
				std::cout << "* Cliquez sur le penalty superieur\n";
				afficherText = false;
				break;
			case 8:
				std::cout << "* Cliquez sur le poteau superieur droit\n";
				afficherText = false;
				break;
			case 9:
				std::cout << "* Cliquez sur le poteau superieur gauche\n";
				afficherText = false;
				break;
			case 10:
				std::cout
						<< "* Cliquez sur le coin de la surface superieur droit\n";
				afficherText = false;
				break;
			case 11:
				std::cout
						<< "* Cliquez sur le coin de la surface superieur gauche\n";
				afficherText = false;
				break;
			case 12:
				std::cout << "* Cliquez sur le point de penalty inferieur\n";
				afficherText = false;
				break;
			case 13:
				std::cout << "* Cliquez sur le poteau inferieur droite\n";
				afficherText = false;
				break;
			case 14:
				std::cout << "* Cliquez sur le poteau inferieur gauche\n";
				afficherText = false;
				break;
			case 15:
				std::cout
						<< "* Cliquez sur le coin de la surface inferieur droit\n";
				afficherText = false;
				break;
			case 16:
				std::cout
						<< "* Cliquez sur le coin de la surface inferieur gauche\n";
				afficherText = false;
				break;

			case 17:
				afficherText = false;

				findHomographyPerso(coordCalibrage, homographyMat);

				std::cout << "\n* Cliquez sur la balle\n";
				break;
			case 100:
				img = cvLoadImage(argv[1], 1);
				std::cout << " > Calcul de la distance de la balle au but : \n";

				//Modification coordonné Balle
				cvmSet(matPointEcran, 0, 0, balleEcran.x);
				cvmSet(matPointEcran, 1, 0, balleEcran.y);
				cvmSet(matPointEcran, 2, 0, 1);
				cvMatMul(homographyMat, matPointEcran, matPointTerrainReel);
				balleTerrain.x = cvmGet(matPointTerrainReel, 0, 0) / cvmGet(
						matPointTerrainReel, 2, 0);
				balleTerrain.y = cvmGet(matPointTerrainReel, 1, 0) / cvmGet(
						matPointTerrainReel, 2, 0);

				CvPoint destination = cvPoint(45,0);
				double d = distance(balleTerrain, destination);
				std::cout << "\n distance  = " << d << " metres\n";
				dessinerDistance(img, rayon, d);
				afficherText = false;
				break;
			}
		}

		// wait for a ESC key for exit
		if (cvWaitKey(15) == 27)
			break;
	}

	// release the image
	cvReleaseImage(&img);
	return 0;
}

void mouseCallback(int event, int x, int y, int flags, void* param) {
	switch (event) {
	case CV_EVENT_LBUTTONDOWN:
		if (iSaisie < 17) {
			coordCalibrage[iSaisie] = cvPoint(x, y);
			iSaisie++;
		} else {
			balleEcran = cvPoint(x, y);
			balleTerrain = cvPoint(balleEcran.x,balleEcran.y);
			iSaisie = 100;
		}
		afficherText = true;
		break;
	}
}

double distance(CvPoint p1, CvPoint p2) {
	//	double add = pow(p1.x + p2.x, 2) + pow(p1.y + p2.y, 2);
	//	return sqrt(add);
	double add = pow(abs(p1.x - p2.x), 2) + pow(abs(p1.y - p2.y), 2);
	return sqrt(add);
}

void findHomographyPerso(CvPoint * pts_ecran, CvMat* matHomography) {

	int nbPoints = 17;
	float largeurTerrain = 90;
	float longueurTerrain = 120;
	//Creatin des deux matrices representant des points sur le systeme de coordonnees du terrain et de l'ecran
	CvMat* matPointsTerrainReel = cvCreateMat(nbPoints, 2, CV_32FC1);
	CvMat *matPointsEcran = cvCreateMat(nbPoints, 2, CV_32FC1);

	for (int i = 0; i < nbPoints; i++) {
		cvmSet(matPointsEcran, i, 0, (double) pts_ecran[i].x);
		cvmSet(matPointsEcran, i, 1, (double) pts_ecran[i].y);
	}

	//	for (int i = 0; i < nbPoints; i++) {
	//			float x = cvmGet(matPointsEcran,i, 0);
	//			float y = cvmGet(matPointsEcran,i, 1);
	//			std::cout << x << " & "  << y << std::endl;
	//		}

	//corner sup droit
	cvmSet(matPointsTerrainReel, 0, 0, (double) 0);
	cvmSet(matPointsTerrainReel, 0, 1, (double) 0);
	//corner sup gauche
	cvmSet(matPointsTerrainReel, 1, 0, (double) largeurTerrain);
	cvmSet(matPointsTerrainReel, 1, 1, (double) 0);
	//corner inf droit
	cvmSet(matPointsTerrainReel, 2, 0, (double) 0);
	cvmSet(matPointsTerrainReel, 2, 1, (double) longueurTerrain);
	//corner inf gauche
	cvmSet(matPointsTerrainReel, 3, 0, (double) largeurTerrain);
	cvmSet(matPointsTerrainReel, 3, 1, (double) longueurTerrain);
	//milieu  droit
	cvmSet(matPointsTerrainReel, 4, 0, (double) 0);
	cvmSet(matPointsTerrainReel, 4, 1, (double) longueurTerrain / 2);
	//milieu gauche
	cvmSet(matPointsTerrainReel, 5, 0, (double) largeurTerrain);
	cvmSet(matPointsTerrainReel, 5, 1, (double) longueurTerrain / 2);
	//milieu centre
	cvmSet(matPointsTerrainReel, 6, 0, (double) largeurTerrain / 2);
	cvmSet(matPointsTerrainReel, 6, 1, (double) longueurTerrain / 2);
	//Penalty sup
	cvmSet(matPointsTerrainReel, 7, 0, (double) largeurTerrain / 2);
	cvmSet(matPointsTerrainReel, 7, 1, (double) 11);
	//Poteau sup droit
	cvmSet(matPointsTerrainReel, 8, 0, (double) (largeurTerrain / 2) - 3.65);
	cvmSet(matPointsTerrainReel, 8, 1, (double) 0);
	//Poteau sup gauche
	cvmSet(matPointsTerrainReel, 9, 0, (double) (largeurTerrain / 2) + 3.65);
	cvmSet(matPointsTerrainReel, 9, 1, (double) 0);
	//Coin surface sup droit
	cvmSet(matPointsTerrainReel, 10, 0, (double) (largeurTerrain / 2) - 3.65
			- 16.5);
	cvmSet(matPointsTerrainReel, 10, 1, (double) 16.5);
	//Coin surface sup gauche
	cvmSet(matPointsTerrainReel, 11, 0, (double) (largeurTerrain / 2) + 3.65
			+ 16.5);

	cvmSet(matPointsTerrainReel, 11, 1, (double) 16.5);
	//Penalty inf
	cvmSet(matPointsTerrainReel, 12, 0, (double) largeurTerrain / 2);
	cvmSet(matPointsTerrainReel, 12, 1, (double) longueurTerrain - 11);
	//Poteau inf droit
	cvmSet(matPointsTerrainReel, 13, 0, (double) (largeurTerrain / 2) - 3.65);
	cvmSet(matPointsTerrainReel, 13, 1, (double) longueurTerrain);
	//Poteau inf gauche
	cvmSet(matPointsTerrainReel, 14, 0, (double) (largeurTerrain / 2) + 3.65);
	cvmSet(matPointsTerrainReel, 14, 1, (double) longueurTerrain);
	//Coin surface inf droit
	cvmSet(matPointsTerrainReel, 15, 0, (double) (largeurTerrain / 2) - 3.65
			- 16.5);
	cvmSet(matPointsTerrainReel, 15, 1, (double) longueurTerrain - 16.5);
	//Coin surface inf gauche
	cvmSet(matPointsTerrainReel, 16, 0, (double) (largeurTerrain / 2) + 3.65
			+ 16.5);
	cvmSet(matPointsTerrainReel, 16, 1, (double) longueurTerrain - 16.5);

	cvFindHomography(matPointsEcran, matPointsTerrainReel, matHomography);

	cvReleaseMat(&matPointsEcran);
	cvReleaseMat(&matPointsTerrainReel);
}

void dessinerDistance(IplImage *& img, int rayon, double d) {
	cvCircle(img, balleEcran, rayon, CV_RGB(255,0,0), 2, 8, 0);
	centreButEcran = cvPoint((coordCalibrage[8].x + (coordCalibrage[9].x
			- coordCalibrage[8].x) / 2.0), (coordCalibrage[8].y
			+ (coordCalibrage[9].y - coordCalibrage[8].y) / 2.0));
	cvLine(img, balleEcran, centreButEcran, CV_RGB(255,0,0), 1, CV_AA);
	CvFont font;
	cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5, 0.0, 1);
	char *strDistance = new char[256];
	sprintf(strDistance, "Disance : %lf m", d);
	cvPutText(img, strDistance, cvPoint(balleEcran.x + 10, balleEcran.y + 10),
			&font, cvScalar(127));
	cvShowImage("Terrain de Foot", img);
}