// Marathon1.cpp : Defines the entry point for the console application.
//
#include <stdio.h>
#include <tchar.h>
#include <stdio.h>
#include <stdlib.h> 
#include <time.h>
#include <process.h>
#include <algorithm>
#include <vector> 
#include <malloc.h>
#include <math.h>
#define NOMINMAX 1
#include "windows.h"

#define WAIT_TIME 4000
#define bufferSize 10000
//#define MYDEBUG 1
#define MAX_COLORS 16
#define MY_PI   3.14159265358979323846264338327950288
#define KERNELSTEP 0.25f

/*
===============================================================TEST BEGIN
*/

typedef double compete_float;

typedef struct _compete_color
{
	compete_float r, g, b, a;
} compete_color;

typedef struct _compete_histogram
{
	compete_color				color;
	unsigned char			ored, ogreen, oblue, oalpha;
	int						palIndex;
	compete_color				ditherScale;
	int						ditherIndex[4];
	int						num;
	struct _compete_histogram	*pNext;
	struct _compete_histogram	*pNextInHash;
} compete_histogram;

typedef struct _compete_node
{
	compete_color				dir, avg;
	compete_float					vdif;
	compete_float					err;
	int						num;
	compete_histogram			*pHistogram;
	compete_histogram			*pSplit;
} compete_node;

#define compete_HASH_BITS			16
#define compete_HASH_SIZE			(1 << (compete_HASH_BITS))

typedef struct _compete_data
{
	compete_histogram			*pHash[compete_HASH_SIZE];
	compete_node				node[256];
	int						numColors;
	int						numBitsPerChannel;
	int						optimized;
	int						transparency;
} compete_data;


#ifndef NULL
#define NULL (0)
#endif

#define SCALE_R 1.0f
#define SCALE_G 1.2f
#define SCALE_B 0.8f
#define SCALE_A 1.0f

void				compete_sum_node(compete_node *pNode);
void				compete_optimize_palette(compete_data *pExp, int iter);
void				compete_sort(compete_histogram **ppHist, compete_float(*sortfunc)(const compete_histogram *pHist));
compete_float			compete_sort_by_r(const compete_histogram *pHist);
compete_float			compete_sort_by_g(const compete_histogram *pHist);
compete_float			compete_sort_by_b(const compete_histogram *pHist);
compete_float			compete_sort_by_a(const compete_histogram *pHist);
compete_float			compete_sort_by_dir(const compete_histogram *pHist);
unsigned char		compete_find_nearest_color(compete_data *pExp, compete_color *pColor);
compete_histogram		*compete_find_histogram(compete_data *pExp, unsigned char *pCol);
extern compete_color	compete_sort_dir;

compete_data *compete_init()
{
	int i;
	compete_data *pExq;

	pExq = (compete_data*)malloc(sizeof(compete_data));

	for (i = 0; i < compete_HASH_SIZE; i++)
		pExq->pHash[i] = NULL;

	pExq->numColors = 0;
	pExq->optimized = 0;
	pExq->transparency = 1;
	pExq->numBitsPerChannel = 8;

	return pExq;
}

void compete_no_transparency(compete_data *pExq)
{
	pExq->transparency = 0;
}

void compete_free(compete_data *pExq)
{
	int i;
	compete_histogram *pCur, *pNext;

	for (i = 0; i < compete_HASH_SIZE; i++)
		for (pCur = pExq->pHash[i]; pCur != NULL; pCur = pNext)
		{
			pNext = pCur->pNextInHash;
			free(pCur);
		}

	free(pExq);
}

static unsigned int compete_make_hash(unsigned int rgba)
{
	rgba -= (rgba >> 13) | (rgba << 19);
	rgba -= (rgba >> 13) | (rgba << 19);
	rgba -= (rgba >> 13) | (rgba << 19);
	rgba -= (rgba >> 13) | (rgba << 19);
	rgba -= (rgba >> 13) | (rgba << 19);
	rgba &= compete_HASH_SIZE - 1;
	return rgba;
}

void compete_feed(compete_data *pExq, unsigned char *pData, int nPixels)
{
	int i;
	unsigned int hash;
	unsigned char r, g, b, a;
	compete_histogram *pCur;
	unsigned char channelMask = 0xff00 >> pExq->numBitsPerChannel;

	for (i = 0; i < nPixels; i++)
	{
		r = *pData++; g = *pData++; b = *pData++; a = *pData++;
		hash = compete_make_hash(((unsigned int)r) | (((unsigned int)g) << 8) | (((unsigned int)b) << 16) | (((unsigned int)a) << 24));

		pCur = pExq->pHash[hash];
		while (pCur != NULL && (pCur->ored != r || pCur->ogreen != g ||
			pCur->oblue != b || pCur->oalpha != a))
			pCur = pCur->pNextInHash;

		if (pCur != NULL)
			pCur->num++;
		else
		{
			pCur = (compete_histogram*)malloc(sizeof(compete_histogram));
			pCur->pNextInHash = pExq->pHash[hash];
			pExq->pHash[hash] = pCur;
			pCur->ored = r; pCur->ogreen = g; pCur->oblue = b; pCur->oalpha = a;
			r &= channelMask; g &= channelMask; b &= channelMask;
			pCur->color.r = r / 255.0f * SCALE_R;
			pCur->color.g = g / 255.0f * SCALE_G;
			pCur->color.b = b / 255.0f * SCALE_B;
			pCur->color.a = a / 255.0f * SCALE_A;

			if (pExq->transparency)
			{
				pCur->color.r *= pCur->color.a;
				pCur->color.g *= pCur->color.a;
				pCur->color.b *= pCur->color.a;
			}

			pCur->num = 1;
			pCur->palIndex = -1;
			pCur->ditherScale.r = pCur->ditherScale.g = pCur->ditherScale.b =
				pCur->ditherScale.a = -1;
			pCur->ditherIndex[0] = pCur->ditherIndex[1] = pCur->ditherIndex[2] =
				pCur->ditherIndex[3] = -1;
		}
	}
}


void compete_quantize_ex(compete_data *pExq, int nColors, int hq)
{
	int besti;
	compete_float beste;
	compete_histogram *pCur, *pNext;
	int i, j;

	if (nColors > 256)
		nColors = 256;

	if (pExq->numColors == 0)
	{
		pExq->node[0].pHistogram = NULL;
		for (i = 0; i < compete_HASH_SIZE; i++)
			for (pCur = pExq->pHash[i]; pCur != NULL; pCur = pCur->pNextInHash)
			{
				pCur->pNext = pExq->node[0].pHistogram;
				pExq->node[0].pHistogram = pCur;
			}

		compete_sum_node(&pExq->node[0]);

		pExq->numColors = 1;
	}

	for (i = pExq->numColors; i < nColors; i++)
	{
		beste = 0;
		besti = 0;
		for (j = 0; j < i; j++)
			if (pExq->node[j].vdif >= beste)
			{
				beste = pExq->node[j].vdif;
				besti = j;
			}

		//		printf("node %d: %d, %f\n", besti, pExq->node[besti].num, beste);

		pCur = pExq->node[besti].pHistogram;
		pExq->node[besti].pHistogram = NULL;
		pExq->node[i].pHistogram = NULL;
		while (pCur != NULL && pCur != pExq->node[besti].pSplit)
		{
			pNext = pCur->pNext;
			pCur->pNext = pExq->node[i].pHistogram;
			pExq->node[i].pHistogram = pCur;
			pCur = pNext;
		}

		while (pCur != NULL)
		{
			pNext = pCur->pNext;
			pCur->pNext = pExq->node[besti].pHistogram;
			pExq->node[besti].pHistogram = pCur;
			pCur = pNext;
		}

		compete_sum_node(&pExq->node[besti]);
		compete_sum_node(&pExq->node[i]);

		pExq->numColors = i + 1;
		if (hq)
			compete_optimize_palette(pExq, 1);
	}

	pExq->optimized = 0;
}

void compete_quantize(compete_data *pExq, int nColors)
{
	compete_quantize_ex(pExq, nColors, 0);
}

