#include <stdio.h>
#include <math.h>
#include <string.h>

#include "quant.h"
#include "color.h"


#define CUBE_INC    0.1

static int color_UseWeight;
int color_stat = 0;



static float exp2(float x)
{
	float i;
	float ret = 1.0;

	for (i = 0.0; i < x; i++)
	{
		ret *= 2.0;
	}

	return(ret);
}


void Cube_Print(colorCube_t *c)
{
	printf("(%f %f %f) (%f %f %f)\n", 
		c->r1, c->g1, c->b1,
		c->r2, c->g2, c->b2);
}


void Cube_Copy(colorCube_t *d, colorCube_t *s)
{
	d->r1 = s->r1; d->g1 = s->g1; d->b1 = s->b1;
	d->r2 = s->r2; d->g2 = s->g2; d->b2 = s->b2;
}


int Cube_Point(colorCube_t *c, float r, float g, float b)
{
    if ((r < c->r1)  || (g < c->g1)  || (b < c->b1))  return(0);
    if ((r >= c->r2) || (g >= c->g2) || (b >= c->b2)) return(0);

	return(1);
}


int Cube_Average(colorList_t *clist, colorCube_t *c, 
				  float *dest_r, float *dest_g, float *dest_b)
{
	float count = 0.0;
	color_t *p;

	*dest_r = *dest_g = *dest_b = 0.0;

	for (p = clist->colorHead; p != NULL; p = p->n)
	{
		if (Cube_Point(c, p->r, p->g, p->b))
		{
			*dest_r += (p->r * p->count);
			*dest_g += (p->g * p->count);
			*dest_b += (p->b * p->count);

			count += p->count;
		}
	}

	// make sure there were some colors in cube
	if (count == 0.0) return(0);

	*dest_r = *dest_r / count;
	*dest_g = *dest_g / count;
	*dest_b = *dest_b / count;

    return(1);
}


float Cube_Count(colorList_t *clist, colorCube_t *c)
{
	float count = 0.0;
	color_t *p;

	for (p = clist->colorHead; p != NULL; p = p->n)
	{
		if (Cube_Point(c, p->r, p->g, p->b)) count += 1.0;
	}

	return(count);
}



float Cube_Weight(colorList_t *clist, colorCube_t *c)
{
	float weight = 0.0;
	color_t *p;

    for (p = clist->colorHead; p != NULL; p = p->n)
	{
		if (Cube_Point(c, p->r, p->g, p->b)) weight += p->count;
	}

	return(weight);
}



static float Median_FitCube(colorList_t *clist, colorCube_t *c)
{
	colorCube_t t;
	color_t *p;
	float count = 0.0;

	// set up target cube
	t.r1 = t.g1 = t.b1 = 9999999.0;
	t.r2 = t.g2 = t.b2 = -9999999.0;

	for (p = clist->colorHead; p != NULL; p = p->n)
	{
		// make sure point is in cube
		if (!Cube_Point(c, p->r, p->g, p->b)) continue;

		// ok point is in cube
		if (p->r < t.r1) t.r1 = p->r;
		if (p->r > t.r2) t.r2 = p->r;

		if (p->g < t.g1) t.g1 = p->g;
		if (p->g > t.g2) t.g2 = p->g;

		if (p->b < t.b1) t.b1 = p->b;
		if (p->b > t.b2) t.b2 = p->b;

		count += 1.0;
	}

	// adjust t
	t.r2 += 1.0;
	t.g2 += 1.0;
	t.b2 += 1.0;

	Cube_Copy(c, &t);
	// Cube_Print(c);

	return(count);
}


///////////////////////////////////////////////////////////////////////
////   Split by count
///////////////////////////////////////////////////////////////////////

static void Median_SplitR(colorList_t *clist, colorCube_t *c, 
						  colorCube_t *c1, colorCube_t *c2)
{
	int splitCount = 0;
	float newr, count;
	color_t *p;

	// find total nuber of colors to split
	for (p = clist->colorHead; p != NULL; p = p->n)
	{
		if (Cube_Point(c, p->r, p->g, p->b)) splitCount++;
	}

	splitCount = splitCount / 2;

	// only r changes
	c1->g1 = c2->g1 = c->g1;
	c1->g2 = c2->g2 = c->g2;

	c1->b1 = c2->b1 = c->b1;
	c1->b2 = c2->b2 = c->b2;

	for (newr = c->r1; newr < c->r2; newr += 0.1)
	{
		// form tentative new cubes
		c1->r1 = c->r1;
		c1->r2 = newr;

		c2->r1 = newr;
		c2->r2 = c->r2;

		// count number of points in c1
		count = Cube_Count(clist, c1);

		// if count gets to split count then we are done
		if (count >= splitCount) break;
	}
}


