bool StudentLocalization::stepFindChinContours(const IntensityImage &image, FeatureMap &features) const {
	
	const Feature & mouthCenter = features.getFeature(Feature::FEATURE_MOUTH_CENTER);
	const Feature & chin = features.getFeature(Feature::FEATURE_CHIN);
	Feature & chinContour = features.getFeature(Feature::FEATURE_CHIN_CONTOUR);

	// The mouth could interfere with the localization of the chin contours.
	// Therefore use an offset based on which mouthcorner is the furthest away.
	// This however could still prove troublesome, so add a slight margin; 
	// represented by the 'magic' number. 
	const double radius = chin.getY() - mouthCenter.getY();


	const double smallestOffsetRadiusFactor = 0.75;
	const double biggestOffsetRadiusFactor = 2;
	//

	// 180° matches half a circle.
	// 0° points to the east in goniometrics, where increasing angles are counter-clockwise.
	// Since the chin is usually on the bottompart of the image, the points either need to be
	// found clockwise (-180), or start looking at an offset.
	// 
	const int halfCircle = 180;
	const int angleStepSize = 10;
	// roughly 19 points need to be found.
	// 0 through 18 does just that.
	for (int i = 0; i <= halfCircle / angleStepSize; ++i)
	{
		// Determine the angle of the line among which we try to look for a point on the chin's contour.
		int angle = halfCircle + i * angleStepSize;
		
		// The size of a step in both the horizontal and verticical direction along the line with the current angle.
		double xDir = std::cos(angle * M_PI / 180.0);
		double yDir = -std::sin(angle * M_PI / 180.0);
		
		// Normalized size of a step that is made alongside the current angle.
		const double increment = 1;

		// Make multiple steps alongside the current angle within bounds where it is likely a contour for the chin.
		for (double distance = radius * smallestOffsetRadiusFactor; distance < radius * biggestOffsetRadiusFactor; distance += increment)
		{
			// Determine the location to check for a contour of the chin.
			double xCheck = mouthCenter.getX() + (xDir * distance);
			double yCheck = mouthCenter.getY() + (yDir * distance);
			// Image should be thresholded grayscale, where the pixels are eiher 0 or 255; check in the middle of it.
			const unsigned char treshhold = 128;
			if (image.getPixel(xCheck, yCheck) <= treshhold)
			{
				chinContour.addPoint(Point2D<double>(xCheck, yCheck));
				std::cout << "Found contour at: " << xCheck << "; " << yCheck << "\n";
				break;
			}
		}
	}

	return (chinContour.getPoints().size() > 0);
}
IntensityImageStudent::IntensityImageStudent(const IntensityImage &other) : IntensityImage(other.getWidth(), other.getHeight()), pixelMap(nullptr) {
	const int SIZE = other.getSize();
	if(SIZE > 0) {
		pixelMap = new Intensity[SIZE];
		for(int i = 0; i < SIZE; i++) {
			pixelMap[i] = other.getPixel(i);
		}
	}
}
int StudentLocalization::getTopOfHead(const IntensityImage &image){
	for (int y = 0; y < image.getHeight(); y++){
		for (int x = 0; x < image.getWidth(); x++) {
			if (image.getPixel(x, y) == 0){
				return y;
			}
		}
	}
	return -1;
}
void HereBeDragons::HerLoveForWhoseDearLoveIRiseAndFall(const IntensityImage &src, cv::Mat &dst) {
	int w = src.getWidth();
	int h = src.getHeight();

	dst.create(h, w, CV_8UC1);

	for (int x = 0; x < dst.cols; x++) {
		for (int y = 0; y < dst.rows; y++) {
			dst.at<uchar>(y, x) = src.getPixel(x, y);
		}
	}
}
void IntensityImageStudent::set(const IntensityImage &other) {
	const int	SIZE = other.getSize();
	IntensityImage::set(other.getWidth(), other.getHeight());

	if (SIZE > 0) {
		delete[] pixelMap;
		pixelMap = new Intensity[SIZE];
		for (int i = 0; i < SIZE; i++) {
			pixelMap[i] = other.getPixel(i);
		}
	}
}
SonnetXVIII HereBeDragons::YetWhoKnowsNotConscienceIsBornOfLove(const IntensityImage &Yet, const int Who, const int Knows, const int Not, const int Conscience){
	SonnetXVIII Is(Conscience);
	int Born = 0;
	for (int y = Knows; y < (Knows + Conscience); y++){
		for (int x = Who; x < (Who + Not); x++){
			if (Yet.getPixel(x, y) == 0){
				Is[Born]++;
			}
		}
		Born++;
	}
	return Is;
}
SonnetXVIII HereBeDragons::LoveIsTooYoungToKnowWhatConscienceIs(const IntensityImage &Love, const int Is, const int Too, const int Young, const int To){
	SonnetXVIII Know(Young);
	int What = 0;
	for (int x = Is; x < (Is + Young); x++){
		for (int y = Too; y < (Too + To); y++){
			if (Love.getPixel(x, y) == 0){
				Know[What]++;
			}
		}
		What++;
	}
	return Know;
}
IntensityImageStudent GaussianFilter::applyFilter(const IntensityImage &image)
{
	IntensityImageStudent filteredImage = IntensityImageStudent(image.getWidth() - 2 * radius, image.getHeight() - 2 * radius);
	
	for (int y = 0; y < filteredImage.getHeight(); y++){
		for (int x = 0; x < filteredImage.getWidth(); x++){
			double filteredIntensity = 0.0;
			for (unsigned int i = 0; i < gaussKernel.size(); i++){
				filteredIntensity += gaussKernel[i] * image.getPixel(x + (i % (2 * radius + 1)), y + i / (2 * radius + 1));
			}
			filteredImage.setPixel(x, y, static_cast<Intensity>(filteredIntensity));
		}
	}
	return filteredImage;
}
bool StudentLocalization::stepFindChinContours(const IntensityImage &image, FeatureMap &features) const {

	//Get the position of the mouth
	Feature Mouth = features.getFeature(Feature::FEATURE_MOUTH_TOP);
	Point2D<double> MouthPosition;
	
	//Initialise the chin feature
	std::vector<Point2D<double>> ChinPositions;
	Feature Chin = Feature(Feature::FEATURE_CHIN);
	Feature ChinContour = Feature(Feature::FEATURE_CHIN_CONTOUR);
	int y = 0;
	int m = 0;
	
	//Draw a half circle starting from the center of the mouth
	for (int j = HALF_CIRCLE; j > 0; j -= DEGREE_STEP){
		
		//reset the position of the mouth
		MouthPosition.set(Mouth.getX(), Mouth.getY());
		MouthPosition.set(drawLine(j, START_POSITION, MouthPosition));
	
		m = m + 1;

		
		for (int i = 0; i < MEASURE_RANGE; i++){

			MouthPosition.set(drawLine(j, MEASURE_STEP, MouthPosition));
			Intensity pixel = image.getPixel(MouthPosition.getX(), MouthPosition.getY());

			// If the intensity of the current pixel is lower than 2 (which means it is black)
			if (pixel < 2){ 
				
				// If the current angle is within the bounds of the half circle
				if (j < RIGHT_HALF && j > LEFT_HALF){
					ChinContour.addPoint(drawLine(j, 2, MouthPosition));
				}
				else{
					//Draw a point on the mouth position, to indicate the detection failed.
					ChinContour.addPoint(MouthPosition);
				}
				break;
			}
		}	
	}	
	features.putFeature(ChinContour);
	return true;
}
bool StudentLocalization::stepFindChinContours(const IntensityImage &image, FeatureMap &features) const {
	// Maak een basetimer object om de tijd bij te houden dat de implementatie nodig heeft.
	BaseTimer basetimer;
	// Start de basetimer.
	basetimer.start();
	// test getal 
	int startStep = 15; 
	bool first = true;
	// Sla het middenpunt van de mond op.
	Point2D<double> MouthCenterPoint = features.getFeature(Feature::FEATURE_MOUTH_CENTER).getPoints()[0];
	// Sla het kin punt op.
	Point2D<double> ChinPoint = features.getFeature(Feature::FEATURE_CHIN).getPoints()[0];
	int range = MouthCenterPoint.getY() - ChinPoint.getY();
	// Object om kincountoer punten in op te slaan.
	Feature output = Feature(Feature::FEATURE_CHIN_CONTOUR);
	int degrees;
	int steps = 15;
	int lastdif;
	double correction = -1;
	int lastSteps = 0;
	int vorigeX = 0;
	// Bereken 20 punten van de kin.
	for (int i = 0; i < 19; i++)
	{
		bool ireg = false;
		if (i>9)
		{ 
			correction = 1; 
		}
		else if (i < 9)
		{ 
			correction =0;
		}
		// Sla middelpunt mond x op.
		int checkX = MouthCenterPoint.getX();
		// Sla middelpunt mond y op.
		int checkY = MouthCenterPoint.getY();
		double gradenInRad = (-90+(i * 10)) *(PI/180);
		steps = startStep;
		Point2D<double> gevondenPunt;
		// Middelste punt van de kin is als het goed is bekend. Dit is punt nummer 9 dus zal worden overgeslagen.
		if (i != 9) 
		{
			while (true)
			{
				if (!first&&steps > startStep + 10)
				{ 
					lastdif / i;
					ireg = true;
					gevondenPunt.set(MouthCenterPoint.getX() + ((lastSteps + correction)* std::sin(gradenInRad)), MouthCenterPoint.getY() + ((lastSteps + correction) * std::cos(gradenInRad)));
					steps = lastSteps + correction;
					break;
				}
				checkX = MouthCenterPoint.getX()+ (steps * std::sin(gradenInRad));
				checkY = MouthCenterPoint.getY()+(steps * std::cos(gradenInRad));
				Intensity pixel = image.getPixel(std::round(checkX), std::round(checkY));
				if (int(pixel) == 0)
				{
					if (checkX - vorigeX <2)
					{
						lastdif / i;
						ireg = true;
						gevondenPunt.set(MouthCenterPoint.getX() + ((lastSteps + correction)* std::sin(gradenInRad)), MouthCenterPoint.getY() + ((lastSteps + correction) * std::cos(gradenInRad)));
						steps = lastSteps + correction;
						break;
					}
					ireg=false;
					gevondenPunt.set(checkX, checkY); break; 
				}
				steps++;
			}
			vorigeX = checkX;
			std::cout << gevondenPunt <<"\n";
			startStep = steps - 5;
			output.addPoint(Point2D<double>(gevondenPunt.x,gevondenPunt.y));
			first=false;
			if (ireg)
			{
				startStep = steps-5;
				lastSteps = steps;
			}
			else
			{
				lastdif = lastSteps - steps;
				lastSteps = steps;
			}
		}
		else
		{ 
			output.addPoint(ChinPoint); 
		}
	}
	features.putFeature(output);
	basetimer.stop();
	std::ofstream myfile;
	myfile.open("tijd.txt", std::ofstream::ate);
	myfile << "Chincontours convert tijd in s: " << basetimer.elapsedSeconds() << " tijd ms:" << basetimer.elapsedMilliSeconds() << " tijd us" << basetimer.elapsedMicroSeconds();
	myfile.close();
	return true;
}