void compete_quantize_hq(compete_data *pExq, int nColors)
{
	compete_quantize_ex(pExq, nColors, 1);
}

compete_float compete_get_mean_error(compete_data *pExq)
{
	int i, n;
	compete_float err;

	n = 0;
	err = 0;
	for (i = 0; i < pExq->numColors; i++)
	{
		n += pExq->node[i].num;
		err += pExq->node[i].err;
	}

	return sqrt(err / n) * 256;
}

void compete_get_palette(compete_data *pExq, unsigned char *pPal, int nColors)
{
	int i, j;
	compete_float r, g, b, a;
	unsigned char channelMask = 0xff00 >> pExq->numBitsPerChannel;

	if (nColors > pExq->numColors)
		nColors = pExq->numColors;

	if (!pExq->optimized)
		compete_optimize_palette(pExq, 4);

	for (i = 0; i < nColors; i++)
	{
		r = pExq->node[i].avg.r;
		g = pExq->node[i].avg.g;
		b = pExq->node[i].avg.b;
		a = pExq->node[i].avg.a;

		if (pExq->transparency == 1 && a != 0)
		{
			r /= a; g /= a; b /= a;
		}

		pPal[0] = (unsigned char)(r / SCALE_R * 255.9f);
		pPal[1] = (unsigned char)(g / SCALE_G * 255.9f);
		pPal[2] = (unsigned char)(b / SCALE_B * 255.9f);
		pPal[3] = (unsigned char)(a / SCALE_A * 255.9f);

		for (j = 0; j < 3; j++)
			pPal[j] = (pPal[j] + (1 << (8 - pExq->numBitsPerChannel)) / 2) & channelMask;
		pPal += 4;
	}
}

void compete_set_palette(compete_data *pExq, unsigned char *pPal, int nColors)
{
	int i;

	pExq->numColors = nColors;

	for (i = 0; i < nColors; i++)
	{
		pExq->node[i].avg.r = *pPal++ * SCALE_R / 255.9f;
		pExq->node[i].avg.g = *pPal++ * SCALE_G / 255.9f;
		pExq->node[i].avg.b = *pPal++ * SCALE_B / 255.9f;
		pExq->node[i].avg.a = *pPal++ * SCALE_A / 255.9f;
	}

	pExq->optimized = 1;
}

void compete_sum_node(compete_node *pNode)
{
	int n, n2;
	compete_color fsum, fsum2, vc, tmp, tmp2, sum, sum2;
	compete_histogram *pCur;
	compete_float isqrt, nv, v;

	n = 0;
	fsum.r = fsum.g = fsum.b = fsum.a = 0;
	fsum2.r = fsum2.g = fsum2.b = fsum2.a = 0;

	for (pCur = pNode->pHistogram; pCur != NULL; pCur = pCur->pNext)
	{
		n += pCur->num;
		fsum.r += pCur->color.r * pCur->num;
		fsum.g += pCur->color.g * pCur->num;
		fsum.b += pCur->color.b * pCur->num;
		fsum.a += pCur->color.a * pCur->num;
		fsum2.r += pCur->color.r * pCur->color.r * pCur->num;
		fsum2.g += pCur->color.g * pCur->color.g * pCur->num;
		fsum2.b += pCur->color.b * pCur->color.b * pCur->num;
		fsum2.a += pCur->color.a * pCur->color.a * pCur->num;
	}
	pNode->num = n;
	if (n == 0)
	{
		pNode->vdif = 0;
		pNode->err = 0;
		return;
	}

	pNode->avg.r = fsum.r / n;
	pNode->avg.g = fsum.g / n;
	pNode->avg.b = fsum.b / n;
	pNode->avg.a = fsum.a / n;

	vc.r = fsum2.r - fsum.r * pNode->avg.r;
	vc.g = fsum2.g - fsum.g * pNode->avg.g;
	vc.b = fsum2.b - fsum.b * pNode->avg.b;
	vc.a = fsum2.a - fsum.a * pNode->avg.a;

	v = vc.r + vc.g + vc.b + vc.a;
	pNode->err = v;
	pNode->vdif = -v;

	if (vc.r > vc.g && vc.r > vc.b && vc.r > vc.a)
		compete_sort(&pNode->pHistogram, compete_sort_by_r);
	else if (vc.g > vc.b && vc.g > vc.a)
		compete_sort(&pNode->pHistogram, compete_sort_by_g);
	else if (vc.b > vc.a)
		compete_sort(&pNode->pHistogram, compete_sort_by_b);
	else
		compete_sort(&pNode->pHistogram, compete_sort_by_a);

	pNode->dir.r = pNode->dir.g = pNode->dir.b = pNode->dir.a = 0;
	for (pCur = pNode->pHistogram; pCur != NULL; pCur = pCur->pNext)
	{
		tmp.r = (pCur->color.r - pNode->avg.r) * pCur->num;
		tmp.g = (pCur->color.g - pNode->avg.g) * pCur->num;
		tmp.b = (pCur->color.b - pNode->avg.b) * pCur->num;
		tmp.a = (pCur->color.a - pNode->avg.a) * pCur->num;
		if (tmp.r * pNode->dir.r + tmp.g * pNode->dir.g +
			tmp.b * pNode->dir.b + tmp.a * pNode->dir.a < 0)
		{
			tmp.r = -tmp.r;
			tmp.g = -tmp.g;
			tmp.b = -tmp.b;
			tmp.a = -tmp.a;
		}
		pNode->dir.r += tmp.r;
		pNode->dir.g += tmp.g;
		pNode->dir.b += tmp.b;
		pNode->dir.a += tmp.a;
	}
	isqrt = 1 / sqrt(pNode->dir.r * pNode->dir.r +
		pNode->dir.g * pNode->dir.g + pNode->dir.b * pNode->dir.b +
		pNode->dir.a * pNode->dir.a);
	pNode->dir.r *= isqrt;
	pNode->dir.g *= isqrt;
	pNode->dir.b *= isqrt;
	pNode->dir.a *= isqrt;

	compete_sort_dir = pNode->dir;
	compete_sort(&pNode->pHistogram, compete_sort_by_dir);

	sum.r = sum.g = sum.b = sum.a = 0;
	sum2.r = sum2.g = sum2.b = sum2.a = 0;
	n2 = 0;
	pNode->pSplit = pNode->pHistogram;
	for (pCur = pNode->pHistogram; pCur != NULL; pCur = pCur->pNext)
	{
		if (pNode->pSplit == NULL)
			pNode->pSplit = pCur;

		n2 += pCur->num;
		sum.r += pCur->color.r * pCur->num;
		sum.g += pCur->color.g * pCur->num;
		sum.b += pCur->color.b * pCur->num;
		sum.a += pCur->color.a * pCur->num;
		sum2.r += pCur->color.r * pCur->color.r * pCur->num;
		sum2.g += pCur->color.g * pCur->color.g * pCur->num;
		sum2.b += pCur->color.b * pCur->color.b * pCur->num;
		sum2.a += pCur->color.a * pCur->color.a * pCur->num;

		if (n == n2)
			break;

		tmp.r = sum2.r - sum.r*sum.r / n2;
		tmp.g = sum2.g - sum.g*sum.g / n2;
		tmp.b = sum2.b - sum.b*sum.b / n2;
		tmp.a = sum2.a - sum.a*sum.a / n2;
		tmp2.r = (fsum2.r - sum2.r) - (fsum.r - sum.r)*(fsum.r - sum.r) / (n - n2);
		tmp2.g = (fsum2.g - sum2.g) - (fsum.g - sum.g)*(fsum.g - sum.g) / (n - n2);
		tmp2.b = (fsum2.b - sum2.b) - (fsum.b - sum.b)*(fsum.b - sum.b) / (n - n2);
		tmp2.a = (fsum2.a - sum2.a) - (fsum.a - sum.a)*(fsum.a - sum.a) / (n - n2);

		nv = tmp.r + tmp.g + tmp.b + tmp.a + tmp2.r + tmp2.g + tmp2.b + tmp2.a;
		if (-nv > pNode->vdif)
		{
			pNode->vdif = -nv;
			pNode->pSplit = NULL;
		}
	}

	if (pNode->pSplit == pNode->pHistogram)
		pNode->pSplit = pNode->pSplit->pNext;

	pNode->vdif += v;
	//	printf("error sum: %f, vdif: %f\n", pNode->err, pNode->vdif);
}

