bool StudentLocalization::stepFindHead(const IntensityImage &image, FeatureMap &features) const {
	int yTop = getTopOfHead(image);
	int alpha = 13;
	int* histogram;
	int left, right;
	int diffFirstLast = -1, maxDiffFirstLast = -1;
	int ySides = -1;
	//number of slices the image will be 'cut' into for the histograms
	int count = ((image.getHeight() - yTop - 1) / alpha) + 1;

	for (int i = 0; i < count; i++) {
		//get histogram of current slice
		histogram = getHistogramX(image, alpha, yTop + i * alpha);
		findSidesInHistogram(histogram, image.getWidth(), &left, &right);
		//if no two sides have been found continue to the next slice
		if (left == -1 || right == -1) {
			continue;
		}

		//find the first peak gap between the left and right side to detect the position of the ears
		diffFirstLast = right - left;
		if (diffFirstLast > maxDiffFirstLast) {
			maxDiffFirstLast = diffFirstLast;
		}
		else {
			//if the gap is lower than the max, a peak has been found
			//and the y position of the ears will be set to the middle of the slice 
			//where the peak is detected
			ySides = yTop + (i + 1)  * alpha;
			break;
		}
	}
	//if no peak is detected return false
	if (ySides == -1) {
		return false;
	}

	//set the results in the feature map
	Point2D<double> * pointLeftSideHead = new Point2D<double>(left, ySides);
	Feature * leftSideHead = new Feature(Feature::FEATURE_HEAD_LEFT_SIDE, *pointLeftSideHead);
	features.putFeature(*leftSideHead);

	Point2D<double> * pointRightSideHead = new Point2D<double>(right, ySides);
	Feature * rightSideHead = new Feature(Feature::FEATURE_HEAD_RIGHT_SIDE, *pointRightSideHead);
	features.putFeature(*rightSideHead);

	Point2D<double> * pointTopOfHead = new Point2D<double>((left + right) / 2, yTop);
	Feature * topOfHead = new Feature(Feature::FEATURE_HEAD_TOP, *pointTopOfHead);
	features.putFeature(*topOfHead);
	
	return true;
}
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::stepFindExactEyes(const IntensityImage &image, FeatureMap &features) const {

   int temp = 0;
	ImageIO::saveIntensityImage(image, ImageIO::getDebugFileName("input.png"));

	IntensityImageStudent copy(image);

	double** d_kern = new double*[3];
	for (int y = 0; y < 3; y++) {
		d_kern[y] = new double[3];
	}
	d_kern[0][0] = 0; 
	d_kern[0][1] = 1;
	d_kern[0][2] = 0;
	d_kern[1][0] = 1;
	d_kern[1][1] = 0;
	d_kern[1][2] = 1;
	d_kern[2][0] = 0;
	d_kern[2][1] = 1;
	d_kern[2][2] = 0;

	StudentKernel dilation = StudentKernel(d_kern, 3, 3, 0);

   Feature top = features.getFeature(Feature::FEATURE_HEAD_TOP);
   Feature bottom = features.getFeature(Feature::FEATURE_CHIN);
	Feature nose_bottom = features.getFeature(Feature::FEATURE_NOSE_BOTTOM);
	Feature headsideleft = features.getFeature(Feature::FEATURE_HEAD_LEFT_SIDE);
	Feature headsideright = features.getFeature(Feature::FEATURE_HEAD_RIGHT_SIDE);

   const int nose_to_top = nose_bottom.getY() - top.getY();

#if UNNECESSARY_CODE
#endif
	Point2D<double> headsidelp = headsideleft.getPoints()[0];
	headsidelp.setY(top.getY() + (nose_to_top*3/5));
	headsidelp.setX((headsidelp.getX()));


	Point2D<double> headsiderp = headsideright.getPoints()[0];
   headsiderp.setY(nose_bottom.getY() - (nose_to_top*1/5));
	headsiderp.setX((headsiderp.getX() + 3));


   IntensityImageStudent eyes = ImageUtils::subimage(&image, headsidelp, headsiderp);
	IntensityImageStudent eyes_dilated = dilation.dilate(&eyes);

   StudentHistogram histo{eyes_dilated.getWidth()};
   int zero_points[2] = { 0, histo.get_length() * 1 / 7 };
   int lowest_found[2] = { eyes_dilated.getHeight(), eyes_dilated.getHeight() };

   int left_out = 0, left_out_x = 0,
       right_out = 0, right_out_x = 0,
       left_in = 0, left_in_x = 0,
       right_in = 0, right_in_x = 0;

   for (int x = 0; x < histo.get_length(); x++){
       temp = 0;
       for (int y = 0; y < eyes_dilated.getHeight(); y++){
           temp += eyes_dilated.getPixel(x, y) > 127 ? 0 : 1;
       }
       histo.set_value(x, temp);
       if (x < histo.get_length() * 3 / 10 && temp <= lowest_found[0]){
           lowest_found[0] = temp;
           zero_points[0] = x;
       }
       if (x > histo.get_length() * 7 / 10 && temp < lowest_found[1]){
           lowest_found[1] = temp;
           zero_points[1] = x;
       }
   }

   headsidelp.setX(headsidelp.getX() + zero_points[0]);
   headsiderp.setX(headsiderp.getX() - (histo.get_length() - zero_points[1]));

   headsidelp.setY(top.getY() + (nose_to_top * 2/ 5));
   headsiderp.setY(nose_bottom.getY());

   IntensityImageStudent eyes2(ImageUtils::subimage(&image, headsidelp, headsiderp));
   IntensityImageStudent eyes_copy2(dilation.dilate(&eyes2));

   histo.cut_to_size(zero_points[0], zero_points[1]);
   for (int x = 0; x < histo.get_length(); x++){
       if (x < histo.get_length() * 2 / 9){
           left_out_x = histo.get_value(x) > left_out ? x : left_out_x;
           left_out = histo.get_value(x) > left_out ? histo.get_value(x) : left_out;
       }
       else if (x < histo.get_length() * 4 / 9 && x > histo.get_length() * 2 / 9){
           left_in_x = histo.get_value(x) > left_in ? x : left_in_x;
           left_in = histo.get_value(x) > left_in ? histo.get_value(x) : left_in;
       }
       else if (x > histo.get_length() * 7 / 9){
           right_out_x = histo.get_value(x) > right_out ? x : right_out_x;
           right_out = histo.get_value(x) > right_out ? histo.get_value(x) : right_out;
       }
       else if (x > histo.get_length() * 5 / 9){
           right_in_x = histo.get_value(x) > right_in ? x : right_in_x;
           right_in = histo.get_value(x) > right_in ? histo.get_value(x) : right_in;
       }
   }

   int forehead_val = 10, forehead_y = 0;
   StudentHistogram histo2{eyes_copy2.getHeight()};
   for (int y = 0; y < histo2.get_length(); y++){
       temp = 0;
       for (int x = 0; x < eyes_copy2.getWidth(); x++){
           temp += eyes_copy2.getPixel(x, y) > 127 ? 0 : 1;
       }
       histo2.set_value(y, temp);
   }
   for (int y = 0; y < histo2.get_length(); y++){
       if (histo2.get_value(y) <= forehead_val){
           forehead_val = histo2.get_value(y);
           forehead_y = y;
       }
       else if (histo2.get_value(y) > 35 && forehead_val != 10) break;
   }

   headsidelp.setY(top.getY() + (nose_to_top * 2 / 5) + forehead_y);
   forehead_val = 15, forehead_y = histo2.get_length() -1;

   for (int y = histo2.get_length() -1; y > 0; --y){
       if (histo2.get_value(y) <= forehead_val){
           forehead_val = histo2.get_value(y);
           forehead_y = y;
       }
       else if (histo2.get_value(y) > 40 && forehead_val != 15) break;
   }

   headsiderp.setY(nose_bottom.getY() - (histo2.get_length() - forehead_y + 4));

   Point2D<double> precise_ref{headsidelp};
   IntensityImageStudent eyes3(ImageUtils::subimage(&image, headsidelp, headsiderp));
   IntensityImageStudent eyes_copy3(dilation.dilate(&eyes3));

#ifdef DEBUG
   ImageIO::saveIntensityImage(eyes2, ImageIO::getDebugFileName("first eye cut.png"));
   ImageIO::saveIntensityImage(eyes_copy3, ImageIO::getDebugFileName("subimage_test3.png"));
#endif // DEBUG

   StudentHistogram histo4{ eyes_copy3.getHeight() };
   for (int y = 0; y < histo4.get_length(); y++){
       temp = 0;
       for (int x = 0; x < eyes_copy3.getWidth(); x++){
           temp += eyes_copy3.getPixel(x, y) > 127 ? 0 : 1;
       }
       histo4.set_value(y, temp);
   }

#ifdef DEBUG
   ImageIO::saveIntensityImage(*histo2.get_debug_image(), ImageIO::getDebugFileName("histo2.png"));
#endif // DEBUG

   int step = 0, bottom_eye_y = headsiderp.getY(), top_eye_y = headsidelp.getY() +10;
   for (int y = histo4.get_length(); y > 0; --y){
       if (step == 0 && histo4.get_value(y) > 30){ bottom_eye_y = y; step++; }
       if (step == 1 && histo4.get_value(y) < histo4.get_value(bottom_eye_y)){
           top_eye_y = y;
           break;
       }
   }

   IntensityImageStudent eyes_precise{eyes3};
   for (int x = 0; x < eyes_precise.getWidth(); x++){ eyes_precise.setPixel(x, top_eye_y, 127); eyes_precise.setPixel(x, bottom_eye_y + 2, 127); }
   for (int y = 0; y < eyes_precise.getHeight(); y++){
       eyes_precise.setPixel(left_in_x, y, 127);
       eyes_precise.setPixel(right_in_x, y, 127);
       eyes_precise.setPixel(left_out_x, y, 127);
       eyes_precise.setPixel(right_out_x, y, 127);
   }
   bottom_eye_y += 2;
   IntensityImageStudent eyes_precise2 = ImageUtils::subimage( &eyes_precise, Point2D<double>{0.0, (double)(top_eye_y)}, Point2D<double>{(double)eyes_precise.getWidth(), (double)bottom_eye_y} );

#ifdef DEBUG
   ImageIO::saveIntensityImage(eyes_precise2, ImageIO::getDebugFileName("eyes_precise.png"));
   ImageIO::saveIntensityImage(*histo.get_debug_image(), ImageIO::getDebugFileName("histo.png"));

#endif // DEBUG

   StudentHistogram histoTopBottom{eyes_precise2.getWidth()};
   for (int i = 0; i < histoTopBottom.get_length(); i++){
       temp = 0;
       for (int j = 0; j < eyes_precise2.getHeight(); j++){
           temp += eyes_precise2.getPixel(i, j) > 127? 0: 1;
       }
       histoTopBottom.set_value(i, temp);
   }

#ifdef DEBUG
   ImageIO::saveIntensityImage(*histoTopBottom.get_debug_image(), ImageIO::getDebugFileName("histo.png"));
#endif // DEBUG

   int outsideLeftX = 0, insideLeftX = 0, insideRightX = 0, outsideRightX = histoTopBottom.get_length()-1;
   for (int i = 0; i < histoTopBottom.get_length(); i++){
       if (histoTopBottom.get_value(i) > eyes_precise2.getHeight() / 2 && i < histoTopBottom.get_length()*1/4){
           outsideLeftX = i;
           i = histoTopBottom.get_length() * 1 / 4;
       }
       else if (histoTopBottom.get_value(i) < eyes_precise2.getHeight() / 3 && i < histoTopBottom.get_length() / 2 && i > histoTopBottom.get_length() * 1 / 4){
           insideLeftX = i;
           i = histoTopBottom.get_length() / 2;
       }
       else if (histoTopBottom.get_value(i) > eyes_precise2.getHeight() / 2 && i < histoTopBottom.get_length() * 3 / 4 && i > histoTopBottom.get_length() / 2){
           insideRightX = i;
           i = histoTopBottom.get_length() * 3 / 4;
       }
       else if (histoTopBottom.get_value(i) < eyes_precise2.getHeight() / 3 && i > histoTopBottom.get_length() * 3 / 4){
           outsideRightX = i;
           break;
       }
   }

#ifdef DEBUG
   IntensityImageStudent eyes_precise21 = ImageUtils::subimage(&eyes_precise, Point2D<double>{(double)outsideLeftX, (double)(top_eye_y)}, Point2D<double>{(double)insideLeftX, (double)bottom_eye_y});
   ImageIO::saveIntensityImage(eyes_precise21, ImageIO::getDebugFileName("left_eye.png"));


   IntensityImageStudent eyes_precise22 = ImageUtils::subimage(&eyes_precise, Point2D<double>{(double)insideRightX, (double)(top_eye_y)}, Point2D<double>{(double)outsideRightX, (double)bottom_eye_y});
   ImageIO::saveIntensityImage(eyes_precise22, ImageIO::getDebugFileName("right_eye.png"));
#endif

   Feature left = Feature(Feature::FEATURE_EYE_LEFT_RECT);
   Feature right = Feature(Feature::FEATURE_EYE_RIGHT_RECT);

   left.addPoint(precise_ref + Point2D<double>{(double)outsideLeftX, (double)(top_eye_y)});
   left.addPoint(precise_ref + Point2D<double>{(double)insideLeftX, (double)bottom_eye_y});

   right.addPoint(precise_ref + Point2D<double>{(double)insideRightX, (double)(top_eye_y)});
   right.addPoint(precise_ref + Point2D<double>{(double)outsideRightX, (double)bottom_eye_y});

	features.putFeature(right);
	features.putFeature(left);


	for (int y = 0; y < 3; y++) {
		delete[] d_kern[y];
	}
	delete[] d_kern;

	return true;
}
bool StudentLocalization::stepFindHead(const IntensityImage &image, FeatureMap &features) const {
	Histogram AxisY(image, 50, 50, 0, 0, Axis::y);
	
	//determine top of the head

	int topHead = -1;

	for (int i = 0; i < AxisY.getSize(); ++i) {
		if (AxisY.getValue(i) > 2) {
			topHead = i;
			break;
		}
	}
	if (topHead == -1) {
		return false;
	}
	
	//determine sides

	int lastLeftX = -1;
	int lastRightX = -1;
	for (int y = topHead; y < image.getHeight(); y += 20) {
		Histogram layerX(image, 0, 0, y, (image.getHeight() - y - 20), Axis::x);
		
		//determine leftside of the head
		int leftX = -1;
		for (int i = 0; i < layerX.getSize(); ++i) {
			if (layerX.getValue(i) > 2) {
				leftX = i;
				break;
			}
		}

		//determine rightside of the head
		int rightX = -1;
		for (int i = layerX.getSize() - 1; i >= 0 ; --i) {
			if (layerX.getValue(i) > 2) {
				rightX = i;
				break;
			}
		}
		
		// if not both sides are found skip the rest of the loop
		if (leftX == -1 || rightX == -1) {
			continue;
		}

		int currentHeadWidth = rightX - leftX;
		int lastHeadWidth = lastRightX - lastLeftX;

		if (currentHeadWidth < lastHeadWidth) {
			Feature * headUpperBound = new Feature(Feature::FEATURE_HEAD_TOP, Point2D<double>(lastRightX - (currentHeadWidth / 2), topHead));
			Feature * headLeftBound = new Feature(Feature::FEATURE_HEAD_LEFT_SIDE, Point2D<double>(lastLeftX, y));
			Feature * headRightBound = new Feature(Feature::FEATURE_HEAD_RIGHT_SIDE, Point2D<double>(lastRightX, y));

			features.putFeature(*headUpperBound);
			features.putFeature(*headLeftBound);
			features.putFeature(*headRightBound);
					
			return true;
		}

		lastLeftX = leftX;
		lastRightX = rightX;
	}
	
	return false;
}
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;
}