#include <iomanip.h>
#include <fstream.h>
#include <iostream.h>
#include <cv.h>
#include <highgui.h>
#include <stdio.h>
#include <stdlib.h>
#define IndexRangeChecking
#include <compat.h>
#include <matpack.h>
#include <libxml/xmlreader.h>
#include <libxml/parser.h>
#include <libxml/tree.h>
#include <string.h>
using namespace MATPACK;
#define MaxDigits 24
using namespace std;
#define cvCvtPixToPlane cvSplit

struct PointDatabase
	{
		int ptnr;
		int x;
		int y;
		
		PointDatabase()
		{
			ptnr = x = y = 0;
		}
	};

struct Image
   {
       string imname;
       string file;
       string folder;
       int imnr;
       int vispoints;
       PointDatabase *points;

       Image(int pixelcount)
       {
           imname = "";
           file = ""; 
           folder = "";
    	   imnr = 0;
    	   vispoints = 0;
    	   points = new PointDatabase[pixelcount];
    	  
       }
       ~Image() 
       { 
    	   if (points) 
           { 
    		   delete []points; 
               points = NULL; 
           } 
       }
   };

   struct ImageDatabase 
   { 
       Image *pImage; 
    
       ~ImageDatabase() 
       { 
           if (pImage) 
           { 
               delete pImage; 
               pImage = NULL; 
           } 
       } 
   };
   

   
struct SearchWindow
   {
	   int ptnr;
	   int x;
	   int xmin;
	   int xmax;
	   int y;
	   int ymin;
	   int ymax;
       int R;
       int G;
       int B;

       SearchWindow()
       {
           R = G = B = x = xmin = xmax = y = ymin = ymax = ptnr = 0;
       }
   };

struct TemplateValues
{
   	int ptnr;
   	int x;
   	int y;
   	int R;
   	int G;
   	int B;

   	TemplateValues()
   	{
   		R = G = B = x = y = ptnr = 0;
   	}
};   
   
struct SearchValues
{
	int ptnr;
	int x;
	int y;
	int R;
	int G;
	int B;

	SearchValues()
	{
		R = G = B = x = y = ptnr = 0;
	}
};
   