void compete_optimize_palette(compete_data *pExq, int iter)
{
	int n, i, j;
	compete_histogram *pCur;

	pExq->optimized = 1;

	for (n = 0; n < iter; n++)
	{
		for (i = 0; i < pExq->numColors; i++)
			pExq->node[i].pHistogram = NULL;

		for (i = 0; i < compete_HASH_SIZE; i++)
			for (pCur = pExq->pHash[i]; pCur != NULL; pCur = pCur->pNextInHash)
			{
				j = compete_find_nearest_color(pExq, &pCur->color);
				pCur->pNext = pExq->node[j].pHistogram;
				pExq->node[j].pHistogram = pCur;
			}

		for (i = 0; i < pExq->numColors; i++)
			compete_sum_node(&pExq->node[i]);
	}
}

void compete_map_image(compete_data *pExq, int nPixels, unsigned char *pIn,
	unsigned char *pOut)
{
	int i;
	compete_color c;
	compete_histogram *pHist;

	if (!pExq->optimized)
		compete_optimize_palette(pExq, 4);

	for (i = 0; i < nPixels; i++)
	{
		pHist = compete_find_histogram(pExq, pIn);
		if (pHist != NULL && pHist->palIndex != -1)
		{
			*pOut++ = (unsigned char)pHist->palIndex;
			pIn += 4;
		}
		else
		{
			c.r = *pIn++ / 255.0f * SCALE_R;
			c.g = *pIn++ / 255.0f * SCALE_G;
			c.b = *pIn++ / 255.0f * SCALE_B;
			c.a = *pIn++ / 255.0f * SCALE_A;

			if (pExq->transparency)
			{
				c.r *= c.a; c.g *= c.a; c.b *= c.a;
			}

			*pOut = compete_find_nearest_color(pExq, &c);
			if (pHist != NULL)
				pHist->palIndex = *pOut;
			pOut++;
		}
	}
}



void compete_map_image_dither(compete_data *pExq, int width, int height,
	unsigned char *pIn, unsigned char *pOut, int ordered)
{
	int x, y, i, j, d;
	compete_color p, scale, tmp;
	compete_histogram *pHist;
	const compete_float dither_matrix[4] = { -0.375, 0.125, 0.375, -0.125 };

	if (!pExq->optimized)
		compete_optimize_palette(pExq, 4);

	for (y = 0; y < height; y++)
		for (x = 0; x < width; x++)
		{
			if (ordered)
				d = (x & 1) + (y & 1) * 2;
			else
				d = rand() & 3;
			pHist = compete_find_histogram(pExq, pIn);
			p.r = *pIn++ / 255.0f * SCALE_R;
			p.g = *pIn++ / 255.0f * SCALE_G;
			p.b = *pIn++ / 255.0f * SCALE_B;
			p.a = *pIn++ / 255.0f * SCALE_A;

			if (pExq->transparency)
			{
				p.r *= p.a; p.g *= p.a; p.b *= p.a;
			}

			if (pHist == NULL || pHist->ditherScale.r < 0)
			{
				i = compete_find_nearest_color(pExq, &p);
				scale.r = pExq->node[i].avg.r - p.r;
				scale.g = pExq->node[i].avg.g - p.g;
				scale.b = pExq->node[i].avg.b - p.b;
				scale.a = pExq->node[i].avg.a - p.a;
				tmp.r = p.r - scale.r / 3;
				tmp.g = p.g - scale.g / 3;
				tmp.b = p.b - scale.b / 3;
				tmp.a = p.a - scale.a / 3;
				j = compete_find_nearest_color(pExq, &tmp);
				if (i == j)
				{
					tmp.r = p.r - scale.r * 3;
					tmp.g = p.g - scale.g * 3;
					tmp.b = p.b - scale.b * 3;
					tmp.a = p.a - scale.a * 3;
					j = compete_find_nearest_color(pExq, &tmp);
				}
				if (i != j)
				{
					scale.r = (pExq->node[j].avg.r - pExq->node[i].avg.r) * 0.8f;
					scale.g = (pExq->node[j].avg.g - pExq->node[i].avg.g) * 0.8f;
					scale.b = (pExq->node[j].avg.b - pExq->node[i].avg.b) * 0.8f;
					scale.a = (pExq->node[j].avg.a - pExq->node[i].avg.a) * 0.8f;
					if (scale.r < 0) scale.r = -scale.r;
					if (scale.g < 0) scale.g = -scale.g;
					if (scale.b < 0) scale.b = -scale.b;
					if (scale.a < 0) scale.a = -scale.a;
				}
				else
					scale.r = scale.g = scale.b = scale.a = 0;

				if (pHist != NULL)
				{
					pHist->ditherScale.r = scale.r;
					pHist->ditherScale.g = scale.g;
					pHist->ditherScale.b = scale.b;
					pHist->ditherScale.a = scale.a;
				}
			}
			else
			{
				scale.r = pHist->ditherScale.r;
				scale.g = pHist->ditherScale.g;
				scale.b = pHist->ditherScale.b;
				scale.a = pHist->ditherScale.a;
			}

			if (pHist != NULL && pHist->ditherIndex[d] >= 0)
				*pOut++ = (unsigned char)pHist->ditherIndex[d];
			else
			{
				tmp.r = p.r + scale.r * dither_matrix[d];
				tmp.g = p.g + scale.g * dither_matrix[d];
				tmp.b = p.b + scale.b * dither_matrix[d];
				tmp.a = p.a + scale.a * dither_matrix[d];
				*pOut = compete_find_nearest_color(pExq, &tmp);
				if (pHist != NULL)
					pHist->ditherIndex[d] = *pOut;
				pOut++;
			}
		}
}

void compete_map_image_ordered(compete_data *pExq, int width, int height,
	unsigned char *pIn, unsigned char *pOut)
{
	compete_map_image_dither(pExq, width, height, pIn, pOut, 1);
}

void compete_map_image_random(compete_data *pExq, int nPixels,
	unsigned char *pIn, unsigned char *pOut)
{
	compete_map_image_dither(pExq, nPixels, 1, pIn, pOut, 0);
}

compete_histogram *compete_find_histogram(compete_data *pExq, unsigned char *pCol)
{
	unsigned int hash;
	int r, g, b, a;
	compete_histogram *pCur;

	r = *pCol++; g = *pCol++; b = *pCol++; a = *pCol++;
	hash = compete_make_hash(((unsigned int)r) | (((unsigned int)g) << 8) | (((unsigned int)b) << 16) | (((unsigned int)a) << 24));

	pCur = pExq->pHash[hash];
	while (pCur != NULL && (pCur->ored != r || pCur->ogreen != g ||
		pCur->oblue != b || pCur->oalpha != a))
		pCur = pCur->pNextInHash;

	return pCur;
}

unsigned char compete_find_nearest_color(compete_data *pExq, compete_color *pColor)
{
	compete_float bestv;
	int besti, i;
	compete_color dif;

	bestv = 16;
	besti = 0;
	for (i = 0; i < pExq->numColors; i++)
	{
		dif.r = pColor->r - pExq->node[i].avg.r;
		dif.g = pColor->g - pExq->node[i].avg.g;
		dif.b = pColor->b - pExq->node[i].avg.b;
		dif.a = pColor->a - pExq->node[i].avg.a;
		if (dif.r*dif.r + dif.g*dif.g + dif.b*dif.b + dif.a*dif.a < bestv)
		{
			bestv = dif.r*dif.r + dif.g*dif.g + dif.b*dif.b + dif.a*dif.a;
			besti = i;
		}
	}

	return (unsigned char)besti;
}

