// Written by Nadav Benedek 2017


#include "SparseConnectedComponents.h"
#include <iostream>
#include "opencv2/opencv.hpp"

SparseConnectedComponents::SparseConnectedComponents() {
}

SparseConnectedComponents::~SparseConnectedComponents() {
}

void SparseConnectedComponents::Reset() {
	hashGraphUnionFind.clear();
}

node* SparseConnectedComponents::MakeSet(int x, int y) {
	node n;

	n.rank      = 0;
	n.totalX    = x;
	n.totalY    = y;
	n.numPixels = 1;

	hashGraphUnionFind.emplace(x * MAX_COL + y, n);

	node * nodePointer = &hashGraphUnionFind[x*MAX_COL + y];

	nodePointer->parentNode = nodePointer;

	return nodePointer; // Must return a pointer to the memory inside the hash, not to our local stack
}

node* SparseConnectedComponents::Find(node* a) {         // with path compression
	if (a->parentNode == a) return a;
	else {
		a->parentNode = Find(a->parentNode);
		return a->parentNode;
	}
}


node* SparseConnectedComponents::getNode(int x, int y) {         // with path compression


	if (hashGraphUnionFind.find(x*MAX_COL+y) != hashGraphUnionFind.end()) { // if key is found
		return &hashGraphUnionFind[x*MAX_COL + y];
	} else {
		return 0;
	}
}



void SparseConnectedComponents::Union(node* a0, node* a1) {      // union by rank
	if (a0 == a1) return;

	node *a2 = Find(a0);
	node *a3 = Find(a1);

	if (a2 == a3) return;

	if (a2->rank < a3->rank) {
		a2->parentNode = a3;
		a3->numPixels += a2->numPixels;
		a3->totalX += a2->totalX;
		a3->totalY += a2->totalY;
	} else if (a3->rank < a2->rank) {
		a3->parentNode = a2;
		a2->numPixels += a3->numPixels;
		a2->totalX += a3->totalX;
		a2->totalY += a3->totalY;
	} else {
		a2->parentNode = a3;
		a3->rank++;
		a3->numPixels += a2->numPixels;
		a3->totalX += a2->totalX;
		a3->totalY += a2->totalY;
	}
}


void SparseConnectedComponents::processListOf2DPoints(std::vector<cv::Point>  & listOfPointsToProcess) {
	for (cv::Point & point : listOfPointsToProcess) {
		node * n = MakeSet(point.x, point.y);

		if (hashGraphUnionFind.find((point.x - 1) *MAX_COL + point.y) != hashGraphUnionFind.end()) { // if key is found on left
			Union(&hashGraphUnionFind[(point.x - 1) *MAX_COL + point.y], n);
		}

		if (hashGraphUnionFind.find((point.x - 1) *MAX_COL + (point.y-1)) != hashGraphUnionFind.end()) { // if key is found on up-left
			Union(&hashGraphUnionFind[(point.x - 1) * MAX_COL + (point.y-1)], n);
		}

		if (hashGraphUnionFind.find((point.x) * MAX_COL + (point.y - 1)) != hashGraphUnionFind.end()) { // if key is found on top
			Union(&hashGraphUnionFind[(point.x) * MAX_COL + (point.y - 1)], n);
		}

		if (hashGraphUnionFind.find((point.x + 1) * MAX_COL + (point.y - 1)) != hashGraphUnionFind.end()) { // if key is found on top-right
			Union(&hashGraphUnionFind[(point.x + 1) * MAX_COL + (point.y - 1)], n);
		}
	}
}

void testSparseConnectedComponents() {
	SparseConnectedComponents  scc;
	node * n1 = scc.MakeSet(100, 100);

	std::cout << n1->totalX << "  " << n1->isRoot() << std::endl;

	node * n2 = scc.MakeSet(200, 200);

	std::cout << "Test: " << scc.getNode(200,200)->totalX << std::endl;

	std::cout << n2->totalX << "  " << n2->isRoot() << std::endl;
	scc.Union(n1, n2);
	std::cout << "n1: " << n1->isRoot() << " totalx: " << n1->totalX << " centerX: " << n1->getCenterX() <<  std::endl;
	std::cout << "n2: " << n2->isRoot() << " totalx: " << n2->totalX << " centerX: " << n2->getCenterX() << std::endl;

	node * n3 = scc.MakeSet(300, 300);

	std::cout << n3->totalX << "  " << n3->isRoot() << std::endl;
	scc.Union(n1, n3);

	std::cout << "n1: " << n1->isRoot() << " totalx: " << n1->totalX << " centerX: " << n1->getCenterX() << std::endl;
	std::cout << "n2: " << n2->isRoot() << " totalx: " << n2->totalX << " centerX: " << n2->getCenterX() << std::endl;
	std::cout << "n3: " << n3->isRoot()  << " totalx: " << n3->totalX << " centerX: " << n3->getCenterX() << std::endl;

	std::cout << "iterate" << std::endl;

	for (std::pair<const int, node> & pair : scc.hashGraphUnionFind) {

		int x = pair.first / scc.MAX_COL;
		int y = pair.first % scc.MAX_COL;

		std::cout << "key: " << x << "," << y << " value: " << pair.second.getCenterX() << "," << pair.second.getCenterY() << std::endl;
	}
}