#include "MooreBoundaryTrackingAlgorithm.h"
#include "avansvisionlib20.h"

//direction values
//starting at position 0. Definition of relative positions: 
//          7  0  1
//          6  X  2
//          5  4  3
volatile int rotateX[] = {  0,  1, 1, 1, 0, -1, -1, -1 };
volatile int rotateY[] = { -1, -1, 0, 1, 1,  1,  0, -1 };

int allContours(Mat binaryImage, vector<vector<Point>>& contourVecVec)
{
	Mat labeledImage;
	vector<Point2d *> firstPixelVec2;
	vector<Point2d *> posVec2;
	vector<int> areaVec2;
	int numberOfBlobs = labelBLOBsInfo(binaryImage, labeledImage, firstPixelVec2, posVec2, areaVec2);

	Point b0, b1, c0, c1, firstb0, firstb1;

	for (Point2d *ptr : firstPixelVec2) {
		vector<Point> contourVec;

		firstb0 = Point((int)ptr->y, (int)ptr->x);
		b0 = Point((int)ptr->y, (int)ptr->x);
		c0 = Point((int)(ptr->y - 1), (int)ptr->x);

			int firstDirection = discoverNext(binaryImage, b0, 6);
			b1 = getDirPos(b0, firstDirection);
			firstb1 = getDirPos(b0, firstDirection);
			c1 = getDirPos(b0, (firstDirection + 7) % 8);

			contourVec.push_back(b0);
			contourVec.push_back(b1);

			do {

				b0 = Point(b1.x, b1.y);
				c0 = Point(c1.x, c1.y);

				int dir = discoverNext(binaryImage, b0, discoverNextRelativeDirection(b0, c0));

				b1 = getDirPos(b0, dir);
				c1 = getDirPos(b0, (dir + 7) % 8);

				if (!contains(contourVec, b1))
					contourVec.push_back(b1);

			} while (b0 != firstb0 && b1 != firstb1);

			if (contourVec.size() > 4)
			{
				cout << "Klaar met het vinden van contouren" << endl;
				contourVecVec.push_back(contourVec);
			}
	}

	return numberOfBlobs; //number of objects
}

int discoverNext(Mat &binaryImage, const cv::Point &pos, int beginDirection)
{
	if (beginDirection < 0 || beginDirection > 8) {
		cout << "Begin direction is invalid : " << beginDirection << endl;
		return -1;
	}
	for (int i = 0; i < 8; i++) {
		int dir = (i + beginDirection) % 8;
		
		int newX = pos.x + rotateX[dir];
		int newY = pos.y + rotateY[dir];

		if (getEntryImage(binaryImage, newY, newX) == 1) {
			return dir;
		}
	}
	
	return -1;
}

int discoverNextRelativeDirection(const cv::Point &pos, const cv::Point &target)
{
	for (int i = 0; i < 8; i++) {
		int dir = i;

		int newX = pos.x + rotateX[dir];
		int newY = pos.y + rotateY[dir];

		if (target.x == newX && target.y == newY)
			return dir;
	}

	return -1;
}

cv::Point getDirPos(const cv::Point &pos, int dir)
{
	int x, y;
	x = pos.x + rotateX[dir];
	y = pos.y + rotateY[dir];

	return cv::Point(x, y);
}

bool contains(vector<Point> vec, Point p)
{
	for (Point pnt : vec)
	{
		if (pnt.x == p.x && pnt.y == p.y)
			return true;
	}

	return false;
}

void flip(vector<vector<Point>> &vec)
{
	for (vector<Point> &pvec : vec)
	{
		for (Point &p : pvec)
		{
			int x = p.x;
			int y = p.y;

			p.x = y;
			p.y = x;
		}
	}
}

double getBendingEnergy(const vector<Point>& contour)
{
	double energy = 0;
	int dir = 0;
	int prevdir = 0;

	Point previousPoint = contour[contour.size() - 1];

	for (Point p : contour)
	{
		dir = discoverNextRelativeDirection(previousPoint, p);
		energy += (dir - prevdir + 8) % 8;
		prevdir = dir;
		previousPoint = p;
	}

	return energy;
}