void compete_sort(compete_histogram **ppHist, compete_float(*sortfunc)(const compete_histogram *pHist))
{
	compete_histogram *pLow, *pHigh, *pCur, *pNext;
	int n = 0;
	compete_float sum = 0;

	for (pCur = *ppHist; pCur != NULL; pCur = pCur->pNext)
	{
		n++;
		sum += sortfunc(pCur);
	}

	if (n < 2)
		return;

	sum /= n;

	pLow = pHigh = NULL;
	for (pCur = *ppHist; pCur != NULL; pCur = pNext)
	{
		pNext = pCur->pNext;
		if (sortfunc(pCur) < sum)
		{
			pCur->pNext = pLow;
			pLow = pCur;
		}
		else
		{
			pCur->pNext = pHigh;
			pHigh = pCur;
		}
	}

	if (pLow == NULL)
	{
		*ppHist = pHigh;
		return;
	}
	if (pHigh == NULL)
	{
		*ppHist = pLow;
		return;
	}

	compete_sort(&pLow, sortfunc);
	compete_sort(&pHigh, sortfunc);

	*ppHist = pLow;
	while (pLow->pNext != NULL)
		pLow = pLow->pNext;

	pLow->pNext = pHigh;
}

compete_float compete_sort_by_r(const compete_histogram *pHist)
{
	return pHist->color.r;
}

compete_float compete_sort_by_g(const compete_histogram *pHist)
{
	return pHist->color.g;
}

compete_float compete_sort_by_b(const compete_histogram *pHist)
{
	return pHist->color.b;
}

compete_float compete_sort_by_a(const compete_histogram *pHist)
{
	return pHist->color.a;
}

compete_color compete_sort_dir;

compete_float compete_sort_by_dir(const compete_histogram *pHist)
{
	return pHist->color.r * compete_sort_dir.r +
		pHist->color.g * compete_sort_dir.g +
		pHist->color.b * compete_sort_dir.b +
		pHist->color.a * compete_sort_dir.a;
}

/*
============================================================== TEST END
*/






unsigned char* myDATA = NULL;
int W, H;
int mode;

unsigned char COLORMAP[16 * 3];

clock_t begin_time = clock();
CRITICAL_SECTION glCS;


typedef struct { unsigned char r, g, b; } rgb_t;

typedef struct {
	unsigned char r;
	unsigned char g;
	unsigned char b;
} RGB;

typedef struct {
	int w, h;
	unsigned char *pix;
} image_t, *image;

typedef struct {
	RGB rgb;
	int count;
} COUNTER;

bool sortByCOUNTER(const COUNTER &v1, const COUNTER &v2) { return v1.count < v2.count; }

typedef struct {
	int w, h;
	double *kernel;
} matrix;

void calcAll(int dither, bool first);

matrix getGaussian(int height, int width, double center = -1.0)
{
	matrix mat;
	mat.w = width;
	mat.h = height;
	mat.kernel = (double*)malloc(height * width * sizeof(double));

	double sigma = 1.0;
	double sum = 0.0;
	int i, j;
	double s = 2.0 * sigma * sigma;
	double s1 = MY_PI * s;
	double d = 1;

	for (i = 0; i < height; i++) {
		int y = i - height / 2;
		for (j = 0; j < width; j++) {
		
			int x = j - width / 2;			
			//if (x<0 ||  y>0) d = 0;
			if (center>0) if ((x==0 && y==0)) d=center;
			else {
				double r = sqrt(x * x + y * y);
				d = exp(-(r*r) / s) / s1;
			}
			mat.kernel[i*width + j] = d;
			sum += d;
		}
	}


	for (i = 0; i < height; i++) {
		for (j = 0; j < width; j++) {
			mat.kernel[i*width+j] /= sum;
		}
	}

	return mat;
}


#ifdef MYDEBUG
int write_ppm(image im, char *fn)
{
	FILE *fp = fopen(fn, "wb");
	if (!fp) return 0;
	fprintf(fp, "P6\n%d %d\n255\n", im->w, im->h);
	fwrite(im->pix, 1, 3 * im->w * im->h, fp);
	fclose(fp);
	return 1;
}
#endif

image img_new(int w, int h)
{
	image im = (image)malloc(sizeof(image_t) + h * w * 3);
	im->w = w; im->h = h;
	im->pix = (unsigned char *)(im + 1);
	return im;
}


bool loadInput(const char* fileName)
{
	char tempBuffer[bufferSize];


	FILE* in;
	char* next = NULL;
	char* c;
	unsigned char d;
	int idx;
	int i;

	in = fopen(fileName, "r");
	if (in == NULL) return false;


	fgets(tempBuffer, bufferSize, in);
	
	sscanf(tempBuffer, "%d", &mode);

	if (mode == 0)
	{
		fgets(tempBuffer, bufferSize, in);
		sscanf(tempBuffer, "%d %d", &W, &H);

		myDATA = (unsigned char*)malloc(W*H * 3 * sizeof(unsigned char*));
		//glValue = (unsigned short*)__mingw_aligned_malloc(N*M * sizeof(unsigned short), 128);

		int totalRows = 0;

		unsigned char* glVal = myDATA;
		for (i = 0; i < H; ++i) {
			fgets(tempBuffer, bufferSize, in);
			c = tempBuffer;
			idx = 0;
			int limit = 3 * W;
			unsigned int score = 0;
			do
			{
				next = strstr(c, " ");
				if (next) next[0] = 0;
				d = (unsigned char)atoi(c);
				c = next + 1;
				*glVal = d;
				glVal++;
				idx++;
			} while (idx < limit && next);
		}
	}
	else if (mode == 1) {
		//unsigned char _r, _g, _b;
		int colorIdx = 0;
		for (int k = 0; k < 16; k++) {
			fgets(tempBuffer, bufferSize, in);
			int _r, _g, _b;
			sscanf(tempBuffer, "%d %d %d", &_r, &_g, &_b);
			COLORMAP[colorIdx] = _r;
			COLORMAP[colorIdx + 1] = _g;
			COLORMAP[colorIdx + 2] = _b;
			colorIdx += 3;
		}

		fgets(tempBuffer, bufferSize, in);
		sscanf(tempBuffer, "%d %d", &W, &H);
		//myDATA = (unsigned char*)_aligned_malloc(W*H*sizeof(unsigned char*), 128);
		myDATA = (unsigned char*)malloc(W*H * sizeof(unsigned char*));
		unsigned char* glVal = myDATA;
		for (i = 0; i < H; ++i) {
			fgets(tempBuffer, bufferSize, in);
			c = tempBuffer;
			idx = 0;
			int limit = W;
			unsigned int score = 0;
			do
			{
				next = strstr(c, " ");
				if (next) next[0] = 0;
				d = (unsigned char)atoi(c);
				c = next + 1;
				*glVal = d;
				glVal++;
				idx++;
			} while (idx < limit && next);
		}

	}	
	return true;
}