static void Median_SplitG(colorList_t *clist, colorCube_t *c, 
						  colorCube_t *c1, colorCube_t *c2)
{
	int splitCount = 0;
	color_t *p;
	float newg, count;

	// find total nuber of colors to split
	for (p = clist->colorHead; p != NULL; p = p->n)
	{
		if (Cube_Point(c, p->r, p->g, p->b)) splitCount++;
	}

	splitCount = splitCount / 2;

	// only g changes
	c1->r1 = c2->r1 = c->r1;
	c1->r2 = c2->r2 = c->r2;

	c1->b1 = c2->b1 = c->b1;
	c1->b2 = c2->b2 = c->b2;

	for (newg = c->g1; newg < c->g2; newg += 0.1)
	{
		// form tentative new cubes
		c1->g1 = c->g1;
		c1->g2 = newg;

		c2->g1 = newg;
		c2->g2 = c->g2;

		// count number of points in c1
		count = Cube_Count(clist, c1);

		// if count gets to split count then we are done
		if (count >= splitCount) break;
	}
}


static void Median_SplitB(colorList_t *clist, colorCube_t *c, 
						  colorCube_t *c1, colorCube_t *c2)
{
	int splitCount = 0;
	color_t *p;
	float newb, count;

	// find total nuber of colors to split
	for (p = clist->colorHead; p != NULL; p = p->n)
	{
		if (Cube_Point(c, p->r, p->g, p->b)) splitCount++;
	}

	splitCount = splitCount / 2;

	// only b changes
	c1->r1 = c2->r1 = c->r1;
	c1->r2 = c2->r2 = c->r2;

	c1->g1 = c2->g1 = c->g1;
	c1->g2 = c2->g2 = c->g2;

	for (newb = c->b1; newb < c->b2; newb += 0.1)
	{
		// form tentative new cubes
		c1->b1 = c->b1;
		c1->b2 = newb;

		c2->b1 = newb;
		c2->b2 = c->b2;

		// count number of points in c1
		count = Cube_Count(clist, c1);

		// if count gets to split count then we are done
		if (count >= splitCount) break;
	}
}



////////////////////////////////////////////////////////////////////////
/// Split by weight
////////////////////////////////////////////////////////////////////////

static void Median_WSplitR(colorList_t *clist, colorCube_t *c, 
						   colorCube_t *c1, colorCube_t *c2)
{
	float splitWeight = 0.0;
	color_t *p;
	float newr, weight;

	// find total weight to split
	for (p = clist->colorHead; p != NULL; p = p->n)
	{
		// if (Cube_Point(c, r, g, b)) splitCount++;
		if (Cube_Point(c, p->r, p->g, p->b)) 
		{
			splitWeight += p->count;
		}
	}

	splitWeight /= 2.0;

	// only r changes
	c1->g1 = c2->g1 = c->g1;
	c1->g2 = c2->g2 = c->g2;

	c1->b1 = c2->b1 = c->b1;
	c1->b2 = c2->b2 = c->b2;

	for (newr = c->r1; newr < c->r2; newr += 0.1)
	{
		// form tentative new cubes
		c1->r1 = c->r1;
		c1->r2 = newr;

		c2->r1 = newr;
		c2->r2 = c->r2;

		// get weight in cube
		weight = Cube_Weight(clist, c1);

		// if count gets to split weight then we are done
		if (weight >= splitWeight) break;
	}
}


static void Median_WSplitG(colorList_t *clist, colorCube_t *c, 
					    colorCube_t *c1, colorCube_t *c2)
{
	float splitWeight = 0.0;
	color_t *p;
	float newg, weight;

	// find total nuber of colors to split
	for (p = clist->colorHead; p != NULL; p = p->n)
	{
		//if (Cube_Point(c, r, g, b)) splitCount++;
		if (Cube_Point(c, p->r, p->g, p->b)) 
		{
			splitWeight += p->count;
		}
	}

	splitWeight /= 2.0;

	// only g changes
	c1->r1 = c2->r1 = c->r1;
	c1->r2 = c2->r2 = c->r2;

	c1->b1 = c2->b1 = c->b1;
	c1->b2 = c2->b2 = c->b2;

	for (newg = c->g1; newg < c->g2; newg += 0.1)
	{
		// form tentative new cubes
		c1->g1 = c->g1;
		c1->g2 = newg;

		c2->g1 = newg;
		c2->g2 = c->g2;

		// get weight in c1
		weight = Cube_Weight(clist, c1);

		// if count gets to split count then we are done
		if (weight >= splitWeight) break;
	}
}


static void Median_WSplitB(colorList_t *clist, colorCube_t *c, 
						  colorCube_t *c1, colorCube_t *c2)
{
	float splitWeight = 0.0;
	color_t *p;
	float newb, weight;

	// find total nuber of colors to split
	for (p = clist->colorHead; p != NULL; p = p->n)
	{
		// if (Cube_Point(c, r, g, b)) splitCount++;
		if (Cube_Point(c, p->r, p->g, p->b)) 
		{
			splitWeight += p->count;
		}
	}

	splitWeight /= 2.0;

	// only b changes
	c1->r1 = c2->r1 = c->r1;
	c1->r2 = c2->r2 = c->r2;

	c1->g1 = c2->g1 = c->g1;
	c1->g2 = c2->g2 = c->g2;

	for (newb = c->b1; newb < c->b2; newb += 0.1)
	{
		// form tentative new cubes
		c1->b1 = c->b1;
		c1->b2 = newb;

		c2->b1 = newb;
		c2->b2 = c->b2;

		// count number of points in c1
		weight = Cube_Weight(clist, c1);

		// if count gets to split count then we are done
		if (weight >= splitWeight) break;
	}
}




