#pragma once

class CDither {
private:
	
	static unsigned int FindNearestColorIndex(RGBA Find, BGRA* Palette, unsigned int Size) {
		int Nearest = 0xff * 0xff + 0xff * 0xff + 0xff * 0xff + 1;
		unsigned int NearestIndex = 0;

		for (int i = 0; i < Size; i++) {
			int Color = (Palette[i].Red - Find.Red) * (Palette[i].Red - Find.Red) +
			(Palette[i].Green - Find.Green) * (Palette[i].Green - Find.Green) +
			(Palette[i].Blue - Find.Blue) * (Palette[i].Blue - Find.Blue);
			
			if (Color < Nearest) {
				Nearest = Color;
				NearestIndex = i;
			}
		}
		return NearestIndex;
	}

	static unsigned char SaturateAdd(int a, int b) {
		return a + b > 255 ? 255 : (a + b < 0 ? 0 : a + b);
	}

public:
	static bool DitherFloydSteinberg(RGBA* Buffer, unsigned int Width, unsigned int Height, BGRA *Palette, unsigned int PaletteSize) {
		for (int x = 0; x < Width; x++) {
			for (int y = 0; y < Height; y++) {
				unsigned int Index = FindNearestColorIndex(Buffer[y * Width + x], Palette, PaletteSize);
				
				int er = Buffer[y * Width + x].Red - Palette[Index].Red;
				int eg = Buffer[y * Width + x].Green - Palette[Index].Green;
				int eb = Buffer[y * Width + x].Blue - Palette[Index].Blue;

				Buffer[y * Width + x].Red = Palette[Index].Red;
				Buffer[y * Width + x].Green = Palette[Index].Green;
				Buffer[y * Width + x].Blue = Palette[Index].Blue;

				if (x + 1 < Width) {
					Buffer[(y + 0) * Width + (x + 1)].Red = SaturateAdd(Buffer[(y + 0) * Width + (x + 1)].Red, (7 * er / 16));
					Buffer[(y + 0) * Width + (x + 1)].Green = SaturateAdd(Buffer[(y + 0) * Width + (x + 1)].Green, (7 * eg / 16));
					Buffer[(y + 0) * Width + (x + 1)].Blue = SaturateAdd(Buffer[(y + 0) * Width + (x + 1)].Blue, (7 * eb / 16));
				}
				if (y + 1 < Height) {
					if (x - 1 > 0) {
						Buffer[(y + 1) * Width + (x - 1)].Red = SaturateAdd(Buffer[(y + 1) * Width + (x - 1)].Red, (3 * er / 16));
						Buffer[(y + 1) * Width + (x - 1)].Green = SaturateAdd(Buffer[(y + 1) * Width + (x - 1)].Green, (3 * eg / 16));
						Buffer[(y + 1) * Width + (x - 1)].Blue = SaturateAdd(Buffer[(y + 1) * Width + (x - 1)].Blue, (3 * eb / 16));
					}
					Buffer[(y + 1) * Width + (x + 0)].Red = SaturateAdd(Buffer[(y + 1) * Width + (x + 0)].Red, (5 * er / 16));
					Buffer[(y + 1) * Width + (x + 0)].Green = SaturateAdd(Buffer[(y + 1) * Width + (x + 0)].Green, (5 * eg / 16));
					Buffer[(y + 1) * Width + (x + 0)].Blue = SaturateAdd(Buffer[(y + 1) * Width + (x + 0)].Blue, (5 * eb / 16));
					if (x + 1 < Width) {
						Buffer[(y + 1) * Width + (x + 1)].Red = SaturateAdd(Buffer[(y + 1) * Width + (x + 1)].Red, (1 * er / 16));
						Buffer[(y + 1) * Width + (x + 1)].Green = SaturateAdd(Buffer[(y + 1) * Width + (x + 1)].Green, (1 * eg / 16));
						Buffer[(y + 1) * Width + (x + 1)].Blue = SaturateAdd(Buffer[(y + 1) * Width + (x + 1)].Blue, (1 * eb / 16));
					}
				}
			}
		}
		return true;
	}
};