RGB GetBestValue(unsigned char needeVal, unsigned char delta, int startIdx)
{
	RGB rgb;

	int bestIdx = -1; 
	int bestDiff = 10;
	int bestVal = 5;
	int neededStart = (needeVal - 1)*delta;
	int neededEnd =(needeVal)*delta-1;
	if (delta == 1) neededStart = neededEnd = needeVal;

	for (int k = startIdx; k < 16; k++)
	{
		unsigned char _b = COLORMAP[k*3 + 2]%10;
		if (_b>=neededEnd && _b<=neededStart){
			bestIdx = k;
			bestDiff = 0;
			bestVal = _b;
			break;
		}
		
		int diff = std::min(abs(neededStart-_b),abs(neededEnd-_b));
		if (diff<bestDiff){
			bestIdx = k;
			bestDiff = diff;
			bestVal = _b;
		}
		//fprintf(out,"%d\n",glBestIdx[k]);	
	}
	rgb.r = COLORMAP[bestIdx*3];
	rgb.g = COLORMAP[bestIdx*3+1];
	rgb.b = COLORMAP[bestIdx*3+2];
	
	#ifdef MYDEBUG
	printf("Best idx(%d) color %d %d %d for %d\n",bestIdx,rgb.r,rgb.g,rgb.b,needeVal);
	#endif
	if (bestDiff>0)
	{
		int addVal = 0;
		if (neededEnd<bestVal) addVal=neededEnd-bestVal;
		else if (neededStart>bestVal) addVal=neededStart-bestVal;
		//printf("New  offset %d (%d %d %d %d)\n",addVal,neededStart,neededEnd,bestVal, delta);
		int testVal = rgb.b+addVal;
		if (testVal<0) testVal=0;
		else if (testVal>255) testVal=255;
		rgb.b=testVal;
		#ifdef MYDEBUG
		printf("New  color %d %d %d for %d\n",rgb.r,rgb.g,rgb.b,needeVal);
		#endif
	}
	
	if (startIdx!=bestIdx)
	{
		COLORMAP[bestIdx*3] = COLORMAP[startIdx*3];
		COLORMAP[bestIdx*3+1] = COLORMAP[startIdx*3+1];
		COLORMAP[bestIdx*3+2] = COLORMAP[startIdx*3+2];
		COLORMAP[startIdx*3] = rgb.r;
		COLORMAP[startIdx*3+1] = rgb.g;
		COLORMAP[startIdx*3+2] = rgb.b;
	}
	return rgb;
}

int bestG1 = -1; //height
int bestG2 = -1; //width
int bestDither = -1;
int bestMtxCenter = 0;

bool sortBy(const RGB &v1, const RGB &v2) { return (v1.r + v1.g + v1.b) < (v2.r + v2.g + v2.b); }
bool sortReverseBy(const RGB &v1, const RGB &v2) { return (v1.r + v1.g + v1.b) > (v2.r + v2.g + v2.b); }

void saveResult(unsigned char* data, const char* outfile)
{
	
	//statistics
	if (bestG2 > 0 || bestG1 > 0)
	{
		std::vector<COUNTER> counter;
		unsigned char *pData = data;
		int totalPixels = W * H;
		RGB temp;
		for (int z = 0; z < totalPixels; z++) {
			temp.r = *pData++;
			temp.g = *pData++;
			temp.b = *pData++;
			bool found = false;
			for (int f = 0; f < counter.size(); f++) {
				COUNTER *ctr = &counter[f];
				if (ctr->rgb.r == temp.r && ctr->rgb.g == temp.g && ctr->rgb.b == temp.b) {
					ctr->count++;
					found = true;
					break;
				}
			}
			if (!found) {
				COUNTER ctr;
				ctr.count = 1;
				ctr.rgb = temp;
				counter.push_back(ctr);
			}
		}
		std::sort(counter.begin(), counter.end(), sortByCOUNTER);
		for (int k = 0; k < 16; k++) {
			RGB temp = counter[k].rgb;
			COLORMAP[k * 3] = temp.r;
			COLORMAP[k * 3 + 1] = temp.g;
			COLORMAP[k * 3 + 2] = temp.b;
			#ifdef MYDEBUG
			printf("%d IDX:%d CNT:%d\n", k, counter[k].rgb.b, counter[k].count);
			#endif
		}
	}

	
	//for (int k = 0; k < 16; k++) printf("%d %d %d\n", COLORMAP[k*3], COLORMAP[k*3 + 1], COLORMAP[k*3 + 2]);
	
	if (bestG2<=1 && bestG1<=1)
	{
		std::vector<RGB> _vr;
		for (int k = 0; k < 16; k++){
			RGB _rgb;
			_rgb.r = COLORMAP[k*3];
			_rgb.g = COLORMAP[k*3+1];
			_rgb.b = COLORMAP[k*3+2];
			_vr.push_back(_rgb);
		}
		if (bestDither == 2) sort(_vr.begin(), _vr.end(), sortReverseBy);
		else sort(_vr.begin(), _vr.end(), sortBy);
		
		for (int k = 0; k < 16; k++) 
		{		
			COLORMAP[k*3] = _vr[k].r;
			COLORMAP[k*3 + 1] = _vr[k].g;
			COLORMAP[k*3 + 2] = _vr[k].b;
		}
	}else{
		GetBestValue(bestMtxCenter+1, 1, 0);
		GetBestValue(bestG1, 2, 1);
		if (bestG2 >= 5) {
			int diff = bestG2 - 5;
			bestG2 = 5;
			if (diff > 8) diff = 8;
			GetBestValue(bestG2, 2, 2);
			GetBestValue(diff+1, 1, 3);
		}
		else {
			GetBestValue(bestG2, 2, 2);
		}
		if (bestDither == 2) 
		{
#ifdef MYDEBUG
			for (int k = 0; k < 16; k++) printf("%d %d %d\n", COLORMAP[k * 3], COLORMAP[k * 3 + 1], COLORMAP[k * 3 + 2]);
#endif
			std::vector<RGB> _vr;
			for (int k = 4; k < 16; k++) {
				RGB _rgb;
				_rgb.r = COLORMAP[k * 3];
				_rgb.g = COLORMAP[k * 3 + 1];
				_rgb.b = COLORMAP[k * 3 + 2];
				_vr.push_back(_rgb);
			}
			sort(_vr.begin(), _vr.end(), sortReverseBy);
			for (int k = 4; k < 16; k++)
			{
				COLORMAP[k * 3] = _vr[k-4].r;
				COLORMAP[k * 3 + 1] = _vr[k-4].g;
				COLORMAP[k * 3 + 2] = _vr[k-4].b;
			}
		}
	}	
	
	FILE* out;
	out = fopen(outfile, "w");
	int colorIdx = 0;
	for (int k = 0; k < 16; k++)
	{
		fprintf(out, "%d %d %d\n", COLORMAP[colorIdx], COLORMAP[colorIdx + 1], COLORMAP[colorIdx + 2]);
		colorIdx += 3;
		//fprintf(out,"%d\n",glBestIdx[k]);	
	}
	
	#ifdef MYDEBUG
	//for (int k = 0; k < 16; k++) printf("%d %d %d\n", COLORMAP[k*3], COLORMAP[k*3 + 1], COLORMAP[k*3 + 2]);
	#endif

	for (int y = 0; y < H; y++) {
		for (int x = 0; x < W; x++) {
			unsigned char r = *data++;
			unsigned char g = *data++;
			unsigned char b = *data++;
			
			int distance = 255 * 255 * 3;
			short bestColor = 0;
			for (int k = 0; k < 16; k++) {
				unsigned char r1 = COLORMAP[k * 3];
				unsigned char g1 = COLORMAP[k * 3 + 1];
				unsigned char b1 = COLORMAP[k * 3 + 2];
				//int curDistance = pow(r1 - r, 2) + pow(g1 - g, 2) + pow(b1 - b, 2);
				int curDistance = (r1 - r)*(r1 - r) + (b1 - b)*(b1 - b) + (g1 - g)*(g1 - g);
				if (curDistance < distance) {
					distance = curDistance;
					bestColor = k;
				}
			}
			fprintf(out, "%d ", bestColor);
		}
		fprintf(out, "\n");
	}
	fclose(out);
}