static void Median_SplitCube(colorList_t *clist, colorCube_t *c, 
							 colorCube_t *c1, colorCube_t *c2)
{
	float dr, dg, db;

	// find longest axis
	dr = c->r2 - c->r1;
	dg = c->g2 - c->g1;
	db = c->b2 - c->b1;

	if ((dr >= dg) && (dr >= db))
	{
		// r axis is longest
		if (color_UseWeight) Median_WSplitR(clist, c, c1, c2);
        else                 Median_SplitR(clist, c, c1, c2);
	}

	else if ((dg >= dr) && (dg >= db))
	{
		// g axis is longest
		if (color_UseWeight) Median_WSplitG(clist, c, c1, c2);
		else                 Median_SplitG(clist, c, c1, c2);
	}

	else
	{
		// b axis is longest
		if (color_UseWeight) Median_WSplitB(clist, c, c1, c2);
		else                 Median_SplitB(clist, c, c1, c2);
	}
}


static void Median_SplitList(colorList_t *source, colorList_t *dest, colorCube_t *c)
{
	color_t *p, *q;

	Color_Init(dest);

	for (p = source->colorHead; p != NULL;)
	{
		// if p is a perm node then don't take out
		if (p->flags & CF_Perm)
		{
			p = p->n;
			continue;
		}

		// if this node is in cube then add to dest
		if (Cube_Point(c, p->r, p->g, p->b))
		{
			// take p out of list, return next good point in list
			q = Color_RemoveNode(source, p);

			// put node in new list
			Color_AddHead(dest, p);

			// very important
			p = q;
		}
		else
		{
			p = p->n;
		}
	}
}

static void P_CheckList(colorList_t *clist)
{
	color_t *p;
	int count = 0;

	for (p = clist->colorHead; p != NULL; p = p->n)
	{
		if ((p->flags & CF_Perm) == 0)
		{
			count++;
		}
	}

	if (count)
	{
        printf("WARNING -- %d Nodes left in clist!!!!!!!!!!!\n", count);
	}
}


	
static void Median_Reduce_r(colorList_t *clist, colorList_t *dlist, 
							colorCube_t *c, int cur_depth, int depth)
{
	colorCube_t c1, c2;
	colorList_t left, right;
	float r, g, b;

    // see if we are done
	if (cur_depth == depth) 
	{
		// average colors in cube and insert into dlist
		// if cube_average returns 0 then no colors were found
		// in cube
		if (!Cube_Average(clist, c, &r, &g, &b)) return;

		// add color
		// fixme: add weight
		Color_AddTuple(dlist, r, g, b, 1);

/*
		if (Sys_CheckParm("-v"))
		{
		    printf("Median: Added %d colors.\n", dlist->colorCount);
		}
*/
		return;
	}

	// fit cube around its points, Median_FirCube returns number
	// of points found in cube.  If no points found then
	// don't continue processing.
	if (Median_FitCube(clist, c) <= 0.0) return;

	// split cube into two cubes with same number of points
	// in each cube
	Median_SplitCube(clist, c, &c1, &c2);

	// we could actually break list into two lists at this point
	// Median_SplitList will init new clist
	Median_SplitList(clist, &left,  &c1);
	Median_SplitList(clist, &right, &c2);

	P_CheckList(clist);

	if (depth < 2)
	{
		printf("split: %d, %d\n", left.colorCount, right.colorCount);
	}

	// recurse with each cube
	Median_Reduce_r(&left,  dlist, &c1, cur_depth + 1, depth);
	Median_Reduce_r(&right, dlist, &c2, cur_depth + 1, depth);

	Color_Free(&left);
	Color_Free(&right);
}




void Median_Reduce(colorList_t *clist, int depth, int useWeight)
{
	colorCube_t c;
	color_t *p;
	colorList_t dlist;

	color_UseWeight = useWeight;

	// destination list
	Color_Init(&dlist);

	// set up initial cube
	c.r1 = c.g1 = c.b1 = 0.0;
	c.r2 = c.g2 = c.b2 = 256.0;

	// rock
	Median_Reduce_r(clist, &dlist, &c, 0, depth);	

	// we need to take all perm nodes out of clist and pu them in
	// dlist
	for (p = clist->colorHead; p != NULL; p = p->n)
	{
		if (p->flags & CF_Perm)
		{
			Color_ForceTuple(&dlist, p->index, p->r, p->g, p->b, p->flags, p->count);
		}
	}

	// clear clist
	Color_Free(clist);
	Color_Set(clist, &dlist);
}


void Median_Begin(void)
{
	color_stat = 1;
}


void Median_End(void)
{
	color_stat = 0;
	printf("\n");
}