// Initialize global alignment of all images.
int initGlobalAlign(const vector<FeatureSet> &fs, int minMatches, MotionModel m, float f, int width, int height, int nRANSAC, double RANSACthresh, AlignMatrix &am, vector<CTransform3x3> &ms) {
	int n = fs.size();
	
	CTransform3x3 transMat;
	vector<FeatureMatch> matches;

	// create the n-by-n alignment matrix
	am.resize(n);
	for (int i=0; i<n; i++)
		am[i].resize(n);
	
	for (int i=0; i<n; i++) {
		for (int j=0; j<n; j++) {
			if (i != j) {
				printf("matching image %d with image %d, ", i, j);

				// BEGIN TODO
				// write code to fill in the information for am[i][j]
				//
				// you'll need to call your feature matching routine,
				// then your pair alignment routine
				matches.clear();
				int count = 0;
				FeatureSet f1 = fs[i];
				int m = f1.size();
				FeatureSet f2 = fs[j];
				int n = f2.size();
				double d;
				double dBest[2];
				int idBest;
				FeatureMatch feamatch;
				for (int i = 0; i < m; i++) 
				{
					dBest[0] = 1e64;
					dBest[1] = 1e64 + 1;
					idBest = 0;

					for (int j = 0; j < n; j++)
					{
						d = Euclidean(f1[i], f2[j]);
						if (d < dBest[0]) {
							dBest[1] = dBest[0];
							dBest[0] = d;
							idBest = f2[j].id;
						}
						else if (d < dBest[1])
						{
							dBest[1] = d;
						}
					}
					if (sqrt(dBest[0] / dBest[1]) < 1.0)
					{
						feamatch.id = idBest;
						matches.push_back(feamatch);
						count++;
					}
					else
					{
						feamatch.id = -1;
						matches.push_back(feamatch);
					}

				}
				if (count < minMatches)
				{
					am[i][j].matches = matches;
					am[i][j].inliers.clear();
					am[i][j].r = transMat;
				}
				else
				{
					am[i][j].matches = matches;
					alignImagePair(fs[i], fs[j], matches, eRotate3D, f, width, height, nRANSAC, RANSACthresh, transMat, am[i][j].inliers);
					am[i][j].r = transMat;
				}
				// END TODO

				printf("%d inliers\n", am[i][j].inliers.size());

				if ((int) am[i][j].inliers.size() < minMatches)
					am[i][j].inliers.clear();
			}
		}
	}

	vector<AlignmentImage> nodes(n);
	for (int i=1; i<n; i++) {
		nodes[i].added = false;
		nodes[i].nBest = 0;
		nodes[i].imageID = i;
		nodes[i].parentID = -1;
	}

	// create the image heap
	ImageHeap heap(nodes);

	// add the first image and update the match quality of
	// its neighbors
	nodes[0].added = true;

	for (int j=1; j<n; j++) {
		int nMatches = am[0][j].inliers.size();
		if (nodes[j].nBest < nMatches) {
			heap.increaseKey(nodes[j].heapIndex, nMatches);
			nodes[j].parentID = 0;
		}
	}

	AlignmentImage *nextImage;

	// add the rest of the images
	for (int i=0; i<n-1; i++) {
		nextImage = heap.extractMax();
		
		if (nextImage->nBest == 0) {
			// image set seems to be disconnected
			return -1;
		}

		nextImage->added = true;
		int id = nextImage->imageID;
		int pid = nextImage->parentID;

		// compute the global alignment of the extracted image
		nextImage->r = am[pid][id].r * nodes[pid].r;

		// update the match quality for its neighbor images
		for (int j=0; j<n; j++) {
			if ((id != j) && (!nodes[j].added)) {
				int nMatches = am[id][j].inliers.size();
				if (nodes[j].nBest < nMatches) {
					heap.increaseKey(nodes[j].heapIndex, nMatches);
					nodes[j].parentID = id;
				}
			}
		}
	}

	ms.clear();

	// put the global transformations into the output array
	for (int i=0; i<n; i++) {
		ms.push_back(nodes[i].r);
	}

	return 0;
}
// Initialize global alignment of all images.
int initGlobalAlign(const vector<FeatureSet> &fs, int minMatches, MotionModel m, float f, int width, int height, int nRANSAC, double RANSACthresh, AlignMatrix &am, vector<CTransform3x3> &ms) {
    int n = fs.size();

    // create the n-by-n alignment matrix
    am.resize(n);
    for (int i=0; i<n; i++)
        am[i].resize(n);

    for (int i=0; i<n; i++) {
        for (int j=0; j<n; j++) {
            if (i != j) {
                printf("matching image %d with image %d \n", i, j);

                // BEGIN TODO
                // write code to fill in the information for am[i][j]
                //
                // you'll need to call your feature matching routine,
                // then your pair alignment routine
                am[i][j].matches = matchFeatures(fs[i], fs[j]);
                alignImagePair(fs[i], fs[j], am[i][j].matches, m, f, width, height, nRANSAC, RANSACthresh, am[i][j].r, am[i][j].inliers);
                // END TODO

                printf("%d inliers\n", am[i][j].inliers.size());

                if ((int) am[i][j].inliers.size() < minMatches)
                    am[i][j].inliers.clear();
            }
        }
    }
    printf("done..\n");
    vector<AlignmentImage> nodes(n);
    for (int i=1; i<n; i++) {
        nodes[i].added = false;
        nodes[i].nBest = 0;
        nodes[i].imageID = i;
        nodes[i].parentID = -1;
    }

    // create the image heap
    ImageHeap heap(nodes);

    // add the first image and update the match quality of
    // its neighbors
    nodes[0].added = true;

    for (int j=1; j<n; j++) {
        int nMatches = am[0][j].inliers.size();
        if (nodes[j].nBest < nMatches) {
            heap.increaseKey(nodes[j].heapIndex, nMatches);
            nodes[j].parentID = 0;
        }
    }

    AlignmentImage *nextImage;

    // add the rest of the images
    for (int i=0; i<n-1; i++) {
        nextImage = heap.extractMax();

        if (nextImage->nBest == 0) {
            // image set seems to be disconnected
            return -1;
        }

        nextImage->added = true;
        int id = nextImage->imageID;
        int pid = nextImage->parentID;

        // compute the global alignment of the extracted image
        nextImage->r = am[pid][id].r * nodes[pid].r;

        // update the match quality for its neighbor images
        for (int j=0; j<n; j++) {
            if ((id != j) && (!nodes[j].added)) {
                int nMatches = am[id][j].inliers.size();
                if (nodes[j].nBest < nMatches) {
                    heap.increaseKey(nodes[j].heapIndex, nMatches);
                    nodes[j].parentID = id;
                }
            }
        }
    }

    ms.clear();

    // put the global transformations into the output array
    for (int i=0; i<n; i++) {
        ms.push_back(nodes[i].r);
    }

    return 0;
}