void applyEffect(matrix mtx, image im, image blured, int startX, int startY, int endX, int endY)
{
	int offsetX = mtx.w / 2;
	int offsetY = mtx.h / 2;
	int offset = 0;
	int Wx3 = W*3;
	for (int y = startY; y < endY; y++)
	{
		offset = (y*Wx3);
		unsigned char* srcImg = im->pix + offset;
		unsigned char* dstImg = blured->pix + offset;

		int startFy = 0;
		if (y < offsetY) startFy = offsetY - y;
		int endFy = mtx.h;
		if (y >= H - offsetY) endFy = offsetY + (H - y);

		for (int x = startX; x < endX; x++) {
			//apply matrix
			double _r = 0, _g = 0, _b = 0;
			double rest = 0;
			int x3 = x*3;

			int startFx = 0;
			if (x < offsetX) startFx = offsetX - x;
			int endFx = mtx.w;
			if (x >= W - offsetX) endFx = offsetX + (W - x);

			bool clipNeeded = (startFy != 0) || (endFy != mtx.h) || (startFx != 0) || (endFx != mtx.w);
			if (clipNeeded) {
				for (int py = 0; py < mtx.h; py++)
				{
					for (int px = 0; px < mtx.w; px++)
					{
						if (!((startFy <= py && py < endFy) && (px >= startFx && px < endFx)))
						{
							rest += mtx.kernel[py*mtx.w + px];
						}
					}
				}
				unsigned char* srcImg3 = srcImg + x3;
				_r += *srcImg3 * rest;
				_g += *(srcImg3 + 1) * rest;
				_b += *(srcImg3 + 2) * rest;
			}

			int offsetXinside = x3 - (offsetX - startFx) * 3 - offsetY* Wx3;
			for (int fy = startFy; fy < endFy; fy++)
			{
				unsigned char* srcImg2 = srcImg + fy * Wx3  + offsetXinside;
										
				for (int fx = startFx; fx < endFx; fx++)
				{
					double d = mtx.kernel[fy*mtx.w + fx];
					_r += *srcImg2++ *d;
					_g += *srcImg2++ *d;
					_b += *srcImg2++ *d;
				}
			}

			unsigned char* dstImg2 = dstImg + x3;
			if (_r > 255) _r = 255;  else if (_r < 0) _r = 0;
			if (_g > 255) _g = 255;  else if (_g < 0) _g = 0;
			if (_b > 255) _b = 255;  else if (_b < 0) _b = 0;
			*dstImg2++ = (unsigned char)(_r);
			*dstImg2++ = (unsigned char)(_g);
			*dstImg2++ = (unsigned char)(_b);

		}

	}
}

long bestScore = 2147483647;


//image imageResult1 = NULL;
image imageResult2 = NULL;
image imageResult3 = NULL;
int glActiveThreads = 0;
unsigned int __stdcall calcAllThread(void* mydata)
{
	if (*(int*)mydata == 0) calcAll(0, true);
	else if (*(int*)mydata == 2) calcAll(2, true);
	else if (*(int*)mydata == 10) calcAll(0 , false);
	else if (*(int*)mydata == 20) calcAll(2, false);
	//printf("Finish %d\n", *(int*)mydata);
	glActiveThreads--;
	return 0;
}

void calcAll(int dither, bool first)
{
	int startX = W * 0.2; //check speed?
	int endX = W - startX;
	int startY = H * 0.2;
	int endY = H - startY;
	#ifdef MYDEBUG
	printf("CUT: %d %d %d %d\n", startX, startY, endX, endY);
	#endif
	image im = NULL;
	//if (dither == 1) im = imageResult1;
	//else 
	if (dither == 0) im = imageResult2;
	else if (dither == 2) im = imageResult3;

	long totalScore = 0;
	unsigned char *v1 = im->pix;
	unsigned char *v2 = myDATA;

	/*
	for (int l = 0; l < totalColors; l++) {
		printf("R:%d G:%d B:%d\n", COLORMAP[l * 3], COLORMAP[l * 3 + 1], COLORMAP[l * 3 + 2]);
	}*/

	int mtxCenterStart = 4;
	int mtxCenterEnd = 9;
	if (!first) {
		mtxCenterStart=0;
		mtxCenterEnd = 4;
	}
	int offset = 0;
	//int mtxCenter = 3;
	//blur and check offset	
	image blured = img_new(W, H);
	for (int mtxG2 = 1; mtxG2 < 10; mtxG2++) //14 //checks
		for (int mtxCenter = mtxCenterStart; mtxCenter < mtxCenterEnd; mtxCenter++)
		{
		for (int mtxG1 = 1; mtxG1 < 6; mtxG1++) //6
		{
			if (mtxG1>3 && mtxG2>3 && mtxCenter>0) continue; //check

			if (clock() - begin_time > WAIT_TIME) return;
			matrix mtx = getGaussian(((mtxG1 * 2) - 1), ((mtxG2 * 2) - 1), KERNELSTEP*(float)mtxCenter);
			applyEffect(mtx, im, blured, startX, startY, endX, endY);

			long totalScore = 0;
			int diff = 0;
			for (int yy = startY; yy < endY; yy++)
			{
				offset = yy * W * 3 + startX * 3;
				v1 = blured->pix +offset;
				v2 = myDATA + offset;
				for (int xx = startX; xx < endX; xx++) {
					diff = (*v1++ - *v2++);
					totalScore += (diff * diff); //R
					diff = (*v1++ - *v2++); 
					totalScore += (diff * diff); //G
					diff = (*v1++ - *v2++);
					totalScore += (diff * diff); //B
				}
			}
			//if (mtxG1==1 && mtxG2==1 && dither == 0) totalScore = totalScore * 1.15; //check


			//printf("Matrix(%d,%d,%d) Total diff blured: %d (mode:%d)\n", mtxG1, mtxG2, mtxS, totalScore, dither);
			EnterCriticalSection(&glCS);
			if (bestScore > totalScore) {
				bestScore = totalScore;
				bestG1 = mtxG1;
				bestG2 = mtxG2;
				bestDither = dither;
				bestMtxCenter = mtxCenter;
			}
			LeaveCriticalSection(&glCS);

			free(mtx.kernel);
		}
	}
	free(blured);
}

void saveSecondResult(const char* outfile, unsigned char* data)
{
	FILE* out;
	out = fopen(outfile, "w");
	unsigned char* glVal = data;
	for (int y = 0; y < H; ++y) {
		for (int x = 0; x < W; ++x) {
			unsigned char _r = *glVal++; 
			unsigned char _g = *glVal++; 
			unsigned char _b = *glVal++;
			fprintf(out, "%d %d %d ", _r, _g, _b);			
		}
		fprintf(out, "\n");
	}
	fclose(out);
}

inline long calcPos(unsigned char* p, int x, int y)
{
	int closeWeight=1;
	int closeRatio = 2;
	long totalCost = 0;
	unsigned char _r2,_g2,_b2;
	unsigned char d = *p;
	unsigned char _r = COLORMAP[d*3];
	unsigned char _g = COLORMAP[d*3+1];
	unsigned char _b = COLORMAP[d*3+2];
	if (x>0) {
		int tmp = *(p-1);
		_r2 = COLORMAP[tmp*3];
		_g2 = COLORMAP[tmp*3+1];
		_b2 = COLORMAP[tmp*3+2];
		totalCost+= closeWeight*closeRatio*((_r - _r2)*(_r - _r2) + (_g - _g2)*(_g - _g2) + (_b - _b2)*(_b - _b2));
	}
	if(x<W-1) {
		int tmp = *(p+1);
		_r2 = COLORMAP[tmp*3];
		_g2 = COLORMAP[tmp*3+1];
		_b2 = COLORMAP[tmp*3+2];
		totalCost+= closeWeight* closeRatio*((_r - _r2)*(_r - _r2) + (_g - _g2)*(_g - _g2) + (_b - _b2)*(_b - _b2));
	}
	
	if(y>0) {
		int tmp = *(p-W);
		_r2 = COLORMAP[tmp*3];
		_g2 = COLORMAP[tmp*3+1];
		_b2 = COLORMAP[tmp*3+2];
		totalCost+= closeWeight* closeRatio*((_r - _r2)*(_r - _r2) + (_g - _g2)*(_g - _g2) + (_b - _b2)*(_b - _b2));
		/*
		if(x>0){
			tmp = *(p-W-1);
			_r2 = COLORMAP[tmp*3];
			_g2 = COLORMAP[tmp*3+1];
			_b2 = COLORMAP[tmp*3+2];
			totalCost+= closeWeight*(_r - _r2)*(_r - _r2) + (_g - _g2)*(_g - _g2) + (_b - _b2)*(_b - _b2);
		}
		if(x<W-1) {
			tmp = *(p-W+1);
			_r2 = COLORMAP[tmp*3];
			_g2 = COLORMAP[tmp*3+1];
			_b2 = COLORMAP[tmp*3+2];
			totalCost+= closeWeight*(_r - _r2)*(_r - _r2) + (_g - _g2)*(_g - _g2) + (_b - _b2)*(_b - _b2);
		}
		*/
	}
	if(y<H-1) {
		int tmp = *(p+W);
		_r2 = COLORMAP[tmp*3];
		_g2 = COLORMAP[tmp*3+1];
		_b2 = COLORMAP[tmp*3+2];
		totalCost+= closeWeight* closeRatio*((_r - _r2)*(_r - _r2) + (_g - _g2)*(_g - _g2) + (_b - _b2)*(_b - _b2));
		/*
		if(x>0){
			tmp = *(p+W-1);
			_r2 = COLORMAP[tmp*3];
			_g2 = COLORMAP[tmp*3+1];
			_b2 = COLORMAP[tmp*3+2];
			totalCost+= closeWeight*(_r - _r2)*(_r - _r2) + (_g - _g2)*(_g - _g2) + (_b - _b2)*(_b - _b2);
		}
		if(x<W-1) {
			tmp = *(p+W+1);
			_r2 = COLORMAP[tmp*3];
			_g2 = COLORMAP[tmp*3+1];
			_b2 = COLORMAP[tmp*3+2];
			totalCost+= closeWeight*(_r - _r2)*(_r - _r2) + (_g - _g2)*(_g - _g2) + (_b - _b2)*(_b - _b2);
		}*/
	}
	
	return totalCost;
}