int main()
{
	int nrofimages = 0;
	int actualimage = 0;
	int pixelcount = 1;
	int pixelcount2 = 0;
	int counter = 0;
	int pointiterator = 0;
	int matchiterator = 0;
	int NodeCheck = 0;
	int checkerX = 0;
	int checkerY = 0;
	int ret;
	int SearchSize = 25;
	int XFlag = 0, YFlag = 0, NrFlag = 0;
	int xi = 0, yi = 0, nr = 0;
	int fxy = 0, gxy = 0;
	double x = 0.0, y = 0.0;
	double a = 0.0, b = 0.0, c = 0.0, d = 0.0, e = 0.0, f = 0.0;
	double gradient = 0.0;
	CvScalar t;
	CvScalar s;
	
	Matrix Affine1(1,3,1,3);
	Vector Affine2(1,3);
	Vector Affine3(1,3);
	Vector dUda(1,2);
	Vector dUdb(1,2);
	Vector dUdc(1,2);
	Vector dUdd(1,2);
	Vector dUde(1,2);
	Vector dUdf(1,2);
	Vector dgUda(1,2);
	Vector dgUdb(1,2);
	Vector dgUdc(1,2);
	Vector dgUdd(1,2);
	Vector dgUde(1,2);
	Vector dgUdf(1,2);	
	Vector U(1,2);
	Matrix A(0,(SearchSize * SearchSize) * 2,0,5);
	Matrix P(0,(SearchSize * SearchSize) * 2,0,(SearchSize * SearchSize) * 2);
	Matrix N(0,5,0,5);
	Vector deltal(0,(SearchSize * SearchSize) * 2);
	Vector Delta(1,6);
	
	//
	P = 1;
	//
	
	IplImage* templa = NULL;
	//IplImage* templaGray = NULL;
	//IplImage* templaGrad = NULL;
	IplImage* search = NULL;
	IplImage* searchGray = NULL;
	IplImage* searchGrad = NULL;
	
	fstream photocount;
	photocount.open("parameters.xml", ios::in); // Define pointcloud input file
	photocount << setprecision(16);
	string temporary;
	size_t found;
	// Read out the XML-Parameter file and count the number of photographs by finding the imagenumber-tag
		while(!photocount.eof()){
            
			getline(photocount,temporary);
            found=temporary.find("Imagenumber");
            if (found!=string::npos){
            nrofimages++;
            }
        }
      
	photocount.close();
	cout << "--== Number of photos counted ==--\n";
	
	fstream pointcloud;
    pointcloud.open("pointcloud.xyz", ios::in); // Define pointcloud input file
    pointcloud << setprecision(16);
    
    while(!pointcloud.eof())
    {      
    	getline(pointcloud,temporary);
        pixelcount++;
    } 
    
    
    cout << "--== Maximum amount of pixel counted ==--\n";
	
	
	ImageDatabase *IDB = new ImageDatabase[nrofimages];
	IDB[actualimage].pImage  = new Image(pixelcount);
	SearchWindow *SW = new SearchWindow[2];
	SearchValues *SV = new SearchValues[SearchSize * SearchSize];
	TemplateValues *TV = new TemplateValues[SearchSize * SearchSize];
	
	LIBXML_TEST_VERSION;
	xmlTextReaderPtr reader;
    
    reader = xmlReaderForFile("projfine3.xml", NULL, 0);
    if (reader != NULL) {
           ret = xmlTextReaderRead(reader);
           while (ret == 1) {
        	   const xmlChar *name, *value;
    	       name = xmlTextReaderConstName(reader);
    	       if (name == NULL)
    	       name = BAD_CAST "--";

    	       value = xmlTextReaderConstValue(reader);
    	       
    	       if (xmlTextReaderDepth(reader) == 2 && xmlTextReaderNodeType(reader) == 1 && xmlStrcmp(name, (xmlChar*)"Imagenumber") == 0)
    	       NodeCheck = 1;      	       
    	       if (xmlTextReaderNodeType(reader) == 3 && NodeCheck == 1)
    	       {
    	       	IDB[actualimage].pImage -> imnr = atoi((const char*)value);
    	       	NodeCheck = 0;        	       	 
    	       }
    	              	     
    	       if (xmlTextReaderDepth(reader) == 2 && xmlTextReaderNodeType(reader) == 1 && xmlStrcmp(name, (xmlChar*)"Path") == 0)
       	       NodeCheck = 2;      	       
       	       if (xmlTextReaderNodeType(reader) == 3 && NodeCheck == 2)
       	       {
       	    	if (actualimage != 0)
       	    	{
       	    	IDB[actualimage].pImage  = new Image(pixelcount);
       	    	IDB[actualimage-1].pImage -> vispoints = pixelcount2;
       	    	}
       	       	IDB[actualimage].pImage -> imname = (const char*)value;
 				found=IDB[actualimage].pImage -> imname.find_last_of("/\\");
 				IDB[actualimage].pImage -> folder = IDB[actualimage].pImage -> imname.substr(0, found);
 				IDB[actualimage].pImage -> file = IDB[actualimage].pImage -> imname.substr(found+1);
 				found = (int)IDB[actualimage].pImage -> file.length();
 				IDB[actualimage].pImage -> file = IDB[actualimage].pImage -> file.substr(0,found-1);
 				cout << actualimage <<  " Loading: " << IDB[actualimage].pImage -> file << endl;
       	       	NodeCheck = 0;
       	       	nr = 0;
       	       	pixelcount2 = 0;
       	       	actualimage++;
       	       }
       	    
       	    if (xmlTextReaderDepth(reader) == 2
					&& xmlTextReaderNodeType(reader) == 1 && xmlStrcmp(name,
					(xmlChar*)"Nr") == 0) {
				NodeCheck = 3;
			}
			if (xmlTextReaderNodeType(reader) == 3 && NodeCheck == 3) {
				nr = atoi((const char*)value);
				NodeCheck = 0;
				NrFlag = 1;
			}
       	       
       	    if (xmlTextReaderDepth(reader) == 2
					&& xmlTextReaderNodeType(reader) == 1 && xmlStrcmp(name,
					(xmlChar*)"X") == 0) {
				NodeCheck = 4;
			}
			if (xmlTextReaderNodeType(reader) == 3 && NodeCheck == 4) {
				x = atof((const char*)value);
				xi = (int)x; 
				NodeCheck = 0;
				XFlag = 1;
			}

			if (xmlTextReaderDepth(reader) == 2
					&& xmlTextReaderNodeType(reader) == 1 && xmlStrcmp(name,
					(xmlChar*)"Y") == 0) {
				NodeCheck = 5;
			}
			if (xmlTextReaderNodeType(reader) == 3 && NodeCheck == 5) {
				y = atof((const char*)value);
				yi = (int)y;
				NodeCheck = 0;
				YFlag = 1;
			}
			if (XFlag + YFlag == 2) {
				if ((x > SearchSize) && (x < 4288 - SearchSize) && (y > SearchSize) && (y < 2848 - SearchSize))
				{
					IDB[actualimage - 1].pImage -> points[nr].ptnr = nr;
					IDB[actualimage - 1].pImage -> points[nr].x = xi;
					IDB[actualimage - 1].pImage -> points[nr].y = yi;
					XFlag = YFlag = 0;
					pixelcount2++;
				}
			}
       	       
       	       
               ret = xmlTextReaderRead(reader);
           }
           IDB[actualimage-1].pImage -> vispoints = pixelcount2;
           xmlFreeTextReader(reader);
           if (ret != 0) {
               fprintf(stderr, "%s : failed to parse\n", "projfine3.xml");
           }
       } else {
           fprintf(stderr, "Unable to open %s\n", "projfine3.xml");
       }
	
    // DEFINE TEMPLATE
    
    //cout << IDB[16].pImage -> points[162688].ptnr << endl;
    templa = cvLoadImage(IDB[16].pImage -> file.data(), -1);
    search = cvLoadImage(IDB[15].pImage -> file.data(), -1);
    cvSetImageCOI(search, 2 );
    CvSize imgSize;
    imgSize.width = 4288;
    imgSize.height = 2848;
    searchGray = cvCreateImage(imgSize, 8, 1);
    searchGrad = cvCreateImage(imgSize, 8, 1);
    //cvCvtColor(search ,searchGray, CV_RGB2GRAY);
    cvCopy(search, searchGray);
    cvSobel(searchGray, searchGrad, 2, 2, 7);
    //cvLaplace(templaGray, templaGrad, 1);
    //cvNamedWindow("image0", 1);
    //cvShowImage("image0", searchGrad);
    //cvWaitKey(0);
    //cout << IDB[16].pImage -> points[9861].ptnr;
    while (pointiterator != 15000)
    {
    	if (IDB[16].pImage -> points[pointiterator].ptnr !=0 && IDB[15].pImage -> points[pointiterator].ptnr)
    	{
    		SW[0].xmin = (int)IDB[16].pImage -> points[pointiterator].x - (SearchSize / 2) - 0.5;
			SW[0].xmax = (int)IDB[16].pImage -> points[pointiterator].x + (SearchSize / 2) + 0.5;
    		SW[0].ymin = (int)IDB[16].pImage -> points[pointiterator].y - (SearchSize / 2) - 0.5;
    		SW[0].ymax = (int)IDB[16].pImage -> points[pointiterator].y + (SearchSize / 2) + 0.5;
    		SW[1].xmin = (int)IDB[15].pImage -> points[pointiterator].x - (SearchSize / 2) - 0.5;
    		SW[1].xmax = (int)IDB[15].pImage -> points[pointiterator].x + (SearchSize / 2) + 0.5;
    		SW[1].ymin = (int)IDB[15].pImage -> points[pointiterator].y - (SearchSize / 2) - 0.5;
    		SW[1].ymax = (int)IDB[15].pImage -> points[pointiterator].y + (SearchSize / 2) + 0.5;
    		
    		checkerX = SW[0].xmin;
    		checkerY = SW[0].ymin;
    		
    		// Read in the gray values into the template
    		while (checkerX != SW[0].xmax)
    		{
    			while (checkerY != SW[0].ymax)
    			{
    				t = cvGet2D(templa, checkerY, checkerX);
    				TV[counter].x = checkerX;
    				TV[counter].y = checkerY;
    				TV[counter].G = (int)t.val[1];
    				checkerY++;
    				counter++;
    			}
    			checkerY = SW[0].ymin;
    			checkerX++;
    		}
			counter = 0;
    		checkerX = SW[1].xmin;
    		checkerY = SW[1].ymin;
    		
    		// Read in the gray values into the search
    		while (checkerX != SW[1].xmax)
    		{
    			while (checkerY != SW[1].ymax)
    			{
    				s = cvGet2D(search, checkerY, checkerX);
    				SV[counter].x = checkerX;
    				SV[counter].y = checkerY;
    				SV[counter].G = (int)s.val[1];
    				checkerY++;
    				counter++;
    			}
    			checkerY = SW[1].ymin;
    			checkerX++;
    		}
    		checkerX = SW[1].xmin;
    		// Perform the matching or something
    		
    		// Matrizenwerte definieren (Matrize Oben definieren)
    		
    		a = b = c = d = 0;
    		e = f = 1;
    		
    		while (matchiterator != (SearchSize * SearchSize) * 2)
    		{
    			// Define L
    			t = cvGet2D(templa, (int)Affine2[2], (int)Affine2[1]);
    			fxy = t.val[0];
    			s = cvGet2D(search, (int)Affine2[2], (int)Affine2[1]);
    			gxy = s.val[0];
    			
    			//deltal[matchiterator] = fxy - gxy;
    			
    			// Define Matrix-Elements
    			Affine1[1][1] = a;
	    		Affine1[1][2] = b;
	    		Affine1[2][1] = c;
	    		Affine1[2][2] = d;
    			
    			Affine2[1] = SV[matchiterator].x;
    			Affine2[2] = SV[matchiterator].y;
    			
    			Affine3[1] = e;
		    	Affine3[2] = f;
    			
    			// Deffine Derivatives
    			s = cvGet2D(searchGrad, Affine2[2], Affine2[1]);
		    	gradient = s.val[0];
		    	
		    	//cout << matchiterator << " " << gradient << endl;
		    	
		    	// Derivates of matrices
		    	dUda[1] = SV[matchiterator].x;
		    	dUda[2] = 0;
		    	
		    	dUdb[1] = SV[matchiterator].y;
		    	dUdb[2] = 0;
		    	
		    	dUdc[1] = 0;
		    	dUdc[2] = SV[matchiterator].x;
		    	
		    	dUdd[1] = 0;
		    	dUdd[2] = SV[matchiterator].y;
		    	
		    	dUde[1] = 1;
		    	dUde[2] = 0;
		    	
		    	dUdf[1] = 0;
		    	dUdf[2] = 1;
		    	
		    	// Combine with gradient
		    	dgUda = gradient * dUda;
		    	dgUdb = gradient * dUdb;
		    	dgUdc = gradient * dUdc;
		    	dgUdd = gradient * dUdd;
		    	dgUde = gradient * dUde;
		    	dgUdf = gradient * dUdf;
		    	
		    	//cout << gradient * dUda << endl;
		    	
		    	A[matchiterator][0] = dgUda[1];
		    	A[matchiterator][1] = dgUdb[1];
		    	A[matchiterator][2] = dgUdc[1];
		    	A[matchiterator][3] = dgUdd[1];
		    	A[matchiterator][4] = dgUde[1];
		    	A[matchiterator][5] = dgUdf[1];
		    	A[matchiterator + 1][0] = dgUda[2];
		    	A[matchiterator + 1][1] = dgUdb[2];
		    	A[matchiterator + 1][2] = dgUdc[2];
		    	A[matchiterator + 1][3] = dgUdd[2];
		    	A[matchiterator + 1][4] = dgUde[2];
		    	A[matchiterator + 1][5] = dgUdf[2];
		    	
		    	//cout << matchiterator << " " << A[matchiterator][0] << " " << A[matchiterator][1] << " " << A[matchiterator][2] << " " << A[matchiterator][3] << " " << A[matchiterator][4] << " " << A[matchiterator][5] << endl;
		    	
    			matchiterator = matchiterator + 2;
    		}
    		
    		//cout << P[625][625] << " " << A[0][6] << endl;
    		//cout << N << endl;// << P << endl;
			// A Matrize plus Ausgleichungsgschmeus
			N = A.Transpose() * P * A;
			//cout << N << endl;
			Delta = N.Inverse() * A.Transpose() * P;
			// Delta  Anbringen. Vorsicht - doppel-Schlaufe! (einmal durch pixel und einmal bis konv).
    		
    	}    	
    	pointiterator++;
    	cout << pointiterator << endl;
    }
    
	cout << "--== Done ==--\n";
	
	
	
	cvReleaseImage( &templa);
	cout << "--== Release Template ==--\n";
	cvReleaseImage( &search);
	cvReleaseImage( &searchGray);
	cvReleaseImage( &searchGrad);
	cout << "--== Release Search ==--\n";
	

	delete []IDB;
	cout << "--== Release IDB ==--\n";
	delete []SW;
	cout << "--== Release SW ==--\n";
	delete []SV;
	cout << "--== Release SV ==--\n";
	delete []TV;
	cout << "--== Release TV ==--\n";
	
	/*
	 *Free the global variables that may
	 *have been allocated by the parser.
	 */
	xmlCleanupParser();
	cout << "--== Cleanup ==--\n"; 
	/*
	 * this is to debug memory for regression tests
	 */
	xmlMemoryDump();
	cout << "--== MemoryDump ==--\n";
	
	
}