void calcSecond() 
{
	bool orderedUp = true;
	bool orderedDown = true;
	bool errorCorrection = true;
	int score = 0;
	for (int k = 0; k < 16; k++)
	{
		int tmp = COLORMAP[k * 3] + COLORMAP[k * 3 + 1] + COLORMAP[k * 3 + 2];
		if (tmp > score) score = tmp;
		else {
			orderedUp = false;
			break;
		}
	}
	score = 255 * 5;
	for (int k = 0; k < 16; k++)
	{
		int tmp = COLORMAP[k * 3] + COLORMAP[k * 3 + 1] + COLORMAP[k * 3 + 2];
		if (tmp < score) score = tmp;
		else {
			orderedDown = false;
			break;
		}
	}
	bool ordered = orderedUp || orderedDown;
	if (orderedDown) errorCorrection = false;
	unsigned char mtxG2=1;
	unsigned char mtxG1 = 1;
	int mtxCenter = 0;
	if (!ordered)
	{
		mtxCenter = COLORMAP[0 * 3 + 2] % 10;//center
		mtxCenter--;
		if (mtxCenter < 0) mtxCenter = 0;

		unsigned char tmp = COLORMAP[3 * 3 + 2] % 10;//G2 part 2
		unsigned char mtxS = tmp;
		if (mtxS>0) mtxS--;
		tmp = COLORMAP[2*3+2]%10;//bestG2
		if (tmp>=0 && tmp<=1) mtxG2=1;
		else if (tmp>=2 && tmp<=3) mtxG2=2;
		else if (tmp>=4 && tmp<=5) mtxG2=3;
		else if (tmp>=6 && tmp<=7) mtxG2=4;
		else if (tmp>=8 && tmp<=9) mtxG2=5;

		if (mtxG2 == 5) mtxG2 += mtxS;
		
		tmp = COLORMAP[1*3+2]%10;//mtxG1
		if (tmp>=0 && tmp<=1) mtxG1=1;
		else if (tmp>=2 && tmp<=3) mtxG1=2;
		else if (tmp>=4 && tmp<=5) mtxG1=3;
		else if (tmp>=6 && tmp<=7) mtxG1=4;
		else if (tmp>=8 && tmp<=9) mtxG1=5;		

		bool secondStageOrder = true;
		int score = 255+255+255;
		for (int k = 4; k < 16; k++)
		{
			int tmp = COLORMAP[k * 3] + COLORMAP[k * 3 + 1] + COLORMAP[k * 3 + 2];
			if (tmp <= score) score = tmp;
			else {
				secondStageOrder = false;
				break;
			}
		}

		if (secondStageOrder) errorCorrection = false;

		//if (mtxG2>=2 && mtxG1>=2) errorCorrection = false; //CHECK

	}
	//errorCorrection = true; //check
	//ordered = true; //check

	
	bool correctH = true;
	bool correctW = true;
	if (mtxG2>=3 || mtxG1>=3)
	{
		correctH = false;
		correctW = false;
		if (mtxG1>=3)  correctW = true;
		if (mtxG2>=3)  correctH = true;
	}

	// errorCorrection = true;
	// correctH = correctW = false;
	// if (mtxG1>mtxG2)  correctW = true;
	// else correctH = true;
	
	errorCorrection = true;
	correctH = correctW = true;

	//if (mtxG2 >= 2 || mtxG1 >= 2)
	//{
	//	correctH = false;
	//	correctW = false;
	//	if (mtxG1 >= 3)  correctW = true;
	//	if (mtxG2 >= 3)  correctH = true;
	//}

	if (mtxG2==mtxG1 && mtxCenter==0 && mtxG1>1)  errorCorrection=0;
	
	#ifdef MYDEBUG
	printf("Ordered:%d  mtxG2:%d mtxG1:%d (center:%d)\n",ordered,mtxG2,mtxG1, mtxCenter);
	printf("Error correction: %d\n", errorCorrection);
	printf("Max pissible errors: %d\n", W*H / 5);
	#endif
	
	image imgData = img_new(W, H);

	//fix corrupted pix here
	if (errorCorrection)
	{
		unsigned char* processed = (unsigned char*)malloc(H*W * sizeof(unsigned char));
		memset(processed, 0, H*W);
		int toH = H - 1;
		int toW = W - 1;
		int totalSwaps = 0;
		for (int loop = 0; loop < 3; loop++)
		{
			for (int y = 0; y < toH; y++)
			{
				unsigned char* srcImg = myDATA + (y*W);
				unsigned char*  pProcessed = processed + y * W;
				unsigned char temp;
				for (int x = 0; x < toW; x++)
				{
					float swapRatioY = 0;
					float swapRatioH = 0;
					if ((*pProcessed == 0) || (*(pProcessed + W) == 0) || (*(pProcessed + 1) == 0))
					{
						if (correctW && ((*pProcessed == 0) || *(pProcessed + W) == 0))
						{
							if (*srcImg != *(srcImg + W))
							{
								int cost1 = calcPos(srcImg, x, y);
								int cost2 = calcPos(srcImg + W, x, y+1);
								temp = *srcImg;
								*srcImg = *(srcImg + W);
								*(srcImg + W) = temp;
								int cost3 = calcPos(srcImg, x, y);
								int cost4 = calcPos(srcImg + W, x, y+1);

								temp = *srcImg;
								*srcImg = *(srcImg + W);
								*(srcImg + W) = temp;

								if (cost3 + cost4 < cost1 + cost2)
								{
									swapRatioY = (float)(cost1 + cost2) / (cost3 + cost4);
								}
							}
						}

						if (correctH && ((*pProcessed == 0) || *(pProcessed + 1) == 0))
						{
							if (*srcImg != *(srcImg + 1))
							{
								int cost1 = calcPos(srcImg, x, y);
								int cost2 = calcPos(srcImg + 1, x, y);
								temp = *srcImg;
								*srcImg = *(srcImg + 1);
								*(srcImg + 1) = temp;
								int cost3 = calcPos(srcImg, x, y);
								int cost4 = calcPos(srcImg + 1, x+1, y);
								temp = *srcImg;
								*srcImg = *(srcImg + 1);
								*(srcImg + 1) = temp;
								if (cost3 + cost4 < cost1 + cost2)
								{
									swapRatioH = (float)(cost1 + cost2) / (cost3 + cost4);
								}
							}
						}

						if (swapRatioY >= swapRatioH && swapRatioY > 1.80) //check 1.60
						{

							totalSwaps++;
							temp = *srcImg;
							*srcImg = *(srcImg + W);
							*(srcImg + W) = temp;
							*pProcessed = 1;
							*(pProcessed + W) = 1;
						}
						else if (swapRatioY < swapRatioH &&  swapRatioH >= 1.80)//check
						{
							totalSwaps++;
							temp = *srcImg;
							*srcImg = *(srcImg + 1);
							*(srcImg + 1) = temp;
							*pProcessed = 1;
							*(pProcessed + 1) = 1;
						}

					}
					srcImg++;
					pProcessed++;
				}
			}
		}//loop passes
		#ifdef MYDEBUG	
		printf("Total swaps:%d\n", totalSwaps);
		totalSwaps = 0;
		#endif
		free(processed);
		
	}
	

	
	int totalPixels = W*H;
	unsigned char* glVal = myDATA;	
	unsigned char* pData = imgData->pix;
	
	for (int p = 0; p < totalPixels; p++) 
	{
		unsigned char colorIdx = *glVal * 3;
		glVal++;
		*pData++ = COLORMAP[colorIdx]; 
		*pData++ = COLORMAP[colorIdx+1];
		*pData++ = COLORMAP[colorIdx+2];
	}
	
	#ifdef MYDEBUG	
	//write_ppm(imgData, (char*)"afterCorrection.ppm");
	#endif
	if (!ordered){
		image blured = img_new(W, H);
		double center = (KERNELSTEP*(float)mtxCenter - 0.5);
		if (mtxCenter >= 7) center -= 0.50; //check		
		//double center = (KERNELSTEP*(float)mtxCenter);
		if (center < 0) center = 0;
		matrix mtx = getGaussian(((mtxG1 * 2) - 1), ((mtxG2 * 2) - 1), center);
		#ifdef MYDEBUG
		 for (int z = 0; z < mtx.h; z++)
		 {
			 for (int m = 0; m < mtx.w; m++)
			 {
				 printf("%.03f ", mtx.kernel[z*mtx.w + m]);
			 }
			 printf("\n");
		 }
		#endif
		applyEffect(mtx, imgData, blured, 0, 0, W, H);

		// if (mtxG1 > 3 || mtxG2 > 3) {
			// memcpy(imgData->pix, blured->pix, W*H * 3);
			// applyEffect(mtx, imgData, blured, 0, 0, W, H);
		// }

		saveSecondResult("imrec.out", blured->pix);
		free(blured);

	} else{
		saveSecondResult("imrec.out", imgData->pix);
	}
	free(imgData);
	
	
}



int _tmain(int argc, _TCHAR* argv[])
{	
	
	/*
	matrix mtx = getGaussian(5, 5, 0.10);
	for (int z = 0; z < mtx.h; z++)
	{
		for (int m = 0; m < mtx.w; m++)
		{
			printf("%.03f ", mtx.kernel[z*mtx.w + m]);
		}
		printf("\n");
	}
	*/

	InitializeCriticalSection(&glCS);
	if (!loadInput("imrec.in")) return -1;
	//loadInput("00.in");
	//loadInput("D:\\temp\\visualizer_windows\\viz\\tests\\02.in");
	//loadInput("E:\\c++\\marathon2\\visualizer_windows\\viz\\tests\\03.in");
	//image im = read_ppm((char*)"test.ppm");

#ifdef MYDEBUG
	printf("Load in %dms\n", (clock() - begin_time));
#endif

	if (mode == 0) {
		
		unsigned char* buffer = (unsigned char*)malloc(W*H * 4);
		unsigned char* bufferOut = (unsigned char*)malloc(W*H);
		int totalK = W * H;
		unsigned char* pBuf = buffer;
		unsigned char* pSrc = myDATA;
		for (int k = 0; k < totalK; k++) {
			*pBuf++ = *pSrc++; //R
			*pBuf++ = *pSrc++;//G
			*pBuf++ = *pSrc++;//B
			*pBuf++; //Alpha
		}

		compete_data *pExq = compete_init();
		compete_no_transparency(pExq);
		compete_feed(pExq, buffer, totalK);
		compete_quantize_ex(pExq, MAX_COLORS, 1);

		unsigned char* pPalette = (unsigned char*)malloc(16 * 4);
		compete_get_palette(pExq, pPalette, MAX_COLORS);

		//compete_map_image(pExq, totalK, buffer, bufferOut);
		/*
		compete_map_image_dither(pExq, W, H, buffer, bufferOut, false);

		imageResult1 = img_new(W, H);
		pBuf = imageResult1->pix;
		pSrc = bufferOut;
		for (int k = 0; k < totalK; k++) {
			int idx = *pSrc++;
			*pBuf++ = pPalette[idx * 4]; //R
			*pBuf++ = pPalette[idx * 4+1];//G
			*pBuf++ = pPalette[idx * 4+2];//B
		}
		*/


		compete_map_image(pExq, totalK, buffer, bufferOut);
		imageResult2 = img_new(W, H);
		pBuf = imageResult2->pix;
		pSrc = bufferOut;
		for (int k = 0; k < totalK; k++) {
			int idx = *pSrc++;
			*pBuf++ = pPalette[idx * 4]; //R
			*pBuf++ = pPalette[idx * 4 + 1];//G
			*pBuf++ = pPalette[idx * 4 + 2];//B
		}

		compete_map_image_dither(pExq, W, H, buffer, bufferOut, true);
		imageResult3 = img_new(W, H);
		pBuf = imageResult3->pix;
		pSrc = bufferOut;
		for (int k = 0; k < totalK; k++) {
			int idx = *pSrc++;
			*pBuf++ = pPalette[idx * 4]; //R
			*pBuf++ = pPalette[idx * 4 + 1];//G
			*pBuf++ = pPalette[idx * 4 + 2];//B
		}

		//CALC color map
		for (int l = 0; l < 16; l++) {
			COLORMAP[l * 3] = pPalette[l * 4];
			COLORMAP[l * 3+1] = pPalette[l * 4+1];
			COLORMAP[l * 3+2] = pPalette[l * 4+2];
		}

	
		free(buffer);
		free(bufferOut);
		free(pPalette);
		compete_free(pExq);




		//int scanmode1 = 1;
		//glActiveThreads = 0;
		//glActiveThreads++;
		//_beginthreadex(0, 0, &calcAllThread, (void*)&scanmode1, 0, NULL);
		int scanmode0 = 0;
		glActiveThreads++;
		_beginthreadex(0, 0, &calcAllThread, (void*)&scanmode0, 0, NULL);
		int scanmode10 = 10;
		glActiveThreads++;
		_beginthreadex(0, 0, &calcAllThread, (void*)&scanmode10, 0, NULL);		
		
		int scanmode2 = 2;
		glActiveThreads++;
		_beginthreadex(0, 0, &calcAllThread, (void*)&scanmode2, 0, NULL);
		int scanmode20 = 20;
		glActiveThreads++;
		_beginthreadex(0, 0, &calcAllThread, (void*)&scanmode20, 0, NULL);		
		
		while (glActiveThreads > 0) Sleep(1);


		#ifdef MYDEBUG
		printf("Best Matrix(%d,%d) Total diff blured: %d (mode:%d , mtxCenter:%d)\n", bestG1, bestG2, bestScore, bestDither, bestMtxCenter);
		#endif

		// if (bestDither == 1) {
			// saveResult(imageResult1->pix, "imrec.out");
		// }
		// else 
		//if ((bestDither == 2) && (bestG1 <= 2) && (bestG2 <= 2)) bestDither = 0; //check

		if (bestDither == 0) {
			saveResult(imageResult2->pix, "imrec.out");
		}
		else if (bestDither == 2) {
			saveResult(imageResult3->pix, "imrec.out");
		}
		//free(imageResult1);
		free(imageResult2);
		free(imageResult3);
	}
	else {
		calcSecond();
	}

	DeleteCriticalSection(&glCS);
#ifdef MYDEBUG
	printf("Done in %dms\n", (clock() - begin_time));
#endif
}